% Simulate 4 tank level process with an Extended Kalman Filter (EKF)
% in paralell
clear all 
clc

% Non-minimum phase system (izero=2)
% Minimum phase system (izero=1)
izero=1;

% Step length and time interval parameters
h=1; t0=0; t1=1200; N=(t1-t0)/h;

% Linearized continuous time state space model
[Ac,Bc,Dc]=linmod_4tank(izero);

% Discrete time linear state space modell matrices
[A,B,D,E]=c2dm(Ac,Bc,Dc,zeros(2,2),h,'zoh');

% Nominal plant parameters
[h10,h20,h30,h40,u10,u20,k1,k2,g1,g2]=param_4tank_nominal(izero);

% Need kc model parameter only
[A1,A2,A3,A4,a1,a2,a3,a4,kc,g]=param_4tank_model;
r0=[kc*h10;kc*h20];            % Nominal reference for output y=kc*[h10;h20]

% Initializing for simulation
f=zeros(4,1); U=zeros(N,2); Y=zeros(N,2); X=zeros(N,4); T=zeros(N,1);
x=[h10;h20;h30;h40]; x_old=x; y_old=D*x_old; 
u=[u10;u20]; u_old=u;
r=r0;

% Initial values Kalman filter
xb=x*0.5; 
Xb=eye(4)*1000;
W=eye(2); V=eye(4)*100;
Xb_a=zeros(N,4); Xh_a=zeros(N,4); K_a=zeros(N,8);
K=dlqe(A,eye(4),D,V,W)

% maxe input experiment
iexp=2;    % iexp=1 gives input experiment
if iexp==1
 du1=0.1*prbs1(N,200,500);
 du2=0.1*prbs1(N,150,350);
else
 du1=0.1*prbs1(N,50,150);
 du2=0.1*prbs1(N,75,175);
end
 Uexp=[u10 + du1, u20 + du2];
%

iexp=1; 
x=x*0; % Start with empty tanks
% Simulate closed loop system
for i=1:N
   %i
   y=D*x; %+randn(2,1)*0.05+ones(2,1)*0.1;   
   
   if iexp==1
       u=Uexp(i,:)';
   elseif iexp==2
       u=[u10;u20];   % Simulate for nominal outputs and states
   end
   
   % The Kalman filter
    yb=D*xb;     % Calculate predicted output
    K=Xb*D'*inv(D*Xb*D'+W);  % K=Xb*D/(Xb*D^2+W); 
    K_a(i,:)=K(:)';              % Save Kalman gains
    ep=y - yb;             % The innovations process
    xh = xb + K*ep;        % The aposteriori state estimate
    Xb_a(i,:)=xb';            % Storing apriori estimates
    Xh_a(i,:)=xh';            % Storing aposteriori state estimates
    
    fk=fun_4tank(xh,u,izero);
    xb=xh+h*fk;
    %xb=A*xh + B*u;      % Update apriori state estimate
    
    Xh =(eye(4)-K*D)*Xb*(eye(4)-K*D)' + K*W*K';
    Xb = A*Xh*A'+V;
   % End Kalman filter eqs'..
   
     
   U(i,:)=u'; Y(i,:)=y'; X(i,:)=x'; T(i)=t0;
   
   % Update process, calculate x(k+1)=x(k)+dt*f
   f=fun_4tank(x,u,izero);
   x=x+h*f+randn(4,1)*0.01;
   t0=t0+h;                                    % Update time.
end

figure(1)
subplot(221), plot(T,U(:,1)), grid, title('u_1: (volt to pump 1)')
subplot(222), plot(T,U(:,2)), grid, title('u_2: (volt to pump 2)')
subplot(223), plot(T,Y(:,1),'b-'), grid, title('y_1: Level tank 1')
legend('y_1'), xlabel('Time [s]')
subplot(224), plot(T,Y(:,2),'b-'), grid, title('y_2: Level tank 2')
legend('y_2'), xlabel('Time [s]')

figure(2)
subplot(221), plot(T,X(:,1),'b-',T, Xb_a(:,1),'k--'), grid, title('x_1: (level tank 1)')
legend('x_1','xb_1')
subplot(222), plot(T,X(:,2),'b-',T,Xb_a(:,2),'k--'), grid, title('x_2: (level 2)')
legend('x_2','xb_2')
subplot(223), plot(T,X(:,3),'b-',T,Xb_a(:,3),'k--'), grid, title('x_3: Level tank 3')
legend('x_3','xb_3')
subplot(224), plot(T,X(:,4),'b-',T,Xb_a(:,4),'k--'), grid, title('x_4: Level tank 4')
legend('x_4','xb_4'), xlabel('Time [s]')