text:
%% Analyzing Neural Time Series Data
% Matlab code for Chapter 29
% Mike X Cohen
%
% This code accompanies the book, titled "Analyzing Neural Time Series Data"
% (MIT Press). Using the code without following the book may lead to confusion,
% incorrect data analyses, and misinterpretations of results.
% Mike X Cohen assumes no responsibility for inappropriate or incorrect use of this code.

%% Figure 29.2

% create two signals
time    = 0:.0001:1;
signal1 = sin(2*pi*10*time);
signal2 = rand(size(signal1))*2-1; % uniform random numbers in the same scale as the sine wave

% plot signals
figure
subplot(221)
plot(time,signal1)
set(gca,'xlim',[time(1) time(end)])
subplot(222)
plot(time,signal2,'r')
set(gca,'xlim',[time(1) time(end)])

% bin data (via Matlab function hist)
nbins = 50;
[hdat1,x1] = hist(signal1,nbins);
[hdat2,x2] = hist(signal2,nbins);

% convert histograms to probability values
hdat1 = hdat1./sum(hdat1);
hdat2 = hdat2./sum(hdat2);

% plot histograms
subplot(223)
plot(x1,hdat1)
hold on
plot(x2,hdat2,'r')
legend({'Sine wave';'Random data'})
xlabel('Value bins')
ylabel('Probability')

subplot(224)
plot(sort(signal1))
hold on
plot(sort(signal2),'r')
set(gca,'xlim',[0 length(signal1)])

% The following code using the 'eval' command may seem a bit needlessly
% complex, but it introduces you to this useful Matlab function. The
% main advantage here is to call different variables inside a loop,
% which would otherwise not be possible because it is the variable
% names that are different, not indices into variables. For example,
% notice how the variables hdat1 and hdat2 are called.
for i=1:2
    eval([ 'entro(' num2str(i) ') = -sum(hdat' num2str(i) '.*log2(hdat' num2str(i) '+eps));' ]);
end

% the following code will do this same thing:
% entro(1) = -sum(hdat1.*log2(hdat1+eps));
% entro(2) = -sum(hdat2.*log2(hdat2+eps));

disp([ 'Entropies of sine wave and random noise are ' num2str(entro(1)) ' and ' num2str(entro(2)) '.' ]);

%% Figure 29.3

% range of bin numbers
nbins = 10:2000;

entropyByBinSize=zeros(size(nbins));

for nbini=1:length(nbins)

    % bin data, transform to probability, and eliminate zeros
    hdat = hist(signal1,nbins(nbini));
    hdat = hdat./sum(hdat);

    % compute entropy
    entropyByBinSize(nbini) = -sum(hdat.*log2(hdat+eps));
end

figure
plot(nbins,entropyByBinSize)
xlabel('Number of bins'), ylabel('Entropy')

%% Figure 29.4

% optimal number of bins for histogram based on a few different guidelines
n = length(signal1);
maxmin_range = max(signal1)-min(signal1);

% note: the function iqr (inter-quartile range) is in the stats toolbox.
% If you don't have this toolbox, you can write your own similar function
% by sorting the values, finding the values that are 25% and 75% of the
% sorted distribution, and then subtracting the 25% number from the 75% number.
fd_bins      = ceil(maxmin_range/(2.0*iqr(signal1)*n^(-1/3))); % Freedman-Diaconis
scott_bins   = ceil(maxmin_range/(3.5*std(signal1)*n^(-1/3))); % Scott
sturges_bins = ceil(1+log2(n)); % Sturges

figure
subplot(211)
% plot up to 50 bins
[junk,maxNbins] = min(abs(nbins-50)); % index of nbins that most closely matches 50
plot(nbins(1:maxNbins),entropyByBinSize(1:maxNbins))

hold on
plot([fd_bins fd_bins],get(gca,'ylim'),'m','linew',2)
plot([scott_bins scott_bins],get(gca,'ylim'),'k','linew',2)
plot([sturges_bins sturges_bins],get(gca,'ylim'),'r','linew',2)

legend({'entropy';'Freedman-Diaconis';'Scott';'Sturges'})
xlabel('Number of bins'), ylabel('Entropy')

subplot(223)
[y,x]=hist(signal1,fd_bins);
h=bar(x,y,'histc');
set(h,'linestyle','none');
set(gca,'xlim',[min(signal1) max(signal1)*1.1])
xlabel('Value'), ylabel('Count')
title('Optimal number of bins (FD rule)')

subplot(224)
[y,x]=hist(signal1,2000);
h=bar(x,y,'histc');
set(gca,'xlim',[min(signal1) max(signal1)]*1.05)
xlabel('Value'), ylabel('Count')
title('Too many bins (2000)')

%% Figure 29.5

load sampleEEGdata

% entropy over all sensors
time4entropy = [  100  400 ]; % in ms
base4entropy = [ -400 -100 ]; % in ms
topo_entropy = zeros(size(EEG.chanlocs));

timeidx=zeros(size(time4entropy)); baseidx=zeros(size(base4entropy));
for i=1:2
    [junk,timeidx(i)] = min(abs(EEG.times-time4entropy(i)));
    [junk,baseidx(i)] = min(abs(EEG.times-base4entropy(i)));
end

for chani=1:EEG.nbchan

    % entropy during task
    tempdat = EEG.data(chani,timeidx(1):timeidx(2),:);
    hdat = hist(tempdat(:),25);
    hdat = hdat./sum(hdat);
    task_entropy =  -sum(hdat.*log2(hdat+eps));

    % entropy during pre-stim baseline
    tempdat = EEG.data(chani,baseidx(1):baseidx(2),:);
    hdat = hist(tempdat(:),25);
    hdat = hdat./sum(hdat);
    base_entropy =  -sum(hdat.*log2(hdat+eps));

    % compute entropy
    topo_entropy(chani) = task_entropy - base_entropy;
end

figure
% Note: topoplot is a function in the eeglab toolbox
topoplot(topo_entropy,EEG.chanlocs,'maplimits',[-.5 .5],'plotrad',.53,'electrodes','off','numcontour',0);

% entropy over time in one electrode (fcz or po8)
sensor4entropy = 'fcz';
times2save     = -300:50:1200;
timewindow     = 400; % ms

timewindowidx = round(timewindow/(1000/EEG.srate)/2);
times2saveidx = zeros(size(times2save));
for i=1:length(times2save)
    [junk,times2saveidx(i)]=min(abs(EEG.times-times2save(i)));
end

electrodeidx = find(strcmpi(sensor4entropy,{EEG.chanlocs.labels}));

timeEntropy = zeros(1,length(times2save));

for timei = 1:length(times2save)

    tempdata = EEG.data(electrodeidx,times2saveidx(timei)-timewindowidx:times2saveidx(timei)+timewindowidx,1:30);
    hdat = hist(tempdata(:),25);
    hdat = hdat./sum(hdat);

    % compute entropy
    timeEntropy(1,timei) = -sum(hdat.*log2(hdat+eps));

    tempdata = EEG.data(electrodeidx,times2saveidx(timei)-timewindowidx:times2saveidx(timei)+timewindowidx,end-30:end);
    hdat = hist(tempdata(:),25);
    hdat = hdat./sum(hdat);

    % compute entropy
    timeEntropy(2,timei) = -sum(hdat.*log2(hdat+eps));
end

figure
plot(times2save,timeEntropy)
xlabel('Time (ms)'), ylabel('Entropy (bits)')
title([ 'Entropy over time from electrode ' sensor4entropy ])
legend({'First 30 trials';'last 30 trials'})

%% Figure 29.6

% Note about this figure: The panels use the same code but with different
% input signals. Comment out some of the lines below to recreate each panel.

% right panel: random noise
signal1 = rand(size(signal1))*2-1;
signal2 = rand(size(signal1))*2-1;

% center panel: one pure sine wave and one sine wave plus random noise
signal1 = sin(2*pi*10*time);
signal2 = signal1 + randn(size(signal1))/2;

% left panel: one pure sine wave and its inverse
signal1 = sin(2*pi*10*time);
signal2 = -signal1;

% determine the optimal number of bins for each variable
n            = length(signal1);
maxmin_range = max(signal1)-min(signal1);
fd_bins1     = ceil(maxmin_range/(2.0*iqr(signal1)*n^(-1/3))); % Freedman-Diaconis

n            = length(signal2);
maxmin_range = max(signal2)-min(signal2);
fd_bins2     = ceil(maxmin_range/(2.0*iqr(signal2)*n^(-1/3))); % Freedman-Diaconis

% and use the average...
fd_bins = ceil((fd_bins1+fd_bins2)/2);

% bin data (using histc this time)
edges = linspace(min(signal1),max(signal1),fd_bins+1);
[nPerBin1,bins1] = histc(signal1,edges);

edges = linspace(min(signal2),max(signal2),fd_bins+1);
[nPerBin2,bins2] = histc(signal2,edges);

% compute joint frequency table
jointprobs = zeros(fd_bins);
for i1=1:fd_bins
    for i2=1:fd_bins
        jointprobs(i1,i2) = sum(bins1==i1 & bins2==i2);
    end
end
jointprobs=jointprobs./sum(jointprobs(:));

figure
subplot(211)
plot(time,signal1)
subplot(212)
plot(time,signal2)

figure
imagesc(jointprobs)
colormap gray
set(gca,'clim',[0 .01],'ydir','normal')
colorbar
xlabel('Signal 2 bin'), ylabel('Signal 1 bin')

%% Figure 29.7

figure
subplot(221)
x = 0:.001:1;
y = x;
plot(x,y,'.')
title([ 'MI=' num2str(mutualinformationx(x,y)) ', r_s=' num2str(corr(x(:),y(:),'type','s')) ])
axis square

subplot(222)
x = 0:.001:1;
y = -x.^3;
plot(x,y,'.')
title([ 'MI=' num2str(mutualinformationx(x,y)) ', r_s=' num2str(corr(x(:),y(:),'type','s')) ])
axis square

subplot(223)
x=cos(0:.01:2*pi);
y=sin(0:.01:2*pi);
plot(x,y,'.')
title([ 'MI=' num2str(mutualinformationx(x,y)) ', r_s=' num2str(corr(x(:),y(:),'type','s')) ])
axis square

subplot(224)
x=[cos(0:.01:2*pi) cos(0:.01:2*pi)+1];
y=[sin(0:.01:2*pi) sin(0:.01:2*pi)-1];
plot(x,y,'.')
title([ 'MI=' num2str(mutualinformationx(x,y)) ', r_s=' num2str(corr(x(:),y(:),'type','s')) ])
axis square

%% Figure 29.8

% Theoretical size of errors as a function of histogram bins and N
% the Matlab 'inline' function is a useful tool to substitute small
% functions (1-2 lines of code) that only need to be run locally.
% However, as mentioned in chapter 26, this function will be removed in
% future versions of Matlab.
entropy_error = inline('(b-1)./(2.*n.*log(2))');
mutinfo_error = inline('(b-1).^2./(2.*n.*log(2))');

n = 20:300;
nfixed = 15;

figure
subplot(211)
plot(n,entropy_error(nfixed,n))
hold on
plot(n,entropy_error(ceil(1+log2(n)),n),'r')
legend({[ num2str(nfixed) ' bins' ];'Sturges'' rule'})
xlabel('Number of data points'), ylabel('Entropy error (bits)')
set(gca,'xlim',[n(1) n(end)])

subplot(212)
plot(n,mutinfo_error(nfixed,n))
hold on
plot(n,mutinfo_error(ceil(1+log2(n)),n),'r')
legend({[ num2str(nfixed) ' bins' ];'Sturges'' rule'})
xlabel('Number of data points'), ylabel('Mutual information error')
set(gca,'xlim',[n(1) n(end)])

%% Figure 29.9

x = [cos(0:.01:2*pi) cos(0:.01:2*pi)+1];
y = [sin(0:.01:2*pi) sin(0:.01:2*pi)-1];

figure
subplot(221)
plot(x,y,'.')
axis([-3 3 -3 3])

subplot(212)

noiselevels = 0:.01:1;
mi = zeros(size(noiselevels));
for ni=1:length(noiselevels)
    mi(ni) = mutualinformationx(x,y+randn(size(y))*noiselevels(ni),20);
end

plot(noiselevels,mi)
xlabel('Noise level'), ylabel('Mutual information')

subplot(222)
plot(x,y+randn(size(y))*noiselevels(round(ni/2)),'.');
axis([-3 3 -3 3])

%% Figure 29.9d (takes a while to run)

nrange = 300:205:3000;
noiselevels = 0:.01:1;

mi = zeros(length(nrange),length(noiselevels));
b  = zeros(1,length(nrange)); % number of histogram bins

for ni=1:length(nrange)

    % define time
    t = linspace(0,2*pi,nrange(ni));
    % define signals
    x = [cos(t) cos(t)+1];
    y = [sin(t) sin(t)-1];

    for noi=1:length(noiselevels)
        if noi==1 % keep number of bins constant across noise levels within each number of points
            [mi(ni,noi),~,b(ni)] = mutualinformationx(x,y+randn(size(y))*noiselevels(noi));
        else
            mi(ni,noi) = mutualinformationx(x,y+randn(size(y))*noiselevels(noi),b(ni));
        end
    end
end

% convert to % change from best-case scenario (no noise, large N)
mip=100.*(mi-mi(end,1))./mi(end,1);

figure
contourf(noiselevels,nrange,mip,40,'linecolor','none')
set(gca,'clim',[-100 0])
colorbar
xlabel('Noise level'), ylabel('N (data length)')
title('Percent decrease in MI due to noise')

