卡爾曼最優預測以及最優濾波方程的相關推導--最優濾波篇

上一篇幫大家回憶了相關數學基礎，介紹了相關問題背景以及最優預測方程的推導，本篇將開啓卡爾曼最優濾波方程的推導。同時，在本篇的最後會給出兩個詳細的實際應用的例子幫助大家理解應用卡爾曼最優濾波。

一、問題的提法

對於線性離散系統，其狀態空間描述爲：

$\left\{ \begin{array}{ll} X(k+1) = \Phi(k+1,k)X(k)+G(k+1,k)U(k) + \Gamma(k+1,k)W(k) \\ Z(k)=H(k)X(k)+V(k) \end{array} \right.$

在最優濾波篇中，我們的基本假設跟前面的最優預測的部分相同。因此我們可以把問題描述爲：在給出觀測序列的條件下，要求找出的線性最優估計 $\hat{X(k+1|k+1)}$ ，使得估計值 $\hat{X(k+|k+1)}$ 與之間的誤差： $X(k+1|k+1)=X(k+1)-\hat{X(k+1|k+1)}$ 的方差最小，同時要求估計值 $\hat{X(k+1|k+1)}$ 是觀測序列的線性函數，且估計是無偏的，即 $E[\hat{X(k+1|k+1)}]=E[X(k+1)]$

二、線性離散系統的卡爾曼最優濾波方程

通常在推導卡爾曼最優濾波方程時，我們同樣先忽略控制輸入信號的作用，即認爲輸入爲0，這樣我們可以得到系統的狀態空間描述爲：

$\left\{ \begin{array}{ll} X(k+1) = \Phi(k+1,k)X(k) + \Gamma(k+1,k)W(k) \\ Z(k)=H(k)X(k)+V(k) \end{array} \right.$

在獲取到觀測序列後，假定我們已經找到狀態的最優濾波估計 $\hat{X(k|k)}$ ，同樣在時刻，在還未獲取到觀測值的情況下，我們可以先得到一步最優估計 $\hat{X(k+1|k)}$ ，由於爲零均值的白噪聲序列，其最優估計爲，因此：

$\hat{X(k+1|k)} = \Phi(k+1,k)\hat{X(k|k)} \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ (2.1)$

同樣也是零均值的白噪聲序列，我們可以得到觀測值的預測值：

$\hat{Z(k+1|k)}=H(k+1)\hat{X(k+1|k)}$

在我們獲取到觀測值之後，跟在最優預測裏面一樣，我們來討論一下觀測值以及其預測值 $\hat{Z(k+1|k)}$ 之間的誤差，以期用於修正對的預測值 $\hat{X(k+1|k)}$ 從而得到 $\hat{X(k+1|k+1)}$ ：

$Z(k+1|k)=Z(k+1)-\hat{Z(k+1|k)}$

$=H(k+1)X(k+1)+V(k+1)-H(k+1)\hat{X(k+1|k)}$

式中的 $X(k+1|k)=X(k+1)-\hat{X(k+1|k)}$ 是狀態的預測估計誤差。不難看出，誤差主要由以及的觀測誤差項構成。一般來講的維數與的維數不同，因而也不能直接使用來修正 $\hat{X(k+1|k)}$ 。同樣，由於採用的是線性最小方差估計，因此跟最優預測一樣，我們使用加權的方法來修正，同樣引入一個待定的最優濾波增益矩陣，此時我們可以獲得的線性最小方差估計 $\hat{X(k+1|k+1)}$ ：

$\hat{X(k+1|k+1)} = \hat{X(k+1|k)} + K(k+1)Z(k+1|k)$

也就是：

$\hat{X(k+1|k+1)} = \hat{X(k+1|k)} + K(k+1)[Z(k+1)-H(k+1)\hat{X(k+1|k)}] \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ (2.2)$

也可以寫爲：

$\hat{X(k+1|k+1)} = \hat{X(k+1|k)} + K(k+1)[H(k+1)X(k+1|k) + V(k+1)]$

下面我們來求最優增益濾波矩陣。由正交定理，的濾波估計誤差：

$X(k+1|k+1) = X(k+1)-\hat{X(k+1|k+1)}$

$=X(k+1)-\hat{X(k+1|k)} - K(k+1)[H(k+1)X(k+1|k) + V(k+1)]$

應與觀測值正交，也就是：

即：

$E\left \{ [X(k+1|k)-K(k+1)H(k+1)X(k+1|k)-K(k+1)V(k+1)][H(k)X(k+1)+V(k+1)]^T \right \}$

$= E[X(k+1|k)X(k+1)^TH(k+1)^T] + \\ {\ \ \ \ \ \ \ \ E[X(k+1|k)V(k+1)^T]} - \\ {\ \ \ \ \ \ \ \ E[K(k+1)H(k+1)X(k+1|k)X(k+1)^TH(k+1)^T]} - \\ {\ \ \ \ \ \ \ \ E[K(k+1)H(k+1)X(k+1|k)V(k+1)^T]} - \\ {\ \ \ \ \ \ \ \ E[K(k+1)V(k+1)X(k+1)^TH(k+1)^T]} - \\ {\ \ \ \ \ \ \ \ E[K(k+1)V(k+1)V(k+1)^T]}$

$= E[X(k+1|k)X(k+1)^T]H(k+1)^T + \\ {\ \ \ \ \ \ \ \ E[X(k+1|k)V(k+1)^T]} - \\ {\ \ \ \ \ \ \ \ K(k+1)H(k+1)E[X(k+1|k)X(k+1)^T]H(k+1)^T} - \\ {\ \ \ \ \ \ \ \ K(k+1)H(k+1)E[X(k+1|k)V(k+1)^T]} - \\ {\ \ \ \ \ \ \ \ K(k+1)E[V(k+1)X(k+1)^T]H(k+1)^T} - \\ {\ \ \ \ \ \ \ \ K(k+1)E[V(k+1)V(k+1)^T]}=0$

觀察上式不難看出，右側第2、4、5項均爲0，令：

$P(k+1|k)=E[X(k+1|k){X(k+1|k)^T}]$

爲最優預測誤差方差陣。則上式可化簡爲：

${P(k+1|k)H(k+1)^T - K(k+1)H(k+1)P(k+1|k)H(k+1)^T - {K(k+1)R_{k+1}}=0}$

由此我們可以計算出最優濾波增益矩陣:

$K(k+1) = P(k+1|k)H(k+1)^T[H(k+1)P(k+1|k)H(k+1)^T + R_{k+1}]^{-1} \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ (2.3)$

由於引入了新的最優預測誤差方差陣，下面我們來計算。

由：

$\hat{X(k+1|k)} = \Phi(k+1,k)\hat{X(k|k)}$

我們可以得到：

$X(k+1|k)=X(k+1)-\hat{X(k+1|k)} \\ { \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ = \Phi(k+1,k)X(k) + \Gamma(k+1,k)W(k) - \Phi(k+1,k)\hat{X(k|k)}} \\ { \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ = \Phi(k+1,k)X(k|k) + \Gamma(k+1,k)W(k)}$

又：

$P(k+1|k)=E[X(k+1|k){X(k+1|k)^T}] \\ {\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ = E[\Phi(k+1,k)X(k|k) + \Gamma(k+1,k)W(k)][\Phi(k+1,k)X(k|k) + \Gamma(k+1,k)W(k)]^T}$

$= E[\Phi(k+1,k)X(k|k)X(k|k)^T\Phi(k+1,k)^T] + \\ {\ \ \ \ \ \ \ \ E[\Phi(k+1,k)X(k|k)W(k)^T\Gamma(k+1,k)^T]} + \\ {\ \ \ \ \ \ \ \ E[\Gamma(k+1,k)W(k)X(k|k)^T\Phi(k+1,k)^T]}+ \\ {\ \ \ \ \ \ \ \ E[\Gamma(k+1,k)W(k)W(k)^T\Gamma(k+1,k)^T]}$

$=\Phi(k+1,k)E[X(k|k)X(k|k)^T]\Phi(k+1,k)^T + \\ {\ \ \ \ \ \ \ \ \Phi(k+1,k)E[X(k|k)W(k)^T]\Gamma(k+1,k)^T} + \\ {\ \ \ \ \ \ \ \ \Gamma(k+1,k)E[W(k)X(k|k)^T]\Phi(k+1,k)^T}+ \\ {\ \ \ \ \ \ \ \ \Gamma(k+1,k)E[W(k)W(k)^T]\Gamma(k+1,k)^T}$

觀察上式不難發現第2、3項爲0，因此上式可以化簡爲：

$P(k+1|k)=\Phi(k+1,k)P(k|k)\Phi(k+1,k)^T + {\Gamma(k+1,k)Q_k\Gamma(k+1,k)^T} \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ (2.4)$

上式中 $P(k|k)=E[X(k|k){X(k|k)^T}]$ 爲最優濾波誤差方差陣，下面我們來計算其表達式。由定義：

$P(k+1|k+1)=E[X(k+1|k+1){X(k+1|k+1)^T}] \\ {\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ =E[X(k+1|k)-K(k+1)H(k+1)X(k+1|k)-K(k+1)V(k+1)]} \\ {\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ [X(k+1|k)-K(k+1)H(k+1)X(k+1|k)-K(k+1)V(k+1)]^T}$

$=E[X(k+1|k)X(k+1|k)^T] - \\ { \ \ \ \ \ \ \ \ E[X(k+1|k)X(k+1|k)^TH(k+1)^TK(k+1)^T]} - \\ { \ \ \ \ \ \ \ \ E[X(k+1|k)V(k+1)^TK(k+1)^T]} - \\ { \ \ \ \ \ \ \ \ E[K(k+1)H(k+1)X(k+1|k)X(k+1|k)^T]} + \\ { \ \ \ \ \ \ \ \ E[K(k+1)H(k+1)X(k+1|k)X(k+1|k)^TH(k+1)^TK(k+1)^T]} + \\ { \ \ \ \ \ \ \ \ E[K(k+1)H(k+1)X(k+1|k)V(k+1)^TK(k+1)^T]} - \\ { \ \ \ \ \ \ \ \ E[K(k+1)V(k+1)X(k+1|k)^T]} + \\ { \ \ \ \ \ \ \ \ E[K(k+1)V(k+1)X(k+1|k)^TH(k+1)^TK(k+1)^T]} + \\ { \ \ \ \ \ \ \ \ E[K(k+1)V(k+1)V(k+1)^TK(k+1)^T]}$

$=E[X(k+1|k)X(k+1|k)^T] - \\ { \ \ \ \ \ \ \ \ E[X(k+1|k)X(k+1|k)^T]H(k+1)^TK(k+1)^T} - \\ { \ \ \ \ \ \ \ \ E[X(k+1|k)V(k+1)^T]K(k+1)^T} - \\ { \ \ \ \ \ \ \ \ K(k+1)H(k+1)E[X(k+1|k)X(k+1|k)^T]} + \\ { \ \ \ \ \ \ \ \ K(k+1)H(k+1)E[X(k+1|k)X(k+1|k)^T]H(k+1)^TK(k+1)^T} + \\ { \ \ \ \ \ \ \ \ K(k+1)H(k+1)E[X(k+1|k)V(k+1)^T]K(k+1)^T} - \\ { \ \ \ \ \ \ \ \ K(k+1)E[V(k+1)X(k+1|k)^T]} + \\ { \ \ \ \ \ \ \ \ K(k+1)E[V(k+1)X(k+1|k)^T]H(k+1)^TK(k+1)^T} + \\ { \ \ \ \ \ \ \ \ K(k+1)E[V(k+1)V(k+1)^T]K(k+1)^T}$

觀察上式，不難看出第3、6、7、8項均爲0，即可進一步化簡爲：

$P(k+1|k+1)=P(k+1|k) - \\ { \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ P(k+1|k)H(k+1)^TK(k+1)^T} - \\ { \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ K(k+1)H(k+1)P(k+1|k)} + \\ { \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ K(k+1)H(k+1)P(k+1|k)H(k+1)^TK(k+1)^T} + \\ { \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ K(k+1)R_{k+1}K(k+1)^T}$

$=P(k+1|k) - P(k+1|k)H(k+1)^TK(k+1)^T - K(k+1)H(k+1)P(k+1|k) + \\ { \ \ \ \ \ \ \ \ K(k+1)[H(k+1)P(k+1|k)H(k+1)^T + R_{k+1}]K(k+1)^T}$

$=P(k+1|k) - P(k+1|k)H(k+1)^TK(k+1)^T - K(k+1)H(k+1)P(k+1|k) + \\ { \ \ \ \ \ \ \ \ P(k+1|k)H(k+1)^T[H(k+1)P(k+1|k)H(k+1)^T + R_{k+1}]^{-1}[H(k+1)P(k+1|k)H(k+1)^T + R_{k+1}]K(k+1)^T}$

$=P(k+1|k) - P(k+1|k)H(k+1)^TK(k+1)^T - K(k+1)H(k+1)P(k+1|k) \\ { \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ +P(k+1|k)H(k+1)^TK(k+1)^T}$

將式代入上式，由此得到：

$P(k+1|k+1) = P(k+1|k) - \\ { \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ P(k+1|k)H(k+1)^T[H(k+1)P(k+1|k)H(k+1)^T + R_{k+1}]^{-1}H(k+1)P(k+1|k) \ \ \ \ \ (2.5)}$

方程即構成了卡爾曼最優濾波方程組，將它們寫到一起就是：

(1)最優預測估計方程：

$\hat{X(k+1|k)} = \Phi(k+1,k)\hat{X(k|k)}$

(2)最優濾波估計方程：

$\hat{X(k+1|k+1)} = \hat{X(k+1|k)} + K(k+1)[Z(k+1)-H(k+1)\hat{X(k+1|k)}]$

(3)最優濾波增益矩陣方程：

$K(k+1) = P(k+1|k)H(k+1)^T[H(k+1)P(k+1|k)H(k+1)^T + R_{k+1}]^{-1}$

(4)最優預測估計誤差方差陣方程：

$P(k+1|k)=\Phi(k+1,k)P(k|k)\Phi(k+1,k)^T + {\Gamma(k+1,k)Q_k\Gamma(k+1,k)^T}$

(5)最優濾波估計誤差方差陣方程：

$P(k+1|k+1) = P(k+1|k) - \\ { \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ P(k+1|k)H(k+1)^T[H(k+1)P(k+1|k)H(k+1)^T + R_{k+1}]^{-1}H(k+1)P(k+1|k)}$

當然，很多時候我們經常看到的是另一種形式的，由之前的推導可知：

式中的爲單位陣。

實際系統中，很多時候系統的輸入並不爲0，也就是 $U(k) \neq 0$ ，則僅需將最優預測估計方程修改爲：

$\hat{X(k+1|k)} = \Phi(k+1,k)\hat{X(k|k)}+G(k+1,k)U(k)$

而其他方程無需變化，具體推導過程類似。

此外，五組方程也經常寫爲：

$1.\hat{X(k|k-1)} = \Phi(k,k-1)\hat{X(k-1|k-1)}+G(k,k-1)U(k-1)$

$2.\hat{X(k|k)} = \hat{X(k|k-1)} + K(k)[Z(k)-H(k)\hat{X(k|k-1)}]$

$3.K(k) = P(k|k-1)H(k)^T[H(k)P(k|k-1)H(k)^T + R_{k}]^{-1}$

$4.P(k|k-1)=\Phi(k,k-1)P(k-1|k-1)\Phi(k,k-1)^T + {\Gamma(k,k-1)Q_{k-1}\Gamma(k,k-1)^T}$

三、線性離散系統的卡爾曼最優濾波的算法實現

(1)將上一次的最優濾波估計值 $\hat{X(k|k)}$ 代入最優預測估計方程計算出一步最優預測 $\hat{X(k+1|k)}$ 。

(2)根據上一次的最優濾波誤差方差陣代入最優預測估計誤差方差陣方程計算出。

(3)由上一步計算出的，代入最優濾波增益矩陣方程計算出。

(4)在獲取到新的觀測值後，用上一步得到的分別代入最優濾波估計方程，得到最優濾波估計值 $\hat{X(k+1|k+1)}$

(5)最後由代入最優濾波估計誤差方差陣，更新濾波估計誤差方差陣以便於下一次迭代。

四、舉例實現卡爾曼最優濾波算法

1.在很多數模混合系統中，我們經常需要使用ADC測量模擬輸出以便下一步的數字處理。但是實際模擬系統中，各放大器、電源等均存在噪聲，因此反映在輸出的模擬信號中也存在大量的隨機噪聲，同時ADC採集的時候也會存在測量噪聲，且這兩種噪聲一般都相互獨立，且絕大部分噪聲均服從均值爲0的高斯分佈，因此滿足卡爾曼濾波的基本假設條件，可以使用卡爾曼濾波獲得最優估計值。下面我們以一維卡爾曼濾波爲例，具體實現一下卡爾曼濾波算法。

首先對系統進行建模，對於一個採樣系統來說，我們可以假定系統沒有輸入，在很短的時間內系統的狀態幾乎不變但是噪聲每時每刻都存在，且觀測值就是實際ADC的輸出值，但是夾帶着觀測噪聲，也就是：

$\left\{ \begin{array}{ll} X(k) = X(k-1) + W(k-1) \\ Z(k-1)=X(k-1) + V(k-1) \end{array} \right.$

因此：

系統矩陣 $\Phi(k,k-1) = 1$

輸入矩陣

輸入向量

系統噪聲的方差爲且恆定， $\Gamma(k,k-1) = 1$

輸出矩陣

觀測噪聲的方差爲且恆定。

接下來我們用C代碼來一步一步寫出一維的卡爾曼濾波算法。首先聲明，這裏給出的算法僅僅是演示用，既不高效也不節省空間，僅僅是爲了將算法步驟說清楚，不涉及優化的內容，若讀者有興趣可以自行進行算法優化。

先給出所需的數據結構：

typedef struct 
{
    float Q;
    float R;
    float P_K_1[1];
    float P_K[1];
    float K;
    float state_vector[1];
    float optimal_state_vector[1];
    float input_vector[1];
    float obsered_vector[1];
}kalman_filter_1d_t;

結構體中：

Q爲系統噪聲的方差，需根據實際系統參數確定

R爲測量噪聲的方差，需根據ADC相關參數確定

P_K_1[1]也就是方程裏面的，即最優預測估計誤差方差陣，這裏爲一個 $1\times 1$ 矩陣，也就是一個一維變量

P_K[1]也就是方程裏面的，即最優濾波估計誤差方差陣

K爲最優濾波增益矩陣，這裏也是一維的變量

state_vector爲狀態向量，同時存儲一步最優預測值

optimal_state_vector爲當前時刻最優濾波估計值

input_vector爲系統輸入向量

obsered_vector爲系統觀測向量

接下來是相關定義以及初始化，在此不再贅述，假定代碼中的Q,R爲已知的。

kalman_filter_1d_t filter = 
{
    .Q = Q,
    .R = R,
    .P_K_1[0] = {0.0f},
    .P_K[0] = {0.0f},
    .K = 0.0f,
    .state_vector[0] = {0.0f},
    .optimal_state_vector[0] = {0.0f},
    .input_vector[0] = {0.0f},
    .observed_vector[0] = {0.0f}
};

下面我們來實現算法：

1)根據最優預測估計方程以及存儲在結構體中的上一時刻的最優濾波估計值optimal_state_vector計算當前時刻的一步最優預測並存儲到state_vector中，由於 $\Phi(k,k-1) = 1$ 以及即：

filter.state_vector[0] = filter.optimal_state_vector[0] + filter.input_vector[0];

2)根據最優預測估計誤差方差陣方程以及上一時刻的最優濾波誤差方差陣P_K計算當前時刻的最優預測估計誤差方差陣P_K_1，且 $\Phi(k,k-1) = 1$ 以及 $\Gamma(k,k-1) = 1$ ，則：

filter.P_K_1 = filter.P_K + filter.Q;

3)根據最優濾波增益矩陣方程以及在上一步中獲得的最優預測估計誤差方差陣P_K_1來計算當前時刻最優濾波增益矩陣K。本來方程中涉及到矩陣求逆運算(這裏其實是一個很大的問題，但是不在本文的討論的範圍之內，後續有空可以細說一下)，但是這裏僅僅是一個 $1\times 1$ 矩陣，因此求逆變得非常容易：

