% Author: Tomas Trafina, Bc., system control/robotics field
% Czech Technical University in Prague, Dept. of Control Engineering
% email address: ttrafina@gmail.com
% May 2018; Last revision: 17-May-2018

%------------- BEGIN CODE --------------

%% load and adjust data
M = csvread("ImuEfData_2018_05_04_11_35.csv");
N = csvread("RtkData_2018_05_04_11_35.pos", 0, 1,[0 1 7702 3]);
M(:,3) = M(:,3)-2.2;    % get heading measurements closer to zero for easier observation of plots
M(:,6) = M(:,6)+1;      % remove measured gravitational acceleration (only approximation, other axes effect neglected)
M(:,4:6) = M(:,4:6)*9.81;   % conversion of linear acceleration from units of G to m/s^2

%% data manual alignment
imuTs = 0.004;      % sampling time of IMU sensor
rtkTs = 0.05;       % sampling time of RTK sensor
imut = (size(M,1)-1)*imuTs;     % time interval of IMU data
rtkt = (size(N,1)-1)*rtkTs;     % time interval of RTK data
timeDiff = 6.876;   % delay of IMU data behind RTK data
clear imuTs rtkTs

%% measurements plotting
t0 = 0;     % x axis lower limit
t1 = 378;   % x axis upper limit

figure
ha(1)=subplot(4,1,4);
rtkTime = linspace(0,rtkt,size(N,1));
plot(rtkTime,N(:,1)-6000,'r', rtkTime,N(:,2),'g', rtkTime,N(:,3),'b')
xlim([t0 t1])
grid on
title('Measured NED position')
xlabel('time [s]')
ylabel('distance [m]')
legend('north', 'east', 'down')

ha(2)=subplot(4,1,2);
imuTime = linspace(-timeDiff,imut-timeDiff,size(M,1));
plot(imuTime,M(:,1),'r', imuTime,M(:,2),'g', imuTime,M(:,3),'b')
xlim([t0 t1])
grid on
title('Measured RPY angles')
xlabel('time [s]')
ylabel('angle [rad]')
legend('roll', 'pitch', 'yaw')

ha(3)=subplot(4,1,1);
plot(linspace(0,imut,size(M,1)), M(:,7:9))
xlim([t0 t1])
grid on
title('Measured angular acceleration')
xlabel('time [s]')
ylabel('ang. acc. [rad/s^2]')
legend('roll acc.', 'pitch acc.', 'yaw acc.')

ha(4)=subplot(4,1,3);
plot(linspace(0,imut,size(M,1)), M(:,4:6))
xlim([t0 t1])
grid on
title('Measured linear acceleration')
xlabel('time [s]')
ylabel('lin. acc. [m/s^2]')
legend('X acc.', 'Y acc.', 'Z acc.')

linkaxes(ha, 'x');
clear t0 t1 ha

%% structured data storage
sensorImu.time = transpose(imuTime);
sensorImu.signals.values = [M(:,7:9) M(:,4:6) M(:,1:3)];    % saved order (ang. acc., lin. acc., euler angles)
sensorRtk.time = transpose(rtkTime);
sensorRtk.signals.values = [N(:,1)-6000 N(:,2:3)];

clear M N imut rtkt timeDiff imuTime rtkTime