MATLAB-卡尔曼滤波简单运用示例

1、角度和弧度之间的转换公式？ 
设角度为 angle，弧度为 radian 
radian = angle * pi / 180; 
angle = radian * 180 / pi; 
所以在matlab中经常设置一个参数，用于角度与弧度之间的转换：deg_rad=0.01745329252e0;

2、注意下面角度Angint的表示方法： 
Angint=[0,10,0]*deg_rad； 
则：Angint(0) = 0;Angint(1) = 0.0175;Angint(2) = 0; 
这种表示方法可以在四元数中应用： 
例如: 
q=[cos(Angint(3)/2)*sin(Angint(2)/2)*cos(Angint(1)/2)+sin(Angint(3)/2)*cos(Angint(2)/2)*sin(Angint(1)/2) 
cos(Angint(3)/2)*cos(Angint(2)/2)*sin(Angint(1)/2)-sin(Angint(3)/2)*sin(Angint(2)/2)*cos(Angint(1)/2) 
-sin(Angint(3)/2)*cos(Angint(2)/2)*cos(Angint(1)/2)+cos(Angint(3)/2)*sin(Angint(2)/2)*sin(Angint(1)/2) 
cos(Angint(3)/2)*cos(Angint(2)/2)*cos(Angint(1)/2)+sin(Angint(3)/2)*sin(Angint(2)/2)*sin(Angint(1)/2)]; 
可以用q(0)、q(1)、q(2)、q(3)来代入公式计算三轴姿态角。

3、在滤波的过程中，要明确滤波时间和采样频率；

4、IMU数据仿真分析： 
（1）先模拟加速度计和陀螺仪的真实输出： 
[ Angle,Wibb,Fb ] = imu_true_out( tt,ynong,T );%tt=tt+TT;TT=1/f为时间间隔 
注意：加速度计的输出要进行坐标转换： ao=Cnb*([0,0,g]’， 
其中：Cnb 

                

如果你要在加速度计的输出上添加一个随机干扰(可模拟非重力加速度干扰)，可以使用函数awgn();  %Add white Gaussian noise to a signal. 
ao=Cnb*([0,0,g]’+[0,awgn(0,ynong),0]’);   %如果在指点的时间内添加这种干扰，可以加一个if函数 
（2）模拟加速度计和陀螺仪的误差： 
[Gyro_b,Gyro_r,Gyro_wg,Acc_r]=imu_err_random(tt,TT,Gyro_b,Gyro_r,Gyro_wg,Acc_r);

function [Gyro_b,Gyro_r,Gyro_wg,Acc_r]=imu_err_random(t,T,Gyro_b,Gyro_r,Gyro_wg,Acc_r)

g=9.7803698;         %重力加速度    （单位：米/秒/秒）
Wie=7.292115147e-5;  %地球自转角速度（单位：弧度/秒）
deg_rad=0.01745329252e0;% Transfer from angle degree to rad

Da_bias=[0.001; 0.001; 0.001]*g;  %加速度零偏（应与非随机性误差中的加速度零偏保持一致）

Ta=1800.0; %加速度一阶马尔可夫过程相关时间
Tg=3600.0; %陀螺一阶马尔可夫过程相关时间

%%%%%%%%%%%%%%%%%%随机性误差%%%%%%%%%%%%%%%
if( t==0 )
  Acc_r=Da_bias.*randn(3,1); %加速度一阶马尔可夫过程1.0e-4g

  Gyro_b=0.01*deg_rad/3600.0*randn(3,1); %随机常数0.1(deg/h)
  Gyro_r=0.01*deg_rad/3600.0*randn(3,1); %陀螺一阶马尔可夫过程0.1(deg/h)
  Gyro_wg=0.01*deg_rad/3600.0*randn(3,1);%陀螺白噪声0.1(deg/h)
else  
  Acc_wa=sqrt(2*T/Ta)*Da_bias.*randn(3,1);%加速度一阶马尔可夫过程白噪声
  Acc_r=exp(-1.0*T/Ta)*Acc_r; %加速度一阶马尔可夫过程

  Gyro_wr=0.01*sqrt(2*T/Tg)*deg_rad/3600.0*randn(3,1);%陀螺一阶马尔可夫过程白噪声0.1(deg/h)
  Gyro_r=exp(-1.0*T/Tg)*Gyro_r+Gyro_wr;%陀螺一阶马尔可夫过程0.1(deg/h)
  Gyro_wg=0.01*deg_rad/3600.0*randn(3,1);%陀螺白噪声0.1(deg/h)
end 

然后再在while(1)中将真实值+误差值按时间序列存储在数组中以备用，如下：

while tt<=T;
    [ Angle,Wibb,Fb ] = imu_true_out( tt,ynong,T );
    [Gyro_b,Gyro_r,Gyro_wg,Acc_r]=imu_err_random(tt,TT,Gyro_b,Gyro_r,Gyro_wg,Acc_r);
    Ture_Angle(:,kk)=Angle/deg_rad;%模拟输出的三轴姿态角
    gyro(:,kk)=Wibb+Gyro_b+Gyro_r+Gyro_wg;%模拟输出的陀螺仪输出
    acc(:,kk)=Fb+Acc_r;%模拟输出的加速度计输出
    tt=tt+TT;
    kk=kk+1;%这里TT=1/f=1/100为采样时间间隔，f为采样频率，T为采样时间，文中设置为30s
end

5、函数randn的意思： 
R = randn(3,1)；%产生3行1列的随机R矩阵，R矩阵满足方差为1，均值为0；注意这里不是说这三个数的方差为1，均值为0，而是每次运行randn时，当运行的量足够大时，所有的R(0)的方差为1，均值为0，R(1)、R(2)同理。 
例如1. randn(1) ；%生成一个随机数，要想满足方差为1，均值为0，也必须运行足够多的次数 
例如2. x = .6 + sqrt(0.1) * randn(1)；%产生均值为0.6，方差为0.1的一个5*5的随机数；sqrt(0.1)对0.1进行开方，直接写0.1那这里就是表示标准差了

randi([m,n],a,b)    %[m,n] 的  a*b 随机数

6、axis()函数的用法：
axis([xmin xmax ymin ymax])；%定义x轴和y轴之间的间距

7、set()函数的用法：
plot(t,acc(2,:),'linewidth',1.3); 
set(gcf,'Position',[100 100 400 300]);  %这里的100是指生成的图片距离左下角的距离，400和300分别表示长和宽 
axis([0 T -4.9 2.0]); 
set(gca, 'YTick', [-4.9 -2.4 -0.1 2.0]) 
set(gca,'fontname','宋体','fontsize',10); %用于改变坐标轴坐标大小，10代表坐标大小； 
xlabel('时间/s','fontname','