diff --git a/bagconvert/src/main.cpp b/bagconvert/src/main.cpp index b010026..309e014 100644 --- a/bagconvert/src/main.cpp +++ b/bagconvert/src/main.cpp @@ -83,6 +83,15 @@ int main(int argc, char **argv) { dataIMU.push_back(s1->angular_velocity.y); dataIMU.push_back(s1->angular_velocity.z); } + else if(imuTopic != m.getTopic()) + { + ROS_INFO("Imu topic %s, Does not match any in rosbag: %s\n" + "Please provide the correct topic name," + "qualified by appropriate namespaces\n" + "eg: ///\n", + imuTopic.c_str(),pathBag.c_str()); + exit(EXIT_FAILURE); + } } diff --git a/data/adis16364_accel.jpg b/data/adis16364_accel.jpg new file mode 100644 index 0000000..4d261c9 Binary files /dev/null and b/data/adis16364_accel.jpg differ diff --git a/data/adis16364_gyro.jpg b/data/adis16364_gyro.jpg new file mode 100644 index 0000000..cc809ee Binary files /dev/null and b/data/adis16364_gyro.jpg differ diff --git a/data/allan_accel.fig b/data/allan_accel.fig new file mode 100644 index 0000000..4447a9b Binary files /dev/null and b/data/allan_accel.fig differ diff --git a/data/allan_accel.jpg b/data/allan_accel.jpg new file mode 100644 index 0000000..63814d2 Binary files /dev/null and b/data/allan_accel.jpg differ diff --git a/data/allan_gyro.fig b/data/allan_gyro.fig new file mode 100644 index 0000000..6c370a3 Binary files /dev/null and b/data/allan_gyro.fig differ diff --git a/data/allan_gyro.jpg b/data/allan_gyro.jpg new file mode 100644 index 0000000..cc809ee Binary files /dev/null and b/data/allan_gyro.jpg differ diff --git a/data/results_20200620T184752_accel.png b/data/results_20200620T184752_accel.png new file mode 100644 index 0000000..e757155 Binary files /dev/null and b/data/results_20200620T184752_accel.png differ diff --git a/data/results_20200620T184752_gyro.png b/data/results_20200620T184752_gyro.png new file mode 100644 index 0000000..505354c Binary files /dev/null and b/data/results_20200620T184752_gyro.png differ diff --git a/data/results_20200621T112259_accel.png b/data/results_20200621T112259_accel.png new file mode 100644 index 0000000..d72713c Binary files /dev/null and b/data/results_20200621T112259_accel.png differ diff --git a/data/results_20200621T112259_gyro.png b/data/results_20200621T112259_gyro.png new file mode 100644 index 0000000..6078101 Binary files /dev/null and b/data/results_20200621T112259_gyro.png differ diff --git a/matlab/SCRIPT_allan_matparallel.m b/matlab/SCRIPT_allan_matparallel.m index b124afb..49a2f5d 100644 --- a/matlab/SCRIPT_allan_matparallel.m +++ b/matlab/SCRIPT_allan_matparallel.m @@ -8,7 +8,7 @@ % Our bag information %mat_path = '../data/imu_mtig700.mat'; %mat_path = '../data/imu_tango.mat'; -mat_path = '../data/imu_visensor.mat'; +mat_path = '/home/talha/rosbag_data/imu.mat'; % IMU information (todo: move this to the yaml file) %update_rate = 400; diff --git a/matlab/SCRIPT_process_results.m b/matlab/SCRIPT_process_results.m index 2f9d734..770272d 100644 --- a/matlab/SCRIPT_process_results.m +++ b/matlab/SCRIPT_process_results.m @@ -13,8 +13,8 @@ %titlestr = 'Tango Yellowstone #1'; %mat_path = '../data/bags/results_20171031T115123.mat'; -titlestr = 'ADIS16448 VI-Sensor'; -mat_path = '../data/bags/results_20180206T140217.mat'; +titlestr = 'ADIS16364BMLZ'; +mat_path = '../data/results_20200621T112259.mat'; % Load the mat file (should load "data_imu" matrix) fprintf('=> opening the mat file.\n') @@ -24,7 +24,7 @@ %% Get the calculated sigmas fprintf('=> plotting accelerometer.\n') -[fh1,sigma_a,sigma_ba] = gen_chart(results_ax.tau1,results_ax.sig2,... +[fh1,sigma_a,sigma_ba] = gen_chart1(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)'); diff --git a/matlab/functions/gen_chart1.m b/matlab/functions/gen_chart1.m new file mode 100644 index 0000000..2d53d95 --- /dev/null +++ b/matlab/functions/gen_chart1.m @@ -0,0 +1,91 @@ +function [fh1, sigmaw, sigmab] = gen_chart1(tau,sigx,sigy,sigz,titlestr,name,unit0,unit1,unit2) + +% Calculate our average sigma values +sigavg = mean([sigx;sigy;sigz]); + +% ======================================================================= +% Plot the results on a figure +mfh1 = figure; +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]'); +ylabel(['Normal Allan Deviation [',unit0,']']); +legend(['x-',name],['y-',name],['z-',name],'average','Location','southeast'); + +% ======================================================================= +% Find location of tau=1 by finding where difference is near zero +tauref = 1; +taudiff = abs(tau-tauref); +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; +%window = tauid1-windowsize:tauid1+windowsize; +minid = find(sigavg == min(sigavg)); % find where the min is +window = 1:minid-windowsize; % go from start to min + +% Get our x and y values +x = tau(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)); + +% Convert from logarithmic scale to linear scale and plot +h_fit1 = tau.^coeffs(1).*exp(coeffs(2)); +pltlin = plot(tau, h_fit1,'Color','r','LineWidth',2); hold on; +pltlin.Color(4) = 0.5; % make it semi-transparent + + +% ======================================================================= +% Next we should fit a 1/2 line to the bias side of the allan plot +minid = 1200; % find where the min is +window = minid+windowsize:1870; % go from min to the end + +% Get our x and y values +x = tau(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)); + +% Convert from logarithmic scale to linear scale and plot +h_fit2 = tau.^coeffs(1).*exp(coeffs(2)); +pltlin = plot(tau, h_fit2,'Color','b','LineWidth',2); hold on; +pltlin.Color(4) = 0.5; % make it semi-transparent + + +% Get what our bias should be (should be at a tau of 3) +tauref = 3; +taudiff = abs(tau-tauref); +tauid2 = find(taudiff == min(taudiff),1); +fprintf('tau = %.2f | tauid2 = %d\n',tau(tauid2),tauid2); + + +% ======================================================================= +% Record the values for the output +sigmaw = h_fit1(tauid1); +sigmab = h_fit2(tauid2); + +% Plot the values +str1 = sprintf('\\sigma = %.6f %s',h_fit1(tauid1),unit1); +str2 = sprintf('\\sigma_{b} = %.6f %s',h_fit2(tauid2),unit2); +text(.1,.15,{str1,str2},'Units','normalized'); %'FontWeight','bold' + + +end +