姿态的数学：旋转、四元数与 IMU 仿真器

先看三个数：同一份 120 秒的 IMU 数据，只用陀螺仪积分，姿态误差最后涨到 41.9 度；只用加速度计加磁力计逐点求解（TRIAD），整段的均方根误差是 5.702 度；Mahony 互补滤波是 4.881 度。这三个数里最有意思的是最后那个：它在 20 到 60 秒的平静段只有 0.758 度，在 90 到 100 秒的磁干扰段是 14.851 度。一个滤波器的好坏，要看它怎么对付”传感器在说谎”的时刻。

这一篇不做卡尔曼滤波。它把后面四篇需要的全部材料备齐：姿态的数学、一个能造出这种谎言的仿真器，和三个不用协方差的基线，卡尔曼滤波器之后要和它们比。

本篇检查点

这一篇结束时你会有：make ch04 通过，data/imu_seed1.csv（12000 个样本的仿真 IMU 日志），out/ch04/ 下 gyro、triad、mahony 三种方法在四种语言下的输出，三张图；C、C++、Rust 里的四元数模块（kl_quat、quat.hpp、quat.rs）、Mahony 滤波器和 TRIAD。四元数恒等式的自检在每个语言的 app 里先于数据处理运行。前置：第 3 篇。需要的数学：向量叉乘、矩阵指数的概念。

约定#

这些约定在这一篇陈述一次，之后原样沿用。

世界系 NED（北东地），机体系 FRD（前右下）。重力加速度 $g = 9.80665$ m/s²，世界系下 $\mathbf g_w = [0,0,g]^T$ 。
四元数用 Hamilton 约定，标量在前： $q = [w,x,y,z]$ 。 $q$ 把机体系向量旋转到世界系： $v_w = q\otimes v_b\otimes q^*$ ，对应的旋转矩阵记作 $C(q) = C_{wb}$ （机体到世界）。
运动学 $\dot q = \tfrac12 q\otimes[0,\omega_b]$ ，离散形式 $q_{k+1} = q_k\otimes\exp(\tfrac12\omega\Delta t)$ （这里的 $\exp$ 在下面定义）。
误差定义（右乘，局部/机体系误差）： $q_{\mathrm{true}} = \hat q\otimes\exp(\tfrac12\delta\theta)$ ，姿态误差向量 $\delta\theta = 2\log(\hat q^{-1}\otimes q_{\mathrm{true}})$ ，规范化为 $w\ge0$ 。
欧拉角用 ZYX 顺序（偏航、俯仰、横滚），只用于显示和报告误差，误差折到 $[-180^\circ,180^\circ)$ 。我会报告三个轴各自的欧拉角 RMSE（度）和总的旋转角误差 $|\delta\theta|$ 的 RMSE（度）。
传感器模型：
- 陀螺： $\omega_m = \omega + b + n_g$ ， $b$ 随机游走。
- 加速度计测比力： $f_m = C(q)^T(a_w-\mathbf g_w) + n_a$ 。
- 磁力计： $m_m = C(q)^Tm_w + n_m$ ， $m_w = [\cos50^\circ,0,\sin50^\circ]^T$ （单位长度，磁倾角 50 度向下，磁偏角 0）。

数学#

旋转矩阵与 SO(3)#

一个把机体系坐标变成世界系坐标的旋转，写成矩阵 $C\in\mathbb R^{3\times3}$ ，满足 $C^TC = I$ 和 $\det C = +1$ 。所有这样的矩阵组成群 SO(3)：两个旋转相乘还是旋转，逆就是转置。它有 9 个数，但只有 3 个自由度，6 个约束把它压到一个三维曲面上。这个”3 个自由度、不能随便加减”的性质，是整个姿态估计比标量滤波难的原因。

欧拉角与万向锁#

最直观的 3 参数表示是欧拉角。ZYX 顺序：先绕 $x$ 转横滚角 $\phi$ ，再绕 $y$ 转俯仰角 $\theta$ ，再绕 $z$ 转偏航角 $\psi$ ，

C = R_z(\psi)R_y(\theta)R_x(\phi) = \begin{bmatrix}c_\psi c_\theta & c_\psi s_\theta s_\phi - s_\psi c_\phi & c_\psi s_\theta c_\phi + s_\psi s_\phi\\ s_\psi c_\theta & s_\psi s_\theta s_\phi + c_\psi c_\phi & s_\psi s_\theta c_\phi - c_\psi s_\phi\\ -s_\theta & c_\theta s_\phi & c_\theta c_\phi\end{bmatrix}

（ $c$ 、 $s$ 是余弦和正弦的缩写。）它的毛病在 $\theta = 90^\circ$ ： $c_\theta = 0$ ， $s_\theta = 1$ ，矩阵化简成

C = \begin{bmatrix}0 & \sin(\phi-\psi) & \cos(\phi-\psi)\\ 0 & \cos(\phi-\psi) & -\sin(\phi-\psi)\\ -1 & 0 & 0\end{bmatrix}

只依赖 $\phi-\psi$ ，而不是它们各自的值：偏航和横滚退化成同一个自由度，丢了一个。这就是万向锁。数值验证： $(\psi,\phi) = (20^\circ,10^\circ)$ 和 $(57^\circ,47^\circ)$ （差值都是 $-10^\circ$ ）在 $\theta=90^\circ$ 给出的矩阵，最大元素差是 $8.33\times10^{-17}$ ，是同一个旋转。

绕一个接近 y 轴的轴匀速旋转时的欧拉角：俯仰上升到 90 度之后，偏航和横滚在大约 20 度的旋转范围里同时跳变 170 度，而旋转本身是光滑的

在奇异点附近，一个很小的旋转会让偏航和横滚剧烈变化。我这份仿真数据里，真值的俯仰角在 $[-77.3^\circ,80.9^\circ]$ 之间摆动，离奇异点不到 10 度，这意味着后面报告的偏航、横滚的欧拉角 RMSE 会被放大。总旋转角误差 $|\delta\theta|$ 没有这个毛病，所以我把它当作主要指标。结论：欧拉角只用来显示，滤波器内部不用。

轴角、Rodrigues 与指数映射#

另一个 3 参数表示是旋转向量 $v = \theta n$ ：绕单位轴 $n$ 转角度 $\theta$ 。Rodrigues 公式把它变成矩阵：

C = I + \sin\theta\,[n]_\times + (1-\cos\theta)\,[n]_\times^2,\qquad [n]_\times = \begin{bmatrix}0&-n_z&n_y\\n_z&0&-n_x\\-n_y&n_x&0\end{bmatrix}

$[a]_\times b = a\times b$ 。推导：把向量 $x$ 分解成沿轴的分量 $(n\cdot x)n$ 和垂直分量 $x_\perp$ ；沿轴的不变，垂直分量在垂直平面内转 $\theta$ ： $x_\perp\cos\theta + (n\times x)\sin\theta$ 。合起来 $Cx = (n\cdot x)n + x_\perp\cos\theta + (n\times x)\sin\theta$ ，用 $x_\perp = x - (n\cdot x)n$ 和 $[n]_\times^2x = n\times(n\times x) = (n\cdot x)n - x$ 整理即得。

这个 $v\mapsto C$ 就是 SO(3) 上的指数映射 $\exp([v]_\times)$ ，反方向是对数映射。它是后面所有章节的基本工具：小的旋转，用 3 维向量表示，可以随便加、随便做线性运算；大的旋转，用指数映射接到群上。

四元数#

四元数是 4 维的 $q = [w,u]$ ， $u\in\mathbb R^3$ 。乘法（Hamilton 积）：

a\otimes b = \big[a_wb_w - a_u\cdot b_u,\ \ a_wb_u + b_wa_u + a_u\times b_u\big]

共轭 $q^* = [w,-u]$ ，范数 $|q|^2 = w^2+u\cdot u$ ，单位四元数 $|q|=1$ 的逆等于共轭。单位四元数 $q = [\cos\tfrac\theta2,\ n\sin\tfrac\theta2]$ 表示绕 $n$ 转 $\theta$ 。这里有一个双重覆盖： $q$ 与 $-q$ 是同一个旋转（ $\theta$ 与 $\theta+2\pi$ 给出相反符号）。我的约定是误差四元数规范化到 $w\ge0$ 。

四元数旋转向量。 把 $v$ 嵌成 $[0,v]$ ，计算 $q\otimes[0,v]\otimes q^*$ 。先算 $p = q\otimes[0,v] = [-u\cdot v,\ wv+u\times v]$ ，再乘 $q^* = [w,-u]$ ，向量部分是

p_0(-u) + w\,p_v + p_v\times(-u) = (u\cdot v)u + w^2v + w\,u\times v - w\,v\times u - (u\times v)\times u

$-w\,v\times u = w\,u\times v$ ； $(u\times v)\times u = (u\cdot u)v - (u\cdot v)u$ （三重积展开）。代入得

C(q)v = (w^2-u\cdot u)\,v + 2(u\cdot v)\,u + 2w\,(u\times v)

这个式子很重要：它的第一种写法是 $q\otimes v\otimes q^*$ （便于推导），第二种写法是矩阵 $C(q)$ （便于实现），对应代码里的 qrot。第 5 篇求雅可比时会用到它。

指数映射与对数映射。 旋转向量 $v$ 对应的四元数是

\exp(v) = \big[\cos\tfrac{|v|}2,\ \ \tfrac{v}{|v|}\sin\tfrac{|v|}2\big]

（我写 $\exp(v)$ 指的是旋转向量 $v$ 对应的四元数，在级数记号里它是 $\exp(\tfrac12[0,v])$ 。） $|v|\to0$ 时 $\sin(|v|/2)/|v|\to\tfrac12$ ， $\cos\to1-|v|^2/8$ ，代码里 $|v|<10^{-8}$ 就用这个一阶展开，免得除以零。反方向： $\log q = 2\operatorname{atan2}(|u|,w)\,u/|u|$ （先把 $q$ 规范化到 $w\ge0$ ）。用 $\operatorname{atan2}$ 而不是 $\arccos$ ，在小角度时数值上稳得多。

运动学。 设机体以角速度 $\omega_b$ （机体系表示）转动。经过一小段 $dt$ ，机体系相对于上一时刻的机体系，转了一个小旋转 $\omega_bdt$ ，所以

q(t+dt) = q(t)\otimes\exp(\omega_bdt)\approx q(t)\otimes\big[1,\tfrac12\omega_bdt\big]

（右乘，因为 $\omega_b$ 是在机体系里表示的。）展开一阶项， $\dot q = \tfrac12q\otimes[0,\omega_b]$ 。如果 $\omega_b$ 在整个 $\Delta t$ 内恒定，那么 $q_{k+1} = q_k\otimes\exp(\omega\Delta t)$ 是精确的，不是近似：这正是离散模型。这条精确式的好处是不需要小步长假设，也永远保持单位范数（至多舍入误差，代码里再归一化一次）。

小角度近似。 $\exp(\delta\theta)\approx[1,\tfrac12\delta\theta]$ ，误差是 $O(|\delta\theta|^2)$ 。第 6 篇的误差状态滤波器整个建立在它上面。

斜对称矩阵。 $[v]_\times$ 把叉乘变成矩阵乘法，上面已经定义。后面经常用到性质： $[a]_\times b = -[b]_\times a$ 。

