showSpherical.m

clear all; home;

%% Load the data
dataSetFolder = fullfile(fileparts(which(mfilename)), '..' , '..', 'data', 'set-2', 'roll-and-tilt-at-45-90');
%dataSetFolder = fullfile(fileparts(which(mfilename)), '..' , '..', 'data', 'set-2', 'rotate-ccw-around-x-pointing-forward');
%dataSetFolder = fullfile(fileparts(which(mfilename)), '..' , '..', 'data', 'set-2', 'rotate-ccw-around-y-pointing-left');
%dataSetFolder = fullfile(fileparts(which(mfilename)), '..' , '..', 'data', 'set-2', 'rotate-ccw-around-x-pointing-up');
%dataSetFolder = fullfile(fileparts(which(mfilename)), '..' , '..', 'data', 'set-2', 'rotate-ccw-around-z-pointing-up');
[accelerometer, ~, compass, ~] = loadData(dataSetFolder, true);

% resample the time series
[acceleration, compass] = lerpTimeSeries(accelerometer, compass);

% extract time vector
time = acceleration.Time;
N = acceleration.Length;

%% Get roll, pitch and yaw
hwb = waitbar(0, 'Calculating states ...');

rtpa = nan(N, 3);
rtpm = nan(N, 3);

for i=1:N
    % fetch accelerometer and magnetometer
    a = acceleration.Data(i, :);
    m = compass.Data(i, :);

    % convert accelerometer to spherical
    r       = sqrt(a(1)^2 + a(2)^2 + a(3)^2);
    theta   = acosd(a(3)/r);
    phi     = atan2d(a(2), a(1));
    rtpa(i,:) = [r theta phi];
    
    % convert magnetometer to spherical
    r       = sqrt(m(1)^2 + m(2)^2 + m(3)^2);
    theta   = acosd(m(3)/r);
    phi     = atan2d(m(2), m(1));
    rtpm(i,:) = [r theta phi];
    
    waitbar(i/N, hwb);
end
close(hwb);

%% Plot data
figureHandle = figure('Name', 'Accelerometer and magnetometer in spherical coordinates', ...
    'NumberTitle', 'off', ...
    'Color', [0.027 0.211 0.259] ...
    );

% define base colors
lineColor(1, :) = [1 0.25 0]; % x axis
lineColor(2, :) = [0.5 1 0]; % y axis
lineColor(3, :) = [0 0.5 1]; % z axis

lineColor(4, :) = [1 1 0]; % x axis
lineColor(5, :) = [1 0 1]; % y axis
lineColor(6, :) = [1.0 0.75 0.25]; % z axis

meanColor = [1 1 1];
axesColor = [0.473 0.473 0.473];
plotBackground = [0.15 0.15 0.15];
titleColor = [1 1 1];


%% Radius
axisRpy(1) = subplot(3, 2, 1, ...
    'Parent', figureHandle, ...
    'XGrid', 'on', ...
    'XColor', axesColor, ...
    'YGrid', 'on', ...
    'YColor', axesColor, ...
    'Color', plotBackground ...
    );

t = time;
r = rtpa(:, 1);
line(t, r, ...
    'Parent', axisRpy(1), ...
    'LineStyle', 'none', ...
    'Marker', '.', ...
    'MarkerSize', 2, ...
    'Color', lineColor(4, :) ...
    ); 

xlim([0 t(end)]);
ylim([-1.5 1.5]);

title('Radius', ...
    'Color', titleColor ...
    );
ylabel('Radius');
xlabel('t [s]');
%legendHandle = legend('DCM raw', 'DCM int', 'axis int');
%set(legendHandle, 'TextColor', [1 1 1]);

%% Theta
axisRpy(2) = subplot(3, 2, 3, ...
    'Parent', figureHandle, ...
    'XGrid', 'on', ...
    'XColor', axesColor, ...
    'YGrid', 'on', ...
    'YColor', axesColor, ...
    'Color', plotBackground ...
    );

