武汉大学组合导航综合实习算法

项目开源地址：https://github.com/whu-qqf/IntegratedNav

IntegratedNav 自编程序说明文档

本程序包用于 GNSS/INS 松组合导航实验。核心程序采用 C++ 编写，主要实现纯惯导解算、GNSS/INS 松组合滤波、杆臂改正、IMU 误差反馈、可选高程/NHC 约束、离线前后向处理以及 UDP 伪实时松组合解算。辅助程序采用 Python 和 PowerShell 编写，用于编译运行封装、误差统计、绘图和伪实时数据播发。

1. 编程工具和版本说明

本工程主要在 Windows 环境下开发和运行。

核心导航解算程序使用 C++ 编写，编译工具为 Microsoft Visual C++，工程编译标准为 C++17。代码编辑和工程管理主要使用 Visual Studio Code。C++ 程序负责实现惯导机械编排、GNSS/INS 松组合 Kalman 滤波、GNSS 位置速度量测更新、杆臂观测模型、IMU 零偏反馈、约束更新和 UDP 伪实时解算。

辅助程序主要使用 Python 和 PowerShell 编写。Python 脚本用于误差统计、结果绘图、报告图片生成、UDP 数据播发和实时轨迹显示；绘图主要使用 matplotlib。PowerShell 脚本用于封装编译、离线运行、实时运行和数据播发流程，不参与核心算法计算。

主要工具如下：

版本号可在本机通过以下命令查看：

cl
python --version
py -0p

2. 程序包文件结构

提交程序包主要包含 config、src、script、build-nmake 和 README.md。其中 src 为 C++ 源程序，script 为辅助脚本，config 为示例配置文件，build-nmake 中保存可执行程序。

IntegratedNav/
├─ config/
│  └─ dataset_lc_template.ini
│
├─ src/
│  ├─ main.cpp
│  ├─ Config.cpp / Config.h
│  ├─ Struct.h
│  ├─ const.h
│  ├─ Matrix.cpp / Matrix.h
│  ├─ Coordinate.cpp / Coordinate.h
│  ├─ INS.cpp / INS.h
│  ├─ GNSS.cpp / GNSS.h
│  ├─ LooseCouple.cpp / LooseCouple.h
│  └─ RealtimeLC.cpp / RealtimeLC.h
│
├─ script/
│  ├─ build_cl.ps1
│  ├─ run_lc.ps1
│  ├─ run_realtime_lc.ps1
│  ├─ run_realtime_view.ps1
│  ├─ run_udp_replay.ps1
│  ├─ run_udp_live_plot.ps1
│  ├─ plot_lc.py
│  ├─ plot_report_figures.py
│  ├─ plot_realtime_error.py
│  ├─ compare_ref.py
│  ├─ lc_error_stats.py
│  ├─ plot_rtk.py
│  ├─ plot_pure_ins.py
│  ├─ udp_replay.py
│  └─ udp_live_plot.py
│
├─ build-nmake/
│  └─ ins_init.exe
│
└─ README.md

主要 C++ 源文件功能如下：

辅助脚本功能如下：

此处可在报告中插入源文件列表截图。

3. 编译成功信息

编译命令为：

powershell -NoProfile -ExecutionPolicy Bypass -File .\script\build_cl.ps1

编译成功后生成：

build-nmake\ins_init.exe

编译成功时终端会显示类似信息：

Visual Studio Developer Command Prompt
[vcvarsall.bat] Environment initialized for: 'x64'
main.cpp
Config.cpp
Matrix.cpp
Coordinate.cpp
INS.cpp
GNSS.cpp
LooseCouple.cpp
RealtimeLC.cpp
正在生成代码...

此处可在报告中插入编译成功截图。

4. 自编可执行程序使用说明

自编可执行程序文件名为：

build-nmake\ins_init.exe

该程序需要配置文件。示例配置文件为：

config\dataset_lc_template.ini

4.1 离线松组合运行

直接运行 C++ 可执行程序：

.\build-nmake\ins_init.exe config\dataset_lc_template.ini

也可以使用封装脚本一键运行：

powershell -NoProfile -ExecutionPolicy Bypass -File .\script\run_lc.ps1

若已经编译完成，可跳过编译：

