EKF SLAM with Landmarks

In SLAM the robot builds the map and locates itself in it at the same time. EKF SLAM puts the robot pose and every landmark position into one big state vector, with one covariance matrix that also tracks how their errors are linked.

Odometry noise Sensor range

The joint state

With $N$ landmarks, the state and covariance are

$$\mathbf{x} = (x, y, \theta,\ m_{1x}, m_{1y},\ \dots,\ m_{Nx}, m_{Ny}) \qquad P \in \mathbb{R}^{(3+2N)\times(3+2N)}$$

A new landmark is added with large uncertainty. Each range-bearing reading updates the whole state through the EKF, including landmarks not being seen, because the off-diagonal terms of $P$ link their errors to the robot's.

What to notice

As the robot drives on, its own ellipse grows, and newly seen landmarks inherit that uncertainty. When it comes back and sees the first landmarks again, a loop closure, its ellipse shrinks sharply, and so do the ellipses of landmarks it has not seen for a while, because their errors were correlated with the robot's. The cost grows with the square of the number of landmarks, which limits EKF SLAM to small maps.