11.3: EKF SLAM

The key idea in EKF SLAM is to extend the state vector from the robot’s position to contain the position of all features. Thus, the state

\[\boldsymbol{x̂}_{k'|k-1}=(x,y,\theta )^{T}\]

becomes

\[\boldsymbol{x̂}_{k}=(x,y,\theta ,\alpha _{1},...,\alpha _{N},r_{N})^{T}\]

assuming N features, which is a (3 + 2N)x1 vector. The action update (or “prediction update”) is identical to that if features are already known; the robot simply updates its position using odometry and updates the variance of it s position using error propagation. The covariance matrix is now a (3+2N)x(3+2N) matrix that initially holds the variances on position and those of each feature in its diagonal.

The interesting things happen during the perception update. Here it is important that only one feature is observed at a time. Thus, if the robot observes multiple features at once, one needs to do multiple, consecutive perception updates. Care needs to be taken that the matrix multiplications work out. In practice you will need to set only those values of the observation vector (a (3+2N)x1 vector) that correspond to the feature that you observe. Similar considerations apply to the observation function and its Jacobian.

---