% Displacement.m
%
% This script... 
% Date          Author          Notes
% 20/11/2011    B. Kieft        Attempting to extract linear displacement. 


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



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 = .02;   %Sample Period; 50 Hz sample rate. 

    acc_wrld(:,1) = AHRSData.acc_x_wrld;
    acc_wrld(:,2) = AHRSData.acc_y_wrld;
    acc_wrld(:,3) = AHRSData.acc_z_wrld;

    acc_sens(:,1) = AHRSData.acc_x_sens;
    acc_sens(:,2) = AHRSData.acc_y_sens;
    acc_sens(:,3) = AHRSData.acc_z_sens;
    
    time = AHRSData.sample*Ts;
    
    xImu = 0;
  end
end

%
% Try subtracting the mean of an initial quiescent period, should it
% exist:
if( exist('tStart','var') )
  tStartIndx = max(find(time<tStart));
  biasesE    = mean(acc_wrld(1:tStartIndx,:));
  stdBiasesE = std(acc_wrld(1:tStartIndx,:));
  fprintf(['Accelerometer BiasesE = %.4f %.4f %.4f g''s.\n',...
	  'Accelerometer stdE  = %.4f %.4f %.4f; deg/sec\n'],...
	  biasesE, stdBiasesE);

  biases    = mean(acc_sens(1:tStartIndx,:));	 
  stdBiases = std(acc_wrld(1:tStartIndx,:));
  fprintf(['Accelerometer Biases = %.4f %.4f %.4f g''s.\n',...
	  'Accelerometer std  = %.4f %.4f %.4f; deg/sec\n'],...
	  biases, stdBiases); 
  fprintf('Subtracting biases.\n');
  acc_wrld = acc_wrld - ones(length(acc_wrld),1)*biasesE;
  acc_sens = acc_sens - ones(length(acc_sens),1)*biases;
end

figure('Name', 'Acceleration Earth-Fixed frame');
set(gcf,'position',position)
hold on;
plot(time, acc_wrld(:,1), 'r');
plot(time, acc_wrld(:,2), 'g');
plot(time, acc_wrld(:,3), 'b');
str = sprintf( 'Acceleration Earth-Fixed frame; Kp = %5.2e, Ki = %5.2e',Kp, Ki);
title(str);
xlabel('Time (s)');
ylabel('g');
legend('x', 'y', 'z');
hold off;
grid on

figure('Name', 'Acceleration Sensor frame');
set(gcf,'position',position)
hold on;
plot(time, acc_sens(:,1), 'r');
plot(time, acc_sens(:,2), 'g');
plot(time, acc_sens(:,3), 'b');
str = sprintf( 'Acceleration Sensor frame; Kp = %5.2e, Ki = %5.2e',Kp, Ki);
title(str);
xlabel('Time (s)');
ylabel('g');
legend('x', 'y', 'z');
hold off;
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=(cumsum(acc_wrld(:,1)*g))*Ts;
dispX=(cumsum(velocityX)*Ts);
velocityY=cumsum((acc_wrld(:,2)*g))*Ts;
dispY=(cumsum(velocityY)*Ts);
velocityZ=(cumsum(acc_wrld(:,3)*g)*Ts);
dispZ=(cumsum(velocityZ)*Ts);

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

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

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


%% End of script
