基于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/180