filter.K = filter.P_K_1 / (filter.P_K_1 + filter.R);

4)假設我們已經獲取到當前時刻的觀測值爲adc_value，則根據最優濾波估計方程以及上一步得出的最優濾波增益矩陣K計算出當前時刻的最優濾波估計值optimal_state_vector：

filter.observed_vector[0] = adc_value;
filter.optimal_state_vector[0] = filter.state_vector[0] + \ 
filter.K * (filter.observed_vector[0] - filter.state_vector[0]);

5)最後更新當前時刻的最優濾波估計誤差方差陣以便於下一次迭代，也就是：

filter.P_K = (1.0f - filter.K) * filter.P_K_1;

至此完成一次迭代，完整版代碼如下：

float kalman_filter_1d_calc(float adc_value)
{
    filter.observed_vector[0] = adc_value;
    filter.state_vector[0] = filter.optimal_state_vector[0] + filter.input_vector[0];
    filter.P_K_1 = filter.P_K + filter.Q;
    filter.K = filter.P_K_1 / (filter.P_K_1 + filter.R);
    filter.optimal_state_vector[0] = filter.state_vector[0] + \ 
    filter.K * (filter.observed_vector[0] - filter.state_vector[0]);
    filter.P_K = (1.0f - filter.K) * filter.P_K_1;
    return filter.optimal_state_vector[0];
}

