4
$\begingroup$

I have implemented an EKF on a mobile robot (x,y,theta coordinates), but now I've a problem. When I detect a landmark, I would like to correct my estimate only on a defined direction. As an example, if my robot is travelling on the plane, and meets a landmark with orientation 0 degrees, I want to correct the position estimate only on a direction perpendicular to the landmark itself (i.e. 90 degrees).

This is how I'm doing it for the position estimate:

  • I update the x_posterior as in the normal case, and store it in x_temp.
  • I calculate the error x_temp - x_prior.
  • I project this error vector on the direction perpendicular to the landmark.
  • I add this projected quantity to x_prior.

This is working quite well, but how can I do the same for the covariance matrix? Basically, I want to shrink the covariance only on the direction perpendicular to the landmark.

Thank you for your help.

$\endgroup$
  • $\begingroup$ I highly doubt that the orientation can be exactly zero due to noise. You probably need to elaborate more about your ultimate objective. Covariance matrix holds the uncertainty and the correlation about the elements of the state vector, therefore, updating some elements affect the whole covariance matrix. $\endgroup$ – CroCo Oct 8 '15 at 1:37
0
$\begingroup$

Here is my solution, that is just an approximation but it is working quite well.

Essentially this is what I do:

  • I store the a priori covariance matrix in $P_{old}$.
  • I execute the normal (uncorrected) update step, and store the covariance matrix in $P_{new}$.
  • Given the direction of the landmark $d$, I compute the parallel component (to this direction $d$) of the axis of the ellipse represented by $P_{old}$. Then, I select the component with maximum length and store it in $a_{par}$.
  • I compute the perpendicular component (to the same direction $d$) of the axis of the ellipse represented by $P_{new}$. Then, I select the component with maximum length and store it in $a_{perp}$.
  • Finally I generate the corrected ellipse from the 2 axis $a_{par}$ and $a_{perp}$ and store the covariance matrix in $P$.

This is represented in the following example (direction of the landmark $d$ = 0°) figure, where the red ellipse is represented by $P_{old}$, the green ellipse by $P_{new}$, and the blue ellipse is the corrected one represented by $P$, where only the component perpendicular to $d$ has been shrunk.

enter image description here

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.