卡尔曼滤波

周日回顾一些概率和统计分析知识点，重点学习卡夫曼滤波知识，直观感觉跟贝叶斯估计有相当的关联。

一些基本概念

来自： 卡尔曼滤波原理解读

“卡尔曼滤波本质上是一种最优加权平均”

理解起点：两把温度计，一把水银，一把电子，测温结果

$T_1 = T + \varepsilon_1$
$T_2 = T + \varepsilon_2$

噪声满足：
$\varepsilon_1\sim N(0,\sigma_1^2),\quad \varepsilon_2\sim N(0,\sigma_2^2)$
$E[\varepsilon_1\varepsilon_2] = 0$

$\varepsilon_1 \sim N(0, \sigma_1^2)$ --- 表示：$\varepsilon_1$ 服从均值 0、方差** $\sigma_1^2$ 的正态分布
$E[\varepsilon_1 \varepsilon_2]$ 是统计学中的 期望（Expectation），在这里用于描述 两个噪声之间是否相关。$\text{Cov}(\varepsilon_1,\varepsilon_2) = E[\varepsilon_1 \varepsilon_2]$

含义：

• 两个不同精度的传感器测同一个真实温度 T
• $T_1$ 和 $T_2$ 有不同大小的噪声

这就是“状态融合”的基本输入。

平均值与方差

对两把温度计的结果可以采用两种方式融合，一种是算术平均，一种是加权平均。

算术平均方式

最直观的方式就是求平均值: $\hat{T}_{est1} = \frac{T_1 + T_2}{2}$
代噪声模型 $\hat{T}_{est1} = \frac{(T + \varepsilon_1) + (T + \varepsilon_2)}{2} = T + \frac{\varepsilon_1 + \varepsilon_2}{2}$
求方差
T 是常数 → 无方差， 噪声互不相关 → 方差可简单相加：
$D[\hat{T}_{est1}] = D\left(\frac{\varepsilon_1+\varepsilon_2}{2}\right)$ $= \frac{1}{4}D[\varepsilon_1] + \frac{1}{4}D[\varepsilon_2]$ $= \frac{1}{4}\sigma_1^2 + \frac{1}{4}\sigma_2^2$

$\boxed{D[\hat{T}_{est1}] = \frac{\sigma_1^{2} + \sigma_2^{2}}{4}}$

加权平均方式

$\alpha越小T_1的权重越小，T_2的权重越大$
$\hat{T}_{est2}=\alpha T_1 + (1-\alpha)T_2$
代噪声模型 $\hat{T}_{est2}=\alpha (T + \varepsilon_1) + (1-\alpha)(T + \varepsilon_2)$
展开： $= T + \alpha \varepsilon_1 + (1-\alpha)\varepsilon_2$
求方差 噪声独立 → 直接加：
$D[\hat{T}_{est2}] = D(\alpha \varepsilon_1 + (1-\alpha)\varepsilon_2)$
$= \alpha^2 D[\varepsilon_1] + (1-\alpha)^2D[\varepsilon_2]$

$\boxed{ D[\hat{T}_{est2}] = \alpha^2\sigma_1^2 + (1-\alpha)^2\sigma_2^2 }$

求最优权重 α：
目标： 让 $\alpha^2\sigma_1^2 + (1-\alpha)^2\sigma_2^2$ 最小。
求导： $2\alpha\sigma_1^2 - 2(1-\alpha)\sigma_2^2 = 0$
解得：
$\boxed{ \alpha = \frac{\sigma_2^2}{\sigma_1^2 + \sigma_2^2} }$

这说明：

• 噪声越小 → 权重越大
• 噪声越大 → 权重越小
  完全符合直觉： 📌 越“准”的传感器占比越高。

将 α 代回去得到最小方差：
代入：
$D_{min} = \frac{\sigma_1^2 \sigma_2^2}{\sigma_1^2 + \sigma_2^2}$
即：
$\boxed{D_{min} = \frac{\sigma_1^2\sigma_2^2}{\sigma_1^2 + \sigma_2^2}}$

