Kalman Filter Merged code --- WIP

Merged Kalman Filter code. Test cycle to be started on top of this
build.