plotquadstatevariables.m

A Quadrotor simulation that is performed in MATLAB and SIMULINK

function [sys,x0,str,ts] = plotuavstatevariables(t,x,u,flag,C)
%
% modified 2/17/03 - Randy Beard
% modified 11/30/07 - RB
% modified 2/15/08 - RB

switch flag,

  %%%%%%%%%%%%%%%%%%
  % Initialization %
  %%%%%%%%%%%%%%%%%%
  case 0,
    [sys,x0,str,ts] = mdlInitializeSizes(C);

  %%%%%%%%%%%%%%%
  % Derivatives %
  %%%%%%%%%%%%%%%
  case 1,
    sys=mdlDerivatives(t,x,u,C);

  %%%%%%%%%%
  % Update %
  %%%%%%%%%%
  case 2,
    sys=mdlUpdate(t,x,u,C);

    
  %%%%%%%%%%%
  % Outputs %
  %%%%%%%%%%%
  case 3,
    sys=mdlOutputs(t,x,u,C);

  %%%%%%%%%%%%%
  % Terminate %
  %%%%%%%%%%%%%
  case 9,                                                
    sys = []; % do nothing

  %%%%%%%%%%%%%%%%%%%%
  % Unexpected flags %
  %%%%%%%%%%%%%%%%%%%%
  otherwise
    error(['unhandled flag = ',num2str(flag)]);
end

%end dsfunc

%
%=======================================================================
% mdlInitializeSizes
% Return the sizes, initial conditions, and sample times for the S-function.
%=======================================================================
%
function [sys,x0,str,ts] = mdlInitializeSizes(C)

sizes = simsizes;
sizes.NumContStates  = 0;
sizes.NumDiscStates  = 1+1+1+12+12+12+4;
sizes.NumOutputs     = 1+1;
sizes.NumInputs      = 12+12+12+4+4;
sizes.DirFeedthrough = 1;
sizes.NumSampleTimes = 1;


sys = simsizes(sizes);

x0 = [zeros(sizes.NumDiscStates,1)];
str = [];
ts  = [C.Ts 0]; 

%
%=============================================================================
% mdlDerivatives
% Return the derivatives for the continuous states.
%=============================================================================
%
function sys=mdlDerivatives(t,x,uu,C)
  
  
sys = [];

% end mdlDerivatives


% end mdlInitializeSizes
%
%=======================================================================
% mdlUpdate
% Handle discrete state updates, sample time hits, and major time step
% requirements.
%=======================================================================
%
function xup = mdlUpdate(t,x,uu,C)

  error_tracking = x(1);
  error_estimation = x(2);
  NN = 2;
  initialize   = x(1+NN);          % initial graphics
  fig_handles  = x(2+NN:end);      % figure handles
  NN = 0;
  px         = uu(1+NN);     % actual states
  py         = uu(2+NN);
  pz         = uu(3+NN);
  u          = uu(4+NN);
  v          = uu(5+NN);
  w          = uu(6+NN);
  phi        = uu(7+NN);          
  theta      = uu(8+NN);          
  psi        = uu(9+NN);          
  p          = uu(10+NN);
  q          = uu(11+NN);
  r          = uu(12+NN);
  NN = NN + 12;
  px_hat         = uu(1+NN);     % estimated states
  py_hat         = uu(2+NN);
  pz_hat         = uu(3+NN);
  pxdot_hat          = uu(4+NN);
  pydot_hat          = uu(5+NN);
  pzdot_hat          = uu(6+NN);
  phi_hat        = uu(7+NN);          
  theta_hat      = uu(8+NN);          
  psi_hat        = uu(9+NN);          
  p_hat          = uu(10+NN);
  q_hat          = uu(11+NN);
  r_hat          = uu(12+NN);
  NN = NN + 12;
  px_d         = uu(1+NN);     % desired states
  py_d         = uu(2+NN);
  pz_d         = uu(3+NN);
  pxdot_d          = uu(4+NN);
  pydot_d          = uu(5+NN);
  pzdot_d          = uu(6+NN);
  phi_d        = uu(7+NN);          
  theta_d      = uu(8+NN);          
  psi_d        = uu(9+NN);          
  p_d          = uu(10+NN);
  q_d          = uu(11+NN);
  r_d          = uu(12+NN);
  NN = NN + 12;
  pwm_f = sat(uu(NN+1),0,1);  % pwm commend for front motor
  pwm_r = sat(uu(NN+2),0,1);  % pwm commend for right motor
  pwm_b = sat(uu(NN+3),0,1);  % pwm commend for back motor
  pwm_l = sat(uu(NN+4),0,1);  % pwm commend for left motor
  NN = NN + 4;
  tn = uu(1+NN);  % target information
  te = uu(2+NN);
  td = uu(3+NN);
  tpsi = uu(4+NN);
