程序源码分享-MATLAB仿真数据融合卡尔曼滤波算法

本文将用MATLAB模拟加速度计得到的加速度和陀螺仪的到的角速度进行卡尔曼滤波数据融合，得到更为精确的输出。

一、用卡尔曼滤波的数据融合算法的原因

加速度求解的数学原理是三角函数关系。通过将测得的加速度矢量分解在三轴上，再根据反三角函数计算就可得到角度。在物体处于静止状态下时，此种方法的精度能够得到保证，但是当物体处在运动状态下，这种方法带有一定的不准确性：加速度测量的结果除有重力加速度外还有物体运动的加速度，这样就导致通过加速度的反三角函数计算得到的角度值不准确。另外，加速度测量的值本身精度较低，测到的结果波动较大，解算结果的精度也较差。
陀螺仪求解的数学原理是将角速度积分获得角度。通过陀螺仪可以直接测量传感器的角速度，通过数值积分即可获得角度。积分就会有累积误差，再加上陀螺仪长时间使用会有零点漂移，角度就会随着时间的累积导致误差越来越大。
通过以上介绍可以明显看出两种方法各有利弊，但是都不准确。所以本设计采用软件算法集两者之长避两者之短，保证输出精度的同时，避免长时间造成累积误差。陀螺仪的精度能直接影响解算和输出的准确性，所以解决陀螺仪问题的重中之重就是设法找到一个既能提高陀螺仪精度，又能减小陀螺仪误差的方法。对于三轴陀螺仪来说,其测量结果的精度与构成三轴陀螺仪的各单轴陀螺仪的零偏误差、刻度系数误差、随机漂移误差以及各单轴陀螺仪敏感轴之间的不正交安装误差相关。陀螺仪输出的数据不仅具有零点漂移而且还带有噪声，需要将数据校准后再滤波才能得到较为准确的输出。本系统选用卡尔曼滤波算法对加速度计和陀螺仪的输出进行数据融合，通过迭代的方法，利用对系统状态的估计结合观测数据对系统状态进行估计并更新，从而使输出数据接近真实值。

二、卡尔曼滤波的原理

卡尔曼滤波算法可以根据加速度和陀螺仪数据的协方差，根据统计学算出置信系数并且实时调节数据融合的比例系数，综合加速度求解和陀螺仪求解的优点，输出精确的三轴姿态角。具体公式见笔者的文章基于单片机的云台姿态测量系统设计，文中对其公式进行了较为详细的介绍。

三、程序设计思路

为了更好地理解卡尔曼滤波的原理、更直观地看出卡尔曼滤波的优点，我们用MATLAB进行卡尔曼滤波的仿真。仿真思路如下：
随机生成加速度计和陀螺仪的过程噪声各500个，并分别计算噪声的协方差，随机生成500个测量噪声，并计算协方差。计算加速度计和陀螺仪的真实状态，再由真实状态得到测量数据。以卡尔曼滤波的五个公式为基础，开始对加速度计和陀螺仪的输出进行预测更新过程以及数据融合。最后一步是做出仿真结果图，其中卡尔曼滤波的结果由红线表示，传感器的测量值由绿线表示，真实状态由蓝线来表示，仿真结果如图所示。从图可以清楚直观地看出卡尔曼滤波的作用效果，由观测值得到的卡尔曼滤波结果和真实状态相比符合程度较高。

四、程序分享

clc
clear
close all
N=500;
j=randn(1,N); 
j(1)=0;
P=var(j); 
t=randn(1,N);
t(1)=0;
Q=var(t); 
v=randn(1,N);
R=var(v);
x_true(1)=0;
A=1;
for k=2:N
    x_true(k)=A*x_true(k-1)+j(k-1);  
end
y_true(1)=0;
A=1;
for k=2:N
    y_true(k)=A*y_true(k-1)+t(k-1);  
end
for k=2:N
   angle_true(k)=A*sqrt(x_true(k)^2+y_true(k)^2);  
H=0.2;
z1=H*x_true+v;
z2=H*y_true+v;
z=sqrt(z1.^2+z2.^2);
x_update(1)=x_true(1);
P1_update(1)=0; 
for t=2:N
    x_predict(t) = A*x_update(t-1); 
    P1_predict(t)=A*P1_update(t-1)*A'+P;    
    K1(t)=H*P1_predict(t) / (H*P1_predict(t)*H'+R);
    x_update(t)=x_predict(t)  +  K1(t) * (z1(t)-H*x_predict(t));
    P1_update(t)=P1_predict(t) - H*K1(t)*P1_predict(t);
end
y_update(1)=y_true(1);
P2_update(1)=0; 
for t=2:N
    y_predict(t) = A*y_update(t-1);    
    P2_predict(t)=A*P2_update(t-1)*A'+Q; 
    K2(t)=H*P2_predict(t) / (H*P2_predict(t)*H'+R);
    y_update(t)=y_predict(t)  +  K2(t) * (z2(t)-H*y_predict(t));
    P2_update(t)=P2_predict(t) - H*K2(t)*P2_predict(t);
end
for t=2:N
K=2.1;
angle(t) = K*sqrt((K1(t)/(K1(t)+K2(t)) *x_update(t))^2 + ((1-K1(t)/(K1(t)+K2(t))) *y_update(t))^2) ;
end
t=1:N;
plot(t,angle,'r',t,z,'g',t,angle_true,'b');

原本是有注释的，不知道什么原因我给删了，有不清楚的地方欢迎各位批评指正。