Mahony 滤波（Mahony Complementary Filter）分析¶

#define sampleFreq  1000.0f     // 采样频率
#define twoKpDef    (2.0f * 0.5f)   // 2 * 比例增益
#define twoKiDef    (2.0f * 0.0f)   // 2 * 积分增益

volatile float twoKp = twoKpDef;           // 比例增益 (可动态调整)
volatile float twoKi = twoKiDef;           // 积分增益 (可动态调整)
volatile float integralFBx = 0.0f;        // 积分反馈 X
volatile float integralFBy = 0.0f;        // 积分反馈 Y
volatile float integralFBz = 0.0f;        // 积分反馈 Z

float invSqrt(float x) {
    float halfx = 0.5f * x;
    float y = x;
    long i = *(long*)&y;               // 把float的位模式当成long
    i = 0x5f3759df - (i>>1);           // 快速近似
    y = *(float*)&i;                   // 把long的位模式当成float
    y = y * (1.5f - (halfx * y * y));  // 牛顿迭代一次
    return y;
}

y = 1.0f / sqrtf(x);  // 慢，需要硬件支持

void MahonyAHRSupdate(float q[4], float gx, float gy, float gz, 
                     float ax, float ay, float az, 
                     float mx, float my, float mz)

// Use IMU algorithm if magnetometer measurement invalid
if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
    MahonyAHRSupdateIMU(q, gx, gy, gz, ax, ay, az);
    return;
}

// Compute feedback only if accelerometer measurement valid
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
    // ... 计算 ...
}

// Normalise accelerometer measurement
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;

// Auxiliary variables to avoid repeated arithmetic
q0q0 = q[0] * q[0];
q0q1 = q[0] * q[1];
q0q2 = q[0] * q[2];
q0q3 = q[0] * q[3];
q1q1 = q[1] * q[1];
q1q2 = q[1] * q[2];
q1q3 = q[1] * q[3];
q2q2 = q[2] * q[2];
q2q3 = q[2] * q[3];
q3q3 = q[3] * q[3];

// Reference direction of Earth's magnetic field
hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));
bx = sqrt(hx * hx + hy * hy);
bz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2));

// Estimated direction of gravity and magnetic field
halfvx = q1q3 - q0q2;              // 重力向量 X 的一半
halfvy = q0q1 + q2q3;              // 重力向量 Y 的一半
halfvz = q0q0 - 0.5f + q3q3;       // 重力向量 Z 的一半

halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);
halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);

// Error is sum of cross product between estimated and measured direction
halfex = (ay * halfvz - az * halfvy) + (my * halfwz - mz * halfwy);
halfey = (az * halfvx - ax * halfvz) + (mz * halfwx - mx * halfwz);
halfez = (ax * halfvy - ay * halfvx) + (mx * halfwy - my * halfwx);

// Compute and apply integral feedback if enabled
if(twoKi > 0.0f) {
    // 积分项： 积分 ＝ 过去累加 ＋ 误差 * dt （累积过去的误差）
    // 这里的 (1.0f / sampleFreq) 就是 dt
    integralFBx += twoKi * halfex * (1.0f / sampleFreq);
    integralFBy += twoKi * halfey * (1.0f / sampleFreq);
    integralFBz += twoKi * halfez * (1.0f / sampleFreq);

    // 应用 I 项补偿到角速度
    gx += integralFBx;
    gy += integralFBy;
    gz += integralFBz;
}
else {
    // 清零，防止积分饱和
    integralFBx = 0.0f;
    integralFBy = 0.0f;
    integralFBz = 0.0f;
}

// 应用 P 项补偿到角速度
gx += twoKp * halfex;
gy += twoKp * halfey;
gz += twoKp * halfez;

// 1. 先加上 I（积分）项补偿
gx += integralFBx; 
// 2. 再加上 P（比例）项补偿
gx += twoKp * halfex; 

// Integrate rate of change of quaternion
// 代码临时 让修正后的角速度 gx = gx * 0.5 * dt
gx *= (0.5f * (1.0f / sampleFreq));  // 预处理：dt/2
gy *= (0.5f * (1.0f / sampleFreq));
gz *= (0.5f * (1.0f / sampleFreq));

qa = q[0];
qb = q[1];
qc = q[2];

