從全狀態觀測器到卡爾曼濾波器（二）
寫在開頭
在（一）中通過一個簡單的例子介紹了設計連續時間全狀態觀測器的基本思路，鏈接：https://zhuanlan.zhihu.com/p/338269917，通過數字計算平台實現狀態觀測器需要其離散時間形式，本篇將沿用（一）中的例子介紹全狀態觀測器的離散實現與MIMO推廣。
0 回顧（一）中的例子
我們現在希望用一個超聲波測距傳感器測量固定在直線軌道上的小車位置\(x\)，但這個傳感器不是完全准確的，其測量噪聲服從期望為0的正態分布
\[z(t)={x}(t)+{v}(t), {v}(t) \sim {N}(0, r)\\ \]
其中\(z(t)\)為\(t\)時刻測量值，\(x(t)\)為\(t\)時刻溫度，\(v(t)\)為為\(t\)時刻測量噪聲。實際上，狀態觀測器除了能減小測量噪聲對最終結果的影響，更重要的應用是估計不可測的狀態量，比如這個例子中的速度\(\dot x\)。
在上一篇中我們設計了連續時間形式的全狀態觀測器用於估計小車的真實位置
\[\dot{\hat{\mathbf{x}}}(t) = A\hat{\mathbf{x}}(t) +Bu(t) + L(z(t) -C\hat {\mathbf x}(t))\\ \]
其中
\[\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 L = \left[\begin{array}{c} l_1 \\ l_2 \end{array}\right]\\ \]
1 觀測器的離散化
1.1 歐拉方法
歐拉方法是一種解決數值常微分方程的最基本的一類顯型方法，以以下微分方程為例，已知
\[y^{\prime}(t)=f(t, y(t)), \quad y\left(t_{0}\right)=y_{0}\\ \]
可通過\(y\)在某點附近的線性近似求得其近似解，利用\(t_k\)時刻的數值，采用單步歐拉方法可得到\(t_{k+1} = t_k+\Delta t\)時刻的近似解
\[y_{k+1}=y_{k}+\Delta t f\left(t_{k}, y_{k}\right)\\ \]
1.2 離散化
我們通過歐拉方法得到狀態觀測器的遞推方程，即將觀測器由連續時間形式的微分方程轉為離散時間形式的差分方程
\[\hat{\mathbf{x}}_{k+1} = \hat{\mathbf{x}}_k + \Delta t\dot{\hat{\mathbf{x}}}_k\\ \]
將\(\dot{\hat{\mathbf{x}}}(t) = A\hat{\mathbf{x}}(t) +Bu(t) + L(z(t) -C\hat {\mathbf x}(t))\)代入
\[\hat{\mathbf{x}}_{k+1} = \hat{\mathbf{x}}_k + \Delta t(A\hat{\mathbf{x}}_k +Bu_k + L(z_k -C\hat{\mathbf{x}}_k))\\ \]
化簡得
\[\hat{\mathbf{x}}_{k+1} = F\hat{\mathbf{x}}_k +B^-u_k + K(z_k -C\hat{\mathbf{x}}_k)\\ \]
其中
\[F = I+\Delta t A =\left[\begin{array}{c} 1 \quad \Delta t \\ 0 \quad 1 \end{array}\right]\quad B^- = \Delta t B =\left[\begin{array}{c} 0 \\ \frac{\Delta t}{m} \end{array}\right]\quad K = \Delta t L =\left[\begin{array}{c} \Delta tl_1 \\ \Delta tl_2 \end{array}\right]\\ \]
1.3 反饋的滯后
上面的離散形式存在一個嚴重的問題，即測量值滯后於估計值一個周期，因為我們計算\(k+1\)時刻的估計值時使用的是\(k\)時刻的反饋\(z_k -C\hat{\mathbf{x}}_k\)。為解決反饋的滯后問題，需要將上面的單個差分方程拆分成兩個：預測與修正
預測以得到先驗估計\(\hat{\mathbf{x}}_{k+1|k}\)
\[\hat{\mathbf{x}}_{k+1|k} = F\hat{\mathbf{x}}_{k|k} +B^-u_k\\ \]
測量值參與修正以得到后驗估計\(\hat{\mathbf{x}}_{k+1|k+1}\)
\[\hat{\mathbf{x}}_{k+1|k+1} = \hat{\mathbf{x}}_{k+1|k} +K(z_{k+1} - C\hat{\mathbf{x}}_{k+1|k})\\ \]
這樣一來，我們就得到了全狀態觀測器完整的離散形式，即先預測、后修正。
1.4 步長的選取
離散狀態觀測器在數字計算平台中的更新頻率即步長的選取不應過長也不應過短。
步長過短會增大單位時間內計算設備的計算量，在計算設備算力允許的情況下一般不會有太惡劣的影響。
但步長過長則會導致模型誤差增大從而嚴重影響預測精度。以勻速模型為例，步長越長則周期內系統加速度的值或速度的變化就越可能不符合模型的勻速假設。若測量值更新緩慢，如視覺目標跟蹤中預測目標位置設置ROI的場合，目標位置的測量更新頻率受制於攝像頭幀率與算法計算速度。可通過提高觀測器階數，即由勻速模型擴展為勻加速模型以提高預測精度。
2 直觀理解
對初學者而言，盡管明白狀態觀測器的數學原理，其矩陣的表達形式也難免會讓初學者感到雲里霧里。接下來我們通過將上面例子的狀態觀測器中矩陣與向量乘開，稍加變形得到更直觀的表達形式，以觀察這個觀測器是如何得到位置與速度估計的。
2.1 計算標量代數式
簡單起見，我們假設觀測期間系統無輸入，即\(u=0\)，有
\[\hat{\mathbf{x}}_{k+1|k} = F\hat{\mathbf{x}}_k\\ \]
\[\hat{\mathbf{x}}_{k+1|k+1} = \hat{\mathbf{x}}_{k+1|k} +K(z_{k+1}-C\hat{\mathbf{x}}_{k+1|k})\\ \]
將矩陣\(F\)代入預測
\[\hat{\mathbf{x}}_{k+1|k} = \left[\begin{array}{c} \hat x_{k+1|k} \\ \hat {\dot x}_{k+1|k} \end{array}\right] = \left[\begin{array}{c} 1 \quad \Delta t \\ 0 \quad 1 \end{array}\right] \left[\begin{array}{c} \hat x_{k|k} \\ \hat {\dot x}_{k|k} \end{array}\right]\\ \]
得到兩個狀態量的先驗估計
\[\hat x_{k+1|k} =\hat x_{k|k} + \Delta t\hat {\dot x}_{k|k}\\ \hat {\dot x}_{k+1|k} = \hat {\dot x}_{k|k}\\ \]
設\(k_1 = \Delta tl_1,k_2 = \Delta tl_2\)，將矩陣\(K,C\)代入修正
\[\hat{\mathbf{x}}_{k+1|k} = \left[\begin{array}{c} \hat x_{k+1|k+1} \\ \hat {\dot x}_{k+1|k+1} \end{array}\right] = \left[\begin{array}{c} \hat x_{k+1|k} \\ \hat {\dot x}_{k+1|k} \end{array}\right]+ \left[\begin{array}{c} k_1 \\ k_2 \end{array}\right](z_{k+1} - [1 \quad0] \left[\begin{array}{c} \hat x_{k+1|k} \\ \hat {\dot x}_{k+1|k} \end{array}\right] )\\ \]
得到兩個狀態量的后驗估計
\[\hat x_{k+1|k+1} =\hat x_{k+1|k} + k_1(z_{k+1} - \hat x_{k+1|k})\\ \hat {\dot x}_{k+1|k+1} = \hat {\dot x}_{k+1|k} - k_2(z_{k+1} - \hat x_{k+1|k})\\ \]
2.2 位置估計
我們觀察位置的估計\(\hat x_{k+1|k+1}\)，對其變形，有
\[\hat x_{k+1|k+1} =(1-k_1)\hat x_{k+1|k} + k_1z_{k+1}\\ \]
我們發現這是一個典型的互補濾波，對根據模型計算的預測值與通過傳感器測得的測量值進行互補濾波
2.3 速度估計
再觀察速度的估計值\(\hat {\dot x}_{k+1|k+1}\)，我們發現無法像位置估計表達式那樣直接變形，因為等式右側沒有任何同類項。因此我們將上面得到的先驗估計表達式\(\hat x_{k+1|k} =\hat x_{k|k} + \Delta t\hat {\dot x}_{k|k}， \hat {\dot x}_{k+1|k} = \hat {\dot x}_{k|k}\)代入，有
\[\hat {\dot x}_{k+1|k+1} = \hat {\dot x}_{k|k} - k_2(z_{k+1} - \hat x_{k|k} + \Delta t\hat {\dot x}_{k|k})\\ \]
合並同類項得
\[\hat {\dot x}_{k+1|k+1} = (1-\Delta tk_2)\hat {\dot x}_{k|k} - k_2(z_{k+1} - \hat x_{k|k})\\ \]
合並后我們並未得到如位置估計值\(\hat x_{k+1|k+1} =(1-k_1)\hat x_{k+1|k} + k_1z_{k+1}\)那樣顯然的互補形式。我們回顧離散時間域中計算信號一階導數的常用方法——后向差分：
\[\dot x_k = \frac{x_k - x_{k-1}}{\Delta t}\\ \]
我們發現式\(\hat {\dot x}_{k+1|k+1} = (1-\Delta tk_2)\hat {\dot x}_{k|k} - k_2(z_{k+1} - \hat x_{k|k})\)中\(z_{k+1} - \hat x_{k|k}\)恰好具有后向差分分子的形式，故做如下變形
\[z_{k+1} - \hat x_{k|k} = \Delta t\frac {z_{k+1} - \hat x_{k|k}}{\Delta t}\\ \]
代入速度估計表達式中，有
\[\hat {\dot x}_{k+1|k+1} = (1-\Delta tk_2)\hat {\dot x}_{k|k} - \Delta tk_2\frac {z_{k+1} - \hat x_{k|k}}{\Delta t}\\ \]
得到信號一階差分形式的同時，也恰好將等式右側配置為了互補形式，即對信號后向差分得到的一階導做一階低通濾波。
3 擴展到MIMO系統
通過將狀態觀測器展開成標量代數表達式，我們發現狀態觀測器並沒有想象中的那么玄學。有了對狀態觀測器原理的一定直觀理解，我們將上面一維的例子擴展到二維，即通過兩個傳感器分別測量小車在平面中的\(x,y\)位置，系統輸入為分別施加在小車沿\(x,y\)方向上的力，設計狀態觀測器以獲取小車在平面中的坐標與速度。
3.1 觀測器動態
回顧一維問題中的狀態向量\(\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]\\ \]
擴展到二維直接添加\(y,\dot y\)即可
\[\mathbf x=\left[\begin{array}{c} x_1 \\ x_2 \\ x_3 \\ x_4 \end{array}\right]=\left[\begin{array}{c} x \\ y \\ \dot x\\ \dot y \end{array}\right]\\ \]
根據運動學公式，有
\[\left[\begin{array}{c} \hat x_{k+1|k} \\ \hat y_{k+1|k} \\ \hat {\dot x}_{k+1|k}\\ \hat {\dot y}_{k+1|k}\\ \end{array}\right] = \left[\begin{array}{c} 1 \quad 0 \quad \Delta t \quad0\\ 0 \quad 1 \quad 0\quad \Delta t \\ 0 \quad 0 \quad 1\quad 0\\ 0 \quad 0 \quad 0\quad 1 \end{array}\right] \left[\begin{array}{c} \hat x_{k|k} \\ \hat y_{k|k} \\ \hat {\dot x}_{k|k}\\ \hat {\dot y}_{k|k}\\ \end{array}\right]\\ \]
則狀態轉移矩陣\(F\)為
\[F = \left[\begin{array}{c} 1 \quad 0 \quad \Delta t \quad0\\ 0 \quad 1 \quad 0\quad \Delta t \\ 0 \quad 0 \quad 1\quad 0\\ 0 \quad 0 \quad 0\quad 1 \end{array}\right]\\ \]
根據牛頓第二定律，有
\[\hat {\dot x}_{k+1|k} = \hat {\dot x}_{k|k} + \Delta t \frac{F_x}{m}\\ \hat {\dot y}_{k+1|k} = \hat {\dot y}_{k|k} + \Delta t \frac{F_y}{m}\\ \]
設系統輸入向量為
\[\mathbf u =\left[\begin{array}{c} F_x \\ F_y \end{array}\right]\\ \]
則控制矩陣\(B\)為
\[B =\left[\begin{array}{c} 0 \quad 0\\ 0 \quad 0\\ \frac{\Delta t}{m} \quad 0\\ 0 \quad \frac{\Delta t}{m} \end{array}\right]\\ \]
考慮預測方程的標准形式
\[\hat{\mathbf{x}}_{k+1|k} = F\hat{\mathbf{x}}_{k|k} +B\mathbf u_k\\ \]
將有關向量和矩陣代入，得到離散時間狀態觀測器的預測方程
\[\hat{\mathbf{x}}_{k+1|k} = \left[\begin{array}{c} \hat x_{k+1|k} \\ \hat y_{k+1|k} \\ \hat {\dot x}_{k+1|k}\\ \hat {\dot y}_{k+1|k}\\ \end{array}\right] = \left[\begin{array}{c} 1 \quad 0 \quad \Delta t \quad0\\ 0 \quad 1 \quad 0\quad \Delta t \\ 0 \quad 0 \quad 1\quad 0\\ 0 \quad 0 \quad 0\quad 1 \end{array}\right] \left[\begin{array}{c} \hat x_{k|k} \\ \hat y_{k|k} \\ \hat {\dot x}_{k|k}\\ \hat {\dot y}_{k|k}\\ \end{array}\right] + \left[\begin{array}{c} 0 \quad 0\\ 0 \quad 0\\ \frac{\Delta t}{m} \quad 0\\ 0 \quad \frac{\Delta t}{m} \end{array}\right] \left[\begin{array}{c} F_x \\ F_y \end{array}\right]\\ \]
3.2 觀測器輸出
考慮到我們有兩個傳感器分別測量小車在平面中的\(x,y\)位置，故設觀測器輸出為
\[\mathbf {\hat y_k} = \left[\begin{array}{c} \hat x_{k|k} \\ \hat y_{k|k} \\ \end{array}\right]= \left[\begin{array}{c} 1 \quad 0 \quad 0 \quad 0\\ 0 \quad 1 \quad 0\quad 0 \\ \end{array}\right]\left[\begin{array}{c} \hat x_{k|k} \\ \hat y_{k|k} \\ \hat {\dot x}_{k|k}\\ \hat {\dot y}_{k|k}\\ \end{array}\right]\\ \]
即輸出矩陣\(C\)為
\[C = \left[\begin{array}{c} 1 \quad 0 \quad 0 \quad 0\\ 0 \quad 1 \quad 0\quad 0 \\ \end{array}\right]\\ \]
3.3 完整形式
將傳感器測量值加入修正過程即可得到狀態觀測器的完整形式，這里我們仍不考慮噪聲，根據觀測器輸出的定義\(\mathbf {\hat y_k} = [\hat x_{k|k} \quad \hat y_{k|k} ]^T\)，設測量向量\(z_k\)為
\[z_k = \left[\begin{array}{c} x_{k} \\ y_{k} \\ \end{array}\right]\\ \]
考慮修正方程標准形式
\[\hat{\mathbf{x}}_{k+1|k+1} = \hat{\mathbf{x}}_{k+1|k} +K(z_{k+1} - C\hat{\mathbf{x}}_{k+1|k})\\ \]
將有關向量和矩陣代入，得到離散時間狀態觀測器的修正方程
\[\hat{\mathbf{x}}_{k+1|k+1} = \left[\begin{array}{c} \hat x_{k+1|k+1} \\ \hat y_{k+1|k+1} \\ \hat {\dot x}_{k+1|k+1}\\ \hat {\dot y}_{k+1|k+1}\\ \end{array}\right] = \left[\begin{array}{c} \hat x_{k+1|k} \\ \hat y_{k+1|k} \\ \hat {\dot x}_{k+1|k}\\ \hat {\dot y}_{k+1|k}\\ \end{array}\right] + \left[\begin{array}{c} k_{11} \quad k_{12}\\ k_{21} \quad k_{22}\\ k_{31} \quad k_{32}\\ k_{41} \quad k_{42} \end{array}\right] (\left[\begin{array}{c} x_{k} \\ y_{k} \\ \end{array}\right] - \left[\begin{array}{c} 1 \quad 0 \quad 0 \quad 0\\ 0 \quad 1 \quad 0\quad 0 \\ \end{array}\right]\left[\begin{array}{c} \hat x_{k+1|k} \\ \hat y_{k+1|k} \\ \hat {\dot x}_{k+1|k}\\ \hat {\dot y}_{k+1|k}\\ \end{array}\right])\\ \]
最后再通過極點配置確定\(K\)矩陣的值即可完成狀態觀測器的設計。
