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:

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