%% Figure 29.10

electrodes4mi = {'fz';'o1'};
timewindow = 400; % in ms
times2save = -400:100:1200;

% convert ms to indices
timewindowidx = round(timewindow/(1000/EEG.srate)/2);
times2saveidx = zeros(size(times2save));
for i=1:length(times2save)
    [junk,times2saveidx(i)]=min(abs(EEG.times-times2save(i)));
end

electrodesidx(1) = find(strcmpi(electrodes4mi{1},{EEG.chanlocs.labels}));
electrodesidx(2) = find(strcmpi(electrodes4mi{2},{EEG.chanlocs.labels}));

% initialize outputs
entropy = zeros(3,length(times2save));
mi      = zeros(2,length(times2save));
nbins   = zeros(1,length(times2save));

for timei = 1:length(times2save)
    datax = EEG.data(electrodesidx(1),times2saveidx(timei)-timewindowidx:times2saveidx(timei)+timewindowidx,:);
    datay = EEG.data(electrodesidx(2),times2saveidx(timei)-timewindowidx:times2saveidx(timei)+timewindowidx,:);

    [mi(1,timei),entropy(:,timei),nbins(timei)] = mutualinformationx(datax,datay);
    [mi(2,timei),entropy(:,timei)             ] = mutualinformationx(datax,datay,70);
end

figure
set(gcf,'name',[ 'Mutual information between ' electrodes4mi{1} ' and ' electrodes4mi{2} ])

subplot(221)
plot(times2save,mi(1,:))
xlabel('Time (ms)'), ylabel('MI (bits)')
title('Variable bin length')
set(gca,'xlim',[times2save(1)-50 times2save(end)+50],'ylim',[min(mi(:))-.01 max(mi(:))+.01])

subplot(222)
plot(nbins,mi(1,:),'.')
xlabel('bin length'), ylabel('MI (bits)')
title('Bin length vs. MI')
set(gca,'ylim',[min(mi(:))-.01 max(mi(:))+.01])

subplot(223)
plot(times2save,mi(2,:))
xlabel('Time (ms)'), ylabel('MI (bits)')
title('Constant bin length')
set(gca,'xlim',[times2save(1)-50 times2save(end)+50],'ylim',[min(mi(:))-.01 max(mi(:))+.01])

%% Figure 29.11

% (Figure 10 must be generated before running this cell.)
frex = logspace(log10(4),log10(40),20);
baselinetime = [-500 -200];

% baseline from ms to idx
[junk,baseidx(1)] = min(abs(times2save-baselinetime(1)));
[junk,baseidx(2)] = min(abs(times2save-baselinetime(2)));

% specify convolution and wavelet info
time = -1:1/EEG.srate:1;
half_of_wavelet_size = (length(time)-1)/2;
n_wavelet     = length(time);
n_data        = EEG.pnts*EEG.trials;
n_convolution = n_wavelet+n_data-1;
% FFT of data
fft_EEG1 = fft(reshape(EEG.data(electrodesidx(1),:,:),1,EEG.pnts*EEG.trials),n_convolution);
fft_EEG2 = fft(reshape(EEG.data(electrodesidx(2),:,:),1,EEG.pnts*EEG.trials),n_convolution);

% initialize outputs
mi   = zeros(2,length(frex),length(times2save));
ispc = zeros(length(frex),length(times2save));
powc = zeros(length(frex),length(times2save));

for fi=1:length(frex)
    % create wavelet and get its FFT
    fft_wavelet = fft(exp(2*1i*pi*frex(fi).*time) .* exp(-time.^2./(2*(4/(2*pi*frex(fi)))^2)),n_convolution);

    % convolution of each electrode with wavelet
    convres     = ifft(fft_wavelet.*fft_EEG1,n_convolution);
    analytic1   = reshape(convres(half_of_wavelet_size+1:end-half_of_wavelet_size),EEG.pnts,EEG.trials);
    convres     = ifft(fft_wavelet.*fft_EEG2,n_convolution);
    analytic2   = reshape(convres(half_of_wavelet_size+1:end-half_of_wavelet_size),EEG.pnts,EEG.trials);

    for timei = 1:length(times2save)
        datax = analytic1(times2saveidx(timei)-timewindowidx:times2saveidx(timei)+timewindowidx,:);
        datay = analytic2(times2saveidx(timei)-timewindowidx:times2saveidx(timei)+timewindowidx,:);

        % compute MI
        mi(1,fi,timei) = mutualinformationx(log10(abs(datax).^2),log10(abs(datay).^2),50);
        mi(2,fi,timei) = mutualinformationx(angle(datax),angle(datay),20);

        % also compute ISPC-time for comparison
        ispc(fi,timei) = mean(abs(mean(exp(1i*(angle(datay)-angle(datax))),2)),1);

        % also compute power correlations-time
        dataxr = tiedrank(abs(datax));
        datayr = tiedrank(abs(datay));
        n = timewindowidx*2+1;
        powc(fi,timei) = mean( 1-6*sum((dataxr-datayr).^2)/(n*(n^2-1)) );
    end

    disp([ 'Finished frequency ' num2str(fi) ' out of ' num2str(length(frex)) ]);
end

figure, set(gcf,'name',[ 'Mutual information between ' electrodes4mi{1} ' and ' electrodes4mi{2} ])