powershell -NoProfile -ExecutionPolicy Bypass -File .\script\run_lc.ps1 -SkipBuild

离线运行主要输出：

output\pure_ins.txt
output\loose_coupling.txt
output\loose_coupling_forward.txt
output\loose_coupling_backward.txt
output\lc_diagnostics.txt
output\lc_smooth_diagnostics.txt
output\lc_imu_error.txt
output\lc_vs_ref_summary.txt
output\lc_vs_ref_timeseries.txt
output\lc_vs_ref_trajectory.png
output\lc_vs_ref_error.png
output\lc_trajectory_all.png
output\lc_error.png
output\lc_states.png

其中 output\loose_coupling.txt 为松组合主结果文件。

4.2 UDP 伪实时松组合运行

实时松组合可执行程序运行方式为：

.\build-nmake\ins_init.exe config\dataset_lc_template.ini udp

推荐使用两个终端运行。

终端 1：启动实时松组合和实时轨迹显示：

powershell -NoProfile -ExecutionPolicy Bypass -File .\script\run_realtime_view.ps1 -SkipBuild

等待输出：

Realtime LC UDP started.

终端 2：启动 IMU/GNSS 伪实时播发：

powershell -NoProfile -ExecutionPolicy Bypass -File .\script\run_udp_replay.ps1 -Speed 1

-Speed 1 表示按原始采样时间播发，IMU 约 200 Hz，GNSS 约 1 Hz。

实时运行主要输出：

output\realtime_lc.txt
output\realtime_lc_imu_error.txt

5. 示例配置文件说明

程序运行需要配置文件。提交程序包中提供示例配置文件：

config\dataset_lc_template.ini

配置文件主要包括数据路径、时间范围、初始状态、坐标系、杆臂、IMU 参数、GNSS 参数、滤波参数、约束开关、UDP 端口和参考结果路径。

配置文件节选如下：

[files]
gnss_file = data/gnss_data/gnss_20260602_100202_517487.pos
imu_file = data/imu_data/imu.txt

[time]
time_system = GPST
gps_week = 2421
start_sow = 180441.418130
end_sow = 182331.00

[initial_state]
init_sow = 180441.418130
velocity_source = zero
ecef_x_m = -2267689.2389
ecef_y_m = 5009392.1939
ecef_z_m = 3220979.2851
vn_mps = 0.0
ve_mps = 0.0
vd_mps = 0.0
manual_align = true
roll_deg = 0.0
pitch_deg = 0.0
yaw_deg = 0.0

[lever_arm]
frame = IMU_RFU_MEASURED
x_m = -0.0005
y_m = 0.14
z_m = -0.005
to_body_frd = y,x,-z

[lc_filter]
use_lever_arm_observation = true
use_forward_backward = true

[lc_constraint]
use_height_constraint = true
use_vertical_velocity_constraint = true
use_lateral_velocity_constraint = true

[reference]
ref_pos_file = data/ref/RTK_LC.pos
compare_attitude = false

常用配置说明：

• [files]：IMU 和 GNSS 文件路径。
• [time]：解算起止 GPS 周内秒。
• [initial_state]：初始位置、速度、姿态和静止对准时间段。
• [lever_arm]：杆臂，定义为 IMU 中心指向 GNSS 天线相位中心。
• [lc_filter]：滤波初值、过程噪声、反馈开关和前后向开关。
• [lc_constraint]：高程约束、垂向速度约束和 NHC 侧向速度约束开关。
• [udp_replay]：UDP 播发端口和播发速度。
• [realtime_lc]：实时松组合输出文件和超时设置。
• [reference]：参考结果路径，当前使用 data/ref/RTK_LC.pos。

若只进行普通 INS/GNSS 松组合，不使用约束和前后向处理，可设置：

[lc_filter]
use_forward_backward = false

[lc_constraint]
use_height_constraint = false
use_vertical_velocity_constraint = false
use_lateral_velocity_constraint = false

实时 UDP 模式始终只做前向滤波，不进行后向平滑。

6. 其他辅助程序使用说明

生成实验报告用图片：

py -3 .\script\plot_report_figures.py

输出包括：