权重方式下最优 $\alpha$下的方差一定优于简单的算术平均下的方差（方差反映偏离程度，也就是说前者的偏离程度一定更小）。

$\frac{\sigma_1^2\sigma_2^2}{\sigma_1^2+\sigma_2^2} \le \frac{\sigma_1^{2}+\sigma_2^{2}}{4}$

说明：
➡ 最优加权融合一定比简单平均更准
➡ 即使两传感器噪声很大，融合仍能降低不确定性。

这就是卡尔曼滤波存在的根本理由。

卡尔曼滤波

预测（Prediction）阶段

① 状态预测
$\hat{x}_k^- = A_k \hat{x}_{k-1} + B_k u_k$
② 协方差预测
$P_k^- = A_k P_{k-1} A_k^T + Q_k$

更新（Update / Correction）阶段

③ 卡尔曼增益
$K_k = P_k^- C_k^T \left(C_k P_k^- C_k^T + R_k \right)^{-1}$
④ 状态更新（融合预测和观测）
$\hat{x}_k = \hat{x}_k^- + K_k \left(z_k - C_k \hat{x}_k^- \right)$
⑤ 协方差更新（更新后的最优不确定度）
$P_k = (I - K_k C_k) P_k^-$

① 状态预测

根据上一时刻的最佳估计 + 控制输入，预测当前位置的状态，纯预测（没有用到观测值）。

$\boxed{\hat{x}_k^- = A_k \hat{x}_{k-1} + B_k u_k}$

$A_k$：系统如何从上一时刻演变到下一时刻（状态转移矩阵），转移矩阵实际上就是模型
若状态为： $x = \begin{bmatrix} \text{位置} \\ \text{速度} \end{bmatrix}$
那么状态转移矩阵： $A = \begin{bmatrix} 1 & \Delta t\\ 0 & 1 \end{bmatrix}$
含义：
$\text{位置} = \text{旧位置} + \text{速度}\cdot \Delta t$
$\text{速度} = \text{旧速度}$急症室

$u_k$：控制输入（输入量）
$B_k$：控制矩阵（告诉你：输入如何影响状态）

例子：
若： $u_k = \text{加速度}$
那控制矩阵可能为： $B_k = \begin{bmatrix} 0.5 \Delta t^2 \\ \Delta t \end{bmatrix}$
含义：

• 加速度让位置变成：$0.5at^2$
• 加速度让速度变成：$at$

举例汽车定位： $x = \begin{bmatrix} \text{位置} \\ \text{速度} \end{bmatrix}$
上一帧估计： $x_{k-1} = \begin{bmatrix} 10m \\ 2m/s \end{bmatrix}$
加速度输入：$u_k = 1m/s^2$，时间间隔 1 秒
那么： $A = \begin{bmatrix} 1 & 1\\ 0 & 1 \end{bmatrix} ,\quad B = \begin{bmatrix} 0.5 \\ 1 \end{bmatrix}$
预测： $x_k^- = A x_{k-1} + B u_k = \begin{bmatrix} 1 & 1\\ 0 & 1 \end{bmatrix} \begin{bmatrix} 10 \\ 2 \end{bmatrix} + \begin{bmatrix} 0.5 \\ 1 \end{bmatrix}(1)$
计算： $x_k^- = \begin{bmatrix} 12.5 \\ 3 \end{bmatrix}$
解释：

• 位置从 $10 → 12.5$
• 速度从 $2 → 3$

② 协方差预测

$P_k^- = A_k P_{k-1} A_k^T + Q_k$

误差会随着系统模型被传递、放大、旋转等，而同时真实世界还会加入额外噪声 Q，因此预测的不确定性会增加。

A 左乘 → 对误差进行变换   
Aᵀ 右乘 → 对误差区域进行“逆变换”的配套操作，使得协方差保持对称、正定   

