Kalman Filter - 技术栈

[1 马尔科夫模型](#1 马尔科夫模型)

[2 隐马尔科夫模型](#2 隐马尔科夫模型)

[2 贝叶斯定理](#2 贝叶斯定理)

[3 KF](#3 KF)

[3.1 状态预测](#3.1 状态预测)

[3.2 利用测量修正状态](#3.2 利用测量修正状态)

[3.2.1 两个独立高斯分布相乘](#3.2.1 两个独立高斯分布相乘)

[3.2.2 状态修正](#3.2.2 状态修正)

[3.3 KF图解和公式总结](#3.3 KF图解和公式总结)

[3.3.1 预测](#3.3.1 预测)

[3.3.2 更新](#3.3.2 更新)

在现实世界中，任何测量都伴随着噪声和不确定性 。无论是追踪卫星轨迹、预测经济指标，还是实现自动驾驶汽车的精准定位，我们都需要一种方法从杂乱的数据中提取出真实的信号。卡尔曼滤波（Kalman Filter，KF ）正是为解决这一问题而诞生的强大工具。KF是一种高效率的递归滤波器（自回归滤波器），它能够从一系列的不完全及包含噪声的测量中，估计动态系统的状态。

1 马尔科夫模型

‌ 马尔科夫模型‌（Markov Model）是一种基于概率的统计模型，其核心特性是**"无后效性"**------即系统在某一时刻的状态仅依赖于前一时刻的状态，而与更早的历史状态无关。这种性质被称为‌马尔可夫性质‌，数学表达式为：

P(Xₙ₊₁=x|X₀,X₁,...,Xₙ) = P(Xₙ₊₁=x|Xₙ)

2 隐马尔科夫模型

隐马尔可夫模型‌（Hidden Markov Model, HMM）是一种统计模型，用来描述一个含有隐含未知参数的马尔可夫过程。其核心思想是：系统的真实状态无法直接观测，但可以通过与这些状态相关的‌可观测事件‌来推断隐藏状态的演变规律。HMM 被称为"双重随机过程"，因为它包含两个层次： ‌

隐藏的状态序列‌：遵循马尔可夫性质，即当前状态只依赖于前一时刻的状态。 ‌
可观测的输出序列‌：每个观测值由当前隐藏状态以一定概率生成。

例如，你无法看到外面的天气（隐藏状态），但可以通过观察隔壁邻居是否跑步、游泳或做健身操（可观测行为）来推测天气情况。HMM 通常由以下五个要素构成，包括2个状态集合和3个概率矩阵：

隐含状态集合‌（S）：系统真实但不可见的状态，如天气（晴天、多云、雨天）。
可观测状态集合‌（O）：我们可以观察到的输出，如某人当天的活动（跑步、健身、游泳）。
初始状态概率矩阵‌（π）：表示模型在初始时刻处于各个隐含状态的概率。
状态转移概率矩阵‌（A）：描述隐含状态之间随时间转移的概率，如"今天是晴天，明天是雨天"的概率。
观测概率矩阵‌（B）：描述在某个隐含状态下，产生特定观测值的概率，如"雨天时某人选择游泳"的概率。

2 贝叶斯定理

贝叶斯定理的公式为：

其中：

P(A)是‌先验概率‌：指在进行试验或获取新信息前，对事件 A发生概率的主观估计。
P(A∣B)是‌后验概率‌：指在已知事件 B发生的情况下，事件 A发生的概率，是结合新信息修正后的概率。
P(B∣A)是似然度：指在事件 A发生的条件下，事件 B发生的概率。
P(B)是标准化常量：确保后验概率的总和为1。

3 KF

KF滤波问题是一个隐马尔科夫模型 与贝叶斯定理 的联合实现。一般的状态模型可分为状态转移方程 和观测方程 ，而状态一般都是无法直接观测到的，所以是隐马尔科夫模型。然后，它将上一时刻获得的状态信息的后验分布作为新的先验分布，利用贝叶斯定理，建立一个贝叶斯递推过程，从而得到了贝叶斯递推公式。

其核心思想基于预测‌和‌更新‌两个步骤：‌ ‌

预测‌：根据系统模型和上一时刻的状态，估计当前状态（如位置、速度）。 ‌
更新‌：利用当前时刻的实际测量值，修正预测值，得到更精确的状态估计。

KF核心思想就像一位经验丰富的船长：他会根据船的当前速度和方向（预测），结合偶尔看到的灯塔位置（有误差的测量），在脑海里绘制出最可能的航线。预测给了他连续性，测量帮他修正偏差，两者结合，路线越来越准。

以机器人为例介绍卡尔曼滤波的原理，我们的任务是要预测机器人的状态x，包括位置p与速度v，即可表示为：

某个时刻，我们不知道真实的位置与速度到底是多少，二者存在一个所有可能性的组合。卡尔曼滤波假设状态所有的变量都是随机的且都服从高斯分布，每个变量都有其对应的均值以及方差（它代表了不确定性）。但是速度和位置是有关系的，所以这两个状态量并不是独立的，而是具有相关性的。这种状态量的相关性可以由协方差矩阵表示，矩阵的每个元素是第i个状态变量和第j个状态变量之间的相关度。

3.1 状态预测

我们需要估计在时间k时刻的两个信息：最优估计以及它的协方差矩阵，我们可以写成下面矩阵形式：

我们需要某种方式来查看当前状态（k-1时刻）并预测在时刻处k的状态，将加度记为a：

写成矩阵形式：

是预测矩阵或者叫做状态转移矩阵，该矩阵可以帮助我们计算下一个时刻的状态。称为控制矩阵，为控制向量。（对于没有外部影响的简单系统，可以忽略控制项）

但我们仍然不知道如何更新状态的协方差矩阵 ，其实过程也是很简单，如果我们将分布中的每个点乘以矩阵A，那么其协方差矩阵将更新为：

于是协方差矩阵的状态更新为：

通过在每个预测步骤之后添加一些新的不确定性，我们可以对与"世界"相关的不确定性进行建模（如我们无法跟踪的事物）。这样一来，由于新增的不确定性原始估计中的每个状态都可能迁移到多个状态。我们用高斯分布进行建模，所以在状态量的协方差中增加额外的协方差。

预测阶段 完整的状态转移方程为：

3.2 利用测量修正状态

假设我们有几个传感器，这些传感器可以向我们提供有关系统状态的信息。就目前而言，测量什么量都无关紧要，也许一个读取位置，另一个读取速度。每个传感器都告诉我们有关状态的一些间接信息（换句话说，传感器在状态下运作并产生一组测量读数）。**我们使用矩阵对传感器的测量值进行建模，**我们期望传感器的值可以被建模成如下形式：