%% Exam Simulation - Kalman Filters - Prof. M. Taragna
%% Setup

clear all, close all
rng('default')

%% LTI system
% Consider the following discrete-time linear time-invariant system S, 
% with D=0 and with the following features:
% v_1 is a white noise with zero mean and variance V1;
% v_2 is a white noise with zero mean and variance V2;
% moreover, v_1 and v_2 are uncorrelated.
% Assume that the initial state is a random vector with zero mean and
% variance P1.

A = [ 2.7258, -1.2424, 0.7577;
      2.0000,  0.0000, 0.0000;
      0.0000,  0.5000, 0.0000 ];
B = [ 0.026;
      0.000;
      0.000 ];
C = [ 1, -0.45, -0.05 ];
D = 0;


Bv1 = 0.05*[0.026 0 0]';
V1 = Bv1*Bv1';
V2 = 0.0004;

%%
% It is available the input from data2.mat. You can stimulate the system S 
% with this input.

load('data2.mat');

P1 = eye(3);
x_avg = [ 0 0 0 ]';
x0 = [50; 100; 50]; % Set the initial state of S to some nonzero vector

%% System S simulation
% To simulate the system S a _for_ cycle is used with T=2000 iterations.
% At each iteration, random noises v_1 and v_2 are generated.
% The time evolutions of the three states are plotted on different figures.
% Notice that the system is asymptotically stable, because all its
% eigenvalues are less than 1 in magnitude.

ddamp(A)
%%

N = 2000;

x(:,1) = x0;
for t = 1:N,
    v1(:,t) = mvnrnd(zeros(1,3),Bv1*Bv1')';
    v2(t) = sqrt(V2)*randn;
    x(:,t+1) = A*x(:,t)+B*u(t)+v1(:,t);
    y(t) = C*x(:,t)+v2(t);
end

figure(1),
subplot(3,1,1), plot([0:N], x(1,:)),title('Simulation of x_1(t)'),xlabel('t'),grid on;
subplot(3,1,2), plot([0:N], x(2,:)),title('Simulation of x_2(t)'),xlabel('t'),grid on; 
subplot(3,1,3), plot([0:N], x(3,:)),title('Simulation of x_3(t)'),xlabel('t'),grid on; 

figure(2), 
plot(y), title('Simulated Output y(t)'), grid on;

%% Steady-state Kalman filter Finf
% First, the state-space description of the system S is created 
% by means of the Matlab command ss.
% Second, the Finf parameters are evaluated by means of the 
% Matlab command kalman.
% Then, the steady-state predicted state is initialized, assuming that the 
% estimated initial state could be reasonably given by its mean value.
% Finally, a for cycle implements the overall steady-state filtering.

sys = ss(A,[B,eye(3)],C,[D,zeros(1,3)],1);
[Kinf,Kbar,Pbar,K0bar] = kalman(sys,V1,V2,0);
Kbar, K0bar

x_stim(:,1) = x_avg;
for t = 1:N,
    y_stim(t) = C*x_stim(:,t);
    e(t) = y(t)-y_stim(t);
    x_stim(:,t+1) = A*x_stim(:,t)+B*u(t)+Kbar*e(t);
    x_fil(:,t) = x_stim(:,t)+K0bar*e(t);
    y_fil(t) = C*x_fil(:,t);    
end

figure,
subplot(3,1,1), plot([0:N-1], x_fil(1,:), [0:N], x(1,:)),
title('Filtered State x_1(t) by F_{inf}'),xlabel('t'),grid on,
legend('Filtered','Simulated');
subplot(3,1,2), plot([0:N-1], x_fil(2,:), [0:N], x(2,:)),
title('Filtered State x_2(t) by F_{inf}'),xlabel('t'),grid on,
legend('Filtered','Simulated');
subplot(3,1,3), plot([0:N-1], x_fil(3,:), [0:N], x(3,:)),
title('Filtered State x_3(t) by F_{inf}'),xlabel('t'),grid on,
legend('Filtered','Simulated');

figure, 
plot([0:N-1], y_fil, [0:N-1], y), title('Filtered Output y(t) by F_{inf}'), 
legend('Filtered','Simulated'), xlabel('t'),grid on;

%% Dynamic Kalman one-step predictor Kpc
% The dynamic predictor state and its variance are initialized, assuming 
% that the estimated initial state could be reasonably given by 
% its mean value.
% A for cycle implements the overall dynamic prediction.

x_stim2(:,1) = x_avg;
P = P1;
for t = 1:N,
    K0 = P*C'*(C*P*C'+V2)^(-1);
    P0 = (eye(3)-K0*C)*P;
    P = A*P0*A'+V1;
    y_stim2(t) = C*x_stim2(:,t);
    e(t) = y(t)-y_stim2(t);
    x_fil2(:,t)=x_stim2(:,t)+K0*e(t);
    x_stim2(:,t+1) = A*x_fil2(:,t)+B*u(t);
end
K0

figure,
subplot(3,1,1), plot([0:N], x_stim2(1,:), [0:N], x(1,:)),
title('Predicted State x_1(t) by K_{pc}'),xlabel('t'),grid on,
legend('Predicted','Simulated');
subplot(3,1,2), plot([0:N], x_stim2(2,:), [0:N], x(2,:)),
title('Predicted State x_2(t) by K_{pc}'),xlabel('t'),grid on,
legend('Predicted','Simulated');
subplot(3,1,3), plot([0:N], x_stim2(3,:), [0:N], x(3,:)),
title('Predicted State x_3(t) by K_{pc}'),xlabel('t'),grid on,
legend('Predicted','Simulated');

figure, 
plot([0:N-1], y_stim2, [0:N-1], y), title('Predicted Output y(t) by K_{pc}'), 
legend('Predicted','Simulated'), xlabel('t'),grid on;

%% RMSE evaluations

N0 = 100 % 0, 100
for i=1:3
    RMSE_Finf(i) = sqrt(1/(N-N0))*norm(x(i,N0+1:N)-x_fil(i,N0+1:N));
    RMSE_Kinf(i) = sqrt(1/(N-N0))*norm(x(i,N0+1:N)-x_stim(i,N0+1:N));
    RMSE_Fpc(i) = sqrt(1/(N-N0))*norm(x(i,N0+1:N)-x_fil2(i,N0+1:N));
    RMSE_Kpc(i) = sqrt(1/(N-N0))*norm(x(i,N0+1:N)-x_stim2(i,N0+1:N));
end

RMSE_Finf, RMSE_Kinf
RMSE_Fpc,  RMSE_Kpc