output\report_01_trajectory_compare.png/.svg
output\report_02_position_error_ned.png/.svg
output\report_03_velocity_ned.png/.svg
output\report_03b_velocity_error_ned.png/.svg
output\report_04_attitude_rpy.png/.svg
output\report_05_gyro_bias.png/.svg
output\report_06_accel_bias.png/.svg
output\report_07_filter_std.png/.svg
output\report_08_smooth_filter_std.png/.svg

离线结果与参考结果对比：

py -3 .\script\compare_ref.py

伪实时结果误差绘图和 RMS 统计：

py -3 .\script\plot_realtime_error.py

输出包括：

output\realtime_02_position_error_ned.png/.svg
output\realtime_03_velocity_error_ned.png/.svg
output\realtime_lc_vs_ref_summary.txt
output\realtime_lc_vs_ref_timeseries.txt

单独启动实时轨迹显示：

powershell -NoProfile -ExecutionPolicy Bypass -File .\script\run_udp_live_plot.ps1

若 py -3 不可用，可根据本机 Python 安装情况改用：

python .\script\plot_report_figures.py
python .\script\plot_realtime_error.py

7. 算法流程说明

松组合滤波状态量为 15 维：

位置误差(3) + 速度误差(3) + 姿态误差(3) + 陀螺零偏(3) + 加速度计零偏(3)

离线松组合流程如下：

1. 读取配置文件、IMU 数据和 GNSS/RTK 结果。
2. 根据初始静止段完成初始状态设置和 IMU 零偏估计。
3. 每个 IMU 历元进行惯导机械编排，同时进行滤波器 P 阵时间更新。
4. 若 GNSS 历元位于两个 IMU 历元之间，则先将 IMU 线性插值到 GNSS 时刻，再在该时刻进行 GNSS 更新。
5. GNSS 更新后反馈位置、速度、姿态和 IMU 零偏。
6. 若配置开启约束，则进行高程、垂向速度和 NHC 侧向速度约束更新。
7. 若配置开启前后向滤波，则先保存前向结果，再进行后向滤波，最后对位置和速度进行三通道平滑，姿态保留前向结果。

UDP 伪实时模式与前向离线滤波流程一致，也会在 GNSS 位于两个 IMU 历元之间时进行 IMU 插值，但实时模式不进行后向滤波。

IntegratedNav 自编程序说明文档

本程序包用于 GNSS/INS 松组合导航实验。核心程序采用 C++ 编写，主要实现纯惯导解算、GNSS/INS 松组合滤波、杆臂改正、IMU 误差反馈、可选高程/NHC 约束、离线前后向处理以及 UDP 伪实时松组合解算。辅助程序采用 Python 和 PowerShell 编写，用于编译运行封装、误差统计、绘图和伪实时数据播发。

1. 编程工具和版本说明

本工程主要在 Windows 环境下开发和运行。

核心导航解算程序使用 C++ 编写，编译工具为 Microsoft Visual C++，工程编译标准为 C++17。代码编辑和工程管理主要使用 Visual Studio Code。C++ 程序负责实现惯导机械编排、GNSS/INS 松组合 Kalman 滤波、GNSS 位置速度量测更新、杆臂观测模型、IMU 零偏反馈、约束更新和 UDP 伪实时解算。

辅助程序主要使用 Python 和 PowerShell 编写。Python 脚本用于误差统计、结果绘图、报告图片生成、UDP 数据播发和实时轨迹显示；绘图主要使用 matplotlib。PowerShell 脚本用于封装编译、离线运行、实时运行和数据播发流程，不参与核心算法计算。

主要工具如下：

版本号可在本机通过以下命令查看：

cl
python --version
py -0p

2. 程序包文件结构

提交程序包主要包含 config、src、script、build-nmake 和 README.md。其中 src 为 C++ 源程序，script 为辅助脚本，config 为示例配置文件，build-nmake 中保存可执行程序。

