基於卡爾曼的濾波演算法是怎樣將加速度計的數據融入姿態數據的？

01-08

本人大二學生，在做四軸飛行器的飛控，目前在做姿態解算。本人剛上完自動控制原理，目前正在自學現代控制理論，以求對卡爾曼濾波有更好的理解，但是好像不夠，在想是不是要看看信號檢測與估計之類的書。
如果有這方面的書，希望能推薦下，謝謝！
看卡爾曼在姿態解算方面的應用的時候，始終沒搞明白：
1. 卡爾曼濾波是一種最優線性估計，它是根據前段時間的量測值{Zk}估計狀態量Xk+1嗎？
2. 如果用在姿態解算中，感覺系統矩陣矩陣A是與陀螺儀輸出w有關的，那麼這裡的輸入是啥，量測值又是啥？

3. 卡爾曼濾波的最大作用是將三個感測器的數據融合，是怎樣融合的呢
感覺自己問的問題就是壓根不理解卡爾曼濾波演算法，但是希望能得到指點，Thanks

如果是四旋翼，那麼你的系統實際上假定了幾個前提，而這些前提也是你能使用加速度計來測量姿態的原因。

1，你的飛行器在n系下三軸的加速度不大，加減速時間短。

2，你的飛行器大部分時間是保持水平，或說姿態機動幅度小時間短。

在這兩個前提下，你可以用加速度計來測量n系下的重力矢量Gn[0 0 9.8]"在b系下的輸出Gb，而且他們有關係Gb=Cn_b*Gn，Cn_b代表n繫到b系的旋轉矩陣。對磁羅盤而言也是同理。

如果你使用卡爾曼濾波來融合陀螺儀，加計，磁羅盤，實際上你是將陀螺儀的輸出做為系統的增量預測，而姿態的增量方程即為卡爾曼的狀態方程（其實就是四元數微分方程），之後再觀測重力矢量和磁矢量做為測量方程。這樣就得到了融合。

卡爾曼的優勢在於，對於線性系統中的高斯雜訊而言，卡爾曼濾波器是最優而且無偏的。但要達到最優無偏，你必須保證系統線性，而且只有高斯雜訊，沒有其它有色雜訊，意味著你的系統模型必須儘可能準確（與線性衝突），把系統中的各個有色雜訊都在狀態方程中體現出來。同時，你還要儘可能準確地估計狀態與測量雜訊的能量（方差）。

如果以上條件不滿足，卡爾曼並非是最好的選擇。這也是目前絕大多數航模級遙控四旋翼不需要使用卡爾曼的原因——它們的感測器各種有色雜訊太多且難以估計，而互補濾波足夠簡單且性能優良。

首先，要明確卡爾曼濾波器本質是估計器，只是因為觀測量帶有雜訊，通常重新估計後獲得的值，也認為是濾波後的值；

其次，卡爾曼濾波器是需要模型的，也就是你所疑惑的，在姿態解算中，什麼是輸入，什麼是觀測，這個要基於你所設計的模型，每個人用的模型不一樣，自然系統矩陣A、觀測矩陣C都是不一樣的。目前常見的有用四元數做狀態量，加速度計和地磁的輸出作為觀測量，還有px4原生固件的EKF，狀態量如下，具體的演算法細節看源碼；

最後，說白了，如何融合的？是根據你所設計的模型，模型的狀態量最好是你想獲得的信息，比如這邊的角度，觀測量自然是你感測器能獲得的數據，經過演算法的迭代，會估計出你的狀態量，角度自然就獲得了。可以參考一些文章，建立一個姿態解算的模型，用實際的感測器數據，模擬實驗一下。

PX4飛控中利用EKF估計姿態角代碼詳解我的這篇blog應該能回答你的問題

最近正在研究這個，來說一下原理上的東西吧，詳細的推導見維基百科，這裡只說說他們之間的關係。

狀態估計（先驗估計/後驗估計）

實際上在線性系統中可以做的一般包括上面四種估計，這裡我們要注意的是中間兩種。

後驗估計：以所有已獲得的測量值，包括 k=5時刻的測量值為條件計算x_k的期望

先驗估計：以時刻 k 之前所有的測量值但是不包括時刻 k 的測量值為條件計算 x_k 的期望

估計誤差協方差矩陣

對於狀態我們可以根據一些已知的狀態和外部測量值進行估計，那麼如何評價一個估計是否足夠好呢？這裡我們就需要計算估計誤差的協方差矩陣（後面會說用途）：

卡爾曼增益

協方差矩陣中 x_k 為真實狀態，但是我們不知道是什麼，也不需要知道，推導式中這項被替換掉了。將後驗估計誤差的協方差矩陣展開之後，裡面會有一項未知量 K （也就是卡爾曼增益），為了使該協方差矩陣最小，這裡問題就轉換為了一個關於卡爾曼增益的最小二乘問題。求導之後，可以求出卡爾曼增益：

