卡尔曼滤波入门（二）
核心思想
卡尔曼滤波的核心就是在不确定中寻找最优，那么怎么定义最优呢？答案是均方误差最小的，便是最优。
卡尔曼滤波本质上是一种动态系统状态估计器，它回答了这样一个问题：
如何从充满噪声的观测数据中，还原出系统真实状态的最优估计？
这一问题的解决融合了三个关键思想：
贝叶斯推断：通过先验知识（系统模型）和观测数据（传感器信息）更新对状态的认知
递归优化：以最小均方误差（MMSE）为目标，动态调整预测与观测的权重
协方差传播：量化并传播不确定性，形成闭环的误差修正机制
数学建模
这是状态空间与噪声博弈，我们假设系统状态方程如下：
x
k
=
F
k
−
1
x
k
−
1
+
B
k
−
1
u
k
−
1
+
w
k
−
1
x_k = F_{k-1} x_{k-1} + B_{k-1} u_{k-1} + w_{k-1}
xk=Fk−1xk−1+Bk−1uk−1+wk−1
该方程假设了系统的真实模型特性，其中参数如下:
F：状态转移矩阵（描述系统动力学，如牛顿运动定律）
B：控制输入矩阵（描述输入对状态的影响，如电机推力对位置的影响）
w：过程噪声（系统内部噪声，建模未考虑的外部扰动）
观测方程（传感器模型）如下：
z
k
=
H
k
x
k
+
v
k
z_k = H_k x_k + v_k
zk=Hkxk+vk
该方程描述了对系统的状态观测结果，其中参数如下:
H：观测矩阵（传感器如何感知状态，如GPS测量位置）
v：观测噪声（传感器误差）
协方差矩阵（不确定性量化）：
P：状态估计误差的协方差（代表对预测的信心程度）
Q：过程噪声协方差（模型误差的统计特性）
R：观测噪声协方差（传感器精度的数学描述）
算法流程
预测阶段
状态预测：
x
k
∣
k
−
1
=
F
x
k
−
1
∣
k
−
1
+
B
u
k
−
1
x_{k|k-1} = F x_{k-1|k-1} + B u_{k-1}
xk∣k−1=Fxk−1∣k−1+Buk−1
基于物理模型推演下一时刻状态（如根据速度预测位置）
协方差预测:
P
k
∣
k
−
1
=
F
P
k
−
1
∣
k
−
1
F
T
+
Q
P_{k|k-1} = F P_{k-1|k-1} F^T + Q
Pk∣k−1=FPk−1∣k−1FT+Q
传播不确定性：模型误差（Q）越大，预测不确定性增长越快
更新阶段（测量更新）
卡尔曼增益计算:
K
k
=
P
k
∣
k
−
1
H
T
(
H
P
k
∣
k
−
1
H
T
+
R
)
−
1
K_k = P_{k|k-1} H^T (H P_{k|k-1} H^T + R)^{-1}
Kk=Pk∣k−1HT(HPk∣k−1HT+R)−1
物理意义：在预测与观测之间分配信任度
当传感器精度高（R小）→ 增益K增大，更相信观测
当模型精度高（P小）→ 增益K减小，更相信预测
状态修正：
x
k
∣
k
=
x
k
∣
k
−
1
+
K
k
(
z
k
−
H
x
k
∣
k
−
1
)
x_{k|k} = x_{k|k-1} + K_k (z_k - H x_{k|k-1})
xk∣k=xk∣k−1+Kk(zk−Hxk∣k−1)
用观测残差（新息）修正预测值，类似“误差反馈调节”
协方差更新：
P
k
∣
k
=
(
I
−
K
k
H
)
P
k
∣
k
−
1
P_{k|k} = (I - K_k H) P_{k|k-1}
Pk∣k=(I−KkH)Pk∣k−1
更新后的不确定性必然小于预测阶段，体现信息增益