一、系统概述

       IMU-DVL融合系统是一个基于扩展卡尔曼滤波（EKF）的惯性导航系统，融合IMU（惯性测量单元）和DVL（多普勒测速仪）数据，实现高精度的位置、速度和姿态估计。

1、主要功能

• 姿态解算：基于四元数
• 速度估计：DVL直接测量 + IMU加速度积分
• 位置估计：速度二阶积分
• 零偏标定：静止状态下自动标定IMU零偏
• 数据滤波：中值滤波抑制噪声
• 传感器融合：DVL有效时用DVL，失效时用纯惯导

2、数据频率

• IMU：高频（通常200-250 Hz）
• DVL：低频（通常10 Hz）

二、输入数据

1、IMU标定数据（ID_IMU_CalibrationData）

typedef struct _ID_IMU_CalibrationData
{
    double Acc[9];      // 加速度计标定矩阵（3x3）
    double AccX;        // 加速度X轴偏置
    double AccY;        // 加速度Y轴偏置
    double AccZ;        // 加速度Z轴偏置
    double Gyro[9];     // 陀螺仪标定矩阵（3x3）
    double GyroX;       // 陀螺仪X轴偏置
    double GyroY;       // 陀螺仪Y轴偏置
    double GyroZ;       // 陀螺仪Z轴偏置
} ID_IMU_CalibrationData;

用途： - 校正IMU原始数据的安装误差和零偏 - 在IMU_CalibCoordChange()中应用标定矩阵

2、DVL标定数据（ID_DVL_CalibrationData）

typedef struct _ID_DVL_CalibrationData
{
    double Vel[9];      // 速度标定矩阵（3x3）
    double VelX;        // 速度X轴偏置
    double VelY;        // 速度Y轴偏置
    double VelZ;        // 速度Z轴偏置
} ID_DVL_CalibrationData;

用途： - 校正DVL测量值的安装误差 - 在DVL_CalibCoordChange()中应用标定矩阵

3、IMU测量数据（ID_IMU_Data）

typedef struct _ID_IMU_Data
{
    bool imuData_Valid;     // 数据有效性标志
    long long timestamp;    // 时间戳（毫秒）
    double accX;            // X轴加速度（单位：G）
    double accY;            // Y轴加速度（单位：G）
    double accZ;            // Z轴加速度（单位：G）
    double gyroX;           // X轴角速度（单位：度/秒）
    double gyroY;           // Y轴角速度（单位：度/秒）
    double gyroZ;           // Z轴角速度（单位：度/秒）
    float temperature;      // 温度（℃）
} ID_IMU_Data;

数据特性： - 坐标系：IMU body系（根据安装方向定义） - 单位： - 加速度：G（重力加速度，1G ≈ 9.80665 m/s?） - 角速度：度/秒（需转换为弧度/秒） - 频率：高频（200-250 Hz）

处理流程： 1. 标定校正（IMU_CalibCoordChange） 2. 单位转换（G → m/s?，度/秒 → 弧度/秒） 3. 零偏去除（减去标定零偏） 4. 中值滤波（抑制噪声）

4、DVL测量数据（ID_DVL_Data）

typedef struct _ID_DVL_Data
{
    bool dvlData_Valid;         // 数据有效性标志
    long long timestamp;        // 时间戳（毫秒）
    double velX;                // X轴速度（单位：m/s）
    double velY;                // Y轴速度（单位：m/s）
    double velZ;                // Z轴速度（单位：m/s）
    double velPrecision;        // 速度精度（m/s）
    unsigned char validFlag;    // 测量值有效标志（1=有效，0=无效）
} ID_DVL_Data;

数据特性： - 坐标系：DVL body系（与IMU对齐） - 单位：m/s - 频率：低频（约10 Hz） - 有效性：validFlag == 1表示DVL测量有效

处理流程： 1. 有效性检查（validFlag == 1） 2. 标定校正（DVL_CalibCoordChange） 3. 中值滤波（可选） 4. 坐标转换（body系 → 导航系） 5. EKF更新（修正速度和零偏）

三、输出数据

1、融合输出（ID_Fusion_Out）

typedef struct _ID_Fusion_Out
{
    bool data_Valid;            // 数据有效性标志
    long long timestamp;        // 时间戳（?U）
    ID_Location_Pos position;   // 位置（X, Y, Z，单位：米）
    ID_Location_Euler euler;    // 姿态（Yaw, Pitch, Roll，单位：弧度）
    float disTotal;             // 累计距离（单位：米）
} ID_Fusion_Out;

四、数据处理流程

五、数据流示意图

六、融合策略

七、展示效果

八、总结

1、输入数据汇总

数据类型	频率	坐标系	单位	用途
IMU加速度	250Hz	Body系	G	速度/位置积分
IMU角速度	250Hz	Body系	度/秒	姿态解算
DVL速度	10Hz	Body系	m/s	速度修正
标定矩阵	一次	-	-	误差校正

2、输出数据汇总

数据类型	频率	坐标系	单位	说明
位置	250Hz	导航系	米	3D位置
姿态	250Hz	-	弧度	Roll/Pitch/Yaw
累计距离	250Hz	-	米	3D路径长度

九、常见问题

QQ：187100248.

Q1：为什么DVL失效后位置会漂移？

A：纯惯导模式下，速度误差累积导致位置二次积分漂移。建议： - 定期使用其他传感器（GPS、视觉）修正位置 - 缩短DVL失效时间。

Q2：如何调整EKF噪声参数？

A： 1. 收集静止和运动数据 2. 分析传感器噪声统计特性 3. 调整sigma_accel、sigma_dvl、sigma_accel_bias 4. 观察位置/速度协方差收敛情况。

Q3：姿态角度单位是什么？

A：输出为弧度，如需角度需乘以180/π。