function [InstAxisOfRotation, AngularRate] = ComputeAngularDisplacement(q,dt,tau);

addpath('quaternions');

%if(tau > 0)
%Filter q
hrange = 0.4;
hbias = 0.4;
low  = max(min(hbias - (hrange./2), 1), 0);
high = max(min(hbias + (hrange./2), 1), 0);
%low = 0;
%high = 1;
hrangeLimited = high - low;

alpha = 1-exp(-dt/tau);

y = q(1,:);  %Initial Filter State
qout = zeros(size(q));  %Allocate Filter Output
qout(1,:) = y;

for ii = 2:length(q)
   x = q(ii,:);  %New Sample
   qdiff = qmult(y,qconj(x));  
   [Vdiff,dist] = qdecomp(qdiff);  %Angular distance (radians) between x and y returned in dist
   
   % Normalize dist output to the range [low, high];
   hlpf =  alpha; %(dist./pi).*hrangeLimited + low;
   y = slerp(y',x',hlpf)';
   qout(ii,:) = y;  %New Estimate
end
%else
%    qout = q;
%end


for i = 2:length(q)
    Qdiff = qmult(qout(i,:),qconj(qout(i-1,:)));
    [InstAxisOfRotation(i,:),PHIdiff(i)] = qdecomp(Qdiff);
end
AngularRate = (180/pi)*PHIdiff/dt;




end