%   px = -cos(psi)*(tn-pn) - sin(psi)*(te-pe);
%   py = sin(psi)*(tn-pn) - cos(psi)*(te-pe);
%   psi = tpsi - psi;
  % computer thrust produced by each motor
  thrust_front = C.kmotor_lift*pwm_f;
  thrust_right = C.kmotor_lift*pwm_r;
  thrust_back  = C.kmotor_lift*pwm_b;
  thrust_left  = C.kmotor_lift*pwm_l;
  % computer torque produced by each motor
  torque_front = C.kmotor_drag*pwm_f;
  torque_right = C.kmotor_drag*pwm_r;
  torque_back  = C.kmotor_drag*pwm_b;
  torque_left  = C.kmotor_drag*pwm_l;
  % compute body frame torques
  F = thrust_front + thrust_right + thrust_left + thrust_back;
  T_p = C.l*(thrust_left - thrust_right); % torque about roll axis
  T_q = C.l*(thrust_front - thrust_back); % torque about pitch axis
  T_r = torque_front + torque_right - torque_back - torque_left;  % torque about yaw axis
  pxdot = cos(theta)*u + sin(phi)*sin(theta)*v + cos(phi)*sin(theta)*2;
  pydot = cos(phi)*v - sin(phi)*w;
  pzdot = -sin(theta)*u + sin(phi)*cos(theta)*v + cos(phi)*cos(theta)*w;
  
  % tracking error
  error_tracking = error_tracking + C.Ts*( abs(px) + abs(py) + abs(psi) );
  
  % performance error
  error_estimation = error_estimation + C.Ts*(...
        abs(px-px_hat)...
      + abs(py-py_hat)...
      + abs(pz-pz_hat)...
      + abs(pxdot-pxdot_hat)...
      + abs(pydot-pydot_hat)...
      + abs(pzdot-pzdot_hat)...
      + abs(phi-phi_hat)...
      + abs(theta-theta_hat)...
      + abs(psi-psi_hat)...
      + abs(p-p_hat)...
      + abs(q-q_hat)...
      + abs(r-r_hat)...
      );



  if initialize==0, 
    % initialize the plot
    initialize = 1;
    figure(2), clf
    set(gcf,'DoubleBuffer','on')

    % state plots
    % pn
    NN = 0;
    subplot(8,2,1)
    hold on
    fig_handles(1+NN) = graph_y(t,px,[],'b');
    fig_handles(2+NN) = graph_y(t,px_hat,[],'g');
    fig_handles(3+NN) = graph_y(t,px_d,[],'r');   
    ylabel('p_x')
    set(get(gca,'YLabel'),'Rotation',0.0);

    NN = NN+3;
    subplot(8,2,3)
    hold on
    fig_handles(1+NN) = graph_y(t,py,[],'b');
    fig_handles(2+NN) = graph_y(t,py_hat,[],'g');
    fig_handles(3+NN) = graph_y(t,py_d,[],'r');   
    ylabel('p_y')
    set(get(gca,'YLabel'),'Rotation',0.0);

    NN = NN+3;
    subplot(8,2,5)
    hold on
    fig_handles(1+NN) = graph_y(t,pz,[],'b');
    fig_handles(2+NN) = graph_y(t,pz_hat,[],'g');
    fig_handles(3+NN) = graph_y(t,pz_d,[],'r');   
    ylabel('p_z')
    set(get(gca,'YLabel'),'Rotation',0.0);

    NN = NN+3;
    subplot(8,2,7)
    hold on
    fig_handles(1+NN) = graph_y(t,pxdot,[],'b');
    fig_handles(2+NN) = graph_y(t,pxdot_hat,[],'g');
    fig_handles(3+NN) = graph_y(t,pxdot_d,[],'r');   
    ylabel('pxdot')
    set(get(gca,'YLabel'),'Rotation',0.0);

    NN = NN+3;
    subplot(8,2,9)
    hold on
    fig_handles(1+NN) = graph_y(t,pydot,[],'b');
    fig_handles(2+NN) = graph_y(t,pydot_hat,[],'g');
    fig_handles(3+NN) = graph_y(t,pydot_d,[],'r');   
    ylabel('pydot')
    set(get(gca,'YLabel'),'Rotation',0.0);

    NN = NN+3;
    subplot(8,2,11)
    hold on
    fig_handles(1+NN) = graph_y(t,pzdot,[],'b');
    fig_handles(2+NN) = graph_y(t,pzdot_hat,[],'g');
    fig_handles(3+NN) = graph_y(t,pzdot_d,[],'r');   
    ylabel('pzdot')
    set(get(gca,'YLabel'),'Rotation',0.0);

    % phi
    NN = NN+3;
    subplot(8,2,2)
    hold on
    fig_handles(1+NN) = graph_y(t,(180/pi)*phi,[],'b');
    fig_handles(2+NN) = graph_y(t,(180/pi)*phi_hat,[],'g');
    fig_handles(3+NN) = graph_y(t,(180/pi)*phi_d,[],'r');   
    ylabel('\phi')
    set(get(gca,'YLabel'),'Rotation',0.0);

    % theta
    NN = NN+3;
    subplot(8,2,4)
    hold on
    fig_handles(1+NN) = graph_y(t,(180/pi)*theta,[],'b');
    fig_handles(2+NN) = graph_y(t,(180/pi)*theta_hat,[],'g');
    fig_handles(3+NN) = graph_y(t,(180/pi)*theta_d,[],'r');   
    ylabel('\theta')
    set(get(gca,'YLabel'),'Rotation',0.0);

    % psi
    NN = NN+3;
    subplot(8,2,6)
    hold on
    fig_handles(1+NN) = graph_y(t,(180/pi)*psi,[],'b');
    fig_handles(2+NN) = graph_y(t,(180/pi)*psi_hat,[],'g');
    fig_handles(3+NN) = graph_y(t,(180/pi)*psi_d,[],'r');   
    ylabel('\psi')
    set(get(gca,'YLabel'),'Rotation',0.0);

    % p
    NN = NN+3;
    subplot(8,2,8)
    hold on
    fig_handles(1+NN) = graph_y(t,(180/pi)*p,[],'b');
    fig_handles(2+NN) = graph_y(t,(180/pi)*p_hat,[],'g');
    fig_handles(3+NN) = graph_y(t,(180/pi)*p_d,[],'r');
    ylabel('p')
    set(get(gca,'YLabel'),'Rotation',0.0);    

    % q
    NN = NN+3;
    subplot(8,2,10)
    hold on
    fig_handles(1+NN) = graph_y(t,(180/pi)*q,[],'b');
    fig_handles(2+NN) = graph_y(t,(180/pi)*q_hat,[],'g');
    fig_handles(3+NN) = graph_y(t,(180/pi)*q_d,[],'r');
    ylabel('q')
    set(get(gca,'YLabel'),'Rotation',0.0);    

    % r
    NN = NN+3;
    subplot(8,2,12)
    hold on
    fig_handles(1+NN) = graph_y(t,(180/pi)*r,[],'b');
    fig_handles(2+NN) = graph_y(t,(180/pi)*r_hat,[],'g');
    fig_handles(3+NN) = graph_y(t,(180/pi)*r_d,[],'r');
    ylabel('r')
    set(get(gca,'YLabel'),'Rotation',0.0);    

    % F
    NN = NN+3;
    subplot(8,2,13)
    hold on
    fig_handles(1+NN) = graph_y(t,F,[],'b');
    ylabel('F')
    set(get(gca,'YLabel'),'Rotation',0.0);    

    % T_p
    NN = NN+1;
    subplot(8,2,15)
    hold on
    fig_handles(1+NN) = graph_y(t,T_p,[],'b');
    ylabel('Tp')
    set(get(gca,'YLabel'),'Rotation',0.0);    

      % T_q
    NN = NN+1;
    subplot(8,2,14)
    hold on
    fig_handles(1+NN) = graph_y(t,T_q,[],'b');
    ylabel('Tq')
    set(get(gca,'YLabel'),'Rotation',0.0);    

    % T_r
    NN = NN+1;
    subplot(8,2,16)
    hold on
    fig_handles(1+NN) = graph_y(t,T_r,[],'b');
    ylabel('Tr')
    set(get(gca,'YLabel'),'Rotation',0.0);    

  else  % do this at every time step
      
    % pn
    NN = 0;    
    graph_y(t, px,     fig_handles(1+NN));
    graph_y(t, px_hat, fig_handles(2+NN));
    graph_y(t, px_d,   fig_handles(3+NN));
    % drawnow

    % pe
    NN = NN+3;    
    graph_y(t, py,     fig_handles(1+NN));
    graph_y(t, py_hat, fig_handles(2+NN));
    graph_y(t, py_d,   fig_handles(3+NN));
    % drawnow
    
    % pd
    NN = NN+3;    
    graph_y(t, pz,     fig_handles(1+NN));
    graph_y(t, pz_hat, fig_handles(2+NN));
    graph_y(t, pz_d,   fig_handles(3+NN));
    % drawnow

    % u
    NN = NN+3;    
    graph_y(t, pxdot,     fig_handles(1+NN));
    graph_y(t, pxdot_hat, fig_handles(2+NN));
    graph_y(t, pxdot_d,   fig_handles(3+NN));
    % drawnow
    
    % v
    NN = NN+3;    
    graph_y(t, pydot,     fig_handles(1+NN));
    graph_y(t, pydot_hat, fig_handles(2+NN));
    graph_y(t, pydot_d,   fig_handles(3+NN));
    % drawnow
    
    % w
    NN = NN+3;    
    graph_y(t, pzdot,     fig_handles(1+NN));
    graph_y(t, pzdot_hat, fig_handles(2+NN));
    graph_y(t, pzdot_d,   fig_handles(3+NN));
    % drawnow

    % psi
    NN = NN+3;    
    graph_y(t, (180/pi)*phi,     fig_handles(1+NN));
    graph_y(t, (180/pi)*phi_hat, fig_handles(2+NN));
    graph_y(t, (180/pi)*phi_d,   fig_handles(3+NN));
    % drawnow

    % theta
    NN = NN+3;    
    graph_y(t, (180/pi)*theta,     fig_handles(1+NN));
    graph_y(t, (180/pi)*theta_hat, fig_handles(2+NN));
    graph_y(t, (180/pi)*theta_d,   fig_handles(3+NN));
    % drawnow

    % psi
    NN = NN+3;    
    graph_y(t, (180/pi)*psi,     fig_handles(1+NN));
    graph_y(t, (180/pi)*psi_hat, fig_handles(2+NN));
    graph_y(t, (180/pi)*psi_d,   fig_handles(3+NN));
    % drawnow

    % p
    NN = NN+3;
    graph_y(t, (180/pi)*p,     fig_handles(1+NN));
    graph_y(t, (180/pi)*p_hat, fig_handles(2+NN));
    graph_y(t, (180/pi)*p_d,   fig_handles(3+NN));
    % drawnow

    % q
    NN = NN+3;
    graph_y(t, (180/pi)*q,     fig_handles(1+NN));
    graph_y(t, (180/pi)*q_hat, fig_handles(2+NN));
    graph_y(t, (180/pi)*q_d,   fig_handles(3+NN));
    % drawnow

    % r
    NN = NN+3;
    graph_y(t, (180/pi)*r,     fig_handles(1+NN));
    graph_y(t, (180/pi)*r_hat, fig_handles(2+NN));
    graph_y(t, (180/pi)*r_d,   fig_handles(3+NN));
    % drawnow

    % F
    NN = NN+3;
    graph_y(t, F  ,             fig_handles(1+NN));
    % drawnow

    % Tp
    NN = NN+1;
    graph_y(t, T_p,             fig_handles(1+NN));
    % drawnow

    % Tq
    NN = NN+1;
    graph_y(t, T_q,             fig_handles(1+NN));
    % drawnow

    % Tr
    NN = NN+1;
    graph_y(t, T_r,             fig_handles(1+NN));
    % drawnow
    