for i=1:2
    % plot baseline-subtracted corrected MI
    subplot(2,2,i)
    contourf(times2save,frex,squeeze(mi(i,:,:))-repmat(mean(mi(i,:,baseidx(1):baseidx(2)),3)',1,length(times2save)),40,'linecolor','none')
    set(gca,'clim',[-.075 .075],'yscale','log','ytick',round(logspace(log10(frex(1)),log10(frex(end)),6)))
end

% plot power correlations
subplot(223)
contourf(times2save,frex,powc-repmat(mean(powc(:,baseidx(1):baseidx(2)),2),1,length(times2save)),40,'linecolor','none')
set(gca,'clim',[-.2 .2],'yscale','log','ytick',round(logspace(log10(frex(1)),log10(frex(end)),6)))

% plot phase synchronization
subplot(224)
contourf(times2save,frex,ispc-repmat(mean(ispc(:,baseidx(1):baseidx(2)),2),1,length(times2save)),40,'linecolor','none')
set(gca,'clim',[-.1 .1],'yscale','log','ytick',round(logspace(log10(frex(1)),log10(frex(end)),6)))

%% Figure 29.12

time    = 0:.0001:1;
signal1 = sin(2*pi*10*time);
signal2 = -signal1;

lagz=1:10:1500;
milags=zeros(size(lagz));

for li=1:length(lagz)
    milags(li) = mutualinformationx(signal1,[signal2(lagz(li):end) signal2(1:lagz(li)-1)],15);
end

figure
plot(lagz/(1/mean(diff(time))),milags)
xlabel('Lag (seconds)'), ylabel('mutual information (bits)')

% now on real data (6 Hz power MI from figure 11)

time = -1:1/EEG.srate:1;
fft_wavelet = fft(exp(2*1i*pi*frex(5).*time) .* exp(-time.^2./(2*(4/(2*pi*frex(5)))^2)),n_convolution);

% convolution of each electrode with wavelet
convres = ifft(fft_wavelet.*fft_EEG1,n_convolution);
pow1    = log10(abs(reshape(convres(half_of_wavelet_size+1:end-half_of_wavelet_size),EEG.pnts,EEG.trials)).^2);
convres = ifft(fft_wavelet.*fft_EEG2,n_convolution);
pow2    = log10(abs(reshape(convres(half_of_wavelet_size+1:end-half_of_wavelet_size),EEG.pnts,EEG.trials)).^2);

onecycle = round(1000/frex(5));
onecycleidx = round(onecycle/(1000/EEG.srate));

lagz=-onecycleidx:onecycleidx;
milags=zeros(size(lagz));

for li=1:length(lagz)

    if lagz(li)<0
        milags(li) = mutualinformationx(pow1(1:end+lagz(li),:),pow2(-lagz(li)+1:end,:),30); % reverse sign for negative lags
    elseif lagz(li)==0
        milags(li) = mutualinformationx(pow1,pow2,30); % special case for no lag
    elseif lagz(li)>0
        milags(li) = mutualinformationx(pow1(lagz(li)+1:end,:),pow2(1:end-lagz(li),:),30);
    end
end

figure
plot(1000*lagz/(1/mean(diff(time))),milags)
xlabel([ electrodes4mi{1} ' leads ' electrodes4mi{2} ' ... Lag (ms) ... ' electrodes4mi{2} ' leads ' electrodes4mi{1} ]), ylabel('mutual information (bits)')
set(gca,'xlim',[-onecycle onecycle])

%% end.

author: mikexcohen

repo: AnalyzingNeuralTimeSeries


text:
function value = filon_tab_cos ( n, ftab, a, b, t )

%*****************************************************************************80
%
%% FILON_TAB_COS uses Filon's method on integrals with a cosine factor.
%
%  Discussion:
%
%    The integral to be approximated has the form:
%
%      Integral ( A <= X <= B ) F(X) * COS(T*X) dX
%
%    where T is user specified.
%
%    The function is interpolated over each subinterval by
%    a parabolic arc.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 May 2014
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz, Irene Stegun,
%    Handbook of Mathematical Functions,
%    National Bureau of Standards, 1964,
%    ISBN: 0-486-61272-4,
%    LC: QA47.A34.
%
%    Stephen Chase, Lloyd Fosdick,
%    An Algorithm for Filon Quadrature,
%    Communications of the Association for Computing Machinery,
%    Volume 12, Number 8, August 1969, pages 453-457.
%
%    Stephen Chase, Lloyd Fosdick,
%    Algorithm 353:
%    Filon Quadrature,
%    Communications of the Association for Computing Machinery,
%    Volume 12, Number 8, August 1969, pages 457-458.
%
%    Philip Davis, Philip Rabinowitz,
%    Methods of Numerical Integration,
%    Second Edition,
%    Dover, 2007,
%    ISBN: 0486453391,
%    LC: QA299.3.D28.
%
%  Parameters:
%
%    Input, integer N, the number of data points.
%    N must be odd, and greater than 1.
%
%    Input, real FTAB(N), contains the value of the function
%    at A, A+H, A+2*H, ... , B-H, B, where H = (B-A)/(N-1).
%
%    Input, real A, B, the limits of integration.
%
%    Input, real T, the multiplier of the X argument of the cosine.
%
%    Output, real VALUE, the approximate value of the integral.
%
  if ( a == b )
    value = 0.0;
    return
  end

  if ( n <= 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'FILON_TAB_COS - Fatal error!\n' );
    fprintf ( 1, '  N < 2\n' );
    fprintf ( 1, '  N = %d\n', n );
    error ( 'FILON_TAB_COS - Fatal error!' );
  end

  if ( mod ( n, 2 ) ~= 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'FILON_TAB_COS - Fatal error!\n' );
    fprintf ( 1, '  N must be odd.\n' );
    fprintf ( 1, '  N = %d\n', n );
    error ( 'FILON_TAB_COS - Fatal error!' );
  end
%
%  Set the X values.
%
  x = linspace ( a, b, n );
  h = ( b - a ) / ( n - 1 );

  theta = t * h;
  sint = sin ( theta );
  cost = cos ( theta );

  if ( 6.0 * abs ( theta ) <= 1.0 )

    alpha = 2.0 * theta^3 /   45.0 ...
          - 2.0 * theta^5 /  315.0 ...
          + 2.0 * theta^7 / 4725.0;

    beta =  2.0           /     3.0 ...
          + 2.0 * theta^2 /    15.0 ...
          - 4.0 * theta^4 /   105.0 ...
          + 2.0 * theta^6 /   567.0 ...
          - 4.0 * theta^8 / 22275.0;

    gamma = 4.0           /      3.0 ...
          - 2.0 * theta^2 /     15.0 ...
          +       theta^4 /    210.0 ...
          -       theta^6 /  11340.0;

  else

    alpha = ( theta^2 + theta * sint * cost - 2.0 * sint^2 ) / theta^3;

    beta = ( 2.0 * theta + 2.0 * theta * cost^2 ...
      - 4.0 * sint * cost ) / theta^3;

    gamma = 4.0 * ( sint - theta * cost ) / theta^3;

  end

  c2n = sum ( ftab(1:2:n) .* cos ( t * x(1:2:n) ) ) ...
    - 0.5 * ( ftab(n) * cos ( t * x(n) ) ...
            + ftab(1) * cos ( t * x(1) ) );

  c2nm1 = sum ( ftab(2:2:n-1) .* cos ( t * x(2:2:n-1) ) );

  value = h * ( ...
      alpha * ( ftab(n) * sin ( t * x(n) ) ...
              - ftab(1) * sin ( t * x(1) ) ) ...
    + beta * c2n ...
    + gamma * c2nm1 );

  return
end

author: johannesgerer

repo: jburkardt-m


text:
% estimateWorldCameraPose Estimate camera pose from 3-D to 2-D point correspondences
%   [worldOrientation, worldLocation] = estimateWorldCameraPose(imagePoints, worldPoints, cameraParams)
%   returns the orientation and location of a calibrated camera in the world coordinate
%   system in which worldPoints are defined.
%
%   The function solves the Perspective-n-Point (PnP) problem using the P3P algorithm.
%   The function eliminates spurious correspondences using the M-estimator SAmple Consensus
%   (MSAC) algorithm
%
%   Inputs            Description
%   ------            -----------
%   imagePoints       M-by-2 array of [x,y] coordinates of undistorted image points,
%                     with M >= 4.
%
%   worldPoints       M-by-3 array of [x,y,z] coordinates of world points.
%
%   cameraParams      a cameraParameters or cameraIntrinsics object
%
%   Outputs           Description
%   -------            -----------
%   worldOrientation  orientation of the camera in the world coordinates specified
%                     as a 3-by-3 matrix
%   worldLocation     location of the camera in the world coordinates specified as a
%                     1-by-3 vector
%
%   [..., inlierIdx] = estimateWorldCameraPose(...) additionally returns the indices of the
%   inliers used to compute the camera pose. inlierIdx is an M-by-1 logical vector,
%   with the values of true corresponding to the inliers.
%
%   [..., status] = estimateWorldCameraPose(...) additionally returns a status code. If the
%   status output is not specified, the function will issue an error if the number of
%   input points or the number of inliers is less than 4. The status can have the following
%   values:
%
%       0: No error.
%       1: imagePoints and worldPoints do not contain enough points.
%          At least 4 points are required.
%       2: Not enough inliers found. At least 4 inliers are required.
%
%   [...] = estimateWorldCameraPose(..., Name, Value) specifies additional name-value pair
%   arguments described below:
%
%   'MaxNumTrials'          Positive integer scalar.
%                           Specifies the number of random trials for finding
%                           the outliers. The actual number of trials depends
%                           on imagePoints, worldPoints, and the values of the
%                           MaxReprojectionError and Confidence parameters. Increasing
%                           this value will improve the robustness of the output
%                           at the expense of additional computation.
%
%                           Default: 1000
%
%   'Confidence'            Scalar value greater than 0 and less than 100.
%                           Specifies the desired confidence (in percentage)
%                           for finding the maximum number of inliers. Increasing
%                           this value will improve the robustness of the output
%                           at the expense of additional computation.
%
%                           Default: 99
%
%   'MaxReprojectionError'  Positive numeric scalar specifying the reprojection
%                           error threshold in pixels for finding outliers. Increasing
%                           this value will make the algorithm converge faster,
%                           but may reduce the accuracy of the result.
%
%                           Default: 1
%
%    Notes
%    -----
%    The function does not account for lens distortion. You can either
%    undistort the images using the undistortImage function before
%    detecting the image points, or you can undistort the image points
%    themselves using the undistortPoints function.
%
%    Class Support
%    -------------
%    imagePoints and worldPoints must be of the same class, which can be
%    double or single. cameraParams must be a cameraParameters or
%    cameraIntrinsics object. orientation and location are of class double.
%
%    Example: Determine camera pose from world-to-image correspondences
%    ------------------------------------------------------------------
%    data = load('worldToImageCorrespondences.mat');
%    [worldOrientation, worldLocation] = estimateWorldCameraPose(...
%        data.imagePoints, data.worldPoints, data.cameraParams);
%    pcshow(data.worldPoints, 'VerticalAxis', 'Y', 'VerticalAxisDir', 'down', ...
%        'MarkerSize', 30);
%    hold on
%    plotCamera('Size', 10, 'Orientation', worldOrientation, 'Location',...
%        worldLocation);
%
%    See also relativeCameraPose, viewSet, triangulateMultiview, bundleAdjustment,
%             plotCamera, pcshow, extrinsics, triangulate,
%             cameraPoseToExtrinsics

%  Copyright 2015 The MathWorks, Inc.

% References:
% ----------
% [1] X.-S. Gao, X.-R. Hou, J. Tang, and H.-F. Cheng, "Complete Solution
%     Classification for the Perspective-Three-Point Problem", IEEE Trans.
%     Pattern Analysis and Machine Intelligence, vol. 25, no. 8, pp. 930-943,
%     2003.

%#codegen

function [orientation, location, inlierIdx, status, err] = ...
    estimateWorldCameraPoseNew(imagePoints, worldPoints, cameraParams, varargin)

[params, outputClass, imagePts, worldPts] = parseInputs(imagePoints, worldPoints, cameraParams, ...
    varargin{:});

if isa(cameraParams, 'cameraIntrinsics')
    cameraParams = cameraParams.CameraParameters;
end

% List of status codes
statusCode = struct(...
    'NoError',           int32(0),...
    'NotEnoughPts',      int32(1),...
    'NotEnoughInliers',  int32(2));

% Additional RANSAC parameters
params.sampleSize = 4;
params.recomputeModelFromInliers = false;
params.defaultModel.R = nan(3);
params.defaultModel.t = nan(1, 3);

% RANSAC function handles
funcs.fitFunc = @solveCameraPose;
funcs.evalFunc = @evalCameraPose;
funcs.checkFunc = @check;

numPoints = size(worldPts, 1);
if numPoints < params.sampleSize
   status = statusCode.NotEnoughPts;
   [orientation, location, inlierIdx] = badPose(numPoints);
else
    % Compute the pose using RANSAC
    points = pack(imagePts, worldPts);
    [isFound, pose, inlierIdx] = vision.internal.ransac.msac(...
        points, params, funcs, cameraParams.IntrinsicMatrix, outputClass);

%     isFound = true;
%     pose = solveCameraPose_world_image(worldPts, imagePts
    if isFound
        % Convert from extrinsics to orientation and location
        orientation =  pose.R';
        location    = -pose.t * pose.R';
        err = pose.err / numPoints;
        status = statusCode.NoError;
    else
        % Could not compute the pose
        status = statusCode.NotEnoughInliers;
        [orientation, location, inlierIdx] = badPose(numPoints);
        err = nan;
    end
end

if nargout < 4
    checkRuntimeStatus(statusCode, status);
end

%==========================================================================
% Check runtime status and report error if there is one
%==========================================================================
function checkRuntimeStatus(statusCode, status)
coder.internal.errorIf(status==statusCode.NotEnoughPts, ...
    'vision:points:notEnoughMatchedPts', 'imagePoints', 'worldPoints', 4);

coder.internal.errorIf(status==statusCode.NotEnoughInliers, ...
    'vision:points:notEnoughInlierMatches', 'imagePoints', ...
    'worldPoints');

%--------------------------------------------------------------------------
function [orientation, location, inlierIdx] = badPose(numPoints)
orientation = nan(3);
location = nan(1, 3);
inlierIdx = false(numPoints, 1);

function pose = solveCameraPose(points, varargin)
[worldPoints, imagePoints] = unpack(points);

intrinsicMatrix = varargin{1};
% N = size(worldPoints, 1);
% [R,T,Xc,best_solution] = efficient_pnp_gauss([worldPoints, ones(N, 1)], [imagePoints, ones(N, 1)], intrinsicMatrix', true);

% Get up to 4 solutions for the pose using 3 points
[Rs, Ts] = vision.internal.calibration.solveP3P(...
    imagePoints, worldPoints, intrinsicMatrix);

% Choose the best solution using the 4th point
p = [worldPoints(4, :), 1]; % homogeneous coordinates
q = imagePoints(4, :);
% pose_pnp = chooseBestSolution(p, q, R', T', intrinsicMatrix);

pose.R = nan(3);
pose.t = nan(1, 3);
if ~isempty(Rs)
    pose = chooseBestSolution(p, q, Rs, Ts, intrinsicMatrix);
end

function pose = solveCameraPose_world_image(worldPoints, imagePoints, K)

intrinsicMatrix = K;
% N = size(worldPoints, 1);
% [R,T,Xc,best_solution] = efficient_pnp_gauss([worldPoints, ones(N, 1)], [imagePoints, ones(N, 1)], intrinsicMatrix', true);

% Get up to 4 solutions for the pose using 3 points
[Rs, Ts] = vision.internal.calibration.solveP3P(...
    imagePoints, worldPoints, intrinsicMatrix);

% Choose the best solution using the 4th point
p = [worldPoints(4, :), 1]; % homogeneous coordinates
q = imagePoints(4, :);
% pose_pnp = chooseBestSolution(p, q, R', T', intrinsicMatrix);

pose.R = nan(3);
pose.t = nan(1, 3);
if ~isempty(Rs)
    pose = chooseBestSolution(p, q, Rs, Ts, intrinsicMatrix);
end
pose.err = 0;
% pose = pose_pnp;

%--------------------------------------------------------------------------
function pose = solveCameraPose_(points, varargin)
[worldPoints, imagePoints] = unpack(points);

intrinsicMatrix = varargin{1};
NN = size(imagePoints, 1);
bbb = [imagePoints, ones(NN, 1)] / (intrinsicMatrix);
rrr = worldPoints(:, 1 : 3);
% for i = 1 : NN
%     bbb(i, :) = bbb(i, :) ./ norm(bbb(i, :));
%     rrr(i, :) = rrr(i, :) ./ norm(rrr(i, :));
% end
% [RR, tt, ss, xs] = apnp_algebraic(bbb, rrr, 0, 0, intrinsicMatrix', eye(4));

% Get up to 4 solutions for the pose using 3 points
[Rs, Ts] = vision.internal.calibration.solveP3P(...
    imagePoints, worldPoints, intrinsicMatrix);

% Choose the best solution using the 4th point
p = [worldPoints(4, :), 1]; % homogeneous coordinates
q = imagePoints(4, :);

pose.R = nan(3);
pose.t = nan(1, 3);
pose.err = 1;
if ~isempty(Rs)
    pose = chooseBestSolution(p, q, Rs, Ts, intrinsicMatrix);
end

function [R_, t_, s_, xs, min_val] = apnp_algebraic(bbb, rrr, Xw, U, K, Rt)
    N = size(bbb, 1);
    b = zeros(3, N);
    r = zeros(3, N);
    RR = Rt(1 : 3, 1 : 3);
    tt = Rt(1 : 3, 4);
    for i = 1 : N
        b(:, i) = [bbb(i, 1 : 3)]';
        r(:, i) = rrr(i, 1 : 3)';
%
%         b(:, i) - RR * r(:, i) - tt
    end
    b_bar = zeros(3, 1);
    r_bar = zeros(3, 1);
    for i = 1 : N
        b_bar = b_bar + 1 / N * b(:, i);
        r_bar = r_bar + 1 / N * r(:, i);
    end

    P = zeros(4, 4);
    for i = 1 : N
        for j = 1 : 3
            str = sprintf('M = M%d_matrix(b(:, i) - b_bar);', j);
            eval(str);
            P = P + 1 / N * r(j, i) * M;
        end
    end
    [V, ~] = eig(P);

    q11 = V(:, 1); q_1 = q11 ./ norm(q11);
    q22 = V(:, 2); q_2 = q22 ./ norm(q22);
    q33 = V(:, 3); q_3 = q33 ./ norm(q33);
    q44 = V(:, 4); q_4 = q44 ./ norm(q44);
    x_count = 4 + 1;
    xs_ = [
        q_1';
        q_2';
        q_3';
        q_4';
        ];
    xs = zeros(4, 9);

    for i = 1 : x_count - 1
        q0 = xs_(i, 1);
        q1 = xs_(i, 2);
        q2 = xs_(i, 3);
        q3 = xs_(i, 4);
        R = quat2dcm([q0, q1, q2, q3]);
        aa = 0;
        bb = 0;
        for j = 1 : N
            aa = aa + 1 / N * (b(:, j)' * (b(:, j) - b_bar));
            bb = bb + 1 / N * (b(:, j)' * R * (r(:, j) - r_bar));
        end
        ss = bb / aa;
        s = ss;
        t = s * b_bar - R * r_bar;
        xs(i, :) = [q0, q1, q2, q3, t', s, ss];
    end

    Ls = zeros(x_count - 1, 1);
    for i = 1 : x_count - 1
        RR = quat2dcm(xs(i, 1 : 4));
        tt = xs(i, 5 : 7)';
        Ls(i) = J_func(RR, tt, bbb, rrr, K);
    end
    [minimum, idx] = sort(Ls);
    R_ = quat2dcm(xs(idx(1), 1 : 4));
    t_ = xs(idx(1), 5 : 7)';
    s_ = xs(idx(1), 9);
    min_val = minimum;

function J = J_func(R, t, b, r, K)
J = 0;
len = size(b, 2);
for j = 1 : len
    xb = b(j, :);
    bb = xb';
    xr = r(j, :)';
    xr = (R * xr + t);
    res = bb - xr;
    J = J + 1 / len * trace(res' * res);
end

%--------------------------------------------------------------------------
% Find the solution that results in smallest squared reprojection error for
% the 4th point.
% worldPoint must be in homogeneous coordinates (1-by-4)
function pose = chooseBestSolution(worldPoint, imagePoint, Rs, Ts, intrinsicMatrix)

pose.R = zeros(3);
pose.t = zeros(1, 3);

numSolutions = size(Ts, 1);
errors = zeros(numSolutions, 1, 'like', worldPoint);

for i = 1:numSolutions
    cameraMatrix = [Rs(:,:,i); Ts(i,:)] * intrinsicMatrix;
    projectedPoint = worldPoint * cameraMatrix;
    projectedPoint = projectedPoint(1:2) ./ projectedPoint(3);
    d = imagePoint - projectedPoint;
    errors(i) = d * d';
end

[~, idx] = min(errors);
idx = idx(1); % in case we have two identical errors
pose.t = Ts(idx, :);
pose.R = Rs(:,:,idx);
pose.err = errors(idx);

%--------------------------------------------------------------------------
% Compute reprojection errors
function dis = evalCameraPose(pose, points, varargin)
[worldPoints, imagePoints] = unpack(points);

intrinsicMatrix = varargin{1};
cameraMatrix = [pose.R; pose.t] * intrinsicMatrix;

% Project world points into the image
numPoints = size(worldPoints, 1);
worldPointsHomog = [worldPoints, ones(numPoints, 1, 'like', worldPoints)];
projectedPointsHomog = worldPointsHomog * cameraMatrix;
projectedPoints  = bsxfun(@rdivide, projectedPointsHomog(:, 1:2), ...
    projectedPointsHomog(:, 3));

% Compute reprojection errors
diffs = imagePoints - projectedPoints;
dis = sum(diffs.^2, 2);

%--------------------------------------------------------------------------
% Pack points into a single entity for RANSAC
function points = pack(imagePoints, worldPoints)
points = [imagePoints, worldPoints];

%--------------------------------------------------------------------------
% Unpack the points
function [worldPoints, imagePoints] = unpack(points)
imagePoints = points(:, 1:2);
worldPoints = points(:, 3:end);

%--------------------------------------------------------------------------
function r = check(pose, varargin)
r = ~isempty(pose) && ~isempty(pose.R) && ~isempty(pose.t);

%--------------------------------------------------------------------------
function [ransacParams, outputClass, imagePts, worldPts] = ...
    parseInputs(imagePoints, worldPoints, cameraParams, varargin)

validatePoints(imagePoints, worldPoints);
imagePts = double(imagePoints);
worldPts = double(worldPoints);
outputClass = class(imagePts);
validateattributes(cameraParams, {'cameraParameters','cameraIntrinsics'}, ...
    {'scalar'}, mfilename, 'cameraParams');

defaults = struct('MaxNumTrials',1000, 'Confidence',99, 'MaxDistance', 1);

if isempty(coder.target)
    ransacParams = parseRANSACParamsMatlab(defaults, varargin{:});
else
    ransacParams = parseRANSACParamsCodegen(defaults, varargin{:});
end

%--------------------------------------------------------------------------
function ransacParams = parseRANSACParamsMatlab(defaults, varargin)
parser = inputParser;
parser.FunctionName = mfilename;
parser.addParameter('MaxNumTrials', defaults.MaxNumTrials, @checkMaxNumTrials);
parser.addParameter('Confidence', defaults.Confidence, @checkConfidence);
parser.addParameter('MaxReprojectionError', defaults.MaxDistance, @checkMaxDistance);

parser.parse(varargin{:});
ransacParams.confidence = parser.Results.Confidence;
ransacParams.maxDistance = parser.Results.MaxReprojectionError^2;
ransacParams.maxNumTrials = parser.Results.MaxNumTrials;
ransacParams.verbose = false;

%--------------------------------------------------------------------------
function ransacParams = parseRANSACParamsCodegen(defaults, varargin)
% Instantiate an input parser
parms = struct( ...
    'MaxNumTrials',       uint32(0), ...
    'Confidence',         uint32(0), ...
    'MaxReprojectionError',        uint32(0));

popt = struct( ...
    'CaseSensitivity', false, ...
    'StructExpand',    true, ...
    'PartialMatching', false);

% Specify the optional parameters
optarg = eml_parse_parameter_inputs(parms, popt, varargin{:});
ransacParams.maxNumTrials = eml_get_parameter_value(optarg.MaxNumTrials,...
    defaults.MaxNumTrials, varargin{:});
ransacParams.confidence   = eml_get_parameter_value(optarg.Confidence,...
    defaults.Confidence, varargin{:});
ransacParams.maxDistance  = eml_get_parameter_value(...
    optarg.MaxReprojectionError, defaults.MaxDistance, varargin{:});
ransacParams.verbose  = false;

checkMaxNumTrials(ransacParams.maxNumTrials);
checkConfidence  (ransacParams.confidence);
checkMaxDistance (ransacParams.maxDistance);

ransacParams.maxDistance = ransacParams.maxDistance^2;

%--------------------------------------------------------------------------
function validatePoints(imagePoints, worldPoints)
validateattributes(imagePoints, {'double', 'single'}, ...
    {'real', 'nonsparse', 'nonempty', '2d', 'ncols', 2}, ...
    mfilename, 'imagePoints');

validateattributes(worldPoints, {'double', 'single'}, ...
    {'real', 'nonsparse', 'nonempty', '2d', 'ncols', 3}, ...
    mfilename, 'worldPoints');

coder.internal.errorIf(~isa(imagePoints, class(worldPoints)), ...
    'vision:points:ptsClassMismatch', 'imagePoints', 'worldPoints');
coder.internal.errorIf(size(imagePoints, 1) ~= size(worldPoints, 1), ...
    'vision:points:numPtsMismatch', 'imagePoints', 'worldPoints');

%--------------------------------------------------------------------------
function tf = checkMaxNumTrials(value)
validateattributes(value, {'numeric'}, ...
    {'scalar', 'nonsparse', 'real', 'integer', 'positive'}, mfilename, ...
    'MaxNumTrials');
tf = true;

%--------------------------------------------------------------------------
function tf = checkConfidence(value)
validateattributes(value, {'numeric'}, ...
    {'scalar', 'nonsparse', 'real', 'positive', '<', 100}, mfilename, ...
    'Confidence');
tf = true;

%--------------------------------------------------------------------------
function tf = checkMaxDistance(value)
validateattributes(value,{'single','double'}, ...
    {'real', 'nonsparse', 'scalar','nonnegative','finite'}, mfilename, ...
    'MaxDistance');
tf = true;

author: zarathustr

repo: LibQPEP

save_path: github-repos/MATLAB/zarathustr-LibQPEP


text:
function [d_out] = qpsk_srrc(d_in)

    persistent buf

    OS_RATE = 8;
    f = SRRC;

    if isempty(buf)
        buf = complex(zeros(1,OS_RATE*2+1),zeros(1,OS_RATE*2+1));
    end

    buf = [buf(2:end) d_in];

    d_out = buf*f;

end

author: Sable

repo: mcbench-benchmarks


text: function x=uplus(x)

author: opencobra

repo: cobratoolbox


text:
% here's part from class
% load data ...
% do wavelets ..
% take svd ...
data = [5+ 2*randn(32*32, 80),  .1*randn(32*32, 80)];
[u,s,v] = svd(data,0);

% *** so replace above with the real cat and dog data  ***

% set up
xdog=v(1:80,2:4);
xcat=v(81:160,2:4);
xtrain=[xdog(1:50,:);xcat(1:50,:)];
xtest=[xdog(51:80,:);xcat(51:80,:)];
Ctrain=[ones(50,1); 2*ones(50,1)];
Ctest=[ones(30,1); 2*ones(30,1)];

% k-NN
knn = fitcknn(xtrain, Ctrain);
predKNN = predict(knn,xtest);

cMatKNN = confusionmat(Ctest, predKNN)
errorKNN = sum(abs(predKNN-Ctest))/60*100

% LDA
predLDA = classify(xtest, xtrain, Ctrain);

cMatLDA = confusionmat(Ctest, predLDA)
errorLDA = sum(abs(predLDA-Ctest))/60*100

% Naive Bayes
nb=fitNaiveBayes(xtrain,Ctrain);
predNB=nb.predict(xtest);

cMatNB=confusionmat(Ctest, predNB)
errorNB=sum(abs(predNB-Ctest))/60*100

% SVM
svm = svmtrain(xtrain,Ctrain);
predSVM = svmclassify(svm,xtest);

cMatSVM=confusionmat(Ctest,predSVM)
errorSVM=sum(abs(predSVM-Ctest))/60*100

% AdaBoost
% pick AdaBoost, 100 classifiers, weak learners are Discriminants
ab = fitensemble(xtrain,Ctrain,'AdaBoostM1',100,'Discriminant');
predAB = predict(ab,xtest);

cMatAB = confusionmat(Ctest,predAB)
errorAB = sum(abs(predAB-Ctest))/60*100

% EM / Gaussian mixture models
gm = fitgmdist(xtrain, 2); % choose number of clusters
predKM = cluster(gm,xtest); % can use clustering to classify new data

cMatKM = confusionmat(Ctest,predKM)
errorKM = sum(abs(predKM-Ctest))/60*100

% some cool ways to view these clustering results are here:
% http://www.mathworks.com/help/stats/gaussian-mixture-models.html#brajyl2

% k-means
km = kmeans(xtrain, 2); % choose number of clusters
% I don't think they have a built-in way to evaluate this on new data?

author: dynamicslab


text:
% Representation-Free Model Predictive Control for Dynamic Quadruped Panther
% Author: Yanran Ding
% Last modified: 2020/12/21
%
% Code accompanying the paper:
% Yanran Ding, Abhishek Pandala, Chuanzheng Li, Young-Ha Shin, Hae-Won Park
% "Representation-Free Model Predictive Control for Dynamic Motions in Quadrupeds"
% Transactions on Robotics
%
% preprint available at: https://arxiv.org/abs/2012.10002
% video available at: https://www.youtube.com/watch?v=iMacEwQisoQ&t=101s

%% initialization
clear all;close all;clc
addpath fcns fcns_MPC

%% --- parameters ---
% ---- gait ----
% 0-trot; 1-bound; 2-pacing 3-gallop; 4-trot run; 5-crawl
gait = 1;
p = get_params(gait);
p.playSpeed = 1;
p.flag_movie = 1;       % 1 - make movie
use_qpSWIFT = 1;        % 0 - quadprog, 1 - qpSWIFT (external)

dt_sim = p.simTimeStep;
SimTimeDuration = 0.5;  % [sec]
MAX_ITER = floor(SimTimeDuration/p.simTimeStep);

% desired trajectory
p.acc_d = 1;
p.vel_d = [0.5;0];
p.yaw_d = 0;

%% Model Predictive Control
% --- initial condition ---
% Xt = [pc dpc vR wb pf]': [30,1]
if gait == 1
    [p,Xt,Ut] = fcn_bound_ref_traj(p);
else
    [Xt,Ut] = fcn_gen_XdUd(0,[],[1;1;1;1],p);
end

% --- logging ---
tstart = 0;
tend = dt_sim;

[tout,Xout,Uout,Xdout,Udout,Uext,FSMout] = deal([]);

% --- simulation ----
h_waitbar = waitbar(0,'Calculating...');
tic
for ii = 1:MAX_ITER
    % --- time vector ---
    t_ = dt_sim * (ii-1) + p.Tmpc * (0:p.predHorizon-1);

    % --- FSM ---
    if gait == 1
        [FSM,Xd,Ud,Xt] = fcn_FSM_bound(t_,Xt,p);
    else
        [FSM,Xd,Ud,Xt] = fcn_FSM(t_,Xt,p);
    end

    % --- MPC ----
    % form QP
    [H,g,Aineq,bineq,Aeq,beq] = fcn_get_QP_form_eta(Xt,Ut,Xd,Ud,p);

    if ~use_qpSWIFT
        % solve QP using quadprog
        [zval] = quadprog(H,g,Aineq,bineq,Aeq,beq,[],[]);
    else
        % interface with the QP solver qpSWIFT
        [zval,basic_info] = qpSWIFT(sparse(H),g,sparse(Aeq),beq,sparse(Aineq),bineq);
    end

    Ut = Ut + zval(1:12);

    % --- external disturbance ---
    [u_ext,p_ext] = fcn_get_disturbance(tstart,p);
    p.p_ext = p_ext;        % position of external force
    u_ext = 0*u_ext;

    % --- simulate ---
    [t,X] = ode45(@(t,X)dynamics_SRB(t,X,Ut,Xd,0*u_ext,p),[tstart,tend],Xt);

    % --- update ---
    Xt = X(end,:)';
    tstart = tend;
    tend = tstart + dt_sim;

    % --- log ---
    lent = length(t(2:end));
    tout = [tout;t(2:end)];
    Xout = [Xout;X(2:end,:)];
    Uout = [Uout;repmat(Ut',[lent,1])];
    Xdout = [Xdout;repmat(Xd(:,1)',[lent,1])];
    Udout = [Udout;repmat(Ud(:,1)',[lent,1])];
    Uext = [Uext;repmat(u_ext',[lent,1])];
    FSMout = [FSMout;repmat(FSM',[lent,1])];

    waitbar(ii/MAX_ITER,h_waitbar,'Calculating...');
end
close(h_waitbar)
fprintf('Calculation Complete!\n')
toc

%% Animation
[t,EA,EAd] = fig_animate(tout,Xout,Uout,Xdout,Udout,Uext,p);

author: YanranDing

repo: RF-MPC

save_path: github-repos/MATLAB/YanranDing-RF-MPC


text:
function varargout = norm(varargin)
%NORM (overloaded)
%
% t = NORM(x,P)
%
% The variable t can only be used in convexity preserving
% operations such as t<0, max(t,y)<1, minimize t etc.
%
%    For matrices...
%      NORM(X) models the largest singular value of X, max(svd(X)).
%      NORM(X,2) is the same as NORM(X).
%      NORM(X,1) models the 1-norm of X, the largest column sum, max(sum(abs(X))).
%      NORM(X,inf) models the infinity norm of X, the largest row sum, max(sum(abs(X'))).
%      NORM(X,'fro') models the Frobenius norm, sqrt(sum(diag(X'*X))).
%    For vectors...
%      NORM(V) = norm(V,2) = standard Euclidean norm.
%      NORM(V,inf) = max(abs(V)).
%      NORM(V,1) = sum(abs(V))
%
% SEE ALSO SUMK, SUMABSK

%% ***************************************************
% This file defines a nonlinear operator for YALMIP
%
% It can take three different inputs
% For DOUBLE inputs, it returns standard double values
% For SDPVAR inputs, it generates an internal variable
%
% When first input is 'model' it returns the graph
% in the first output and structure describing some
% properties of the operator.

%% ***************************************************
switch class(varargin{1})

    case 'double' % What is the numerical value of this argument (needed for displays etc)
        % SHOULD NEVER HAPPEN, THIS SHOULD BE CAUGHT BY BUILT-IN
        error('Overloaded SDPVAR/NORM CALLED WITH DOUBLE. Report error')

    case 'sdpvar' % Overloaded operator for SDPVAR objects. Pass on args and save them.
        if nargin == 1
            varargout{1} = yalmip('addextendedvariable',mfilename,varargin{1},2);
        else
            switch varargin{2}
                case {1,2,inf,'inf','fro'}
                    varargout{1} = yalmip('addextendedvariable',mfilename,varargin{:});
                otherwise
                    error('norm(x,P) only supported for P = 1, 2, inf and ''fro''');
            end
        end

    case 'char' % YALMIP sends 'model' when it wants the epigraph or hypograph
        switch varargin{1}
            case 'graph'
                t = varargin{2};
                X = varargin{3};
                p = varargin{4};

                % Code below complicated by two things
                % 1: Absolute value for complex data -> cone constraints on
                %    elements
                % 2: SUBSREF does not call SDPVAR subsref -> use extsubsref.m

                % FIX : Exploit symmetry to create smaller problem
                switch p
                    case 1
                        z = sdpvar(size(X,1),size(X,2),'full');
                        if min(size(X))>1
                            if isreal(X)
                                F = (-z <= X <= z);
                            else
                                F = ([]);
                                for i = 1:size(X,1)
                                    for j = 1:size(X,2)
                                        xi = extsubsref(X,i,j);
                                        zi = extsubsref(z,i,j);
                                        F = F + (cone([real(xi);imag(xi)],zi));
                                    end
                                end
                            end
                            F = F + (sum(z,1) <= t);
                        else
                            if isreal(X)
                                F = (-z <= X <= z) + (sum(z) <= t);
                                [M,m] = derivebounds(X);
                                bounds(z,0,max(abs([M -m]),[],2));
                                bounds(t,0,sum(max(abs([M -m]),[],2)));
                            else
                                F = ([]);
                                for i = 1:length(X)
                                    xi = extsubsref(X,i);
                                    zi = extsubsref(z,i);
                                    F = F + (cone([real(xi);imag(xi)],zi));
                                end
                                F = F + (sum(z) <= t);
                            end
                        end
                    case 2
                        z = sdpvar(size(X,1),size(X,2));
                        if min(size(X))>1
                            F = ([t*eye(size(X,1)) X;X' t*eye(size(X,2))])>=0;
                        else
                            F = (cone(X(:),t));
                        end
                    case {inf,'inf'}
                        if min(size(X))>1
                            z = sdpvar(size(X,1),size(X,2),'full');
                            if isreal(X)
                                F = (-z <= X <= z);
                            else
                                F = ([]);
                                for i = 1:size(X,1)
                                    for j = 1:size(X,2)
                                        xi = extsubsref(X,i,j);
                                        zi = extsubsref(z,i,j);
                                        F = F + (cone([real(xi);imag(xi)],zi));
                                    end
                                end
                            end
                            F = F + (sum(z,2) <= t);
                        else
                            if isreal(X)
                                F = (-t <= X <= t);
                                [M,m,infbound] = derivebounds(X);
                                if ~infbound
                                    F = F + (0<=t<=max(M));
                                end
                            else
                                F = ([]);
                                for i = 1:length(X)
                                    xi = extsubsref(X,i);
                                    F = F + (cone([real(xi);imag(xi)],t));
                                end
                            end
                        end
                    case 'fro'
                        X.dim(1)=X.dim(1)*X.dim(2);
                        X.dim(2)=1;
                        F = (cone(X,t));
                    otherwise
                end
                varargout{1} = F;
                varargout{2} = struct('convexity','convex','monotonicity','none','definiteness','positive');
                varargout{3} = X;
            case 'milp'

                t = varargin{2};
                X = varargin{3};
                p = varargin{4};
                if ~isreal(X) | isequal(p,2) | isequal(p,'fro') | min(size(X))>1 % Complex valued data, matrices and 2-norm not supported
                    varargout{1} = [];
                    varargout{2} = [];
                    varargout{3} = [];
                else
                    if p==1
                        X     = reshape(X,length(X),1);
                        absX  = sdpvar(length(X),1);
                        d     = binvar(length(X),1);
                        [M,m] = derivebounds(X);
                         F = ([]);
                         positive = find(m >= 0);
                         negative = find(M <= 0);

                         % d(find(positive)) = 1;
                         % d(find(negative)) = 0;
                         if ~isempty(positive)
                             d = subsasgn(d,struct('type','()','subs',{{positive}}),1);
                         end
                         if ~isempty(negative)
                             d = subsasgn(d,struct('type','()','subs',{{negative}}),0);
                         end

                         F = F + (X <= M.*d)     + (2*m.*d     <= absX+X <= 2*M.*d);
                         F = F + (X >= m.*(1-d)) + (2*m.*(1-d) <= absX-X <= 2*M.*(1-d));
                         F = F + (t - sum(absX) == 0);

                    else

                        if 0

                            %2^n cases
                            %e.g in 2d,
                            %norm([x;y],inf) = y, y>0, y>x.y>-x
                            %                = x, x>0, x>y.x>-y

                            n = length(X);
                            X     = reshape(X,n,1);
                            absX  = sdpvar(n,1);
                            d     = binvar(n,1);
                            [M,m] = derivebounds(X);

                            F = (sum(d)==0);

                            top = 1;
                            for i = 1:n
                                xi = extsubsref(X,i);
                                y = extsubsref(X,setdiff(1:n,i));
                                for sign_abs_largest_variable = -1:2:1
                                    di = extsubsref(d,top);
                                    for j = setdiff(1:n,i)
                                        y = extsubsref(X,j);
                                        for sign_other = -1:2:1
                                            F = F + (xi*sign_abs_largest_variable >= sign_other*y);
                                            F = F + (-M*100*(1-di) <= xi*sign_abs_largest_variable-t <= t+M*100*(1-di));
                                        end
                                    end
                                    top = top + 1;
                                end
                            end

                        else
                            % OLD
                            n = length(X);
                            X     = reshape(X,n,1);
                            absX  = sdpvar(n,1);
                            d     = binvar(n,1);
                            [M,m] = derivebounds(X);
                            F = ([]);
                            F = F + (X <= M.*d)     + (2*m.*d     <= absX+X <= 2*M.*d);
                            F = F + (X >= m.*(1-d)) + (2*m.*(1-d) <= absX-X <= 2*M.*(1-d));
                            M = max(M,-m);
                            d = binvar(n,1);
                            F = F + (sum(d)==1);
                            F = F + (absX <= t <= absX + M.*(1-d));

                            kk = [];
                            ii = [];
                            for i = 1:n
                                k = [1:1:i-1 i+1:1:n]';
                                ii = [ii;repmat(i,n-1,1)];
                                kk = [kk;k];
                                Mm = M(k);
                            end
                            xii = extsubsref(absX,ii);
                            dii = extsubsref(d,ii);
                            xkk = extsubsref(absX,kk);
                            F = F + (xkk <= xii+(M(kk)-m(ii)).*(1-dii));
                        end

                        %   for i = 1:n
                        %       xi = extsubsref(absX,i);
                        %       di = extsubsref(d,i);
                        %       for k = [1:1:i-1 i+1:1:n]
                        %           xk = extsubsref(absX,k);
                        %           F = F + (xk <= xi+M(k)*(1-di));
                        %       end
                        %   end

                    end
                    varargout{1} = F;
                    varargout{2} = struct('convexity','milp','monotonicity','milp','definiteness','positive');
                    varargout{3} = X;
                end
            otherwise
                error('SDPVAR/NORM called with CHAR argument?');
        end
    otherwise
        error('Strange type on first argument in SDPVAR/NORM');
end


text:
%%********************************************************************
%% blkbarrier: calculate
%% [-v(p)*logdet(X{p}),   v(p)*logdet(Z{p}) + n*v(p)*(1-log(v(p)))]
%% [-v(p)*log(gam(X{p})), v(p)*log(gam(Z{p})) + v(p)]
%% [-v(p)*log(X{p}),      v(p)*log(Z{p}) + n*v(p)*(1-log(v(p)))]
%%********************************************************************

  function objadd = blkbarrier(blk,X,Z,Xchol,Zchol,v);

   objadd = zeros(1,2); tmp = zeros(1,2);
   for p = 1:size(blk,1)
      pblk = blk(p,:);
      vp = v{p};
      idx = find(vp > 0);
      if ~isempty(idx)
         vpsub = vp(idx);
         if size(vpsub,1) < size(vpsub,2); vpsub = vpsub'; end
         if strcmp(pblk{1},'s')
            ss = [0, cumsum(pblk{2})];
            logdetX = 2*log(diag(Xchol{p}));
            logdetZ = 2*log(diag(Zchol{p}));
            logdetXsub = zeros(length(idx),1);
            logdetZsub = zeros(length(idx),1);
            for k = 1:length(idx)
               idxtmp = [ss(idx(k))+1:ss(idx(k)+1)];
               logdetXsub(k) = sum(logdetX(idxtmp));
               logdetZsub(k) = sum(logdetZ(idxtmp));
            end
            tmp(1) = -sum(vpsub.*logdetXsub);
            tmp(2) = sum(vpsub.*logdetZsub + (pblk{2}(idx)').*vpsub.*(1-log(vpsub)));
         elseif strcmp(pblk{1},'q')
            gamX = sqrt(qops(pblk,X{p},X{p},2));
            gamZ = sqrt(qops(pblk,Z{p},Z{p},2));
            tmp(1) = -sum(vpsub.*log(gamX(idx)));
            tmp(2) = sum(vpsub.*log(gamZ(idx)) + vpsub);
         elseif strcmp(pblk{1},'l')
            logX = log(X{p}); logZ = log(Z{p});
            tmp(1) = -sum(vpsub.*logX(idx));
            tmp(2) = sum(vpsub.*logZ(idx) + vpsub.*(1-log(vpsub)));
         end
         objadd = objadd + tmp;
      end
   end
%%********************************************************************

repo: REST


text:
function i4vec_nonzero_first_test ( )

%*****************************************************************************80
%
%% I4VEC_NONZERO_FIRST_TEST tests I4VEC_NONZERO_FIRST.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 April 2009
%
%  Author:
%
%    John Burkardt
%
  n = 10;
  test_num = 5;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'I4VEC_NONZERO_FIRST_TEST\n' );
  fprintf ( 1, '  For an integer vector:\n' );
  fprintf ( 1, '  I4VEC_NONZERO_FIRST left shifts the nonzero entries\n' );
  fprintf ( 1, '  of an I4VEC so they appear first.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, ...
    '  ----------Before--------------    ----------After---------------\n' );
  fprintf ( 1, '\n' );
  seed = 123456789;

  ilo = -1;
  ihi = +2;

  for test = 1 : test_num

    [ a, seed ] = i4vec_uniform_ab ( n, ilo, ihi, seed );
    a_save(1:n) = a(1:n);
    [ a, nz, indx ] = i4vec_nonzero_first ( n, a );
    fprintf ( 1, '  ' );
    for i = 1 : n
      fprintf ( 1, '%3d', a_save(i) );
    end
    fprintf ( 1, '    ' );
    for i = 1 : n
      fprintf ( 1, '%3d', a(i) );
    end
    fprintf ( 1, '\n' );

  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The value NZ counts the nonzeros, and\n' );
  fprintf ( 1, '  the vector INDX indicates the original positions:\n' );
  fprintf ( 1, '\n' );

  [ a, seed ] = i4vec_uniform_ab ( n, ilo, ihi, seed );
  a_save(1:n) = a(1:n);
  [ a, nz, indx ] = i4vec_nonzero_first ( n, a );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Original vector:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  ' );
  for i = 1 : n
    fprintf ( 1, '%3d', a_save(i) );
  end
  fprintf ( 1, '\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of nonzeros NZ = %d\n', nz );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Shifted vector:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  ' );
  for i = 1 : n
    fprintf ( 1, '%3d', a(i) );
  end
  fprintf ( 1, '\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Index vector:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  ' );
  for i = 1 : n
    fprintf ( 1, '%3d', indx(i) );
  end
  fprintf ( 1, '\n' );

  return
end


text:
function outvec = SHSetValue(invec,value,l,m,N,lmax,both)

% outvec = SHSetValue(invec,value,l,m[,N,lmax][,both])
%
% Sets the spherical harmonic coefficient vector entry corresponding
% to degree l and order m to the specified value. If optional arguments
% N and lmax are specified, invec is treated as a set of sections as
% described by lmax, and the value corresponding to sections N is set.
% If lmax is specified, it must be a vector describing the spherical
% harmonics degrees lmax for each of the sections 1 .. length(lmax).
% If 'both' is given, set both positive and negative order coefficients.

outvec = invec;

if l<0
    error('invalid usage: l has to be a non-negative integer');
end

if m>l
    error('invalid usage: l has to be greater or equal to m');
end

if nargin < 5
    N=1;
end

n=zeros(length(N),1);

if nargin == 5
    error('please provide sixth argument: array of lmax values');
elseif nargin > 5
    if find(N>length(lmax))>0 | find(N<1)>0 %#ok<OR2>
        error('unable to set the value since section N is not present');
    elseif find(lmax(N)<l)
        error('unable to set the values for l,m: degree is less than l');
    else
        for j=1:length(N)
            for i=1:N(j)-1
                n(j) = n(j) + SHl2n(lmax(i));
            end
        end
    end
end

n = n + SHlm2n(l,m);

if n>length(invec)
    error('invalid usage: index %d exceeds the length of the vector',n);
end

outvec(n)=value;

if nargin <= 6
    return;
end

n = n - SHlm2n(l,m) + SHlm2n(l,-m);

outvec(n)=value;


text:
function varargout = figureResized( screenratio, varargin )
% Creates a figures that takes up certain area of screen.
%
% Almost same as figure, except get to specify what fraction of available
% screen space figure should take up.  Figure appears in center of screen.
%
% USAGE
%  varargout = figureResized( screenratio, varargin )
%
% INPUTS
%  screenratio - controls fraction of screen image takes out (<=.8)
%  varargin    - parameters to figure
%
% OUTPUTS
%  varargout   - out from figure
%
% EXAMPLE
%  figureResized( .75 )
%
% See also FIGURE
%
% Piotr's Image&Video Toolbox      Version 2.0
% Copyright 2012 Piotr Dollar.  [pdollar-at-caltech.edu]
% Please email me if you find bugs, or have suggestions or questions!
% Licensed under the Simplified BSD License [see external/bsd.txt]

if( nargin<1 ); screenratio=.3; end
if( screenratio>1 ); error('screenratio must be <= 1'); end

% call figure
h = figure( varargin{:} );
if( nargout ); varargout = {h}; end;

% get dimensions of screen and what want figure to be
units = get(0,'Units');
ss = get(0,'ScreenSize');
st = (1 - screenratio)/2;
pos = [st*ss(3), st*ss(4), screenratio*ss(3), screenratio*ss(4)];

% set dimensions of figure
set( h, 'Units', units );
set( h, 'Position', pos );

author: zouchuhang

repo: LayoutNet


text:
function [v,f] = max_kur(x,d0,maxit,show)

%
% [v,f] = max_kur(x,d,maxit,show)
%
% Compute direction maximizing the kurtosis of the projections
% Observations passed to the routine should be standardized
% Uses Newton's method and an augmented Lagrangian merit function
% To be used as a subroutine of kur_rce
%
% Inputs:  x, observations (by rows)
%          d, initial estimate of the direction (optional)
%          maxit, a limit to the number of Newton iterations
%          show, <>0 generates output for each iteration
% Outputs: v, max. kurtosis direction (local optimizer)
%          f, max. kurtosis value
%

% Daniel Pena/Francisco J Prieto 23/5/00

% Initialization

maxitdefault = 30;

if nargin < 4,
  show = 0;
end
if nargin < 3,
  maxit = maxitdefault;
end
if maxit <= 0,
  maxit = maxitdefault;
end
if nargin < 2,
  d0 = [];
end

%% Tolerances

maxitini = 1;
tol = 1.0e-4;
tol1 = 1.0e-7;
tol2 = 1.0e-2;
beta = 1.0e-4;
rho0 = 0.1;

[n,p] = size(x);

%% Initial estimate of the direction

if length(d0) == 0,

  uv = sum((x.*x)')';
  uw = 1../(eps + sqrt(uv));
  uu = x.*(uw*ones(1,p));

  Su = cov(uu);
  [V,D] = eig(Su);

  r = [];
  for i = 1:p,
    r = [ r val_kur(x,V(:,i)) ];
  end
  [v,ik] = max(r);
  a = V(:,ik(1));

  itini = 1;
  difa = 1;
  while (itini <= maxitini)&(difa > tol2),
    z = x*a;
    zaux = z.^2;
    xaux = x'.*(ones(p,1)*zaux');
    H = 12*xaux*x;
    [V,E] = eig(H);
    [vv,iv] = max(diag(E));
    aa = V(:,iv(1));
    difa = norm(a - aa);
    a = aa;
    itini = itini + 1;
  end
else
  a = d0/norm(d0);
end

%% Values at iteration 0 for the optimization algorithm

z = x*a;
sk = sum(z.^4);
lam = 2*sk;
f = sk;
g = (4*(z.^3)'*x)';
zaux = z.^2;
xaux = x'.*(ones(p,1)*zaux');
H = 12*xaux*x;

al = 0;
it = 0;
diff = 1;
rho = rho0;
clkr = 0;

c = 0;

% Newton method starting from the initial direction

if show,
  disp(' It.      Obj.F.       | g |           c         alpha      rho');
end

while 1,

%% Check termination conditions

  gl = g - 2*lam*a;

  A = 2*a';
  [Q,W] = qr(a);
  Z = Q(:,(2:p));

  if show,
    aa = sprintf('%3.0f  %12.5e %13.4e',it,f,norm(gl));
    bb = sprintf(' %13.4e %8.3f %11.2e',abs(a'*a-1),al,rho);
    disp([ aa bb ]);
  end

  crit = norm(gl) + abs(c);
  if (crit <= tol)|(it >= maxit),
    break
  end

%% Compute search direction

  Hl = H - 2*lam*eye(p,p);
  Hr = Z'*Hl*Z;
  [V,E] = eig(Hr);
  Es = min(-abs(E),-1.0e-4);
  Hs = V*Es*V';

  py = - c/(A*A');
  rhs = Z'*(g + H*A'*py);
  pz = - Hs\rhs;
  pp = Z*pz + py*A';

  dlam = (2*a)\(gl + H*pp);

%% Adjust penalty parameter

  f0d = gl'*pp - 2*rho*c*a'*pp - dlam*c;
  crit1 = beta*norm(pp)^2;

  if f0d < crit1,
    rho1 = 2*(crit1 - f0d)/(eps + c^2);
    rho = max([2*rho1 1.5*rho rho0]);
    f = sk - lam*c - 0.5*rho*c^2;
    f0d = gl'*pp - 2*rho*c*a'*pp - dlam*c;
    clkr = 0;
  elseif (f0d > 1000*crit1)&(rho > rho0),
    rho1 = 2*(crit1 - gl'*pp + dlam*c)/(eps + c^2);
    if (clkr == 4)&(rho > 2*rho1),
      rho = 0.5*rho;
      f = sk - lam*c - 0.5*rho*c^2;
      f0d = gl'*pp - 2*rho*c*a'*pp - dlam*c;
      clkr = 0;
    else
      clkr = clkr + 1;
    end
  end
  if (abs(f0d/(norm(g-2*rho*a*c)+norm(c))) < tol1),
    break
  end

%% Line search

  al = 1;
  itbl = 0;
  while itbl < 20,
    aa = a + al*pp;
    lama = lam + al*dlam;
    zz = x*aa;
    cc = aa'*aa - 1;
    sk = sum(zz.^4);
    ff = sk - lama*cc - 0.5*rho*cc^2;

    if ff > f + 0.0001*al*f0d,
      break
    end
    al = al/2;
    itbl = itbl + 1;
  end
  if itbl >= 20,
    if show,
      disp('Error in the line search');
    end
    break
  end

%% Update values for the next iteration

  a = aa;
  lam = lama;
  z = zz;

  nmd2 = a'*a;
  c = nmd2 - 1;
  f = sk - lam*c - 0.5*rho*c^2;
  g = (4*(z.^3)'*x)';
  zaux = z.^2;
  xaux = x'.*(ones(p,1)*zaux');
  H = 12*xaux*x;

  it = it + 1;

end

% Values to be returned

v = a/norm(a);
xa = x*v;
f = sum(xa.^4)/n;

author: marianux

repo: ecg-kit

save_path: github-repos/MATLAB/marianux-ecg-kit


text:
function [IX0,SigmaX0,deltaMuX0,suffStat] = VBA_IX0(X0,y,posterior,suffStat,dim,u,options)
% Gauss-Newton update of initial conditions

if options.DisplayWin % Display progress
    set(options.display.hm(1),'string','VB Gauss-Newton on initial conditions... ');
    set(options.display.hm(2),'string',' ');
    drawnow
end

% check infinite precision transition pdf
iQx0 = VBA_inv(options.priors.iQx{1},options.params2update.x{1},'replace');
IN = diag(~~diag(iQx0));

% Get precision parameters
alphaHat = posterior.a_alpha./posterior.b_alpha;

% Preallocate intermediate variables
indIn = options.params2update.x0;
muX0 = options.priors.muX0;
x0 = muX0;
x0(indIn) = X0;

% Evaluate evolution function at current mode
[fx0,dF_dX0] = VBA_evalFun('f',x0,posterior.muTheta,u(:,1),options,dim,1);

% error terms
dx = IN*(posterior.muX(:,1)- fx0);
dx2 = dx'*iQx0*dx;
dx0 = muX0-x0;

% posterior covariance matrix terms
Q = options.priors.SigmaX0(indIn,indIn);
iQ = VBA_inv(Q);
iSigmaX0 = iQ + alphaHat.*dF_dX0(indIn,:)*iQx0(indIn,indIn)*dF_dX0(indIn,:)';

% posterior covariance matrix
SigmaX0 = VBA_inv(iSigmaX0);

% mode
tmp = iQ*dx0(indIn) + alphaHat.*dF_dX0(indIn,:)*iQx0(indIn,indIn)*dx;
deltaMuX0 = SigmaX0*tmp;

% variational energy
IX0 = -0.5.*dx0'*iQ*dx0 - 0.5*alphaHat.*dx2;
if VBA_isWeird (IX0)
    div = 1;
    IX0 = -Inf;
else
    div = 0;
end

% update sufficient statistics
dx20 = suffStat.dx(:,1)'*iQx0*suffStat.dx(:,1);
suffStat.dx2 = suffStat.dx2 - dx20 + dx2; % correct states squared error
suffStat.dx(:,1) = dx;
suffStat.dx0 = dx0;
suffStat.div = div;
suffStat.IX0 = IX0;

author: MBB-team

repo: VBA-toolbox


text:
function [rhsHx, rhsHy, rhsHz, rhsEx, rhsEy, rhsEz] = MaxwellRHS3D(Hx,Hy,Hz,Ex,Ey,Ez)

% function [rhsHx, rhsHy, rhsHz, rhsEx, rhsEy, rhsEz] = MaxwellRHS3D(Hx,Hy,Hz,Ex,Ey,Ez)
% Purpose  : Evaluate RHS flux in 3D Maxwell equations

Globals3D;

% storage for field differences at faces
dHx = zeros(Nfp*Nfaces,K); dHy = dHx; dHz = dHx;
dEx = zeros(Nfp*Nfaces,K); dEy = dEx; dEz = dEx;

% form field differences at faces
dHx(:)  = Hx(vmapP)-Hx(vmapM);  dEx(:)  = Ex(vmapP)-Ex(vmapM);
dHy(:)  = Hy(vmapP)-Hy(vmapM); 	dEy(:)  = Ey(vmapP)-Ey(vmapM);
dHz(:)  = Hz(vmapP)-Hz(vmapM);  dEz(:)  = Ez(vmapP)-Ez(vmapM);

% make boundary conditions all reflective (Ez+ = -Ez-)
dHx(mapB) = 0;  dEx(mapB) = -2*Ex(vmapB);
dHy(mapB) = 0;  dEy(mapB) = -2*Ey(vmapB);
dHz(mapB) = 0;  dEz(mapB) = -2*Ez(vmapB);

alpha=1; % => full upwinding

ndotdH = nx.*dHx + ny.*dHy + nz.*dHz;
ndotdE = nx.*dEx + ny.*dEy + nz.*dEz;

fluxHx = -ny.*dEz + nz.*dEy + alpha*(dHx - ndotdH.*nx);
fluxHy = -nz.*dEx + nx.*dEz + alpha*(dHy - ndotdH.*ny);
fluxHz = -nx.*dEy + ny.*dEx + alpha*(dHz - ndotdH.*nz);

fluxEx =  ny.*dHz - nz.*dHy + alpha*(dEx - ndotdE.*nx);
fluxEy =  nz.*dHx - nx.*dHz + alpha*(dEy - ndotdE.*ny);
fluxEz =  nx.*dHy - ny.*dHx + alpha*(dEz - ndotdE.*nz);

% evaluate local spatial derivatives
[curlHx,curlHy,curlHz] = Curl3D(Hx,Hy,Hz);
[curlEx,curlEy,curlEz] = Curl3D(Ex,Ey,Ez);

% calculate Maxwell's right hand side
rhsHx = -curlEx + LIFT*(Fscale.*fluxHx/2);
rhsHy = -curlEy + LIFT*(Fscale.*fluxHy/2);
rhsHz = -curlEz + LIFT*(Fscale.*fluxHz/2);

rhsEx =  curlHx + LIFT*(Fscale.*fluxEx/2);
rhsEy =  curlHy + LIFT*(Fscale.*fluxEy/2);
rhsEz =  curlHz + LIFT*(Fscale.*fluxEz/2);
return;

repo: Aero-matlab


text:
function f_x = ParFor3(in1)
%PARFOR3
%    F_X = PARFOR3(IN1)

%    This function was generated by the Symbolic Math Toolbox version 8.2.
%    20-Sep-2019 09:35:35

u = in1(:,1);
ux = in1(:,4);
uxx = in1(:,5);
uxxx = in1(:,6);
f_x = (u.*1.034220113405542e16+ux.*6.575036155887616e15+uxx.*1.215253266628608e17-uxxx.*4.601209472843776e15-u.*ux.*5.099459193028608e15+u.*uxx.*5.032641492680704e16+u.*uxxx.*4.967741975658496e15-2.603211137867776e15)./(u.*3.126267420934144e16+4.755897990407782e16);

repo: SINDy-PI


text:
function F = acotd(F, varargin)
%ACOTD   Inverse cotangent of a CHEBFUN, result in degrees.
%   ACOTD(F) computes the inverse cotangent (in degrees) of the CHEBFUN F.
%
%   ACOTD(F, PREF) does the same but uses the CHEBFUNPREF object PREF when
%   computing the composition.
%
% See also COTD, ACOT.

% Copyright 2017 by The University of Oxford and The Chebfun Developers. See
% http://www.chebfun.org/ for Chebfun information.

% Call the compose method:
F = compose(F, @acotd, varargin{:});

end

author: chebfun

repo: chebfun

save_path: github-repos/MATLAB/chebfun-chebfun


text:
function [doBvecs, dwParams] = dtiInitBuildBVs(dwDir,dwParams)
%
%   [doBvecs dwDir dwParams] = dtiInitBVs(dwDir,dwParams);
%
% Generate bvecs file and a bvals file from a dwepi.grads file.
%
% *** FIX ME: get the bval and grad dir file number from the dicom header.
% *** IS THIS FUNCTION STILL USED IN THE MODERN AGE?  (BW)
%
% NOTE:
% Most sequences will need the bvecs reoriented based on the
% scanner-to-image transform stored in qto_ijk. But, for the sequence we
% used many years ago from Roland Bammer sequence, the bvecs were rotated
% during image acquisition.
%
% INPUTS
%       (dwDir,dwParams) - passed in from dtiInit
% RETURNS
%       [doBvecs dwDir dwParams] - with paths to built bvecs and bvals
%                                  files
% (C) Stanford VISTA, 8/2011 [lmp]

%% Generate bvecs file and a bvals file from a dwepi.grads file.

warning('** Building bvecs and bvals from dtiInitBuildBVs **')

doBvecs = true;

% Directory with gradient direction code files
gradsDir = fullfile(dwDir.mrDiffusionDir,'gradFiles');

% Get bval and gradient directions code from name file name
if ~exist('dwParams.bvalue','var') || isempty(dwParams.bvalue)
    [dwParams.bvalue, gradDirsCodeTmp] = dtiRawGetBvalVecFromName(dwDir.inBaseName);
end

if(~exist('dwParams.gradDirsCode','var')||isempty(dwParams.gradDirsCode))
    if(exist('gradDirsCodeTmp','var'))
        dwParams.gradDirsCode = gradDirsCodeTmp;
    else
        [dwParams.bvalue, dwParams.gradDirsCode] = dtiRawGetBvalVecFromName(filename);
    end
end

% If we can't guess the gradDirsCode we ask for it.
if(isempty(dwParams.gradDirsCode))
    [f,p] = uigetfile({'*.grads';'*.*'},'Select the GE grads file...',gradsDir);
    if isnumeric(f); error('Canceled.'); end
    dwParams.gradDirsCode = fullfile(p,f);
end

% Set the gradsFile to use
if(ischar(dwParams.gradDirsCode))
    gradsFile = dwParams.gradDirsCode;
else
    gradsFile = fullfile(gradsDir,sprintf('dwepi.%d.grads',dwParams.gradDirsCode));
end

% Actually build the bvecs/vals
dtiRawBuildBvecs(size(dwRaw.data,4), eye(4), gradsFile, dwParams.bvalue,...
                 dwDir.inBaseDir, dwParams.flipLrApFlag);

fprintf('bvalsFile = %s; %% (dwParams.bvalue = %0.3f)\nbvecsFile = %s; %%(dwParams.gradDirsCode = %d)\n', dwDir.bvalsFile,dwParams.bvalue,dwDir.bvecsFile,dwParams.gradDirsCode);

return

author: vistalab

repo: vistasoft

save_path: github-repos/MATLAB/vistalab-vistasoft


text:
function tf=in_range_exclusive(value, R)
    % in_range_exclusive returns true where value in interval (A B), where  R = [A B];
    %
    % example:
    %  >> in_range_exclusive( 1:5 , [2 4])  % returns logical aray [0 0 1 0 0]
    %
    % see also in_range, in_range_inclusive
    if numel(R)==2
        tf = R(1) < value & value < R(2);
    else
        error('range should be a 1x2 vector of [minval maxval]');
    end
end

author: CelsoReyes


text:
% closest fly, based on dcenter
function [data,units,mind] = compute_closestfly_center(trx,n,dosave_d)

if nargin < 3,
  dosave_d = true;
end

flies = trx.exp2flies{n};
nflies = numel(flies);
closestfly = cell(1,nflies);
mind = cell(1,nflies);

parfor i1 = 1:nflies,
  fly1 = flies(i1);
  flies2 = flies(trx.roi(fly1)==trx.roi(flies));
  dcenter = inf(numel(flies2),trx(fly1).nframes);
  for i2 = 1:numel(flies2),
    fly2 = flies2(i2);
    if fly1 == fly2,
      continue;
    end
    dcenter(i2,:) = dcenter_pair(trx,fly1,fly2);
  end
  [mind{i1},closesti] = min(dcenter,[],1);
  closestfly{i1} = flies2(closesti);
  closestfly{i1}(isnan(mind{i1})) = nan;
end

% so that we don't compute dcenter twice
if dosave_d,
  data = mind; %#ok<NASGU>
  units = parseunits('mm'); %#ok<NASGU>
  filename = trx.GetPerFrameFile('dcenter',n);
  save(filename,'data','units');
end

data = closestfly;
units = parseunits('unit');

author: kristinbranson

repo: JAABA


text:
function [nm] = km2nm(km)
% Convert length from kilometers to nanometers.
% Chad A. Greene 2012
nm = km*1000000000000;


text:
%%********************************************************************
%% igmres: compute ideal GMRES polynomial of deg m
%%         of a matrix B.
%%
%%    min || p(B) ||_2
%%    where p is a polynomial of degree <= m and p(0) = 1.
%%
%%    B = a general square matrix.
%%-----------------------------------------------------------
%%
%% [blk,Avec,C,b,X0,y0,Z0,objval,p] = igmres(B,m,feas,solve);
%%
%% B = a square matrix.
%% m = degree of polynomial.
%% feas  = 1 if want feasible starting point
%%       = 0 if otherwise.
%% solve = 0 just to initialize
%%       = 1 if want to solve the problem.
%%
%% p = ideal GMRES polynomial in Matlab format.
%%*****************************************************************
%% SDPT3: version 4.0
%% Copyright (c) 1997 by
%% Kim-Chuan Toh, Michael J. Todd, Reha H. Tutuncu
%% Last Modified: 16 Sep 2004
%%*****************************************************************

 function [blk,Avec,C,b,X0,y0,Z0,objval,p] = igmres(B,m,feas,solve)

   if (nargin <= 3); solve = 0; end;
   if (nargin <= 2); feas  = 0; end;

   N = length(B);
   if (m >= N); error('degree >= size of B'); end;

   A = cell(1,m+1);
   [V,H,R] = orthbasis(B,m-1);
   A{1} = eye(N);
   for k = [1:m];  A{k+1} = B*V{k};  end;
   [blk,Avec,C,b,X0,y0,Z0,objval,xx] = norm_min(A,feas,solve);
   if (solve)
      y = xx;
      x1 = R(1:m,1:m)*y(1:m);
      p = [x1(m:-1:1); 1];
   else
      objval = []; p = [];
   end
%%********************************************************************

author: yu-jiang

repo: radpbook

save_path: github-repos/MATLAB/yu-jiang-radpbook


text:
function H=circle(center,radius,NOP,style)
%---------------------------------------------------------------------------------------------
% H=CIRCLE(CENTER,RADIUS,NOP,STYLE)
% This routine draws a circle with center defined as
% a vector CENTER, radius as a scaler RADIS. NOP is
% the number of points on the circle. As to STYLE,
% use it the same way as you use the rountine PLOT.
% Since the handle of the object is returned, you
% use routine SET to get the best result.
%
%   Usage Examples,
%
%   circle([1,3],3,1000,':');
%   circle([2,4],2,1000,'--');
%
%   Zhenhai Wang <zhenhai@ieee.org>
%   Version 1.00
%   December, 2002
%---------------------------------------------------------------------------------------------

if (nargin <3),
 error('Please see help for INPUT DATA.');
elseif (nargin==3)
    style='b-';
end;
THETA=linspace(0,2*pi,NOP);
RHO=ones(1,NOP)*radius;
[X,Y] = pol2cart(THETA,RHO);
X=X+center(1);
Y=Y+center(2);
H=plot(X,Y,style);
axis square;

author: aludnam

repo: MATLAB

save_path: github-repos/MATLAB/aludnam-MATLAB


text:
function keep(varargin);
%KEEP keeps the caller workspace variables of your choice and clear the rest.
%       Its usage is just like "clear" but only for variables.
%
%       Xiaoning (David) Yang   xyang@lanl.gov 1998
%       Revision based on comments from Michael McPartland,
%       michael@gaitalf.mgh.harvard.edu, 1999

%       Keep all
if isempty(varargin)
        return
end

%       See what are in caller workspace
wh = evalin('caller','who');

%       Check workspace variables
if isempty(wh)
        error('  There is nothing to keep!')
end

%       Construct a string containing workspace variables delimited by ":"
variable = [];
for i = 1:length(wh)
        variable = [variable,':',wh{i}];
end
variable = [variable,':'];

%       Extract desired variables from string
flag = 0;
for i = 1:length(varargin)
        I = findstr(variable,[':',varargin{i},':']);
        if isempty(I)
                disp(['       ',varargin{i}, ' does not exist!'])
                flag = 1;
        elseif I == 1
                variable = variable(1+length(varargin{i})+1:length(variable));
        elseif I+length(varargin{i})+1 == length(variable)
                variable = variable(1:I);
        else
                variable = [variable(1:I),variable(I+length(varargin{i})+2:length(variable))];
        end
end

%       No delete if some input variables do not exist
if flag == 1
        disp('       No variables are deleted!')
        return
end

%       Convert string back to cell and delete the rest
I = findstr(variable,':');
if length(I) ~= 1
        for i = 1:length(I)-1
                if i ~= length(I)-1
                        del(i) = {[variable(I(i)+1:I(i+1)-1),' ']};
                else
                        del(i) = {variable(I(i)+1:length(variable)-1)};
                end
        end
        evalin('caller',['clear ',del{:}])
end


text:
%  This function takes a problem in SeDuMi MATLAB format and writes it out
%  in SDPpack format.
%
%  Usage:
%
%  writesdp(fname,A,b,c,K)
%
%      fname           Name of SDPpack file, in quotes
%      A,b,c,K         Problem in SeDuMi form
%
%  Notes:
%
%     Problems with complex data are not allowed.
%
%     Rotated cone constraints are not supported.
%
%     Nonsymmetric A.s and C.s matrices are symmetrized with A=(A+A')/2
%     a warning is given when this happens.
%
%     Floating point numbers are written out with 18 decimal digits for
%     accuracy.
%
%  Please contact the author (Brian Borchers, borchers@nmt.edu) with any
%  questions or bug reports.
%
function writesdp(fname,A,b,c,K)

%From: 	Brian Borchers[SMTP:borchers@nmt.edu]
%Sent: 	Wednesday, December 08, 1999 12:33 PM
%To: 	J.Sturm@KE.UNIMAAS.NL
%
%Here's a MATLAB routine that will take a problem in SeDuMi's MATLAB format
%and write it out in SDPpack format.

%
%  First, check for complex numbers in A, b, or c.
%
if (isreal(A) ~= 1),
  disp('A is not real!');
  return;
end;
if (isreal(b) ~= 1),
  disp('b is not real!');
  return;
end;
if (isreal(c) ~= 1),
  disp('c is not real!');
  return;
end;
%
%  Check for any rotated cone constraints.
%
if (isfield(K,'r') && (~isempty(K.r)) && (K.r ~= 0)),
  disp('rotated cone constraints are not yet supported.');
  return;
end;
%
%  Get the size data.
%
if (isfield(K,'l')),
  nlin=K.l;
  sizelin=nlin;
  if (isempty(sizelin)),
    sizelin=0;
    nlin=0;
  end;
else
  nlin=0;
  sizelin=0;
end;

if (isfield(K,'s')),
  nsdpblocks=length(K.s);
  %sizesdp=sum((K.s).^2);
  %if (isempty(sizesdp)),
    %sizesdp=0;
    %nsdpblocks=0;
  %end;
else
  %sizesdp=0;
  nsdpblocks=0;
end;

if (isfield(K,'q')),
  nqblocks=length(K.q);
  sizeq=sum(K.q);
  if (isempty(sizeq)),
    sizeq=0;
    nqblocks=0;
  end;
else
  nqblocks=0;
  sizeq=0;
end;

m=length(b);

%
%  Open up the file for writing.
%
fid=fopen(fname,'w');
%
%  Print out m, the number of constraints.
%
fprintf(fid,'%d \n',m);
%
%  Next, b, with one entry per line.
%
fprintf(fid,'%.18e\n',full(b));
%
%  Next, the semidefinite part.
%
if (nsdpblocks == 0),
  fprintf(fid,'0\n');
else
%
%  Print out the number of semidefinite blocks.
%
  fprintf(fid,'%d\n',nsdpblocks);
%
%  For each block, print out its size.
%
  fprintf(fid,'%d\n',full(K.s));
%
%  Next, the cost matrix C.s.
%
%
%  First, calculate where in c things start.
%
  base=sizelin+sizeq+1;
%
%  Next, work through the blocks.
%
  for i=1:nsdpblocks,
    fprintf(fid,'1\n');
    work=c(base:base+K.s(i)^2-1);
    if nnz(work ~= work'),
    if (work ~= work'),
      disp('Non symmetric C.s matrix!');
      work=(work+work')/2;
    end;
    work=triu(work);
    [II,JJ,V]=find(work);
    cnt=length(II);
    fprintf(fid,'%d\n',cnt);
    if (cnt ~= 0),
      fprintf(fid,'%d\n%d\n%.18e\n',[II JJ V]');
    end;
%
%  Next, update to the next base.
%
    base=base+K.s(i)^2;
  end;
%
%  Now, loop through the constraints, one at a time.
%
  for cn=1:m,
%
%  Print out the SDP part of constraint cn.
%
    base=sizelin+sizeq+1;
    for i=1:nsdpblocks,
      fprintf(fid,'1\n');
      if nnz(work ~= work'),
      work=reshape(work,K.s(i),K.s(i));
      if (work ~= work'),
        disp('Non symmetric A.s matrix!');
        work=(work+work')/2;
      end;
      work=triu(work);

      [II,JJ,V]=find(work);
      cnt=length(II);
      fprintf(fid,'%d\n',cnt);
      if (cnt ~= 0),
        fprintf(fid,'%d\n%d\n%.18e\n',[II JJ V]');
      end;
%
%  Next, update to the next base.
%
      base=base+K.s(i)^2;
    end;
%
% Done with constraint cn
%
  end;
%
% Done with SDP part.
%
end;
%
% Next, handle the Quadratic part.
%
%
% Describe the Q blocks.
%
if (nqblocks == 0),
  fprintf(fid,'0\n');
else
  fprintf(fid,'%d\n',nqblocks);
  fprintf(fid,'%d\n',full(K.q));
%
%  Find C.q.
%
  base=sizelin+1;
  cq=c(base:base+sizeq-1);
%
% Print out the C.q coefficients.
%
  fprintf(fid,'%.18e\n',full(cq));
%
%  Next, the constraint matrix A.q.
%
  Aq=A(:,base:base+sizeq-1);
%
%  Print out the count of nonzeros.
%
  [II,JJ,V]=find(Aq);
  cnt=length(II);
  fprintf(fid,'1\n');
  fprintf(fid,'%d\n',cnt);
  if (cnt ~= 0),
    fprintf(fid,'%d\n%d\n%.18e\n',[II JJ V]');
  end;
%
% End of handling quadratic part.
%
end;
%
%
% Finally, handle the linear part.
%
if (nlin == 0),
  fprintf(fid,'0\n');
else
%
% Print out the number of linear variables.
%
  fprintf(fid,'%d\n',nlin);
%
% Print out C.l
%
  fprintf(fid,'%.18e\n',full(c(1:nlin)));
%
%  Print out the A matrix.
%
  Al=A(:,1:nlin);
  [II,JJ,V]=find(Al);
  cnt=length(II);
  fprintf(fid,'1\n');
  fprintf(fid,'%d\n',cnt);
  if (cnt ~= 0),
    fprintf(fid,'%d\n%d\n%.18e\n',[II JJ V]');
  end;
end;


text:
%kcmindist_printsig 'Print a Minimum Distance Signature '
% This MatLab function was automatically generated by a converter (KhorosToMatLab) from the Khoros cmindist_printsig.pane file
%
% Parameters:
% InputFile: isig 'Input Signature', required: 'input signature file'
% MultiChoice: omode 'Output Mode', default: 1: 'output mode toggle'
%    Choices are:
%   1: 'Text'
%   2: 'LaTeX'
%   3: 'HTML'
%   4: 'CSV'
% OutputFile: o 'Output', optional: 'output signature info'
%
% Example: o = kcmindist_printsig(isig, {'isig','';'omode',1;'o',''})
%
% Khoros helpfile follows below:
%
%  PROGRAM
% cmindist_printsig - Print a Minimum Distance Signature
%
%  DESCRIPTION
% This operator prints out the contents of a Minimum Distance Signature file created with "cmindist_signature". It expects a valid Minimum Distance signature file in the parameter [-isig], so its input must be either the output of a \fIcmindist_signature\fP or the output of \fIkappend\fP which appended several signatures together in the time dimension. Its output can be formatted so it can be used in reports, etc. and will be redirected to kstdout unless a file name is passed with the parameter [-o].
% The output of this program can be in four formats, depending on the value of the parameter [-omode] :
% The Text (ASCII) format option ([-omode 1]) formats the output so it can be viewed with simple text editors or in kstdout.
% The LaTeX format option ([-omode 2]) formats the output so it can be included in a LaTeX file with the \\input command.
% The HTML format option ([-omode 3]) formats the output in a HTML table so it can be read into a HTML file.
% The CSV format option ([-omode 4]) formats the output in a Comma-delimited format so it can be read by spreadsheets, etc.
%
%
%
%  EXAMPLES
% All examples for the Classify toolbox are listed on the Classify Toolbox Manual. For an example of this routine, please see the example workspace Classify:workspaces:MINDIST-Signatures.
%
%  "SEE ALSO"
% cmindist_signature
%
%  RESTRICTIONS
% Expect the input file to be a valid signature file for the Minimum Distance classifier.
%
%  REFERENCES
% All references for the Classify toolbox are listed on the Classify Toolbox Manual.
%
%  COPYRIGHT
% Copyright (C) 1997 Rafael Santos. Khoros (C) Khoral Research, Inc.
%

function varargout = kcmindist_printsig(varargin)
if nargin ==0
  Inputs={};arglist={'',''};
elseif nargin ==1
  Inputs=varargin{1};arglist={'',''};
elseif nargin ==2
  Inputs=varargin{1}; arglist=varargin{2};
else error('Usage: [out1,..] = kcmindist_printsig(Inputs,arglist).');
end
if size(arglist,2)~=2
  error('arglist must be of form {''ParameterTag1'',value1;''ParameterTag2'',value2}')
 end
narglist={'isig', '__input';'omode', 1;'o', '__output'};
maxval={0,0,1};
minval={0,0,1};
istoggle=[0,0,1];
was_set=istoggle * 0;
paramtype={'InputFile','MultiChoice','OutputFile'};
% identify the input arrays and assign them to the arguments as stated by the user
if ~iscell(Inputs)
Inputs = {Inputs};
end
NumReqOutputs=0; nextinput=1; nextoutput=1;
  for ii=1:size(arglist,1)
  wasmatched=0;
  for jj=1:size(narglist,1)
   if strcmp(arglist{ii,1},narglist{jj,1})  % a given argument was matched to the possible arguments
     wasmatched = 1;
     was_set(jj) = 1;
     if strcmp(narglist{jj,2}, '__input')
      if (nextinput > length(Inputs))
        error(['Input ' narglist{jj,1} ' has no corresponding input!']);
      end
      narglist{jj,2} = 'OK_in';
      nextinput = nextinput + 1;
     elseif strcmp(narglist{jj,2}, '__output')
      if (nextoutput > nargout)
        error(['Output nr. ' narglist{jj,1} ' is not present in the assignment list of outputs !']);
      end
      if (isempty(arglist{ii,2}))
        narglist{jj,2} = 'OK_out';
      else
        narglist{jj,2} = arglist{ii,2};
      end

      nextoutput = nextoutput + 1;
      if (minval{jj} == 0)
         NumReqOutputs = NumReqOutputs - 1;
      end
     elseif isstr(arglist{ii,2})
      narglist{jj,2} = arglist{ii,2};
     else
        if strcmp(paramtype{jj}, 'Integer') & (round(arglist{ii,2}) ~= arglist{ii,2})
            error(['Argument ' arglist{ii,1} ' is of integer type but non-integer number ' arglist{ii,2} ' was supplied']);
        end
        if (minval{jj} ~= 0 | maxval{jj} ~= 0)
          if (minval{jj} == 1 & maxval{jj} == 1 & arglist{ii,2} < 0)
            error(['Argument ' arglist{ii,1} ' must be bigger or equal to zero!']);
          elseif (minval{jj} == -1 & maxval{jj} == -1 & arglist{ii,2} > 0)
            error(['Argument ' arglist{ii,1} ' must be smaller or equal to zero!']);
          elseif (minval{jj} == 2 & maxval{jj} == 2 & arglist{ii,2} <= 0)
            error(['Argument ' arglist{ii,1} ' must be bigger than zero!']);
          elseif (minval{jj} == -2 & maxval{jj} == -2 & arglist{ii,2} >= 0)
            error(['Argument ' arglist{ii,1} ' must be smaller than zero!']);
          elseif (minval{jj} ~= maxval{jj} & arglist{ii,2} < minval{jj})
            error(['Argument ' arglist{ii,1} ' must be bigger than ' num2str(minval{jj})]);
          elseif (minval{jj} ~= maxval{jj} & arglist{ii,2} > maxval{jj})
            error(['Argument ' arglist{ii,1} ' must be smaller than ' num2str(maxval{jj})]);
          end
        end
     end
     if ~strcmp(narglist{jj,2},'OK_out') &  ~strcmp(narglist{jj,2},'OK_in')
       narglist{jj,2} = arglist{ii,2};
     end
   end
   end
   if (wasmatched == 0 & ~strcmp(arglist{ii,1},''))
        error(['Argument ' arglist{ii,1} ' is not a valid argument for this function']);
   end
end
% match the remaining inputs/outputs to the unused arguments and test for missing required inputs
 for jj=1:size(narglist,1)
     if  strcmp(paramtype{jj}, 'Toggle')
        if (narglist{jj,2} ==0)
          narglist{jj,1} = '';
        end;
        narglist{jj,2} = '';
     end;
     if  ~strcmp(narglist{jj,2},'__input') && ~strcmp(narglist{jj,2},'__output') && istoggle(jj) && ~ was_set(jj)
          narglist{jj,1} = '';
          narglist{jj,2} = '';
     end;
     if strcmp(narglist{jj,2}, '__input')
      if (minval{jj} == 0)  % meaning this input is required
        if (nextinput > size(Inputs))
           error(['Required input ' narglist{jj,1} ' has no corresponding input in the list!']);
        else
          narglist{jj,2} = 'OK_in';
          nextinput = nextinput + 1;
        end
      else  % this is an optional input
        if (nextinput <= length(Inputs))
          narglist{jj,2} = 'OK_in';
          nextinput = nextinput + 1;
        else
          narglist{jj,1} = '';
          narglist{jj,2} = '';
        end;
      end;
     else
     if strcmp(narglist{jj,2}, '__output')
      if (minval{jj} == 0) % this is a required output
        if (nextoutput > nargout & nargout > 1)
           error(['Required output ' narglist{jj,1} ' is not stated in the assignment list!']);
        else
          narglist{jj,2} = 'OK_out';
          nextoutput = nextoutput + 1;
          NumReqOutputs = NumReqOutputs-1;
        end
      else % this is an optional output
        if (nargout - nextoutput >= NumReqOutputs)
          narglist{jj,2} = 'OK_out';
          nextoutput = nextoutput + 1;
        else
          narglist{jj,1} = '';
          narglist{jj,2} = '';
        end;
      end
     end
  end
end
if nargout
   varargout = cell(1,nargout);
else
  varargout = cell(1,1);
end
global KhorosRoot
if exist('KhorosRoot') && ~isempty(KhorosRoot)
w=['"' KhorosRoot];
else
if ispc
  w='"C:\Program Files\dip\khorosBin\';
else
[s,w] = system('which cantata');
w=['"' w(1:end-8)];
end
end
[varargout{:}]=callKhoros([w 'cmindist_printsig"  '],Inputs,narglist);


text:
function grav = GravityForces(thetalist, g, Mlist, Glist, Slist)
% *** CHAPTER 8: DYNAMICS OF OPEN CHAINS ***
% Takes thetalist: A list of joint variables,
%       g: 3-vector for gravitational acceleration,
%       Mlist: List of link frames i relative to i-1 at the home position,
%       Glist: Spatial inertia matrices Gi of the links,
%       Slist: Screw axes Si of the joints in a space frame, in the format
%              of a matrix with the screw axes as the columns.
% Returns grav: The joint forces/torques required to overcome gravity at
%               thetalist
% This function calls InverseDynamics with Ftip = 0, dthetalist = 0, and
% ddthetalist = 0.
% Example Input (3 Link Robot):
%
% clear; clc;
% thetalist = [0.1; 0.1; 0.1];
% g = [0; 0; -9.8];
% M01 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0.089159]; [0, 0, 0, 1]];
% M12 = [[0, 0, 1, 0.28]; [0, 1, 0, 0.13585]; [-1, 0 ,0, 0]; [0, 0, 0, 1]];
% M23 = [[1, 0, 0, 0]; [0, 1, 0, -0.1197]; [0, 0, 1, 0.395]; [0, 0, 0, 1]];
% M34 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0.14225]; [0, 0, 0, 1]];
% G1 = diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]);
% G2 = diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]);
% G3 = diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]);
% Glist = cat(3, G1, G2, G3);
% Mlist = cat(3, M01, M12, M23, M34);
% Slist = [[1; 0; 1;      0; 1;     0], ...
%        [0; 1; 0; -0.089; 0;     0], ...
%        [0; 1; 0; -0.089; 0; 0.425]];
% grav = GravityForces(thetalist, g, Mlist, Glist, Slist)
%
% Output:
% grav =
%   28.4033
%  -37.6409
%   -5.4416

n = size(thetalist, 1);
grav = InverseDynamics(thetalist, zeros(n, 1), zeros(n, 1) ,g, ...
                       [0; 0; 0; 0; 0; 0], Mlist, Glist, Slist);
end

author: ShuoYangRobotics

repo: QuadrupedSim