A 与 Aᵀ 必须成对出现，才能保证协方差保持对称矩阵。

$Q_k$ 表示真实世界中模型没有描述的误差。

一维的推算：
误差的方差 $P = E[e^2] = D(e)$
根据一维误差传播： $e_k^- = a e_{k-1}$
方差： $P_k^- = E[(e_k^-)^2] = E[(a e_{k-1})^2]$ $= a^2 E[e_{k-1}^2]$ $= a^2 P_{k-1}$
⭐ 一维误差传播： $P_k^- = a^2 P_{k-1}$ 这就是 $A P Aᵀ$ 的一维形式。

假设状态是二维：
$x = \begin{bmatrix} x_1 \\ x_2 \end{bmatrix}$
误差： $e = x - \hat{x}$
误差的协方差： $P = E[ee^T]$
状态转移为： $x_k = A x_{k-1}$
预测： $\hat{x}_k^- = A\hat{x}_{k-1}$
误差传播： $e_k^- = x_k - \hat{x}_k^- = A(x_{k-1} - \hat{x}_{k-1}) = A e_{k-1}$
然后计算方差（注意现在是矩阵）： $P_k^- = E[e_k^- (e_k^-)^T]$
代入 $e_k^- = A e_{k-1}$： $P_k^- = E[(A e_{k-1})(A e_{k-1})^T]$ $= E[A e_{k-1} e_{k-1}^T A^T]$
由于 A 是常数矩阵： $= A E[e_{k-1} e_{k-1}^T] A^T$
而： $E[e_{k-1} e_{k-1}^T] = P_{k-1}$
因此：⭐ 误差传播公式（二维） $P_k^- = A P_{k-1} A^T$

③ 卡尔曼增益

$K_k = P_k^- C_k^T \left(C_k P_k^- C_k^T + R_k \right)^{-1}$

Kₖ = 最优权重（Optimal Weight）含义是：该相信预测（模型）多少？ 该相信观测（测量）多少？
- 如果预测不确定性大 $P^- \uparrow，Kₖ ↑$ → 更相信观测
- 如果观测噪声大 $R \uparrow，Kₖ ↓$ → 更相信模型

一维情况，前面已经写过：$K = \frac{P^-}{P^- + R}$，
其中： $P^-$：预测误差（模型不确定性）；$R$：观测误差（传感器不确定性）
$K$ 是一个 权重：

• 如果预测很不准（P⁻ 大） → K 大 → 多信观测；如果观测噪声大（R 大） → K 小 → 多信预测
  这是 最优加权融合。

多维下公式变复杂了，但含义是一样的。

多维情况，
因为状态通常是多维的，例如： $x = \begin{bmatrix} p_x \\ p_y \\ v_x \\ v_y \end{bmatrix}$
观测也是多维的： $z = \begin{bmatrix} x \\ y \end{bmatrix}$
而且观测并不一定是“直接等于状态”，而是： $z_k = C_k x_k + v_k$
$P_k^- C_k^T$: 把状态预测的不确定性（P⁻）投影到观测空间。表示：“预测的不确定性，会影响你对观测方向上误差的判断。”也就是：预测有多不确定，会反映到观测中来。
$C_k P_k^- C_k^T + R_k$：$C P⁻ Cᵀ$ ：表示如果按当前预测状态去计算，会得到怎样的观测？其不确定性是多少？即：“预测观测值的不确定性（在传感器空间中）”**。R 表示观测噪声
$\underbrace{C P^- C^T}_{\text{预测误差变成观测误差}} + \underbrace{R}_{\text{传感器噪声}}$

④ 状态更新（融合预测和观测）

$\hat{x}_k = \hat{x}_k^- + K_k \left(z_k - C_k \hat{x}_k^- \right)$

• Cₖ 是观测模型
• $Cₖ\hat{x}_k^-$ 是预测出的“应该看到的观测”
• zₖ 是实际“看到的观测”

前述：

