A Strong Tracking SLAM Algorithm Based on the Suboptimal Fading Factor

Pseudocode 1

STUFastSLAM algorithm.

Initialization parameters

for 𝑘 = 1 𝑡𝑜 𝑀

 % Robot state estimation

 Extract the robot position from sigma points set 𝑋_𝑡−1 (10)

 Predict mean (13) and covariance (14) of robot

 Associate observation information data

 % The calculation of fading factor

 Calculate the fading factor (4) from the prediction of the covariance , the autocovariance (21) and the cross covariance (22).

 % Introduce the fading factor

 Obtain the predicted covariance of the robot after the introduction fading factor (15)

 Obtain the autocovariance (25) and the cross-covariance (26) of the robot after the introduction fading factor

 for =known feature

  Update mean (28) and covariance (29) of the robot

  Update sigma points (30)

  Calculate importance weight (17)

 end for

 % Environmental features position estimation

 if =new feature

  Initialize new feature mean and covariance

 else

  Update mean (35) and covariance (36) of features

 end if

 for unobserved features

  ,

 end for

  Add updated {, , , } points set

end for

% Resampling strategy

for 𝑘 = 1 𝑡𝑜 𝑀

 Normalize weight and calculate (37)

 if

  Resample

 else

  Maintain the original particle weight

end for

Add new particles to

Return