循環調用該函數並傳入每次ADC的測量值即可不斷迭代獲得最優濾波估計。同時還可以輸出P_K進行觀察，若P_K很小接近於0，那濾波就是有效的。

2.在上一篇提到我接觸卡爾曼濾波就是從調試慣性MEMS單元那兒來的，卡爾曼濾波在慣性MEMS數據融合方面也有很多應用。這裏就拿筆者熟悉的MPU6050爲例做介紹，其他慣性MEMS處理方法均類似。MPU6050是一顆6軸慣性單元，其中包含3軸陀螺儀跟3軸加速度計。

正如大家所熟知的，陀螺儀輸出的是角速度值 $\omega(t)$ ，想要獲取角度值必須對其進行積分，也就是：

$angle = \int \omega(t) dt$

對其離散化之後可寫成：

$angle = \sum \omega(t) \Delta t$

其短時具有很高的精度，但是隨着時間的推移，積分誤差會逐漸增大，並且其同時存在漂移，雖然漂移會隨時間變化，但是在短時間內我們可以認爲其漂移近似不變。而加速度計直接輸出三個維度的加速度值，在物體處於平衡狀態時其合力爲0，也就是加速度和爲0，且重力加速度恆定爲1g，此時若能獲取到三個維度的加速度值，就能反推出物體的姿態角。(單純依靠三軸加速度計不能獲取到偏航角，但是這又是另一個話題了，在此不再展開細說。)但是一旦物體離開平衡狀態，也就是物體開始做非勻速直線運動，那麼其就有其他方向的加速度，則單純依靠加速度計獲得的姿態角精度將極大降低，但是其不像陀螺儀一樣會存在積分誤差。因此可以對陀螺儀跟加速度計的數據做融合，提高精度。

