绘制卡尔曼滤波运行流程图

绘制卡尔曼滤波运行流程图

flyfish

2024-06-28
2024-07-01 修正错别字
绘图软件是LibreOffice Draw,里面的数学公式用的是TexMaths插件，语法是LaTeX

下面的文章都是为了理解卡尔曼滤波
积分的可视化

详细的介绍匀加速运动的物理方程是如何转化为卡尔曼滤波的状态空间模型的

（目标跟踪算法）中的状态向量与状态转移矩阵

（目标跟踪算法）中卡尔曼滤波器中的更新

（目标跟踪算法）中卡尔曼增益的理解

（目标跟踪算法）中的卡尔曼滤波 - 看了就会的状态转移矩阵

（目标跟踪算法）中的初始化卡尔曼滤波器的状态向量和协方差矩阵

（目标跟踪算法）中卡尔曼滤波器中的预测

（目标跟踪算法）中自由度决定卡方分布的形状

（目标跟踪算法）中的解三角方程计算标准化残差（解线性方程组）

（目标跟踪算法）中的计算观测值与状态估计的马氏距离

（目标跟踪算法）中的马氏距离详解（很详细）

（目标跟踪算法）中 可以设置阈值进行异常检测或目标跟踪的原因

（目标跟踪算法）中的数值表格与调参的关系

从最小二乘法的角度来理解卡尔曼滤波（1）

从最小二乘法的角度来理解卡尔曼滤波（2）

（目标跟踪算法）卡尔曼滤波中的贝叶斯定理

Determinant 一个翻译很不友好的名字 行列式 det(A)

（目标跟踪算法） 卡尔曼滤波 状态向量是如何映射到观测向量（测量向量）的即观测矩阵的构建方式

（目标跟踪算法） 卡尔曼滤波 状态空间模型的理解

包含公式

图上卡尔曼滤波的五个主要公式的说明

卡尔曼滤波器由两个阶段组成：预测阶段和更新阶段。每个阶段有一组方程。

1. 初始化

在第一个时刻，需要初始化状态向量和误差协方差矩阵：

• 状态向量（State Vector） ${\mathbf{x}}_0$：对系统初始状态的估计。
• 误差协方差矩阵（Error Covariance Matrix） ${\mathbf{P}}_0$：初始状态估计的不确定性。

2. 预测步骤（Prediction Step）

预测系统在下一个时刻的状态和误差协方差矩阵。

• 预测状态（Predicted State） ${\hat{\mathbf{x}}}_{k|k-1}$：${\hat{\mathbf{x}}}_{k|k-1}={\mathbf{A}}_{k-1}{\hat{\mathbf{x}}}_{k-1|k-1}+{\mathbf{B}}_{k-1}{\mathbf{u}}_{k-1}$其中，${\mathbf{A}}_{k-1}$ 是状态转移矩阵，${\mathbf{B}}_{k-1}$ 是控制输入矩阵，${\mathbf{u}}_{k-1}$ 是控制输入。
• 预测误差协方差矩阵（Predicted Error Covariance Matrix） ${\mathbf{P}}_{k|k-1}$：${\mathbf{P}}_{k|k-1}={\mathbf{A}}_{k-1}{\mathbf{P}}_{k-1|k-1}{\mathbf{A}}_{k-1}^T+{\mathbf{Q}}_{k-1}$其中，${\mathbf{Q}}_{k-1}$ 是过程噪声协方差矩阵。

3. 更新步骤（Update Step）

利用新测量数据更新预测的状态和误差协方差矩阵。

• 卡尔曼增益（Kalman Gain） ${\mathbf{K}}_k$：${\mathbf{K}}_k={\mathbf{P}}_{k|k-1}{\mathbf{H}}_k^T({\mathbf{H}}_k{\mathbf{P}}_{k|k-1}{\mathbf{H}}_k^T+{\mathbf{R}}_k{)}^{-1}$其中，${\mathbf{H}}_k$ 是观测矩阵，${\mathbf{R}}_k$ 是观测噪声协方差矩阵。
• 更新状态（Updated State） ${\hat{\mathbf{x}}}_{k|k}$：${\hat{\mathbf{x}}}_{k|k}={\hat{\mathbf{x}}}_{k|k-1}+{\mathbf{K}}_k({\mathbf{z}}_k-{\mathbf{H}}_k{\hat{\mathbf{x}}}_{k|k-1})$其中，${\mathbf{z}}_k$ 是测量向量。
• 更新误差协方差矩阵（Updated Error Covariance Matrix） ${\mathbf{P}}_{k|k}$：${\mathbf{P}}_{k|k}=(\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k){\mathbf{P}}_{k|k-1}$其中，$\mathbf{I}$ 是单位矩阵。