三个传感器测的是什么#

陀螺仪测机体系下的角速度，加上零偏和噪声。零偏是缓慢变化的，不能被忽略：我的仿真里初始零偏是 $[0.02,-0.01,0.015]$ rad/s，不处理它，120 秒会积出 $0.02\times120 = 2.4$ rad 的误差。
加速度计测的不是重力加速度，而是比力（specific force）： $f_m = C^T(a_w-\mathbf g_w)+n_a$ 。静止时 $a_w=0$ ， $f = -C^T\mathbf g_w$ ，机体水平时是 $[0,0,-g]$ ：加速度计”感觉到的”是地面向上的支持力，在 FRD 系下 $z$ 轴向下，所以读数是负的。线性加速度会直接污染它，这是它的致命弱点。
磁力计测地磁场在机体系下的方向。我们已知世界系下的磁场 $m_w$ 。

为什么加速度计只给横滚和俯仰，磁力计给偏航#

加速度计提供一个世界系已知方向（竖直方向）在机体系下的观测。一个方向观测只约束两个自由度：把竖直轴对齐之后，绕竖直轴转任意角度，观测都不变，那就是偏航。所以加速度计能得到横滚和俯仰，得不到偏航。磁力计提供第二个方向（磁场方向），它和重力方向不平行（有 50 度磁倾角），两个不平行的方向合起来约束全部 3 个自由度。

TRIAD 就是用这一点的最小方案：用两个方向观测直接构造出姿态。机体系观测 $b_1$ （比力的方向）、 $b_2$ （磁场方向），世界系参考 $r_1 = [0,0,-1]$ 、 $r_2 = m_w$ 。分别构造正交标架：

t_1 = v_1,\quad t_2 = \frac{v_1\times v_2}{|v_1\times v_2|},\quad t_3 = t_1\times t_2

$C_{wb} = [t_1^r\ t_2^r\ t_3^r]\,[t_1^b\ t_2^b\ t_3^b]^T$ ，把机体系标架旋转到世界系标架。第一个向量（这里是比力方向）被精确对齐，第二个向量只有垂直于第一个向量的部分被使用，这意味着 TRIAD 把全部可信度给了加速度计，磁力计只用来定偏航。它没有滤波，每个样本独立计算，噪声和扰动直接传进来。

仿真器#

仿真器只在 Python 里（python/kl/imu_sim.py）：C、C++、Rust 的任务是处理已经记录好的数据，这也正是硬件上的工作流。

真值。 机体系角速度

\omega_b(t) = \big[0.8\sin(2\pi\cdot0.20\,t),\ 0.6\sin(2\pi\cdot0.13\,t+1.0),\ 1.0\sin(2\pi\cdot0.07\,t+2.0)\big]\ \text{rad/s}

以 1 kHz 用精确四元数指数积分（每步用区间中点的角速度），初始姿态是单位四元数，每 10 步取一个样本，得到 100 Hz 的 IMU 数据，120 秒共 12000 个样本。三个轴用互不通约的频率，是为了让姿态在 120 秒内不重复。

陀螺。 陀螺读数是区间平均角速度 $\operatorname{rotvec}(q_{k-1}^{-1}\otimes q_k)/\Delta t$ 加零偏加噪声。用区间平均而不是采样时刻的瞬时值，是因为第 3 篇那个教训：离散滤波模型 $q_k = q_{k-1}\otimes\exp(\omega_m\Delta t)$ 隐含的就是平均角速度，瞬时值会在滤波器和仿真器之间留下一个系统性的模型差。

噪声与零偏。 陀螺每样本噪声 $\sigma_g = 0.005$ rad/s，零偏初值 $[0.02,-0.01,0.015]$ rad/s，随机游走 $\sigma_{bw} = 10^{-4}$ rad/s/ $\sqrt{\text{s}}$ （每样本标准差 $\sigma_{bw}\sqrt{\Delta t}$ ）。加速度计 $\sigma_a = 0.05$ m/s²，磁力计 $\sigma_m = 0.01$ （单位场）。这些是典型消费级 MEMS 的量级，不是某个具体型号的数据手册。

两段扰动。

线性加速度： $t\in[60,70)$ 秒内加上世界系下 $a_w = [2\sin(2\pi\cdot0.5\,t),0,0]$ m/s²，CSV 里 acc_dist = 1。
磁干扰： $t\in[90,100)$ 秒内世界系磁场被加上 $[0.3,0.3,0]$ ，CSV 里 mag_dist = 1。

CSV 的列是 t,gx,gy,gz,ax,ay,az,mx,my,mz,qw,qx,qy,qz,bx,by,bz,acc_dist,mag_dist。滤波器不允许读后面的 9 列（真值四元数、真零偏和扰动标志），它们只在评估时用。种子 1 用于单次运行，之后的蒙特卡洛用种子 1 到 50。

Mahony 互补滤波#

第 1 篇留了一个观察：常增益的一维卡尔曼滤波，就是对”陀螺推进后的预测”做一阶低通，加速度计提供低频、陀螺提供高频。Mahony 滤波器是它在 SO(3) 上的非线性版本。

设当前估计 $\hat q$ ，它预测的世界系方向在机体系下是 $\hat v_a = C(\hat q)^T[0,0,-1]^T$ （竖直）和 $\hat v_m = C(\hat q)^Tm_w$ （磁场）。实际测到的单位向量 $a_n = f_m/|f_m|$ ， $m_n = m_m/|m_m|$ 。我们要一个小的机体系旋转 $\delta$ ，使预测向量对准测量向量。给 $\hat q$ 右乘 $\exp(\delta)$ 之后，新预测是 $C(\hat q\otimes\exp(\delta))^T v_w = \exp(\delta)^T\hat v\approx\hat v - \delta\times\hat v = \hat v+\hat v\times\delta$ 。要它等于测量 $v$ ，解 $\hat v\times\delta = v-\hat v$ ；对单位向量，垂直于 $\hat v$ 的那部分有解 $\delta = v\times\hat v$ （验证： $\hat v\times(v\times\hat v) = v - (\hat v\cdot v)\hat v$ ，正好是 $v$ 垂直于 $\hat v$ 的分量）。两个观测的修正取和：

e = a_n\times\hat v_a + m_n\times\hat v_m

$e$ 就是”当前估计与测量之间的失配”，是个机体系下的小旋转向量。Mahony 把它当作角速度的修正项，并且对它积分来估计零偏：

b\leftarrow b - k_ie\,\Delta t,\qquad \hat q\leftarrow\hat q\otimes\exp\big((\omega_m - b + k_pe)\Delta t\big)

$b$ 的符号是这么确定的：稳态下 $k_pe = -(b_{\mathrm{true}}-\hat b)$ （失配平衡掉零偏误差），所以 $e$ 是负的零偏误差的 $1/k_p$ 倍；要让 $\hat b$ 趋向 $b_{\mathrm{true}}$ ，需要 $\hat b$ 沿 $-e$ 方向增长，即 $\hat b\leftarrow\hat b - k_ie\Delta t$ 。

$k_p$ 控制”多快相信测量”，对应第 1 篇 $K_\infty/\Delta t$ （时间常数的倒数）； $k_i$ 控制零偏估计多快。我取 $k_p = 1.0$ ， $k_i = 0.05$ 作为基线，没有调。

它的局限也很明显： $e$ 只是方向失配的几何修正，没有协方差，不知道哪个传感器此刻更可信，加速度计和磁力计的权重固定相等；磁干扰会通过 $m_n\times\hat v_m$ 直接改变横滚和俯仰，而不仅仅是偏航（下面的数据会看到）。第 5 篇之后的卡尔曼滤波器要解决的就是这几件事。

另外两个基线：纯陀螺积分（初始姿态用第一个样本的 TRIAD，之后只积分陀螺），TRIAD（每个样本独立求解）。

Python 实现#

四元数#

1
"""Quaternion helpers. Hamilton convention, scalar first q = [w, x, y, z].
2

3
q rotates body to world: v_w = q (x) [0, v_b] (x) q*, and C(q) = C_wb.
4
`qexp(v)` is the unit quaternion of the rotation by rotation vector v, i.e. the
5
exp(v/2) of the series notation: [cos(|v|/2), sin(|v|/2) v/|v|].
6
"""
7

8
import numpy as np
9

10
SMALL = 1e-8  # below this angle (rad) use the Taylor expansion
11

12

13
def qmul(a, b):
14
    aw, ax, ay, az = a
15
    bw, bx, by, bz = b
16
    return np.array([
17
        aw * bw - ax * bx - ay * by - az * bz,
18
        aw * bx + ax * bw + ay * bz - az * by,
19
        aw * by - ax * bz + ay * bw + az * bx,
20
        aw * bz + ax * by - ay * bx + az * bw,
21
    ])
22

23

24
def qconj(q):
25
    return np.array([q[0], -q[1], -q[2], -q[3]])
26

27

28
def qnormalize(q):
29
    return q / np.sqrt(q @ q)
30

31

32
def qexp(v):
33
    """Rotation vector -> unit quaternion."""
34
    th = np.sqrt(v @ v)
35
    if th < SMALL:
36
        return np.array([1.0 - th * th / 8.0, *(0.5 * v)])
37
    s = np.sin(0.5 * th) / th
38
    return np.array([np.cos(0.5 * th), *(s * v)])
39

40

41
def qlog(q):
42
    """Unit quaternion -> rotation vector, using the sign with w >= 0 (angle in [0, pi])."""
43
    if q[0] < 0:
44
        q = -q
45
    u = q[1:]
46
    n = np.sqrt(u @ u)
47
    if n < SMALL:
48
        return 2.0 * u / q[0]
49
    return (2.0 * np.arctan2(n, q[0]) / n) * u
50

51

52
def qrot(q):
53
    """Direction cosine matrix C(q) = C_wb (body -> world)."""
54
    w, x, y, z = q
55
    return np.array([
56
        [1 - 2 * (y * y + z * z), 2 * (x * y - w * z), 2 * (x * z + w * y)],
57
        [2 * (x * y + w * z), 1 - 2 * (x * x + z * z), 2 * (y * z - w * x)],
58
        [2 * (x * z - w * y), 2 * (y * z + w * x), 1 - 2 * (x * x + y * y)],
59
    ])
60

61

62
def dcm_to_quat(C):
63
    """C_wb -> unit quaternion with w >= 0 (Shepperd's method)."""
64
    t = C[0, 0] + C[1, 1] + C[2, 2]
65
    if t > 0:
66
        s = 2.0 * np.sqrt(1.0 + t)
67
        q = np.array([0.25 * s, (C[2, 1] - C[1, 2]) / s, (C[0, 2] - C[2, 0]) / s, (C[1, 0] - C[0, 1]) / s])
68
    elif C[0, 0] >= C[1, 1] and C[0, 0] >= C[2, 2]:
69
        s = 2.0 * np.sqrt(1.0 + C[0, 0] - C[1, 1] - C[2, 2])
70
        q = np.array([(C[2, 1] - C[1, 2]) / s, 0.25 * s, (C[0, 1] + C[1, 0]) / s, (C[0, 2] + C[2, 0]) / s])
71
    elif C[1, 1] >= C[2, 2]:
