卡爾曼濾波
濾波的方法有很多種，針對不同的情況選用的最優濾波方法也是不同的。卡爾曼濾波的特點就是采用遞歸方法解決線性濾波問題，只需要知道當前的測量值和上一時刻的最優值，就能對此刻進行最優值計算，計算量小，不需要大量儲存空間，適合性能不太強的單片機處理。二階卡爾曼濾波更加可靠，但計算量較大，通常使用的是一階。
現在網絡上卡爾曼濾波的資料有很多，大多是一位大佬生產，說不清的碼農搬磚，想要真正理解卡爾曼濾波的道理，還需靜下心來從數學理論慢慢體會。分享一個正在研究的文檔，比較深入的那種。
https://pan.baidu.com/s/11NCpqgciVc1KIx4H66upAA
既然要真正搞懂很難，那我就反其道而行之，邊用邊學吧。
卡爾曼濾波由五個基本方程式組成，列出了方程式，濾波也就完成了。
深入一點的方程式從那個文章中拷進來的，看一下就好，我們分析淺顯點的。
先把初始化賦值說一下，免得直接懵逼。在飛控中初始化時常用賦值如下：P(k-1)=0.02 Kg(k)=P(k)=X(k)=0 Q=0.001 R=0.5
1.預測狀態方程
X(k)=A X(k-1)+B U(k) -----深
X(k)= X(k-1) ----------淺
簡單的說就是把上一時刻的卡爾曼濾波的最優值 X(k-1) 乘以一個系數賦給當前預測值，這個系數常為1。U(k) 為控制增益，一般為0。
2.預測協方差方程
P(k)=A P(k-1) A’+Q -----深
P(k)= P(k-1) +Q ----------淺
P(k)是此刻系統協方差，P(k-1)是k-1時刻系統協方差，Q是系統過程噪聲的協方差。在飛控中，這些值的初始化值是根據經驗和計算得來的，最后統一賦一下。
3.卡爾曼增益方程
Kg(k)= P(k) H’ / (H P(k) H’ + R) -------深
Kg(k)=P(k)/（P(k)+R） ----------淺
Kg(k)叫做卡爾曼增益，P(k)是剛算出來的此刻系統協方差，R是對象測量噪聲的協方差，初始化時直接給出，調試時可以根據需要調整大小，R大點時波形幅度減小，效果滯后，小點時相反，根據效果調整就好。
4.更新最優值方程
X(k)= X(k)+Kg(k) (Z(k)-H X(k)) -------深
X(k)= X(k)+Kg(k) (Z(k)- X(k)) ----------淺
X(k)就此刻的最優值，可以直接用的那種，X(k)是上一時刻的最優值，Z(k)是此刻對象的測量值，換句話說就是卡爾曼濾波函數的輸入值，要是測量值都沒了，還濾毛線啊，是吧。
5.更新協防差方程
P(k-1)= （I-Kg(k) H ）P(k) -------深
P(k-1)= （I-Kg(k) ）P(k) ----------淺
此刻的系統協方差一經計算變成舊的協方差，供下次使用。
這就結束了，被濾波的值是Z(k)，濾波的結果是X(k)。