卡爾曼濾波(2)

在今天將主要討論 3 個方面

  • 數據融合(Data Fusion)
  • 協方差矩陣(Covariance Matrix)
  • 狀態空間表示(State Space Representation)

數據融合

假設我們手上有兩個測量儀器,他們都存在誤差而且測量值都服從正態分布,用協方差來表示他們精度。
\begin{aligned} z_1 = 30g & \sigma_1 = 2g\\ z_2 = 32g & \sigma_2 = 4g\\ \end{aligned}
都服從正態分布,在標準差之間覆蓋率 68.4\%,現在有了分別用這兩個測量儀器進行測量得到了兩個測量結果,然后我們需要根據測量結果對真實值進行估計。首先我們看 z_1 的測量精度要比 z_2 高,所以我們估計值應該更靠近 z_1。這樣就會用到上一次分享,

估算真實值\hat{z} = z_1 + K(z_2 - z_1) K \in [0,1]
這里 K 取值在 0 到 1 之間,接下來看一看 K 在取 0 或 1 時估計值的取值情況。
\begin{aligned} K = 0 & \hat{z} = z_1\\ K = 1 & \hat{z} = z_2\\ \end{aligned}

我們目標就是求解 K 使得 \sigma_{\hat{z}} (方差)最小,從而得到 Var(\hat{z}) 最小。
\begin{aligned} \sigma_{\hat{z}} = Var(z_1 + K(z_2 - z_1))\\ = Var(z_1 - Kz_1 + Kz_2)\\ = Var((1-K)z_1 + Kz_2) \\ = Var((1 - K)z_1) + Var(Kz_2)\\ = (1 - K)^2 Var(z_1) + K^2Var(z_2)\\ = (1 - K)^2\sigma_1^2 + K^2\sigma_2^2 \end{aligned}

上一系列公式是我們對方差求解過程的推導,這里來一行一行地給大家解釋,首先我們寫出\hat{z}的方差,然后將K(z_2 - z_1) K 乘進去寫成第二行形式,接下來我們將公式寫成Var((1 - K)z_1) + Var(Kz_2) 這種形式后因為z_1z_2是相互獨立,根據方差的性質又可以寫成Var((1 - K)z_1)Var(Kz_2) 取和的形式。最后將常數提取出來得到 (1 - K)^2 Var(z_1) + K^2Var(z_2)

然后大家都知道求極值問題就是求導的問題,我們通過求導數可以求極限問題。

\frac{d \sigma_{\hat{z}}^2}{d K} = 0
\begin{aligned} -2(1 - K)\sigma_1^2 + 2 K \sigma_2^2 = 0\\ - \sigma_1^2 + K\sigma_1^2 + K\sigma_2^2 = 0\\ K = \frac{\sigma_1^2}{\sigma_1^2 + \sigma_2^2} = \frac{2^2}{2^2 + 4^2} = \frac{4}{4 + 16} = 0.2 \end{aligned}

\hat{z} = z_1 + K(z_2 - z_1) = 30 + 0.2 (32 - 30) = 30.4
\sigma_{\hat{z}}^2 = (1 -0.2)^22^2 + 0.2^24^2 = 3.2
\sigma_{\hat{z}}^2 = 1.79

這就是數據融合的過程

協方差矩陣

協方差在一個矩陣中表現出來,變量間的聯動關系

球員 身高 體重 年齡
瓦爾迪 179 74 33
奧巴梅揚 187 80 31
薩拉赫 175 71 28

\sigma_x^2 = \frac{1}{3}((height - average \,height))
\sigma_x\sigma_y = \frac{1}{3}((height - average\,height)(weight - average \, height)) = \sigma_y \sigma_x
如果得到正值表示這兩個變量是正相關,也就是這兩個變量變化是一致的,反之亦然。

