四軸mpu6050姿態角卡爾曼濾波代碼分析

　　卡爾曼濾波（Kalman filtering）一種利用線性系統狀態方程，通過系統輸入輸出觀測數據，對系統狀態進行最優估計的算法。由于觀測數據中包括系統中的噪聲和干擾的影響，所以最優估計也可看作是濾波過程。

　　斯坦利·施密特（Stanley Schmidt）首次實現了卡爾曼濾波器?？柭?a href="http://www.qd573.com/tags/nas/" target="_blank">NASA埃姆斯研究中心訪問時，發現他的方法對于解決阿波羅計劃的軌道預測很有用，后來阿波羅飛船的導航電腦使用了這種濾波器。 關于這種濾波器的論文由Swerling （1958）， Kalman （1960）與 Kalman and Bucy （1961）發表。

　　數據濾波是去除噪聲還原真實數據的一種數據處理技術， Kalman濾波在測量方差已知的情況下能夠從一系列存在測量噪聲的數據中，估計動態系統的狀態。 由于， 它便于計算機編程實現， 并能夠對現場采集的數據進行實時的更新和處理， Kalman濾波是目前應用最為廣泛的濾波方法， 在通信， 導航， 制導與控制等多領域得到了較好的應用。

　　性質

　?、倏柭鼮V波是一個算法，它適用于線性、離散和有限維系統。每一個有外部變量的自回歸移動平均系統（ARMAX）或可用有理傳遞函數表示的系統都可以轉換成用狀態空間表示的系統，從而能用卡爾曼濾波進行計算。

　?、谌魏我唤M觀測數據都無助于消除x（t）的確定性。增益K（t）也同樣地與觀測數據無關。

　?、郛斢^測數據和狀態聯合服從高斯分布時用卡爾曼遞歸公式計算得到的是高斯隨機變量的條件均值和條件方差，從而卡爾曼濾波公式給出了計算狀態的條件概率密度的更新過程線性最小方差估計，也就是最小方差估計。

　　卡爾曼濾波理解及代碼分析

　　鑒于網上的代碼以及分析的各種錯誤，所以寫一個正確的詳細的分析。

　　過程方程以及量測方程

　　X（K）=AX（K-1）+BU（K-1）+W（K-1）

　　Z（K）=HX（K）+V（K）

　　說明，下面帶T的表示轉置。

　　卡爾曼濾波的黃金五條公式

　　X（k|k-1）=AX（k-1|k-1）+BU（k）………。先驗估計

　　P（k|k-1）=A P（k-1| k-1） AT+Q………。誤差協方差

　　Kg（k）= P（k|k-1） HT / （H P（k|k-1） HT + R）………。計算卡爾曼增益

　　X（k|k）= X（k|k-1） + Kg（k）（Z（k） - H X（k|k-1））………。修正估計

　　P（k|k）=（ I-Kg（k） H） P（k|k-1）………。更新誤差協方差

　　下面的程序主要針對MPU6050的姿態角的濾波。

　　float Q_angle=0.001; //陀螺儀噪聲的協方差

　　float Q_gyro=0.003; //陀螺儀漂移噪聲的協方差

　　float R_angle=0.5; // 加速度計的協方差

　　float dt=0.005;

　　char C_0 = 1;

　　float Q_bias=0， Angle_err=0; //Q_bias為陀螺儀漂移

　　float PCt_0=0， PCt_1=0， E=0;

　　float K_0=0， K_1=0， t_0=0， t_1=0;

　　float Pdot［4］ ={0，0，0，0};

　　float PP［2］［2］ = { { 1， 0 }，{ 0， 1 } };12345678910

　　首先建立的是過程方程，這里的狀態變量是angle以及Q_bias，角度以及陀螺儀的漂移。

　　那么已經建立了這里的預測方程，沒有加上噪聲。

　　void Kalman_Filter（float Gyro，float Accel）

