上研究生时写的扩展卡尔曼滤波程序（EKF），带测试例子，可编译执行-When the graduate writing program of the extended Kalman filter (EKF), with the test example, the implementation can be compiled