Algorithm 2 Landmark estimation algorithm.

Initialisation while Robot is moving do     $dat{a}_{obs}\leftarrow Observation$     $dat{a}_{mov}\leftarrow Odometry$     if Landmark detected then         while Landmark list $\ne empty$ do            if New landmark detected then                Add to the map the new location            end if            if Old landmark detected then                Load into the state vector the old location                Update system LPV matrices (22) )                LPV-KF $\leftarrow x_k,u_k$                LPV-KF $\to x_{k+1}$                Store new estimation            end if         end while         if No landmark detected then            $x_k=x_k[1:6]$            LPV-KF $\leftarrow x_k,u_k$            LPV-KF $\to x_{k+1}$         end if     end if     Update robot position, and wait until next movement end while