Kalman Filetering theory and practice using MATLAB - Grewal,Andrew S.