
% CONTENT:

% This simulation file contains the implementation of an EKF that uses the
% differential equations between two samples.
% In this file you can play with Q and P0, the initial condition of the
% observer and the sampling time.

% The lines which you may work on are 44-68
% Of course feel free to change the code further, if you think it is
% necessary or will improve your observer

%#######################################################################
%#######################################################################
%#######################################################################


% Header
%#######################
clear all
set(0,'defaultaxesfontsize',14);
clc
scrsz = get(0,'ScreenSize')/1.6;
addpath('m_files')


% Simulate the system
%#######################

C = [0 1 0 0 ; 0 0 1 0]; 

% Initial Condition of Plant
x0 = [0.2 0.3 0.1];

% Simulation step width
h  = 0.01;               
t  = 0:h:500;

% Measurment Noise
R = diag([0.01^2 0.01^2]);

% Call the simulation file
Messdaten
% Tuning the Observer
%######################################

% Observer Initial Condition
x0 = [0.1,0.1,0.1,0.1];

% Covariance Matrix of measurement noise
R_obsv = diag([0.01^2,0.01^2]);

% Covariance Matrix of plant noise
q1 = 1e-5;
q2 = 1e-5;
q3 = 1e-5;
q4 = 1e-5;
% q1 = ;    % blue line
% q2 = ;  % red line
% q3 = ;  % green line
% q4 = ;   % black line
Q =  diag([q1,q2,q3,q4]);

% Covariance Matrix of initial error
% P0 = ;
P0 = Q*200;

% Sampling time, must be a multiple of h, line 34
t_sample = 1;

% Endtime
endtime = 100;
% Call the Observer
%#######################################
% The m-file (EKF2) can be found in the folder m_files

param.x0 = x0;
param.P0 = P0;
param.Q = Q;
param.R = R_obsv;
param.C = C;
param.t_meas = t_meas(1:t_sample/h:endtime/h);
param.y_meas = y_meas(1:t_sample/h:endtime/h,:);
param.x = x;
param.h = h;

[x_est,t_vec,h2] = EKF2('Model_obsv',param);

t_vec = t_vec(1:end);
x_est = x_est(:,1:end);

% 
% Plot the real values of the states and the estimates
cont_observer = figure('Name','Cont_Observer','Position',[1 1 scrsz(3) scrsz(4)-68]);
p1=plot(t(1:endtime/h),x(1:endtime/h,1));
hold on
plot(t(1:endtime/h),x(1:endtime/h,2),'r','LineWidth',2)
plot(t(1:endtime/h),x(1:endtime/h,3),'g','LineWidth',2)
plot(t(1:endtime/h),x(1:endtime/h,4),'k','LineWidth',2)
plot(t_vec,x_est(1,:),'--','LineWidth',2)
plot(t_vec,x_est(2,:),'r--','LineWidth',2)
plot(t_vec,x_est(3,:),'g--','LineWidth',2)
plot(t_vec,x_est(4,:),'k--','LineWidth',2)
xlabel('Time [s]','Fontsize',20)
ylabel('State Value','Fontsize',20)
legend('cA','cB','cC','kAB','cA_{obsv}','cB_{obsv}','cC_{obsv}','kAB_{obsv}')
title('Observer using continuous equations','FontSize',14)
%title(sprintf('Observer using continuous equations. t_{Sample}:%g sec,
%Q=diag(%g,%g,%g,%g)
%R=diag(%g,%g)',t_sample,q1,q2,q3,q4,R_obsv(1,1),R_obsv(2,2)),'FontSize',15
%)