狀態估計和KalmanFilter公式的推導與應用

狀態估計的概率解釋

運動和觀測方程：

\[\left\lbrace \begin{array}{l} x_k = f(x_{k_1}, u_k) + w_k \\ z_k = h(y_j, x_k) + v_{k,j} \end{array} \right. \qquad {k = 1,\dots,N, j = 1,\dots,M} \tag{1.1} \]

其中，各個參數的含義如下：

  • \(x_k\) ：機器人的位姿。

  • \(u_k\) ：系統在k時刻的輸入量。

  • \(w_k\)：位姿變化的隨機雜訊。

  • \(z_k\) ：系統的觀測值，感測器採集的觀測數據。

  • \(y_j\) ：路標，或者說是觀測點。

  • \(v_{k,j}\)：觀測過程中的隨機雜訊。

我們的目標則是利用系統在k時刻的輸入量\(u_k\)和系統的觀測量\(z_k\)，估計機器的位姿\(x_k\)和路標點\(y_j\)的概率分布。

在比較常見且合理的情況下，我們可以假設狀態量和雜訊項服從高斯分布——這意味這我們在程式中只需要存儲他們的均值和協方差矩陣即可。均值可以看作變數的最最優估計，協方差則可以度量變數的不確定性。

由於位姿\(x_k\)和路標點\(y_j\)都是需要我們估計的變數，這裡我們改變符號的意義。令\(x_k\)為k時刻所有的未知量，記作：

\[x_k \overset{\text{def}}{=} \{x_k, y_1,\dots,y_m\} \tag{1.2} \]

根據上述(1.1)和(1.2)可以將運動方程和觀測方程寫成如下形式：

\[\left\lbrace \begin{array}{l} x_k = f(x_{k_1}, u_k) + w_k \\ z_k = h(x_k) + v_{k,j} \end{array} \right. \qquad {k = 1,\dots,N} \tag{1.3} \]

現在考慮第k時刻的情況，我們希望使用過去0到時刻的數據來估計現在的狀態分布：

\[P(x_k|x_0,u_{1:k}, z_{1:k}) \tag{1.4} \]

根據貝葉斯公式，可以得到如下公式：

\[P(x_k|x_0,u_{1:k}, z_{1:k}) \propto P(z_k|x_k) P(x_k|x_0,u_{1:k}, z_{1:k-1}) \tag{1.5} \]

這裡的第一項稱為似然，第二項稱為先驗。似然由觀測方程給定，而先驗部分，\(x_k\)是基於過去所有狀態估計而來的。至少，它會受到\(x_{k-1}\)的影響，於是我們以\(x_{k-1}\)時刻為條件概率展開：

\[P(x_k|x_0,u_{1:k}, z_{1:k-1})= \int P(x_k|x_{k-1},x_0, u_{1:k}, z_{1:k-1}) P(x_{k-1}|x_0,u_{1:k}, z_{1:k-1}) dx_{k-1} \tag{1.6} \]

對於後續的操作，有很多的方法。

  • 其中一種方法就是假設馬爾可夫性。

    一階馬爾可夫性： k時刻的狀態只和k-1時刻的狀態有關，而與再之前的無關。

    如果這樣假設，我們得到的是以擴展卡爾曼濾波（EKF）為代表的濾波器方法。

  • 另一種方法是依然考慮k時刻和之前所有狀態的關係，此時得到的是非線性優化為主體的優化框架。

在這裡，我們先了解卡爾曼濾波的原理和應用。

線性系統和卡爾曼濾波

根據上文，我們假設了這個系統符合馬爾可夫性，我們可以對公式(1.6)做出一些簡化。

  • 公式右側第一部分可以簡化成如下形式：

    \[P(x_k|x_{k-1},x_0, u_{1:k}, z_{1:k-1}) = P(x_k|x_{k-1}, u_{1:k}) \tag{2.1} \]

  • 公式右側第二部分：(已知k時刻只和k-1時刻的狀態相關)

    \[P(x_{k-1}|x_0,u_{1:k}, z_{1:k-1}) = P(x_{k-1}|x_0,u_{1:k-1}, z_{1:k-1}) \tag{2.2} \]

觀察上述公式，我們可以知道，我們實際上在做「如何把k-1時刻的狀態分布推導至k時刻」這一件事請。

我們假設狀態量服從高斯分布，從最簡單的線性高斯系統開始，得到如下公式：

