For previous two weeks I was building up the framework of EM-SLAM. The framework is showed as following:

And as well as the demo by using direct observation (states can be directly observed with gaussian noise):

This demo works properly as expected (although there are some discrepency between true and estimated landmarks), however, it is based on an ideal direct observation.

I think one of the hardest sub-problem is, if we modeled sensor as radar, which measures distance and bearing relative to landmarks, i.e. d, b ~ N(u, sigma), the nonlinear transformation to direct observation will cause a very complicated pdf, even with mean and variance calculated, we still can't assume gaussian distribution after a nonlinear transformation. As a result, we lose all good properties of gaussian and will eventually diverge as time goes up.

Next step would be:

  1. use von mises distribution on all orientation estimation
  2. Use gaussian blur as an estimated pdf on orientation (this method is widely applied in image processing, but not sure it will apply here)
  3. Use UKF

Next Post Previous Post