下面開始對系統建模，先僅研究某一方向的姿態角。對於陀螺儀獲取角度值，我們有：

$Angle(k) = Angle(k-1) + [Gyro(k-1) - Bias(k-1)] * dt + W_{angle}$

式中：

爲當前時刻計算出的角度值

爲上一時刻計算出的角度值

爲獲取到的陀螺儀輸出值

爲上一時刻陀螺儀的漂移

$W_{angle}$ 爲陀螺儀系統噪聲

爲積分時間間隔

前面提到雖然漂移會隨時間變化，但是在短時間內我們可以認爲其漂移近似不變，但是混雜着噪聲，因此對於漂移，我們有：

$Bias(k) = Bias(k-1)+W_{bias}$

式中：

爲當前時刻陀螺儀的漂移

爲上一時刻陀螺儀的漂移

$W_{bias}$ 爲漂移噪聲

綜合上述兩個方程，我們可以選取姿態角跟漂移量作爲系統的狀態變量。系統的輸入量爲陀螺儀的輸出值，因此將兩方程合併並寫成矩陣方程的形式有：

$\begin{bmatrix} Angle(k) \\ Bias(k) \end{bmatrix} = \begin{bmatrix} 1 & -dt \\ 0 & 1\\ \end{bmatrix} \begin{bmatrix} Angle(k-1) \\ Bias(k-1) \end{bmatrix} + \begin{bmatrix} dt \\ 0 \end{bmatrix}[Gyro(k-1)] + \begin{bmatrix} 1 & 0 \\ 0 & 1\\ \end{bmatrix} \begin{bmatrix} W_{angle} \\ W_{bias} \end{bmatrix}$

