%
%  This function determines the state variable feedback gains and
%  system gain to place the poles at a specified location for 
%  two degree of motion (fourth order) system. It them simulates the system with these values
%
function state_variables_3carts(Amp,A,B,C,k,final_time,filename)
%
  D = [0];
%
%  Now see where the poles are
%
 fprintf('The closed loop poles are: \n')
 disp(eig(A-B*k))
%
%  Find the system gain
%
  f = -1/(C*inv(A-B*k)*B);
  fprintf('K_pf = %f \n', f);
%
%  Next simulate the system
%
  CC = [1 0 0 0 0 0 ; 0 0 1 0 0 0; 0 0 0 0 1 0]; % we are only looking at the positions of the three carts
%
  H = ss(A-B*k,B*f,CC,D);
  t = linspace(0,final_time,100000);
  u = Amp*ones(1,length(t));
  y = lsim(H,u,t);
%
%  Now plot the results
%
  if (~isempty(filename))
    orient landscape
    encoder = 1;
    data = importdata(filename);
    tdata = data(:,encoder+1);
    x1data = data(:,encoder+3)/2196;
    x2data = data(:,encoder+4)/2196;
    x3data = data(:,encoder+5)/2196;
    subplot(3,1,1);
    plot(t,y(:,1),':',tdata,x1data,'-'); grid;
    ymin = min(min(y(:,1)),min(x1data));
    ymax = max(max(y(:,1)),max(x1data));
    axis([0 final_time ymin ymax]);
    legend('Model x_1','Actual x_1',4);
    xlabel('Time (sec)');
    ylabel('Displacement (cm)');
    title([' k_1 = ',num2str(k(1),3),', k_2 = ',num2str(k(2),3),', k_3 = ',num2str(k(3),3), ...
            ', k_4 = ',num2str(k(4),3),', k_5 = ', num2str(k(5),3),' k_6 = ',num2str(k(6),3),', k_{pf} = ', num2str(f,4)]);
    subplot(3,1,2);
    plot(t,y(:,2),':',tdata,x2data,'-'); grid;
    ymin = min(min(y(:,2)),min(x2data));
    ymax = max(max(y(:,2)),max(x2data));
    axis([0 final_time ymin ymax]);
    legend('Model x_2','Actual x_2',4);
    xlabel('Time (sec)');
    ylabel('Displacement (cm)');
    subplot(3,1,3);
    plot(t,y(:,3),':',tdata,x3data,'-'); grid;
    ymin = min(min(y(:,3)),min(x3data));
    ymax = max(max(y(:,3)),max(x3data));
    axis([0 final_time ymin ymax]);
    legend('Model x_3','Actual x_3',4);
    xlabel('Time (sec)');
    ylabel('Displacement (cm)');
  else    % no data to compare to
    orient landscape
    subplot(3,1,1);
    plot(t,y(:,1),'-'); grid;
    ymin = min(y(:,1));
    ymax = max(y(:,1));
    axis([0 final_time ymin ymax]);
    legend('Model x_1',4);
    xlabel('Time (sec)');
    ylabel('Displacement (cm)');
     title([' k_1 = ',num2str(k(1),3),', k_2 = ',num2str(k(2),3),', k_3 = ',num2str(k(3),3),...
            ', k_4 = ',num2str(k(4),3),', k_5 = ', num2str(k(5),3),' k_6 = ',num2str(k(6),3),', k_{pf} = ', num2str(f,3)]);
    subplot(3,1,2);
    plot(t,y(:,2),'-'); grid;
    ymin = min(y(:,2));
    ymax = max(y(:,2));
    axis([0 final_time ymin ymax]);
    legend('Model x_2',4);
    xlabel('Time (sec)');
    ylabel('Displacement (cm)');
    subplot(3,1,3);
    plot(t,y(:,3),'-'); grid;
    ymin = min(y(:,3));
    ymax = max(y(:,3));
    axis([0 final_time ymin ymax]);
    legend('Model x_3',4);
    xlabel('Time (sec)');
    ylabel('Displacement (cm)');
  end;
%
  return;