%
%  This file modifies the model determined by the system identification
%  routine, and then compares with the simulation
%
%  A,B,C,D are the original estimated state variable matrices obtained
%  from [A,B,C,D] = ssdata(whatever);
%  u is the true input, y is the true output, t is the true time
%
function compare_sys_id(A,B,C,D,u,y,t);
%
%   first let's clean up the state model, C should have a unit matrix for
%   the first 2x2 section
%
  C = [C; 0 0 1];
  Ts = t(2)-t(1);
  P = inv(C);
  Pinv = inv(P);
  Ad = Pinv*A*P
  Bd = Pinv*B
  Cd = C*P;
  Dd = [D; 0];
%
%  try to determine analytically the discrete time system
%
%  w = 18.28; z = 0.0377; K = 3.20;
   w = 13.2; z = 0.1; K = 28.9;
  Ac = [0 1; -w^2 -2*z*w];
  Bc = [0;K*w^2];
  Cc = eye(2);
  Dc = zeros(2,1);
  Hc = ss(Ac,Bc,Cc,Dc);
  Hd = c2d(Hc,Ts,'zoh');
  [AA,BB,CC,DD] = ssdata(Hd);
%   Ab = [AA zeros(2,1); 0 0 0];
%   Bb = [BB; 1];
%   Cb = [CC*inv(AA) -CC*inv(AA)*BB; 0 0 1];
%   Db = [DD; 0];
%   P = inv(Cb);
%   Pinv = inv(P);
%   Abob = Pinv*Ab*P
%   Bbob = Pinv*Bb
%   Cbob = Cb*P;
%   Dbob = Db;
  Abob = [AA BB; 0 0 0];
  Bbob = [0; 0; 1];
  Cbob = eye(3,3);
  Dbob = [0;0;0];
%
%  Now simulate the system forward in time
%
  Hbob = ss(Abob,Bbob,Cbob,Dbob,Ts);
  [ybob,tbob,xbob] = lsim(Hbob,u);
%
%  Now simulate the system forward in time
%
  H = ss(Ad,Bd,Cd,Dd,Ts);
  [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] = stairs(t,y(:,1)*scale);
  [td,x2] = stairs(t,y(:,2)*scale);
  [td,x3] = stairs(t,y(:,3));
  [tb,x1_bob] = stairs(tbob,ybob(:,1)*scale);
  [tb,x2_bob] = stairs(tbob,ybob(:,2)*scale);
  [tb,x3_bob] = stairs(tbob,ybob(:,3));
  figure;
  orient tall
  subplot(3,1,1); plot(td,x1_d,'r-',td,x1,'b-',tb,x1_bob,'g--'); grid; legend('Model','Measured','bob'); ylabel('x_1');
  subplot(3,1,2); plot(td,x2_d,'r-',td,x2,'b-',tb,x2_bob,'g--'); grid; legend('Model','Measured','bob'); ylabel('x_2'); 
  subplot(3,1,3); plot(td,x3_d,'r-',td,x3,'b--',tb,x3_bob,'g--'); grid; legend('Model','Measured','bob'); ylabel('x_3'); xlabel('Time (sec)');
  
 % save test_205 Ad Bd Cd Dd
   save test_210 Ad Bd Cd Dd