卡尔曼滤波及其他的衍生算法在SSM体系里是一个极其重要的存在，这里比这就稍微介绍一下这个传统版本的卡尔曼滤波。
先给出我们熟知的卡尔曼滤波的公式：

卡尔曼滤波公式

1. 预测步骤（Predict）

状态预测（先验估计）：

协方差预测：

2. 更新步骤（Update）

新息（测量残差）：

新息协方差：

卡尔曼增益：

状态更新（后验估计）：

协方差更新：

变量说明（Notation）

符号 含义
$x$ 系统状态向量
$u$ 控制输入向量
$z$ 实际观测值
$F$ 状态转移矩阵
$B$ 控制输入矩阵
$H$ 观测矩阵
$P$ 估计误差协方差矩阵
$Q$ 过程噪声协方差矩阵
$R$ 观测噪声协方差矩阵
$K$ 卡尔曼增益
$^-$/$^+$ 先验/后验估计（上标）
$w_k$ 噪声

注：公式中上标 - 表示先验估计（预测值），上标 + 表示后验估计（修正后值）

通俗解释

看不懂对不？这里就通俗解释一下
首先我们从一个简单的物理模型下手：假设有一个物体是正在进行匀速直线运动切没有噪声，那么他的状态方程就是$xk = x{k-1} + vt$（这就是差分方程）,但是这毕竟是一个预测模型，因为你是从当前的运动状出发判断这个物体的运动状态，因此我们把这个方程称之为状态方程。既然有预测那就有现实观测进行误差修正，既然如此我们就把这个进行误差修正的方程称之为观测方程，他可以表示为

其中，$H_k$是一个观测模型，它把真实状态空间映射成观测空间也就是让你预测的跟实际上反馈的进行一个颗粒度对齐，$u_k$是观测噪声，其均值为零，协方差矩阵为$R_k$且服从正态分布（这是一个先验假设）。
作者是匀速运动，如果他是变速运动呢？
那么他的公式就变成了:

然后我们反过来看咱们的原本的状态方程:

我们可以惊喜地发现这个公式成功统一了起来。
因此我们可以下个定义就是状态方程就是一个用来预测的线性的物理模型。
既然这是预测的，那么肯定还有进行后验估计修正的。

后验修正其实就是用观测值对预测值进行“纠偏”。
具体做法是：先用预测模型得到一个状态估计（先验），然后用实际观测值和预测值的差异（新息 $y_k$）来修正这个估计。修正的幅度由卡尔曼增益 $K_k$ 决定，增益越大说明观测值越可信，修正幅度越大；增益越小则更相信模型预测。

修正公式如下：

也就是说，最终的状态估计是“预测值 + 修正量”。
修正量本质上就是“观测和预测的差异 × 权重（卡尔曼增益）”，这样可以动态融合模型和观测的优点，实现更精准的估计。

最后，协方差也要更新，反映修正后的不确定性：

这样一来，每一步都能根据实际观测不断优化自己的状态估计，卡尔曼滤波就实现了“预测-修正-再预测”的循环迭代过程

比如我们还是用物体匀速运动的例子。假设你用状态方程预测物体的位置，得到 $xk^- = x{k-1} + vt$，但实际测量时发现传感器读数 $z_k$ 和预测值有偏差。此时你就用卡尔曼滤波的后验修正步骤，把观测值和预测值加权融合，得到更接近真实的估计。

比如：

  • 预测：$xk^- = x{k-1} + vt$
  • 实际观测：$z_k$
  • 新息：$y_k = z_k - x_k^-$
  • 卡尔曼增益 $K_k$ 决定你更信观测还是更信预测

最终修正后的位置估计就是：

如果观测很准（$K_k$ 大），修正幅度大，估计值更靠近观测；如果观测噪声大（$K_k$ 小），则更信预测。
这样每一步都能根据实际观测不断调整自己的估计，既不会完全依赖模型，也不会完全依赖观测，实现了动态融合。
顺便给一个 python实现：

代码实现

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
import numpy as np

class SimpleKalmanFilter:
    def __init__(self, F=1, H=1, Q=1e-5, R=0.01, x0=0, P0=1):
        self.F = F  # 状态转移系数
        self.H = H  # 观测系数
        self.Q = Q  # 过程噪声协方差
        self.R = R  # 观测噪声协方差
        self.x = x0 # 初始状态
        self.P = P0 # 初始协方差

    def predict(self):
        self.x = self.F * self.x
        self.P = self.F * self.P * self.F + self.Q

    def update(self, z):
        y = z - self.H * self.x
        S = self.H * self.P * self.H + self.R
        K = self.P * self.H / S
        self.x = self.x + K * y
        self.P = (1 - K * self.H) * self.P
        return self.x

# 示例：物体匀速运动的观测
np.random.seed(0)
true_v = 1.0
steps = 50
x_true = np.arange(steps) * true_v
z = x_true + np.random.normal(0, 0.1, steps)  # 加入观测噪声

kf = SimpleKalmanFilter(x0=0)
results = []

for measurement in z:
    kf.predict()
    estimate = kf.update(measurement)
    results.append(estimate)

# 打印前10步结果
for i in range(10):
    print(f"Step {i}: True={x_true[i]:.2f}, Measured={z[i]:.2f}, Estimated={results[i]:.2f}")