% Filename: losn_reak_kf.m
% Solution to Kalman-filter part in exercise 7.
% Topic: System identification and optimal estimation.

clear all

% Rector process parameters and steady state values, states, control, disturbance.
x1s=2.5; x2s=1; ths=10; us=25;
k1=50; k2=100; k3=10;

% Time horizon, sampling interval etc. for the simulations
dt=0.001; t0=0; t1=0.2;
t=t0:dt:t1; N=length(t);

ifilt=0; 
ifilt=dread('Stationary constant K) or Extended Kalman filter ?(0-1)',ifilt);

% Initialization of arrays used to hold results from the simulations
X=zeros(N,2); Y=zeros(N,1); U=ones(N,1)*us; f=zeros(2,1);
Xh=X;

% Linearized continuous time model matrices
Dc=[0,1];
a11=-k1-2*k3*x1s-us; a12=0;
a21=k1;              a22=-k2-us;
Ac=[a11,a12;a21,a22];
Bc=[ths-x1s;-x2s];
Cc=[us;0];

% Linearized discrete time model matrices using explicit Euler
A=eye(2)+dt*Ac;
B=dt*Bc;
C=dt*Cc;
D=Dc;

% Discrete process and measurements white noise.
v=0.1*randn(N,2); 
w=0.0001*randn(N,1);

% Steady state (stationary) Kalman-filter gain matrix K.
%W=1; V=diag([10,10]);
W=1e-8; V=diag([0.01 0.01]);
[K,Xb,Xh]=dlqe(A,eye(2),D,V,W);

% Simulation
x=[x1s;x2s];            % Initial state
xb=x+0.1;               % Initial a-priori state estimate
xb=zeros(2,1);
for k=1:N
   % Process measurement at time k.
   y=D*x; %+w(k);
   
   Y(k,:)=y';
   X(k,:)=x';
   
   if k<20; u=U(k); else; u=26; end
   
   % Kalman filter
   yb=D*xb;
   xh=xb+K*(y-yb);          % A-posteriori state estimate
   
   if ifilt==1
      K=Xb*D'*inv(D*Xb*D'+W);
   end
    
   % Save estimates
   Xhat(k,:)=xh'; 
   Xbar(k,:)=xb';
   
   %xb=A*xh+B*u+C*ths;       % Update the a-priori state estimate.
   f(1)=-k1*xh(1)-k3*xh(1)^2+(ths-xh(1))*u;
   f(2)=k1*xh(1)-k2*xh(2)-xh(2)*u;
   xb=xh+dt*f;
   if ifilt==1
      Xh=(eye(2)-K*D)*Xb*(eye(2)-K*D)'+K*W*K';
      Xb=A*Xh*A'+V;
   end  
     
   
   % Updating (integrating) the process state using the non-linear model.
   f(1)=-k1*x(1)-k3*x(1)^2+(ths-x(1))*u +v(k,1);
   f(2)=k1*x(1)-k2*x(2)-x(2)*u +v(k,2);
   x=x+dt*f;
end

% Plot results
plot(t,X(:,2),'r-',t,Xhat(:,2),'b--',t,Xbar(:,2),'k-.');
legend('True state x_2','Aposteriori estimate','Apriori estimate')