added new planetary dataset & ins_visual/ins_visual_gnss loosely-coup…

…led fusion

examples/planetary-data/canada-planetary-data/scripts/Optimizer.m

@@ -0,0 +1,47 @@
+% Johann Diep ([email protected]) - November 2021
+%
+% This script must be adapted each time an optimization needs to be run.
+%
+% It is noted that this containts rewriting the imu_structure.m script such
+% that it accept parameter input. This functionality was removed after the
+% optimal parameters were found.
+
+clear; clc;
+
+%% Optimization
+
+Index_p1 = 1;
+for p_1 = 1
+    Index_p2 = 1;
+    for p_2 = 1
+        try
+            imu_structure(p_1,p_2);
+
+            load('gnss_planetary.mat'); load('imu_planetary.mat'); load('gnss_planetary_r.mat'); load('visual_planetary.mat');
+            switch FusionCase
+                case 'inertial_gnss'
+                    nav_e = ins_gnss(imu_planetary,gnss_planetary,'dcm');
+                case 'inertial_visual'
+                    nav_e = ins_visual(imu_planetary,gnss_planetary_r,visual_planetary,'dcm');
+                case 'inertial_visual_gnss'
+                    nav_e = ins_visual_gnss(imu_planetary,gnss_planetary,visual_planetary,'dcm');
+            end
+            [nav_i,gnss_planetary_r] = navego_interpolation (nav_e, gnss_planetary_r);
+
+            %% Optimization
+
+            % Position Errors
+            [RN,RE]  = radius(nav_i.lat);
+            LAT2M = RN + nav_i.h;
+            LON2M = (RE + nav_i.h).*cos(nav_i.lat);
+
+            ErrorValue(Index_p1,Index_p2) = sqrt(rms(LAT2M.*(nav_i.lat - gnss_planetary_r.lat))^2 + rms(LON2M.*(nav_i.lon - gnss_planetary_r.lon))^2);
+            ParameterValue(Index_p1,Index_p2,1) = p_1;
+            ParameterValue(Index_p1,Index_p2,2) = p_2;
+        catch
+            disp('An error occured in the estimation.');
+        end
+        Index_p2 = Index_p2 + 1;
+    end
+    Index_p1 = Index_p1 + 1;
+end

examples/planetary-data/canada-planetary-data/scripts/RunFusion.m