\[\left\lbrace \begin{array}{l} x_k = A_kx_{k-1} + u_k + w_k \\ z_k = C_kx_k + v_{k,j} \end{array} \right. \qquad {k = 1,\dots,N} \tag{2.3} \]

假設所有的狀態和雜訊都符合高斯分布，這裡的雜訊可以記作：（這裡省略了R和Q的下標）

\[w_k \sim N(0, R) \quad v_k \sim N(0, Q) \tag{2.4} \]

利用馬爾可夫性，假設我們已知k-1時刻的狀態，也就是k-1時刻的後驗狀態估計\(\hat{x}_{k-1}\)及其協方差\(\hat{P}_{k-1}\)，現在要根據k時刻的輸入，確認\(x_k\)的後驗。

這裡我們使用\(\hat{x}_{k}\)表示後驗分布，使用\(\tilde{x}_k\)表示先驗分布。

卡爾曼濾波第一步： 通過運動方程確認\(x_k\)的先驗分布。這一步是線性的，高斯分布的線性變換依然是高斯分布，所以可以得到如下公式：

\[P(x_k|x_{k-1},x_0, u_{1:k}, z_{1:k-1}) = N(A_k\hat{x}_{k-1} + u_k, A_k\hat{P}_{k-1}A^T_k + R) \tag{2.5} \]

這裡協方差的推導可以參考《概率論與數理統計》的P112頁，關於n維正態隨機變數的協方差矩陣。

這一步稱為預測。可以記作：

\[\tilde{x}_k = A_k\hat{x}_{k-1} + u_k, \quad \tilde{P}_k = A_k\hat{P}_{k-1}A^T_k + R \tag{2.6} \]

卡爾曼濾波第二步： 根據觀測方程，我們可以計算莫格時刻應該產生怎樣的觀測數據：

\[P(z_k| x_k) = N(C_kx_k, Q) \tag{2.7} \]

我們已經假設狀態量符合高斯分布，根據貝葉斯公式，可以得到如下公式：

\[N(\hat{x}_k, \hat{P}_k) = \eta N(C_kx_k, Q) \cdot N(\tilde{x}_k, \tilde{P}_k) \tag{2.8} \]

兩側都是高斯分布，我們帶入高斯分布的公式，只需要保證指數部分相同，無需理會前面的因子部分。可以得到如下公式：

\[{(x_k - \hat{x}_k)}^T {\hat{P}^{-1}}_k (x_k - \hat{x}_k) = {(z_k -C_kx_k)}^T \hat{Q}^{-1}_k (z_k - C_kx_k) + {(x_k - \tilde{x}_k)^T} {\tilde{P}^{-1}_{k}} (x_k - \tilde{x}_k) \tag{2.9} \]

我們需要根據上述這個公式推導出\(\hat{x}_k\)和\(\hat{P}_k\)。

這裡是通過係數相等進行了化簡，我在這裡簡寫一下：

\[\left\lbrace \begin{array}{l} \hat{P}^{-1}_k = C^T_kQ^{-1}C_k + \tilde{P}^{-1}_k \\ \\ -2\hat{x}^T_k\hat{P}^{-1}_kx_k = -2z^T_k Q^{-1}C_kx_k - 2\tilde{x}^T_k\tilde{P}^{-1}_kx_k \end{array} \right. \tag{2.10} \]

我們記作\(K = \hat{P}_kC^T_kQ^{-1}\)得到如下公式：

\[\left\lbrace \begin{array}{l} \hat{P}_k =(I - KC_k) \tilde{P}_k \\ \\ \hat{x}_k = \tilde{x}_k + K(z_k - C_k \hat{x}_k) \end{array} \right. \tag{2.11} \]

這個部稱為更新。

總結kalmanFilter的用法

  1. 預測

    \[\tilde{x}_k = A_k\hat{x}_{k-1} + u_k, \quad \tilde{P}_k = A_k\hat{P}_{k-1}A^T_k + R \tag{2.12} \]

  2. 更新：先計算K（卡爾曼增益）， 然後更新後驗概率的分布。

    \[\begin{array}{l} K = \hat{P}_kC^T_k(C_k\tilde{P}_kC^T_k + Q_k)^{-1} \\ \\ \hat{P}_k =(I - KC_k) \tilde{P}_k \\ \\ \hat{x}_k = \tilde{x}_k + K(z_k - C_k \hat{x}_k) \end{array} \tag{2.13} \]

Tags:

VirMach 便宜 VPS