72
        s = 2.0 * np.sqrt(1.0 + C[1, 1] - C[0, 0] - C[2, 2])
73
        q = np.array([(C[0, 2] - C[2, 0]) / s, (C[0, 1] + C[1, 0]) / s, 0.25 * s, (C[1, 2] + C[2, 1]) / s])
74
    else:
75
        s = 2.0 * np.sqrt(1.0 + C[2, 2] - C[0, 0] - C[1, 1])
76
        q = np.array([(C[1, 0] - C[0, 1]) / s, (C[0, 2] + C[2, 0]) / s, (C[1, 2] + C[2, 1]) / s, 0.25 * s])
77
    return -q if q[0] < 0 else q
78

79

80
def skew(v):
81
    return np.array([[0.0, -v[2], v[1]], [v[2], 0.0, -v[0]], [-v[1], v[0], 0.0]])
82

83

84
def euler_zyx(q):
85
    """(yaw, pitch, roll) in radians for C_wb = Rz(yaw) Ry(pitch) Rx(roll)."""
86
    w, x, y, z = q
87
    yaw = np.arctan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z))
88
    pitch = np.arcsin(np.clip(2 * (w * y - z * x), -1.0, 1.0))
89
    roll = np.arctan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y))
90
    return yaw, pitch, roll
91

92

93
def att_error(q_est, q_true):
94
    """Attitude error vector dtheta = 2 log(q_est^-1 (x) q_true), in the estimate's body frame."""
95
    return qlog(qmul(qconj(q_est), q_true))
96

97

98
def wrap_pi(a):
99
    """Wrap an angle to [-pi, pi)."""
100
    return (a + np.pi) % (2 * np.pi) - np.pi

每个函数都对应上面的一条公式。几点实现细节：

qexp 和 qlog 在 SMALL = 1e-8 以下走泰勒展开，这个阈值在四种语言里完全一致，否则小角度那一支的舍入会出现跨语言差异。
qlog 先把 $w<0$ 的四元数取反，保证返回的是 $[0,\pi]$ 内最短的旋转。
dcm_to_quat 是 Shepperd 方法：对矩阵的迹和三个对角元里最大的那个分情况，避免对接近零的数开方后再相除。
euler_zyx 里 np.clip 防止舍入误差把 arcsin 的参数推到 $\pm1$ 之外。
att_error 返回 $\log(\hat q^{-1}\otimes q_{\mathrm{true}})$ ，没有乘 2：qlog 本身已经返回旋转向量（含了那个 2），它就是约定里的 $\delta\theta$ 。

仿真器#

1
"""IMU simulator: truth attitude, gyro, accelerometer, magnetometer (NED world, FRD body)."""
2

3
from pathlib import Path
4

5
import numpy as np
6

7
from .io import write_csv
8

9
ROOT = Path(__file__).resolve().parents[2]
10

11
G = 9.80665
12
GRAVITY_W = np.array([0.0, 0.0, G])
13
DIP = np.radians(50.0)
14
MAG_W = np.array([np.cos(DIP), 0.0, np.sin(DIP)])
15

16
GYRO_SIGMA = 0.005  # rad/s per sample
17
BIAS0 = np.array([0.02, -0.01, 0.015])  # rad/s
18
BIAS_RW = 1e-4  # rad/s/sqrt(s)
19
ACC_SIGMA = 0.05  # m/s^2
20
MAG_SIGMA = 0.01  # unit-norm field
21
ACC_WINDOW = (60.0, 70.0)
22
MAG_WINDOW = (90.0, 100.0)
23
MAG_DISTURB = np.array([0.3, 0.3, 0.0])
24

25

26
def rate_b(t):
27
    """True body angular rate (rad/s)."""
28
    return np.array([
29
        0.8 * np.sin(2 * np.pi * 0.20 * t),
30
        0.6 * np.sin(2 * np.pi * 0.13 * t + 1.0),
31
        1.0 * np.sin(2 * np.pi * 0.07 * t + 2.0),
32
    ])
33

34

35
def _qmul(a, b):  # plain-float quaternion product, hot loop
36
    aw, ax, ay, az = a
37
    bw, bx, by, bz = b
38
    return (aw * bw - ax * bx - ay * by - az * bz,
39
            aw * bx + ax * bw + ay * bz - az * by,
40
            aw * by - ax * bz + ay * bw + az * bx,
41
            aw * bz + ax * by - ay * bx + az * bw)
42

43

44
def _qexp(v):
45
    th = (v[0] * v[0] + v[1] * v[1] + v[2] * v[2]) ** 0.5
46
    if th < 1e-8:
47
        return (1.0 - th * th / 8.0, 0.5 * v[0], 0.5 * v[1], 0.5 * v[2])
48
    s = np.sin(0.5 * th) / th
49
    return (np.cos(0.5 * th), s * v[0], s * v[1], s * v[2])
50

51

52
def a_world(t):
53
    """Linear acceleration disturbance (world frame, m/s^2)."""
54
    if ACC_WINDOW[0] <= t < ACC_WINDOW[1]:
55
        return np.array([2.0 * np.sin(2 * np.pi * 0.5 * t), 0.0, 0.0])
56
    return np.zeros(3)
57

58

59
def simulate(seed, duration=120.0, imu_rate=100.0, truth_rate=1000.0, write=True):
60
    from .quat import qconj, qlog, qrot
61

62
    rng = np.random.default_rng(seed)
63
    dt = 1.0 / imu_rate
64
    sub = int(round(truth_rate / imu_rate))
65
    h = 1.0 / truth_rate
66
    n = int(round(duration * imu_rate))
67

68
    # truth attitude at the IMU sample instants, integrated at truth_rate
69
    # (exact quaternion exponential of the mid-interval rate); one extra step
70
    # before t = 0 gives the first sample its average rate as well.
71
    q = (1.0, 0.0, 0.0, 0.0)
72
    qs = np.empty((n, 4))
73
    for k in range(n):
74
        qs[k] = q
75
        for j in range(sub):
76
            t_mid = k * dt + (j + 0.5) * h
77
            q = _qmul(q, _qexp(rate_b(t_mid) * h))
78
        nq = (q[0] ** 2 + q[1] ** 2 + q[2] ** 2 + q[3] ** 2) ** 0.5
79
        q = tuple(c / nq for c in q)
80

81
    # gyro reports the rate averaged over the sample interval: rotvec(q_{k-1}^-1 q_k) / dt.
82
    # For k = 0 there is no previous sample, so use the instantaneous rate.
83
    avg = np.empty((n, 3))
84
    avg[0] = rate_b(0.0)
85
    for k in range(1, n):
86
        avg[k] = qlog(_qmul_np(qconj(qs[k - 1]), qs[k])) / dt
87

88
    bias = np.empty((n, 3))
89
    bias[0] = BIAS0
90
    bias[1:] = BIAS0 + np.cumsum(BIAS_RW * np.sqrt(dt) * rng.standard_normal((n - 1, 3)), axis=0)
91
    gyro = avg + bias + GYRO_SIGMA * rng.standard_normal((n, 3))
92

93
    t = np.arange(n) * dt
94
    acc = np.empty((n, 3))
95
    mag = np.empty((n, 3))
96
    acc_dist = np.zeros(n)
97
    mag_dist = np.zeros(n)
98
    na = ACC_SIGMA * rng.standard_normal((n, 3))
99
    nm = MAG_SIGMA * rng.standard_normal((n, 3))
100
    for k in range(n):
101
        C = qrot(qs[k])
102
        aw = a_world(t[k])
103
        mw = MAG_W.copy()
104
        if MAG_WINDOW[0] <= t[k] < MAG_WINDOW[1]:
105
            mw = mw + MAG_DISTURB
106
            mag_dist[k] = 1.0
107
        if ACC_WINDOW[0] <= t[k] < ACC_WINDOW[1]:
108
            acc_dist[k] = 1.0
109
        acc[k] = C.T @ (aw - GRAVITY_W) + na[k]
110
        mag[k] = C.T @ mw + nm[k]
111

112
    cols = {
113
        "t": t,
114
        "gx": gyro[:, 0], "gy": gyro[:, 1], "gz": gyro[:, 2],
115
        "ax": acc[:, 0], "ay": acc[:, 1], "az": acc[:, 2],
116
        "mx": mag[:, 0], "my": mag[:, 1], "mz": mag[:, 2],
117
        "qw": qs[:, 0], "qx": qs[:, 1], "qy": qs[:, 2], "qz": qs[:, 3],
118
        "bx": bias[:, 0], "by": bias[:, 1], "bz": bias[:, 2],
119
        "acc_dist": acc_dist, "mag_dist": mag_dist,
120
    }
121
    if write:
122
        write_csv(ROOT / "data" / f"imu_seed{seed}.csv", cols)
123
    return cols
124

125

126
def _qmul_np(a, b):
127
    from .quat import qmul
128

129
    return qmul(a, b)

热点循环（每个样本里 10 步指数积分）用普通元组实现 _qmul、_qexp，不走 numpy 小数组，120 秒的数据 4 秒内生成完。simulate 的结构：先积分真值，再由真值算出区间平均角速度，再叠加零偏和噪声，最后由真值姿态算加速度计和磁力计读数。

Mahony 与基线#

1
"""Mahony explicit complementary filter with an integral bias term."""
2

3
import numpy as np
4

5
from .imu_sim import MAG_W
6
from .quat import qexp, qmul, qnormalize, qrot
7

8

9
class Mahony:
10
    """q <- q (x) exp((w_m - b + kp e) dt), b <- b - ki e dt,
11
    e = a_meas x a_pred + m_meas x m_pred (unit vectors in the body frame)."""
12

13
    def __init__(self, kp, ki, q0, m_w=MAG_W):
14
        self.kp, self.ki = kp, ki
15
        self.q = np.asarray(q0, dtype=float).copy()
16
        self.b = np.zeros(3)
17
        self.m_w = m_w
18

19
    def step(self, gyro, accel, mag, dt):
20
        C = qrot(self.q)
21
        a_pred = C.T @ np.array([0.0, 0.0, -1.0])
22
        m_pred = C.T @ self.m_w
23
        a_n = accel / np.sqrt(accel @ accel)
24
        m_n = mag / np.sqrt(mag @ mag)
25
        e = np.cross(a_n, a_pred) + np.cross(m_n, m_pred)
26
        self.b = self.b - self.ki * e * dt
27
        w = gyro - self.b + self.kp * e
28
        self.q = qnormalize(qmul(self.q, qexp(w * dt)))
29
        return self.q

1
"""Attitude baselines: TRIAD snapshot and gyro-only integration."""
2

3
import numpy as np
4

5
from .quat import dcm_to_quat, qexp, qmul, qnormalize
6

7

8
def triad(accel, mag, m_w):
9
    """Attitude q (body -> world) from one accelerometer + magnetometer snapshot.
10

11
    Specific force at rest points opposite to gravity, [0, 0, -1] in the NED world frame.
12
    """
13
    r1 = np.array([0.0, 0.0, -1.0])
14
    r2 = m_w
15
    b1 = accel / np.sqrt(accel @ accel)
16
    b2 = mag / np.sqrt(mag @ mag)
17

18
    def frame(v1, v2):
19
        t2 = np.cross(v1, v2)
20
        t2 = t2 / np.sqrt(t2 @ t2)