@@ -0,0 +1,192 @@
+% Johann Diep ([email protected]) - November 2021
+%
+% This script runs the fusion of all sensor parameters. 
+
+clear; clc; close all;
+
+FusionCase = "inertial_visual_gnss";
+Sparse = "true";
+
+%% Generating Data
+
+if FusionCase == "inertial_gnss"
+    imu_structure;
+    if Sparse == "true"
+        gnss_sparse_structure;
+    else
+        gnss_structure;
+    end
+else
+    imu_structure;
+    visual_structure;
+    if Sparse == "true"
+        gnss_sparse_structure;
+    else
+        gnss_structure;
+    end
+end
+
+%% Estimation
+
+switch FusionCase
+    case "inertial_gnss"
+        nav_e = ins_gnss(imu_planetary,gnss_planetary,'dcm');
+    case "inertial_visual"
+        nav_e = ins_visual(imu_planetary,gnss_planetary_r,visual_planetary,'dcm');
+    case "inertial_visual_gnss"
+        nav_e = ins_visual_gnss(imu_planetary,gnss_planetary,visual_planetary,'dcm');
+end
+
+[nav_i,gnss_planetary_r] = navego_interpolation (nav_e, gnss_planetary_r);
+
+if Sparse == "true"
+    [gnss_i,gnss_planetary_r_sparse] = navego_interpolation(gnss_planetary,gnss_planetary_r_sparse);
+else
+    [gnss_i,gnss_planetary_r] = navego_interpolation(gnss_planetary,gnss_planetary_r);
+
+end
+
+%% Plotting
+
+switch FusionCase 
+    %% Plotting: IMU + GNSS
+    case "inertial_gnss"
+
+    % Position
+    figure();
+    hold on;
+    plot(rad2deg(gnss_planetary.lon),rad2deg(gnss_planetary.lat),'.', 'Color', ones(1,3) * 0.75, 'LineWidth', 1.5);
+    scatter(rad2deg(gnss_planetary_r.lon),rad2deg(gnss_planetary_r.lat),'y.','MarkerEdgeAlpha',.6);
+    plot(rad2deg(nav_e.lon),rad2deg(nav_e.lat),'Color', [0, 0, 0], 'LineWidth', 1.5);
+    grid on;
+    xlabel('Longitude [deg]');
+    ylabel('Latitude [deg]');
+    legend('degraded GNSS','RTK','IMU + degraded GNSS','Location','Southeast');
+    axis equal;
+
+    % Position Errors
+    [RN,RE]  = radius(nav_i.lat);
+    LAT2M = RN + nav_i.h;
+    LON2M = (RE + nav_i.h).*cos(nav_i.lat);
+
+    [RN,RE]  = radius(gnss_i.lat);
+    LAT2M_GR = RN + gnss_i.h;
+    LON2M_GR = (RE + gnss_i.h).*cos(gnss_i.lat);
+
+    figure();
+    subplot(2,1,1);
+    hold on;
+    plot(gnss_i.t,  LAT2M_GR.*(gnss_i.lat - gnss_planetary_r.lat), '.', 'Color', ones(1,3) * 0.75, 'LineWidth', 1.5)
+    plot(nav_i.t, LAT2M.*(nav_i.lat - gnss_planetary_r.lat),'Color', [0, 0, 0], 'LineWidth', 1.5)
+    grid on;
+    xlabel('Time [s]')
+    ylabel('[m]')
+    legend('GNSS', 'IMU + degraded GNSS', 'Location', 'northoutside');
+    title('Latitude Error');
+    xlim([0,max(gnss_planetary.t)]);
+
+    subplot(2,1,2);
+    hold on;
+    plot(gnss_i.t, LON2M_GR.*(gnss_i.lon - gnss_planetary_r.lon), '.', 'Color', ones(1,3) * 0.75, 'LineWidth', 1.5)
+    plot(nav_i.t, LON2M.*(nav_i.lon - gnss_planetary_r.lon),'Color', [0, 0, 0], 'LineWidth', 1.5)
+    grid on;
+    xlabel('Time [s]')
+    ylabel('[m]')
+    legend('GNSS', 'IMU + degraded GNSS', 'Location', 'northoutside');
+    title('Longitude Error');
+    xlim([0,max(gnss_planetary.t)]);
+
+    %% Plotting: Vision + IMU
+    case "inertial_visual"
+
+    % Position
+    figure();
+    hold on;
+    plot(rad2deg(visual_planetary.lon),rad2deg(visual_planetary.lat),'.', 'Color', ones(1,3) * 0.75, 'LineWidth', 1.5);
+    scatter(rad2deg(gnss_planetary_r.lon),rad2deg(gnss_planetary_r.lat),'y.','MarkerEdgeAlpha',.6);
+    plot(rad2deg(nav_e.lon),rad2deg(nav_e.lat),'Color', [0, 0, 0], 'LineWidth', 1.5);
+    grid on;
+    xlabel('Longitude');
+    ylabel('Latitude');
+    legend('OpenVINS','RTK','IMU + OpenVINS','Location','Southeast');
+    axis equal;
+
+    % Position Errors
+    [RN,RE]  = radius(nav_i.lat);
+    LAT2M = RN + nav_i.h;
+    LON2M = (RE + nav_i.h).*cos(nav_i.lat);
+
+    [RN,RE]  = radius(gnss_i.lat);
+    LAT2M_GR = RN + gnss_i.h;
+    LON2M_GR = (RE + gnss_i.h).*cos(gnss_i.lat);
+
+    figure();
+    subplot(2,1,1);
+    hold on;
+    plot(gnss_i.t,  LAT2M_GR.*(gnss_i.lat - gnss_planetary_r.lat), '.', 'Color', ones(1,3) * 0.75, 'LineWidth', 1.5)
+    plot(nav_i.t, LAT2M.*(nav_i.lat - gnss_planetary_r.lat),'Color', [0, 0, 0], 'LineWidth', 1.5)
+    grid on;
+    xlabel('Time [s]')
+    ylabel('[m]')
+    legend('GNSS', 'IMU + OpenVINS', 'Location', 'northoutside');
+    title('Latitude Error');
+    xlim([0,max(gnss_planetary.t)]);
+
+    subplot(2,1,2);
+    hold on;
+    plot(gnss_i.t, LON2M_GR.*(gnss_i.lon - gnss_planetary_r.lon), '.', 'Color', ones(1,3) * 0.75, 'LineWidth', 1.5)
+    plot(nav_i.t, LON2M.*(nav_i.lon - gnss_planetary_r.lon),'Color', [0, 0, 0], 'LineWidth', 1.5)
+    grid on;
+    xlabel('Time [s]')
+    ylabel('[m]')
+    legend('GNSS', 'IMU + OpenVINS', 'Location', 'northoutside');
+    title('Longitude Error');
+    xlim([0,max(gnss_planetary.t)]);
+
+    %% Plotting: Vision + GNSS + INS
+    case "inertial_visual_gnss"
+
+    % Position
+    figure();
+    hold on;
+    plot(rad2deg(gnss_planetary.lon),rad2deg(gnss_planetary.lat),'.', 'Color', ones(1,3) * 0.75, 'LineWidth', 1.5);
+    scatter(rad2deg(gnss_planetary_r.lon),rad2deg(gnss_planetary_r.lat),'y.','MarkerEdgeAlpha',.6);
+    plot(rad2deg(nav_e.lon),rad2deg(nav_e.lat),'Color', [0, 0, 0], 'LineWidth', 1.5);
+    grid on;
+    xlabel('Longitude [deg]');
+    ylabel('Latitude [deg]');
+    legend('degraded GNSS','RTK','IMU + degraded GNSS + OpenVINS','Location','Southeast');
+    axis equal;
+
+    % Position Errors
+    [RN,RE]  = radius(nav_i.lat);
+    LAT2M = RN + nav_i.h;
+    LON2M = (RE + nav_i.h).*cos(nav_i.lat);
+
+    [RN,RE]  = radius(gnss_i.lat);
+    LAT2M_GR = RN + gnss_i.h;
+    LON2M_GR = (RE + gnss_i.h).*cos(gnss_i.lat);
+
+    figure();
+    subplot(2,1,1);
+    hold on;
+    plot(gnss_i.t,  LAT2M_GR.*(gnss_i.lat - gnss_planetary_r.lat), '.', 'Color', ones(1,3) * 0.75, 'LineWidth', 1.5)
+    plot(nav_i.t, LAT2M.*(nav_i.lat - gnss_planetary_r.lat),'Color', [0, 0, 0], 'LineWidth', 1.5)
+    grid on;
+    xlabel('Time [s]')
+    ylabel('[m]')
+    legend('GNSS', 'IMU + degraded GNSS + OpenVINS', 'Location', 'northoutside');
+    title('Latitude Error');
+    xlim([0,max(gnss_planetary.t)]);
+
+    subplot(2,1,2);
+    hold on;
+    plot(gnss_i.t, LON2M_GR.*(gnss_i.lon - gnss_planetary_r.lon), '.', 'Color', ones(1,3) * 0.75, 'LineWidth', 1.5)
+    plot(nav_i.t, LON2M.*(nav_i.lon - gnss_planetary_r.lon),'Color', [0, 0, 0], 'LineWidth', 1.5)
+    grid on;
+    xlabel('Time [s]')
+    ylabel('[m]')
+    legend('GNSS', 'IMU + degraded GNSS + OpenVINS', 'Location', 'northoutside');
+    title('Longitude Error');
+    xlim([0,max(gnss_planetary.t)]);
+end

