多传感器融合（三）--- NaveGO工具箱

最新推荐文章于 2025-01-27 09:17:23 发布

星期五不见面

最新推荐文章于 2025-01-27 09:17:23 发布

阅读量1.4k

点赞数 4

CC 4.0 BY-SA版权

文章标签：硬件工程 github matlab

本文链接：https://blog.csdn.net/weixin_65303135/article/details/131904458

NaveGo是一个用于处理集成导航系统，如INS/GNSS、INS/GNSS/MAG和VISUAL/INS/GNSS的开源MATLAB/GNU-Octave工具箱。它支持惯性传感器分析、仿真和GNSS中断的可视化。工具箱提供艾伦方差技术评估传感器性能，并包含多个示例，包括真实和合成数据的处理。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

一介绍

NaveGo：一个开源的MATLAB/GNU-Octave工具箱，用于处理集成导航系统并执行惯性传感器剖析分析。

NaveGo 是一个开源的 MATLAB/GNU Octave 工具箱，用于处理集成导航系统、模拟惯性传感器和 GNSS 接收器。它还使用艾伦方差对惯性传感器进行分析。它可以在线免费获得。它是在MATLAB / GNU-Octave下开发的，因为这种编程语言已成为物理系统工程和建模的事实标准。

NaveGo的座右铭是“为大众带来综合导航”。

二特征

NaveGo的主要特点是：

惯性导航系统（INS）的处理。
处理松散耦合的综合导航系统（INS/GNSS）。
处理带有磁力计（INS/GNSS/MAG）的松散耦合集成导航系统。
新增功能处理松散耦合的集成视觉导航系统（VISUAL/INS/GNSS）。
使用磁力计的指南针航向。
惯性传感器和全球导航卫星系统的仿真。
零速度更新（ZUPT）检测算法。
艾伦方差技术来表征惯性传感器的确定性和随机性误差。
更好地可视化 GNSS 中断。

三例子

该文件夹包含几种类型的示例，这些示例试图成为使用NaveGo的一种介绍。example

3.1 Allan variance example

此示例可以通过执行文件来分析。首先，将艾伦方差应用于Sensonor STIM2 IMU的300小时实际静态测量。然后，创建近 5 小时的合成惯性数据，并在这些模拟数据上运行 Allan 方差。navego_example_allan.m

3.2 INS/GNSS integration example using synthetic (simulated) data 合成数据

具有合成数据的 NaveGo 示例基于轨迹生成器的输出。该程序为先前定义的轨迹提供了真实加速度和角速度。NaveGo不提供轨迹生成器。该文件试图逐步公开如何使用NaveGo。根据ADIS16405 IMU和ADIS16488 IMU的误差曲线仿真两个IMU测量值。然后，使用模拟的GNSS传感器融合两个IMU。最后，比较了两个模拟INS/GNSS系统的性能。navego_example_synth.m

3.3 INS/GNSS integration example using real data 真实数据

提供了两个如何使用NaveGo对真实数据进行后处理的示例，一个用于战术级Ekinox-D IMU，另一个用于消费级MPU-6000 IMU。两个IMU都与Ekinox-D GNSS集成。这些数据集是通过驾驶车辆穿过都灵市（意大利）的街道生成的。navego_example_real_xxxx.m

3.4 VISUAL/INS/GNSS integration example using real data 真实数据

为VISUAL/INS/GNSS 集成提供了两个示例：examples/visual-data/

1.navego_example_canada_data.m

2.navego_example_katwijk_data.m

本文重点介绍此示例内容。

两个示例的区别是，一个使用的是katwijk数据集，一个是canada数据集，代码结构上没有区别。

4 源码解释

4.1 程序初始化，并引入脚本，以及添加路径，并打印信息

clc                                 %用于清除MATLAB的命令行窗口
close all                           %关闭了MATLAB中所有已打开的图形窗口，以便在开始任务前清除之前的图形。
clear
matlabrc   %在启动MATLAB时运行MATLAB启动脚本，该脚本可以设置MATLAB的默认选项和环境变量。

addpath ../../ins/               %将指定的目录添加到MATLAB的搜索路径中，以便MATLAB可以在这些目录中找到所需的函数和脚本。
addpath ../../ins-gnss/
addpath ../../ins-visual/
addpath ../../conversions/
addpath ../../performance-analysis/
addpath ../../plot/
addpath ../../misc/
addpath ../../simulation/
addpath ./canada-planetary-data/data/
addpath ./canada-planetary-data/scripts/

navego_print_version;  %用于打印NaveGo工具箱的版本信息。

fprintf('\nNaveGo: starting Canada planetary data integration... \n')   %打印一条消息，表示NaveGo正在开始Katwijk行星数据的整合工作。

4.2 参数设置，用于判断是否生成数据，选择哪种融合方式，是否产生GPS失灵等干扰场景

%% PARAMETERS

% 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  
%检查是否已经定义了名为GEN_DATA的变量，如果没有，则将GEN_DATA的值设置为字符串'OFF'。这样，如果代码的其他地方没有定义GEN_DATA变量，它默认为'OFF'。

