基于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*');

clear all
N=100;
T=4*pi/N;
t=0:4*pi/N:4*pi-T;
w=2*pi/（24*3600）;
X1=zeros（15N）;
X2=zeros（15N）;
L=zeros（6N）;
X2（:1）=[100000000000000]
X1（:1）=X2（:1）;
E=eye（15）;
W=[0 -w 0;w 0 0;0 0 0];
A=zeros（1515）;
A（1:34:6）=eye（3）;
A（4:64:6）=-2*W;
A（7:97:9）=-W;
for i=10:12
    A（ii）=-1/7200;
end
for i=13:15
    A（ii）=-1/1800;
end
A=eye（15）+A*T+A*A*（T.^2）/2;
Z1=zeros（1