卡尔曼滤波器(15) -- 多维卡尔曼滤波器(状态更新方程)

最新推荐文章于 2024-07-09 16:07:15 发布

Yemiekai

最新推荐文章于 2024-07-09 16:07:15 发布

阅读量2.3k

点赞数 1

分类专栏：卡尔曼滤波

原文链接：https://www.kalmanfilter.net/stateUpdate.html

版权

卡尔曼滤波专栏收录该内容

3 篇文章

订阅专栏

本文深入探讨了多维卡尔曼滤波器中的状态更新方程，通过矩阵形式进行展示，并提供了具体实例来说明如何根据观测数据更新状态估计。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

This blog is translated from https://www.kalmanfilter.net/default.aspx.
It’s an excellent tutorial about Kalman Filter written by Alex Becker.

这部分是本教程最短的。关于状态更新方程的详细描述已经在 “α−β−γ滤波器” 和 “一维卡尔曼滤波器” 讲过。

矩阵形式的状态更新方程： $\pmb{\hat{x}_{n,n}} = \pmb{\hat{x}_{n,n-1}} + \pmb{K_n}(\pmb{z_n} - \pmb{H} \pmb{\hat{x}_{n,n-1}})$ 其中：
$\pmb{\hat{x}_{n,n}}$ 是在时刻 $n$ 的估计系统状态向量
$\pmb{\hat{x}_{n,n-1}}$ 是在时刻 $n - 1$ 的预测系统状态向量
$\pmb{K_n}$ 是卡尔曼增益
$\pmb{z_n}$ 是测量值
$\pmb{H}$ 是观察矩阵(observation matrix)

除了矩阵形式的卡尔曼增益以外，你应该熟悉状态更新方程中的所有成分，稍后我们会推导卡尔曼增益。

你需要注意这里的维度，例如，如果状态向量有 5 个维度，而仅有 3 个纬度是可测量的（第1，第3，第5个状态）： $\pmb{x(n)} = \begin{bmatrix} x_1 \\ x_2 \\ x_3 \\ x_4 \\ x_5 \end{bmatrix} \pmb{z(n)} = \begin{bmatrix} z_1 \\ z_2 \\ z_3 \end{bmatrix}$

观察矩阵是一个 $\times 5$ 的矩阵： $\pmb{H} = \begin{bmatrix} 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 \end{bmatrix}$

更新(innovation)（ $\pmb{z_n} - \pmb{H}\pmb{\hat{x}_{n,n-1}}$ ）得到： $\pmb{z_n} - \pmb{H}\pmb{\hat{x}_{n,n-1}} = \begin{bmatrix} z_1 \\ z_2 \\ z_3 \end{bmatrix} - \begin{bmatrix} 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} \hat{x}_1 \\ \hat{x}_2 \\ \hat{x}_3 \\ \hat{x}_4 \\ \hat{x}_5 \end{bmatrix} = \begin{bmatrix} (z_1 - \hat{x}_1) \\ (z_3 - \hat{x}_3) \\ (z_5 - \hat{x}_5) \end{bmatrix}$

卡尔曼增益的维度应该为 $\times 3$

状态更新方程的维度

下面的表展示了状态更新方程变量的维度

变量（variable）	描述（Description）	维度（Dimension）
$\pmb{x}$	状态向量（state vector）	$n_x \times 1$
$\pmb{z}$	输出向量（output vector）	$n_z \times 1$
$\pmb{H}$	观察矩阵（observation matrix）	$n_z \times n_x$
$\pmb{K}$	卡尔曼增益（kalman gain）	$n_x \times n_z$