%============================================================================
% 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'])


function out = getfisx(fis, arg1, arg2, arg3, arg4, arg5)
%GETFISX Get fuzzy inference system properties.
%
%   See getfis for syntax and explanation.
%
%   It is almost clone of MATALAB's getfis
%   with some modifications, so it's compatible
%   with extendent fuzzy rule structure.
%
%   Compare also lines 189-224 of this function
%   with lines 209-239 of the original getfis.

%   Per Konstantin A. Sidelnikov, 2009.

if isfield(fis, 'input')
    numInputs=length(fis.input);
else
    numInputs=0;
end

if isfield(fis, 'output')
    numOutputs=length(fis.output);
else
    numOutputs=0;
end

switch nargin
    case 1,
        % ===============================================
        % Handle generic inquiries related to the whole fis
        % ===============================================

        fprintf('      Name      = %s\n',fis.name);
        fprintf('      Type      = %s\n',fis.type);
        fprintf('      NumInputs = %s\n',num2str(numInputs));
        fprintf('      InLabels  = \n');
        if numInputs~=0,
            for i=1:length(fis.input)
                fprintf('            %s\n',fis.input(i).name);
            end
        end
        fprintf('      NumOutputs = %s\n',num2str(numOutputs));
        fprintf('      OutLabels = \n');
        if numOutputs~=0,
            for i=1:length(fis.output)
                fprintf('            %s\n',fis.output(i).name);
            end
        end
        fprintf('      NumRules = %s\n',num2str(length(fis.rule)));
        fprintf('      AndMethod = %s\n',fis.andMethod);
        fprintf('      OrMethod = %s\n',fis.orMethod);
        fprintf('      ImpMethod = %s\n',fis.impMethod);
        fprintf('      AggMethod = %s\n',fis.aggMethod);
        fprintf('      DefuzzMethod = %s\n',fis.defuzzMethod);
        out=fis.name;

    case 2,
        % ===============================================
        propName=lower(arg1);
        switch propName
            case 'name'
                out=fis.name;
            case 'type';
                out=fis.type;
            case 'numinputs'
                out=numInputs;
            case 'numoutputs'
                out=numOutputs;
            case 'numinputmfs'
                numInputMFs=[];
                for i=1:length(fis.input)
                    numInputMFs(i)=length(fis.input(i).mf);
                end
                out=numInputMFs;
            case 'numoutputmfs'
                numOutputMFs=[];
                for i=1:length(fis.output)
                    numOutputMFs(i)=length(fis.output(i).mf);
                end
                out=numOutputMFs;

            case 'numrules'
                out=length(fis.rule);
            case 'andmethod'
                out=fis.andMethod;
            case 'ormethod'
                out=fis.orMethod;
            case 'impmethod'
                out=fis.impMethod;
            case 'aggmethod'
                out=fis.aggMethod;
            case 'defuzzmethod'
                out=fis.defuzzMethod;

            case 'inlabels'
                out=[];
                for i=1:numInputs
                    out=strvcat(out, fis.input(i).name);
                end

            case 'outlabels'
                out=[];
                for i=1:numOutputs
                    out=strvcat(out, fis.output(i).name);
                end

            case 'inrange'
                for i=1:numInputs
                    out(i, 1:2)=fis.input(i).range;
                end

            case 'outrange'
                for i=1:numOutputs
                    out(i,1:2)=fis.output(i).range;
                end

            case 'inmfs'
                for i=1:numInputs
                    out(i)=length(fis.input(i).mf);
                end

            case 'outmfs'
                for i=1:numOutputs
                    out(i)=length(fis.output(i).mf);
                end

            case 'inmflabels'
                out=[];
                for i=1:numInputs
                    for j=1:length(fis.input(i).mf)
                        out=strvcat(out, fis.input(i).mf(j).name);
                    end
                end

            case 'outmflabels'
                out=[];
                for i=1:numOutputs
                    for j=1:length(fis.output(i).mf)
                        out=strvcat(out, fis.output(i).mf(j).name);
                    end
                end

            case 'inmftypes'
                out=[];
                for i=1:numInputs
                    for j=1:length(fis.input(i).mf)
                        out=strvcat(out, fis.input(i).mf(j).type);
                    end
                end

            case 'outmftypes'
                out=[];
                for i=1:numOutputs
                    for j=1:length(fis.output(i).mf)
                        out=strvcat(out, fis.output(i).mf(j).type);
                    end
                end

            case 'inmfparams'
                numInputMFs=[];
                for i=1:length(fis.input)
                    numInputMFs(i)=length(fis.input(i).mf);
                end
                totalInputMFs=sum(numInputMFs);
                k=1;
                out=zeros(totalInputMFs, 4);
                for i=1:numInputs
                    for j=1:length(fis.input(i).mf)
                        temp=fis.input(i).mf(j).params;
                        out(k,1:length(temp))=temp;
                        k=k+1;
                    end
                end

            case 'outmfparams'
                numOutputMFs=[];
                for i=1:length(fis.output)
                    numOutputMFs(i)=length(fis.output(i).mf);
                end
                totalOutputMFs=sum(numOutputMFs);
                k=1;
                out=zeros(totalOutputMFs, 4);
                for i=1:numOutputs
                    for j=1:length(fis.output(i).mf)
                        temp=fis.output(i).mf(j).params;
                        out(k, 1:length(temp))=temp;
                        k=k+1;
                    end
                end

            case 'rulelist'
                out = {};

                if ~isempty(fis.rule)
                    ifempty = @(x) isempty(x);

                    in_rules = {fis.rule.antecedent}';
                    i = find(cellfun(ifempty, in_rules), 1, 'first');
                    if ~isempty(i)
                        error('FuzzyLogic:ruleError', ...
                            'antecedent of rule %d is empty.', i);
                    end

                    out_rules = {fis.rule.consequent}';
                    i = find(cellfun(ifempty, out_rules), 1, 'first');
                    if ~isempty(i)
                        error('FuzzyLogic:ruleError', ...
                            'consequent of rule %d is empty.', i);
                    end

                    weights = {fis.rule.weight}';
                    i = find(cellfun(ifempty, weights), 1, 'first');
                    if ~isempty(i)
                        error('FuzzyLogic:ruleError', ...
                            'weight of rule %d is empty.', i);
                    end

                    connections = {fis.rule.connection}';
                    i = find(cellfun(ifempty, connections), 1, 'first');
                    if ~isempty(i)
                        error('FuzzyLogic:ruleError', ...
                            'connection of rule %d is empty.', i);
                    end

                    out = [in_rules, out_rules, weights, connections];
                end

            case 'inputs'
                fprintf('      Name =       %s\n',fis.name);
                out.Name = fis.name;
                fprintf('      NumInputs =  %s\n',num2str(numInputs));
                out.NumInputs = numInputs;
                fprintf('      InLabels  = \n');
                if numInputs~=0,
                    for i=1:length(fis.input)
                        fprintf('            %s\n',fis.input(i).name);
                        out = setfield(out, ['input' num2str(i)], fis.input(i).name);
                    end
                end

            case 'outputs'
                fprintf('      Name =       %s\n',fis.name);
                out.Name = fis.name;
                fprintf('      NumOutputs = %s\n',num2str(numOutputs));
                out.NumOutputs = numOutputs;
                fprintf('      OutLabels = \n');
                if numOutputs~=0,
                    for i=1:length(fis.output)
                        fprintf('            %s\n',fis.output(i).name);
                        out = setfield(out, ['output' num2str(i)], fis.output(i).name);
                    end
                end

            otherwise
                error('FuzzyLogic:FISPropertyError', ...
                    sprintf('There is no FIS system property called ''%s''', propName));
        end

    case 3,
        % ===============================================
        % Handle generic inquiries related to VARIABLES
        % ===============================================
        if strcmp(arg1,'input') | strcmp(arg1,'output'),
            varType=lower(arg1);
            varIndex=arg2;

            numMFs=getfis(fis,varType,varIndex,'NumMFs');
            out.Name = getfis(fis,varType,varIndex,'Name');
            fprintf('      Name =     %s\n',out.Name);
            fprintf('      NumMFs =   %s\n',num2str(numMFs));
            out.NumMFs = numMFs;
            fprintf('      MFLabels = \n');
            if numMFs~=0,
                mfLabels=getfis(fis,varType,varIndex,'MFLabels');
                for n=1:numMFs,
                    fprintf('            %s\n',mfLabels(n,:));
                    out = setfield(out, ['mf' num2str(n)], deblank(mfLabels(n,:)));
                end
            end
            range=getfis(fis,varType,varIndex,'Range');
            fprintf('      Range =    %s\n',mat2str(range));
            out.range = range;

        end

    case 4,
        % ===============================================
        % Handle specific inquiries related to VARIABLES
        % ===============================================
        varType=lower(arg1);
        varIndex=arg2;
        varProp=lower(arg3);
        switch varType
            case 'input',
                if varIndex>numInputs,
                    error('FuzzyLogic:parameterMismatch', ...
                        sprintf('%i is not a valid input index.', varIndex));
                end

                switch varProp
                    case 'name'
                        out=fis.input(varIndex).name;
                    case 'range'
                        out=fis.input(varIndex).range;
                    case 'nummfs'
                        out=length(fis.input(varIndex).mf);
                    case 'mflabels'
                        numMFs=length(fis.input(varIndex).mf);
                        MFList=[];
                        for n=1:numMFs,
                            MFList=strvcat(MFList,fis.input(varIndex).mf(n).name);
                        end
                        out=MFList;
                    otherwise
                        error('FuzzyLogic:FISVariablePropertiesError', ...
                            sprintf(['Invalid variable properties : ''%s'' \n' ...
                            'Valid entries are: \n'...
                            '\tname \n' ...
                            '\trange \n' ...
                            '\tnummfs \n'...
                            '\tmflabels '], varProp));
                end

            case 'output',
                if varIndex>numOutputs,
                    error('FuzzyLogic:parameterMismatch', ...
                        sprintf('%i is not a valid output index.', varIndex));
                end

                switch varProp
                    case 'name'
                        out=fis.output(varIndex).name;
                    case 'range',
                        out=fis.output(varIndex).range;
                    case 'nummfs',
                        out=length(fis.output(varIndex).mf);
                    case 'mflabels',
                        numMFs=length(fis.output(varIndex).mf);
                        MFList=[];
                        for n=1:numMFs,
                            MFList=strvcat(MFList,fis.output(varIndex).mf(n).name);
                        end
                        out=MFList;
                    otherwise
                        error('FuzzyLogic:FISVariablePropertiesError', ...
                            sprintf(['Invalid variable property : ''%s'' \n' ...
                            'Valid properties are: \n'...
                            '\tname \n' ...
                            '\trange \n' ...
                            '\tnummfs \n'...
                            '\tmflabels '], varProp));
                end

            otherwise
                        error('FuzzyLogic:FISVariableError', ...
                            'Variable type must be either "input" or "output"');

        end

    case 5,
        % ===============================================
        % Handle generic inquiries related to MEMBERSHIP FUNCTIONS
        % ===============================================
        if strcmp(arg1,'input') | strcmp(arg1,'output'),
            varType=lower(arg1);
            varIndex=arg2;
            MFIndex=arg4;

            MFLabels=getfis(fis,varType,varIndex,'MFLabels');
            out.Name = getfis(fis,varType,varIndex,'MF',MFIndex,'Name');
            fprintf('      Name = %s\n',out.Name);
            out.Type = getfis(fis,varType,varIndex,'MF',MFIndex,'Type');
            fprintf('      Type = %s\n',out.Type);
            params=getfis(fis,varType,varIndex,'MF',MFIndex,'Params');
            out.params = params;
            fprintf('      Params = %s\n',mat2str(params))
        end

    case 6,
        % ===============================================
        % Handle specific inquiries related to MEMBERSHIP FUNCTIONS
        % ===============================================
        varType=lower(arg1);
        varIndex=arg2;
        MFIndex=arg4;
        MFProp=lower(arg5);

        switch varType
            case 'input'
                if varIndex>numInputs,
                    errStr=['There are only ',int2str(length(fis.input)), ...
                        ' input variables'];
                    error('FuzzyLogic:FISVariablesError', ...
                        errStr)
                end

                if MFIndex>length(fis.input(varIndex).mf),
                    errStr=['There are only ',int2str(length(fis.input(varIndex).mf)), ...
                        ' MFs associated with that variable'];
                    error('FuzzyLogic:FISVariablesError', ...
                        errStr)
                end

                switch MFProp
                    case 'name'
                        out=fis.input(varIndex).mf(MFIndex).name;
                    case 'type'
                        out=fis.input(varIndex).mf(MFIndex).type;
                    case 'params'
                        out=fis.input(varIndex).mf(MFIndex).params;
                    otherwise
                    error('FuzzyLogic:InvalidMembershipFunctionPropertyError', ...
                        sprintf(['Invalid Membership Function property : ''%s'' \n' ...
                            'Valid properties are : \n' ...
                            '\tname \n' ...
                            '\ttype \n' ...
                            '\tparams \n'], MFProp));
                end

            case 'output'
                if varIndex>numOutputs,
                    errStr=['There are only ',int2str(length(fis.output)), ...
                        ' output variables'];
                    error('FuzzyLogic:InvalidMembershipFunctionPropertyError', errStr)
                end

                if MFIndex>length(fis.output(varIndex).mf),
                    errStr=['There are only ',int2str(length(fis.output(varIndex).mf)), ...
                        ' MFs associated with that variable'];
                    error('FuzzyLogic:InvalidMembershipFunctionPropertyError', errStr)
                end

                switch MFProp
                    case 'name'
                        out=fis.output(varIndex).mf(MFIndex).name;
                    case 'type'
                        out=fis.output(varIndex).mf(MFIndex).type;
                    case 'params'
                        out=fis.output(varIndex).mf(MFIndex).params;
                    otherwise
                        error('FuzzyLogic:InvalidMembershipFunctionPropertyError', ...
                            sprintf(['Invalid Membership Function property : ''%s'' \n' ...
                            'Valid properties are : \n' ...
                            '\tname \n' ...
                            '\ttype \n' ...
                            '\tparams \n'], MFProp));
                end

            otherwise
                error('FuzzyLogic:FISVariableError', ...
                    'Variable type must be either "input" or "output"');

        end

