Kalman Filter (2)

接下來我們來看看當系統有雜訊時，如何設計觀測器。考慮如下所示之離散時間動態系統： \[ \begin{array}{rcl} {\bf x}(k+1) & = & {\bf F}(k){\bf x}(k)+{\bf G}(k){\bf u}(k)+{\bf w}(k) \\ {\bf y}(k) & = & {\bf H}(k){\bf x}(k)+{\bf v}(k) \end{array} \] 其中，${\bf x},{\bf y},{\bf u},{\bf F},{\bf G},{\bf H}$與前一篇文章中完全相同(參考Kalman Filter (1))，${\bf w}(k)$與${\bf v}(k)$為系統的雜訊干擾，在工程的應用上，一般可假設${\bf w}$與${\bf v}$為高斯雜訊。雜訊在工程上是以隨機程序來表示，可以想成是一個時間的函數，只是它的函數值無法確定，僅能用機率的方式來表示。

依據上一篇的概念，我們同樣試著設計一個觀測器如下：
\[ \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} \] (備註：此處可以先令${\bf u}$為零，得到的結果是一樣的)其中$\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}(k)$裡面，換句話說$\delta{\bf x}(k)$是一個隨機程序。接下來的問題一樣就是要選擇${\bf L}$使得$\delta{\bf x}$可以收斂，但是因為$\delta{\bf x}$是隨機程序，所以既使${\bf F}(k) - {\bf L}(k){\bf H}(k)$是穩定矩陣，也無法保證$\delta{\bf x}$會收斂(事實上在雜訊存在的情況下，$\delta{\bf x}$不可能收斂到零)，是否有其他的性能指標，可以用來判斷觀測器的優劣。在這裡，Kalman想到的辦法就是選擇${\bf L}(k)$使得$\delta{\bf x}(k)$的協方差(covariance)為最小，也就是要選擇${\bf L}(k)$，使得\[ trace({\bf P}_k) = trace\left( E\left[\delta{\bf x}(k)\delta{\bf x}^T(k)\right]\right)\]
為最小；其中$trace(\cdot)$為矩陣的跡數，$E[\cdot]$表示期望值。將上面這個方程式對${\bf L}(k)$微分，並令微分結果為零，即可求出此最佳增益值，一般也稱為Kalman gain。這邊我們先暫時避開數學的推導，假設求得的增益值為$\hat{\bf L}(k)$，帶入觀測器之中得到的就是所謂的Kalman濾波器，\[ \begin{array}{rcl} \hat{\bf x}(k+1) & = & {\bf F}(k)\hat{\bf x}(k)+{\bf G}(k){\bf u}(k) + \hat{\bf L}(k)({\bf y}(k)-\hat{\bf y}(k)) \\ \hat{\bf y}(k) & = & {\bf H}(k)\hat{\bf x}(k) \end{array} \]也可將上述方程式改寫為：\[ \begin{array}{rcl} \hat{\bf x}(k+1) & = & {\bf F}(k)\hat{\bf x}(k)+ \hat{\bf L}(k){\bf e}(k)+{\bf G}(k){\bf u}(k)  \\ {\bf e}(k) & = & {\bf y}(k) - {\bf H}(k)\hat{\bf x}(k) \end{array} \] 其中，${\bf e}(k)$就是在估測理論中，非常有名的"innovation process"。

這裡介紹的是最基本Kalman濾波器(KF)的概念，這幾年來衍伸出各種不同的KF，例如AKF, EKF, UKF, 大KF, 小KF, 不大不小中KF等，雖然形式不同，但是本質上都是在設計一個濾波器，使得估測誤差$\delta{\bf x}(k)$的協方差為最小。




(待續)