EKF-SLAM: Shrink covariance matrix on one direction

Question

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.

Answer 1

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.