Improve unit tests for correctness of Kalman filter results
Following #738, we still need to
- Fix all bugs that result in unit tests crashing
- Improve unit tests to compare likelihood results for consistency
- Update and enable other unit tests for Kalman filter in kalman/likelihood directory