從全狀態觀測器到卡爾曼濾波器（一）

! https://zhuanlan.zhihu.com/p/338269917

從全狀態觀測器到卡爾曼濾波器（一）

寫在開頭

一年前聽學長提了個很高端的東西，叫卡爾曼濾波器。學習過程中遇到了很多問題，讀了很多博客也還是一頭霧水。我讀的第一篇博客是：https://zhuanlan.zhihu.com/p/39912633，讀完以后基本理解了作者的意思，但是還遠不具備設計一個卡爾曼濾波器的能力，因為不懂如何設計\(Q 、R、 H\)三個矩陣。於是用 卡爾曼濾波器 實例 應用 為關鍵詞搜了些博客，發現很多人用的都是一維溫度測量的例子：狀態向量就只有溫度一個變量，\(Q、R\)不變最后整個卡爾曼濾波器收斂為一個一階低通濾波器。看完后大概了解了\(Q、R\)矩陣的含義，但還是對高維情況下\(H\)矩陣的設計一頭霧水。最后從Google搜kalman filter application得到的一些材料才解決了我之前的疑惑：https://towardsdatascience.com/kalman-filter-an-algorithm-for-making-sense-from-the-insights-of-various-sensors-fused-together-ddf67597f35e、https://www.cs.cornell.edu/courses/cs4758/2012sp/materials/MI63slides.pdf

看完這些后自己對卡爾曼濾波器也有了一個大致的概念，但對卡爾曼濾波器“最優”的特性與其本身估計狀態的原理仍然不是非常明確。這主要是因為缺乏對卡爾曼濾波器原理推導過程的了解，於是乎就去翻了翻教科書中卡爾曼濾波器的推導，但其中大量矩陣運算的定理我是不理解的，再加上我這個人比較愚鈍而且很浮躁，所以關於卡爾曼濾波器推導過程的學習最終也就不了了之了。

前些日子在知乎讀到了這篇文章：https://zhuanlan.zhihu.com/p/88535121，又回想起自己初學卡爾曼濾波器時候對\(H\)矩陣設計以及卡爾曼濾波器本身原理的一頭霧水，故打算以“從全狀態觀測器到卡爾曼濾波器”為題，從全狀態觀測器是如何估計不可測信息出發，用觀測器增益\(L\)類比卡爾曼增益\(K\)，並最后給出關於卡爾曼濾波器的不嚴謹推導，希望能給和我初學時一樣一頭霧水的朋友一點幫助。另外會給出在我寫的適用於cortex-M系列內核單片機的基於CMSIS DSP庫矩陣運算的卡爾曼濾波器C語言實現。疫情期間廢寢忘食開發這個C語言實現，日夜顛倒兩三天，修復了各種各樣的小bug，投入了很多心血，故希望分享出來幫助更多人：https://github.com/CharlesW1970/kalman-filter-C-implementation

---

0 從一個簡單的例子出發

我們現在希望用一個超聲波測距傳感器測量固定在直線軌道上的小車位置\(x\)，但這個傳感器不是完全准確的，其測量噪聲服從期望為0的正態分布

\[z(t)={x}(t)+{v}(t), {v}(t) \sim {N}(0, r)\\ \]

其中\(z(t)\)為\(t\)時刻測量值，\(x(t)\)為\(t\)時刻溫度，\(v(t)\)為為\(t\)時刻測量噪聲。

1 均值濾波

若小車是靜止的，我們很自然想到可以通過在一段時間內多此測量取平均值的方法來消除噪聲的影響以獲得一個准確的估計值\(\hat x\)

\[\hat x = \frac{z(t_0)+z(t_0 +\Delta t)+z(t_0 +2\Delta t)+\cdots+z(t_0 +k\Delta t)}{k+1}\\ \]

根據大數定律，當測量次數\(k\)足夠多的時候，測量值中噪聲的影響的總和為0的概率為1

\[\lim _{k \rightarrow \infty} P(|\bar{v}-0|<\epsilon)=1\\ \]

也就是說，我們測量的數據越多，測量的時間越長，就越有可能通過取平均值的方法獲取沒有誤差\(e = x-\hat x\)的估計值。

2 狀態空間

