%
%  This file modifies the model determined by the system identification
%  routine, and then compares with the simulation
%
%  A,B,C,D are the constinous time state space matrices
%  G,H,C,D are the discrete-time estimates
%
%  inputs: u = the system input 
%          y = the system output
%          t = the sample times
%           model = the model name 
%  (u,y,t) come from the simulink model
%   (model) comes from the system identification toolbox
%          Tend = the las time to plot
%
function compare_sys_id(u,y,t,model,Tend);
%
%  first get the sample time
%
  Ts = t(2)-t(1);
%
%  first read in the continuous time model, sample it, and put it into 
%  a form that represents the 1-sample delay
%
  load state_model_1dof_model205
  Hc = ss(A,B,C,D);
%  
%  discretize the state space model, then get
%  the state variable model back
%
  Hd = c2d(Hc,Ts,'zoh');
  [GG,HH,CC,DD] = ssdata(Hd);
%
%  Now construct the discrete-time state model with the delay
%
  Gc = [GG HH; zeros(1,3)];
  Hc = [zeros(2,1); 1];
  Cc = eye(3,3);
  Dc = 0;
%
%  form a new state model
%
  F = ss(Gc,Hc,Cc,Dc,Ts);
%
%  Now simulate this model
%
  [yc,tc,xc] = lsim(F,u);
%
%  Now we utilize the model from the system identification toolbox
%
%
%  first we load the data from the system identification toolbox
%
  [A,B,C,D] = ssdata(model);
%
%  we have to add one more state so the model accounts for the delay
%
  C = [C; 0 0 1];
%  
%  We need to clean things up a bit, so the output of the system consistes
%  of the states, so we'll change bases so C is diagonal
%
  P = inv(C);
  Pinv = inv(P);
  Gd = Pinv*A*P;
  Hd = Pinv*B;
  Cd = C*P;
  Dd = [D; 0];
%
%  Make a state space model for matlab
%
  H = ss(Gd,Hd,Cd,Dd,Ts);
%
%  Now simulate the system forward in time
%
  [yd,td,xd] = lsim(H,u);
%
%  Now plot them
%
  scale = 180/pi;
  scale = 1;
  [td,x1_d] = stairs(t,yd(:,1)*scale);
  [td,x2_d] = stairs(t,yd(:,2)*scale);
  [td,x3_d] = stairs(t,yd(:,3));
  [td,x1_c] = stairs(t,yc(:,1)*scale);
  [td,x2_c] = stairs(t,yc(:,2)*scale);
  [td,x3_c] = stairs(t,yc(:,3));
  [td,x1_r] = stairs(t,y(:,1)*scale);
  [td,x2_r] = stairs(t,y(:,2)*scale);
  [td,x3_r] = stairs(t,y(:,3));
%
  figure;
  max_x1 = max([x1_r; x1_c; x1_d]);
  min_x1 = min([x1_r; x1_c; x1_d]);
  max_x2 = max([x2_r; x2_c; x2_d]);
  min_x2 = min([x2_r; x2_c; x2_d]);
 
  
  orient landscape
  subplot(2,1,1); 
    hold on
    plot(td,x1_d,'g','Linewidth',0.5);
    plot(td,x1_c,'r:','Linewidth',1);
    plot(td,x1_r,'b-','Linewidth',2); grid; 
    legend('sys id','c2d','Real'); ylabel('x_1 (displacement cm)');
    axis([0 Tend min_x1 max_x1]);
    hold off
  subplot(2,1,2); 
    hold on
    plot(td,x2_d,'g','Linewidth',0.5);
    plot(td,x2_c,'r:','Linewidth',1);
    plot(td,x2_r,'b-','Linewidth',2); grid; 
    legend('sys id','c2d','Real'); ylabel('x_2 (velocity cm/sec)');
    axis([0 Tend min_x2 max_x2]);
    hold off
%
%  rename the variables so they are convenient later
%
  G = Gd; H = Hd; C = Cd; D = Dd;
  save sys_id_1dof_model210 G H C D
  G = Gc; H = Hc; C = Cc; D = Dc;
  save c2d_1dof_model210 G H C D
  