多旋翼无人机

    返回首页    发表留言
本文作者:李德强
          第六节 卡尔曼滤波公式推导
 
 

        卡尔曼滤波在无人机导航和控制系统中起着至关重要的作用。由于很多传感器的读数并不是十分准确,而且噪声很大,无人机的控制模型的不确定性,在许多情况下传感器的不确定性可以假设其噪声误差满足高斯分布。卡尔曼滤波器是通过不断对上一时刻的状态进行迭代,得出当前时刻的状态,其在更新迭代过程中用到了5个重要的公式:

        其中,X ̂_t为状态预测;F_t为状态转换矩阵;P_t为预测值协方差;R_t为测量噪声;H_t为观测矩阵;Q_t为系统噪声;K为卡尔曼增益;Z_t为系统测量;P_t^'为状态值协方差。
        由:

        设x和y符合高斯分布,于是有x和y的联合分布:

        根据高斯特性有:

        求x,y分布:

        其中:

        先求x:

        其中:

        再求y:

        其中:

        于是,求期望:

        方差:

        对(9)式子中等号右边矩阵的每一项进行计算:
        1)左上:

 

        2)右下

        3)左下

        4)右上

        接下来计算(6)中的期望:

        得到的结果就是公式(4),而且式中的K就是公式(3),即卡尔曼增益。再来计算(6)中的方差:

        得到的式子就是公式(5)。再由公式(7)得出(1)。由:

        得到式子(10)就是公式(2)。到此,卡尔曼滤波的5个重要公式推导完毕。

    返回首页    返回顶部
  看不清?点击刷新

 

  Copyright © 2015-2019 问渠网 辽ICP备15013245号