Fixed - Double sqrt error (should fix #10)

ReadMe.md

@@ -25,19 +25,30 @@ Accelerometer "random walk" | `accelerometer_random_walk` | <img src="https://la
     * Use the included `bagconvert` ROS package to do this
     * Example: `rosrun bagconvert bagconvert imu.bag /imu0`
 3. Run the included matlab scripts to generate an allan deviation plot for the readings
-    * We have multiple versions of the script
-    * We do *not* recommend the ROS version as matlab parsing of raw ROS bags is slow
-    * If using ROS version, it uses the matlab robotics toolbox
     * If using the parallel version, it uses the matlab parallel toolbox
+    * Need to specify the mat file that the bagconverter made, and the rate of IMU messages
 4. Interpret the generated charts to find noise values
     * Run the process results script
     * Will fit a -1/2 line to the left side of the allan plot
     * White noise is at tau=1 (according to [kalibr wiki](https://github.com/ethz-asl/kalibr/wiki/IMU-Noise-Model#from-the-allan-standard-deviation-ad))
     * Will fit a 1/2 line to the right side of the allan plot
     * Random walk is at tau=3 (according to [kalibr wiki](https://github.com/ethz-asl/kalibr/wiki/IMU-Noise-Model#from-the-allan-standard-deviation-ad))
+5. Some example data can be found this **[this](https://drive.google.com/drive/folders/1a3Es85JDKl7tSpVWEUZryOwtsXB8793o?usp=sharing)**
+    * XSENS MTI-G-700
+    * Tango Yellowstone Tablet
+    * ASL-ETH VI-Sensor
 
+### Example Plot - XSENS MTI-G-700
+![allan chart acceleration](data/results_20170908T182715_accel.png)
 
-## Example Plots (xsens mti-G-700)
+![allan chart angular velocity](data/results_20170908T182715_gyro.png)
+
+### Example Plot - Tango Yellowstone Tablet
 ![allan chart acceleration](data/results_20171031T115123_accel.png)
 
 ![allan chart angular velocity](data/results_20171031T115123_gyro.png)
+
+### Example Plot - ASL-ETH VI-Sensor
+![allan chart acceleration](data/results_20180206T140217_accel.png)
+
+![allan chart angular velocity](data/results_20180206T140217_gyro.png)

matlab/SCRIPT_allan_matparallel.m

@@ -7,11 +7,13 @@
 
 % Our bag information
 %mat_path = '../data/imu_mtig700.mat';
-mat_path = '../data/imu_tango.mat';
+%mat_path = '../data/imu_tango.mat';
+mat_path = '../data/imu_visensor.mat';
 
 % IMU information (todo: move this to the yaml file)
 %update_rate = 400;
-update_rate = 100;
+%update_rate = 100;
+update_rate = 200;
 
 
 %% Data processing
@@ -99,7 +101,6 @@
 %% Save workspace
 filename = ['results_',datestr(now,30),'.mat'];
 fprintf('saving to: %s\n',filename);
-%save(['../data/',filename])
 save(['../data/',filename],'update_rate','ts_imua','ts_imuw','tau','taumax','results_ax','results_ay','results_az','results_wx','results_wy','results_wz')
 fprintf('done saving!\n');
 

matlab/SCRIPT_process_results.m

@@ -7,28 +7,30 @@
 addpath('functions/allan_v3')
 
 % Our bag information
-titlestr = 'XSENS MTi-G-710';
-mat_path = '../data/results_20170908T182715.mat';
+%titlestr = 'XSENS MTi-G-710';
+%mat_path = '../data/bags/results_20170908T182715.mat';
 
 %titlestr = 'Tango Yellowstone #1';
-%mat_path = '../data/results_20171031T115123.mat';
+%mat_path = '../data/bags/results_20171031T115123.mat';
 
+titlestr = 'ADIS16448 VI-Sensor';
+mat_path = '../data/bags/results_20180206T140217.mat';
 
 % Load the mat file (should load "data_imu" matrix)
-fprintf('opening the mat file.\n')
+fprintf('=> opening the mat file.\n')
 load(mat_path);
 
 
 %% Get the calculated sigmas
 
-fprintf('plotting accelerometer.\n')
+fprintf('=> plotting accelerometer.\n')
 [fh1,sigma_a,sigma_ba] = gen_chart(results_ax.tau1,results_ax.sig2,...
                                     results_ay.sig2,results_az.sig2,...
                                     titlestr,'acceleration','m/s^2',...
                                     'm/s^2sqrt(Hz)','m/s^3sqrt(Hz)');
 
 
-fprintf('plotting gyroscope.\n')
+fprintf('=> plotting gyroscope.\n')
 [fh2,sigma_g,sigma_ga] = gen_chart(results_wx.tau1,results_wx.sig2,...
                                     results_wy.sig2,results_wz.sig2,...
                                     titlestr,'gyroscope','rad/s',...
@@ -37,6 +39,7 @@
 
 
 %% Print out for easy copying
+fprintf('=> final results\n');
 % Accelerometer
 fprintf('accelerometer_noise_density = %.8f\n',sigma_a);
 fprintf('accelerometer_random_walk   = %.8f\n',sigma_ba);

matlab/functions/gen_chart.m

@@ -6,10 +6,10 @@
 % =======================================================================
 % Plot the results on a figure
 fh1 = figure;
-loglog(tau, sqrt(sigx)); hold on;
-loglog(tau, sqrt(sigy)); hold on;
-loglog(tau, sqrt(sigz)); hold on;
-loglog(tau, sqrt(sigavg)); hold on;
+loglog(tau, sigx); hold on;
+loglog(tau, sigy); hold on;
+loglog(tau, sigz); hold on;
+loglog(tau, sigavg); hold on;
 grid on;
 title([titlestr,' ',name]);
 xlabel('\tau [sec]');
@@ -20,26 +20,26 @@
 % Find location of tau=1 by finding where difference is near zero
 tauref = 1;
 taudiff = abs(tau-tauref);
-tauid1 = find(taudiff == min(taudiff));
+tauid1 = find(taudiff == min(taudiff),1);
 fprintf('tau = %.2f | tauid1 = %d\n',tau(tauid1),tauid1);
 
 % We will fit our "white-noise" line where tau is 1
-%windowsize = 50;
+windowsize = 50;
 %window = tauid1-windowsize:tauid1+windowsize;
 minid = find(sigavg == min(sigavg)); % find where the min is
-window = 1:minid; % go from start to min
+window = 1:minid-windowsize; % go from start to min
 
 % Get our x and y values
 x = tau(window);
-y = sqrt(sigavg(window));
+y = sigavg(window);
 nanx = isfinite(y);
 
 % Fit to log-log scale (slope of -1/2)
 %coeffs = polyfit(log(x(nanx)), log(y(nanx)), 1);
 coeffs(1)= -0.5;
 intcs = log(y(nanx)./x(nanx).^coeffs(1));
 coeffs(2) = mean(intcs);
-%fprintf('h_fit1 slope = %.4f | y-intercept = %.4f\n',coeffs(1),coeffs(2));
+fprintf('h_fit1 slope = %.4f | y-intercept = %.4f\n',coeffs(1),coeffs(2));
 
 % Convert from logarithmic scale to linear scale and plot
 h_fit1 = tau.^coeffs(1).*exp(coeffs(2));
@@ -50,18 +50,18 @@
 % =======================================================================
 % Next we should fit a 1/2 line to the bias side of the allan plot
 minid = find(sigavg == min(sigavg)); % find where the min is
-window = minid:length(tau); % go from min to the end
+window = minid+windowsize:length(tau); % go from min to the end
 
 % Get our x and y values
 x = tau(window);
-y = sqrt(sigavg(window));
+y = sigavg(window);
 nanx = isfinite(y);
 
 % Calculate the intercept given the slope of +1/2
 coeffs(1)= 0.5;
 intcs = log(y(nanx)./x(nanx).^coeffs(1));
 coeffs(2) = mean(intcs);
-%fprintf('h_fit2 slope = %.4f | y-intercept = %.4f\n',0.5,mean(intcs));
+fprintf('h_fit2 slope = %.4f | y-intercept = %.4f\n',0.5,mean(intcs));
 
 % Convert from logarithmic scale to linear scale and plot
 h_fit2 = tau.^coeffs(1).*exp(coeffs(2));
@@ -72,7 +72,7 @@
 % Get what our bias should be (should be at a tau of 3)
 tauref = 3;
 taudiff = abs(tau-tauref);
-tauid2 = find(taudiff == min(taudiff));
+tauid2 = find(taudiff == min(taudiff),1);
 fprintf('tau = %.2f | tauid2 = %d\n',tau(tauid2),tauid2);