% 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 = 2x2
%   G           Dim = 1x2
%   H           Dim = 1x1
%   Q           Dim = 1x1
%   R           Dim = 1x1
%   P0          Dim = 2x2
% Initialization
I = eye(2,2);
%                       Input

 T = input(' Please Enter Sampling period ');
 q = input(' Please Enter Plant Noise q =  ');
 R = input(' Please Enter Measurement Noise  R =  ');
 delta = input(' Please Enter Damping factor  ');
 w = input(' Please Enter natural frequency w =  ');
 Pkp=eye(2,2);
 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(2,1) = Pkp(1,2);
% Noise and other inputs
% T = 0.01;
% q = 4.47;
% R = 0.01;
% delta = 0.2;
% w = 5;
% Pkp = [ 2 0;
 %       0 2];
% Initializer
Pk11 = eye(1);
Pk12 = eye(1);
Pk22 = eye(1);
Kkb1 = eye(1);
Kkb2 = eye(1);
% Discretized with Sampling period of T

Phi = [1 T;
       -T*w^2 (1-T*delta*w)];
G = [0;
     T];
H = [1 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);
        Pk12(k) = Pkp(1,2);
        Pk22(k) = Pkp(2,2);
        Kkb1(k) = Kkb(1);
        Kkb2(k) = Kkb(2);
    k = k+1;
end
k = 0:T:1.19;
plot(k,Pk11);
title(' P11 ');
xlabel(' Time: sec');
ylabel(' Mean-squared posistion uncertainty ft^2 ');
pause
k = 0:T:1.19;
plot(k,Pk12);
title(' P12 ');
xlabel(' Time: sec');
ylabel(' Position-velocity cross covariance ft^2/s ');
pause
k = 0:T:1.19;
plot(k,Pk22);
title(' P22 ');
xlabel(' Time: sec');
ylabel(' Mean-squared velocity uncertainty ft^2 ');
pause
k = 0:T:1.19;
plot(k,Kkb1);
title(' Kkb 1 ');
xlabel(' Time: sec');
ylabel(' Position Kalman Gain ');
pause
k = 0:T:1.19;
plot(k,Kkb2);
title(' Kkb 2 ');
xlabel(' Time: sec');
ylabel(' Velocity Kalman Gain ');
