2019-03-15

Author: rodralez

F_update.m

@@ -5,24 +5,24 @@
 % -------------------------------------------------------------------------
 %
 % INPUT
-%   imu_sta. Input data structure must contains the following fields:
+%   imu_sta, input data structure must contains the following fields:
 %
 %       fb, Nx3 matrix, accelerations [X Y Z] (m/s^2).
 %       wb, Nx3 matrix, turn rates [X Y Z] (rad/s).
 %       t,  Nx1, time vector (s).
 %
-% - verbose. Verbose level for allan_overlap function.
+%   verbose. Verbose level for allan_overlap function.
+%       0 = silent & no data plots; 1 = status messages; 2 = all messages    
 %
 % OUTPUT
-%   imu. Output data structure with the following fields:
+%   imu, input data structure with the following addtional fields:
 %
 %       arw, 1x3 vector, angle random walk (rad/root-s). Value is taken 
 %       straightfoward from the plot at t = 1 s.
 %       Note: units of rad/s from the plot have to be transformed to 
 %       rad/root-s. This is done by multiplying (rad/s * root-s/root-s) = 
 %       (rad/s * root-s/1) = rad/root-s, since root-s = 1 for tau = 1, time 
-%       at which random walk is evaluated.
-%     
+%       at which random walk is evaluated.     
 %
 %       vrw, 1x3 vector, velocity random walk (m/s/root-s). Value is taken 
 %       straightfoward from the plot at t = 1 s.
@@ -38,46 +38,30 @@
 %       from the plot at the minimun value.
 %
 %       gb_corr, 1x3 vector, gyros correlation times (s).
-%
 %       ab_corr, 1x3 vector, accs correlation times (s).
 %
 %       g_std, 1x3 vector, gyros standard deviations (rad/s).
-%
 %       a_std, 1x3 vector, accs standard deviations (m/s^2).
 %
 %       g_max, 1x3 vector, gyros maximum values (rad/s).
-%
 %       a_max, 1x3 vector, accs maximum values (m/s^2).
 %
 %       g_min, 1x3 vector, gyros minimum values (rad/s).
-%
 %       a_min, 1x3 vector, accs maximum values (m/s^2).
 %
 %       g_mean, 1x3 vector, gyros mean values (rad/s).
-%
 %       a_meam, 1x3 vector, accs mean values (m/s^2).
 %
 %       g_median, 1x3 vector, gyros median values (rad/s).
-%
 %       a_median, 1x3 vector, accs median values (m/s^2).
 %
-%       fb_tau, Mx3 with time vector from AV for accelerometers [X Y Z],
-%       respectively.
-%
-%       fb_allan, Mx3 with AV vector for accelerometers [X Y Z],
-%       respectively.
-%
-%       fb_error, Mx3 with AV errors for accelerometers [X Y Z],
-%       respectively.
-%
-%       wb_tau, Mx3 with time vector from AV for gyros [X Y Z],
-%       respectively.
-%
-%       wb_allan, Mx3 with AV vector for gyros [X Y Z],
-%       respectively.
+%       fb_tau, Mx3 with time vector from AV for accelerometers [X Y Z].
+%       fb_allan, Mx3 with AV vector for accelerometers [X Y Z].
+%       fb_error, Mx3 with AV errors for accelerometers [X Y Z].
 %
-%       wb_error, Mx3 with AV errors for gyros [X Y Z],
-%       respectively.
+%       wb_tau, Mx3 with time vector from AV for gyros [X Y Z].
+%       wb_allan, Mx3 with AV vector for gyros [X Y Z].
+%       wb_error, Mx3 with AV errors for gyros [X Y Z].
 %
 % -------------------------------------------------------------------------
 %
@@ -120,8 +104,8 @@
 %
 % -------------------------------------------------------------------------
 %
-% Version: 006
-% Date:    2018/03/26
+% Version: 007
+% Date:    2019/02/18
 % Author:  Rodrigo Gonzalez <[email protected]>
 % URL:     https://github.com/rodralez/navego
 %
@@ -155,13 +139,15 @@
 imu.a_mean   = zeros(1,3);
 imu.a_median = zeros(1,3);    
 imu.a_outliers = zeros(1,3);    
