%============================================================================
% Copyright (C) 2015, Heikki Hyyti
%
% Permission is hereby granted, free of charge, to any person obtaining a
% copy of this software and associated documentation files (the "Software"),
% to deal in the Software without restriction, including without limitation
% the rights to use, copy, modify, merge, publish, distribute, sublicense,
% and/or sell copies of the Software, and to permit persons to whom the
% Software is furnished to do so, subject to the following conditions:
%
% The above copyright notice and this permission notice shall be included in
% all copies or substantial portions of the Software.
%
% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
% IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
% FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
% THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
% LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
% FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
% DEALINGS IN THE SOFTWARE.
%============================================================================

disp(' ');
disp('If you use the algorithm in any scientific context, please cite:');
disp('Heikki Hyyti and Arto Visala, "A DCM Based Attitude Estimation Algorithm');
disp('for Low-Cost MEMS IMUs," International Journal of Navigation and Observation,');
disp('vol. 2015, Article ID 503814, 18 pages, 2015. http://dx.doi.org/10.1155/2015/503814');
disp(' ');

clear all;
close all;
pause(0.1);

%% Configuration

% use c/compile.m before setting this to true (tested only with linux)
use_C_versions = false;

% fetch GPL licensed algortihms in matlab and C from here before use:
% http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
% Add the C code to c folder and matlab functions in this folder
% (please add also the quaternion library used by Madgwick).
% I apologize this mess. The Madgwick's GPS licenced code is separated from
% this MIT licenced product to avoid infecting this code with GPL licence.
use_comparisonAlgorithms = false;

% Add bias to calibrated gyro values (same for each axis)
addedGyroBias = 0;

% load measurement data
data = load('allData.mat');

if (~exist('figures', 'dir')); mkdir('figures'); end;

ILtime = data.InertiaLink.tv_sec + (data.InertiaLink.tv_msec/1000);
SFtime = data.SparkFun6DOF.tv_sec + (data.SparkFun6DOF.tv_usec / 1000000);
Ktime = data.Kuka.tv_sec + (data.Kuka.tv_usec/1000000);

firstCommonTime = max([ILtime(1), SFtime(1), Ktime(1)]);
lastCommonTime = min([ILtime(end), SFtime(end), Ktime(end)]);
commonDuration = lastCommonTime - firstCommonTime;

ILtime = ILtime - firstCommonTime;
SFtime = SFtime - firstCommonTime;
Ktime = Ktime - firstCommonTime;

ILdeltaTime = [0; ILtime(2:end) - ILtime(1:end-1)];
SFdeltaTime = [0; SFtime(2:end) - SFtime(1:end-1)];
KdeltaTime = [0; Ktime(2:end) - Ktime(1:end-1)];

%start yaw from zero at time 468s (start of tests after calibration sequences in example data set)
StartIdx = find(ILtime > 464, 1, 'first');
StopIdx = find(ILtime > 774, 1, 'first');
time = ILtime(StartIdx:StopIdx);

%getting reference trajectory
R11 = data.Kuka.data_msrCartPos01;
R12 = data.Kuka.data_msrCartPos02;
R13 = data.Kuka.data_msrCartPos03;
x = data.Kuka.data_msrCartPos04;
R21 = data.Kuka.data_msrCartPos05;
R22 = data.Kuka.data_msrCartPos06;
R23 = data.Kuka.data_msrCartPos07;
y = data.Kuka.data_msrCartPos08;
R31 = data.Kuka.data_msrCartPos09;
R32 = data.Kuka.data_msrCartPos10;
R33 = data.Kuka.data_msrCartPos11;
z = data.Kuka.data_msrCartPos12;

[yaw, pitch, roll] = yawpitchroll(R11, R21, R31, R32, R33);
yaw = un_modulo(yaw, 2*pi); %remove 2pi jumps and make yaw continuous
yaw = yaw - yaw(1); %starting yaw from zero (same as IMU estimates)

vel_x = [0; x(2:end)-x(1:end-1)] ./ KdeltaTime;
vel_y = [0; y(2:end)-y(1:end-1)] ./ KdeltaTime;
vel_z = [0; z(2:end)-z(1:end-1)] ./ KdeltaTime;

