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