IntegratedNav/
├─ config/
│  └─ dataset_lc_template.ini
│
├─ src/
│  ├─ main.cpp
│  ├─ Config.cpp / Config.h
│  ├─ Struct.h
│  ├─ const.h
│  ├─ Matrix.cpp / Matrix.h
│  ├─ Coordinate.cpp / Coordinate.h
│  ├─ INS.cpp / INS.h
│  ├─ GNSS.cpp / GNSS.h
│  ├─ LooseCouple.cpp / LooseCouple.h
│  └─ RealtimeLC.cpp / RealtimeLC.h
│
├─ script/
│  ├─ build_cl.ps1
│  ├─ run_lc.ps1
│  ├─ run_realtime_lc.ps1
│  ├─ run_realtime_view.ps1
│  ├─ run_udp_replay.ps1
│  ├─ run_udp_live_plot.ps1
│  ├─ plot_lc.py
│  ├─ plot_report_figures.py
│  ├─ plot_realtime_error.py
│  ├─ compare_ref.py
│  ├─ lc_error_stats.py
│  ├─ plot_rtk.py
│  ├─ plot_pure_ins.py
│  ├─ udp_replay.py
│  └─ udp_live_plot.py
│
├─ build-nmake/
│  └─ ins_init.exe
│
└─ README.md

主要 C++ 源文件功能如下：

辅助脚本功能如下：

此处可在报告中插入源文件列表截图。

3. 编译成功信息

编译命令为：

powershell -NoProfile -ExecutionPolicy Bypass -File .\script\build_cl.ps1

编译成功后生成：

build-nmake\ins_init.exe

编译成功时终端会显示类似信息：

Visual Studio Developer Command Prompt
[vcvarsall.bat] Environment initialized for: 'x64'
main.cpp
Config.cpp
Matrix.cpp
Coordinate.cpp
INS.cpp
GNSS.cpp
LooseCouple.cpp
RealtimeLC.cpp
正在生成代码...

此处可在报告中插入编译成功截图。

4. 自编可执行程序使用说明

自编可执行程序文件名为：

build-nmake\ins_init.exe

该程序需要配置文件。示例配置文件为：

config\dataset_lc_template.ini

4.1 离线松组合运行

直接运行 C++ 可执行程序：

.\build-nmake\ins_init.exe config\dataset_lc_template.ini

也可以使用封装脚本一键运行：

powershell -NoProfile -ExecutionPolicy Bypass -File .\script\run_lc.ps1

若已经编译完成，可跳过编译：

powershell -NoProfile -ExecutionPolicy Bypass -File .\script\run_lc.ps1 -SkipBuild

离线运行主要输出：

output\pure_ins.txt
output\loose_coupling.txt
output\loose_coupling_forward.txt
output\loose_coupling_backward.txt
output\lc_diagnostics.txt
output\lc_smooth_diagnostics.txt
output\lc_imu_error.txt
output\lc_vs_ref_summary.txt
output\lc_vs_ref_timeseries.txt
output\lc_vs_ref_trajectory.png
output\lc_vs_ref_error.png
output\lc_trajectory_all.png
output\lc_error.png
output\lc_states.png

其中 output\loose_coupling.txt 为松组合主结果文件。

4.2 UDP 伪实时松组合运行

实时松组合可执行程序运行方式为：

.\build-nmake\ins_init.exe config\dataset_lc_template.ini udp

推荐使用两个终端运行。

终端 1：启动实时松组合和实时轨迹显示：

powershell -NoProfile -ExecutionPolicy Bypass -File .\script\run_realtime_view.ps1 -SkipBuild

等待输出：

Realtime LC UDP started.

终端 2：启动 IMU/GNSS 伪实时播发：

powershell -NoProfile -ExecutionPolicy Bypass -File .\script\run_udp_replay.ps1 -Speed 1

-Speed 1 表示按原始采样时间播发，IMU 约 200 Hz，GNSS 约 1 Hz。

实时运行主要输出：

output\realtime_lc.txt
output\realtime_lc_imu_error.txt

5. 示例配置文件说明

程序运行需要配置文件。提交程序包中提供示例配置文件：

config\dataset_lc_template.ini

配置文件主要包括数据路径、时间范围、初始状态、坐标系、杆臂、IMU 参数、GNSS 参数、滤波参数、约束开关、UDP 端口和参考结果路径。

配置文件节选如下：

[files]
gnss_file = data/gnss_data/gnss_20260602_100202_517487.pos
imu_file = data/imu_data/imu.txt

[time]
time_system = GPST
gps_week = 2421
start_sow = 180441.418130
end_sow = 182331.00

