function out = linearlcc(in, quantization)
%  @inproceedings{zhang2017learning,
%    title={Learning Deep CNN Denoiser Prior for Image Restoration},
%    author={Zhang, Kai and Zuo, Wangmeng and Gu, Shuhang and Zhang, Lei},
%    booktitle={IEEE Conference on Computer Vision and Pattern Recognition},
%    pages={3929--3938},
%    year={2017},
%  }
%quantization = 0;
% in : uint8; RGB or Bayer pattern [0~255];
% out: double/uint8: RGB [0~255];

[m,n,ch]=size(in);
% if ch==3
%     B=zeros(m,n);
%     B(1:2:m,1:2:n)=in(1:2:m,1:2:n,2);
%     B(2:2:m,2:2:n)=in(2:2:m,2:2:n,2);
%     B(1:2:m,2:2:n)=in(1:2:m,2:2:n,1);
%     B(2:2:m,1:2:n)=in(2:2:m,1:2:n,3);
%     in=B;
%     clear B;
% end

in = double(in);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% function out = lcc1(in)
% returns the color interpolated image using Laplacian
% second-order color correction I
%
% Assumptions : in has following color patterns
%
%  ------------------> grbg
%  |  G R G R ...
%  |  B G B G ...
%  |  G R G R ...
%  |  B G B G ...
%  |  . . . . .

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

inR = in; inG = in; inB = in;
out = in;
outR = inR; outG = inG; outB = inB;

% G channel
for i=4:2:m-2,
    for j=3:2:n-3,
        delta_H = abs(inB(i,j-2)+inB(i,j+2)-2*inB(i,j))+abs(inG(i,j-1)-inG(i,j+1));
        delta_V = abs(inB(i-2,j)+inB(i+2,j)-2*inB(i,j))+abs(inG(i-1,j)-inG(i+1,j));
        if delta_H < delta_V,
            outG(i,j) = 1/2*(inG(i,j-1)+inG(i,j+1))+1/4*(2*inB(i,j)-inB(i,j-2)-inB(i,j+2));
        elseif delta_H > delta_V,
            outG(i,j) = 1/2*(inG(i-1,j)+inG(i+1,j))+1/4*(2*inB(i,j)-inB(i-2,j)-inB(i+2,j));
        else
            outG(i,j) = 1/4*(inG(i,j-1)+inG(i,j+1)+inG(i-1,j)+inG(i+1,j))+1/8*(4*inB(i,j)-inB(i,j-2)-inB(i,j+2)-inB(i-2,j)-inB(i+2,j));
        end
    end
end

for i=3:2:m-3,
    for j=4:2:n-2,
        delta_H = abs(inR(i,j-2)+inR(i,j+2)-2*inR(i,j))+abs(inG(i,j-1)-inG(i,j+1));
        delta_V = abs(inR(i-2,j)+inR(i+2,j)-2*inR(i,j))+abs(inG(i-1,j)-inG(i+1,j));
        if delta_H < delta_V,
            outG(i,j) = 1/2*(inG(i,j-1)+inG(i,j+1))+1/4*(2*inR(i,j)-inR(i,j-2)-inR(i,j+2));
        elseif delta_H > delta_V,
            outG(i,j) = 1/2*(inG(i-1,j)+inG(i+1,j))+1/4*(2*inR(i,j)-inR(i-2,j)-inR(i+2,j));
        else
            outG(i,j) = 1/4*(inG(i,j-1)+inG(i,j+1)+inG(i-1,j)+inG(i+1,j))+1/8*(4*inR(i,j)-inR(i,j-2)-inR(i,j+2)-inR(i-2,j)-inR(i+2,j));
        end
    end
end

% R channel
for i=1:2:m-1,
    outR(i,3:2:n-1) = 1/2*(inR(i,2:2:n-2)+inR(i,4:2:n))+1/4*(2*outG(i,3:2:n-1)-outG(i,2:2:n-2)-outG(i,4:2:n));
end

for i=2:2:m-2,
    outR(i,2:2:n) = 1/2*(inR(i-1,2:2:n)+inR(i+1,2:2:n))+1/4*(2*outG(i,2:2:n)-outG(i-1,2:2:n)-outG(i+1,2:2:n));
end

for i=2:2:m-2,
    for j=3:2:n-1,
        delta_P = abs(inR(i-1,j+1)-inR(i+1,j-1))+abs(2*outG(i,j)-outG(i-1,j+1)-outG(i+1,j-1));
        delta_N = abs(inR(i-1,j-1)-inR(i+1,j+1))+abs(2*outG(i,j)-outG(i-1,j-1)-outG(i+1,j+1));
        if delta_N < delta_P,
            outR(i,j) = 1/2*(inR(i-1,j-1)+inR(i+1,j+1))+1/2*(2*outG(i,j)-outG(i-1,j-1)-outG(i+1,j+1));
        elseif delta_N > delta_P,
            outR(i,j) = 1/2*(inR(i-1,j+1)+inR(i+1,j-1))+1/2*(2*outG(i,j)-outG(i-1,j+1)-outG(i+1,j-1));
        else
            outR(i,j) = 1/4*(inR(i-1,j-1)+inR(i-1,j+1)+inR(i+1,j-1)+inR(i+1,j+1))+1/4*(4*outG(i,j)-outG(i-1,j-1)-outG(i-1,j+1)-outG(i+1,j-1)-outG(i+1,j+1));
        end
    end
end

% B channel
for i=2:2:m,
    outB(i,2:2:n-2) = 1/2*(inB(i,1:2:n-3)+inB(i,3:2:n-1))+1/4*(2*outG(i,2:2:n-2)-outG(i,1:2:n-3)-outG(i,3:2:n-1));
end

for i=3:2:m-1,
    outB(i,1:2:n-1) =  1/2*(inB(i-1,1:2:n-1)+inB(i+1,1:2:n-1))+1/4*(2*outG(i,1:2:n-1)-outG(i-1,1:2:n-1)-outG(i+1,1:2:n-1));
end

for i=3:2:m-1,
    for j=2:2:n-2,
        delta_P = abs(inB(i-1,j+1)-inB(i+1,j-1))+abs(2*outG(i,j)-outG(i-1,j+1)-outG(i+1,j-1));
        delta_N = abs(inB(i-1,j-1)-inB(i+1,j+1))+abs(2*outG(i,j)-outG(i-1,j-1)-outG(i+1,j+1));
        if delta_N < delta_P,
            outB(i,j) = 1/2*(inB(i-1,j-1)+inB(i+1,j+1))+1/2*(2*outG(i,j)-outG(i-1,j-1)-outG(i+1,j+1));
        elseif delta_N > delta_P,
            outB(i,j) = 1/2*(inB(i-1,j+1)+inB(i+1,j-1))+1/2*(2*outG(i,j)-outG(i-1,j+1)-outG(i+1,j-1));
        else
            outB(i,j) = 1/4*(inB(i-1,j-1)+inB(i-1,j+1)+inB(i+1,j-1)+inB(i+1,j+1))+1/4*(4*outG(i,j)-outG(i-1,j-1)-outG(i-1,j+1)-outG(i+1,j-1)-outG(i+1,j+1));
        end
    end
end

out(:,:,1) = outR;
out(:,:,2) = outG;
out(:,:,3) = outB;

if quantization
    out = uint8(out);
end


% =========================================================================
% (c) 2018 Ronald Nissel, ronald.nissel@gmail.com
% =========================================================================
% Allows to reproduce Figure 2, 3, 4 and 5 of "Doubly-Selective Channel
% Estimation in FBMC-OQAM and OFDM Systems", R. Nissel, et al, IEEE VTC
% Fall, 2018. In particular, this script simulates an FBMC and OFDM
% transmission over a doubly-selective channel, including doubly-selective
% MMSE channel estimation and interference cancellation. Note that, to
% truly reproduce the figures, the lines 41-46 must be uncommented.

clear; close all;
addpath('./Theory');

%% Parameters
% Simulation
M_SNR_dB                  = [10:5:40];              % Signal-to-Noise Ratio in dB
NrRepetitions             = 25;                     % Number of Monte Carlo repetition (different channel realizations)
ZeroThresholdSparse       = 8;                      % Set some matrix values, smaller than "10^(-ZeroThresholdSparse)", to zero.
PlotIterationStepsSNRdB   = 35;                     % Plot the BER over iteration step for an SNR of 35dB.

% FBMC and OFDM parameters
L                         = 12*2;                   % Number of Subcarriers, one resource block consists of 12 subcarriers (and 0.5ms in time)
F                         = 15e3;                   % Subcarrier Spacing in Hz
SamplingRate              = F*12*2;                 % Sampling rate in (Samples/s)
NrSubframes               = 1;                      % Number of subframes. One subframe requires 1ms for F=15kHz.
QAM_ModulationOrder       = 256;                    % QAM signal constellation order, 4, 16, 64, 256, 1024,...

% Channel estimation parameters
PilotToDataPowerOffset    = 2;                      % Pilot to data power offset for OFDM. In FBMC data spreading, the power offset is twice this number.
PilotToDataPowerOffsetAux = 4.685;                  % Pilot to data power offset for FBMC, auxiliary method.
NrIterations              = 4;                      % Number of iterations for interference cancellation scheme.

% Channel
Velocity_kmh              = 500;                    % Velocity in km/h. Note that [mph]*1.6=[kmh] and [m/s]*3.6=[kmh]
PowerDelayProfile         = 'VehicularA';           % Channel model, either string or vector: 'Flat', 'AWGN', 'PedestrianA', 'PedestrianB', 'VehicularA', 'VehicularB', 'ExtendedPedestrianA', 'ExtendedPedestrianB', or 'TDL-A_xxns','TDL-B_xxns','TDL-C_xxns' (with xx the RMS delay spread in ns, e.g. 'TDL-A_30ns'), or [1 0 0.2] (Self-defined power delay profile which depends on the sampling rate)

% ###########################################################################
% % In the paper:
% M_SNR_dB                = [10:2:40];
% PlotIterationStepsSNRdB = 32;
% NrRepetitions           = 1000;
% SamplingRate            = F*14*14;
% NrSubframes             = 2;
% ###########################################################################

%% FBMC Object
FBMC = Modulation.FBMC(...
    L,...                               % Number subcarriers
    30*NrSubframes,...                  % Number FBMC symbols
    F,...                               % Subcarrier spacing (Hz)
    SamplingRate,...                    % Sampling rate (Samples/s)
    0,...                               % Intermediate frequency first subcarrier (Hz)
    false,...                           % Transmit real valued signal
    'Hermite-OQAM',...                  % Prototype filter (Hermite, PHYDYAS, RRC) and OQAM or QAM,
    8, ...                              % Overlapping factor (also determines oversampling in the frequency domain)
    0, ...                              % Initial phase shift
    true ...                            % Polyphase implementation
    );

%% OFDM Object (Add zeroes to the OFDM signal so that it fits the FBMC signal)
ZeroGuardTimeLength = ((FBMC.Nr.SamplesTotal-(round((1/15e3/14)*SamplingRate)+round(SamplingRate/15e3))*14*NrSubframes)/2)/SamplingRate;
OFDM = Modulation.OFDM(...
    L,...                           % Number Subcarriers
    14*NrSubframes,...              % Number OFDM Symbols
    F,...                           % Subcarrier spacing (Hz)
    SamplingRate,...                % Sampling rate (Samples/s)
    0,...                           % Intermediate frequency first subcarrier (Hz)
    false,...                       % Transmitreal valued signal
    1/15e3/14, ...                  % Cyclic prefix length (s)
    ZeroGuardTimeLength ...         % Zero guard length (s)
    );

%% Check Number of Samples
if  OFDM.Nr.SamplesTotal~=FBMC.Nr.SamplesTotal
   error('Total number of samples must be the same for OFDM and FBMC.');
end
N = OFDM.Nr.SamplesTotal;

%% PAM and QAM Object
PAM = Modulation.SignalConstellation(sqrt(QAM_ModulationOrder),'PAM');
QAM = Modulation.SignalConstellation(QAM_ModulationOrder,'QAM');

%% Pilot Matrices, 0="Data", 1=Pilot
PilotMatrix_OFDM = zeros(OFDM.Nr.Subcarriers,14);
PilotMatrix_OFDM(2:2*6:end,2:2*3.5:end)=1;
PilotMatrix_OFDM(5:2*6:end,6:2*3.5:end)=1;
PilotMatrix_OFDM(8:2*6:end,2:2*3.5:end)=1;
PilotMatrix_OFDM(11:2*6:end,6:2*3.5:end)=1;
PilotMatrix_OFDM = repmat(PilotMatrix_OFDM,[1 NrSubframes]);

PilotMatrix_FBMC =  zeros(FBMC.Nr.Subcarriers,30);
PilotMatrix_FBMC(2:12:end,3:16:end) = 1;
PilotMatrix_FBMC(5:12:end,11:16:end) = 1;
PilotMatrix_FBMC(8:12:end,3+1:16:end) = 1;
PilotMatrix_FBMC(11:12:end,11+1:16:end) = 1;
PilotMatrix_FBMC = repmat(PilotMatrix_FBMC,[1 NrSubframes]);

AuxilaryPilotMatrix_FBMC = PilotMatrix_FBMC;
[a,b] = find(PilotMatrix_FBMC);
for i_pilot = 1:length(a)
    AuxilaryPilotMatrix_FBMC(a(i_pilot)+1,b(i_pilot))=-1;
    AuxilaryPilotMatrix_FBMC(a(i_pilot)-1,b(i_pilot))=-1;
    AuxilaryPilotMatrix_FBMC(a(i_pilot),b(i_pilot)+1)=-1;
    AuxilaryPilotMatrix_FBMC(a(i_pilot),b(i_pilot)-1)=-1;
end

%% Cancel Imaginary Interference At Pilot Position Object (Precoding Matrix)
AuxiliaryMethod = ChannelEstimation.ImaginaryInterferenceCancellationAtPilotPosition(...
    'Auxiliary', ...                                    % Cancellation method
    AuxilaryPilotMatrix_FBMC, ...                       % PilotAndAuxiliaryMatrix
    FBMC.GetFBMCMatrix, ...                             % Imaginary interference matrix
    28, ...                                             % Cancel 28 closest interferers
    PilotToDataPowerOffsetAux ...                       % Pilot to data power offset
    );
CodingMethod = ChannelEstimation.ImaginaryInterferenceCancellationAtPilotPosition(...
    'Coding', ...                                       % Cancellation method
    PilotMatrix_FBMC, ...                               % PilotMatrix
    FBMC.GetFBMCMatrix, ...                             % Imaginary interference matrix
    20, ...                                             % Cancel 28 closest interferers
    2*PilotToDataPowerOffset ...                        % Pilot to data power offset
    );

NrPilotSymbols = sum(PilotMatrix_OFDM(:)==1);
NrDataSymbols_OFDM = sum(PilotMatrix_OFDM(:)==0);

