Kalman Filter (1)

(這篇文章拖了有點久，JK同學已經在催了....) 想要瞭解Kalman Filter最好還是應該先瞭解線性系統的基本概念，以及一些隨機程序的觀念。這裡先介紹一點很簡單的線性系統理論，特別是狀態空間表示法(State-Space Representation)。 一個離散時間線性系統可以狀態空間方程式表示如下： \[ \begin{array}{rcl} {\bf x}(k+1) & = & {\bf F}(k){\bf x}(k)+{\bf G}(k){\bf u}(k) \\ {\bf y}(k) & = & {\bf H}(k){\bf x}(k) \end{array} \] 這裡我們討論離散時間的系統，因為Kalman Filter利用離散時間動態系統來解釋較容易理解。上述方程式中，第一個式子稱為狀態方程式(state equation)，第二個式子稱為量測方程式(measurement equation)。${\bf y}(k)$為量測輸出，${\bf u}(k)$為系統輸入。對於一個實際的物理系統，我們量測到的輸出(亦即${\bf y}$)，可能只是狀態的一部分，例如一個慣性元件(加速規或者陀螺儀)，我們可以取得的輸出量可能只有載具的位置，角速度、加速度與速度較不容易量測，而實際上若是可以量測得到載具的速度、加速度以及角速度，我們可以對系統做更好的控制。此時，可以利用Kalman所提出來的觀測器的概念來解決這個問題。從數學上來看，載具的加速度、速度以及外力之間的關係，可以寫成一個線性的二階微分方程式：位置(position)的微分是速度、速度的微分是加速度；同樣地，在電路學中，一個RLC電路的電流或者電壓的關係，也可以寫成一個二階微分方程式：以電容儲存的電荷(charge)為例，電荷對時間的微分是電流，而電流對時間的微分又正比於電感上的電壓。從數學上的關係來看，若是電荷量可以透過適當的比例因子與載具的位置建立一個正比的關係，例如：position = k $\times$ charge，k為常數；利用常係數微分方程式的線性關係，可知載具的速度與加速度，和RLC電路中的電流以及電壓也會存在正比的關係。換句話說，我們可以從RLC電路中，得知載具的速度以及加速度的資訊(或者至少可以從"數學上"得知這些訊息)。很明顯的，在電路中量測電荷量、電流以及電壓要簡單多了，因此可以將這個RLC電路，看成載具中慣性元件的一種"觀測器"(observer or estimator)。了解觀測器的概念之後，我們接著來看在數學上如何設計觀測器。

圖 1. 觀測器架構 

從上面的說明，不難發現，在觀測器中有一部分的動態方程式，必須是從"受控體"(plant)中複製過來的；另外，受控體的輸入以及量測輸出的資訊，也必須同時提供給觀測器；因此，不妨假設觀測器的狀態空間方程式如下(參考圖1)：
\[ \begin{array}{rcl} \hat{\bf x}(k+1) & = & {\bf F}(k)\hat{\bf x}(k)+{\bf G}(k){\bf u}(k) + {\bf L}(k)({\bf y}(k)-\hat{\bf y}(k)) \\ \hat{\bf y}(k) & = & {\bf H}(k)\hat{\bf x}(k) \end{array} \] 其中$\hat{\bf x}$代表觀測器的狀態，用以估測實際的系統狀態${\bf x}$，${\bf L}$則是一個待決定的參數，稱為觀測器增益向量(observer gain vector)。定義觀測器誤差為：$\delta{\bf x}(k) := {\bf x} - \hat{\bf x}$，將前面的兩組方程式相減之後，可得到觀測誤差的動態方程式為：
\[ \delta{\bf x}(k+1) = ({\bf F}(k) - {\bf L}(k){\bf H}(k))\delta{\bf x}(k). \] 從前面介紹的觀測器的概念來看，我們當然希望觀測誤差為零，或者至少在經過足夠的時間之後，誤差為零；從上面這個動態方程式來看，$\delta{\bf x}$最後會趨近於零的充分且必要條件為：${\bf F}(k) - {\bf L}(k){\bf H}(k)$這個矩陣必須是漸近穩定的(asymptotically stable)。因此，觀測器的問題現在變成是要找到觀測器的增益向量${\bf L}$使得${\bf F}(k) - {\bf L}(k){\bf H}(k)$為漸近穩定。Kalman告訴我們，是否找的到這樣的向量，與系統的可觀測性(Observability)有關。這邊，我們暫時不討論可觀測性的問題，有興趣的讀者可以參考線性系統的書籍，大部分都有介紹。假設找的到這樣的矩陣${\bf L}$，則可確保由觀測器所得到之狀態空間估測值$\hat{\bf x}$，最後會"追"上系統真正的狀態空間變數${\bf x}$。

但是，若系統有雜訊的話，會發生甚麼問題嗎?由於雜訊多半僅能以機率的方式來描述，從數學上來看，觀測器的狀態無法真正追上受控系統的狀態，狀態空間變數的誤差$\delta{\bf x}(k)$不會趨近於零，而且因為系統有雜訊，$\delta{\bf x}(k)$會是一個隨機程序。我們知道，對一個隨機程序，最好的描述方式是用統計平均值，包括mean, variance等。既然現在無法令$\delta{\bf x}(k)$趨近於零，那在"最佳"的情況下，我們能做到甚麼程度呢？從統計的觀點來看，一個直覺地評估我們設計出來的觀測器的好壞的方式，第一個當然希望$\delta{\bf x}(k)$期望值為零(也就是說，至少可以"期望"${\bf x}$與$\hat{\bf x}$長時間下來的平均值是一樣的)，第二個可能就是希望$\delta{\bf x}(k)$每次估測出來，"跳動"不會太大，在統計學的術語上就是"標準差"(standard deviation，其平方稱為變異量，variance)。這樣應該可以稱為是"最佳"的觀測器。選擇觀測器增益向量${\bf L}$，使得估測誤差$\delta{\bf x}$的mean為零，variance為最小，就是所謂的Kalman Filter。

(待續)