%File Kalmorb.m 

%Matlab tutorial script for a linearized orbital motion with 
%random perturbations and random noise in the observation of 
%radius and angular position. 
%Innovation covariance matrix P

Pold=eye(4)*1e+3;
pause
u1=5*randn(1,500);
u2=3*randn(1,500);

v1=100*randn(1,500); 
v2=200*randn(1,500);

R=5*diag([10000 200^2])
pause



A=[0 1 0 0; .03/9 0 0 .2/3; 0 0 0 1; 0 -.2/3 0 0]

H=[1 0 0 0; 0 0 1 0]

pause

Phi=expm(A);
B=[0 1 0 0; 0 0 0 1]'; 
Q=.01*expm(A/2)*B*diag([25 9])*B'*expm(A/2);



%IC



xhm(:,1)=[0 0 0 0]';
xhp(:,1)=[0 0 0 0]';
x(:,1)=100*[0 0.0149 0.8939 0]';
Pk_1plus = eye(4)*1e+2;

for k=2:500, 

%process

%x - state equation
x(:,k)=Phi*x(:,k-1)+0.1*B*[u1(k-1);u2(k-1)];

%z - observation equation
z(:,k)=H*x(:,k)+[v1(k);v2(k)];

%filter 

xhm(:,k)=Phi*xhp(:,k-1);
Pnew=Phi*Pk_1plus*Phi'+Q;
pmin1(:,k)=Pnew(:,1); 
pmin2(:,k)=Pnew(:,2); 
pmin3(:,k)=Pnew(:,3); 
pmin4(:,k)=Pnew(:,4);

Knew=Pnew*H'*inv(H*Pnew*H'+R); 
K1(:,k)=Knew(:,1); 
K2(:,k)=Knew(:,2); 
K=Knew; 
Pkplus=(eye(4)-K*H)*Pnew; 
Pk_1plus=Pkplus; 
xhp(:,k)=xhm(:,k-1)+K*(z(:,k)-H*xhm(:,k-1)); 

end 



plot(x') 
hold
pause;
plot(xhm') 
hold off 

%end of Matlab script 



