扩展卡尔曼滤波线性化及仿真

EKF滤波线性化近似模拟及仿真程序。每次迭代都会进行线性化处理，同时也带入了误差，就像惯性导航一样，这样迭代时间长了误差必然越来越大，出现发散的情况，由于本例子是一个回环运动，迭代误差并没有那么明显，当物体做非回环运动时迭代发散情况就特别明显。所以在每次线性化近似时，都要进行卡尔曼估计以减小因线性近似时而带入的误差累积，即每次迭代结束后的卡尔曼估计值作为下一次迭代线性化近似的初始值。