KalmanSetup now works, all algos run