diff --git a/examples/planetary-data/canada-planetary-data/scripts/Optimizer.m b/examples/planetary-data/canada-planetary-data/scripts/Optimizer.m
new file mode 100644
index 0000000..c4b6843
--- /dev/null
+++ b/examples/planetary-data/canada-planetary-data/scripts/Optimizer.m
@@ -0,0 +1,47 @@
+% Johann Diep (johann.diep@esa.int) - 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
\ No newline at end of file
diff --git a/examples/planetary-data/canada-planetary-data/scripts/RunFusion.m b/examples/planetary-data/canada-planetary-data/scripts/RunFusion.m
new file mode 100644
index 0000000..f03f186
--- /dev/null
+++ b/examples/planetary-data/canada-planetary-data/scripts/RunFusion.m
@@ -0,0 +1,192 @@
+% Johann Diep (johann.diep@esa.int) - 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
\ No newline at end of file
diff --git a/examples/planetary-data/canada-planetary-data/scripts/gnss_sparse_structure.m b/examples/planetary-data/canada-planetary-data/scripts/gnss_sparse_structure.m
new file mode 100644
index 0000000..5cd807b
--- /dev/null
+++ b/examples/planetary-data/canada-planetary-data/scripts/gnss_sparse_structure.m
@@ -0,0 +1,93 @@
+% Johann Diep (johann.diep@esa.int) - 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');
\ No newline at end of file
diff --git a/examples/planetary-data/canada-planetary-data/scripts/gnss_structure.m b/examples/planetary-data/canada-planetary-data/scripts/gnss_structure.m
new file mode 100644
index 0000000..fb739c1
--- /dev/null
+++ b/examples/planetary-data/canada-planetary-data/scripts/gnss_structure.m
@@ -0,0 +1,83 @@
+% Johann Diep (johann.diep@esa.int) - 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 = 5 * [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
+[gnss_planetary,~] = gnss_gen(gnss_planetary, 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.mat','gnss_planetary');
\ No newline at end of file
diff --git a/examples/planetary-data/canada-planetary-data/scripts/imu_structure.m b/examples/planetary-data/canada-planetary-data/scripts/imu_structure.m
new file mode 100644
index 0000000..8f90781
--- /dev/null
+++ b/examples/planetary-data/canada-planetary-data/scripts/imu_structure.m
@@ -0,0 +1,109 @@
+% Johann Diep (johann.diep@esa.int) - November 2021
+%
+% This function builds the structure for the Canadian IMU dataset.
+% It is written in order to allow for the optimization for the IMU
+% parameters.
+%
+% Input:
+% - p_arw: value for the angle random walks
+% - p_vrw: value for the velocity random walks
+% - p_gb_sta: value for gyroscopic static biases
+% - p_ab_sta: value for acceleration static biases
+% - p_gb_dyn: value for gyroscopic dynamic biases
+% - p_ab_dyn: value for acceleration dynamic biases
+% - p_a_std: value for acceleration standard deviations
+% - p_g_std: value for gyroscopic standard deviations
+% - p_ab_psd: value for acceleration dynamic biases PSD
+% - p_gb_psd: value for gyroscopic dynamic biases PSD
+% - m_psd: value for magnetometer biases PSD
+%
+% Output:
+% - imu_planetary: struct containing the IMU data
+%
+% It is noted that in order to save time, this function was rewritten in
+% its parameters for each optimization round (2 values were optimized each).
+% This functionality was then removed for better overview.
+
+function imu_planetary = imu_structure()
+ %% 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
+
+ %% IMU ERROR PROFILE
+
+ imu_data_planetary = readtable('Data/Canada/IMU/imu.txt');
+
+ % IMU data structure:
+ % t: Ix1 time vector (seconds).
+ % fb: Ix3 accelerations vector in body frame XYZ (m/s^2).
+ % wb: Ix3 turn rates vector in body frame XYZ (radians/s).
+ % arw: 1x3 angle random walks (rad/s/root-Hz).
+ % arrw: 1x3 angle rate random walks (rad/s^2/root-Hz).
+ % vrw: 1x3 velocity random walks (m/s^2/root-Hz).
+ % vrrw: 1x3 velocity rate random walks (m/s^3/root-Hz).
+ % g_std: 1x3 gyros standard deviations (radians/s).
+ % a_std: 1x3 accrs standard deviations (m/s^2).
+ % gb_sta: 1x3 gyros static biases or turn-on biases (radians/s).
+ % ab_sta: 1x3 accrs static biases or turn-on biases (m/s^2).
+ % gb_dyn: 1x3 gyros dynamic biases or bias instabilities (radians/s).
+ % ab_dyn: 1x3 accrs dynamic biases or bias instabilities (m/s^2).
+ % gb_corr: 1x3 gyros correlation times (seconds).
+ % ab_corr: 1x3 accrs correlation times (seconds).
+ % gb_psd: 1x3 gyros dynamic biases PSD (rad/s/root-Hz).
+ % ab_psd: 1x3 accrs dynamic biases PSD (m/s^2/root-Hz).
+ % freq: 1x1 sampling frequency (Hz).
+ % ini_align: 1x3 initial attitude at t(1), [roll pitch yaw] (rad).
+ % ini_align_err: 1x3 initial attitude errors at t(1), [roll pitch yaw] (rad).
+
+ % Parameters with optimized values.
+ p_arw = 8.0000e-05;
+ p_vrw = 2.0000e-05;
+ gb_sta = -0.0050;
+ ab_sta = -0.0190;
+ gb_dyn = 4.0000e-05;
+ ab_dyn = 5.0000e-05;
+ a_std = 0.001;
+ g_std = 0.001;
+ ab_psd = 0;
+ gb_psd = 0;
+ m_psd = 0;
+
+ imu_planetary.arw = [p_arw,p_arw,p_arw];
+ imu_planetary.arrw = [0,0,0];
+ imu_planetary.vrw = [p_vrw,p_vrw,p_vrw];
+ imu_planetary.vrrw = [0,0,0];
+ imu_planetary.gb_sta = [gb_sta gb_sta gb_sta];
+ imu_planetary.ab_sta = [ab_sta ab_sta ab_sta];
+ imu_planetary.gb_dyn = [gb_dyn,gb_dyn,gb_dyn];
+ imu_planetary.ab_dyn = [ab_dyn,ab_dyn,ab_dyn];
+ imu_planetary.gb_corr = [1000 1000 1000];
+ imu_planetary.ab_corr = [1000 1000 1000];
+ imu_planetary.a_std = [a_std,a_std,a_std];
+ imu_planetary.g_std = [g_std,g_std,g_std];
+ imu_planetary.ab_psd = [ab_psd,ab_psd,ab_psd];
+ imu_planetary.gb_psd = [gb_psd,gb_psd,gb_psd];
+ imu_planetary.m_psd = [m_psd,m_psd,m_psd];
+
+ % time
+ timestamps = table2array(imu_data_planetary(:,1));
+ timestamps = (timestamps - timestamps(1));
+ dt = mean(diff(timestamps));
+ imu_planetary.t = (timestamps(1):dt:timestamps(end))';
+ imu_planetary.freq = 1/dt;
+
+ imu_planetary.ini_align_err = [10 10 10] .* D2R; % initial attitude align errors for matrix P in Kalman filter, [roll pitch yaw] (radians)
+ imu_planetary.ini_align = [0 0 0] .* D2R; % Initial attitude align at t(1) (radians)
+
+ imu_planetary.fb = table2array(imu_data_planetary(:,2:4)); % acceleration measurements
+ imu_planetary.wb = table2array(imu_data_planetary(:,5:7)); % gyroscopic measurements
+
+ %% Saving
+
+ save('NaveGo-master/examples/planetary-data/canada-planetary-data/data/imu_planetary.mat','imu_planetary');
+end
diff --git a/examples/planetary-data/canada-planetary-data/scripts/utm2deg.m b/examples/planetary-data/canada-planetary-data/scripts/utm2deg.m
new file mode 100644
index 0000000..003a15d
--- /dev/null
+++ b/examples/planetary-data/canada-planetary-data/scripts/utm2deg.m
@@ -0,0 +1,127 @@
+function [Lat,Lon] = utm2deg(xx,yy,utmzone)
+% -------------------------------------------------------------------------
+% [Lat,Lon] = utm2deg(x,y,utmzone)
+%
+% Description: Function to convert vectors of UTM coordinates into Lat/Lon vectors (WGS84).
+% Some code has been extracted from UTMIP.m function by Gabriel Ruiz Martinez.
+%
+% Inputs:
+% x, y , utmzone.
+%
+% Outputs:
+% Lat: Latitude vector. Degrees. +ddd.ddddd WGS84
+% Lon: Longitude vector. Degrees. +ddd.ddddd WGS84
+%
+% Example 1:
+% x=[ 458731; 407653; 239027; 230253; 343898; 362850];
+% y=[4462881; 5126290; 4163083; 3171843; 4302285; 2772478];
+% utmzone=['30 T'; '32 T'; '11 S'; '28 R'; '15 S'; '51 R'];
+% [Lat, Lon]=utm2deg(x,y,utmzone);
+% fprintf('%11.6f ',lat)
+% 40.315430 46.283902 37.577834 28.645647 38.855552 25.061780
+% fprintf('%11.6f ',lon)
+% -3.485713 7.801235 -119.955246 -17.759537 -94.799019 121.640266
+%
+% Example 2: If you need Lat/Lon coordinates in Degrees, Minutes and Seconds
+% [Lat, Lon]=utm2deg(x,y,utmzone);
+% LatDMS=dms2mat(deg2dms(Lat))
+%LatDMS =
+% 40.00 18.00 55.55
+% 46.00 17.00 2.01
+% 37.00 34.00 40.17
+% 28.00 38.00 44.33
+% 38.00 51.00 19.96
+% 25.00 3.00 42.41
+% LonDMS=dms2mat(deg2dms(Lon))
+%LonDMS =
+% -3.00 29.00 8.61
+% 7.00 48.00 4.40
+% -119.00 57.00 18.93
+% -17.00 45.00 34.33
+% -94.00 47.00 56.47
+% 121.00 38.00 24.96
+%
+% Author:
+% Rafael Palacios
+% Universidad Pontificia Comillas
+% Madrid, Spain
+% Version: Apr/06, Jun/06, Aug/06
+% Aug/06: corrected m-Lint warnings
+%-------------------------------------------------------------------------
+% Argument checking
+%
+error(nargchk(3, 3, nargin)); %3 arguments required
+n1=length(xx);
+n2=length(yy);
+n3=size(utmzone,1);
+if (n1~=n2 || n1~=n3)
+ error('x,y and utmzone vectors should have the same number or rows');
+end
+c=size(utmzone,2);
+if (c~=4)
+ error('utmzone should be a vector of strings like "30 T"');
+end
+
+
+% Memory pre-allocation
+%
+Lat=zeros(n1,1);
+Lon=zeros(n1,1);
+% Main Loop
+%
+for i=1:n1
+ if (utmzone(i,4)>'X' || utmzone(i,4)<'C')
+ fprintf('utm2deg: Warning utmzone should be a vector of strings like "30 T", not "30 t"\n');
+ end
+ if (utmzone(i,4)>'M')
+ hemis='N'; % Northern hemisphere
+ else
+ hemis='S';
+ end
+ x=xx(i);
+ y=yy(i);
+ zone=str2double(utmzone(i,1:2));
+ sa = 6378137.000000 ; sb = 6356752.314245;
+
+% e = ( ( ( sa ^ 2 ) - ( sb ^ 2 ) ) ^ 0.5 ) / sa;
+ e2 = ( ( ( sa ^ 2 ) - ( sb ^ 2 ) ) ^ 0.5 ) / sb;
+ e2cuadrada = e2 ^ 2;
+ c = ( sa ^ 2 ) / sb;
+% alpha = ( sa - sb ) / sa; %f
+% ablandamiento = 1 / alpha; % 1/f
+ X = x - 500000;
+
+ if hemis == 'S' || hemis == 's'
+ Y = y - 10000000;
+ else
+ Y = y;
+ end
+
+ S = ( ( zone * 6 ) - 183 );
+ lat = Y / ( 6366197.724 * 0.9996 );
+ v = ( c / ( ( 1 + ( e2cuadrada * ( cos(lat) ) ^ 2 ) ) ) ^ 0.5 ) * 0.9996;
+ a = X / v;
+ a1 = sin( 2 * lat );
+ a2 = a1 * ( cos(lat) ) ^ 2;
+ j2 = lat + ( a1 / 2 );
+ j4 = ( ( 3 * j2 ) + a2 ) / 4;
+ j6 = ( ( 5 * j4 ) + ( a2 * ( cos(lat) ) ^ 2) ) / 3;
+ alfa = ( 3 / 4 ) * e2cuadrada;
+ beta = ( 5 / 3 ) * alfa ^ 2;
+ gama = ( 35 / 27 ) * alfa ^ 3;
+ Bm = 0.9996 * c * ( lat - alfa * j2 + beta * j4 - gama * j6 );
+ b = ( Y - Bm ) / v;
+ Epsi = ( ( e2cuadrada * a^ 2 ) / 2 ) * ( cos(lat) )^ 2;
+ Eps = a * ( 1 - ( Epsi / 3 ) );
+ nab = ( b * ( 1 - Epsi ) ) + lat;
+ senoheps = ( exp(Eps) - exp(-Eps) ) / 2;
+ Delt = atan(senoheps / (cos(nab) ) );
+ TaO = atan(cos(Delt) * tan(nab));
+ longitude = (Delt *(180 / pi ) ) + S;
+ latitude = ( lat + ( 1 + e2cuadrada* (cos(lat)^ 2) - ( 3 / 2 ) * e2cuadrada * sin(lat) * cos(lat) * ( TaO - lat ) ) * ( TaO - lat ) ) * ...
+ (180 / pi);
+
+ Lat(i)=latitude;
+ Lon(i)=longitude;
+
+end
\ No newline at end of file
diff --git a/examples/planetary-data/canada-planetary-data/scripts/visual_structure.m b/examples/planetary-data/canada-planetary-data/scripts/visual_structure.m
new file mode 100644
index 0000000..cb43981
--- /dev/null
+++ b/examples/planetary-data/canada-planetary-data/scripts/visual_structure.m
@@ -0,0 +1,100 @@
+% Johann Diep (johann.diep@esa.int) - August 2021
+%
+% This script builds the structure for the visual dataset.
+
+%% 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
+
+%% OpenVINS PLANETARY DATASET ERROR PROFILE
+
+load('gnss_planetary_r.mat');
+load('imu_planetary.mat');
+
+visual_data_planetary_pose = readtable('Data/Canada/pose_data.csv');
+visual_data_planetary_twist = readtable('Data/Canada/twist_data.csv');
+visual_data_planetary_posecov = readtable('Data/Canada/posecov_data.csv');
+visual_data_planetary_twistcov = readtable('Data/Canada/twistcov_data.csv');
+
+% visual, visual data structure
+% t: Vx1 time vector (seconds)
+% lat: Vx1 latitude (radians).
+% lon: Vx1 longitude (radians).
+% h: Vx1 altitude (m).
+% vel: Vx3 NED velocities (m/s).
+% covvm: Vx36 velocity and position covariance matrices (m^2 and m^2/s^2).
+% covv: Vx9 velocity covariance matrices (m^2/s^2).
+% larm: 3x1 lever arm from IMU to camera (x-fwd, y-right, z-down) (m).
+% freq: 1x1 sampling frequency (Hz).
+% eps: 1x1 time interval to compare current IMU time to current GNSS time vector (s).
+
+Cutoff = 1500;
+
+% parsing the timestamps of the measurements
+Seconds = table2array(visual_data_planetary_pose(Cutoff:end,4));
+Nanoseconds = table2array(visual_data_planetary_pose(Cutoff:end,5));
+Timestamps = Seconds + Nanoseconds/10^(9);
+visual_planetary.t = Timestamps - Timestamps(1);
+
+% position and velocity preprocessing
+Positions = table2array(visual_data_planetary_pose(Cutoff:end,6:7))-table2array(visual_data_planetary_pose(Cutoff,6:7));
+Heights = table2array(visual_data_planetary_pose(Cutoff:end,8))-table2array(visual_data_planetary_pose(Cutoff,8));
+RotationAngle = deg2rad(-100);
+RotationMatrix = [cos(RotationAngle),-sin(RotationAngle);sin(RotationAngle),cos(RotationAngle)];
+Positions_rotated = RotationMatrix * Positions';
+
+[visual_planetary.lat, visual_planetary.lon, visual_planetary.h] = ned2geodetic(Positions_rotated(2,:)',Positions_rotated(1,:)',-Heights, ...
+ gnss_planetary_r.lat(1),gnss_planetary_r.lon(1),gnss_planetary_r.h(1),wgs84Ellipsoid,'radians');
+
+Velocities = table2array(visual_data_planetary_twist(Cutoff:end,1:2));
+Velocities_rotated = RotationMatrix * Velocities';
+visual_planetary.vel(:,1:2) = Velocities_rotated';
+visual_planetary.vel(:,3) = -table2array(visual_data_planetary_twist(Cutoff:end,3));
+
+% parsing the covariances
+Count = 1;
+for i = Cutoff:size(table2array(visual_data_planetary_posecov),1)
+ PoseCovariance = table2array(visual_data_planetary_posecov(i,:));
+ TwistCovariance = table2array(visual_data_planetary_twistcov(i,:));
+
+ PoseCovariance = reshape(PoseCovariance,[6,6])';
+ TwistCovariance = reshape(TwistCovariance,[6,6])';
+
+ PositionCovariance = PoseCovariance(1:3,1:3);
+ VelocityCovariance = TwistCovariance(1:3,1:3);
+
+ % [~,Std] = corrcov(PositionCovariance);
+ % Std_x(i) = Std(1);
+ % Std_y(i) = Std(2);
+ % Std_z(i) = Std(3);
+
+ IntermediateCovariance = [VelocityCovariance,zeros(3,3);zeros(3,3),PositionCovariance];
+
+ visual_planetary.covv(Count,:) = reshape(VelocityCovariance',[1,9]);
+ visual_planetary.covvm(Count,:) = reshape(IntermediateCovariance',[1,36]);
+ Count = Count + 1;
+end
+
+visual_planetary.larm = [0,0,0]'; % no lever arm needed in case of OpenVINS
+visual_planetary.eps = mean(diff(imu_planetary.t))*20; % rule of thumb for choosing eps
+
+% downsample
+visual_planetary.t = downsample(visual_planetary.t,33);
+visual_planetary.lat = downsample(visual_planetary.lat,33);
+visual_planetary.lon = downsample(visual_planetary.lon,33);
+visual_planetary.h = downsample(visual_planetary.h,33);
+visual_planetary.vel = downsample(visual_planetary.vel,33);
+visual_planetary.covv = downsample(visual_planetary.covv,33);
+visual_planetary.covvm = downsample(visual_planetary.covvm,33);
+
+visual_planetary.freq = 1/mean(diff(visual_planetary.t)); % estimating the frequency
+
+save('NaveGo-master/examples/planetary-data/canada-planetary-data/data/visual_planetary.mat','visual_planetary');
\ No newline at end of file
diff --git a/examples/planetary-data/katwijk-planetary-data/scripts/Optimizer.m b/examples/planetary-data/katwijk-planetary-data/scripts/Optimizer.m
new file mode 100644
index 0000000..c4b6843
--- /dev/null
+++ b/examples/planetary-data/katwijk-planetary-data/scripts/Optimizer.m
@@ -0,0 +1,47 @@
+% Johann Diep (johann.diep@esa.int) - 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
\ No newline at end of file
diff --git a/examples/planetary-data/katwijk-planetary-data/scripts/RunFusion.m b/examples/planetary-data/katwijk-planetary-data/scripts/RunFusion.m
new file mode 100644
index 0000000..f03f186
--- /dev/null
+++ b/examples/planetary-data/katwijk-planetary-data/scripts/RunFusion.m
@@ -0,0 +1,192 @@
+% Johann Diep (johann.diep@esa.int) - 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
\ No newline at end of file
diff --git a/examples/planetary-data/katwijk-planetary-data/scripts/gnss_sparse_structure.m b/examples/planetary-data/katwijk-planetary-data/scripts/gnss_sparse_structure.m
new file mode 100644
index 0000000..a06c015
--- /dev/null
+++ b/examples/planetary-data/katwijk-planetary-data/scripts/gnss_sparse_structure.m
@@ -0,0 +1,101 @@
+% Johann Diep (johann.diep@esa.int) - November 2021
+%
+% This script builds the structure for the Katwijk 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/Katwijk/GNSS/gps-latlong.txt');
+load('NaveGo-master/examples/planetary-data/katwijk-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 = 0.01 * [1 1 1]; % degradation
+
+% GNSS positions
+gnss_planetary.lat = table2array(gnss_data_planetary(:,3)).*D2R;
+gnss_planetary.lon = table2array(gnss_data_planetary(:,4)).*D2R;
+gnss_planetary.h = table2array(gnss_data_planetary(:,5));
+
+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
+Date = cell2mat(table2array(gnss_data_planetary(:,1)));
+Year = str2num(Date(:,1:4));
+Month = str2num(Date(:,6:7));
+Day = str2num(Date(:,9:10));
+Hour = str2num(Date(:,12:13))-1;
+Minutes = str2num(Date(:,15:16));
+Seconds = str2num(Date(:,18:19));
+MilliSeconds = str2num(Date(:,21:23));
+DateTime = datetime(Year, Month, Day, Hour, Minutes, Seconds, MilliSeconds);
+Timestamp = posixtime(DateTime);
+gnss_planetary.t = Timestamp - Timestamp(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,0,0]'; % 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/katwijk-planetary-data/data/gnss_planetary_r.mat','gnss_planetary_r');
+save('NaveGo-master/examples/planetary-data/katwijk-planetary-data/data/gnss_planetary_sparse_r.mat','gnss_planetary_r_sparse');
+save('NaveGo-master/examples/planetary-data/katwijk-planetary-data/data/gnss_sparse_planetary.mat','gnss_planetary');
\ No newline at end of file
diff --git a/examples/planetary-data/katwijk-planetary-data/scripts/gnss_structure.m b/examples/planetary-data/katwijk-planetary-data/scripts/gnss_structure.m
new file mode 100644
index 0000000..f9b74fb
--- /dev/null
+++ b/examples/planetary-data/katwijk-planetary-data/scripts/gnss_structure.m
@@ -0,0 +1,91 @@
+% Johann Diep (johann.diep@esa.int) - November 2021
+%
+% This script builds the structure for the Katwijk 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/Katwijk/GNSS/gps-latlong.txt');
+load('NaveGo-master/examples/planetary-data/katwijk-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 = 5 * [1 1 1]; % degradation
+
+% GNSS positions
+gnss_planetary.lat = table2array(gnss_data_planetary(:,3)).*D2R;
+gnss_planetary.lon = table2array(gnss_data_planetary(:,4)).*D2R;
+gnss_planetary.h = table2array(gnss_data_planetary(:,5));
+
+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
+Date = cell2mat(table2array(gnss_data_planetary(:,1)));
+Year = str2num(Date(:,1:4));
+Month = str2num(Date(:,6:7));
+Day = str2num(Date(:,9:10));
+Hour = str2num(Date(:,12:13))-1;
+Minutes = str2num(Date(:,15:16));
+Seconds = str2num(Date(:,18:19));
+MilliSeconds = str2num(Date(:,21:23));
+DateTime = datetime(Year, Month, Day, Hour, Minutes, Seconds, MilliSeconds);
+Timestamp = posixtime(DateTime);
+gnss_planetary.t = Timestamp - Timestamp(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,0,0]'; % 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.2: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
+[gnss_planetary,~] = gnss_gen(gnss_planetary, gnss_planetary); % degredation
+
+%% Saving
+
+save('NaveGo-master/examples/planetary-data/katwijk-planetary-data/data/gnss_planetary_r.mat','gnss_planetary_r');
+save('NaveGo-master/examples/planetary-data/katwijk-planetary-data/data/gnss_planetary.mat','gnss_planetary');
\ No newline at end of file
diff --git a/examples/planetary-data/katwijk-planetary-data/scripts/imu_structure.m b/examples/planetary-data/katwijk-planetary-data/scripts/imu_structure.m
new file mode 100644
index 0000000..0025a7c
--- /dev/null
+++ b/examples/planetary-data/katwijk-planetary-data/scripts/imu_structure.m
@@ -0,0 +1,72 @@
+% Johann Diep (johann.diep@esa.int) - August 2021
+%
+% This script builds the structure for the Katwijk IMU dataset.
+
+%% 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
+
+%% IMU ERROR PROFILE
+
+imu_data_planetary = readtable('Data/Katwijk/IMU/imu0.csv'); % read planetary IMU data
+
+% IMU data structure:
+% t: Ix1 time vector (seconds).
+% fb: Ix3 accelerations vector in body frame XYZ (m/s^2).
+% wb: Ix3 turn rates vector in body frame XYZ (radians/s).
+% arw: 1x3 angle random walks (rad/s/root-Hz).
+% arrw: 1x3 angle rate random walks (rad/s^2/root-Hz).
+% vrw: 1x3 velocity random walks (m/s^2/root-Hz).
+% vrrw: 1x3 velocity rate random walks (m/s^3/root-Hz).
+% g_std: 1x3 gyros standard deviations (radians/s).
+% a_std: 1x3 accrs standard deviations (m/s^2).
+% gb_sta: 1x3 gyros static biases or turn-on biases (radians/s).
+% ab_sta: 1x3 accrs static biases or turn-on biases (m/s^2).
+% gb_dyn: 1x3 gyros dynamic biases or bias instabilities (radians/s).
+% ab_dyn: 1x3 accrs dynamic biases or bias instabilities (m/s^2).
+% gb_corr: 1x3 gyros correlation times (seconds).
+% ab_corr: 1x3 accrs correlation times (seconds).
+% gb_psd: 1x3 gyros dynamic biases PSD (rad/s/root-Hz).
+% ab_psd: 1x3 accrs dynamic biases PSD (m/s^2/root-Hz);
+% freq: 1x1 sampling frequency (Hz).
+% ini_align: 1x3 initial attitude at t(1), [roll pitch yaw] (rad).
+% ini_align_err: 1x3 initial attitude errors at t(1), [roll pitch yaw] (rad).
+
+imu_planetary.arw = [0.008017806365449,0.008107120830822,0.008142094400848];
+imu_planetary.arrw = [0,0,0];
+imu_planetary.vrw = [0.001968757359111,0.002080847480414,0.001698689979355];
+imu_planetary.vrrw = [0,0,0];
+imu_planetary.gb_sta = [0.007440407532675,0.044356514264794,0.062953158661843];
+imu_planetary.ab_sta = [0.012290855897547,0.012470873935767,9.830174007054483];
+imu_planetary.gb_dyn = [3.589857650346221e-04,3.897194991641202e-04,2.195082701413355e-04];
+imu_planetary.ab_dyn = [2.936833659299578e-04,3.012048724015608e-04,2.990266040668724e-04];
+imu_planetary.gb_corr = [1000 1000 1000];
+imu_planetary.ab_corr = [1000 1000 700];
+imu_planetary.a_std = [0.021363952880684,0.022855668192465,0.017875212169832];
+imu_planetary.g_std = [0.087135482784513,0.089545793244810,0.089085782298092];
+imu_planetary.ab_psd = [0.009287083472434,0.009524934391293,0.007911500297531];
+imu_planetary.gb_psd = [0.011352126650874,0.012324012659387,0.006941460988902];
+imu_planetary.m_psd = [0 0 0];
+
+% time
+timestamps = table2array(imu_data_planetary(:,1));
+timestamps = (timestamps - timestamps(1));
+dt = mean(diff(timestamps));
+imu_planetary.t = (timestamps(1):dt:timestamps(end))';
+imu_planetary.freq = 1/dt;
+
+imu_planetary.ini_align_err = [5 5 5] .* D2R; % initial attitude align errors for matrix P in Kalman filter, [roll pitch yaw] (radians)
+imu_planetary.ini_align = [30 75 0] .* D2R; % Initial attitude align at t(1) (radians)
+
+imu_planetary.fb = table2array(imu_data_planetary(:,5:7)); % acceleration measurements
+imu_planetary.wb = table2array(imu_data_planetary(:,2:4)); % gyroscope measurements
+
+%% Saving
+
+save('NaveGo-master/examples/planetary-data/katwijk-planetary-data/data/imu_planetary.mat','imu_planetary');
\ No newline at end of file
diff --git a/examples/planetary-data/katwijk-planetary-data/scripts/visual_structure.m b/examples/planetary-data/katwijk-planetary-data/scripts/visual_structure.m
new file mode 100644
index 0000000..d919b1e
--- /dev/null
+++ b/examples/planetary-data/katwijk-planetary-data/scripts/visual_structure.m
@@ -0,0 +1,97 @@
+% Johann Diep (johann.diep@esa.int) - August 2021
+%
+% This script builds the structure for the visual dataset.
+
+%% 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
+
+%% OpenVINS PLANETARY DATASET ERROR PROFILE
+
+load('gnss_planetary_r.mat'); % loading GNSS struct
+load('imu_planetary.mat'); % loading inertial struct
+
+% visual_data_planetary_pose = readtable('Data/Katwijk/1/pose_data.csv'); % read timestamps, position and attitude data
+% visual_data_planetary_twist = readtable('Data/Katwijk/1/twist_data.csv'); % read linear and angular velocity data
+% visual_data_planetary_posecov = readtable('Data/Katwijk/1/posecov_data.csv'); % read position and attitude covariance data
+% visual_data_planetary_twistcov = readtable('Data/Katwijk/1/twistcov_data.csv'); % read linear and angular velocity covariance data
+visual_data_planetary_pose = readtable('Data/Canada/pose_data.csv'); % read timestamps, position and attitude data
+visual_data_planetary_twist = readtable('Data/Canada/twist_data.csv'); % read linear and angular velocity data
+visual_data_planetary_posecov = readtable('Data/Canada/posecov_data.csv'); % read position and attitude covariance data
+visual_data_planetary_twistcov = readtable('Data/Canada/twistcov_data.csv'); % read linear and angular velocity covariance data
+
+% visual, visual data structure
+% t: Vx1 time vector (seconds)
+% lat: Vx1 latitude (radians).
+% lon: Vx1 longitude (radians).
+% h: Vx1 altitude (m).
+% vel: Vx3 NED velocities (m/s).
+% covvm: Vx36 velocity and position covariance matrices (m^2 and m^2/s^2).
+% covv: Vx9 velocity covariance matrices (m^2/s^2).
+% larm: 3x1 lever arm from IMU to camera (x-fwd, y-right, z-down) (m).
+% freq: 1x1 sampling frequency (Hz).
+% eps: 1x1 time interval to compare current IMU time to current GNSS time vector (s).
+
+% parsing the timestamps of the measurements
+Seconds = table2array(visual_data_planetary_pose(:,4));
+Nanoseconds = table2array(visual_data_planetary_pose(:,5));
+Timestamps = Seconds + Nanoseconds/10^(9);
+visual_planetary.t = Timestamps - Timestamps(1);
+
+% position and velocity preprocessing
+Positions = table2array(visual_data_planetary_pose(:,6:7));
+Heights = table2array(visual_data_planetary_pose(:,8));
+%RotationAngle = deg2rad(-70);
+RotationAngle = deg2rad(-110);
+RotationMatrix = [cos(RotationAngle),-sin(RotationAngle);sin(RotationAngle),cos(RotationAngle)];
+Positions_rotated = RotationMatrix * Positions';
+[visual_planetary.lat, visual_planetary.lon, visual_planetary.h] = ned2geodetic(Positions_rotated(2,:)',Positions_rotated(1,:)',-Heights, ...
+ gnss_planetary_r.lat(1),gnss_planetary_r.lon(1),gnss_planetary_r.h(1),wgs84Ellipsoid,'radians');
+
+Velocities = table2array(visual_data_planetary_twist(:,1:2));
+Velocities_rotated = RotationMatrix * Velocities';
+visual_planetary.vel(:,1:2) = Velocities_rotated';
+visual_planetary.vel(:,3) = -table2array(visual_data_planetary_twist(:,3));
+
+% parsing the covariances
+for i = 1:size(table2array(visual_data_planetary_posecov),1)
+ PoseCovariance = table2array(visual_data_planetary_posecov(i,:));
+ TwistCovariance = table2array(visual_data_planetary_twistcov(i,:));
+
+ PoseCovariance = reshape(PoseCovariance,[6,6])';
+ TwistCovariance = reshape(TwistCovariance,[6,6])';
+
+ PositionCovariance = PoseCovariance(1:3,1:3);
+ VelocityCovariance = TwistCovariance(1:3,1:3);
+
+ IntermediateCovariance = [VelocityCovariance,zeros(3,3);zeros(3,3),PositionCovariance];
+
+ visual_planetary.covv(i,:) = reshape(VelocityCovariance',[1,9]);
+ visual_planetary.covvm(i,:) = reshape(IntermediateCovariance',[1,36]);
+end
+
+visual_planetary.larm = [0,0,0]'; % no lever arm needed
+%visual_planetary.eps = mean(diff(imu_planetary.t))*12; % rule of thumb for choosing eps
+visual_planetary.eps = mean(diff(imu_planetary.t))/3; % rule of thumb for choosing eps
+
+% downsample
+visual_planetary.t = downsample(visual_planetary.t,20);
+visual_planetary.lat = downsample(visual_planetary.lat,20);
+visual_planetary.lon = downsample(visual_planetary.lon,20);
+visual_planetary.h = downsample(visual_planetary.h,20);
+visual_planetary.vel = downsample(visual_planetary.vel,20);
+visual_planetary.covv = downsample(visual_planetary.covv,20);
+visual_planetary.covvm = downsample(visual_planetary.covvm,20);
+
+visual_planetary.freq = 1/mean(diff(visual_planetary.t)); % estimating the frequency
+
+%save('NaveGo-master/examples/planetary-data/katwijk-planetary-data/visual_planetary.mat','visual_planetary');
+save('NaveGo-master/examples/planetary-data/canada-planetary-data/visual_planetary.mat','visual_planetary');
\ No newline at end of file
diff --git a/ins_visual/F_update.m b/ins_visual/F_update.m
new file mode 100644
index 0000000..7368293
--- /dev/null
+++ b/ins_visual/F_update.m
@@ -0,0 +1,220 @@
+function [F, G] = F_update(upd, DCMbn, imu, MAG)
+% F_update: updates F and G matrices before the execution of Kalman filter.
+%
+% INPUT
+% upd, 1x8 vector with data from the INS.
+% DCMbn, DCM body-to-nav.
+% imu, IMU data structure.
+%
+% OUTPUT
+% F, 15x15 state transition matrix.
+% G, 15x12 control-input matrix.
+%
+% Copyright (C) 2014, Rodrigo Gonzalez, all rights reserved.
+%
+% This file is part of NaveGo, an open-source MATLAB toolbox for
+% simulation of integrated navigation systems.
+%
+% NaveGo is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License (LGPL)
+% version 3 as published by the Free Software Foundation.
+%
+% This program is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+% GNU Lesser General Public License for more details.
+%
+% You should have received a copy of the GNU Lesser General Public
+% License along with this program. If not, see
+% .
+%
+% References:
+%
+% Titterton, D.H. and Weston, J.L. (2004). Strapdown
+% Inertial Navigation Technology (2nd Ed.). Institution
+% of Engineering and Technology, USA. Eq. 12.18, p. 345, matrix F.
+%
+% Farrell, J. (2008). Aided Navigation: GPS With High Rate
+% Sensors. McGraw-Hill Professional, USA. Eq. 11.108, p. 407, matrix G.
+%
+% R. Gonzalez, J. Giribet, and H. Patiño. NaveGo: a
+% simulation framework for low-cost integrated navigation systems,
+% Journal of Control Engineering and Applied Informatics, vol. 17,
+% issue 2, pp. 110-120, 2015. Eq. 26.
+%
+% R. Gonzalez, J. Giribet, and H. Patiño. An approach to
+% benchmarking of loosely coupled low-cost navigation systems,
+% Mathematical and Computer Modelling of Dynamical Systems, vol. 21,
+% issue 3, pp. 272-287, 2015. Eq. 22.
+%
+% Version: 007
+% Date: 2021/03/26
+% Author: Rodrigo Gonzalez
+% URL: https://github.com/rodralez/navego
+
+if nargin < 4, MAG = 'OFF'; end
+
+Vn = upd(1);
+Ve = upd(2);
+Vd = upd(3);
+lat = upd(4);
+h = upd(5);
+
+fn = upd(6:8);
+wn = upd(9:11);
+
+Om = 7.292115e-5;
+I = eye(3);
+Z = zeros(3);
+
+[RM,RN] = radius(lat);
+
+RO = sqrt(RN*RM) + h;
+
+%% ATTITUDE MATRICES
+
+a11 = 0;
+a12 = -( (Om * sin(lat)) + (Ve / RO * tan(lat)) );
+a13 = Vn / RO;
+a21 = (Om * sin(lat)) + (Ve / RO * tan(lat));
+a22 = 0 ;
+a23 = (Om * cos(lat)) + (Ve / RO) ;
+a31 = -Vn / RO;
+a32 = -Om * cos(lat) - (Ve / RO);
+a33 = 0;
+F11 = [a11 a12 a13; a21 a22 a23; a31 a32 a33;];
+
+% Groves, 14.64
+% F11 = skewm(wn);
+
+a11 = 0;
+a12 = 1 / RO;
+a13 = 0;
+a21 = -1 / RO;
+a22 = 0;
+a23 = 0;
+a31 = 0;
+a32 = -tan(lat) / RO;
+a33 = 0;
+F12 = [a11 a12 a13; a21 a22 a23; a31 a32 a33;];
+
+a11 = -Om * sin(lat);
+a12 = 0;
+a13 = -Ve / (RO^2);
+a21 = 0 ;
+a22 = 0 ;
+a23 = Vn / (RO^2);
+a31 = -Om * cos(lat) - (Ve / ((RO) * (cos(lat))^2));
+a32 = 0 ;
+a33 = (Ve * tan(lat)) / (RO^2) ;
+F13 = [a11 a12 a13; a21 a22 a23; a31 a32 a33;];
+
+%% VELOCITY MATRICES
+
+F21 = skewm(fn);
+
+a11 = Vd / RO;
+a12 = -2 * ((Om * sin(lat)) + ((Ve / RO) * tan(lat))) ;
+a13 = Vn / RO ;
+a21 = (2 * Om * sin(lat)) + ( (Ve / RO) * tan(lat) );
+a22 = (1 / RO) * ((Vn * tan(lat)) + Vd) ;
+a23 = 2 * Om * cos(lat) + (Ve / RO);
+a31 = (-2 * Vn) / RO;
+a32 = -2 * (Om * cos(lat) + (Ve / RO)) ;
+a33 = 0;
+F22 = [a11 a12 a13; a21 a22 a23; a31 a32 a33;];
+
+e = 0.0818191908425; % WGS84 eccentricity
+res = RN * sqrt( cos(lat)^2 + (1-e^2)^2 * sin(lat)^2);
+g = gravity(lat,h);
+g0 = g(3);
+
+a11 = -Ve * ((2 * Om * cos(lat)) + (Ve / (RO * (cos(lat))^2)));
+a12 = 0 ;
+a13 = (1 / RO^2) * ( (Ve^2 * tan(lat)) - (Vn * Vd) );
+a21 = 2 * Om * ( (Vn * cos(lat)) - (Vd * sin(lat)) ) + ( (Vn * Ve) / (RO * (cos(lat))^2) ) ;
+a22 = 0 ;
+a23 = -(Ve / RO^2) * (Vn * tan(lat) + Vd);
+a31 = 2 * Om * Ve * sin(lat);
+a32 = 0;
+% a33 = (1 / RO^2) * (Vn^2 + Ve^2);
+a33 = Ve^2 / (RN+h)^2 + Vn^2 / (RM+h)^2 - 2 * g0 / res;
+F23 = [a11 a12 a13; a21 a22 a23; a31 a32 a33;];
+
+%% POSITIONING MATRICES
+
+F31 = zeros(3);
+
+a11 = 1 / RO;
+a12 = 0;
+a13 = 0;
+a21 = 0;
+a22 = 1 / (RO * cos(lat));
+a23 = 0;
+a31 = 0;
+a32 = 0;
+a33 = -1;
+F32 = [a11 a12 a13; a21 a22 a23; a31 a32 a33;];
+
+a11 = 0;
+a12 = 0;
+a13 = -Vn / RO^2;
+a21 = (Ve * tan(lat)) / (RO * cos(lat));
+a22 = 0;
+a23 = -Ve / (RO^2 * cos(lat));
+a31 = 0;
+a32 = 0;
+a33 = 0;
+F33 = [a11 a12 a13; a21 a22 a23; a31 a32 a33;];
+
+if (isinf(imu.gb_corr))
+ Fgg = Z;
+else
+ Fgg = -diag( 1./ imu.gb_corr);
+ Fbg = I;
+% Fbg = -diag(sqrt (2 ./ imu.gb_corr .* imu.gb_dyn.^2));
+end
+
+if (isinf(imu.ab_corr))
+ Faa = Z;
+else
+ Faa = -diag(1 ./ imu.ab_corr);
+ Fba = I;
+% Fba = -diag(sqrt (2 ./ imu.ab_corr .* imu.ab_dyn.^2));
+end
+
+if (strcmp(MAG,'OFF'))
+
+
+ F = [F11 F12 F13 DCMbn Z ;
+ F21 F22 F23 Z DCMbn ;
+ F31 F32 F33 Z Z ;
+ Z Z Z Fgg Z ;
+ Z Z Z Z Faa ;
+ ];
+
+ G = [DCMbn Z Z Z ;
+ Z DCMbn Z Z ;
+ Z Z Z Z ;
+ Z Z Fbg Z ;
+ Z Z Z Fba ;
+ ];
+
+else
+
+ F = [F11 F12 F13 DCMbn Z zeros(3,1);
+ F21 F22 F23 Z DCMbn zeros(3,1);
+ F31 F32 F33 Z Z zeros(3,1);
+ Z Z Z Fgg Z zeros(3,1);
+ Z Z Z Z Faa zeros(3,1);
+ zeros(1,16);
+ ];
+
+ G = [DCMbn Z Z Z zeros(3,1);
+ Z DCMbn Z Z zeros(3,1);
+ Z Z Z Z zeros(3,1);
+ Z Z I Z zeros(3,1);
+ Z Z Z I zeros(3,1);
+ zeros(1,12) 1;
+ ];
+end
diff --git a/ins_visual/ins_gnss_mag.m b/ins_visual/ins_gnss_mag.m
new file mode 100644
index 0000000..209ba31
--- /dev/null
+++ b/ins_visual/ins_gnss_mag.m
@@ -0,0 +1,479 @@
+function [nav_e] = ins_gnss_mag(imu, gnss, mag, att_mode)
+% ins_gnss_mag: loosely-coupled integrated navigation system.
+%
+% ins_gnss integrates IMU, GNSS and MAG measurements by using an Extended Kalman filter.
+%
+% INPUT
+% imu, IMU data structure.
+% t: Ix1 time vector (seconds).
+% fb: Ix3 accelerations vector in body frame XYZ (m/s^2).
+% wb: Ix3 turn rates vector in body frame XYZ (radians/s).
+% arw: 1x3 angle random walks (rad/s/root-Hz).
+% vrw: 1x3 velocity random walks (m/s^2/root-Hz).
+% g_std: 1x3 gyros standard deviations (radians/s).
+% a_std: 1x3 accrs standard deviations (m/s^2).
+% gb_sta: 1x3 gyros static biases or turn-on biases (radians/s).
+% ab_sta: 1x3 accrs static biases or turn-on biases (m/s^2).
+% gb_dyn: 1x3 gyros dynamic biases or bias instabilities (PAGOS ELECTRONICOS:LINK PAGOS:21988704 RED BANELCO:21988704radians/s).
+% ab_dyn: 1x3 accrs dynamic biases or bias instabilities (m/s^2).
+% gb_corr: 1x3 gyros correlation times (seconds).
+% ab_corr: 1x3 accrs correlation times (seconds).
+% gb_psd: 1x3 gyros dynamic biases PSD (rad/s/root-Hz).
+% ab_psd: 1x3 accrs dynamic biases PSD (m/s^2/root-Hz);
+% freq: 1x1 sampling frequency (Hz).
+% ini_align: 1x3 initial attitude at t(1).
+% ini_align_err: 1x3 initial attitude errors at t(1).
+%
+% gnss, GNSS data structure.
+% t: Gx1 time vector (seconds).
+% lat: Gx1 latitude (radians).
+% lon: Gx1 longitude (radians).
+% h: Gx1 altitude (m).
+% vel: Gx3 NED velocities (m/s).
+% std: 1x3 position standard deviations (rad, rad, m).
+% stdm: 1x3 position standard deviations (m, m, m).
+% stdv: 1x3 velocity standard deviations (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 current IMU time to current GNSS time vector (s).
+%
+% mag, magnetometer data structure.
+% m: Mx3 magnetometers measurements (Tesla).
+% dec: local magnetic declination (rad).
+% inc: local magnetic inclination (rad).
+% std: standard deviation (rad).
+%
+% att_mode: attitude mode string.
+% 'quaternion': attitude updated in quaternion format. Default value.
+% 'dcm': attitude updated in Direct Cosine Matrix format.
+%
+% OUTPUT
+% nav_e, INS/GNSS navigation estimates data structure.
+% t: Ix1 INS time vector (seconds).
+% tg: Gx1 GNSS time vector, when Kalman filter was executed (seconds).
+% roll: Ix1 roll (radians).
+% pitch: Ix1 pitch (radians).
+% yaw: Ix1 yaw (radians).
+% yawm: Mx1 magnetic yaw (radians).
+% vel: Ix3 NED velocities (m/s).
+% lat: Ix1 latitude (radians).
+% lon: Ix1 longitude (radians).
+% h: Ix1 altitude (m).n
+% xi: Gxn Kalman filter a priori states.
+% xp: Gxn Kalman filter a posteriori states.
+% z: Gxr INS/GNSS measurements
+% v: Gxr Kalman filter innovations.
+% b: Gxr Kalman filter biases compensations, [gb_dyn ab_dyn].
+% A: Gxn^2 Kalman filter transition-state matrices, one matrix per
+% row ordered by columns.
+% Pp: Gxn^2 Kalman filter a posteriori covariance matrices, one
+% matrix per row ordered by columns.
+% Pi: Gxn^2 Kalman filter a priori covariance matrices, one matrix
+% per row ordered by columns.
+% K: Gx(n*r) Kalman gain matrices
+% S: Gxr^2 Innovation matrices
+% ob: Gx1 Number of observable states after each GNSS data arriving
+%
+% Copyright (C) 2014, Rodrigo Gonzalez, all rights reserved.
+%
+% This file is part of NaveGo, an open-source MATLAB toolbox for
+% simulation of integrated navigation systems.
+%
+% NaveGo is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License (LGPL)
+% version 3 as published by the Free Software Foundation.
+%
+% This program is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+% GNU Lesser General Public License for more details.
+%
+% You should have received a copy of the GNU Lesser General Public
+% License along with this program. If not, see
+% .
+%
+% References:
+%
+% R. Gonzalez, J. Giribet, and H. Patiño. NaveGo: a
+% simulation framework for low-cost integrated navigation systems,
+% Journal of Control Engineering and Applied Informatics, vol. 17,
+% issue 2, pp. 110-120, 2015. Alg. 2.
+%
+% Groves, P.D. (2013), Principles of GNSS, Inertial, and
+% Multisensor Integrated Navigation Systems (2nd Ed.). Artech House.
+
+% ZUPT algothim based on Groves, Chapter 15, "INS Alignment, Zero Updates,
+% and Motion Constraints".
+%
+% ins_gps.m, ins_gnss function is based on that previous NaveGo function.
+%
+% Version: 001
+% Date: 2021/03/17
+% Author: Rodrigo Gonzalez
+% URL: https://github.com/rodralez/navego
+
+if nargin < 4, att_mode = 'quaternion'; end
+
+%% ZUPT ALGORITHM
+
+zupt_flag = false;
+
+%% PREALLOCATION
+
+% Kalman filter dimensions
+n = 15; % number of states
+r = 7; % number of sensors
+q = 12; % number of inputs
+
+% Constant matrices
+I = eye(3);
+O = zeros(3);
+
+% Length of INS time vector
+LI = length(imu.t);
+
+% Length of GNSS time vector
+LG = length(gnss.t);
+
+% Preallocation of attitude vectors
+roll_e = zeros (LI, 1);
+pitch_e = zeros (LI, 1);
+yaw_e = zeros (LI, 1);
+yawm_e = zeros (LI, 1);
+
+% Preallocation of velocity vector
+vel_e = zeros (LI, 3);
+
+% Preallocation of gravity vector
+gn_e = zeros (LI, 3);
+
+% Preallocation of position vectors
+lat_e = zeros (LI, 1);
+lon_e = zeros (LI, 1);
+h_e = zeros (LI, 1);
+
+% Preallocation of Kalman filter matrices for later performance analysis
+xi = zeros(LG, n); % Evolution of Kalman filter a priori states
+xp = zeros(LG, n); % Evolution of Kalman filter a posteriori states
+z = zeros(LG, r); % INS/GNSS measurements
+v = zeros(LG, r); % Kalman filter innovations
+
+A = zeros(LG, n^2); % Transition-state matrices
+Pi = zeros(LG, n^2); % A priori covariance matrices
+Pp = zeros(LG, n^2); % A posteriori covariance matrices
+K = zeros(LG, n*r); % Kalman gain matrices
+S = zeros(LG, r^2); % Innovation matrices
+ob = zeros(LG, 1); % Number of observable states at each GNSS data arriving
+
+b = zeros(LG, 6); % Biases compensantions after Kalman filter correction
+
+%% INITIAL VALUES AT INS TIME = 1
+
+% Initial attitude
+roll_e(1) = imu.ini_align(1);
+pitch_e(1) = imu.ini_align(2);
+yaw_e(1) = imu.ini_align(3);
+yawm_e(1) = imu.ini_align(3);
+DCMnb = euler2dcm([roll_e(1); pitch_e(1); yaw_e(1);]);
+DCMbn = DCMnb';
+qua = euler2qua([roll_e(1) pitch_e(1) yaw_e(1)]);
+
+% Initial velocity
+vel_e(1,:) = gnss.vel(1,:);
+
+% Initial position
+lat_e(1) = gnss.lat(1);
+lon_e(1) = gnss.lon(1);
+h_e(1) = gnss.h(1);
+
+% Initial dynamic biases
+gb_dyn = imu.gb_dyn';
+ab_dyn = imu.ab_dyn';
+% m_dyn = mag.b_dyn;
+
+%% INITIALIZATION OF KALMAN FILTER MATRICES
+
+% Prior estimates
+kf.xi = [ zeros(1,9), imu.gb_dyn, imu.ab_dyn ]'; % Error vector state
+kf.Pi = diag([ imu.ini_align_err, gnss.stdv, gnss.std, imu.gb_dyn, imu.ab_dyn ].^2);
+
+kf.Q = diag([ imu.arw, imu.vrw, imu.gb_psd, imu.ab_psd ].^2); % mag.psd
+
+fn = DCMbn * imu.fb(1,:)' - ab_dyn - imu.ab_sta';
+wn = DCMbn * imu.wb(1,:)' - gb_dyn - imu.gb_sta';
+
+% Vector to update matrix F
+upd = [gnss.vel(1,:) gnss.lat(1) gnss.h(1) fn' wn'];
+
+% Update matrices F and G
+[kf.F, kf.G] = F_update(upd, DCMbn, imu, 'OFF');
+
+[RM,RN] = radius(gnss.lat(1));
+Tpr = diag([(RM + gnss.h(1)), (RN + gnss.h(1)) * cos(gnss.lat(1)), -1]); % radians-to-meters
+
+% Update matrix H
+kf.H = [ %0 0 norm(mag.m(1,:)) zeros(1,13) ;
+ [0 0 1]*DCMbn zeros(1,12) ;
+ O I O O O ;
+ O O Tpr O O ; ];
+kf.R = diag([mag.std gnss.stdv gnss.stdm]).^2;
+kf.z = [mag.std, gnss.stdv, gnss.stdm]';
+
+% Propagate prior estimates to get xp(1) and Pp(1)
+kf = kf_update( kf );
+
+% Initial matrices for Kalman filter performance analysis
+xi(1,:) = kf.xi';
+xp(1,:) = kf.xp';
+Pi(1,:) = reshape(kf.Pi, 1, n^2);
+Pp(1,:) = reshape(kf.Pp, 1, n^2);
+K(1,:) = reshape(kf.K, 1, n*r);
+S(1,:) = reshape(kf.S, 1, r^2);
+v(1,:) = kf.v';
+z(1,:) = kf.z';
+b(1,:) = [gb_dyn', ab_dyn'];
+
+%% INS (IMU) TIME IS THE MASTER CLOCK
+for i = 2:LI
+
+ %% INERTIAL NAVIGATION SYSTEM (INS)
+
+ % Print a dot on console every 10,000 INS executions
+ if (mod(i,10000) == 0), fprintf('. '); end
+ % Print a return on console every 200,000 INS executions
+ if (mod(i,200000) == 0), fprintf('\n'); end
+
+ % IMU sampling interval
+ dti = imu.t(i) - imu.t(i-1);
+
+ % Turn-rates update
+ omega_ie_n = earth_rate(lat_e(i-1));
+ omega_en_n = transport_rate(lat_e(i-1), vel_e(i-1,1), vel_e(i-1,2), h_e(i-1));
+
+ % Gravity update
+ gn_e(i,:) = gravity(lat_e(i-1), h_e(i-1));
+
+ % Inertial sensors corrected with a posteriori KF biases estimation and
+ % deterministic static biases
+ wb_corrected = imu.wb(i,:)' - gb_dyn - imu.gb_sta';
+ fb_corrected = imu.fb(i,:)' - ab_dyn - imu.ab_sta';
+ fn = DCMbn * fb_corrected;
+ wn = DCMbn * wb_corrected;
+
+ % Attitude update
+ [qua, DCMbn, euler] = att_update(wb_corrected, DCMbn, qua, ...
+ omega_ie_n, omega_en_n, dti, att_mode);
+ roll_e(i) = euler(1);
+ pitch_e(i)= euler(2);
+ yaw_e(i) = euler(3);
+
+ % Velocity update
+ vel = vel_update(fn, vel_e(i-1,:), omega_ie_n, omega_en_n, gn_e(i,:)', dti);
+ vel_e (i,:) = vel;
+
+ % Position update
+ pos = pos_update([lat_e(i-1) lon_e(i-1) h_e(i-1)], vel_e(i,:), dti);
+ lat_e(i) = pos(1);
+ lon_e(i) = pos(2);
+ h_e(i) = pos(3);
+
+ % Turn-rates update with both updated velocity and position
+ omega_ie_n = earth_rate(lat_e(i));
+ omega_en_n = transport_rate(lat_e(i), vel_e(i,1), vel_e(i,2), h_e(i));
+
+ % Magnetic heading update
+ yawm_e(i) = mag_compass_update(mag.m(i,:), mag.dec, mag.inc, DCMbn', ...
+ roll_e(i), pitch_e(i));
+
+ %% ZUPT DETECTION ALGORITHM
+ idz = floor( gnss.zupt_win / dti ); % Index to set ZUPT window time
+
+ if ( i > idz )
+
+ % Mean velocity value for the ZUPT window time
+ vel_m = mean (vel_e(i-idz:i , :));
+
+ % If mean velocity value is under the ZUPT threshold velocity...
+ if (abs(vel_m) < gnss.zupt_th)
+
+ % Current attitude is equal to the mean of previous attitudes
+ % inside the ZUPT window time
+ roll_e(i) = mean (roll_e(i-idz:i , :));
+ pitch_e(i) = mean (pitch_e(i-idz:i , :));
+ yaw_e(i) = mean (yaw_e(i-idz:i , :));
+
+ % Current position is equal to the mean of previous positions
+ % inside the ZUPT window time
+ lat_e(i) = mean (lat_e(i-idz:i , :));
+ lon_e(i) = mean (lon_e(i-idz:i , :));
+ h_e(i) = mean (h_e(i-idz:i , :));
+
+ % Alternative attitude ZUPT correction
+ % roll_e(i) = (roll_e(i-idz , :));
+ % pitch_e(i) = (pitch_e(i-idz , :));
+ % yaw_e(i) = (yaw_e(i-idz, :));
+ % lat_e(i) = (lat_e(i-idz:i , :));
+ % lon_e(i) = (lon_e(i-idz:i , :));
+ % h_e(i) = (h_e(i-idz:i , :));
+
+ zupt_flag = true;
+
+ % fprintf(' z\n') % DEBUG
+ end
+ end
+
+ %% KALMAN FILTER UPDATE
+
+ % Check if there is a new GNSS measurement to process at current INS time
+ gdx = find (gnss.t >= (imu.t(i) - gnss.eps) & gnss.t < (imu.t(i) + gnss.eps));
+
+ if ( ~isempty(gdx) && gdx > 1)
+
+ % gdx % DEBUG
+
+ %% MEASUREMENTS
+
+ % Meridian and normal radii of curvature update
+ [RM,RN] = radius(lat_e(i));
+
+ % Radians-to-meters matrix
+ Tpr = diag([(RM + h_e(i)), (RN + h_e(i)) * cos(lat_e(i)), -1]);
+
+ % Position innovations in meters with lever arm correction
+ zp = Tpr * ([lat_e(i); lon_e(i); h_e(i);] - [gnss.lat(gdx); gnss.lon(gdx); gnss.h(gdx);]) ...
+ + (DCMbn * gnss.larm);
+
+ % Velocity innovations with lever arm correction
+ zv = (vel_e(i,:) - gnss.vel(gdx,:) - ((omega_ie_n + omega_en_n) .* (DCMbn * gnss.larm))' ...
+ + (DCMbn * skewm(wb_corrected) * gnss.larm )' )';
+
+ % Heading innovation
+ zy = correct_yaw ( yaw_e(i) - yawm_e(i)) ;
+ % Groves, Eq. 6.8
+% zy = correct_yaw ( ( pitch_e(i) * sin(yawm_e(i)) - roll_e(i) * cos(yawm_e(i)) ) * tan(mag.inc) );
+
+ %% KALMAN FILTER
+
+ % GNSS sampling interval
+ dtg = gnss.t(gdx) - gnss.t(gdx-1);
+
+ % Vector to update matrix F
+ upd = [vel_e(i,:) lat_e(i) h_e(i) fn' wn'];
+
+ % Matrices F and G update
+ [kf.F, kf.G] = F_update(upd, DCMbn, imu, 'OFF');
+
+ % Matrix H update
+ if(zupt_flag == false)
+ kf.H = [ [0 0 1]*DCMbn zeros(1,12) ; %
+ O I O O O ;
+ O O Tpr O O ; ];
+ kf.R = diag([mag.std gnss.stdv gnss.stdm]).^2;
+ kf.z = [ zy zv' zp' ]'; %
+ else
+ kf.H = [ O I O O O ; ];
+ kf.R = diag([gnss.stdv]).^2;
+ kf.z = zv;
+ end
+
+ % Execution of the extended Kalman filter
+ kf.xp(1:9) = 0.0; % states 1 to 9 are forced to be zero (error-state approach)
+ kf = kalman(kf, dtg);
+
+ %% OBSERVABILITY
+
+ % Number the observable states at current GNSS time
+ ob(gdx) = rank(obsv(kf.F, kf.H));
+
+ %% INS/GNSS CORRECTIONS
+
+ % Quaternion corrections
+ % Crassidis. Eq. 7.34 and A.174a.
+ antm = [0.0 qua(3) -qua(2); -qua(3) 0.0 qua(1); qua(2) -qua(1) 0.0];
+ qua = qua + 0.5 .* [qua(4)*eye(3) + antm; -1.*[qua(1) qua(2) qua(3)]] * kf.xp(1:3);
+ qua = qua / norm(qua); % Brute-force normalization
+
+ % DCM correction
+ DCMbn = qua2dcm(qua);
+
+ % Attitude correction, method 1
+ % euler = qua2euler(qua);
+ % roll_e(i) = euler(1);
+ % pitch_e(i)= euler(2);
+ % yaw_e(i) = euler(3);
+
+ % Attitude correction, method 2
+ roll_e(i) = roll_e(i) - kf.xp(1);
+ pitch_e(i) = pitch_e(i) - kf.xp(2);
+ yaw_e(i) = yaw_e(i) - kf.xp(3);
+
+ % Velocity correction
+ vel_e(i,1) = vel_e(i,1) - kf.xp(4);
+ vel_e(i,2) = vel_e(i,2) - kf.xp(5);
+ vel_e(i,3) = vel_e(i,3) - kf.xp(6);
+
+ % Position correction
+ lat_e(i) = lat_e(i) - kf.xp(7);
+ lon_e(i) = lon_e(i) - kf.xp(8);
+ h_e(i) = h_e(i) - kf.xp(9);
+
+ % Biases estimation
+ gb_dyn = -kf.xp(10:12);
+ ab_dyn = -kf.xp(13:15);
+% m_dyn = -kf.xp(16);
+
+ % Matrices for later Kalman filter performance analysis
+ xi(gdx,:) = kf.xi';
+ xp(gdx,:) = kf.xp';
+ b(gdx,:) = [gb_dyn', ab_dyn'];
+ A(gdx,:) = reshape(kf.A, 1, n^2);
+ Pi(gdx,:) = reshape(kf.Pi, 1, n^2);
+ Pp(gdx,:) = reshape(kf.Pp, 1, n^2);
+
+ if(zupt_flag == false)
+ v(gdx,:) = kf.v';
+ z(gdx,:) = kf.z';
+ K(gdx,:) = reshape(kf.K, 1, n*r);
+ S(gdx,:) = reshape(kf.S, 1, r^2);
+ else
+ zupt_flag = false;
+ z(gdx,:) = [ 0 kf.z' 0 0 0 ]';
+ v(gdx,:) = [ 0 kf.v' 0 0 0 ]';
+ K(gdx,1:n*3) = reshape(kf.K, 1, n*3);
+ S(gdx,1:9) = reshape(kf.S, 1, 9);
+ end
+ end
+end
+
+%% Summary from INS/GNSS integration
+
+nav_e.t = imu.t(1:i, :); % INS time vector
+nav_e.tg = gnss.t; % GNSS time vector, which is the time vector when the Kalman filter was executed
+nav_e.roll = roll_e(1:i, :); % Roll
+nav_e.pitch = pitch_e(1:i, :); % Pitch
+nav_e.yaw = yaw_e(1:i, :); % Yaw
+nav_e.yawm = yawm_e(1:i, :); % Magnetic heading
+nav_e.vel = vel_e(1:i, :); % NED velocities
+nav_e.lat = lat_e(1:i, :); % Latitude
+nav_e.lon = lon_e(1:i, :); % Longitude
+nav_e.h = h_e(1:i, :); % Altitude
+nav_e.gn = gn_e(1:i, :); % Gravity estimation in the nav-frame.
+
+nav_e.xi = xi; % A priori states
+nav_e.xp = xp; % A posteriori states
+nav_e.z = z; % INS/GNSS measurements
+nav_e.v = v; % Kalman filter innovations
+nav_e.b = b; % Biases compensations
+
+nav_e.A = A; % Transition matrices
+nav_e.Pi = Pi; % A priori covariance matrices
+nav_e.Pp = Pp; % A posteriori covariance matrices
+nav_e.K = K; % Kalman gain matrices
+nav_e.S = S; % Innovation matrices
+nav_e.ob = ob; % Number of observable states after each GNSS data arriving
+
+fprintf('\n');
+
+end
diff --git a/ins_visual/ins_visual.m b/ins_visual/ins_visual.m
new file mode 100644
index 0000000..a5ddbe5
--- /dev/null
+++ b/ins_visual/ins_visual.m
@@ -0,0 +1,474 @@
+function [nav_e] = ins_visual(imu, gnss, visual, att_mode)
+% ins_visual: loosely-coupled integrated navigation system.
+%
+% ins_visual integrates IMU and visual measurements by using an Extended Kalman filter.
+%
+% INPUT
+% imu, IMU data structure.
+% t: Ix1 time vector (seconds).
+% fb: Ix3 accelerations vector in body frame XYZ (m/s^2).
+% wb: Ix3 turn rates vector in body frame XYZ (radians/s).
+% arw: 1x3 angle random walks (rad/s/root-Hz).
+% vrw: 1x3 velocity random walks (m/s^2/root-Hz).
+% g_std: 1x3 gyros standard deviations (radians/s).
+% a_std: 1x3 accrs standard deviations (m/s^2).
+% gb_sta: 1x3 gyros static biases or turn-on biases (radians/s).
+% ab_sta: 1x3 accrs static biases or turn-on biases (m/s^2).
+% gb_dyn: 1x3 gyros dynamic biases or bias instabilities (radians/s).
+% ab_dyn: 1x3 accrs dynamic biases or bias instabilities (m/s^2).
+% gb_corr: 1x3 gyros correlation times (seconds).
+% ab_corr: 1x3 accrs correlation times (seconds).
+% gb_psd: 1x3 gyros dynamic biases PSD (rad/s/root-Hz).
+% ab_psd: 1x3 accrs dynamic biases PSD (m/s^2/root-Hz);
+% freq: 1x1 sampling frequency (Hz).
+% ini_align: 1x3 initial attitude at t(1).
+% ini_align_err: 1x3 initial attitude errors at t(1).
+%
+% gnss, GNSS data structure.
+% t: Gx1 time vector (seconds).
+% lat: Gx1 latitude (radians).
+% lon: Gx1 longitude (radians).
+% h: Gx1 altitude (m).
+% vel: Gx3 NED velocities (m/s).
+% std: 1x3 position standard deviations (rad, rad, m).
+% stdm: 1x3 position standard deviations (m, m, m).
+% stdv: 1x3 velocity standard deviations (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 current IMU time to current GNSS time vector (s).
+%
+% visual, visual data structure
+% t: Vx1 time vector (seconds)
+% lat: Vx1 latitude (radians).
+% lon: Vx1 longitude (radians).
+% h: Vx1 altitude (m).
+% vel: Vx3 NED velocities (m/s).
+% covvm: Vx36 velocity and position covariance matrices (m^2 and m^2/s^2).
+% covv: Vx9 velocity covariance matrices (m^2/s^2).
+% larm: 3x1 lever arm (m).
+% freq: 1x1 sampling frequency (Hz).
+% eps: 1x1 time interval to compare current IMU time to current visual time vector (s).
+%
+% att_mode: attitude mode string.
+% 'quaternion': attitude updated in quaternion format. Default value.
+% 'dcm': attitude updated in Direct Cosine Matrix format.
+%
+% OUTPUT
+% nav_e, INS/visual navigation estimates data structure.
+% t: Ix1 INS time vector (seconds).
+% tg: Vx1 VISUAL time vector, when Kalman filter was executed (seconds).
+% roll: Ix1 roll (radians).
+% pitch: Ix1 pitch (radians).
+% yaw: Ix1 yaw (radians).
+% vel: Ix3 NED velocities (m/s).
+% lat: Ix1 latitude (radians).
+% lon: Ix1 longitude (radians).
+% h: Ix1 altitude (m).
+% xi: Vxn Kalman filter a priori states.
+% xp: Vxn Kalman filter a posteriori states.
+% z: Vxr INS/visual measurements
+% v: Vxr Kalman filter innovations.
+% b: Vxr Kalman filter biases compensations, [gb_dyn ab_dyn].
+% A: Vxn^2 Kalman filter transition-state matrices, one matrix per
+% row ordered by columns.
+% Pp: Vxn^2 Kalman filter a posteriori covariance matrices, one
+% matrix per row ordered by columns.
+% Pi: Vxn^2 Kalman filter a priori covariance matrices, one matrix
+% per row ordered by columns.
+% K: Vx(n*r) Kalman gain matrices
+% S: Vxr^2 Innovation matrices
+% ob: Vx1 Number of observable states after each visual data arriving
+%
+% Copyright (C) 2014, Rodrigo Gonzalez, all rights reserved.
+%
+% This file is part of NaveGo, an open-source MATLAB toolbox for
+% simulation of integrated navigation systems.
+%
+% NaveGo is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License (LGPL)
+% version 3 as published by the Free Software Foundation.
+%
+% This program is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+% GNU Lesser General Public License for more details.
+%
+% You should have received a copy of the GNU Lesser General Public
+% License along with this program. If not, see
+% .
+%
+% References:
+%
+% R. Gonzalez, J. Giribet, and H. Patiño. NaveGo: a
+% simulation framework for low-cost integrated navigation systems,
+% Journal of Control Engineering and Applied Informatics, vol. 17,
+% issue 2, pp. 110-120, 2015. Alg. 2.
+%
+% Groves, P.D. (2013), Principles of GNSS, Inertial, and
+% Multisensor Integrated Navigation Systems (2nd Ed.). Artech House.
+
+% ZUPT algothim based on Groves, Chapter 15, "INS Alignment, Zero Updates,
+% and Motion Constraints".
+%
+% ins_gps.m, ins_gnss function is based on that previous NaveGo function.
+%
+% Version: 009
+% Date: 2021/03/16
+% Author: Rodrigo Gonzalez
+% URL: https://github.com/rodralez/navego
+
+if nargin < 3, att_mode = 'quaternion'; end
+
+%% ZUPT ALGORITHM
+
+zupt_flag = false;
+
+%% PREALLOCATION
+
+% Kalman filter dimensions
+n = 15; % number of states
+r = 6; % number of sensors
+q = 12; % number of inputs
+
+% Constant matrices
+I = eye(3);
+O = zeros(3);
+
+% Length of INS time vector
+LI = length(imu.t);
+
+% Length of visual time vector
+LV = length(visual.t);
+
+% Preallocation of attitude vectors
+roll_e = zeros (LI, 1);
+pitch_e = zeros (LI, 1);
+yaw_e = zeros (LI, 1);
+
+% Preallocation of velocity vector
+vel_e = zeros (LI, 3);
+
+% Preallocation of gravity vector
+gn_e = zeros (LI, 3);
+
+% Preallocation of position vectors
+lat_e = zeros (LI, 1);
+lon_e = zeros (LI, 1);
+h_e = zeros (LI, 1);
+
+% Preallocation of Kalman filter matrices for later performance analysis
+xi = zeros(LV, n); % Evolution of Kalman filter a priori states
+xp = zeros(LV, n); % Evolution of Kalman filter a posteriori states
+z = zeros(LV, r); % INS/visual measurements
+v = zeros(LV, r); % Kalman filter innovations
+
+A = zeros(LV, n^2); % Transition-state matrices
+Pi = zeros(LV, n^2); % A priori covariance matrices
+Pp = zeros(LV, n^2); % A posteriori covariance matrices
+K = zeros(LV, n*r); % Kalman gain matrices
+S = zeros(LV, r^2); % Innovation matrices
+ob = zeros(LV, 1); % Number of observable states at each visual data arriving
+
+b = zeros(LV, r); % Biases compensantions after Kalman filter correction
+
+%% INITIAL VALUES AT INS TIME = 1
+
+% Initial attitude
+roll_e(1) = imu.ini_align(1);
+pitch_e(1) = imu.ini_align(2);
+yaw_e(1) = imu.ini_align(3);
+DCMnb = euler2dcm([roll_e(1); pitch_e(1); yaw_e(1);]);
+DCMbn = DCMnb';
+qua = euler2qua([roll_e(1) pitch_e(1) yaw_e(1)]);
+
+% Initial velocity
+vel_e(1,:) = gnss.vel(1,:);
+
+% Initial position
+lat_e(1) = gnss.lat(1);
+lon_e(1) = gnss.lon(1);
+h_e(1) = gnss.h(1);
+
+% Initial dynamic biases
+gb_dyn = imu.gb_dyn';
+ab_dyn = imu.ab_dyn';
+
+%% INITIALIZATION OF KALMAN FILTER MATRICES
+
+% Prior estimates
+kf.xi = [ zeros(1,9), imu.gb_dyn, imu.ab_dyn ]'; % Error vector state
+kf.Pi = diag([imu.ini_align_err, gnss.stdv, gnss.std, imu.gb_dyn, imu.ab_dyn].^2);
+
+kf.Q = diag([imu.arw, imu.vrw, imu.gb_psd, imu.ab_psd].^2);
+
+fb_corrected = imu.fb(1,:)' - ab_dyn - imu.ab_sta';
+fn = DCMbn * fb_corrected;
+wn_corrected = imu.wb(1,:)' - gb_dyn - imu.gb_sta';
+wn = DCMbn * wn_corrected;
+
+% Vector to update matrix F
+upd = [gnss.vel(1,:) gnss.lat(1) gnss.h(1) fn' wn'];
+
+% Update matrices F and G
+[kf.F, kf.G] = F_update(upd, DCMbn, imu);
+
+[RM,RN] = radius(gnss.lat(1));
+Tpr = diag([(RM + gnss.h(1)), (RN + gnss.h(1)) * cos(gnss.lat(1)), -1]); % radians-to-meters
+
+% Update matrix H
+kf.H = [ O I O O O ;
+ O O Tpr O O ; ];
+kf.R = diag([gnss.stdv gnss.stdm]).^2;
+kf.z = [ gnss.stdv, gnss.stdm ]';
+
+% Propagate prior estimates to get xp(1) and Pp(1)
+kf = kf_update( kf );
+
+% Initial matrices for Kalman filter performance analysis
+xi(1,:) = kf.xi';
+xp(1,:) = kf.xp';
+Pi(1,:) = reshape(kf.Pi, 1, n^2);
+Pp(1,:) = reshape(kf.Pp, 1, n^2);
+K(1,:) = reshape(kf.K, 1, n*r);
+S(1,:) = reshape(kf.S, 1, r^2);
+v(1,:) = kf.v';
+z(1,:) = kf.z';
+b(1,:) = [gb_dyn', ab_dyn'];
+
+%% INS (IMU) TIME IS THE MASTER CLOCK
+for i = 2:LI
+
+ %% INERTIAL NAVIGATION SYSTEM (INS)
+
+ % Print a dot on console every 10,000 INS executions
+ if (mod(i,10000) == 0), fprintf('. '); end
+ % Print a return on console every 200,000 INS executions
+ if (mod(i,200000) == 0), fprintf('\n'); end
+
+ % IMU sampling interval
+ dti = imu.t(i) - imu.t(i-1);
+
+ % Turn-rates update
+ omega_ie_n = earth_rate(lat_e(i-1));
+ omega_en_n = transport_rate(lat_e(i-1), vel_e(i-1,1), vel_e(i-1,2), h_e(i-1));
+
+ % Gravity update
+ gn_e(i,:) = gravity(lat_e(i-1), h_e(i-1));
+
+ % Inertial sensors corrected with a posteriori KF biases estimation and
+ % deterministic static biases
+ wb_corrected = imu.wb(i,:)' - gb_dyn - imu.gb_sta';
+ fb_corrected = imu.fb(i,:)' - ab_dyn - imu.ab_sta';
+ fn = DCMbn * fb_corrected;
+ wn = DCMbn * wb_corrected;
+
+ % Attitude update
+ [qua, DCMbn, euler] = att_update(wb_corrected, DCMbn, qua, ...
+ omega_ie_n, omega_en_n, dti, att_mode);
+ roll_e(i) = euler(1);
+ pitch_e(i)= euler(2);
+ yaw_e(i) = euler(3);
+
+ % Velocity update
+ vel = vel_update(fn, vel_e(i-1,:), omega_ie_n, omega_en_n, gn_e(i,:)', dti);
+ vel_e (i,:) = vel;
+
+ % Position update
+ pos = pos_update([lat_e(i-1) lon_e(i-1) h_e(i-1)], vel_e(i,:), dti);
+ lat_e(i) = pos(1);
+ lon_e(i) = pos(2);
+ h_e(i) = pos(3);
+
+ % Turn-rates update with both updated velocity and position
+ omega_ie_n = earth_rate(lat_e(i));
+ omega_en_n = transport_rate(lat_e(i), vel_e(i,1), vel_e(i,2), h_e(i));
+
+ %% ZUPT DETECTION ALGORITHM
+ idz = floor( gnss.zupt_win / dti ); % Index to set ZUPT window time
+
+ if ( i > idz )
+
+ % Mean velocity value for the ZUPT window time
+ vel_m = mean (vel_e(i-idz:i , :));
+
+ % If mean velocity value is under the ZUPT threshold velocity...
+ if (abs(vel_m) < gnss.zupt_th)
+ disp("ZUPT condition applied");
+
+ % Current attitude is equal to the mean of previous attitudes
+ % inside the ZUPT window time
+ roll_e(i) = mean (roll_e(i-idz:i , :));
+ pitch_e(i) = mean (pitch_e(i-idz:i , :));
+ yaw_e(i) = mean (yaw_e(i-idz:i , :));
+
+ % Current position is equal to the mean of previous positions
+ % inside the ZUPT window time
+ lat_e(i) = mean (lat_e(i-idz:i , :));
+ lon_e(i) = mean (lon_e(i-idz:i , :));
+ h_e(i) = mean (h_e(i-idz:i , :));
+
+ % Alternative attitude ZUPT correction
+ % roll_e(i) = (roll_e(i-idz , :));
+ % pitch_e(i) = (pitch_e(i-idz , :));
+ % yaw_e(i) = (yaw_e(i-idz, :));
+ % lat_e(i) = (lat_e(i-idz:i , :));
+ % lon_e(i) = (lon_e(i-idz:i , :));
+ % h_e(i) = (h_e(i-idz:i , :));
+
+ zupt_flag = true;
+
+ % fprintf(' z\n') % DEBUG
+ end
+ end
+
+ %% KALMAN FILTER UPDATE
+
+ % Check if there is a new visual measurement to process at current INS time
+ gdx = find (visual.t >= (imu.t(i) - visual.eps) & visual.t < (imu.t(i) + visual.eps));
+
+ if (~isempty(gdx) && gdx(1) > 1)
+ for index = 1:size(gdx,1)
+ % disp("updated with visual measurements");
+
+ %% MEASUREMENTS
+
+ % Meridian and normal radii of curvature update
+ [RM,RN] = radius(lat_e(i));
+
+ % Radians-to-meters matrix
+ Tpr = diag([(RM + h_e(i)), (RN + h_e(i)) * cos(lat_e(i)), -1]);
+
+ % Position innovations in meters with lever arm correction
+ zp = Tpr * ([lat_e(i); lon_e(i); h_e(i);] - [visual.lat(gdx(index)); visual.lon(gdx(index)); visual.h(gdx(index));]) ...
+ + (DCMbn * visual.larm);
+
+ % Velocity innovations with lever arm correction
+ zv = (vel_e(i,:) - visual.vel(gdx(index),:) - ((omega_ie_n + omega_en_n) .* (DCMbn * visual.larm))' ...
+ + (DCMbn * skewm(wb_corrected) * visual.larm )' )';
+
+ %% KALMAN FILTER
+
+ % visual sampling interval
+ dtg = visual.t(gdx(index)) - visual.t(gdx(index)-1);
+
+ % Vector to update matrix F
+ upd = [vel_e(i,:) lat_e(i) h_e(i) fn' wn'];
+
+ % Matrices F and G update
+ [kf.F, kf.G] = F_update(upd, DCMbn, imu);
+
+ % Matrix H update
+ if(zupt_flag == false)
+ kf.H = [ O I O O O ;
+ O O Tpr O O ; ];
+ kf.R = reshape(visual.covvm(gdx(index),:),[6,6])';
+ kf.z = [ zv' zp' ]';
+ else
+ kf.H = [ O I O O O ; ];
+ kf.R = reshape(visual.covv(gdx(index),:),[3,3])';
+ kf.z = zv;
+ end
+
+ % Execution of the extended Kalman filter
+ kf.xp(1:9) = 0.0; % states 1 to 9 are forced to be zero (error-state approach)
+ kf = kalman(kf, dtg);
+
+ %% OBSERVABILITY
+
+ % Number the observable states at current visual time
+ ob(gdx(index)) = rank(obsv(kf.F, kf.H));
+
+ %% INS/visual CORRECTIONS
+
+ % Quaternion corrections
+ % Crassidis. Eq. 7.34 and A.174a.
+ antm = [0.0 qua(3) -qua(2); -qua(3) 0.0 qua(1); qua(2) -qua(1) 0.0];
+ qua = qua + 0.5 .* [qua(4)*eye(3) + antm; -1.*[qua(1) qua(2) qua(3)]] * kf.xp(1:3);
+ qua = qua / norm(qua); % Brute-force normalization
+
+ % DCM correction
+ DCMbn = qua2dcm(qua);
+
+ % Attitude correction, method 1
+ % euler = qua2euler(qua);
+ % roll_e(i) = euler(1);
+ % pitch_e(i)= euler(2);
+ % yaw_e(i) = euler(3);
+
+ % Attitude correction, method 2
+ roll_e(i) = roll_e(i) - kf.xp(1);
+ pitch_e(i) = pitch_e(i) - kf.xp(2);
+ yaw_e(i) = yaw_e(i) - kf.xp(3);
+
+ % Velocity correction
+ vel_e(i,1) = vel_e(i,1) - kf.xp(4);
+ vel_e(i,2) = vel_e(i,2) - kf.xp(5);
+ vel_e(i,3) = vel_e(i,3) - kf.xp(6);
+
+ % Position correction
+ lat_e(i) = lat_e(i) - kf.xp(7);
+ lon_e(i) = lon_e(i) - kf.xp(8);
+ h_e(i) = h_e(i) - kf.xp(9);
+
+ % Biases estimation
+ gb_dyn = -kf.xp(10:12);
+ ab_dyn = -kf.xp(13:15);
+
+ % Matrices for later Kalman filter performance analysis
+ xi(gdx(index),:) = kf.xi';
+ xp(gdx(index),:) = kf.xp';
+ b(gdx(index),:) = [gb_dyn', ab_dyn'];
+ A(gdx(index),:) = reshape(kf.A, 1, n^2);
+ Pi(gdx(index),:) = reshape(kf.Pi, 1, n^2);
+ Pp(gdx(index),:) = reshape(kf.Pp, 1, n^2);
+
+ if(zupt_flag == false)
+ v(gdx(index),:) = kf.v';
+ z(gdx(index),:) = kf.z';
+ K(gdx(index),:) = reshape(kf.K, 1, n*r);
+ S(gdx(index),:) = reshape(kf.S, 1, r^2);
+ else
+ zupt_flag = false;
+ z(gdx(index),:) = [ kf.z' 0 0 0 ]';
+ v(gdx(index),:) = [ kf.v' 0 0 0 ]';
+ K(gdx(index),1:n*3) = reshape(kf.K, 1, n*3);
+ S(gdx(index),1:9) = reshape(kf.S, 1, 3^2);
+ end
+ end
+ end
+
+ if imu.t(i) > max(visual.t)
+ break;
+ end
+end
+
+%% Summary from INS/visual integration
+
+nav_e.t = imu.t(1:i, :); % INS time vector
+nav_e.tg = visual.t; % visual time vector, which is the time vector when the Kalman filter was executed
+nav_e.roll = roll_e(1:i, :); % Roll
+nav_e.pitch = pitch_e(1:i, :); % Pitch
+nav_e.yaw = yaw_e(1:i, :); % Yaw
+nav_e.vel = vel_e(1:i, :); % NED velocities
+nav_e.lat = lat_e(1:i, :); % Latitude
+nav_e.lon = lon_e(1:i, :); % Longitude
+nav_e.h = h_e(1:i, :); % Altitude
+nav_e.gn = gn_e(1:i, :); % Gravity estimation in the nav-frame.
+
+nav_e.xi = xi; % A priori states
+nav_e.xp = xp; % A posteriori states
+nav_e.z = z; % INS/visual measurements
+nav_e.v = v; % Kalman filter innovations
+nav_e.b = b; % Biases compensations
+
+nav_e.A = A; % Transition matrices
+nav_e.Pi = Pi; % A priori covariance matrices
+nav_e.Pp = Pp; % A posteriori covariance matrices
+nav_e.K = K; % Kalman gain matrices
+nav_e.S = S; % Innovation matrices
+nav_e.ob = ob; % Number of observable states after each visual data arriving
+
+fprintf('\n');
+end
diff --git a/ins_visual/kalman.m b/ins_visual/kalman.m
new file mode 100644
index 0000000..8f0d508
--- /dev/null
+++ b/ins_visual/kalman.m
@@ -0,0 +1,69 @@
+function kf = kalman(kf, dt)
+% kalman: Kalman filter algorithm.
+%
+% INPUT
+% kf, data structure with at least the following fields:
+% xp: nx1 a posteriori state vector (old).
+% z: rx1 measurement vector.
+% F: nxn state transition matrix.
+% H: rxn observation matrix.
+% Q: qxq process noise covariance matrix.
+% R: rxr observation noise covariance matrix.
+% Pp: nxn a posteriori error covariance matrix.
+% G: nxq control-input matrix.
+% dt: sampling interval.
+%
+% OUTPUT
+% kf, the following fields are updated:
+% xi: nx1 a priori state vector (updated).
+% xp: nx1 a posteriori state vector (updated).
+% v: rx1 innovation vector.
+% A: nxn state transition matrix.
+% K: nxr Kalman gain matrix.
+% Qd: nxn discrete process noise covariance matrix.
+% Pi: nxn a priori error covariance.
+% Pp: nxn a posteriori error covariance.
+% S: rxr innovation (not residual) covariance matrix.
+%
+% Copyright (C) 2014, Rodrigo Gonzalez, all rights reserved.
+%
+% This file is part of NaveGo, an open-source MATLAB toolbox for
+% simulation of integrated navigation systems.
+%
+% NaveGo is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License (LGPL)
+% version 3 as published by the Free Software Foundation.
+%
+% This program is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+% GNU Lesser General Public License for more details.
+%
+% You should have received a copy of the GNU Lesser General Public
+% License along with this program. If not, see
+% .
+%
+% Reference:
+%
+% R. Gonzalez, J. Giribet, and H. Patiño. NaveGo: a
+% simulation framework for low-cost integrated navigation systems,
+% Journal of Control Engineering and Applied Informatics, vol. 17,
+% issue 2, pp. 110-120, 2015. Alg. 1.
+%
+% Dan Simon. Optimal State Estimation. Chapter 5. John Wiley
+% & Sons. 2006.
+%
+% Version: 007
+% Date: 2019/04/19
+% Author: Rodrigo Gonzalez
+% URL: https://github.com/rodralez/navego
+
+%% PREDICTION STEP
+
+kf = kf_prediction(kf, dt);
+
+%% UPDATE STEP
+
+kf = kf_update(kf);
+
+end
diff --git a/ins_visual/kf_prediction.m b/ins_visual/kf_prediction.m
new file mode 100644
index 0000000..547d4f3
--- /dev/null
+++ b/ins_visual/kf_prediction.m
@@ -0,0 +1,65 @@
+function kf = kf_prediction(kf, dt)
+% kf_prediction: prediction update part of the Kalman filter algorithm.
+%
+% INPUT
+% kf, data structure with at least the following fields,
+% xp: nx1 a posteriori state vector (old).
+% Pp: nxn a posteriori error covariance matrix (old).
+% F: nxn state transition matrix.
+% Q: qxq process noise covariance matrix.
+% G: nxq control-input matrix.
+% dt: sampling interval.
+%
+% OUTPUT
+% kf, the following fields are updated,
+% xi: nx1 a priori state vector (updated).
+% Pi: nxn a priori error covariance matrix.
+% A: nxn state transition matrix.
+% Qd: nxn discrete process noise covariance matrix.
+%
+% Copyright (C) 2014, Rodrigo Gonzalez, all rights reserved.
+%
+% This file is part of NaveGo, an open-source MATLAB toolbox for
+% simulation of integrated navigation systems.
+%
+% NaveGo is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License (LGPL)
+% version 3 as published by the Free Software Foundation.
+%
+% This program is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+% GNU Lesser General Public License for more details.
+%
+% You should have received a copy of the GNU Lesser General Public
+% License along with this program. If not, see
+% .
+%
+% Reference:
+%
+% R. Gonzalez, J. Giribet, and H. Patiño. NaveGo: a
+% simulation framework for low-cost integrated navigation systems,
+% Journal of Control Engineering and Applied Informatics, vol. 17,
+% issue 2, pp. 110-120, 2015. Alg. 1.
+%
+% Dan Simon. Optimal State Estimation. Chapter 5. John Wiley
+% & Sons. 2006.
+%
+% Version: 001
+% Date: 2019/04/19
+% Author: Rodrigo Gonzalez
+% URL: https://github.com/rodralez/navego
+
+% Discretization of continous-time system
+kf.A = expm(kf.F * dt); % Exact solution for linear systems
+% kf.A = I + (kf.F * dt); % Approximated solution by Euler method
+kf.Qd = (kf.G * kf.Q * kf.G') .* dt; % Digitalized covariance matrix
+
+% Step 1, predict the a priori state vector, xi
+kf.xi = kf.A * kf.xp;
+
+% Step 2, update the a priori covariance matrix, Pi
+kf.Pi = (kf.A * kf.Pp * kf.A') + kf.Qd;
+kf.Pi = 0.5 .* (kf.Pi + kf.Pi'); % Force Pi to be a symmetric matrix
+
+end
diff --git a/ins_visual/kf_update.m b/ins_visual/kf_update.m
new file mode 100644
index 0000000..6ef9451
--- /dev/null
+++ b/ins_visual/kf_update.m
@@ -0,0 +1,75 @@
+function kf = kf_update(kf)
+% kf_update: measurement update part of the Kalman filter algorithm.
+%
+% INPUT
+% kf, data structure with at least the following fields,
+% xi: nx1 a priori state vector.
+% Pi: nxn a priori error covariance matrix.
+% z: rx1 measurement vector.
+% H: rxn observation matrix.
+% R: rxr observation noise covariance matrix.
+%
+% OUTPUT
+% kf, the following fields are updated,
+% xp: nx1 a posteriori state vector (updated).
+% Pp: nxn a posteriori error covariance matrix (updated).
+% v: rx1 innovation vector.
+% K: nxr Kalman gain matrix.
+% S: rxr innovation (not residual) covariance matrix.
+%
+% Copyright (C) 2014, Rodrigo Gonzalez, all rights reserved.
+%
+% This file is part of NaveGo, an open-source MATLAB toolbox for
+% simulation of integrated navigation systems.
+%
+% NaveGo is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License (LGPL)
+% version 3 as published by the Free Software Foundation.
+%
+% This program is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+% GNU Lesser General Public License for more details.
+%
+% You should have received a copy of the GNU Lesser General Public
+% License along with this program. If not, see
+% .
+%
+% Reference:
+%
+% R. Gonzalez, J. Giribet, and H. Patiño. NaveGo: a
+% simulation framework for low-cost integrated navigation systems,
+% Journal of Control Engineering and Applied Informatics, vol. 17,
+% issue 2, pp. 110-120, 2015. Alg. 1.
+%
+% Dan Simon. Optimal State Estimation. Chapter 5. John Wiley
+% & Sons. 2006.
+%
+% Version: 002
+% Date: 2021/03/21
+% Author: Rodrigo Gonzalez
+% URL: https://github.com/rodralez/navego
+
+% I = eye(size(kf.F));
+
+% Step 3, update Kalman gain
+kf.S = (kf.R + kf.H * kf.Pi * kf.H'); % Innovation covariance matrix
+kf.v = kf.z - kf.H * kf.xi; % Innovation vector
+
+r = length(kf.v);
+if rank (kf.S) < r
+ error('kf_update: innovation covariance matrix S is not invertable.')
+end
+
+kf.K = (kf.Pi * kf.H') * (kf.S)^(-1) ; % Kalman gain matrix
+
+% Step 4, update the a posteriori state vector, xp
+kf.xp = kf.xi + kf.K * kf.v;
+
+% Step 5, update the a posteriori covariance matrix, Pp
+kf.Pp = kf.Pi - kf.K * kf.S * kf.K';
+% J = (I - S.K * S.H); % Joseph stabilized version
+% S.Pp = J * S.Pi * J' + S.K * S.R * S.K'; % Alternative implementation
+kf.Pp = 0.5 .* (kf.Pp + kf.Pp'); % Force Pp to be a symmetric matrix
+
+end
diff --git a/ins_visual_gnss/F_update.m b/ins_visual_gnss/F_update.m
new file mode 100644
index 0000000..7368293
--- /dev/null
+++ b/ins_visual_gnss/F_update.m
@@ -0,0 +1,220 @@
+function [F, G] = F_update(upd, DCMbn, imu, MAG)
+% F_update: updates F and G matrices before the execution of Kalman filter.
+%
+% INPUT
+% upd, 1x8 vector with data from the INS.
+% DCMbn, DCM body-to-nav.
+% imu, IMU data structure.
+%
+% OUTPUT
+% F, 15x15 state transition matrix.
+% G, 15x12 control-input matrix.
+%
+% Copyright (C) 2014, Rodrigo Gonzalez, all rights reserved.
+%
+% This file is part of NaveGo, an open-source MATLAB toolbox for
+% simulation of integrated navigation systems.
+%
+% NaveGo is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License (LGPL)
+% version 3 as published by the Free Software Foundation.
+%
+% This program is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+% GNU Lesser General Public License for more details.
+%
+% You should have received a copy of the GNU Lesser General Public
+% License along with this program. If not, see
+% .
+%
+% References:
+%
+% Titterton, D.H. and Weston, J.L. (2004). Strapdown
+% Inertial Navigation Technology (2nd Ed.). Institution
+% of Engineering and Technology, USA. Eq. 12.18, p. 345, matrix F.
+%
+% Farrell, J. (2008). Aided Navigation: GPS With High Rate
+% Sensors. McGraw-Hill Professional, USA. Eq. 11.108, p. 407, matrix G.
+%
+% R. Gonzalez, J. Giribet, and H. Patiño. NaveGo: a
+% simulation framework for low-cost integrated navigation systems,
+% Journal of Control Engineering and Applied Informatics, vol. 17,
+% issue 2, pp. 110-120, 2015. Eq. 26.
+%
+% R. Gonzalez, J. Giribet, and H. Patiño. An approach to
+% benchmarking of loosely coupled low-cost navigation systems,
+% Mathematical and Computer Modelling of Dynamical Systems, vol. 21,
+% issue 3, pp. 272-287, 2015. Eq. 22.
+%
+% Version: 007
+% Date: 2021/03/26
+% Author: Rodrigo Gonzalez
+% URL: https://github.com/rodralez/navego
+
+if nargin < 4, MAG = 'OFF'; end
+
+Vn = upd(1);
+Ve = upd(2);
+Vd = upd(3);
+lat = upd(4);
+h = upd(5);
+
+fn = upd(6:8);
+wn = upd(9:11);
+
+Om = 7.292115e-5;
+I = eye(3);
+Z = zeros(3);
+
+[RM,RN] = radius(lat);
+
+RO = sqrt(RN*RM) + h;
+
+%% ATTITUDE MATRICES
+
+a11 = 0;
+a12 = -( (Om * sin(lat)) + (Ve / RO * tan(lat)) );
+a13 = Vn / RO;
+a21 = (Om * sin(lat)) + (Ve / RO * tan(lat));
+a22 = 0 ;
+a23 = (Om * cos(lat)) + (Ve / RO) ;
+a31 = -Vn / RO;
+a32 = -Om * cos(lat) - (Ve / RO);
+a33 = 0;
+F11 = [a11 a12 a13; a21 a22 a23; a31 a32 a33;];
+
+% Groves, 14.64
+% F11 = skewm(wn);
+
+a11 = 0;
+a12 = 1 / RO;
+a13 = 0;
+a21 = -1 / RO;
+a22 = 0;
+a23 = 0;
+a31 = 0;
+a32 = -tan(lat) / RO;
+a33 = 0;
+F12 = [a11 a12 a13; a21 a22 a23; a31 a32 a33;];
+
+a11 = -Om * sin(lat);
+a12 = 0;
+a13 = -Ve / (RO^2);
+a21 = 0 ;
+a22 = 0 ;
+a23 = Vn / (RO^2);
+a31 = -Om * cos(lat) - (Ve / ((RO) * (cos(lat))^2));
+a32 = 0 ;
+a33 = (Ve * tan(lat)) / (RO^2) ;
+F13 = [a11 a12 a13; a21 a22 a23; a31 a32 a33;];
+
+%% VELOCITY MATRICES
+
+F21 = skewm(fn);
+
+a11 = Vd / RO;
+a12 = -2 * ((Om * sin(lat)) + ((Ve / RO) * tan(lat))) ;
+a13 = Vn / RO ;
+a21 = (2 * Om * sin(lat)) + ( (Ve / RO) * tan(lat) );
+a22 = (1 / RO) * ((Vn * tan(lat)) + Vd) ;
+a23 = 2 * Om * cos(lat) + (Ve / RO);
+a31 = (-2 * Vn) / RO;
+a32 = -2 * (Om * cos(lat) + (Ve / RO)) ;
+a33 = 0;
+F22 = [a11 a12 a13; a21 a22 a23; a31 a32 a33;];
+
+e = 0.0818191908425; % WGS84 eccentricity
+res = RN * sqrt( cos(lat)^2 + (1-e^2)^2 * sin(lat)^2);
+g = gravity(lat,h);
+g0 = g(3);
+
+a11 = -Ve * ((2 * Om * cos(lat)) + (Ve / (RO * (cos(lat))^2)));
+a12 = 0 ;
+a13 = (1 / RO^2) * ( (Ve^2 * tan(lat)) - (Vn * Vd) );
+a21 = 2 * Om * ( (Vn * cos(lat)) - (Vd * sin(lat)) ) + ( (Vn * Ve) / (RO * (cos(lat))^2) ) ;
+a22 = 0 ;
+a23 = -(Ve / RO^2) * (Vn * tan(lat) + Vd);
+a31 = 2 * Om * Ve * sin(lat);
+a32 = 0;
+% a33 = (1 / RO^2) * (Vn^2 + Ve^2);
+a33 = Ve^2 / (RN+h)^2 + Vn^2 / (RM+h)^2 - 2 * g0 / res;
+F23 = [a11 a12 a13; a21 a22 a23; a31 a32 a33;];
+
+%% POSITIONING MATRICES
+
+F31 = zeros(3);
+
+a11 = 1 / RO;
+a12 = 0;
+a13 = 0;
+a21 = 0;
+a22 = 1 / (RO * cos(lat));
+a23 = 0;
+a31 = 0;
+a32 = 0;
+a33 = -1;
+F32 = [a11 a12 a13; a21 a22 a23; a31 a32 a33;];
+
+a11 = 0;
+a12 = 0;
+a13 = -Vn / RO^2;
+a21 = (Ve * tan(lat)) / (RO * cos(lat));
+a22 = 0;
+a23 = -Ve / (RO^2 * cos(lat));
+a31 = 0;
+a32 = 0;
+a33 = 0;
+F33 = [a11 a12 a13; a21 a22 a23; a31 a32 a33;];
+
+if (isinf(imu.gb_corr))
+ Fgg = Z;
+else
+ Fgg = -diag( 1./ imu.gb_corr);
+ Fbg = I;
+% Fbg = -diag(sqrt (2 ./ imu.gb_corr .* imu.gb_dyn.^2));
+end
+
+if (isinf(imu.ab_corr))
+ Faa = Z;
+else
+ Faa = -diag(1 ./ imu.ab_corr);
+ Fba = I;
+% Fba = -diag(sqrt (2 ./ imu.ab_corr .* imu.ab_dyn.^2));
+end
+
+if (strcmp(MAG,'OFF'))
+
+
+ F = [F11 F12 F13 DCMbn Z ;
+ F21 F22 F23 Z DCMbn ;
+ F31 F32 F33 Z Z ;
+ Z Z Z Fgg Z ;
+ Z Z Z Z Faa ;
+ ];
+
+ G = [DCMbn Z Z Z ;
+ Z DCMbn Z Z ;
+ Z Z Z Z ;
+ Z Z Fbg Z ;
+ Z Z Z Fba ;
+ ];
+
+else
+
+ F = [F11 F12 F13 DCMbn Z zeros(3,1);
+ F21 F22 F23 Z DCMbn zeros(3,1);
+ F31 F32 F33 Z Z zeros(3,1);
+ Z Z Z Fgg Z zeros(3,1);
+ Z Z Z Z Faa zeros(3,1);
+ zeros(1,16);
+ ];
+
+ G = [DCMbn Z Z Z zeros(3,1);
+ Z DCMbn Z Z zeros(3,1);
+ Z Z Z Z zeros(3,1);
+ Z Z I Z zeros(3,1);
+ Z Z Z I zeros(3,1);
+ zeros(1,12) 1;
+ ];
+end
diff --git a/ins_visual_gnss/ins_gnss_mag.m b/ins_visual_gnss/ins_gnss_mag.m
new file mode 100644
index 0000000..209ba31
--- /dev/null
+++ b/ins_visual_gnss/ins_gnss_mag.m
@@ -0,0 +1,479 @@
+function [nav_e] = ins_gnss_mag(imu, gnss, mag, att_mode)
+% ins_gnss_mag: loosely-coupled integrated navigation system.
+%
+% ins_gnss integrates IMU, GNSS and MAG measurements by using an Extended Kalman filter.
+%
+% INPUT
+% imu, IMU data structure.
+% t: Ix1 time vector (seconds).
+% fb: Ix3 accelerations vector in body frame XYZ (m/s^2).
+% wb: Ix3 turn rates vector in body frame XYZ (radians/s).
+% arw: 1x3 angle random walks (rad/s/root-Hz).
+% vrw: 1x3 velocity random walks (m/s^2/root-Hz).
+% g_std: 1x3 gyros standard deviations (radians/s).
+% a_std: 1x3 accrs standard deviations (m/s^2).
+% gb_sta: 1x3 gyros static biases or turn-on biases (radians/s).
+% ab_sta: 1x3 accrs static biases or turn-on biases (m/s^2).
+% gb_dyn: 1x3 gyros dynamic biases or bias instabilities (PAGOS ELECTRONICOS:LINK PAGOS:21988704 RED BANELCO:21988704radians/s).
+% ab_dyn: 1x3 accrs dynamic biases or bias instabilities (m/s^2).
+% gb_corr: 1x3 gyros correlation times (seconds).
+% ab_corr: 1x3 accrs correlation times (seconds).
+% gb_psd: 1x3 gyros dynamic biases PSD (rad/s/root-Hz).
+% ab_psd: 1x3 accrs dynamic biases PSD (m/s^2/root-Hz);
+% freq: 1x1 sampling frequency (Hz).
+% ini_align: 1x3 initial attitude at t(1).
+% ini_align_err: 1x3 initial attitude errors at t(1).
+%
+% gnss, GNSS data structure.
+% t: Gx1 time vector (seconds).
+% lat: Gx1 latitude (radians).
+% lon: Gx1 longitude (radians).
+% h: Gx1 altitude (m).
+% vel: Gx3 NED velocities (m/s).
+% std: 1x3 position standard deviations (rad, rad, m).
+% stdm: 1x3 position standard deviations (m, m, m).
+% stdv: 1x3 velocity standard deviations (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 current IMU time to current GNSS time vector (s).
+%
+% mag, magnetometer data structure.
+% m: Mx3 magnetometers measurements (Tesla).
+% dec: local magnetic declination (rad).
+% inc: local magnetic inclination (rad).
+% std: standard deviation (rad).
+%
+% att_mode: attitude mode string.
+% 'quaternion': attitude updated in quaternion format. Default value.
+% 'dcm': attitude updated in Direct Cosine Matrix format.
+%
+% OUTPUT
+% nav_e, INS/GNSS navigation estimates data structure.
+% t: Ix1 INS time vector (seconds).
+% tg: Gx1 GNSS time vector, when Kalman filter was executed (seconds).
+% roll: Ix1 roll (radians).
+% pitch: Ix1 pitch (radians).
+% yaw: Ix1 yaw (radians).
+% yawm: Mx1 magnetic yaw (radians).
+% vel: Ix3 NED velocities (m/s).
+% lat: Ix1 latitude (radians).
+% lon: Ix1 longitude (radians).
+% h: Ix1 altitude (m).n
+% xi: Gxn Kalman filter a priori states.
+% xp: Gxn Kalman filter a posteriori states.
+% z: Gxr INS/GNSS measurements
+% v: Gxr Kalman filter innovations.
+% b: Gxr Kalman filter biases compensations, [gb_dyn ab_dyn].
+% A: Gxn^2 Kalman filter transition-state matrices, one matrix per
+% row ordered by columns.
+% Pp: Gxn^2 Kalman filter a posteriori covariance matrices, one
+% matrix per row ordered by columns.
+% Pi: Gxn^2 Kalman filter a priori covariance matrices, one matrix
+% per row ordered by columns.
+% K: Gx(n*r) Kalman gain matrices
+% S: Gxr^2 Innovation matrices
+% ob: Gx1 Number of observable states after each GNSS data arriving
+%
+% Copyright (C) 2014, Rodrigo Gonzalez, all rights reserved.
+%
+% This file is part of NaveGo, an open-source MATLAB toolbox for
+% simulation of integrated navigation systems.
+%
+% NaveGo is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License (LGPL)
+% version 3 as published by the Free Software Foundation.
+%
+% This program is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+% GNU Lesser General Public License for more details.
+%
+% You should have received a copy of the GNU Lesser General Public
+% License along with this program. If not, see
+% .
+%
+% References:
+%
+% R. Gonzalez, J. Giribet, and H. Patiño. NaveGo: a
+% simulation framework for low-cost integrated navigation systems,
+% Journal of Control Engineering and Applied Informatics, vol. 17,
+% issue 2, pp. 110-120, 2015. Alg. 2.
+%
+% Groves, P.D. (2013), Principles of GNSS, Inertial, and
+% Multisensor Integrated Navigation Systems (2nd Ed.). Artech House.
+
+% ZUPT algothim based on Groves, Chapter 15, "INS Alignment, Zero Updates,
+% and Motion Constraints".
+%
+% ins_gps.m, ins_gnss function is based on that previous NaveGo function.
+%
+% Version: 001
+% Date: 2021/03/17
+% Author: Rodrigo Gonzalez
+% URL: https://github.com/rodralez/navego
+
+if nargin < 4, att_mode = 'quaternion'; end
+
+%% ZUPT ALGORITHM
+
+zupt_flag = false;
+
+%% PREALLOCATION
+
+% Kalman filter dimensions
+n = 15; % number of states
+r = 7; % number of sensors
+q = 12; % number of inputs
+
+% Constant matrices
+I = eye(3);
+O = zeros(3);
+
+% Length of INS time vector
+LI = length(imu.t);
+
+% Length of GNSS time vector
+LG = length(gnss.t);
+
+% Preallocation of attitude vectors
+roll_e = zeros (LI, 1);
+pitch_e = zeros (LI, 1);
+yaw_e = zeros (LI, 1);
+yawm_e = zeros (LI, 1);
+
+% Preallocation of velocity vector
+vel_e = zeros (LI, 3);
+
+% Preallocation of gravity vector
+gn_e = zeros (LI, 3);
+
+% Preallocation of position vectors
+lat_e = zeros (LI, 1);
+lon_e = zeros (LI, 1);
+h_e = zeros (LI, 1);
+
+% Preallocation of Kalman filter matrices for later performance analysis
+xi = zeros(LG, n); % Evolution of Kalman filter a priori states
+xp = zeros(LG, n); % Evolution of Kalman filter a posteriori states
+z = zeros(LG, r); % INS/GNSS measurements
+v = zeros(LG, r); % Kalman filter innovations
+
+A = zeros(LG, n^2); % Transition-state matrices
+Pi = zeros(LG, n^2); % A priori covariance matrices
+Pp = zeros(LG, n^2); % A posteriori covariance matrices
+K = zeros(LG, n*r); % Kalman gain matrices
+S = zeros(LG, r^2); % Innovation matrices
+ob = zeros(LG, 1); % Number of observable states at each GNSS data arriving
+
+b = zeros(LG, 6); % Biases compensantions after Kalman filter correction
+
+%% INITIAL VALUES AT INS TIME = 1
+
+% Initial attitude
+roll_e(1) = imu.ini_align(1);
+pitch_e(1) = imu.ini_align(2);
+yaw_e(1) = imu.ini_align(3);
+yawm_e(1) = imu.ini_align(3);
+DCMnb = euler2dcm([roll_e(1); pitch_e(1); yaw_e(1);]);
+DCMbn = DCMnb';
+qua = euler2qua([roll_e(1) pitch_e(1) yaw_e(1)]);
+
+% Initial velocity
+vel_e(1,:) = gnss.vel(1,:);
+
+% Initial position
+lat_e(1) = gnss.lat(1);
+lon_e(1) = gnss.lon(1);
+h_e(1) = gnss.h(1);
+
+% Initial dynamic biases
+gb_dyn = imu.gb_dyn';
+ab_dyn = imu.ab_dyn';
+% m_dyn = mag.b_dyn;
+
+%% INITIALIZATION OF KALMAN FILTER MATRICES
+
+% Prior estimates
+kf.xi = [ zeros(1,9), imu.gb_dyn, imu.ab_dyn ]'; % Error vector state
+kf.Pi = diag([ imu.ini_align_err, gnss.stdv, gnss.std, imu.gb_dyn, imu.ab_dyn ].^2);
+
+kf.Q = diag([ imu.arw, imu.vrw, imu.gb_psd, imu.ab_psd ].^2); % mag.psd
+
+fn = DCMbn * imu.fb(1,:)' - ab_dyn - imu.ab_sta';
+wn = DCMbn * imu.wb(1,:)' - gb_dyn - imu.gb_sta';
+
+% Vector to update matrix F
+upd = [gnss.vel(1,:) gnss.lat(1) gnss.h(1) fn' wn'];
+
+% Update matrices F and G
+[kf.F, kf.G] = F_update(upd, DCMbn, imu, 'OFF');
+
+[RM,RN] = radius(gnss.lat(1));
+Tpr = diag([(RM + gnss.h(1)), (RN + gnss.h(1)) * cos(gnss.lat(1)), -1]); % radians-to-meters
+
+% Update matrix H
+kf.H = [ %0 0 norm(mag.m(1,:)) zeros(1,13) ;
+ [0 0 1]*DCMbn zeros(1,12) ;
+ O I O O O ;
+ O O Tpr O O ; ];
+kf.R = diag([mag.std gnss.stdv gnss.stdm]).^2;
+kf.z = [mag.std, gnss.stdv, gnss.stdm]';
+
+% Propagate prior estimates to get xp(1) and Pp(1)
+kf = kf_update( kf );
+
+% Initial matrices for Kalman filter performance analysis
+xi(1,:) = kf.xi';
+xp(1,:) = kf.xp';
+Pi(1,:) = reshape(kf.Pi, 1, n^2);
+Pp(1,:) = reshape(kf.Pp, 1, n^2);
+K(1,:) = reshape(kf.K, 1, n*r);
+S(1,:) = reshape(kf.S, 1, r^2);
+v(1,:) = kf.v';
+z(1,:) = kf.z';
+b(1,:) = [gb_dyn', ab_dyn'];
+
+%% INS (IMU) TIME IS THE MASTER CLOCK
+for i = 2:LI
+
+ %% INERTIAL NAVIGATION SYSTEM (INS)
+
+ % Print a dot on console every 10,000 INS executions
+ if (mod(i,10000) == 0), fprintf('. '); end
+ % Print a return on console every 200,000 INS executions
+ if (mod(i,200000) == 0), fprintf('\n'); end
+
+ % IMU sampling interval
+ dti = imu.t(i) - imu.t(i-1);
+
+ % Turn-rates update
+ omega_ie_n = earth_rate(lat_e(i-1));
+ omega_en_n = transport_rate(lat_e(i-1), vel_e(i-1,1), vel_e(i-1,2), h_e(i-1));
+
+ % Gravity update
+ gn_e(i,:) = gravity(lat_e(i-1), h_e(i-1));
+
+ % Inertial sensors corrected with a posteriori KF biases estimation and
+ % deterministic static biases
+ wb_corrected = imu.wb(i,:)' - gb_dyn - imu.gb_sta';
+ fb_corrected = imu.fb(i,:)' - ab_dyn - imu.ab_sta';
+ fn = DCMbn * fb_corrected;
+ wn = DCMbn * wb_corrected;
+
+ % Attitude update
+ [qua, DCMbn, euler] = att_update(wb_corrected, DCMbn, qua, ...
+ omega_ie_n, omega_en_n, dti, att_mode);
+ roll_e(i) = euler(1);
+ pitch_e(i)= euler(2);
+ yaw_e(i) = euler(3);
+
+ % Velocity update
+ vel = vel_update(fn, vel_e(i-1,:), omega_ie_n, omega_en_n, gn_e(i,:)', dti);
+ vel_e (i,:) = vel;
+
+ % Position update
+ pos = pos_update([lat_e(i-1) lon_e(i-1) h_e(i-1)], vel_e(i,:), dti);
+ lat_e(i) = pos(1);
+ lon_e(i) = pos(2);
+ h_e(i) = pos(3);
+
+ % Turn-rates update with both updated velocity and position
+ omega_ie_n = earth_rate(lat_e(i));
+ omega_en_n = transport_rate(lat_e(i), vel_e(i,1), vel_e(i,2), h_e(i));
+
+ % Magnetic heading update
+ yawm_e(i) = mag_compass_update(mag.m(i,:), mag.dec, mag.inc, DCMbn', ...
+ roll_e(i), pitch_e(i));
+
+ %% ZUPT DETECTION ALGORITHM
+ idz = floor( gnss.zupt_win / dti ); % Index to set ZUPT window time
+
+ if ( i > idz )
+
+ % Mean velocity value for the ZUPT window time
+ vel_m = mean (vel_e(i-idz:i , :));
+
+ % If mean velocity value is under the ZUPT threshold velocity...
+ if (abs(vel_m) < gnss.zupt_th)
+
+ % Current attitude is equal to the mean of previous attitudes
+ % inside the ZUPT window time
+ roll_e(i) = mean (roll_e(i-idz:i , :));
+ pitch_e(i) = mean (pitch_e(i-idz:i , :));
+ yaw_e(i) = mean (yaw_e(i-idz:i , :));
+
+ % Current position is equal to the mean of previous positions
+ % inside the ZUPT window time
+ lat_e(i) = mean (lat_e(i-idz:i , :));
+ lon_e(i) = mean (lon_e(i-idz:i , :));
+ h_e(i) = mean (h_e(i-idz:i , :));
+
+ % Alternative attitude ZUPT correction
+ % roll_e(i) = (roll_e(i-idz , :));
+ % pitch_e(i) = (pitch_e(i-idz , :));
+ % yaw_e(i) = (yaw_e(i-idz, :));
+ % lat_e(i) = (lat_e(i-idz:i , :));
+ % lon_e(i) = (lon_e(i-idz:i , :));
+ % h_e(i) = (h_e(i-idz:i , :));
+
+ zupt_flag = true;
+
+ % fprintf(' z\n') % DEBUG
+ end
+ end
+
+ %% KALMAN FILTER UPDATE
+
+ % Check if there is a new GNSS measurement to process at current INS time
+ gdx = find (gnss.t >= (imu.t(i) - gnss.eps) & gnss.t < (imu.t(i) + gnss.eps));
+
+ if ( ~isempty(gdx) && gdx > 1)
+
+ % gdx % DEBUG
+
+ %% MEASUREMENTS
+
+ % Meridian and normal radii of curvature update
+ [RM,RN] = radius(lat_e(i));
+
+ % Radians-to-meters matrix
+ Tpr = diag([(RM + h_e(i)), (RN + h_e(i)) * cos(lat_e(i)), -1]);
+
+ % Position innovations in meters with lever arm correction
+ zp = Tpr * ([lat_e(i); lon_e(i); h_e(i);] - [gnss.lat(gdx); gnss.lon(gdx); gnss.h(gdx);]) ...
+ + (DCMbn * gnss.larm);
+
+ % Velocity innovations with lever arm correction
+ zv = (vel_e(i,:) - gnss.vel(gdx,:) - ((omega_ie_n + omega_en_n) .* (DCMbn * gnss.larm))' ...
+ + (DCMbn * skewm(wb_corrected) * gnss.larm )' )';
+
+ % Heading innovation
+ zy = correct_yaw ( yaw_e(i) - yawm_e(i)) ;
+ % Groves, Eq. 6.8
+% zy = correct_yaw ( ( pitch_e(i) * sin(yawm_e(i)) - roll_e(i) * cos(yawm_e(i)) ) * tan(mag.inc) );
+
+ %% KALMAN FILTER
+
+ % GNSS sampling interval
+ dtg = gnss.t(gdx) - gnss.t(gdx-1);
+
+ % Vector to update matrix F
+ upd = [vel_e(i,:) lat_e(i) h_e(i) fn' wn'];
+
+ % Matrices F and G update
+ [kf.F, kf.G] = F_update(upd, DCMbn, imu, 'OFF');
+
+ % Matrix H update
+ if(zupt_flag == false)
+ kf.H = [ [0 0 1]*DCMbn zeros(1,12) ; %
+ O I O O O ;
+ O O Tpr O O ; ];
+ kf.R = diag([mag.std gnss.stdv gnss.stdm]).^2;
+ kf.z = [ zy zv' zp' ]'; %
+ else
+ kf.H = [ O I O O O ; ];
+ kf.R = diag([gnss.stdv]).^2;
+ kf.z = zv;
+ end
+
+ % Execution of the extended Kalman filter
+ kf.xp(1:9) = 0.0; % states 1 to 9 are forced to be zero (error-state approach)
+ kf = kalman(kf, dtg);
+
+ %% OBSERVABILITY
+
+ % Number the observable states at current GNSS time
+ ob(gdx) = rank(obsv(kf.F, kf.H));
+
+ %% INS/GNSS CORRECTIONS
+
+ % Quaternion corrections
+ % Crassidis. Eq. 7.34 and A.174a.
+ antm = [0.0 qua(3) -qua(2); -qua(3) 0.0 qua(1); qua(2) -qua(1) 0.0];
+ qua = qua + 0.5 .* [qua(4)*eye(3) + antm; -1.*[qua(1) qua(2) qua(3)]] * kf.xp(1:3);
+ qua = qua / norm(qua); % Brute-force normalization
+
+ % DCM correction
+ DCMbn = qua2dcm(qua);
+
+ % Attitude correction, method 1
+ % euler = qua2euler(qua);
+ % roll_e(i) = euler(1);
+ % pitch_e(i)= euler(2);
+ % yaw_e(i) = euler(3);
+
+ % Attitude correction, method 2
+ roll_e(i) = roll_e(i) - kf.xp(1);
+ pitch_e(i) = pitch_e(i) - kf.xp(2);
+ yaw_e(i) = yaw_e(i) - kf.xp(3);
+
+ % Velocity correction
+ vel_e(i,1) = vel_e(i,1) - kf.xp(4);
+ vel_e(i,2) = vel_e(i,2) - kf.xp(5);
+ vel_e(i,3) = vel_e(i,3) - kf.xp(6);
+
+ % Position correction
+ lat_e(i) = lat_e(i) - kf.xp(7);
+ lon_e(i) = lon_e(i) - kf.xp(8);
+ h_e(i) = h_e(i) - kf.xp(9);
+
+ % Biases estimation
+ gb_dyn = -kf.xp(10:12);
+ ab_dyn = -kf.xp(13:15);
+% m_dyn = -kf.xp(16);
+
+ % Matrices for later Kalman filter performance analysis
+ xi(gdx,:) = kf.xi';
+ xp(gdx,:) = kf.xp';
+ b(gdx,:) = [gb_dyn', ab_dyn'];
+ A(gdx,:) = reshape(kf.A, 1, n^2);
+ Pi(gdx,:) = reshape(kf.Pi, 1, n^2);
+ Pp(gdx,:) = reshape(kf.Pp, 1, n^2);
+
+ if(zupt_flag == false)
+ v(gdx,:) = kf.v';
+ z(gdx,:) = kf.z';
+ K(gdx,:) = reshape(kf.K, 1, n*r);
+ S(gdx,:) = reshape(kf.S, 1, r^2);
+ else
+ zupt_flag = false;
+ z(gdx,:) = [ 0 kf.z' 0 0 0 ]';
+ v(gdx,:) = [ 0 kf.v' 0 0 0 ]';
+ K(gdx,1:n*3) = reshape(kf.K, 1, n*3);
+ S(gdx,1:9) = reshape(kf.S, 1, 9);
+ end
+ end
+end
+
+%% Summary from INS/GNSS integration
+
+nav_e.t = imu.t(1:i, :); % INS time vector
+nav_e.tg = gnss.t; % GNSS time vector, which is the time vector when the Kalman filter was executed
+nav_e.roll = roll_e(1:i, :); % Roll
+nav_e.pitch = pitch_e(1:i, :); % Pitch
+nav_e.yaw = yaw_e(1:i, :); % Yaw
+nav_e.yawm = yawm_e(1:i, :); % Magnetic heading
+nav_e.vel = vel_e(1:i, :); % NED velocities
+nav_e.lat = lat_e(1:i, :); % Latitude
+nav_e.lon = lon_e(1:i, :); % Longitude
+nav_e.h = h_e(1:i, :); % Altitude
+nav_e.gn = gn_e(1:i, :); % Gravity estimation in the nav-frame.
+
+nav_e.xi = xi; % A priori states
+nav_e.xp = xp; % A posteriori states
+nav_e.z = z; % INS/GNSS measurements
+nav_e.v = v; % Kalman filter innovations
+nav_e.b = b; % Biases compensations
+
+nav_e.A = A; % Transition matrices
+nav_e.Pi = Pi; % A priori covariance matrices
+nav_e.Pp = Pp; % A posteriori covariance matrices
+nav_e.K = K; % Kalman gain matrices
+nav_e.S = S; % Innovation matrices
+nav_e.ob = ob; % Number of observable states after each GNSS data arriving
+
+fprintf('\n');
+
+end
diff --git a/ins_visual_gnss/ins_visual_gnss.m b/ins_visual_gnss/ins_visual_gnss.m
new file mode 100644
index 0000000..91aeaa9
--- /dev/null
+++ b/ins_visual_gnss/ins_visual_gnss.m
@@ -0,0 +1,603 @@
+function [nav_e] = ins_visual_gnss(imu, gnss, visual, att_mode)
+% ins_gnss: loosely-coupled integrated navigation system.
+%
+% ins_visual_gnss integrates IMU with visual and GNSS measurements by using an Extended Kalman filter.
+%
+% INPUT
+% imu, IMU data structure.
+% t: Ix1 time vector (seconds).
+% fb: Ix3 accelerations vector in body frame XYZ (m/s^2).
+% wb: Ix3 turn rates vector in body frame XYZ (radians/s).
+% arw: 1x3 angle random walks (rad/s/root-Hz).
+% vrw: 1x3 velocity random walks (m/s^2/root-Hz).
+% g_std: 1x3 gyros standard deviations (radians/s).
+% a_std: 1x3 accrs standard deviations (m/s^2).
+% gb_sta: 1x3 gyros static biases or turn-on biases (radians/s).
+% ab_sta: 1x3 accrs static biases or turn-on biases (m/s^2).
+% gb_dyn: 1x3 gyros dynamic biases or bias instabilities (radians/s).
+% ab_dyn: 1x3 accrs dynamic biases or bias instabilities (m/s^2).
+% gb_corr: 1x3 gyros correlation times (seconds).
+% ab_corr: 1x3 accrs correlation times (seconds).
+% gb_psd: 1x3 gyros dynamic biases PSD (rad/s/root-Hz).
+% ab_psd: 1x3 accrs dynamic biases PSD (m/s^2/root-Hz);
+% freq: 1x1 sampling frequency (Hz).
+% ini_align: 1x3 initial attitude at t(1).
+% ini_align_err: 1x3 initial attitude errors at t(1).
+%
+% gnss, GNSS data structure.
+% t: Gx1 time vector (seconds).
+% lat: Gx1 latitude (radians).
+% lon: Gx1 longitude (radians).
+% h: Gx1 altitude (m).
+% vel: Gx3 NED velocities (m/s).
+% std: 1x3 position standard deviations (rad, rad, m).
+% stdm: 1x3 position standard deviations (m, m, m).
+% stdv: 1x3 velocity standard deviations (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 current IMU time to current GNSS time vector (s).
+%
+% visual, visual data structure
+% t: Vx1 time vector (seconds)
+% lat: Vx1 latitude (radians).
+% lon: Vx1 longitude (radians).
+% h: Vx1 altitude (m).
+% vel: Vx3 NED velocities (m/s).
+% covvm: Vx36 velocity and position covariance matrices (m^2 and m^2/s^2).
+% covv: Vx9 velocity covariance matrices (m^2/s^2).
+% larm: 3x1 lever arm (m).
+% freq: 1x1 sampling frequency (Hz).
+% eps: 1x1 time interval to compare current IMU time to current visual time vector (s).
+%
+% att_mode: attitude mode string.
+% 'quaternion': attitude updated in quaternion format. Default value.
+% 'dcm': attitude updated in Direct Cosine Matrix format.
+%
+% OUTPUT
+% nav_e, INS/visual/GNSS navigation estimates data structure.
+% t: Ix1 INS time vector (seconds).
+% tg: Gx1 GNSS time vector, when Kalman filter was executed (seconds).
+% roll: Ix1 roll (radians).
+% pitch: Ix1 pitch (radians).
+% yaw: Ix1 yaw (radians).
+% vel: Ix3 NED velocities (m/s).
+% lat: Ix1 latitude (radians).
+% lon: Ix1 longitude (radians).
+% h: Ix1 altitude (m).
+% xi: Vxn Kalman filter a priori states.
+% xp: Vxn Kalman filter a posteriori states.
+% z: Vxr INS/GNSS measurements
+% v: Vxr Kalman filter innovations.
+% b: Vxr Kalman filter biases compensations, [gb_dyn ab_dyn].
+% A: Vxn^2 Kalman filter transition-state matrices, one matrix per
+% row ordered by columns.
+% Pp: Vxn^2 Kalman filter a posteriori covariance matrices, one
+% matrix per row ordered by columns.
+% Pi: Vxn^2 Kalman filter a priori covariance matrices, one matrix
+% per row ordered by columns.
+% K: Vx(n*r) Kalman gain matrices
+% S: Vxr^2 Innovation matrices
+% ob: Vx1 Number of observable states after each GNSS data arriving
+%
+% Copyright (C) 2014, Rodrigo Gonzalez, all rights reserved.
+%
+% This file is part of NaveGo, an open-source MATLAB toolbox for
+% simulation of integrated navigation systems.
+%
+% NaveGo is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License (LGPL)
+% version 3 as published by the Free Software Foundation.
+%
+% This program is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+% GNU Lesser General Public License for more details.
+%
+% You should have received a copy of the GNU Lesser General Public
+% License along with this program. If not, see
+% .
+%
+% References:
+%
+% R. Gonzalez, J. Giribet, and H. Patiño. NaveGo: a
+% simulation framework for low-cost integrated navigation systems,
+% Journal of Control Engineering and Applied Informatics, vol. 17,
+% issue 2, pp. 110-120, 2015. Alg. 2.
+%
+% Groves, P.D. (2013), Principles of GNSS, Inertial, and
+% Multisensor Integrated Navigation Systems (2nd Ed.). Artech House.
+
+% ZUPT algothim based on Groves, Chapter 15, "INS Alignment, Zero Updates,
+% and Motion Constraints".
+%
+% ins_gps.m, ins_gnss function is based on that previous NaveGo function.
+%
+% Version: 009
+% Date: 2021/03/16
+% Author: Rodrigo Gonzalez
+% URL: https://github.com/rodralez/navego
+
+if nargin < 3, att_mode = 'quaternion'; end
+
+%% ZUPT ALGORITHM
+
+zupt_flag = false;
+
+%% PREALLOCATION
+
+% Kalman filter dimensions
+n = 15; % number of states
+r = 6; % number of sensors
+q = 12; % number of inputs
+
+% Constant matrices
+I = eye(3);
+O = zeros(3);
+
+% Length of INS time vector
+LI = length(imu.t);
+
+% Length of GNSS time vector
+LG = length(gnss.t);
+
+% Length of visual time vector
+LV = length(visual.t);
+
+% Preallocation of attitude vectors
+roll_e = zeros (LI, 1);
+pitch_e = zeros (LI, 1);
+yaw_e = zeros (LI, 1);
+
+% Preallocation of velocity vector
+vel_e = zeros (LI, 3);
+
+% Preallocation of gravity vector
+gn_e = zeros (LI, 3);
+
+% Preallocation of position vectors
+lat_e = zeros (LI, 1);
+lon_e = zeros (LI, 1);
+h_e = zeros (LI, 1);
+
+% Preallocation of Kalman filter matrices for later performance analysis
+xi = zeros(LV, n); % Evolution of Kalman filter a priori states
+xp = zeros(LV, n); % Evolution of Kalman filter a posteriori states
+z = zeros(LV, r); % INS/GNSS or INS/visual measurements
+v = zeros(LV, r); % Kalman filter innovations
+
+A = zeros(LV, n^2); % Transition-state matrices
+Pi = zeros(LV, n^2); % A priori covariance matrices
+Pp = zeros(LV, n^2); % A posteriori covariance matrices
+K = zeros(LV, n*r); % Kalman gain matrices
+S = zeros(LV, r^2); % Innovation matrices
+ob_gnss = zeros(LV, 1); % Number of observable states at each GNSS data arriving
+ob_visual = zeros(LV, 1); % Number of observable states at each visual data arriving
+
+b = zeros(LV, r); % Biases compensantions after Kalman filter correction
+
+%% INITIAL VALUES AT INS TIME = 1
+
+% Initial attitude
+roll_e(1) = imu.ini_align(1);
+pitch_e(1) = imu.ini_align(2);
+yaw_e(1) = imu.ini_align(3);
+DCMnb = euler2dcm([roll_e(1); pitch_e(1); yaw_e(1);]);
+DCMbn = DCMnb';
+qua = euler2qua([roll_e(1) pitch_e(1) yaw_e(1)]);
+
+% Initial velocity
+vel_e(1,:) = gnss.vel(1,:);
+
+% Initial position
+lat_e(1) = gnss.lat(1);
+lon_e(1) = gnss.lon(1);
+h_e(1) = gnss.h(1);
+
+% Initial dynamic biases
+gb_dyn = imu.gb_dyn';
+ab_dyn = imu.ab_dyn';
+
+%% INITIALIZATION OF KALMAN FILTER MATRICES
+
+% Prior estimates
+kf.xi = [ zeros(1,9), imu.gb_dyn, imu.ab_dyn ]'; % Error vector state
+kf.Pi = diag([imu.ini_align_err, gnss.stdv, gnss.std, imu.gb_dyn, imu.ab_dyn].^2);
+
+kf.Q = diag([imu.arw, imu.vrw, imu.gb_psd, imu.ab_psd].^2);
+
+fb_corrected = imu.fb(1,:)' - ab_dyn - imu.ab_sta';
+fn = DCMbn * fb_corrected;
+wn_corrected = imu.wb(1,:)' - gb_dyn - imu.gb_sta';
+wn = DCMbn * wn_corrected;
+
+% Vector to update matrix F
+upd = [gnss.vel(1,:) gnss.lat(1) gnss.h(1) fn' wn'];
+
+% Update matrices F and G
+[kf.F, kf.G] = F_update(upd, DCMbn, imu);
+
+[RM,RN] = radius(gnss.lat(1));
+Tpr = diag([(RM + gnss.h(1)), (RN + gnss.h(1)) * cos(gnss.lat(1)), -1]); % radians-to-meters
+
+% Update matrix H
+kf.H = [ O I O O O ;
+ O O Tpr O O ; ];
+kf.R = diag([gnss.stdv gnss.stdm]).^2;
+kf.z = [ gnss.stdv, gnss.stdm ]';
+
+% Propagate prior estimates to get xp(1) and Pp(1)
+kf = kf_update( kf );
+
+% Initial matrices for Kalman filter performance analysis
+xi(1,:) = kf.xi';
+xp(1,:) = kf.xp';
+Pi(1,:) = reshape(kf.Pi, 1, n^2);
+Pp(1,:) = reshape(kf.Pp, 1, n^2);
+K(1,:) = reshape(kf.K, 1, n*r);
+S(1,:) = reshape(kf.S, 1, r^2);
+v(1,:) = kf.v';
+z(1,:) = kf.z';
+b(1,:) = [gb_dyn', ab_dyn'];
+
+GNSSCounter = 0;
+
+%% INS (IMU) TIME IS THE MASTER CLOCK
+for i = 2:LI
+
+ %% INERTIAL NAVIGATION SYSTEM (INS)
+
+ % Print a dot on console every 10,000 INS executions
+ if (mod(i,10000) == 0), fprintf('. '); end
+ % Print a return on console every 200,000 INS executions
+ if (mod(i,200000) == 0), fprintf('\n'); end
+
+ % IMU sampling interval
+ dti = imu.t(i) - imu.t(i-1);
+
+ % Turn-rates update
+ omega_ie_n = earth_rate(lat_e(i-1));
+ omega_en_n = transport_rate(lat_e(i-1), vel_e(i-1,1), vel_e(i-1,2), h_e(i-1));
+
+ % Gravity update
+ gn_e(i,:) = gravity(lat_e(i-1), h_e(i-1));
+
+ % Inertial sensors corrected with a posteriori KF biases estimation and
+ % deterministic static biases
+ wb_corrected = imu.wb(i,:)' - gb_dyn - imu.gb_sta';
+ fb_corrected = imu.fb(i,:)' - ab_dyn - imu.ab_sta';
+ fn = DCMbn * fb_corrected;
+ wn = DCMbn * wb_corrected;
+
+ % Attitude update
+ [qua, DCMbn, euler] = att_update(wb_corrected, DCMbn, qua, ...
+ omega_ie_n, omega_en_n, dti, att_mode);
+ roll_e(i) = euler(1);
+ pitch_e(i)= euler(2);
+ yaw_e(i) = euler(3);
+
+ % Velocity update
+ vel = vel_update(fn, vel_e(i-1,:), omega_ie_n, omega_en_n, gn_e(i,:)', dti);
+ vel_e (i,:) = vel;
+
+ % Position update
+ pos = pos_update([lat_e(i-1) lon_e(i-1) h_e(i-1)], vel_e(i,:), dti);
+ lat_e(i) = pos(1);
+ lon_e(i) = pos(2);
+ h_e(i) = pos(3);
+
+ % Turn-rates update with both updated velocity and position
+ omega_ie_n = earth_rate(lat_e(i));
+ omega_en_n = transport_rate(lat_e(i), vel_e(i,1), vel_e(i,2), h_e(i));
+
+ %% ZUPT DETECTION ALGORITHM
+ idz = floor( gnss.zupt_win / dti ); % Index to set ZUPT window time
+
+ if ( i > idz )
+
+ % Mean velocity value for the ZUPT window time
+ vel_m = mean (vel_e(i-idz:i , :));
+
+ % If mean velocity value is under the ZUPT threshold velocity...
+ if (abs(vel_m) < gnss.zupt_th)
+
+ % Current attitude is equal to the mean of previous attitudes
+ % inside the ZUPT window time
+ roll_e(i) = mean (roll_e(i-idz:i , :));
+ pitch_e(i) = mean (pitch_e(i-idz:i , :));
+ yaw_e(i) = mean (yaw_e(i-idz:i , :));
+
+ % Current position is equal to the mean of previous positions
+ % inside the ZUPT window time
+ lat_e(i) = mean (lat_e(i-idz:i , :));
+ lon_e(i) = mean (lon_e(i-idz:i , :));
+ h_e(i) = mean (h_e(i-idz:i , :));
+
+ % Alternative attitude ZUPT correction
+ % roll_e(i) = (roll_e(i-idz , :));
+ % pitch_e(i) = (pitch_e(i-idz , :));
+ % yaw_e(i) = (yaw_e(i-idz, :));
+ % lat_e(i) = (lat_e(i-idz:i , :));
+ % lon_e(i) = (lon_e(i-idz:i , :));
+ % h_e(i) = (h_e(i-idz:i , :));
+
+ zupt_flag = true;
+
+ % fprintf(' z\n') % DEBUG
+ end
+ end
+
+ % Check if there is a new visual measurement to process at current INS time
+ gdx = find (visual.t >= (imu.t(i) - visual.eps) & visual.t < (imu.t(i) + visual.eps));
+
+ if (~isempty(gdx) && gdx(1) > 1)
+ for index = 1:size(gdx,1)
+ % disp("updated with visual measurements");
+
+ %% MEASUREMENTS
+
+ % Meridian and normal radii of curvature update
+ [RM,RN] = radius(lat_e(i));
+
+ % Radians-to-meters matrix
+ Tpr = diag([(RM + h_e(i)), (RN + h_e(i)) * cos(lat_e(i)), -1]);
+
+ % Position innovations in meters with lever arm correction
+ zp = Tpr * ([lat_e(i); lon_e(i); h_e(i);] - [visual.lat(gdx(index)); visual.lon(gdx(index)); visual.h(gdx(index));]) ...
+ + (DCMbn * visual.larm);
+
+ % Velocity innovations with lever arm correction
+ zv = (vel_e(i,:) - visual.vel(gdx(index),:) - ((omega_ie_n + omega_en_n) .* (DCMbn * visual.larm))' ...
+ + (DCMbn * skewm(wb_corrected) * visual.larm )' )';
+
+ %% KALMAN FILTER
+
+ % visual sampling interval
+ dtg = visual.t(gdx(index)) - visual.t(gdx(index)-1);
+
+ % Vector to update matrix F
+ upd = [vel_e(i,:) lat_e(i) h_e(i) fn' wn'];
+
+ % Matrices F and G update
+ [kf.F, kf.G] = F_update(upd, DCMbn, imu);
+
+ if GNSSCounter < 15
+ Weight = 100;
+ else
+ Weight = 1;
+ end
+
+ % Matrix H update
+ if(zupt_flag == false)
+ kf.H = [ O I O O O ;
+ O O Tpr O O ; ];
+ kf.R = Weight*reshape(visual.covvm(gdx(index),:),[6,6])';
+ kf.z = [ zv' zp' ]';
+ else
+ kf.H = [ O I O O O ; ];
+ kf.R = Weight*reshape(visual.covv(gdx(index),:),[3,3])';
+ kf.z = zv;
+ end
+
+ % Execution of the extended Kalman filter
+ kf.xp(1:9) = 0.0; % states 1 to 9 are forced to be zero (error-state approach)
+ kf = kalman(kf, dtg);
+
+ %% OBSERVABILITY
+
+ % Number the observable states at current visual time
+ ob(gdx(index)) = rank(obsv(kf.F, kf.H));
+
+ %% INS/visual CORRECTIONS
+
+ % Quaternion corrections
+ % Crassidis. Eq. 7.34 and A.174a.
+ antm = [0.0 qua(3) -qua(2); -qua(3) 0.0 qua(1); qua(2) -qua(1) 0.0];
+ qua = qua + 0.5 .* [qua(4)*eye(3) + antm; -1.*[qua(1) qua(2) qua(3)]] * kf.xp(1:3);
+ qua = qua / norm(qua); % Brute-force normalization
+
+ % DCM correction
+ DCMbn = qua2dcm(qua);
+
+ % Attitude correction, method 1
+ % euler = qua2euler(qua);
+ % roll_e(i) = euler(1);
+ % pitch_e(i)= euler(2);
+ % yaw_e(i) = euler(3);
+
+ % Attitude correction, method 2
+ roll_e(i) = roll_e(i) - kf.xp(1);
+ pitch_e(i) = pitch_e(i) - kf.xp(2);
+ yaw_e(i) = yaw_e(i) - kf.xp(3);
+
+ % Velocity correction
+ vel_e(i,1) = vel_e(i,1) - kf.xp(4);
+ vel_e(i,2) = vel_e(i,2) - kf.xp(5);
+ vel_e(i,3) = vel_e(i,3) - kf.xp(6);
+
+ % Position correction
+ lat_e(i) = lat_e(i) - kf.xp(7);
+ lon_e(i) = lon_e(i) - kf.xp(8);
+ h_e(i) = h_e(i) - kf.xp(9);
+
+ % Biases estimation
+ gb_dyn = -kf.xp(10:12);
+ ab_dyn = -kf.xp(13:15);
+
+ % Matrices for later Kalman filter performance analysis
+ xi(gdx(index),:) = kf.xi';
+ xp(gdx(index),:) = kf.xp';
+ b(gdx(index),:) = [gb_dyn', ab_dyn'];
+ A(gdx(index),:) = reshape(kf.A, 1, n^2);
+ Pi(gdx(index),:) = reshape(kf.Pi, 1, n^2);
+ Pp(gdx(index),:) = reshape(kf.Pp, 1, n^2);
+
+ if(zupt_flag == false)
+ v(gdx(index),:) = kf.v';
+ z(gdx(index),:) = kf.z';
+ K(gdx(index),:) = reshape(kf.K, 1, n*r);
+ S(gdx(index),:) = reshape(kf.S, 1, r^2);
+ else
+ zupt_flag = false;
+ z(gdx(index),:) = [ kf.z' 0 0 0 ]';
+ v(gdx(index),:) = [ kf.v' 0 0 0 ]';
+ K(gdx(index),1:n*3) = reshape(kf.K, 1, n*3);
+ S(gdx(index),1:9) = reshape(kf.S, 1, 3^2);
+ end
+ end
+ end
+
+ if imu.t(i) > max(visual.t)
+ break;
+ end
+
+ %% KALMAN FILTER GNSS UPDATE
+
+ GNSSCounter = GNSSCounter + 1;
+
+ % Check if there is a new GNSS measurement to process at current INS time
+ gdx = find (gnss.t >= (imu.t(i) - gnss.eps) & gnss.t < (imu.t(i) + gnss.eps));
+
+ if ( ~isempty(gdx) && gdx > 1)
+ %disp("updated with GNSS measurements");
+
+ GNSSCounter = 0; % counter reset
+
+ %% MEASUREMENTS
+
+ % Meridian and normal radii of curvature update
+ [RM,RN] = radius(lat_e(i));
+
+ % Radians-to-meters matrix
+ Tpr = diag([(RM + h_e(i)), (RN + h_e(i)) * cos(lat_e(i)), -1]);
+
+ % Position innovations in meters with lever arm correction
+ zp = Tpr * ([lat_e(i); lon_e(i); h_e(i);] - [gnss.lat(gdx); gnss.lon(gdx); gnss.h(gdx);]) ...
+ + (DCMbn * gnss.larm);
+
+ % Velocity innovations with lever arm correction
+ zv = (vel_e(i,:) - gnss.vel(gdx,:) - ((omega_ie_n + omega_en_n) .* (DCMbn * gnss.larm))' ...
+ + (DCMbn * skewm(wb_corrected) * gnss.larm )' )';
+
+ %% KALMAN FILTER
+
+ % GNSS sampling interval
+ dtg = gnss.t(gdx) - gnss.t(gdx-1);
+
+ % Vector to update matrix F
+ upd = [vel_e(i,:) lat_e(i) h_e(i) fn' wn'];
+
+ % Matrices F and G update
+ [kf.F, kf.G] = F_update(upd, DCMbn, imu);
+
+ % Matrix H update
+ if(zupt_flag == false)
+ kf.H = [ O I O O O ;
+ O O Tpr O O ; ];
+ kf.R = 0.001*diag([gnss.stdv gnss.stdm]).^2;
+ kf.z = [ zv' zp' ]';
+ else
+ kf.H = [ O I O O O ; ];
+ kf.R = 0.001*diag([gnss.stdv]).^2;
+ kf.z = zv;
+ end
+
+ % Execution of the extended Kalman filter
+ kf.xp(1:9) = 0.0; % states 1 to 9 are forced to be zero (error-state approach)
+ kf = kalman(kf, dtg);
+
+ %% OBSERVABILITY
+
+ % Number the observable states at current GNSS time
+ ob_gnss(gdx) = rank(obsv(kf.F, kf.H));
+
+ %% INS/GNSS CORRECTIONS
+
+ % Quaternion corrections
+ % Crassidis. Eq. 7.34 and A.174a.
+ antm = [0.0 qua(3) -qua(2); -qua(3) 0.0 qua(1); qua(2) -qua(1) 0.0];
+ qua = qua + 0.5 .* [qua(4)*eye(3) + antm; -1.*[qua(1) qua(2) qua(3)]] * kf.xp(1:3);
+ qua = qua / norm(qua); % Brute-force normalization
+
+ % DCM correction
+ DCMbn = qua2dcm(qua);
+
+ % Attitude correction, method 1
+ % euler = qua2euler(qua);
+ % roll_e(i) = euler(1);
+ % pitch_e(i)= euler(2);
+ % yaw_e(i) = euler(3);
+
+ % Attitude correction, method 2
+ roll_e(i) = roll_e(i) - kf.xp(1);
+ pitch_e(i) = pitch_e(i) - kf.xp(2);
+ yaw_e(i) = yaw_e(i) - kf.xp(3);
+
+ % Velocity correction
+ vel_e(i,1) = vel_e(i,1) - kf.xp(4);
+ vel_e(i,2) = vel_e(i,2) - kf.xp(5);
+ vel_e(i,3) = vel_e(i,3) - kf.xp(6);
+
+ % Position correction
+ lat_e(i) = lat_e(i) - kf.xp(7);
+ lon_e(i) = lon_e(i) - kf.xp(8);
+ h_e(i) = h_e(i) - kf.xp(9);
+
+ % Biases estimation
+ gb_dyn = -kf.xp(10:12);
+ ab_dyn = -kf.xp(13:15);
+
+ % Matrices for later Kalman filter performance analysis
+ xi(gdx,:) = kf.xi';
+ xp(gdx,:) = kf.xp';
+ b(gdx,:) = [gb_dyn', ab_dyn'];
+ A(gdx,:) = reshape(kf.A, 1, n^2);
+ Pi(gdx,:) = reshape(kf.Pi, 1, n^2);
+ Pp(gdx,:) = reshape(kf.Pp, 1, n^2);
+
+ if(zupt_flag == false)
+ v(gdx,:) = kf.v';
+ z(gdx,:) = kf.z';
+ K(gdx,:) = reshape(kf.K, 1, n*r);
+ S(gdx,:) = reshape(kf.S, 1, r^2);
+ else
+ zupt_flag = false;
+ z(gdx,:) = [ kf.z' 0 0 0 ]';
+ v(gdx,:) = [ kf.v' 0 0 0 ]';
+ K(gdx,1:n*3) = reshape(kf.K, 1, n*3);
+ S(gdx,1:9) = reshape(kf.S, 1, 3^2);
+ end
+ end
+end
+
+%% Summary from INS/GNSS integration
+
+nav_e.t = imu.t(1:i, :); % INS time vector
+nav_e.tg = gnss.t; % GNSS time vector, which is the time vector when the Kalman filter was executed
+nav_e.roll = roll_e(1:i, :); % Roll
+nav_e.pitch = pitch_e(1:i, :); % Pitch
+nav_e.yaw = yaw_e(1:i, :); % Yaw
+nav_e.vel = vel_e(1:i, :); % NED velocities
+nav_e.lat = lat_e(1:i, :); % Latitude
+nav_e.lon = lon_e(1:i, :); % Longitude
+nav_e.h = h_e(1:i, :); % Altitude
+nav_e.gn = gn_e(1:i, :); % Gravity estimation in the nav-frame.
+
+nav_e.xi = xi; % A priori states
+nav_e.xp = xp; % A posteriori states
+nav_e.z = z; % INS/GNSS measurements
+nav_e.v = v; % Kalman filter innovations
+nav_e.b = b; % Biases compensations
+
+nav_e.A = A; % Transition matrices
+nav_e.Pi = Pi; % A priori covariance matrices
+nav_e.Pp = Pp; % A posteriori covariance matrices
+nav_e.K = K; % Kalman gain matrices
+nav_e.S = S; % Innovation matrices
+nav_e.ob_gnss = ob_gnss; % Number of observable states after each GNSS data arriving
+nav_e.ob_visual = ob_visual; % Number of observable states after each visual data arriving
+
+fprintf('\n');
+
+end
diff --git a/ins_visual_gnss/kalman.m b/ins_visual_gnss/kalman.m
new file mode 100644
index 0000000..8f0d508
--- /dev/null
+++ b/ins_visual_gnss/kalman.m
@@ -0,0 +1,69 @@
+function kf = kalman(kf, dt)
+% kalman: Kalman filter algorithm.
+%
+% INPUT
+% kf, data structure with at least the following fields:
+% xp: nx1 a posteriori state vector (old).
+% z: rx1 measurement vector.
+% F: nxn state transition matrix.
+% H: rxn observation matrix.
+% Q: qxq process noise covariance matrix.
+% R: rxr observation noise covariance matrix.
+% Pp: nxn a posteriori error covariance matrix.
+% G: nxq control-input matrix.
+% dt: sampling interval.
+%
+% OUTPUT
+% kf, the following fields are updated:
+% xi: nx1 a priori state vector (updated).
+% xp: nx1 a posteriori state vector (updated).
+% v: rx1 innovation vector.
+% A: nxn state transition matrix.
+% K: nxr Kalman gain matrix.
+% Qd: nxn discrete process noise covariance matrix.
+% Pi: nxn a priori error covariance.
+% Pp: nxn a posteriori error covariance.
+% S: rxr innovation (not residual) covariance matrix.
+%
+% Copyright (C) 2014, Rodrigo Gonzalez, all rights reserved.
+%
+% This file is part of NaveGo, an open-source MATLAB toolbox for
+% simulation of integrated navigation systems.
+%
+% NaveGo is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License (LGPL)
+% version 3 as published by the Free Software Foundation.
+%
+% This program is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+% GNU Lesser General Public License for more details.
+%
+% You should have received a copy of the GNU Lesser General Public
+% License along with this program. If not, see
+% .
+%
+% Reference:
+%
+% R. Gonzalez, J. Giribet, and H. Patiño. NaveGo: a
+% simulation framework for low-cost integrated navigation systems,
+% Journal of Control Engineering and Applied Informatics, vol. 17,
+% issue 2, pp. 110-120, 2015. Alg. 1.
+%
+% Dan Simon. Optimal State Estimation. Chapter 5. John Wiley
+% & Sons. 2006.
+%
+% Version: 007
+% Date: 2019/04/19
+% Author: Rodrigo Gonzalez
+% URL: https://github.com/rodralez/navego
+
+%% PREDICTION STEP
+
+kf = kf_prediction(kf, dt);
+
+%% UPDATE STEP
+
+kf = kf_update(kf);
+
+end
diff --git a/ins_visual_gnss/kf_prediction.m b/ins_visual_gnss/kf_prediction.m
new file mode 100644
index 0000000..547d4f3
--- /dev/null
+++ b/ins_visual_gnss/kf_prediction.m
@@ -0,0 +1,65 @@
+function kf = kf_prediction(kf, dt)
+% kf_prediction: prediction update part of the Kalman filter algorithm.
+%
+% INPUT
+% kf, data structure with at least the following fields,
+% xp: nx1 a posteriori state vector (old).
+% Pp: nxn a posteriori error covariance matrix (old).
+% F: nxn state transition matrix.
+% Q: qxq process noise covariance matrix.
+% G: nxq control-input matrix.
+% dt: sampling interval.
+%
+% OUTPUT
+% kf, the following fields are updated,
+% xi: nx1 a priori state vector (updated).
+% Pi: nxn a priori error covariance matrix.
+% A: nxn state transition matrix.
+% Qd: nxn discrete process noise covariance matrix.
+%
+% Copyright (C) 2014, Rodrigo Gonzalez, all rights reserved.
+%
+% This file is part of NaveGo, an open-source MATLAB toolbox for
+% simulation of integrated navigation systems.
+%
+% NaveGo is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License (LGPL)
+% version 3 as published by the Free Software Foundation.
+%
+% This program is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+% GNU Lesser General Public License for more details.
+%
+% You should have received a copy of the GNU Lesser General Public
+% License along with this program. If not, see
+% .
+%
+% Reference:
+%
+% R. Gonzalez, J. Giribet, and H. Patiño. NaveGo: a
+% simulation framework for low-cost integrated navigation systems,
+% Journal of Control Engineering and Applied Informatics, vol. 17,
+% issue 2, pp. 110-120, 2015. Alg. 1.
+%
+% Dan Simon. Optimal State Estimation. Chapter 5. John Wiley
+% & Sons. 2006.
+%
+% Version: 001
+% Date: 2019/04/19
+% Author: Rodrigo Gonzalez
+% URL: https://github.com/rodralez/navego
+
+% Discretization of continous-time system
+kf.A = expm(kf.F * dt); % Exact solution for linear systems
+% kf.A = I + (kf.F * dt); % Approximated solution by Euler method
+kf.Qd = (kf.G * kf.Q * kf.G') .* dt; % Digitalized covariance matrix
+
+% Step 1, predict the a priori state vector, xi
+kf.xi = kf.A * kf.xp;
+
+% Step 2, update the a priori covariance matrix, Pi
+kf.Pi = (kf.A * kf.Pp * kf.A') + kf.Qd;
+kf.Pi = 0.5 .* (kf.Pi + kf.Pi'); % Force Pi to be a symmetric matrix
+
+end
diff --git a/ins_visual_gnss/kf_update.m b/ins_visual_gnss/kf_update.m
new file mode 100644
index 0000000..6ef9451
--- /dev/null
+++ b/ins_visual_gnss/kf_update.m
@@ -0,0 +1,75 @@
+function kf = kf_update(kf)
+% kf_update: measurement update part of the Kalman filter algorithm.
+%
+% INPUT
+% kf, data structure with at least the following fields,
+% xi: nx1 a priori state vector.
+% Pi: nxn a priori error covariance matrix.
+% z: rx1 measurement vector.
+% H: rxn observation matrix.
+% R: rxr observation noise covariance matrix.
+%
+% OUTPUT
+% kf, the following fields are updated,
+% xp: nx1 a posteriori state vector (updated).
+% Pp: nxn a posteriori error covariance matrix (updated).
+% v: rx1 innovation vector.
+% K: nxr Kalman gain matrix.
+% S: rxr innovation (not residual) covariance matrix.
+%
+% Copyright (C) 2014, Rodrigo Gonzalez, all rights reserved.
+%
+% This file is part of NaveGo, an open-source MATLAB toolbox for
+% simulation of integrated navigation systems.
+%
+% NaveGo is free software: you can redistribute it and/or modify
+% it under the terms of the GNU Lesser General Public License (LGPL)
+% version 3 as published by the Free Software Foundation.
+%
+% This program is distributed in the hope that it will be useful,
+% but WITHOUT ANY WARRANTY; without even the implied warranty of
+% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+% GNU Lesser General Public License for more details.
+%
+% You should have received a copy of the GNU Lesser General Public
+% License along with this program. If not, see
+% .
+%
+% Reference:
+%
+% R. Gonzalez, J. Giribet, and H. Patiño. NaveGo: a
+% simulation framework for low-cost integrated navigation systems,
+% Journal of Control Engineering and Applied Informatics, vol. 17,
+% issue 2, pp. 110-120, 2015. Alg. 1.
+%
+% Dan Simon. Optimal State Estimation. Chapter 5. John Wiley
+% & Sons. 2006.
+%
+% Version: 002
+% Date: 2021/03/21
+% Author: Rodrigo Gonzalez
+% URL: https://github.com/rodralez/navego
+
+% I = eye(size(kf.F));
+
+% Step 3, update Kalman gain
+kf.S = (kf.R + kf.H * kf.Pi * kf.H'); % Innovation covariance matrix
+kf.v = kf.z - kf.H * kf.xi; % Innovation vector
+
+r = length(kf.v);
+if rank (kf.S) < r
+ error('kf_update: innovation covariance matrix S is not invertable.')
+end
+
+kf.K = (kf.Pi * kf.H') * (kf.S)^(-1) ; % Kalman gain matrix
+
+% Step 4, update the a posteriori state vector, xp
+kf.xp = kf.xi + kf.K * kf.v;
+
+% Step 5, update the a posteriori covariance matrix, Pp
+kf.Pp = kf.Pi - kf.K * kf.S * kf.K';
+% J = (I - S.K * S.H); % Joseph stabilized version
+% S.Pp = J * S.Pi * J' + S.K * S.R * S.K'; % Alternative implementation
+kf.Pp = 0.5 .* (kf.Pp + kf.Pp'); % Force Pp to be a symmetric matrix
+
+end