在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。
需要帶有完整公式的PDF格式內容，請E-mail：huangkui@lbsense.com