IMU融合定位实战：手把手教你用ESKF搞定无人机状态估计（附Python代码）

当无人机在GPS信号丢失的室内或复杂环境中飞行时，如何仅凭IMU数据实现厘米级定位？误差状态卡尔曼滤波（ESKF）正是解决这一难题的利器。本文将带您从零实现一个完整的ESKF框架，使用EuRoC MAV数据集进行实战验证，并分享工程化过程中的关键技巧。

1. 环境准备与数据加载

在开始编码前，我们需要搭建Python科学计算环境。推荐使用Anaconda创建独立环境：

conda create -n eskf python=3.8
conda activate eskf
pip install numpy scipy matplotlib pyquaternion pandas

EuRoC MAV数据集提供了高质量的IMU和真值数据，我们以 MH_01_easy 序列为例。数据集目录结构通常包含：

├── mav0
│   ├── imu0
│   │   ├── data.csv    # IMU测量值
│   ├── leica0
│   │   ├── data.csv    # 真值轨迹

使用Pandas加载IMU数据的核心代码：

import pandas as pd

def load_imu_data(csv_path):
    cols = ['timestamp','gyro_x','gyro_y','gyro_z','acc_x','acc_y','acc_z']
    df = pd.read_csv(csv_path, header=None, names=cols)
    df['timestamp'] /= 1e9  # 纳秒转秒
    return df.to_numpy()

注意：实际应用中需要校准IMU的bias和scale参数，EuRoC数据已进行过标定

2. ESKF核心算法实现

2.1 状态变量定义

与传统EKF不同，ESKF维护三个状态层次：

状态类型	变量组成	特点
名义状态	位置、速度、姿态(四元数)、bias	不含噪声的理想状态
误差状态	δp, δv, δθ, δbg, δba	小量，服从高斯分布
真实状态	名义状态 ⊕ 误差状态	实际物理系统状态

Python中的状态类实现：

class NominalState:
    def __init__(self):
        self.p = np.zeros(3)  # 位置
        self.v = np.zeros(3)  # 速度
        self.q = Quaternion() # 姿态(四元数)
        self.bg = np.zeros(3) # 陀螺bias
        self.ba = np.zeros(3) # 加速度计bias

class ErrorState:
    def __init__(self):
        self.dp = np.zeros(3)
        self.dv = np.zeros(3) 
        self.dtheta = np.zeros(3)
        self.dbg = np.zeros(3)
        self.dba = np.zeros(3)

2.2 预测步实现

IMU的预测分为名义状态预测和误差状态预测两部分。名义状态通过IMU测量值直接积分：

def predict_nominal(imu_data, dt):
    # 去除bias后的测量值
    gyro = imu_data.gyro - state.bg  
    acc = imu_data.acc - state.ba
    
    # 姿态更新 (四元数积分)
    q_dot = 0.5 * state.q * Quaternion(0, *gyro)
    new_q = state.q + q_dot * dt
    
    # 速度更新
    global_gravity = np.array([0, 0, -9.81])
    global_acc = state.q.rotate(acc) + global_gravity
    new_v = state.v + global_acc * dt
    
    # 位置更新
    new_p = state.p + state.v * dt + 0.5 * global_acc * dt**2
    
    return NominalState(new_p, new_v, new_q, state.bg, state.ba)

误差状态预测需要计算状态转移矩阵F和噪声协方差Q：

def compute_F(dt, gyro, acc, state):
    F = np.eye(15)
    # 填充各状态间的耦合关系
    F[0:3, 3:6] = np.eye(3) * dt
    F[3:6, 6:9] = -skew(state.q.rotate(acc)) * dt
    # ... 其他非零块矩阵
    return F

def predict_error(F, Q, dt):
    # 误差状态协方差预测
    new_P = F @ state.P @ F.T + Q
    return new_P

2.3 更新步实现

当视觉或GPS观测到来时，执行ESKF更新：

def update(z, H, R):
    # 计算卡尔曼增益
    S = H @ state.P @ H.T + R
    K = state.P @ H.T @ np.linalg.inv(S)
    
    # 更新误差状态
    delta_x = K @ (z - predict_measurement())
    state.dp += delta_x[0:3]
    state.dv += delta_x[3:6]
    # ... 其他状态更新
    
    # 更新协方差
    I_KH = np.eye(15) - K @ H
    state.P = I_KH @ state.P @ I_KH.T + K @ R @ K.T

3. 工程实践关键技巧

3.1 数值稳定性处理

ESKF实现中容易遇到的数值问题及解决方案：

• 四元数归一化 ：每次预测后执行 q.normalize()
• 协方差矩阵对称性 ：每次更新后执行 P = (P + P.T) * 0.5
• Cholesky分解失败 ：添加小量对角矩阵 P += 1e-6 * np.eye(15)

3.2 参数调优指南

关键噪声参数对系统性能的影响：

参数	物理意义	调大效果	调小效果
gyro_noise	陀螺测量噪声	更信任观测	更信任IMU
acc_noise	加速度计测量噪声	减小加速度计影响	增大加速度计影响
bg_walk	陀螺bias随机游走	bias变化更灵敏	bias更稳定

典型初始值设置（EuRoC数据集）：

params = {
    'gyro_noise': 0.005,    # rad/s
    'acc_noise': 0.05,      # m/s²
    'bg_walk': 1e-6,        # rad/s²
    'ba_walk': 1e-5,        # m/s³
}

3.3 多传感器时间对齐

实际系统中各传感器时间戳不同步会导致性能下降。解决方法：

1. 硬件同步 ：使用PPS信号触发所有传感器
2. 软件插值 ：对IMU数据进行线性插值匹配视觉时间戳
3. 运动补偿 ：考虑相机曝光期间的IMU运动

时间对齐的插值实现示例：

def interpolate_imu(t_target, imu_before, imu_after):
    alpha = (t_target - imu_before.t) / (imu_after.t - imu_before.t)
    gyro = imu_before.gyro * (1-alpha) + imu_after.gyro * alpha
    acc = imu_before.acc * (1-alpha) + imu_after.acc * alpha
    return IMUData(t_target, gyro, acc)

4. 结果可视化与分析

使用Matplotlib绘制轨迹对比图：

def plot_trajectory(gt, eskf):
    fig = plt.figure(figsize=(10, 8))
    ax = fig.add_subplot(111, projection='3d')
    ax.plot(gt[:,0], gt[:,1], gt[:,2], label='Ground Truth')
    ax.plot(eskf[:,0], eskf[:,1], eskf[:,2], label='ESKF')
    ax.set_xlabel('X [m]'); ax.set_ylabel('Y [m]'); ax.set_zlabel('Z [m]')
    plt.legend(); plt.show()

性能评估指标计算：

def compute_ate(gt, est):
    """计算绝对轨迹误差"""
    alignment = umeyama(est.T, gt.T)
    aligned = (alignment[:3,:3] @ est.T + alignment[:3,3:4]).T
    return np.sqrt(np.mean(np.sum((aligned - gt)**2, axis=1)))

在EuRoC数据集上的典型性能：

序列	ATE (m)	最大误差 (m)	运行频率 (Hz)
MH_01_easy	0.12	0.35	200
MH_03_medium	0.28	0.91	180