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.