Derivation of EKF slam landmark initialization

Question

I have read a couple posts which gives the formula for initializing a new landmark in EKF slam but the derivation is not given.

EKF-SLAM initialize new landmark in covariance matrix

Could someone sketch how it is done?

Answer 1

You simply expand the size of your state vector and covariance matrix.

Lets say our current state is simply a 2D robot pose.

$$s = \begin{bmatrix} x \\ y \\ \theta \end{bmatrix}$$

which will have a covariance matrix of

$$cov = \begin{bmatrix} \sigma_{xx} & \sigma_{xy} & \sigma_{x\theta} \\ \sigma_{yx} & \sigma_{yy} & \sigma_{y\theta} \\ \sigma_{\theta x} & \sigma_{\theta y} & \sigma_{\theta \theta} \end{bmatrix}$$

Now we want to add our new landmark($l_x,l_y$) to our state vector so we simply append it to our current one. The first values of $l_x,l_y$ would come from some sort of initialization routine.

$$s* = \begin{bmatrix} x \\ y \\ \theta \\ l_x \\ l_y \end{bmatrix}$$

And we augment our covariance matrix also to a new size.

$$cov* = \begin{bmatrix} \sigma_{xx} & \sigma_{xy} & \sigma_{x\theta} & \sigma_{x,l_x} & \sigma_{x,l_y} \\ \sigma_{yx} & \sigma_{yy} & \sigma_{y\theta} & \sigma_{y,l_x} & \sigma_{y,l_y} \\ \sigma_{\theta x} & \sigma_{\theta y} & \sigma_{\theta \theta} & \sigma_{\theta,l_x} & \sigma_{\theta,l_y} \\ \sigma_{l_x, x} & \sigma_{l_x, y} & \sigma_{l_x, \theta} & \sigma_{l_x,l_x} & \sigma_{l_x,l_y} \\ \sigma_{l_y, x} & \sigma_{l_y, y} & \sigma_{l_y, \theta} & \sigma_{l_y,l_x} & \sigma_{l_y,l_y} \end{bmatrix}$$

One can notice that the 3x3 top left block is the same as our previous covariance, so those values are just copied over. So we just need to determine the cross covariances( values like $\sigma_{x,l_x}$) and the variance ($\sigma_{l_x,l_x}$) of the landmark.

The cross covariances are simply set to 0. Since we don't know how the landmark currently correlates with the rest of the state. You don't have to worry about them because as the Kalman filter continues to run they will update automatically.

The landmark variances ($\sigma_{l_x,l_x}$ and $\sigma_{l_y,l_y}$) is dependent on your application. Things like sensor type, sensor quality, how you are computing landmark pose all play a role. Generally people will just manually guess a desired value. So something like we generally believe our landmark initialization is about 10cm accurate so $\sigma_{l_x,l_x}=\sigma_{l_y,l_y}=0.1$. You can increase it our decrease it depending on your confidence in the landmark initialization process. You can also tune it by running your filter on the same dataset multiple times, and seeing which values perform the best.

So your new covariance matrix on the first initialization would look something like this(assuming we use the 10cm of error for the landmark init).

$$cov* = \begin{bmatrix} \sigma_{xx} & \sigma_{xy} & \sigma_{x\theta} & 0 & 0 \\ \sigma_{yx} & \sigma_{yy} & \sigma_{y\theta} & 0 & 0 \\ \sigma_{\theta x} & \sigma_{\theta y} & \sigma_{\theta \theta} & 0 & 0 \\ 0 & 0 & 0 & 0.1 & 0 \\ 0 & 0 & 0 & 0 & 0.1 \end{bmatrix}$$

If we were to add even more landmarks to the state then we would do the same thing. Increase the size of the state vector. Copy over your old covariance matrix to the new one. Set the cross correlations of your new landmark to 0, and set some initial uncertainty values. This can be visualized in block matrix form as seen below.

$$modified\:cov = \begin{bmatrix} old\:cov & 0 \\ 0 & landmark\:cov \end{bmatrix}$$

$old\:cov$ is our old covariance matrix copied over. The 0s are all the cross correlation terms, and the $landmark\:cov$ is just the covariance of the single new landmark.

$$landmark\:cov = \begin{bmatrix} \sigma_{l_x,l_x} & 0 \\ 0 & \sigma_{l_y,l_y} \end{bmatrix}$$

You can see this in action in this EKF slam in this project.