[initial_state]
init_sow = 180441.418130
velocity_source = zero
ecef_x_m = -2267689.2389
ecef_y_m = 5009392.1939
ecef_z_m = 3220979.2851
vn_mps = 0.0
ve_mps = 0.0
vd_mps = 0.0
manual_align = true
roll_deg = 0.0
pitch_deg = 0.0
yaw_deg = 0.0

[lever_arm]
frame = IMU_RFU_MEASURED
x_m = -0.0005
y_m = 0.14
z_m = -0.005
to_body_frd = y,x,-z

[lc_filter]
use_lever_arm_observation = true
use_forward_backward = true

[lc_constraint]
use_height_constraint = true
use_vertical_velocity_constraint = true
use_lateral_velocity_constraint = true

[reference]
ref_pos_file = data/ref/RTK_LC.pos
compare_attitude = false

常用配置说明：

• [files]：IMU 和 GNSS 文件路径。
• [time]：解算起止 GPS 周内秒。
• [initial_state]：初始位置、速度、姿态和静止对准时间段。
• [lever_arm]：杆臂，定义为 IMU 中心指向 GNSS 天线相位中心。
• [lc_filter]：滤波初值、过程噪声、反馈开关和前后向开关。
• [lc_constraint]：高程约束、垂向速度约束和 NHC 侧向速度约束开关。
• [udp_replay]：UDP 播发端口和播发速度。
• [realtime_lc]：实时松组合输出文件和超时设置。
• [reference]：参考结果路径，当前使用 data/ref/RTK_LC.pos。

若只进行普通 INS/GNSS 松组合，不使用约束和前后向处理，可设置：

[lc_filter]
use_forward_backward = false

[lc_constraint]
use_height_constraint = false
use_vertical_velocity_constraint = false
use_lateral_velocity_constraint = false

实时 UDP 模式始终只做前向滤波，不进行后向平滑。

6. 其他辅助程序使用说明

生成实验报告用图片：

py -3 .\script\plot_report_figures.py

输出包括：

output\report_01_trajectory_compare.png/.svg
output\report_02_position_error_ned.png/.svg
output\report_03_velocity_ned.png/.svg
output\report_03b_velocity_error_ned.png/.svg
output\report_04_attitude_rpy.png/.svg
output\report_05_gyro_bias.png/.svg
output\report_06_accel_bias.png/.svg
output\report_07_filter_std.png/.svg
output\report_08_smooth_filter_std.png/.svg

离线结果与参考结果对比：

py -3 .\script\compare_ref.py

伪实时结果误差绘图和 RMS 统计：

py -3 .\script\plot_realtime_error.py

输出包括：

output\realtime_02_position_error_ned.png/.svg
output\realtime_03_velocity_error_ned.png/.svg
output\realtime_lc_vs_ref_summary.txt
output\realtime_lc_vs_ref_timeseries.txt

单独启动实时轨迹显示：

powershell -NoProfile -ExecutionPolicy Bypass -File .\script\run_udp_live_plot.ps1

若 py -3 不可用，可根据本机 Python 安装情况改用：

python .\script\plot_report_figures.py
python .\script\plot_realtime_error.py

7. 算法流程说明

松组合滤波状态量为 15 维：

位置误差(3) + 速度误差(3) + 姿态误差(3) + 陀螺零偏(3) + 加速度计零偏(3)

离线松组合流程如下：

1. 读取配置文件、IMU 数据和 GNSS/RTK 结果。
2. 根据初始静止段完成初始状态设置和 IMU 零偏估计。
3. 每个 IMU 历元进行惯导机械编排，同时进行滤波器 P 阵时间更新。
4. 若 GNSS 历元位于两个 IMU 历元之间，则先将 IMU 线性插值到 GNSS 时刻，再在该时刻进行 GNSS 更新。
5. GNSS 更新后反馈位置、速度、姿态和 IMU 零偏。
6. 若配置开启约束，则进行高程、垂向速度和 NHC 侧向速度约束更新。
7. 若配置开启前后向滤波，则先保存前向结果，再进行后向滤波，最后对位置和速度进行三通道平滑，姿态保留前向结果。

UDP 伪实时模式与前向离线滤波流程一致，也会在 GNSS 位于两个 IMU 历元之间时进行 IMU 插值，但实时模式不进行后向滤波。