1
$\begingroup$

I've implemented a simple robot simulation based on the equations from EKF localization with known correspondence found in Probabilistic Robotics by Thrun et. al. Everything seems to be working but I noticed the covariance matrix has an odd behavior when doing prediction only.

When I move the robot with a forward velocity and some angular velocity and no correction the covariance grows (size of eigenvalues), as expected. But when I move the robot backwards with some angular velocity the covariance shrinks a bit before growing again. I expect the covariance to always increase if there is no correction.

I checked my implementation many times for errors but I now suspect this issue may be attributed to the fact that the Jacobians V and M uses signed values of velocity and angular velocity, instead of absolute?

Here is a video showing the covariance shrinking and growing. In this video the true pose of the robot is in green with an imaginary depth sensor shown as a green cone. There are no landmarks, so no correction step. The gray is the estimated pose plus covariance ellipse at 95% confidence interval.

https://www.youtube.com/watch?v=RcHkCijyG7c&feature=youtu.be

UPDATE: I've attached a graph that illustrates this issue better. Two graphs are shown below. The graph in red is running the EKF prediction on noise free input and plotting the area of the ellipsoid of the covariance. The area (uncertainty) monotonically increases as expected. I repeat the same thing but at t=10 I inverted the velocity, resulting in the blue graph. There is an oscillation in the area for some reason yet to be determined.

enter image description here

The Octave script I wrote can be found here http://pastebin.com/rQyczVbm

$\endgroup$
2
  • $\begingroup$ Welcome to Robotics Stack Exchange. When it comes to simulation, it is a good thing to add some numbers, plots, results, ...,etc. Adding a minimal working example is highly recommended. It is hard to tell what exactly the thing(s) that causes what you believe the unexpected results. $\endgroup$
    – CroCo
    Feb 12 '17 at 19:24
  • $\begingroup$ Thanks for the feedback. I've updated my post to include a video. $\endgroup$
    – Nghia
    Feb 13 '17 at 23:58

Your Answer

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

Browse other questions tagged or ask your own question.