21
        return np.column_stack([v1, t2, np.cross(v1, t2)])
22

23
    C = frame(r1, r2) @ frame(b1, b2).T  # world <- body
24
    return dcm_to_quat(C)
25

26

27
def gyro_only(gyro, dt, q0):
28
    """Integrate raw gyro samples 1..n-1 starting from q0 at sample 0."""
29
    n = len(gyro)
30
    q = np.asarray(q0, dtype=float).copy()
31
    out = np.empty((n, 4))
32
    out[0] = q
33
    for k in range(1, n):
34
        q = qnormalize(qmul(q, qexp(gyro[k] * dt)))
35
        out[k] = q
36
    return out

Mahony.step 是上面三个公式的逐行翻译。triad 里的 frame 构造标架（三个列向量），C = frame(r1, r2) @ frame(b1, b2).T 是 $C_{wb}$ 。

C、C++、Rust 实现#

四元数是这一章最典型的”同一个东西，三种语言三种惯用法”。

C：四元数是 kl_real[4]，函数把结果写入调用者给的输出数组，输出不与输入别名。没有类型的区别，kl_real a[4] 和 kl_real v[3] 在函数签名里只是文档。
C++： Quat<T> 是带四个成员的结构体，重载 operator*，Quat::exp(Vec3) 是静态方法，q.log() 返回 Vec3。Vec3<T> 是第 2 篇的 Mat<3,1,T>，所以向量运算（+、-、* 标量）直接可用。
Rust： Quat 是 Copy 的结构体，impl Mul；向量用 [Real; 3]，旋转矩阵用 Mat<3, 3>。

四元数乘法和指数/对数，三个版本放在一起看：

C#

1
#include "kl/kl_quat.h"
2

3
#define SMALL ((kl_real)1e-8) /* below this angle (rad) use the Taylor expansion */
4

5
void kl_quat_mul(const kl_real *a, const kl_real *b, kl_real *out)
6
{
7
  out[0] = a[0] * b[0] - a[1] * b[1] - a[2] * b[2] - a[3] * b[3];
8
  out[1] = a[0] * b[1] + a[1] * b[0] + a[2] * b[3] - a[3] * b[2];
9
  out[2] = a[0] * b[2] - a[1] * b[3] + a[2] * b[0] + a[3] * b[1];
10
  out[3] = a[0] * b[3] + a[1] * b[2] - a[2] * b[1] + a[3] * b[0];
11
}
12

13
void kl_quat_conj(const kl_real *a, kl_real *out)
14
{
15
  out[0] = a[0];
16
  out[1] = -a[1];
17
  out[2] = -a[2];
18
  out[3] = -a[3];
19
}
20

21
void kl_quat_normalize(kl_real *q)
22
{
23
  kl_real n = KL_SQRT(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
24
  for (int i = 0; i < 4; i++)
25
    q[i] /= n;
26
}
27

28
void kl_quat_exp(const kl_real v[3], kl_real q[4])
29
{
30
  kl_real th = KL_SQRT(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
31
  if (th < SMALL) {
32
    q[0] = 1 - th * th / 8;
33
    for (int i = 0; i < 3; i++)
34
      q[1 + i] = (kl_real)0.5 * v[i];
35
    return;
36
  }
37
  kl_real s = KL_SIN((kl_real)0.5 * th) / th;
38
  q[0] = KL_COS((kl_real)0.5 * th);
39
  for (int i = 0; i < 3; i++)
40
    q[1 + i] = s * v[i];
41
}
42

43
void kl_quat_log(const kl_real q[4], kl_real v[3])
44
{
45
  kl_real sg = q[0] < 0 ? -1 : 1;
46
  kl_real w = sg * q[0], u[3] = {sg * q[1], sg * q[2], sg * q[3]};
47
  kl_real n = KL_SQRT(u[0] * u[0] + u[1] * u[1] + u[2] * u[2]);
48
  kl_real k = n < SMALL ? 2 / w : 2 * KL_ATAN2(n, w) / n;
49
  for (int i = 0; i < 3; i++)
50
    v[i] = k * u[i];
51
}

kl_quat_mul(a, b, out) 的 out 不能与 a、b 是同一块内存：每个分量用到所有输入。Mahony 一步：

1
    f->m_w[i] = m_w[i];
2
  }
3
}
4

5
void kl_mahony_step(kl_mahony *f, const kl_real gyro[3], const kl_real accel[3],
6
        const kl_real mag[3], kl_real dt)
7
{
8
  kl_real C[9], a_pred[3], m_pred[3], a_n[3], m_n[3], e1[3], e2[3], e[3], w[3], dq[4], qn[4];
9
  kl_quat_to_dcm(f->q, C);
10
  /* a_pred = C^T [0,0,-1], m_pred = C^T m_w */
11
  for (int i = 0; i < 3; i++) {
12
    a_pred[i] = -C[6 + i];
13
    m_pred[i] = C[i] * f->m_w[0] + C[3 + i] * f->m_w[1] + C[6 + i] * f->m_w[2];
14
  }
15
  unit(accel, a_n);
16
  unit(mag, m_n);
17
  cross(a_n, a_pred, e1);
18
  cross(m_n, m_pred, e2);
19
  for (int i = 0; i < 3; i++) {
20
    e[i] = e1[i] + e2[i];
21
    f->b[i] = f->b[i] - f->ki * e[i] * dt;
22
    w[i] = (gyro[i] - f->b[i] + f->kp * e[i]) * dt;
23
  }
24
  kl_quat_exp(w, dq);
25
  kl_quat_mul(f->q, dq, qn);
26
  kl_quat_normalize(qn);
27
  for (int i = 0; i < 4; i++)
28
    f->q[i] = qn[i];
29
}

a_pred = -C[6 + i]： $C^T[0,0,-1]^T$ 就是 $-1$ 乘以 $C$ 的第三行，不需要完整的矩阵向量乘法。

完整源码：c/include/kl/kl_quat.h、c/src/kl_quat.c、c/include/kl/kl_mahony.h、c/src/kl_mahony.c、c/src/kl_baselines.c

1
/* Quaternions, Hamilton convention, scalar first q = [w, x, y, z].
2
 * q rotates body to world: v_w = q (x) [0, v_b] (x) q*, and C(q) = C_wb (row-major 3x3).
3
 * Output buffers never alias inputs unless stated. */
4
#ifndef KL_QUAT_H
5
#define KL_QUAT_H
6

7
#include "kl/kl_real.h"
8

9
/* out = a (x) b */
10
void kl_quat_mul(const kl_real *a, const kl_real *b, kl_real *out);
11
void kl_quat_conj(const kl_real *a, kl_real *out);
12
/* q = q / |q|, in place */
13
void kl_quat_normalize(kl_real *q);
14
/* Rotation vector -> unit quaternion [cos(|v|/2), sin(|v|/2) v/|v|]. */
15
void kl_quat_exp(const kl_real v[3], kl_real q[4]);
16
/* Unit quaternion -> rotation vector with angle in [0, pi] (q and -q give the same v). */
17
void kl_quat_log(const kl_real q[4], kl_real v[3]);
18
/* C(q) = C_wb */
19
void kl_quat_to_dcm(const kl_real q[4], kl_real C[9]);
20
/* C_wb -> unit quaternion with w >= 0 (Shepperd's method) */
21
void kl_dcm_to_quat(const kl_real C[9], kl_real q[4]);
22
/* S v = [v]x w = v x w */
23
void kl_skew(const kl_real v[3], kl_real S[9]);
24
/* C_wb = Rz(yaw) Ry(pitch) Rx(roll) */
25
void kl_quat_to_euler_zyx(const kl_real q[4], kl_real *yaw, kl_real *pitch, kl_real *roll);
26
/* v = log(est^* (x) tru): attitude error in the estimate's body frame */
27
void kl_quat_error(const kl_real est[4], const kl_real tru[4], kl_real v[3]);
28

29
#endif

1
#include "kl/kl_quat.h"
2

3
#define SMALL ((kl_real)1e-8) /* below this angle (rad) use the Taylor expansion */
4

5
void kl_quat_mul(const kl_real *a, const kl_real *b, kl_real *out)
6
{
7
  out[0] = a[0] * b[0] - a[1] * b[1] - a[2] * b[2] - a[3] * b[3];
8
  out[1] = a[0] * b[1] + a[1] * b[0] + a[2] * b[3] - a[3] * b[2];
9
  out[2] = a[0] * b[2] - a[1] * b[3] + a[2] * b[0] + a[3] * b[1];
10
  out[3] = a[0] * b[3] + a[1] * b[2] - a[2] * b[1] + a[3] * b[0];
11
}
12

13
void kl_quat_conj(const kl_real *a, kl_real *out)
14
{
15
  out[0] = a[0];
16
  out[1] = -a[1];
17
  out[2] = -a[2];
18
  out[3] = -a[3];
19
}
20

21
void kl_quat_normalize(kl_real *q)
22
{
23
  kl_real n = KL_SQRT(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
24
  for (int i = 0; i < 4; i++)
25
    q[i] /= n;
26
}
27

28
void kl_quat_exp(const kl_real v[3], kl_real q[4])
29
{
30
  kl_real th = KL_SQRT(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
31
  if (th < SMALL) {
32
    q[0] = 1 - th * th / 8;
33
    for (int i = 0; i < 3; i++)
34
      q[1 + i] = (kl_real)0.5 * v[i];
35
    return;
36
  }
37
  kl_real s = KL_SIN((kl_real)0.5 * th) / th;
38
  q[0] = KL_COS((kl_real)0.5 * th);
39
  for (int i = 0; i < 3; i++)
40
    q[1 + i] = s * v[i];
41
}
42

43
void kl_quat_log(const kl_real q[4], kl_real v[3])
44
{
45
  kl_real sg = q[0] < 0 ? -1 : 1;
46
  kl_real w = sg * q[0], u[3] = {sg * q[1], sg * q[2], sg * q[3]};
47
  kl_real n = KL_SQRT(u[0] * u[0] + u[1] * u[1] + u[2] * u[2]);
48
  kl_real k = n < SMALL ? 2 / w : 2 * KL_ATAN2(n, w) / n;
49
  for (int i = 0; i < 3; i++)
50
    v[i] = k * u[i];
51
}
52

53
void kl_quat_to_dcm(const kl_real q[4], kl_real C[9])
54
{
55
  kl_real w = q[0], x = q[1], y = q[2], z = q[3];
56
  C[0] = 1 - 2 * (y * y + z * z);
57
  C[1] = 2 * (x * y - w * z);
58
  C[2] = 2 * (x * z + w * y);
59
  C[3] = 2 * (x * y + w * z);
60
  C[4] = 1 - 2 * (x * x + z * z);
61
  C[5] = 2 * (y * z - w * x);
62
  C[6] = 2 * (x * z - w * y);
63
  C[7] = 2 * (y * z + w * x);
64
  C[8] = 1 - 2 * (x * x + y * y);
65
}
66

67
void kl_dcm_to_quat(const kl_real C[9], kl_real q[4])
68
{
69
  kl_real t = C[0] + C[4] + C[8], s;
70
  if (t > 0) {
71
    s = 2 * KL_SQRT(1 + t);
72
    q[0] = (kl_real)0.25 * s;
73
    q[1] = (C[7] - C[5]) / s;
74
    q[2] = (C[2] - C[6]) / s;
75
    q[3] = (C[3] - C[1]) / s;
76
  } else if (C[0] >= C[4] && C[0] >= C[8]) {
77
    s = 2 * KL_SQRT(1 + C[0] - C[4] - C[8]);
78
    q[0] = (C[7] - C[5]) / s;
79
    q[1] = (kl_real)0.25 * s;
80
    q[2] = (C[1] + C[3]) / s;
81
    q[3] = (C[2] + C[6]) / s;
82
  } else if (C[4] >= C[8]) {
83
    s = 2 * KL_SQRT(1 + C[4] - C[0] - C[8]);
84
    q[0] = (C[2] - C[6]) / s;
85
    q[1] = (C[1] + C[3]) / s;
86
    q[2] = (kl_real)0.25 * s;
87
    q[3] = (C[5] + C[7]) / s;
88
  } else {
89
    s = 2 * KL_SQRT(1 + C[8] - C[0] - C[4]);
90
    q[0] = (C[3] - C[1]) / s;
91
    q[1] = (C[2] + C[6]) / s;
92
    q[2] = (C[5] + C[7]) / s;
93
    q[3] = (kl_real)0.25 * s;
94
  }
95
  if (q[0] < 0)
96
    for (int i = 0; i < 4; i++)
97
      q[i] = -q[i];
98
}
99

100
void kl_skew(const kl_real v[3], kl_real S[9])
101
{
102
  S[0] = 0;
103
  S[1] = -v[2];
104
  S[2] = v[1];
105
  S[3] = v[2];
106
  S[4] = 0;
107
  S[5] = -v[0];
108
  S[6] = -v[1];
109
  S[7] = v[0];
110
  S[8] = 0;
111
}
112

113
void kl_quat_to_euler_zyx(const kl_real q[4], kl_real *yaw, kl_real *pitch, kl_real *roll)
114
{
115
  kl_real w = q[0], x = q[1], y = q[2], z = q[3];
116
  kl_real sp = 2 * (w * y - z * x);
117
  sp = sp > 1 ? 1 : (sp < -1 ? -1 : sp);
118
  *yaw = KL_ATAN2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z));
119
  *pitch = KL_ASIN(sp);
120
  *roll = KL_ATAN2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y));
