
%
% Simple script to look at the performance of the Vector IMU. All
% scaling assumes the BD935-INS sensors. The script is only setup for
% static data!
%
% Needs a viewdat output:
%
% viewdat -d35:3 -mb -oIMU.txt YourFile.T04
%
%
%load IMU.txt

% When 1 the allan variance code plots extra graphs and provides
% additional data to the terminal
verbose = 0;

% When 1 plots the PSD ... still needs some work
plotPSD = 0;

accelFig = figure();
gyroFig  = figure();
accelTimeSeriesFig = figure();
gyroTimeSeriesFig  = figure();

clear err;
clear adev;

figure
plot((2:length(IMU))*0.005/3600,diff(IMU(:,4)))
xlabel('Time [Hours]');
ylabel('Time Tag Delta [secs]');
grid
savePlot('SensorTimeDelta',0);



for type=5:10

  if( (type >= 5) & (type <= 7) )
    Str = 'Gyro';
    tmp = type - 5;
    figure(gyroTimeSeriesFig)
    subplot(3,1,tmp+1)
    SF = 50; % To convert to degrees/sec
    TimeSeries = (1/SF) * IMU(:,type);
    plot((1:length(IMU))*0.005/3600,TimeSeries)
    ylabel('deg/sec');
    SF = 50/3600; % To convert to degrees/hour
    units = 'deg/hr';
  else
    Str = 'Accel';
    if( (IMU(1,1) == 76) | (IMU(1,1) == 78) )
      % SCA3300
      SF = 1350/1e6; % To convert to micro-g
    else
      SF = 1962/1e6; % To convert to micro-g
    end
    tmp = type - 8;
    units = '\mug';
    figure(accelTimeSeriesFig)
    subplot(3,1,tmp+1)
    TimeSeries = (1/SF) * IMU(:,type);
    plot((1:length(IMU))*0.005/3600,TimeSeries)
    ylabel(units)
  end

  sig = std(TimeSeries);
  mu = mean(TimeSeries);

  if(tmp == 0)
    AxisStr = 'X';
    color = 'b';
  elseif(tmp == 1)
    AxisStr = 'Y';
    color = 'r';
  else
    AxisStr = 'Z';
    color = 'g';
  end

  grid
  if( (type >= 5) & (type <= 7) )
    title([AxisStr,' ',Str,' Time Series: \mu=',num2str(mu,'%.3f'),' deg/sec \sigma=',num2str(sig,'%.3f'),' deg/sec']);
    disp([AxisStr,' ',Str,' Bias= ',num2str(mu,'%.3f'),' deg/sec']);
    disp(['Cal File ',AxisStr,' ',Str,' Bias= ',num2str(mu*3600,'%.0f'),' deg/hour']);
  else
    title([AxisStr,' ',Str,' Time Series: \mu=',num2str(mu,'%.3f'),' \mug \sigma=',num2str(sig,'%.3f'),' \mug']);

    % We expect gravity in the Z-axis accel
    if(type == 10)
      mu = mu + 1e6;
    elseif(type == 8)
      disp('****************************************************************');
    end
    disp([AxisStr,' ',Str,' Bias= ',num2str(mu,'%.3f'),' micro g']);
    disp(['Cal File ',AxisStr,' ',Str,' Bias= ',num2str(mu,'%.0f'),' micro g']);
  end

  if(tmp == 2)
    xlabel('Time [Hours]');
    if(type == 7)
      savePlot('GyroTimeSeries',0);
    else
      savePlot('AccelTimeSeries',0);
    end
  end


  % Remove the DC bias
  x = (1/SF) * (IMU(:,type) - mean(IMU(:,type)));

  if(plotPSD == 1)
    NFFT=4096*256;
    L=length(IMU);	 	 
    X=fftshift(fft(x,NFFT));	 	 
    PSD=X.*conj(X)/(NFFT*L); 

    % 200Hz sampled data
    figure
    freq=200*(-NFFT/2:NFFT/2-1)/NFFT;	 	 
    plot(freq,10*log10(PSD))
    title([AxisStr,' ',Str,' - PSD']);
    xlabel('Freq [Hz]')	 	 
    ylabel('Power Spectral Density [dB]');
    grid
  end

  clear data;
  data.freq = x;
  data.time = (0:0.005:((length(IMU)-1)/200))';
  data.rate = 200;
  tau = [1/8 1/4 1/2 1 2 4 8 16 32 64 128 256 512 1024 2048 4096];
  name = [AxisStr,' ',Str];
  if(verbose)
    %% Verbose output - outputs to terminal and also extra figures
    [retval, s, errorb, tau] = allan(data,tau,name,2); 
    ylabel(['Allan \sigma [',units,']']);
  else
    [retval, s, errorb, tau] = allan(data,tau,name,0);
  end

  % Save the error bars for later so we can add to the combined plot
  % after we have added the legend
  err(type-4,:) = errorb;
  adev(type-4,:) = retval;

  if(type<=7)
    figure(gyroFig);
  else
    figure(accelFig);
  end

  loglog(tau,retval,'.-','color',color,'markersize',20,'linewidth',3)
  hold on
  grid on
  xlabel('Time [Sec]')

