% Displacement.m
%
% This script demonstrates use of the MadgwickAHRS and MahonyAHRS algorithm
% classes with sensor data. Input data contains calibrated gyroscope,
% accelerometer and magnetometer data logged from an ADIS16405 device.
% This script relies on Madgwick's AHRS algorithm. 
%
% Note that the Euler angle plot shows erratic behaviour in phi and psi
% when theta approaches �90 degrees. This due to a singularity in the Euler
% angle sequence known as 'Gimbal lock'.  This issue does not exist for a
% quaternion or rotation matrix representation.
%
% Date          Author          Notes
% 28/09/2011    SOH Madgwick    Initial release
% 20/11/2011    B. Kieft        Attempting to extract linear displacement. 
%                               Will need to convert Euler to quaternion eventually

%% Start of script

addpath('quaternion_library');      % include quaternion library
%close all;                          % close all figures
%clear;                              % clear all variables
clc;                                % clear the command terminal

%% Import and plot sensor data

%load('ExampleData.mat');

if( exist('position') ~= 1 ) position = [3 59 1213 1070]; end


if( exist('Kp') ~=1) Kp = .002;end
if( exist('Ki') ~=1) Ki = 0;end

if( exist('fileName') )
  fullFileName = strcat(fileName,'.csv');
  if( exist(fullFileName) == 2 )

    AHRSData = mfcsvread(fullFileName);
    Ts = .01;   %Sample Period; 100 Hz sample rate. 

    Accelerometer(:,1) = AHRSData.X_acc;
    Accelerometer(:,2) = AHRSData.Y_acc;
    Accelerometer(:,3) = AHRSData.Z_acc;
    
    Gyroscope(:,1) = AHRSData.X_Gyro;
    Gyroscope(:,2) = AHRSData.Y_Gyro;
    Gyroscope(:,3) = AHRSData.Z_Gyro;
    
    % Convert from milliGauss
    Magnetometer(:,1) = AHRSData.X_Mag/1000;
    Magnetometer(:,2) = AHRSData.Y_Mag/1000;
    Magnetometer(:,3) = AHRSData.Z_Mag/1000;
    
    time = AHRSData.Sample*Ts;
    
    xImu = 0;
  else

    fullFileName = strcat(fileName,'.mat');
    if( exist(fullFileName) == 2 )
      load(fullFileName);
      Ts = 1/256;
      xImu = 1;
    else
      fprintf('File %s not found with either .csv or.mat suffix.\n', ...
	      fileName);
      return
    end
  end
else
  fprintf('Running zero-motion case with g and b hardcoded constants.\n');
  xImu = 0;
  Ts = .01;                             %Sample Period; 100 Hz sample rate. 
  npts = 1000;
  Accelerometer      =  zeros(npts,3);
  Accelerometer(:,3) = -ones(npts,1);
  Gyroscope          =  zeros(npts,3);
  Magnetometer       =  ones(npts,3)*diag([.2124 .0210 -.4414]);
  time               =  [1:npts]'*Ts;
end

figure('Name', 'Sensor Data');
set(gcf,'position',position)
axis(1) = subplot(3,1,1);
hold on;
plot(time, Gyroscope(:,1), 'r');
plot(time, Gyroscope(:,2), 'g');
plot(time, Gyroscope(:,3), 'b');
grid on
legend('X', 'Y', 'Z');
xlabel('Time (s)');
ylabel('Angular rate (deg/s)');
title('Gyroscope');
hold off;
axis(2) = subplot(3,1,2);
hold on;
plot(time, Accelerometer(:,1), 'r');
plot(time, Accelerometer(:,2), 'g');
plot(time, Accelerometer(:,3), 'b');
grid on
legend('X', 'Y', 'Z');
xlabel('Time (s)');
ylabel('Raw Acceleration in Sensor-Fixed Frame (g)');
title('Accelerometer');
hold off;
axis(3) = subplot(3,1,3);
hold on;
plot(time, Magnetometer(:,1), 'r');
plot(time, Magnetometer(:,2), 'g');
plot(time, Magnetometer(:,3), 'b');
legend('X', 'Y', 'Z');
xlabel('Time (s)');
ylabel('Flux (G)');
title('Magnetometer');
grid on
hold off;
linkaxes(axis, 'x');

%% Process sensor data through algorithm

AHRS = MadgwickAHRS('SamplePeriod', Ts, 'Beta', Kp);
%AHRS = MahonyAHRS('SamplePeriod', Ts, 'Kp', Kp,'Ki', Ki);

quaternion = zeros(length(time), 4);
for t = 1:length(time)
    AHRS.Update(deg2rad(Gyroscope(t,:)), Accelerometer(t,:), Magnetometer(t,:));
    quaternion(t, :) = AHRS.Quaternion;
end

%% Plot algorithm output as Euler angles

euler = quatern2euler(quaternConj(quaternion));	% use conjugate for sensor frame relative to Earth

