function [ vel acc jerk ] = vel_acc_with_butter(x, sample_rate, params) % VEL_ACC_WITH_BUTTER - % [ vel, acc, jerk ] = vel_acc_with_butter(x, sample_rate, params) - smooth and get: % velocity, acceleration and jerk (= change of accelartion) % input: % x - position % params - % .draw (default= 0) % .cutoff (default = 5hz) for Butter filter % .do_acc, do_jerk = do higher derivatives (default= 0) % .position_already_filtered (=0) % % 2012-11-19: compute usually only Vel to save computing time. % 2014-01-09: using Butter filter from Jason % % NOTE - this give Vel unit as ticks/sec not tics/sample (factor *50) as % vel_acc_with_bfilt, need to match if this it to be used with other MG_ functions % % Default Values for Optional Inputs % if ~exist('sample_rate','var'), sample_rate = 50; end if ~exist('params','var') || ~isfield(params,'draw'), params.draw = 0; end params = add_default( params, 'cutoff', 5); % threshold for filter Hz params = add_default( params, 'do_acc', 0); params = add_default( params, 'do_jerk', 0); params = add_default( params, 'position_already_filtered', 0); if params.do_jerk, params.do_acc = 1;end % check that is 1-D; row/col if size(x,1) < size(x,2), x = x'; row_vec = 1; else row_vec = 0; end if size(x,2) > 1, error(' should be 1-D') end % Position (filter if needed BEFORE diff) if ~params.position_already_filtered, [x,B,A] = jason_butter_filter(x, params.cutoff, sample_rate); else % then just do once here to get the B,A for filter (takes time) [~,B,A] = jason_butter_filter(x, params.cutoff, sample_rate); end dt = 1 / sample_rate; % Velocity vel = [0; diff(x)./dt]; vel = filtfilt(B,A,vel); % -- 06.04.2014, Try, force flat-postiion=>zero-vel (filtter makes a bit jittery) small_pos_error = max(x) * 10^-10; ix = find( abs(diff(x)) < small_pos_error ) + 1; vel( ix ) = 0; % Acceleration if ~params.do_acc, acc = []; else acc = [0; diff(vel)./dt]; acc = filtfilt(B,A,acc); % BUG!!!! 08-09-2016, no need to zero acc for small positon errors, did for vel above (this was % probably copy/pasted). %acc( ix ) = 0; end % Jerk if ~params.do_jerk, jerk = []; else jerk = [0; diff(acc)./dt]; jerk = filtfilt(B,A,jerk); jerk( ix ) = 0; end if row_vec, vel = vel'; acc = acc'; jerk = jerk'; end % plot if needed if params.draw, clf for k = 1 : 4, subplot(4,1,k); switch k, case 1, plot(x); title('position'); case 2, plot(vel); title('velocity'); case 3, plot(acc); title('acceleration'); case 4, plot(jerk); title('jerk'); end end end