UKF filter resolution used in Google Open source slam software cartographer

　　In the Google Open source SLAM software cartographer, the more complex, better-performing scan matching and UKF algorithm is used relative to SLAM for Dummies, where the cartographer algorithm used in UKF is briefly described.

　　(i) filter parameter setting

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;

(ii) Key member variables

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_;

(iii) core function processing flow

1　　 ,Predict functions

This function is basically the same as the UKF standard algorithm. Use the predictive function g to update the state variable x and its covariance. Specific to the slam scenario, the predictive function is generally based on the IMU sensor-aware data or the robot's mobile execution instructions to update the LIDAR posture.

(1) Check whether the covariance is a symmetric positive definite matrix, if not, then exit, otherwise the mean value of the state variable x , the square root of the covariance, this step is the most computationally complex step.

(2) Based on the mean value, the square root of the covariance constructs the 2n+1 Sigma Point set, and N is the dimension of the state variable x :

(3) The mean value of each point in the Sigma Point set is performed by the transformation of the nonlinear prediction function g , the mean value and covariance are computed, and the covariance is confirmed as symmetric matrix. The prediction function computes only the sum of the translation and does not calculate the sum of the direction angles, so it is necessary to estimate and maintain the direction angle at the directional angle in order to evaluate the LiDAR pose from the state variable x .

(4) Covariance plus the original covariance, as the covariance of predictor variables, mean value as the mean of the Predictor variable.

The above steps are also called UT transformations.

2　　 ,Observe functions

The observe function is always called after the Predict function, and the Observer function H is used to further correct the state variable x and its covariance, as well as the Kalman gain. In the slam scenario, in general, the observed function refers to the function relationship between the value of the LIDAR scan sector and the LiDAR posture. In cartographer, there is no closed form of observation function Hbecause the observed values are used to modify the LiDAR pose directly using scan-submap matching. In order to solve this problem, cartographer uses the vector difference between the LiDAR posture revision value and the System state variable mean value after the Scan-submap match as the predictive function H. Therefore, the corresponding observation process differs from the standard Kalman filter.

(1) Check if the covariance is a symmetric positive definite matrix, if not, then exit, otherwise get the mean and covariance of the predictor variable. Calculates the square root of the covariance.

(2) Based on the mean value, the square root of the covariance constructs the 2n+1 Sigma Point set, and N is the dimension of the state variable x :

(3) The mean value of each point in the Sigma Point concentration is performed by the transformation of the non-linear observing function H , and the mean value of the observed variable is calculated as the mean value, and the covariance is confirmed as the symmetric matrix. Covariance plus covariance as the covariance of observational variables. In cartographer, see the Posetracker::addposeobservation function, the observation function H is expressed as:, which represents the scan-submap matches the resulting pose, The minus sign indicates the vector difference (that is, the translation difference and the azimuth orientation difference).

　　(4) Calculate the covariance between the predictor and the observed variable.

　　(5) calculates the Kalman gain and updates the mean and covariance of the system state variable x as modified after the observed process based on the Kalman gain.

(iv) the core algorithm behind

See Baidu Library "UKF Filter Algorithm", the site is http://wenku.baidu.com/link?url=er-Uo_5unEZGIcyg14wzZydaB5cqaUE-yAQddxCC7rS_ Rbtbbtsfgtvkvetrwzts30b3bykffqgaozquxrt-mkfd3yibchmfhs0bktjjocy.

Need PDF format with full formula, please e-mail:[email protected]

UKF filter resolution used in Google Open source slam software cartographer