% 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
% Generate 31 data points with plant noise and measurement noise equal to zero
initial = [0 0.0];
[x,num_y] = ode45('noise_f',0,20,initial);

x1_free = num_y(:,2);
x2_free = num_y(:,1);
% 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 =  ');

% Noise and other inputs
 
 Pkp = [ 2 0;
         0 2];
xh0 = [0;
      0];

% 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];
Uk = [0;
     12];


for k = 1:148
        x_free = [x1_free(k);
                  x2_free(k)];
        zk = H*x_free+randn(1,1)*sqrt(R);
        xhm = Phi*xh0;
	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';
        xhp = xhm+Kkb*(zk-H*xhm);
        xh0 = xhp;
        Pk11(k) = Pkp(1,1);
        Pk12(k) = Pkp(1,2);
        Pk22(k) = Pkp(2,2);
        Kkb1(k) = Kkb(1);
        Kkb2(k) = Kkb(2);
        xhp1(k) = xhp(1,1);
        x1_n(k) = zk;
        xhp2(k) = xhp(2,1);        
    k = k+1;
end
clg
k = 0:147;

subplot(211),plot(k,x1_n,k,x1_free,k,xhp1)
title(' Estimate Position ')
subplot(212),plot(k,xhp2,k,x2_free)
title(' Estimate velocity ');
