MAP_Gait_Dynamics/div_wolf_fixed_evolv.m

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'); % Find states min_dist from starting point. 
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;

% If multiple points are found in this range, the one which points best in
% the same direction as the old nearby point is selected. 

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 % if Necessary, the direction limit is repeatedly doubled to maximally pi radians
        dist_mult = 1;
        while isempty(Index) && dist_mult <= max_dist_mult % Whenever points cannot be found, the distance limit is increased stepwise to maximally five times the original limit. 
            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'); % The nearest neighbours are found by selecting data point closest to X1, from separate cycles with a temporal separation larger than twice the first minimum in the mutual information function. 
        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)); % The dt between neighbouring trajectories in state space were computed as a function of time, and averaged over many original pairs of initially nearest neighbours. 
% The maximum Lyapunov exponent is determined by averaging the rate of exponential divergence between the reference 
% and nearby point as the trajectory evolves between replacement steps.
Lambda = (log(DistEnd/DistStart) + LnDistReplacementsSum)/((is-1)/fs);