2.1 系統的狀態

但當小車不處於靜止狀態的時候，長時間測量多組數據取平均值的方法就不再有效了：為了更好的抑制噪聲我們需要測量更多數據，但更多的歷史數據意味着估計值的滯后越大。因此我們需要優化我們的模型：小車不靜止，意味着小車位置\(x\)存在變化，那么我們需要一個可以描述小車位置\(x\)變化的量來豐富我們的模型，即小車的速度\(v = \dot x\)。現在我們拿到了兩個量，一個是我們關注的對象位置\(x\)，一個是位置\(x\)的一階導，設

\[x_1 = x\\ x_2 = \dot x \]

我們很自然的可以將這兩個標量組合成一個向量\(\mathbf x\)

\[\mathbf x=\left[\begin{array}{c} x_1 \\ x_2 \end{array}\right]=\left[\begin{array}{c} x \\ \dot x \end{array}\right]\\ \]

我們稱向量\(\mathbf x\)為狀態向量，用於描述系統或對象的狀態。系統的狀態在不斷變化，而微分方程是描述動態系統變化的好方法，因此我們需要建立一個微分方程用於描述小車狀態的變化情況，一般來說我們需要構建形式如下的微分方程

\[\dot {\mathbf x} = A\mathbf x\\ \]

我們稱\(A\)為系統矩陣，根據\(x_1 = x,x_2 = \dot x\)這個假設我們很容易確定矩陣\(A\)

\[\dot {\mathbf x} = \left[\begin{array}{c} \dot x_1 \\ \dot x_2 \end{array}\right] =\left[\begin{array}{c} x_2 \\ 0 \end{array}\right]= \left[\begin{array}{c} 0 \quad 1 \\ 0 \quad 0 \end{array}\right] \left[\begin{array}{c} x_1 \\ x_2 \end{array}\right] \\ \]

即

\[A = \left[\begin{array}{c} 0 \quad 1 \\ 0 \quad 0 \end{array}\right]\\ \]

注意這里我們建立的是勻速模型，即\(\dot x_2 = 0\)。若假設勻加速模型則可設置三維狀態向量\(\mathbf x = [x \quad \dot x \quad \ddot x]^T\)。如果小車加速度較小，而且我們迭代微分方程的周期\(\Delta t\)足夠小，我們就完全可以采用勻速模型，因為更小的維數意味着更少的算式更小的計算量。

2.2 加入控制

如果我們是為反饋控制系統反饋的需要而估計小車的位置，那么我們一定能獲取反饋控制器的輸出，這樣一來我們就可以結合控制器輸出更准確的估計狀態。根據控制器輸出我們可以將向量方程\(\dot {\mathbf x} = A\mathbf x\)擴展為以下形式

\[\dot {\mathbf x} = A\mathbf x +B\mathbf u\\ \]

其中\(B\)為輸入矩陣或控制矩陣，而\(\mathbf u\)為輸入向量或控制向量。對於單輸入系統而已輸入向量\(\mathbf u\)為標量，即\(\mathbf u = u\)，而矩陣\(B\)的維數由輸入和狀態向量的維數共同確定，具體元素值由輸入與狀態的關系確定。我們假作用在小車上沿直線軌道與小車位置\(x\)具有相同正方向的力\(F\)為控制器的輸出，作為輸入\(u\)作用於小車系統。根據牛頓第二定律，有

\[u=F = ma = m\ddot x = m\dot x_2\\ \]

即

\[\dot x_2 = \frac{u}{m}\\ \]

根據上式我們可以確定矩陣\(B\)

\[\dot {\mathbf x} = \left[\begin{array}{c} \dot x_1 \\ \dot x_2 \end{array}\right] = \left[\begin{array}{c} 0 \quad 1 \\ 0 \quad 0 \end{array}\right] \left[\begin{array}{c} x_1 \\ x_2 \end{array}\right] + \left[\begin{array}{c} 0 \\ \frac{1}{m} \end{array}\right]u \\ \]

即

\[B = \left[\begin{array}{c} 0 \\ \frac{1}{m} \end{array}\right]\\ \]

2.3 確定輸出