121
}
122

123
void kl_quat_error(const kl_real est[4], const kl_real tru[4], kl_real v[3])
124
{
125
  kl_real c[4], d[4];
126
  kl_quat_conj(est, c);
127
  kl_quat_mul(c, tru, d);
128
  kl_quat_log(d, v);
129
}

1
/* Mahony explicit complementary filter with an integral bias term.
2
 * q <- q (x) exp((w_m - b + kp e) dt), b <- b - ki e dt (b is updated first),
3
 * e = a_meas x a_pred + m_meas x m_pred, unit vectors in the body frame.
4
 * Quaternion conventions: see kl_quat.h. */
5
#ifndef KL_MAHONY_H
6
#define KL_MAHONY_H
7

8
#include "kl/kl_real.h"
9

10
typedef struct {
11
  kl_real kp, ki;
12
  kl_real q[4], b[3], m_w[3];
13
} kl_mahony;
14

15
/* m_w: reference magnetic field in the world frame (NED), e.g. [cos 50deg, 0, sin 50deg]. */
16
void kl_mahony_init(kl_mahony *f, kl_real kp, kl_real ki, const kl_real q0[4], const kl_real m_w[3]);
17
void kl_mahony_step(kl_mahony *f, const kl_real gyro[3], const kl_real accel[3],
18
        const kl_real mag[3], kl_real dt);
19

20
#endif

1
#include "kl/kl_mahony.h"
2

3
#include "kl/kl_quat.h"
4

5
static void cross(const kl_real *a, const kl_real *b, kl_real *c)
6
{
7
  c[0] = a[1] * b[2] - a[2] * b[1];
8
  c[1] = a[2] * b[0] - a[0] * b[2];
9
  c[2] = a[0] * b[1] - a[1] * b[0];
10
}
11