4. 循环迭代

重复预测步骤和更新步骤，处理每个新的测量数据。

各个字母的含义

1. 预测步骤（Prediction Step）

预测步骤包括状态预测和误差协方差预测。

状态预测（State Prediction）

$${\hat{\mathbf{x}}}_{k|k-1}={\mathbf{A}}_{k-1}{\mathbf{x}}_{k-1}+{\mathbf{B}}_{k-1}{\mathbf{u}}_{k-1}$$

• ${\hat{\mathbf{x}}}_{k|k-1}$：预测的状态向量（在时间 $k$ 时的先验估计）。
• ${\mathbf{A}}_{k-1}$：状态转移矩阵，描述系统从时间 $k-1$ 到时间 $k$ 的动态。
• ${\mathbf{x}}_{k-1}$：在时间 $k-1$ 时的状态向量（后验估计）。
• ${\mathbf{B}}_{k-1}$：控制输入矩阵，描述控制输入对系统状态的影响。
• ${\mathbf{u}}_{k-1}$：在时间 $k-1$ 时的控制输入向量。

误差协方差预测（Error Covariance Prediction）

$${\mathbf{P}}_{k|k-1}={\mathbf{A}}_{k-1}{\mathbf{P}}_{k-1|k-1}{\mathbf{A}}_{k-1}^T+{\mathbf{Q}}_{k-1}$$

• ${\mathbf{P}}_{k|k-1}$：预测的误差协方差矩阵（在时间 $k$ 时的先验估计的不确定性）。
• ${\mathbf{P}}_{k-1|k-1}$：在时间 $k-1$ 时的误差协方差矩阵（后验估计的不确定性）。
• ${\mathbf{A}}_{k-1}$：状态转移矩阵。
• ${\mathbf{Q}}_{k-1}$：过程噪声协方差矩阵，描述系统过程噪声的不确定性。

2. 更新步骤（Update Step）

更新步骤包括计算卡尔曼增益、更新状态估计和更新误差协方差矩阵。

计算卡尔曼增益（Kalman Gain Calculation）

$${\mathbf{K}}_k={\mathbf{P}}_{k|k-1}{\mathbf{H}}_k^T({\mathbf{H}}_k{\mathbf{P}}_{k|k-1}{\mathbf{H}}_k^T+{\mathbf{R}}_k{)}^{-1}$$

• ${\mathbf{K}}_k$：卡尔曼增益矩阵，描述测量值对状态估计的影响。
• ${\mathbf{P}}_{k|k-1}$：预测的误差协方差矩阵。
• ${\mathbf{H}}_k$：观测矩阵，描述状态向量到测量向量的映射。
• ${\mathbf{R}}_k$：测量噪声协方差矩阵，描述测量噪声的不确定性。

更新状态估计（State Update）

$${\mathbf{x}}_{k|k}={\hat{\mathbf{x}}}_{k|k-1}+{\mathbf{K}}_k({\mathbf{z}}_k-{\mathbf{H}}_k{\hat{\mathbf{x}}}_{k|k-1})$$

• ${\mathbf{x}}_{k|k}$：更新后的状态向量（在时间 $k$ 时的后验估计）。
• ${\hat{\mathbf{x}}}_{k|k-1}$：预测的状态向量。
• ${\mathbf{K}}_k$：卡尔曼增益矩阵。
• ${\mathbf{z}}_k$：在时间 $k$ 时的实际测量向量。
• ${\mathbf{H}}_k$：观测矩阵。
• ${\mathbf{H}}_k{\hat{\mathbf{x}}}_{k|k-1}$：预测测量值。

更新误差协方差矩阵（Error Covariance Update）

$${\mathbf{P}}_{k|k}=(\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k){\mathbf{P}}_{k|k-1}$$

• ${\mathbf{P}}_{k|k}$：更新后的误差协方差矩阵（在时间 $k$ 时的后验估计的不确定性）。
• $\mathbf{I}$：单位矩阵。
• ${\mathbf{K}}_k$：卡尔曼增益矩阵。
• ${\mathbf{H}}_k$：观测矩阵。
• ${\mathbf{P}}_{k|k-1}$：预测的误差协方差矩阵。

不包含公式的版本