由此我們得到系統的狀態方程。

式中：

$\begin{bmatrix} Angle(k) \\ Bias(k) \end{bmatrix}$ 爲系統的狀態向量，也就是

$\begin{bmatrix} 1 & -dt \\ 0 & 1\\ \end{bmatrix}$ 爲系統矩陣，也就是 $\Phi(k,k-1)$

$\begin{bmatrix} dt \\ 0 \end{bmatrix}$ 爲系統的輸入矩陣，也就是

爲系統的輸入向量，也就是

$\begin{bmatrix} W_{angle} \\ W_{bias} \end{bmatrix}$ 爲陀螺儀噪聲與漂移噪聲矩陣，也就是

$\begin{bmatrix} 1 & 0 \\ 0 & 1\\ \end{bmatrix}$ 爲方程中的 $\Gamma(k,k-1)$

同時我們還可以從加速度計獲取到姿態角度，因此我們可以將加速度計解算出的姿態角作爲觀測值，但是其同樣存在噪聲，並且不能獲取到陀螺儀的漂移值，因此我們可以寫出觀測方程：

$[Angle_{acce}(k-1)]=\begin{bmatrix} 1 & 0 \end{bmatrix}\begin{bmatrix} Angle(k-1) \\ Bias(k-1) \end{bmatrix} + V(k-1)$

式中：

