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