卡尔曼滤波入门（二）

核心思想

卡尔曼滤波的核心就是在不确定中寻找最优，那么怎么定义最优呢？答案是均方误差最小的，便是最优。

卡尔曼滤波本质上是一种动态系统状态估计器，它回答了这样一个问题：

如何从充满噪声的观测数据中，还原出系统真实状态的最优估计？

这一问题的解决融合了三个关键思想：

贝叶斯推断：通过先验知识（系统模型）和观测数据（传感器信息）更新对状态的认知
递归优化：以最小均方误差（MMSE）为目标，动态调整预测与观测的权重
协方差传播：量化并传播不确定性，形成闭环的误差修正机制

数学建模

这是状态空间与噪声博弈，我们假设系统状态方程如下：
$x_k=F_{k-1}{x}_{k-1}+B_{k-1}{u}_{k-1}+w_{k-1}$

该方程假设了系统的真实模型特性，其中参数如下:

F：状态转移矩阵（描述系统动力学，如牛顿运动定律）
B：控制输入矩阵（描述输入对状态的影响，如电机推力对位置的影响）
w：过程噪声（系统内部噪声，建模未考虑的外部扰动）

观测方程（传感器模型）如下：
$z_k=H_k{x}_k+v_k$
该方程描述了对系统的状态观测结果，其中参数如下:

H：观测矩阵（传感器如何感知状态，如GPS测量位置）
v：观测噪声（传感器误差）

协方差矩阵（不确定性量化）：

P：状态估计误差的协方差（代表对预测的信心程度）
Q：过程噪声协方差（模型误差的统计特性）
R：观测噪声协方差（传感器精度的数学描述）

算法流程

预测阶段

状态预测：

$x_{k|k-1}=F{x}_{k-1|k-1}+B{u}_{k-1}$
基于物理模型推演下一时刻状态（如根据速度预测位置）

协方差预测:

$P_{k|k-1}=F{P}_{k-1|k-1}{F}^T+Q$
传播不确定性：模型误差（Q）越大，预测不确定性增长越快

更新阶段（测量更新）

卡尔曼增益计算:

$K_k=P_{k|k-1}{H}^T(H{P}_{k|k-1}{H}^T+R{)}^{-1}$
物理意义：在预测与观测之间分配信任度
当传感器精度高（R小）→ 增益K增大，更相信观测
当模型精度高（P小）→ 增益K减小，更相信预测

状态修正：

$x_{k|k}=x_{k|k-1}+K_k(z_k-H{x}_{k|k-1})$
用观测残差（新息）修正预测值，类似“误差反馈调节”

协方差更新：

$P_{k|k}=(I-K_{k}H)P_{k|k-1}$
更新后的不确定性必然小于预测阶段，体现信息增益