ad_kalmanf.rar_GPS matlab_Kalman 抗_gps抗差_抗差滤波_抗差自适应

for j=1:300 %--观测次数 x(1,j)=0,y(1,j)=10; %侦察机坐标 x(2,j)=6,y(2,j)=0; x(3,j)=12,y(3,j)=0; x(4,j)=20+0.0441*j,y(4,j)=30+sqrt(x(4,j)-20); %观测间隔10s,X方向速度恒为16km/h % if(j<=150) % x(4,j)=20+0.0654*j ,y(4,j)=30+0.0654*j; %时间间隔5s % else % x(4,j)=20+0.0825*j ,y(4,j)=30+0.0654*150; % end b=0.0027778; HV(:,:)=[b,b,b,b,b,b,b,b]'; for i=1:3 v(i,j)=normrnd(0,0.00524);%--观测噪声 h(i,j)=atan((y(4,j)-y(i,j))/(x(4,j)-x(i,j))); a(i,j)=h(i,j)+v(i,j); %-----观测值 end %---迭代初值的计算，用算术平均法 x(5,j)=(y(1,j)-y(2,j)-x(1,j)*tan(a(1,j))+x(2,j)*tan(a(2,j)))/(tan(a(2,j))-tan(a(1,j))); y(5,j)=(y(1,j)*tan(a(2,j))-y(2,j)*tan(a(1,j))-x(1,j)*tan(a(1,j))*tan(a(2,j))+x(2,j)*tan(a(1,j))*tan(a(2,j)))/(tan(a(2,j))-tan(a(1,j))); x(6,j)=(y(2,j)-y(3,j)-x(2,j)*tan(a(2,j))+x(3,j)*tan(a(3,j)))/(tan(a(3,j))-tan(a(2,j))); y(6,j)=(y(2,j)*tan(a(3,j))-y(3,j)*tan(a(2,j))-x(2,j)*tan(a(2,j))*tan(a(3,j))+x(3,j)*tan(a(2,j))*tan(a(3,j)))/(tan(a(3,j))-tan(a(2,j))); x(7,j)=(y(3,j)-y(1,j)-x(3,j)*tan(a(3,j))+x(1,j)*tan(a(1,j)))/(tan(a(1,j))-tan(a(3,j))); y(7,j)=(y(3,j)*tan(a(1,j))-y(1,j)*tan(a(3,j))-x(3,j)*tan(a(3,j))*tan(a(1,j))+x(1,j)*tan(a(3,j))*tan(a(1,j)))/(tan(a(1,j))-tan(a(3,j))); x(8,j)=(x(5,j)+x(6,j)+x(7,j))/3; y(8,j)=(y(5,j)+y(6,j)+y(7,j))/3; %--最小二乘法 X(:,:,j)=[x(8,j) y(8,j)]'; n=1; while(n<50 ) h(1,j)=atan((X(2,1,j)-y(1,j))/(X(1,1,j)-x(1,j))); h(2,j)=atan((X(2,1,j)-y(2,j))/(X(1,1,j)-x(2,j))); h(3,j)=atan((X(2,1,j)-y(3,j))/(X(1,1,j)-x(3,j))); ZK(:,:,j)=[a(1,j)-h(1,j);a(2,j)-h(2,j); a(3,j)-h(3,j)]; r1=(y(8,j)-y(1,j))^2+(x(8,j)-x(1,j))^2; r2=(y(8,j)-y(2,j))^2+(x(8,j)-x(2,j))^2; r3=(y(8,j)-y(3,j))^2+(x(8,j)-x(3,j))^2; H=[-(y(8,j)-y(1,j))/r1,(x(8,j)-x(1,j))/r1;-(y(8,j)-y(2,j))/r2 (x(8,j)-x(2,j))/r2 ;-(y(8,j)-y(3,j))/r3 (x(8,j)-x(3,j))/r3]; W=inv(H'*H)*H'*ZK(:,:,j); X(:,:,j)=X(:,:,j)+W; n=n+1; end if(j>=3) if(j<9) RATEX=(X(1,1,2)-X(1,1,1))/b; RATEY=(X(2,1,2)-X(2,1,1))/b; else MX(:,:)=[(X(1,1,j-7)-X(1,1,j-8)),(X(1,1,j-6)-X(1,1,j-7)),(X(1,1,j-5)-X(1,1,j-6)),(X(1,1,j-4)-X(1,1,j-5)),(X(1,1,j-3)-X(1,1,j-4)),(X(1,1,j-2)-X(1,1,j-3)),(X(1,1,j-1)-X(1,1,j-2)),(X(1,1,j)-X(1,1,j-1))]'; RATEX=inv(HV'*HV)*HV'*MX; MY(:,:)=[(X(2,1,j-7)-X(2,1,j-8)),(X(2,1,j-6)-X(2,1,j-7)),(X(2,1,j-5)-X(2,1,j-6)),(X(2,1,j-4)-X(2,1,j-5)),(X(2,1,j-3)-X(2,1,j-4)),(X(2,1,j-2)-X(2,1,j-3)),(X(2,1,j-1)-X(2,1,j-2)),(X(2,1,j)-X(2,1,j-1))]'; RATEY=inv(HV'*HV)*HV'*MY; end %--卡尔曼滤波 % b=0.0027778;%10秒钟 S(:,:,2)=[X(1,1,2) (X(1,1,2)-X(1,1,1))/b X(2,1,2) (X(2,1,2)-X(2,1,1))/b]';%--初值 lx=1.5 ,ly=2.0 ,lz=4.57 ,r=0.4; %lx=0.15 ,ly=0.20 ,lz=0.457 ,r=0.4; P(:,:,2)=[lx^2 lx^2/b r*lx*ly r*lx*ly/b; lx^2/b lz^2+2*lx^2/b^2 r*lx*ly/b 2*r*lx*ly/b^2 ; r*lx*ly r*lx*ly/b ly^2 ly^2/b; r*lx*ly/b 2*r*lx*ly/b^2 ly^2/b lz^2+2*ly^2/b^2];%--初始协方差 A=[1 b 0 0;0 1 0 0;0 0 1 b;0 0 0 1];%--状态转移阵 Q=[0 0 0 0;0 lz^2 0 lz^2; 0 0 0 0;0 lz^2 0 lz^2]; C=[1 0 0 0;0 0 1 0];%--观测阵 R=[lx^2 r*lx*ly;r*lx*ly ly^2]; Z=X; P(:,:,j)=A*P(:,:,j-1)*A'+Q; K(:,:,j)=P(:,:,j)*C'*inv(C*P(:,:,j)*C'+R); S(:,:,j)=A*S(:,:,j-1)+K(:,:,j)*[Z(:,:,j)-C*A*S(:,:,j-1)]; % v1=(S(1,1,j)-S(1,1,j-1))/b; % v2=(S(3,1,j)-S(3,1,j-1))/b; S(2,1,j)=RATEX; S(4,1,j)=RATEY; P(:,:,j)=P(:,:,j)-K(:,:,j)*C*P(:,:,j); end end %---作图 xxxxx=reshape(x(8,:),1,300); yyyyy=reshape(y(8,:),1,300); plot(xxxxx,yyyyy,'bo'); title('直接算术平均得出的结果'); hold on xxx=reshape(x(4,:),1,300); yyy=reshape(y(4,:),1,300); plot(xxx,yyy,'r'); hold off figure S(:,:,1)=[X(1,1,1) 0 X(2,1,1) 0]'; xx=reshape(S(1,1,:),1,300); yy=reshape(S(3,1,:),1,300); plot(xx,yy,'b+',xxx,yyy,'r'); title('卡尔曼滤波结果'); figure xxxx=reshape(X(1,1,:),1,300); yyyy=reshape(X(2,1,:),1,300); plot(xxxx,yyyy,'b+',xxx,yyy,'r'); title('最小二乘法结果'); figure k4=polyfit(xx,yy,4); y4=polyval(k4,xx); plot(xx,y4,'b',xxx,yyy,'r'); title('卡尔曼滤波拟合曲线'); figure k5=polyfit(xxxx,yyyy,4); y5=polyval(k5,xxxx); plot(xxxx,y5,'b',xxx,yyy,'r'); title('最小二乘法拟合曲线') figure k6=polyfit(xxxxx,yyyyy,4); y6=polyval(k6,xxxxx); plot(xxxxx,y6,'g',xx,y4,'b',xxxx,y5,'k',xxx,yyy,'r'); title('几种拟合曲线的比较')