项目开源地址https://github.com/whu-qqf/IntegratedNavIntegratedNav 自编程序说明文档本程序包用于 GNSS/INS 松组合导航实验。核心程序采用 C 编写主要实现纯惯导解算、GNSS/INS 松组合滤波、杆臂改正、IMU 误差反馈、可选高程/NHC 约束、离线前后向处理以及 UDP 伪实时松组合解算。辅助程序采用 Python 和 PowerShell 编写用于编译运行封装、误差统计、绘图和伪实时数据播发。1. 编程工具和版本说明本工程主要在 Windows 环境下开发和运行。核心导航解算程序使用 C 编写编译工具为 Microsoft Visual C工程编译标准为 C17。代码编辑和工程管理主要使用 Visual Studio Code。C 程序负责实现惯导机械编排、GNSS/INS 松组合 Kalman 滤波、GNSS 位置速度量测更新、杆臂观测模型、IMU 零偏反馈、约束更新和 UDP 伪实时解算。辅助程序主要使用 Python 和 PowerShell 编写。Python 脚本用于误差统计、结果绘图、报告图片生成、UDP 数据播发和实时轨迹显示绘图主要使用matplotlib。PowerShell 脚本用于封装编译、离线运行、实时运行和数据播发流程不参与核心算法计算。主要工具如下工具用途Windows程序运行平台Visual Studio Code代码编辑和工程管理Microsoft Visual C编译 C 导航解算程序C17核心算法语言标准Python 3绘图、误差统计、UDP 数据播发matplotlib绘制轨迹图、误差图、状态图PowerShell编译和运行流程封装UDP SocketIMU/GNSS 伪实时播发与接收版本号可在本机通过以下命令查看cl python--version py-0p2. 程序包文件结构提交程序包主要包含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 源文件功能如下文件功能main.cpp程序入口根据参数选择离线解算或 UDP 实时解算Config.cpp/.hINI 配置文件读取Struct.hIMU、GNSS、姿态、位置、滤波状态等数据结构const.h常量和单位转换参数Matrix.cpp/.h矩阵、向量、四元数和方向余弦矩阵运算Coordinate.cpp/.h坐标转换、地球曲率半径和重力模型INS.cpp/.hIMU 读取、初始对准、零偏估计和纯惯导机械编排GNSS.cpp/.hGNSS 结果读取、质量判断和标准差加权LooseCouple.cpp/.h离线 GNSS/INS 松组合滤波、约束更新和前后向处理RealtimeLC.cpp/.hUDP 伪实时 GNSS/INS 松组合解算辅助脚本功能如下文件功能build_cl.ps1调用 MSVC 编译 C 程序run_lc.ps1一键运行离线松组合、统计和绘图plot_report_figures.py生成实验报告用 PNG/SVG 图片compare_ref.py松组合结果与参考结果对比lc_error_stats.py松组合结果与 GNSS/RTK 对比统计plot_realtime_error.py伪实时结果误差统计和绘图udp_replay.py按真实频率 UDP 播发 IMU/GNSS 数据udp_live_plot.py实时接收并绘制 LC 轨迹run_realtime_view.ps1启动实时松组合和实时轨迹显示此处可在报告中插入源文件列表截图。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.ini4.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 HzGNSS 约 1 Hz。实时运行主要输出output\realtime_lc.txt output\realtime_lc_imu_error.txt5. 示例配置文件说明程序运行需要配置文件。提交程序包中提供示例配置文件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.py7. 算法流程说明松组合滤波状态量为 15 维位置误差(3) 速度误差(3) 姿态误差(3) 陀螺零偏(3) 加速度计零偏(3)离线松组合流程如下读取配置文件、IMU 数据和 GNSS/RTK 结果。根据初始静止段完成初始状态设置和 IMU 零偏估计。每个 IMU 历元进行惯导机械编排同时进行滤波器 P 阵时间更新。若 GNSS 历元位于两个 IMU 历元之间则先将 IMU 线性插值到 GNSS 时刻再在该时刻进行 GNSS 更新。GNSS 更新后反馈位置、速度、姿态和 IMU 零偏。若配置开启约束则进行高程、垂向速度和 NHC 侧向速度约束更新。若配置开启前后向滤波则先保存前向结果再进行后向滤波最后对位置和速度进行三通道平滑姿态保留前向结果。UDP 伪实时模式与前向离线滤波流程一致也会在 GNSS 位于两个 IMU 历元之间时进行 IMU 插值但实时模式不进行后向滤波。IntegratedNav 自编程序说明文档本程序包用于 GNSS/INS 松组合导航实验。核心程序采用 C 编写主要实现纯惯导解算、GNSS/INS 松组合滤波、杆臂改正、IMU 误差反馈、可选高程/NHC 约束、离线前后向处理以及 UDP 伪实时松组合解算。辅助程序采用 Python 和 PowerShell 编写用于编译运行封装、误差统计、绘图和伪实时数据播发。1. 编程工具和版本说明本工程主要在 Windows 环境下开发和运行。核心导航解算程序使用 C 编写编译工具为 Microsoft Visual C工程编译标准为 C17。代码编辑和工程管理主要使用 Visual Studio Code。C 程序负责实现惯导机械编排、GNSS/INS 松组合 Kalman 滤波、GNSS 位置速度量测更新、杆臂观测模型、IMU 零偏反馈、约束更新和 UDP 伪实时解算。辅助程序主要使用 Python 和 PowerShell 编写。Python 脚本用于误差统计、结果绘图、报告图片生成、UDP 数据播发和实时轨迹显示绘图主要使用matplotlib。PowerShell 脚本用于封装编译、离线运行、实时运行和数据播发流程不参与核心算法计算。主要工具如下工具用途Windows程序运行平台Visual Studio Code代码编辑和工程管理Microsoft Visual C编译 C 导航解算程序C17核心算法语言标准Python 3绘图、误差统计、UDP 数据播发matplotlib绘制轨迹图、误差图、状态图PowerShell编译和运行流程封装UDP SocketIMU/GNSS 伪实时播发与接收版本号可在本机通过以下命令查看cl python--version py-0p2. 程序包文件结构提交程序包主要包含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 源文件功能如下文件功能main.cpp程序入口根据参数选择离线解算或 UDP 实时解算Config.cpp/.hINI 配置文件读取Struct.hIMU、GNSS、姿态、位置、滤波状态等数据结构const.h常量和单位转换参数Matrix.cpp/.h矩阵、向量、四元数和方向余弦矩阵运算Coordinate.cpp/.h坐标转换、地球曲率半径和重力模型INS.cpp/.hIMU 读取、初始对准、零偏估计和纯惯导机械编排GNSS.cpp/.hGNSS 结果读取、质量判断和标准差加权LooseCouple.cpp/.h离线 GNSS/INS 松组合滤波、约束更新和前后向处理RealtimeLC.cpp/.hUDP 伪实时 GNSS/INS 松组合解算辅助脚本功能如下文件功能build_cl.ps1调用 MSVC 编译 C 程序run_lc.ps1一键运行离线松组合、统计和绘图plot_report_figures.py生成实验报告用 PNG/SVG 图片compare_ref.py松组合结果与参考结果对比lc_error_stats.py松组合结果与 GNSS/RTK 对比统计plot_realtime_error.py伪实时结果误差统计和绘图udp_replay.py按真实频率 UDP 播发 IMU/GNSS 数据udp_live_plot.py实时接收并绘制 LC 轨迹run_realtime_view.ps1启动实时松组合和实时轨迹显示此处可在报告中插入源文件列表截图。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.ini4.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 HzGNSS 约 1 Hz。实时运行主要输出output\realtime_lc.txt output\realtime_lc_imu_error.txt5. 示例配置文件说明程序运行需要配置文件。提交程序包中提供示例配置文件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.py7. 算法流程说明松组合滤波状态量为 15 维位置误差(3) 速度误差(3) 姿态误差(3) 陀螺零偏(3) 加速度计零偏(3)离线松组合流程如下读取配置文件、IMU 数据和 GNSS/RTK 结果。根据初始静止段完成初始状态设置和 IMU 零偏估计。每个 IMU 历元进行惯导机械编排同时进行滤波器 P 阵时间更新。若 GNSS 历元位于两个 IMU 历元之间则先将 IMU 线性插值到 GNSS 时刻再在该时刻进行 GNSS 更新。GNSS 更新后反馈位置、速度、姿态和 IMU 零偏。若配置开启约束则进行高程、垂向速度和 NHC 侧向速度约束更新。若配置开启前后向滤波则先保存前向结果再进行后向滤波最后对位置和速度进行三通道平滑姿态保留前向结果。UDP 伪实时模式与前向离线滤波流程一致也会在 GNSS 位于两个 IMU 历元之间时进行 IMU 插值但实时模式不进行后向滤波。