end


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 rule = subsrule(obj, name)
    % Return the string subsititution rule for the symbolic variable vector
    %
    % Return values:
    % rule: the string subsititution rules @type SymExpression

    assert(isempty(regexp(name, '\W', 'once')),...
        'SymExpression:invalidSymbol', ...
        'Invalid symbol name, can NOT contain special characters.');

    assert(isempty(regexp(name, '_', 'once')),...
        'SymExpression:invalidSymbol', ...
        'Invalid symbol name, can NOT contain ''_''.');

    if isscalar(obj)
        str = eval_math(['{' obj.s, '->HoldForm@Global`', name,'[[0]]}'],false);
        rule = SymExpression(str);
    elseif isvectorform(obj)
        siz = numel(obj);
        fstr= obj.s;
        str = eval_math(['((' fstr '[[#+1]]-> HoldForm@Global`', name '[[#]]&)/@(Range[', num2str(siz),']-1))']);
        rule = SymExpression(str,false);
    elseif ismatrix(obj)
        siz = numel(obj);
        fsym= tovector(obj);
        fstr = fsym.s;
        str = eval_math(['((' fstr '[[#+1]]-> HoldForm@Global`', name '[[#]]&)/@(Range[', num2str(siz),']-1))']);
        rule = SymExpression(str,false);
    else
        error('Unsupported format.');
    end