狀態向量中包含多個量，稱其中我們真正關注的對象為系統輸出\(\mathbf y\)。其形式如下

\[\mathbf y = C\mathbf x +D\mathbf u\\ \]

其中\(C\)為輸出矩陣，為直接傳動項，在本系列中我們只關注\(\mathbf y = C\mathbf x\)。與輸入向量\(\mathbf u\)類似，對於單輸出系統而已輸出向量\(\mathbf y\)為標量，即\(\mathbf y = y\)，而對於多輸入系統而言，我們可以根據變量是否可測設置系統輸出，即誰可測誰作輸出的一部分。在小車這個單輸入單輸出中，我們關注小車位置\(x\)，並且小車位置\(x\)是可測的，因此設置\(x_1\)為系統輸出，即可確定輸出矩陣\(C\)

\[y = x = x_1 = [1 \quad 0]\left[\begin{array}{c} x_1 \\ x_2 \end{array}\right]\\ \]

即

\[C = [1\quad 0]\\ \]

到這一步我們已經完整表示了這個線性動態系統與被控對象動態，即

\[\dot {\mathbf x} = A\mathbf x +B\mathbf u\\ \mathbf y = C\mathbf x +D\mathbf u\\ \]

其中

\[\mathbf x=\left[\begin{array}{c} x \\ \dot x \end{array}\right] \quad \mathbf u=[F] \quad A = \left[\begin{array}{c} 0 \quad 1 \\ 0 \quad 0 \end{array}\right]\quad B = \left[\begin{array}{c} 0 \\ \frac{1}{m} \end{array}\right]\quad C = [1\quad 0] \quad D = [0]\\ \]

這種描述系統的方法被稱為狀態空間法，狀態空間的思想來源於描述微分方程的狀態變量法。在狀態變量法中，通過關於系統狀態向量的一階微分方程描述一個動態系統的微分方程組，而方程組的解則可以看作狀態向量在空間（相圖）中的一條軌跡。狀態空間方程與經典控制理論中的傳遞函數類似，均是用於描述動態系統的手段。相比傳遞函數，狀態空間方程有許多優點，比如對非零初始條件的系統仍能保持簡單的表示形式、很容易描述多輸入多輸出系統等。

3 全階狀態觀測器

3.1 用測量值糾正

回到最初的問題，我們需要尋找一種方法來估計狀態\(\mathbf x\)。通過狀態空間法描述小車這個動態系統后，我們便可以可以設計全階狀態觀測器來估計小車的狀態。

根據系統的輸入\(u\)，我們可以通過向量微分方程\(\dot {\mathbf x} = A\mathbf x +Bu\)計算狀態\(\mathbf x\)的變化情況，進而結合系統的初始狀態\(\mathbf x_0\)積分得到狀態\(\mathbf x\)的估計值\(\hat {\mathbf x}\)。但實際情況是初始狀態\(\mathbf x_0\)與模型本身均存在誤差，這個誤差會隨着時間不斷累積，導致估計值與真實值的誤差越來越大。When in trouble, use feedback. 通過借助反饋消除誤差，即將傳感器測量值與估計值之間的誤差反饋到觀測器中，並用誤差不斷糾正觀測器的估計值。

估計狀態向量中的位置估計值\(\hat x(t)\)為

\[\hat x(t) = \hat x_1(t) = C\hat {\mathbf x}(t)\\ \]

位置測量值為\(z(t)\)與位置估計值\(\hat x(t)\)之間誤差為

\[z(t) -C\hat {\mathbf x}(t)\\ \]

與反饋控制器誤差乘增益得到控制器輸出類似，估計誤差乘一個增益補償到觀測器\(\dot{\hat{\mathbf{x}}}(t) = A\hat{\mathbf{x}}(t) +Bu(t)\)中，有

\[\dot{\hat{\mathbf{x}}}(t) = A\hat{\mathbf{x}}(t) +Bu(t) + L(z(t) -C\hat {\mathbf x}(t))\\ \]

其中\(L\)為觀測增益。

3.2 確定觀測增益\(L\)

