%% 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.


%% Dynamic Matrix Control Tutorial
% Dynamic Matrix Control (DMC) was the first Model Predictive Control (MPC)
% algorithm introduced in early 1980s. Nowadays, DMC is available in almost
% all commercial industrial distributed control systems and process
% simulation software packages. This tutorial intends to explain the
% features of DMC using the dmc function developed by the author.

%% Example: A Water Heater
% Consider a water heater as shown in the following figure, where the cold
% water is heated by means of a gas burner. The aim of DMC is by
% manipulating valve to control the gas flow so that the outlet temperature
% is at desired level.
%%
%
% <<WaterHeater.PNG>>
%

%% The Step Response Model
% The DMC algorithm works with a step response model. The first step to
% design a DMC controller is to perform a step test on the plant to
% generate a step response model. The step response of the water heater is
% obtained through such a test and given as follows:
p.sr = [0;0;0.271;0.498;0.687;0.845;0.977;1.087;1.179;1.256;...
        1.320;1.374;1.419;1.456;1.487;1.513;1.535;1.553;1.565;1.581;...
        1.592;1.600;1.608;1.614;1.619;1.632;1.627;1.630;1.633;1.635];

% The response is converged after 30 steps.

%% DMC without Setpoint Prediction
% Setup the DMC
N=120;  % Total simulation length (samples)
p.p=10; % Prediction horizon
p.m=5;  % Moving horizon
p.la=1; % Control weight
% Reference (setpoint)
R=[ones(30,1);zeros(30,1);ones(30,1);zeros(30,1)];
p.y=0;  % Initial output
p.v=[]; % empty past input to indicate initialization
% buffer of input to cope with time delay
u=zeros(3,1);
% Initialization of variables for results
Y=zeros(N,1);
U=zeros(N,1);
% DMC Simulation
for k=1:120
    p.a=0;
    p.r=R(k);   % DMC only knows current setpoint
    if k>60     % change smoothing factor for second half simulation
        p.a=0.7;
    end
    p=dmc(p);
    Y(k)=p.y;
    U(k)=p.u;
    u=[u(2:3);p.u];
    p.y=0.8351*p.y+0.2713*u(1); % actual plant output
end
% DMC results
subplot(211)
plot(1:N,Y,'b-',1:N,R,'r--',[60 60],[-0.5 1.5],':','linewidth',2)
title('solid: output, dashed: reference')
text(35,1,'\alpha=0')
text(95,1,'\alpha=0.7')
axis([0 120 -0.5 1.5])
subplot(212)
[xx,yy]=stairs(1:N,U);
plot(xx,yy,'-',[60 60],[-0.5 1.5],':','linewidth',2)
axis([0 120 -0.5 1.5])
title('input, \lambda=1')
xlabel('time, min')

%% DMC with Setpoint Prediction
% Setup the DMC
N=120;  % Total simulation length (samples)
p.p=10; % Prediction horizon
p.m=5;  % Moving horizon
p.la=1; % Control weight
% Reference (setpoint)
R=[ones(30,1);zeros(30,1);ones(30,1);zeros(30,1)];
p.y=0;  % Initial output
p.v=[]; % empty past input to indicate initialization
% buffer of input to cope with time delay
u=zeros(3,1);
% Initialization of variables for results
Y=zeros(N,1);
U=zeros(N,1);
% DMC Simulation
for k=1:120
    p.a=0;
    p.r=R(k:min(N,k+p.p)); % DMC knows future setpoint
    if k>60     % change smoothing factor for second half simulation
        p.a=0.7;
    end
    p=dmc(p);
    Y(k)=p.y;
    U(k)=p.u;
    u=[u(2:3);p.u];
    p.y=0.8351*p.y+0.2713*u(1); % actual plant output
end
% DMC results
subplot(211)
plot(1:N,Y,'b-',1:N,R,'r--',[60 60],[-0.5 1.5],':','linewidth',2)
title('solid: output, dashed: reference')
text(35,1,'\alpha=0')
text(95,1,'\alpha=0.7')
axis([0 120 -0.5 1.5])
subplot(212)
[xx,yy]=stairs(1:N,U);
plot(xx,yy,'-',[60 60],[-0.5 1.5],':','linewidth',2)
axis([0 120 -0.5 1.5])
title('input, \lambda=1')
xlabel('time, min')

%% DMC with Different Control Weight
% Setup the DMC
N=120;  % Total simulation length (samples)
p.p=10; % Prediction horizon
p.m=5;  % Moving horizon
p.la=0.1; % Control weight
% Reference (setpoint)
R=[ones(30,1);zeros(30,1);ones(30,1);zeros(30,1)];
p.y=0;  % Initial output
p.v=[]; % empty past input to indicate initialization
% buffer of input to cope with time delay
u=zeros(3,1);
% Initialization of variables for results
Y=zeros(N,1);
U=zeros(N,1);
% DMC Simulation
for k=1:120
    p.a=0;
    p.r=R(k:min(N,k+p.p)); % DMC knows future setpoint
    if k>60     % change smoothing factor for second half simulation
        p.a=0.7;
    end
    p=dmc(p);
    Y(k)=p.y;
    U(k)=p.u;
    u=[u(2:3);p.u];
    p.y=0.8351*p.y+0.2713*u(1); % actual plant output
end
% DMC results
subplot(211)
plot(1:N,Y,'b-',1:N,R,'r--',[60 60],[-0.5 1.5],':','linewidth',2)
title('solid: output, dashed: reference')
text(35,1,'\alpha=0')
text(95,1,'\alpha=0.7')
axis([0 120 -0.5 1.5])
subplot(212)
[xx,yy]=stairs(1:N,U);
plot(xx,yy,'-',[60 60],[-0.5 1.5],':','linewidth',2)
axis([0 120 -1 2])
title('input, \lambda=0.1')
xlabel('time, min')

%% Reference
% Camacho, E.F. and Bordons, C., Model Predictive Control, Springer-Verlag,
% 1999.


function [R,L_lb,L_cpd] = rankest(T,options)
%RANKEST Estimate rank.
%   rankest(T) plots an L-curve of number of rank-one terms in a canonical
%   polyadic decomposition. The x-axis corresponds to the number of
%   rank-one terms, and the y-axis corresponds to the relative error of the
%   CPD in that many rank-one terms. Additionally, the corner R of the
%   resulting L-curve is estimated.
%
%   R = rankest(T) does not plot anything and instead returns the number of
%   rank-one terms R corresponding to the corner of the L-curve.
%
%   [R,L_lb,L_cpd] = rankest(T) also returns the L-curve ranks L_lb(:,1)
%   and L_cpd(:,1) and the corresponding lower bound on the truncation
%   error L_lb(:,2) and relative error of the CPD approximation L_cpd(:,2),
%   respectively.
%
%   rankest(T,options) may be used to set the following options:
%
%      options.MaxR =           - Maximum number of rank-one terms to try.
%      numel(T)/max(size_tens)
%      options.MinR = 1         - Minimum number of rank-one terms to try.
%      options.MinRelErr = 1e-2 - Determines an upper threshold for the
%                                 number of rank-one terms R to try.
%      options.Solver = @cpd    - The solver used to compute the CPD for
%                                 each number of rank-one terms R. Called
%                                 as options.Solver(T,R, ...
%                                 options.SolverOptions), where
%                                 options.SolverOptions is an options
%                                 structure passed to the solver.
%      options.XMultiplier = 1  - The importance of the number of rank-one
%                                 terms in determining the L-curve corner,
%                                 relative to the importance of the
%                                 relative error of the approximation.
%
%   See also mlrankest.

%   Authors: Laurent Sorber (Laurent.Sorber@cs.kuleuven.be)
%            Marc Van Barel (Marc.VanBarel@cs.kuleuven.be)
%            Lieven De Lathauwer (Lieven.DeLathauwer@kuleuven-kulak.be)
%
%   References:
%   [1] J.L. Castellanos, S. Gomez, V. Guerra, "The triangle method for
%       finding the corner of the L-curve," Applied Numerical Mathematics,
%       Vol. 43, No. 4, 2002, pp. 359-373.

% Incomplete/sparse tensors not supported yet.
T = fmt(T,true);
if isstruct(T), size_tens = T.size;
else size_tens = size(T); end

% Check the options structure.
if nargin < 2, options = struct; end
if ~isfield(options,'MaxR')
    options.MaxR = prod(size_tens)/max(size_tens);
