Started implementation of particle filters.
Added support for encoders and direct drive mode