狀態觀測器中的\(A、 B、 C\)三個矩陣均由系統本身確定，而確定觀測增益矩陣\(L\)是設計觀測器的核心。觀測增益\(L\)的設計目的是使\(t \rightarrow \infty\) 時，\(\hat{\mathbf{x}}(t) \rightarrow \mathbf{x}(t)\)，即估計誤差隨時間衰減為0。

定義估計誤差如下

\[\mathbf e(t) = \mathbf x(t) - \mathbf {\hat x}(t)\\ \]

等號兩邊對時間\(t\)求導

\[\dot{\mathbf{e}}(t)=\dot{\mathbf{x}}(t)-\dot{\hat{\mathbf{x}}}(t)\\ \]

代入狀態觀測器公式有

\[\dot{\mathbf{e}}(t)={A} {\mathbf{x}}(t)+{B} u(t)-{A} \hat{\mathbf{x}}(t)-{B} u(t)-{L}(z(t)-{C} \hat{\mathbf{x}}(t))\\ \]

我們最初給測量值\(z(t)\)的定義如下

\[z(t)={x}(t)+{v}(t), {v}(t) \sim {N}(0, r)\\ \]

其中\(x(t)\)為\(t\)時刻溫度，\(v(t)\)為為\(t\)時刻測量噪聲。這里我們先暫時不考慮測量噪聲，即測量絕對准確

\[z(t)={x}(t)=C\mathbf x\\ \]

代入估計誤差微分方程中，可將其簡化為關於估計誤差\(\mathbf e\)形如\(\dot {\mathbf x} = A\mathbf x +B\mathbf u\)的狀態空間方程

\[\dot{\mathbf{e}}(t)={A} {\mathbf{x}}(t)-{A} \hat{\mathbf{x}}(t)-{L}C\mathbf x(t)+L{C} \hat{\mathbf{x}}(t)\\ =(A - LC)(\mathbf x(t) - \mathbf {\hat x}) \\= (A - LC)\mathbf e(t) \]

想要找到合適的增益矩陣\(L\)使估計誤差漸近穩定，需要先分析這個狀態空間描述的動態系統的動態響應。通過拉普拉斯變換將估計誤差\(\mathbf e\)的微分方程（狀態空間方程）由時域變換至\(s\)域

\[s \mathbf{E}(s)-\mathbf{e}(0)={(A - LC)} \mathbf{E}(s)\\ \]

化簡得

\[\mathbf{E}(s) = (sI - (A - LC))^{-1}\mathbf{e}(0) =\frac{(sI - (A - LC))^{*}}{|sI - (A - LC)|}\mathbf{e}(0)\\ \]

顯然，\(E(s)\)的極點恰好為矩陣\(A - LC\)的特征值。故觀測器的特征方程為

\[|sI - (A - LC)| = 0\\ \]

只要特征方程的根全部位於復平面左半平面，即特征值實部小於0，對於任意初始觀測誤差\(\mathbf{e}(0)\)，當使\(t \rightarrow \infty\) 時，均有\(\mathbf e(t) \rightarrow 0\)。因此，觀測器的設計問題便簡化為尋找合適的觀測增益\(L\)使特征方程的根全部位於復平面左半平面，即極點配置。

全狀態觀測器與全狀態反饋控制器設計思路基本一致，甚至他們在數學上是等價的。值得注意的是在狀態觀測器收斂速度一般來說應快於反饋控制器。經驗上講，觀測器極點應比控制器極點遠2~6倍。但如果傳感器噪聲很大以至於會影響控制品質，則可以相應減小觀測器的收斂速度以抑制噪聲。

根據經驗配置觀測器極點不容易得到狀態的最優估計，尤其是在測量噪聲或過程噪聲（模型誤差）較大的情況下。根據最有估計理論，觀測器增益的最佳選擇取決於測量噪聲\(v\)與過程噪聲\(w\)的大小關系，而卡爾曼濾波器就是一種根據測量噪聲\(v\)與過程噪聲\(w\)確定增益的最優觀測器，而觀測增益確定方法的不同即是卡爾曼濾波器與現代控制理論中全狀態觀測器最核心的區別（狀態觀測器通過極點配置設計增益，而卡爾曼濾波器通過結合測量噪聲\(v\)與過程噪聲\(w\)與狀態估計誤差的方差確定增益K）。