状态更新方程

本章是本教程里最短的一章。在 $α - β - γ$ 滤波器一章和一维卡尔曼滤波一章里已经对状态更新方程进行了非常详尽的阐述。

矩阵形式的状态更新方程为：

{\hat{x}}_{n, n} = {\hat{x}}_{n, n - 1} + K_{n} (z_{n} - {H \hat{x}}_{n, n - 1})

式中：

你应该对状态更新方程中的每一项——除了矩阵形式的卡尔曼增益——都很熟悉了。下一章将会推导矩阵形式的卡尔曼增益。

同时你应该多注意维度问题。假如状态向量是5维的，并且其中只有3个维度是可观测的（第1、3、5个状态）：

x_{n} = [\begin{matrix} x_{1} \\ x_{2} \\ x_{3} \\ x_{4} \\ x_{5} \end{matrix}] z_{n} = [\begin{matrix} z_{1} \\ z_{3} \\ z_{5} \end{matrix}]

那么观测矩阵应该是一个 $3 \times 5$ 的矩阵：

H = [\begin{matrix} 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 \end{matrix}]

更新量 $(z_{n} - {H \hat{x}}_{n, n - 1})$ 便是：

(z_{n} - {H \hat{x}}_{n, n - 1}) = [\begin{matrix} z_{1} \\ z_{3} \\ z_{5} \end{matrix}] - [\begin{matrix} 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 \end{matrix}] [\begin{matrix} {\hat{x}}_{1} \\ {\hat{x}}_{2} \\ {\hat{x}}_{3} \\ {\hat{x}}_{4} \\ {\hat{x}}_{5} \end{matrix}] = [\begin{matrix} (z_{1} - {\hat{x}}_{1}) \\ (z_{3} - {\hat{x}}_{3}) \\ (z_{5} - {\hat{x}}_{5}) \end{matrix}]

The Kalman Gain dimensions shall be $5 \times 3$ . 卡尔曼增益的维度应该是 $5 \times 3$ .

状态更新方程的维度