P = \begin{bmatrix} \sigma_x^2 & \sigma_x\sigma_y & \sigma_x\sigma_z \\ \sigma_y\sigma_x & \sigma_y^2 & \sigma_y\sigma_z \\ \sigma_z\sigma_x & \sigma_z\sigma_y & \sigma_z^2 \\ \end{bmatrix}
求解過渡矩陣
a = \begin{bmatrix} x_1 & y_1 & z_1\\ x_2 & y_2 & z_2\\ x_3 & y_3 & z_3\\ \end{bmatrix} - \frac{1}{3} \begin{bmatrix} 1 & 1 & 1\\ 1 & 1 & 1\\ 1 & 1 & 1 \end{bmatrix} \begin{bmatrix} x_1 & y_1 & z_1\\ x_2 & y_2 & z_2\\ x_3 & y_3 & z_3\\ \end{bmatrix}

P = \frac{1}{3}a^Ta

狀態空間

狀態空間是一個完整的控制體系,現在控制理論就是以狀態空間方程為基礎。彈簧震動阻尼系統,質量 m 施加的力是 F 移動位移是 x 彈簧系數是 k ,B 是阻尼系數。
接下來寫出彈簧阻尼系統的動態方程表達式
m\ddot{x} + B\dot{x} + Kx = F(=u)

  • 其中 F 是系統輸入,隨后用小寫 u 來表示

現在我們就要把這個方程化成狀態空間的表達形式,先定義狀態變量。
\begin{aligned} x_1 = x\\ x_2 = \dot{x} \end{aligned}
這樣就得到下面,其中\dot{x_2} 可以通過上面動態方程進行求解表示
\begin{aligned} \dot{x_1} = x_2\\ \dot{x_2} = \ddot{x} = \frac{1}{m}u - \frac{B}{m}\dot{x} - \frac{K}{m}x = \frac{1}{m}u - \frac{B}{m}x_2 - \frac{K}{m}x_1 \end{aligned}

現在就用 2 個 1 階微分方程表示了 x_1x_2
如果還有兩個狀態變量的測量量,可以用 z_1z_2 分別表示x_1x_2 的測量量。
\begin{aligned} z_1 = x = x_1 (position)\\ z_2 = \dot{x} = x_2 (speed)\\ \dot{x_1} = x_2\\ \dot{x_2} = \frac{1}{m}u - \frac{B}{m}x_2 - \frac{K}{m}x_1 \end{aligned}

接下來我們用矩陣的形式將上面狀態變量和測量變量表示出來就得到

\begin{bmatrix} \dot{x_1}\\ \dot{x_2} \end{bmatrix} = \begin{bmatrix} 0 & 1\\ \frac{k}{m} & -\frac{B}{m} \end{bmatrix}\begin{bmatrix} x_1\\ x_2 \end{bmatrix} + \begin{bmatrix} 0\\ \frac{1}{m} \end{bmatrix} u

\begin{bmatrix} z_1\\ z_2 \end{bmatrix} = \begin{bmatrix} 1 & 0\\ 0 & 1 \end{bmatrix} \begin{bmatrix} x_1\\ x_2 \end{bmatrix}
我們用矩陣形式表達出上面方程自然而然就聯想到空間問題,這就是用狀態空間來表示上面問題。

\begin{aligned} X_k = AX_{k-1} + BU_k \\ Z_k = HX_k \end{aligned}
這里的下標 k 表示狀態變量對應的時刻,也就是表示采樣時間。
真實情況充滿了不缺定性,所以需要引入w_{k-1}表示過程噪音,而v_k 表示測量噪音。

\begin{aligned} X_k = AX_{k-1} + Bu_k + w_{k-1}\\ Z_k = HX_k + v_k \end{aligned}

在模型也準確以及測量也不準確情況下如何估計出\hat{X_k} 這就是卡爾曼濾波器的用途,就是根據不準確的模型和不準確的測量值來估計出一個相對準確的估計值。

?著作權歸作者所有,轉載或內容合作請聯系作者
平臺聲明:文章內容(如有圖片或視頻亦包括在內)由作者上傳并發布,文章內容僅代表作者本人觀點,簡書系信息發布平臺,僅提供信息存儲服務。