matlab-kalman算法对GPS接收机误差的修正 可以纠正GPS的一定误差范围

clear all close all TT=1; N=30;%观测点数 Te=1; Tn=1; De=9; Dn=9; %模拟真值，xx为位置真值,vv为速度真值,aa为加速度真值 for i=1:N xxe(i)=0; vve(i)=0; xxn(i)=0; vvn(i)=0; end aae=randn(1,N); aae=aae*1.5; aan=randn(1,N); aan=aan*1.5; xxe(1)=100; xxn(1)=30; vve(1)=5; vvn(1)=5; aae(1)=0.5; aan(1)=0.5; for k=1:N-1 vve(k+1)=vve(k)+aae(k); xxe(k+1)=xxe(k)+vve(k)+0.5*aae(k) vvn(k+1)=vvn(k)+aan(k); xxn(k+1)=xxn(k)+vvn(k)+0.5*aan(k); end %测量值，zx为位置测量值,zv为速度测量值 v1=randn(1,N); v2=randn(1,N); for k=1:N zxe(k)=xxe(k)+v1(k); zve(k)=vve(k)+v2(k); zxn(k)=xxn(k)+v1(k); zvn(k)=vvn(k)+v2(k); end %初始化各矩阵 for i=1:N xe(i)=0; xn(i)=0;%位置的估计值 ve(i)=0; vn(i)=0;%速度的估计值 ae(i)=0; an(i)=0;%加速度的估计值 P(:,:,i)=zeros(6,6); end xe(1)=100; xn(1)=30;%X(0) ve(1)=5; vn(1)=5; ae(1)=0.5; an(1)=0.5; X=[xe;ve;ae;xn;vn;an];%状态向量 FX=[100;5;0.5;30;5;0.5];%状态向量的一步预测 Z=[zxe;zve;zxn;zvn];%观测向量 P=[10,0,0,0,0,0;0,10,0,0,0,0;0,0,10,0,0,0;0,0,0,10,0,0;0,0,0,0,10,0;0,0,0,0,0,10];%估计误差方差阵 H=[1 0 0 0 0 0;0 1 0 0 0 0;0 0 0 1 0 0;0 0 0 0 1 0];%观测矩阵 R=[9,0,0,0;0,5,0,0;0,0,9,0;0,0,0,5]; %R=[1,0,0,0;0,1,0,0;0,0,1,0;0,0,0,1]; K=zeros(6,4);%卡尔曼滤波增益矩阵阵 PF=zeros(6,6);%估计误差方差的一步预测阵 I=eye(size(P));%六维的单位矩阵 Q11=De*TT.^5/10/Te; Q12=De*TT.^4/4/Te; Q13=De*TT.^3/3/Te; Q21=De*TT.^4/4/Te; Q22=2*De*TT.^2/3/Te; Q23=De*TT.^2/Te; Q31=De*TT.^3/3/Te; Q32=De*TT.^2/Te; Q33=2*De*TT/Te; Q44=Dn*TT.^5/10/Tn; Q45=Dn*TT.^4/4/Tn; Q46=Dn*TT.^3/3/Tn; Q54=Dn*TT.^4/4/Tn; Q55=2*Dn*TT.^2/3/Tn; Q56=Dn*TT.^2/Tn; Q64=Dn*TT.^3/3/Tn; Q65=Dn*TT.^2/Tn; Q66=2*Dn*TT/Tn; Q=[Q11,Q12,Q13,0,0,0;Q21,Q22,Q23,0,0,0;Q31,Q32,Q33,0,0,0; 0,0,0,Q44,Q45,Q46;0,0,0,Q54,Q55,Q56;0,0,0,Q64,Q65,Q66]; A1=[1,TT,0.5*TT.^2,0,0,0;0,1,TT,0,0,0;0,0,1,0,0,0; 0,0,0,1,TT,0.5*TT.^2;0,0,0,0,1,TT;0,0,0,0,0,1]; exp1=1/exp(TT/Te); exp2=1/exp(TT/Tn); A13=Te.^2*(TT/Te-1+exp1); A23=Te*(1-exp1); A33=exp1; A46=Tn.^2*(TT/Tn-1+exp2); A56=Tn*(1-exp2); A66=exp2; A=[1,TT,A13,0,0,0;0,1,A23,0,0,0;0,0,A33,0,0,0; 0,0,0,1,TT,A46;0,0,0,0,1,A56;0,0,0,0,0,A66]; %卡尔曼滤波 for i=2:N FX=A1*X(:,i-1);%求一步预测 PF=A*P*A'+Q; Temp=inv(H*PF*H'+R); K=PF*H'*Temp; X(:,i)=FX+K*(Z(:,i)-H*FX); R2=I-K*H; P=R2*PF; %P(:,:,i)=R2*PF*R2'+K*R*K'; end for i=2:N xe(i)=X(1,i); xn(i)=X(4,i); ve(i)=X(2,i); vn(i)=X(5,i); ae(i)=X(3,i); an(i)=X(6,i); end %输出结果 t=0:N-1; figure(1); plot(t,xxe,'b:',t,xe,'r',t,zxe,'k--'); title('东向位置估计'); xlabel('时刻(s)'); ylabel('东向位置(m)'); legend('真实值','估计值','测量值'); figure(2); plot(t,xxn,'b:',t,xn,'r',t,zxn,'k--'); title('北向位置估计'); xlabel('时刻(s)'); ylabel('北向位置(m)'); legend('真实值','估计值','测量值'); figure(3); plot(t,xe-xxe,'b:',t,xn-xxn,'r'); title('估计误差'); xlabel('时刻(s)'); ylabel('估计误差(m)'); legend('东向估计误差','北向估计误差');