Fix error in Kalman design matrix & test w/ PD controller