%  CONtest.m
%  
%  Illustration of the controllability test
%  b.w. bequette
%  1 Oct 09
%  29 Nov 2011 -- revised for MPC 2011
%  26 Sept 2013 -- MPC 2013
%
%  use the three tank example
%   first, continuous state space model
  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
% --------------------------------------------------------------------
% 
% discrete time model
  delt = 0.5;                        % sample time of 0.5 minutes
  tankssz = c2d(lintank,delt,'zoh')  % create discrete state space model from continuous
%
%  extract the matrix information
%
  [phi,gamma,cmat,dmat] = ssdata(tankssz)   % cmat will equal c
%
%
  CONTmat = [phi*phi*gamma phi*gamma gamma]
%
  rank(CONTmat)
%
%   input the initial condition
%
  x0 = input('the initial condition is [1;1;1] ')
%
  xdis(:,1) = x0;
%
  x3  = input('the desired final state is [0;0;0] ')
%
  u = inv(CONTmat)*(x3 - (phi^3)*x0)
%
  time = [0 0.5 1 1.5];
%
  tplot = [];
  yplot = [];
  xplot = [];
%
  ydis(1) = xdis(3,1);       % first measured output
%
  for k = 1:3;
      [tdummy,xdummy] = ode45('linodepar',[time(k) time(k+1)],xdis(:,k),[],a,b,u(k));
      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
%
  u(k+1) = u(k)
%
  ydis
%  
  figure(2000)
  plot(tplot,xplot)
%
  [tt,uu] = stairs(time,u);
  figure(2001)
  subplot(2,1,1)
  plot(tplot,xplot)
  subplot(2,1,2)
  plot(tt,uu)