基于GPS_IMU组合定位的kalman滤波

clear all N=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 -w 0;w 0 0;0 0 0]; A=zeros(15,15); A(1:3,4:6)=eye(3); A(4:6,4:6)=-2*W; A(7:9,7:9)=-W; for i=10:12 A(i,i)=-1/7200; end for i=13:15 A(i,i)=-1/1800; end A=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); for i=1:6 H(i,i)=1; end for i=1:N L(:,i)=zeros(6,1); L(1,i)=randn(1); end for i=2:N X1(:,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; end plot(t,L(1,:),'g*'); hold on; plot(t,X1(1,:),'r*');