function  [Freq_half_analog,Peri_half_analog_dB]=accelPSD(Acceleration,Time);

%
% Estimate the Power Spectrum Density (PSD) of 
% accelerometer measurements on the docking AUV.
% Use this program after using the 'accel' function. Acceleration values
% must already be loaded in the directy.
%
% Yanwu Zhang
%
% July 2006
%
% Adapted into a function by Francois Cazenave,july 17, 2006.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


lendata = length(Acceleration);

% Data is divided in chunks to be processed. len_fft is the length of each
% chunk.
% Define the length of 
len_fft = 256;
Peri = zeros(1, len_fft);
cnt = 1;

Difftime = diff(Time);
deltat = Difftime(1);
% Calculate the sampling frequency

freq_sampling = 1 / deltat; % In Hz.

%
Indjump = find(Difftime > (2.0*deltat));
numgrps = length(Indjump) + 1;
%
for indgrp = 1 : numgrps
    if (indgrp == 1)
        Indstart(indgrp) = 1;
    else
        Indstart(indgrp) = Indjump(indgrp-1) + 1;
    end
%        
    if (indgrp == numgrps)
        Indend(indgrp) = lendata;
    else
        Indend(indgrp) = Indjump(indgrp);
    end
end
%
Lenseg = Indend - Indstart + 1; % lenseg is the length of each segment
Indsegpsd = find(Lenseg >= len_fft);
numsegs_psd = length(Indsegpsd);
%
for indseg = 1 : numsegs_psd
    indtmp = Indsegpsd(indseg);
    ind_start = Indstart(indtmp);
    ind_end = Indend(indtmp);
    Numfftsegs(indseg) = floor(Lenseg(indtmp)/len_fft);
% Find the begining and ending of each group.       
    for indfftseg = 1 : Numfftsegs(indseg)
        % Begining of the segment (group)
        ind1 = ind_start + (indfftseg-1)*len_fft;
        % Ending of the segment (group)
        ind2 = ind_start + indfftseg*len_fft - 1;
        % Remove the DC part
        Vectmp(cnt, :) = Acceleration(ind1:ind2) - mean(Acceleration(ind1:ind2));
        Peri_tmp = ((abs(fft(Vectmp(cnt,:), len_fft))).^2) / len_fft;
        % Calculation checks
        Pwr_freqdomain(cnt) = (sum(Peri_tmp)) * (1.0/len_fft);
        Pwr_timedomain(cnt) = (sum((Vectmp(cnt,:)).^2)) / len_fft;
        Peri = Peri + Peri_tmp;
        cnt = cnt + 1;
    end
%
end
% Compute the average
Peri = Peri / sum(Numfftsegs);
% Remove negative frequency
Peri_half_digital = Peri(1 : ((len_fft/2)+1));
Peri_half_analog = Peri_half_digital * deltat;
Peri_half_analog_dB = 10.0*log10(Peri_half_analog);
%
Vec_k_half = [0 : len_fft/2]; % Symetric about len_fft/2.
Freq_half_digital = Vec_k_half/len_fft;
Freq_half_analog = Freq_half_digital * freq_sampling; % In Hz.

% Calculation checks
pwr_freq_digital = (sum(Peri_half_digital)) * (1.0/len_fft) * 2.0;
pwr_time_digital = mean(Pwr_timedomain);
pwr_freq_analog = sum(Peri_half_analog) * ((1.0/len_fft)*freq_sampling) * 2.0;
% Checked that pwr_time = pwr_freq
%%
figure; orient landscape;
plot(Freq_half_analog, Peri_half_analog_dB); hold on;
set(gca, 'xlim', [0 max(Freq_half_analog)]);
set(gca, 'ylim', [max(Peri_half_analog_dB)-40.0 max(Peri_half_analog_dB)]);
xlabel('Frequency (Hz)', 'fontsize', 14);
ylabel('10log_{10}((m/s^2)^2/Hz)', 'fontsize', 14);
title('Estimated PSD of Acceleration measurements', 'fontsize', 14);

%%