$[Angle_{acce}(k-1)]$ 爲系統的觀測值，也就是

$\begin{bmatrix} 1 & 0 \end{bmatrix}$ 爲系統的輸出矩陣，也就是

爲系統的觀測噪聲

假定我們已知陀螺儀噪聲方差 $Q_{angle}$ ，漂移噪聲方差 $Q_{bias}$ 以及加速度計的噪聲方差 $R_{acce}$ ，並且由於陀螺儀的積分性質， $Q_{angle}$ 與 $Q_{bias}$ 均與時間相關，但是各噪聲之間互不相關，且均服從均值爲0的高斯分佈，則陀螺儀噪聲與漂移噪聲的協方差矩陣爲一個對角陣，且對角元素爲各自的方差，也就是：

$Q(k-1)=\begin{bmatrix} Q_{angle}*dt & 0 \\ 0 & Q_{bias}*dt\\ \end{bmatrix}$

而加速度計的噪聲方差陣爲：

$R(k-1) = R_{acce}$

易知該系統滿足卡爾曼濾波的基本假設條件，接下來我們來實現卡爾曼濾波器算法。

首先給出用到的數據結構：

typedef struct 
{
    float dt;
    float Q[2][2];
    float R;
    float P_K_1[2][2];
    float P_K[2][2];
    float K[2];
    float state_vector[2];
    float optimal_state_vector[2];
    float input_vector[1];
    float observed_vector[1];
}imu_kalman_filter_2d_t;

結構體中：

dt爲陀螺儀的積分時間間隔

Q[2][2]爲陀螺儀噪聲以及漂移噪聲的協方差矩陣，也就是基本方程裏面的 $Q_{k-1}$

R爲加速度計的噪聲方差，也就是基本方程裏面的 $R_{k-1}$

P_K_1[2][2]爲最優預測估計誤差方差陣，也就是基本方程裏的

P_K[2][2]爲最優濾波估計誤差方差陣，也就是基本方程裏面的

K[2]爲最優濾波增益矩陣，也就是基本方程裏面的

state_vector[2]存儲一步最優預測估計值

optimal_state_vector[2]爲當前時刻最優濾波估計值

input_vector[1]爲系統的輸入，也就是陀螺儀的輸出值

observed_vector[1]爲系統的觀測值，也就是加速度計的輸出值

接下來是相關定義以及初始化，在此不再贅述，假定代碼中的Q0,Q1,R爲已知的，CALC_PERIOD爲計算週期。

void kalman_filter_init(imu_kalman_filter_2d_t * instance)
{
    instance->dt = CALC_PERIOD;

    instance->Q[0][0] = Q0;
    instance->Q[0][1] = 0.0f;
    instance->Q[1][0] = 0.0f;
    instance->Q[1][1] = Q1;

    instance->R = R;

    instance->input_vector[0] = 0.0f;
    instance->observed_vector[0] = 0.0f;

    instance->state_vector[0] = 0.0f;
    instance->state_vector[1] = 0.0f;

    instance->optimal_state_vector[0] = 0.0f;
    instance->optimal_state_vector[1] = 0.0f;

    instance->K[0] = 0.0f;
    instance->K[1] = 0.0f;

    instance->P_K[0][0] = 0.0f;
    instance->P_K[0][1] = 0.0f;
    instance->P_K[1][0] = 0.0f;
    instance->P_K[1][1] = 0.0f;

    instance->P_K_1[0][0] = 0.0f;
    instance->P_K_1[0][1] = 0.0f;
    instance->P_K_1[1][0] = 0.0f;
    instance->P_K_1[1][1] = 0.0f;

}

下面我們來實現算法：

1)根據最優預測估計方程計算一步最優估計值，也就是方程：

$\hat{X(k|k-1)} = \Phi(k,k-1)\hat{X(k-1|k-1)}+G(k,k-1)U(k-1)$

在我們的系統中也就是：

${\begin{bmatrix} state\_vector[0] \\ state\_vector[1] \end{bmatrix} = \begin{bmatrix} 1 & -dt \\ 0 & 1\\ \end{bmatrix} \begin{bmatrix} optimal\_state\_vector[0] \\ optimal\_state\_vector[1] \end{bmatrix} + \begin{bmatrix} dt \\ 0 \end{bmatrix}[input\_vector[0]]}$

代碼裏我們可以寫成：

instance->state_vector[0] = (instance->optimal_state_vector[0] - \ 
                            instance->optimal_state_vector[1] * instance->dt) + \
                            instance->input_vector[0] * instance->dt;
instance->state_vector[1] = instance->optimal_state_vector[1];

2)根據最優預測估計誤差方差陣方程計算出，也就是方程：

$P(k|k-1)=\Phi(k,k-1)P(k-1|k-1)\Phi(k,k-1)^T + {\Gamma(k,k-1)Q_{k-1}\Gamma(k,k-1)^T}$

在我們系統中也就是：

${\begin{bmatrix}P\_K\_1[0][0] & P\_K\_1[0][1]\\ P\_K\_1[1][0] & P\_K\_1[1][1] \end{bmatrix} = \begin{bmatrix} 1 & -dt \\ 0 & 1\\ \end{bmatrix} \begin{bmatrix} P\_K[0][0] &P\_K[0][1] \\ P\_K[1][0] & P\_K[1][1]\end{bmatrix} \begin{bmatrix} 1 & 0 \\ -dt & 1\\ \end{bmatrix}+ \begin{bmatrix} 1 & 0 \\ 0 & 1\\ \end{bmatrix}\begin{bmatrix} Q0*dt & 0 \\ 0 & Q1*dt\\ \end{bmatrix}\begin{bmatrix} 1 & 0 \\ 0 & 1\\ \end{bmatrix}}$