• 模型的预测 $\hat{x}_k^- = A_k \hat{x}_{k-1} + B_k u_k$
• 最优权重$K_k = P_k^- C_k^T (C_kP_k^-C_k^T + R_k )^{-1}$ -- > 预测值和观测值谁更可信？
  • 如果预测不确定性大 $P^- \uparrow，Kₖ ↑$ → 更相信观测
  • 如果观测噪声大 $R \uparrow，Kₖ ↓$ → 更相信模型

创新（innovation）或**残差（residual）**部分：$z_k - C_k \hat{x}_k^-$，实际观测 － 预测得到的“观测”,传感器给我们的数据，到底与我们预测的差了多少？

公式逻辑如下：
1️⃣ 先用模型预测状态 $\hat{x}^-_k$
2️⃣ 再看看传感器实际看到的$zₖ$
3️⃣ 对比得到预测误差（创新）
4️⃣ $Kₖ$ 决定误差要吸收多少
5️⃣ 加到预测值中，得到最终最优估计 $\hat{x}_k$

⑤ 协方差更新（更新后的最优不确定度）

$P_k = (I - K_k C_k) P_k^-$

$I = \begin{bmatrix} 1 & 0 & 0 & \cdots & 0 \\ 0 & 1 & 0 & \cdots & 0 \\ 0 & 0 & 1 & \cdots & 0 \\ \vdots & \vdots & \vdots & \ddots & \vdots \\ 0 & 0 & 0 & \cdots & 1 \end{bmatrix}$

所以 (I - K C) 表示：从“保持原样”开始（I），再减去 “被修正的部分” (K C)，预测误差 = 原预测误差 – 被观测修正掉的误差。
理解：I 是“100%”；K C 是“纠正比例”； (I − K C) 是“剩下的误差比例”。

. 问题

Q1：既然转移矩阵 $A_k$实际上就是模型，那么能不能把$B_k u_k$ 部分也纳入到转移矩阵中，作为模型的一部分？这样不就简化成：$\hat x_k^- = A\hat x_{k-1}$?
A: 数学上确实可以合并 $x'_k = \begin{bmatrix} x_k \\ u_k \end{bmatrix}$ ==> $x'_k = A'_k \, x'_{k-1}$
但工程应用中通常不这么干，原因是 uₖ（控制量）不仅仅是状态，下一帧可能变化，合并意味着下一帧完全由上一帧的状态决定。

控制输入$B_k u_k$ 与状态具有不同含义和不同物理来源，需要分开表示。

但在一些完全不能控制、估算的的因素，有时反而必须纳入到状态转移矩阵中，作为模型的一部分。构建扩展状态如：
$x = \begin{bmatrix}位置 \\ 速度 \\ 姿态 \\ 振动偏置 b_g \\ 加速度偏置 b_a \end{bmatrix}$

Q： $A_k$ ,$C_k$的区别是什么？
✔ Aₖ 是动力学模型（系统怎么动），系统通上一帧到当前帧状态怎么转换的。回答“系统自身在物理规律下，会如何运动？”
✔ Cₖ 是观测模型（传感器怎么看），Cₖ 描述了传感器观测值 zₖ 与状态 xₖ 之间的线性关系，即：状态在传感器中的呈现方式（观测模型）。 回答“传感器看到状态的哪一部分？以什么关系看到？”。“预测值在传感器眼中应该长什么样””

Cₖ 提供观测，使得卡尔曼滤波可以用 zₖ 来修正由 Aₖ 预测产生的误差。

真实系统：  x ----(A)---➡️ x预测   
传感器：    x ----(C)---➡️ z观测   

Aₖ 负责预测。
Cₖ 负责描述传感器看到什么。
用 zₖ 与 $Cₖ\hat{x}^-$ 的差距来修正 Aₖ 的误差。
修正的核心由 $K_k$(最优权重（Optimal Weight）) 完成。

预测（靠 Aₖ） + 修正（靠 Cₖ）。

• ← Previous Post
• Next Post →