Research Article
A Strong Tracking SLAM Algorithm Based on the Suboptimal Fading Factor
| 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 |
|
