文章目录
卡尔曼滤波 （Kalman Filter）是嵌入式开发中极为核心的 状态估计算法 ，尤其适用于存在噪声的 传感器数据融合 和 动态系统 状态估计。
一、卡尔曼滤波的本质与适用场景
卡尔曼滤波是一种 最优递归数据融合算法 ，在线性高斯假设下，通过预测（系统模型） 与更新（测量） 两个步骤，不断迭代得到系统状态的最小均方误差估计。
核心特点：
-
递推计算，无需存储全部历史数据，适合嵌入式实时系统
-
融合多个噪声源（过程噪声、测量噪声）的统计特性
-
输出不仅包含状态估计值，还包含估计的不确定性（协方差）
适用前提：
-
系统模型为线性 （可用扩展 卡尔曼滤波EKF 处理 非线性）
-
噪声服从高斯分布（零均值、白噪声）
-
系统状态可观测或至少可检测
二、卡尔曼滤波的数学模型
1、状态方程与观测方程
离散时间系统（嵌入式常用）：
x k = F k x k − 1 + B k u k + w k z k = H k x k + v k \begin{aligned} \mathbf{x}_k &= \mathbf{F}k \mathbf{x}{k-1} + \mathbf{B}_k \mathbf{u}_k + \mathbf{w}_k \\ \mathbf{z}_k &= \mathbf{H}_k \mathbf{x}_k + \mathbf{v}_k \end{aligned} xkzk=Fkxk−1+Bkuk+wk=Hkxk+vk
其中 w k ∼ N ( 0 , Q k ) \mathbf{w}_k \sim \mathcal{N}(0, \mathbf{Q}_k) wk∼N(0,Qk)， v k ∼ N ( 0 , R k ) \mathbf{v}_k \sim \mathcal{N}(0, \mathbf{R}_k) vk∼N(0,Rk)。
2、卡尔曼滤波两步法（五大方程）
预测(时间更新)
x ^ k ∣ k − 1 = F k x ^ k − 1 ∣ k − 1 + B k u k P k ∣ k − 1 = F k P k − 1 ∣ k − 1 F k ⊤ + Q k \begin{aligned} \hat{\mathbf{x}}_{k|k-1} &= \mathbf{F}k \hat{\mathbf{x}}{k-1|k-1} + \mathbf{B}_k \mathbf{u}k \\ \mathbf{P}{k|k-1} &= \mathbf{F}k \mathbf{P}{k-1|k-1} \mathbf{F}_k^\top + \mathbf{Q}_k \end{aligned} x^k∣k−1Pk∣k−1=Fkx^k−1∣k−1+Bkuk=FkPk−1∣k−1Fk⊤+Qk
更新(测量更新)
K k = P k ∣ k − 1 H k ⊤ ( H k P k ∣ k − 1 H k ⊤ + R k ) − 1 x ^ k ∣ k = x ^ k ∣ k − 1 + K k ( z k − H k x ^ k ∣ k − 1 ) P k ∣ k = ( I − K k H k ) P k ∣ k − 1 \begin{aligned} \mathbf{K}k &= \mathbf{P}{k|k-1} \mathbf{H}_k^\top \left( \mathbf{H}k \mathbf{P}{k|k-1} \mathbf{H}k^\top + \mathbf{R}k \right)^{-1} \\ \hat{\mathbf{x}}{k|k} &= \hat{\mathbf{x}}{k|k-1} + \mathbf{K}_k \left( \mathbf{z}k - \mathbf{H}k \hat{\mathbf{x}}{k|k-1} \right) \\ \mathbf{P}{k|k} &= \left( \mathbf{I} - \mathbf{K}_k \mathbf{H}k \right) \mathbf{P}{k|k-1} \end{aligned} Kkx^k∣kPk∣k=Pk∣k−1Hk⊤(HkPk∣k−1Hk⊤+Rk)−1=x^k∣k−1+Kk(zk−Hkx^k∣k−1)=(I−KkHk)Pk∣k−1