　　{ //Gyro陀螺儀的測量值，Accel加速度計的角度計算值

　　Angle+=（Gyro - Q_bias） * dt;

　　//角度測量模型方程

　　//就漂移來說認為每次都是相同的Q_bias=Q_bias；

　　//由此得到矩陣1234567

　　上面的代碼就對應著預測方程。對應著卡爾曼濾波的五個公式的第一條：X（k|k-1）=AX（k-1|k-1）+BU（k）

　　這里再分析第二條公式，P（k|k-1）=A P（k-1| k-1） AT+Q?？梢栽谥翱闯?，A=［1，-dt;0，1］。而Q的定義如下：

　　因為角度噪聲和陀螺儀的角速度的漂移噪聲相互獨立，所以為一個對角矩陣。然后，Q_angle，Q_gyro再程序開頭已經給出。所以設P=［a，b;c，d］

　　的出一個更新的式子，

　?。踑cbd］=［10?dt1］［acbd］［1?dt01］+［Qangle00Qgyro］

　　最后的到的更新的方法

　?。踑cbd］=［a?c?dt?b?dt+d?dt?dtc?d?dtb?d?dtd］+［Qangle00Qgyro］

　　所以看代碼，可以看出寫的極為的不合理，但是都是這樣寫的，先看一看。

　　Pdot［0］=Q_angle - PP［0］［1］ - PP［1］［0］;

　　Pdot［1］ = -PP［1］［1］;

　　Pdot［2］ = -PP［1］［1］;

　　Pdot［3］ = Q_gyro;

　　PP［0］［0］ += Pdot［0］ * dt;

　　PP［0］［1］ += Pdot［1］ * dt;

　　PP［1］［0］ += Pdot［2］ * dt;

　　PP［1］［1］ += Pdot［3］ * dt;123456789

　　對照上面的公式，還是可以看出來，PP就是［a，b;c，d］的，但是注意，Pdot只是矩陣運算的中間值，但是不知為什么要叫成Pdot，誤人子弟。而且最大的錯誤在于這樣寫，Q乘以了一個dt，但是最后并不會怎么影響，因為Q也是初始給的一個值而已，但是這樣寫還是有問題的，還是按照推導來寫比較好。

　　再是第三個公式來計算卡爾曼增益，Kg（k）= P（k|k-1） HT / （H P（k|k-1） HT + R），所以這里要做的就是再建立一個量測方程，這里測量的值是加速度計算出來的角度值，所以

　　Accelangle=［10］［angleQbias］+R

　　所以H=［1 0］，卡爾曼增益就是一個二維向量［k0，k1］T。

　　直接帶入計算第三個公式。

　　PCt_0 = C_0 * PP［0］［0］;//矩陣乘法的中間變量

　　PCt_1 = C_0 * PP［1］［0］;//C_0=1

　　//分母

　　E = R_angle + C_0 * PCt_0;

　　//增益值

　　K_0 = PCt_0 / E;

　　K_1 = PCt_1 / E; 1234567

　　基本還算比較清楚，但是命名的話真的不忍心吐槽。

　　再看第四個公式，X（k|k）= X（k|k-1） + Kg（k）（Z（k） - H X（k|k-1））。

　　Angle_err = Accel - Angle; //Accel是加速度計的值，算出來的角度的測量值。

　　Angle += K_0 * Angle_err; //對狀態的卡爾曼估計。

　　Q_bias += K_1 * Angle_err;

　　Gyro_x = Gyro - Q_bias; //計算得角速度值，這里由于每次對Q_bias更新，就更準確，比初始矯正后不管肯定要好很多。123456

　　第五個公式對PP進行更新，P（k|k）=（ I-Kg（k） H） P（k|k-1）；

　　t_0 = PCt_0; //矩陣計算中間變量

　　t_1 = C_0 * PP［0］［1］;

　　PP［0］［0］ -= K_0 * t_0;

　　PP［0］［1］ -= K_0 * t_1;

　　PP［1］［0］ -= K_1 * t_0;

　　PP［1］［1］ -= K_1 * t_1;

　　}12345678

　　已經不忍心吐槽他的命名了。到這里基本分析完畢，至于卡爾曼濾波的證明推導，可以參考其他，這里只是分析代碼，但是網上基本都是這樣寫的，有分析的，但是要么就是直接甩代碼，要么就是分析的很多錯誤，太亂，決定寫一個完整的正確的分析mpu6050卡爾曼濾波的程序。