% This script file is for example 4.4 and exam #2
%
%  The Plane Model is follows:
%
%                              X(k) = Phi*X(k-1) + G*W(k-1)
%
% The Observation Model is:
%
%                              Z(k) = H*X(k) + D*V(k)
%
% Input to this script is:
%
%   Phi         Dim = 6x6
%   G           Dim = 6x2
%   H           Dim = 2x6
%   Q           Dim = 2x2
%   R           Dim = 2x2
%   P0          Dim = 6x6
%  Role(R),T,sigma1,sigma2(s1,s2),sigmar,and sigmap(sr,sp)
%  s1,s2,sr, and sp are square value
%  s1 and s2 are for process noise
%  sr and sp are for measurement noise
%  T is sampling period             
%                       Input
I = eye(6,6);
a = input(' What would you like to evaluate? 1,2,3 Evaluating Pkp,Pkm, and Kkb ');
if (a==1|a==2)
P=input(' What Would you like to evaluate 1,2,3,4,5,6 = P(1,1),P(2,2).... ');
end
if (a==3)
Pj=input(' You are evaluating Kkb; Please Enter Kkb Row ');
Pk=input(' You are evaluating Kkb; Please Enter Kkb Columm ');
end
e = input('Would you like to Plot or save to file? 1=save,2=plot ');
if a == 1
% Initialized Pkp vector to 16x1 
 b =2;
 c =16;
 Pkpp5=zeros(16,1);
 Pkpp10=zeros(16,1);
 Pkpp15=zeros(16,1);
end
% Initialized Pkm vector to 15x1
if a == 2
% input(' You are evaluating Pkm');
 b =1;
 c =15;
 Pkpp5=zeros(15,1);
 Pkpp10=zeros(15,1);
 Pkpp15=zeros(15,1);
end
if a == 3
% Initialized Kkb vector to 15x1
 b =1;
 c =15;
Kkpp5=zeros(15,1);
Kkpp10=zeros(15,1);
Kkpp15=zeros(15,1);
end
% T = input(' Enter the Sampling Time in second');
%
% Sampling Freq. at 5 Second
%
T =5;
r=0.5;

% Plant Noise

s1 = input('Enter Plant noise sigma1 = ');
s2 = input('Enter Plant noise sigma2 = ');

% Measurement noise

sr = input(' Enter measurement noise sigma_r = ');
sp = input(' Enter measurement noise sigma_t = ');

Phi = [1,T,0,0,0,0;
       0,1,1,0,0,0;
       0,0,r,0,0,0;
       0,0,0,1,T,0;
       0,0,0,0,1,1;
       0,0,0,0,0,r];

G = [0,0;
     0,0;
     1,0;
     0,0;
     0,0;
     0,1];


Q = [s1,0 ;
      0,s2];

R = [sr,0;
     0,sp];
H = [1,0,0,0,0,0;
     0,0,0,1,0,0];

P0p = [sr  ,sr/T           ,0 ,0   ,0                ,0;
       sr/T,(2*sr)/(T^2)+s1,0 ,0   ,0                ,0;
       0   ,0              ,s1,0   ,0                ,0;
       0   ,0              ,0 ,sp  ,sp/T             ,0;
       0   ,0              ,0 ,sp/T,(2*sp)/(T^2)+s2  ,0;
       0   ,0              ,0 ,0   ,0              ,s2];
        


if a ==1
Pkpp5(1)=P0p(P,P);
end
for k =b:c
	Pkm = Phi*P0p*Phi'+G*Q*G';
	Kkb = Pkm*H'*inv(H*Pkm*H'+R);
        Pkp = (I-Kkb*H)*Pkm*(I-Kkb*H)'+Kkb*R*Kkb';
if a == 1
        Pkpp5(k)=Pkp(P,P);	
end
if a == 2
        Pkpp5(k)=Pkm(P,P);	
end
if a == 3
        Kkpp5(k)=Kkb(Pj,Pk);	
end
	P0p = Pkp;
	k =k+1;

end
%
%   Sampling Freq. at 10 Second.
%
T =10;

Phi = [1,T,0,0,0,0;
       0,1,1,0,0,0;
       0,0,r,0,0,0;
       0,0,0,1,T,0;
       0,0,0,0,1,1;
       0,0,0,0,0,r];

G = [0,0;
     0,0;
     1,0;
     0,0;
     0,0;
     0,1];


Q = [s1,0 ;
      0,s2];

R = [sr,0;
     0,sp];
H = [1,0,0,0,0,0;
     0,0,0,1,0,0];

P0p = [sr  ,sr/T           ,0 ,0   ,0                ,0;
       sr/T,(2*sr)/(T^2)+s1,0 ,0   ,0                ,0;
       0   ,0              ,s1,0   ,0                ,0;
       0   ,0              ,0 ,sp  ,sp/T             ,0;
       0   ,0              ,0 ,sp/T,(2*sp)/(T^2)+s2  ,0;
       0   ,0              ,0 ,0   ,0              ,s2];
        


if a == 1
 Pkpp10(1)=P0p(P,P);
end
for k =b:c
	Pkm = Phi*P0p*Phi'+G*Q*G';
	Kkb = Pkm*H'*inv(H*Pkm*H'+R);
        Pkp = (I-Kkb*H)*Pkm*(I-Kkb*H)'+Kkb*R*Kkb';
