【自动驾驶】卡尔曼滤波直观理解、数学公式及代码理解

直观理解

你住在深圳，你的好朋友小明住在乌鲁木齐。有一天，小明打算来找你玩，你想预订一家五星级餐馆，在他到深圳的当天晚上和他一起吃晚饭。但是小明是一名户外运动爱好者，他偏偏选择走路来深圳，你打开地图发现卧槽，这走路得走好几十天。

然而五星级餐厅如果不提前几天预定好话，当天是没有位置的。所以你问小明，哪一天才能到？然而他却没有正面回复，而是告诉你他从乌鲁木齐市中心出发以及他的速度。

乌鲁木齐到深圳全程约4275.3公里，这样的话四天多就能到了！但是根据你对小明体能的了解，你觉得这个估算不太靠谱。而且他路上还会经过雪岭、草岭、荒漠、戈壁各种地形，还有各种天气的原因，他很难保证每天100公里的前进速度。

于是你想了个办法，你先假设小明速度每天100km，然后每天晚上让他发个定位，告诉你他的位置在哪，然后你可以根据他的定位更新他的数学模型，预测之后的位置。

但是有个新的问题，就是他的GPS定位不是很准，可能是有十几公里的偏差，因此你也不能完全相信他的定位。

最后你决定两手抓，既相信自己的建立的数学模型，同时也参考GPS的定位，综合两者的结论来预测小明的位置。

例如，出发后第1天，根据你的数学模型，小明此时应该已经到了达坂城古镇，然而根据他发来的GPS定位显示还没走出市区，今天只走了10km。显然，你认为今天GPS的可信度高于自己的数学模型。

因此你综合“自己的数学模型”和“GPS信息”，得到一个新的数学模型——你认为小明目前的真正位置应该在距离市中心15km的地方，他的速度为每天12km，因此他第二天的位置在距离市中心27km的地方（这些数字是随便取得，大家看一看数字的变化就好）

之后的第三天、第四天、第N天你都采用上面的方法，随着你不断修正自己的数学模型，你的预测也会越来越准确，上面就是卡尔曼滤波的思想。

 

数学公式

接下来我们认真看一下卡尔曼滤波的公式，分为Station prediction（状态估计）与Measurement update（测量更新）两部分：

 

Station prediction

（1）公式一

在状态估计阶段，我们要估计物体的状态有位置$p_x$和速度$v_x$，因此状态$x$可由向量的形式表示为：

若第N-1时刻的状态用$x$表示（即估计前），第N时刻的状态用$x^{\prime }$表示（即估计后），则$x^{\prime }$可写作：

其中，$F$为状态转移矩阵，即表示第N时刻状态与第N-1时刻状态的关系：

这个很好理解，用高中物理知识就能明白，矩阵$F$与$x$相称后含义即为：

1. 第N时刻的位置（即$x^{\prime }$中的$p_x$）= 第N-1时刻的位置（即$x$中的$p_x$）+ 第N-1时刻的速度（即$x$中的$v_x$）*时间间隔（即$F$中的$\Delta t$）；
2. 第N时刻的速度（即$x^{\prime }$中的$v_x$）= 第N-1时刻的速度（即$x$中的$v_x$）（这里认为物体时匀速的）。

$B$输入增益矩阵，$u$表示输入向量，$B\ast u$表示有外部力量对物体施加力的作用。

同时，由于物体运动过程中存在噪声，我们认为该过程噪声为一个均值为0，协方差矩阵为Q的正态分布。

所以最终得到第一个公式：

 

（2）公式二

$P$表示协方差矩阵，即表示状态的不确定性，$F$为上面中提到的状态转移矩阵，$Q$表示状态转移中的噪声(也就是上面的 v v