% function microcart_imu_analysis_1

close all;  clc; 
filename = 'imu.csv';
delimiter = ',';
formatSpec = '%f%f%f%f%f%f%f%[^\n\r]';
fileID = fopen(filename,'r');
dataArray = textscan(fileID, formatSpec, 'Delimiter', delimiter, 'EmptyValue' ,NaN, 'ReturnOnError', false);
fclose(fileID);
t = dataArray{:, 1};
ax = dataArray{:, 2};
ay = dataArray{:, 3};
az = dataArray{:, 4};
gx = dataArray{:, 5};
gy = dataArray{:, 6};
gz = dataArray{:, 7};
clearvars filename delimiter formatSpec fileID dataArray ans;

% magnitude of acceleration in gs 
a = ( ax.^2 + ay.^2 + az.^2 ).^0.5 ; 

% magnitude of deviation from ideal 
ea1 = 1 - a ;

% magnitude of deviation from mean 
ea2 = a - mean(a) ; 

% RMS error 
ea1rms = sqrt(1/length(ea1)*(ea1')*ea1) ; 
ea2rms = sqrt(1/length(ea2)*(ea2')*ea2) ; 

subplot(3,1,1); 
plot(t, a , t , ones(length(t),1)); axis([min(t),max(t),min([a;1])-0.01,max([a;1])+0.01]); grid;
xlabel('Time (s)'); ylabel('Acceleration (gs)'); 
legend('Measured','Ideal'); 


subplot(3,1,2); 
plot(t, ea1 ); axis([min(t),max(t),min(ea1)-0.01,max(ea1)+0.01]); grid;
xlabel('Time (s)'); ylabel('Error From Ideal (gs)'); 

subplot(3,1,3); 
plot(t, ea2 ); axis([min(t),max(t),min(ea2)-0.01,max(ea2)+0.01]); grid;
xlabel('Time (s)'); ylabel('Error From Mean (gs)'); 

% 
figure; 
X = fft(ea2); 
N = length(ea2); 
fs = 1/(t(2)-t(1)); 
F = ([-N/2:N/2-1]/N)*fs; %frequency axis for plotting FFTs 
% subplot(2,1,1); 
plot(F,fftshift(abs(X))); grid; 
xlabel('frequency (Hz)');
ylabel(['|FFT(x)|']);
% subplot(2,1,2); 
% plot(F,20*log10(fftshift(abs(X)))); grid; 
% xlabel('frequency (Hz)');
% ylabel('|FFT(x)| dB');
suptitle('x = Error From Mean (gs)'); 


eax = ax - mean(ax); 

vx = var(ax); 
sigmax = sqrt(vx);  

figure; 

bins = unique(eax) ; 
dbins = bins(2)-bins(1); % the quantization levels consistent 

bins_right = bins + dbins/2; 
bins_left = bins - dbins/2; 
cgauss_right = normcdf(bins_right,0,sigmax); 
cgauss_left = normcdf(bins_left,0,sigmax); 

empf = length(eax)*(cgauss_right-cgauss_left); 
stairs(bins_left,empf,'r','LineWidth',1) ; 
xlabel('Error From Mean (gs)'); 
ylabel('Occurences'); 

grid; 
hold on; 
h = hist(eax,bins); 
stem(bins,h); 

legend(['Quantized Gaussian Noise: N(0,',num2str(vx),')'],'Emperical Distribution')


% quantized_accel_noise_gauss_anal( ax )



% end