% FusionCase = "inertial_gnss";       
FusionCase = "inertial_visual";          %设置了名为FusionCase的变量，它的值是字符串"inertial_visual"。该变量可能用于选择数据融合的方案。
%FusionCase = "inertial_visual_gnss";

% Method to remove GNSS measurements for a certain time window. This is 
% done to showcase that visual measurements provide a failsafe option in 
% case satellite data is not available.   该变量用于控制是否在一定时间窗口内移除GNSS测量数据，以演示当卫星数据不可用时，视觉测量提供的故障容错选项。
% Sparse = "true";     %制作特殊场景
Sparse = "false";

fprintf('NaveGo: parameter FusionCase = %s \n', FusionCase)
fprintf('NaveGo: parameter Sparse = %s \n', Sparse)

4.3 生成数据或者加载数据

%% Generating Data   生成数据

if strcmp(GEN_DATA, 'ON')    %用于检查GEN_DATA变量的值是否等于字符串'ON'。如果是，则执行条件内的代码块，否则跳过该代码块。

    if FusionCase == "inertial_gnss"   %用于检查FusionCase变量的值是否等于字符串"inertial_gnss"。如果FusionCase是"inertial_gnss"，则生成GNSS数据，要么是正常的GNSS数据要么是稀疏的GNSS数据，具体是哪种数据由Sparse参数控制。
        fprintf('NaveGo: generating IMU data... \n')  %打印一条消息，指示正在生成惯性测量单元（IMU）数据。
        imu_structure;  %函数调用，它可能会生成IMU数据的结构。

        fprintf('NaveGo: generating GNSS data... \n')
        if Sparse == "true"
            gnss_sparse_structure;
        else
            gnss_structure;
        end
    else  %如果FusionCase不是"inertial_gnss"，则继续生成视觉（visual）数据，然后根据Sparse参数的设置生成相应的GNSS数据。
        fprintf('NaveGo: generating IMU data... \n')
        imu_structure;
        fprintf('NaveGo: generating VISUAL data... \n')
        visual_structure;
        if Sparse == "true"
            fprintf('NaveGo: generating GNSS data... \n')
            gnss_sparse_structure;
        else
            gnss_structure;
        end
    end
else  %这段代码块负责加载数据而不是生成数据。
    fprintf('NaveGo: loading data... \n')   %里加载先前保存的数据而不是生成新的数据。

    if FusionCase == "inertial_gnss"
        load imu_planetary
        if Sparse == "true"
            load gnss_planetary_r
            load gnss_planetary_sparse_r
            load gnss_planetary
        else
            load gnss_planetary_r
            load gnss_planetary
        end
    else
        load imu_planetary
        load visual_planetary
        if Sparse == "true"
            load gnss_planetary_r
            load gnss_planetary_sparse_r
            load gnss_planetary
        else
            load gnss_planetary_r
            load gnss_planetary
        end
    end
end

4.4 开始融合并计算导航解，以及插值计算后的导航解

%% Estimation    估计

switch FusionCase     %这是一个switch语句，它将根据FusionCase变量的值选择执行不同的代码块。
    case "inertial_gnss"   %匹配FusionCase等于"inertial_gnss"的情况。
        fprintf('NaveGo: processing INS/GNSS integration... \n')   %打印函数，用于在MATLAB命令行窗口打印不同数据融合方法的处理信息。
        nav_e = ins_gnss(imu_planetary,gnss_planetary,'dcm');  
        %这行代码根据"惯性-导航系统/GNSS"（Inertial Navigation System/GNSS）融合方案，调用ins_gnss函数进行数据融合。函数接受IMU数据和GNSS数据作为输入，并输出融合后的导航解。
    
    case "inertial_visual"
        fprintf('NaveGo: processing INS/VISUAL integration... \n')
        nav_e = ins_visual(imu_planetary,gnss_planetary_r,visual_planetary,'dcm'); 
        % 这行代码根据"惯性-导航系统/视觉"（Inertial Navigation System/Visual）融合方案，调用ins_visual函数进行数据融合。函数接受IMU数据、GNSS数据和视觉数据作为输入，并输出融合后的导航解。
    
    case "inertial_visual_gnss"
        fprintf('NaveGo: processing INS/GNSS/VISUAL integration... \n')
        nav_e = ins_visual_gnss(imu_planetary,gnss_planetary,visual_planetary,'dcm');
        %这行代码根据"惯性-导航系统/视觉/GNSS"（Inertial Navigation System/Visual/GNSS）融合方案，调用ins_visual_gnss函数进行数据融合。函数接受IMU数据、GNSS数据和视觉数据作为输入，并输出融合后的导航解。
end

[nav_i, nav_ref] = navego_interpolation (nav_e, gnss_planetary_r);  
%这行代码调用navego_interpolation函数对导航解进行插值处理。这个函数接受导航解和GNSS数据作为输入，并输出插值后的导航解和参考数据。

if Sparse == "true"
    [gnss_i,gnss_ref] = navego_interpolation(gnss_planetary,gnss_planetary_sparse_r);
else
    [gnss_i,gnss_ref] = navego_interpolation(gnss_planetary,gnss_planetary_r);
end

