diff --git a/examples/planetary-data/canada-planetary-data/data/gnss_planetary_r.mat b/examples/planetary-data/canada-planetary-data/data/gnss_planetary_r.mat
index 42df73e..eca9689 100644
Binary files a/examples/planetary-data/canada-planetary-data/data/gnss_planetary_r.mat and b/examples/planetary-data/canada-planetary-data/data/gnss_planetary_r.mat differ
diff --git a/examples/planetary-data/canada-planetary-data/data/gnss_planetary_sparse_r.mat b/examples/planetary-data/canada-planetary-data/data/gnss_planetary_sparse_r.mat
index 16c2534..5792bcf 100644
Binary files a/examples/planetary-data/canada-planetary-data/data/gnss_planetary_sparse_r.mat and b/examples/planetary-data/canada-planetary-data/data/gnss_planetary_sparse_r.mat differ
diff --git a/examples/planetary-data/canada-planetary-data/data/gnss_sparse_planetary.mat b/examples/planetary-data/canada-planetary-data/data/gnss_sparse_planetary.mat
index 08d3558..aba9521 100644
Binary files a/examples/planetary-data/canada-planetary-data/data/gnss_sparse_planetary.mat and b/examples/planetary-data/canada-planetary-data/data/gnss_sparse_planetary.mat differ
diff --git a/examples/planetary-data/canada-planetary-data/data/imu_planetary.mat b/examples/planetary-data/canada-planetary-data/data/imu_planetary.mat
index a9e5d88..2511d5e 100644
Binary files a/examples/planetary-data/canada-planetary-data/data/imu_planetary.mat and b/examples/planetary-data/canada-planetary-data/data/imu_planetary.mat differ
diff --git a/examples/planetary-data/canada-planetary-data/data/visual_planetary.mat b/examples/planetary-data/canada-planetary-data/data/visual_planetary.mat
index 75adf34..c0a4cac 100644
Binary files a/examples/planetary-data/canada-planetary-data/data/visual_planetary.mat and b/examples/planetary-data/canada-planetary-data/data/visual_planetary.mat differ
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
index 877373e..8d0f9dd 100755
--- a/examples/planetary-data/canada-planetary-data/scripts/gnss_sparse_structure.m
+++ b/examples/planetary-data/canada-planetary-data/scripts/gnss_sparse_structure.m
@@ -83,11 +83,11 @@
 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
+gnss_planetary_sparse_r = gnss_planetary; % reference
+[gnss_planetary,~] = gnss_gen(gnss_planetary_sparse_r, gnss_planetary); % degredation
 
 %% Saving 
 
 save('../data/gnss_planetary_r.mat','gnss_planetary_r');
-save('../data/gnss_planetary_sparse_r.mat','gnss_planetary_r_sparse');
+save('../data/gnss_planetary_sparse_r.mat','gnss_planetary_sparse_r');
 save('../data/gnss_sparse_planetary.mat','gnss_planetary');
\ No newline at end of file
diff --git a/examples/planetary-data/canada-planetary-data/scripts/navego_example_canada_data.asv b/examples/planetary-data/canada-planetary-data/scripts/navego_example_canada_data.asv
deleted file mode 100644
index ffcad23..0000000
--- a/examples/planetary-data/canada-planetary-data/scripts/navego_example_canada_data.asv
+++ /dev/null
@@ -1,239 +0,0 @@
-% navego_example_canada_data: post-processing integration of  
-% IMU, GNSS and visual data.
-%
-% The main goal is to integrate IMU and GNSS measurements from Ekinox-D 
-% sensor which includes both IMU and GNSS sensors.
-%
-% Sensors dataset was generated driving a car through the streets of 
-% Turin city (Italy).
-%
-%   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
-%   <http://www.gnu.org/licenses/>.
-%
-% References:
-%
-%
-% Version: 001
-% Date:    2021/11/01
-% Author:  Johann Diep <johanndiep@gmail.com>
-% URL:     https://github.com/rodralez/navego
-%
-% NOTE: NaveGo assumes that IMU is aligned with respect to body-frame as
-% X-forward, Y-right and Z-down.
-%
-% NOTE: NaveGo assumes that yaw angle (heading) is positive clockwise.
-%  - November 2021
-%
-% This script runs the fusion of all sensor parameters. 
-
-clear; clc; close all;
-
-addpath ../data/
-
-navego_print_version;
-
-fprintf('\nNaveGo: starting visual INS/GNSS integration... \n')
-
-% FusionCase = "inertial_gnss";
-% FusionCase = "inertial_visual";
-FusionCase = "inertial_visual_gnss";
-Sparse = "true";
-
-fprintf('NaveGo: FusionCase = %s... \n', FusionCase)
-
-%% Generating Data
-
-fprintf('NaveGo: Generating Data... \n')
-
-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
-
-fprintf('NaveGo: integration... \n')
-
-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'); % note: figure out why we input gnss_planetary_r 
-    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/navego_example_canada_data.m b/examples/planetary-data/canada-planetary-data/scripts/navego_example_canada_data.m
index 68f1553..15c5461 100755
--- a/examples/planetary-data/canada-planetary-data/scripts/navego_example_canada_data.m
+++ b/examples/planetary-data/canada-planetary-data/scripts/navego_example_canada_data.m
@@ -1,5 +1,5 @@
 % navego_example_canada_data: post-processing integration of
