Kalman working + code cleaning