自適應四元數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那這裏就是表示標準差了