-% IMU, GNSS and visual data.
+% IMU, GNSS and visual data for the Canada dataset.
 %
 % The main goal is to integrate IMU and GNSS measurements from Ekinox-D
 % sensor which includes both IMU and GNSS sensors.
@@ -37,9 +37,6 @@
 % X-forward, Y-right and Z-down.
 %
 % NOTE: NaveGo assumes that yaw angle (heading) is positive clockwise.
-%  - November 2021
-%
-% This script runs the fusion of all sensor parameters.
 
 clc
 close all
@@ -66,15 +63,16 @@
 % Comment any of the following parameters in order to NOT execute a
 % particular portion of code
 
-% GEN_DATA     = 'ON';
+GEN_DATA     = 'ON';
 
 if (~exist('GEN_DATA','var')), GEN_DATA = 'OFF'; end
-if (~exist('PLOT','var')),     PLOT     = 'OFF'; end
 
 % FusionCase = "inertial_gnss";
 % FusionCase = "inertial_visual";
 FusionCase = "inertial_visual_gnss";
+
 Sparse = "true";
+% Sparse = "false";
 
 fprintf('NaveGo: parameter FusionCase = %s \n', FusionCase)
 fprintf('NaveGo: parameter Sparse = %s \n', Sparse)
@@ -83,11 +81,10 @@
 
 if strcmp(GEN_DATA, 'ON')
 
-    fprintf('NaveGo: generating data... \n')
-
     if FusionCase == "inertial_gnss"
         fprintf('NaveGo: generating IMU data... \n')
         imu_structure;
+
         fprintf('NaveGo: generating GNSS data... \n')
         if Sparse == "true"
             gnss_sparse_structure;
@@ -141,8 +138,6 @@
 end
 %% Estimation
 
-fprintf('NaveGo: integration... \n')
-
 switch FusionCase
     case "inertial_gnss"
         fprintf('NaveGo: processing INS/GNSS integration... \n')
@@ -155,12 +150,12 @@
         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);
+[nav_i, nav_ref] = navego_interpolation (nav_e, gnss_planetary_r);
 
 if Sparse == "true"
-    [gnss_i,gnss_planetary_r_sparse] = navego_interpolation(gnss_planetary,gnss_planetary_r_sparse);
+    [gnss_i,gnss_ref] = navego_interpolation(gnss_planetary,gnss_planetary_sparse_r);
 else
-    [gnss_i,gnss_planetary_r] = navego_interpolation(gnss_planetary,gnss_planetary_r);
+    [gnss_i,gnss_ref] = navego_interpolation(gnss_planetary,gnss_planetary_r);
 end
 
 %% Plotting
@@ -194,8 +189,8 @@
         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)
+        plot(gnss_i.t,LAT2M_GR.*(gnss_i.lat - gnss_ref.lat), '.', 'Color', ones(1,3) * 0.75, 'LineWidth', 1.5)
+        plot(nav_i.t, LAT2M.*(nav_i.lat - nav_ref.lat),'Color', [0, 0, 0], 'LineWidth', 1.5)
         grid on;
         xlabel('Time [s]')
         ylabel('[m]')