end


classdef TestPointPolygonTest
    %TestPointPolygonTest

    methods (Static)
        function test_contour_matrix
            cntr = [0 0; 1 0; 2 2; 3 3; 3 4];
            b = cv.pointPolygonTest(cntr, [2.3 2.4]);
            validateattributes(b, {'double'}, {'scalar'});

            % purely-integer points (both contour and query)
            b = cv.pointPolygonTest(int32(cntr), [2 2]);
            validateattributes(b, {'double'}, {'scalar'});
        end

        function test_contour_cellarray
            cntr = {[0 0], [1 0], [2 2], [3 3], [3 4]};
            b = cv.pointPolygonTest(cntr, [2.3 2.4]);
            validateattributes(b, {'double'}, {'scalar'});
        end

        function test_measure_dist
            cntr = [0 0; 1 0; 2 2; 3 3; 3 4];

            b = cv.pointPolygonTest(cntr, [2.3,2.4], 'MeasureDist',true);
            validateattributes(b, {'double'}, {'scalar', 'real'});

            b = cv.pointPolygonTest(cntr, [2.3,2.4], 'MeasureDist',false);
            validateattributes(b, {'double'}, {'scalar', 'integer'});
            assert(b == 0 || b == 1 || b == -1);
        end

        function test_error_argnum
            try
                cv.pointPolygonTest();
                throw('UnitTest:Fail');
            catch e
                assert(strcmp(e.identifier,'mexopencv:error'));
            end
        end
    end

