| Require: waypoints |
| Global //global variables |
| = Initialization (0); //initialize global variables |
| Threshold = 8; //The limited number of active features |
| |
| WHILE waypoints not 0DO //main loop |
| , , , ); //new vehicle pose |
| Add random noise to nominal control values: |
| //Motion model |
| Predict_FEJ(, , , ) //prediction |
| Predict state |
| Compute Equation (37) //estimate at |
| Compute Ψ Equation (12) with |
| Predict information matrix Equation (10) |
| Predict information vector Equation (11) |
| Mean recovery: |
| Get observations : set of rang-bearing observation |
| Add random measurement noise |
| //Data association |
| //: index tags for each landmark; |
| ; |
| //Update step |
| IF not empty THEN // is not a new feature |
| Observe_Update(, , ) |
| FOR from 1 to size(, 2) DO |
| Compute Equation (38) |
| Compute Equation (15) |
| Compute Equation (13) |
| Compute Equation (14) |
| END FOR |
| END IF |
| //Augmentation step |
| IF not empty THEN //new feature |
| SEIF_Augment() |
| Compute at |
| Compute Equation (21) |
| Compute Equation (19) |
| Compute Equation (20) |
| END IF |
| Recover mean |
| IF the number of active features > Threshold THEN |
| SEIF_Sparse(); //sparsification |
| END IF |
| END WHILE |
