DynamicsResponse(t,u,y,yd,tol)

function [TachGradient,Vel_rms,Vel_max,Acc,U,T,Y,Yd,VEL,index] = DynamicsResponse(t,u,y,yd,tol)
% DynamicsResponse:
%
% In gear driven high accuracy positioners (antennas, teloscope mounts,
% some robots, etc...) on axis position data is collected from one sensor 
% (synchro, resolver, encoder, etc..) mounted on axis and velocity data is 
% collected from another sensor (tach, resolver RDC, encoder, etc...)
% mounted on the actuator.  The scaling, aka tach gradient, between the 
% measured velocity and the on axis velocity is determined by gear ratio, 
% sensor characteristics (ie tach Kt), measurement scaling (ie at ADC or 
% RDC), and software.  Although it is possible to predetermine the tach
% gradient, it is often necessary to measure the tach gradient to confirm
% design expectations during installation and test.  During installation
% and testing is also often necessary to measure on axis velocity and
% acceleration to confirm the design goals are met.
%
% Assuming that the on axis sensor is calibrated seperately, it is possible
% to use this sensor, during large steps, to measure the tach gradient,
% velocity and acceleration.  Large steps are defined as steps large
% enough to allow the axis to accelerate to full velocity and to maintain
% full velocity for a short time.
%
% This function estimates the tach gradient, on axis velocity and 
% acceleration when a large step is taken and time, command, on axis 
% position, and non-on axis velocity are recorded.
%
% This function is primarily intended to analyze measured data but can 
% also be used on simulation results.
%
% useage:  [TachGradient,Vel_rms,Vel_max,Acc,U,T,Y,Yd,VEL,index] = DynamicsResponse(t,u,y,yd,tol)
%
% arguments (input):
% - t:  the time vector
% - u:  the step command vector.  Must contain only 1 step command that
% starts at 0 and moves to +/-step in 1 time sample.  Must be the same
% length as t.
% - y:  the on axis position response vector.  Must be the same length as t
% - yd:  the measured velocity response vector.  Must be the same length as
% t
% - tol:  tolerance used to compare floating point numbers.
%
% arguments (output):
% TachGradient:  Scalar Tach Gradient that scales Yd to on axis velocity
% Vel_rms:  Scalar RMS on axis velocity between when velocity is 1st at 80% 
% and is last at 80%
% Vel_max:  Scalar max on axis velocity
% Acc:  Scalar on axis acceleration.  Determined by the change in velocity
% from 20% to 80% of max velocity over the time necessary to do the same.
% U:  truncated command vector, includes only the data after the step command
% is provided
% T:  truncated time vector, includes only the data after the step command
% is provided
% Y:  truncated on axis position response vector, includes only the data 
% after the step command is provided
% Yd:  truncated measured velocity response vector, includes only the data 
% after the step command is provided
% Vel:  on axis velocity vector, includes only the data after the step 
% command is provided
% index:  vector of the T Y, Yd, and VEL locations where: velocity is at
% max, velocity is 1st at 80% of max, velocity is last at 80% of max and,
% velocity is at 20% of max
% 
% Required functions:
% - spline
% - ppval
% - polyfit
%
% Example: This example uses tf and lsim to generate the data to be
% analyzed and therefor requires the Control Systems Toolbox.
% % classic second order system
% w = 1;
% z = 0.7;
% Hs1 = tf(w^2,[1,2*z*w,w^2]);
% Hs2 = tf([w^2,0],[1,2*z*w,w^2]);
% 
% % create input arguments
% t = [0:0.001:10];
% u = 100*ones(size(t)); u(1:200) = zeros(200,1);
% y = lsim(Hs1,u,t)+0.03*(rand(size(t'))-0.5);
% vel = lsim(Hs2,u,t);
% yd = 0.2*vel+0.5*(rand(size(t'))-0.5);
% tol = 10^-6;
%
% % call DynamicsResponse
% [TachGradient,Vel_rms,Vel_max,Acc,U,T,Y,Yd,VEL,index] = DynamicsResponse(t,u,y,yd,tol);
% 
% % display results
% disp(strcat('Tach Gradient :',num2str(TachGradient)))
% disp(strcat('RMS Velocity :',num2str(Vel_rms)))
% disp(strcat('Max Velocity :',num2str(Vel_max)))
% disp(strcat('Acceleration from 20% to 80% of max velocity :',num2str(Acc)))
% 
% % plot results
% subplot(3,2,1)
% plot(t,u,t,y)
% grid
% ylabel('Command and Response')
% title('Starting Data')
% subplot(3,2,3)
% plot(t,vel)
% grid
% ylabel('On Axis Velocity')
% subplot(3,2,5)
% plot(t,yd)
% grid
% ylabel('Measured Velocity')
% xlabel('Time')
% 
% subplot(3,2,2)
% plot(T,U,T,Y)
% grid
% ylabel('Command and Response')
% title('Truncated Data')
% subplot(3,2,4)
% plot(T,VEL,T(index),VEL(index),'*')
% grid
% ylabel('On Axis Velocity')
% subplot(3,2,6)
% plot(T,Yd)
% grid
% ylabel('Measured Velocity')
% xlabel('Time')
%
% Acknowledgements:
% John D'Errico - for some many useful comments and examples on the MatLab
% File exchange
% Brad Smith and Rick Gattoni - for recommendations and explanations of
% existing systems
%
% Author:  Clinton Cathey
% E-mail:  [e-mail]
% Release: 1.0
% Date:  3/17/2009

% error check input
    % check number of input arguments
    if (nargin < 5) || (nargin > 5)
        error('StepResponse must have 5 arguments only')
    end
    % verify input argments are vectors
    if (min(size(t)) > 1) || (min(size(u)) > 1) || (min(size(y)) > 1) || (min(size(yd)) > 1)
        error('StepResponse arguments t, u, y, and yd must be vectors, not matrices')
    end
    if (max(size(t)) < 2) || (max(size(u)) < 2) || (max(size(y)) < 2) || (max(size(yd)) < 2)
        error('StepResponse arguments t, u, y, and yd must be vectors, not scalars')
    end
    % verify input argument tol is a scalar
    if (length(tol) > 1)
        error('StepResponse arguments tol must be a scalars')
    end
    % verify input arguments are the same length
    if (length(t) > length(u)+tol) || (length(t) < length(u)-tol) || (length(t) > length(y)+tol) || (length(t) < length(y)-tol) || (length(t) > length(y)+tol) || (length(t) < length(y)-tol)
        error('StepResponse arguments t, u, y, and yd must be vectors of the same length')
    end
    
% transpose input vectors as needed
    [M,N] = size(t);
    if N > M,
        t = t';
    end
    [M,N] = size(u);
    if N > M,
        u = u';
    end
    [M,N] = size(y);
    if N > M,
        y = y';
    end
    [M,N] = size(yd);
    if N > M,
        yd = yd';
    end    
    
% find time when step command was given    
    t0 = 0; % initialize t0, the vector index when the step command is given
    for J = 1:length(t),
        % search for change in u.  the 1st change above the tol is assumed
        % to be the step command
        if (t0 == 0) && (abs(u(J)) >= tol),
            t0 = J;
        end
        % continue to search for changes in u.  if additional changes
        % greater than tol are found, generate an error
        if (t0 > 0) && ((abs(u(J)) > abs(u(t0)) + tol) || (abs(u(J)) < abs(u(t0)) - tol))
            error('StepResponse must have only 1 step command.  The step command must start at 0units and move to +/- step size in 1 time sample.')
        end
    end
    
% truncate data series to t0 and above.  zero the t vector.  initialize
% index vector
    T = t(t0:length(t))-t(t0);
    U = u(t0:length(t));
    Y = y(t0:length(t));
    Yd = yd(t0:length(t));
    index = zeros(4,1);
       
% determine tach gradient
    % fit a spline to the position data
    PP_pos = spline(T,Y);
    
    % take the derivative of the spline
    PP_vel = PP_pos;
    PP_vel.coefs(:,4) = PP_vel.coefs(:,3);
    PP_vel.coefs(:,3) = 2*PP_vel.coefs(:,2);
    PP_vel.coefs(:,2) = 3*PP_vel.coefs(:,1);
    PP_vel.coefs(:,1) = zeros(size(PP_vel.coefs(:,1)));
    
    % evaluate the derivative of position spline along T - dirty on axis
    % velocity
    vel = ppval(PP_vel,T);
    
    % fit a 1st order polynomial to corrolate vel and Yd;
    TG = polyfit(Yd,vel,1);
    TachGradient = TG(1);
    
% determine rms and max velocity
    % scale Yd to find cleanner on axis velocity
    VEL = Yd*TachGradient;
    
    % calculate max velocity
    [Vel_max,Imax] = max(abs(VEL));
    
    % search response vector
    vel_ucount = 0; % initialize vel counters
    vel_lcount = 0; 
    acc_count = 0; % initialize acc counter, used below
    for J = 1:length(T), 
        % search for when VEL is 1st greater than ~20% of max
        if (acc_count == 0) && (abs(VEL(J)) >= 0.2*Vel_max)
            acc_count = J;
        end
        % search for when VEL is 1st greater than ~80% of max
        if (vel_lcount == 0) && (abs(VEL(J)) > 0.80*Vel_max)
            vel_lcount = J;
        end
        % search for when VEL is last greater than ~80% of max
        if (J > Imax) && (vel_ucount == 0) && (abs(VEL(J)) < 0.80*Vel_max)
            vel_ucount = J;
        end        
    end
    
    % calculate rms vel
    Vel_rms = (sum(VEL(vel_lcount:vel_ucount).^2)/length(VEL(vel_lcount:vel_ucount)))^0.5;
    
    % update the index vector
    index(1,1) = Imax;
    index(2,1) = vel_lcount;
    index(3,1) = vel_ucount;
    index(4,1) = acc_count;
    
% determine acceleration
    % determine change in velocity from ~20% to ~80% of max velocity
    dV = abs(VEL(vel_lcount)-VEL(acc_count));
    
    % determine change time from  ~20% to ~80% of max velocity
    dt = T(vel_lcount)-T(acc_count);
    
    % calculate acceleration
    Acc = dV/dt;