多旋翼无人机

          第六节 卡尔曼滤波公式推导

 

 

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