% Observability Lecture
% bwb 22 Sept 13
%  for MPC 2013, lecture 8 23 Sept 13
%  extended to interacting tanks -- 26 Sept 13
%
% continuous 3-state tank level problem
  a = [-1 0 0;1 -1 0;0 1 -1]
  b = [1;0;0]
  c = [0 0 1]
  d = 0
%
  lintank = ss(a,b,c,d)              % defines continuous state space model
%
% construct observability matrix for 3 measurement matrices
%
  c1 = [1 0 0];
  c2 = [0 1 0];
  c3 = [0 0 1];
%
  obsvmat1 = [c1;c1*a;c1*a^2]
%
  rank1 = rank(obsvmat1)
%
  obsvmat2 = [c2;c2*a;c2*a^2]
%
  rank2 = rank(obsvmat2)
%
  obsvmat3 = [c3;c3*a;c3*a^2]
%
  rank3 = rank(obsvmat3)
% -------------------------------------------------------------
%
% discrete state space model
%
  delt = 0.5;                        % sample time of 0.5 minutes
  tankssz = c2d(lintank,delt,'zoh')  % create discrete state space model from continuous
%
% get the phi and gamma matrices
%
  [phi,gamma,cmat,dmat] = ssdata(tankssz)   % cmat will equal c
%
% discrete observability tests for the 3 different measurement matrices
%
  obsvmat1d = [c1;c1*phi;c1*phi^2]
%
  rank1d = rank(obsvmat1d)
%
  obsvmat2d = [c2;c2*phi;c2*phi^2]
%
  rank2d = rank(obsvmat2d)
%
  obsvmat3d = [c3;c3*phi;c3*phi^2]
%
  rank3d = rank(obsvmat3d)
%
% --------------------------------------------------------------
% Simulation example
% create measured outputs at t = 0.5, 1.0 and 1.5 minutes
%  *** assume no input changes in this simulation ****
%
% set up a for loop and integrate over 0.5 minute time steps

%   start the simulation at t = 0
  tbeg = 0;
  tend = 1.5;
  x0   = [1; 1; 0]; % initial perturbation
%
  tplot = [];
  yplot = [];
  xplot = [];
%
  time = tbeg:delt:tend;   % generate the time vector

  xdis(:,1) = x0;  % state initial condition
  ydis(1)   = c*x0; % initial measurement
%
  for k = 1:length(time)-1;
%          generates tdummy (time vector) and xdummy(states array) for time points from
%          time(k) to time(k+1)
      [tdummy,xdummy] = ode45('linodepar',[time(k) time(k+1)],xdis(:,k),[],a,b,0);
      ndum = length(tdummy);
      xdis(:,k+1) = xdummy(ndum,:);   % plant state
      ydis(k+1) = c*xdis(:,k+1);      % plant output (measured)
      tplot = [tplot;tdummy];
      yplot = [yplot;xdummy*c'];
      xplot = [xplot;xdummy];
  end
%
  ydis
%
  figure(1)
  plot(tplot,yplot,'r',time,ydis,'ro')
  title('3-tank problem, tank 3 level')
  xlabel('t, min')
  ylabel('h3')
%
  figure(2)
  plot(tplot,xplot,time,ydis,'ro')
  title('3-tank problem, all levels')
  xlabel('t, min')
  ylabel('x')
%
% calculate the initial condition from y0,y1,y2 and observability matrix
%
  x0calc = inv(obsvmat3d)*[ydis(1:3)']
%
% confirm with more robust inverse method
%
  x0check = obsvmat3d\ydis(1:3)'
%
% also check with pinv
% 
  x0pinv = pinv(obsvmat3d)*ydis(1:3)'
% ---------------------------------------------------------------------
% 
%  interacting tank example
%
   A_IT = [-1 1 0; 1 -2 1; 0 1 -2];
   B_IT = [1;0;0];
   C_IT1 = [1 0 0];
   C_IT2 = [0 1 0];
   C_IT3 = [0 0 1]; % third state measured
   D_IT  = 0;
   
   obsvmat1IT = [C_IT1;C_IT1*A_IT;C_IT1*A_IT^2]
%
  rank1IT = rank(obsvmat1IT)
%
  obsvmat2IT = [C_IT2;C_IT2*A_IT;C_IT2*A_IT^2]
%
  rank2IT = rank(obsvmat2IT)
%
  obsvmat3IT = [C_IT3;C_IT3*A_IT;C_IT3*A_IT^2]
%
  rank3IT = rank(obsvmat3IT)