% This script file is for Chapter 4 problem
%
%  The Plane Model is follows:
%
%                              X(k) = Phi*X(k-1) + G*W(k-1) + C*Uk
%
% The Observation Model is:
%
%                              Z(k) = H*X(k) + D*V(k)
%
% Input to this script is:
%
%   Phi         Dim = 3x3
%   G           Dim = 1x3
%   H           Dim = 1x3
%   q           Dim = 1x1
%   R           Dim = 1x1
%   P0          Dim = 3x3
% Initialization
I = eye(3,3);
%                       Input

 T = input(' Please Enter Sampling period ');
 q = input(' Please Enter Plant Noise q =  ');
 R = input(' Please Enter Measurement Noise  R =  ');
 w = input(' Please Enter natural frequency w =  ');
 Pkp(1,1) = input(' Please Enter Pk0(1,1) =  ');
 Pkp(1,2) = input(' Please Enter Pk0(1,2) =  ');
 Pkp(2,2) = input(' Please Enter Pk0(2,2) =  ');
 Pkp(3,3) = input(' Please Enter Pk0(3,3) =  ');
 x3 = input(' Please Enter x3-NOM ');
 x2 = input(' Please Enter x2-NOM ');

 Pkp(1,3) = Pkp(1,2);
 Pkp(2,1) = Pkp(1,2);
 Pkp(2,3) = Pkp(1,2);
 Pkp(3,1) = Pkp(1,2);
 Pkp(3,2) = Pkp(1,2);
Pk11 = eye(1);
Pk22 = eye(1);
Pk33 = eye(1);
Kkb1 = eye(1);
Kkb2 = eye(1);
Kkb3 = eye(1);

% Noise and other inputs
% T = 0.01;
% q = 4.47;
% R = 0.01;
% w = 5;
% Pkp = [2 0 0;
%        0 2 0;
%        0 0 2];

% Linear approximation equations about x2 and x3

Phi = [1 T 0;
       -T*w^2 (1-10*T*x3) (-10*T*x2);
        0 0 1];
G = [0;
     T;
     0];

H = [1 0 0];

for k = 1:(1.2/T)
        
	Pkm = Phi*Pkp*Phi'+G*q*G';
	Kkb = Pkm*H'*inv(H*Pkm*H'+R);
        Pkp = (I-Kkb*H)*Pkm*(I-Kkb*H)'+Kkb*R*Kkb';
        Pk11(k) = Pkp(1,1);
        Pk22(k) = Pkp(2,2);
        Pk33(k) = Pkp(3,3);
        Kkb1(k) = Kkb(1);
        Kkb2(k) = Kkb(2);
        Kkb3(k) = Kkb(3);
    k = k+1;
end
k = 0:119;
plot(k,Pk11);
title(' P11 ');
xlabel(' Time: sec');
ylabel(' Mean-squared posistion uncertainty ft^2 ');
pause
k = 0:119;
plot(k,Pk22);
title(' P22 ');
xlabel(' Time: sec');
ylabel(' Mean-squared velocity uncertainty ft^2 ');
pause
k = 0:119;
plot(k,Pk33);
title(' P33 ');
xlabel(' Time: sec');
ylabel(' Covariance of damping coefficient P33 ');
pause
k = 0:119;
plot(k,Kkb1);
title(' Kkb 1 ');
xlabel(' Time: sec');
ylabel(' Position Kalman Gain ');
pause
k = 0:119;
plot(k,Kkb2);
title(' Kkb 2 ');
xlabel(' Time: sec');
ylabel(' Velocity Kalman Gain ');
pause
k = 0:119;
plot(k,Kkb3);
title(' Kkb 3 ');
xlabel(' Time: sec');
ylabel(' damping coefficient Kalman Gain ');