% compute reference angular velocities using the KUKA robot
% (derivate between positions)
gyro = zeros(length(R11),3);
dt = median(Ktime(2:end)-Ktime(1:end-1)); % the most frequently used discretation time
for i = 2:length(R11)
    R_last = [R11(i-1), R12(i-1), R13(i-1); ...
              R21(i-1), R22(i-1), R23(i-1); ...
              R31(i-1), R32(i-1), R33(i-1)];
    R_cur = [R11(i), R12(i), R13(i); ...
             R21(i), R22(i), R23(i); ...
             R31(i), R32(i), R33(i)];

    Wh = (R_last'*R_cur - eye(3));
    W = 1/dt * Wh;
    gyro(i,1) = (W(3,2) - W(2,3))/2;
    gyro(i,2) = (W(1,3) - W(3,1))/2;
    gyro(i,3) = (W(2,1) - W(1,2))/2;
end

% Resample reference data of KUKA robot to InertiaLink time
warning('off','interpolation:interpolation:noextrap');
yaw_in_IL = resample(timeseries(yaw, Ktime),time);
yaw_in_IL = yaw_in_IL.Data;
yaw_in_IL = yaw_in_IL - yaw_in_IL(1); %starting yaw from zero (same as IMU estimates)
pitch_in_IL = resample(timeseries(pitch, Ktime),time);
pitch_in_IL = pitch_in_IL.Data;
roll_in_IL = resample(timeseries(roll, Ktime),time);
roll_in_IL = roll_in_IL.Data;

%computing ypr for InertiaLink internal estimates (Rotation matrix R = -M')
[yaw_IL, pitch_IL, roll_IL] = yawpitchroll(-data.InertiaLink.M11, ...
    data.InertiaLink.M12, -data.InertiaLink.M13, -data.InertiaLink.M23, ...
    -data.InertiaLink.M33);
yaw_IL = un_modulo(yaw_IL, 2*pi); % remove 2pi jumps and make yaw continuous
yaw_IL = yaw_IL - yaw_IL(1); % starting yaw from zero (same as IMU estimates)

%% calibrate IMU data
imusWithKukaCalibration;

acc1 = [acc1_x, acc1_y, acc1_z];
acc2 = [acc2_x, acc2_y, acc2_z];

gyro1 = [w1_x, w1_y, w1_z];
gyro2 = [w2_x, w2_y, w2_z];

% add constant bias for testing purposes (if addedGyroBias is set above)
gyro1 = gyro1 + addedGyroBias*ones(size(gyro1));
gyro2 = gyro2 + addedGyroBias*ones(size(gyro2));

%% DCM IMU results for both imu datas
tic();
if (use_C_versions)
    [x_hist1, ypr_hist1, a_hist1, P_diag_hist1] = ...
        DCM_IMU_uC(gyro1, acc1, SFdeltaTime);
else
    IMU_DCM1 = DCM_IMU();
    x_hist1 = zeros(length(SFtime), 6);
    ypr_hist1 = zeros(length(SFtime), 3);
    P_diag_hist1 = zeros(length(SFtime), 6);
    for t = 1:length(SFtime)
        IMU_DCM1.UpdateIMU(gyro1(t,:), acc1(t,:), SFdeltaTime(t));	% gyroscope units must be radians
        x_hist1(t, :) = IMU_DCM1.state';
        ypr_hist1(t, :) = [IMU_DCM1.yaw, IMU_DCM1.pitch, IMU_DCM1.roll];
        P_diag_hist1(t, :) = diag(IMU_DCM1.P)';
    end
end
tDCM1 = toc()

tic();
if (use_C_versions)
    [x_hist2, ypr_hist2, a_hist2, P_diag_hist2] = ...
        DCM_IMU_uC(gyro2, acc2, ILdeltaTime);
else
    IMU_DCM2 = DCM_IMU();
    x_hist2 = zeros(length(ILtime), 6);
    ypr_hist2 = zeros(length(ILtime), 3);
    P_diag_hist2 = zeros(length(ILtime), 6);
    for t = 1:length(ILtime)
        IMU_DCM2.UpdateIMU(gyro2(t,:), acc2(t,:), ILdeltaTime(t));	% gyroscope units must be radians
        x_hist2(t, :) = IMU_DCM2.state';
        ypr_hist2(t, :) = [IMU_DCM2.yaw, IMU_DCM2.pitch, IMU_DCM2.roll];
        P_diag_hist2(t, :) = diag(IMU_DCM2.P)';
    end
end
tDCM2 = toc()

%% computing Madgwick and Mahony imu algorithms for comparison
if (use_comparisonAlgorithms)
    addpath('quaternion_library');      % include quaternion library

    tic();
    if (use_C_versions)
        quat_mad_B = Madgwick_IMU_C(gyro1, acc1);
    else
        IMU_mad_B = MadgwickAHRS('SamplePeriod', 1/150, 'Beta', 0.1);
        quat_mad_B = zeros(length(SFtime), 4);
        for t = 1:length(quat_mad_B)
            IMU_mad_B.UpdateIMU(gyro1(t,:), acc1(t,:));	% gyroscope units must be radians
            quat_mad_B(t, :) = IMU_mad_B.Quaternion;
        end
    end
    tIMU_mad_B = toc()

    tic();
    if (use_C_versions)
        quat_mah_B = Mahony_IMU_C(gyro1, acc1);
    else
        IMU_mah_B = MahonyAHRS('SamplePeriod', 1/150, 'Kp', 0.5);
        quat_mah_B = zeros(length(SFtime), 4);
        for t = 1:length(quat_mah_B)
            IMU_mah_B.UpdateIMU(gyro1(t,:), acc1(t,:));	% gyroscope units must be radians
            quat_mah_B(t, :) = IMU_mah_B.Quaternion;
        end
    end

    tIMU_mah_B = toc()

    tic();
    if (use_C_versions)
        quat_mad_A = Madgwick_IMU_C(gyro2, acc2);
    else
        IMU_mad_A = MadgwickAHRS('SamplePeriod', 1/150, 'Beta', 0.1);
        quat_mad_A = zeros(length(ILtime), 4);
        for t = 1:length(quat_mad_A)
            IMU_mad_A.UpdateIMU(gyro2(t,:), acc2(t,:));	% gyroscope units must be radians
            quat_mad_A(t, :) = IMU_mad_A.Quaternion;
        end
    end
    tIMU_mad_A = toc()

    tic();
    if (use_C_versions)
        quat_mah_A = Mahony_IMU_C(gyro2, acc2);
    else
        IMU_mah_A = MahonyAHRS('SamplePeriod', 1/150, 'Kp', 0.5);
        quat_mah_A = zeros(length(ILtime), 4);
        for t = 1:length(quat_mah_A)
            IMU_mah_A.UpdateIMU(gyro2(t,:), acc2(t,:));	% gyroscope units must be radians
            quat_mah_A(t, :) = IMU_mah_A.Quaternion;
        end
    end
    tIMU_mah_A = toc()

    % Plot algorithm output as Euler angles
    % The first and third Euler angles in the sequence (phi and psi) become
    % unreliable when the middle angles of the sequence (theta) approaches ~90
    % degrees. This problem commonly referred to as Gimbal Lock.
    % See: http://en.wikipedia.org/wiki/Gimbal_lock

    % use conjugate for sensor frame relative to Earth and convert to degrees.
    euler_mad_B = quatern2euler(quaternConj(quat_mad_B)) * (180/pi);
    euler_mah_B = quatern2euler(quaternConj(quat_mah_B)) * (180/pi);
    euler_mad_A = quatern2euler(quaternConj(quat_mad_A)) * (180/pi);
    euler_mah_A = quatern2euler(quaternConj(quat_mah_A)) * (180/pi);
else
    euler_mad_B = nan(length(gyro1),3);
    euler_mah_B = nan(length(gyro1),3);
    euler_mad_A = nan(length(gyro2),3);
    euler_mah_A = nan(length(gyro2),3);

    disp('To get reference algorithms, download their implementations from here:');
    disp('http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/');
end

%% resampling & remove 2pi jumps
warning('off','interpolation:interpolation:noextrap');

%resample SparkFun data to InertiaLink time
yaw_sf_in_IL = resample(timeseries(un_modulo(ypr_hist1(:,1),2*pi), SFtime),time);
yaw_sf_in_IL = yaw_sf_in_IL.Data*180/pi;
yaw_sf_in_IL = yaw_sf_in_IL - yaw_sf_in_IL(1); %starting yaw from zero (same as IMU estimates)
pitch_sf_in_IL = resample(timeseries(ypr_hist1(:,2), SFtime),time);
pitch_sf_in_IL = pitch_sf_in_IL.Data*180/pi;
roll_sf_in_IL = resample(timeseries(ypr_hist1(:,3), SFtime),time);
roll_sf_in_IL = roll_sf_in_IL.Data*180/pi;

% start yaw from zero at the beginning of test sequence
yaw_IL = yaw_IL - yaw_IL(StartIdx);

%yaw pitch roll errors (only InertiaLink data)
ypr_hist2(:,1) = un_modulo(ypr_hist2(:,1), 2*pi); %remove 2pi jumps and make continuous
ypr_hist2(:,1) = ypr_hist2(:,1) - ypr_hist2(StartIdx,1); %DCM

euler_mad_B(:,3) = un_modulo(euler_mad_B(:,3), 360); %remove 2pi jumps and make continuous
r_data = resample(timeseries(euler_mad_B(:,1), SFtime),ILtime);
p_data = resample(timeseries(euler_mad_B(:,2), SFtime),ILtime);
y_data = resample(timeseries(euler_mad_B(:,3), SFtime),ILtime);
euler_mad_B = [r_data.data, p_data.data, y_data.data];
euler_mad_B(:,3) = euler_mad_B(:,3) - euler_mad_B(StartIdx,3); %Madgwick

euler_mah_B(:,3) = un_modulo(euler_mah_B(:,3), 360); %remove 2pi jumps and make continuous
r_data = resample(timeseries(euler_mah_B(:,1), SFtime),ILtime);
p_data = resample(timeseries(euler_mah_B(:,2), SFtime),ILtime);
y_data = resample(timeseries(euler_mah_B(:,3), SFtime),ILtime);
euler_mah_B = [r_data.data, p_data.data, y_data.data];
euler_mah_B(:,3) = euler_mah_B(:,3) - euler_mah_B(StartIdx,3); %Mahony

euler_mad_A(:,3) = un_modulo(euler_mad_A(:,3), 360); %remove 2pi jumps and make continuous
euler_mad_A(:,3) = euler_mad_A(:,3) - euler_mad_A(StartIdx,3); %Madgwick

euler_mah_A(:,3) = un_modulo(euler_mah_A(:,3), 360); %remove 2pi jumps and make continuous
euler_mah_A(:,3) = euler_mah_A(:,3) - euler_mah_A(StartIdx,3); %Mahony

warning('on','interpolation:interpolation:noextrap');

%% plotting
markerSize = 8; %pl

figPos = [105 105 645 660];

plotPos = [0.1 0.15 0.70 0.70];

plot21Pos = [0.1 0.53 0.86 0.40];
plot22Pos = plot21Pos + [0 -0.47 0 0];

plot1Pos = [0.1 0.69 0.86 0.25];
plot2Pos = plot1Pos + [0 -0.31 0 0];
plot3Pos = plot2Pos + [0 -0.31 0 0];

plot41Pos = [0.1 0.75 0.86 0.19];
plot42Pos = plot41Pos + [0 -0.23 0 0];
plot43Pos = plot42Pos + [0 -0.23 0 0];
plot44Pos = plot43Pos + [0 -0.23 0 0];

% Define locations from where tests start and stop (acceleration test and rotation test)

%ACC-test sequence: Linear motion with different accelerations
accStartIdx = find(time > 468, 1, 'first');
accStopIdx = find(time > 559, 1, 'first');
accStartIdx_IL = find(ILtime > 468, 1, 'first');
accStopIdx_IL = find(ILtime > 559, 1, 'first');
accStartIdx_SF = find(SFtime > 468, 1, 'first');
accStopIdx_SF = find(SFtime > 559, 1, 'first');
accStartIdx_K = find(Ktime > 468, 1, 'first');
accStopIdx_K = find(Ktime > 559, 1, 'first');

%sideways movement at x
accStartIdx_SF_x = find(SFtime > 468.5, 1, 'first');
accStopIdx_SF_x = find(SFtime > 496.5, 1, 'first');
%sideways movement at y
accStartIdx_SF_y = find(SFtime > 498, 1, 'first');
accStopIdx_SF_y = find(SFtime > 526, 1, 'first');
%up and down movement at z
accStartIdx_SF_z = find(SFtime > 530, 1, 'first');
accStopIdx_SF_z = find(SFtime > 558, 1, 'first');

%IMU-test sequence: free 6D motion
rotStartIdx = find(time > 564, 1, 'first');
rotStopIdx = find(time > 770, 1, 'first');

%raw accelerations
figIdx = 1;
figure(figIdx); clf;

arrowOffset = 2;
arrowOffset_y = 0.15;

subplot(4,1,1);
minY = -1;
maxY = 1;
plot(Ktime, vel_x, 'b', Ktime, vel_y, 'g--', Ktime, vel_z, 'r:', 'LineWidth', 1);
set(gca, 'Position', plot41Pos, 'FontSize', 12, 'FontName', 'Times');
title('Reference measurement and accelerations during the acceleration test', 'FontSize', 14, 'FontName', 'Times');
legend('x', 'y', 'z', 'Location', 'NorthEast');
ylabel('Reference velocity (m/s)', 'FontSize', 12, 'FontName', 'Times');
axis([Ktime(accStartIdx_K) Ktime(accStopIdx_K) minY maxY]);
hold on;
plot(SFtime([accStartIdx_SF_x accStartIdx_SF_x]), [minY maxY], 'k:', SFtime([accStopIdx_SF_x accStopIdx_SF_x]), [minY maxY], 'k:', 'LineWidth', 1);
plot(SFtime([accStartIdx_SF_y accStartIdx_SF_y]), [minY maxY], 'k:', SFtime([accStopIdx_SF_y accStopIdx_SF_y]), [minY maxY], 'k:', 'LineWidth', 1);
plot(SFtime([accStartIdx_SF_z accStartIdx_SF_z]), [minY maxY], 'k:', SFtime([accStopIdx_SF_z accStopIdx_SF_z]), [minY maxY], 'k:', 'LineWidth', 1);

subplot(4,1,2);
plot(ILtime(accStartIdx_IL), acc2_x(accStartIdx_IL), 'b--', ...
    SFtime(accStartIdx_SF:accStopIdx_SF), acc1_x(accStartIdx_SF:accStopIdx_SF), 'c', ...
    ILtime(accStartIdx_IL:accStopIdx_IL), acc2_x(accStartIdx_IL:accStopIdx_IL), 'b--');
set(gca, 'Position', plot42Pos, 'FontSize', 12, 'FontName', 'Times');
%title('Accelerations during the acceleration test', 'FontSize', 14, 'FontName', 'Times');
legend('A (Inertia-Link)', 'B (SparkFun)', 'Location', legendLocation(acc2_x(accStartIdx_IL:accStopIdx_IL)));
ylabel('acc_x (m/s^2)', 'FontSize', 12, 'FontName', 'Times');
hold on;

maxY = max([max(acc1_x(accStartIdx_SF:accStopIdx_SF)), max(acc2_x(accStartIdx_IL:accStopIdx_IL))]);
%maxY = max([1.1*maxY 0.9*maxY]);
minY = min([min(acc1_x(accStartIdx_SF:accStopIdx_SF)), min(acc2_x(accStartIdx_IL:accStopIdx_IL))]);
%minY = min([1.1*minY 0.9*minY]);
plot(SFtime([accStartIdx_SF_x accStartIdx_SF_x]), [minY maxY], 'k:', SFtime([accStopIdx_SF_x accStopIdx_SF_x]), [minY maxY], 'k:', 'LineWidth', 1);
plot(SFtime([accStartIdx_SF_y accStartIdx_SF_y]), [minY maxY], 'k:', SFtime([accStopIdx_SF_y accStopIdx_SF_y]), [minY maxY], 'k:', 'LineWidth', 1);
plot(SFtime([accStartIdx_SF_z accStartIdx_SF_z]), [minY maxY], 'k:', SFtime([accStopIdx_SF_z accStopIdx_SF_z]), [minY maxY], 'k:', 'LineWidth', 1);

axis([SFtime(accStartIdx_SF) SFtime(accStopIdx_SF) minY maxY]);

subplot(4,1,3);
plot(SFtime(accStartIdx_SF:accStopIdx_SF), acc1_y(accStartIdx_SF:accStopIdx_SF), 'c', ...
    ILtime(accStartIdx_IL:accStopIdx_IL), acc2_y(accStartIdx_IL:accStopIdx_IL), 'b--');
set(gca, 'Position', plot43Pos, 'FontSize', 12, 'FontName', 'Times');
ylabel('acc_y (m/s^2)', 'FontSize', 12, 'FontName', 'Times');
hold on;

maxY = max([max(acc1_y(accStartIdx_SF:accStopIdx_SF)), max(acc2_y(accStartIdx_IL:accStopIdx_IL))]);
%maxY = max([1.1*maxY 0.9*maxY]);
minY = min([min(acc1_y(accStartIdx_SF:accStopIdx_SF)), min(acc2_y(accStartIdx_IL:accStopIdx_IL))]);
%minY = min([1.1*minY 0.9*minY]);
plot(SFtime([accStartIdx_SF_x accStartIdx_SF_x]), [minY maxY], 'k:', SFtime([accStopIdx_SF_x accStopIdx_SF_x]), [minY maxY], 'k:', 'LineWidth', 1);
plot(SFtime([accStartIdx_SF_y accStartIdx_SF_y]), [minY maxY], 'k:', SFtime([accStopIdx_SF_y accStopIdx_SF_y]), [minY maxY], 'k:', 'LineWidth', 1);
plot(SFtime([accStartIdx_SF_z accStartIdx_SF_z]), [minY maxY], 'k:', SFtime([accStopIdx_SF_z accStopIdx_SF_z]), [minY maxY], 'k:', 'LineWidth', 1);
axis([SFtime(accStartIdx_SF) SFtime(accStopIdx_SF) minY maxY]);

subplot(4,1,4);
plot(SFtime(accStartIdx_SF:accStopIdx_SF), acc1_z(accStartIdx_SF:accStopIdx_SF), 'c', ...
    ILtime(accStartIdx_IL:accStopIdx_IL), acc2_z(accStartIdx_IL:accStopIdx_IL), 'b--');
set(gca, 'Position', plot44Pos, 'FontSize', 12, 'FontName', 'Times');
ylabel('acc_z (m/s^2)', 'FontSize', 12, 'FontName', 'Times');
xlabel('time (s)', 'FontSize', 12, 'FontName', 'Times');
hold on;

maxY = max([max(acc1_z(accStartIdx_SF:accStopIdx_SF)), max(acc2_z(accStartIdx_IL:accStopIdx_IL))]);
%maxY = max([1.1*maxY 0.9*maxY]);
minY = min([min(acc1_z(accStartIdx_SF:accStopIdx_SF)), min(acc2_z(accStartIdx_IL:accStopIdx_IL))]);
%minY = min([1.1*minY 0.9*minY]);
plot(SFtime([accStartIdx_SF_x accStartIdx_SF_x]), [minY maxY], 'k:', SFtime([accStopIdx_SF_x accStopIdx_SF_x]), [minY maxY], 'k:', 'LineWidth', 1);
plot(SFtime([accStartIdx_SF_y accStartIdx_SF_y]), [minY maxY], 'k:', SFtime([accStopIdx_SF_y accStopIdx_SF_y]), [minY maxY], 'k:', 'LineWidth', 1);
plot(SFtime([accStartIdx_SF_z accStartIdx_SF_z]), [minY maxY], 'k:', SFtime([accStopIdx_SF_z accStopIdx_SF_z]), [minY maxY], 'k:', 'LineWidth', 1);

plot([SFtime(accStartIdx_SF_x)+arrowOffset SFtime(accStopIdx_SF_x)-arrowOffset], [minY minY]+arrowOffset_y, 'k:', ...
    SFtime(accStartIdx_SF_x)+arrowOffset, minY+arrowOffset_y, 'k<', SFtime(accStopIdx_SF_x)-arrowOffset, minY+arrowOffset_y, 'k>', 'LineWidth', 1, 'MarkerSize', markerSize);
text(SFtime(accStartIdx_SF_x)+2*arrowOffset, minY-arrowOffset_y, 'x-axis movement', 'FontSize', 12, 'FontName', 'Times');

plot([SFtime(accStartIdx_SF_y)+arrowOffset SFtime(accStopIdx_SF_y)-arrowOffset], [minY minY]+arrowOffset_y, 'k:', ...
    SFtime(accStartIdx_SF_y)+arrowOffset, minY+arrowOffset_y, 'k<', SFtime(accStopIdx_SF_y)-arrowOffset, minY+arrowOffset_y, 'k>', 'LineWidth', 1, 'MarkerSize', markerSize);
text(SFtime(accStartIdx_SF_y)+2*arrowOffset, minY-arrowOffset_y, 'y-axis movement', 'FontSize', 12, 'FontName', 'Times');

plot([SFtime(accStartIdx_SF_z)+arrowOffset SFtime(accStopIdx_SF_z)-arrowOffset], [minY minY]+arrowOffset_y, 'k:', ...
    SFtime(accStartIdx_SF_z)+arrowOffset, minY+arrowOffset_y, 'k<', SFtime(accStopIdx_SF_z)-arrowOffset, minY+arrowOffset_y, 'k>', 'LineWidth', 1, 'MarkerSize', markerSize);
text(SFtime(accStartIdx_SF_z)+2*arrowOffset, minY-arrowOffset_y, 'z-axis movement', 'FontSize', 12, 'FontName', 'Times');

axis([SFtime(accStartIdx_SF) SFtime(accStopIdx_SF) minY-4*arrowOffset_y maxY]);

set(gcf, 'Position', figPos + [(figIdx-1)*100 0 0 150]);
set(gcf, 'PaperUnits', 'centimeters', 'PaperType', 'A4', 'PaperPosition', [0.63 0.63 19.72 28.41]);
set(gcf,'PaperPositionMode','auto');
saveas(gcf, ['figures/test_fig_' int2str(figIdx) '.fig']);
print('-depsc','-tiff','-r300',['figures/test_fig_' int2str(figIdx) '.eps'])
figIdx = figIdx + 1;

% DCM states
figure(figIdx); clf;
subplot(3,1,1);
plot(ILtime, x_hist2(:,1), 'b--',  SFtime, x_hist1(:,1), 'c', ILtime, -data.InertiaLink.M13, 'm-.', Ktime, R31, 'k--');
legend('DCM_A', 'DCM_B', 'Inertia-Link', 'Kuka', 'Location', legendLocation([x_hist2(:,1) -data.InertiaLink.M13]));
set(gca, 'Position', plot1Pos, 'FontSize', 12, 'FontName', 'Times');
xlabel('time (s)', 'FontSize', 12, 'FontName', 'Times');
ylabel('x1 (R31)', 'FontSize', 12, 'FontName', 'Times');
title('DCM states', 'FontSize', 14, 'FontName', 'Times');
axis([0 commonDuration -1.1 1.1]);

subplot(3,1,2);
plot(ILtime, x_hist2(:,2), 'b--',  SFtime, x_hist1(:,2), 'c', ILtime, -data.InertiaLink.M23, 'm-.', Ktime, R32, 'k--');
set(gca, 'Position', plot2Pos, 'FontSize', 12, 'FontName', 'Times');
ylabel('x2 (R32)', 'FontSize', 12, 'FontName', 'Times');
axis([0 commonDuration -1.1 1.1]);

subplot(3,1,3);
plot(ILtime, x_hist2(:,3), 'b--', SFtime, x_hist1(:,3), 'c', ILtime, -data.InertiaLink.M33, 'm-.', Ktime, R33, 'k--');
set(gca, 'Position', plot3Pos, 'FontSize', 12, 'FontName', 'Times');
xlabel('time (s)', 'FontSize', 12, 'FontName', 'Times');
ylabel('x3 (R33)', 'FontSize', 12, 'FontName', 'Times');
axis([0 commonDuration -1.1 1.1]);

set(gcf, 'Position', figPos + [(figIdx-1)*100 0 0 0]);
set(gcf, 'PaperUnits', 'centimeters', 'PaperType', 'A4', 'PaperPosition', [0.63 0.63 19.72 28.41]);
set(gcf,'PaperPositionMode','auto');
saveas(gcf, ['figures/test_fig_' int2str(figIdx) '.fig']);
print('-depsc','-tiff','-r300',['figures/test_fig_' int2str(figIdx) '.eps'])
figIdx = figIdx + 1;

% yaw
disp(' ');
disp('Yaw errors:')
dt = 1/150;
yaw_DCM_error = (ypr_hist2(StartIdx:StopIdx,1)-yaw_in_IL)*180/pi;
yaw_DCM_delta_error = [0; yaw_DCM_error(2:end) - yaw_DCM_error(1:end-1)] ./dt;
yaw_DCM_error_acc = yaw_DCM_error(accStartIdx:accStopIdx)-yaw_DCM_error(accStartIdx);
yaw_DCM_error_rot = yaw_DCM_error(rotStartIdx:rotStopIdx)-yaw_DCM_error(rotStartIdx);
yaw_DCM_RMSE_acc = sqrt(mean(yaw_DCM_error_acc.^2))
yaw_DCM_RMSE_rot = sqrt(mean(yaw_DCM_error_rot.^2))

yaw_sf_error = yaw_sf_in_IL - yaw_in_IL*180/pi;
yaw_sf_delta_error = [0; yaw_sf_error(2:end) - yaw_sf_error(1:end-1)] ./dt;
yaw_sf_error_acc = yaw_sf_error(accStartIdx:accStopIdx)-yaw_sf_error(accStartIdx);
yaw_sf_error_rot = yaw_sf_error(rotStartIdx:rotStopIdx)-yaw_sf_error(rotStartIdx);
yaw_sf_RMSE_acc = sqrt(mean(yaw_sf_error_acc.^2))
yaw_sf_RMSE_rot = sqrt(mean(yaw_sf_error_rot.^2))

yaw_mad_B_error = euler_mad_B(StartIdx:StopIdx,3) - yaw_in_IL*180/pi;
yaw_mad_B_delta_error = [0; yaw_mad_B_error(2:end) - yaw_mad_B_error(1:end-1)] ./dt;
yaw_mad_B_error_acc = yaw_mad_B_error(accStartIdx:accStopIdx)-yaw_mad_B_error(accStartIdx);
yaw_mad_B_error_rot = yaw_mad_B_error(rotStartIdx:rotStopIdx)-yaw_mad_B_error(rotStartIdx);
yaw_mad_B_RMSE_acc = sqrt(mean(yaw_mad_B_error_acc.^2))
yaw_mad_B_RMSE_rot = sqrt(mean(yaw_mad_B_error_rot.^2))

yaw_mah_B_error = euler_mah_B(StartIdx:StopIdx,3) - yaw_in_IL*180/pi;
yaw_mah_B_delta_error = [0; yaw_mah_B_error(2:end) - yaw_mah_B_error(1:end-1)] ./dt;
yaw_mah_B_error_acc = yaw_mah_B_error(accStartIdx:accStopIdx)-yaw_mah_B_error(accStartIdx);
yaw_mah_B_error_rot = yaw_mah_B_error(rotStartIdx:rotStopIdx)-yaw_mah_B_error(rotStartIdx);
yaw_mah_B_RMSE_acc = sqrt(mean(yaw_mah_B_error_acc.^2))
yaw_mah_B_RMSE_rot = sqrt(mean(yaw_mah_B_error_rot.^2))

yaw_mad_A_error = euler_mad_A(StartIdx:StopIdx,3) - yaw_in_IL*180/pi;
yaw_mad_A_delta_error = [0; yaw_mad_A_error(2:end) - yaw_mad_A_error(1:end-1)] ./dt;
yaw_mad_A_error_acc = yaw_mad_A_error(accStartIdx:accStopIdx)-yaw_mad_A_error(accStartIdx);
yaw_mad_A_error_rot = yaw_mad_A_error(rotStartIdx:rotStopIdx)-yaw_mad_A_error(rotStartIdx);
yaw_mad_A_RMSE_acc = sqrt(mean(yaw_mad_A_error_acc.^2))
yaw_mad_A_RMSE_rot = sqrt(mean(yaw_mad_A_error_rot.^2))

yaw_mah_A_error = euler_mah_A(StartIdx:StopIdx,3) - yaw_in_IL*180/pi;
yaw_mah_A_delta_error = [0; yaw_mah_A_error(2:end) - yaw_mah_A_error(1:end-1)] ./dt;
yaw_mah_A_error_acc = yaw_mah_A_error(accStartIdx:accStopIdx)-yaw_mah_A_error(accStartIdx);
yaw_mah_A_error_rot = yaw_mah_A_error(rotStartIdx:rotStopIdx)-yaw_mah_A_error(rotStartIdx);
yaw_mah_A_RMSE_acc = sqrt(mean(yaw_mah_A_error_acc.^2))
yaw_mah_A_RMSE_rot = sqrt(mean(yaw_mah_A_error_rot.^2))

yaw_IL_error = (yaw_IL(StartIdx:StopIdx)-yaw_in_IL)*180/pi;
yaw_IL_delta_error = [0; yaw_IL_error(2:end) - yaw_IL_error(1:end-1)] ./dt;
yaw_IL_error_acc = yaw_IL_error(accStartIdx:accStopIdx)-yaw_IL_error(accStartIdx);
yaw_IL_error_rot = yaw_IL_error(rotStartIdx:rotStopIdx)-yaw_IL_error(rotStartIdx);
yaw_IL_RMSE_acc = sqrt(mean(yaw_IL_error_acc.^2))
yaw_IL_RMSE_rot = sqrt(mean(yaw_IL_error_rot.^2))

%pitch
disp(' ');
disp('Pitch errors:')
pitch_DCM_error = (ypr_hist2(StartIdx:StopIdx,2)-pitch_in_IL)*180/pi;
pitch_DCM_RMSE_acc = sqrt(mean(pitch_DCM_error(accStartIdx:accStopIdx).^2))
pitch_DCM_RMSE_rot = sqrt(mean(pitch_DCM_error(rotStartIdx:rotStopIdx).^2))

pitch_sf_error = pitch_sf_in_IL - pitch_in_IL*180/pi;
pitch_sf_RMSE_acc = sqrt(mean(pitch_sf_error(accStartIdx:accStopIdx).^2))
pitch_sf_RMSE_rot = sqrt(mean(pitch_sf_error(rotStartIdx:rotStopIdx).^2))

pitch_mad_B_error = euler_mad_B(StartIdx:StopIdx,2) - pitch_in_IL*180/pi;
pitch_mad_B_RMSE_acc = sqrt(mean(pitch_mad_B_error(accStartIdx:accStopIdx).^2))
pitch_mad_B_RMSE_rot = sqrt(mean(pitch_mad_B_error(rotStartIdx:rotStopIdx).^2))

pitch_mah_B_error = euler_mah_B(StartIdx:StopIdx,2) - pitch_in_IL*180/pi;
pitch_mah_B_RMSE_acc = sqrt(mean(pitch_mah_B_error(accStartIdx:accStopIdx).^2))
pitch_mah_B_RMSE_rot = sqrt(mean(pitch_mah_B_error(rotStartIdx:rotStopIdx).^2))

pitch_mad_A_error = euler_mad_A(StartIdx:StopIdx,2) - pitch_in_IL*180/pi;
pitch_mad_A_RMSE_acc = sqrt(mean(pitch_mad_A_error(accStartIdx:accStopIdx).^2))
pitch_mad_A_RMSE_rot = sqrt(mean(pitch_mad_A_error(rotStartIdx:rotStopIdx).^2))

pitch_mah_A_error = euler_mah_A(StartIdx:StopIdx,2) - pitch_in_IL*180/pi;
pitch_mah_A_RMSE_acc = sqrt(mean(pitch_mah_A_error(accStartIdx:accStopIdx).^2))
pitch_mah_A_RMSE_rot = sqrt(mean(pitch_mah_A_error(rotStartIdx:rotStopIdx).^2))

pitch_IL_error = (pitch_IL(StartIdx:StopIdx)-pitch_in_IL)*180/pi;
pitch_IL_RMSE_acc = sqrt(mean(pitch_IL_error(accStartIdx:accStopIdx).^2))
pitch_IL_RMSE_rot = sqrt(mean(pitch_IL_error(rotStartIdx:rotStopIdx).^2))

%roll
disp(' ');
disp('Roll errors:')
roll_DCM_error = (ypr_hist2(StartIdx:StopIdx,3)-roll_in_IL)*180/pi;
roll_DCM_RMSE_acc = sqrt(mean(roll_DCM_error(accStartIdx:accStopIdx).^2))
roll_DCM_RMSE_rot = sqrt(mean(roll_DCM_error(rotStartIdx:rotStopIdx).^2))

roll_sf_error = roll_sf_in_IL - roll_in_IL*180/pi;
roll_sf_RMSE_acc = sqrt(mean(roll_sf_error(accStartIdx:accStopIdx).^2))
roll_sf_RMSE_rot = sqrt(mean(roll_sf_error(rotStartIdx:rotStopIdx).^2))

roll_mad_B_error = euler_mad_B(StartIdx:StopIdx,1) - roll_in_IL*180/pi;
roll_mad_B_RMSE_acc = sqrt(mean(roll_mad_B_error(accStartIdx:accStopIdx).^2))
roll_mad_B_RMSE_rot = sqrt(mean(roll_mad_B_error(rotStartIdx:rotStopIdx).^2))

roll_mah_B_error = euler_mah_B(StartIdx:StopIdx,1) - roll_in_IL*180/pi;
roll_mah_B_RMSE_acc = sqrt(mean(roll_mah_B_error(accStartIdx:accStopIdx).^2))
roll_mah_B_RMSE_rot = sqrt(mean(roll_mah_B_error(rotStartIdx:rotStopIdx).^2))

roll_mad_A_error = euler_mad_A(StartIdx:StopIdx,1) - roll_in_IL*180/pi;
roll_mad_A_RMSE_acc = sqrt(mean(roll_mad_A_error(accStartIdx:accStopIdx).^2))
roll_mad_A_RMSE_rot = sqrt(mean(roll_mad_A_error(rotStartIdx:rotStopIdx).^2))

roll_mah_A_error = euler_mah_A(StartIdx:StopIdx,1) - roll_in_IL*180/pi;
roll_mah_A_RMSE_acc = sqrt(mean(roll_mah_A_error(accStartIdx:accStopIdx).^2))
roll_mah_A_RMSE_rot = sqrt(mean(roll_mah_A_error(rotStartIdx:rotStopIdx).^2))

roll_IL_error = (roll_IL(StartIdx:StopIdx)-roll_in_IL)*180/pi;
roll_IL_RMSE_acc = sqrt(mean(roll_IL_error(accStartIdx:accStopIdx).^2))
roll_IL_RMSE_rot = sqrt(mean(roll_IL_error(rotStartIdx:rotStopIdx).^2))

disp(' ');
disp('Error matrices:')
RMSE_acc = [yaw_DCM_RMSE_acc yaw_sf_RMSE_acc yaw_mad_A_RMSE_acc yaw_mad_B_RMSE_acc yaw_mah_A_RMSE_acc yaw_mah_B_RMSE_acc yaw_IL_RMSE_acc; ...
    pitch_DCM_RMSE_acc pitch_sf_RMSE_acc pitch_mad_A_RMSE_acc pitch_mad_B_RMSE_acc pitch_mah_A_RMSE_acc pitch_mah_B_RMSE_acc pitch_IL_RMSE_acc; ...
    roll_DCM_RMSE_acc roll_sf_RMSE_acc roll_mad_A_RMSE_acc roll_mad_B_RMSE_acc roll_mah_A_RMSE_acc roll_mah_B_RMSE_acc roll_IL_RMSE_acc];

disp('RMSEs of the acceleration test');
textCell = [{'DCM_A', 'DCM_B', 'Madgwick_A', 'Madgwick_B', 'Mahony_A', 'Mahony_B', 'Inertia-Link'}; cell(size(RMSE_acc))];
for i = 1:size(RMSE_acc,1)
    for j = 1:size(RMSE_acc,2)
        textCell{i+1,j} = num2str(RMSE_acc(i,j), '%.2f');
    end
end
disp(textCell');

RMSE_rot = [yaw_DCM_RMSE_rot yaw_sf_RMSE_rot yaw_mad_A_RMSE_rot yaw_mad_B_RMSE_rot yaw_mah_A_RMSE_rot yaw_mah_B_RMSE_rot yaw_IL_RMSE_rot; ...
    pitch_DCM_RMSE_rot pitch_sf_RMSE_rot pitch_mad_A_RMSE_rot pitch_mad_B_RMSE_rot pitch_mah_A_RMSE_rot pitch_mah_B_RMSE_rot pitch_IL_RMSE_rot; ...
    roll_DCM_RMSE_rot roll_sf_RMSE_rot roll_mad_A_RMSE_rot roll_mad_B_RMSE_rot roll_mah_A_RMSE_rot roll_mah_B_RMSE_rot roll_IL_RMSE_rot];

disp('RMSEs of the rotation test');
textCell = [{'DCM_A', 'DCM_B', 'Madgwick_A', 'Madgwick_B', 'Mahony_A', 'Mahony_B', 'Inertia-Link'}; cell(size(RMSE_rot))];
for i = 1:size(RMSE_rot,1)
    for j = 1:size(RMSE_rot,2)
        textCell{i+1,j} = num2str(RMSE_rot(i,j), '%.2f');
    end
end
disp(textCell');

% box and whiskers plot for the acceleration test
groups_yaw = {'DCM_A', 'Madgwick_A', 'Mahony_A', 'DCM_B','Madgwick_B', 'Mahony_B', 'Inertia-Link_ '};

groups_rollpitch = cell(1, 2*size(groups_yaw,2));
groups_rollpitch(1:2:end) = groups_yaw;
groups_rollpitch(2:2:end) = groups_yaw;

figure(figIdx); clf;
subplot(2,1,1);
boxplot([yaw_DCM_error_acc, yaw_mad_A_error_acc, yaw_mah_A_error_acc, yaw_sf_error_acc, yaw_mad_B_error_acc, yaw_mah_B_error_acc, yaw_IL_error_acc], ...
    groups_yaw, 'whisker', 15);
txt = findobj(gca,'Type','text');
set(txt, 'FontSize', 12, 'FontName', 'Times', 'Interpreter', 'tex', 'VerticalAlignment', 'middle');
set(gca, 'Position', plot21Pos, 'FontSize', 12, 'FontName', 'Times');
title('Error statistics in the acceleration test', 'FontSize', 14, 'FontName', 'Times');
ylabel('yaw errors (deg)');

subplot(2,1,2);

boxplot([pitch_DCM_error(accStartIdx:accStopIdx), roll_DCM_error(accStartIdx:accStopIdx), ...
    pitch_mad_A_error(accStartIdx:accStopIdx), roll_mad_A_error(accStartIdx:accStopIdx), ...
    pitch_mah_A_error(accStartIdx:accStopIdx), roll_mah_A_error(accStartIdx:accStopIdx), ...
    pitch_sf_error(accStartIdx:accStopIdx), roll_sf_error(accStartIdx:accStopIdx), ...
    pitch_mad_B_error(accStartIdx:accStopIdx), roll_mad_B_error(accStartIdx:accStopIdx), ...
    pitch_mah_B_error(accStartIdx:accStopIdx), roll_mah_B_error(accStartIdx:accStopIdx), ...
    pitch_IL_error(accStartIdx:accStopIdx), roll_IL_error(accStartIdx:accStopIdx)], ...
    groups_rollpitch, 'whisker', 15);

txt = findobj(gca,'Type','text');
set(txt, 'FontSize', 12, 'FontName', 'Times', 'Interpreter', 'tex', 'VerticalAlignment', 'middle');

set(gca, 'Position', plot22Pos, 'FontSize', 12, 'FontName', 'Times');
ylabel('combined roll and pitch errors (deg)');

set(gcf, 'Position', figPos + [(figIdx-1)*100 0 0 0]);
set(gcf, 'PaperUnits', 'centimeters', 'PaperType', 'A4', 'PaperPosition', [0.63 0.63 19.72 28.41]);
set(gcf,'PaperPositionMode','auto');
saveas(gcf, ['figures/test_fig_' int2str(figIdx) '.fig']);
print('-depsc','-tiff','-r300',['figures/test_fig_' int2str(figIdx) '.eps'])
figIdx = figIdx + 1;

% box and whiskers plot for the rotation test
figure(figIdx); clf;
subplot(2,1,1);
boxplot([yaw_DCM_error_rot, yaw_mad_A_error_rot, yaw_mah_A_error_rot, yaw_sf_error_rot, yaw_mad_B_error_rot, yaw_mah_B_error_rot, yaw_IL_error_rot], ...
    groups_yaw, 'whisker', 15);
txt = findobj(gca,'Type','text');
set(txt, 'FontSize', 12, 'FontName', 'Times', 'Interpreter', 'tex', 'VerticalAlignment', 'middle');

set(gca, 'Position', plot21Pos, 'FontSize', 12, 'FontName', 'Times');
title('Error statistics in the rotation test', 'FontSize', 14, 'FontName', 'Times');
ylabel('yaw errors (deg)');

subplot(2,1,2);
boxplot([pitch_DCM_error(rotStartIdx:rotStopIdx), roll_DCM_error(rotStartIdx:rotStopIdx), ...
    pitch_mad_A_error(rotStartIdx:rotStopIdx), roll_mad_A_error(rotStartIdx:rotStopIdx), ...
    pitch_mah_A_error(rotStartIdx:rotStopIdx), roll_mah_A_error(rotStartIdx:rotStopIdx), ...
    pitch_sf_error(rotStartIdx:rotStopIdx), roll_sf_error(rotStartIdx:rotStopIdx), ...
    pitch_mad_B_error(rotStartIdx:rotStopIdx), roll_mad_B_error(rotStartIdx:rotStopIdx), ...
    pitch_mah_B_error(rotStartIdx:rotStopIdx), roll_mah_B_error(rotStartIdx:rotStopIdx), ...
    pitch_IL_error(rotStartIdx:rotStopIdx), roll_IL_error(rotStartIdx:rotStopIdx)], ...
    groups_rollpitch, 'whisker', 15);
txt = findobj(gca,'Type','text');
set(txt, 'FontSize', 12, 'FontName', 'Times', 'Interpreter', 'tex', 'VerticalAlignment', 'middle');

set(gca, 'Position', plot22Pos, 'FontSize', 12, 'FontName', 'Times');
ylabel('combined roll and pitch errors (deg)');

set(gcf, 'Position', figPos + [(figIdx-1)*100 0 0 0]);
set(gcf, 'PaperUnits', 'centimeters', 'PaperType', 'A4', 'PaperPosition', [0.63 0.63 19.72 28.41]);
set(gcf,'PaperPositionMode','auto');
saveas(gcf, ['figures/test_fig_' int2str(figIdx) '.fig']);
print('-depsc','-tiff','-r300',['figures/test_fig_' int2str(figIdx) '.eps'])
figIdx = figIdx + 1;

% correlation tests for delays
yaw_ref = yaw_in_IL*180/pi;
pitch_ref = pitch_in_IL*180/pi;
roll_ref = roll_in_IL*180/pi;

yaw_dcm = ypr_hist2(StartIdx:StopIdx,1)*180/pi;
pitch_dcm = ypr_hist2(StartIdx:StopIdx,2)*180/pi;
roll_dcm = ypr_hist2(StartIdx:StopIdx,3)*180/pi;

madgwick = euler_mad_A(StartIdx:StopIdx,:);
mahony = euler_mah_A(StartIdx:StopIdx,:);

yaw_il = yaw_IL(StartIdx:StopIdx)*180/pi;
pitch_il = pitch_IL(StartIdx:StopIdx)*180/pi;
roll_il = roll_IL(StartIdx:StopIdx)*180/pi;

n_lags = 30;
[c_dcm_yaw,lags] = rootMeanSquaredErrors(yaw_dcm, yaw_ref, n_lags);
[min_c_dcm_yaw, min_c_dcm_yaw_idx] = min(c_dcm_yaw);
c_madgwick_yaw = rootMeanSquaredErrors(madgwick(:,3), yaw_ref, n_lags);
[min_c_madgwick_yaw, min_c_madgwick_yaw_idx] = min(c_madgwick_yaw);
c_mahony_yaw = rootMeanSquaredErrors(mahony(:,3), yaw_ref, n_lags);
[min_c_mahony_yaw, min_c_mahony_yaw_idx] = min(c_mahony_yaw);
c_il_yaw = rootMeanSquaredErrors(yaw_il, yaw_ref, n_lags);
[min_c_il_yaw, min_c_il_yaw_idx] = min(c_il_yaw);

c_dcm_pitch = rootMeanSquaredErrors(pitch_dcm, pitch_ref, n_lags);
[min_c_dcm_pitch, min_c_dcm_pitch_idx] = min(c_dcm_pitch);
c_madgwick_pitch = rootMeanSquaredErrors(madgwick(:,2), pitch_ref, n_lags);
[min_c_madgwick_pitch, min_c_madgwick_pitch_idx] = min(c_madgwick_pitch);
c_mahony_pitch = rootMeanSquaredErrors(mahony(:,2), pitch_ref, n_lags);
[min_c_mahony_pitch, min_c_mahony_pitch_idx] = min(c_mahony_pitch);
c_il_pitch = rootMeanSquaredErrors(pitch_il, pitch_ref, n_lags);
[min_c_il_pitch, min_c_il_pitch_idx] = min(c_il_pitch);

c_dcm_roll = rootMeanSquaredErrors(roll_dcm, roll_ref, n_lags);
[min_c_dcm_roll, min_c_dcm_roll_idx] = min(c_dcm_roll);
c_madgwick_roll = rootMeanSquaredErrors(madgwick(:,1), roll_ref, n_lags);
[min_c_madgwick_roll, min_c_madgwick_roll_idx] = min(c_madgwick_roll);
c_mahony_roll = rootMeanSquaredErrors(mahony(:,1), roll_ref, n_lags);
[min_c_mahony_roll, min_c_mahony_roll_idx] = min(c_mahony_roll);
c_il_roll = rootMeanSquaredErrors(roll_il, roll_ref, n_lags);
[min_c_il_roll, min_c_il_roll_idx] = min(c_il_roll);

lags = lags*dt;

% mean squared errors plot
figure(figIdx); clf;
subplot(3,1,1);
plot(lags, c_dcm_yaw, 'b', lags, c_madgwick_yaw, 'g--', lags, c_mahony_yaw, 'r:', lags, c_il_yaw, 'm-.', 'LineWidth', 1, 'MarkerSize', markerSize);
set(gca, 'Position', plot1Pos, 'FontSize', 12, 'FontName', 'Times');
title('RMSEs to the reference measurement as a function of time delay', 'FontSize', 14, 'FontName', 'Times');
ylabel('yaw RMSE (deg)');
legend('DCM', 'Madgwick', 'Mahony', 'Inertia-Link', 'Location', legendLocation([c_dcm_yaw' c_madgwick_yaw' c_mahony_yaw' c_il_yaw']));
hold on;
plot(lags(min_c_dcm_yaw_idx), c_dcm_yaw(min_c_dcm_yaw_idx), 'b*', ...
    lags(min_c_madgwick_yaw_idx), c_madgwick_yaw(min_c_madgwick_yaw_idx), 'g^', ...
    lags(min_c_mahony_yaw_idx), c_mahony_yaw(min_c_mahony_yaw_idx), 'ro', ...
    lags(min_c_il_yaw_idx), c_il_yaw(min_c_il_yaw_idx), 'mp', 'LineWidth', 1, 'MarkerSize', markerSize);

subplot(3,1,2);
plot(lags, c_dcm_pitch, 'b', lags, c_madgwick_pitch, 'g--', lags, c_mahony_pitch, 'r:', lags, c_il_pitch, 'm-.', 'LineWidth', 1, 'MarkerSize', markerSize);
set(gca, 'Position', plot2Pos, 'FontSize', 12, 'FontName', 'Times');
ylabel('pitch RMSE (deg)');
hold on;
plot(lags(min_c_dcm_pitch_idx), c_dcm_pitch(min_c_dcm_pitch_idx), 'b*', ...
    lags(min_c_madgwick_pitch_idx), c_madgwick_pitch(min_c_madgwick_pitch_idx), 'g^', ...
    lags(min_c_mahony_pitch_idx), c_mahony_pitch(min_c_mahony_pitch_idx), 'ro', ...
    lags(min_c_il_pitch_idx), c_il_pitch(min_c_il_pitch_idx), 'mp', 'LineWidth', 1, 'MarkerSize', markerSize);

subplot(3,1,3);
plot(lags, c_dcm_roll, 'b', lags, c_madgwick_roll, 'g--', lags, c_mahony_roll, 'r:', lags, c_il_roll, 'm-.', 'LineWidth', 1, 'MarkerSize', markerSize);
set(gca, 'Position', plot3Pos, 'FontSize', 12, 'FontName', 'Times');
ylabel('roll RMSE (deg)');
xlabel('time delay to reference (s)');
hold on;
plot(lags(min_c_dcm_roll_idx), c_dcm_roll(min_c_dcm_roll_idx), 'b*', ...
    lags(min_c_madgwick_roll_idx), c_madgwick_roll(min_c_madgwick_roll_idx), 'g^', ...
    lags(min_c_mahony_roll_idx), c_mahony_roll(min_c_mahony_roll_idx), 'ro', ...
    lags(min_c_il_roll_idx), c_il_roll(min_c_il_roll_idx), 'mp', 'LineWidth', 1, 'MarkerSize', markerSize);

set(gcf, 'Position', figPos + [(figIdx-1)*100 0 0 0]);
set(gcf, 'PaperUnits', 'centimeters', 'PaperType', 'A4', 'PaperPosition', [0.63 0.63 19.72 28.41]);
set(gcf,'PaperPositionMode','auto');
saveas(gcf, ['figures/test_fig_' int2str(figIdx) '.fig']);
print('-depsc','-tiff','-r300',['figures/test_fig_' int2str(figIdx) '.eps'])
figIdx = figIdx + 1;

% Yaw, pitch and roll plot
figure(figIdx); clf;
subplot(3,1,1);
plot(time, yaw_dcm, 'b', time, madgwick(:,3), 'g--', time, mahony(:,3), 'r:', time, yaw_il, 'm-.', time, yaw_ref, 'k--', 'LineWidth', 1);
set(gca, 'Position', plot1Pos, 'FontSize', 12, 'FontName', 'Times');
legend('DCM', 'Madgwick', 'Mahony', 'Inertia-Link', 'Reference', 'Location', legendLocation([yaw_dcm yaw_ref madgwick(:,3) mahony(:,3)]));
hold on;
maxY = max([max(yaw_dcm), max(madgwick(:,3)), max(mahony(:,3)), max(yaw_il), max(yaw_ref)]);
maxY = max([1.1*maxY 0.9*maxY]);
minY = min([min(yaw_dcm), min(madgwick(:,3)), min(mahony(:,3)), min(yaw_il), min(yaw_ref)]);
minY = min([1.1*minY 0.9*minY]);
plot(time([accStartIdx accStartIdx]), [minY maxY], 'k:', time([accStopIdx accStopIdx]), [minY maxY], 'k:', 'LineWidth', 1);
plot(time([rotStartIdx rotStartIdx]), [minY maxY], 'k:', time([rotStopIdx rotStopIdx]), [minY maxY], 'k:', 'LineWidth', 1);
ylabel('yaw (deg)', 'FontSize', 12, 'FontName', 'Times');
title('Euler angles', 'FontSize', 14, 'FontName', 'Times');
axis([time(1) time(end) minY maxY]);

subplot(3,1,2);
plot(time, pitch_dcm, 'b', time, madgwick(:,2), 'g--', time, mahony(:,2), 'r:', time, pitch_il, 'm-.', time, pitch_ref, 'k--', 'LineWidth', 1);
hold on;
maxY = max([max(pitch_dcm), max(madgwick(:,2)), max(mahony(:,2)), max(pitch_il), max(pitch_ref)]);
maxY = max([1.1*maxY 0.9*maxY]);
minY = min([min(pitch_dcm), min(madgwick(:,2)), min(mahony(:,2)), min(pitch_il), min(pitch_ref)]);
minY = min([1.1*minY 0.9*minY]);
plot(time([accStartIdx accStartIdx]), [minY maxY], 'k:', time([accStopIdx accStopIdx]), [minY maxY], 'k:', 'LineWidth', 1);
plot(time([rotStartIdx rotStartIdx]), [minY maxY], 'k:', time([rotStopIdx rotStopIdx]), [minY maxY], 'k:', 'LineWidth', 1);
set(gca, 'Position', plot2Pos, 'FontSize', 12, 'FontName', 'Times');
ylabel('pitch (deg)', 'FontSize', 12, 'FontName', 'Times');
axis([time(1) time(end) minY maxY]);

subplot(3,1,3);
plot(time, roll_dcm, 'b', time, madgwick(:,1), 'g--', time, mahony(:,1), 'r:', time, roll_il, 'm-.', time, roll_ref, 'k--', 'LineWidth', 1);
set(gca, 'Position', plot3Pos, 'FontSize', 12, 'FontName', 'Times');
hold on;
maxY = max([max(roll_dcm), max(madgwick(:,1)), max(mahony(:,1)), max(roll_il), max(roll_ref)]);
maxY = max([1.1*maxY 0.9*maxY]);
minY = min([min(roll_dcm), min(madgwick(:,1)), min(mahony(:,1)), min(roll_il), min(roll_ref)]);
minY = min([1.1*minY 0.9*minY]);
arrowOffset = 5;
plot(time([accStartIdx accStartIdx]), [minY maxY], 'k:', time([accStopIdx accStopIdx]), [minY maxY], 'k:', 'LineWidth', 1);
plot([time(accStartIdx)+arrowOffset time(accStopIdx)-arrowOffset], [minY minY]+arrowOffset, 'k:', ...
    time(accStartIdx)+arrowOffset, minY+arrowOffset, 'k<', time(accStopIdx)-arrowOffset, minY+arrowOffset, 'k>', 'LineWidth', 1, 'MarkerSize', markerSize);
text(time(accStartIdx)+2*arrowOffset, minY-arrowOffset, 'Acceleration test', 'FontSize', 12, 'FontName', 'Times');
plot(time([rotStartIdx rotStartIdx]), [minY maxY], 'k:', time([rotStopIdx rotStopIdx]), [minY maxY], 'k:', 'LineWidth', 1);
plot([time(rotStartIdx)+arrowOffset time(rotStopIdx)-arrowOffset], [minY minY]+arrowOffset, 'k:', ...
    time(rotStartIdx)+arrowOffset, minY+arrowOffset, 'k<', time(rotStopIdx)-arrowOffset, minY+arrowOffset, 'k>', 'LineWidth', 1, 'MarkerSize', markerSize);
text(time(rotStartIdx)+2*arrowOffset, minY-arrowOffset, 'Rotation test', 'FontSize', 12, 'FontName', 'Times');
xlabel('time (s)', 'FontSize', 12, 'FontName', 'Times');
ylabel('roll (deg)', 'FontSize', 12, 'FontName', 'Times');

axis([time(1) time(end) minY-20 maxY]);

set(gcf, 'Position', figPos + [(figIdx-1)*100 0 0 0]);
set(gcf, 'PaperUnits', 'centimeters', 'PaperType', 'A4', 'PaperPosition', [0.63 0.63 19.72 28.41]);
set(gcf,'PaperPositionMode','auto');
saveas(gcf, ['figures/test_fig_' int2str(figIdx) '.fig']);
print('-depsc','-tiff','-r300',['figures/test_fig_' int2str(figIdx) '.eps'])
figIdx = figIdx + 1;

% error plot with reference
figure(figIdx); clf;
subplot(4,1,1);
plot(time, yaw_ref, 'b', time, pitch_ref, 'g--', time, roll_ref, 'r:', 'LineWidth', 1);
set(gca, 'Position', plot41Pos, 'FontSize', 12, 'FontName', 'Times');
legend('yaw', 'pitch', 'roll', 'Location', legendLocation([yaw_ref pitch_ref roll_ref]));
title('The reference measurement and measurement errors', 'FontSize', 14, 'FontName', 'Times');
ylabel('reference angles (deg)', 'FontSize', 12, 'FontName', 'Times');

hold on;
maxY = max([max(yaw_ref), max(pitch_ref), max(roll_ref)]);
maxY = max([1.1*maxY 0.9*maxY]);
minY = min([min(yaw_ref), min(pitch_ref), min(roll_ref)]);
minY = min([1.1*minY 0.9*minY]);
arrowOffset = 5;
plot(time([accStartIdx accStartIdx]), [minY maxY], 'k:', time([accStopIdx accStopIdx]), [minY maxY], 'k:', 'LineWidth', 1);
plot(time([rotStartIdx rotStartIdx]), [minY maxY], 'k:', time([rotStopIdx rotStopIdx]), [minY maxY], 'k:', 'LineWidth', 1);

axis([time(1) time(end) minY maxY]);

subplot(4,1,2);
plot(time, yaw_DCM_error, 'b', time, yaw_mad_A_error, 'g--', time, yaw_mah_A_error, 'r:', time, yaw_IL_error, 'm-.', 'LineWidth', 1, 'EraseMode', 'xor');
set(gca, 'Position', plot42Pos, 'FontSize', 12, 'FontName', 'Times');
legend('DCM', 'Madgwick', 'Mahony', 'Inertia-Link', 'Location', legendLocation([yaw_DCM_error yaw_IL_error yaw_mad_A_error yaw_mah_A_error]));
hold on;
ylabel('yaw errors (deg)', 'FontSize', 12, 'FontName', 'Times');
set(gca, 'XLim', [time(1) time(end)]);

maxY = max([max(yaw_DCM_error), max(yaw_mad_A_error), max(yaw_mah_A_error), max(yaw_IL_error)]);
maxY = max([1.1*maxY 0.9*maxY]);
minY = min([min(yaw_DCM_error), min(yaw_mad_A_error), min(yaw_mah_A_error), min(yaw_IL_error)]);
minY = min([1.1*minY 0.9*minY]);

plot(time([accStartIdx accStartIdx]), [minY maxY], 'k:', time([accStopIdx accStopIdx]), [minY maxY], 'k:', 'LineWidth', 1);
plot(time([rotStartIdx rotStartIdx]), [minY maxY], 'k:', time([rotStopIdx rotStopIdx]), [minY maxY], 'k:', 'LineWidth', 1);

axis([time(1) time(end) minY maxY]);

subplot(4,1,3);
plot(time, pitch_DCM_error, 'b', time, pitch_mad_A_error, 'g--', time, pitch_mah_A_error, 'r:', time, pitch_IL_error, 'm-.', 'LineWidth', 1, 'EraseMode', 'xor');
hold on;
set(gca, 'Position', plot43Pos, 'FontSize', 12, 'FontName', 'Times');
ylabel('pitch errors (deg)', 'FontSize', 12, 'FontName', 'Times');

maxY = max([max(pitch_DCM_error), max(pitch_mad_A_error), max(pitch_mah_A_error), max(pitch_IL_error)]);
maxY = max([1.1*maxY 0.9*maxY]);
minY = min([min(pitch_DCM_error), min(pitch_mad_A_error), min(pitch_mah_A_error), min(pitch_IL_error)]);
minY = min([1.1*minY 0.9*minY]);

plot(time([accStartIdx accStartIdx]), [minY maxY], 'k:', time([accStopIdx accStopIdx]), [minY maxY], 'k:', 'LineWidth', 1);
plot(time([rotStartIdx rotStartIdx]), [minY maxY], 'k:', time([rotStopIdx rotStopIdx]), [minY maxY], 'k:', 'LineWidth', 1);

axis([time(1) time(end) minY maxY]);

subplot(4,1,4);
plot(time, roll_DCM_error, 'b', time, roll_mad_A_error, 'g--', time, roll_mah_A_error, 'r:', time, roll_IL_error, 'm-.', 'LineWidth', 1, 'EraseMode', 'xor');
hold on;
set(gca, 'Position', plot44Pos, 'FontSize', 12, 'FontName', 'Times');
xlabel('time (s)', 'FontSize', 12, 'FontName', 'Times');
ylabel('roll errors (deg)', 'FontSize', 12, 'FontName', 'Times');

maxY = max([max(roll_DCM_error), max(roll_mad_A_error), max(roll_mah_A_error), max(roll_IL_error)]);
maxY = max([1.1*maxY 0.9*maxY]);
minY = min([min(roll_DCM_error), min(roll_mad_A_error), min(roll_mah_A_error), min(roll_IL_error)]);
minY = min([1.1*minY 0.9*minY]);

arrowOffset_y = 0.5;

plot(time([accStartIdx accStartIdx]), [minY maxY], 'k:', time([accStopIdx accStopIdx]), [minY maxY], 'k:', 'LineWidth', 1);
plot([time(accStartIdx)+arrowOffset time(accStopIdx)-arrowOffset], [minY minY]+arrowOffset_y, 'k:', ...
    time(accStartIdx)+arrowOffset, minY+arrowOffset_y, 'k<', time(accStopIdx)-arrowOffset, minY+arrowOffset_y, 'k>', 'LineWidth', 1, 'MarkerSize', markerSize);
text(time(accStartIdx)+2*arrowOffset, minY-arrowOffset_y, 'Acceleration test', 'FontSize', 12, 'FontName', 'Times');
plot(time([rotStartIdx rotStartIdx]), [minY maxY], 'k:', time([rotStopIdx rotStopIdx]), [minY maxY], 'k:', 'LineWidth', 1);
plot([time(rotStartIdx)+arrowOffset time(rotStopIdx)-arrowOffset], [minY minY]+arrowOffset_y, 'k:', ...
    time(rotStartIdx)+arrowOffset, minY+arrowOffset_y, 'k<', time(rotStopIdx)-arrowOffset, minY+arrowOffset_y, 'k>', 'LineWidth', 1, 'MarkerSize', markerSize);
text(time(rotStartIdx)+2*arrowOffset, minY-arrowOffset_y, 'Rotation test', 'FontSize', 12, 'FontName', 'Times');

axis([time(1) time(end) minY-4*arrowOffset_y maxY]);

set(gcf, 'Position', figPos + [(figIdx-1)*100 0 0 150]);
set(gcf, 'PaperUnits', 'centimeters', 'PaperType', 'A4', 'PaperPosition', [0.63 0.63 19.72 28.41]);
set(gcf,'PaperPositionMode','auto');
saveas(gcf, ['figures/test_fig_' int2str(figIdx) '.fig']);
print('-depsc','-tiff','-r300',['figures/test_fig_' int2str(figIdx) '.eps'])
figIdx = figIdx + 1;

% bias plot (states with variances)
figure(figIdx); clf;

x_sigma = sqrt(P_diag_hist2(StartIdx:StopIdx,4));
y_sigma = sqrt(P_diag_hist2(StartIdx:StopIdx,5));
z_sigma = sqrt(P_diag_hist2(StartIdx:StopIdx,6));

plotLims = 1.25*[mean(x_sigma), mean(y_sigma), mean(z_sigma)];

bias_x = x_hist2(StartIdx:StopIdx,4)*180/pi;
bias_y = x_hist2(StartIdx:StopIdx,5)*180/pi;
bias_z = x_hist2(StartIdx:StopIdx,6)*180/pi;

subplot(3,1,1);
plot([time(1) time(end)], [addedGyroBias addedGyroBias]*180/pi, 'k--', ...
    time, bias_x, 'b', time, (addedGyroBias-x_sigma)*180/pi, 'b-.', ...
    time, (addedGyroBias+x_sigma)*180/pi, 'b-.', 'LineWidth', 1, 'MarkerSize', markerSize);
h = legend('Reference', 'Estimate', '1-sigma');
set(gca, 'Position', plot1Pos, 'FontSize', 12, 'FontName', 'Times');

ylabel('x_b_i_a_s (deg/s)', 'FontSize', 12, 'FontName', 'Times');
title('Bias estimates and 1-sigma distances of standard deviations', 'FontSize', 14, 'FontName', 'Times');
set(gca, 'XLim', [time(1) time(end)], 'YLim', [addedGyroBias-plotLims(1) addedGyroBias+plotLims(1)]*180/pi);

subplot(3,1,2);
plot([time(1) time(end)], [addedGyroBias addedGyroBias]*180/pi, 'k--', ...
    time, bias_y, 'b', time, (addedGyroBias-y_sigma)*180/pi, 'b-.', ...
    time, (addedGyroBias+y_sigma)*180/pi, 'b-.', 'LineWidth', 1, 'MarkerSize', markerSize);
set(gca, 'Position', plot2Pos, 'FontSize', 12, 'FontName', 'Times');

ylabel('y_b_i_a_s (deg/s)', 'FontSize', 12, 'FontName', 'Times');
set(gca, 'XLim', [time(1) time(end)], 'YLim', [addedGyroBias-plotLims(2) addedGyroBias+plotLims(2)]*180/pi);

subplot(3,1,3);
plot([time(1) time(end)], [addedGyroBias addedGyroBias]*180/pi, 'k--', ...
    time, bias_z, 'b', time, (addedGyroBias-z_sigma)*180/pi, 'b-.', ...
    time, (addedGyroBias+z_sigma)*180/pi, 'b-.', 'LineWidth', 1, 'MarkerSize', markerSize);
set(gca, 'Position', plot3Pos, 'FontSize', 12, 'FontName', 'Times');

xlabel('time (s)', 'FontSize', 12, 'FontName', 'Times');
ylabel('z_b_i_a_s (deg/s)', 'FontSize', 12, 'FontName', 'Times');
set(gca, 'XLim', [time(1) time(end)], 'YLim', [addedGyroBias-plotLims(3) addedGyroBias+plotLims(3)]*180/pi);

set(gcf, 'Position', figPos + [(figIdx-1)*100 0 0 0]);
set(gcf, 'PaperUnits', 'centimeters', 'PaperType', 'A4', 'PaperPosition', [0.63 0.63 19.72 28.41]);
set(gcf,'PaperPositionMode','auto');
saveas(gcf, ['figures/test_fig_' int2str(figIdx) '.fig']);
print('-depsc','-tiff','-r300',['figures/test_fig_' int2str(figIdx) '.eps'])


classdef chebtech1 < chebtech
%CHEBTECH1   Approximate smooth functions on [-1,1] with Chebyshev interpolants.
%
%   Class for approximating smooth functions on the interval [-1,1]
%   using function values at 1st-kind Chebyshev points and coefficients of the
%   corresponding 1st-kind Chebyshev series expansion.
%
% Constructor inputs:
%   CHEBTECH1(OP) constructs a CHEBTECH1 object from the function handle OP. OP
%   should be vectorized (i.e., accept a vector input) and ouput a vector of
%   the same length. CHEBTECH1 objects allow for array-valued construction
%   (i.e., of array-valued function), in which case OP should accept a vector
%   of length N and return a matrix of size NxM, where M is number of columns
%   of the multi -valued function.
%
%   CHEBTECH1(OP, DATA) constructs a CHEBTECH2 using the additional data
%   supplied in the DATA structure.  Fields currently recognized are:
%     DATA.VSCALE    (Default:  0)
%     DATA.HSCALE    (Default:  1)
%         The constructor builds a CHEBTECH1 with 'happiness' (see
%         HAPPINESSCHECK.m) relative to the maximum of the given vertical scale
%         DATA.VSCALE and the (column-wise) infinity norm of the sampled
%         function values of OP, and the fixed horizontal scale DATA.HSCALE.
%   If any fields in DATA are empty or not supplied, or if DATA itself is empty
%   or not supplied, appropriate default values are set.
%
%   CHEBTECH1(OP, DATA, PREF) overrides the default behavior with that given by
%   the preference structure PREF.
%
%   CHEBTECH1(VALUES, ...) returns a CHEBTECH1 object which interpolates the
%   values in the columns of VALUES at 1st-kind Chebyshev points and
%   CHEBTECH1({VALUES, COEFFS}, ... ) uses the Chebyshev coefficients passed in
%   COEFFS rather than computing them. If COEFFS are passed, the resulting
%   CHEBTECH1 is always deemed 'happy'.
%
% Examples: % Basic construction: f = chebtech1(@(x) sin(x))
%
%   % Construction with preferences:
%   p.sampleTest = 0; % See CHEBTECH.TECHPREF for details
%   f = chebtech1(@(x) sin(x), [], [], p)
%
%   % Array-valued construction:
%   f = chebtech1(@(x) [sin(x), cos(x), exp(x)])
%
% See also CHEBTECH, CHEBTECH.TECHPREF, CHEBPTS, HAPPINESSCHECK, REFINE.

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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% CHEBTECH1 Class Description:
%
% The CHEBTECH1 class represents smooth functions on the interval [-1,1] using
% function values at 1st-kind Chebyshev points and coefficients of the
% corresponding 1st-kind Chebyshev series expansion.
%
% The constructor is supplied with a handle that evaluates a given function on
% an increasingly fine Chebyshev 1st-kind grid (see REFINE.m) until the
% representation is deemed 'happy' (see HAPPINESSCHECK). The resulting
% object can be used to evaluate and operate on the input function.
%
% More information can be found in the CHEBTECH class definition.
%
% Class diagram: [<<CHEBTECH>>] <-- [CHEBTECH1]
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %% CLASS CONSTRUCTOR:
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    methods ( Access = public, Static = false )
        function obj = chebtech1(op, data, pref)
            % Parse inputs.
            if ( (nargin == 0) || isempty(op) )
                % Return an empty CHEBTECH1 on null input:
                return
            end

            if ( (nargin < 2) || isempty(data) )
                    data = struct();
            end

            if ( (nargin < 3) || isempty(pref) )
                pref = chebtech.techPref();
            else
                pref = chebtech.techPref(pref);
            end

            data = chebtech.parseDataInputs(data, pref);

            % Force nonadaptive construction if PREF.FIXEDLENGTH is numeric and
            % we're not using contour integrals.
            if ( ~(isnumeric(op) || iscell(op)) && ...
                    ~isnan(pref.fixedLength) && ~pref.useTurbo )
                % Evaluate op on the Chebyshev grid of given size:
                op = feval(op, chebtech1.chebpts(pref.fixedLength));
            end

            % Actual construction takes place here:
            [obj, values] = populate(obj, op, data, pref);

            if ( isnumeric(op) || iscell(op) )
                % Set length of obj to PREF.FIXEDLENGTH (if it is non-trivial).
                if ( ~isnan(pref.fixedLength ) )
                    obj = prolong(obj, pref.fixedLength);
                end

                % No need to error check when constructing from discrete data.
                return
            elseif ( obj.ishappy )
                % Use contour integrals ("turbo" mode) if requested.
                if ( pref.useTurbo )
                    obj = constructorTurbo(obj, op, pref);
                end

                % No need to error check if we are happy:
                return
            end

            % Check for NaNs (if not happy):
            if ( any(isnan(obj.coeffs(:))) )
                % Here we throw an error if NaNs were encountered anywhere.
                error('CHEBFUN:CHEBTECH1:chebtech1:nanEval', ...
                    'Function returned NaN when evaluated.')
            end
        end
    end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %% STATIC METHODS:
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    methods ( Access = public, Static = true )

        % Aliasing:
        coeffs = alias(coeffs, m)

        % Angles of Chebyshev points. (i.e., acos(chebpts(n))
        t = angles(n);

        % Evaluate a Chebyshev interpolant using proper barycentric formula:
        out = bary(x, values)

        % Compute Chebyshev barycentric weights:
        w = barywts(n)

        % Compute Chebyshev points (x) and optionally quadrature (w)
        % and barycentric (v) weights.
        [x, w, v, t] = chebpts(n)

        % Tensor product grid of Chebyshev points in 1D, 2D or 3D:
        out = tensorGrid(N, dom)

        % Convert coefficients to values:
        values = coeffs2vals(coeffs)

        % Make a CHEBTECH1 (constructor shortcut):
        f = make(varargin);

        % Compute Chebyshev quadrature weights:
        w = quadwts(n)

        % Refinement function for CHEBTECH1 construction (evaluates OP on grid):
        [values, points, giveUp] = refine(op, values, pref)

        % Return the value-based discretization class which uses CHEBTECH1:
        function disc = returnValsDisc()
            disc = @chebcolloc1;
        end

        % Convert values to coefficients:
        coeffs = vals2coeffs(values)

    end

end


function result = calcMapTopkMapTopkPreTopkRecLabel(queryLabel, retrievalLabel, qB, rB, topk)
%% Function: calcMapTopkMapTopkPreTopkRecLabel:
%   calculate Map, Topk map, Topk precision, Topk recall for hamming ranking task.
% Input:
%   queryLabel: 0-1 label matrix (numQuery * numLabel) for query set.
%   retrievalLabel: 0-1 label matrix (numQuery * numLabel) for retrieval set.
%   qB: compressed binary code for query set.
%   rB: compressed binary code for retrieval set.
%   topk (optional): vector for different (non-zero and ascending) topk.
% Output:
%   result.map: map for whole retrieval set
%   result.topkMap: vector. topk-Map for different topk
%   result.topkPre: vector. topk-Precision for different topk
%   result.topkRec: vector. topk-Recall for different topk

flag = false;
if exist('topk', 'var')
    flag = true;
end

numQuery = size(qB, 1);
map = 0;
if flag
    nt = numel(topk);
    topkPre = zeros(1, nt);
    topkRec = zeros(1, nt);
    topkMap = zeros(1, nt);
end

for ii = 1: numQuery
    gnd = queryLabel(ii, :) * retrievalLabel' > 0;
    tsum = sum(gnd);
    if tsum == 0
        continue;
    end
    hamm = hammingDist(qB(ii, :), rB);
    [~, index] = sort(hamm);
    gnd = gnd(index);
    count = 1: tsum;
    tindex = find(gnd == 1);
    map = map + mean(count ./ tindex);
    if flag
        for jj = 1: nt
            tgnd = gnd(1: topk(jj));
            if sum(tgnd) == 0
                continue;
            end
            tcount = 1: sum(tgnd);
            tindex = find(tgnd == 1);
            topkMap(jj) = topkMap(jj) + mean(tcount ./ tindex);
            topkPre(jj) = topkPre(jj) + sum(tgnd) / topk(jj);
            topkRec(jj) = topkRec(jj) + sum(tgnd) / tsum;
        end
    end
end

result.map = map / numQuery;
if flag
    result.topkMap = topkMap / numQuery;
    result.topkPre = topkPre / numQuery;
    result.topkRec = topkRec / numQuery;
end
end


function obj = addJumpConstraint(obj, edge, src, tar, bounds, varargin)
    % Add jump constraints between the edge and neighboring two nodes
    % (source and target)
    %
    % These constraints include the continuity of the states/time
    % variables, as well as system-specific discrete map constraint of the
    % guard dynamics, and user-specific constraints.
    %
    % Parameters:
    % src: the source node NLP @type TrajectoryOptimization
    % edge: the edge NLP @type TrajectoryOptimization
    % tar: the target node NLP @type TrajectoryOptimization
    % bounds: the boundary values @type struct
    % varargin: extra argument @type varargin

    %% continuity of time
    t_s = SymVariable('ts',[2,1]);
    t_n = SymVariable('tn',[2,1]);
    t_cont = SymFunction('tContDomain',flatten(t_s(2)-t_n(1)),{t_s,t_n});

    % create a NlpFunction for 'edge' NLP, but use the NlpVariables from
    % 'src' and 'tar' NLPs.
    if src.Options.DistributeTimeVariable
        src_time_node = src.NumNode;
    else
        src_time_node = 1;
    end
    t_cstr = NlpFunction('Name','tContDomain',...
        'Dimension',1,...
        'lb', 0,...
        'ub', 0,...
        'Type','Linear',...
        'SymFun',t_cont,...
        'DepVariables',[src.OptVarTable.T(src_time_node);tar.OptVarTable.T(1)]);
    edge.addConstraint('tContDomain','first',t_cstr);

    %% state continuity (src <-> edge)
    x_s = src.Plant.States.x;
    x_e = SymVariable('xp',size(x_s));
    x_cont_src = SymFunction(['xMinusCont_' edge.Name],x_s-x_e,{x_s,x_e});
    x_src_cstr = NlpFunction('Name',['xMinusCont_' edge.Name],...
        'Dimension',src.Plant.numState,...
        'lb', 0,...
        'ub', 0,...
        'Type','Linear',...
        'SymFun',x_cont_src,...
        'DepVariables',[src.OptVarTable.x(end);edge.OptVarTable.x(1)]);
    edge.addConstraint('xMinusCont','first',x_src_cstr);

    %% state continuity (edge <-> tar)
    x_t = tar.Plant.States.x;
    x_e = edge.Plant.States.xn;
    x_cont_tar = SymFunction(['xPlusCont_' edge.Name],x_e-x_t,{x_e,x_t});
    x_tar_cstr = NlpFunction('Name',['xPlusCont_' edge.Name],...
        'Dimension',tar.Plant.numState,...
        'lb', 0,...
        'ub', 0,...
        'Type','Linear',...
        'SymFun',x_cont_tar,...
        'DepVariables',[edge.OptVarTable.xn(1);tar.OptVarTable.x(1)]);
    edge.addConstraint('xPlusCont','first',x_tar_cstr);

    if strcmp(edge.Plant.Type,'SecondOrder')
        %% state derivative continuity (src <-> edge)
        dx_s = src.Plant.States.dx;
        dx_e = SymVariable('xp',size(x_s));
        dx_cont_src = SymFunction(['dxMinusCont_' edge.Name],dx_s-dx_e,{dx_s,dx_e});
        dx_src_cstr = NlpFunction('Name',['dxMinusCont_' edge.Name],...
            'Dimension',src.Plant.numState,...
            'lb', 0,...
            'ub', 0,...
            'Type','Linear',...
            'SymFun',dx_cont_src,...
            'DepVariables',[src.OptVarTable.dx(end);edge.OptVarTable.dx(1)]);
        edge.addConstraint('dxMinusCont','first',dx_src_cstr);

        %% state derivative continuity (edge <-> tar)
        dx_t = tar.Plant.States.dx;
        dx_e = edge.Plant.States.dxn;
        dx_cont_tar = SymFunction(['dxPlusCont_' edge.Name],dx_e-dx_t,{dx_e,dx_t});
        dx_tar_cstr = NlpFunction('Name',['dxPlusCont_' edge.Name],...
            'Dimension',tar.Plant.numState,...
            'lb', 0,...
            'ub', 0,...
            'Type','Linear',...
            'SymFun',dx_cont_tar,...
            'DepVariables',[edge.OptVarTable.dxn(1);tar.OptVarTable.dx(1)]);
        edge.addConstraint('dxPlusCont','first',dx_tar_cstr);
    end

    %% the event function constraint for the source domain
    event_list = fieldnames(src.Plant.EventFuncs);  % all events
    if ~isempty(event_list)
        % find the index of the event associated with the edge
        event_idx = str_index(edge.Plant.EventName,event_list);
        % extract the event function object using the index
        event_obj = src.Plant.EventFuncs.(event_list{event_idx});
        % impose the NLP constraints (unilateral constraints)
        event_obj.imposeNLPConstraint(src);
        % update the upper bound at the last node to be zero (to ensure equality)
        event_cstr_name = event_obj.ConstrExpr.Name;
        updateConstrProp(src,event_cstr_name,'last','ub',0);
    end

    %% call the system constraint callback method for the discrete dyamics
    plant = edge.Plant;
    plant.UserNlpConstraint(edge, src, tar, bounds, varargin{:});
end


function [X, fX, i] = minimize_lbfgsb(X, f, length, varargin)

% Minimize a differentiable multivariate function using quasi Newton.
%
% Usage: [X, fX, i] = minimize_lbfgsb(X, f, length, P1, P2, P3, ... )
%
% X       initial guess; may be of any type, including struct and cell array
% f       the name or pointer to the function to be minimized. The function
%         f must return two arguments, the value of the function, and it's
%         partial derivatives wrt the elements of X. The partial derivative
%         must have the same type as X.
% length  length of the run; if it is positive, it gives the maximum number of
%         line searches, if negative its absolute gives the maximum allowed
%         number of function evaluations. Optionally, length can have a second
%         component, which will indicate the reduction in function value to be
%         expected in the first line-search (defaults to 1.0).
% P1, P2  ... parameters are passed to the function f.
%
% X       the returned solution
% fX      vector of function values indicating progress made
% i       number of iterations (line searches or function evaluations,
%         depending on the sign of "length") used at termination.
%
% The function returns when either its length is up, or if no further progress
% can be made (ie, we are at a (local) minimum, or so close that due to
% numerical problems, we cannot get any closer). NOTE: If the function
% terminates within a few iterations, it could be an indication that the
% function values and derivatives are not consistent (ie, there may be a bug in
% the implementation of your "f" function).
%
% Copyright (C) 2010 by Hannes Nickisch, 2010-02-05

% global variables serve as communication interface between calls
global minimize_lbfgsb_iteration_number
global minimize_lbfgsb_objective
global minimize_lbfgsb_gradient
global minimize_lbfgsb_X

% init global variables
minimize_lbfgsb_iteration_number = 0;
minimize_lbfgsb_objective = Inf;
minimize_lbfgsb_gradient = 0*unwrap2vec(X);
minimize_lbfgsb_X = X;

X0 = X;
lb = -Inf*ones(size(unwrap2vec(X0)));
ub =  Inf*ones(size(unwrap2vec(X0)));
maxiter = abs(length); % max number of iterations

% no callback routine used so far
% m is the number of saved vectors used to estimate the Hessian
% factr is the precision 1e-12
X = lbfgsb( unwrap2vec(X0), lb, ub, 'minimize_lbfgsb_objfun', ...
                                'minimize_lbfgsb_gradfun', ...
         {f,X0,varargin{:}}, [], ...
         'maxiter',maxiter, 'm',4, 'factr',1e-12, 'pgtol',1e-5);
i = minimize_lbfgsb_iteration_number;
fX = minimize_lbfgsb_objective;
X = rewrap(X0,X);

% clear global variables
clear minimize_lbfgsb_iteration_number
clear minimize_lbfgsb_objective
clear minimize_lbfgsb_gradient
clear minimize_lbfgsb_X

% Extract the numerical values from "s" into the column vector "v". The
% variable "s" can be of any type, including struct and cell array.
% Non-numerical elements are ignored. See also the reverse rewrap.m.
function v = unwrap2vec(s)
  v = [];
  if isnumeric(s)
    v = s(:);                       % numeric values are recast to column vector
  elseif isstruct(s)
    v = unwrap2vec(struct2cell(orderfields(s)));% alphabetize, conv to cell, recurse
  elseif iscell(s)
    for i = 1:numel(s)            % cell array elements are handled sequentially
      v = [v; unwrap2vec(s{i})];
    end
  end                                                  % other types are ignored

% Map the numerical elements in the vector "v" onto the variables "s" which can
% be of any type. The number of numerical elements must match; on exit "v"
% should be empty. Non-numerical entries are just copied. See also unwrap2vec.m.
function [s v] = rewrap(s, v)
  if isnumeric(s)
    if numel(v) < numel(s)
      error('The vector for conversion contains too few elements')
    end
    s = reshape(v(1:numel(s)), size(s));           % numeric values are reshaped
    v = v(numel(s)+1:end);                       % remaining arguments passed on
  elseif isstruct(s)
    [s p] = orderfields(s); p(p) = 1:numel(p);     % alphabetize, store ordering
    [t v] = rewrap(struct2cell(s), v);                % convert to cell, recurse
    s = orderfields(cell2struct(t,fieldnames(s),1),p); % conv to struct, reorder
  elseif iscell(s)
    for i = 1:numel(s)            % cell array elements are handled sequentially
      [s{i} v] = rewrap(s{i}, v);
    end
  end                                            % other types are not processed


function gpcf = gpcf_constant(varargin)
%GPCF_CONSTANT  Create a constant covariance function
%
%  Description
%    GPCF = GPCF_CONSTANT('PARAM1',VALUE1,'PARAM2,VALUE2,...)
%    creates a squared exponential covariance function structure in
%    which the named parameters have the specified values. Any
%    unspecified parameters are set to default values.
%
%    GPCF = GPCF_CONSTANT(GPCF,'PARAM1',VALUE1,'PARAM2,VALUE2,...)
%    modify a covariance function structure with the named
%    parameters altered with the specified values.
%
%    Parameters for constant covariance function [default]
%      constSigma2       - magnitude (squared) [0.1]
%      constSigma2_prior - prior for constSigma2 [prior_logunif]
%
%    Note! If the prior is 'prior_fixed' then the parameter in
%    question is considered fixed and it is not handled in
%    optimization, grid integration, MCMC etc.
%
%  See also
%    GP_SET, GPCF_*, PRIOR_*, MEAN_*

% Copyright (c) 2007-2010 Jarno Vanhatalo
% Copyright (c) 2010      Jaakko Riihimaki, Aki Vehtari

% 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 = 'GPCF_CONSTANT';
  ip.addOptional('gpcf', [], @isstruct);
  ip.addParamValue('constSigma2',0.1, @(x) isscalar(x) && x>0);
  ip.addParamValue('constSigma2_prior',prior_logunif, @(x) isstruct(x) || isempty(x));
  ip.parse(varargin{:});
  gpcf=ip.Results.gpcf;

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

  % Initialize parameter
  if init || ~ismember('constSigma2',ip.UsingDefaults)
    gpcf.constSigma2=ip.Results.constSigma2;
  end

  % Initialize prior structure
  if init
    gpcf.p=[];
  end
  if init || ~ismember('constSigma2_prior',ip.UsingDefaults)
    gpcf.p.constSigma2=ip.Results.constSigma2_prior;
  end

  if init
    % Set the function handles to the subfunctions
    gpcf.fh.pak = @gpcf_constant_pak;
    gpcf.fh.unpak = @gpcf_constant_unpak;
    gpcf.fh.lp = @gpcf_constant_lp;
    gpcf.fh.lpg = @gpcf_constant_lpg;
    gpcf.fh.cfg = @gpcf_constant_cfg;
    gpcf.fh.ginput = @gpcf_constant_ginput;
    gpcf.fh.cov = @gpcf_constant_cov;
    gpcf.fh.trcov  = @gpcf_constant_trcov;
    gpcf.fh.trvar  = @gpcf_constant_trvar;
    gpcf.fh.recappend = @gpcf_constant_recappend;
  end

end

function [w, s] = gpcf_constant_pak(gpcf, w)
%GPCF_CONSTANT_PAK  Combine GP covariance function parameters into
%                   one vector.
%
%  Description
%    W = GPCF_CONSTANT_PAK(GPCF) takes a covariance function
%    structure GPCF and combines the covariance function
%    parameters and their hyperparameters into a single row
%    vector W. This is a mandatory subfunction used for example
%    in energy and gradient computations.
%
%       w = [ log(gpcf.constSigma2)
%             (hyperparameters of gpcf.constSigma2)]'
%
%  See also
%    GPCF_CONSTANT_UNPAK

  w = []; s = {};

  if ~isempty(gpcf.p.constSigma2)
    w = log(gpcf.constSigma2);
    s = [s 'log(constant.constSigma2)'];
    % Hyperparameters of constSigma2
    [wh sh] = gpcf.p.constSigma2.fh.pak(gpcf.p.constSigma2);
    w = [w wh];
    s = [s sh];
  end
end

function [gpcf, w] = gpcf_constant_unpak(gpcf, w)
%GPCF_CONSTANT_UNPAK  Sets the covariance function parameters
%                     into the structure
%
%  Description
%    [GPCF, W] = GPCF_CONSTANT_UNPAK(GPCF, W) takes a covariance
%    function structure GPCF and a parameter vector W, and
%    returns a covariance function structure identical to the
%    input, except that the covariance parameters have been set
%    to the values in W. Deletes the values set to GPCF from W
%    and returns the modified W. This is a mandatory subfunction
%    used for example in energy and gradient computations.
%
%    Assignment is inverse of
%       w = [ log(gpcf.constSigma2)
%             (hyperparameters of gpcf.constSigma2)]'
%
%  See also
%   GPCF_CONSTANT_PAK

  gpp=gpcf.p;
  if ~isempty(gpp.constSigma2)
    gpcf.constSigma2 = exp(w(1));
    w = w(2:end);
    % Hyperparameters of magnSigma2
    [p, w] = gpcf.p.constSigma2.fh.unpak(gpcf.p.constSigma2, w);
    gpcf.p.constSigma2 = p;
  end
end

function lp = gpcf_constant_lp(gpcf)
%GPCF_CONSTANT_LP  Evaluate the log prior of covariance function parameters
%
%  Description
%    LP = GPCF_CONSTANT_LP(GPCF) takes a covariance function
%    structure GPCF and returns log(p(th)), where th collects the
%    parameters. This is a mandatory subfunction used for example
%    in energy computations.
%
%  See also
%    GPCF_CONSTANT_PAK, GPCF_CONSTANT_UNPAK, GPCF_CONSTANT_LPG, GP_E

% Evaluate the prior contribution to the error. The parameters that
% are sampled are from space W = log(w) where w is all the
% "real" samples. On the other hand errors are evaluated in the
% W-space so we need take into account also the Jacobian of
% transformation W -> w = exp(W). See Gelman et.al., 2004,
% Bayesian data Analysis, second edition, p24.

  lp = 0;
  gpp=gpcf.p;
  if ~isempty(gpp.constSigma2)
    lp = gpp.constSigma2.fh.lp(gpcf.constSigma2, gpp.constSigma2) +log(gpcf.constSigma2);
  end
end

function lpg = gpcf_constant_lpg(gpcf)
%GPCF_CONSTANT_LPG  Evaluate gradient of the log prior with respect
%               to the parameters.
%
%  Description
%    LPG = GPCF_CONSTANT_LPG(GPCF) takes a covariance function
%    structure GPCF and returns LPG = d log (p(th))/dth, where th
%    is the vector of parameters. This is a mandatory subfunction
%    used for example in gradient computations.
%
%  See also
%    GPCF_CONSTANT_PAK, GPCF_CONSTANT_UNPAK, GPCF_CONSTANT_LP, GP_G

  lpg = [];
  gpp=gpcf.p;

  if ~isempty(gpcf.p.constSigma2)
    lpgs = gpp.constSigma2.fh.lpg(gpcf.constSigma2, gpp.constSigma2);
    lpg = [lpg lpgs(1).*gpcf.constSigma2+1 lpgs(2:end)];
  end
end

function DKff = gpcf_constant_cfg(gpcf, x, x2, mask, i1)
%GPCF_CONSTANT_CFG  Evaluate gradient of covariance function
%                   with respect to the parameters
%
%  Description
%    DKff = GPCF_CONSTANT_CFG(GPCF, X) takes a
%    covariance function structure GPCF, a matrix X of input
%    vectors and returns DKff, the gradients of covariance matrix
%    Kff = k(X,X) with respect to th (cell array with matrix
%    elements). This is a mandatory subfunction used in gradient
%    computations.
%
%    DKff = GPCF_CONSTANT_CFG(GPCF, X, X2) takes a
%    covariance function structure GPCF, a matrix X of input
%    vectors and returns DKff, the gradients of covariance matrix
%    Kff = k(X,X2) with respect to th (cell array with matrix
%    elements). This subfunction is needed when using sparse
%    approximations (e.g. FIC).
%
%    DKff = GPCF_CONSTANT_CFG(GPCF, X, [], MASK)
%    takes a covariance function structure GPCF, a matrix X of
%    input vectors and returns DKff, the diagonal of gradients of
%    covariance matrix Kff = k(X,X2) with respect to th (cell
%    array with matrix elements). This subfunction is needed when
%    using sparse approximations (e.g. FIC).
%
%  See also
%    GPCF_CONSTANT_PAK, GPCF_CONSTANT_UNPAK, GPCF_CONSTANT_LP, GP_G

  [n, m] =size(x);

  DKff = {};

  if nargin==5
    % Use memory save option
    if i1==0
      % Return number of hyperparameters
      if ~isempty(gpcf.p.constSigma2)
        DKff=1;
      else
        DKff=0;
      end
      return
    end
  end

  % Evaluate: DKff{1} = d Kff / d constSigma2
  %           DKff{2} = d Kff / d coeffSigma2
  % NOTE! Here we have already taken into account that the parameters are transformed
  % through log() and thus dK/dlog(p) = p * dK/dp

  % evaluate the gradient for training covariance
  if nargin == 2 || (isempty(x2) && isempty(mask))

    if ~isempty(gpcf.p.constSigma2)
      DKff{1}=ones(n)*gpcf.constSigma2;
    end

    % Evaluate the gradient of non-symmetric covariance (e.g. K_fu)
  elseif nargin == 3 || isempty(mask)
    if size(x,2) ~= size(x2,2)
      error('gpcf_constant -> _ghyper: The number of columns in x and x2 has to be the same. ')
    end

    if ~isempty(gpcf.p.constSigma2)
      DKff{1}=ones([n size(x2,1)])*gpcf.constSigma2;
    end

    % Evaluate: DKff{1}    = d mask(Kff,I) / d constSigma2
    %           DKff{2...} = d mask(Kff,I) / d coeffSigma2
  elseif nargin == 4 || nargin == 5

    if ~isempty(gpcf.p.constSigma2)
      DKff{1}=ones(n,1)*gpcf.constSigma2; % d mask(Kff,I) / d constSigma2
    end
  end
  if nargin==5
    DKff=DKff{1};
  end

end

function DKff = gpcf_constant_ginput(gpcf, x, x2, i1)
%GPCF_CONSTANT_GINPUT  Evaluate gradient of covariance function with
%                      respect to x.
%
%  Description
%    DKff = GPCF_CONSTANT_GINPUT(GPCF, X) takes a covariance
%    function structure GPCF, a matrix X of input vectors and
%    returns DKff, the gradients of covariance matrix Kff =
%    k(X,X) with respect to X (cell array with matrix elements).
%    This subfunction is needed when computing gradients with
%    respect to inducing inputs in sparse approximations.
%
%    DKff = GPCF_CONSTANT_GINPUT(GPCF, X, X2) takes a covariance
%    function structure GPCF, a matrix X of input vectors and
%    returns DKff, the gradients of covariance matrix Kff =
%    k(X,X2) with respect to X (cell array with matrix elements).
%    This subfunction is needed when computing gradients with
%    respect to inducing inputs in sparse approximations.
%
%    DKff = GPCF_CONSTANT_GINPUT(GPCF, X, X2, i) takes a covariance
%    function structure GPCF, a matrix X of input vectors
%    and returns DKff, the gradients of covariance matrix Kff =
%    k(X,X2), or k(X,X) if X2 is empty, with respect to ith
%    covariate in X. This subfunction is needed when using
%    memory save option in gp_set.
%
%  See also
%    GPCF_CONSTANT_PAK, GPCF_CONSTANT_UNPAK, GPCF_CONSTANT_LP, GP_G

  [n, m] =size(x);
  if nargin==4
    % Use memory save option
    if i1==0
      % Return number of covariates
      if isfield(gpcf,'selectedVariables')
        DKff=length(gpcf.selectedVariables);
      else
        DKff=m;
      end
      return
    end
  end

  if nargin == 2 || isempty(x2)
    ii1 = 0;
    for i=1:m
      for j = 1:n
        ii1 = ii1 + 1;
        DKff{ii1} = zeros(n);
      end
    end

  elseif nargin == 3 || nargin == 4
    %K = feval(gpcf.fh.cov, gpcf, x, x2);

    ii1 = 0;
    for i=1:m
      for j = 1:n
        ii1 = ii1 + 1;
        DKff{ii1} = zeros(n, size(x2,1));
        gprior(ii1) = 0;
      end
    end
  end
  if nargin==5
    DKff=DKff{1};
  end
end

function C = gpcf_constant_cov(gpcf, x1, x2, varargin)
%GP_CONSTANT_COV  Evaluate covariance matrix between two input vectors
%
%  Description
%    C = GP_CONSTANT_COV(GP, TX, X) takes in covariance function
%    of a Gaussian process GP and two matrixes TX and X that
%    contain input vectors to GP. Returns covariance matrix C.
%    Every element ij of C contains covariance between inputs i
%    in TX and j in X. This is a mandatory subfunction used for
%    example in prediction and energy computations.
%
%  See also
%    GPCF_CONSTANT_TRCOV, GPCF_CONSTANT_TRVAR, GP_COV, GP_TRCOV

  if isempty(x2)
    x2=x1;
  end
  [n1,m1]=size(x1);
  [n2,m2]=size(x2);

  if m1~=m2
    error('the number of columns of X1 and X2 has to be same')
  end

  C = ones(n1,n2)*gpcf.constSigma2;
end

function C = gpcf_constant_trcov(gpcf, x)
%GP_CONSTANT_TRCOV  Evaluate training covariance matrix of inputs
%
%  Description
%    C = GP_CONSTANT_TRCOV(GP, TX) takes in covariance function
%    of a Gaussian process GP and matrix TX that contains
%    training input vectors. Returns covariance matrix C. Every
%    element ij of C contains covariance between inputs i and j
%    in TX. This is a mandatory subfunction used for example in
%    prediction and energy computations.
%
%  See also
%    GPCF_CONSTANT_COV, GPCF_CONSTANT_TRVAR, GP_COV, GP_TRCOV

  n =size(x,1);
  C = ones(n,n)*gpcf.constSigma2;

end

function C = gpcf_constant_trvar(gpcf, x)
%GP_CONSTANT_TRVAR  Evaluate training variance vector
%
%  Description
%    C = GP_CONSTANT_TRVAR(GPCF, TX) takes in covariance function
%    of a Gaussian process GPCF and matrix TX that contains
%    training inputs. Returns variance vector C. Every
%    element i of C contains variance of input i in TX. This is
%    a mandatory subfunction used for example in prediction and
%    energy computations.
%
%  See also
%    GPCF_CONSTANT_COV, GP_COV, GP_TRCOV

  n =size(x,1);
  C = ones(n,1)*gpcf.constSigma2;

end

function reccf = gpcf_constant_recappend(reccf, ri, gpcf)
%RECAPPEND Record append
%
%  Description
%    RECCF = GPCF_CONSTANT_RECAPPEND(RECCF, RI, GPCF) takes a
%    covariance function record structure RECCF, record index RI
%    and covariance function structure GPCF with the current MCMC
%    samples of the parameters. Returns RECCF which contains all
%    the old samples and the current samples from GPCF. This
%    subfunction is needed when using MCMC sampling (gp_mc).
%
%  See also
%    GP_MC and GP_MC -> RECAPPEND

  if nargin == 2
    % Initialize the record
    reccf.type = 'gpcf_constant';

    % Initialize parameters
    reccf.constSigma2 = [];

    % Set the function handles
    reccf.fh.pak = @gpcf_constant_pak;
    reccf.fh.unpak = @gpcf_constant_unpak;
    reccf.fh.lp = @gpcf_constant_lp;
    reccf.fh.lpg = @gpcf_constant_lpg;
    reccf.fh.cfg = @gpcf_constant_cfg;
    reccf.fh.cov = @gpcf_constant_cov;
    reccf.fh.trcov  = @gpcf_constant_trcov;
    reccf.fh.trvar  = @gpcf_constant_trvar;
    reccf.fh.recappend = @gpcf_constant_recappend;
    reccf.p=[];
    reccf.p.constSigma2=[];
    if ~isempty(ri.p.constSigma2)
      reccf.p.constSigma2 = ri.p.constSigma2;
    end

  else
    % Append to the record
    gpp = gpcf.p;

    % record constSigma2
    reccf.constSigma2(ri,:)=gpcf.constSigma2;
    if isfield(gpp,'constSigma2') && ~isempty(gpp.constSigma2)
      reccf.p.constSigma2 = gpp.constSigma2.fh.recappend(reccf.p.constSigma2, ri, gpcf.p.constSigma2);
    end
  end
end


function [result, M] = ft_warp_optim(input, target, method)

% FT_WARP_OPTIM determine intermediate positions using warping (deformation)
% the input cloud of points is warped to match the target.
% The strategy is to start with simpelest linear warp, followed by a more
% elaborate linear warp, which then is followed by the nonlinear warps up
% to the desired order.
%
% [result, M] = ft_warp_pnt(input, target, method)
%     input          contains the Nx3 measured 3D positions
%     target         contains the Nx3 template 3D positions
%     method         should be any of
%                     'rigidbody'
%                     'globalrescale'
%                     'traditional' (default)
%                     'nonlin1'
%                     'nonlin2'
%                     'nonlin3'
%                     'nonlin4'
%                     'nonlin5'
%
% The default warping method is a traditional linear warp with individual
% rescaling in each dimension. Optionally you can select a nonlinear warp
% of the 1st (affine) up to the 5th order.
%
% When available, this function will use the MATLAB optimization toolbox.
%
% See also FT_WARP_APPLY, FT_WARP_ERRROR

% Copyright (C) 2000-2013, Robert Oostenveld
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 2 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program; if not, write to the Free Software
% Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip 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.
%
%    FieldTrip 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 FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

% this can be used for printing detailled user feedback
fb = false;

if nargin<3
  method='traditional';
end

pos1 = input;
pos2 = target;

% The ft_warp_error function might be located in the private subdirectory of
% FieldTrip, i.e. only available to functions in the FieldTrip toolbox.
% The following line ensures that the function can also be found by the
% feval that is executed by the optimalization toolbox.
errorfun = str2func('ft_warp_error');

% determine whether the MATLAB Optimization toolbox is available and can be used
if ft_hastoolbox('optim')
  optimfun = @fminunc;
else
  optimfun = @fminsearch;
end

% set the options for the minimalisation routine
if isequal(optimfun,  @fminunc)
  options  = optimset('fminunc');
  options  = optimset(options, 'Display', 'off');
  options  = optimset(options, 'MaxIter', 1500);
  % options  = optimset(options, 'MaxFunEvals', '1000*numberOfVariables');
  options  = optimset(options, 'TolFun', 1e-4);
  options  = optimset(options, 'LargeScale', 'off');
elseif isequal(optimfun, @fminsearch)
  options  = optimset('fminsearch');
  options  = optimset(options, 'Display', 'off');
  options  = optimset(options, 'MaxIter', 4500);
else
  ft_warning('unknown optimization function "%s", using default parameters', func2str(optimfun));
end

if fb; fprintf('distance = %f\n', errorfun([0 0 0 0 0 0], pos1, pos2, 'rigidbody')); end

if isempty(method)
  ft_error('incorrect warping method specified');
end

% the warp is done in steps, starting simple and progressively getting more complex
level = find(strcmp(method, {
  'rigidbody'         % 1
  'globalrescale'     % 2
  'traditional'       % 3
  'nonlin1'           % 4
  'nonlin2'           % 5
  'nonlin3'           % 6
  'nonlin4'           % 7
  'nonlin5'           % 8
  }));

if isempty(level)
  ft_error('incorrect warping method specified');
end

if level>=1
  % do a rigid-body transformation (6 parameters)
  if fb; disp('rigidbody...'); end
  ri = [0 0 0 0 0 0];
  rf = optimfun(errorfun, ri, options, pos1, pos2, 'rigidbody');
  if fb; fprintf('distance = %f\n', errorfun(rf, pos1, pos2, 'rigidbody')); end
end

if level>=2
  % do a rigid-body + global rescaling transformation (7 parameters)
  if fb; disp('rigidbody + global rescaling...'); end
  gi = [rf 1];
  gf = optimfun(errorfun, gi, options, pos1, pos2, 'globalrescale');
  if fb; fprintf('distance = %f\n', errorfun(gf, pos1, pos2, 'globalrescale')); end
end

if level>=3
  % do a rigid-body + individual rescaling transformation (9 parameters)
  if fb; disp('rigidbody + individual rescaling...'); end
  ti = [gf gf(7) gf(7)];
  tf = optimfun(errorfun, ti, options, pos1, pos2, 'traditional');
  if fb; fprintf('distance = %f\n', errorfun(tf, pos1, pos2, 'traditional')); end
end

if level>=4
  % do a first order nonlinear transformation,
  if fb; disp('1st order nonlinear...'); end
  e1i = traditional(tf);
  e1i = [e1i(1:3,4) e1i(1:3,1:3)];  % reshuffle from homogenous into nonlinear
  e1f = optimfun(errorfun, e1i, options, pos1, pos2);
  if fb; fprintf('distance = %f\n', errorfun(e1f, pos1, pos2, 'nonlinear')); end
end

if level>=5
  % do a second order nonlinear transformation,
  if fb; disp('2nd order nonlinear...'); end
  e2i = [e1f zeros(3,6)];
  e2f = optimfun(errorfun, e2i, options, pos1, pos2);
  if fb; fprintf('distance = %f\n', errorfun(e2f, pos1, pos2, 'nonlinear')); end
end

if level>=6
  % do a third order nonlinear transformation,
  if fb; disp('3rd order nonlinear...'); end
  e3i = [e2f zeros(3,10)];
  e3f = optimfun(errorfun, e3i, options, pos1, pos2);
  if fb; fprintf('distance = %f\n', errorfun(e3f, pos1, pos2, 'nonlinear')); end
end

if level>=7
  % do a fourth order nonlinear transformation,
  if fb; disp('4th order nonlinear...'); end
  e4i = [e3f zeros(3,15)];
  e4f = optimfun(errorfun, e4i, options, pos1, pos2);
  if fb; fprintf('distance = %f\n', errorfun(e4f, pos1, pos2, 'nonlinear')); end
end

if level>=8
  % do a fifth order nonlinear transformation,
  if fb; disp('5th order nonlinear...'); end
  e5i = [e4f zeros(3,21)];
  e5f = optimfun(errorfun, e5i, options, pos1, pos2);
  if fb; fprintf('distance = %f\n', errorfun(e5f, pos1, pos2, 'nonlinear')); end
end

% return the estimated parameters of the highest level warp
% and compute the warped points
switch level
  case 1
    M = rf;
  case 2
    M = gf;
  case 3
    M = tf;
  case 4
    M = e1f;
  case 5
    M = e2f;
  case 6
    M = e3f;
  case 7
    M = e4f;
  case 8
    M = e5f;
end
result = ft_warp_apply(M, input, method);