t = time;
theta = rtpa(:, 2);
line(t, theta, ...
    'Parent', axisRpy(2), ...
    'LineStyle', 'none', ...
    'Marker', '.', ...
    'MarkerSize', 2, ...
    'Color', lineColor(5, :) ...
    ); 

xlim([0 t(end)]);
ylim([-180 180]);

title('Theta (elevation)', ...
    'Color', titleColor ...
    );
ylabel('angle [\circ]');
xlabel('t [s]');
%legendHandle = legend('DCM raw', 'DCM int', 'axis int');
%set(legendHandle, 'TextColor', [1 1 1]);

%% Phi
axisRpy(3) = subplot(3, 2, 5, ...
    'Parent', figureHandle, ...
    'XGrid', 'on', ...
    'XColor', axesColor, ...
    'YGrid', 'on', ...
    'YColor', axesColor, ...
    'Color', plotBackground ...
    );

t = time;
phi = rtpa(:, 3);
line(t, phi, ...
    'Parent', axisRpy(3), ...
    'LineStyle', 'none', ...
    'Marker', '.', ...
    'MarkerSize', 2, ...
    'Color', lineColor(6, :) ...
    ); 

xlim([0 t(end)]);
ylim([-180 180]);

title('Phi (azimuth)', ...
    'Color', titleColor ...
    );
ylabel('angle [\circ]');
xlabel('t [s]');
%legendHandle = legend('DCM raw', 'DCM int', 'axis int');
%set(legendHandle, 'TextColor', [1 1 1]);


%% Radius
axisRpy(4) = subplot(3, 2, 2, ...
    'Parent', figureHandle, ...
    'XGrid', 'on', ...
    'XColor', axesColor, ...
    'YGrid', 'on', ...
    'YColor', axesColor, ...
    'Color', plotBackground ...
    );

t = time;
r = rtpm(:, 1);
line(t, r, ...
    'Parent', axisRpy(4), ...
    'LineStyle', 'none', ...
    'Marker', '.', ...
    'MarkerSize', 2, ...
    'Color', lineColor(4, :) ...
    ); 

xlim([0 t(end)]);
ylim([-1.5 1.5]);

title('Radius', ...
    'Color', titleColor ...
    );
ylabel('Radius');
xlabel('t [s]');
%legendHandle = legend('gyro int');
%set(legendHandle, 'TextColor', [1 1 1]);

%% Theta
axisRpy(5) = subplot(3, 2, 4, ...
    'Parent', figureHandle, ...
    'XGrid', 'on', ...
    'XColor', axesColor, ...
    'YGrid', 'on', ...
    'YColor', axesColor, ...
    'Color', plotBackground ...
    );

t = time;
theta = rtpm(:, 2);
line(t, theta, ...
    'Parent', axisRpy(5), ...
    'LineStyle', 'none', ...
    'Marker', '.', ...
    'MarkerSize', 2, ...
    'Color', lineColor(5, :) ...
    ); 

xlim([0 t(end)]);
ylim([-180 180]);

title('Theta (elevation)', ...
    'Color', titleColor ...
    );
ylabel('angle [\circ]');
xlabel('t [s]');
%legendHandle = legend('gyro int');
%set(legendHandle, 'TextColor', [1 1 1]);

%% Phi
axisRpy(6) = subplot(3, 2, 6, ...
    'Parent', figureHandle, ...
    'XGrid', 'on', ...
    'XColor', axesColor, ...
    'YGrid', 'on', ...
    'YColor', axesColor, ...
    'Color', plotBackground ...
    );

t = time;
phi = rtpm(:, 3);
line(t, phi, ...
    'Parent', axisRpy(6), ...
    'LineStyle', 'none', ...
    'Marker', '.', ...
    'MarkerSize', 2, ...
    'Color', lineColor(6, :) ...
    ); 


xlim([0 t(end)]);
ylim([-180 180]);

title('Phi (azimuth)', ...
    'Color', titleColor ...
    );
ylabel('angle [\circ]');
xlabel('t [s]');
%legendHandle = legend('gyro int');
%set(legendHandle, 'TextColor', [1 1 1]);


%% Plot refining
% link the axes
linkaxes(axisRpy, 'x');