卡尔曼滤波推导过程|MATLAB滤波|MATLAB卡尔曼|MATLAB卡尔曼导航

卡尔曼滤波的推导过程

3.1 系统模型

卡尔曼滤波的基本框架由状态方程和观测方程构成:

  1. 状态方程

    xk=Fkxk−1+Bkuk+wkxk​=Fk​xk−1​+Bk​uk​+wk​

    其中:

    • xkxk​:在时间 kk 的状态向量。
    • FkFk​:状态转移矩阵。
    • BkBk​:控制输入模型。
    • ukuk​:控制输入(外部影响)。
    • wkwk​:过程噪声,假设为高斯分布 wk∼N(0,Qk)wk​∼N(0,Qk​)。
  2. 观测方程

    zk=Hkxk+vkzk​=Hk​xk​+vk​

    其中:

    • zkzk​:在时间 kk 的观测向量。
    • HkHk​:观测矩阵。
    • vkvk​:观测噪声,假设为高斯分布 vk∼N(0,Rk)vk​∼N(0,Rk​)。

3.2 预测步骤

在每个时间步,首先进行预测:

  1. 状态预测

    x^k∣k−1=Fkx^k−1∣k−1+Bkukx^k∣k−1​=Fk​x^k−1∣k−1​+Bk​uk​

  2. 协方差预测

    Pk∣k−1=FkPk−1∣k−1FkT+QkPk∣k−1​=Fk​Pk−1∣k−1​FkT​+Qk​

3.3 更新步骤

一旦获得观测值 zkzk​,就执行更新步骤:

  1. 计算卡尔曼增益

    Kk=Pk∣k−1HkT(HkPk∣k−1HkT+Rk)−1Kk​=Pk∣k−1​HkT​(Hk​Pk∣k−1​HkT​+Rk​)−1

  2. 更新状态估计

    x^k∣k=x^k∣k−1+Kk(zk−Hkx^k∣k−1)x^k∣k​=x^k∣k−1​+Kk​(zk​−Hk​x^k∣k−1​)

  3. 更新协方差

    Pk∣k=(I−KkHk)Pk∣k−1Pk∣k​=(I−Kk​Hk​)Pk∣k−1​

3.4 公式的解释

  • 状态预测:根据上一个时刻的状态和控制输入来预测当前时刻的状态。
  • 协方差预测:预估当前状态的不确定性,考虑过程噪声的影响。
  • 卡尔曼增益:平衡预测和观测之间的权重,增益越高,表示观测对估计的影响越大。
  • 状态更新:通过观测值修正预测的状态。
  • 协方差更新:更新后的协方差表示新的不确定性。

 

上一篇:物流系统原有40T数据加上每天至少要比之前多3G数据产品,这种该怎么解决


下一篇:代码随想录训练营第56天|广度优先搜索-100. 岛屿的最大面积