end
if ~isfield(options,'MinR'), options.MinR = 1; end
if ~isfield(options,'MinRelErr'), options.MinRelErr = 1e-2; end
if ~isfield(options,'Solver'), options.Solver = @cpd; end
if ~isfield(options,'SolverOptions'), options.SolverOptions = struct; end
if ~isfield(options.SolverOptions,'Compression')
    options.SolverOptions.Compression = false;
end
if ~isfield(options.SolverOptions,'Display')
    options.SolverOptions.Display = false;
end
if ~isfield(options,'XMultiplier'), options.XMultiplier = 1; end

% Compute lower bound on truncation error.
frobT = frob(T);
if ~isstruct(T)
    [~,~,sv] = mlsvd(T);
    Rmax = max(cellfun(@length,sv))-1;
    lb = max(cell2mat(cellfun( ...
             @(s)[sqrt(flipud(cumsum(flipud(s(:).^2)))).'/frobT ...
             zeros(1,Rmax+1-length(s))],sv(:),'UniformOutput',false)));
    lb = lb(2:end);
else
    Rmax = 0;
    lb = [];
end

% Determine the range of rank-one terms to test.
if ~isempty(lb)
    R = find(lb(options.MinR:end)<=options.MinRelErr,1,'first')+ ...
        options.MinR-1;
    if isempty(R), R = max(options.MinR,Rmax+1); end
    R = R:options.MaxR;
else
    R = 1:options.MaxR;
end

% Compute the relative error for each number of rank-one terms R(r).
relerr = zeros(1,length(R));
for r = 1:length(R)

    % Compute the CPD in R rank-one terms.
    [U,output] = options.Solver(T,R(r),options.SolverOptions);
    if isfield(output,'Refinement') && ...
       isfield(output.Refinement,'fval')
        relerr(r) = sqrt(2*output.Refinement.fval(end))/frobT;
    elseif isfield(output,'Algorithm') && ...
           isfield(output.Algorithm,'fval')
        relerr(r) = sqrt(2*output.Algorithm.fval(end))/frobT;
    elseif isfield(output,'fval')
        relerr(r) = sqrt(2*output.fval(end))/frobT;
    else
        relerr(r) = frob(cpdres(T,U))/frobT;
    end

    % Stop is options.MinRelErr has been reached.
    if relerr(r) <= options.MinRelErr
        R = R(1:r);
        relerr = relerr(1:r);
        break;
    end

    % Update the plot.
    if r > 1 && r < length(R) && nargout == 0, Lcurve(true); end

end

% Compute L-curve corner using the triangle method [1].
logx = options.XMultiplier*R(:);
logy = log10(relerr(:));
if R(1) ~= 1 && ~isempty(lb)
    logx = [options.XMultiplier;logx];
    logy = [log10(lb(1));logy];
end
ab = cat(3,bsxfun(@minus,logx,logx.'),bsxfun(@minus,logy,logy.'));
ac = cat(3,logx(end)-logx.',logy(end)-logy.');
area = bsxfun(@times,ab(:,:,1),ac(:,:,2)) - ...
       bsxfun(@times,ab(:,:,2),ac(:,:,1));
cosa = bsxfun(@times,ab(:,:,1),ac(:,:,1)) + ...
       bsxfun(@times,ab(:,:,2),ac(:,:,2));
cosa = bsxfun(@rdivide,cosa./sqrt(ab(:,:,1).^2+ab(:,:,2).^2), ...
                       sqrt(ac(:,:,1).^2+ac(:,:,2).^2));
cosa(area >= 0 | tril(true(size(cosa))) | cosa <= cos(7*pi/8)) = -1;
[a,opt] = max(max(cosa(:,2:end)));
if isempty(a), opt = 1; end
if a == -1, opt = size(cosa,2)-1; end

% Display output.
L_lb = [(1:Rmax).' lb(:)];
L_cpd = [R(:) relerr(:)];
if nargout == 0, Lcurve(false); end
R = R(opt);

function Lcurve(update)
    if R(r) <= 1, return; end
    style = {'Marker','+','MarkerSize',2.5};
    if ~update
        semilogy(R(opt),relerr(opt),'rs','LineStyle','none'); hold on;
    end
    semilogy(R(1:length(relerr)),relerr,'r',style{:}); hold on;
    semilogy(1:length(lb),lb,style{:});
    semilogy([1 R(r)],options.MinRelErr*[1 1],'k:');
    text(1,options.MinRelErr,'MinRelErr','VerticalAlignment','Top');
    hold off;
    ylim([min(options.MinRelErr/(10^0.5),min(relerr(1:r))) lb(1)]);
    xlim([1 R(r)]);
    ylabel('frob(cpdres(T,U))/frob(T)'); xlabel('R');
    xt = get(gca,'XTick'); set(gca,'XTick',xt(mod(xt,1) == 0));
    if update
        if ~isempty(lb)
            legend(['CPD error (trying R = ' int2str(R(r+1)) '...)'], ...
                'Lower bound on error','Location','NE');
        else
            legend(['CPD error (trying R = ' int2str(R(r+1)) '...)'], ...
                'Location','NE');
        end
    else
        if ~isempty(lb)
            legend(['L-curve corner at R = ' int2str(R(opt))], ...
                'CPD error','Lower bound on error','Location','NE');
        else
            legend(['L-curve corner at R = ' int2str(R(opt))], ...
                'CPD error','Location','NE');
        end
    end
    set(datacursormode(gcf),'UpdateFcn',@datacursor);
    drawnow;
end

function txt = datacursor(~,event_obj)
    pos = get(event_obj,'Position');
    txt = {['R: ' int2str(pos(1))], ...
           ['relative error: ' num2str(pos(2))]};
end

end


function Serr=specerr(S,J,err,trialave,numsp)
% Function to compute lower and upper confidence intervals on the spectrum
% Usage: Serr=specerr(S,J,err,trialave,numsp)
% Outputs: Serr (Serr(1,...) - lower confidence level, Serr(2,...) upper confidence level)
%
% Inputs:
% S - spectrum
% J - tapered fourier transforms
% err - [errtype p] (errtype=1 - asymptotic estimates; errchk=2 - Jackknife estimates;
%                   p - p value for error estimates)
% trialave - 0: no averaging over trials/channels
%            1 : perform trial averaging
% numsp    - number of spikes in each channel. specify only when finite
%            size correction required (and of course, only for point
%            process data)
%
% Outputs:
% Serr - error estimates. Only for err(1)>=1. If err=[1 p] or [2 p] Serr(...,1) and Serr(...,2)
% contain the lower and upper error bars with the specified method.
if nargin < 4; error('Need at least 4 input arguments'); end;
if err(1)==0; error('Need err=[1 p] or [2 p] for error bar calculation. Make sure you are not asking for the output of Serr'); end;
[nf,K,C]=size(J);
errchk=err(1);
p=err(2);
pp=1-p/2;
qq=1-pp;

if trialave
   dim=K*C;
   C=1;
   dof=2*dim;
   if nargin==5; dof = fix(1/(1/dof + 1/(2*sum(numsp)))); end
   J=reshape(J,nf,dim);
else
   dim=K;
   dof=2*dim*ones(1,C);
   for ch=1:C;
     if nargin==5; dof(ch) = fix(1/(1/dof + 1/(2*numsp(ch)))); end
   end;
end;
Serr=zeros(2,nf,C);
if errchk==1;
   Qp=chi2inv(pp,dof);
   Qq=chi2inv(qq,dof);
   Serr(1,:,:)=dof(ones(nf,1),:).*S./Qp(ones(nf,1),:);
   Serr(2,:,:)=dof(ones(nf,1),:).*S./Qq(ones(nf,1),:);
elseif errchk==2;
   tcrit=tinv(pp,dim-1);
   for k=1:dim;
       indices=setdiff(1:dim,k);
       Jjk=J(:,indices,:); % 1-drop projection
       eJjk=squeeze(sum(Jjk.*conj(Jjk),2));
       Sjk(k,:,:)=eJjk/(dim-1); % 1-drop spectrum
   end;
   sigma=sqrt(dim-1)*squeeze(std(log(Sjk),1,1)); if C==1; sigma=sigma'; end;
   conf=repmat(tcrit,nf,C).*sigma;
   conf=squeeze(conf);
   Serr(1,:,:)=S.*exp(-conf); Serr(2,:,:)=S.*exp(conf);
end;
Serr=squeeze(Serr);


function ccl_sparse_test ( )

%*****************************************************************************80
%
%% CCL_SPARSE_TEST uses CCL to build a sparse grid.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 April 2014
%
%  Author:
%
%    John Burkardt
%
%  Local parameters:
%
%    Local, integer D, the spatial dimension.
%
%    Local, integer MAXK, the maximum level to check.
%
  d = 10;
  maxk = 7;
  func = 'prod( exp(-(x/2).^2/2)/2/sqrt(2*pi), 2)';
  trueval = fu_integral ( d );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'CCL_SPARSE_TEST:\n' );
  fprintf ( 1, '  CCL sparse grid:\n' );
  fprintf ( 1, '  Clenshaw-Curtis Linear sparse grid.\n' );
  fprintf ( 1, '  Exact integral is %g\n', trueval );
  fprintf ( 1, '\n' );
  fprintf ( 1, '   D  Level   Nodes    SG error    MC error\n' );
  fprintf ( 1, '\n' );

  for k = 1 : maxk
