卡尔曼滤波 核心概念引入 卡尔曼滤波是一种用于从一系列包含噪声的观测数据中，估计动态系统状态的优化估计算法。其核心思想是：结合系统的预测模型（基于上一时刻状态和已知的物理规律）和当前时刻的实测数据，通过加权平均的方式，得出对当前状态的最优估计。这里的“最优”是指在最小均方误差意义下的最优。 基本原理与模型 卡尔曼滤波基于两个基本模型： 状态空间模型 ：描述系统状态如何随时间演化。 状态方程： x_k = F * x_{k-1} + B * u_k + w_k 其中， x_k 是当前时刻的状态向量（如位置、速度）， F 是状态转移矩阵（描述状态如何从前一时刻变化而来）， B 是控制输入矩阵， u_k 是控制向量， w_k 是过程噪声（服从均值为0的高斯分布，协方差为 Q ）。 观测模型 ：描述我们能测量到什么。 观测方程： z_k = H * x_k + v_k 其中， z_k 是当前时刻的观测向量（传感器的读数）， H 是观测矩阵（将状态空间映射到观测空间）， v_k 是观测噪声（服从均值为0的高斯分布，协方差为 R ）。 核心算法步骤（两个阶段循环） 卡尔曼滤波是一个“预测-更新”的递归循环过程，对每个时刻 k 执行以下步骤： 阶段一：预测（先验估计） 预测状态： x̂_k^- = F * x̂_{k-1} + B * u_k （基于上一刻最优估计预测当前状态） 预测估计误差协方差： P_k^- = F * P_{k-1} * F^T + Q （量化本次预测的不确定性， P 是状态估计的协方差矩阵） 阶段二：更新（后验估计） 计算卡尔曼增益： K_k = P_k^- * H^T * (H * P_k^- * H^T + R)^{-1} 这是算法的核心。增益 K 权衡了预测（ P_k^- ）和观测（ R ）的不确定性。当观测噪声 R 很小时， K 增大，更信任新观测；当预测不确定性 P_k^- 很小时， K 减小，更信任预测。 更新状态估计（融合观测）： x̂_k = x̂_k^- + K_k * (z_k - H * x̂_k^-) 用预测值 x̂_k^- 加上一个修正项得到最优估计 x̂_k 。修正项是增益 K 乘以“新观测值与预测观测值之间的残差”。 更新估计误差协方差： P_k = (I - K_k * H) * P_k^- 由于融入了观测信息，状态估计的不确定性 P_k 相较于预测的不确定性 P_k^- 降低了。 关键特性与应用场景 递归性 ：只需要前一时刻的状态估计和当前观测，无需存储历史数据，计算高效，适合实时处理。 最优性 ：在模型准确且噪声为高斯白噪声的假设下，提供最小均方误差意义的最优估计。 应用场景 ：广泛应用于需要实时、精准状态估计的领域，如：导航与制导（GPS/INS组合导航）、目标跟踪、机器人定位与建图（SLAM）、金融时间序列分析、工业过程控制等。 扩展与局限 扩展 ：针对非线性系统，发展出了扩展卡尔曼滤波（EKF，通过线性化近似）和无迹卡尔曼滤波（UKF，使用无迹变换）。 局限 ：性能严重依赖于准确的系统模型（ F , H ）和已知的噪声统计特性（ Q , R ）。若模型不准确或噪声非高斯，滤波效果可能下降甚至发散。