將其展開併合並化簡有：

${\begin{bmatrix}P\_K\_1[0][0] & P\_K\_1[0][1]\\ P\_K\_1[1][0] & P\_K\_1[1][1] \end{bmatrix} = \begin{bmatrix} P\_K[0][0] &P\_K[0][1] \\ P\_K[1][0] & P\_K[1][1]\end{bmatrix} + \begin{bmatrix} (Q0 - P\_K[0][1]-P\_K[1][0]+P\_K[1][1]*dt)*dt & -P\_K[1][1]*dt \\ -P\_K[1][1]*dt & Q1*dt\\ \end{bmatrix}}$

寫成代碼也就是：

instance->P_K_1[0][0] = instance->P_K[0][0] + instance->Q[0][0] * instance->dt - \ 
                        instance->P_K[0][1] * instance->dt - instance->P_K[1][0] * instance->dt + \ 
                        instance->P_K[1][1] * instance->dt * instance->dt;

instance->P_K_1[0][1] = instance->P_K[0][1] - instance->P_K[1][1] * instance->dt + \ 
                        instance->Q[0][1] * instance->dt;

instance->P_K_1[1][0] = instance->P_K[1][0] - instance->P_K[1][1] * instance->dt + \ 
                        instance->Q[1][0] * instance->dt;

instance->P_K_1[1][1] = instance->P_K[1][1] + instance->Q[1][1] * instance->dt;

注意上面代碼段中Q[0][1]以及Q[1][0]均爲0

3)根據最優濾波增益矩陣方程計算最優濾波增益矩陣，也就是方程：

$K(k) = P(k|k-1)H(k)^T[H(k)P(k|k-1)H(k)^T + R_{k}]^{-1}$

即：

${\begin{bmatrix}K[0] \\ K[1] \end{bmatrix} = \begin{bmatrix} P\_K\_1[0][0] &P\_K\_1[0][1] \\ P\_K\_1[1][0] & P\_K\_1[1][1]\end{bmatrix} \begin{bmatrix} 1 \\ 0 \\ \end{bmatrix}+ {[\begin{bmatrix} 1 & 0 \end{bmatrix}\begin{bmatrix} P\_K\_1[0][0] &P\_K\_1[0][1] \\ P\_K\_1[1][0] & P\_K\_1[1][1]\end{bmatrix} \begin{bmatrix} 1 \\ 0 \end{bmatrix}} + R]^{-1}}$

拆開併合並得到：

${\begin{bmatrix}K[0] \\ K[1] \end{bmatrix} = \begin{bmatrix} P\_K\_1[0][0] \\ P\_K\_1[1][0] \end{bmatrix} + {[P\_K\_1[0][0]} + R]^{-1}}$

代碼如下：

instance->K[0] = instance->P_K_1[0][0] / (instance->P_K_1[0][0] + instance->R);
instance->K[1] = instance->P_K_1[1][0] / (instance->P_K_1[0][0] + instance->R);

4)根據最優濾波估計方程計算最優濾波估計值，方程爲：

$\hat{X(k|k)} = \hat{X(k|k-1)} + K(k)[Z(k)-H(k)\hat{X(k|k-1)}]$

即：

${\begin{bmatrix} optimal\_state\_vector[0] \\ optimal\_state\_vector[1] \end{bmatrix} = \begin{bmatrix} state\_vector[0] \\ state\_vector[1] \end{bmatrix} + \begin{bmatrix} K[0] \\ K[1] \end{bmatrix}[observed\_vector[0]-\begin{bmatrix} 1 & 0 \end{bmatrix}\begin{bmatrix} state\_vector[0] \\ state\_vector[1] \end{bmatrix}]}$

也就是：

代碼爲：

instance->optimal_state_vector[0] = instance->state_vector[0] + \
                                    instance->K[0] * (instance->observed_vector[0] - \     
                                    instance->state_vector[0]);
instance->optimal_state_vector[1] = instance->state_vector[1] + \
                                    instance->K[1] * (instance->observed_vector[1] - \ 
                                    instance->state_vector[1]);

5)最後根據最優濾波估計誤差方差陣方程更新最優濾波估計誤差方差陣，也就是方程：

也就是：

$\begin{bmatrix} P\_K[0][0] &P\_K[0][1] \\ P\_K[1][0] & P\_K[1][1]\end{bmatrix} =[\begin{bmatrix} 1 & 0 \\ 0 & 1\\ \end{bmatrix} - \begin{bmatrix} K[0] \\ K[1] \end{bmatrix} \begin{bmatrix} 1 & 0 \end{bmatrix}] \begin{bmatrix} P\_K\_1[0][0] & P\_K\_1[0][1] \\ P\_K\_1[1][0] & P\_K\_1[1][1]\end{bmatrix}}$

化簡後有：

$\begin{bmatrix} P\_K[0][0] &P\_K[0][1] \\ P\_K[1][0] & P\_K[1][1]\end{bmatrix} =[\begin{bmatrix} 1-K[0] & 0 \\ 0 - K[1] & 1\\ \end{bmatrix} ] \begin{bmatrix} P\_K\_1[0][0] & P\_K\_1[0][1] \\ P\_K\_1[1][0] & P\_K\_1[1][1]\end{bmatrix}}$

