在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的变换，得到，计算的均值作为观测变量的均值，及的协方差，并确认为对称矩阵。协方差加上协方差作为观测变量的协方差。在cartographer中，参见PoseTracker::AddPoseObservation函数，观测函数h表示为：，其中表示Scan-Submap匹配得到的位姿，减号表示矢量差（即包含translation差和方位角orientation差）。
（4）计算预测变量与观测变量之间的协方差。
（5）计算Kalman增益，基于Kalman增益更新经过观测过程后修正的系统状态变量x的均值及其协方差。
（四）背后的核心算法
见百度文库《UKF滤波算法》，网址为http://wenku.baidu.com/link?url=er-Uo_5unEZGIcyg14wzZydaB5cqaUE-yAQddxCC7rS_rBtBBtsFgtVkVETRWZTs30B3bYkffqGaOzquxRT-MKFd3yibcHMfhs0BKtjjOCy。
需要带有完整公式的PDF格式内容，请E-mail：[email protected]