// q0 = q0 + (-q1*gx - q2*gy - q3*gz)
q[0] += (-qb * gx - qc * gy - q[3] * gz);
// q1 = q1 + (q0*gx + q2*gz - q3*gy)
q[1] += (qa * gx + qc * gz - q[3] * gy);
// q2 = q2 + (q0*gy - q1*gz + q3*gx)
q[2] += (qa * gy - qb * gz + q[3] * gx);
// q3 = q3 + (q0*gz + q1*gy - q2*gx)
q[3] += (qa * gz + qb * gy - qc * gx);

// 代码临时令 gx = gx * 0.5 * dt
gx *= (0.5f * (1.0f / sampleFreq)); 
gy *= (0.5f * (1.0f / sampleFreq));
gz *= (0.5f * (1.0f / sampleFreq));

// Normalise quaternion
recipNorm = invSqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
q[0] *= recipNorm;
q[1] *= recipNorm;
q[2] *= recipNorm;
q[3] *= recipNorm;

// IMU版：只用加速度计
halfex = (ay * halfvz - az * halfvy);
halfey = (az * halfvx - ax * halfvz);
halfez = (ax * halfvy - ay * halfvx);

// 完整版：加速度计 + 磁力计
halfex = (ay * halfvz - az * halfvy) + (my * halfwz - mz * halfwy);
halfey = (az * halfvx - ax * halfvz) + (mz * halfwx - mx * halfwz);
halfez = (ax * halfvy - ay * halfvx) + (mx * halfwy - my * halfwx);

title: ## 💡 进阶思考与工程坑点（备忘）
collapse: close

### 1. 源码中为什么到处都是 "half" 和 "two"？（代码小巧思）
* **现象**：源码中计算估计向量和误差时都乘以了 `0.5f`（如 `halfex = ...`），而增益宏定义却是 `twoKpDef (2.0f * 0.5f)`。
* **原理**：在四元数微分方程中，有公式 $\dot{q} = \frac{1}{2} q \otimes \boldsymbol{\omega}$，这里天然自带一个 $\frac{1}{2}$。
* **骚操作**：Mahony 在计算过程中故意让误差保留在“实际误差的一半（half）”，然后把控制增益定义为“理论增益的两倍（twoKp）”。当它们在修正角速度相乘时：


$$
\text{修正量} = \text{twoKp} \times \text{halfex} = (2K_p) \times \left(\frac{1}{2}e_x\right) = K_p \cdot e_x
$$


  **2 和 0.5 完美抵消！** 这直接在 MCU 的最内层循环里省去了好几次浮点数乘法，是早期单片机算力不足时，极其优雅的硬件级代码优化。

### 2. 这里的 Ki（积分项）到底是干嘛用的？
* **核心功能**：**陀螺仪零偏的在线自动追踪（Online Bias Estimation）**。
* **对比**：一阶互补滤波和简易版的 Madgwick 库由于没有积分项，一旦陀螺仪随温度漂移，角度就会越飘越远。而 Mahony 里的 `integralFBx` 会把误差一直累加。只要存在定常的角度偏差，积分项就会变大，反向逼迫并扣除陀螺仪的静态零偏，直到误差清零。
* **工程建议**：静态初始化（校准）时可以把 `Ki` 调大，让零偏快速收敛；正常运行后要把 `Ki` 调得非常小（甚至为 0），防止由于剧烈运动导致积分饱和（Integral Windup），引发姿态振荡。

### 3. 地磁融合的“一损俱损”隐患（磁干扰的致命缺陷）
* **坑点**：源码中把加速度计算出的重力误差 `halfex_acc` 和磁力计算出的地磁误差 `halfex_mag` **直接相加** 成了统一的 `halfex`。
* **隐患**：由于两个误差共享同一个 PI 控制器，一旦机器人靠近电机、铁轨或水下钢结构，磁力计会产生巨大的错误误差。这个错误会直接污染 `halfex`，不仅导致航向角（Yaw）完蛋，还会**强行拉歪原本很准的横滚（Roll）和俯仰（Pitch）**。
* **避坑指南**：在强磁干扰环境下，最稳妥的做法是直接将磁力计代码注释掉（或把磁力计权重设为 0），让 Mahony 退化为标准的 IMU（六轴）姿态解算，把 Roll/Pitch 保住。航向角（Yaw）的漂移交由其他传感器（如双目、激光雷达或里程计）去修正。

此文档为Ai+古法微调 感谢Ai 感谢中国