+imu.a_linear = zeros(3,2);  
 
 imu.g_std    = zeros(1,3);
 imu.g_max    = zeros(1,3);
 imu.g_min    = zeros(1,3);
 imu.g_mean   = zeros(1,3);
 imu.g_median = zeros(1,3);   
 imu.g_outliers = zeros(1,3); 
+imu.g_linear = zeros(3,2);  
 
 %% TIME VECTOR FOR ALLAN VARIANCE
 
@@ -225,6 +211,7 @@
     imu.a_min(i)    = s.min;
     imu.a_median(i) = s.median;
     imu.a_outliers(i) = s.outliers;
+    imu.a_linear(i,:) = s.linear;
 end
 
 % Plot ACCRS
@@ -263,6 +250,7 @@
     imu.g_min(i)    = s.min;
     imu.g_median(i) = s.median;
     imu.g_outliers(i) = s.outliers;
+    imu.g_linear(i,:) = s.linear;
 end
 
 imu.freq = data.rate;

LICENSE

@@ -187,7 +187,7 @@
 if isfield(data,'time')
     if size(data.time,2) > size(data.time,1), data.time=data.time'; end % ensure columns
     s.linear=polyfit(data.time(1:length(data.freq)),data.freq,1);
-elseif isfield(data,'rate') && data.rate ~= 0;
+elseif isfield(data,'rate') && data.rate ~= 0
     s.linear=polyfit((1/data.rate:1/data.rate:length(data.freq)/data.rate)',data.freq,1);
 else
     error('Either "time" or "rate" must be present in DATA. Type "help allan_overlap" for details. [err1]');
@@ -212,8 +212,17 @@
     outliers = data.freq(odl);
     fprintf(1, 'allan_overlap: OUTLIERS: There appear to be %d outliers in the frequency data.\n', length(outliers));       
 
-%     idl = (abs(medianfreq) < 5*MAD);
-%     data.freq = data.freq(idl);
+    % ELIMINATE OUTLIERS FROM DATA
+    fit_line = polyval(s.linear, (1/data.rate:1/data.rate:length(data.freq)/data.rate)') - s.median;    
+    idl = ( medianfreq < (3*MAD + fit_line) );
+    data.freq = data.freq(idl);
+    medianfreq = medianfreq(idl);
+ 
+    fit_line = polyval(s.linear, (1/data.rate:1/data.rate:length(data.freq)/data.rate)') - s.median;
+    idl = ( medianfreq > (-3*MAD + fit_line) );
+    data.freq = data.freq(idl);
+    medianfreq = medianfreq(idl);
+    
     s.outliers = length(outliers);
 
 else 
@@ -478,7 +487,7 @@
 %%%%%%%%
 %% Plotting
 
-if verbose >= 2 % show all data
+if verbose >= 1 % show all data
 
     % plot the frequency data, centered on median
     if size(dtime,2) > size(dtime,1), dtime=dtime'; end % this should not be necessary, but dsplot 1.1 is a little bit brittle
@@ -497,12 +506,18 @@
     fx = xlim;
     % plot([fx(1) fx(2)],[s.median s.median],'-k');
     plot([fx(1) fx(2)],[0 0],':k');
+    
     % show 5x Median Absolute deviation (MAD) values
     hm=plot([fx(1) fx(2)],[5*MAD 5*MAD],'-r');
     plot([fx(1) fx(2)],[-5*MAD -5*MAD],'-r');
+    
     % show linear fit line
     hf=plot(xlim,polyval(s.linear,xlim)-s.median,'-g');    
     title(['Data: ' name],'FontSize',FontSize+2,'FontName','Arial');
+    
+    plot(xlim,polyval(s.linear,xlim)-3*MAD,'--m'); 
+    plot(xlim,polyval(s.linear,xlim)+3*MAD,'--m');
+    
     %set(get(gca,'Title'),'Interpreter','none');
     xlabel('Time [sec]','FontSize',FontSize,'FontName',FontName);
     if isfield(data,'units')
@@ -514,7 +529,6 @@
     legend([hd hm hf],{'data (centered on median)','5x MAD outliers',['Linear Fit (' num2str(s.linear(1),'%g') ')']},'FontSize',max(10,FontSize-2));
     % tighten up
     xlim([dtime(1) dtime(end)]);
-
 
 end % end plot raw data