深入理解如何不费吹灰之力搭建一个无人驾驶车（三）2D-小车拓展部分（AMCL+EKF）（EKF协方差矩阵如何写？）

三、2D小车定位拓展

本文讲一下不是很难的高中部分，主要是AMCL及EKF，两种定位方法

3.1 AMCL定位

首先讲一下AMCL算法，自适应蒙特卡罗粒子滤波定位，其实在gmapping中，我们就要用轮式里程计的odom产生粒子，有些东西，讲大家可能不懂，可以见概率机器人或机器人学状态估计，在贝叶斯滤波中，置信度的更新是靠观测，即一个归一化系数*P(Zt|xt)*预测置信度bel(xt)，这个预测bel一般上面有一横，CSDN不知道怎么打，这个预测bel是由运动方程及上一刻的置信度积分得到，总之，粒子滤波就是让符合观测P(Xt|Zt)的现在的粒子（预测的bel）更容易下一次被采样到，其实就是用离散的方法去近似连续，从而减少计算量，但粒子滤波容易出现粒子缺乏和粒子冗余，故我们用Augmented MCL改善了粒子缺乏，用KLD-MCL改善了粒子冗余。
原生粒子滤波如下：

如何根据运动模型采样粒子呢，对t-1时刻的粒子知道车子向x方向运动了10米，那么把t-1时刻的粒子的x坐标都加10，这是最简单的做法，但是根据里程计运动模型：实际上运动到那后你还要加上一随机增量，这个随机增量服从高斯分布，且协方差由下面的公式决定：

这个很关键，我们下一篇说。
那么如何通过计算机程序从一个高斯分布采样呢，高斯分布如下：（图来自百度百科）

我们把-3到3的函数拉成一条线，每段的长度等于其函数的高度及y值，然后生成一个随机数，看他落在哪个区间就好了，这种是用直方图的方法（使用时常取一段区间，用面积表示拉成直线的长度），但我希望我是直接建立从概率到值的映射，这里用Box-Muller方法：
随机抽出两个从[0,1]均匀分布的数字u和v。然后： 参考这篇文章

则Z1，Z2服从标准正态分布，若指定期望为E，方差为V，则令：
Z= Z * V + E;
也可以使用c++ 的 random库
Augmented MCL如下：

KLD MCL如下：

Augmented MCL说白了就是因为比方说位姿服从高斯分布，向两边延伸概率越来越小，但粒子滤波却不能在那里产生粒子，如果此时你把机器人蒙上眼睛和雷达抱到另一个地方了，那你就悲剧了，粒子难以收敛，所以AugmentedMCL改进就是如果粒子难以收敛到一点附近，那么我们就在全局随机产生一些粒子，越不收敛因随机产生的粒子越多，具体见概率机器人定位那章。

KLD MCL说白了就是如果传感器太牛逼了，最后粒子都收敛到一点附近，那这一点精确度已经够高了，粒子数对精确度提升不大，所以我们希望减少一些粒子，这个衡量单位是在一个栅格中的粒子。

AMCL就是这两者结合，你也可以尝试去自定义粒子分布的形状，这些概率机器人都有，就不说了，蛮简单的。

要使用AMCL你需要用mapserver先保存gmapping或其他算法建的地图，在linux命令行输入

rosrun map_server map_saver -f mymap

之后在launch文件加上你保存地图的路径并运行movebase：

在加上AMCL的launch

详见amcl参数含义及《概率机器人》及gmapping的解释，比如KLD那个参数就是概率机器人KLD MCL里的，这些参数都很简单，看名字都能懂，如果实在不懂点赞给我发信息，谢谢。

之后启动rviz发一个目标就能进行导航了，不过地图是直接出现了，就是你之前建好的地图，且不能更新。

3.2 EKF定位及协方差矩阵如何写

EKF全称是拓展卡尔曼滤波，那么卡尔曼滤波(KF)是个什么东西呢