@@ -204,8 +199,8 @@
         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)
+        plot(gnss_i.t, LON2M_GR.*(gnss_i.lon - gnss_ref.lon), '.', 'Color', ones(1,3) * 0.75, 'LineWidth', 1.5)
+        plot(nav_i.t, LON2M.*(nav_i.lon - nav_ref.lon),'Color', [0, 0, 0], 'LineWidth', 1.5)
         grid on;
         xlabel('Time [s]')
         ylabel('[m]')
@@ -239,8 +234,8 @@
         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)
+        plot(gnss_i.t,  LAT2M_GR.*(gnss_i.lat - gnss_ref.lat), '.', 'Color', ones(1,3) * 0.75, 'LineWidth', 1.5)
+        plot(nav_i.t, LAT2M.*(nav_i.lat - nav_ref.lat),'Color', [0, 0, 0], 'LineWidth', 1.5)
         grid on;
         xlabel('Time [s]')
         ylabel('[m]')
@@ -249,8 +244,8 @@
         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)
+        plot(gnss_i.t, LON2M_GR.*(gnss_i.lon - gnss_ref.lon), '.', 'Color', ones(1,3) * 0.75, 'LineWidth', 1.5)
+        plot(nav_i.t, LON2M.*(nav_i.lon - nav_ref.lon),'Color', [0, 0, 0], 'LineWidth', 1.5)
         grid on;
         xlabel('Time [s]')
         ylabel('[m]')
@@ -285,8 +280,8 @@
         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)
+        plot(gnss_i.t,  LAT2M_GR.*(gnss_i.lat - gnss_ref.lat), '.', 'Color', ones(1,3) * 0.75, 'LineWidth', 1.5)
+        plot(nav_i.t, LAT2M.*(nav_i.lat - nav_ref.lat),'Color', [0, 0, 0], 'LineWidth', 1.5)
         grid on;
         xlabel('Time [s]')
         ylabel('[m]')
@@ -295,8 +290,8 @@
         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)
+        plot(gnss_i.t, LON2M_GR.*(gnss_i.lon - gnss_ref.lon), '.', 'Color', ones(1,3) * 0.75, 'LineWidth', 1.5)
+        plot(nav_i.t, LON2M.*(nav_i.lon - nav_ref.lon),'Color', [0, 0, 0], 'LineWidth', 1.5)
         grid on;
         xlabel('Time [s]')
         ylabel('[m]')
diff --git a/examples/planetary-data/katwijk-planetary-data/data/gnss_planetary.mat b/examples/planetary-data/katwijk-planetary-data/data/gnss_planetary.mat
index 85ee53d..3e68e54 100644
Binary files a/examples/planetary-data/katwijk-planetary-data/data/gnss_planetary.mat and b/examples/planetary-data/katwijk-planetary-data/data/gnss_planetary.mat differ
diff --git a/examples/planetary-data/katwijk-planetary-data/data/gnss_planetary_r.mat b/examples/planetary-data/katwijk-planetary-data/data/gnss_planetary_r.mat
index a6a466f..66424ba 100644
Binary files a/examples/planetary-data/katwijk-planetary-data/data/gnss_planetary_r.mat and b/examples/planetary-data/katwijk-planetary-data/data/gnss_planetary_r.mat differ
diff --git a/examples/planetary-data/katwijk-planetary-data/data/gnss_planetary_sparse_r.mat b/examples/planetary-data/katwijk-planetary-data/data/gnss_planetary_sparse_r.mat
index a8ab5ae..73003e7 100644
Binary files a/examples/planetary-data/katwijk-planetary-data/data/gnss_planetary_sparse_r.mat and b/examples/planetary-data/katwijk-planetary-data/data/gnss_planetary_sparse_r.mat differ
diff --git a/examples/planetary-data/katwijk-planetary-data/data/gnss_sparse_planetary.mat b/examples/planetary-data/katwijk-planetary-data/data/gnss_sparse_planetary.mat
index c93414b..4b67dd2 100644
Binary files a/examples/planetary-data/katwijk-planetary-data/data/gnss_sparse_planetary.mat and b/examples/planetary-data/katwijk-planetary-data/data/gnss_sparse_planetary.mat differ
diff --git a/examples/planetary-data/katwijk-planetary-data/data/imu_planetary.mat b/examples/planetary-data/katwijk-planetary-data/data/imu_planetary.mat
index 97a7f7f..33e033c 100644
Binary files a/examples/planetary-data/katwijk-planetary-data/data/imu_planetary.mat and b/examples/planetary-data/katwijk-planetary-data/data/imu_planetary.mat differ
diff --git a/examples/planetary-data/katwijk-planetary-data/data/visual_planetary.mat b/examples/planetary-data/katwijk-planetary-data/data/visual_planetary.mat
index be98f01..f57584c 100644
Binary files a/examples/planetary-data/katwijk-planetary-data/data/visual_planetary.mat and b/examples/planetary-data/katwijk-planetary-data/data/visual_planetary.mat differ
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
index 13348b0..bc0061e 100755
--- a/examples/planetary-data/katwijk-planetary-data/scripts/gnss_sparse_structure.m
+++ b/examples/planetary-data/katwijk-planetary-data/scripts/gnss_sparse_structure.m
@@ -91,11 +91,11 @@
 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
