扩展卡尔曼滤波在姿态估计中的应用与实践

千纸鹤Amanda

1. 姿态估计与扩展卡尔曼滤波概述

在三维空间中准确估计物体的姿态（即方向）是一个基础但极具挑战性的问题。无论是无人机在空中保持稳定飞行，还是VR头盔追踪用户头部运动，都需要实时、精确的姿态信息。这个问题看似简单，实则涉及复杂的数学建模和信号处理技术。

姿态通常用欧拉角（俯仰、横滚、偏航）或四元数表示。四元数因其计算效率和避免万向节锁问题而成为首选。但直接从传感器获取的数据往往充满噪声：陀螺仪会随时间累积误差，加速度计对运动敏感，磁力计易受环境磁场干扰。这就是为什么我们需要扩展卡尔曼滤波（EKF）——它能将这些不完美的传感器数据融合，输出更可靠的姿态估计。

关键点：EKF不是简单的数据平均，而是通过概率模型动态调整各传感器的信任权重。当设备静止时更相信加速度计，快速转动时则依赖陀螺仪。

2. 系统建模与状态定义

2.1 状态向量构建

我们定义状态向量为7维：

code复制x = [q0 q1 q2 q3 ω_bias_x ω_bias_y ω_bias_z]'

其中前四个分量是姿态四元数，后三个是陀螺仪的零偏（随时间缓慢变化的误差）。这种设计允许EKF同时估计姿态和校准传感器。

四元数的微分方程由角速度决定：

code复制dq/dt = 0.5 * q ⊗ [0, ω]

其中⊗表示四元数乘法，ω是去除了零偏的角速度测量值。这个非线性关系正是需要EKF而非普通卡尔曼滤波的原因。

2.2 传感器模型

加速度计模型：
静止时加速度计只测量重力方向。将估计的重力方向（[0,0,1]在全局坐标系）转换到载体坐标系后应与测量值一致：

code复制z_acc = R(q)^T * [0; 0; 1] + v_acc

R(q)是四元数对应的旋转矩阵，v_acc是测量噪声。

磁力计模型：
类似地，磁力计测量地磁场方向（假设水平指向北方）：

code复制z_mag = R(q)^T * [m_n; 0; m_d] + v_mag

m_n和m_d是地磁场的北向和垂直分量。

3. EKF实现细节

3.1 预测步骤

状态预测：
使用陀螺仪测量ω_k，更新四元数：
```
code复制q_{k|k-1} = q_{k-1} ⊗ exp(0.5*Δt*(ω_k - ω_bias))
```
零偏建模为随机游走过程（基本保持不变）。
协方差预测：
计算状态转移矩阵F（对f(x)在x_{k-1}处线性化）：
```
code复制F = I + Δt * [∂f/∂q, ∂f/∂bias]
```
过程噪声Q包含陀螺噪声和零偏噪声：
```
code复制Q = diag([σ_gyro^2, σ_bias^2])
```
协方差更新：
```
code复制P_{k|k-1} = F P_{k-1} F^T + Q
```

3.2 更新步骤

当加速度计或磁力计数据到达时：

计算残差：
```
code复制y = z - h(x_{k|k-1})
```
h(x)是前述的传感器模型。
卡尔曼增益：
```
code复制K = P H^T (H P H^T + R)^{-1}
```
H是测量矩阵（h(x)的雅可比），R是传感器噪声协方差。
状态修正：
```
code复制x_k = x_{k|k-1} + K y
```
注意四元数部分需要重新归一化。
协方差更新：
```
code复制P_k = (I - K H) P_{k|k-1}
```

4. MATLAB实现关键代码解析

matlab复制% 初始化
e0 = 1; e1 = 0; e2 = 0; e3 = 0;  % 单位四元数
pb = 0; qb = 0; rb = 0;          % 陀螺零偏
P = diag([ones(1,4)*0.01, ones(1,3)*0.001]); % 协方差矩阵

% 噪声参数
Q = diag([[1 1 1 1]*0.00005, [1 1 1]*0.000001].^2);
R_acc = diag([1 1 1]*0.045^2);
R_mag = diag([1 1 1]*0.015^2);

for i = 2:length(time)
    dt = time(i) - time(i-1);
    
    % 预测步骤
    omega = gyro_data(i,:) - [pb qb rb];
    q = quatmultiply(q_prev, expq(0.5*dt*omega));
    F = calc_F(q_prev, omega, dt);  % 计算状态转移矩阵
    P = F*P*F' + Q;
    
    % 更新步骤（加速度计）
    if acc_update
        [H_acc, z_pred] = acc_obs_model(q);
        y = acc_data(i,:) - z_pred';
        S = H_acc*P*H_acc' + R_acc;
        K = P*H_acc'/S;
        x = x + K*y;
        q = x(1:4)/norm(x(1:4));  % 归一化
        P = (eye(7)-K*H_acc)*P;
    end
    
    % 更新步骤（磁力计）类似...
end