EKF扩展卡尔曼滤波程序

function main（）
%产生观测数据
total=3*60;%总的时间长度
global T;%采样周期
T=1;
N=total/T;%数据长度
a=20;
var_rx=100;
var_ry=100;

X=[];%观测数据
X_ideal=[];%理想数据

for i=1:N
    [rxry]=track（i*T20）;
    X_ideal=[X_ideal[rx;ry]];
    rx=rx+var_rx*randn（11）;
    ry=ry+var_ry*randn（11）;
    X=[X[rx;ry]];
end

X_filter=zeros（size（X））;%滤波后数据
X_mean=X_filter;%蒙特卡洛平均数据
Error_var=zeros（size（X））;
M=10;%蒙特卡洛仿真次数

for iCount=1:M
    X_filter=Trace（X）;
    X_mean=X_mean+X_filter;
    Error_var=Error_var+（X_ideal-X_filter）.^2;
end

X_mean=X_mean/M;
Error_var=Error_var/M;
Error_mean=X_ideal-X_mean;%误差均值
Error_var=sqrt（Error_var-Error_mean.^2）;


plot（X（1:）X（2:）X_mean（1:）X_mean（2:））;
axis equal;
legend（‘真实轨迹‘‘滤波轨迹‘）;

figure;
k=1:N;
subplot（211）plot（kError_mean（1:））;title（‘x方向误差均值‘）;xlabel（‘采样次数‘）ylabel（‘误差均值（米）‘）;
subplot（212）plot（kError_var（1:））;title（‘x方向误差标准值‘）;xlabel（‘采样次数‘）ylabel（‘误差标准值（米）‘）;

figure;
subplot（211）plot（kError_mean（2:））;title（‘y方向误差均值‘）;xlabel（‘采样次数‘）ylabel（‘误差均值（米）‘）;
subplot（212）plot（kError_var（2:））;title（‘y方向误差标准值‘）;xlabel（‘采样次数‘）ylabel（‘误差标准值（米）‘）;
    
%理想航迹方程
function [xy]=track（ta）
%   t:时间
%   x:横轴位移
%   y:纵轴位移
%   a:转弯处加速度 
%   r:初始位置
%   v:初始速度

r=[-200000]‘;
v=300+randn（11）;
w=a/v;%角速度
t1=-r（1）/v;
t2=t1+pi/w;
D=v^2/a*2;%圆周运动直径
if t<=0
   x=-20000y=0;
elseif t>0&&t<=t1
    x=r（1）+v*t;
    y=r（2）;
elseif t>t1&&t<=t2
    angel=（t-t1）*w;
    x=D/2*sin（angel）;
    y=-D*（sin（angel/2））^2;
else
    x=-v*（t-t2）;
    y=-D;
end

function R=Trace（X）
%飞行器跟踪模拟
%    X:观测数据