自适应四元数kalman滤波matlab学习笔记（一）

1、Matlab中的clc,clear,close命令区别？
clc：清除命令窗口的内容，对工作环境中的全部变量无任何影响；
clear：清除工作空间的所有变量；
clear all：清除工作空间的所有变量，函数，和MEX文件；
close：关闭当前的Figure窗口；
close all:关闭所有的Figure窗口。%如果不关闭，会在原来的figure上重复画图。

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

3、单位矩阵的表示方法？
学会在comment window页面print相关函数：

4、注意下面角度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)来代入公式计算三轴姿态角。

5、陀螺仪和加速度计的误差有哪些？
陀螺仪：随机误差+白噪声+一阶马尔可夫过程
加速度计：白噪声+一阶马尔可夫过程
如图：

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

7、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

8、函数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那这里就是表示标准差了