Google开源SLAM软件cartographer中使用的UKF滤波器解析

转自：http://www.cnblogs.com/location-sensing/p/6208557.html

在Google开源SLAM软件cartographer中，相对《SLAM for dummies》使用了更为复杂、性能更好的Scan匹配与UKF算法，这里简单介绍下cartographer中使用的UKF算法。

　　（一）滤波器参数设定

  　　　　constexpr static FloatType kAlpha = 1e-3;

  　　　　constexpr static FloatType kKappa = 0.;

 　　　　 constexpr static FloatType kBeta = 2.;

  　　　　constexpr static FloatType kLambda = sqr(kAlpha) * (N + kKappa) - N;

 　　　　 constexpr static FloatType kMeanWeight0 = kLambda / (N + kLambda);

  　　　　constexpr static FloatType kCovWeight0 = kLambda / (N + kLambda) + (1. - sqr(kAlpha) + kBeta);

  　　　　constexpr static FloatType kMeanWeightI = 1. / (2. * (N + kLambda));

  　　　　constexpr static FloatType kCovWeightI = kMeanWeightI;

　　（二）关键成员变量

  　　　　GaussianDistribution<FloatType, N> belief_;

 　　　　const std::function<StateType(const StateType& state, const StateType& delta)>  add_delta_;

  　　　　const std::function<StateType(const StateType& origin, const StateType& target)>  compute_delta_;

　　（三）核心函数处理流程

　　1、Predict函数

　　该函数与UKF标准算法基本相同。利用预测函数g更新状态变量x及其协方差。具体到SLAM场景下，一般说来，预测函数是基于IMU传感器感知数据或者机器人的移动执行指令，来更新Lidar的位姿。

　　（1）检查协方差是否为对称正定矩阵，如果不是，则退出，否则计算状态变量x的均值，协方差的平方根，这一步是计算复杂度最高的一步。

　　（2）基于均值，协方差平方根构造2N+1个Sigma点集，N为状态变量x的维度：

 　　（3）对Sigma点集中每个点执行非线性预测函数g的变换，计算其均值及协方差，并确认协方差为对称矩阵。预测函数仅计算Translation之和，不计算方向角之和，因此在从状态变量x估计Lidar位姿时需要在方向角上乘以IMU估计并维护的方向角。

　　（4）协方差加上原始协方差，作为预测变量的协方差，均值作为预测变量的均值。

　　以上步骤又称为UT变换。

　　2、Observe函数

　　Observe函数总在Predict函数之后调用，利用观测函数h进一步修正状态变量x及其协方差，以及Kalman增益。具体到SLAM场景下，一般说来，观测函数指观测值即Lidar扫描扇区的值与Lidar位姿之间的函数关系。在cartographer中，由于观测值直接使用Scan-Submap匹配修订了Lidar位姿，因此不存在闭合形式的观测函数h。为了解决这个问题，cartographer采用了Scan-Submap匹配之后得到的Lidar位姿修订值与系统状态变量均值的矢量差作为预测函数h。因此，对应的观测过程与标准Kalman滤波有所差别。

　　（1）检查协方差是否为对称正定矩阵，如果不是，则退出，否则获得预测变量的均值及协方差。计算协方差的平方根。

　　（2）基于均值，协方差平方根构造2N+1个Sigma点集，N为状态变量x的维度：

　　（3）对Sigma点集中每个点执行非线性观测函数h的变换，并计算变换后Sigma点集的均值作为观测变量的均值，及其协方差，并确认协方差为对称矩阵。协方差加上观测方程的高斯噪声协方差作为观测变量的协方差。在cartographer中，观测函数h的定义参见PoseTracker::AddPoseObservation函数。

　　（4）计算预测变量与观测变量之间的协方差。

　　（5）计算Kalman增益，基于Kalman增益更新经过观测过程后修正的系统状态变量x的均值及其协方差。

　　（四）背后的核心算法

　　见百度文库《UKF滤波算法》，网址为http://wenku.baidu.com/link?url=er-Uo_5unEZGIcyg14wzZydaB5cqaUE-yAQddxCC7rS_rBtBBtsFgtVkVETRWZTs30B3bYkffqGaOzquxRT-MKFd3yibcHMfhs0BKtjjOCy。