对，位姿的x方向里程估计的数据有这两组，我们更愿意相信哪个数据呢？是不是数据二，因为它方差更小，这就是KF的原理，KF系属于高斯滤波（连续贝叶斯滤波），但是假设分布时高斯分布在实际情况往往是极其不准确的，因为你建立的误差模型可能不是服从高斯分布的，那你就必须通过计算把你的误差模型转换为高斯分布，这是极其麻烦的，所以现在都是用粒子滤波（离散贝叶斯滤波）较多，具体见下图：引用自概率机器人：
假设置信度预测就是最左边那个分布，而观测P(Zt|Xt)就是最右边那个分布，KF的结果是将这两者叠加得到中间实线那个分布，我们知道高斯分布曲线越胖，方差越大，所以我们通过这种高斯滤波方法让对位姿估计的不确定性降低了，但是当机器人继续运动的时候，置信度分布曲线还是会变胖，所以我们要继续高斯滤波，这样循环往复。
KF系属贝叶斯滤波其公式为：

而KF的伪代码为：

是不是就是贝叶斯滤波啊。而EKF是因为，比方说，你运动轨迹是一个圆，由运动模型如下，见《概率机器人》

是不是我们会由平移及角度计算X方向及Z方向的增量（用cos，sin这些三角函数），但其不是线性的，那我们将其泰勒展开，保留前两项（最多到一阶），之前无法知道KF的A和B和C这三个参数无法算协方差，展开后我知道了，于是就可以算协方差了。
而EKF伪代码如下：

另一种常用的KF系算法就UKF，通过无损变换进行滤波，伪代码如下：

UKF肯定比EKF好些，但是因为假定高斯分布，准确性在现实情况不高，需要一种比较好的误差模型构造支持。
ROS中我们常用到robot_pose_ekf这个包，其实我发现CSDN上没有一篇讲的清楚的，因为一些东西卡了很久，他需要的东西如下，首先是wheel_odom包括协方差矩阵，（不需要到base_link的tf，这个很重要），imu包括协方差矩阵，imu到tf的信息,vo,gps，后面两个可选，有了这些运行这个包就OK
launch如下定义：

然后关键是要建立正确的误差模型。这里我根据里程计运动模型写了个协方差矩阵的计算，它是6*6的矩阵，即(x,y,z,r,p,y)3个直角坐标，3个旋转角。若这6个相互独立，那么协方差矩阵只有对角线有值，(注：如果是单纯EKF建议直接写雅可比矩阵的值)，代码如下：

    for(int i = 0 ; i < 36 ; i++){
		odom_.pose.covariance[i] = 0;
    }

    //i*6+j  因为是二维就只有xy及转角θ
    odom_.pose.covariance[0] = cov_a3x_*delta_x*delta_x + cov_a4x_*(delta_pre_theta_*delta_pre_theta_+delta_theta*delta_theta); //x
    odom_.pose.covariance[7] = cov_a3y_*delta_y*delta_y + cov_a4y_*(delta_pre_theta_*delta_pre_theta_+delta_theta*delta_theta); //y
    odom_.pose.covariance[14] = pow(0.1); //z
    odom_.pose.covariance[21] = pow(0.1); //r
    odom_.pose.covariance[28] = pow(0.1); //p
    odom_.pose.covariance[35] = cov_a1_*(delta_theta*delta_theta) + cov_a2_*(delta_dis*delta_dis); //theta

	if( !delta_x || !delta_y || !delta_theta ){
		odom_.pose.covariance[0] = pow(0.1);
    	odom_.pose.covariance[7] = pow(0.1);
 		odom_.pose.covariance[35] = pow(0.1);
	}
    odom_pub_.publish(odom_);
    delta_pre_theta_ = delta_theta; //在下一次算的时候能用到上一次的数据

公式来自概率机器人里程计运动模型

而imu的误差模型包括噪声(Bias and Noise)、尺度因子(Scale errors)和轴偏差(Axis misalignments)。在ROS上协方差矩阵是只需提供(r,p,y)的方差，若这6个相互独立，那么协方差矩阵只有对角线有值。校准见imu误差模型。
然后把协方差公式写入代码即可。
好了EKF就讲完了，希望大家多多关注。下一章讲如何从头到尾用ROS自己创建一个slam算法