IMU 融合算法

卡尔曼滤波基础

卡尔曼滤波（Kalman Filter）是 GNSS/IMU 融合的核心算法，用于在噪声环境中最优估计系统状态。

基本原理

系统模型：
  状态方程：x(k) = F·x(k-1) + B·u(k) + w(k)
  观测方程：z(k) = H·x(k) + v(k)

其中：
  x：状态向量（位置、速度、姿态、IMU 偏差）
  F：状态转移矩阵（IMU 积分模型）
  u：控制输入（IMU 测量值）
  z：观测值（GNSS 位置/速度）
  H：观测矩阵
  w：过程噪声（IMU 噪声）
  v：观测噪声（GNSS 噪声）

卡尔曼滤波两步：
  预测步（IMU 驱动）：
    x̂(k|k-1) = F·x̂(k-1|k-1) + B·u(k)
    P(k|k-1) = F·P(k-1|k-1)·Fᵀ + Q
  
  更新步（GNSS 观测）：
    K = P(k|k-1)·Hᵀ·(H·P(k|k-1)·Hᵀ + R)⁻¹
    x̂(k|k) = x̂(k|k-1) + K·(z(k) - H·x̂(k|k-1))
    P(k|k) = (I - K·H)·P(k|k-1)

状态向量设计

GNSS/IMU 融合状态向量（15维）：
  位置：[px, py, pz]（3维）
  速度：[vx, vy, vz]（3维）
  姿态：[roll, pitch, yaw]（3维，欧拉角或四元数）
  加速度计偏差：[bax, bay, baz]（3维）
  陀螺仪偏差：[bgx, bgy, bgz]（3维）
  
  共 15 个状态量

姿态表示方法

欧拉角

Roll（横滚角）：绕 X 轴旋转
Pitch（俯仰角）：绕 Y 轴旋转
Yaw（偏航角）：绕 Z 轴旋转

优点：直观，易于理解
缺点：存在万向节死锁（Gimbal Lock）
      不适合大角度旋转

四元数

四元数：q = [w, x, y, z]，满足 w² + x² + y² + z² = 1

优点：无万向节死锁，计算效率高
缺点：不直观

四元数更新（陀螺仪积分）：
  q(t+Δt) = q(t) + 0.5 × q(t) ⊗ [0, ωx, ωy, ωz] × Δt
  
  其中 ⊗ 是四元数乘法

简化 DR 实现

二维平面 DR（适合车辆）

#include <math.h>

typedef struct {
    double x;       // 东向位置（m）
    double y;       // 北向位置（m）
    double heading; // 航向角（rad，北偏东为正）
    double speed;   // 速度（m/s）
} VehicleState;

// 车辆运动模型（自行车模型）
void dr_update(VehicleState *state, 
               float gyro_z,    // 偏航角速度（rad/s）
               float odo_speed, // 车速（m/s）
               float dt)        // 时间间隔（s）
{
    // 更新航向角
    state->heading += gyro_z * dt;
    
    // 归一化到 [-π, π]
    while (state->heading > M_PI) state->heading -= 2 * M_PI;
    while (state->heading < -M_PI) state->heading += 2 * M_PI;
    
    // 更新速度
    state->speed = odo_speed;
    
    // 更新位置
    double distance = odo_speed * dt;
    state->x += distance * sin(state->heading);  // 东向
    state->y += distance * cos(state->heading);  // 北向
}

// GNSS 更新（修正 DR 误差）
void gnss_update(VehicleState *state,
                 double gnss_x, double gnss_y,
                 float gnss_accuracy)
{
    // 简单加权融合
    float weight = 1.0f / (gnss_accuracy * gnss_accuracy);
    float dr_weight = 1.0f;  // DR 权重（可根据时间调整）
    
    float total_weight = weight + dr_weight;
    
    state->x = (gnss_x * weight + state->x * dr_weight) / total_weight;
    state->y = (gnss_y * weight + state->y * dr_weight) / total_weight;
}

误差累积分析

陀螺仪积分误差：
  角度误差 = 陀螺仪偏差 × 时间
  
  示例：偏差 0.1°/s，行驶 60s
  角度误差 = 0.1 × 60 = 6°
  
  位置误差（1km 后）：
  横向误差 ≈ 1000 × sin(6°) ≈ 105m（很大！）
  
  → 需要 GNSS 定期修正，或使用高精度陀螺仪

车速脉冲误差：
  距离误差 = 标定误差 × 行驶距离
  
  示例：标定误差 0.1%，行驶 1km
  距离误差 = 1000 × 0.001 = 1m（可接受）

LG69T 融合输出解析

# LG69T 输出的 DR 融合位置（PQTMDR 语句）
$PQTMDR,1,2,31.2304,121.4737,10.5,0.5,1.2,0.8,90.5,0.3*XX

字段解析：
  1：消息版本
  2：定位状态（1=GNSS, 2=DR融合, 3=纯DR）
  31.2304：纬度
  121.4737：经度
  10.5：海拔（m）
  0.5：水平精度（m）
  1.2：速度（m/s）
  0.8：速度精度（m/s）
  90.5：航向角（度）
  0.3：航向精度（度）

实际项目注意事项

IMU 安装要求

1. 刚性安装：
   IMU 必须与车身刚性连接，不能有松动
   松动会导致 IMU 测量值与车辆运动不一致

2. 安装位置：
   尽量靠近车辆重心
   避免安装在发动机附近（振动大）
   避免安装在车门（开关门时冲击）

3. 安装角度：
   记录安装角（Roll/Pitch/Yaw）
   通过标定程序自动估计或手动测量

4. 温度影响：
   IMU 偏差随温度变化
   车规级 IMU 有温度补偿
   消费级 IMU 需要软件温度补偿

常见问题

问题1：隧道内漂移严重
原因：陀螺仪偏差未标定，或标定不准
解决：
  - 使用高精度陀螺仪（偏差 < 0.01°/s）
  - 增加车速脉冲输入
  - 延长 GNSS 标定时间

问题2：出隧道后位置跳变
原因：DR 累积误差 + GNSS 突然修正
解决：
  - 使用卡尔曼滤波平滑过渡
  - 设置 GNSS 更新权重（不要完全信任 GNSS）

问题3：低速时精度差
原因：低速时陀螺仪噪声相对较大
解决：
  - 低速时降低陀螺仪权重
  - 使用车速脉冲作为主要速度来源