if a == 1
        Pkpp10(k)=Pkp(P,P);	
end
if a == 2
        Pkpp10(k)=Pkm(P,P);	
end
if a == 3
        Kkpp10(k)=Kkb(Pj,Pk);	
end
	P0p = Pkp;
	k =k+1;

end
%
% Sampling Freq. at 15 Second.
%
T =15;

Phi = [1,T,0,0,0,0;
       0,1,1,0,0,0;
       0,0,r,0,0,0;
       0,0,0,1,T,0;
       0,0,0,0,1,1;
       0,0,0,0,0,r];

G = [0,0;
     0,0;
     1,0;
     0,0;
     0,0;
     0,1];


Q = [s1,0 ;
      0,s2];

R = [sr,0;
     0,sp];
H = [1,0,0,0,0,0;
     0,0,0,1,0,0];

P0p = [sr  ,sr/T           ,0 ,0   ,0                ,0;
       sr/T,(2*sr)/(T^2)+s1,0 ,0   ,0                ,0;
       0   ,0              ,s1,0   ,0                ,0;
       0   ,0              ,0 ,sp  ,sp/T             ,0;
       0   ,0              ,0 ,sp/T,(2*sp)/(T^2)+s2  ,0;
       0   ,0              ,0 ,0   ,0              ,s2];
        


if a == 1
 Pkpp15(1)=P0p(P,P);
end
for k =b:c
	Pkm = Phi*P0p*Phi'+G*Q*G';
	Kkb = Pkm*H'*inv(H*Pkm*H'+R);
        Pkp = (I-Kkb*H)*Pkm*(I-Kkb*H)'+Kkb*R*Kkb';
if a == 1
        Pkpp15(k)=Pkp(P,P);	
end
if a == 2
        Pkpp15(k)=Pkm(P,P);	
end
if a == 3
        Kkpp15(k)=Kkb(Pj,Pk);	
end

	P0p = Pkp;
	k =k+1;

end
%************************** Plot or Save rountine ******************
if e == 1
   if (a==1|a==2)

     data(: ,1) = Pkpp5;
     data(: ,2) = Pkpp10;
     data(: ,3) = Pkpp15;
     save br.dat data /ascii
   end
   if (a==3)
     data(: ,1) = Kkpp5;
     data(: ,2) = Kkpp10;
     data(: ,3) = Kkpp15;
     save br.dat data /ascii
   end
end
if e == 2
 k = 0:1:c-1;
 if (a==1|a==2)
     plot(k,Pkpp5,'+',k,Pkpp5,k,Pkpp10,'o',k,Pkpp10,k,Pkpp15,'*',k,Pkpp15)
%      plot(k,Pkpp5,'r',k,Pkpp10,'g',k,Pkpp15,'b')
 end
 if (a==3)
    plot(k,Kkpp5,k,Kkpp5,'+r',k,Kkpp10,k,Kkpp10,'og',k,Kkpp15,k,Kkpp15,'*b')
 end
 grid on
legend(' T = 5',' ',' T = 10',' ', 'T = 15',' ');
% legend(' T = 5',' T = 10', 'T = 15');
if a == 1
if P ==1
 title('Pkp (1,1)');
ylabel(' Range Covariance')
end
if P ==2
 title('Pkp (2,2)');
ylabel(' Range Rate Covariance')
end
if P ==3
 title('Pkp (3,3)');
ylabel(' Range Noise Rate Covariance')
end
if P ==4
 title('Pkp (4,4)');
ylabel(' Bearing Covariance')
end
if P ==5
 title('Pkp (5,5)');
ylabel(' Bearing Rate Covariance')
end
if P ==6
 title('Pkp (6,6)');
ylabel(' Bearing Rate Noise Covariance')
end
end
if a == 2
if P ==1
 title('Pkm (1,1)');
ylabel(' Range Covariance')
end
if P ==2
 title('Pkm (2,2)');
ylabel(' Range Rate Covariance')
end
if P ==3
 title('Pkm (3,3)');
ylabel(' Range Rate Noise Covariance')
end
if P ==4
 title('Pkm (4,4)');
ylabel(' Bearing Covariance')
end
if P ==5
 title('Pkm (5,5)');
ylabel(' Bearing Rate Covariance')
end
if P ==6
 title('Pkm (6,6)');
 ylabel(' Bearing Rate Noise Covariance')
end
end
if a == 3
if Pj ==1&Pk==1
 title('Kkb (1,1)');
ylabel(' Range Kalman Gain')
end
if Pj ==2&Pk==1
 title('Kkb (2,1)');
ylabel(' Range Rate Kalman Gain')
end
if Pj ==3&Pk==1
 title('Kkb (3,1)');
ylabel(' Range Rate Noise Noise Kalman Gain')
end
if Pj ==4&Pk==2
 title('Kkb (4,2)');
 ylabel(' Bearing Kalman Gain')
end
if Pj ==5&Pk==2
 title('Kkb (5,2)');
ylabel(' Bearing Rate Kalman Gain')
end
if Pj ==6&Pk==2
 title('Kkb (6,2)');
 ylabel(' Bearing Rate Noise Kalman Gain')
end
end
end
clc
xlabel(' Cylcle Number')
% ylabel(' Bearing Rate Noise Covariance')
% end
% v=[1,15,0.5e6,1.0e6]
% axis(v)
% axis('square')