+gnss_planetary_sparse_r = gnss_planetary; % reference
+[gnss_planetary,~] = gnss_gen(gnss_planetary_sparse_r, gnss_planetary); % degredation
 
 %% Saving 
 
 save('../data/gnss_planetary_r.mat','gnss_planetary_r');
-save('../data/gnss_planetary_sparse_r.mat','gnss_planetary_r_sparse');
+save('../data/gnss_planetary_sparse_r.mat','gnss_planetary_sparse_r');
 save('../data/gnss_sparse_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
index e95619e..538a3dc 100755
--- a/examples/planetary-data/katwijk-planetary-data/scripts/imu_structure.m
+++ b/examples/planetary-data/katwijk-planetary-data/scripts/imu_structure.m
@@ -56,7 +56,7 @@
 
 % time 
 timestamps = table2array(imu_data_planetary(:,1));
-timestamps = (timestamps - timestamps(1));
+timestamps = (timestamps - timestamps(1)) / 1e9;
 dt = mean(diff(timestamps));
 imu_planetary.t = (timestamps(1):dt:timestamps(end))';
 imu_planetary.freq = 1/dt; 
diff --git a/examples/planetary-data/katwijk-planetary-data/scripts/navego_example_katwijk_data.m b/examples/planetary-data/katwijk-planetary-data/scripts/navego_example_katwijk_data.m
index 1b99187..cb2cd9b 100755
--- a/examples/planetary-data/katwijk-planetary-data/scripts/navego_example_katwijk_data.m
+++ b/examples/planetary-data/katwijk-planetary-data/scripts/navego_example_katwijk_data.m
@@ -1,28 +1,141 @@
-% Johann Diep (johann.diep@esa.int) - November 2021
+% navego_example_katwijk_data: post-processing integration of
+% IMU, GNSS and visual data for the Katwijk dataset.
 %
-% This script runs the fusion of all sensor parameters. 
+% The main goal is to integrate IMU and GNSS measurements from Ekinox-D
+% sensor which includes both IMU and GNSS sensors.
+%
+% Sensors dataset was generated driving a car through the streets of
+% Turin city (Italy).
+%
+%   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
+%   <http://www.gnu.org/licenses/>.
+%
+% References:
+%
+%
+% Version: 001
+% Date:    2021/11/01
+% Author:  Johann Diep <johanndiep@gmail.com>
+% URL:     https://github.com/rodralez/navego
+%
+% NOTE: NaveGo assumes that IMU is aligned with respect to body-frame as
+% X-forward, Y-right and Z-down.
+%
+% NOTE: NaveGo assumes that yaw angle (heading) is positive clockwise.
+
+clc
+close all
+clear
+matlabrc
+
+addpath ../../../../ins/
+addpath ../../../../ins-gnss/
+addpath ../../../../ins-visual/
+addpath ../../../../ins-visual-gnss/
+addpath ../../../../conversions/
+addpath ../../../../performance-analysis/
+addpath ../../../../plot/
+addpath ../../../../misc/
+addpath ../../../../simulation/
+addpath ../data/
+
+navego_print_version;
+
+fprintf('\nNaveGo: starting Katwijk planetary data integration... \n')
 
-clear; clc; close all;
+%% PARAMETERS
 
+% Comment any of the following parameters in order to NOT execute a
+% particular portion of code
+
+GEN_DATA     = 'ON';
+
+if (~exist('GEN_DATA','var')), GEN_DATA = 'OFF'; end
+
+% FusionCase = "inertial_gnss";
+% FusionCase = "inertial_visual";
 FusionCase = "inertial_visual_gnss";
+
 Sparse = "true";
+% Sparse = "false";
+
+fprintf('NaveGo: parameter FusionCase = %s \n', FusionCase)
+fprintf('NaveGo: parameter Sparse = %s \n', Sparse)
 
 %% Generating Data
 
-if FusionCase == "inertial_gnss"
-    imu_structure;
-    if Sparse == "true"
-        gnss_sparse_structure;
+%% Generating Data
+
+if strcmp(GEN_DATA, 'ON')
+
+    if FusionCase == "inertial_gnss"
+        fprintf('NaveGo: generating IMU data... \n')
+        imu_structure;
+
+        fprintf('NaveGo: generating GNSS data... \n')
+        if Sparse == "true"
+            gnss_sparse_structure;
+        else
+            gnss_structure;
+        end
     else
-        gnss_structure;
+        imu_structure;
+        fprintf('NaveGo: generating IMU data... \n')
+        visual_structure;
+        fprintf('NaveGo: generating VISUAL data... \n')
+        if Sparse == "true"
+            fprintf('NaveGo: generating GNSS data... \n')
+            gnss_sparse_structure;
+        else
+            gnss_structure;
+        end
     end
 else
-    imu_structure;
-    visual_structure;
-    if Sparse == "true"
-        gnss_sparse_structure;
+    fprintf('NaveGo: loading data... \n')
+
+    if FusionCase == "inertial_gnss"
+        %         imu_structure;
+        load imu_planetary
+        if Sparse == "true"
+            %             gnss_sparse_structure;
+            load gnss_planetary_r
+            load gnss_planetary_sparse_r
+            load gnss_planetary
+        else
+            %             gnss_structure;
+            load gnss_planetary_r
+            load gnss_planetary
+        end
     else
-        gnss_structure;
+        %         imu_structure;
+        load imu_planetary
+        %         visual_structure;
+        load visual_planetary
+        if Sparse == "true"
+            %             gnss_sparse_structure;
+            load gnss_planetary_r
+            load gnss_planetary_sparse_r
+            load gnss_planetary
+        else
+            %             gnss_structure;
+            load gnss_planetary_r
+            load gnss_planetary
+        end
     end
 end
 
@@ -30,24 +143,29 @@
 
 switch FusionCase
     case "inertial_gnss"
+        fprintf('NaveGo: processing INS/GNSS integration... \n')
         nav_e = ins_gnss(imu_planetary,gnss_planetary,'dcm');
     case "inertial_visual"
-        nav_e = ins_visual(imu_planetary,gnss_planetary_r,visual_planetary,'dcm');
+        fprintf('NaveGo: processing INS/VISUAL integration... \n')
+        nav_e = ins_visual(imu_planetary,gnss_planetary_r,visual_planetary,'dcm'); % note: figure out why we input gnss_planetary_r
     case "inertial_visual_gnss"
+        fprintf('NaveGo: processing INS/GNSS/VISUAL integration... \n')
         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);
