Add unit tests for Kalman filter calls from Matlab