扩展卡尔曼滤波（EKF）仿真

假设离散时间状态空间模型为：

                                                                                                          


                                                                

普通的扩展卡尔曼滤波公式如下：

     


     具体的代码可参考：http://blog.csdn.net/u010703122/article/details/49074297

     注：

     1、先验估计状态方程和其雅克比矩阵中的输入状态应该是上一时刻的后验估计状态；

     2、观测方程中的状态输入和其雅克比矩阵的输入状态应该是当前时刻的先验估计状态；

     3、求增益Kk时，注意矩阵逆是否可得