diff --git a/gaitanalysis/octave/2d_inverse_dynamics/myfiltfilt.m b/gaitanalysis/octave/2d_inverse_dynamics/myfiltfilt.m index 01b1765..aee08b3 100644 --- a/gaitanalysis/octave/2d_inverse_dynamics/myfiltfilt.m +++ b/gaitanalysis/octave/2d_inverse_dynamics/myfiltfilt.m @@ -1,4 +1,4 @@ -function [y, yd, ydd] = myfiltfilt(t, x, f0); +function [y, yd, ydd] = myfiltfilt(t, x, f0) % performs low pass filtering and differentiation % method is the same as used in HBM but bidirectional to eliminate lag @@ -9,28 +9,44 @@ % f0 (scalar) Corner frequency % % Outputs -% y (Nsamples x 1) Filtered signal -% yd (Nsamples x 1) First derivative -% ydd (Nsamples x 1) Second derivative +% y (Nsamples x Nchannels) Filtered signal +% yd (Nsamples x Nchannels) First derivative +% ydd (Nsamples x Nchannels) Second derivative - C = 0.802; % correction factor for dual pass 2nd order filter (Winter book) - y = rtfilter_batch(-flipud(t), flipud(x), f0/C); % filter backwards in time - [y, yd, ydd] = rtfilter_batch(t, flipud(y), f0/C); % filter forward in time + C = 0.802; % correction factor for dual pass 2nd order filter (Winter book) + y = filter_batch(-flipud(t), flipud(x), f0/C); % filter backwards in time + [y, yd, ydd] = filter_batch(t, flipud(y), f0/C); % filter forward in time end %=================================================================================== -function [y, yd, ydd] = rtfilter_batch(t,x,f0) +function [y, yd, ydd] = filter_batch(t,x,f0) % filters a time series and also returns derivatives % uses real-time second order Butterworth filter (rtfilter.m) + + % some constants we will need + a = (2*pi*f0)^2; + b = sqrt(2)*(2*pi*f0); + % allocate memory for the results n = size(x,1); y = zeros(size(x)); yd = zeros(size(x)); ydd = zeros(size(x)); - for i = 1:n - [y(i,:), yd(i,:), ydd(i,:)] = rtfilter(t(i),x(i,:),f0); + % Integrate the filter state equation using the midpoint Euler method with step h + % initial conditions are y=0 and yd=0 + for i = 2:n + h = t(i)-t(i-1); % time step + denom = 4 + 2*h*b + h^2*a; + A = (4 + 2*h*b - h^2*a)/denom; + B = 4*h/denom; + C = -4*h*a/denom; + D = (4 - 2*h*b - h^2*a)/denom; + E = 2*h^2*a/denom; + F = 4*h*a/denom; + y(i) = A*y(i-1) + B*yd(i-1) + E*(x(i)+x(i-1))/2; + yd(i) = C*y(i-1) + D*yd(i-1) + F*(x(i)+x(i-1))/2; + ydd(i) = (yd(i)-yd(i-1))/h; end - end -%=================================================================================== \ No newline at end of file +%=================================================================================== diff --git a/gaitanalysis/octave/2d_inverse_dynamics/rtfilter.m b/gaitanalysis/octave/2d_inverse_dynamics/rtfilter.m deleted file mode 100644 index 67964e1..0000000 --- a/gaitanalysis/octave/2d_inverse_dynamics/rtfilter.m +++ /dev/null @@ -1,52 +0,0 @@ -function [y, yd, ydd] = rtfilter(t, x, f0) - -% Real-time low pass Butterworth filter, processes one data sample at a time. -% -% Inputs: -% t time stamp of input x (s) -% x input data at time t (may be a scalar, array, or matrix) -% f0 cutoff frequency (Hz) -% -% Outputs: -% y filtered x -% yd filtered derivative of x -% ydd filtered second derivative of x - - % Internal state of the filter must be preserved between function calls - persistent state - - % If this is the first call to this function, or we have gone back in time, reset the filter - if isempty(state) || (t <= state.t) - y = x; - yd = zeros(size(x)); - ydd = zeros(size(x)); - % Otherwise, solve the state equation by Euler integration - else - % Calculate time between current and previous sample - h = t - state.t; - - % Compute coefficients of the state equation - a = (2*pi*f0)^2; - b = sqrt(2)*(2*pi*f0); - - % Integrate the filter state equation using the midpoint Euler method with step h - denom = 4 + 2*h*b + h^2*a; - A = (4 + 2*h*b - h^2*a)/denom; - B = 4*h/denom; - C = -4*h*a/denom; - D = (4 - 2*h*b - h^2*a)/denom; - E = 2*h^2*a/denom; - F = 4*h*a/denom; - y = A*state.y + B*state.yd + E*(x+state.x)/2; - yd = C*state.y + D*state.yd + F*(x+state.x)/2; - ydd = (yd-state.yd)/h; - - end - - % Store the filter state - state.t = t; - state.x = x; - state.y = y; - state.yd = yd; - -end