% ExampleScript.m
%
% This script demonstrates use of the MadgwickAHRS and MahonyAHRS algorithm
% classes with example data. ExampleData.mat contains calibrated gyroscope,
% accelerometer and magnetometer data logged from an AHRS device (x-IMU)
% while it was sequentially rotated from 0 degrees, to +90 degree and then
% to -90 degrees around the X, Y and Z axis.  The script first plots the
% example sensor data, then processes the data through the algorithm and
% plots the output as Euler angles.
%
% 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

%% 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

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');
legend('X', 'Y', 'Z');
xlabel('Time (s)');
ylabel('Angular rate (deg/s)');
title('Gyroscope');
hold off;
grid on
axis(2) = subplot(3,1,2);
hold on;
plot(time, Accelerometer(:,1), 'r');
plot(time, Accelerometer(:,2), 'g');
plot(time, Accelerometer(:,3), 'b');
legend('X', 'Y', 'Z');
xlabel('Time (s)');
ylabel('Acceleration (g)');
title('Accelerometer');
hold off;
grid on
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');
hold off;
grid on
linkaxes(axis, 'x');

%% Process sensor data through algorithm

AHRS = MadgwickAHRS('SamplePeriod', 1/256, 'Beta', 0.1);
%AHRS = MahonyAHRS('SamplePeriod', 1/256, 'Kp', 0.5);

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 = rad2deg(quatern2euler(quaternConj(quaternion)));	% use conjugate for sensor frame relative to Earth

figure('Name', 'Euler Angles');
set(gcf,'position',position)
hold on;
plot(time, euler(:,1), 'r');
plot(time, euler(:,2), 'g');
plot(time, euler(:,3), 'b');
title('Euler angles');
xlabel('Time (s)');
ylabel('Angle (deg)');
legend('phi', 'theta', 'psi');
hold off;
grid on


phi   = euler(:,1)*pi/180;
theta = euler(:,2)*pi/180;
psi   = euler(:,3)*pi/180;

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_N = [ 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];

  T_B_N = [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];
  
  linAccN(i,:) = Accelerometer(i,:) * T_B_N;
  %
  % Subtract gravity in g's.
  linAccN(i,3) = linAccN(i,3) - 1;
 
end


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

figure('Name', 'linAccN');
set(gcf,'position',position)
hold on;
plot(time, linAccN);
title('linAccN');
xlabel('Time (s)');
ylabel('(g)');
legend('X', 'Y', 'Z');
grid on

fNorm = 1/(10/2);
[b,a] = butter(10, fNorm, 'low');
linAccN = filtfilt(b,a,linAccN);

figure('Name', 'Filtered linAccN');
set(gcf,'position',position)
hold on;
plot(time, linAccN);
title('Filtered linAccN');
xlabel('Time (s)');
ylabel('(g)');
legend('X', 'Y', 'Z');
grid on


%where
%accel is your accelometer data in m/s/s 
%ratehz = is the sample rate of the data in Hz

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

velocityX=(cumtrapz(linAccN(:,1)/g)/256);
dispX=(cumtrapz(velocityX)/256);
velocityY=(cumtrapz(linAccN(:,2)/g)/256);
dispY=(cumtrapz(velocityY)/256);
velocityZ=(cumtrapz(linAccN(:,3)/g)/256);
dispZ=(cumtrapz(velocityZ)/256);

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