12
static void unit(const kl_real *v, kl_real *u)
13
{
14
  kl_real n = KL_SQRT(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
15
  for (int i = 0; i < 3; i++)
16
    u[i] = v[i] / n;
17
}
18

19
void kl_mahony_init(kl_mahony *f, kl_real kp, kl_real ki, const kl_real q0[4], const kl_real m_w[3])
20
{
21
  f->kp = kp;
22
  f->ki = ki;
23
  for (int i = 0; i < 4; i++)
24
    f->q[i] = q0[i];
25
  for (int i = 0; i < 3; i++) {
26
    f->b[i] = 0;
27
    f->m_w[i] = m_w[i];
28
  }
29
}
30

31
void kl_mahony_step(kl_mahony *f, const kl_real gyro[3], const kl_real accel[3],
32
        const kl_real mag[3], kl_real dt)
33
{
34
  kl_real C[9], a_pred[3], m_pred[3], a_n[3], m_n[3], e1[3], e2[3], e[3], w[3], dq[4], qn[4];
35
  kl_quat_to_dcm(f->q, C);
36
  /* a_pred = C^T [0,0,-1], m_pred = C^T m_w */
37
  for (int i = 0; i < 3; i++) {
38
    a_pred[i] = -C[6 + i];
39
    m_pred[i] = C[i] * f->m_w[0] + C[3 + i] * f->m_w[1] + C[6 + i] * f->m_w[2];
40
  }
41
  unit(accel, a_n);
42
  unit(mag, m_n);
43
  cross(a_n, a_pred, e1);
44
  cross(m_n, m_pred, e2);
45
  for (int i = 0; i < 3; i++) {
46
    e[i] = e1[i] + e2[i];
47
    f->b[i] = f->b[i] - f->ki * e[i] * dt;
48
    w[i] = (gyro[i] - f->b[i] + f->kp * e[i]) * dt;
49
  }
50
  kl_quat_exp(w, dq);
51
  kl_quat_mul(f->q, dq, qn);
52
  kl_quat_normalize(qn);
53
  for (int i = 0; i < 4; i++)
54
    f->q[i] = qn[i];
55
}

1
/* Attitude baselines: TRIAD snapshot and gyro-only integration. Conventions: see kl_quat.h. */
2
#ifndef KL_BASELINES_H
3
#define KL_BASELINES_H
4

5
#include "kl/kl_real.h"
6

7
/* Attitude q (body -> world) from one accelerometer + magnetometer snapshot.
8
 * Specific force at rest points opposite to gravity, [0, 0, -1] in the NED world frame. */
9
void kl_triad(const kl_real accel[3], const kl_real mag[3], const kl_real m_w[3], kl_real q[4]);
10
/* q = normalize(q (x) exp(gyro dt)), in place */
11
void kl_gyro_step(kl_real q[4], const kl_real gyro[3], kl_real dt);
12

13
#endif

1
#include "kl/kl_baselines.h"
2

3
#include "kl/kl_quat.h"
4

5
static void cross(const kl_real *a, const kl_real *b, kl_real *c)
6
{
7
  c[0] = a[1] * b[2] - a[2] * b[1];
8
  c[1] = a[2] * b[0] - a[0] * b[2];
9
  c[2] = a[0] * b[1] - a[1] * b[0];
10
}
11

12
static void unit(const kl_real *v, kl_real *u)
13
{
14
  kl_real n = KL_SQRT(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
15
  for (int i = 0; i < 3; i++)
16
    u[i] = v[i] / n;
17
}
18

19
/* Columns [v1, t2, v1 x t2] with t2 = unit(v1 x v2), stored row-major in M. */
20
static void frame(const kl_real *v1, const kl_real *v2, kl_real *M)
21
{
22
  kl_real t2[3], t3[3];
23
  cross(v1, v2, t2);
24
  unit(t2, t2);
25
  cross(v1, t2, t3);
26
  for (int i = 0; i < 3; i++) {
27
    M[i * 3 + 0] = v1[i];
28
    M[i * 3 + 1] = t2[i];
29
    M[i * 3 + 2] = t3[i];
30
  }
31
}
32

33
void kl_triad(const kl_real accel[3], const kl_real mag[3], const kl_real m_w[3], kl_real q[4])
34
{
35
  const kl_real r1[3] = {0, 0, -1};
36
  kl_real b1[3], b2[3], Fr[9], Fb[9], C[9];
37
  unit(accel, b1);
38
  unit(mag, b2);
39
  frame(r1, m_w, Fr);
40
  frame(b1, b2, Fb);
41
  /* C = Fr Fb^T: world <- body */
42
  for (int i = 0; i < 3; i++)
43
    for (int j = 0; j < 3; j++) {
44
      kl_real s = 0;
45
      for (int k = 0; k < 3; k++)
46
        s += Fr[i * 3 + k] * Fb[j * 3 + k];
47
      C[i * 3 + j] = s;
48
    }
49
  kl_dcm_to_quat(C, q);
50
}
51

52
void kl_gyro_step(kl_real q[4], const kl_real gyro[3], kl_real dt)
53
{
54
  kl_real w[3] = {gyro[0] * dt, gyro[1] * dt, gyro[2] * dt}, dq[4], qn[4];
55
  kl_quat_exp(w, dq);
56
  kl_quat_mul(q, dq, qn);
57
  kl_quat_normalize(qn);
58
  for (int i = 0; i < 4; i++)
59
    q[i] = qn[i];
60
}

C++#

1
template <class T = double>
2
struct Quat {
3
  static constexpr T SMALL = T(1e-8);  // below this angle (rad) use the Taylor expansion
4

5
  T w = 1, x = 0, y = 0, z = 0;
6

7
  static constexpr Quat identity() { return Quat{}; }
8

9
  constexpr Quat operator*(const Quat& b) const {
10
    return {w * b.w - x * b.x - y * b.y - z * b.z, w * b.x + x * b.w + y * b.z - z * b.y,
11
      w * b.y - x * b.z + y * b.w + z * b.x, w * b.z + x * b.y - y * b.x + z * b.w};
12
  }
13

14
  constexpr Quat conj() const { return {w, -x, -y, -z}; }
15

16
  Quat normalized() const {
17
    const T n = std::sqrt(w * w + x * x + y * y + z * z);
18
    return {w / n, x / n, y / n, z / n};
19
  }
20

21
  // Rotation vector -> unit quaternion.
22
  static Quat exp(const Vec3<T>& v) {
23
    const T th = std::sqrt(dot(v, v));
24
    if (th < SMALL) return {1 - th * th / 8, v(0, 0) / 2, v(1, 0) / 2, v(2, 0) / 2};
25
    const T s = std::sin(th / 2) / th;
26
    return {std::cos(th / 2), s * v(0, 0), s * v(1, 0), s * v(2, 0)};
27
  }
28

29
  // Unit quaternion -> rotation vector, using the sign with w >= 0 (angle in [0, pi]).
30
  Vec3<T> log() const {
31
    const Quat q = w < 0 ? Quat{-w, -x, -y, -z} : *this;
32
    const Vec3<T> u{{q.x, q.y, q.z}};
33
    const T n = std::sqrt(dot(u, u));
34
    if (n < SMALL) return u * (2 / q.w);
35
    return u * (2 * std::atan2(n, q.w) / n);
36
  }

1
  Mahony(T kp, T ki, const Quat<T>& q0, const Vec3<T>& m_w = default_mag_w<T>())
2
      : kp(kp), ki(ki), q(q0), m_w(m_w) {}
3

4
  const Quat<T>& step(const Vec3<T>& gyro, const Vec3<T>& accel, const Vec3<T>& mag, T dt) {
5
    const Mat<3, 3, T> Ct = q.dcm().transpose();
6
    const Vec3<T> a_pred = Ct * Vec3<T>{{0, 0, -1}};
7
    const Vec3<T> m_pred = Ct * m_w;
8
    const Vec3<T> a_n = accel * (1 / std::sqrt(dot(accel, accel)));
9
    const Vec3<T> m_n = mag * (1 / std::sqrt(dot(mag, mag)));
10
    const Vec3<T> e = cross(a_n, a_pred) + cross(m_n, m_pred);
11
    b = b - e * (ki * dt);  // update the bias before forming the rate
12
    const Vec3<T> w = gyro - b + e * kp;
13
    q = (q * Quat<T>::exp(w * dt)).normalized();
14
    return q;
15
  }
16
};

step 读起来几乎就是公式：cross(a_n, a_pred) + cross(m_n, m_pred) 是 $e$ ，b - e * (ki * dt) 是零偏更新，(q * Quat<T>::exp(w * dt)).normalized() 是姿态更新。算术都是第 2 篇的 Mat<3,1,T> 运算符，没有手写循环。

完整源码：cpp/include/kl/quat.hpp、mahony.hpp、baselines.hpp

1
// Hamilton quaternions, scalar first q = [w, x, y, z].
2
// q rotates body to world: v_w = q (x) [0, v_b] (x) q*, and dcm() is C(q) = C_wb.
3
// exp(v) maps a rotation vector to the unit quaternion [cos(|v|/2), sin(|v|/2) v/|v|].
4
#pragma once
5
#include <cmath>
6

7
#include "kl/mat.hpp"
8

9
namespace kl {
10

11
template <class T = double>
12
using Vec3 = Mat<3, 1, T>;
13

14
template <class T = double>
15
constexpr Vec3<T> cross(const Vec3<T>& a, const Vec3<T>& b) {
16
  return Vec3<T>{{a(1, 0) * b(2, 0) - a(2, 0) * b(1, 0), a(2, 0) * b(0, 0) - a(0, 0) * b(2, 0),
17
      a(0, 0) * b(1, 0) - a(1, 0) * b(0, 0)}};
18
}
19

20
template <class T = double>
21
constexpr T dot(const Vec3<T>& a, const Vec3<T>& b) {
22
  return a(0, 0) * b(0, 0) + a(1, 0) * b(1, 0) + a(2, 0) * b(2, 0);
23
}
24

25
// [v]_x, so that skew(a) * b = a x b
26
template <class T = double>
27
constexpr Mat<3, 3, T> skew(const Vec3<T>& v) {
28
  return Mat<3, 3, T>{{0, -v(2, 0), v(1, 0), v(2, 0), 0, -v(0, 0), -v(1, 0), v(0, 0), 0}};
29
}
30

31
template <class T = double>
32
struct Quat {
33
  static constexpr T SMALL = T(1e-8);  // below this angle (rad) use the Taylor expansion
34

35
  T w = 1, x = 0, y = 0, z = 0;
36

37
  static constexpr Quat identity() { return Quat{}; }
38

39
  constexpr Quat operator*(const Quat& b) const {
40
    return {w * b.w - x * b.x - y * b.y - z * b.z, w * b.x + x * b.w + y * b.z - z * b.y,
41
      w * b.y - x * b.z + y * b.w + z * b.x, w * b.z + x * b.y - y * b.x + z * b.w};
42
  }
43

44
  constexpr Quat conj() const { return {w, -x, -y, -z}; }
45

46
  Quat normalized() const {
47
    const T n = std::sqrt(w * w + x * x + y * y + z * z);
48
    return {w / n, x / n, y / n, z / n};
49
  }
50

51
  // Rotation vector -> unit quaternion.
52
  static Quat exp(const Vec3<T>& v) {
53
    const T th = std::sqrt(dot(v, v));
54
    if (th < SMALL) return {1 - th * th / 8, v(0, 0) / 2, v(1, 0) / 2, v(2, 0) / 2};
55
    const T s = std::sin(th / 2) / th;
56
    return {std::cos(th / 2), s * v(0, 0), s * v(1, 0), s * v(2, 0)};
57
  }
58

59
  // Unit quaternion -> rotation vector, using the sign with w >= 0 (angle in [0, pi]).
60
  Vec3<T> log() const {
61
    const Quat q = w < 0 ? Quat{-w, -x, -y, -z} : *this;
62
    const Vec3<T> u{{q.x, q.y, q.z}};
63
    const T n = std::sqrt(dot(u, u));
64
    if (n < SMALL) return u * (2 / q.w);
65
    return u * (2 * std::atan2(n, q.w) / n);
66
  }
67

68
  // C(q) = C_wb, body to world.
69
  constexpr Mat<3, 3, T> dcm() const {
70
    return Mat<3, 3, T>{{1 - 2 * (y * y + z * z), 2 * (x * y - w * z), 2 * (x * z + w * y),
71
             2 * (x * y + w * z), 1 - 2 * (x * x + z * z), 2 * (y * z - w * x),
72
             2 * (x * z - w * y), 2 * (y * z + w * x), 1 - 2 * (x * x + y * y)}};
73
  }
74

75
  // C_wb -> unit quaternion with w >= 0 (Shepperd's method).
76
  static Quat from_dcm(const Mat<3, 3, T>& C) {
77
    const T t = C(0, 0) + C(1, 1) + C(2, 2);
78
    Quat q;
79
    if (t > 0) {
80
      const T s = 2 * std::sqrt(1 + t);
81
      q = {s / 4, (C(2, 1) - C(1, 2)) / s, (C(0, 2) - C(2, 0)) / s, (C(1, 0) - C(0, 1)) / s};
82
    } else if (C(0, 0) >= C(1, 1) && C(0, 0) >= C(2, 2)) {
83
      const T s = 2 * std::sqrt(1 + C(0, 0) - C(1, 1) - C(2, 2));
84
      q = {(C(2, 1) - C(1, 2)) / s, s / 4, (C(0, 1) + C(1, 0)) / s, (C(0, 2) + C(2, 0)) / s};
85
    } else if (C(1, 1) >= C(2, 2)) {
86
      const T s = 2 * std::sqrt(1 + C(1, 1) - C(0, 0) - C(2, 2));
87
      q = {(C(0, 2) - C(2, 0)) / s, (C(0, 1) + C(1, 0)) / s, s / 4, (C(1, 2) + C(2, 1)) / s};
88
    } else {
89
      const T s = 2 * std::sqrt(1 + C(2, 2) - C(0, 0) - C(1, 1));
90
      q = {(C(1, 0) - C(0, 1)) / s, (C(0, 2) + C(2, 0)) / s, (C(1, 2) + C(2, 1)) / s, s / 4};
91
    }
92
    return q.w < 0 ? Quat{-q.w, -q.x, -q.y, -q.z} : q;
93
  }
94

95
  // (yaw, pitch, roll) in radians for C_wb = Rz(yaw) Ry(pitch) Rx(roll).
96
  Vec3<T> euler_zyx() const {
97
    const T yaw = std::atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z));
98
    const T sp = std::fmax(T(-1), std::fmin(T(1), 2 * (w * y - z * x)));
99
    const T roll = std::atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y));
100
    return Vec3<T>{{yaw, std::asin(sp), roll}};
101
  }
102
};
103

104
// Attitude error vector log(est^-1 (x) truth), in the estimate's body frame.
105
template <class T>
106
Vec3<T> att_error(const Quat<T>& est, const Quat<T>& truth) {
107
  return (est.conj() * truth).log();
108
}
109

110
}  // namespace kl

1
// Mahony explicit complementary filter with an integral bias term.
2
// q <- q (x) exp((w_m - b + kp e) dt), b <- b - ki e dt,
3
// e = a_meas x a_pred + m_meas x m_pred (unit vectors in the body frame).
4
#pragma once
5
#include "kl/quat.hpp"
6

7
namespace kl {
8

9
// Reference magnetic field in the NED world frame: unit norm, 50 deg dip, no declination.
10
template <class T = double>
11
Vec3<T> default_mag_w() {
12
  const T dip = T(50) * T(3.14159265358979323846) / 180;
13
  return Vec3<T>{{std::cos(dip), 0, std::sin(dip)}};
14
}
15

16
template <class T = double>
17
class Mahony {
18
public:
19
  T kp, ki;
20
  Quat<T> q;
21
  Vec3<T> b{};
22
  Vec3<T> m_w;
23

24
  Mahony(T kp, T ki, const Quat<T>& q0, const Vec3<T>& m_w = default_mag_w<T>())
25
      : kp(kp), ki(ki), q(q0), m_w(m_w) {}
26

27
  const Quat<T>& step(const Vec3<T>& gyro, const Vec3<T>& accel, const Vec3<T>& mag, T dt) {
28
    const Mat<3, 3, T> Ct = q.dcm().transpose();
29
    const Vec3<T> a_pred = Ct * Vec3<T>{{0, 0, -1}};
30
    const Vec3<T> m_pred = Ct * m_w;
31
    const Vec3<T> a_n = accel * (1 / std::sqrt(dot(accel, accel)));
32
    const Vec3<T> m_n = mag * (1 / std::sqrt(dot(mag, mag)));
33
    const Vec3<T> e = cross(a_n, a_pred) + cross(m_n, m_pred);
34
    b = b - e * (ki * dt);  // update the bias before forming the rate
35
    const Vec3<T> w = gyro - b + e * kp;
36
    q = (q * Quat<T>::exp(w * dt)).normalized();
37
    return q;
38
  }
39
};
40

41
}  // namespace kl

1
// Attitude baselines: TRIAD snapshot and gyro-only integration.
2
#pragma once
3
#include "kl/quat.hpp"
4

5
namespace kl {
6

7
// Attitude q (body -> world) from one accelerometer + magnetometer snapshot.
8
// Specific force at rest points opposite to gravity, [0, 0, -1] in the NED world frame.
9
template <class T>
10
Quat<T> triad(const Vec3<T>& accel, const Vec3<T>& mag, const Vec3<T>& m_w) {
11
  auto frame = [](const Vec3<T>& v1, const Vec3<T>& v2) {
12
    Vec3<T> t2 = cross(v1, v2);
13
    t2 = t2 * (1 / std::sqrt(dot(t2, t2)));
14
    const Vec3<T> t3 = cross(v1, t2);
15
    Mat<3, 3, T> f{};
16
    for (std::size_t i = 0; i < 3; i++) {
17
      f(i, 0) = v1(i, 0);
18
      f(i, 1) = t2(i, 0);
19
      f(i, 2) = t3(i, 0);
20
    }
21
    return f;
22
  };
23
  const Vec3<T> b1 = accel * (1 / std::sqrt(dot(accel, accel)));
24
  const Vec3<T> b2 = mag * (1 / std::sqrt(dot(mag, mag)));
25
  const Mat<3, 3, T> C = frame(Vec3<T>{{0, 0, -1}}, m_w) * frame(b1, b2).transpose();
26
  return Quat<T>::from_dcm(C);
27
}
28

29
// One gyro-only integration step, q <- normalize(q (x) exp(gyro dt)).
30
template <class T>
31
void gyro_step(Quat<T>& q, const Vec3<T>& gyro, T dt) {
32
  q = (q * Quat<T>::exp(gyro * dt)).normalized();
33
}
34

35
}  // namespace kl