end


% {make_libsvm} compiles Libsvm and the mex interface for Libsvm (C++).
%
%      make_libsvm
%
% Author: Stefano Melacci (2009)
%         mela@dii.unisi.it

delete *.obj
delete *.o
mex -c svm.cpp
if exist('svm.obj','file')>0
    mex mexGramSVMTrain.cpp svm.obj
elseif exist('svm.o','file')>0
    mex mexGramSVMTrain.cpp svm.o
end


clear all;
% path to db files
addpath('/media/carsen/DATA1/2P/dbfiles/')
compile_dbs;

% path to saved F files (and root directory)
ops0.RootDir                = '//zserver.cortexlab.net/Data/Subjects/';
ops0.ResultsSavePath        = '/media/carsen/DATA1/2P/F/';

% choose experiment
iexp = 31;
db = db0(iexp);

% build ops
ops = build_ops3(db, ops0);
root = ops.ResultsSavePath;

% load mean image and stats for each plane in recording
stat = [];
xL = [];
yL = [];
clear mimg;
for iplane = 1:ops.nplanes
    fname  = sprintf('F_%s_%s_plane%d.mat', db.mouse_name, db.date, iplane);
    dat = load(fullfile(root, fname));

    mimg{iplane} = dat.ops.mimg1(dat.ops.yrange, dat.ops.xrange);

    stat = cat(2, stat, dat.stat);
    yL(iplane) = numel(dat.ops.yrange);
    xL(iplane) = numel(dat.ops.xrange);
end

