mahony算法，加速度计磁强计陀螺仪数据融合MATLAB程序

本文主要是介绍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('值');
 

这篇关于mahony算法，加速度计磁强计陀螺仪数据融合MATLAB程序的文章就介绍到这儿，希望我们推荐的文章对大家有所帮助，也希望大家多多支持为之网！