Grewal Andrews.Kalman filtering..theory and practice using MATLAB2ed.Wiley 卡尔曼滤波算法及matlab实现里面有大量理论方程推导更重要的是有相应的matlab程序参考