end
  
  xup = [error_tracking; error_estimation; initialize; fig_handles];


%end mdlUpdate

%
%=============================================================================
% mdlOutputs
% Return the block outputs.
%=============================================================================
%
function sys=mdlOutputs(t,x,u,C)
  
sys = [x(1); x(2)];

% end mdlOutputs


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% graph y with lable mylabel
function handle = graph_y(t, y, handle, color)
  
  if isempty(handle),
    handle    = plot(t,y,color);
  else
    set(handle,'Xdata',[get(handle,'Xdata'),t]);
    set(handle,'Ydata',[get(handle,'Ydata'),y]);
    %drawnow
  end




%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% plot the variable y in blue, its estimated value yhat in green, and its 
% desired value yd in red, lab is the label on the graph
function [handle_y, handle_yhat, handle_yd] = graph_y_yhat_yd(t, y, yhat, yd, lab, handle)
  
  if isempty(handle),
    handle_y    = plot(t,y,'b');
    handle_yhat = plot(t,yhat,'g');
    handle_yd   = plot(t,yd,'r');
    ylabel(lab)
    set(get(gca,'YLabel'),'Rotation',0.0);
  else
    set(handle_y,'Xdata',[get(handle_y,'Xdata'),t]);
    set(handle_y,'Ydata',[get(handle_y,'Ydata'),y]);
    set(handle_yhat,'Xdata',[get(handle_yhat,'Xdata'),t]);
    set(handle_yhat,'Ydata',[get(handle_yhat,'Ydata'),yhat]);
    set(handle_yd,'Xdata',[get(handle_yd,'Xdata'),t]);
    set(handle_yd,'Ydata',[get(handle_yd,'Ydata'),yd]);     
    %drawnow
  end

%
%=============================================================================
% sat
% saturates the input between high and low
%=============================================================================
%
function out=sat(in, low, high)

  if in < low,
      out = low;
  elseif in > high,
      out = high;
  else
      out = in;
  end

% end sat