%
%  Compute the sparse grid estimate.
%
    [ x, w ] = nwspgr ( 'ccl', d, k );
    fx = eval ( func );
    SGappr = w' * fx;
    SGerror = sqrt ( ( SGappr - trueval ).^2 ) / trueval;
%
%  Average 1000 Monte Carlo estimates.
%
    numnodes = length ( w );
    sim = zeros ( 1000, 1 );
    for r = 1 : 1000
      x = rand ( numnodes, d );
      fx = eval ( func );
      sim(r) = mean ( fx );
    end
    simerror = sqrt ( mean ( ( sim - trueval ).^2 ) ) / trueval;

    fprintf( '  %2d     %2d  %6d  %10.5g  %10.5g\n', ...
      d, k, numnodes, SGerror, simerror )

  end

  return
end


function a = gk323_inverse ( n )

%*****************************************************************************80
%
%% GK323_INVERSE returns the inverse of the GK323 matrix.
%
%  Properties:
%
%    A is symmetric: A' = A.
%
%    Because A is symmetric, it is normal.
%
%    Because A is normal, it is diagonalizable.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of A.
%
%    Output, real A(N,N), the matrix.
%
  a = zeros ( n, n );

  for i = 1 : n
    for j = 1 : n

      if ( i == j )
        if ( i == 1 | i == n )
          a(i,j) = - 0.5 * ( n - 2 ) / ( n - 1 );
        else
          a(i,j) = - 1.0;
        end
      elseif ( i == j+1 | i == j-1 )
        a(i,j) = 0.5;
      elseif ( i == 1 & j == n )
        a(i,j) = 0.5 / ( n - 1 );
      elseif ( i == n & j == 1 )
        a(i,j) = 0.5 / ( n - 1 );
      else
        a(i,j) = 0.0;
      end

    end
  end

  return
end


function [zPred,PzPred,otherInfo]=sqrtKalmanMeasPred(xPred,SPred,H)
%%SQRTKALMANMEASPRED Perform the measurement prediction part of the
%           measurement update step of the square-root Kalman filter. The
%           function sqrtKalmanUpdateWithPred can be used to complete the
%           measurement update. Separating the measurement prediction step
%           from the rest of the update step can make the creation of
%           multiple measurement association hypotheses from a single
%           target prediction more efficient. The full measurement update
%           function is sqrtKalmanUpdate.
%
%INPUTS: xPred The xDimXnumComp set of numComp predicted target states.
%        SPred The xDimXxDimXnumComp lower-triangular predicted square-root
%              state covariance matrices.
%            H The zDimXxDim measurement matrix. The measurement is modeled
%              as z=H*x+noise and the model is the same for all components
%              given in xPred and SPred.
%
%OUTPUTS: zPred The zDimXnumComp measurement predictions from the filter.
%        PzPred The zDimXzDimXnumComp covariance matrix associated with
%               zPred.
%     otherInfo A structure containing members of intermediate results of
%               this function that can be passed to sqrtKalmanUpdate
%               when updating with a measurement.
%
%The mathematics behind the specific square root implementation used here
%are described in Appendix G of [1].
%
%EXAMPLE:
%With this example, we demonstrate that one gets the same result using
%KalmanUpdate versus calling sqrtKalmanMeasPred and then
%sqrtKalmanUpdateWithPred.
% xPred=[1e3;-2e3;100;200];
% SPred=chol([28,   3.5,    6,  8.5;
%            3.5,    23,  8.5,   11;
%              6,   8.5,   18, 13.5;
%            8.5,    11, 13.5,   13],'lower');
% z=1e3*[-5.498856156296510;
%        1.199241491470584];
% SR=eye(2);
% H=[0, 4, 9, 8;
%    6, 3, 0, 6];
%
% %The update in one step.
% [xUpdate,SUpdate,innov,Szz,W]=sqrtKalmanUpdate(xPred,SPred,z,SR,H);
% %The update in two steps.
% [zPred,PzPred,otherInfo]=sqrtKalmanMeasPred(xPred,SPred,H);
% [xUpdate1,SUpdate1,innov1,Szz1,W1]=sqrtKalmanUpdateWithPred(z,SR,zPred,otherInfo);
% %One will see that the one and two step updates agree.
% max(abs([xUpdate1-xUpdate;SUpdate1(:)-SUpdate(:);innov1(:)-innov;Szz1(:)-Szz(:);W1(:)-W(:)]))
%
%REFERENCES
%[1] David F. Crouse , "Basic tracking using nonlinear 3D monostatic and
%    bistatic measurements," IEEE Aerospace and Electronic Systems
%    Magazine, vol. 29, no. 8, Part II, pp. 4-53, Aug. 2014.
%
%June 2018 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

zDim=size(H,1);
xDim=size(H,2);
numComp=size(xPred,2);

zPred=H*xPred;

PzPred=zeros(zDim,zDim,numComp);
Pxz=zeros(xDim,zDim,numComp);
for k=1:numComp
    temp=H*SPred(:,:,k);
    PzPred(:,:,k)=temp*temp';

    Pxz(:,:,k)=SPred(:,:,k)*SPred(:,:,k)'*H';
    %Pxz is not needed for the measurement prediction, but we compute it
    %here, so that it need not be recomputed again and again if
    %sqrtKalmanUpdateWithPred is called for multiple measurements.
end
otherInfo.xPred=xPred;
otherInfo.SPred=SPred;
otherInfo.Pxz=Pxz;
otherInfo.H=H;

end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


function ns3de_test02 ( )

%*****************************************************************************80
%
%% NS3DE_TEST02 samples the residual at the initial time.
%
%  Location:
%
%    http://people.sc.fsu.edu/~jburkardt/m_src/navier_stokes_3d_exact/ns3de_test02.m
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 January 2015
%
%  Author:
%
%    John Burkardt
%
  a = pi / 4.0;
  d = pi / 2.0;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'NS3DE_TEST02\n' );
  fprintf ( 1, '  Sample the Navier-Stokes residuals' );
  fprintf ( 1, '  at the initial time T = 0, using a region that is \n' );
  fprintf ( 1, '  the cube centered at (0,0,0) with "radius" 1.0,\n' );
  fprintf ( 1, '  Parameter A = %g\n', a );
  fprintf ( 1, '  Parameter D = %g\n', d );

  n = 1000;
  x = 2.0 * rand ( n, 1 ) - 1.0;
  y = 2.0 * rand ( n, 1 ) - 1.0;
  z = 2.0 * rand ( n, 1 ) - 1.0;
  t = 0.0;

  [ ur, vr, wr, pr ] = resid_ethier ( a, d, n, x, y, z, t );

  fprintf ( 1, '\n' );
  fprintf ( 1, '           Minimum       Maximum\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Ur:  %14.6g  %14.6g\n', min ( abs ( ur ) ), max ( abs ( ur ) ) );
  fprintf ( 1, '  Vr:  %14.6g  %14.6g\n', min ( abs ( vr ) ), max ( abs ( vr ) ) );
  fprintf ( 1, '  Wr:  %14.6g  %14.6g\n', min ( abs ( wr ) ), max ( abs ( wr ) ) );
  fprintf ( 1, '  Pr:  %14.6g  %14.6g\n', min ( abs ( pr ) ), max ( abs ( pr ) ) );

  return
end


function geometry_test036 ( )

