扩展卡尔曼滤波算法的matlab程序

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 xyerror xyerror/t;

xerror/t 2*xerror/(t^2) xyerror/t 2*xyerror/(t^2);

xyerror xyerror/t yerror yerror/t;

xyerror/t 2*xyerror/(t^2) yerror/t 2*yerror/(t^2)];

xestimate=[Zmeasure(2,i)*cos(Zmeasure(1,i)) 0 Zmeasure(2,i)*sin(Zmeasure(1,i)) 0 ]';

end

cho=(chol(P*(L+ramda)))';%

for j=1:L

xgamaP1(:,j)=xestimate+cho(:,j);

xgamaP2(:,j)=xestimate-cho(:,j);

end

Xsigma=[xestimate xgamaP1 xgamaP2];

F=A;

Xsigmapre=F*Xsigma;

xpred=zeros(Anoise,1);

for j=1:2*L+1

xpred=xpred+Wm(j)*Xsigmapre(:,j);

end

Noise1=Anoise;

ppred=zeros(Noise1,Noise1);

for j=1:2*L+1

ppred=ppred+Wc(j)*(Xsigmapre(:,j)-xpred)*(Xsigmapre(:,j)-xpred)';

end

ppred=ppred+processerror;

chor=(chol((L+ramda)*ppred))';

for j=1:L

XaugsigmaP1(:,j)=xpred+chor(:,j);

XaugsigmaP2(:,j)=xpred-chor(:,j);

end

Xaugsigma=[xpred XaugsigmaP1 XaugsigmaP2 ];

for j=1:2*L+1

Ysigmapre(1,j)=atan(Xaugsigma(3,j)/Xaugsigma(1,j)) ;

Ysigmapre(2,j)=sqrt((Xaugsigma(1,j))^2+(Xaugsigma(3,j))^2);

end

ypred=zeros(2,1);

for j=1:2*L+1

ypred=ypred+Wm(j)*Ysigmapre(:,j);

end

Pzz=zeros(2,2);

for j=1:2*L+1

Pzz=Pzz+Wc(j)*(Ysigmapre(:,j)-ypred)*(Ysigmapre(:,j)-ypred)';

end

Pzz=Pzz+measureerror;

Pxy=zeros(Anoise,2);

for j=1:2*L+1

Pxy=Pxy+Wc(j)*(Xaugsigma(:,j)-xpred)*(Ysigmapre(:,j)-ypred)';

end

K=Pxy*inv(Pzz);

xestimate=xpred+K*(Zmeasure(:,i)-ypred);

P=ppred-K*Pzz*K';

xukf(i)=xestimate(1,1);

yukf(i)=xestimate(3,1);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%% EKF PRO%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if i==1

ekf_p=[xerror xerror/t xyerror xyerror/t;

xerror/t 2*xerror/(t^2) xyerror/t 2*xyerror/(t^2);

xyerror xyerror/t yerror yerror/t;

xyerror/t 2*xyerror/(t^2) yerror/t 2*yerror/(t^2)];

ekf_xestimate=[Zmeasure(2,i)*cos(Zmeasure(1,i)) 0 Zmeasure(2,i)*sin(Zmeasure(1,i)) 0 ]';

ekf_xpred=ekf_xestimate;

end;

F=A;

ekf_xpred=F*ekf_xestimate;

ekf_ppred=F*ekf_p*F'+processerror;

H=[-ekf_xpred(3)/(ekf_xpred(3)^2+ekf_xpred(1)^2) 0 ekf_xpred(1)/(ekf_xpred(3)^2+ekf_xpred(1)^2) 0;

ekf_xpred(1)/sqrt(ekf_xpred(3)^2+ekf_xpred(1)^2)

ekf_xpred(3)/sqrt(ekf_xpred(3)^2+ekf_xpred(1)^2) 0];

ekf_z(1,1)=atan(ekf_xpred(3)/ekf_xpred(1)) ;

ekf_z(2,1)=sqrt((ekf_xpred(1))^2+(ekf_xpred(3))^2);

PHHP=H*ekf_ppred*H'+measureerror;

ekf_K=ekf_ppred*H'*inv(PHHP);

ekf_p=(eye(L)-ekf_K*H)*ekf_ppred;

ekf_xestimate=ekf_xpred+ekf_K*(Zmeasure(:,i)-ekf_z);

traceekf(i)=trace(ekf_p);

xekf(i)=ekf_xestimate(1,1);

yekf(i)=ekf_xestimate(3,1);

errorx(i)=xx(i)+xradarpositon-x(i); 0

errory(i)=yy(i)+yradarpositon-y(i);

ukferrorx(i)=xestimate(1)+xradarpositon-x(i);

ukferrory(i)=xestimate(3)+yradarpositon-y(i);

ekferrorx(i)=ekf_xestimate(1)+xradarpositon-x(i); ekferrory(i)=ekf_xestimate(3)+yradarpositon-y(i);

aa(i)=xx(i)+xradarpositon-x(i);;

bb(i)=yy(i)+yradarpositon-y(i);

sumx=sumx+(errorx(i)^2);

sumy=sumy+(errory(i)^2);

sumxukf=sumxukf+(ukferrorx(i)^2);

sumyukf=sumyukf+(ukferrory(i)^2);

sumxekf=sumxekf+(ekferrorx(i)^2);

sumyekf=sumyekf+(ekferrory(i)^2);

mseerrorx(i)=sqrt(sumx/(i-1));%噪声的统计均方误差 mseerrory(i)=sqrt(sumy/(i-1));

mseerrorxukf(i)=sqrt(sumxukf/(i-1));%UKF的统计均方误差 mseerroryukf(i)=sqrt(sumyukf/(i-1));

mseerrorxekf(i)=sqrt(sumxekf/(i-1));%EKF的统计均方误差 mseerroryekf(i)=sqrt(sumyekf/(i-1));

end

figure(1);

plot(mseerrorxukf,'r');

hold on;

plot(mseerrorxekf,'g');

hold on;

plot(mseerrorx,'.');

hold on;

ylabel('MSE of X axis','fontsize',15);

xlabel('sample number','fontsize',15);

legend('UKF','EKF','measurement error');

figure(2)

plot(mseerroryukf,'r');

hold on;

plot(mseerroryekf,'g');

hold on;

plot(mseerrory,'.');

hold on;

ylabel('MSE of Y axis','fontsize',15); xlabel('sample number','fontsize',15); legend('UKF','EKF','measurement error');

figure(3)

plot(x,y);

hold on;

plot(xekf,yekf,'g');

hold on;

plot(xukf,yukf,'r');

hold on;

plot(xx,yy,'m');

ylabel(' X ','fontsize',15);

xlabel('Y','fontsize',15);

legend('TRUE','UKF','EKF','measurements');