这是一个可在 Linux 上直接编译的轻量级 AHRS（姿态航向参考系统）实现。它融合陀螺仪、加速度计和磁力计数据，输出设备的四元数姿态以及 yaw、pitch、roll 欧拉角。
代码由 AHRS.lib 的 ARM 反汇编与调试信息整理而来，保留了原库的公开接口、重力模型和滤波流程。当前版本使用 C 标准 <math.h>，不依赖 CMSIS 或 ARM 静态库。
| 文件 | 用途 |
|---|---|
AHRS.c / AHRS.h |
姿态算法与公开接口 |
AHRS_middleware.c / AHRS_middleware.h |
标准 C 数学函数和平台配置 |
将这三个源文件加入工程，并链接数学库：
gcc -std=c99 -Wall -Wextra -I. \
main.c AHRS.c AHRS_middleware.c -lm -o ahrs_demo-lm 用于链接 sinf、cosf、sqrtf、atan2f 等标准数学函数。
所有三轴数组顺序均为 [x, y, z]，且三个传感器必须映射到同一机体坐标系。
| 输入 | 单位 | 用途 |
|---|---|---|
gyro[3] |
rad/s | 跟踪快速转动 |
accel[3] |
m/s² | 提供重力方向，纠正 roll/pitch 漂移 |
mag[3] |
任意一致单位 | 仅在初始化时确定初始 yaw |
dt |
s | 两次更新之间的真实时间间隔 |
如果陀螺仪输出单位为 °/s，必须先转换：
gyro_rad_s = gyro_deg_s * ANGLE_TO_RAD;#include "AHRS.h"
static fp32 quat[4];
void attitude_init(void)
{
fp32 accel[3] = {ax_mps2, ay_mps2, az_mps2};
fp32 mag[3] = {mx, my, mz};
AHRS_init(quat, accel, mag);
}
void attitude_update(void)
{
fp32 gyro[3] = {gx_rad_s, gy_rad_s, gz_rad_s};
fp32 accel[3] = {ax_mps2, ay_mps2, az_mps2};
fp32 mag[3] = {mx, my, mz};
fp32 yaw, pitch, roll;
if (AHRS_update(quat, 0.001f, gyro, accel, mag))
{
get_angle(quat, &yaw, &pitch, &roll);
yaw *= RAD_TO_ANGLE;
pitch *= RAD_TO_ANGLE;
roll *= RAD_TO_ANGLE;
}
}示例中的 0.001f 对应 1 kHz 更新频率。实际项目应尽量传入由定时器测得的真实 dt，而不是固定估计值。
内部状态是单位四元数：
quat = [w, x, y, z]
每次更新的流程如下：
当前四元数 ──→ 预测重力方向 v
加速度计 ────→ 归一化后的实测方向 a
│
error = a × v
│
陀螺仪 ─────────────→ gyro + Kp × error
│
四元数积分、归一化
陀螺仪负责快速运动，但零偏会积分成漂移；加速度计在设备静止或匀速时主要测得重力，可长期校正横滚和俯仰。叉乘误差 a × v 给出预测重力转向实测重力所需的校正方向。
AHRS_init() 会：
- 从
AHRS_get_latitude()和AHRS_get_height()获取纬度与高度； - 估算当地重力加速度；
- 由加速度计获得初始 roll、pitch；
- 用倾斜补偿后的磁力计得到初始 yaw；
- 将欧拉角转换为四元数。
重力模型为：
g₀ = 9.780327 × [1 + 0.0053024·sin²(φ) - 0.0000058·sin²(2φ)]
g = g₀ / (1 + h / 6378137)²
其中 φ 是纬度（弧度）、h 是高度（米）。默认中间层返回北纬 22°、高度 0 m；如有 GPS、气压计或配置数据，可修改 AHRS_get_latitude() 和 AHRS_get_height()。
加速度计并非始终可靠：加速、急停、撞击和振动都会使其读数偏离真实重力。本实现会根据以下两个量调整比例增益 Kp：
abs(norm(accel) - carrier_gravity)：加速度模长与当地重力的差；abs(gx) + abs(gy) + abs(gz)：总角速度大小。
稳定时 Kp = 0.2。当加速度模长偏差超过 0.1 m/s² 或总角速度超过 0.12 rad/s 时，增益按误差减小；误差过大时该周期的加速度计校正会被关闭。
这意味着：平稳时更多相信加速度计纠正漂移；剧烈运动时更多相信陀螺仪跟踪短时姿态。
- 在线
AHRS_update()不使用磁力计，磁力计仅用于初始化 yaw。 - 因此 yaw 会受陀螺仪零偏影响，长时间运行会逐渐漂移；roll、pitch 通常更稳定。
- 初始化时应让设备尽量静止，并完成陀螺仪、加速度计和磁力计校准。
- 若姿态方向反了，优先检查传感器坐标轴映射、轴交换和正负号，而不是先改滤波参数。
- 传入空指针或非正
dt时，AHRS_update()返回 0。
若需要长期稳定的航向，应在在线更新中增加经过磁干扰检测的磁力计校正，或引入 GPS、视觉等外部航向观测。