%  Tutorial 03 of Advanced Process Control, ws2010/2011 
%  Chair of Process Dynamics and Operations, TU Dortmund

%  Dealing with modell errors.
%  #########################################################
%  It is assumed that the reaction coefficient kAB is wrong in the observer model.
%  #########################################################
%  There are three parts of this script:

%    Part 1.1: Define the system, assign initial conditions

%    Part 2.1: Luenberger observer with model error
%    Part 2.2: Kalman filter with model error

%    Part 3.1: Zero steady state error between plant and observer is
%    achieved

% This scripts calls 'Plant_observed_part_a.mdl', 'Kalman_Filter_part_a.mdl'
%  'plot_luenberger_observer','plot_luenberger_kalman'

% Part 1.1 
%  defines the system model and the observer model
% ############ Parameter ############
kAB=1.5;
kBC=3;
kCB=2;
dvdt=1;
VR=10;
A_plant = [-kAB-dvdt/VR 0 0; kAB -kBC-dvdt/VR kCB; 0 kBC -kCB-dvdt/VR];
StateNoiseGain = [1 1 1]*2; % state noise
MeasurementNoiseGain = 1;   % measurement noise

%  ############ Model plant mismatch: kAB_obsv != kAB ############
kAB_obsv=kAB*0.5;
A_obsv = [-kAB_obsv-dvdt/VR 0 0; kAB_obsv -kBC-dvdt/VR kCB; 0 kBC -kCB-dvdt/VR];
B=[dvdt/VR;0;0];
C = [0 1 0];

% ############ Observability #############
isobsv = rank(obsv(A_obsv,C));

% ############## Initial Conditions #############
Initial = [0 0 0];
Initial_obsv = [0.2 0.2 0.2];
% part 2.1
% ############ Luenberger observer with model error #############
[V,D] = eig(A_obsv);
observer_poles = [-1 -5.1 -1.6];
K=place(A_obsv',C',observer_poles);
K = K';
sim('Plant_observed_luenberger',150);
plot_luenberger_observer;
% steady state offset can be observed
pause

% Part 2.2
% ############ Kalmanfilter with model error #############
R = 0.1;
Q = diag([1 1 1]);
P0 = diag([1 1 1]);
%P0 = diag([0.2^2 0.2^2 0.2^2]);
sim('Plant_Kalman',150)
plot_kalman;
% steady state offset can be observed
pause

% Part 3.1
% ############ Luenberger observer dealing with model error #############
L3 = A_obsv(3,2)*(A_obsv(3,3)*(A_obsv(2,1)+A_obsv(1,1))-A_obsv(3,1)*(A_obsv(2,3)+A_obsv(1,3)));
L2 = 10;
L1 = A_obsv(2,2)+A_obsv(1,2)- L2- (A_obsv(3,2)-L3)*(A_obsv(2,3)+A_obsv(1,3))/A_obsv(3,3);
K = [L1 L2 L3]';
sim('Plant_observed_luenberger',150);
plot_luenberger_observer;
