扩展卡尔曼滤波python代码

某物体在XY平面做运动，采样周期为1s，该运动系统的状态方程如式 (2-1) 所示， 其中,为系统的状态向量，各状态变量对应地分别表示方向的位置、方向速度、方向的位置、方向的速度。 为零均值高斯白噪声, 。 采用方位角传感器测量运动系统的方位角，作为系统的输出。系统的输出方程如式（2-2）所示： 其中 是零均值高斯白噪声，。 假设系统的初始状态, ，=0.02。 试利用扩展卡尔曼滤波理论求出的最优估计。 要求： （1）利用Matlab或Python 编写仿真程序。 （2）给出各状态变量的真值和估计值曲线变化图。 （3）分别给出的真值与估计值之间的误差曲线变化图，并求出误差的均值和方差。 （4）对滤波效果进行分析。