一天精通无人机第 43 讲 高级篇系列:卡尔曼滤波 5 个重要公式的推导

file

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

file

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

file

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

file

根据高斯特性有:

file

求x,y分布:

file

其中:

file

先求x:

file

其中:

file

再求y:

file

其中:

file

于是,求期望:

file

方差:

file

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

  1. 左上:

file

  1. 右下:

file

  1. 左下:

file

  1. 右上:

file

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

file

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

file

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

file

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

下期预告:《状态估计(上):坐标系变换》

file