+[nav_i, nav_ref] = navego_interpolation (nav_e, gnss_planetary_r);
 
 if Sparse == "true"
-    [gnss_i,gnss_planetary_r_sparse] = navego_interpolation(gnss_planetary,gnss_planetary_r_sparse);
+    [gnss_i,gnss_ref] = navego_interpolation(gnss_planetary,gnss_planetary_sparse_r);
 else
-    [gnss_i,gnss_planetary_r] = navego_interpolation(gnss_planetary,gnss_planetary_r);
+    [gnss_i,gnss_ref] = navego_interpolation(gnss_planetary,gnss_planetary_r);
 end
 
 %% Plotting
 
-switch FusionCase 
+fprintf('NaveGo: plotting... \n')
+
+switch FusionCase
     %% Plotting: IMU + GNSS
     case "inertial_gnss"
 
@@ -62,32 +180,30 @@
         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)
+        plot(gnss_i.t,LAT2M_GR.*(gnss_i.lat - gnss_ref.lat), '.', 'Color', ones(1,3) * 0.75, 'LineWidth', 1.5)
+        plot(nav_i.t, LAT2M.*(nav_i.lat - nav_ref.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)
+        plot(gnss_i.t, LON2M_GR.*(gnss_i.lon - gnss_ref.lon), '.', 'Color', ones(1,3) * 0.75, 'LineWidth', 1.5)
+        plot(nav_i.t, LON2M.*(nav_i.lon - nav_ref.lon),'Color', [0, 0, 0], 'LineWidth', 1.5)
         grid on;
         xlabel('Time [s]')
         ylabel('[m]')
@@ -95,9 +211,9 @@
         title('Longitude Error');
         xlim([0,max(gnss_planetary.t)]);
 
-    %% Plotting: Vision + IMU
+        %% Plotting: Vision + IMU
     case "inertial_visual"
-    
+
         % Position
         figure();
         hold on;
@@ -109,7 +225,6 @@
         ylabel('Latitude');
         legend('OpenVINS','RTK','IMU + OpenVINS','Location','Southeast');
         axis equal;
-
         % Position Errors
         [RN,RE]  = radius(nav_i.lat);
         LAT2M = RN + nav_i.h;
@@ -118,33 +233,32 @@
         [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)
+        plot(gnss_i.t,  LAT2M_GR.*(gnss_i.lat - gnss_ref.lat), '.', 'Color', ones(1,3) * 0.75, 'LineWidth', 1.5)
+        plot(nav_i.t, LAT2M.*(nav_i.lat - nav_ref.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)
+        plot(gnss_i.t, LON2M_GR.*(gnss_i.lon - gnss_i.lon), '.', 'Color', ones(1,3) * 0.75, 'LineWidth', 1.5)
+        plot(nav_i.t, LON2M.*(nav_i.lon - nav_ref.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
+
+        %% Plotting: Vision + GNSS + INS
     case "inertial_visual_gnss"
-    
+
         % Position
         figure();
         hold on;
@@ -156,7 +270,7 @@
         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;
@@ -165,23 +279,22 @@
         [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)
+        plot(gnss_i.t,  LAT2M_GR.*(gnss_i.lat - gnss_ref.lat), '.', 'Color', ones(1,3) * 0.75, 'LineWidth', 1.5)
+        plot(nav_i.t, LAT2M.*(nav_i.lat - nav_ref.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)
+        plot(gnss_i.t, LON2M_GR.*(gnss_i.lon - gnss_ref.lon), '.', 'Color', ones(1,3) * 0.75, 'LineWidth', 1.5)
+        plot(nav_i.t, LON2M.*(nav_i.lon - nav_ref.lon),'Color', [0, 0, 0], 'LineWidth', 1.5)
         grid on;
         xlabel('Time [s]')
         ylabel('[m]')
diff --git a/ins-visual-gnss/ins_visual_gnss.m b/ins-visual-gnss/ins_visual_gnss.m
index 9af17e5..661d488 100755
--- a/ins-visual-gnss/ins_visual_gnss.m
+++ b/ins-visual-gnss/ins_visual_gnss.m
@@ -348,7 +348,7 @@
                 + (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))' ...
+            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
@@ -477,7 +477,7 @@
             + (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))' ...
+        zv = (vel_e(i,:) - gnss.vel(gdx,:) - ((omega_ie_n + omega_en_n) * (DCMbn * gnss.larm))' ...
             + (DCMbn * skewm(wb_corrected) * gnss.larm )' )';
         
         %% KALMAN FILTER
@@ -496,7 +496,7 @@
             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 ]';
+            kf.z = [ zv' zp' ]';
         else
             kf.H = [ O I O O O ; ];
             kf.R = 0.001*diag([gnss.stdv]).^2;
diff --git a/ins-visual/ins_visual.m b/ins-visual/ins_visual.m
index a5ddbe5..89aff5e 100755
--- a/ins-visual/ins_visual.m
+++ b/ins-visual/ins_visual.m
@@ -345,7 +345,7 @@
                 + (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))' ...
+            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