4.5.3 扩展Kalman程序及计算结果