核心思想
卡尔曼滤波的核心就是在不确定中寻找最优，那么怎么定义最优呢？答案是均方误差最小的，便是最优。
卡尔曼滤波本质上是一种动态系统状态估计器，它回答了这样一个问题：
如何从充满噪声的观测数据中，还原出系统真实状态的最优估计？
这一问题的解决融合了三个关键思想：
贝叶斯推断：通过先验知识（系统模型）和观测数据（传感器信息）更新对状态的认知
递归优化：以最小均方误差（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
更新后的不确定性必然小于预测阶段，体现信息增益