MAP_Gait_Dynamics/div_wolf_fixed_evolv.m

1 line
4.9 KiB
Matlab

function [Lambda]=div_wolf_fixed_evolv(states, fs, min_dist, max_dist, max_dist_mult, max_theta, period, evolv)
%% Description
% Calculate maximum lyapunov exponent from state space according to Wolf's method:
% Wolf, A., et al., Determining Lyapunov exponents from a time series.
% Physica D: 8 Nonlinear Phenomena, 1985. 16(3): p. 285-317.
%
% Input:
% states: states in state space
% fs: sample frequency
% min_dist: neighbours should be more than this distandce away to be
% selected (assumed noise amplitude )
% max_dist: maximum distance to which the divergence may grow before
% replacing the neighbour
% max_theta: maximum angle between vectors from feducial point to old and
% new neighbours, this is the absolute maximum, first search
% nearer
% period: indicates period of time-wise near samples to exclude in
% nearest neighbour search
% evolv: time step after which the neighbour is replaced
%
% Output:
% Lambda: the estimated Lyapunov exponent
% ExtraArgs: optional struct containing additional info for debugging or
% analysis of the method
%
%% Copyright
% COPYRIGHT (c) 2012 Sietse Rispens, VU University Amsterdam
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program. If not, see <http://www.gnu.org/licenses/>.
%% Author
% Sietse Rispens
%% History
% 23 October 2012, initial version based on div_wolf
[N,D]=size(states);
N_opt = N-evolv;
states_opt = states(1:N_opt,:);
One2N_opt=(1:N_opt)';
is=1;
% search closest point to states(1,:), but further than min_dist away
DeltaFidPoint = zeros(N_opt,D);
for dim = 1:D,
DeltaFidPoint(:,dim) = states_opt(:,dim)-states(is,dim);
end
DeltaFidPointSqr = DeltaFidPoint.^2;
DistSqr = sum(DeltaFidPointSqr,2);
Distances = sqrt(DistSqr);
CosTh = zeros(N_opt,1);
in = find(DistSqr==min(DistSqr(DistSqr>=min_dist^2 & abs(One2N_opt-is)>period)) & abs(One2N_opt-is)>period,1,'first');
if isempty(in)
warning('Cannot calculate Wolf''s Lyapunov exponent, no states further than min_dist from starting point');
Lambda = nan;
return;
end
DistStart = Distances(in);
LnDistReplacementsSum = 0;
for is=(1+evolv):evolv:N,
in = in+evolv;
% fill distance and cos matrices with new values
DeltaOld = (states(in,:)-states(is,:))';
DistOld = sqrt(sum(DeltaOld.^2));
for dim = 1:D,
DeltaFidPoint(:,dim) = states_opt(:,dim)-states(is,dim);
end
DeltaFidPointSqr(:,:) = DeltaFidPoint.^2;
DistSqr(:,:) = sum(DeltaFidPointSqr,2);
Distances(:,:) = sqrt(DistSqr);
CosTh(:,:) = abs(DeltaFidPoint*DeltaOld)./DistOld./Distances;
Index = [];
search_theta = max_theta;
while isempty(Index) && search_theta < pi
dist_mult = 1;
while isempty(Index) && dist_mult <= max_dist_mult
search_dist = dist_mult*max_dist;
Candidates = find(Distances <= search_dist & CosTh >= cos(search_theta) ...
& Distances>= min_dist & abs(One2N_opt-is)>period);
if ~isempty(Candidates)
% find the best of candidates
if numel(Candidates) > 1
Candidates = Candidates(CosTh(Candidates)==max(CosTh(Candidates)));
if numel(Candidates) > 1
Candidates = Candidates(Distances(Candidates)==min(Distances(Candidates)));
if numel(Candidates) > 1
Candidates = Candidates(abs(One2N_opt(Candidates)-is)==max(abs(One2N_opt(Candidates)-is)));
end
end
end
Index = Candidates(1);
end
dist_mult = dist_mult+1;
end
search_theta = search_theta*2;
end
if isempty(Index) && in > N_opt % we can not evolve the old point and no new one was found
Index = find(DistSqr==min(DistSqr(DistSqr>=min_dist^2 & abs(One2N_opt-is)>period))& abs(One2N_opt-is)>period,1,'first');
if isempty(Index)
warning('Cannot calculate Wolf''s Lyapunov exponent, no states further than min_dist from fiducial point');
Lambda = nan;
return;
end
end
if ~isempty(Index)
LnDistReplacementsSum = LnDistReplacementsSum + log(DistOld/Distances(Index));
in = Index;
end
end
DistEnd = sqrt(sum((states(is,:)-states(in,:)).^2));
Lambda = (log(DistEnd/DistStart) + LnDistReplacementsSum)/((is-1)/fs);