examples/planetary-data/canada-planetary-data/scripts/gnss_sparse_structure.m

@@ -0,0 +1,93 @@
+% Johann Diep ([email protected]) - November 2021
+%
+% This script builds the structure for the Canadian GNSS dataset.
+
+warning off;
+
+%% Conversion Constants
+
+G =  9.80665;       % Gravity constant, m/s^2
+G2MSS = G;          % g to m/s^2
+MSS2G = (1/G);      % m/s^2 to g
+D2R = (pi/180);     % degrees to radians
+R2D = (180/pi);     % radians to degrees
+KT2MS = 0.514444;   % knot to m/s
+MS2KMH = 3.6;       % m/s to km/h
+
+%% GNSS ERROR PROFILE
+
+gnss_data_planetary = readtable('Data/Canada/GNSS/global-pose-utm.txt');
+load('NaveGo-master/examples/planetary-data/canada-planetary-data/data/imu_planetary.mat');
+
+% GNSS data structure:
+%         t: Mx1 time vector (seconds).
+%       lat: Mx1 latitude (radians).
+%       lon: Mx1 longitude (radians).
+%         h: Mx1 altitude (m).
+%       vel: Mx3 NED velocities (m/s).
+%       std: 1x3 position standard deviations, [lat lon h] (rad, rad, m).
+%      stdm: 1x3 position standard deviations, [lat lon h] (m, m, m).
+%      stdv: 1x3 velocity standard deviations, [Vn Ve Vd] (m/s).
+%      larm: 3x1 lever arm from IMU to GNSS antenna (x-fwd, y-right, z-down) (m).
+%      freq: 1x1 sampling frequency (Hz).
+%   zupt_th: 1x1 ZUPT threshold (m/s).
+%  zupt_win: 1x1 ZUPT time window (seconds).
+%       eps: 1x1 time interval to compare IMU time vector to GNSS time vector (seconds).
+
+gnss_planetary.stdm = 1 * [1 1 1]; % degradation
+
+% GNSS positions
+[gnss_planetary.lat,gnss_planetary.lon] = utm2deg(table2array(gnss_data_planetary(:,2)),table2array(gnss_data_planetary(:,3)),repelem(['18 T';'18 T'],400,1));
+gnss_planetary.lat = gnss_planetary.lat.*D2R;
+gnss_planetary.lon = gnss_planetary.lon.*D2R;
+gnss_planetary.h = table2array(gnss_data_planetary(:,4));
+
+gnss_planetary = gnss_m2r(gnss_planetary.lat(1),gnss_planetary.h(1),gnss_planetary); % convert error from meters to radians
+
+% parsing the timestamps of the measurements
+timestamps = table2array(gnss_data_planetary(:,1));
+gnss_planetary.t = (timestamps - timestamps(1));
+
+gnss_planetary.freq = 1/mean(diff(gnss_planetary.t)); % estimating the frequency
+[gnss_planetary.vel,~] = pllh2vned(gnss_planetary); % estimating the velocity
+
+gnss_planetary.larm = [-0.156,0.511,0.004]'; % GNSS lever arm from IMU to GNSS antenna
+gnss_planetary.eps = mean(diff(imu_planetary.t))/3; % rule of thumb for choosing eps
+gnss_planetary.stdv = gnss_planetary.stdm/100; % educated guess
+
+% Parameters for ZUPT detection algorithm  copied from above
+gnss_planetary.zupt_th = 0; % ZUPT threshold (m/s).
+gnss_planetary.zupt_win = 4; % ZUPT time window (seconds).
+
+% Increase frequency by interpolation
+t_q = 0:0.05:gnss_planetary.t(end);
+lon_q = interp1(gnss_planetary.t,gnss_planetary.lon,t_q);
+lat_q = interp1(gnss_planetary.t,gnss_planetary.lat,t_q);
+h_q = interp1(gnss_planetary.t,gnss_planetary.h,t_q);
+v_q = interp1(gnss_planetary.t,gnss_planetary.vel,t_q);
+freq_q = 1/mean(diff(t_q));
+
+gnss_planetary.t = t_q'; 
+gnss_planetary.lon = lon_q';
+gnss_planetary.lat = lat_q';
+gnss_planetary.h = h_q';
+gnss_planetary.vel = v_q;
+gnss_planetary.freq = freq_q;
+
+gnss_planetary_r = gnss_planetary; % reference
+
+% remove datapoints
+gnss_planetary.t(2000:4000) = []; 
+gnss_planetary.lon(2000:4000) = [];
+gnss_planetary.lat(2000:4000) = [];
+gnss_planetary.h(2000:4000) = [];
+gnss_planetary.vel(2000:4000,:) = [];
+
+gnss_planetary_r_sparse = gnss_planetary; % reference
+[gnss_planetary,~] = gnss_gen(gnss_planetary_r_sparse, gnss_planetary); % degredation
+
+%% Saving 
+
+save('NaveGo-master/examples/planetary-data/canada-planetary-data/data/gnss_planetary_r.mat','gnss_planetary_r');
+save('NaveGo-master/examples/planetary-data/canada-planetary-data/data/gnss_planetary_sparse_r.mat','gnss_planetary_r_sparse');
+save('NaveGo-master/examples/planetary-data/canada-planetary-data/data/gnss_sparse_planetary.mat','gnss_planetary');