$=\begin{bmatrix} P\_K\_1[0][0]-K[0]* P\_K\_1[0][0] & P\_K\_1[0][1]-K[0]* P\_K\_1[0][1] \\P\_K\_1[1][0] - K[1] * P\_K\_1[0][0] & P\_K\_1[1][1] - K[1] * P\_K\_1[0][1]\end{bmatrix}$

代碼爲：

instance->P_K[0][0] = instance->P_K_1[0][0] - instance->K[0] * instance->P_K_1[0][0];
instance->P_K[0][1] = instance->P_K_1[0][1] - instance->K[0] * instance->P_K_1[0][1];
instance->P_K[1][0] = instance->P_K_1[1][0] - instance->K[1] * instance->P_K_1[0][0];
instance->P_K[1][1] = instance->P_K_1[1][1] - instance->K[1] * instance->P_K_1[0][1];

至此完成一次迭代，完整版代碼如下：

float imu_kalman_filter_2d_calc(imu_kalman_filter_2d_t * instance, float observed_value, float input_value)
{
    instance->observed_vector[0] = observed_value;
    instance->input_vector[0] = input_value;

    instance->state_vector[0] = (instance->optimal_state_vector[0] - instance->optimal_state_vector[1] * instance->dt) + \
                                instance->input_vector[0] * instance->dt;
    instance->state_vector[1] = instance->optimal_state_vector[1];

    instance->P_K_1[0][0] = instance->P_K[0][0] + instance->Q[0][0] * instance->dt - \ 
                            instance->P_K[0][1] * instance->dt - instance->P_K[1][0] * instance->dt + \ 
                            instance->P_K[1][1] * instance->dt * instance->dt;

    instance->P_K_1[0][1] = instance->P_K[0][1] - instance->P_K[1][1] * instance->dt + instance->Q[0][1] * instance->dt;

    instance->P_K_1[1][0] = instance->P_K[1][0] - instance->P_K[1][1] * instance->dt + instance->Q[1][0] * instance->dt;

    instance->P_K_1[1][1] = instance->P_K[1][1] + instance->Q[1][1] * instance->dt;

    instance->K[0] = instance->P_K_1[0][0] / (instance->P_K_1[0][0] + instance->R);
    instance->K[1] = instance->P_K_1[1][0] / (instance->P_K_1[0][0] + instance->R);

    instance->optimal_state_vector[0] = instance->state_vector[0] + \
                                        instance->K[0] * (instance->observed_vector[0] - instance->state_vector[0]);
    instance->optimal_state_vector[1] = instance->state_vector[1] + \
                                        instance->K[1] * (instance->observed_vector[1] - instance->state_vector[1]);

    instance->P_K[0][0] = instance->P_K_1[0][0] - instance->K[0] * instance->P_K_1[0][0];
    instance->P_K[0][1] = instance->P_K_1[0][1] - instance->K[0] * instance->P_K_1[0][1];
    instance->P_K[1][0] = instance->P_K_1[1][0] - instance->K[1] * instance->P_K_1[0][0];
    instance->P_K[1][1] = instance->P_K_1[1][1] - instance->K[1] * instance->P_K_1[0][1];
    return instance->optimal_state_vector[0];
}

循環調用該函數並傳入每次陀螺儀的輸出值即可不斷迭代獲得最優濾波估計。同時還可以輸出P_K進行觀察，若P_K主對角元素很小接近於0，那濾波就是有效的。

最後個人在ESP32的板子(今年回家沒帶其他板子，湊合着試了下)上面驗證了一下算法，裏面包含MPU6050的驅動跟卡爾曼濾波算法，最後效果還行。

代碼鏈接在這：

https://download.csdn.net/download/u013910954/12136477

五、總結

卡爾曼濾波器是一種線性無偏遞推濾波器，其算法簡單，且存儲數據少，實時性很高，因而應用非常廣泛。這兩篇文章僅僅是對卡爾曼最優預測跟最優濾波的一種簡單的推導，其實還有很多種其他推導方法，有興趣的讀者可以查閱相關書籍。

最後貼出個人認爲還不錯的兩個講解卡爾曼濾波以及應用的鏈接：

1.https://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/#comment-1010

2.http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/

六、一點小擴展

之前有提到，在基本方程中的最優濾波增益矩陣中，若觀測方程維數不爲1，則存在矩陣求逆的運算，一般來講普通矩陣求逆是一個很麻煩的事，但是對於上三角或者下三角矩陣求逆就比較簡單，並且在濾波方程裏面的需要求逆的矩陣是一個正定對稱矩陣，由矩陣的三角分解的相關定理，對於滿秩方陣可以做LU分解爲一個單位下三角矩陣與一個上三角矩陣的乘積，由此可以方便的計算出矩陣的逆，讀者若有興趣可以參考矩陣理論的相關章節。

還有一種方法是矩陣分析裏面的Neumann定理，將矩陣求逆轉換成矩陣級數求和的運算，但是要求矩陣的譜半徑小於1且運算量較大，因此不再贅述，這裏給出我們矩陣理論的相關章節的課件，有興趣的讀者可以查閱。

課件鏈接：

https://download.csdn.net/download/u013910954/12136595