PilotMapping_OFDM = zeros(numel(PilotMatrix_OFDM));
PilotMapping_OFDM(PilotMatrix_OFDM(:)==1,1:NrPilotSymbols) = sqrt(PilotToDataPowerOffset)*eye(NrPilotSymbols);
PilotMapping_OFDM(PilotMatrix_OFDM(:)==0,NrPilotSymbols+1:end) = eye(NrDataSymbols_OFDM);
PilotMapping_OFDM = PilotMapping_OFDM/sqrt(mean(diag(PilotMapping_OFDM*PilotMapping_OFDM')));
DataPowerReduction_OFDM = numel(PilotMatrix_OFDM)/(NrPilotSymbols*PilotToDataPowerOffset+NrDataSymbols_OFDM);

Kappa_Aux  = AuxiliaryMethod.PilotToDataPowerOffset*AuxiliaryMethod.DataPowerReduction;
Kappa_Cod  = CodingMethod.PilotToDataPowerOffset*CodingMethod.DataPowerReduction;
Kappa_OFDM = PilotToDataPowerOffset*DataPowerReduction_OFDM;

%% Evaluate only the BER near the center. i.e., ignore edges
ConsideredTimeFrequencyPositions_FBMC = zeros(size(PilotMatrix_FBMC));
ConsideredTimeFrequencyPositions_FBMC(5:end-4,11:end-10)= 1;

ConsideredTimeFrequencyPositions_OFDM = zeros(size(PilotMatrix_OFDM));
ConsideredTimeFrequencyPositions_OFDM(5:end-4,6:end-5)= 1;

for i_lk = 1: AuxiliaryMethod.NrDataSymbols
    xD_Temp = zeros(AuxiliaryMethod.NrDataSymbols,1);
    xD_Temp(i_lk) = 1;
    x_FBMC_Aux = AuxiliaryMethod.PrecodingMatrix*[zeros(NrPilotSymbols,1);xD_Temp];
    ConsideredDataPositions_FBMC_Aux(i_lk) = sum(abs(x_FBMC_Aux(1==ConsideredTimeFrequencyPositions_FBMC.*not(AuxilaryPilotMatrix_FBMC))))>AuxiliaryMethod.DataPowerReduction*0.9;
end
for i_lk = 1: CodingMethod.NrDataSymbols
    xD_Temp = zeros(CodingMethod.NrDataSymbols,1);
    xD_Temp(i_lk) = 1;
    x_FBMC_Cod = CodingMethod.PrecodingMatrix*[zeros(NrPilotSymbols,1);xD_Temp];
    ConsideredDataPositions_FBMC_Cod(i_lk) =not(any(x_FBMC_Cod(not(ConsideredTimeFrequencyPositions_FBMC))));
end
for i_lk = 1: NrDataSymbols_OFDM
    xD_Temp = zeros(NrDataSymbols_OFDM,1);
    xD_Temp(i_lk) = 1;
    x_OFDM = PilotMapping_OFDM*[zeros(NrPilotSymbols,1);xD_Temp];
    ConsideredDataPositions_OFDM(i_lk) = sum(abs(x_OFDM(1==ConsideredTimeFrequencyPositions_OFDM.*not(PilotMatrix_OFDM))))>DataPowerReduction_OFDM*0.9;
end

ConsideredBits_FBMC_Aux = reshape(repmat(ConsideredDataPositions_FBMC_Aux,log2(QAM_ModulationOrder)/2,1),[],1);
ConsideredBits_FBMC_Cod = reshape(repmat(ConsideredDataPositions_FBMC_Cod,log2(QAM_ModulationOrder)/2,1),[],1);
ConsideredBits_OFDM     = reshape(repmat(ConsideredDataPositions_OFDM,log2(QAM_ModulationOrder),1),[],1);

%% Channel Model Object
ChannelModel = Channel.FastFading(...
    SamplingRate,...                                   % Sampling rate (Samples/s)
    PowerDelayProfile,...                              % Power delay profile, either string or vector: 'Flat', 'AWGN', 'PedestrianA', 'PedestrianB', 'VehicularA', 'VehicularB', 'ExtendedPedestrianA', 'ExtendedPedestrianB', or 'TDL-A_xxns','TDL-B_xxns','TDL-C_xxns' (with xx the RMS delay spread in ns, e.g. 'TDL-A_30ns'), or [1 0 0.2] (Self-defined power delay profile which depends on the sampling rate)
    N,...                                              % Number of total samples
    Velocity_kmh/3.6*2.5e9/2.998e8,...                 % Maximum Doppler shift: Velocity_kmh/3.6*CarrierFrequency/2.998e8
    'Jakes',...                                        % Which Doppler model: 'Jakes', 'Uniform', 'Discrete-Jakes', 'Discrete-Uniform'. For "Discrete-", we assume a discrete Doppler spectrum to improve the simulation time. This only works accuratly if the number of samples and the velocity is sufficiently large
    200,...                                            % Number of paths for the WSSUS process. Only relevant for a 'Jakes' and 'Uniform' Doppler spectrum
    1,...                                              % Number of transmit antennas
    1,...                                              % Number of receive antennas
    1 ...                                              % Gives a warning if the predefined delay taps of the channel do not fit the sampling rate. This is usually not much of a problem if they are approximatly the same.
    );
R_vecH = ChannelModel.GetCorrelationMatrix;

%% Precalculate Transmit and Receive Matrices
G_FBMC = FBMC.GetTXMatrix;
Q_FBMC = (FBMC.GetRXMatrix)';

G_OFDM = OFDM.GetTXMatrix;
Q_OFDM = (OFDM.GetRXMatrix)';

GP_FBMC = G_FBMC(:,PilotMatrix_FBMC(:)==1);
GP_OFDM = G_OFDM(:,PilotMatrix_OFDM(:)==1);

QP_FBMC = Q_FBMC(:,PilotMatrix_FBMC(:)==1);
QP_OFDM = Q_OFDM(:,PilotMatrix_OFDM(:)==1);

G_Aux = G_FBMC*AuxiliaryMethod.PrecodingMatrix;
G_Cod = G_FBMC*CodingMethod.PrecodingMatrix;
G_OFDM_PilotMapping = G_OFDM*PilotMapping_OFDM;

%% Calculate Correlation Matrices
disp('Calculate Correlation Matrix of Pilot Estimates (no Noise, no Interference)...');
R_hP_FBMC  = nan(NrPilotSymbols,NrPilotSymbols);
R_hP_OFDM = nan(NrPilotSymbols,NrPilotSymbols);
for j_pilot = 1:NrPilotSymbols
    R_hP_FBMC(:,j_pilot) = sum((QP_FBMC'*reshape(R_vecH*kron(GP_FBMC(:,j_pilot).',QP_FBMC(:,j_pilot)')',N,N)).*(GP_FBMC.'),2);
    R_hP_OFDM(:,j_pilot) = sum((QP_OFDM'*reshape(R_vecH*kron(GP_OFDM(:,j_pilot).',QP_OFDM(:,j_pilot)')',N,N)).*(GP_OFDM.'),2);
end

disp('Calculate Correlation Matrix of Pilot Estimates (no Interference)...');
R_hP_est_noNoise_FBMC_Aux = R_hP_FBMC;
R_hP_est_noNoise_FBMC_Cod = R_hP_FBMC;
R_hP_est_noNoise_OFDM = R_hP_OFDM;
for i_pilots = 1: NrPilotSymbols
    % FBMC Auxiliary Method, Similar as Equation (13) but computationally more efficient
    Temp = kron(sparse(eye(N)),QP_FBMC(:,i_pilots)')/sqrt(Kappa_Aux);
    R_hP_est_noNoise_FBMC_Aux(i_pilots,i_pilots)=abs(sum(sum((G_Aux.'*(Temp*R_vecH*Temp')).*G_Aux',2)));

    % FBMC Coding
    Temp = kron(sparse(eye(N)),QP_FBMC(:,i_pilots)')/sqrt(Kappa_Cod);
    R_hP_est_noNoise_FBMC_Cod(i_pilots,i_pilots)=abs(sum(sum((G_Cod.'*(Temp*R_vecH*Temp')).*G_Cod',2)));

    % OFDM
    Temp = kron(sparse(eye(N)),QP_OFDM(:,i_pilots)')/sqrt(Kappa_OFDM);
    R_hP_est_noNoise_OFDM(i_pilots,i_pilots)=abs(sum(sum((G_OFDM_PilotMapping.'*(Temp*R_vecH*Temp')).*G_OFDM_PilotMapping',2)));
end

disp('Calculate Correlation Matrix of Pilot Estimates...');
R_hP_est_FBMC_Aux = repmat(R_hP_est_noNoise_FBMC_Aux,[1 1 length(M_SNR_dB)]);
R_hP_est_FBMC_Cod = repmat(R_hP_est_noNoise_FBMC_Cod,[1 1 length(M_SNR_dB)]);
R_hP_est_OFDM = repmat(R_hP_est_noNoise_OFDM,[1 1 length(M_SNR_dB)]);
for i_SNR = 1:length(M_SNR_dB)
    SNR_dB = M_SNR_dB(i_SNR);
    Pn_time = SamplingRate/(F*L)*10^(-SNR_dB/10);
    for i_pilots = 1: NrPilotSymbols
        R_hP_est_FBMC_Aux(i_pilots,i_pilots,i_SNR)=R_hP_est_noNoise_FBMC_Aux(i_pilots,i_pilots)+Pn_time*QP_FBMC(:,i_pilots)'*QP_FBMC(:,i_pilots)/(Kappa_Aux);
        R_hP_est_FBMC_Cod(i_pilots,i_pilots,i_SNR)=R_hP_est_noNoise_FBMC_Cod(i_pilots,i_pilots)+Pn_time*QP_FBMC(:,i_pilots)'*QP_FBMC(:,i_pilots)/(Kappa_Cod);
        R_hP_est_OFDM(i_pilots,i_pilots,i_SNR)=R_hP_est_noNoise_OFDM(i_pilots,i_pilots)+Pn_time*QP_OFDM(:,i_pilots)'*QP_OFDM(:,i_pilots)/Kappa_OFDM;
    end
end

R_hP_est_noInterference_FBMC_Aux = R_hP_est_FBMC_Aux-repmat((R_hP_est_noNoise_FBMC_Aux-R_hP_FBMC),[1 1 length(M_SNR_dB)]);
R_hP_est_noInterference_FBMC_Cod = R_hP_est_FBMC_Cod-repmat((R_hP_est_noNoise_FBMC_Cod-R_hP_FBMC),[1 1 length(M_SNR_dB)]);
R_hP_est_noInterference_OFDM = R_hP_est_OFDM-repmat((R_hP_est_noNoise_OFDM-R_hP_OFDM),[1 1 length(M_SNR_dB)]);

disp('Calculate Correlation Matrix between Transmission-Matrix D and Pilot Estimates...');
R_Dij_hP_FBMC = sparse(size(G_FBMC,2)^2,NrPilotSymbols);
R_Dij_hP_OFDM = sparse(size(G_OFDM,2)^2,NrPilotSymbols);
for i_pilot = 1: NrPilotSymbols
    R_Dij_hP_FBMC_Temp = reshape(Q_FBMC'*reshape(R_vecH*kron(GP_FBMC(:,i_pilot).',QP_FBMC(:,i_pilot)')',N,N)*G_FBMC,[],1);
    R_Dij_hP_OFDM_Temp = reshape(Q_OFDM'*reshape(R_vecH*kron(GP_OFDM(:,i_pilot).',QP_OFDM(:,i_pilot)')',N,N)*G_OFDM,[],1);

    R_Dij_hP_FBMC_Temp(abs(R_Dij_hP_FBMC_Temp)<10^(-ZeroThresholdSparse))=0;
    R_Dij_hP_OFDM_Temp(abs(R_Dij_hP_OFDM_Temp)<10^(-ZeroThresholdSparse))=0;

    R_Dij_hP_FBMC(:,i_pilot) = R_Dij_hP_FBMC_Temp;
    R_Dij_hP_OFDM(:,i_pilot) = R_Dij_hP_OFDM_Temp;
end
clear R_vecH;

%% Calculate SIR at Pilot Positions
SIR_P_FBMC_Aux_dB = 10*log10(trace(abs(R_hP_FBMC))./trace(abs(R_hP_est_noNoise_FBMC_Aux-R_hP_FBMC)));
SIR_P_FBMC_Cod_dB = 10*log10(trace(abs(R_hP_FBMC))./trace(abs(R_hP_est_noNoise_FBMC_Cod-R_hP_FBMC)));
SIR_P_OFDM_dB     = 10*log10(trace(abs(R_hP_OFDM))./trace(abs(R_hP_est_noNoise_OFDM-R_hP_OFDM)));

%% Calculate MMSE Estimation Matrix Using Correlation
disp('Calculate MMSE Solution ...');
W_MMSE_FBMC_Aux = sparse(size(G_FBMC,2)*size(G_FBMC,2)*NrPilotSymbols,length(M_SNR_dB));
W_MMSE_FBMC_Cod = sparse(size(G_FBMC,2)*size(G_FBMC,2)*NrPilotSymbols,length(M_SNR_dB));
W_MMSE_OFDM     = sparse(size(G_OFDM,2)*size(G_OFDM,2)*NrPilotSymbols,length(M_SNR_dB));
for i_SNR = 1:length(M_SNR_dB)
    W_MMSE_FBMC_Aux_Temp = reshape(R_Dij_hP_FBMC*pinv(R_hP_est_FBMC_Aux(:,:,i_SNR)),size(G_FBMC,2)*size(G_FBMC,2)*NrPilotSymbols,1);
    W_MMSE_FBMC_Cod_Temp = reshape(R_Dij_hP_FBMC*pinv(R_hP_est_FBMC_Cod(:,:,i_SNR)),size(G_FBMC,2)*size(G_FBMC,2)*NrPilotSymbols,1);
    W_MMSE_OFDM_Temp     = reshape(R_Dij_hP_OFDM*pinv(R_hP_est_OFDM(:,:,i_SNR)),size(G_OFDM,2)*size(G_OFDM,2)*NrPilotSymbols,1);

    W_MMSE_FBMC_Aux_Temp(abs(W_MMSE_FBMC_Aux_Temp)<10^(-ZeroThresholdSparse))=0;
    W_MMSE_FBMC_Cod_Temp(abs(W_MMSE_FBMC_Cod_Temp)<10^(-ZeroThresholdSparse))=0;
    W_MMSE_OFDM_Temp(abs(W_MMSE_OFDM_Temp)<10^(-ZeroThresholdSparse))=0;

    W_MMSE_FBMC_Aux(:,i_SNR) = W_MMSE_FBMC_Aux_Temp;
    W_MMSE_FBMC_Cod(:,i_SNR) = W_MMSE_FBMC_Cod_Temp;
    W_MMSE_OFDM(:,i_SNR)     = W_MMSE_OFDM_Temp;
end

%% Calculate MMSE Estimation if no Interference is present at the pilot positions
disp('Calculate MMSE Solution (no Interference)...');
W_MMSE_noInterference_FBMC_Aux = sparse(size(G_FBMC,2)*size(G_FBMC,2)*NrPilotSymbols,length(M_SNR_dB));
W_MMSE_noInterference_FBMC_Cod = sparse(size(G_FBMC,2)*size(G_FBMC,2)*NrPilotSymbols,length(M_SNR_dB));
W_MMSE_noInterference_OFDM     = sparse(size(G_OFDM,2)*size(G_OFDM,2)*NrPilotSymbols,length(M_SNR_dB));
for i_SNR = 1:length(M_SNR_dB)
    W_MMSE_noInterference_FBMC_Aux_Temp = reshape(R_Dij_hP_FBMC*pinv(R_hP_est_noInterference_FBMC_Aux(:,:,i_SNR)),size(G_FBMC,2)*size(G_FBMC,2)*NrPilotSymbols,1);
    W_MMSE_noInterference_FBMC_Cod_Temp = reshape(R_Dij_hP_FBMC*pinv(R_hP_est_noInterference_FBMC_Cod(:,:,i_SNR)),size(G_FBMC,2)*size(G_FBMC,2)*NrPilotSymbols,1);
    W_MMSE_noInterference_OFDM_Temp = reshape(R_Dij_hP_OFDM*pinv(R_hP_est_noInterference_OFDM(:,:,i_SNR)),size(G_OFDM,2)*size(G_OFDM,2)*NrPilotSymbols,1);

    W_MMSE_noInterference_FBMC_Aux_Temp(abs(W_MMSE_noInterference_FBMC_Aux_Temp)<10^(-ZeroThresholdSparse))=0;
    W_MMSE_noInterference_FBMC_Cod_Temp(abs(W_MMSE_noInterference_FBMC_Cod_Temp)<10^(-ZeroThresholdSparse))=0;
    W_MMSE_noInterference_OFDM_Temp(abs(W_MMSE_noInterference_OFDM_Temp)<10^(-ZeroThresholdSparse))=0;

    W_MMSE_noInterference_FBMC_Aux(:,i_SNR) = W_MMSE_noInterference_FBMC_Aux_Temp;
    W_MMSE_noInterference_FBMC_Cod(:,i_SNR) = W_MMSE_noInterference_FBMC_Cod_Temp;
    W_MMSE_noInterference_OFDM(:,i_SNR)     = W_MMSE_noInterference_OFDM_Temp;
end

%% Theoretical BEP for a Doubly Flat Rayleigh Channel
M_SNR_dB_morePoints = min(M_SNR_dB):1:max(M_SNR_dB);
BitErrorProbability = BitErrorProbabilityDoublyFlatRayleigh(M_SNR_dB_morePoints,QAM.SymbolMapping,QAM.BitMapping);

%% Preallocate for Parfor
BER_FBMC_Aux_PerfectCSI_InterferenceCancellation        = nan(length(M_SNR_dB),NrRepetitions,NrIterations);
BER_FBMC_Aux_PerfectCSI_InterferenceCancellation_NoEdge = nan(length(M_SNR_dB),NrRepetitions,NrIterations);
BER_FBMC_Cod_PerfectCSI_InterferenceCancellation        = nan(length(M_SNR_dB),NrRepetitions,NrIterations);
BER_FBMC_Cod_PerfectCSI_InterferenceCancellation_NoEdge = nan(length(M_SNR_dB),NrRepetitions,NrIterations);
BER_OFDM_PerfectCSI_InterferenceCancellation            = nan(length(M_SNR_dB),NrRepetitions,NrIterations);
BER_OFDM_PerfectCSI_InterferenceCancellation_NoEdge     = nan(length(M_SNR_dB),NrRepetitions,NrIterations);
BER_FBMC_Aux_OneTapEqualizer_PerfectCSI                 = nan(length(M_SNR_dB),NrRepetitions);
BER_FBMC_Aux_OneTapEqualizer_PerfectCSI_NoEdge          = nan(length(M_SNR_dB),NrRepetitions);
BER_FBMC_Cod_OneTapEqualizer_PerfectCSI                 = nan(length(M_SNR_dB),NrRepetitions);
BER_FBMC_Cod_OneTapEqualizer_PerfectCSI_NoEdge          = nan(length(M_SNR_dB),NrRepetitions);
BER_OFDM_OneTapEqualizer_PerfectCSI                     = nan(length(M_SNR_dB),NrRepetitions);
BER_OFDM_OneTapEqualizer_PerfectCSI_NoEdge              = nan(length(M_SNR_dB),NrRepetitions);
BER_FBMC_Aux_InterferenceCancellation                   = nan(length(M_SNR_dB),NrRepetitions,NrIterations);
BER_FBMC_Cod_InterferenceCancellation                   = nan(length(M_SNR_dB),NrRepetitions,NrIterations);
BER_OFDM_InterferenceCancellation                       = nan(length(M_SNR_dB),NrRepetitions,NrIterations);
BER_FBMC_Aux_InterferenceCancellation_NoEdge            = nan(length(M_SNR_dB),NrRepetitions,NrIterations);
BER_FBMC_Cod_InterferenceCancellation_NoEdge            = nan(length(M_SNR_dB),NrRepetitions,NrIterations);
BER_OFDM_InterferenceCancellation_NoEdge                = nan(length(M_SNR_dB),NrRepetitions,NrIterations);
BER_FBMC_Aux_OneTapEqualizer                            = nan(length(M_SNR_dB),NrRepetitions);
BER_FBMC_Aux_OneTapEqualizer_NoEdge                     = nan(length(M_SNR_dB),NrRepetitions);
BER_FBMC_Cod_OneTapEqualizer                            = nan(length(M_SNR_dB),NrRepetitions);
BER_FBMC_Cod_OneTapEqualizer_NoEdge                     = nan(length(M_SNR_dB),NrRepetitions);
BER_OFDM_OneTapEqualizer                                = nan(length(M_SNR_dB),NrRepetitions);
BER_OFDM_OneTapEqualizer_NoEdge                         = nan(length(M_SNR_dB),NrRepetitions);

%% Start Simulation
tic
disp('Monte Carlo Simulation ...');
for i_rep = 1:NrRepetitions
    %% Update Channel
    ChannelModel.NewRealization;

    %% Binary Data
    BinaryDataStream_FBMC_Aux = randi([0 1],AuxiliaryMethod.NrDataSymbols*log2(PAM.ModulationOrder),1);
    BinaryDataStream_FBMC_Cod = randi([0 1],CodingMethod.NrDataSymbols*log2(PAM.ModulationOrder),1);
    BinaryDataStream_OFDM     = randi([0 1],NrDataSymbols_OFDM*log2(QAM.ModulationOrder),1);

    %% Data Symbols
    xD_FBMC_Aux = PAM.Bit2Symbol(BinaryDataStream_FBMC_Aux);
    xD_FBMC_Cod = PAM.Bit2Symbol(BinaryDataStream_FBMC_Cod);
    xD_OFDM     = QAM.Bit2Symbol(BinaryDataStream_OFDM);

    %% Pilot Symbols
    xP_FBMC = PAM.SymbolMapping(randi(PAM.ModulationOrder,AuxiliaryMethod.NrPilotSymbols,1));
    xP_FBMC = xP_FBMC./abs(xP_FBMC);
    xP_OFDM = QAM.SymbolMapping(randi(QAM.ModulationOrder,NrPilotSymbols,1));
    xP_OFDM = xP_OFDM./abs(xP_OFDM);

    %% Transmitted Data Symbols (Map bin to symbol)
    x_FBMC_Aux = AuxiliaryMethod.PrecodingMatrix*[xP_FBMC;xD_FBMC_Aux];
    x_FBMC_Cod = CodingMethod.PrecodingMatrix*[xP_FBMC;xD_FBMC_Cod];
    x_OFDM     = PilotMapping_OFDM*[xP_OFDM;xD_OFDM];

    %% Transmitted Signal (time domain)
    s_FBMC_Aux = G_FBMC*x_FBMC_Aux(:); % Same as "FBMC.Modulation(x_FBMC_Aux)" which is computationally more efficient. But G_FBMC is consistent with the paper.
    s_FBMC_Cod = G_FBMC*x_FBMC_Cod(:);
    s_OFDM     = G_OFDM*x_OFDM(:);

    %% Channel
    ConvolutionMatrix = ChannelModel.GetConvolutionMatrix{1};

    r_FBMC_Aux_noNoise = ConvolutionMatrix*s_FBMC_Aux;
    r_FBMC_Cod_noNoise = ConvolutionMatrix*s_FBMC_Cod;
    r_OFDM_noNoise     = ConvolutionMatrix*s_OFDM;

    %% Transmission Matrix
    D_FBMC = Q_FBMC'*ConvolutionMatrix*G_FBMC;
    D_OFDM = Q_OFDM'*ConvolutionMatrix*G_OFDM;

    %% One-Tap Channel (for perfect channel knowledge)
    h_FBMC = diag(D_FBMC);
    h_OFDM = diag(D_OFDM);

    for i_SNR = 1:length(M_SNR_dB)
        %% Add Noise
        SNR_dB  = M_SNR_dB(i_SNR);
        Pn_time = SamplingRate/(F*L)*10^(-SNR_dB/10);
        noise   = sqrt(Pn_time/2)*(randn(size(s_OFDM))+1j*randn(size(s_OFDM)));

        r_FBMC_Aux = r_FBMC_Aux_noNoise+noise;
        r_FBMC_Cod = r_FBMC_Cod_noNoise+noise;
        r_OFDM     = r_OFDM_noNoise+noise;

        %% Demodulate FBMC signal
        y_FBMC_Aux         = Q_FBMC'*r_FBMC_Aux; % Same as "FBMC.Demodulation(r_FBMC_Aux)"
        y_FBMC_Cod         = Q_FBMC'*r_FBMC_Cod; % Same as "FBMC.Demodulation(r_FBMC_Cod)"
        y_FBMC_Cod_PostCod = CodingMethod.PrecodingMatrix'*y_FBMC_Cod;
        y_OFDM             = Q_OFDM'*r_OFDM; % Same as "OFDM.Demodulation(r_OFDM)"

        %% Channel Estimation at Pilot Position
        hP_est_FBMC_Aux = y_FBMC_Aux(PilotMatrix_FBMC==1)./xP_FBMC/sqrt(Kappa_Aux);
        hP_est_FBMC_Cod = y_FBMC_Cod(PilotMatrix_FBMC==1)./xP_FBMC/sqrt(Kappa_Cod);
        hP_est_OFDM     = y_OFDM(PilotMatrix_OFDM==1)./xP_OFDM/sqrt(Kappa_OFDM);

        %% Estimate Transmit Matix
        D_FBMC_est_Aux = sum(bsxfun(@times,...
            reshape(full(W_MMSE_FBMC_Aux(:,i_SNR)),size(G_FBMC,2),size(G_FBMC,2),NrPilotSymbols),...
            reshape(hP_est_FBMC_Aux,1,1,[])),3);
        D_FBMC_est_Cod = sum(bsxfun(@times,...
            reshape(full(W_MMSE_FBMC_Cod(:,i_SNR)),size(G_FBMC,2),size(G_FBMC,2),NrPilotSymbols),...
            reshape(hP_est_FBMC_Cod,1,1,[])),3);
        D_OFDM_est = sum(bsxfun(@times,...
            reshape(full(W_MMSE_OFDM(:,i_SNR)),size(G_OFDM,2),size(G_OFDM,2),NrPilotSymbols),...
            reshape(hP_est_OFDM,1,1,[])),3);

        %% One-Tap Equalizer
        h_est_FBMC_Aux = diag(D_FBMC_est_Aux);
        x_est_OneTapEqualizer_FBMC_Aux = y_FBMC_Aux./h_est_FBMC_Aux;
        xD_est_OneTapEqualizer_FBMC_Aux = real(x_est_OneTapEqualizer_FBMC_Aux(AuxilaryPilotMatrix_FBMC(:)==0)./sqrt(AuxiliaryMethod.DataPowerReduction));
        DetectedBitStream_OneTapEqualizer_FBMC_Aux = PAM.Symbol2Bit(xD_est_OneTapEqualizer_FBMC_Aux);
        BER_FBMC_Aux_OneTapEqualizer(i_SNR,i_rep) = mean(BinaryDataStream_FBMC_Aux~=DetectedBitStream_OneTapEqualizer_FBMC_Aux);
        BER_FBMC_Aux_OneTapEqualizer_NoEdge(i_SNR,i_rep) = mean(BinaryDataStream_FBMC_Aux(ConsideredBits_FBMC_Aux)~=DetectedBitStream_OneTapEqualizer_FBMC_Aux(ConsideredBits_FBMC_Aux));

        h_est_FBMC_Cod = diag(D_FBMC_est_Cod);
        x_est_OneTapEqualizer_FBMC_Cod = CodingMethod.PrecodingMatrix'*(y_FBMC_Cod./h_est_FBMC_Cod);
        xD_est_OneTapEqualizer_FBMC_Cod = real(x_est_OneTapEqualizer_FBMC_Cod(NrPilotSymbols+1:end))/CodingMethod.DataPowerReduction;
        DetectedBitStream_OneTapEqualizer_FBMC_Cod = PAM.Symbol2Bit(xD_est_OneTapEqualizer_FBMC_Cod);
        BER_FBMC_Cod_OneTapEqualizer(i_SNR,i_rep) = mean(BinaryDataStream_FBMC_Cod~=DetectedBitStream_OneTapEqualizer_FBMC_Cod);
        BER_FBMC_Cod_OneTapEqualizer_NoEdge(i_SNR,i_rep) = mean(BinaryDataStream_FBMC_Cod(ConsideredBits_FBMC_Cod)~=DetectedBitStream_OneTapEqualizer_FBMC_Cod(ConsideredBits_FBMC_Cod));

        h_est_OFDM = diag(D_OFDM_est);
        x_est_OneTapEqualizer_OFDM = y_OFDM./h_est_OFDM;
        xD_est_OneTapEqualizer_OFDM = x_est_OneTapEqualizer_OFDM(PilotMatrix_OFDM(:)==0)./sqrt(DataPowerReduction_OFDM);
        DetectedBitStream_OneTapEqualizer_OFDM = QAM.Symbol2Bit(xD_est_OneTapEqualizer_OFDM);
        BER_OFDM_OneTapEqualizer(i_SNR,i_rep) = mean(BinaryDataStream_OFDM~=DetectedBitStream_OneTapEqualizer_OFDM);
        BER_OFDM_OneTapEqualizer_NoEdge(i_SNR,i_rep) = mean(BinaryDataStream_OFDM(ConsideredBits_OFDM)~=DetectedBitStream_OneTapEqualizer_OFDM(ConsideredBits_OFDM));

        %% One-Tap Equalizer, Perfect Channel Knowledge
        x_est_OneTapEqualizer_FBMC_Aux_PerfectCSI = y_FBMC_Aux./h_FBMC;
        xD_est_OneTapEqualizer_FBMC_Aux_PerfectCSI = real(x_est_OneTapEqualizer_FBMC_Aux_PerfectCSI(AuxilaryPilotMatrix_FBMC(:)==0)./sqrt(AuxiliaryMethod.DataPowerReduction));
        DetectedBitStream_OneTapEqualizer_FBMC_Aux_PerfectCSI = PAM.Symbol2Bit(xD_est_OneTapEqualizer_FBMC_Aux_PerfectCSI);
        BER_FBMC_Aux_OneTapEqualizer_PerfectCSI(i_SNR,i_rep) = mean(BinaryDataStream_FBMC_Aux~=DetectedBitStream_OneTapEqualizer_FBMC_Aux_PerfectCSI);
        BER_FBMC_Aux_OneTapEqualizer_PerfectCSI_NoEdge(i_SNR,i_rep) = mean(BinaryDataStream_FBMC_Aux(ConsideredBits_FBMC_Aux)~=DetectedBitStream_OneTapEqualizer_FBMC_Aux_PerfectCSI(ConsideredBits_FBMC_Aux));

        x_est_OneTapEqualizer_FBMC_Cod_PerfectCSI = CodingMethod.PrecodingMatrix'*(y_FBMC_Cod./h_FBMC);
        xD_est_OneTapEqualizer_FBMC_Cod_PerfectCSI = real(x_est_OneTapEqualizer_FBMC_Cod_PerfectCSI(NrPilotSymbols+1:end))/CodingMethod.DataPowerReduction;
        DetectedBitStream_OneTapEqualizer_FBMC_Cod_PerfectCSI = PAM.Symbol2Bit(xD_est_OneTapEqualizer_FBMC_Cod_PerfectCSI);
        BER_FBMC_Cod_OneTapEqualizer_PerfectCSI(i_SNR,i_rep) = mean(BinaryDataStream_FBMC_Cod~=DetectedBitStream_OneTapEqualizer_FBMC_Cod_PerfectCSI);
        BER_FBMC_Cod_OneTapEqualizer_PerfectCSI_NoEdge(i_SNR,i_rep) = mean(BinaryDataStream_FBMC_Cod(ConsideredBits_FBMC_Cod)~=DetectedBitStream_OneTapEqualizer_FBMC_Cod_PerfectCSI(ConsideredBits_FBMC_Cod));

        x_est_OneTapEqualizer_OFDM_PerfectCSI = y_OFDM./h_OFDM;
        xD_est_OneTapEqualizer_OFDM_PerfectCSI = x_est_OneTapEqualizer_OFDM_PerfectCSI(PilotMatrix_OFDM(:)==0)./sqrt(DataPowerReduction_OFDM);
        DetectedBitStream_OneTapEqualizer_OFDM_PerfectCSI = QAM.Symbol2Bit(xD_est_OneTapEqualizer_OFDM_PerfectCSI);
        BER_OFDM_OneTapEqualizer_PerfectCSI(i_SNR,i_rep) = mean(BinaryDataStream_OFDM~=DetectedBitStream_OneTapEqualizer_OFDM_PerfectCSI);
        BER_OFDM_OneTapEqualizer_PerfectCSI_NoEdge(i_SNR,i_rep) = mean(BinaryDataStream_OFDM(ConsideredBits_OFDM)~=DetectedBitStream_OneTapEqualizer_OFDM_PerfectCSI(ConsideredBits_OFDM));

        %% Improved Channel Estimation and Data Detection
        xD_est_FBMC_Aux_Temp = xD_est_OneTapEqualizer_FBMC_Aux; % initialize with one tap estimates
        xD_est_FBMC_Cod_Temp = xD_est_OneTapEqualizer_FBMC_Cod; % initialize with one tap estimates
        xD_est_OFDM_Temp     = xD_est_OneTapEqualizer_OFDM; % initialize with one tap estimates
        xD_est_FBMC_Aux_PerfectCSI_Temp = xD_est_OneTapEqualizer_FBMC_Aux_PerfectCSI; % initialize with one tap estimates
        xD_est_FBMC_Cod_PerfectCSI_Temp = xD_est_OneTapEqualizer_FBMC_Cod_PerfectCSI; % initialize with one tap estimates
        xD_est_OFDM_PerfectCSI_Temp     = xD_est_OneTapEqualizer_OFDM_PerfectCSI; % initialize with one tap estimates
        D_FBMC_est_Aux_Temp  = D_FBMC_est_Aux;
        D_FBMC_est_Cod_Temp  = D_FBMC_est_Cod;
        D_OFDM_est_Temp      = D_OFDM_est;
        h_est_FBMC_Aux_Temp  = h_est_FBMC_Aux;
        h_est_FBMC_Cod_Temp  = h_est_FBMC_Cod;
        h_est_OFDM_Temp      = h_est_OFDM;
        for i_iteration = 1:NrIterations
            y_FBMC_Aux_InterferenceCancellation = (y_FBMC_Aux(:) - (D_FBMC_est_Aux_Temp-diag(h_est_FBMC_Aux_Temp))*AuxiliaryMethod.PrecodingMatrix*[xP_FBMC;PAM.SymbolQuantization(xD_est_FBMC_Aux_Temp)]);
            y_FBMC_Cod_InterferenceCancellation = (y_FBMC_Cod - (D_FBMC_est_Cod_Temp-diag(h_est_FBMC_Cod_Temp))*CodingMethod.PrecodingMatrix*[xP_FBMC;PAM.SymbolQuantization(xD_est_FBMC_Cod_Temp)]);
            y_OFDM_InterferenceCancellation     = (y_OFDM - (D_OFDM_est_Temp-diag(h_est_OFDM_Temp))*PilotMapping_OFDM*[xP_OFDM;QAM.SymbolQuantization(xD_est_OFDM_Temp)]);

            % New Channel Estimates at Pilot Positions
            hP_est_FBMC_Aux_Temp = y_FBMC_Aux_InterferenceCancellation(PilotMatrix_FBMC==1)./xP_FBMC/sqrt(Kappa_Aux);
            hP_est_FBMC_Cod_Temp = y_FBMC_Cod_InterferenceCancellation(PilotMatrix_FBMC==1)./xP_FBMC/sqrt(Kappa_Cod);
            hP_est_OFDM_Temp     = y_OFDM_InterferenceCancellation(PilotMatrix_OFDM==1)./xP_OFDM/sqrt(Kappa_OFDM);

            % Improved Channel Estimation
            if i_iteration<=NrIterations/2
                D_FBMC_est_Aux_Temp = sum(bsxfun(@times,...
                    reshape(full(W_MMSE_FBMC_Aux(:,i_SNR)),size(G_FBMC,2),size(G_FBMC,2),NrPilotSymbols),...
                    reshape(hP_est_FBMC_Aux_Temp,1,1,[])),3);
                D_FBMC_est_Cod_Temp = sum(bsxfun(@times,...
                    reshape(full(W_MMSE_FBMC_Cod(:,i_SNR)),size(G_FBMC,2),size(G_FBMC,2),NrPilotSymbols),...
                    reshape(hP_est_FBMC_Cod_Temp,1,1,[])),3);
                D_OFDM_est_Temp = sum(bsxfun(@times,...
                    reshape(full(W_MMSE_OFDM(:,i_SNR)),size(G_OFDM,2),size(G_OFDM,2),NrPilotSymbols),...
                    reshape(hP_est_OFDM_Temp,1,1,[])),3);
            else
                D_FBMC_est_Aux_Temp = sum(bsxfun(@times,...
                    reshape(full(W_MMSE_noInterference_FBMC_Aux(:,i_SNR)),size(G_FBMC,2),size(G_FBMC,2),NrPilotSymbols),...
                    reshape(hP_est_FBMC_Aux_Temp,1,1,[])),3);
                D_FBMC_est_Cod_Temp = sum(bsxfun(@times,...
                    reshape(full(W_MMSE_noInterference_FBMC_Cod(:,i_SNR)),size(G_FBMC,2),size(G_FBMC,2),NrPilotSymbols),...
                    reshape(hP_est_FBMC_Cod_Temp,1,1,[])),3);
                D_OFDM_est_Temp = sum(bsxfun(@times,...
                    reshape(full(W_MMSE_noInterference_OFDM(:,i_SNR)),size(G_OFDM,2),size(G_OFDM,2),NrPilotSymbols),...
                    reshape(hP_est_OFDM_Temp,1,1,[])),3);
            end

            % One-Tap Channel
            h_est_FBMC_Aux_Temp = diag(D_FBMC_est_Aux_Temp);
            h_est_FBMC_Cod_Temp = diag(D_FBMC_est_Cod_Temp);
            h_est_OFDM_Temp     = diag(D_OFDM_est_Temp);

            x_est_FBMC_Aux_Temp = y_FBMC_Aux_InterferenceCancellation(:)./h_est_FBMC_Aux_Temp;
            x_est_FBMC_Cod_Temp = CodingMethod.PrecodingMatrix'*(y_FBMC_Cod_InterferenceCancellation./h_est_FBMC_Cod_Temp);
            x_est_OFDM_Temp     =  y_OFDM_InterferenceCancellation./h_est_OFDM_Temp;

            xD_est_FBMC_Aux_Temp = real(x_est_FBMC_Aux_Temp(AuxilaryPilotMatrix_FBMC(:)==0)./sqrt(AuxiliaryMethod.DataPowerReduction));
            xD_est_FBMC_Cod_Temp = real(x_est_FBMC_Cod_Temp(NrPilotSymbols+1:end))/CodingMethod.DataPowerReduction;
            xD_est_OFDM_Temp     = x_est_OFDM_Temp(PilotMatrix_OFDM(:)==0)./sqrt(DataPowerReduction_OFDM);

            DetectedBitStream_FBMC_Aux_Temp = PAM.Symbol2Bit(xD_est_FBMC_Aux_Temp);
            DetectedBitStream_FBMC_Cod_Temp = PAM.Symbol2Bit(xD_est_FBMC_Cod_Temp);
            DetectedBitStream_OFDM_Temp     = QAM.Symbol2Bit(xD_est_OFDM_Temp);

            BER_FBMC_Aux_InterferenceCancellation(i_SNR,i_rep,i_iteration) = mean(BinaryDataStream_FBMC_Aux~=DetectedBitStream_FBMC_Aux_Temp);
            BER_FBMC_Cod_InterferenceCancellation(i_SNR,i_rep,i_iteration) = mean(BinaryDataStream_FBMC_Cod~=DetectedBitStream_FBMC_Cod_Temp);
            BER_OFDM_InterferenceCancellation(i_SNR,i_rep,i_iteration)     = mean(BinaryDataStream_OFDM~=DetectedBitStream_OFDM_Temp);

            BER_FBMC_Aux_InterferenceCancellation_NoEdge(i_SNR,i_rep,i_iteration) = mean(BinaryDataStream_FBMC_Aux(ConsideredBits_FBMC_Aux)~=DetectedBitStream_FBMC_Aux_Temp(ConsideredBits_FBMC_Aux));
            BER_FBMC_Cod_InterferenceCancellation_NoEdge(i_SNR,i_rep,i_iteration) = mean(BinaryDataStream_FBMC_Cod(ConsideredBits_FBMC_Cod)~=DetectedBitStream_FBMC_Cod_Temp(ConsideredBits_FBMC_Cod));
            BER_OFDM_InterferenceCancellation_NoEdge(i_SNR,i_rep,i_iteration)     = mean(BinaryDataStream_OFDM(ConsideredBits_OFDM)~=DetectedBitStream_OFDM_Temp(ConsideredBits_OFDM));

            % Perfect Channel Knoweledge
            y_FBMC_Aux_InterferenceCancellation_PerfectCSI = (y_FBMC_Aux(:) - (D_FBMC-diag(h_FBMC))*AuxiliaryMethod.PrecodingMatrix*[xP_FBMC;PAM.SymbolQuantization(xD_est_FBMC_Aux_PerfectCSI_Temp)]);
            y_FBMC_Cod_InterferenceCancellation_PerfectCSI = (y_FBMC_Cod - (D_FBMC-diag(h_FBMC))*CodingMethod.PrecodingMatrix*[xP_FBMC;PAM.SymbolQuantization(xD_est_FBMC_Cod_PerfectCSI_Temp)]);
            y_OFDM_InterferenceCancellation_PerfectCSI     = (y_OFDM - (D_OFDM-diag(h_OFDM))*PilotMapping_OFDM*[xP_OFDM;QAM.SymbolQuantization(xD_est_OFDM_PerfectCSI_Temp)]);

            x_est_FBMC_Aux_PerfectCSI_Temp = y_FBMC_Aux_InterferenceCancellation_PerfectCSI./h_FBMC;
            xD_est_FBMC_Aux_PerfectCSI_Temp = real(x_est_FBMC_Aux_PerfectCSI_Temp(AuxilaryPilotMatrix_FBMC(:)==0)./sqrt(AuxiliaryMethod.DataPowerReduction));
            DetectedBitStream_FBMC_Aux_PerfectCSI_Temp = PAM.Symbol2Bit(xD_est_FBMC_Aux_PerfectCSI_Temp);
            BER_FBMC_Aux_PerfectCSI_InterferenceCancellation(i_SNR,i_rep,i_iteration) = mean(BinaryDataStream_FBMC_Aux~=DetectedBitStream_FBMC_Aux_PerfectCSI_Temp);
            BER_FBMC_Aux_PerfectCSI_InterferenceCancellation_NoEdge(i_SNR,i_rep,i_iteration) = mean(BinaryDataStream_FBMC_Aux(ConsideredBits_FBMC_Aux)~=DetectedBitStream_FBMC_Aux_PerfectCSI_Temp(ConsideredBits_FBMC_Aux));

            x_est_FBMC_Cod_PerfectCSI_Temp = CodingMethod.PrecodingMatrix'*(y_FBMC_Cod_InterferenceCancellation_PerfectCSI./h_FBMC);
            xD_est_FBMC_Cod_PerfectCSI_Temp = real(x_est_FBMC_Cod_PerfectCSI_Temp(NrPilotSymbols+1:end))/CodingMethod.DataPowerReduction;
            DetectedBitStream_FBMC_Cod_PerfectCSI_Temp = PAM.Symbol2Bit(xD_est_FBMC_Cod_PerfectCSI_Temp);
            BER_FBMC_Cod_PerfectCSI_InterferenceCancellation(i_SNR,i_rep,i_iteration) = mean(BinaryDataStream_FBMC_Cod~=DetectedBitStream_FBMC_Cod_PerfectCSI_Temp);
            BER_FBMC_Cod_PerfectCSI_InterferenceCancellation_NoEdge(i_SNR,i_rep,i_iteration) = mean(BinaryDataStream_FBMC_Cod(ConsideredBits_FBMC_Cod)~=DetectedBitStream_FBMC_Cod_PerfectCSI_Temp(ConsideredBits_FBMC_Cod));

            x_est_OFDM_PerfectCSI_Temp = y_OFDM_InterferenceCancellation_PerfectCSI./h_OFDM;
            xD_est_OFDM_PerfectCSI_Temp = x_est_OFDM_PerfectCSI_Temp(PilotMatrix_OFDM(:)==0)./sqrt(DataPowerReduction_OFDM);
            DetectedBitStream_OFDM_PerfectCSI_Temp = QAM.Symbol2Bit(xD_est_OFDM_PerfectCSI_Temp);
            BER_OFDM_PerfectCSI_InterferenceCancellation(i_SNR,i_rep,i_iteration) = mean(BinaryDataStream_OFDM~=DetectedBitStream_OFDM_PerfectCSI_Temp);
            BER_OFDM_PerfectCSI_InterferenceCancellation_NoEdge(i_SNR,i_rep,i_iteration) = mean(BinaryDataStream_OFDM(ConsideredBits_OFDM)~=DetectedBitStream_OFDM_PerfectCSI_Temp(ConsideredBits_OFDM));

        end

    end
    TimeNeededSoFar = toc;
    disp([int2str(i_rep/NrRepetitions*100) '% Completed! Time Left: ' int2str(TimeNeededSoFar/i_rep*(NrRepetitions-i_rep)/60) 'min, corresponding to approx. '  int2str(TimeNeededSoFar/i_rep*(NrRepetitions-i_rep)/3600) 'hour']);

    %% Plot results
    % OFDM
    figure(2);
    Markersize = 4;
    hold off;
    semilogy(M_SNR_dB_morePoints, BitErrorProbability,'Color',[1 1 1]*0.75);
    hold on;
    semilogy(M_SNR_dB, nanmean(BER_OFDM_PerfectCSI_InterferenceCancellation(:,:,end),2),'-x black','Markersize',Markersize);
    semilogy(M_SNR_dB, nanmean(BER_OFDM_InterferenceCancellation(:,:,end),2),'-s magenta','Markersize',Markersize);
    semilogy(M_SNR_dB, nanmean(BER_OFDM_InterferenceCancellation_NoEdge(:,:,end),2),'-o blue','Markersize',Markersize);
    semilogy(M_SNR_dB, nanmean(BER_OFDM_OneTapEqualizer_PerfectCSI,2),'-x','Color',[1 1 0]*0.7,'Markersize',Markersize);
    semilogy(M_SNR_dB, nanmean(BER_OFDM_OneTapEqualizer,2),'-s red','Markersize',Markersize);
    ylim([10^-2 0.5]);
    title(['OFDM, Realization ' int2str(i_rep) '/'  int2str(NrRepetitions)])
    legend({'Doubly-Flat Theory','Cancellation (Perfect CSI)','Cancellation','Cancellation (no Edges)','One-Tap (Perfect CSI)','One-Tap'});
    ylabel('Bit Error Ratio');
    xlabel('Signal-to-Noise Ratio [dB]');

    % FBMC, Auxiliary Method
    figure(3);
    hold off;
    semilogy(M_SNR_dB_morePoints, BitErrorProbability,'Color',[1 1 1]*0.75);
    hold on;
    semilogy(M_SNR_dB, nanmean(BER_FBMC_Aux_PerfectCSI_InterferenceCancellation(:,:,end),2),'-x black','Markersize',Markersize);
    semilogy(M_SNR_dB, nanmean(BER_FBMC_Aux_InterferenceCancellation(:,:,end),2),'-s magenta','Markersize',Markersize);
    semilogy(M_SNR_dB, nanmean(BER_FBMC_Aux_InterferenceCancellation_NoEdge(:,:,end),2),'-o blue','Markersize',Markersize);
    semilogy(M_SNR_dB, nanmean(BER_FBMC_Aux_OneTapEqualizer_PerfectCSI,2),'-x','Color',[1 1 0]*0.7,'Markersize',Markersize);
    semilogy(M_SNR_dB, nanmean(BER_FBMC_Aux_OneTapEqualizer,2),'-s red','Markersize',Markersize);
    ylim([10^-2 0.5]);
    semilogy([PlotIterationStepsSNRdB PlotIterationStepsSNRdB],[10^-2 10^-1],'Color',[1 1 1]*0.5,'Linewidth',1);
    title(['FBMC Auxiliary Symbols, Realization ' int2str(i_rep) '/'  int2str(NrRepetitions)])
    legend({'Doubly-Flat Theory','Cancellation (Perfect CSI)','Cancellation','Cancellation (no Edges)','One-Tap (Perfect CSI)','One-Tap'});
    ylabel('Bit Error Ratio');
    xlabel('Signal-to-Noise Ratio [dB]');

    % FBMC, Data Spreading Method
    figure(4);
    hold off;
    semilogy(M_SNR_dB_morePoints, BitErrorProbability,'Color',[1 1 1]*0.75);
    hold on;
    semilogy(M_SNR_dB, nanmean(BER_FBMC_Cod_PerfectCSI_InterferenceCancellation(:,:,end),2),'-x black','Markersize',Markersize);
    semilogy(M_SNR_dB, nanmean(BER_FBMC_Cod_InterferenceCancellation(:,:,end),2),'-s magenta','Markersize',Markersize);
    semilogy(M_SNR_dB, nanmean(BER_FBMC_Cod_InterferenceCancellation_NoEdge(:,:,end),2),'-o blue','Markersize',Markersize);
    semilogy(M_SNR_dB, nanmean(BER_FBMC_Cod_OneTapEqualizer_PerfectCSI,2),'-x','Color',[1 1 0]*0.7,'Markersize',Markersize);
    semilogy(M_SNR_dB, nanmean(BER_FBMC_Cod_OneTapEqualizer,2),'-s red','Markersize',Markersize);
    ylim([10^-2 0.5]);
    title(['FBMC Data Spreading, Realization ' int2str(i_rep) '/'  int2str(NrRepetitions)])
    legend({'Doubly-Flat Theory','Cancellation (Perfect CSI)','Cancellation','Cancellation (no Edges)','One-Tap (Perfect CSI)','One-Tap'});
    ylabel('Bit Error Ratio');
    xlabel('Signal-to-Noise Ratio [dB]');

    % FBMC, Auxiliary Method, BER over Interation
    figure(5);
    hold off;
    semilogy(0:NrIterations, repmat(BitErrorProbability(find(PlotIterationStepsSNRdB==M_SNR_dB_morePoints)),NrIterations+1,1),'Color',[1 1 1]*0.75);
    hold on;
    Index = find(PlotIterationStepsSNRdB==M_SNR_dB);
    semilogy(0:NrIterations, [nanmean(BER_FBMC_Aux_OneTapEqualizer_PerfectCSI(Index,:),2);squeeze(nanmean(BER_FBMC_Aux_PerfectCSI_InterferenceCancellation(Index,:,:),2))],'-x black','Markersize',Markersize);
    semilogy(0:NrIterations, [nanmean(BER_FBMC_Aux_OneTapEqualizer(Index,:),2);squeeze(nanmean(BER_FBMC_Aux_InterferenceCancellation(Index,:,:),2))],'-s magenta','Markersize',Markersize);
    semilogy(0:NrIterations, [nanmean(BER_FBMC_Aux_OneTapEqualizer_NoEdge(Index,:),2);squeeze(nanmean(BER_FBMC_Aux_InterferenceCancellation_NoEdge(Index,:,:),2))],'-o blue','Markersize',Markersize);
    semilogy(0:NrIterations, repmat(nanmean(BER_FBMC_Aux_OneTapEqualizer_PerfectCSI(Index,:),2),NrIterations+1,1),'-x','Color',[1 1 0]*0.7,'Markersize',Markersize);
    semilogy(0:NrIterations, repmat(nanmean(BER_FBMC_Aux_OneTapEqualizer(Index,:),2),NrIterations+1,1),'-s red','Markersize',Markersize);
    title(['FBMC Auxiliary Symbols, Realization ' int2str(i_rep) '/'  int2str(NrRepetitions)])
    legend({'Doubly-Flat Theory','Cancellation (Perfect CSI)','Cancellation','Cancellation (no Edges)','One-Tap (Perfect CSI)','One-Tap'});
    set(gca, 'XTick',0:NrIterations);
    ylabel('Bit Error Ratio');
    xlabel('Iteration Step i');

    pause(0.01);

end

%% Plot Additional Information
fprintf('=============================\n');
fprintf('========= Data Rate =========\n');
fprintf('OFDM       |%7.2f Mbit/s  | \n', length(BinaryDataStream_OFDM)     / (OFDM.PHY.TimeSpacing*OFDM.Nr.MCSymbols)/1e6   );
fprintf('FBMC, Aux. |%7.2f Mbit/s  | \n', length(BinaryDataStream_FBMC_Aux) / (OFDM.PHY.TimeSpacing*OFDM.Nr.MCSymbols)/1e6   );
fprintf('FBMC, Cod. |%7.2f Mbit/s  | \n', length(BinaryDataStream_FBMC_Cod) / (OFDM.PHY.TimeSpacing*OFDM.Nr.MCSymbols)/1e6   );
fprintf('=============================\n');

% The power is normalized so that the average transmit power is one
fprintf('================================================\n');
fprintf('============== Relative SNR Shift ==============\n');
fprintf('================================================\n');
fprintf('           |    SNR    |  Data SNR  | Pilot SNR |\n');
fprintf('OFDM       |   %2.1fdB   |   %2.1fdB   |   %2.1fdB   |\n', 0, 10*log10(DataPowerReduction_OFDM), 10*log10(Kappa_OFDM))
fprintf('FBMC, Aux. |   %2.1fdB   |   %2.1fdB   |   %2.1fdB   |\n', 0, 10*log10(AuxiliaryMethod.DataPowerReduction), 10*log10(Kappa_Aux/2))
fprintf('FBMC, Cod. |   %2.1fdB   |   %2.1fdB   |   %2.1fdB   |\n', 0, 10*log10(CodingMethod.DataPowerReduction), 10*log10(Kappa_Cod/2))
fprintf('================================================\n');


function mask = compute_conv2_valid_mask(imsz, fsz)

% compute a mask of size IMSZ with 1 entry at valid pixel positions and 0
% invalid oens
%
%   Author:  Stefan Roth, Department of Computer Science, TU Darmstadt
%   Contact: sroth@cs.tu-darmstadt.de
%   $Date:  $
%   $Revision: $

% Copyright 2004-2007, Brown University, Providence, RI. USA
% Copyright 2007-2010 TU Darmstadt, Darmstadt, Germany.
%
%                          All Rights Reserved
%
% All commercial use of this software, whether direct or indirect, is
% strictly prohibited including, without limitation, incorporation into in
% a commercial product, use in a commercial service, or production of other
% artifacts for commercial purposes.
%
% Permission to use, copy, modify, and distribute this software and its
% documentation for research purposes is hereby granted without fee,
% provided that the above copyright notice appears in all copies and that
% both that copyright notice and this permission notice appear in
% supporting documentation, and that the name of the author and Brown
% University not be used in advertising or publicity pertaining to
% distribution of the software without specific, written prior permission.
%
% For commercial uses contact the Technology Venture Office of Brown University
%
% THE AUTHOR AND BROWN UNIVERSITY DISCLAIM ALL WARRANTIES WITH REGARD TO
% THIS SOFTWARE, INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND
% FITNESS FOR ANY PARTICULAR PURPOSE.  IN NO EVENT SHALL THE AUTHOR OR
% BROWN UNIVERSITY BE LIABLE FOR ANY SPECIAL, INDIRECT OR CONSEQUENTIAL
% DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR
% PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS
% ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF
% THIS SOFTWARE.

mask = ones(imsz);

h1   = floor(fsz(1)/2);
if fsz(1) == 1
    ;
elseif fsz(1) == 2
    mask(end,:) = 0;
elseif mod(fsz(1), 2) == 0
    mask(1:h1-1, :)         = 0;
    mask(end-h1+1:end, :)   = 0;
else
    mask(1:h1, :)    = 0;
    mask(end-h1+1:end, :)   = 0;
end;

h2   = floor(fsz(2)/2);

if fsz(2) == 1
    ;
elseif fsz(2) == 2
    mask(:, end) = 0;
elseif mod(fsz(2), 2) == 0
    mask(:, 1:h2-1)    = 0;
    mask(:, end-h2+1:end)   = 0;
else
    mask(:, 1:h2)    = 0;
    mask(:, end-h2+1:end)   = 0;
end;


function p = prior_t(varargin)
%PRIOR_T  Student-t prior structure
%
%  Description
%    P = PRIOR_T('PARAM1', VALUE1, 'PARAM2', VALUE2, ...)
%    creates Student's t-distribution prior structure in which the
%    named parameters have the specified values. Any unspecified
%    parameters are set to default values.
%
%    P = PRIOR_T(P, 'PARAM1', VALUE1, 'PARAM2', VALUE2, ...)
%    modify a prior structure with the named parameters altered
%    with the specified values.
%
%    The parameterization is as in Gelman, Carlin, Stern, Dunson, Vehtari,
%    and Rubin (2013). Bayesian Data Analysis, third edition.
%
%    Parameters for Student-t prior [default]
%      mu       - location [0]
%      s2       - scale [1]
%      nu       - degrees of freedom [4]
%      mu_prior - prior for mu [prior_fixed]
%      s2_prior - prior for s2 [prior_fixed]
%      nu_prior - prior for nu [prior_fixed]
%
%  See also
%    PRIOR_*
%
% Copyright (c) 2000-2001,2010 Aki Vehtari
% Copyright (c) 2009 Jarno Vanhatalo
% Copyright (c) 2010 Jaakko Riihimäki

% This software is distributed under the GNU General Public
% License (version 3 or later); please refer to the file
% License.txt, included with the software, for details.

  ip=inputParser;
  ip.FunctionName = 'PRIOR_T';
  ip.addOptional('p', [], @isstruct);
  ip.addParamValue('mu',0, @(x) isscalar(x));
  ip.addParamValue('mu_prior',[], @(x) isstruct(x) || isempty(x));
  ip.addParamValue('s2',1, @(x) isscalar(x) && x>0);
  ip.addParamValue('s2_prior',[], @(x) isstruct(x) || isempty(x));
  ip.addParamValue('nu',4, @(x) isscalar(x) && x>0);
  ip.addParamValue('nu_prior',[], @(x) isstruct(x) || isempty(x));
  ip.parse(varargin{:});
  p=ip.Results.p;

  if isempty(p)
    init=true;
    p.type = 't';
  else
    if ~isfield(p,'type') && ~isequal(p.type,'t')
      error('First argument does not seem to be a valid prior structure')
    end
    init=false;
  end

  % Initialize parameters
  if init || ~ismember('mu',ip.UsingDefaults)
    p.mu = ip.Results.mu;
  end
  if init || ~ismember('s2',ip.UsingDefaults)
    p.s2 = ip.Results.s2;
  end
  if init || ~ismember('nu',ip.UsingDefaults)
    p.nu = ip.Results.nu;
  end
  % Initialize prior structure
  if init
    p.p=[];
  end
  if init || ~ismember('mu_prior',ip.UsingDefaults)
    p.p.mu=ip.Results.mu_prior;
  end
  if init || ~ismember('s2_prior',ip.UsingDefaults)
    p.p.s2=ip.Results.s2_prior;
  end
  if init || ~ismember('nu_prior',ip.UsingDefaults)
    p.p.nu=ip.Results.nu_prior;
  end

  if init
    % set functions
    p.fh.pak = @prior_t_pak;
    p.fh.unpak = @prior_t_unpak;
    p.fh.lp = @prior_t_lp;
    p.fh.lpg = @prior_t_lpg;
    p.fh.recappend = @prior_t_recappend;
  end

end

function [w, s, h] = prior_t_pak(p)
% This is a mandatory subfunction used for example
% in energy and gradient computations.

  w=[];
  s={};
  h=[];
  if ~isempty(p.p.mu)
    w = p.mu;
    s=[s; 't.mu'];
    h = 1;
  end
  if ~isempty(p.p.s2)
    w = [w log(p.s2)];
    s=[s; 'log(t.s2)'];
    h = [h 1];
  end
  if ~isempty(p.p.nu)
    w = [w log(p.nu)];
    s=[s; 'log(t.nu)'];
    h = [h 1];
  end
end

function [p, w] = prior_t_unpak(p, w)
% This is a mandatory subfunction used for example
% in energy and gradient computations.

  if ~isempty(p.p.mu)
    i1=1;
    p.mu = w(i1);
    w = w(i1+1:end);
  end
  if ~isempty(p.p.s2)
    i1=1;
    p.s2 = exp(w(i1));
    w = w(i1+1:end);
  end
  if ~isempty(p.p.nu)
    i1=1;
    p.nu = exp(w(i1));
    w = w(i1+1:end);
  end
end

function lp = prior_t_lp(x, p)
% This is a mandatory subfunction used for example
% in energy computations.

  lp=sum(gammaln((p.nu+1)./2) -gammaln(p.nu./2) -0.5*log(p.nu.*pi.*p.s2) -(p.nu+1)./2.*log(1+(x-p.mu).^2./p.nu./p.s2));

  if ~isempty(p.p.mu)
    lp = lp + p.p.mu.fh.lp(p.mu, p.p.mu);
  end
  if ~isempty(p.p.s2)
    lp = lp + p.p.s2.fh.lp(p.s2, p.p.s2) +log(p.s2);
  end
  if ~isempty(p.p.nu)
    lp = lp + p.p.nu.fh.lp(p.nu, p.p.nu) +log(p.nu);
  end
end

function lpg = prior_t_lpg(x, p)
% This is a mandatory subfunction used for example
% in gradient computations.

 %lpg=(p.nu+1)./p.nu .* (x-p.mu)./p.s2 ./ (1 + (x-p.mu).^2./p.nu./p.s2);
  lpg=-(p.nu+1).* (x-p.mu) ./ (p.nu.*p.s2 + (x-p.mu).^2);

  if ~isempty(p.p.mu)
    lpgmu = sum( (p.nu+1).* (x-p.mu) ./ (p.nu.*p.s2 + (x-p.mu).^2) ) + p.p.mu.fh.lpg(p.mu, p.p.mu);
    lpg = [lpg lpgmu];
  end
  if ~isempty(p.p.s2)
    lpgs2 = (sum( -1./(2.*p.s2) +((p.nu + 1).*(p.mu - x).^2)./(2.*p.s2.*((p.mu-x).^2 + p.nu.*p.s2))) + p.p.s2.fh.lpg(p.s2, p.p.s2)).*p.s2 + 1;
    lpg = [lpg lpgs2];
  end
  if ~isempty(p.p.nu)
    lpgnu = (0.5*sum( digamma1((p.nu+1)./2)-digamma1(p.nu./2)-1./p.nu-log(1+(x-p.mu).^2./p.nu./p.s2)+(p.nu+1)./(1+(x-p.mu).^2./p.nu./p.s2).*(x-p.mu).^2./p.s2./p.nu.^2) + p.p.nu.fh.lpg(p.nu, p.p.nu)).*p.nu + 1;
    lpg = [lpg lpgnu];
  end
end

function rec = prior_t_recappend(rec, ri, p)
% This subfunction is needed when using MCMC sampling (gp_mc).

% The parameters are not sampled in any case.
  rec = rec;
  if ~isempty(p.p.mu)
    rec.mu(ri,:) = p.mu;
  end
  if ~isempty(p.p.s2)
    rec.s2(ri,:) = p.s2;
  end
  if ~isempty(p.p.nu)
    rec.nu(ri,:) = p.nu;
  end
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Spherical Harmonic Modeling and Analysis Toolkit (SPHARM-MAT) is a 3D
% shape modeling and analysis toolkit.
% It is a software package developed at Shenlab in Center for Neuroimaging,
% Indiana University (SpharmMat@gmail.com, http://www.iupui.edu/~shenlab/)
% It is available to the scientific community as copyright freeware
% under the terms of the GNU General Public Licence.
%
% Copyright 2009, 2010, ShenLab, Center for Neuroimaging, Indiana University
%
% This file is part of SPHARM-MAT.
%
% SPHARM-MAT is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% SPHARM-MAT is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with SPHARM-MAT. If not, see <http://www.gnu.org/licenses/>.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% ============================================
% align_cps.m
%
% Goal: Align P to X using ICP
%
% Li Shen
% 07/29/2003 - customized using Hany Farid's icp code.
% 01/08/2004 - customized again for corresponding point set registration.

function [P,M] = align_cps(P,X)

disp('running cps_align');

M = eye(4);

%%%%%%%%%%%%%%%%%%%%
% Added by Li Shen
%%%%%%%%%%%%%%%%%%%%
% center P and X first
[P,cmP] = center(P);
[X,cmX] = center(X);

Xc = X;

muP   = mean(P);
muXc  = mean(Xc);
N     = length(P);
sig   = zeros(3);
for k = 1 : N
  sig = sig + (P(k,:)' * Xc(k,:));
end
sig   = sig/N;
sig   = sig - (muP' * muXc); % cross-covariance matrix
A     = sig - sig';
delta = [A(2,3) ; A(3,1) ; A(1,2)];
Q     = [trace(sig)  delta' ; delta  sig+sig'-trace(sig)*eye(3)];
[V,D] = eig(Q);
[val,ind] = max(diag(D));
qR    = V(:,ind); % optimal rotation
q0    = qR(1);
q1    = qR(2);
q2    = qR(3);
q3    = qR(4);
R     = [q0^2+q1^2-q2^2-q3^2  2*(q1*q2 - q0*q3)  2*(q1*q3 + q0*q2) ; ...
    2*(q1*q2 + q0*q3)  q0^2+q2^2-q1^2-q3^2  2*(q2*q3 - q0*q1) ; ...
    2*(q1*q3 - q0*q2) 2*(q2*q3 + q0*q1)  q0^2+q3^2-q1^2-q2^2];
qT    = muXc' - R*muP'; % optimal translation

Pnew  = (R*P')';
diff  = mean( sum(((Pnew - P).^2)') );

P = Pnew;

P(:,1) = P(:,1) + qT(1);
P(:,2) = P(:,2) + qT(2);
P(:,3) = P(:,3) + qT(3);

%%%%%%%%%%%%%%%%%%%%
% Added by Li Shen
%%%%%%%%%%%%%%%%%%%%
% recover P and X
P(:,1) = P(:,1) + cmX(1);
P(:,2) = P(:,2) + cmX(2);
P(:,3) = P(:,3) + cmX(3);
X(:,1) = X(:,1) + cmX(1);
X(:,2) = X(:,2) + cmX(2);
X(:,3) = X(:,3) + cmX(3);

Mr = eye(4); Mr(1:3,1:3) = R;
Mt = eye(4); Mt(1:3,4) = qT(1:3)+cmX'-cmP';
M = Mt*Mr*M;

return;

%
% center point set
%

function [P,muP] = center(P)

n = size(P,1);
muP = mean(P);
P = P - muP(ones(1,n),:);

return;


function [est_C_b_e_new,est_v_eb_e_new,est_r_eb_e_new,est_IMU_bias_new,...
            P_matrix_new, corrections, Phi_matrix, Q_matrix] = ...
         LC_KF_Epoch(GNSS_epoch, GNSS_r_eb_e,GNSS_v_eb_e,tor_s, est_C_b_e_old,...
                     est_v_eb_e_old,est_r_eb_e_old,est_IMU_bias_old,...
                     P_matrix_old,meas_f_ib_b,est_L_b_old,LC_KF_config,...
                     Q_matrix, R_matrix, meas_omega_ib_b)
%LC_KF_Epoch - Implements one cycle of the loosely coupled INS/GNSS
% Kalman filter plus closed-loop correction of all inertial states
%
% Software for use with "Principles of GNSS, Inertial, and Multisensor
% Integrated Navigation Systems," Second Edition.
%
% This function created 12/4/2012 by Paul Groves
%
% Inputs:
%   GNSS_r_eb_e           GNSS estimated ECEF user position (m)
%   GNSS_v_eb_e           GNSS estimated ECEF user velocity (m/s)
%   tor_s                 propagation interval (s)
%   est_C_b_e_old         prior estimated body to ECEF coordinate
%                         transformation matrix
%   est_v_eb_e_old        prior estimated ECEF user velocity (m/s)
%   est_r_eb_e_old        prior estimated ECEF user position (m)
%   est_IMU_bias_old      prior estimated IMU biases (body axes)
%   P_matrix_old          previous Kalman filter error covariance matrix
%   meas_f_ib_b           measured specific force
%   est_L_b_old           previous latitude solution
%   LC_KF_config
%     .gyro_noise_PSD     Gyro noise PSD (rad^2/s)
%     .accel_noise_PSD    Accelerometer noise PSD (m^2 s^-3)
%     .accel_bias_PSD     Accelerometer bias random walk PSD (m^2 s^-5)
%     .gyro_bias_PSD      Gyro bias random walk PSD (rad^2 s^-3)
%     .pos_meas_SD            Position measurement noise SD per axis (m)
%     .vel_meas_SD            Velocity measurement noise SD per axis (m/s)
%
% Outputs:
%   est_C_b_e_new     updated estimated body to ECEF coordinate
%                      transformation matrix
%   est_v_eb_e_new    updated estimated ECEF user velocity (m/s)
%   est_r_eb_e_new    updated estimated ECEF user position (m)
%   est_IMU_bias_new  updated estimated IMU biases
%     Rows 1-3          estimated accelerometer biases (m/s^2)
%     Rows 4-6          estimated gyro biases (rad/s)
%   P_matrix_new      updated Kalman filter error covariance matrix

% Copyright 2012, Paul Groves
% License: BSD; see GrovesCode/license.txt for details
%
% Edits made by Adam Werries where noted as (Werries), Copyright 2015
% GNSS_epoch
% GNSS_r_eb_e
% GNSS_v_eb_e
% tor_s
% est_C_b_e_old
% est_v_eb_e_old
% est_r_eb_e_old
% est_IMU_bias_old
% P_matrix_old
% meas_f_ib_b
% est_L_b_old
% Q_matrix
% R_matrix
% meas_omega_ib_b
% Constants (sone of these could be changed to inputs at a later date)
c = 299792458; % Speed of light in m/s
omega_ie = 7.292115E-5;  % Earth rotation rate in rad/s
R_0 = 6378137; %WGS84 Equatorial radius in meters
e = 0.0818191908425; %WGS84 eccentricity
Lab = LC_KF_config.lever_arm;
% Begins

% Skew symmetric matrix of Earth rate
Omega_ie = Skew_symmetric([0,0,omega_ie]);

% SYSTEM PROPAGATION PHASE

% 1. Determine transition matrix using (14.50) (first-order approx)
F_21 = -Skew_symmetric(est_C_b_e_old * meas_f_ib_b);
Phi_matrix = eye(15);
Phi_matrix(1:3,1:3) = Phi_matrix(1:3,1:3) - Omega_ie * tor_s;
Phi_matrix(1:3,13:15) = est_C_b_e_old * tor_s;
Phi_matrix(4:6,1:3) = tor_s * F_21;
Phi_matrix(4:6,4:6) = Phi_matrix(4:6,4:6) - 2 * Omega_ie * tor_s;
geocentric_radius = R_0 / sqrt(1 - (e * sin(est_L_b_old))^2) *...
    sqrt(cos(est_L_b_old)^2 + (1 - e^2)^2 * sin(est_L_b_old)^2); % from (2.137)
Phi_matrix(4:6,7:9) = -tor_s * 2 * Gravity_ECEF(est_r_eb_e_old) /...
    geocentric_radius * est_r_eb_e_old' / sqrt (est_r_eb_e_old' *...
    est_r_eb_e_old);
Phi_matrix(4:6,10:12) = est_C_b_e_old * tor_s;
Phi_matrix(7:9,4:6) = eye(3) * tor_s;

% If we were supposed to adapt Q_matrix last iteration, don't replace it.
if GNSS_epoch <= LC_KF_config.n + 1
    % 2. Determine approximate system noise covariance matrix using (14.82)
    Srg = LC_KF_config.gyro_noise_PSD;
    Sra = LC_KF_config.accel_noise_PSD;
    Sbad = LC_KF_config.accel_bias_PSD;
    Sbgd = LC_KF_config.gyro_bias_PSD;
    Q11 = eye(3) * (Srg * tor_s + Sbgd*tor_s^3/3);
    Q21 = (Srg*tor_s^2 / 2 + Sbgd*tor_s^4 / 4) * F_21;
    Q31 = (Srg*tor_s^3 / 3 + Sbgd*tor_s^5 / 5) * F_21;
    Q15 = Sbgd*tor_s^2*est_C_b_e_old / 2;
    Q22 = (Sra*tor_s + Sbad*tor_s^3/3)*eye(3) + (Srg*tor_s^3/3 + Sbgd*tor_s^5/5)*(F_21*F_21');
    Q32 = (Sra*tor_s^2/2 + Sbad*tor_s^4/4)*eye(3) + (Srg*tor_s^4/4 + Sbgd*tor_s^6/6)*(F_21*F_21');
    Q24 = Sbad*tor_s^2*est_C_b_e_old / 2;
    Q25 = Sbgd*tor_s^3*F_21*est_C_b_e_old / 3;
    Q33 = (Sra*tor_s^3/3 + Sbad*tor_s^5/5)*eye(3) + (Srg*tor_s^5/5 + Sbgd*tor_s^7/7)*(F_21*F_21');
    Q34 = Sbad*tor_s^3*est_C_b_e_old/3;
    Q35 = Sbgd*tor_s^4*F_21*est_C_b_e_old/4;
    Q44 = Sbad*tor_s*eye(3);
    Q55 = Sbgd*tor_s*eye(3);
    Q_matrix = [Q11      Q21' Q31' zeros(3) Q15;
                Q21      Q22  Q32' Q24      Q25;
                Q31      Q32  Q33  Q34      Q35;
                zeros(3) Q24' Q34' Q44      zeros(3);
                Q15'     Q25' Q35' zeros(3) Q55;];
else
    Q_matrix = tor_s*Q_matrix;
end
[~,p] = chol(Q_matrix);
if p ~= 0
    Q_matrix = nearestSPD(Q_matrix);
end
% 3. Propagate state estimates using (3.14) noting that all states are zero
% due to closed-loop correction.
x_est_propagated(1:15,1) = 0;

% 4. Propagate state estimation error covariance matrix using (3.46)
P_matrix_propagated = Phi_matrix * P_matrix_old * Phi_matrix' + Q_matrix;

% MEASUREMENT UPDATE PHASE
% 5. Set-up measurement matrix using (14.115)
H_matrix = zeros(6,15);
H_matrix(1:3,1:3) = Skew_symmetric(est_C_b_e_old*Lab);
H_matrix(4:6,1:3) = Skew_symmetric(est_C_b_e_old*cross(meas_omega_ib_b,Lab) ...
                                   - Omega_ie*est_C_b_e_old*Lab);
H_matrix(1:3,7:9) = -eye(3);
H_matrix(4:6,4:6) = -eye(3);
H_matrix(4:6,13:15) = est_C_b_e_old*Skew_symmetric(Lab);

% 6. Set-up measurement noise covariance matrix assuming all components of
% GNSS position and velocity are independent and have equal variance.

% 7. Calculate Kalman gain using (3.21)
K_matrix = P_matrix_propagated * H_matrix' / (H_matrix * P_matrix_propagated * H_matrix' + R_matrix);

% 8. Formulate measurement innovations using (14.102)
% Lever arm from INS to GPS
delta_z(1:3,1) = GNSS_r_eb_e - est_r_eb_e_old - est_C_b_e_old*Lab - est_C_b_e_old*LC_KF_config.gps_correction;
delta_z(4:6,1) = GNSS_v_eb_e - est_v_eb_e_old ...
                  - est_C_b_e_old*cross(meas_omega_ib_b,Lab) ...
                  + Omega_ie*est_C_b_e_old*Lab;

% 9. Update state estimates using (3.24)
x_est_new = x_est_propagated + K_matrix * delta_z;
corrections = x_est_new;

% 10. Update state estimation error covariance matrix using (3.25)
P_matrix_new = (eye(15) - K_matrix * H_matrix) * P_matrix_propagated;
[~,p] = chol(P_matrix_new);
if p ~= 0
    P_matrix_new = nearestSPD(P_matrix_new);
end
% CLOSED-LOOP CORRECTION

% Correct attitude, velocity, and position using (14.7-9)
est_C_b_e_new = (eye(3) - Skew_symmetric(x_est_new(1:3))) * est_C_b_e_old;
est_v_eb_e_new = est_v_eb_e_old - x_est_new(4:6);
est_r_eb_e_new = est_r_eb_e_old - x_est_new(7:9);

% Return residual error
% residual = [GNSS_r_eb_e; GNSS_v_eb_e] - [est_r_eb_e_new; est_v_eb_e_new];
% residual = [est_r_eb_e_old; est_v_eb_e_old] - [est_r_eb_e_new; est_v_eb_e_new];
% Update IMU bias estimates
est_IMU_bias_new = est_IMU_bias_old + x_est_new(10:15);

% Ends


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% vertex.m %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% function [n0,x,y,x1,x2,f1,f2] = vertex(j,n,u,v,v1,x0,f0,ipar,isplit,
% ichild,z,f,l,L)
% computes the base vertex x and the opposite vertex y of the box # j
% of MCS and the 'neighboring vertices' x1 and x2 and their function
% values f1 and f2 needed for separable quadratic interpolation
% Input:
% j       label of the box whose vertices etc. are to be computed
%         (in the main program mcs.m)
% n       dimension of the problem
% [u,v]   original box
% v1      opposite vertex of the original box
% x0(1:n,1:max(L)) coordinates used in the initialization list
%         x^0 = (x0(1,l(1)),...,x0(n,l(n)) base vertex of the original
%	  box
% f0(1:max(L),1:n) function values appertaining to the initialization
%	  list
%         f0(m,i) is the function value at x = (x0(1,istar(1)),...,
%         x0(i-1,istar(i-1)),x0(i,m),x0(i+1,l(i+1)),...,x0(n,l(n)))
% f0(1:max(L),m), m > n  function values appertaining to a later box
%         split according to the initialization list
% ipar(m) label of the parent of box m
% isplit(m) = -(splitting index) of box m  if it is split in the
%             initialization procedure
%           = splitting index of box m  otherwise
% ichild(m) = -(number which child box m is)  if box m was generated
%             by splitting according to the initialization list (in
%             the initialization procedure or later)
%           = number which child box m is  otherwise
% z(1:2,:)  z(1,m) = value of the isplit(m)th coordinate of the base
%           vertex of box m
%           z(2,m) = k  if box m is split according to the
%                       initialization list and f0(:,k) contains the
%                       function values obtained by splitting the box
%           z(2,m) = splitting value of box m  otherwise
% f(1:2,:)  f(1,m) = function value at the base vertex of box m
%           f(2,m) = function value at the splitting point of box m
%                    (if z(2,m) ~= Inf)
% l(1:n)  pointer defining the initial base vertex x^0 (see above)
% L(1:n)  the init. list contains L(i) values for coordinate i
% Output:
% n0(1:n)  n0(i) indicates that the ith coordinate has been split n0(i)
%          times in the history of box j
% x(1:n)   base vertex of box j
% y(1:n)   opposite vertex of box j
% x1(1:n), x2(1:n), f1(1:n), f2(1:n)
%          x1(i) and x2(i) and the corresponding function values f1(i)
%          and f2(i) are used for quadratic interpolation in the ith
%          coordinate
% Since it is not always possible to find for each i two vertices that
% differ from x only in coordinate i, we have to generate 'fictitious'
% vertices and function values using the assumption that the function
% is separable
% Uses the following functions/m-files:
% updtf.m
% vert1.m
% vert2.m
% vert3.m

function [n0,x,y,x1,x2,f1,f2] = vertex(j,n,u,v,v1,x0,f0,ipar,isplit,ichild,z,f,l,L)
% initialization
% The coordinates of x, y, x1 and x2 are initially set to Inf to
% indicate that these quantities haven't been found yet in the course of
% pursuing the history of box j
x = Inf*ones(n,1);
y = Inf*ones(n,1);
x1 = Inf*ones(n,1);
x2 = Inf*ones(n,1);
f1 = zeros(n,1);
f2 = zeros(n,1);
n0 = zeros(n,1);
fold = f(1,j);
m = j;
while m > 1
  i = abs(isplit(ipar(m)));
  n0(i) = n0(i) + 1;
  if ichild(m) == 1
    if x(i) == Inf | x(i) == z(1,ipar(m))
      [x(i),x1(i),x2(i),f1(i),f2(i)] = vert1(2,z(:,ipar(m)),f(:,ipar(m)),x1(i),x2(i),f1(i),f2(i));
    else
      [f1,f2,fold] = updtf(n,i,x1,x2,f1,f2,fold,f(1,ipar(m)));
      [x1(i),x2(i),f1(i),f2(i)] = vert2(1,x(i),z(:,ipar(m)),f(:,ipar(m)),x1(i),x2(i),f1(i),f2(i));
    end
  elseif ichild(m) >= 2
    [f1,f2,fold] = updtf(n,i,x1,x2,f1,f2,fold,f(1,ipar(m)));
    if x(i) == Inf | x(i) == z(2,ipar(m))
      [x(i),x1(i),x2(i),f1(i),f2(i)] = vert1(1,z(:,ipar(m)),f(:,ipar(m)),x1(i),x2(i),f1(i),f2(i));
    else
      [x1(i),x2(i),f1(i),f2(i)] = vert2(2,x(i),z(:,ipar(m)),f(:,ipar(m)),x1(i),x2(i),f1(i),f2(i));
    end
  end
  if 1 <= ichild(m) & ichild(m) <= 2 & y(i) == Inf
    y(i) = split1(z(1,ipar(m)),z(2,ipar(m)),f(1,ipar(m)),f(2,ipar(m)));
  end
  if ichild(m) < 0
    % box m was generated by splitting according to the init. list
    % x0(i,j1) = ith coordinate of the base vertex of box m
    % the ith coordinate of the opposite vertex is the golden section
    % split of x0(i,j2) and x0(i,j2+1) for 1 <= j2 <= L(i) - 1, it is
    % u(i) for j1 = 1 and v(i) for j2 = L(i)
    % x0(i,j1+j3) = ith coordinate of the 'neighboring vertex'
    if u(i) < x0(i,1)
      j1 = ceil(abs(ichild(m))/2);
      j2 = floor(abs(ichild(m))/2);
      if (abs(ichild(m))/2 < j1  & j1 > 1) | j1 == L(i)
        j3 = -1;
      else
        j3 = 1;
      end
    else
      j1 = floor(abs(ichild(m))/2) + 1;
      j2 = ceil(abs(ichild(m))/2);
      if abs(ichild(m))/2 + 1 > j1 & j1 < L(i)
        j3 = 1;
      else
        j3 = -1;
      end
    end
    if isplit(ipar(m)) < 0 % box m was generated in the init. procedure
      k = i;
    else
      k = z(2,ipar(m));
      % box m was generated by a later split according to the init.
      % list; k points to the corresponding function values
    end
    if j1 ~= l(i) | (x(i) ~= Inf & x(i) ~= x0(i,l(i)))
      [f1,f2,fold] = updtf(n,i,x1,x2,f1,f2,fold,f0(l(i),k));
    end
    if x(i) == Inf | x(i) == x0(i,j1)
      x(i) = x0(i,j1);
      if x1(i) == Inf
        [x1(i),x2(i),f1(i),f2(i)] = vert3(j1,x0(i,:),f0(:,k),L(i),x1(i),x2(i),f1(i),f2(i));
      elseif x2(i) == Inf & x1(i) ~= x0(i,j1+j3)
        x2(i) = x0(i,j1+j3);
        f2(i) = f2(i) + f0(j1+j3,k);
      elseif x2(i) == Inf
        if j1 ~= 1 & j1 ~= L(i)
          x2(i) = x0(i,j1-j3);
          f2(i) = f2(i) + f0(j1-j3,k);
        else
          x2(i) = x0(i,j1+2*j3);
          f2(i) = f2(i) + f0(j1+2*j3,k);
        end
      end
    else
      if x1(i) == Inf
        x1(i) = x0(i,j1);
        f1(i) = f1(i) + f0(j1,k);
        if x(i) ~= x0(i,j1+j3)
          x2(i) = x0(i,j1+j3);
          f2(i) = f2(i) + f0(j1+j3,k);
        end
      elseif x2(i) == Inf
        if x1(i) ~= x0(i,j1)
          x2(i) = x0(i,j1);
          f2(i) = f2(i) + f0(j1,k);
        elseif x(i) ~= x0(i,j1+j3)
          x2(i) = x0(i,j1+j3);
          f2(i) = f2(i) + f0(j1+j3,k);
        else
          if j1 ~= 1 & j1 ~= L(i)
            x2(i) = x0(i,j1-j3);
            f2(i) = f2(i) + f0(j1-j3,k);
          else
            x2(i) = x0(i,j1+2*j3);
            f2(i) = f2(i) + f0(j1+2*j3,k);
          end
        end
      end
    end
    if y(i) == Inf
      if j2 == 0
        y(i) = u(i);
      elseif j2 == L(i)
        y(i) = v(i);
      else
        y(i) = split1(x0(i,j2),x0(i,j2+1),f0(j2,k),f0(j2+1,k));
      end
    end
  end
  m = ipar(m);
end
for i = 1:n
  if x(i) == Inf
    x(i) = x0(i,l(i));
    [x1(i),x2(i),f1(i),f2(i)] = vert3(l(i),x0(i,:),f0(:,i),L(i),x1(i),x2(i),f1(i),f2(i));
  end
  if y(i) == Inf
     y(i) = v1(i);
  end
end


function [pval,rho] = SStat_mass_PHTest(X,d,t,Y,stats_PH,maskvtx)
% [pval,rho] = SStat_mass_PHTest(X,d,t,Y,stats_PH,maskvtx)
%
% Vertex/voxel-wise Schoenfeld residuals test for the proportional hazards
% assumption.
%
% Input
% X: Design Matrix with the time-independent covariates. (mxp, m # of
% subjects, p # of covariates).
% d: Logical vector (mx1) indicating censorship status (1 if the subject got
% the failure event or 0 otherwise).
% t: Vector (mx1) whose entries are the survival and censored times (ordered
% according to X).
% Y: Data matrix (mxnv, nv #vertices) whos colums represent the ordered
% data vector (according to X) for each voxel/vertex.
% stats_PH: Structure array containing statistics obtained from SStat_mass_CoxPH.
% maskvtx: Mask's vertices (1-based). Default [] (all vertices included).
%
% Output
% pval: Matrix of p-values (pxnv) indicating the probability of the PH
% assumption for each covariate and at each location.
% rho: Matrix of correlations (pxnv) between Schoenfeld residuals and ranked
% failure times at each location.
%
% $Revision: 1.1 $  $Date: 2015/01/06 17:03:59 $
% Original Author: Jorge Luis Bernal Rusiel
% CVS Revision Info:
%    $Author: mreuter $
%    $Date: 2015/01/06 17:03:59 $
%    $Revision: 1.1 $
% References: Kleinbaum, D.G., Klein, M., 2005. Survival analysis. A self-
% learning approach, second edition. New York: Springer..
%
%Validation of inputs
if nargin < 5
    error('Too few inputs');
elseif nargin < 6
    maskvtx = [];
end;
nv0 = size(Y,2);
if (length(stats_PH)~=nv0)
    error(['The structure array stats_PH must have the same length as the'...
        ' number of colums of Y.']);
end;
if isempty(maskvtx)
    maskvtx = 1:nv0;
end;
Y = Y(:,maskvtx);
nv = size(Y,2);
[m,p] = size(X);
if (length(d)~=m) || (length(t)~=m) || (size(Y,1)~=m)
    error(['The design matrix X, censorship status vector d, time'...
        ' vector t and data matrix Y must all have the same number of rows.']);
end;
%Sort the data by time. If there is a tie between a failure time and a
%censored time then the failure time goes first.
st_ix = find(d==1);
t1 = t(st_ix);
[t1,t1_ix] = sort(t1);
X1 = X(st_ix(t1_ix),:);
Y1 = Y(st_ix(t1_ix),:);
cs_ix = find(d==0);
if ~isempty(cs_ix)
    t2 = t(cs_ix);
    [t2,t2_ix] = sort(t2);
    X2 = X(cs_ix(t2_ix),:);
    Y2 = Y(cs_ix(t2_ix),:);
    count1 = 1; count2 = 1; i = 0;
    while (count1 <= length(t1)) && (count2 <= length(t2))
        i = i + 1;
        if t1(count1) <= t2(count2)
            X(i,:) = X1(count1,:);
            Y(i,:) = Y1(count1,:);
            d(i) = 1;
            t(i) = t1(count1);
            count1 = count1 + 1;
        else
            X(i,:) = X2(count2,:);
            Y(i,:) = Y2(count2,:);
            d(i) = 0;
            t(i) = t2(count2);
            count2 = count2 + 1;
        end;
    end;
    if (count1 > length(t1))
        X(i+1:end,:) = X2(count2:end,:);
        Y(i+1:end,:) = Y2(count2:end,:);
        d(i+1:end) = 0;
        t(i+1:end) = t2(count2:end);
    else
        X(i+1:end,:) = X1(count1:end,:);
        Y(i+1:end,:) = Y1(count1:end,:);
        d(i+1:end) = 1;
        t(i+1:end) = t1(count1:end);
    end;
else
    X = X1;
    Y = Y1;
    t = t1;
end;
%indices of unique failure times in ft_ix (last index when ties happen)
st_ix = find(d==1);
[ft,ft_ix] = unique(t(st_ix),'last');
ft_ix = st_ix(ft_ix);
nft = length(ft);
%handling ties in failure times by substracting a very small random number
rand('state',sum(100*clock));
for j=1:nft
    i = 1;
    while (ft_ix(j)-i>0) && (ft(j)==t(ft_ix(j)-i))
        i = i + 1;
    end;
    nties = i-1;
    tt = t(ft_ix(j)-nties:ft_ix(j)) - 1e-5*(1 + rand(nties+1,1));
    tX = X(ft_ix(j)-nties:ft_ix(j),:);
    tY = Y(ft_ix(j)-nties:ft_ix(j),:);
    [stt,stt_ix] = sort(tt);
    t(ft_ix(j)-nties:ft_ix(j)) = stt;
    X(ft_ix(j)-nties:ft_ix(j),:) = tX(stt_ix,:);
    Y(ft_ix(j)-nties:ft_ix(j),:) = tY(stt_ix,:);
end;
nft = length(st_ix);
p = p+1;
pval = ones(p,nv);
rho = zeros(p,nv);
r = zeros(nft,p);
display('Checking the proportional hazards assumption at each location ...');
for j=1:nv
    if ~isempty(stats_PH(j).Bhat)
        if length(stats_PH(j).Bhat)==p
            XD =  [X Y(:,j)];
            for i=1:nft
                lprv = exp(XD(st_ix(i):end,:)*stats_PH(j).Bhat);
                r(i,:) = XD(st_ix(i),:) - (lprv'*XD(st_ix(i):end,:))./sum(lprv);
            end;
            [rho(:,j),pval(:,j)] = corr(r,[1:nft]');
        else
            error([' The number of elements of stats(' num2str(j) ').Bhat'...
                ' is different from ' num2str(p)]);
        end;
    end;
end


% The function used with the program: Quadrotor control

function xdot = quad_control_fn(t,x)

global Jtp Ixx Iyy Izz b d l m g Kpz Kdz Kpp Kdp Kpt Kdt Kpps Kdps ZdF PhidF ThetadF PsidF ztime phitime thetatime psitime Zinit Phiinit Thetainit Psiinit Uone Utwo Uthree Ufour Ez Ep Et Eps

% The desired values
% Changes in Z start at t = 3, changes in Phi start at t = 1, changes in
% Theta start at the origin, and chanfes in Psi start at t= 2
% If you want that all start at the origin simply remove the conditions

% time for change start of each variable
ztime = 3;
phitime = 1;
thetatime = 0.2;
psitime = 2;
%%% HEIGHT %%%
if t < ztime
 Zd = Zinit;
end

if t >= ztime
 Zd = ZdF;
end
%%%%%%%%%%%%%%
%%% Phi %%%
if t < phitime
 Phid = Phiinit;
end

if t >= phitime
 Phid = PhidF;
end
%%%%%%%%%%%%%%
%%% Theta %%%
if t < thetatime
 Thetad = Thetainit;
end

if t >= thetatime
 Thetad = ThetadF;
end
%%%%%%%%%%%%%%
%%% Psi %%%
if t < psitime
 Psid = Psiinit;
end

if t >= psitime
 Psid = PsidF;
end
%%%%%%%%%%%%%%

PsidF = 2*pi;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

thetaddot = 0;
phiddot = 0;
psiddot = 0;
Zddot = 0;
% Zd = 10;

    % Bounding the angles within the -2*pi / 2*pi range
    if (x(7)> 2*pi || x(7)< - 2*pi)
        x(7) = rem(x(7),2*pi);
    end

    if (x(9)> 2*pi || x(9)< - 2*pi)
        x(9) = rem(x(9),2*pi);
    end

    if (x(11)> 2*pi || x(11)< - 2*pi)
        x(11) = rem(x(11),2*pi);
    end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%PD-Z-Control%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Evaluate the Controls
    U = [];    % The control vector
    U(1) = m*(g + Kpz*(Zd - x(5)) + Kdz*( - x(6)))/(cos(x(9))*cos(x(7)));   % Total Thrust on the body along z-axis
    U(2) = (Kpp*(Phid - x(7)) + Kdp*( - x(8)));   % Roll input
	U(3) = (Kpt*(Thetad - x(9)) + Kdt*( - x(10)));   % Pitch input
	U(4) = (Kpps*(Psid - x(11)) + Kdps*( - x(12)));   % Yawing moment

    U = real(U);
    U = [U(1);U(2);U(3);U(4)];    % The control vector

%     % Bounding the controls
%     if U(1) > 15.7
%         U(1) = 15.7;
%     end
%
%     if U(1) < 0
%         U(1) = 0;
%     end
%
%     for j = 2:4
%         if U(j) > 1
%         U(j) = 1;
%         end
%
%         if U(j) < -1
%         U(j) = -1;
%         end
%     end

    % Calculation of angular velocities
    omegasqr(1) = (1/4*b)*U(1) + (1/2*b*l)*U(3) - (1/4*d)*U(4);
    omegasqr(2) = (1/4*b)*U(1) - (1/2*b*l)*U(2) + (1/4*d)*U(4);
    omegasqr(3) = (1/4*b)*U(1) - (1/2*b*l)*U(3) - (1/4*d)*U(4);
    omegasqr(4) = (1/4*b)*U(1) + (1/2*b*l)*U(2) + (1/4*d)*U(4);
    omegasqr = real(omegasqr);

    omega(1) = sqrt(omegasqr(1));
    omega(2) = sqrt(omegasqr(2));
    omega(3) = sqrt(omegasqr(3));
    omega(4) = sqrt(omegasqr(4));
        % Bounding the angular velocities
    for j = 1:4
        if omega(j) > 523
            omega(j) = 523;
        end

        if omega(j) < 125
            omega(j) = 125;
        end
    end
    omegasqr(1) = (omegasqr(1))^2;
    omegasqr(2) = (omegasqr(2))^2;
    omegasqr(3) = (omegasqr(3))^2;
    omegasqr(4) = (omegasqr(4))^2;
%     % Bounding the angular velocities
%     for j = 1:4
%         if omegasqr(j) > 523
%             omegasqr(j) = 523;
%         end
%
%         if omegasqr(j) < 125
%             omegasqr(j) = 125;
%         end
%     end
    % Disturbance
    Omega = d*(- sqrt(omegasqr(1)) + sqrt(omegasqr(2)) - sqrt(omegasqr(3)) + sqrt(omegasqr(4)));

% Evaluation of the State space wrt H-frame
    xdot(1) = x(2); % Xdot
    xdot(2) = (sin(x(11))*sin(x(7)) + cos(x(11))*sin(x(9))*cos(x(7)))*(U(1)/m);    % Xdotdot
    xdot(3) = x(4); % Ydot
    xdot(4) = (-cos(x(11))*sin(x(7)) + sin(x(11))*sin(x(9))*cos(x(7)))*(U(1)/m);	% Ydotdot
    xdot(5) = x(6); % Zdot
    xdot(6) = - g + (cos(x(9))*cos(x(7)))*(U(1)/m);    % Zdotdot
    xdot(7) = x(8); % phydot
    xdot(8) = ((Iyy - Izz)/Ixx)*x(10)*x(12) - (Jtp/Ixx)*x(10)*Omega + (U(2)/Ixx); % pdot = phydotdot
    xdot(9) = x(10);    % thetadot
    xdot(10) = ((Izz - Ixx)/Iyy)*x(8)*x(12) + (Jtp/Iyy)*x(8)*Omega + (U(3)/Iyy);	% qdot = thetadotdot
    xdot(11) = x(12);   % thetadot
    xdot(12) = ((Ixx - Iyy)/Izz)*x(8)*x(10) + (U(4)/Izz);	% rdot = psidotdot

xdot = xdot';


function doinvers_michael(mycat)
    % doinvers_michael calculates orientation of the stress tensor based on Gephard's algorithm.
    % stress tensor orientation. The actual calculation is done using a call to a fortran program.
    %
    % Stefan Wiemer 03/96

    % todo: HOW DOES THIS DIFFER FRON DOINVERSE?

    global mif1 mif2
    global tmpi cumu2
    % Save path to current folder
    sPath = pwd;
    ZG = ZmapGlobal.Data;
    fs = filesep;

    report_this_filefun();

    % Number of bootstraps
    rep = 2000;
    %if isunix ~= 1
    % errordlg('Misfit calculation only implemented for UNIX version! ');
    % return
    %end

    % making assumption that columns 10-12 were dip, dip idirection, etc.
    if isempty(mycat.Dip) || isempty(mycat.DipDirection) || isempty(mycat.Rake)
        errordlg('You do not have the required fields to calculate misfit');
        return
    end

    hodis = fullfile(hodi, 'external');

    tmpi = [ZG.mycat.Dip , ZG.mycat.DipDirection , ZG.mycat.Rake];

    cd(hodis);

    fid = fopen('data2','w');
    str = ['Inversion data'];
    str = str';

    fprintf(fid,'%s  \n',str');
    fprintf(fid,'%7.3f  %7.3f  %7.3f \n',tmpi');

    fclose(fid);

    delete data2.slboot Xtemp.slboot
    disp('Now doing inversion ... ')
    watchon
    helpdlg('The inversion is running right now ... it will take a few seconds ... please wait until results appear ');

    % slfast calculates the best solution for the stress tensor according to
    % Michael(1987): creates data2.slboot for bootslickw
    %[stat, res] = unix(['.' fs 'slfast data2 ']);

    slick_program = append_system_specific_postfix('slick');
    slfast_program = append_system_specific_postfix('slfast');
    bootslickw_program = append_system_specific_postfix('bootslickw');

    [stat, res] =  system([fullfile(ZG.hodi, slfast_program), ' data2'])

    if strcmp(res,'') == 0
        helpdlg('It seems that the inversion did not run, because the command slick_* data2 could not be executed. Is the executebale slick in the directory extrenal? ','error inverting');
        return
    end
    % slick calculates the best solution for the stress tensor according to
    % Michael(1987): creates data2.oput
    system([fullfile(ZG.hodi, slick_program), ' data2'])

    % Get data from data2.oput
    sFilename = 'data2.oput';
    [fBeta, fStdBeta, fTauFit, fAvgTau, fStdTau] = import_slickoput(sFilename);

    load data2.slboot
    % d0: Best solution for stress tensor
    d0 = data2;

    % bootslickw resamples the data and uses slfast to calculate the best fitting stress tensor
    system(sprintf('"%s" data2 %d 0.5', fullfile('.',bootslickw_program), rep));

    disp(' Done !  ')
    watchoff

    load Xtemp.slboot
    l = 2:2:2*rep;
    % d1: Bootstrap stress tensor solutions
    d1 = Xtemp(l,:);

    % Back to the original folder
    cd(sPath);

    % Now plot the results
    figure
    wulff
    set(gca,'NextPlot','add')

    %gridsize
    radius=0.05;

    % Plotting the bootstrap solutions of S1
    X = [d1(:,3) d1(:,2) ];
    theta = pi*(90-X(:,2))/180;      %az converted to MATLAB angle
    rho = tan(pi*(90-X(:,1))/360);   %projected distance from origin

    colormap(hot);
    brighten(0.5);
    densiter=denserfocalv2(rho,theta,radius);
    [xvec, yvec] =meshgrid(-1:0.01:1,-1:0.01:1);
    zvec=griddata(densiter(:,1),densiter(:,2),densiter(:,3),xvec,yvec);
    p11=contourf(xvec,yvec,zvec);

    %calculate 95% confidence interval
    bpoints(:,1)= rho .* cos(theta); %xval bootstrap
    bpoints(:,2)= rho .* sin(theta); %yval bootstrap

    X = [d0(2,3) d0(2,2) ];
    theta = pi*(90-X(:,2))/180;      %az converted to MATLAB angle
    rho = tan(pi*(90-X(:,1))/360);   %projected distance from origin
    solpoint(:,1)= rho .* cos(theta); %xval solution
    solpoint(:,2)= rho .* sin(theta); %yval solution

    cline = contour_dense(bpoints, solpoint, 0.95, 36);
    plot(cline(:,1),cline(:,2),'k', 'LineWidth',3,'LineStyle','--',...
        'DisplayName','95%');

    % Plotting the bootstrap solutions of S2
    X = [d1(:,5) d1(:,4) ];
    theta = pi*(90-X(:,2))/180;      %az converted to MATLAB angle
    rho = tan(pi*(90-X(:,1))/360);   %projected distance from origin

    densiter=denserfocalv2(rho,theta,radius);
    [xvec, yvec] =meshgrid(-1:0.01:1,-1:0.01:1);
    zvec=griddata(densiter(:,1),densiter(:,2),densiter(:,3),xvec,yvec);
    p12=contourf(xvec,yvec,zvec);

    %calculate 95% confidence interval
    bpoints(:,1)= rho .* cos(theta); %xval bootstrap
    bpoints(:,2)= rho .* sin(theta); %yval bootstrap

    X = [d0(2,5) d0(2,4) ];
    theta = pi*(90-X(:,2))/180;      %az converted to MATLAB angle
    rho = tan(pi*(90-X(:,1))/360);   %projected distance from origin
    solpoint(:,1)= rho .* cos(theta); %xval solution
    solpoint(:,2)= rho .* sin(theta); %yval solution

    cline = contour_dense(bpoints, solpoint, 0.95, 36);
    plot(cline(:,1),cline(:,2),'k', 'LineWidth',3,'LineStyle','--',...
        'DisplayName','95%');

    % Plotting the bootstrap solutions of S3
    X = [d1(:,7) d1(:,6) ];
    theta = pi*(90-X(:,2))/180;      %az converted to MATLAB angle
    rho = tan(pi*(90-X(:,1))/360);   %projected distance from origin

    densiter=denserfocalv2(rho,theta,radius);
    [xvec, yvec] =meshgrid(-1:0.01:1,-1:0.01:1);
    zvec=griddata(densiter(:,1),densiter(:,2),densiter(:,3),xvec,yvec);
    p13=contourf(xvec,yvec,zvec);

    %calculate 95% confidence interval
    bpoints(:,1)= rho .* cos(theta); %xval bootstrap
    bpoints(:,2)= rho .* sin(theta); %yval bootstrap

    X = [d0(2,7) d0(2,6) ];
    theta = pi*(90-X(:,2))/180;      %az converted to MATLAB angle
    rho = tan(pi*(90-X(:,1))/360);   %projected distance from origin
    solpoint(:,1)= rho .* cos(theta); %xval solution
    solpoint(:,2)= rho .* sin(theta); %yval solution

    cline = contour_dense(bpoints, solpoint, 0.95, 36);
    plot(cline(:,1),cline(:,2),'k', 'LineWidth',3,'LineStyle','--',...
        'DisplayName','95%');

    % Plot the best solution
    % Replaced d1 with d0, JW
    X = [d0(2,3) d0(2,2) ];
    theta = pi*(90-X(:,2))/180;      %az converted to MATLAB angle
    rho = tan(pi*(90-X(:,1))/360);   %projected distance from origin
    xp = rho .* cos(theta);
    yp = rho .* sin(theta);
    pl1a = plot(xp,yp,'ks'); %s
    set(pl1a,'LineWidth',2,'MarkerSize',12,'Markerfacecolor','w')
    set(gca,'NextPlot','add')

    X = [d0(2,5) d0(2,4) ];
    theta = pi*(90-X(:,2))/180;      %az converted to MATLAB angle
    rho = tan(pi*(90-X(:,1))/360);   %projected distance from origin
    xp = rho .* cos(theta);
    yp = rho .* sin(theta);
    pl2a = plot(xp,yp,'k^'); %^
    set(pl2a,'LineWidth',2,'MarkerSize',12,'Markerfacecolor','w')

    X = [d0(2,7) d0(2,6) ];
    theta = pi*(90-X(:,2))/180;      %az converted to MATLAB angle
    rho = tan(pi*(90-X(:,1))/360);   %projected distance from origin
    xp = rho .* cos(theta);
    yp = rho .* sin(theta);
    pl3a = plot(xp,yp,'ok'); %o
    set(pl3a,'LineWidth',2,'MarkerSize',12,'Markerfacecolor','w')
    set(gcf,'color','w');

    %doing the legend
    le = legend([pl1a pl2a pl3a],'S1','S2','S3');
    set(le,'pos',[0.1 0.8 0.15 0.1]);
    set(le,'Xcolor','w','ycolor','w','box','off');

    % Compute standard deviation of phi
    fStdPhi = std(d1(:,1),1,'omitnan');

    axes('pos',[0 0 1 1 ]);
    axis off
    text(0.01,0.22,['Variance: ' num2str(d0(1,1),2) ]);
    text(0.01,0.18,['Phi: ' num2str(d0(2,1),2) ' \pm ' num2str(fStdPhi)]);
    text(0.01,0.14,['S1: trend: ' num2str(d0(2,2),4) '; plunge: '  num2str(d0(2,3),4) ]);
    text(0.01,0.1,['S2: trend: ' num2str(d0(2,4),4) '; plunge: '  num2str(d0(2,5),4) ]);
    text(0.01,0.06,['S3: trend: ' num2str(d0(2,6),4) '; plunge: '  num2str(d0(2,7),4) ]);
    text(0.78,0.22,['Tau: ' num2str(fAvgTau,4) ' \pm ' num2str(fStdTau,4)]);
    text(0.78,0.18,['Ratio: ' num2str(fTauFit,4)]);
    text(0.78,0.14,['Beta: ' num2str(fBeta,4) ' \pm ' num2str(fStdBeta,4)]);

    % Determine the faulting style based on Zoback, 1992
    ste = [d0(2,3) d0(2,2)+180 d0(2,5) d0(2,4)+180 d0(2,7) d0(2,6)+180];

    faulting_type = faultType(ste(1),ste(3),ste(5));
    if strcmpi(faulting_type,'Thrust to Strike Slip')
        l4 = pl; %?
    end

    text(0.01,0.02,['Faulting style: ' faulting_type]);
    % Link to World stress map
    uicontrol('Units','normal',...
        'Position',[.4 .0 .1 .04],'String','Info ',...
        'callback',@callbackfun_001);

    % Plot CDF and histogram of phi-values
    figure_w_normalized_uicontrolunits('Name','Phi - ratio of relative stress magnitude','Position',[700   700   472   232])
    % CDF
    subplot(1,2,1)
    vDistribution = d1(:,1);
    % Remove NaN-values
    vSel = ~isnan(vDistribution);
    vPlotDist = vDistribution(vSel,:);
    % Plot the cumulative density function
    nLen = length(vPlotDist);
    vIndices = [1:nLen]/nLen;
    vDist = sort(vPlotDist);
    plot(vDist, vIndices,'Linewidth',1.5,'Color',[0 0 0]);
    set(gca,'NextPlot','add')
    xlabel('\phi','FontSize',12,'Fontweight','bold')
    ylabel('Fraction of cases','FontSize',12,'Fontweight','bold')
    set(gca,'Box','on','Linewidth',1.5,'Xlim',[0 1])
    plot([d0(2,1) d0(2,1)],[0 1],'Color',[0.8 0 0],'Linewidth',1.5)
    legend(' \phi CDF bootstrap','\phi org. data')
    % Histogram
    subplot(1,2,2)
    histogram(vDistribution,0:0.05:1)
    set(gca,'Xlim',[0 1]);
    xlabel('\phi','FontSize',12,'Fontweight','bold')
    ylabel('Frequency','FontSize',12,'Fontweight','bold')

    function callbackfun_001(mysrc,myevt)

        callback_tracker(mysrc,myevt,mfilename('fullpath'));
        web http://www.world-stress-map.org/;
    end

    function ftype = faultType(s1_east, s2_west, s3_north)
        % faultTypes determine faulting ftype from the s1_east,s2_west,s3_north plunge values
        % returns a string description
        ftype = 'Unknow';

        if s1_east>52                         && s3_north<35
            ftype = 'Normal';
        end

        if 40<s1_east  && s1_east<52               && s3_north<20
            ftype = 'Normal to Strike Slip';
        end

        if s1_east<40            && s2_west>45     && s3_north<20
            ftype = 'Strike Slip';
        end

        if s1_east<20            && s2_west>45     && s3_north<40
            ftype = 'Strike Slip';
        end

        if s1_east<20                         && 40<s3_north && s3_north<52
            ftype = 'Thrust to Strike Slip';
        end

        if s1_east<35                         && s3_north>52
            ftype = 'Thrust';
        end
    end

end


% Sample of Linear Blend Skinning in Matlab
% This code is non optimized nor necessarily correct.
% Also, it has only been tested on the data in this folder so I dont know
% how it will behave on other data.

% Aaron Wetzler, 2013

%% -------------------------------------------------------------------------
clf
close all
clear all
clc

% Load the faces, vertices and weights
faces = csvread('handfaces.txt');
V0 = csvread('handverts.txt');
V0=V0';
N = size(V0,1);
weights = csvread('handweights.txt');
sk = loadbvh('HandBase.bvh');

% Mapping to correct the ordering of the weights to the bones for the
% HandBase.bvh file
wmap = [1 2 3 4 0 15 16 17 0 5 6 7 0 8 9 10 0 11 12 13 0 14 0];
wzero = wmap==0;

% This will likely crash a machine if there are two many bones and too many
% animation frames
W = sparse(weights(:,1)+1,weights(:,2)+1,weights(:,3));
W = full(W);%W = W./repmat(sum(W,2),[1 size(W,2)]);

W = W(:,wmap+wzero);
W(:,wzero)=0;

% Currently there are some rows in W which have up to 6 nonzero weights.
% The industry accepted number is 4 so should probably zero the smaller
% ones.

% -------------------------------------------------------------------------

%Display the mesh

cla
for displayWeightIndex=find(~wzero)
    h = trisurf(faces+1,V0(1,:),V0(2,:),V0(3,:),W(:,displayWeightIndex));
    axis equal; axis vis3d;
    shading interp;
    set(h,'FaceAlpha',0.5)
    axis([-200 200 -100 100 -100 500]);
    pause(0.2);
end
hold on

%% -------------------------------------------------------------------------
% Display the skinned armature animation

for j=2:258

    V=V0*0;

    cla
    for i=1:length(sk)
        % -------------------------------------------------------------------------
        % Display the bones
        if sk(i).parent
            s=sk(i).t_xyz(:,j); sp = sk(sk(i).parent).t_xyz(:,j);
            plot3([s(1) sp(1)],[s(2) sp(2)],[s(3) sp(3)],'r','LineWidth',5);
        end
        plot3(sk(i).t_xyz(1,j),sk(i).t_xyz(2,j),sk(i).t_xyz(3,j),'g.','MarkerSize',10);
        if sk(i).Nchannels
            plotax(sk(i).t_xyz(:,j)',sk(i).T(1:3,1:3,j)*sk(i).R0,15)
        end
        % -------------------------------------------------------------------------

        if sk(i).Nchannels
            % Takes a local bone coordinate and puts it into the new pose global position
            poseMatrix(:,:,i,j) = [(sk(i).T(1:3,1:3,j)*sk(i).R0) sk(i).t_xyz(:,j);0 0 0 1];
            % Takes a local bone coordinate and puts it into the base pose global position
            restMatrix(:,:,i) = [((sk(i).R0)) sk(i).head0 ;0 0 0 1];

            % The matrix T for a bone takes a point from global bind space to its new
            % position also in global space as dictated by that bone

            % -------------------------------------------------------------------------------
            % To go from base pose to new pose position we
            % undo the restmatrix and then apply the posematrix i.e.
            % if we have a vertex v from the bind pose then to see where
            % it would be sent by bone i for pose j we would apply
            % vj = M(:,:,j)*[v;1]

            M(:,:,j) = poseMatrix(:,:,i,j)*inv(restMatrix(:,:,i));
            R = M(1:3,1:3,j);
            t = M(1:3,4,j);

            % We add the influence from every bone
            V = V + (R*V0 + repmat(t,1,length(V0))) * spdiags(W(:,i),0,length(V),length(V));

            % --------------------------------------------------------------------------------

        end
    end

    % -------------------------------------------------------------------------
    % Display nicely
    h = trisurf(faces+1,V(1,:),V(2,:),V(3,:),W(:,2)*0);
    axis off;
    axis equal
    shading interp;
    light
    lighting phong
    set(h,'FaceAlpha',0.8)
    drawnow

end
%%

% Code to extract specific joint angles
% Bone i, animation frame j
i = 20; j=2;
T = poseMatrix(1:3,1:3,i,j); Tp = poseMatrix(1:3,1:3,i-1,j); T_0 = restMatrix(1:3,1:3,i); Tp_0 = restMatrix(1:3,1:3,i-1);
R = (T_0')*(Tp_0)*(Tp')*(T);
[Rx Ry Rz] = dcm2angle(R,'XYZ');
Rx = rad2deg(-Rx); Ry = rad2deg(-Ry); Rz = rad2deg(-Rz);

[Rx Ry Rz]


%system model for wander mechanization (with dcm) under small angle
%assumption and no wander error. (Based on Phi-model definition)
%dt==0 -> return cont. time model
%Depending on input arguments this function may return (A,N), (Ad,N),
%(Ac,Qc),(Ad,Qd)
%this function is nothing but sys_wander_large with deleted wander states.

%Although this is for a wander-azimuth mechanization, this implementation
%is singular as it calls geoparam_v000.

%For a real non-singular sys-model implementation see sys_wander_psi_v000

%(For an example usage of this see: example_largeheading)

function [RetA RetB]=sys_wander_dcm(llh, vel_n, Cbn, wander, acc, dt, Aimu, Qimu, Cimu, Rimu)
% position   (1-3) (not it llh but in northing, easting, height)
% velocity   (4-6)
% attitude   (7-9)
% wander     (10-11) ([del_sa, del_ca])

%system disturbance coefs
N=zeros(9,6);
N(7:9,4:6)=-Cbn; %attitude
N(4:6,1:3)=Cbn; %velocity

%system matrix
A=zeros(9);

[Rn, Re, g, sL, cL, WIE_E]=geoparam_v000(llh);
Cgn=[wander(2) wander(1) 0;-wander(1) wander(2) 0; 0 0 1];
Rn_h=Rn+llh(3);
Re_h=Re+llh(3);
R=sqrt(Rn_h*Re_h);
acc_n=Cbn*acc;

mx_b=[wander(2)*(-WIE_E)*sL/Rn_h 0;wander(1)*(-WIE_E)*sL/Rn_h 0; (-WIE_E)*cL/Rn_h 0];
mx_c=[0 1/R 0;-1/R 0 0;0 0 0];
mx_d=[-vel_n(2)/R^2; vel_n(1)/R^2;0];

mx_e=Cgn*[0 1/(Re_h) 0; -1/(Rn_h) 0 0;0 0 0]*Cgn'*vel_n+2*Cgn*[WIE_E*cL; 0; -WIE_E*sL]; %2wie_n+wen_n
mx_f=Cgn*[0 1/(Re_h) 0; -1/(Rn_h) 0 0;0 0 0]*Cgn'*vel_n+Cgn*[WIE_E*cL; 0; -WIE_E*sL]; %wie_n+wen_n

%%position errors
A(1:2,4:5)=Cgn(1:2,1:2)';
A(3,6)=-1;

%%Velocity errors
A(4:6,1:2)=2*skew(vel_n)*mx_b;
A(4:6,3)=skew(vel_n)*mx_d+[0;0;-2*g/R];
A(4:6,4:6)=skew(vel_n)*mx_c-skew(mx_e);
A(4:6,7:9)=skew(acc_n);

%%Attitude errors
A(7:9,1:2)=mx_b;
A(7:9,3)=mx_d;
A(7:9,4:6)=mx_c;
A(7:9,7:9)=-skew(mx_f);

%%%%%%%%
%%Combine navigation models with imu models and return
if ~(isempty(Aimu) || isempty(Qimu) || isempty(Rimu) || isempty(Cimu))
    nst_imu=size(Aimu,1);
    nst_nav=size(A,1);
    Ac=[A N*Cimu;zeros(nst_imu,nst_nav) Aimu];
    Qc=[N*Rimu*N' zeros(nst_nav,nst_imu);zeros(nst_imu,nst_nav) Qimu];

    RetA=Ac;
    RetB=Qc;

    if (dt>0)
        %Discretize the model
        nst=nst_nav+nst_imu;
        mx_a = dt*[-Ac,Qc;zeros(nst),Ac'];
        mx_b = expm(mx_a);
        STM = mx_b(nst+1:2*nst,nst+1:2*nst)';
        Qd = STM*mx_b(1:nst,nst+1:2*nst);
        RetA=STM;
        RetB=Qd;
    end
else
    RetA=A;
    RetB=N;
    if (dt>0)
        RetA=expm(A*dt);
    end
end