finally got pseudo-switching kalman filter running