EKF与Madgwick滤波器在姿态估计中的对比与应用

1. 项目背景与核心需求

在双轮平衡车、自平衡机器人等两轮平台的控制系统中，实时准确的姿态估计是维持系统稳定的关键。其中，俯仰角（pitch angle）的精确测量直接影响控制算法的响应速度和稳定性。传统方案通常采用互补滤波器结合加速度计和陀螺仪数据，但在动态环境下存在明显漂移和噪声干扰问题。

这个项目的核心在于实现两种高阶滤波算法的对比验证：

• 扩展卡尔曼滤波（EKF）：通过状态空间模型处理非线性系统
• Madgwick滤波器：基于梯度下降法的轻量级姿态解算方案

实际工程中常面临三个典型痛点：

1. 低成本IMU传感器噪声显著
2. 运动加速度干扰重力分量测量
3. 陀螺仪积分漂移随时间累积

2. 传感器系统建模

2.1 IMU数据特性分析

两轮平台通常配备6轴IMU（3轴加速度计+3轴陀螺仪），其测量模型为：

加速度计输出：

code复制a_measured = R*(a_motion + g) + b_a + v_a

其中R为旋转矩阵，a_motion为运动加速度，g为重力向量，b_a为零偏，v_a为白噪声

陀螺仪输出：

code复制ω_measured = ω_true + b_g + v_g  

2.2 俯仰角动力学模型

建立两轮平台的状态空间模型：

code复制x = [θ, θ_dot, b_g]^T
θ_ddot = (mgl*sinθ - τ_friction)/J

其中J为转动惯量，τ_friction为摩擦扭矩

3. EKF实现方案

3.1 状态预测方程

离散化后的状态转移：

code复制θ_k = θ_{k-1} + T_s*(ω_gyro - b_g)
b_g_k = b_g_{k-1} + v_b

协方差预测：

code复制P_k|k-1 = F*P_{k-1}*F^T + Q
F = [1 -T_s 0; 0 1 0; 0 0 1] 

3.2 观测更新设计

加速度计观测模型：

code复制z_acc = [sinθ, 0, -cosθ]^T + v_a

雅可比矩阵计算：

code复制H = ∂h/∂x = [cosθ 0 0; 0 0 0; sinθ 0 0]

关键技巧：调整过程噪声Q和观测噪声R矩阵时，建议初始