Porting Kalman to mega2560