当前位置：首页 > news >正文

卡尔曼滤波（Kalman Filter）详解

news 2026/4/21 7:11:19

卡尔曼滤波是一种在存在测量噪声和过程噪声时，对动态系统状态做最优估计的递推算法。它把“模型预测”和“传感器测量”按统计意义融合，得到比单独用模型或单独用传感器更可靠的状态估计。

1. 要解决什么问题

典型场景：

你有一个动态系统（位置、速度、角度、电流等随时间变化）。
你有不完美的模型（简化、参数误差、未建模扰动）。
你有带噪声的测量（ADC、编码器、IMU 等）。

目标：在每个时刻 kk，得到对真实状态 xkxk 的最优估计 x^kx^k（在线、递推、计算量可控）。

“最优”在线性高斯假设下指：估计误差方差最小（与最小均方误差意义一致）。

2. 状态空间模型（线性离散）

状态方程（过程模型）：

xk=Fkxk−1+Bkuk−1+wk−1xk=Fkxk−1+Bkuk−1+wk−1

观测方程（测量模型）：

zk=Hkxk+vkzk=Hkxk+vk

xkxk：状态向量（如位置、速度）。
uk−1uk−1：已知输入（如控制力矩对应的加速度项）。
zkzk：测量向量。
wk−1wk−1：过程噪声，通常假设 w∼N(0,Qk)w∼N(0,Qk)。
vkvk：测量噪声，通常假设 v∼N(0,Rk)v∼N(0,Rk)。

核心假设（标准卡尔曼）：

系统是线性的（F,HF,H 为线性矩阵）。
噪声是零均值高斯，且过程噪声与测量噪声互不相关（常还假设与初始状态不相关）。
初始状态有高斯先验。

在这些假设下，后验仍是高斯，卡尔曼给出该高斯的均值与协方差的闭式递推。

3. 算法结构：预测 + 更新（两步递推）

记 x^k∣k−1x^k∣k−1 为用 k−1k−1 时刻及以前信息对 xkxk 的预测；Pk∣k−1Pk∣k−1 为对应误差协方差。
x^k∣kx^k∣k、Pk∣kPk∣k 为纳入测量 zkzk 之后的后验估计。

3.1 预测（时间更新）

x^k∣k−1=Fkx^k−1∣k−1+Bkuk−1x^k∣k−1=Fkx^k−1∣k−1+Bkuk−1Pk∣k−1=FkPk−1∣k−1Fk⊤+QkPk∣k−1=FkPk−1∣k−1Fk⊤+Qk

含义：按动力学把状态和不确定性往前推；QkQk 表示模型不准带来的协方差增大。

3.2 更新（测量更新）

新息（innovation）（预测测量与真实测量的差）：

yk=zk−Hkx^k∣k−1yk=zk−Hkx^k∣k−1

新息协方差：

Sk=HkPk∣k−1Hk⊤+RkSk=HkPk∣k−1Hk⊤+Rk

卡尔曼增益（决定更信模型还是更信测量）：

Kk=Pk∣k−1Hk⊤Sk−1Kk=Pk∣k−1Hk⊤Sk−1

后验状态与协方差：

x^k∣k=x^k∣k−1+Kkykx^k∣k=x^k∣k−1+KkykPk∣k=(I−KkHk)Pk∣k−1Pk∣k=(I−KkHk)Pk∣k−1

（数值实现里有时用 Joseph 形式等改写 Pk∣kPk∣k，以保证对称、正定、数值稳定。）

3.3 直观理解增益 KkKk

RkRk 大（测量很不可靠）→ SkSk 大 → KkKk 小 → 更信预测。
Pk∣k−1Pk∣k−1 大（模型预测很不确定）→ KkKk 大 → 更信本次测量。

因此卡尔曼滤波本质是：按不确定性大小加权融合。

4. 与贝叶斯推断的关系

可把每一步看成：

预测：用 Chapman–Kolmogorov 式地把上一时刻后验推到当前时刻先验。
更新：用贝叶斯公式把先验与似然 p(zk∣xk)p(zk∣xk) 结合成后验。

在线性高斯下，这一步有解析解，就是上面的公式。非线性时一般没有闭式高斯后验，于是出现 EKF、UKF、粒子滤波等。

5. 连续时间与离散时间

控制与嵌入式里几乎都用离散卡尔曼（固定采样周期 TsTs）。
若先有连续模型 x˙=Ax+Bux˙=Ax+Bu，常通过零阶保持等得到 F≈I+ATsF≈I+ATs（小 TsTs 的一阶近似）或矩阵指数 F=eATsF=eATs，再配离散过程噪声 QdQd（由连续 QcQc 与 TsTs 导出）。

6. 非线性系统：EKF 与 UKF（简述）

实际电机、机器人、导航多为非线性：

xk=f(xk−1,uk−1)+w,zk=h(xk)+vxk=f(xk−1,uk−1)+w,zk=h(xk)+v

扩展卡尔曼（EKF）：在 x^x^ 处对 f,hf,h 一阶泰勒展开，用局部线性化的 Fk=∂f∂xFk=∂x∂f、Hk=∂h∂xHk=∂x∂h 套标准卡尔曼。实现简单，但强非线性或大初始误差时可能不稳定或偏差大。

无迹卡尔曼（UKF）：用确定性采样点（sigma points）近似传播均值和协方差，常比 EKF 在非线性下更稳，计算比粒子滤波小。

还有 ESKF（误差状态卡尔曼）在惯导里很常见：在主积分外对小误差状态做线性卡尔曼，利于保持姿态约束等。

7. 与 Luenberger 观测器、 αβαβ 滤波的关系

Luenberger 观测器：结构类似，增益常按极点配置，不一定显式用 Q,RQ,R 的随机模型。
一阶低通 / αβαβ：可看成极简、固定增益的平滑；卡尔曼是时变增益、且自然处理多传感器与多维耦合。

8. 工程上要注意的点

问题	说明
Q,RQ,R 调参	相对比例决定融合倾向；可结合物理噪声、试验迭代。
可观测性	若 H,FH,F 结构导致某些状态从测量中“看不见”，估计会漂或病态。
数值	协方差对称化、平方根滤波、Joseph 形式等提高稳定性。
异步/多率	预测按控制周期，有测量时再更新；可扩展为信息滤波等形式。
异常值	标准卡尔曼假设高斯；野值需门限、Huber、鲁棒滤波或切换模型。