基于GPS_IMU组合定位的kalman滤波

基于GPS_IMU组合定位的kalman滤波
clearallN=100;T=4*pi/N;t=0:4*pi/N:4*pi-T;w=2*pi/(24*3600);X1=zeros(15,N);X2=zeros(15,N);L=zeros(6,N);X2(:,1)=[1,0,0,0,0,0,0,0,0,0,0,0,0,0,0]X1(:,1)=X2(:,1);E=eye(15);W=[0-w0;w00;000];A=zeros(15,15);A(1:3,4:6)=eye(3);A(4:6,4:6)=-2*W;A(7:9,7:9)=-W;fori=10:12A(i,i)=-1/7200;endfori=13:15A(i,i)=-1/1800;endA=eye(15)+A*T+A*A*(T.^2)/2;Z1=zeros(15,15);Z2=eye(15);R=eye(6);Q=zeros(15,15);Q(15,15)=1;K=zeros(15,6);H=zeros(6,15);fori=1:6H(i,i)=1;endfori=1:NL(:,i)=zeros(6,1);L(1,i)=randn(1);endfori=2:NX1(:,i)=A*X2(:,i-1);Z1=A*Z2*A'+Q;K=Z1*H'*inv(H*Z1*H'+R);X2(:,i)=X1(:,i)+K*(L(:,i)-H*X1(:,i));Z2=[E-K*H]*Z1;endplot(t,L(1,:),'g*');holdon;plot(t,X1(1,:),'r*');