发布时间 : 星期一 文章扩展卡尔曼滤波算法的matlab程序(3)更新完毕开始阅读59924e3683c4bb4cf7ecd147
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) 0
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); 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');