4.5 出图，分为三种情况，但结构相同仅换参数，只以第一中 IMU+GNSS 为例

%% Plotting  标图

fprintf('NaveGo: plotting... \n')

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);  %使用plot函数绘制gnss_planetary的经度和纬度数据，以点的形式表示，并使用灰色显示。
            % 其中: rad2deg 是MATLAB中的一个函数，用于将弧度值转换为角度值。，ones(1,3) 是一个MATLAB函数，它返回一个包含三个元素的行向量，其中所有元素的值都为1。这样得到的行向量表示了一个RGB颜色，因为RGB颜色由三个分量（红、绿和蓝）组成，每个分量的取值范围是0到1。
        
        scatter(rad2deg(gnss_planetary_r.lon),rad2deg(gnss_planetary_r.lat),'y.','MarkerEdgeAlpha',.6);  %使用scatter函数绘制gnss_planetary_r的经度和纬度数据，以黄色点的形式表示，带有边缘透明度。
         %rad2deg(gnss_planetary_r.lon) 和 rad2deg(gnss_planetary_r.lat) 是将GNSS数据中的经度和纬度值从弧度转换为度数。这样得到的经度和纬度值将以度为单位表示，方便在图形上显示。
         %scatter 函数用于绘制散点图。它以经度和纬度值作为输入，以散点的形式在图形上显示这些点。
         %'y.' 设置散点的样式。'y' 表示散点使用黄色（yellow）作为颜色，'.' 表示散点使用小圆点作为标记。
         %'MarkerEdgeAlpha', .6 设置散点的边缘透明度为0.6。这样做可以使散点的边缘有一定的透明度，让散点更具可视化效果。


        plot(rad2deg(nav_e.lon),rad2deg(nav_e.lat),'Color', [0, 0, 0], 'LineWidth', 1.5);  %使用plot函数绘制nav_e的经度和纬度数据，以黑色线条表示。
        grid on;   %显示网格线
        xlabel('Longitude [deg]');   %设置x轴和y轴的标签。
        ylabel('Latitude [deg]');
        legend('degraded GNSS','RTK','IMU + degraded GNSS','Location','Southeast');    %添加图例说明不同数据的含义。
        axis equal;  %将x轴和y轴的刻度比例设置为相等，保持图形的长宽比一致。

        % Position Errors   位置误差
        [RN,RE]  = radius(nav_i.lat);   
        %RN 和 RE 是计算赤道半径（RN）和极半径（RE）的结果。这两个值用于计算地球上不同纬度处的纬度和经度的距离单位。
        %radius 函数计算地球上特定纬度位置的赤道半径和极半径。nav_i.lat 是导航解的纬度值。

        LAT2M = RN + nav_i.h;
        %LAT2M = RN + nav_i.h; 计算了纬度误差的距离单位。它将导航解中的纬度误差 nav_i.lat 转换为米，加上导航解中的高度 nav_i.h，得到了纬度误差对应的地球上的纬度距离。

        LON2M = (RE + nav_i.h).*cos(nav_i.lat);
        %LON2M = (RE + nav_i.h).*cos(nav_i.lat); 计算了经度误差的距离单位。它将导航解中的纬度误差 nav_i.lat 转换为米，乘以对应纬度处的纬度圆的周长，即 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);   % 【 .* 】 是MATLAB中的一个运算符，用于执行逐元素相乘操作。

        figure();
        subplot(2,1,1);  %创建两个子图中的第一个子图，表示纬度误差的变化。
        hold on;
        plot(gnss_i.t,LAT2M_GR.*(gnss_i.lat - gnss_ref.lat), '.', 'Color', ones(1,3) * 0.75, 'LineWidth', 1.5)  
        %gnss_i.t 是GNSS数据的时间向量。
        % LAT2M_GR.*(gnss_i.lat - gnss_ref.lat) 计算了GNSS纬度误差距离单位（LAT2M_GR）乘以 GNSS纬度误差 (gnss_i.lat - gnss_ref.lat)。这样得到的结果是一个向量，表示了纬度误差随时间的变化。
        
        plot(nav_i.t, LAT2M.*(nav_i.lat - nav_ref.lat),'Color', [0, 0, 0], 'LineWidth', 1.5)   %nav_i.t 是IMU+降级GNSS数据的时间向量；；
              % LAT2M.*(nav_i.lat - nav_ref.lat) 计算了IMU+降级GNSS纬度误差距离单位（LAT2M）乘以 IMU+降级GNSS纬度误差 (nav_i.lat - nav_ref.lat)。这样得到的结果是一个向量，表示了纬度误差随时间的变化。
        grid on;
        xlabel('Time [s]')
        ylabel('[m]')
        legend('GNSS', 'IMU + degraded GNSS', 'Location', 'northoutside');
        title('Latitude Error');   %%设置子图形的标题。
        xlim([0,max(gnss_planetary.t)]);  %设置x轴的显示范围。
        subplot(2,1,2);
        hold on;
        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]')
        legend('GNSS', 'IMU + degraded GNSS', 'Location', 'northoutside');
        title('Longitude Error'); 
        xlim([0,max(gnss_planetary.t)]);