mahony算法，加速度计磁强计陀螺仪数据融合MATLAB程序
作者：互联网
%%加速度计，陀螺仪和磁强计的数据融合
a=[0.0088 0.0093 0.998 %%%加速度计测量值
0.0063 0.0088 0.9995
0.0034 0.0049 1.001
0.0024 0.0029 0.9985
0.001 0.0005 0.999
0.0024 0.002 1
0.001 0.0015 0.999
0.001 0.0005 1.002
0.002 0.0015 1.0029
0.0034 0 0.999
0.0039 0.0039 0.998
0.0024 0.0024 0.9985
-0.0015 -0.0034 0.9995
0.0034 0.0063 1.002
0.0005 -0.002 0.999
-0.0015 0.0317 1
-0.0088 0.0156 1
-0.0464 0.0244 1.0005
0.0864 -0.0117 1.0093
0.0234 0.0308 1
0.0718 -0.0156 1.0005
0.0479 -0.0195 1.001
-0.0029 -0.0376 1.0063
0.0146 0.0786 0.9912
0.04 0.0688 0.9961
0.0288 0.0347 0.999
0.0176 0.0366 0.9941
0.0044 0.0444 0.9917
0.0283 0.021 0.9976
0.043 0.0537 0.9917
0.0615 0.0503 0.9893
0.001 0.0171 0.9985
0.0513 -0.001 0.9878
0.0171 0.0073 0.9951
-0.0039 0.0039 0.9976
-0.0034 -0.0088 0.999
0.0088 0.002 0.9985
-0.0122 -0.0024 0.9995
0.0054 0.0015 0.998
0.0044 0.0034 0.9971
0.0088 0.0068 0.998
0.0093 0.0088 0.9976
0.0098 0.0098 0.9985
0.0117 0.0117 1
0.0117 0.0122 0.9985
0.0127 0.0117 0.9961
0.0122 0.0142 0.9985
0.0142 0.0137 0.998
0.0151 0.0146 0.9976
0.0142 0.0161 1.0005
0.0142 0.0137 0.9971
0.0151 0.0161 0.9976
0.0176 0.0161 0.9951
0.0146 0.0156 0.9976
0.0142 0.0161 0.9976
0.0166 0.0166 0.9956
0.0156 0.0171 0.9985
0.0166 0.0156 0.9966
0.0161 0.019 0.9976
0.0161 0.0171 1
0.0171 0.0171 0.9966
0.0156 0.0166 0.9951
0.0151 0.0161 0.999
0.0146 0.0171 0.9976
0.0161 0.0166 0.9971
0.0156 0.0181 0.998
0.0151 0.0166 0.999
0.0156 0.0176 1.001
0.0166 0.0181 0.998
0.0176 0.019 0.998
0.0156 0.0181 1
0.0171 0.0181 0.998
0.0156 0.0156 0.9985
0.0083 0.0117 0.9971
0.0142 0.0049 0.998
0.0034 0.0093 0.9971
0.0132 0.0059 1.0005
0.0103 -0.0249 0.9976
-0.0527 -0.0122 0.9956
0.0122 0.0103 1.0059
0.0117 0.063 0.9961
-0.0151 0.0273 1.0005
0 -0.0015 1.0034
0.041 0.0718 1.0127
0.0342 0.0596 0.9995
-0.0117 0.0117 1.0264
0.0322 -0.0249 0.9922
0.0039 -0.0146 1.0005
0.001 0.0015 0.9976
-0.0073 0.021 0.9937
0.0068 0.083 1.0029
0.0161 0.0146 0.9976
0.0122 0.0176 0.9971
0.0088 0.0132 0.9985
0.0049 0.0068 0.999
-0.002 0.0024 0.9985
0.002 0.0029 0.9995
0 0 0.9961
0.001 0.0024 0.999
0 0.0005 0.9985
0 0.001 0.998
0 0.001 0.9971
0 0.0005 0.9995
-0.0005 0.002 1
-0.001 0.002 0.9971
0 0 0.999
-0.0005 0.0024 0.9995
0.0005 0.0083 0.9971
-0.0005 0.002 0.9956
0 0 0.999
-0.001 0 0.998
0 0.0034 0.9976
-0.0005 0.0034 0.9995
0 0.0015 0.9995
-0.0005 0 0.9985
0 0.0005 0.9995
0 0.001 0.9985
-0.001 0.0015 1.001
-0.001 0.0005 0.999
-0.0005 0.0005 1
0 -0.001 0.9995
-0.001 0.0029 0.9976
-0.0005 0 1.0015
-0.0005 0.0015 0.9966
0 0 0.9966
0.0005 0 1];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
g=[-0.5493 0.6714 -0.9155 %%%gx,gy,gz来自于陀螺仪
-0.3662 0.5493 -0.7935
-4.9438 5.0049 1.8921
0 0.061 -0.1221
0 0 -0.7935
0.1221 0.061 -0.7935
0 0.061 -0.8545
0 0 -0.7935
0.1221 0 -0.7935
0.1831 0 0.6104
0.1221 0 1.0376
-0.1221 0.061 2.8687
-0.3052 0.3662 4.8828
-0.3662 0.1831 6.958
-0.1831 0.1831 5.7373
0.3052 -2.1973 41.0767
0.5493 -2.1973 45.5322
3.54 -3.6621 48.0957
11.3525 -3.7231 68.1152
-0.2441 1.5259 55.481
-0.1831 2.3193 70.1294
3.7842 -2.9907 58.8989
9.2163 -6.7749 49.7437
22.522 -11.2305 37.7197
1.709 1.4038 28.3813
-32.3486 18.6768 9.8267
-0.1221 0.6714 21.4233
-0.2441 1.2817 20.9961
1.3428 -1.2207 32.959
5.8594 -5.7983 63.1714
2.3804 0.3052 54.7485
-8.728 9.8267 34.1187
0.4272 -0.6104 47.7905
-0.6714 -0.4272 45.2881
0.061 -0.4883 37.0483
0 0 15.8691
-0.4883 -0.1831 13.4888
0.7935 -0.7935 16.2964
-0.061 0.3052 -0.8545
1.1597 -1.0376 -1.0376
2.0142 -1.8921 -1.0376
1.4648 -1.1597 -1.0376
0.7324 -0.5493 -0.9155
0.8545 -0.6714 -0.7324
0.6104 -0.4883 -0.9766
0.9155 -0.7935 -0.7324
-0.8545 0.8545 -0.9766
0.6104 -0.6714 -0.9766
0 0.1221 -0.9766
0.1221 -0.1221 -0.4272
0.2441 -0.2441 -0.9155
-0.2441 0.2441 -0.6714
0 0 -0.7324
-0.061 0.1221 -0.9766
0.4883 -0.4883 -0.9155
0 0 -0.8545
0.6104 -0.5493 -0.7935
0.4272 -0.3662 -0.4883
-0.1221 0.1221 -0.6104
-0.3052 0.2441 -0.6104
0 0 -0.6714
0.1221 -0.1831 -0.9155
-0.1831 0.1831 -0.9766
-0.061 0.1221 -0.6104
0.3662 -0.3052 -0.5493
0.3662 -0.3662 -0.7935
0.1831 -0.1221 -0.8545
0 0 -0.7935
0 0.1221 -0.8545
-0.061 0 -0.9155
0.2441 -0.1831 -0.6714
0.061 -0.061 -0.7324
-0.7935 0.9155 -1.0376
-2.5635 2.6855 -6.0425
-2.0752 2.5024 -31.0669
-1.77 2.5635 -59.8755
0.061 -0.061 -69.2749
-0.3052 1.2817 -97.3511
-0.2441 0.6714 -77.3315
1.8311 -0.4272 -99.6094
0 -1.0376 -84.6558
0.3052 -0.6104 -80.3833
-0.7935 -0.1831 -60.4858
31.9824 -13.9771 -4.9438
0.7935 -2.1362 -10.376
-47.3022 35.3394 -34.2407
-3.1128 1.2817 -18.9819
0.1831 0.2441 -0.4883
0.4272 -0.3052 10.9863
5.7983 -5.7983 -13.7939
2.4414 -0.7935 -24.5972
-2.5635 2.5635 6.2866
-1.4648 2.6245 -4.5776
-2.8687 2.9907 -5.9814
-1.6479 1.8921 -0.1221
-0.061 0.3052 0.9766
-0.061 0.4272 -0.9155
-0.9155 0.9766 -0.5493
0.3662 0 -0.6104
0.3662 0 -0.4883
0.2441 0.061 -0.4272
0.3662 0 -0.7935
0.4272 0 -0.6714
0.3662 0 -0.6714
0.3052 0.1221 -0.5493
0.2441 0 -0.7324
0.3662 0.061 -0.6104
0.4272 -0.061 -0.6104
0.2441 0 -0.6104
0.3662 0.061 -0.5493
0.4272 0 -0.6714
0.3662 -0.061 -0.6104
0.1831 0.1221 -0.5493
0.3662 0 -0.6714
0.3662 0 -0.6104
0.3662 0 -0.6714
0.3662 0 -0.6104
0.2441 0 -0.5493
0.3052 0 -0.6104
0.3662 0 -0.6714
0.3662 0 -0.6714
0.3662 0 -0.6714
0.3662 0.061 -0.6714
0.3662 0 -0.6104
0.2441 0.1221 -0.6104
0.3662 0 -0.5493
];
k=g;
h=[39 193 -293 %%%磁强计测量数值
38 198 -300
39 196 -298
39 193 -293
40 195 -292
40 199 -291
40 201 -291
42 200 -292
39 203 -288
41 201 -289
41 199 -289
42 197 -293
40 197 -292
43 199 -294
47 200 -292
48 197 -285
51 193 -290
51 184 -293
54 174 -299
63 165 -307
65 161 -303
69 158 -297
72 155 -292
75 148 -296
77 148 -290
79 141 -295
75 133 -300
77 122 -313
74 114 -317
72 112 -320
77 106 -324
76 106 -320
77 98 -324
79 95 -327
80 91 -331
83 82 -336
81 83 -335
80 84 -337
81 85 -338
77 83 -335
81 78 -333
81 82 -332
78 83 -333
78 80 -332
76 84 -334
78 82 -336
77 82 -336
77 83 -332
75 85 -333
75 85 -330
77 83 -333
77 86 -329
76 86 -334
79 83 -329
78 85 -332
78 83 -332
75 85 -330
76 86 -329
79 87 -327
76 85 -328
75 89 -334
78 86 -330
75 86 -332
77 86 -330
79 86 -333
80 87 -329
76 88 -333
75 87 -332
77 86 -330
80 86 -329
77 88 -330
80 88 -328
80 85 -327
77 85 -329
77 85 -331
80 89 -329
79 91 -328
78 100 -327
76 108 -322
73 113 -323
66 123 -319
54 133 -315
44 145 -304
38 153 -302
37 175 -277
41 200 -260
39 203 -254
38 199 -258
34 209 -253
34 201 -254
27 194 -256
24 193 -258
22 190 -255
21 192 -258
21 191 -261
21 192 -263
24 197 -257
26 200 -252
27 204 -252
26 205 -249
28 205 -248
26 207 -249
26 207 -249
26 205 -251
26 205 -253
28 206 -249
29 207 -245
28 212 -246
30 211 -246
31 207 -245
32 210 -247
31 208 -245
27 209 -250
30 206 -248
29 206 -248
29 207 -244
32 208 -247
28 211 -250
29 204 -245
27 205 -249
27 206 -249
30 205 -249
28 204 -250
29 207 -248
27 201 -249
33 205 -250
];
dt=0.099;%时间
N=126;
q=zeros(N,4);
for i=1:N
q(i,1)=1;
end
for i=1:126 %龙格库塔公式更新
q(i,1)=q(i,1)+0.5*(-q(i,2)*g(i,1)-q(i,3)*g(i,2)-q(i,4)*g(i,3))*dt;
q(i,2)=q(i,2)+0.5*(q(i,1)*g(i,1)+q(i,3)*g(i,3)-q(i,4)*g(i,2))*dt;
q(i,3)=q(i,3)+0.5*(q(i,1)*g(i,2)-q(i,2)*g(i,3)+q(i,4)*g(i,1))*dt;
q(i,4)=q(i,4)+0.5*(q(i,1)*g(i,3)+q(i,2)*g(i,2)-q(i,3)*g(i,1))*dt;
k1=q(i,1);
k2=q(i,2);
k3=q(i,3);
k4=q(i,4);
qy=sqrt(k1*k1+k2*k2+k3*k3+k4*k4); %%%四元数归一化
q(i,1)=k1/qy;
q(i,2)=k2/qy;
q(i,3)=k3/qy;
q(i,4)=k4/qy;
end
%%%%%%%%%%%%%%%%%%角速度更新%%%%%%%%%%%%%%%%%%%%%%%
exint=0;
eyint=0;
ezint=0;
Kp=0.1;
Ki=0.008;
for i=1:N
norm=sqrt(a(i,1)*a(i,1)+a(i,2)*a(i,2)+a(i,3)*a(i,3));%加速度计归一化
a(i,1)=a(i,1)/norm;
a(i,2)=a(i,2)/norm;
a(i,3)=a(i,3)/norm;
normh=sqrt(h(i,1)*h(i,1)+h(i,2)*h(i,2)+h(i,3)*h(i,3));%%磁强计归一化
h(i,1)=h(i,1)/normh;
h(i,2)=h(i,2)/normh;
h(i,3)=h(i,3)/normh;
vx = 2*(q(i,2)*q(i,4)-q(i,1)*q(i,3)); %%%%四元数计算加速度值
vy=2*(q(i,3)*q(i,4)+q(i,1)*q(i,2));
vz=q(i,1)*q(i,1)-q(i,2)*q(i,2)-q(i,3)*q(i,3)+q(i,4)*q(i,4);
wx=2*h(i,1)*(0.5-q(i,3)*q(i,3)-q(i,4)*q(i,4))+2*h(i,3)*(q(i,2)*q(i,3));%%四元数计算地磁值
wy=2*h(i,1)*(q(i,2)*q(i,3)-q(i,1)*q(i,4))+2*h(i,3)*(q(i,1)*q(i,2));
wz=2*h(i,1)*(q(i,1)*q(i,3)+q(i,2)*q(i,4))+2*h(i,3)*(0.5-q(i,2)*q(i,2)-q(i,3)*q(i,3));
ex=(a(i,2)*vz-a(i,3)*vy)+(h(i,2)*wz-h(i,3)*wy); %%%加计磁计误差计算
ey=(a(i,3)*vx-a(i,3)*vz)+(h(i,3)*wz-h(i,1)*wz);
ez=(a(i,1)*vy-a(i,2)*vx)+(h(i,1)*wy-h(i,2)*wx);
exint=exint+ex*Ki*dt;
eyint=eyint+ey*Ki*dt;
ezint=ezint+ez*Ki*dt;
g(i,1)=g(i,1)+Kp*ex+exint;
g(i,2)=g(i,2)+Kp*ex+eyint;
g(i,3)=g(i,3)+Kp*ex+ezint;
% ex=a(i,2)*vz-a(i,3)*vy; %%加速度计融合算法暂时用不到
% ey=a(i,3)*vx-a(i,1)*vz;
% ez=a(i,1)*vy-a(i,2)*vx;%%%%求误差
% Exint=Exint+ex*Ki; %%加速度计融合算法暂时用不到
% Eyint=Eyint+ey*Ki;
% Ezint=Ezint+ez*Ki; %%%求积分
%%%误差补偿到陀螺仪角速度
% g(i,1)=g(i,1)+Kp*ex+Exint; %%%加计融合暂时用不到
% g(i,2)=g(i,2)+Kp*ey+Eyint;
% g(i,3)=g(i,3)+Kp*ez+Ezint;
end
%%disp(g)
figure
hold on;box on;
plot(k(:,1),'-r');
plot(g(:,1),'-k');
legend('测量值','最优估计值');
xlabel('时间');
ylabel('值');
标签：0.3662,0.6714,0.061,加速度计,MATLAB,0.1221,mahony,0.0005,0.6104 来源： https://blog.csdn.net/weixin_47506154/article/details/120607997