AngularVelocity

AngularVelocity - Compute instantaneous angular velocity.

 Compute angular velocity for a time-varying vector X. This is computed as
 the vector product of X and its derivative DX: W(t) = X(t) ^ DX(t)

  USAGE

    omega = AngularVelocity(X,smooth)

    X              position <a href="matlab:help samples">samples</a>
    smooth         optional standard deviation for Gaussian kernel used for
                   differentiating, measured in number of samples
                   (default = no smoothing)

  NOTE

    Angular velocities are returned in radians/s.

  SEE

    See also LinearVelocity.

0001 function W = AngularVelocity(X,smooth)
0002 
0003 %AngularVelocity - Compute instantaneous angular velocity.
0004 %
0005 % Compute angular velocity for a time-varying vector X. This is computed as
0006 % the vector product of X and its derivative DX: W(t) = X(t) ^ DX(t)
0007 %
0008 %  USAGE
0009 %
0010 %    omega = AngularVelocity(X,smooth)
0011 %
0012 %    X              position <a href="matlab:help samples">samples</a>
0013 %    smooth         optional standard deviation for Gaussian kernel used for
0014 %                   differentiating, measured in number of samples
0015 %                   (default = no smoothing)
0016 %
0017 %  NOTE
0018 %
0019 %    Angular velocities are returned in radians/s.
0020 %
0021 %  SEE
0022 %
0023 %    See also LinearVelocity.
0024 
0025 % Copyright (C) 2004-2011 by Michaël Zugaro
0026 %
0027 % This program is free software; you can redistribute it and/or modify
0028 % it under the terms of the GNU General Public License as published by
0029 % the Free Software Foundation; either version 3 of the License, or
0030 % (at your option) any later version.
0031 
0032 if nargin < 1,
0033     error('Incorrect number of parameters (type ''help <a href="matlab:help AngularVelocity">AngularVelocity</a>'' for details).');
0034 end
0035 if nargin >= 2,
0036     if ~isdscalar(smooth,'>=0'),
0037         error('Incorrect smoothing stdev (type ''help <a href="matlab:help AngularVelocity">AngularVelocity</a>'' for details).');
0038     end
0039 else
0040     smooth = 0;
0041 end
0042 
0043 % Norm X
0044 Y = X(:,2:3).*X(:,2:3);
0045 N = sqrt(Y(:,1)+Y(:,2));
0046 X = [X(:,1) X(:,2)./N X(:,3)./N];
0047 
0048 % Differentiate
0049 DX = Diff(X,'smooth',smooth);
0050 
0051 % Compute vectorial product
0052 
0053 W = [X(:,1) X(:,2).*DX(:,3)-X(:,3).*DX(:,2)];