Rust#

1
#[derive(Clone, Copy, Debug)]
2
pub struct Quat {
3
    pub w: Real,
4
    pub x: Real,
5
    pub y: Real,
6
    pub z: Real,
7
}
8

9
impl Mul for Quat {
10
    type Output = Quat;
11
    fn mul(self, b: Quat) -> Quat {
12
        let a = self;
13
        Quat {
14
            w: a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z,
15
            x: a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y,
16
            y: a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x,
17
            z: a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w,
18
        }
19
    }
20
}
21

22
impl Quat {
23
    pub const fn identity() -> Self {
24
        Quat {
25
            w: 1.0,
26
            x: 0.0,
27
            y: 0.0,
28
            z: 0.0,
29
        }
30
    }
31

32
    pub fn conj(self) -> Quat {
33
        Quat {
34
            w: self.w,
35
            x: -self.x,
36
            y: -self.y,
37
            z: -self.z,
38
        }
39
    }
40

41
    pub fn normalized(self) -> Quat {
42
        let n = sqrt(self.w * self.w + self.x * self.x + self.y * self.y + self.z * self.z);
43
        Quat {
44
            w: self.w / n,
45
            x: self.x / n,
46
            y: self.y / n,
47
            z: self.z / n,
48
        }
49
    }
50

51
    /// Rotation vector -> unit quaternion.
52
    pub fn exp(v: [Real; 3]) -> Quat {
53
        let th = sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
54
        if th < SMALL {
55
            return Quat {
56
                w: 1.0 - th * th / 8.0,
57
                x: 0.5 * v[0],
58
                y: 0.5 * v[1],
59
                z: 0.5 * v[2],
60
            };
61
        }
62
        let s = sin(0.5 * th) / th;
63
        Quat {
64
            w: cos(0.5 * th),
65
            x: s * v[0],
66
            y: s * v[1],
67
            z: s * v[2],
68
        }
69
    }
70

71
    /// Unit quaternion -> rotation vector, using the sign with w >= 0.
72
    pub fn log(self) -> [Real; 3] {
73
        let q = if self.w < 0.0 {
74
            Quat {
75
                w: -self.w,
76
                x: -self.x,
77
                y: -self.y,
78
                z: -self.z,
79
            }
80
        } else {
81
            self
82
        };
83
        let n = sqrt(q.x * q.x + q.y * q.y + q.z * q.z);
84
        let k = if n < SMALL {
85
            2.0 / q.w
86
        } else {
87
            2.0 * atan2(n, q.w) / n
88
        };
89
        [k * q.x, k * q.y, k * q.z]
90
    }

1
    pub fn step(&mut self, gyro: [Real; 3], accel: [Real; 3], mag: [Real; 3], dt: Real) {
2
        // Predicted body-frame directions: C^T v
3
        let ct = self.q.dcm().transpose();
4
        let pred = |v: [Real; 3]| -> [Real; 3] {
5
            let mut o = [0.0; 3];
6
            for (i, oi) in o.iter_mut().enumerate() {
7
                *oi = ct.m[i][0] * v[0] + ct.m[i][1] * v[1] + ct.m[i][2] * v[2];
8
            }
9
            o
10
        };
11
        let ea = cross(unit(accel), pred([0.0, 0.0, -1.0]));
12
        let em = cross(unit(mag), pred(self.m_w));
13
        let mut w = [0.0; 3];
14
        for i in 0..3 {
15
            let e = ea[i] + em[i];
16
            self.b[i] -= self.ki * e * dt;
17
            w[i] = (gyro[i] - self.b[i] + self.kp * e) * dt;
18
        }
19
        self.q = (self.q * Quat::exp(w)).normalized();
20
    }
21
}

Quat 派生 Copy，所以 self.q * Quat::exp(w) 不会”移动走” self.q，赋值回 self.q 也没问题。step 里 pred 是个闭包，用 ct（ $C^T$ ）把世界系向量映射到机体系。Rust 版和 C++ 版的数学一样，差别在于：Rust 没有运算符重载到 [Real; 3] 上（数组不是我定义的类型，孤儿规则不允许），所以向量运算是显式的循环；C++ 的 Vec3 是自己的类型，运算符想怎么重载都行。

完整源码：rust/src/quat.rs、mahony.rs、baselines.rs

1
//! Hamilton quaternions, scalar first q = [w, x, y, z].
2
//! q rotates body to world, C(q) = C_wb. `exp(v)` maps a rotation vector to the
3
//! unit quaternion [cos(|v|/2), sin(|v|/2) v/|v|].
4

5
use crate::mat::Mat;
6
use crate::real::{Real, asin, atan2, cos, sin, sqrt};
7
use core::ops::Mul;
8

9
/// Below this angle (rad) use the Taylor expansion.
10
const SMALL: Real = 1e-8;
11

12
#[derive(Clone, Copy, Debug)]
13
pub struct Quat {
14
    pub w: Real,
15
    pub x: Real,
16
    pub y: Real,
17
    pub z: Real,
18
}
19

20
impl Mul for Quat {
21
    type Output = Quat;
22
    fn mul(self, b: Quat) -> Quat {
23
        let a = self;
24
        Quat {
25
            w: a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z,
26
            x: a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y,
27
            y: a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x,
28
            z: a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w,
29
        }
30
    }
31
}
32

33
impl Quat {
34
    pub const fn identity() -> Self {
35
        Quat {
36
            w: 1.0,
37
            x: 0.0,
38
            y: 0.0,
39
            z: 0.0,
40
        }
41
    }
42

43
    pub fn conj(self) -> Quat {
44
        Quat {
45
            w: self.w,
46
            x: -self.x,
47
            y: -self.y,
48
            z: -self.z,
49
        }
50
    }
51

52
    pub fn normalized(self) -> Quat {
53
        let n = sqrt(self.w * self.w + self.x * self.x + self.y * self.y + self.z * self.z);
54
        Quat {
55
            w: self.w / n,
56
            x: self.x / n,
57
            y: self.y / n,
58
            z: self.z / n,
59
        }
60
    }
61

62
    /// Rotation vector -> unit quaternion.
63
    pub fn exp(v: [Real; 3]) -> Quat {
64
        let th = sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
65
        if th < SMALL {
66
            return Quat {
67
                w: 1.0 - th * th / 8.0,
68
                x: 0.5 * v[0],
69
                y: 0.5 * v[1],
70
                z: 0.5 * v[2],
71
            };
72
        }
73
        let s = sin(0.5 * th) / th;
74
        Quat {
75
            w: cos(0.5 * th),
76
            x: s * v[0],
77
            y: s * v[1],
78
            z: s * v[2],
79
        }
80
    }
81

82
    /// Unit quaternion -> rotation vector, using the sign with w >= 0.
83
    pub fn log(self) -> [Real; 3] {
84
        let q = if self.w < 0.0 {
85
            Quat {
86
                w: -self.w,
87
                x: -self.x,
88
                y: -self.y,
89
                z: -self.z,
90
            }
91
        } else {
92
            self
93
        };
94
        let n = sqrt(q.x * q.x + q.y * q.y + q.z * q.z);
95
        let k = if n < SMALL {
96
            2.0 / q.w
97
        } else {
98
            2.0 * atan2(n, q.w) / n
99
        };
100
        [k * q.x, k * q.y, k * q.z]
101
    }
102

103
    /// Direction cosine matrix C(q) = C_wb (body -> world).
104
    pub fn dcm(self) -> Mat<3, 3> {
105
        let Quat { w, x, y, z } = self;
106
        Mat {
107
            m: [
108
                [
109
                    1.0 - 2.0 * (y * y + z * z),
110
                    2.0 * (x * y - w * z),
111
                    2.0 * (x * z + w * y),
112
                ],
113
                [
114
                    2.0 * (x * y + w * z),
115
                    1.0 - 2.0 * (x * x + z * z),
116
                    2.0 * (y * z - w * x),
117
                ],
118
                [
119
                    2.0 * (x * z - w * y),
120
                    2.0 * (y * z + w * x),
121
                    1.0 - 2.0 * (x * x + y * y),
122
                ],
123
            ],
124
        }
125
    }
126

127
    /// C_wb -> unit quaternion with w >= 0 (Shepperd's method).
128
    pub fn from_dcm(c: &Mat<3, 3>) -> Quat {
129
        let c = &c.m;
130
        let t = c[0][0] + c[1][1] + c[2][2];
131
        let q = if t > 0.0 {
132
            let s = 2.0 * sqrt(1.0 + t);
133
            Quat {
134
                w: 0.25 * s,
135
                x: (c[2][1] - c[1][2]) / s,
136
                y: (c[0][2] - c[2][0]) / s,
137
                z: (c[1][0] - c[0][1]) / s,
138
            }
139
        } else if c[0][0] >= c[1][1] && c[0][0] >= c[2][2] {
140
            let s = 2.0 * sqrt(1.0 + c[0][0] - c[1][1] - c[2][2]);
141
            Quat {
142
                w: (c[2][1] - c[1][2]) / s,
143
                x: 0.25 * s,
144
                y: (c[0][1] + c[1][0]) / s,
145
                z: (c[0][2] + c[2][0]) / s,
146
            }
147
        } else if c[1][1] >= c[2][2] {
148
            let s = 2.0 * sqrt(1.0 + c[1][1] - c[0][0] - c[2][2]);
149
            Quat {
150
                w: (c[0][2] - c[2][0]) / s,
151
                x: (c[0][1] + c[1][0]) / s,
152
                y: 0.25 * s,
153
                z: (c[1][2] + c[2][1]) / s,
154
            }
155
        } else {
156
            let s = 2.0 * sqrt(1.0 + c[2][2] - c[0][0] - c[1][1]);
157
            Quat {
158
                w: (c[1][0] - c[0][1]) / s,
159
                x: (c[0][2] + c[2][0]) / s,
160
                y: (c[1][2] + c[2][1]) / s,
161
                z: 0.25 * s,
162
            }
163
        };
164
        if q.w < 0.0 {
165
            Quat {
166
                w: -q.w,
167
                x: -q.x,
168
                y: -q.y,
169
                z: -q.z,
170
            }
171
        } else {
172
            q
173
        }
174
    }
175

176
    /// (yaw, pitch, roll) in radians for C_wb = Rz(yaw) Ry(pitch) Rx(roll).
177
    pub fn euler_zyx(self) -> (Real, Real, Real) {
178
        let Quat { w, x, y, z } = self;
179
        let yaw = atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z));
180
        let s = (2.0 * (w * y - z * x)).clamp(-1.0, 1.0);
181
        let roll = atan2(2.0 * (w * x + y * z), 1.0 - 2.0 * (x * x + y * y));
182
        (yaw, asin(s), roll)
183
    }
184