（這裡可以參考卡爾曼濾波器的維基百科詞條，有詳細推導）

其實卡爾曼濾波說白了就是這麼個最小二乘優化問題。

推導式

一般總結卡爾曼濾波就是下面這五個公式。

比如首先我們已知 k=0 時的初始狀態和它的估計誤差協方差矩陣，那麼我們可以直接求出下一個狀態的先驗估計和先驗估計誤差協方差矩陣（F/G/H/Q是系統參數）。根據先驗估計誤差協方差矩陣又可以求出卡爾曼增益。帶入當前時刻的測量值，結合卡爾曼增益和先驗估計可以求出後驗估計和後驗估計誤差的協方差矩陣。到此時刻 k=1 的狀態就求出來了，後面的狀態依次類推。

Q1 為什麼有狀態信息和測量信息還要進行估計？

因為都他么雜訊太大不準確Q2 那麼為什麼我們要計算這麼多的值呢？

首先先驗估計是根據線性系統的系統時間更新方程可以做出的一個估計。在獲得測量值之後，我們相當於對當前的狀態有了更多的信息，因此我們帶入這個信息，做出一個後驗估計，因為計算時擁有的信息更多，所以期望是後驗估計更加準確！

現在網上大部分開源飛控演算法都是採用的Mahony的互補濾波演算法，輸入三軸加速度計、三軸陀螺儀數據，通過四元數更新演算法，再得到歐拉角。

在這個演算法的輸入端，也就是三軸加速度計的值，可以使用卡爾曼非矩陣濾波演算法，xyz每一個軸都單獨用一個卡爾曼濾波器，先把加速度計的雜訊濾掉，再進行互補濾波。當然在這裡用卡爾曼濾波有點大材小用了，還可以使用二階低通濾波，甚至可以使用均值濾波。更多關於加速度濾波的演算法，具體你可以看這篇文章：四軸加速度計濾波。

當然還有一種更先進的，就是直接採用卡爾曼的濾波演算法，矩陣輸入，矩陣輸出，演算法比較複雜，我也不太清楚，學會了我再來補充吧。。。

1. 卡爾曼濾波是一種最優線性估計，它是根據前段時間的量測值{Zk}估計狀態量Xk+1嗎？

可以理解為先用狀態轉移矩陣進行一步估計，再利用量測值量測值去修正一步估計，從而獲得最優估計

2. 如果用在姿態解算中，感覺系統矩陣矩陣A是與陀螺儀輸出w有關的，那麼這裡的輸入是啥，量測值又是啥？

如果你選取的狀態量是四元數，狀態方程：

你的A就是：

3. 卡爾曼濾波的最大作用是將三個感測器的數據融合，是怎樣融合的呢

這個問題太難回答了。。。簡單來說就是通過測量去修正你的估計，測量量越多，修正的就越准，前提是系統雜訊方差陣和測量雜訊方差陣選取的準確。不知道這樣說是不是很明白。。。

卡爾曼離開我們了……

1. 卡爾曼濾波是一種最優線性估計，它是根據前段時間的量測值{Zk}估計狀態量Xk+1嗎？

是的，跟其他人一樣，我也推薦用互補濾波。

2. 如果用在姿態解算中，感覺系統矩陣矩陣A是與陀螺儀輸出w有關的，那麼這裡的輸入是啥，量測值又是啥？

這裡，狀態就是飛行器的角度，角速度。而量測值就是陀螺儀、加速度計的值。其中陀螺儀的輸出值跟角速度有關係，而加速度計的輸出值跟角度有關，a=g*sin(theta)，這個畫個力學分解圖就可以看出來了。

3. 卡爾曼濾波的最大作用是將三個感測器的數據融合，是怎樣融合的呢

三個感測器的輸出是有雜訊的，有些是系統誤差，例如把sin(theta)簡化成theta，有些是隨機誤差，而卡爾曼濾波的重要作用就在於濾掉隨機誤差，根據隨機誤差的方差來做一個「最正確」的估計。

概率論，線性代數，計算機控制

方案有兩種有一種是姿態的融合，一種是四元素的融合。。。。。。

算是補充樓上的回答吧

第一是：陀螺儀和加速度計各自算姿態，再做融合，一般用互補濾波就行了

第二種是：四元數階段就將加計的數據帶入，做四元數計算，網上有一種mahony演算法。

其實卡曼濾波不能直接用在四旋翼姿態估計上，最起碼也得改進一下。

補充：主要是四旋翼用的器件成本太低，並不合適用卡爾曼濾波做姿態估計，位置估計可以做，apm就是這樣的。