clear all
v=150; %%目标速度 v_sensor=0;%%传感器速度 t=1; %%扫描周期
xradarpositon=0; %%传感器坐标 yradarpositon=0; %%
ppred=zeros(4,4); Pzz=zeros(2,2); Pxx=zeros(4,2); xpred=zeros(4,1); ypred=zeros(2,1); sumx=0; sumy=0; sumxukf=0; sumyukf=0; sumxekf=0;
sumyekf=0; %%%统计的初值 L=4; alpha=1; kalpha=0; belta=2; ramda=3-L;
azimutherror=0.015; %%方位均方误差 rangeerror=100; %%距离均方误差 processnoise=1; %%过程噪声均方差
tao=[t^3/3 t^2/2 0 0; t^2/2 t 0 0; 0 0 t^3/3 t^2/2;
0 0 t^2/2 t]; %% the input matrix of process G=[t^2/2 0 t 0 0 t^2/2 0 t ];
a=35*pi/180; a_v=5/100;
a_sensor=45*pi/180; x(1)=8000; %%初始位置
y(1)=12000;
for i=1:200
x(i+1)=x(i)+v*cos(a)*t; y(i+1)=y(i)+v*sin(a)*t; end
for i=1:200 xradarpositon=0; yradarpositon=0;
Zmeasure(1,i)=atan((y(i)-yradarpositon)/(x(i)-xradarpositon))+random('Normal',0,azimutherror,1,1); Zmeasure(2,i)=sqrt((y(i)-yradarpositon)^2+(x(i)-xradarpositon)^2)+random('Normal',0,rangeerror,1,1);
xx(i)=Zmeasure(2,i)*cos(Zmeasure(1,i));%%观测值 yy(i)=Zmeasure(2,i)*sin(Zmeasure(1,i));
measureerror=[azimutherror^2 0;0 rangeerror^2]; processerror=tao*processnoise; vNoise = size(processerror,1); wNoise = size(measureerror,1);
A=[1 t 0 0; 0 1 0 0; 0 0 1 t; 0 0 0 1]; Anoise=size(A,1);
for j=1:2*L+1
Wm(j)=1/(2*(L+ramda)); Wc(j)=1/(2*(L+ramda)); end
Wm(1)=ramda/(L+ramda);
Wc(1)=ramda/(L+ramda);%+1-alpha^2+belta; %%%权值 if i==1
xerror=rangeerror^2*cos(Zmeasure(1,i))^2+Zmeasure(2,i)^2*azimutherror^2*sin(Zmeasure(1,i))^2; yerror=rangeerror^2*sin(Zmeasure(1,i))^2+Zmeasure(2,i)^2*azimutherror^2*cos(Zmeasure(1,i))^2; xyerror=(rangeerror^2-Zmeasure(2,i)^2*azimutherror^2)*sin(Zmeasure(1,i))*cos(Zmeasure(1,i)); P=[xerror xerror/t x