end


figure(gyroFig);

disp(' ');
disp(' ');
disp('****************************************************************');
tau1SecIndex = find(tau == 1);
disp('Gyro')
for i=1:3
  disp(['Bias Stability = ',num2str(min(adev(i,:)),'%.2f'),' degress per hour @ ',num2str(tau(find(adev(i,:) == min(adev(i,:))))),'s'])
  % Divide the 1 second Allan variance by 60 when the data is in
  % degrees per hour to get the ARW in degrees/sqrt(hour).
  ARW(i) = adev(i,tau1SecIndex) / 60; % deg/sqrt(hr)
  disp(['Angular Random Walk = ',num2str(adev(i,tau1SecIndex) / 60,'%.2f'),' degrees / sqrt(hour)'])
  disp(['Angular Random Walk = ',num2str(adev(i,tau1SecIndex) / 3600,'%.5f'),' degrees / s / sqrt(Hz)'])
  disp(['Angular Random Walk = ',num2str(adev(i,tau1SecIndex) ,'%.2f'),' degrees / hr / sqrt(Hz)'])

  % Angular Error due to random walk = ARW in (deg/sqrt(hour)) * sqrt(Secs/3600)
end

plot([tau; tau],[adev(1,:)+err(1,:); adev(1,:)-err(1,:)],'-k','linewidth',2)
plot([tau; tau],[adev(2,:)+err(2,:); adev(2,:)-err(2,:)],'-k','linewidth',2)
plot([tau; tau],[adev(3,:)+err(3,:); adev(3,:)-err(3,:)],'-k','linewidth',2)
ylabel('\sigma [degrees/hour]');
legend(['X - ',num2str(ARW(1),'%.2f'),' deg/\surd(hour)'], ...
       ['Y - ',num2str(ARW(2),'%.2f'),' deg/\surd(hour)'], ...
       ['Z - ',num2str(ARW(3),'%.2f'),' deg/\surd(hour)'], ...
       'location','best');
title('Gyro Allan Deviation');
savePlot('GyroAllanDeviation',0);



figure(accelFig);
disp(' ')
disp('****************************************************************');
disp(' ')
  
disp('Accelerometer')
for i=4:6
  VRW(i-3) = adev(i,tau1SecIndex); % uq/sqrt(Hz)
  disp(['Bias Stability = ',num2str(min(adev(i,:)),'%.2f'),' micro g @ ',num2str(tau(find(adev(i,:) == min(adev(i,:))))),'s'])
  disp(['Velocity Random Walk = ',num2str(adev(i,tau1SecIndex),'%.2f'),' micro g / sqrt(Hz)'])
end
disp('****************************************************************');
disp(' ');
disp(' ');
% \surd is the latex symbol for sqrt
legend(['X - ',num2str(VRW(1),'%.2f'),' \mug/\surd(Hz)'], ...
       ['Y - ',num2str(VRW(2),'%.2f'),' \mug/\surd(Hz)'], ...
       ['Z - ',num2str(VRW(3),'%.2f'),' \mug/\surd(Hz)'], ...
       'location','best');
plot([tau; tau],[adev(4,:)+err(4,:); adev(4,:)-err(4,:)],'-k','linewidth',2)
plot([tau; tau],[adev(5,:)+err(5,:); adev(5,:)-err(5,:)],'-k','linewidth',2)
plot([tau; tau],[adev(6,:)+err(6,:); adev(6,:)-err(6,:)],'-k','linewidth',2)
ylabel('\sigma [\mug]');
title('Accelerometer Allan Deviation');
savePlot('AccelAllanDeviation',0);

  