185
    /// Attitude error rotation vector log(est^-1 (x) truth), in the estimate's body frame.
186
    pub fn att_error(est: Quat, truth: Quat) -> [Real; 3] {
187
        (est.conj() * truth).log()
188
    }
189
}
190

191
/// Cross-product matrix [v]_x.
192
pub fn skew(v: [Real; 3]) -> Mat<3, 3> {
193
    Mat {
194
        m: [[0.0, -v[2], v[1]], [v[2], 0.0, -v[0]], [-v[1], v[0], 0.0]],
195
    }
196
}

1
//! Mahony explicit complementary filter with an integral bias term.
2
//! q <- q (x) exp((w_m - b + kp e) dt), b <- b - ki e dt,
3
//! e = a_meas x a_pred + m_meas x m_pred (unit vectors in the body frame).
4

5
use crate::quat::Quat;
6
use crate::real::{Real, cos, sin, sqrt};
7

8
/// Reference magnetic field in NED: unit norm, dip 50 deg down, declination 0.
9
pub fn mag_w() -> [Real; 3] {
10
    let dip = 50.0 * core::f64::consts::PI as Real / 180.0;
11
    [cos(dip), 0.0, sin(dip)]
12
}
13

14
pub struct Mahony {
15
    pub kp: Real,
16
    pub ki: Real,
17
    pub q: Quat,
18
    pub b: [Real; 3],
19
    pub m_w: [Real; 3],
20
}
21

22
fn cross(a: [Real; 3], b: [Real; 3]) -> [Real; 3] {
23
    [
24
        a[1] * b[2] - a[2] * b[1],
25
        a[2] * b[0] - a[0] * b[2],
26
        a[0] * b[1] - a[1] * b[0],
27
    ]
28
}
29

30
fn unit(v: [Real; 3]) -> [Real; 3] {
31
    let n = sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
32
    [v[0] / n, v[1] / n, v[2] / n]
33
}
34

35
impl Mahony {
36
    pub fn new(kp: Real, ki: Real, q0: Quat, m_w: [Real; 3]) -> Self {
37
        Self {
38
            kp,
39
            ki,
40
            q: q0,
41
            b: [0.0; 3],
42
            m_w,
43
        }
44
    }
45

46
    pub fn step(&mut self, gyro: [Real; 3], accel: [Real; 3], mag: [Real; 3], dt: Real) {
47
        // Predicted body-frame directions: C^T v
48
        let ct = self.q.dcm().transpose();
49
        let pred = |v: [Real; 3]| -> [Real; 3] {
50
            let mut o = [0.0; 3];
51
            for (i, oi) in o.iter_mut().enumerate() {
52
                *oi = ct.m[i][0] * v[0] + ct.m[i][1] * v[1] + ct.m[i][2] * v[2];
53
            }
54
            o
55
        };
56
        let ea = cross(unit(accel), pred([0.0, 0.0, -1.0]));
57
        let em = cross(unit(mag), pred(self.m_w));
58
        let mut w = [0.0; 3];
59
        for i in 0..3 {
60
            let e = ea[i] + em[i];
61
            self.b[i] -= self.ki * e * dt;
62
            w[i] = (gyro[i] - self.b[i] + self.kp * e) * dt;
63
        }
64
        self.q = (self.q * Quat::exp(w)).normalized();
65
    }
66
}

1
//! Attitude baselines: TRIAD snapshot and gyro-only integration.
2

3
use crate::mat::Mat;
4
use crate::quat::Quat;
5
use crate::real::{Real, sqrt};
6

7
fn cross(a: [Real; 3], b: [Real; 3]) -> [Real; 3] {
8
    [
9
        a[1] * b[2] - a[2] * b[1],
10
        a[2] * b[0] - a[0] * b[2],
11
        a[0] * b[1] - a[1] * b[0],
12
    ]
13
}
14

15
fn unit(v: [Real; 3]) -> [Real; 3] {
16
    let n = sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
17
    [v[0] / n, v[1] / n, v[2] / n]
18
}
19

20
/// Orthonormal triad with columns [v1, t2, v1 x t2].
21
fn frame(v1: [Real; 3], v2: [Real; 3]) -> Mat<3, 3> {
22
    let t2 = unit(cross(v1, v2));
23
    let t3 = cross(v1, t2);
24
    let mut f = Mat::zeros();
25
    for i in 0..3 {
26
        f.m[i] = [v1[i], t2[i], t3[i]];
27
    }
28
    f
29
}
30

31
/// Attitude q (body -> world) from one accelerometer + magnetometer snapshot.
32
/// Specific force at rest points opposite to gravity, [0, 0, -1] in NED.
33
pub fn triad(accel: [Real; 3], mag: [Real; 3], m_w: [Real; 3]) -> Quat {
34
    let c = frame([0.0, 0.0, -1.0], m_w) * frame(unit(accel), unit(mag)).transpose();
35
    Quat::from_dcm(&c)
36
}
37

38
/// One gyro-only integration step: q <- normalize(q (x) exp(gyro dt)).
39
pub fn gyro_step(q: Quat, gyro: [Real; 3], dt: Real) -> Quat {
40
    (q * Quat::exp([gyro[0] * dt, gyro[1] * dt, gyro[2] * dt])).normalized()
41
}

四元数恒等式自检#

三个 app（c/apps/ch04.c、cpp/apps/ch04.cpp、rust/src/bin/ch04.rs）在读数据之前先做一轮自检，任何一项失败就以非零码退出。1000 个确定性的旋转向量（长度从接近 0 一直到 $0.99\pi$ ）：

$\log(\exp(v)) = v$ 。
$C(q)$ 正交： $|CC^T-I|$ 和 $|\det C-1|$ 都小于容差。
$q\otimes q^* = [1,0,0,0]$ 。

容差 double 下是 $10^{-12}$ （第 8 篇的 float 构建里是 $10^{-4}$ ）。

结果#

1
$ make -s ch04
2
quaternion identities hold to 1.78e-15
3
ch04 self-checks passed (worst 1.89e-15)
4
ok   gyro_c.csv: max diff 0.000e+00 <= 1.0e-09
5
ok   gyro_cpp.csv: max diff 3.469e-18 <= 1.0e-09
6
ok   gyro_rs.csv: max diff 0.000e+00 <= 1.0e-09
7
ok   mahony_c.csv: max diff 0.000e+00 <= 1.0e-09
8
ok   mahony_cpp.csv: max diff 4.441e-16 <= 1.0e-09
9
ok   mahony_rs.csv: max diff 0.000e+00 <= 1.0e-09
10
ok   triad_c.csv: max diff 0.000e+00 <= 1.0e-09
11
ok   triad_cpp.csv: max diff 3.331e-16 <= 1.0e-09
12
ok   triad_rs.csv: max diff 0.000e+00 <= 1.0e-09
13

14
truth pitch range: [-77.3, 80.9] deg (gimbal lock is at +-90)
15
pitch = 90 deg: (yaw, roll) = (20, 10) deg and (57, 47) deg give the same rotation: max|C1 - C2| = 8.33e-17
16

17
RMSE over 120 s (deg), seed 1
18
method        yaw    pitch     roll    total
19
gyro       30.183   10.738   20.931   25.979
20
triad       5.312    1.523    2.190    5.702
21
mahony      4.390    1.489    2.250    4.881
22

23
Mahony total error by period (deg RMS)
24
  calm  [20,60) s : 0.758
25
  accel disturbance [60,70) s: 2.331
26
  mag   disturbance [90,100) s: 14.851  (roll/pitch only: 8.437)

C 和 Rust 对 Python 的差是精确的 0：这三个滤波器只用 sin、cos、atan2、sqrt 和加减乘除，我这台机器上这几个函数的结果在 Python 和这两个语言里逐位相同（C++ 有 $10^{-16}$ 量级的差异，大概是编译器对表达式的合并方式不同）。换一个 libm 实现，这个 0 就不一定能保持，所以容差还是 $10^{-9}$ 。

纯陀螺积分。 误差随时间单调变坏：10 秒时总误差 6.6 度，60 秒 23.6 度，120 秒 41.9 度，RMSE 25.979 度。零偏 $0.02$ rad/s 的量级，一阶估算 120 秒会积出 2.4 rad（约 137 度）；实际只有 41.9 度，我猜是因为机体一直在转，零偏造成的误差在世界系里方向不断变化，一部分互相抵消了（这是推测，我没有单独验证）。图里偏航和横滚曲线上的尖峰是欧拉角折到 $\pm180^\circ$ 的缠绕，加上俯仰接近 80 度时的万向锁放大，不是旋转本身的突变。

TRIAD。 没有漂移，没有记忆，每个样本的噪声直接进来，所以横滚和俯仰的”毛刺”最大。线加速度扰动段 $[60,70)$ ，总误差 8.247 度，对比平静段 1.044 度：加速度计把线加速度当成了重力的偏转。磁干扰段，TRIAD 的偏航被拉偏，平均偏 $-17.63^\circ$ 。可以手算验证：磁场的水平分量从 $[\cos50^\circ,0] = [0.643,0]$ 变成 $[0.943,0.3]$ ，方向偏转 $\arctan(0.3/0.943) = 17.65^\circ$ 。

Mahony。 平静段 0.758 度，线加速度段 2.331 度（比 TRIAD 的 8.247 度好得多：陀螺在这十秒里主导，加速度计的坏数据只通过 $k_p$ 慢慢渗进来）。但磁干扰段的总误差是 14.851 度，其中横滚和俯仰部分是 8.437 度。TRIAD 在这一段，横滚和俯仰不受磁力计影响；Mahony 却会：它把磁干扰当作一个”姿态失配”，用 $m_n\times\hat v_m$ 去修正，而这个叉乘有一部分落在横滚和俯仰方向上。偏航被拉偏平均 $-11.36^\circ$ ，比 TRIAD 的少，但没有消除。

整体 RMSE 4.881 度对比 TRIAD 的 5.702 度，好得有限，因为它们共同的最大误差来源是磁干扰那 10 秒。这件事需要的不是更好的增益，而是让滤波器理解”磁力计只能影响偏航”，并在传感器不可信的时候知道。这是第 5、6 篇的主题。

下一篇#

Mahony 滤波器已经能用，但它的两个增益 $k_p$ 和 $k_i$ 是我选的，没有来由，也不会随工况变化。下一篇我们用第 1、2 篇的推导重新做一遍：状态是四元数加陀螺零偏共 7 维，过程模型是 $q\otimes\exp(\tfrac12(\omega_m-b)\Delta t)$ ，测量是加速度计和磁力计的方向向量，非线性，所以要线性化，这就是扩展卡尔曼滤波。它能不能赢过 Mahony，以及为什么 $4\times4$ 的四元数协方差天生有一个方向的特征值是 0，是第 5 篇的内容。

音乐

音乐

约定#

数学#

旋转矩阵与 SO(3)#

欧拉角与万向锁#

轴角、Rodrigues 与指数映射#

四元数#

三个传感器测的是什么#

为什么加速度计只给横滚和俯仰，磁力计给偏航#

仿真器#

Mahony 互补滤波#

Python 实现#

四元数#

仿真器#

Mahony 与基线#

C、C++、Rust 实现#

C#

C++#

Rust#

四元数恒等式自检#

结果#

下一篇#

支持与分享

评论区