基于MPU9250获取姿态航向角stm32及matlab源码

使用正点原子阿波罗F429开发板，读取mpu9250数据，矫正滤波后利用基于四元数的madgwick算法融合数据，求得航向角。四元数也可直接转化为横滚角和俯仰角。（检查了一下，均值滤波那里写的不对，大家可以改一下）：）

function [axayaz] = accelerometer（）
%UNtitleD 此处显示有关此函数的摘要
%   此处显示详细说明
% 加速度计矫正
time=[];
x=[];
y=[];
z=[];
others=[];
for i=1:18
filename = [‘.\加速度计矫正\‘ num2str（i） ‘a.txt‘];
[timex1y1z1others]=textread（filename‘%n%n%n%n%n‘‘delimiter‘‘‘）;
x1=x1‘;
y1=y1‘;
z1=z1‘;
x=[xx1];
y=[yy1];
z=[zz1];

end
%空间二次曲面拟合算法
num_points = length（x）;
%一次项统计平均
x_avr = sum（x）/num_points;
y_avr = sum（y）/num_points;
z_avr = sum（z）/num_points;
%二次项统计平均
xx_avr = sum（x.*x）/num_points;
yy_avr = sum（y.*y）/num_points;
zz_avr = sum（z.*z）/num_points;
xy_avr = sum（x.*y）/num_points;
xz_avr = sum（x.*z）/num_points;
yz_avr = sum（y.*z）/num_points;
%三次项统计平均
xxx_avr = sum（x.*x.*x）/num_points;
xxy_avr = sum（x.*x.*y）/num_points;
xxz_avr = sum（x.*x.*z）/num_points;
xyy_avr = sum（x.*y.*y）/num_points;
xzz_avr = sum（x.*z.*z）/num_points;
yyy_avr = sum（y.*y.*y）/num_points;
yyz_avr = sum（y.*y.*z）/num_points;
yzz_avr = sum（y.*z.*z）/num_points;
zzz_avr = sum（z.*z.*z）/num_points;
%四次项统计平均
yyyy_avr = sum（y.*y.*y.*y）/num_points;
zzzz_avr = sum（z.*z.*z.*z）/num_points;
xxyy_avr = sum（x.*x.*y.*y）/num_points;
xxzz_avr = sum（x.*x.*z.*z）/num_points;
yyzz_avr = sum（y.*y.*z.*z）/num_points;


%计算求解线性方程的系数矩阵
A0 = [yyyy_avr yyzz_avr xyy_avr yyy_avr yyz_avr yy_avr;
     yyzz_avr zzzz_avr xzz_avr yzz_avr zzz_avr zz_avr;
     xyy_avr  xzz_avr  xx_avr  xy_avr  xz_avr  x_avr;
     yyy_avr  yzz_avr  xy_avr  yy_avr  yz_avr  y_avr;
     yyz_avr  zzz_avr  xz_avr  yz_avr  zz_avr  z_avr;
     yy_avr   zz_avr   x_avr   y_avr   z_avr   1;];
%计算非齐次项
b = [-xxyy_avr;
     -xxzz_avr;
     -xxx_avr;
     -xxy_avr;
     -xxz_avr;
     -xx_avr];

resoult = inv（A0）*b;
%resoult = solution_equations_n_yuan（A0b）;

x00 = -resoult（3）/2   ;              %拟合出的x坐标
y00 = -resoult（4）/（2*resoult（1））  ;  %拟合出的y坐标
z00 = -resoult（5）/（2*resoult（2））  ;  %拟合出的z坐标

AA = sqrt（x00*x00 + resoult（1）*y00*y00 + resoult（2）*z00*z00 - resoult（6））;  % 拟合出的x方向上的轴半径
BB = AA/sqrt（resoult（1））   ;                                               % 拟合出的y方向上的轴半径
CC = AA/sqrt（resoult（2））  ;                                                % 拟合出的z方向上的轴半径

% 读入待矫正数据
filename =  ‘ACCELEROMETER_tag_walk_flat.txt‘;
[time1x2y2z2others1]=textread（filename‘%n%n%n%n%n‘‘delimiter‘‘‘）;
aaa=sqrt（AA*AA+BB*BB+CC*CC）;
ax = x2-x00;
ay = y2-y00;
az = z2-z00;
ax = ax/AA*aaa;
ay = ay/BB*aaa;
az = az/CC*aaa;
ax = smooth（ax8）;
ay = smooth（ay8）;
az = smooth（az8）;
end

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2020-06-10 18:09  整合代码\
     目录           0  2020-06-10 18:09  整合代码\matlab\
     文件       93068  2020-05-08 19:28  整合代码\matlab\ACCELEROMETER_tag_walk_flat.txt
     文件        3790  2020-04-30 18:47  整合代码\matlab\AHRSupdate.m
     文件       97980  2020-05-08 19:28  整合代码\matlab\GYROSCOPE_tag_walk_flat.txt
     文件       81007  2020-05-08 19:28  整合代码\matlab\MAGNETICFIELD_tag_walk_flat.txt
     文件       80725  2020-05-08 19:28  整合代码\matlab\ORIENTATION_tag_walk_flat.txt
     文件        2625  2020-04-15 17:45  整合代码\matlab\accelerometer.m
     文件        1117  2020-04-16 22:28  整合代码\matlab\correct.m
     文件         602  2020-04-16 22:07  整合代码\matlab\gyroscope.m
     文件        2435  2020-04-15 14:56  整合代码\matlab\magneticfield.m
     文件        2706  2020-05-09 13:38  整合代码\matlab\main_madgwick.m
     目录           0  2020-06-10 18:09  整合代码\matlab\test\
     目录           0  2020-06-10 18:09  整合代码\matlab\test\1.加速度计\
     文件      576384  2020-04-24 16:32  整合代码\matlab\test\1.加速度计\水平静止.txt
     文件      159127  2020-04-24 17:25  整合代码\matlab\test\1.加速度计\运动状态.txt
     文件       72305  2020-05-10 15:02  整合代码\matlab\test\1.加速度计\运动状态2.txt
     目录           0  2020-06-10 18:09  整合代码\matlab\test\2.陀螺仪\
     文件      639393  2020-04-24 16:32  整合代码\matlab\test\2.陀螺仪\水平静止.txt
     文件       97422  2020-04-25 15:15  整合代码\matlab\test\2.陀螺仪\运动状态.txt
     文件       78668  2020-05-10 15:02  整合代码\matlab\test\2.陀螺仪\运动状态2.txt
     目录           0  2020-06-10 18:09  整合代码\matlab\test\3.磁力计\
     文件      498527  2020-04-24 16:32  整合代码\matlab\test\3.磁力计\水平静止.txt
     文件      291026  2020-04-29 19:57  整合代码\matlab\test\3.磁力计\磁力计矫正.txt
     文件       62277  2020-05-10 16:44  整合代码\matlab\test\3.磁力计\运动.txt
     文件        3350  2020-05-05 13:59  整合代码\matlab\test\correct_tuoqiu.m
     文件         966  2020-05-12 13:45  整合代码\matlab\test\test_acce.m
     文件       35734  2020-04-25 12:30  整合代码\matlab\test\加噪椭球.jpg
     文件       12752  2020-05-05 14:05  整合代码\matlab\test\加速度计校正前.jpg
     文件       45742  2020-05-05 14:04  整合代码\matlab\test\加速度计椭球矫正.txt
     文件        2462  2020-05-04 15:36  整合代码\matlab\test\加速度计椭球矫正1.txt
............此处省略503个文件信息