%*****************************************************************************80
%
%% TEST036 tests SEGMENT_CONTAINS_POINT_1D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  ntest = 4;

  ptest =  [ 3.0,   7.5, 20.0,  5.0 ];
  p1test = [ 2.0,  10.0,  8.0, 88.0 ];
  p2test = [ 6.0, -10.0, 10.0, 88.0 ];

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST036\n' );
  fprintf ( 1, '  SEGMENT_CONTAINS_POINT_1D determines if a point\n' );
  fprintf ( 1, '    lies within a line segment in 1D.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '       P1     P       T\n' );
  fprintf ( 1, '\n' );

  for itest = 1 : ntest

    p1 = p1test(itest);
    p2 = p2test(itest);
    p = ptest(itest);

    t = segment_contains_point_1d ( p1, p2, p );
    fprintf ( 1, '  %10f  %10f  %10f  %12f\n', p1, p2, p, t );

  end

  return
end


% solve_chol - solve linear equations from the Cholesky factorization.
% Solve A*X = B for X, where A is square, symmetric, positive definite. The
% input to the function is R the Cholesky decomposition of A and the matrix B.
% Example: X = solve_chol(chol(A),B);
%
% NOTE: The program code is written in the C language for efficiency and is
% contained in the file solve_chol.c, and should be compiled using matlabs mex
% facility. However, this file also contains a (less efficient) matlab
% implementation, supplied only as a help to people unfamiliar with mex. If
% the C code has been properly compiled and is available, it automatically
% takes precendence over the matlab code in this file.
%
% Copyright (c) by Carl Edward Rasmussen and Hannes Nickisch 2010-09-18.

function X = solve_chol(L, B)

if nargin ~= 2 || nargout > 1
  error('Wrong number of arguments.');
end

if size(L,1) ~= size(L,2) || size(L,1) ~= size(B,1)
  error('Wrong sizes of matrix arguments.');
end

X = L\(L'\B);


function [h0, h1] = dfilters(fname, type)
% DFILTERS	Generate directional 2D filters
%
%	[h0, h1] = dfilters(fname, type)
%
% Input:
%	fname:	Filter name.  Available 'fname' are:
%		'haar':		the "Haar" filters
%		'vk':		McClellan transformed of the filter from
%				    the VK book
%		'ko':		orthogonal filter in the Kovacevic's paper
%		'kos':		smooth 'ko' filter
%		'lax':		17 x 17 by Lu, Antoniou and Xu
%		'sk':		9 x 9 by Shah and Kalker
%		'cd':		7 and 9 McClellan transformed by
%				                       Cohen and Daubechies
%		'pkva':		ladder filters by Phong et al.
%		'oqf_362':	regular 3 x 6 filter
%       'dvmlp'  :  regular linear phase biorthogonal filter with 3 dvm
%		'sinc':		ideal filter (*NO perfect recontruction*)
%       'dmaxflat': diamond maxflat filters obtained from a three stage ladder
%
%	     type:	'd' or 'r' for decomposition or reconstruction filters
%
% Output:
%	h0, h1:	diamond filter pair (lowpass and highpass)

% To test those filters (for the PR condition for the FIR case), verify that:
% conv2(h0, modulate2(h1, 'b')) + conv2(modulate2(h0, 'b'), h1) = 2
% (replace + with - for even size filters)
%
% To test for orthogonal filter
% conv2(h, reverse2(h)) + modulate2(conv2(h, reverse2(h)), 'b') = 2

% The diamond-shaped filter pair
switch fname
    case {'haar'}
	if lower(type(1)) == 'd'
	    h0 = [1, 1] / sqrt(2);
	    h1 = [-1, 1] / sqrt(2);
	else
	    h0 = [1, 1] / sqrt(2);
	    h1 = [1, -1] / sqrt(2);
	end

    case 'vk'	% in Vetterli and Kovacevic book
	if lower(type(1)) == 'd'
	    h0 = [1, 2, 1] / 4;
	    h1 = [-1, -2, 6, -2, -1] / 4;
	else
	    h0 = [-1, 2, 6, 2, -1] / 4;
	    h1 = [-1, 2, -1] / 4;
	end

	% McClellan transfrom to obtain 2D diamond filters
	t = [0, 1, 0; 1, 0, 1; 0, 1, 0] / 4;	% diamond kernel
	h0 = ftrans2(h0, t);
	h1 = ftrans2(h1, t);

    case 'ko'	% orthogonal filters in Kovacevic's thesis
	a0 = 2; a1 = 0.5; a2 = 1;

	h0 = [0,      -a1, -a0*a1, 0;
	      -a2, -a0*a2, -a0,    1;
	      0, a0*a1*a2, -a1*a2, 0];

	% h1 = qmf2(h0);
	h1 = [0, -a1*a2, -a0*a1*a2, 0;
	      1,     a0, -a0*a2, a2;
	      0, -a0*a1,  a1, 0];

	% Normalize filter sum and norm;
	norm = sqrt(2) / sum(h0(:));

	h0 = h0 * norm;
	h1 = h1 * norm;

	if type == 'r'
	    % Reverse filters for reconstruction
	    h0 = h0(end:-1:1, end:-1:1);
	    h1 = h1(end:-1:1, end:-1:1);
	end

    case 'kos'	% Smooth orthogonal filters in Kovacevic's thesis
	a0 = -sqrt(3); a1 = -sqrt(3); a2 = 2+sqrt(3);

	h0 = [0,      -a1, -a0*a1, 0;
	      -a2, -a0*a2, -a0,    1;
	      0, a0*a1*a2, -a1*a2, 0];

	% h1 = qmf2(h0);
	h1 = [0, -a1*a2, -a0*a1*a2, 0;
	      1,     a0, -a0*a2, a2;
	      0, -a0*a1,  a1, 0];

	% Normalize filter sum and norm;
	norm = sqrt(2) / sum(h0(:));

	h0 = h0 * norm;
	h1 = h1 * norm;

	if type == 'r'
	    % Reverse filters for reconstruction
	    h0 = h0(end:-1:1, end:-1:1);
	    h1 = h1(end:-1:1, end:-1:1);
	end

    case 'lax'	% by Lu, Antoniou and Xu
	h = [-1.2972901e-5,  1.2316237e-4, -7.5212207e-5,  6.3686104e-5, ...
	      9.4800610e-5, -7.5862919e-5,  2.9586164e-4, -1.8430337e-4; ...

	      1.2355540e-4, -1.2780882e-4, -1.9663685e-5, -4.5956538e-5, ...
	     -6.5195193e-4, -2.4722942e-4, -2.1538331e-5, -7.0882131e-4; ...

	     -7.5319075e-5, -1.9350810e-5, -7.1947086e-4,  1.2295412e-3, ...
	      5.7411214e-4,  4.4705422e-4,  1.9623554e-3,  3.3596717e-4; ...

	      6.3400249e-5, -2.4947178e-4,  4.4905711e-4, -4.1053629e-3, ...
	     -2.8588307e-3,  4.3782726e-3, -3.1690509e-3, -3.4371484e-3; ...

	      9.6404973e-5, -4.6116254e-5,  1.2371871e-3, -1.1675575e-2, ...
	      1.6173911e-2, -4.1197559e-3,  4.4911165e-3,  1.1635130e-2; ...

	     -7.6955555e-5, -6.5618379e-4,  5.7752252e-4,  1.6211426e-2, ...
	      2.1310378e-2, -2.8712621e-3, -4.8422645e-2, -5.9246338e-3; ...

	      2.9802986e-4, -2.1365364e-5,  1.9701350e-3,  4.5047673e-3, ...
	     -4.8489158e-2, -3.1809526e-3, -2.9406153e-2,  1.8993868e-1; ...

	     -1.8556637e-4, -7.1279432e-4,  3.3839195e-4,  1.1662001e-2, ...
	     -5.9398223e-3, -3.4467920e-3,  1.9006499e-1,  5.7235228e-1];

	h0 = sqrt(2) * [h, h(:, end-1:-1:1); ...
			h(end-1:-1:1, :), h(end-1:-1:1, end-1:-1:1)];

	h1 = modulate2(h0, 'b');

    case 'sk'	% by Shah and Kalker
	h = [ 0.621729,    0.161889,  -0.0126949, -0.00542504, 0.00124838; ...
	      0.161889,   -0.0353769, -0.0162751, -0.00499353, 0; ...
	     -0.0126949,  -0.0162751,  0.00749029, 0, 0; ...
	     -0.00542504,  0.00499353, 0, 0, 0; ...
	      0.00124838, 0, 0, 0, 0];

	h0 = sqrt(2) * [h(end:-1:2, end:-1:2), h(end:-1:2, :); ...
			h(:, end:-1:2), h];

	h1 = modulate2(h0, 'b');

    case 'dvmlp'
    q= sqrt(2); b=.02; b1 = b*b;
    h  = [b/q 0 -2*q*b 0 3*q*b 0 -2*q*b 0 b/q;
          0 -1/(16*q) 0 9/(16*q) 1/q  9/(16*q) 0 -1/(16*q) 0;
          b/q 0 -2*q*b 0 3*q*b 0 -2*q*b 0 b/q];
    g0 = [-b1/q   0   4*b1*q   0    -14*q*b1    0   28*q*b1    0       -35*q*b1  0    28*q*b1    0     -14*q*b1   0    4*b1*q  0     -b1/q;
           0     b/(8*q)   0  -13*b/(8*q) b/q  33*b/(8*q) -2*q*b -21*b/(8*q) 3*q*b -21*b/(8*q)   -2*q*b 33*b/(8*q)  b/q -13*b/(8*q) 0  b/(8*q) 0;
          -q*b1  0  -1/(256*q) + 8*q*b1  0   9/(128*q) - 28*q*b1   -1/(q*16)   -63/(256*q) + 56*q*b1   9/(16*q)   87/(64*q)-70*q*b1      9/(16*q)   -63/(256*q) + 56*q*b1    -1/(q*16)   9/(128*q) - 28*q*b1   0   -1/(256*q) + 8*q*b1   0   -q*b1;
          0     b/(8*q)   0  -13*b/(8*q) b/q  33*b/(8*q) -2*q*b -21*b/(8*q) 3*q*b -21*b/(8*q)   -2*q*b 33*b/(8*q)  b/q -13*b/(8*q) 0  b/(8*q) 0;
          -b1/q   0   4*b1*q   0    -14*q*b1    0   28*q*b1    0       -35*q*b1  0    28*q*b1    0     -14*q*b1   0    4*b1*q  0     -b1/q];
    h1 = modulate2(g0,'b' );
    h0 = h;
    if lower(type(1)) == 'r'
        h1 = modulate2(h ,'b');
        h0 = g0;
    end

    case {'cd', '7-9'}	% by Cohen and Daubechies
	% 1D prototype filters: the '7-9' pair
	h0 = [0.026748757411, -0.016864118443, -0.078223266529, ...
	      0.266864118443, 0.602949018236, 0.266864118443, ...
	      -0.078223266529, -0.016864118443, 0.026748757411];
	g0 = [-0.045635881557, -0.028771763114, 0.295635881557, ...
	      0.557543526229, 0.295635881557, -0.028771763114, ...
	      -0.045635881557];

	if lower(type(1)) == 'd'
	    h1 = modulate2(g0, 'c');
	else
	    h1 = modulate2(h0, 'c');
	    h0 = g0;
	end

	% Use McClellan to obtain 2D filters
	t = [0, 1, 0; 1, 0, 1; 0, 1, 0] / 4;	% diamond kernel
	h0 = sqrt(2) * ftrans2(h0, t);
	h1 = sqrt(2) * ftrans2(h1, t);

    case {'pkva', 'ldtest'}	% Filters from the ladder structure
	% Allpass filter for the ladder structure network
	beta = ldfilter(fname);

	% Analysis filters
	[h0, h1] = ld2quin(beta);

	% Normalize norm
	h0 = sqrt(2) * h0;
	h1 = sqrt(2) * h1;

	% Synthesis filters
	if lower(type(1)) == 'r'
	    f0 = modulate2(h1, 'b');
	    f1 = modulate2(h0, 'b');

	    h0 = f0;
	    h1 = f1;
	end

 case {'pkva-half4'}	% Filters from the ladder structure
	% Allpass filter for the ladder structure network
	beta = ldfilterhalf( 4);

	% Analysis filters
	[h0, h1] = ld2quin(beta);

	% Normalize norm
	h0 = sqrt(2) * h0;
	h1 = sqrt(2) * h1;

	% Synthesis filters
	if lower(type(1)) == 'r'
	    f0 = modulate2(h1, 'b');
	    f1 = modulate2(h0, 'b');

	    h0 = f0;
	    h1 = f1;
	end

    case {'pkva-half6'}	% Filters from the ladder structure
	% Allpass filter for the ladder structure network
	beta = ldfilterhalf( 6);

	% Analysis filters
	[h0, h1] = ld2quin(beta);

	% Normalize norm
	h0 = sqrt(2) * h0;
	h1 = sqrt(2) * h1;

	% Synthesis filters
	if lower(type(1)) == 'r'
	    f0 = modulate2(h1, 'b');
	    f1 = modulate2(h0, 'b');

	    h0 = f0;
	    h1 = f1;
	end

    case {'pkva-half8'}	% Filters from the ladder structure
	% Allpass filter for the ladder structure network
	beta = ldfilterhalf( 8);

	% Analysis filters
	[h0, h1] = ld2quin(beta);

	% Normalize norm
	h0 = sqrt(2) * h0;
	h1 = sqrt(2) * h1;

	% Synthesis filters
	if lower(type(1)) == 'r'
	    f0 = modulate2(h1, 'b');
	    f1 = modulate2(h0, 'b');

	    h0 = f0;
	    h1 = f1;
	end

    case 'sinc'	% The "sinc" case, NO Perfect Reconstruction
	% Ideal low and high pass filters
	flength = 30;

	h0 = fir1(flength, 0.5);
	h1 = modulate2(h0, 'c');

	% Use McClellan to obtain 2D filters
	t = [0, 1, 0; 1, 0, 1; 0, 1, 0] / 4;	% diamond kernel
	h0 = sqrt(2) * ftrans2(h0, t);
	h1 = sqrt(2) * ftrans2(h1, t);

    case {'oqf_362'}	% Some "home-made" filters!
        h0 = sqrt(2) / 64 * ...
             [	sqrt(15),	-3,	0; ...
                 0,		5,	-sqrt(15); ...
                 -2*sqrt(15),	30,	0; ...
                 0,		30,	2*sqrt(15); ...
                 sqrt(15),	5, 	0; ...
                 0, 		-3,	-sqrt(15)]';

         h1 = -reverse2(modulate2(h0, 'b'));

	if type == 'r'
	    % Reverse filters for reconstruction
	    h0 = h0(end:-1:1, end:-1:1);
	    h1 = h1(end:-1:1, end:-1:1);
	end

    case {'test'}	% Only for the shape, not for PR
	h0 = [0, 1, 0; 1, 4, 1; 0, 1, 0];
	h1 = [0, -1, 0; -1, 4, -1; 0, -1, 0];

    case {'testDVM'}	% Only for directional vanishing moment
	h0 = [1, 1; 1, 1] / sqrt(2);
	h1 = [-1, 1; 1, -1] / sqrt(2);

    case 'qmf'	% by Lu, Antoniou and Xu
	% ideal response
    % window
    m=2;
    n=2;
    w=[];
    w1d=kaiser(4*m+1,2.6);
    for n1=-m:m
        for n2 = -n:n
            w(n1+m+1,n2+n+1) = w1d(2*m+n1+n2+1)*w1d(2*m+n1-n2+1) ;
        end
    end
    h=[];
    for n1=-m:m
        for n2 = -n:n
            h(n1+m+1,n2+n+1) =  .5*sinc((n1+n2)/2)*.5*sinc((n1-n2)/2);
        end
    end
    c=sum(sum(h));
    h = sqrt(2)*h/c;
    h0=h.*w;
    h1 = modulate2(h0,'b');

    %h0 = modulate2(h,'r');
    %h1 = modulate2(h,'b');

     case 'qmf2'	% by Lu, Antoniou and Xu
	% ideal response
    % window

    h=[-.001104 .002494 -0.001744 0.004895 -0.000048 -.000311;
    0.008918 -0.002844 -0.025197 -0.017135 0.003905 -0.000081;
    -0.007587 -0.065904 00.100431 -0.055878 0.007023 0.001504;
    0.001725 0.184162 0.632115 0.099414 -0.027006 -0.001110;
    -0.017935 -0.000491 0.191397 -0.001787 -0.010587 0.002060;
    .001353 0.005635 -0.001231 -0.009052 -0.002668 0.000596];
    h0 = h./sum(sum(h));
    h1 = modulate2(h0,'b');

    %h0 = modulate2(h,'r');
    %h1 = modulate2(h,'b');

   case 'dmaxflat4'
       M1 = 1/sqrt(2); M2=M1;
       k1=1-sqrt(2);k3=k1;
       k2 = M1;
       h  = [.25*k2*k3 .5*k2 1+.5*k2*k3]*M1; h = [h fliplr(h(1:end-1))];
       g  = [-.125*k1*k2*k3 0.25*k1*k2 (-0.5*k1-0.5*k3-0.375*k1*k2*k3) 1+ .5*k1*k2]*M2;
       g = [g fliplr(g(1:end-1))];
       B  = dmaxflat(4,0);
       h0 = mctrans(h,B);
       g0 = mctrans(g,B);
       h0 = sqrt(2)*(h0./sum(h0(:)));
       g0 = sqrt(2)*(g0./sum(g0(:)));

       h1 = modulate2(g0,'b' );
    if lower(type(1)) == 'r'
       h1 = modulate2(h0 ,'b');
       h0 = g0;
    end
   case 'dmaxflat5'
       M1 = 1/sqrt(2); M2=M1;
       k1=1-sqrt(2);k3=k1;
       k2 = M1;
       h  = [.25*k2*k3 .5*k2 1+.5*k2*k3]*M1; h = [h fliplr(h(1:end-1))];
       g  = [-.125*k1*k2*k3 0.25*k1*k2 (-0.5*k1-0.5*k3-0.375*k1*k2*k3) 1+ .5*k1*k2]*M2;
       g = [g fliplr(g(1:end-1))];
       B  = dmaxflat(5,0);
       h0 = mctrans(h,B);
       g0 = mctrans(g,B);
       h0 = sqrt(2)*(h0./sum(h0(:)));
       g0 = sqrt(2)*(g0./sum(g0(:)));

       h1 = modulate2(g0,'b' );
    if lower(type(1)) == 'r'
       h1 = modulate2(h0 ,'b');
       h0 = g0;
    end

   case 'dmaxflat6'
       M1 = 1/sqrt(2); M2=M1;
       k1=1-sqrt(2);k3=k1;
       k2 = M1;
       h  = [.25*k2*k3 .5*k2 1+.5*k2*k3]*M1; h = [h fliplr(h(1:end-1))];
       g  = [-.125*k1*k2*k3 0.25*k1*k2 (-0.5*k1-0.5*k3-0.375*k1*k2*k3) 1+ .5*k1*k2]*M2;
       g = [g fliplr(g(1:end-1))];
       B  = dmaxflat(6,0);
       h0 = mctrans(h,B);
       g0 = mctrans(g,B);
       h0 = sqrt(2)*(h0./sum(h0(:)));
       g0 = sqrt(2)*(g0./sum(g0(:)));

       h1 = modulate2(g0,'b' );
    if lower(type(1)) == 'r'
       h1 = modulate2(h0 ,'b');
       h0 = g0;
    end

   case 'dmaxflat7'
       M1 = 1/sqrt(2); M2=M1;
       k1=1-sqrt(2);k3=k1;
       k2 = M1;
       h  = [.25*k2*k3 .5*k2 1+.5*k2*k3]*M1; h = [h fliplr(h(1:end-1))];
       g  = [-.125*k1*k2*k3 0.25*k1*k2 (-0.5*k1-0.5*k3-0.375*k1*k2*k3) 1+ .5*k1*k2]*M2;
       g = [g fliplr(g(1:end-1))];
       B  = dmaxflat(7,0);
       h0 = mctrans(h,B);
       g0 = mctrans(g,B);
       h0 = sqrt(2)*(h0./sum(h0(:)));
       g0 = sqrt(2)*(g0./sum(g0(:)));

       h1 = modulate2(g0,'b' );
    if lower(type(1)) == 'r'
       h1 = modulate2(h0 ,'b');
       h0 = g0;
    end

    otherwise
	% Assume the "degenerated" case: 1D wavelet filters
	[h0, h1] = wfilters(fname, type);
end


function ber = get_ber(V_equal, W_equal)

global Ns Nsym Nr Vn H hMod hDemod;
%one channel realization for Nsym times data streams transmission
% 2 is for real and imaginary
data = randi([0 1],Nsym*Ns*2,1);

%QPSK modulation generate all original signals s in Nsym times
s = reshape(step(hMod,data),Ns, Nsym);

%generate noise vector u
u = sqrt(Vn/2).*(randn(Nr,Nsym)+1i*randn(Nr,Nsym));

%get the receive vector
%colloct Nsym receive vector in the r matrix, where nth column is a
%receive vector at n time
r = W_equal' * H * V_equal * s + W_equal' * u;

%QPSK demodulation
de_data = step(hDemod, r(:));
%get the number of error bits
ber = biterr(data,de_data);


%Combine forward and backward solutions

%The input arguments is highly dependent on the implementation. In the future I
%will probably change script with some other implementation independent
%version.

%dzBck=PbckInv*dx_bckwrd
function [P_smt, Cen_smt, h_smt, Vn_smt, qbn_smt, imu_smt, dxDif]=combine_2filt(fwdnav, fwdcov, Cen_bck, h_bck, Vn_bck, qbn_bck, dzBck, PbckInv)

%Forward filter results
Cen_fwd=reshape(fwdnav(1:9),[3,3]);
h_fwd=fwdnav(10); %elipsoid height
Vn_fwd=fwdnav(11:13);
qbn_fwd=fwdnav(14:17);
imu_fwd=fwdnav(18:end);

Pfwd=mat2vec_v000(fwdcov);
%%IMPORTATNT::In a real implementation you must replace inv operation with cholesky based inverse. DO NOT EVER compute inverse with "inv"
X=inv(eye(size(Pfwd))+Pfwd*PbckInv);
Wt=X*Pfwd;      %Note that (Maybeck, v2p10) uses the transpose of W. I did not understand why he prefers the transpose. (It seems it complicates the results. Could be because of some numeric properties?)

%Compute the difference between the forward solution and the nominal backward
%solution (we are going to estimate the errors on nominal forward solution
%using backward. We choose this approach because PfwdInv*dxFwd=0 as dxFwd=0)
dxDif=zeros(size(Pfwd,1),1);

%pos difference
Rappx=6378137/(sqrt(1.0-0.00669437999014*Cen_fwd(3,3)^2))+h_fwd;
vr_a=Rappx*Cen_fwd(1:2,:)*Cen_bck(3,:)';
dxDif(1:3)=[vr_a;h_bck-h_fwd];

%Vel dif
dxDif(4:6)=Vn_fwd-Vn_bck;

%Att dif
qnb_bck=[qbn_bck(1);-qbn_bck(2:4)];
vr_a=quatmult_v000(qbn_fwd, qnb_bck);
mx_a=quat2dcm_v000(vr_a);
dxDif(7)=mx_a(2,3);
dxDif(8)=mx_a(3,1);
dxDif(9)=mx_a(1,2);

%imu errors
dxDif(10:end)=imu_fwd;

%Smoothed covariance
%P_smt=inv(PfwdInv+PbckInv);
P_smt=X*Pfwd*X'+Wt*PbckInv*Wt';

%Smoothed error estimate on the forward results
dxSmt=P_smt*(PbckInv*dxDif+dzBck);

%Smoothed results
[qbn_smt, Vn_smt, Cen_smt, h_smt]=correctnav_Cen_v000(qbn_fwd, Vn_fwd, Cen_fwd, h_fwd, dxSmt(1:9), 2, 2);
imu_smt=imu_fwd-dxSmt(10:end);


% ***************************************************************************
% Problem definition
% ***************************************************************************
% Author: Chaobin
% Email:  chaubyZou@163.com
% Date: October 2020
% ***************************************************************************
% Language: Matlab
% Also available in: Python
% Required library: None
% ***************************************************************************

clear;

q = [0, 1.6, 2.5, 2, 4, 1.5, 0]';
t = [0, 1, 3, 4.5, 6, 8, 10]';

figure()
plot(t, q, 'ro')
xlabel('time')
ylabel('position')
xlim([-1, 11])
ylim([-1, 5])


%% Testing Will's adiff
clc
clear
%Array of functions to check
funs = {'abs','acos','asin','atan','cos','cosh','exp','log','log10','sin','sinh','sqrt','tan','tanh'};

%Check against both numerical and symbolic gradients
n = length(funs);
ajac = zeros(n,1);
njac = zeros(n,1);
sjac = zeros(n,1);

x0 = abs(0.5+0.2*randn);

%Test each one
for i = 1:n
    fun = str2func(['@(x)' funs{i} '(x)'])
    ajac(i) = autoJac(fun,x0);
    njac(i) = mklJac(fun,x0);
    s = symJac(fun);
    sjac(i) = s(x0);
end

%Display Results
raw = [ajac njac sjac]
acc = [abs(sjac-ajac) abs(sjac-njac)]

%% Index Checking

%% Vection addition1
%double + adiff
clc
clear
a = 2;
b = [1 3 5; 2 4 6];
x = adiff(2);

%2+x1
y2 = a+x(1)

%2+[x..]
y = a+x

%[1..]+x1
y = b+x(1)

%[1..]+[x..]
z = b+x

%% Vector addition2
%adiff + double
clc
clear
a = 2;
b = [1 3 5; 2 4 6];
x = adiff(2);

%x1+2
x(1)+a

%[x..]+2
y = x+a

%x1+[1..]
x(1)+b

%[x..]+[1..]
z = x+b

%% Vector addition3
%adiff + adiff
clc
clear all
x = adiff(2);

%x1+x1
y = x(1)+x(1)

%x1+[x..]
y2 = x(1)+x

%[x..]+x1
z = x+x(1)

%[x..]+[x..]
z2 = x+x

%% Loop Checking
clc
% clear all
b = 1:3;
x = adiff(1:3);
j = x(1)*x(2);
for i = 1:3
    j = j + b(i)*x(i);
end
j

%% Test1 2D
clear all
clc
x0 = [-1;2];
x = adiff(x0);

fun = @(x) 1/(27*sqrt(3)) * ((x(1) - 3)^2 - 9)*x(2)^3;

b = fun(x)

fun(x0)
[f,j] = adiffget(b)

s = symJac(fun);
j2 = s(x0)

%% Test2 3D
clear all
clc
x0 = [-1;2;4];
x = adiff(x0);

fun = @(x) 9 - 8*x(1) - 6*x(2) - 4*x(3) + 2*x(1)^2 + 2*x(2)^2 + x(3)^2 + 2*x(1)*x(2) + 2*x(1)*x(3);

b = fun(x)

fun(x0)
[f,j] = adiffget(b)

s = symJac(fun);
j2 = s(x0)

%% Test3 4D
clear all
clc
x0 = [-1;2;4;-5];
x = adiff(x0);

fun = @(x) 100*(x(2)-x(1)^2)^2 + (1-x(1))^2 + 90*(x(4)-x(3)^2)^2 + (1-x(3))^2 + 10.1*((x(2)-1)^2 + (x(4)-1)^2) + 19.8*(x(2)-1)*(x(4)-1);

b = fun(x)

fun(x0)
[f,j] = adiffget(b)

s = symJac(fun);
j2 = s(x0)

%% Test4 5D
clear all
clc
a = 100;
x0 = [-1;2;4;-5;7];
x = adiff(x0);

fun = @(x) (x(1)-x(2))^2 + (x(2)-x(3))^3 + (x(3)-x(4))^4 + (x(4)-x(5))^4;

fun(x0)
b = fun(x)

[f,j] = adiffget(b)

s = symJac(fun);
j2 = s(x0)

%% Subsasgn, Empty LHS
clear all
clc
x = adiff(1:4);
X = adiff(2);
a = [1;2;3;4];

%Simple indexing
y = x(1)*x(2)

%Simple assign
y2(2) = x(3)*2*x(4)
%Simple 2D assign
y22(2) = x(3)*2*x(4)

% %Multi index, indexing
z = x(1:3)

%Multi index, assign
y3(1:2) = [x(1);x(3)]

%Multi index, assign, longer vec
y4(2:3) = [x(3);x(4)]

%Logical Index
y5(a>2) = x(a>2)

%% Subsasgn, Existing LHS
clear
clc
x = adiff(1:4);
a = [1;2;3;4];

%Add element to vector
y = x(1)
y(2) = x(2)
y(3) = x(3)
y(5) = x(4)

%Add element to indexed vector
y2(1) = x(1)

%Add element to existing vector
% y2(:,2) = x(1:2)
% y2(:,3) = x;

%% Subsagn, Element Numbers
clear
clc
x = adiff(2);

% X([8 9]) = [x(1); x(2)]

% X(:,1:2) = barvec(2)

%% Multiline Equation + multi index
clear
clc
x = adiff(1:4);

f = 3*x.^2 - x(1)
g = sum(f)
z1(1:4) = f
% z1(5) = g
% y = z1(5)*2
%
% %Multi Assign
% c(1,1) = 2*x(1);
% c(2:3,1) = 3*x(2:3).^2;
% c(4,1) = 0.1*x(4)

%% Multi index subs
clear
clc
x = adiff(1:3);

y = sum(x(1:2).*x(2:3))

% y.vecstr

%% Multi2
clear all
clc

b = [1:4]'
x = adiff(1:4);

f = @(x) b(1:2).*x(1:2) + b(3:end).*x(3:end).^2
y = f(x)

x0 = ones(4,1);
f(x0)

%% Multi assign
clear
clc

x = adiff(1:4);

f([2 3]) = 2*x([3 2])
f([1 4]) = 5*x([1 4])

%% Constraint Test
clc
clear
x = adiff(1:5);

nlcon = @(x) [x(1) + 2*x(2) + 3*x(3) - 6;
              x(2) + 2*x(3) + 3*x(4) - 6;
              x(3) + 2*x(4) + 3*x(5) - 6];

b = nlcon(x)

[f,j] = adiffget(b)

s = symJac(nlcon);
j2 = s(ones(5,1))

%% Constraint Test 2
clc
clear
a = 100;
x = adiff(1:5);

c(3) = x(1) + 2*x(2) + 3*x(3) - 6;
c(2) = x(2) + 2*x(3) + 3*x(4) - 6;
c(1) = x(3) + 2*x(4) + 3*x(5) - 6;
c

% y = c*x(1:3)

[f,j] = adiffget(c)

%% DIW

clc
clear all

x = adiff([1 1])

length(x)
size(x)
ndims(x)

n = length(x);
j = @(x) sum((1-x).^2) + sum(100*(x(2:n) - x(1:n-1).^2).^2)

[f,jac] = adiffget(j(x))

mklJac(j,ones(2,1))


function PlotAdaptiveContour2D(u, levels, tol)

% function PlotAdaptiveContour2D(u, levels, tol)
% Purpose: adaptively refine the mesh to approximately locate isocontours

Globals2D;

% build interpolation matrix (coarse->fine)
ri(:,1) = 0.5*(-(r+s)*( 0) + (1+r)*( 0) + (1+s)*(-1) );
si(:,1) = 0.5*(-(r+s)*(-1) + (1+r)*( 0) + (1+s)*( 0) );

ri(:,2) = 0.5*(-(r+s)*(-1) + (1+r)*( 0) + (1+s)*(-1) );
si(:,2) = 0.5*(-(r+s)*(-1) + (1+r)*(-1) + (1+s)*( 0) );

ri(:,3) = 0.5*(-(r+s)*( 1) + (1+r)*( 0) + (1+s)*( 0) );
si(:,3) = 0.5*(-(r+s)*(-1) + (1+r)*( 0) + (1+s)*(-1) );

ri(:,4) = 0.5*(-(r+s)*(-1) + (1+r)*(-1) + (1+s)*( 0) );
si(:,4) = 0.5*(-(r+s)*( 1) + (1+r)*( 0) + (1+s)*( 0) );

%interp = Vandermonde2D(N, ri(:), si(:))*invV;
interp = InterpMatrix2D(ri(:),si(:));

Nlevels = length(levels);

xref = x; yref = y; uref = u; Kref = K;

sk = 1;
F = spalloc(Np,Np,1);
for i=0:N % old ordering
  for j=0:N - i
    if(i+j<=1), F(sk,sk) = 1.; end;
    sk = sk+1;
  end
end

hold on
for nlev=1:Nlevels
  lev = levels(nlev);

  xref = x; yref = y; uref = u; Kref = K; Jref = J;

  err = 1;
  while(err > tol)

    umin = min(uref, [], 1);
    umax = max(uref, [], 1);

    refineflag = (umin <= lev & umax >= lev);

    toref = find( refineflag);
    Nref = length(toref);

    uref = reshape(interp*uref(:,toref), Np, 4*Nref);
    xref = reshape(interp*xref(:,toref), Np, 4*Nref);
    yref = reshape(interp*yref(:,toref), Np, 4*Nref);

    Kref = 4*Nref;

    ufilt = V*(F*(invV*uref));
    err = max(max(abs(ufilt-uref)));

  end

  ri = [-1;1;-1]; si = [-1;-1;1]; refNp = length(ri);
  interp1 = InterpMatrix2D(ri,si);
  xref = interp1*xref; yref = interp1*yref; uref = interp1*uref;

  ltri = delaunay(ri,si);
  Nltri = size(ltri,1);
  tri = zeros(Kref*Nltri, Nfaces);
  ks = (1:Nltri)';
  for k=1:Kref
    tri(ks,:) = ltri + (k-1)*refNp;
    ks = ks + Nltri;
  end

  PlotContour2D(tri, xref(:), yref(:), uref(:), levels)

end
hold off
return


% THE DEMO PRESENTS AN ERROR PROPAGATION USING A MONTE-CARLO METHOD
% The error in the positioning of the joints is propagated to an error
% in the position of the end effector

% Copyright (C) 2012, by Arturo Gil Aparicio
%
% This file is part of ARTE (A Robotics Toolbox for Education).
%
% ARTE is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% ARTE is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with ARTE.  If not, see <http://www.gnu.org/licenses/>.
function draw_errors_monte_carlo
close all

M=3000; %number of particles

%load arm parameters
robot=load_robot('ABB', 'IRB140');
%robot=load_robot('example', '2dofplanar');
robot.graphical.draw_transparent=0;

%standard deviation AT EACH JOINT
sigmaq=0.05;%rad

%find errors around this pose
q=[pi/4 pi/2 -pi/8 0 0 0]';
%q=[pi/2 -pi/2]';

puntos=[];
for i=1:M
    %normrnd is included in the Statistics and Machine Learning Toolbox
    %change to the next line if this package is not included in your
    %qi = q + [normrnd(0, sigmaq, robot.DOF, 1)];
    qi = q + [mygaussian(0, sigmaq, robot.DOF, 1)];
    T=directkinematic(robot, qi);
    puntos=[puntos; T(1,4) T(2,4) T(3,4)];
end

adjust_view(robot)
drawrobot3d(robot,q), hold on
plot3(puntos(:,1),puntos(:,2), puntos(:,3),'r.')

function R=mygaussian(mu, sigma, n, m)

global z1
global generate
%close all

%init randomizer
z1 = rand();
generate = 0;

R=[];
for i=1:n
    for j=1:m
        gauss = generate_gaussian(mu, sigma);
        R(i,j)=gauss;
    end
end


function [wpos,ypos]=rlasymp(G)
G=zpk(G);
zer=G.z{1}; pol=G.p{1}; gain=G.k;
ii=find(abs(zer)<1e10);
if length(ii)>0, zer=zer(ii); end
nExcess=length(pol)-length(zer);
if nExcess>0
   pp=(sum(real(pol))-sum(real(zer)))/nExcess;
   deltaP=pi/nExcess;
   xx=get(gca,'Xlim');
   yy=get(gca,'YLim');
   xx1=(xx(1)-pp)*tan(deltaP);
end
wpos=[pp*ones(1,nExcess); zeros(1,nExcess)];
ypos=zeros(2,nExcess);
for i=1:nExcess
   PAngle=(2*i-1)*deltaP;
   Kslp=tan(PAngle);
   if (pi/2>=PAngle & PAngle>=0)
      xP=xx(2); yP=yy(2);
   elseif (pi>=PAngle&PAngle>pi/2)
      xP=xx(1); yP=yy(2);
   elseif (3*pi/2>=PAngle&PAngle>pi)
      xP=xx(1); yP=yy(1);
   else
      xP=xx(2); yP=yy(1);
   end
   xx1=xP; yy1=Kslp*(xx1-pp);
   if yy1>yy(2) | yy1<yy(1),
      yy1=yP; xx1=yy1/Kslp+pp;
   end
   wpos(2,i)=xx1; ypos(2,i)=yy1;
end


function [Archive,znad] = UpdateArchive(Archive,N)
% Update the archive in DMOEA-eC

%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    %% Detect the non-dominated solutions
    Archive = Archive(NDSort(Archive.objs,1)==1);
    AObj    = Archive.objs;

    %% Select the extreme solutions
    Choose           = false(1,length(Archive));
    [~,extreme1]     = max(AObj,[],1);
    [~,extreme2]     = min(AObj,[],1);
    Choose(extreme1) = true;
    Choose(extreme2) = true;

    %% Select other solutions by truncation
    if sum(Choose) > N
        selected = find(Choose);
        Choose   = selected(randperm(length(selected),N));
    else
        Distance = pdist2(AObj,AObj);
        Distance(logical(eye(length(Distance)))) = inf;
        while sum(Choose) < N && ~all(Choose)
            unSelected = find(~Choose);
            [~,x]      = max(min(Distance(~Choose,Choose),[],2));
            Choose(unSelected(x)) = true;
        end
    end
    Archive = Archive(Choose);

    %% Update the nadir point
    znad = max(Archive.objs,[],1);
end


function [error_avg,error_A] = calc_abundance_error(A1,A2)
%CALC_ABUNDANCE_ERROR Calculate RMSE for each column of A1 and A2
if ~isfloat(A1)
    A1 = double(A1);
end

if length(unique(A1(:))) == 2
    is_real_dataset = 1;
else
    is_real_dataset = 0;
end

P = permute_abundances(A1,A2);
if nanmean(nanmean(abs(eye(size(A1,2)) - P))) ~= 0
    A2_1 = (P*A2')';
else
    A2_1 = A2;
end

if is_real_dataset
    inds = any(A1==1,2);
%     error_A = nanmean(abs(A1(inds,:) - A2_1(inds,:)));
    error_A = calc_abundance_rmse(A1(inds,:), A2_1(inds,:));
else
%     error_A = nanmean(abs(A1-A2_1));
    error_A = calc_abundance_rmse(A1, A2_1);
end

error_avg = nanmean(error_A);

function rmse = calc_abundance_rmse(A1,A2)
rmse = sqrt(nanmean(abs(A1 - A2).^2, 1));


% TestScript.m
%
% This script tests the quaternion library functions to ensure that each
% function output is consistent.
%
% Date          Author          Notes
% 27/09/2011    SOH Madgwick    Initial release

%% Start of script

close all;                          % close all figures
clear;                              % clear all variables
clc;                                % clear the command terminal

%% Axis-angle to rotation matrix

axis = [1 2 3];
axis = axis / norm(axis);
angle = pi/2;

R = axisAngle2rotMat(axis, angle);
num = ' % 1.5f';
a = sprintf('\rAxis-angle to rotation matrix:');
b = sprintf(strcat('\r', num, '\t', num, '\t', num), R(1,:));
c = sprintf(strcat('\r', num, '\t', num, '\t', num), R(2,:));
d = sprintf(strcat('\r', num, '\t', num, '\t', num), R(3,:));
disp(strcat(a,b,c,d));

%% Axis-angle to quaternion

q = axisAngle2quatern(axis, angle);
num = ' % 1.5f';
a = sprintf('\rAxis-angle to quaternion:');
b = sprintf(strcat('\r', num, '\t', num, '\t', num, '\t', num), q);
disp(strcat(a,b));

%% Quaternion to rotation matrix

R = quatern2rotMat(q);
num = ' % 1.5f';
a = sprintf('\rQuaternion to rotation matrix:');
b = sprintf(strcat('\r', num, '\t', num, '\t', num), R(1,:));
c = sprintf(strcat('\r', num, '\t', num, '\t', num), R(2,:));
d = sprintf(strcat('\r', num, '\t', num, '\t', num), R(3,:));
disp(strcat(a,b,c,d));

%% Rotation matrix to quaternion

q = rotMat2quatern(R);
num = ' % 1.5f';
a = sprintf('\rRotation matrix to quaternion:');
b = sprintf(strcat('\r', num, '\t', num, '\t', num, '\t', num), q);
disp(strcat(a,b));

%% Rotation matrix to ZYX Euler angles

euler = rotMat2euler(R);
num = ' % 1.5f';
a = sprintf('\rRotation matrix to ZYX Euler angles:');
b = sprintf(strcat('\r', num, '\t', num, '\t', num), euler);
disp(strcat(a,b));

%% Quaternion to ZYX Euler angles

euler = quatern2euler(q);
num = ' % 1.5f';
a = sprintf('\rQuaternion to ZYX Euler angles:');
b = sprintf(strcat('\r', num, '\t', num, '\t', num), euler);
disp(strcat(a,b));

%% ZYX Euler angles to rotation matrix

R = euler2rotMat(euler(1), euler(2), euler(3));
num = ' % 1.5f';
a = sprintf('\rZYX Euler angles to rotation matrix:');
b = sprintf(strcat('\r', num, '\t', num, '\t', num), R(1,:));
c = sprintf(strcat('\r', num, '\t', num, '\t', num), R(2,:));
d = sprintf(strcat('\r', num, '\t', num, '\t', num), R(3,:));
disp(strcat(a,b,c,d));

%% End of script