DynamicsResponse.rar_dynamics_velocity position

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') % 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 =