the uncertainty of initializing new landmark in EKF-SLAM

Question

In EKF-SLAM (based-feature map) once the robot senses a new landmark, it is augmented to state vector. As a result, the size of the state vector and the covariance matrix are expanded. My question is about the uncertainty of the new landmark and its correlation with other pairs of the covariance matrix. How should I assign them? When I assign them to be zero, the error of the estimation this landmark won't change as time goes. If I assign them with very large value, the estimation is getting better every time the robot reobserves this landmark however, the error approaches to fixed value not to zero. I assume the problem id with assigning the uncertainty. Any suggestions?

Answer 1

You can use the Jacobians of the inverse observation model to initialize the new row/column of the covariance matrix.

Suppose your observation model is $g(\mathbf{x})$, which maps your state $\mathbf{x}$ to a predicted observation $\hat{\mathbf{z}}$. The inverse observation model $g^{-1}(\mathbf{x}, \tilde{\mathbf{z}})$ maps an observation $\tilde{\mathbf{z}}$ to a new entry of your state. For example, if $\tilde{\mathbf{z}}$ is a range and bearing measurement, $g^{-1}$ might determine the global $(x,y)$ coordinates of a newly observed landmark given your current state estimate.

Let $\mathbf{P}$ be the covariance of your state and let $\mathbf{G}_x$ and $\mathbf{G}_z$ be the Jacobians of $g^{-1}$ with respect to $\mathbf{x}$ and $\tilde{\mathbf{z}}$. Then the updated state and covariance matrix after adding the new entry is

$\mathbf{x}\gets \begin{bmatrix} \mathbf{x} \\ g^{-1}(\mathbf{x}, \tilde{\mathbf{z}}) \end{bmatrix}, \qquad \mathbf{P}_{k+1} = \begin{bmatrix} \mathbf{P}_k & \mathbf{P}_k\mathbf{G}_x^\top \\ \mathbf{G}_x\mathbf{P}_k & \mathbf{G}_x\mathbf{P}_k\mathbf{G}_x^\top + \mathbf{G}_z\mathbf{R}_k\mathbf{G}_z^\top \end{bmatrix}$,

where $\mathbf{R}_k$ is the variance (1D) or covariance $\mathbf{z}$.

Answer 2

I don't have experience with SLAM specifically, but I do have a lot of EKF experience so here's my input.

Stick with your second approach. It is normal for the error to approach a non-zero steady-state value. This number can be calculated analytically for nominal cases, but I don't remember the math right now and would probably be too complicated anyhow once you start to include the covariances with all the other landmarks.

Don't worry too much about what specific value you should initialize the covariance to. In the simplest case, just set the variance to some nominal value based on your sensor noise level and use 0 covariance with the other landmarks. Over time the EKF update steps should adjust things to their "proper" values (assuming your initial values were at least in the ballpark).