clear;
clc;
T=1;
num=50;
N0=400/T;
N1=600/T;
a=1/60;
SIGMA=10000;
SIGMAa=0.25;
x=zeros(N1,1);
y=zeros(N1,1);
vx=zeros(N1,1);
vy=zeros(N1,1);
x(1)=-10000;y(1)=2000;
vx(1)=15;vy(1)=0;
ax=0;ay=0;
var=100;
for i=1:N1-1
if(i>N0-1&&i<=N1-1)
ax=-0.075;ay=0.075;
vx(i+1)=vx(i)+ax*T;
vy(i+1)=vy(i)+ax*T;
else
ax=0;ay=0;
vx(i+1)=vx(i);
vy(i+1)=vy(i);
end
x(i+1)=x(i)+vx(1)*T;
y(i+1)=y(i)+vy(i)*T;
end
rex(num,N1)=0;
rey(num,N1)=0;
%
for m=1:num
nx=100*randn(N1,1);
ny=100*randn(N1,1);
zx=x+nx;
zy=y+ny;
rex(m,1)=-10000;
rey(m,1)=2000;
rex(m,2)=-9960;
rey(m,2)=2000;
ki=0;
low=1;high=0;
u=0;ua=0;
e=0.8;
xks(1)=zx(1);
yks(1)=zy(1);
xks(2)=zx(2);
yks(2)=zy(2);
o=[1,T,(-1+a*T+exp(-a*T))/(a*a),0,0,0; 0,1,(1-exp(-a*T))/a,0,0,0; 0,0,exp(-a*T),0,0,0;
0,0,0,1,T,(-1+a*T+exp(-a*T))/(a*a); 0,0,0,0,1,(1-exp(-a*T))/a; 0,0,0,0,0,exp(-a*T)];
Q=2*a*SIGMAa*[(1-exp(-2*a*T)+2*a*T+2*a^3*T^3/3-2*a^2*T^2-4*a*T*exp(-a*T))/(2*a^5),(exp(-2*a*T)+1-2*exp(-a*T)+2*a*T*exp(-a*T)-2*a*T+a^2*T^2)/(2*a^4),(1-exp(-2*a*T)-2*a*T*exp(-a*T))/(2*a^3),0,0,0;
(exp(-2*a*T)+1-2*exp(-a*T)+2*a*T*exp(-a*T)-2*a*T+a^2*T^2)/(2*a^4),(4*exp(-a*T)-3-exp(-2*a*T)+2*a*T)/(2*a^3),(exp(-2*a*T)+1-2*exp(-a*T))/(2*a^2),0,0,0;
(4*exp(-a*T)-3-exp(-2*a*T)+2*a*T)/(2*a^3),(exp(-2*a*T)+1-2*exp(-a*T))/(2*a^2),(1-exp(-2*a*T))/(2*a),0,0,0;
0,0,0,(1-exp(-2*a*T)+2*a*T+2*a^3*T^3/3-2*a^2*T^2-4*a*T*exp(-a*T))/(2*a^5),(exp(-2*a*T)+1-2*exp(-a*T)+2*a*T*exp(-a*T)-2*a*T+a^2*T^2)/(2*a^4),(1-exp(-2*a*T)-2*a*T*exp(-a*T))/(2*a^3);
0,0,0,(exp(-2*a*T)+1-2*exp(-a*T)+2*a*T*exp(-a*T)-2*a*T+a^2*T^2)/(2*a^4),(4*exp(-a*T)-3-exp(-2*a*T)+2*a*T)/(2*a^3),(exp(-2*a*T)+1-2*exp(-a*T))/(2*a^2);
0,0,0,(4*exp(-a*T)-3-exp(-2*a*T)+2*a*T)/(2*a^3),(exp(-2*a*T)+1-2*exp(-a*T))/(2*a^2),(1-exp(-2*a*T))/(2*a)];
h=[1,0,0,0,0,0;0,0,0,1,0,0];
perr=[SIGMA,SIGMA/T,0,0,0,0;SIGMA/T,2*SIGMA/(T^2)+SIGMAa*(2-a^2*T^2+2*a^3*T^3/3-2*a*exp(-a*T))/(a^4*T^2),SIGMAa*(exp(-a*T)+a*T-1)/(a^2*T),0,0,0;
0,SIGMAa*(exp(-a*T)+a*T-1)/(a^2*T),SIGMAa,0,0,0;0,0,0,SIGMA,SIGMA/T,0;
0,0,0,SIGMA/T,2*SIGMA/(T^2)+SIGMAa*(2-a^2*T^2+2*a^3*T^3/3-2*a*exp(-a*T))/(a^4*T^2),SIGMAa*(exp(-a*T)+a*T-1)/(a^2*T);
0,0,0,0,SIGMAa*(exp(-a*T)+a*T-1)/(a^2*T),SIGMAa];
vx=(zx(2)-zx(1))/2;vy=(zy(2)-zy(1))/2;
xk=[zx(1);vx;ax;zy(1);vy;ay];
for r=3:N1
z=[zx(r);zy(r)];
xk1=o*xk;
perr1=o*perr*o'+Q;
k1=perr1*h'*inv(h*perr1*h'+SIGMA*eye(2));
xk=xk1+k1*(z-h*xk1);
perr=(eye(6)-k1*h)*perr1;
xks(r)=xk(1,1);
yks(r)=xk(4,1);
vxks(r)=xk(2,1);
vyks(r)=xk(5,1);
xk1s(r)=xk1(1,1);
yk1s(r)=xk1(4,1);
vxk1s(r)=xk1(2,1);
vyks1(r)=xk1(5,1);
perr11(r)=perr(1,1);
perr12(r)=perr(1,2);
perr22(r)=perr(2,2);
rex(m,r)=xks(r);
rey(m,r)=yks(r);
end
end
ex=0;ey=0;
eqx=0;eqy=0;
ex1(N1,1)=0;ey1(N1,1)=0;
qx(N1,1)=0;qy(N1,1)=0;
for i=1:N1
for j=1:num
ex=ex+x(i)-rex(j,i);
ey=ey+y(i)-rey(j,i);
eqx=eqx+(x(i)-rex(j,i))^2;
eqy=eqy+(y(i)-rey(j,i))^2;
end
ex1(i)=ex/num;
ey1(i)=ey/num;
qx(i)=sqrt((double(eqx)/num-(ex1(i)^2)));
qy(i)=sqrt((double(eqy)/num-(ey1(i)^2)));
ex=0;eqx=0;ey=0;eqy=0;
end
figure;
plot(x,y,'k-',zx,zy,'g:',xks,yks,'r-.');
legend('真实轨迹','观测轨迹','估计轨迹');
figure;
plot(x,qx);
legend('x轴方向误差');
figure;plot(y,qy);
legend('y轴方向误差');