% compute outlines of cells
clear img;
ij = 1;
clf;
for iplane = 1:ops.nplanes
    mimg{iplane} = mimg{iplane} - min(mimg{iplane}(:));
    mimg{iplane} = mimg{iplane} / max(mimg{iplane}(:));

    % background of mean image
    img{iplane} = repmat(mimg{iplane}*2,1,1,3);
    img{iplane} = rgb2hsv(img{iplane});
    img{iplane} = reshape(img{iplane},[],3);

    colormap('gray');
    hold all;
    while stat(ij).iplane == iplane
        if stat(ij).iscell
            % find pixels that are exterior to the cell
            idist  = sqrt(bsxfun(@minus, stat(ij).xpix', stat(ij).xpix).^2 + ...
                bsxfun(@minus, stat(ij).ypix', stat(ij).ypix).^2);
            idist  = idist - diag(NaN*diag(idist));
            extpix = sum(idist <= sqrt(2)) <= 6;
            xext = stat(ij).xpix(extpix);
            yext = stat(ij).ypix(extpix);

            % color the exterior pixels
            ipix = sub2ind([yL(iplane) xL(iplane)], yext, xext);
            img{iplane}(ipix, 1) = rand;
            img{iplane}(ipix, 2) = 1;
            img{iplane}(ipix, 3) = 1;

        end

        ij = ij+1;

        if ij > numel(stat)
            break;
        end
    end

end

%%
clf;
set(gcf,'color','w');

% plot of one plane with circled ROIs
iplane = 5;

imgj = img{iplane};
imagesc(hsv2rgb(reshape(imgj,yL(iplane),xL(iplane),3)))
axis off;
axis square;


function [p,feasible,vol_reduction,seen_x] = propagate_bounds_lp(p,upper,lower,lpsolver,xmin);
LU = [p.lb p.ub];
seen_x = {};
if ~p.options.bmibnb.lpreduce | ((size(p.lpcuts,1)==0) & (any(p.lb(p.linears)<-1e8) & any(p.ub(p.linears)>1e8)))
    vol_reduction = 1;
    p.feasible = 1;
    p.lb(p.integer_variables) = ceil(p.lb(p.integer_variables)-1e-7);
    p.ub(p.integer_variables) = floor(p.ub(p.integer_variables)+1e-7);
    p.lb(p.binary_variables) = ceil(p.lb(p.binary_variables)-1e-7);
    p.ub(p.binary_variables) = floor(p.ub(p.binary_variables)+1e-7);
else
    try
        [p,p.feasible,seen_x] =  boxreduce(p,upper,lower,lpsolver,p.options,xmin);
    catch
        % LINPROG easiy crashes in poorly constrained cases
    end
end
if ~isequal(LU,[p.lb p.ub])
    p.changedbounds = 1;
end
feasible = p.feasible;
vol_reduction = 0;


function planC = nrrd2cerr(filename,scanName,planC,save_flag)
% nii2cerr.m
% http://teem.sourceforge.net/nrrd/format.html#space
%
% Import .nrrd data to CERR
% Usage: planC = nrrd2cerr(filename,scanName,planC,save_flag);
%
% AI 4/10/19 for nii
% EL 06/17/21 adapt for nrrd

if ~exist('planC','var') || isempty(planC)
    planC = [];
end

if ~exist('save_flag','var') || isempty(save_flag)
    save_flag = 0;
end

% read nrrd file
% [vol3M,infoS] = nifti_read_volume(filename);
% nii = load_nii(filename);
headerInfo = nhdr_nrrd_read(filename,1);

% vol3M = nii.img;
vol3M = headerInfo.data;
infoS.Offset = [headerInfo.spaceorigin(1) headerInfo.spaceorigin(2) headerInfo.spaceorigin(3)];
infoS.PixelDimensions = [abs(headerInfo.spacedirections_matrix(1,1)) abs(headerInfo.spacedirections_matrix(2,2)) abs(headerInfo.spacedirections_matrix(3,3))];
infoS.Dimensions = headerInfo.sizes;

scanOffset = 0;
volMin = min(vol3M(:));
if volMin<0
    scanOffset = -volMin;
end

if headerInfo.spacedirections_matrix(1,1) > 0
    vol3M = flip(vol3M,1);
end

if headerInfo.spacedirections_matrix(2,2) > 0
    vol3M = flip(vol3M,2);
end

% infoS.Offset = [0 0 0];
% infoS.PixelDimensions = infoS.pixdim;
% infoS.Dimensions = infoS.dimension(2:4);

planC = mha2cerr(infoS,vol3M,scanOffset,scanName,planC,save_flag);

end


% Test file for @chebfun/unwrap.m.

function pass = test_unwrap(pref)

if (nargin < 1)
    pref = chebfunpref();
end

% Generate a few random points to use as test values.
seedRNG(6178);
x = 2 * rand(100, 1) - 1;

% Test empty case.
pass(1) = isempty(unwrap(chebfun()));

% Check that chebfuns with only one fun aren't unwrapped.
f = chebfun(@(x) exp(x), [-1 1], pref);
pass(2) = isequal(f, unwrap(f));

pref.techPrefs.extrapolate = 1;
f = chebfun(@mysawtooth, [0 2*pi 4*pi 6*pi 8*pi], pref);
uf = unwrap(f);
pass(3) = norm(feval(uf, x) - (x - pi), inf) < 10*vscale(uf)*eps;

% Check tol input.
g = 10*f;
ug = unwrap(g, 10*pi);
pass(4) = norm(feval(ug, x) - 10*(x - pi), inf) < 10*vscale(ug)*eps;

% Check error conditions.
try
    f = chebfun(@(x) [sin(x) cos(x)], [-1 0 1]);
    g = unwrap(f);
    pass(5) = false;
catch ME
    pass(5) = strcmp(ME.identifier, 'CHEBFUN:CHEBFUN:unwrap:array');
end

end

function y = mysawtooth(x)
    y = mod(x, 2*pi) - pi;
end


function inspect_ft_electroderealign

% MEM 3gb
% WALLTIME 00:10:00
% DEPENDENCY ft_electroderealign ft_read_mri ft_read_sens ft_prepare_mesh ft_warp_apply

%% load mri, segmentation and electrode definition
mri = ft_read_mri(dccnpath('/home/common/matlab/fieldtrip/data/ftp/test/ctf/Subject01.mri'));
load(dccnpath('/home/common/matlab/fieldtrip/data/ftp/tutorial/headmodel_eeg/segmentedmri.mat'));
elec = ft_read_sens(dccnpath('/home/common/matlab/fieldtrip/template/electrode/standard_1020.elc'));
temp = ft_read_sens(dccnpath('/home/common/matlab/fieldtrip/template/electrode/standard_1005.elc'));

% create a bem and a fem mesh

cfg = [];
cfg.tissue = {'brain', 'skull', 'scalp'};
cfg.numvertices = [3000 2000 1000];
bem = ft_prepare_mesh(cfg, segmentedmri);

cfg = [];
cfg.method = 'hexahedral';
cfg.tissue = {'brain', 'skull', 'scalp'};
fem = ft_prepare_mesh(cfg, segmentedmri);

%% method: fiducial

nas = mri.hdr.fiducial.mri.nas;
lpa = mri.hdr.fiducial.mri.lpa;
rpa = mri.hdr.fiducial.mri.rpa;

% they are in voxels, hence need to be transformed to head coordinates
nas = ft_warp_apply(mri.transform, nas, 'homogenous');
lpa = ft_warp_apply(mri.transform, lpa, 'homogenous');
rpa = ft_warp_apply(mri.transform, rpa, 'homogenous');

fiducials.chanpos  = [nas; lpa; rpa];
fiducials.label    = {'Nz', 'LPA', 'RPA'};
fiducials.unit     = mri.unit;
fiducials.coordsys = mri.coordsys;

cfg = [];
cfg.method = 'fiducial';
cfg.template = fiducials;
cfg.elec = elec;
cfg.channel = 'all';
cfg.fiducial = {'Nz', 'LPA', 'RPA'};
elec_realigned1 = ft_electroderealign(cfg);

figure
ft_plot_sens(elec_realigned1, 'label', 'on');
ft_plot_axes(elec_realigned1, 'fontcolor', 'k');

%% method: template

cfg = [];
cfg.method = 'template';
cfg.template = elec_realigned1;
cfg.elec = elec;
elec_realigned2 = ft_electroderealign(cfg);

figure
ft_plot_sens(elec_realigned2, 'label', 'on');
ft_plot_axes(elec_realigned2, 'fontcolor', 'k');

%% method: interactive

% rotate    [0 0 -90]
% translate [35 0 40]

cfg = [];
cfg.method = 'interactive';
cfg.headshape = bem(3);
cfg.elec = elec;
elec_realigned3 = ft_electroderealign(cfg);

figure
ft_plot_sens(elec_realigned3, 'label', 'on');
ft_plot_axes(elec_realigned3, 'fontcolor', 'k');

%% method: headshape

cfg = [];
cfg.method = 'headshape';
cfg.headshape = bem(1);
cfg.elec = elec_realigned3;
elec_realigned4 = ft_electroderealign(cfg);

figure
ft_plot_sens(elec_realigned4, 'label', 'on');
ft_plot_axes(elec_realigned4, 'fontcolor', 'k');


function testHarness

%% Lane Marking Identification
% One component of future luxury automobile safety
% systems is warning drivers that they are drifting
% between lanes.  To do this, it is first necessary
% to identify where the lanes are.  In this example,
% we will look at identifying the lanes from a image
% of a road.
%
% Copyright 2007-2013 MathWorks, Inc.
%

%% Setup
clear

%% Initialize Objects
% Video Reader

hvfr = vision.VideoFileReader('viplanedeparture.avi', ...
    'VideoOutputDataType', 'uint8');

%% Configure Video Players
hVidSource = vision.VideoPlayer('Name', 'Source Image');

%% Setup Replay Loop
NumLoops = 1;
for loopIdx = 1:NumLoops

    % Loop through video frames
    while ~isDone(hvfr)
        % Read Frame
        frameRGB = step(hvfr);
        % and display
        step(hVidSource, frameRGB);

        % Call Lane Marking Algorithm
        laneMarkingAlgorithmWithVisualization(frameRGB);

    % End While
    end

    reset(hvfr);
% end loop
end

%% Release resources
release(hvfr   );


function [result] = analyze_accuracy_robustness(experiment, trackers, sequences, varargin)
% analyze_accuracy_robustness Performs ranking analysis
%
% Performs ranking analysis for a given experiment on a set trackers and sequences.
%
% Input:
% - experiment (structure): A valid experiment structures.
% - trackers (cell): A cell array of valid tracker descriptor structures.
% - sequences (cell): A cell array of valid sequence descriptor structures.
% - varargin[Ranking] (boolean): Perform ranking analysis, used by default.
% - varargin[Tags] (cell): An array of tag names that should be used
% instead of sequences.
% - varargin[UsePractical] (boolean): Use practical difference for accuracy.
% - varargin[Alpha] (double): Statistical significance parameter.
% - varargin[Adaptation] (string): Type of rank adaptation. See
% adapter_ranks for more details.
%
% Output:
% - result (structure): A structure with the following fields
%     - accuracy
%          - values: average overlap matrix
%          - ranks: accuracy ranks matrix (if applicable)
%     - robustness
%          - values: number of failures matrix
%          - normalized: number of failures matrix (normalized)
%          - ranks: robustness ranks matrix (if applicable)
%     - lengths: number of frames for individual selectors
%     - tags: names of individual selectors

    usepractical = false;
    ranking = true;
    tags = {};
	adaptation = 'mean';
    alpha = 0.05;

    for i = 1:2:length(varargin)
        switch lower(varargin{i})
            case 'ranking'
                ranking = varargin{i+1} ;
            case 'tags'
                tags = varargin{i+1} ;
            case 'usepractical'
                usepractical = varargin{i+1} ;
            case 'alpha'
                alpha = varargin{i+1};
            case 'adaptation'
                adaptation = varargin{i+1};
            otherwise
                error(['Unknown switch ', varargin{i},'!']) ;
        end
    end

    print_text('Ranking analysis for experiment %s ...', experiment.name);

    if ~any(strcmp(experiment.type, {'supervised', 'realtime'}))
        error('Ranking analysis can only be used in supervised experiment scenario.');
    end;

    if experiment.parameters.repetitions < 5 && ranking
        error('The experiment specifies less than 5 repetitions. Not valid for statistical consideration.');
    end

    if experiment.parameters.repetitions < 15 && ranking
        print_text('Warning: the experiment specifies less than 15 repetitions, the results may be statistically unstable.');
    end;

    print_indent(1);

    experiment_sequences = convert_sequences(sequences, experiment.converter);

    if ~isempty(tags)

        tags = unique(tags); % Remove any potential duplicates.

        selectors = sequence_tag_selectors(experiment, ...
            experiment_sequences, tags);

    else

        selectors = sequence_selectors(experiment, experiment_sequences);

    end;

    if ~ranking
        % Disable ranking
        alpha = -1;
    end;

    [accuracy, robustness, lengths] = trackers_ar(experiment, trackers, ...
        experiment_sequences, selectors, alpha, usepractical, adaptation);

    result = struct('accuracy', accuracy, 'robustness', robustness, 'lengths', lengths);
    result.tags = cellfun(@(x) x.name, selectors, 'UniformOutput', false);

    print_indent(-1);

end

function [accuracy, robustness, lengths] = trackers_ar(experiment, trackers, ...
    sequences, selectors, alpha, usepractical, adaptation)

    N_trackers = length(trackers) ;
    N_selectors = length(selectors) ;

    accuracy.values = zeros(N_selectors, N_trackers);
    robustness.values = zeros(N_selectors, N_trackers);
    robustness.normalized = zeros(N_selectors, N_trackers);

    if alpha > 0
        accuracy.ranks = zeros(N_selectors, N_trackers);
        robustness.ranks = zeros(N_selectors, N_trackers);
    end;

    lengths = zeros(N_selectors, 1);

    for a = 1:length(selectors)

	    print_indent(1);

	    print_text('Processing selector %s ...', selectors{a}.name);

        % rank trackers and calculate statistical significance of differences
        [average_overlap, average_failures, average_failurerate, HA, HR, available] = ...
            trackers_raw_scores_selector(experiment, trackers, sequences, selectors{a}, alpha, usepractical);

        accuracy.values(a, :) = average_overlap;
        robustness.values(a, :) = average_failures;
        robustness.normalized(a, :) = average_failurerate;

        if alpha > 0

            [~, order_by_accuracy] = sort(average_overlap(available), 'descend');
            accuracy_ranks = ones(size(available)) * length(available);
            [~, accuracy_ranks(available)] = sort(order_by_accuracy, 'ascend') ;

            [~, order_by_robustness] = sort(average_failures(available), 'ascend');
            robustness_ranks = ones(size(available)) * length(available);
            [~, robustness_ranks(available)] = sort(order_by_robustness,'ascend');

            % get adapted ranks
            adapted_accuracy_ranks = adapted_ranks(accuracy_ranks, HA, adaptation);
            adapted_robustness_ranks = adapted_ranks(robustness_ranks, HR, adaptation);

            % mask out results that are not available
            adapted_accuracy_ranks(~available) = nan;
            adapted_robustness_ranks(~available) = nan;
            accuracy.ranks(a, :) = adapted_accuracy_ranks;
            robustness.ranks(a, :) = adapted_robustness_ranks;

        end;

        lengths(a) = selectors{a}.length(sequences);

	    print_indent(-1);

    end

end

function [average_accuracy, average_failures, average_failurerate, HA, HR, available] ...
    = trackers_raw_scores_selector(experiment, trackers, sequences, selector, alpha, usepractical)

    cacheA = cell(length(trackers), 1);
    cacheR = cell(length(trackers), 1);

    HA = false(length(trackers)); % results of statistical testing
    HR = false(length(trackers)); % results of statistical testing

    average_accuracy = nan(length(trackers), 1);
    average_failures = nan(length(trackers), 1);
    average_failurerate = nan(length(trackers), 1);

    available = true(length(trackers), 1);

    if alpha > 0 && usepractical
        practical = selector.groundtruth_values(sequences, 'practical');
        practical = cat(1, practical{:});
    else
        practical = [];
    end

	print_indent(1);
    [~, lengths] = selector.length(sequences);

    for t1 = 1:length(trackers)

		print_text('Processing tracker %s ...', trackers{t1}.identifier);

        if isempty(cacheA{t1})
            [O1, F1] = calculate_accuracy_overlap(selector, experiment, trackers{t1}, sequences);
            cacheA{t1} = O1; cacheR{t1} = F1;
        else
            O1 = cacheA{t1}; F1 = cacheR{t1};
        end;

        if isempty(O1)
            available(t1) = false;
			HA(t1, :) = true; HA(:, t1) = true;
			HR(t1, :) = true; HR(:, t1) = true;
            HA(t1, t1) = false; HR(t1, t1) = false;
            continue;
        end

        valid_frames = ~isnan(O1) ;

        % O1 ... stacked per-frame overlaps (already averaged over
        % repeats).
        %
        % F1 ... fragments (rows) x repeats (columns) of raw failure count.

        % Average accuracy is average over valid frames (non NaN).
        if all(valid_frames == 0)
            average_accuracy(t1) = 0;
        else
            average_accuracy(t1) = mean(O1(valid_frames));
        end;

        % Average failures are sum of failures in fragments averaged over
        % repetitions
        average_failures(t1) = mean(sum(F1, 1));

        % Average failure rate is sum of failures in fragments divided by
        % total length of selector, averaged over repetitions
        average_failurerate(t1) = mean(sum(F1, 1) ./ sum(lengths));

        if alpha > 0

            for t2 = t1+1:length(trackers)

                t2

                if isempty(cacheA{t1})
                    [O1, F1] = calculate_accuracy_overlap(selector, experiment, trackers{t1}, sequences);
                    cacheA{t1} = O1; cacheR{t1} = F1;
                else
                    O1 = cacheA{t1}; F1 = cacheR{t1};
                end;

                if isempty(cacheA{t2})
                    [O2, F2] = calculate_accuracy_overlap(selector, experiment, trackers{t2}, sequences);
                    cacheA{t2} = O2; cacheR{t2} = F2;
                else
                    O2 = cacheA{t2}; F2 = cacheR{t2};
                end;

                if isempty(O2)
                    available(t2) = false;
                    continue;
                end

                % If alpha is 0 then we disable the equivalence testing
                if alpha == 0

                    ha = true; hr = true;

                else

                    [ha, hr] = test_significance(O1, F1, O2, F2, alpha, practical);

                end;

                HA(t1, t2) = ha; HA(t2, t1) = HA(t1, t2);
                HR(t1, t2) = hr; HR(t2, t1) = HR(t1, t2);
            end;
        end;
    end;

	print_indent(-1);

    average_accuracy(isnan(average_accuracy)) = 0;

end

function [aggregated_overlap, aggregated_failures] = calculate_accuracy_overlap(selector, experiment, tracker, sequences)

    aggregated_overlap = [];
    aggregated_failures = [];

    burnin = experiment.parameters.burnin;

    groundtruth = selector.groundtruth(sequences);
    trajectories = selector.results(experiment, tracker, sequences);

    repeat = experiment.parameters.repetitions;

    for s = 1:numel(groundtruth)

        accuracy = nan(repeat, length(groundtruth{s}));
        failures = nan(repeat, 1);

        for r = 1:size(trajectories, 2)

            if isempty(trajectories{s, r})
                continue;
            end;

            [~, frames] = estimate_accuracy(trajectories{s, r}, groundtruth{s}, 'burnin', burnin, 'BindWithin', [sequences{s}.width, sequences{s}.height]);

            accuracy(r, :) = frames;

            failures(r) = estimate_failures(trajectories{s, r}, groundtruth{s});

        end;

        frames = num2cell(accuracy, 1);
        sequence_overlaps = cellfun(@(frame) nanmean(frame), frames);

        failures(isnan(failures)) = nanmean(failures);
        sequence_failures = failures';

        if ~isempty(sequence_overlaps)
            aggregated_overlap = [aggregated_overlap, sequence_overlaps]; %#ok<AGROW>
        end;

        if ~isempty(sequence_failures)
            aggregated_failures = [aggregated_failures; sequence_failures]; %#ok<AGROW>
        end;

    end;

    aggregated_failures = aggregated_failures(~isnan(aggregated_failures(:, 1)), :);

end

function [ha, hr, hp] = test_significance(A1, R1, A2, R2, alpha, practical)
% test_significance Verify difference of A-R performance for two trackers
%
% Compare A-R performance of two trackers taking into account statistical
% and practical difference of results.
%
% Input:
% - A1 (double matrix): Per-frame accuracy for first tracker
% - R1 (double matrix): Per-segment robustness for first tracker
% - A2 (double matrix): Per-frame accuracy for second tracker
% - R2 (double matrix): Per-segment robustness for second tracker
% - alpha (double): Confidence parameter
% - practical (boolean): Take into account practical difference for the
% frames
%
% Output:
% - ha (boolean): Is accuracy different
% - hr (boolean): Is robustness different
% - hp (number): Number of frames for which the practical difference test
% was positive
%

    % Testing accuracy significance

    % Statistical test
    dif = A1 - A2;
    valid = ~isnan(dif);
    dif = dif(valid) ;
    if (length(dif) < 25)
        print_text('Warning: less than 25 samples when comparing trackers. Cannot reject hypothesis');
        ha = 1;
    else
        if (is_octave)
            try
            pa = wilcoxon_test(A1(valid), A2(valid));
            ha = (pa <= alpha);
            catch
                %A1(valid) - A2(valid)
                %pa = 0;
                ha = 1;
            end;
        else
            [~, ha, ~] = signrank(dif, [], 'alpha', alpha ) ;
        end;
    end;

    hp = 0;

    % Practical difference of accuracy
    if ~isempty(practical)
        hp = sum(dif' < practical(valid));
        if abs(mean(dif' ./ practical(valid))) < 1
            ha = 0;
        end;

    end;

    % Testing robustness significance
    R1 = R1(:);
    R2 = R2(:);
	%R1 = mean(R1,2);
    %R2 = mean(R2,2);

    if (is_octave)
       pr = u_test(R1, R2);
       hr = (pr <= alpha);
    else
       [~, hr] = ranksum(R1, R2, 'alpha', alpha) ;
    end;

end


classdef nme_bus_acp < mp.nme_bus & mp.form_acp

%   MATPOWER
%   Copyright (c) 2019-2022, Power Systems Engineering Research Center (PSERC)
%   by Ray Zimmerman, PSERC Cornell
%
%   This file is part of MATPOWER.
%   Covered by the 3-clause BSD License (see LICENSE file for details).
%   See https://matpower.org for more info.

%     properties
%         name = 'bus';
%     end

    methods
        function obj = add_vvars(obj, nm, dm, idx)
            dme = obj.data_model_element(dm);
            nb = obj.nk;

            %% prepare angle bounds for ref buses
            va_lb = -Inf(nb, 1);
            va_ub =  Inf(nb, 1);
            k = find(dme.type == mp.NODE_TYPE.REF);
            va_lb(k) = dme.va_start(k);
            va_ub(k) = dme.va_start(k);

            nm.add_var('va', 'Va', nb, dme.va_start, va_lb, va_ub);
            nm.add_var('vm', 'Vm', nb, dme.vm_start, dme.vm_lb, dme.vm_ub);
        end
    end     %% methods
end         %% classdef