Relating EKF Localization with EKF Equations

23 Views Asked by At

I have been trying to understand EKF localization from Probabilistic Robotics by Thrun Burgard and Fox.

There the covariance prediction is given by $$\overline{\Sigma }_t=G_t\Sigma_{t-1}G^T_t+V_tM_{t-1}V^T$$

Where $G_t$ is the Jacobian matrix of the motion model $g(x_{t-1},u_t)$ with respect to $x_{t-1}$ and evaluated at $(\mu_{t-1},u_t)$ and $V_t$ is the Jacobian matrix of the motion model $g(x_{t-1},u_t)$ with respect to $u_{t}$ and evaluated at $(\mu_{t-1},u_t)$ and $M_t$ is the covariance motion noise which is a function of the actions.

I have been trying to relate that covariance prediction equation with the original equation derived earlier in the book $$\overline{\Sigma }_t=G_t\Sigma_{t-1}G^T_t+R_t$$ with no success.

The authors mention that we used the first formulation because we had to map the noise from the control space to the state space but I don't see how it follows from the original EKF derived earlier.

What further confuses me is that there are other EKF formulations which assume that $g$ could be nonlinear in the noise and replace $V$ with the Jacobian matrix of $g(x_{t-1},u_t)$ with respect to the noise (and not the actions as in here.)

1

There are 1 best solutions below

0
On

The update of the pose covariance matrix totally depends on the motion model.

Compare this motion model $$ \hat{x}_k=f(x_{k-1},u_{k-1}, w_{k-1}) = x_{k-1}\oplus(u_{k-1}+w_{k-1}) $$ with this one $$ \hat{x}_k=f(x_{k-1},u_{k-1}, w_{k-1}) = (x_{k-1}\oplus u_{k-1})+w_{k-1} $$

(where $\oplus$ means pose composition).

If you're using the first one, then you have to compute your matrix $V$ as $V=\partial f/ \partial w$.

If you're using the second one, then $V=I$ and you can just skip it.

I hope this helps.