figure('Name', 'Euler Angles');
set(gcf,'position',position)
hold on;
plot(time, rad2deg(euler));
%plot(time, euler(:,1), 'r');
%plot(time, euler(:,2), 'g');
%plot(time, euler(:,3), 'b');
str = sprintf( 'Euler Angles; Kp = %5.2e, Ki = %5.2e',Kp, Ki);
title(str);
xlabel('Time (s)');
ylabel('Angle (deg)');
legend('phi', 'theta', 'psi');
hold off;
grid on

phi   = euler(:,1);
theta = euler(:,2);
psi   = euler(:,3);

for( i=1:length(Accelerometer ))
  cr = cos(phi(i));
  sr = sin(phi(i));
  cp = cos(theta(i));
  sp = sin(theta(i));
  cy = cos(psi(i));
  sy = sin(psi(i));

%  T_B_E = [ cy*cp, -sy*cr+cy*sp*sr, sy*sr+cy*sp*cr
%	          sy*cp, cy*cr+sy*sp*sr, -cy*sr+sy*sp*cr
%	          -sp, cp*sr, cp*cr];

% This seems to be vehicle frame while above (commented out) seems to be earth frame. 
% Not sure why this rotation matrix works and the other doesn't... 
% Copied this technique from Inertial Sensing of Cyclical Movement 
% pg. 2507
% The Journal of Experimental Biology
if( 0 )
  T_B_E = [cp*cy            cp*sy            -sp
           sr*sp*cy-cr*sy   sr*sp*sy+cr*cy   sr*cp
           cr*sp*cy+sr*sy   cr*sp*sy-sr*cy   cr*cp];
end
  
  % Extract the rotation matrix going from the Earth-Fixed frame to the
  % body-fixed frame.
  clear T_B_E
  T_B_E = quatern2rotMat(quaternion(i,:));

  % Recoordinatize the accleration in the Earth-fixed frame, where gravity
  % is constant.
  %accE(i,:) = Accelerometer(i,:) * T_B_E(:,:,i);
  accE(i,:) = Accelerometer(i,:) * T_B_E;

  % Remove gravity to get Proper Acceleration:
  propAccE(i,1:2) = accE(i,1:2);

  if( xImu )
    propAccE(i,3) = accE(i,3) - 1;
  else
    propAccE(i,3) = accE(i,3) + 1;
  end    
    
    
  magAcc(i) = norm(propAccE(i,:));
  
end

magAcc = magAcc';

figure('Name', 'Raw Accels');
set(gcf,'position',position)
hold on;
plot(time, Accelerometer);
title('Raw Acceleration Data in Sensor Frame');
xlabel('Time (s)');
ylabel('(g)');
legend('X', 'Y', 'Z');
grid on

figure('Name', 'Accelerations');
set(gcf,'position',position)
hold on;
plot(time, [propAccE magAcc] );
title('Proper Acceleration of Sensor in Earth-Fixed Frame');
xlabel('Time (s)');
ylabel('(g)');
legend('X', 'Y', 'Z','Magnitude');
grid on

if( 0 )
  % Just for fun, try a low pass filter
  fcutoff = 5;   %Hz
  fNorm = fcutoff*Ts/2;
  [b,a] = butter(10, fNorm, 'low');
  propAccE = filtfilt(b,a,propAccE);


  % Filtered Data plot
  figure('Name', 'Filtered Acceleration');
  set(gcf,'position',position)
  hold on;
  plot(time, propAccE);
  title('Filtered Proper Acceleration');
  xlabel('Time (s)');
  ylabel('(g)');
  legend('X', 'Y', 'Z');
  grid on
end
% Instead, try subtracting the mean:
propAccE = detrend(propAccE);
figure('Name', 'Detrended Proper Acceleration in Earth-Fixed Frame');
set(gcf,'position',position)
hold on;
plot(time, propAccE);
title('Detrended Proper Acceleration in Earth-Fixed Frame');
xlabel('Time (s)');
ylabel('(g)');
legend('X', 'Y', 'Z');
grid on


% Integrate once for velocity and once more for displacement.
% Divide by sampling rate

g = 9.806; % m/s^2 at 38 degrees latitude.

velocityX=(cumtrapz(propAccE(:,1)/g)*Ts);
dispX=(cumtrapz(velocityX)*Ts);
velocityY=(cumtrapz(propAccE(:,2)/g)*Ts);
dispY=(cumtrapz(velocityY)*Ts);
velocityZ=(cumtrapz(propAccE(:,3)/g)*Ts);
dispZ=(cumtrapz(velocityZ)*Ts);

figure('Name', 'Displacement');
set(gcf,'position',position)
hold on;
plot(time, dispX, 'b');
plot(time, dispY, 'g');
plot(time, dispZ, 'r');
title('Displacement');
xlabel('Time (s)');
ylabel('Meters');
legend('X', 'Y', 'Z');
hold off;
grid on
%% End of script
