扩展卡尔曼滤波：把四元数塞进 EKF

平静段 20 到 60 秒，上一篇的 Mahony 互补滤波器误差是 0.758 度，这一篇的扩展卡尔曼滤波器是 0.086 度，差了 8.8 倍。同一份数据，同样的传感器。但整段 120 秒算下来，两者的总均方根误差是 4.881 度对 4.719 度，几乎没分出胜负：它们共同被 90 到 100 秒的磁干扰拖到了同一个水平。

这一篇做的事情：把第 1、2 篇推导的滤波器推广到非线性系统，用它估计三维姿态，然后诚实地看它哪里好、哪里不够。

本篇检查点

这一篇结束时你会有：make ch05 通过，out/ch05/ekf_{py,c,cpp,rs}.csv，四张图；C、C++、Rust 各一个四元数 EKF（kl_ekf_att、ekf_attitude.hpp、ekf_att.rs）和一个共用的测量更新模块（kl_update、update.hpp、update.rs）。雅可比的有限差分检查在每个语言的 app 里先于数据处理运行。前置：第 4 篇（约定、四元数、IMU 仿真器、Mahony 基线）和第 2 篇（线性卡尔曼滤波、Joseph 形式、Cholesky）。

数学#

非线性系统与线性化#

前面的滤波器都是线性的： $x_k = Fx_{k-1}+\dots$ ， $z = Hx+v$ 。姿态问题不是：状态方程里有 $q\otimes\exp(\cdot)$ ，测量方程里有 $C(q)^Tv$ 。设

x_{k+1} = f(x_k,u_k)+w_k,\qquad z_k = h(x_k)+v_k

扩展卡尔曼滤波（EKF）的想法是：在当前估计附近，把 $f$ 和 $h$ 做一阶泰勒展开，然后把展开式当成线性模型套用线性滤波器。

f(x)\approx f(\hat x)+F(x-\hat x),\quad F = \left.\frac{\partial f}{\partial x}\right|_{\hat x};\qquad h(x)\approx h(\hat x^-)+H(x-\hat x^-),\quad H = \left.\frac{\partial h}{\partial x}\right|_{\hat x^-}

结果是：

\hat x^- = f(\hat x,u),\quad P^- = FPF^T+Q;\qquad y = z-h(\hat x^-),\quad S = HP^-H^T+R,\quad K = P^-H^TS^{-1}

\hat x = \hat x^- + Ky,\qquad P = (I-KH)P^-(I-KH)^T+KRK^T

注意状态和测量的”均值”用的是非线性函数本身（ $f(\hat x)$ 、 $h(\hat x^-)$ ），线性化只用在协方差和增益上。

EKF 丢掉了什么#

线性滤波器的最优性来自高斯分布经过线性变换仍是高斯。非线性函数不保持这个性质，所以 EKF 没有任何最优性保证。具体的损失有三项：

线性化误差： $F$ 、 $H$ 是在估计值处求的，不是在真值处。估计差得越远，线性模型越不准。第 6 篇的大初始误差实验会看到这一点。
均值偏差：非线性函数把高斯变成非高斯， $f(\hat x)$ 不是均值的无偏估计（二阶项）。
不一致：线性化的 $P$ 常常偏小，滤波器过于自信，NEES 超出界限（第 3 篇）。

还有一个工程问题，我觉得是最危险的：雅可比写错，滤波器通常不会崩溃，它会给出一个”看着合理但略差”的结果，你可能永远不会发现。所以我强制在每个 app 里先做有限差分检查，用中心差分 $(g(x+\epsilon e_j)-g(x-\epsilon e_j))/2\epsilon$ 对比解析雅可比。检查不通过，程序直接退出。

过程模型#

状态 $x = [q\ (4),\ b\ (3)]$ ，7 维，输入是陀螺读数 $\omega_m$ 。预测：

q^- = q\otimes\exp\big((\omega_m-b)\Delta t\big),\qquad b^- = b

（第 4 篇的记号： $\exp(v)$ 是旋转向量 $v$ 对应的四元数。）

$F_{qq}$ 。 四元数乘法对 $q$ 是线性的： $q\otimes p = R(p)q$ ， $R(p)$ 是 $p$ 的”右乘矩阵”。展开乘法公式就是（ $p = [p_w,p_x,p_y,p_z]$ ）

R(p) = \begin{bmatrix}p_w&-p_x&-p_y&-p_z\\p_x&p_w&p_z&-p_y\\p_y&-p_z&p_w&p_x\\p_z&p_y&-p_x&p_w\end{bmatrix}

所以 $F_{qq} = R(\exp((\omega_m-b)\Delta t))$ ，是精确的，不是近似。

$F_{qb}$ 。 类似地， $q\otimes[0,\omega] = \Xi(q)\omega$ ，

\Xi(q) = \begin{bmatrix}-x&-y&-z\\w&-z&y\\z&w&-x\\-y&x&w\end{bmatrix}

$\exp(\phi)\approx[1,\tfrac12\phi]$ （一阶）， $\phi = (\omega_m-b)\Delta t$ ，所以 $q^-\approx q + \tfrac12\Delta t\,\Xi(q)(\omega_m-b)$ ，对 $b$ 求导： $F_{qb} = -\tfrac{\Delta t}{2}\Xi(q)$ 。这个是一阶近似，不是精确的：精确的导数含 $\exp$ 的精确雅可比，差一个 $O(\Delta t|\phi|)$ 的量。有限差分检查会量化它：差距是 $2.2\times10^{-5}$ ，我给这一项的容差是 $10^{-3}$ 。

$F_{bb} = I$ 。

过程噪声。 陀螺噪声 $n_g\sim\mathcal N(0,\sigma_g^2I)$ 通过 $\omega = \omega_m-b-n_g$ 进入。 $q^-$ 对 $\omega$ 的灵敏度和对 $-b$ 的一样， $\tfrac{\Delta t}{2}\Xi(q)$ ，所以

Q_{qq} = \Big(\frac{\Delta t}2\Big)^2\Xi(q)\,\sigma_g^2I\,\Xi(q)^T,\qquad Q_{bb} = \sigma_{bw}^2\Delta t\,I

$Q_{bb}$ 就是第 2 篇的零偏随机游走。 $\sigma_g = 0.005$ rad/s 是每样本噪声，积分一步的角度噪声是 $\sigma_g\Delta t$ ，和第 1 篇的 $q = (\sigma_n\Delta t)^2$ 同一个意思。

测量模型#

加速度计（静止时）测到的是 $-C^T\mathbf g_w$ ，方向是机体系下的 $C(q)^T[0,0,-1]^T$ 。我们只用它的方向： $z_a = f_m/|f_m|$ ，预测 $h_a(q) = C(q)^T[0,0,-1]^T$ 。单位向量的每个分量的噪声约为 $\sigma_a/g$ ，所以 $R_a = (\sigma_a/g)^2I_3$ 。

磁力计同理： $z_m = m_m/|m_m|$ ， $h_m(q) = C(q)^Tm_w$ ， $R_m = \sigma_m^2I_3$ （磁场本身就是单位长度的）。

加速度计门控。 线性加速度会让 $f_m$ 偏离重力。简单的检测：比力的大小应该接近 $g$ ，当 $\big||f_m|-g\big|>0.5$ m/s² 时跳过这次加速度计更新。这个门限的效果我在结果里给出，会有意外。

磁力计在本篇不设门。 磁干扰窗口故意让磁力计的数据是错的，EKF 照单全收，这个后果留给第 6 篇修。

测量雅可比#

需要 $\partial h/\partial q$ ， $h(q) = C(q)^Tv$ ， $v$ 是常向量。第 4 篇推出过 $C(q)v = (w^2-u\cdot u)v+2(u\cdot v)u+2w(u\times v)$ 。转置对应共轭 $q^*$ ，也就是 $u\to-u$ ：

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

这个多项式形式对任意 $q$ （不限单位长度）都有定义，是 $q$ 的二次齐次函数，我下面求导和做有限差分都用它（实现里的 rot_inv）。对 $w$ ：

\frac{\partial h}{\partial w} = 2wv-2(u\times v)

对 $u$ （逐项）： $\partial[(w^2-u\cdot u)v]/\partial u = -2vu^T$ ； $\partial[2(u\cdot v)u]/\partial u = 2uv^T+2(u\cdot v)I$ ； $\partial[-2w(u\times v)]/\partial u$ ，因为 $u\times v = -[v]_\times u$ ，是 $+2w[v]_\times$ 。合起来：

\frac{\partial h}{\partial u} = 2(u\cdot v)I+2uv^T-2vu^T+2w[v]_\times

$H = [\partial h/\partial q\ \ 0_{3\times3}]$ ， $3\times7$ 。有限差分检查：100 个确定性的状态，两个参考向量（重力方向和磁场）， $\max|H-H_{fd}| = 2.25\times10^{-10}$ ，远小于 $10^{-6}$ 的门槛。

归一化和协方差#

状态里的 $q$ 要保持单位长度，每次更新之后做 $q\leftarrow q/|q|$ 。但是 $P$ 里 $4\times4$ 的块没有被相应处理，这是个结构性问题：真实的姿态不确定度只有 3 个自由度，而 $P_{qq}$ 是 $4\times4$ 的，它的第四个方向（沿着 $q$ 本身）在物理上没有意义。后面的数据里可以看到它的特征值。这是第 6 篇换成误差状态表示的主要动机。

初值#

$q_0$ 取第一个样本的 TRIAD， $b_0 = 0$ ， $P_0 = \operatorname{diag}(0.01I_4,\ 10^{-3}I_3)$ 。噪声参数用仿真里的真实值： $\sigma_g = 0.005$ ， $\sigma_{bw} = 10^{-4}$ ， $\sigma_a = 0.05$ ， $\sigma_m = 0.01$ 。

Python 实现#

共用的测量更新#

三个非线性滤波器（EKF、ESKF、UKF）都需要同一个动作：给定新息 $y$ 、 $H$ 、 $R$ ，算增益、算 Joseph 形式的新 $P$ 、返回修正量 $dx = Ky$ 。我把它单独拿出来：

1
"""Measurement update shared by the nonlinear filters (chapters 5 to 7)."""
2

3
import numpy as np
4
from scipy.linalg import cho_factor, cho_solve
5

6

7
def joseph_update(P, H, R, y):
8
    """Joseph-form covariance update for a given innovation y.
9

10
    Returns (dx, P_new, S) with dx = K y, K^T = S^-1 H P, S = H P H^T + R.
11
    The caller decides what dx means: an additive correction (EKF) or an error
12
    state to inject (ESKF). Raises LinAlgError if S is not positive definite.
13
    """
14
    n = P.shape[0]
15
    S = H @ P @ H.T + R
16
    c = cho_factor(S, lower=True)
17
    K = cho_solve(c, H @ P).T
18
    A = np.eye(n) - K @ H
19
    P_new = A @ P @ A.T + K @ R @ K.T
20
    return K @ y, 0.5 * (P_new + P_new.T), S

返回的是 $dx$ 而不是更新后的状态，因为 EKF 把它加到状态上，误差状态滤波器（第 6 篇）要把它”注入”名义状态。P_new 是对称化过的。

EKF#

1
"""Multiplicative-quaternion EKF: state x = [q (4), b (3)], input = gyro."""
2

3
import numpy as np
4

5
from .imu_sim import G
6
from .quat import qexp, qmul, qnormalize
7
from .update import joseph_update
8

9

10
def rot_inv(q, v):
11
    """C(q)^T v written as a polynomial in q: (w^2 - u.u) v + 2 (u.v) u - 2 w (u x v).
12

13
    Equal to qrot(q).T @ v for unit q, and defined (and differentiable) for any q.
14
    """
15
    w, u = q[0], q[1:]
16
    return (w * w - u @ u) * v + 2 * (u @ v) * u - 2 * w * np.cross(u, v)
17

18

19
def h_jac(q, v):
20
    """d rot_inv(q, v) / d q, a 3x4 matrix [d/dw | d/du]."""
21
    w, u = q[0], q[1:]
22
    dw = 2 * w * v - 2 * np.cross(u, v)
23
    skew_v = np.array([[0.0, -v[2], v[1]], [v[2], 0.0, -v[0]], [-v[1], v[0], 0.0]])
24
    du = 2 * (u @ v) * np.eye(3) + 2 * np.outer(u, v) - 2 * np.outer(v, u) + 2 * w * skew_v
25
    return np.column_stack([dw, du])
26

27

28
def xi(q):
29
    """4x3 matrix with q (x) [0, omega] = xi(q) omega."""
30
    w, x, y, z = q
31
    return np.array([[-x, -y, -z], [w, -z, y], [z, w, -x], [-y, x, w]])
32

33

34
def right_mul(p):
35
    """4x4 matrix with q (x) p = right_mul(p) q."""
36
    pw, px, py, pz = p
37
    return np.array([
38
        [pw, -px, -py, -pz],
39
        [px, pw, pz, -py],
40
        [py, -pz, pw, px],
41
        [pz, py, -px, pw],
42
    ])
43

44

45
class QuatEKF:
46
    def __init__(self, sigma_g, sigma_bw, sigma_a, sigma_m, m_w, q0, P0, accel_gate=0.5):
47
        self.sg, self.sbw, self.sa, self.sm = sigma_g, sigma_bw, sigma_a, sigma_m
48
        self.m_w = np.asarray(m_w, dtype=float)
49
        self.q = np.asarray(q0, dtype=float).copy()
50
        self.b = np.zeros(3)
51
        self.P = np.asarray(P0, dtype=float).copy()
52
        self.accel_gate = accel_gate  # m/s^2 on | |f| - g |; None disables gating
53
        self.last_nis = np.nan
54

55
    def predict(self, gyro, dt):
56
        omega = gyro - self.b
57
        dq = qexp(omega * dt)
58
        X = xi(self.q)
59
        F = np.eye(7)
60
        F[:4, :4] = right_mul(dq)
61
        F[:4, 4:] = -0.5 * dt * X
62
        Q = np.zeros((7, 7))
63
        Q[:4, :4] = (0.5 * dt) ** 2 * self.sg**2 * X @ X.T
64
        Q[4:, 4:] = self.sbw**2 * dt * np.eye(3)
65
        self.q = qmul(self.q, dq)
66
        self.P = F @ self.P @ F.T + Q
67

68
    def _update(self, z, v, r):
69
        H = np.zeros((3, 7))
70
        H[:, :4] = h_jac(self.q, v)
71
        y = z - rot_inv(self.q, v)
72
        dx, self.P, S = joseph_update(self.P, H, r * np.eye(3), y)
73
        self.q = qnormalize(self.q + dx[:4])
74
        self.b = self.b + dx[4:]
75
        self.last_nis = float(y @ np.linalg.solve(S, y))
76

77
    def update_accel(self, accel):
78
        """Gravity-direction update. Returns False when gated out."""
79
        norm = np.sqrt(accel @ accel)
80
        if self.accel_gate is not None and abs(norm - G) > self.accel_gate:
81
            return False
82
        self._update(accel / norm, np.array([0.0, 0.0, -1.0]), (self.sa / G) ** 2)
83
        return True
84

85
    def update_mag(self, mag):
86
        self._update(mag / np.sqrt(mag @ mag), self.m_w, self.sm**2)
87
        return True

rot_inv、h_jac、xi、right_mul 是上面四个矩阵的逐字翻译。predict 里 self.q = qmul(self.q, dq) 没有归一化，_update 里才归一化：单位四元数乘单位四元数，舍入误差每步 $10^{-16}$ 量级，用 qnormalize 管一管。update_accel 的返回值告诉你这次有没有被门控掉。

驱动脚本里每一步依次做三件事：

1
def run_ekf(gyro, acc, mag, accel_gate=0.5, keep_final=False):
2
    n = len(gyro)
3
    f = make_ekf(triad(acc[0], mag[0], MAG_W), accel_gate)
4
    out = np.empty((n, 14))
5
    out[0] = [*f.q, *f.b, *np.diag(f.P)]
6
    for k in range(1, n):
7
        f.predict(gyro[k], DT)
8
        f.update_accel(acc[k])
9
        f.update_mag(mag[k])
10
        out[k] = [*f.q, *f.b, *np.diag(f.P)]
11
    return (out, f) if keep_final else out

C、C++、Rust 实现#

共用的测量更新#

三个语言的共用更新，各自约 25 行。C 版：

1
#include "kl/kl_update.h"
2

3
#include "kl/kl_mat.h"
4

5
int kl_joseph_update(kl_real *P, const kl_real *H, const kl_real *R, const kl_real *y, int n, int m,
6
         kl_real *dx, kl_real *S_out, kl_real *work)
7
{
8
  kl_real *HP = work;
9
  kl_real *S = HP + m * n;
10
  kl_real *L = S + m * m;
11
  kl_real *Kt = L + m * m;
12
  kl_real *K = Kt + m * n;
13
  kl_real *A = K + n * m;
14
  kl_real *T = A + n * n;
15
  kl_real *U = T + n * n;
16
  kl_real *KR = U + n * n;
17

18
  kl_mat_mul(H, P, HP, m, n, n);
19
  kl_mat_mul_bt(HP, H, T, m, n, m);
20
  kl_mat_add(T, R, S, m, m);
21

22
  kl_mat_copy(S, L, m, m);
23
  if (kl_chol(L, m) != 0)
24
    return -1;
25
  kl_mat_copy(HP, Kt, m, n);
26
  kl_chol_solve(L, Kt, m, n);
27
  kl_mat_transpose(Kt, K, m, n);
28
  if (S_out)
29
    kl_mat_copy(S, S_out, m, m);
30
  kl_mat_mul(K, y, dx, n, m, 1);
31

32
  /* A = I - K H; P = A P A^T + K R K^T */
33
  kl_mat_mul(K, H, T, n, m, n);
34
  kl_mat_identity(U, n);
35
  kl_mat_sub(U, T, A, n, n);
36
  kl_mat_mul(A, P, T, n, n, n);
37
  kl_mat_mul_bt(T, A, U, n, n, n);
38
  kl_mat_mul(K, R, KR, n, m, m);
39
  kl_mat_mul_bt(KR, K, T, n, m, n);
40
  kl_mat_add(U, T, P, n, n);
41
  kl_mat_symmetrize(P, n);
42
  return 0;
43
}

C++ 和 Rust 版本的区别：C++ 通过输出引用 dx 返回修正量，返回 bool；Rust 返回 Option<(dx, S)>，None 表示 $S$ 不正定，此时 P 保持不变。

完整源码：c/include/kl/kl_update.h、cpp/include/kl/update.hpp、rust/src/update.rs

1
/* Measurement update shared by the nonlinear filters. */
2
#ifndef KL_UPDATE_H
3
#define KL_UPDATE_H
4

5
#include "kl/kl_real.h"
6

7
/* Workspace (kl_real count) of kl_joseph_update, laid out as
8
 *   HP m*n | S m*m | L m*m | Kt m*n | K n*m | A n*n | T n*n | U n*n | KR n*m */
9
#define KL_JOSEPH_WORK_LEN(n, m) (4 * (m) * (n) + 2 * (m) * (m) + 3 * (n) * (n))
10

11
/* Joseph-form covariance update for a given innovation y (m):
12
 * S = H P H^T + R, K^T = S^-1 H P (Cholesky), dx = K y,
13
 * P <- A P A^T + K R K^T with A = I - K H, then symmetrized.
14
 * The caller decides what dx means: an additive correction (EKF) or an error state to inject.
15
 * P is n x n (in place), H m x n, R m x m. S_out (m x m) may be NULL.
16
 * Returns -1 if S is not positive definite (P is then unchanged), else 0. */
17
int kl_joseph_update(kl_real *P, const kl_real *H, const kl_real *R, const kl_real *y, int n, int m,
18
         kl_real *dx, kl_real *S_out, kl_real *work);
19

20
#endif

1
// Measurement update shared by the nonlinear filters (chapters 5 to 7).
2
#pragma once
3
#include "kl/mat.hpp"
4

5
namespace kl {
6

7
// Joseph-form covariance update for a given innovation y.
8
// Computes dx = K y with K^T = S^-1 H P and S = H P H^T + R, and updates P in place.
9
// The caller decides what dx means: an additive correction (EKF) or an error state
10
// to inject (ESKF). Returns false, leaving P and dx unchanged, if S is not positive
11
// definite.
12
template <std::size_t N, std::size_t M, class T>
13
bool joseph_update(Mat<N, N, T>& P, const Mat<M, N, T>& H, const Mat<M, M, T>& R,
14
       const Mat<M, 1, T>& y, Mat<N, 1, T>& dx, Mat<M, M, T>* S_out = nullptr) {
15
  const Mat<M, N, T> HP = H * P;
16
  const Mat<M, M, T> S = HP * H.transpose() + R;
17
  Mat<M, M, T> L = S;
18
  if (!cholesky(L)) return false;
19
  const Mat<N, M, T> K = chol_solve(L, HP).transpose();
20
  const Mat<N, N, T> A = Mat<N, N, T>::identity() - K * H;
21
  P = (A * P * A.transpose() + K * R * K.transpose()).symmetrized();
22
  dx = K * y;
23
  if (S_out) *S_out = S;
24
  return true;
25
}
26

27
}  // namespace kl

1
//! Measurement update shared by the nonlinear filters.
2

3
use crate::mat::{Mat, chol_solve};
4

5
/// Joseph-form covariance update for a given innovation `y`.
6
/// S = H P H^T + R, K^T = S^-1 (H P) by Cholesky solve. Returns `dx = K y`
7
/// and replaces `p` with A P A^T + K R K^T, symmetrized, A = I - K H.
8
/// The caller decides what `dx` means: an additive correction (EKF) or an
9
/// error state to inject (ESKF). Returns `None` (P unchanged) if S is not
10
/// positive definite.
11
pub fn joseph_update<const N: usize, const M: usize>(
12
    p: &mut Mat<N, N>,
13
    h: &Mat<M, N>,
14
    r: &Mat<M, M>,
15
    y: &Mat<M, 1>,
16
) -> Option<(Mat<N, 1>, Mat<M, M>)> {
17
    let hp = *h * *p;
18
    let s = hp * h.transpose() + *r;
19
    let l = s.cholesky()?;
20
    let k = chol_solve(&l, &hp).transpose();
21
    let a = Mat::<N, N>::identity() - k * *h;
22
    *p = (a * *p * a.transpose() + k * *r * k.transpose()).symmetrized();
23
    Some((k * *y, s))
24
}

C#

C 版的滤波器状态是一个结构体，所有数组都是定长的，包括一块 280 个元素的内联工作区：

1
typedef struct {
2
  kl_real q[4], b[3], P[49];
3
  kl_real sg, sbw, sa, sm; /* gyro, bias random walk, accel, mag noise */
4
  kl_real m_w[3];
5
  kl_real accel_gate; /* m/s^2 on | |f| - g |; negative disables gating */
6
  kl_real work[280];  /* scratch: predict needs 196, accel/mag updates 30 + KL_JOSEPH_WORK_LEN(7, 3) = 279 */
7
} kl_ekf_att;

没有 malloc，没有”调用者要提供缓冲区”的负担，代价是每个 kl_ekf_att 对象带着一块不一定用满的工作区：predict 用到 196 个元素，两个测量更新用到 30 加上 KL_JOSEPH_WORK_LEN(7, 3) 共 279 个，所以是 280。（这个数字我第一次写的是 256，更新步骤会越界写 23 个元素，覆盖紧挨着结构体的下一个变量。第 5 篇的 app 里它恰好没有踩到任何有用的东西，输出还是和 Python 对得上；到了第 8 篇，同一份代码链接进另一个程序，邻居变成了一个全局常量，结果变成了 NaN，我才发现。这是 C 里没有边界检查的代价，也是为什么 C++ 和 Rust 版本没有这个问题：矩阵尺寸是类型的一部分。）预测：

1
void kl_ekf_att_predict(kl_ekf_att *f, const kl_real gyro[3], kl_real dt)
2
{
3
  kl_real *F = f->work, *Q = F + 49, *T = Q + 49, *U = T + 49;
4
  kl_real omega[3], dq[4], X[12], Rm[16], Xs[12], qn[4];
5
  for (int i = 0; i < 3; i++)
6
    omega[i] = (gyro[i] - f->b[i]) * dt;
7
  kl_quat_exp(omega, dq);
8
  kl_ekf_att_xi(f->q, X);
9
  kl_ekf_att_right_mul(dq, Rm);
10

11
  /* F = [[right_mul(dq), -dt/2 Xi], [0, I]] */
12
  kl_mat_identity(F, 7);
13
  for (int i = 0; i < 4; i++) {
14
    for (int j = 0; j < 4; j++)
15
      F[i * 7 + j] = Rm[i * 4 + j];
16
    for (int j = 0; j < 3; j++)
17
      F[i * 7 + 4 + j] = -(kl_real)0.5 * dt * X[i * 3 + j];
18
  }
19
  /* Q = blkdiag((dt/2)^2 sg^2 Xi Xi^T, sbw^2 dt I) */
20
  for (int i = 0; i < 49; i++)
21
    Q[i] = 0;
22
  kl_mat_scale(X, ((kl_real)0.5 * dt) * ((kl_real)0.5 * dt) * f->sg * f->sg, Xs, 4, 3);
23
  kl_mat_mul_bt(Xs, X, Rm, 4, 3, 4);
24
  for (int i = 0; i < 4; i++)
25
    for (int j = 0; j < 4; j++)
26
      Q[i * 7 + j] = Rm[i * 4 + j];
27
  for (int i = 4; i < 7; i++)
28
    Q[i * 8] = f->sbw * f->sbw * dt;
29

30
  kl_quat_mul(f->q, dq, qn);
31
  kl_mat_copy(qn, f->q, 4, 1);
32
  kl_mat_mul(F, f->P, T, 7, 7, 7);
33
  kl_mat_mul_bt(T, F, U, 7, 7, 7);
34
  kl_mat_add(U, Q, f->P, 7, 7);
35
}

更新（加速度计和磁力计共用一个内部函数，差别只在 z、v、r）：

1
/* H = [dh/dq | 0], y = z - h(q); the correction dx is added to the state. */
2
static int update(kl_ekf_att *f, const kl_real z[3], const kl_real v[3], kl_real r)
3
{
4
  kl_real *H = f->work, *R = H + 21, *jw = R + 9;
5
  kl_real Hq[12], hq[3], y[3], dx[7], qn[4];
6
  kl_ekf_att_h_jac(f->q, v, Hq);
7
  for (int i = 0; i < 3; i++)
8
    for (int j = 0; j < 7; j++)
9
      H[i * 7 + j] = j < 4 ? Hq[i * 4 + j] : 0;
10
  kl_ekf_att_rot_inv(f->q, v, hq);
11
  for (int i = 0; i < 3; i++) {
12
    y[i] = z[i] - hq[i];
13
    for (int j = 0; j < 3; j++)
14
      R[i * 3 + j] = (i == j) ? r : 0;
15
  }
16
  if (kl_joseph_update(f->P, H, R, y, 7, 3, dx, NULL, jw) != 0)
17
    return -1;
18
  for (int i = 0; i < 4; i++)
19
    qn[i] = f->q[i] + dx[i];
20
  kl_quat_normalize(qn);
21
  kl_mat_copy(qn, f->q, 4, 1);
22
  for (int i = 0; i < 3; i++)
23
    f->b[i] += dx[4 + i];
24
  return 1;
25
}
26

27
static void unit(const kl_real *v, kl_real *u, kl_real *norm)
28
{
29
  *norm = KL_SQRT(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
30
  for (int i = 0; i < 3; i++)
31
    u[i] = v[i] / *norm;
32
}
33

34
int kl_ekf_att_update_accel(kl_ekf_att *f, const kl_real accel[3])
35
{
36
  kl_real z[3], norm;
37
  const kl_real down[3] = {0, 0, -1};
38
  unit(accel, z, &norm);
39
  if (f->accel_gate >= 0 && KL_FABS(norm - (kl_real)KL_GRAVITY) > f->accel_gate)
40
    return 0;
41
  return update(f, z, down, (f->sa / (kl_real)KL_GRAVITY) * (f->sa / (kl_real)KL_GRAVITY));
42
}
43

44
int kl_ekf_att_update_mag(kl_ekf_att *f, const kl_real mag[3])
45
{
46
  kl_real z[3], norm;
47
  unit(mag, z, &norm);
48
  return update(f, z, f->m_w, f->sm * f->sm);
49
}

C++#

1
  void predict(const Vec3<T>& gyro, T dt) {
2
    const Quat<T> dq = Quat<T>::exp((gyro - b) * dt);
3
    const Mat<4, 3, T> X = xi(q);
4
    Mat<7, 7, T> F = Mat<7, 7, T>::identity();
5
    Mat<7, 7, T> Q{};
6
    const Mat<4, 4, T> Qq = X * X.transpose() * ((T(0.5) * dt) * (T(0.5) * dt) * sg * sg);
7
    const Mat<4, 4, T> Fq = right_mul(dq);
8
    for (std::size_t i = 0; i < 4; i++) {
9
      for (std::size_t j = 0; j < 4; j++) {
10
        F(i, j) = Fq(i, j);
11
        Q(i, j) = Qq(i, j);
12
      }
13
      for (std::size_t j = 0; j < 3; j++) F(i, 4 + j) = -T(0.5) * dt * X(i, j);
14
    }
15
    for (std::size_t i = 4; i < 7; i++) Q(i, i) = sbw * sbw * dt;
16
    q = q * dq;  // not normalized here
17
    P = F * P * F.transpose() + Q;
18
  }
19

20
  // Gravity-direction update. Returns false when gated out or when S is not
21
  // positive definite (state unchanged in both cases).
22
  bool update_accel(const Vec3<T>& accel) {
23
    const T norm = std::sqrt(dot(accel, accel));
24
    if (accel_gate >= 0 && std::fabs(norm - T(GRAVITY)) > accel_gate) return false;
25
    return update(accel * (1 / norm), Vec3<T>{{0, 0, -1}}, (sa / T(GRAVITY)) * (sa / T(GRAVITY)));
26
  }
27

28
  // Magnetic-field update; false if S is not positive definite (state unchanged).
29
  bool update_mag(const Vec3<T>& mag) {
30
    return update(mag * (1 / std::sqrt(dot(mag, mag))), m_w, sm * sm);
31
  }
32

33
private:
34
  bool update(const Vec3<T>& z, const Vec3<T>& v, T r) {
35
    Mat<3, 7, T> H{};
36
    const Mat<3, 4, T> J = h_jac(q, v);
37
    for (std::size_t i = 0; i < 3; i++)
38
      for (std::size_t j = 0; j < 4; j++) H(i, j) = J(i, j);
39
    const Vec3<T> y = z - rot_inv(q, v);
40
    Mat<7, 1, T> dx;
41
    if (!joseph_update(P, H, Mat<3, 3, T>::identity() * r, y, dx)) return false;
42
    q = Quat<T>{q.w + dx(0, 0), q.x + dx(1, 0), q.y + dx(2, 0), q.z + dx(3, 0)}.normalized();
43
    b = b + Vec3<T>{{dx(4, 0), dx(5, 0), dx(6, 0)}};
44
    return true;
45
  }

C++ 版几乎是对着公式写的：F、Q 是 $7\times7$ 的 Mat，F * P * F.transpose() + Q 一行完成协方差预测。update_accel 和 update_mag 共用私有的 update，dx 用 Mat<7,1,T>，修正 q 的四个分量，再 normalized()。

Rust#

1
    pub fn predict(&mut self, gyro: [Real; 3], dt: Real) {
2
        let omega = [
3
            gyro[0] - self.b[0],
4
            gyro[1] - self.b[1],
5
            gyro[2] - self.b[2],
6
        ];
7
        let dq = Quat::exp([omega[0] * dt, omega[1] * dt, omega[2] * dt]);
8
        let xq = xi(self.q);
9
        let mut f = Mat::<7, 7>::identity();
10
        let mut q = Mat::<7, 7>::zeros();
11
        let rm = right_mul(dq);
12
        let qq = (xq * xq.transpose()) * ((0.5 * dt) * (0.5 * dt) * self.sg * self.sg);
13
        for i in 0..4 {
14
            for j in 0..4 {
15
                f.m[i][j] = rm.m[i][j];
16
                q.m[i][j] = qq.m[i][j];
17
            }
18
            for j in 0..3 {
19
                f.m[i][4 + j] = -0.5 * dt * xq.m[i][j];
20
            }
21
        }
22
        for i in 4..7 {
23
            q.m[i][i] = self.sbw * self.sbw * dt;
24
        }
25
        self.q = self.q * dq;
26
        self.p = f * self.p * f.transpose() + q;
27
    }
28

29
    /// Returns false (state unchanged) if S is not positive definite.
30
    fn update(&mut self, z: [Real; 3], v: [Real; 3], r: Real) -> bool {
31
        let mut h = Mat::<3, 7>::zeros();
32
        let hq = h_jac(self.q, v);
33
        for i in 0..3 {
34
            h.m[i][..4].copy_from_slice(&hq.m[i]);
35
        }
36
        let pred = rot_inv(self.q, v);
37
        let y = Mat {
38
            m: [[z[0] - pred[0]], [z[1] - pred[1]], [z[2] - pred[2]]],
39
        };
40
        let Some((dx, _)) = joseph_update(&mut self.p, &h, &(Mat::<3, 3>::identity() * r), &y)
41
        else {
42
            return false;
43
        };
44
        self.q = Quat {
45
            w: self.q.w + dx.m[0][0],
46
            x: self.q.x + dx.m[1][0],
47
            y: self.q.y + dx.m[2][0],
48
            z: self.q.z + dx.m[3][0],
49
        }
50
        .normalized();
51
        for i in 0..3 {
52
            self.b[i] += dx.m[4 + i][0];
53
        }
54
        true
55
    }
56

57
    /// Gravity-direction update. Returns false when gated out or on Cholesky failure.
58
    pub fn update_accel(&mut self, accel: [Real; 3]) -> bool {
59
        let norm = sqrt(dot(accel, accel));
60
        if self.accel_gate >= 0.0 && fabs(norm - G) > self.accel_gate {
61
            return false;
62
        }
63
        let z = [accel[0] / norm, accel[1] / norm, accel[2] / norm];
64
        self.update(z, [0.0, 0.0, -1.0], (self.sa / G) * (self.sa / G))
65
    }
66

67
    /// Returns false on Cholesky failure.
68
    pub fn update_mag(&mut self, mag: [Real; 3]) -> bool {
69
        let n = sqrt(dot(mag, mag));
70
        self.update(
71
            [mag[0] / n, mag[1] / n, mag[2] / n],
72
            self.m_w,
73
            self.sm * self.sm,
74
        )
75
    }

Rust 版里 q 是 Mat<7,7> 的局部变量名和 self.q（Quat）并存，读起来有点绕，这是我给自己挖的坑。let Some((dx, _)) = joseph_update(...) else { return false; }; 是 let-else：失败分支必须发散（这里是提前返回），所以 update 不会在 $S$ 不正定时悄悄修改 self.p，因为 joseph_update 在 Cholesky 失败时不动 p。

完整源码：c/include/kl/kl_ekf_att.h、c/src/kl_ekf_att.c

1
/* Multiplicative-quaternion EKF: state x = [q (4), b (3)], input = gyro.
2
 * Quaternion conventions: see kl_quat.h. All storage is inline; nothing is allocated. */
3
#ifndef KL_EKF_ATT_H
4
#define KL_EKF_ATT_H
5

6
#include "kl/kl_real.h"
7

8
#define KL_GRAVITY 9.80665
9

10
/* C(q)^T v as a polynomial in q: (w^2 - u.u) v + 2 (u.v) u - 2 w (u x v).
11
 * Equals the DCM transpose for unit q and is defined (and differentiable) for any q. */
12
void kl_ekf_att_rot_inv(const kl_real q[4], const kl_real v[3], kl_real out[3]);
13
/* d rot_inv(q, v) / d q, 3x4 row-major [d/dw | d/du] */
14
void kl_ekf_att_h_jac(const kl_real q[4], const kl_real v[3], kl_real J[12]);
15
/* 4x3 row-major matrix with q (x) [0, omega] = xi(q) omega */
16
void kl_ekf_att_xi(const kl_real q[4], kl_real Xi[12]);
17
/* 4x4 row-major matrix with q (x) p = right_mul(p) q */
18
void kl_ekf_att_right_mul(const kl_real p[4], kl_real M[16]);
19

20
typedef struct {
21
  kl_real q[4], b[3], P[49];
22
  kl_real sg, sbw, sa, sm; /* gyro, bias random walk, accel, mag noise */
23
  kl_real m_w[3];
24
  kl_real accel_gate; /* m/s^2 on | |f| - g |; negative disables gating */
25
  kl_real work[280];  /* scratch: predict needs 196, accel/mag updates 30 + KL_JOSEPH_WORK_LEN(7, 3) = 279 */
26
} kl_ekf_att;
27

28
/* p0_diag: initial diagonal of P (7). The bias starts at zero. */
29
void kl_ekf_att_init(kl_ekf_att *f, const kl_real q0[4], const kl_real p0_diag[7], kl_real sigma_g,
30
         kl_real sigma_bw, kl_real sigma_a, kl_real sigma_m, const kl_real m_w[3],
31
         kl_real accel_gate);
32
void kl_ekf_att_predict(kl_ekf_att *f, const kl_real gyro[3], kl_real dt);
33
/* Return 1 if applied, 0 if gated out, -1 on Cholesky failure (state unchanged). */
34
int kl_ekf_att_update_accel(kl_ekf_att *f, const kl_real accel[3]);
35
/* Return 1 if applied, -1 on Cholesky failure (state unchanged). */
36
int kl_ekf_att_update_mag(kl_ekf_att *f, const kl_real mag[3]);
37

38
#endif

1
#include "kl/kl_ekf_att.h"
2

3
#include <stddef.h>
4

5
#include "kl/kl_mat.h"
6
#include "kl/kl_quat.h"
7
#include "kl/kl_update.h"
8

9
void kl_ekf_att_rot_inv(const kl_real q[4], const kl_real v[3], kl_real out[3])
10
{
11
  kl_real w = q[0], u[3] = {q[1], q[2], q[3]};
12
  kl_real uu = u[0] * u[0] + u[1] * u[1] + u[2] * u[2];
13
  kl_real uv = u[0] * v[0] + u[1] * v[1] + u[2] * v[2];
14
  kl_real c[3] = {u[1] * v[2] - u[2] * v[1], u[2] * v[0] - u[0] * v[2], u[0] * v[1] - u[1] * v[0]};
15
  for (int i = 0; i < 3; i++)
16
    out[i] = (w * w - uu) * v[i] + 2 * uv * u[i] - 2 * w * c[i];
17
}
18

19
void kl_ekf_att_h_jac(const kl_real q[4], const kl_real v[3], kl_real J[12])
20
{
21
  kl_real w = q[0], u[3] = {q[1], q[2], q[3]};
22
  kl_real uv = u[0] * v[0] + u[1] * v[1] + u[2] * v[2];
23
  kl_real c[3] = {u[1] * v[2] - u[2] * v[1], u[2] * v[0] - u[0] * v[2], u[0] * v[1] - u[1] * v[0]};
24
  kl_real S[9];
25
  kl_skew(v, S);
26
  for (int i = 0; i < 3; i++) {
27
    J[i * 4] = 2 * w * v[i] - 2 * c[i];
28
    for (int j = 0; j < 3; j++)
29
      J[i * 4 + 1 + j] = 2 * uv * (i == j) + 2 * u[i] * v[j] - 2 * v[i] * u[j] +
30
             2 * w * S[i * 3 + j];
31
  }
32
}
33

34
void kl_ekf_att_xi(const kl_real q[4], kl_real Xi[12])
35
{
36
  kl_real w = q[0], x = q[1], y = q[2], z = q[3];
37
  const kl_real X[12] = {-x, -y, -z, w, -z, y, z, w, -x, -y, x, w};
38
  kl_mat_copy(X, Xi, 4, 3);
39
}
40

41
void kl_ekf_att_right_mul(const kl_real p[4], kl_real M[16])
42
{
43
  kl_real pw = p[0], px = p[1], py = p[2], pz = p[3];
44
  const kl_real R[16] = {pw, -px, -py, -pz, px, pw, pz, -py, py, -pz, pw, px, pz, py, -px, pw};
45
  kl_mat_copy(R, M, 4, 4);
46
}
47

48
void kl_ekf_att_init(kl_ekf_att *f, const kl_real q0[4], const kl_real p0_diag[7], kl_real sigma_g,
49
         kl_real sigma_bw, kl_real sigma_a, kl_real sigma_m, const kl_real m_w[3],
50
         kl_real accel_gate)
51
{
52
  kl_mat_copy(q0, f->q, 4, 1);
53
  kl_mat_copy(m_w, f->m_w, 3, 1);
54
  for (int i = 0; i < 3; i++)
55
    f->b[i] = 0;
56
  for (int i = 0; i < 49; i++)
57
    f->P[i] = 0;
58
  for (int i = 0; i < 7; i++)
59
    f->P[i * 8] = p0_diag[i];
60
  f->sg = sigma_g;
61
  f->sbw = sigma_bw;
62
  f->sa = sigma_a;
63
  f->sm = sigma_m;
64
  f->accel_gate = accel_gate;
65
}
66

67
void kl_ekf_att_predict(kl_ekf_att *f, const kl_real gyro[3], kl_real dt)
68
{
69
  kl_real *F = f->work, *Q = F + 49, *T = Q + 49, *U = T + 49;
70
  kl_real omega[3], dq[4], X[12], Rm[16], Xs[12], qn[4];
71
  for (int i = 0; i < 3; i++)
72
    omega[i] = (gyro[i] - f->b[i]) * dt;
73
  kl_quat_exp(omega, dq);
74
  kl_ekf_att_xi(f->q, X);
75
  kl_ekf_att_right_mul(dq, Rm);
76

77
  /* F = [[right_mul(dq), -dt/2 Xi], [0, I]] */
78
  kl_mat_identity(F, 7);
79
  for (int i = 0; i < 4; i++) {
80
    for (int j = 0; j < 4; j++)
81
      F[i * 7 + j] = Rm[i * 4 + j];
82
    for (int j = 0; j < 3; j++)
83
      F[i * 7 + 4 + j] = -(kl_real)0.5 * dt * X[i * 3 + j];
84
  }
85
  /* Q = blkdiag((dt/2)^2 sg^2 Xi Xi^T, sbw^2 dt I) */
86
  for (int i = 0; i < 49; i++)
87
    Q[i] = 0;
88
  kl_mat_scale(X, ((kl_real)0.5 * dt) * ((kl_real)0.5 * dt) * f->sg * f->sg, Xs, 4, 3);
89
  kl_mat_mul_bt(Xs, X, Rm, 4, 3, 4);
90
  for (int i = 0; i < 4; i++)
91
    for (int j = 0; j < 4; j++)
92
      Q[i * 7 + j] = Rm[i * 4 + j];
93
  for (int i = 4; i < 7; i++)
94
    Q[i * 8] = f->sbw * f->sbw * dt;
95

96
  kl_quat_mul(f->q, dq, qn);
97
  kl_mat_copy(qn, f->q, 4, 1);
98
  kl_mat_mul(F, f->P, T, 7, 7, 7);
99
  kl_mat_mul_bt(T, F, U, 7, 7, 7);
100
  kl_mat_add(U, Q, f->P, 7, 7);
101
}
102

103
/* H = [dh/dq | 0], y = z - h(q); the correction dx is added to the state. */
104
static int update(kl_ekf_att *f, const kl_real z[3], const kl_real v[3], kl_real r)
105
{
106
  kl_real *H = f->work, *R = H + 21, *jw = R + 9;
107
  kl_real Hq[12], hq[3], y[3], dx[7], qn[4];
108
  kl_ekf_att_h_jac(f->q, v, Hq);
109
  for (int i = 0; i < 3; i++)
110
    for (int j = 0; j < 7; j++)
111
      H[i * 7 + j] = j < 4 ? Hq[i * 4 + j] : 0;
112
  kl_ekf_att_rot_inv(f->q, v, hq);
113
  for (int i = 0; i < 3; i++) {
114
    y[i] = z[i] - hq[i];
115
    for (int j = 0; j < 3; j++)
116
      R[i * 3 + j] = (i == j) ? r : 0;
117
  }
118
  if (kl_joseph_update(f->P, H, R, y, 7, 3, dx, NULL, jw) != 0)
119
    return -1;
120
  for (int i = 0; i < 4; i++)
121
    qn[i] = f->q[i] + dx[i];
122
  kl_quat_normalize(qn);
123
  kl_mat_copy(qn, f->q, 4, 1);
124
  for (int i = 0; i < 3; i++)
125
    f->b[i] += dx[4 + i];
126
  return 1;
127
}
128

129
static void unit(const kl_real *v, kl_real *u, kl_real *norm)
130
{
131
  *norm = KL_SQRT(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
132
  for (int i = 0; i < 3; i++)
133
    u[i] = v[i] / *norm;
134
}
135

136
int kl_ekf_att_update_accel(kl_ekf_att *f, const kl_real accel[3])
137
{
138
  kl_real z[3], norm;
139
  const kl_real down[3] = {0, 0, -1};
140
  unit(accel, z, &norm);
141
  if (f->accel_gate >= 0 && KL_FABS(norm - (kl_real)KL_GRAVITY) > f->accel_gate)
142
    return 0;
143
  return update(f, z, down, (f->sa / (kl_real)KL_GRAVITY) * (f->sa / (kl_real)KL_GRAVITY));
144
}
145

146
int kl_ekf_att_update_mag(kl_ekf_att *f, const kl_real mag[3])
147
{
148
  kl_real z[3], norm;
149
  unit(mag, z, &norm);
150
  return update(f, z, f->m_w, f->sm * f->sm);
151
}

完整源码：cpp/include/kl/ekf_attitude.hpp

1
// Multiplicative-quaternion EKF: state x = [q (4), b (3)], input = gyro.
2
// Quaternion conventions as in quat.hpp. P is 7x7 over the raw quaternion
3
// components and the bias; q is renormalized after every update.
4
#pragma once
5
#include "kl/quat.hpp"
6
#include "kl/update.hpp"
7

8
namespace kl {
9

10
constexpr double GRAVITY = 9.80665;
11

12
// C(q)^T v written as a polynomial in q: (w^2 - u.u) v + 2 (u.v) u - 2 w (u x v).
13
// Equals q.dcm().transpose() * v for unit q, and is defined for any q.
14
template <class T>
15
Vec3<T> rot_inv(const Quat<T>& q, const Vec3<T>& v) {
16
  const Vec3<T> u{{q.x, q.y, q.z}};
17
  return v * (q.w * q.w - dot(u, u)) + u * (2 * dot(u, v)) - cross(u, v) * (2 * q.w);
18
}
19

20
// d rot_inv(q, v) / d q, a 3x4 matrix [d/dw | d/du].
21
template <class T>
22
Mat<3, 4, T> h_jac(const Quat<T>& q, const Vec3<T>& v) {
23
  const Vec3<T> u{{q.x, q.y, q.z}};
24
  const Vec3<T> dw = v * (2 * q.w) - cross(u, v) * 2;
25
  Mat<3, 3, T> du = Mat<3, 3, T>::identity() * (2 * dot(u, v)) + skew(v) * (2 * q.w);
26
  for (std::size_t i = 0; i < 3; i++)
27
    for (std::size_t j = 0; j < 3; j++) du(i, j) += 2 * u(i, 0) * v(j, 0) - 2 * v(i, 0) * u(j, 0);
28
  Mat<3, 4, T> J{};
29
  for (std::size_t i = 0; i < 3; i++) {
30
    J(i, 0) = dw(i, 0);
31
    for (std::size_t j = 0; j < 3; j++) J(i, j + 1) = du(i, j);
32
  }
33
  return J;
34
}
35

36
// 4x3 matrix with q (x) [0, omega] = xi(q) omega.
37
template <class T>
38
constexpr Mat<4, 3, T> xi(const Quat<T>& q) {
39
  return Mat<4, 3, T>{{-q.x, -q.y, -q.z, q.w, -q.z, q.y, q.z, q.w, -q.x, -q.y, q.x, q.w}};
40
}
41

42
// 4x4 matrix with q (x) p = right_mul(p) q.
43
template <class T>
44
constexpr Mat<4, 4, T> right_mul(const Quat<T>& p) {
45
  return Mat<4, 4, T>{{p.w, -p.x, -p.y, -p.z, p.x, p.w, p.z, -p.y,
46
           p.y, -p.z, p.w, p.x, p.z, p.y, -p.x, p.w}};
47
}
48

49
template <class T = double>
50
class QuatEKF {
51
public:
52
  Quat<T> q;
53
  Vec3<T> b{};
54
  Mat<7, 7, T> P{};
55
  T sg, sbw, sa, sm;
56
  Vec3<T> m_w;
57
  T accel_gate;  // m/s^2 on | |f| - g |; negative disables gating
58

59
  QuatEKF(T sigma_g, T sigma_bw, T sigma_a, T sigma_m, const Vec3<T>& m_w, const Quat<T>& q0,
60
    const Mat<7, 7, T>& P0, T accel_gate = T(0.5))
61
      : q(q0), P(P0), sg(sigma_g), sbw(sigma_bw), sa(sigma_a), sm(sigma_m), m_w(m_w),
62
        accel_gate(accel_gate) {}
63

64
  void predict(const Vec3<T>& gyro, T dt) {
65
    const Quat<T> dq = Quat<T>::exp((gyro - b) * dt);
66
    const Mat<4, 3, T> X = xi(q);
67
    Mat<7, 7, T> F = Mat<7, 7, T>::identity();
68
    Mat<7, 7, T> Q{};
69
    const Mat<4, 4, T> Qq = X * X.transpose() * ((T(0.5) * dt) * (T(0.5) * dt) * sg * sg);
70
    const Mat<4, 4, T> Fq = right_mul(dq);
71
    for (std::size_t i = 0; i < 4; i++) {
72
      for (std::size_t j = 0; j < 4; j++) {
73
        F(i, j) = Fq(i, j);
74
        Q(i, j) = Qq(i, j);
75
      }
76
      for (std::size_t j = 0; j < 3; j++) F(i, 4 + j) = -T(0.5) * dt * X(i, j);
77
    }
78
    for (std::size_t i = 4; i < 7; i++) Q(i, i) = sbw * sbw * dt;
79
    q = q * dq;  // not normalized here
80
    P = F * P * F.transpose() + Q;
81
  }
82

83
  // Gravity-direction update. Returns false when gated out or when S is not
84
  // positive definite (state unchanged in both cases).
85
  bool update_accel(const Vec3<T>& accel) {
86
    const T norm = std::sqrt(dot(accel, accel));
87
    if (accel_gate >= 0 && std::fabs(norm - T(GRAVITY)) > accel_gate) return false;
88
    return update(accel * (1 / norm), Vec3<T>{{0, 0, -1}}, (sa / T(GRAVITY)) * (sa / T(GRAVITY)));
89
  }
90

91
  // Magnetic-field update; false if S is not positive definite (state unchanged).
92
  bool update_mag(const Vec3<T>& mag) {
93
    return update(mag * (1 / std::sqrt(dot(mag, mag))), m_w, sm * sm);
94
  }
95

96
private:
97
  bool update(const Vec3<T>& z, const Vec3<T>& v, T r) {
98
    Mat<3, 7, T> H{};
99
    const Mat<3, 4, T> J = h_jac(q, v);
100
    for (std::size_t i = 0; i < 3; i++)
101
      for (std::size_t j = 0; j < 4; j++) H(i, j) = J(i, j);
102
    const Vec3<T> y = z - rot_inv(q, v);
103
    Mat<7, 1, T> dx;
104
    if (!joseph_update(P, H, Mat<3, 3, T>::identity() * r, y, dx)) return false;
105
    q = Quat<T>{q.w + dx(0, 0), q.x + dx(1, 0), q.y + dx(2, 0), q.z + dx(3, 0)}.normalized();
106
    b = b + Vec3<T>{{dx(4, 0), dx(5, 0), dx(6, 0)}};
107
    return true;
108
  }
109
};
110

111
}  // namespace kl

完整源码：rust/src/ekf_att.rs

1
//! Multiplicative-quaternion EKF: state x = [q (4), b (3)], input = gyro.
2

3
use crate::mat::Mat;
4
use crate::quat::{Quat, skew};
5
use crate::real::{Real, fabs, sqrt};
6
use crate::update::joseph_update;
7

8
/// Standard gravity (m/s^2).
9
pub const G: Real = 9.80665;
10

11
fn cross(a: [Real; 3], b: [Real; 3]) -> [Real; 3] {
12
    [
13
        a[1] * b[2] - a[2] * b[1],
14
        a[2] * b[0] - a[0] * b[2],
15
        a[0] * b[1] - a[1] * b[0],
16
    ]
17
}
18

19
fn dot(a: [Real; 3], b: [Real; 3]) -> Real {
20
    a[0] * b[0] + a[1] * b[1] + a[2] * b[2]
21
}
22

23
/// C(q)^T v as a polynomial in q: (w^2 - u.u) v + 2 (u.v) u - 2 w (u x v).
24
/// Equal to dcm(q)^T v for unit q, and defined (and differentiable) for any q.
25
pub fn rot_inv(q: Quat, v: [Real; 3]) -> [Real; 3] {
26
    let u = [q.x, q.y, q.z];
27
    let c = cross(u, v);
28
    let (a, b) = (q.w * q.w - dot(u, u), 2.0 * dot(u, v));
29
    [
30
        a * v[0] + b * u[0] - 2.0 * q.w * c[0],
31
        a * v[1] + b * u[1] - 2.0 * q.w * c[1],
32
        a * v[2] + b * u[2] - 2.0 * q.w * c[2],
33
    ]
34
}
35

36
/// d rot_inv(q, v) / d q, a 3x4 matrix [d/dw | d/du].
37
pub fn h_jac(q: Quat, v: [Real; 3]) -> Mat<3, 4> {
38
    let u = [q.x, q.y, q.z];
39
    let c = cross(u, v);
40
    let uv = dot(u, v);
41
    let sv = skew(v);
42
    let mut j = Mat::zeros();
43
    for i in 0..3 {
44
        j.m[i][0] = 2.0 * q.w * v[i] - 2.0 * c[i];
45
        for k in 0..3 {
46
            let id = if i == k { 2.0 * uv } else { 0.0 };
47
            j.m[i][k + 1] = id + 2.0 * u[i] * v[k] - 2.0 * v[i] * u[k] + 2.0 * q.w * sv.m[i][k];
48
        }
49
    }
50
    j
51
}
52

53
/// 4x3 matrix with q (x) [0, omega] = xi(q) omega.
54
pub fn xi(q: Quat) -> Mat<4, 3> {
55
    let Quat { w, x, y, z } = q;
56
    Mat {
57
        m: [[-x, -y, -z], [w, -z, y], [z, w, -x], [-y, x, w]],
58
    }
59
}
60

61
/// 4x4 matrix with q (x) p = right_mul(p) q.
62
pub fn right_mul(p: Quat) -> Mat<4, 4> {
63
    let Quat { w, x, y, z } = p;
64
    Mat {
65
        m: [[w, -x, -y, -z], [x, w, z, -y], [y, -z, w, x], [z, y, -x, w]],
66
    }
67
}
68

69
pub struct QuatEkf {
70
    pub q: Quat,
71
    pub b: [Real; 3],
72
    pub p: Mat<7, 7>,
73
    sg: Real,
74
    sbw: Real,
75
    sa: Real,
76
    sm: Real,
77
    m_w: [Real; 3],
78
    /// Accel gate on | |f| - g | in m/s^2; negative disables gating.
79
    accel_gate: Real,
80
}
81

82
impl QuatEkf {
83
    #[allow(clippy::too_many_arguments)]
84
    pub fn new(
85
        q0: Quat,
86
        p0_diag: [Real; 7],
87
        sigma_g: Real,
88
        sigma_bw: Real,
89
        sigma_a: Real,
90
        sigma_m: Real,
91
        m_w: [Real; 3],
92
        accel_gate: Real,
93
    ) -> Self {
94
        let mut p = Mat::zeros();
95
        for (i, d) in p0_diag.iter().enumerate() {
96
            p.m[i][i] = *d;
97
        }
98
        Self {
99
            q: q0,
100
            b: [0.0; 3],
101
            p,
102
            sg: sigma_g,
103
            sbw: sigma_bw,
104
            sa: sigma_a,
105
            sm: sigma_m,
106
            m_w,
107
            accel_gate,
108
        }
109
    }
110

111
    pub fn predict(&mut self, gyro: [Real; 3], dt: Real) {
112
        let omega = [
113
            gyro[0] - self.b[0],
114
            gyro[1] - self.b[1],
115
            gyro[2] - self.b[2],
116
        ];
117
        let dq = Quat::exp([omega[0] * dt, omega[1] * dt, omega[2] * dt]);
118
        let xq = xi(self.q);
119
        let mut f = Mat::<7, 7>::identity();
120
        let mut q = Mat::<7, 7>::zeros();
121
        let rm = right_mul(dq);
122
        let qq = (xq * xq.transpose()) * ((0.5 * dt) * (0.5 * dt) * self.sg * self.sg);
123
        for i in 0..4 {
124
            for j in 0..4 {
125
                f.m[i][j] = rm.m[i][j];
126
                q.m[i][j] = qq.m[i][j];
127
            }
128
            for j in 0..3 {
129
                f.m[i][4 + j] = -0.5 * dt * xq.m[i][j];
130
            }
131
        }
132
        for i in 4..7 {
133
            q.m[i][i] = self.sbw * self.sbw * dt;
134
        }
135
        self.q = self.q * dq;
136
        self.p = f * self.p * f.transpose() + q;
137
    }
138

139
    /// Returns false (state unchanged) if S is not positive definite.
140
    fn update(&mut self, z: [Real; 3], v: [Real; 3], r: Real) -> bool {
141
        let mut h = Mat::<3, 7>::zeros();
142
        let hq = h_jac(self.q, v);
143
        for i in 0..3 {
144
            h.m[i][..4].copy_from_slice(&hq.m[i]);
145
        }
146
        let pred = rot_inv(self.q, v);
147
        let y = Mat {
148
            m: [[z[0] - pred[0]], [z[1] - pred[1]], [z[2] - pred[2]]],
149
        };
150
        let Some((dx, _)) = joseph_update(&mut self.p, &h, &(Mat::<3, 3>::identity() * r), &y)
151
        else {
152
            return false;
153
        };
154
        self.q = Quat {
155
            w: self.q.w + dx.m[0][0],
156
            x: self.q.x + dx.m[1][0],
157
            y: self.q.y + dx.m[2][0],
158
            z: self.q.z + dx.m[3][0],
159
        }
160
        .normalized();
161
        for i in 0..3 {
162
            self.b[i] += dx.m[4 + i][0];
163
        }
164
        true
165
    }
166

167
    /// Gravity-direction update. Returns false when gated out or on Cholesky failure.
168
    pub fn update_accel(&mut self, accel: [Real; 3]) -> bool {
169
        let norm = sqrt(dot(accel, accel));
170
        if self.accel_gate >= 0.0 && fabs(norm - G) > self.accel_gate {
171
            return false;
172
        }
173
        let z = [accel[0] / norm, accel[1] / norm, accel[2] / norm];
174
        self.update(z, [0.0, 0.0, -1.0], (self.sa / G) * (self.sa / G))
175
    }
176

177
    /// Returns false on Cholesky failure.
178
    pub fn update_mag(&mut self, mag: [Real; 3]) -> bool {
179
        let n = sqrt(dot(mag, mag));
180
        self.update(
181
            [mag[0] / n, mag[1] / n, mag[2] / n],
182
            self.m_w,
183
            self.sm * self.sm,
184
        )
185
    }
186
}

结果#

1
$ make -s ch05
2
Jacobian checks: max|H - H_fd| = 2.25e-10, max|F_qb - F_qb_fd| = 2.22e-05
3
Jacobian checks: max|H - H_fd| = 2.25e-10, max|F_qb - F_qb_fd| = 2.22e-05
4
jacobian check: max|H - H_fd| = 2.249e-10, max|F_qb - F_qb_fd| = 2.216e-05
5
Jacobian checks: max|H - H_fd| = 2.25e-10, max|F_qb - F_qb_fd| = 2.22e-5
6
ok   ekf_c.csv: max diff 9.992e-16 <= 1.0e-09
7
ok   ekf_cpp.csv: max diff 1.221e-15 <= 1.0e-09
8
ok   ekf_rs.csv: max diff 1.665e-15 <= 1.0e-09
9

10
RMSE over 120 s (deg), seed 1
11
method        yaw    pitch     roll    total
12
mahony      4.390    1.489    2.250    4.881
13
ekf         4.504    0.684    1.127    4.719
14

15
Total error by period (deg RMS)
16
                     mahony      ekf
17
calm  [20,60) s       0.758    0.086
18
accel [60,70) s       2.331    2.640
19
mag   [90,100) s     14.851   14.820
20
roll/pitch RMS in [90,100): mahony 8.437, ekf 3.214
21
peak roll/pitch error in [90,100): mahony 22.52, ekf 8.78 deg

三个语言的雅可比检查打出同样的数： $\max|H-H_{fd}| = 2.25\times10^{-10}$ ， $\max|F_{qb}-F_{qb,fd}| = 2.2\times10^{-5}$ 。滤波输出和 Python 的最大差异都在 $2\times10^{-15}$ 以下。

对比 Mahony#

平静段 [20, 60) 秒，EKF 的总误差是 0.086 度，Mahony 是 0.758 度。这是卡尔曼滤波器的价值：它根据陀螺和加速度计各自的噪声水平自己算出增益，不需要我选 $k_p$ 、 $k_i$ 。整段 RMSE 4.719 对 4.881 度的小差距，来自磁干扰窗口：两个滤波器在那 10 秒里的总误差都是 14.8 度，因为磁力计的数据是错的，它们都相信了。

但是细看磁干扰窗口里的横滚和俯仰，EKF 的 RMS 是 3.214 度，Mahony 是 8.437 度；峰值 8.78 度对 22.52 度。EKF 的磁力计更新对横滚俯仰的污染比 Mahony 轻得多，原因我猜是它知道加速度计的置信度很高（ $P$ 在横滚俯仰方向上很小），磁力计的修正主要落在偏航上；Mahony 对两个传感器的权重固定。偏航方向它俩都没有办法，那是 14.8 度总误差的主要来源。

EKF 与 Mahony 的总姿态误差；红色阴影为线加速度扰动，紫色阴影为磁干扰

磁干扰期间的俯仰和横滚误差：EKF（橙）比 Mahony（蓝）小得多，但也不是零

加速度计门控：我设的门限从来没触发#

线性加速度段 [60, 70) 秒，EKF 的总误差 RMS 是 2.640 度，比平静段的 0.086 度差 30 倍，比 Mahony 的 2.331 度还差一点。我的加速度计门控把 $\big||f_m|-g\big|>0.5$ m/s² 的样本排除。结果：

1
accel gating (|f| - g test)
2
 gate (m/s^2)  gated samples  [60,70) RMS  whole run RMS
3
          0.5              0        2.640          4.719
4
          0.1           1018        0.961          4.668
5
          off              0        2.640          4.719
6
max | |f| - g | over the run: 0.332 m/s^2 (horizontal acceleration a adds only about a^2/(2 g): 2^2/(2 g) = 0.204)

门限 0.5 一个样本都没有拦住，12000 个样本里 0 个。原因是几何：扰动是水平方向的， $|f| = \sqrt{g^2+a^2}\approx g+a^2/(2g)$ ， $a=2$ m/s² 时只多出 0.204 m/s²，加上噪声也只有 0.332 m/s²，远没到 0.5。比力的模长对水平加速度几乎是二阶不敏感的。门限收紧到 0.1 之后，1018 个样本被拦住，该段 RMS 从 2.640 降到 0.961 度，整段 4.719 降到 4.668 度。这 1018 个里，507 个落在扰动窗口内（窗口共 1000 个样本，拦住一半），511 个是窗口外 11000 个正常样本的误拦，误拦率 4.6%。

这件事我是跑完才发现的：门控”开着”和”没有门控”的曲线完全重合（下图的蓝线和橙色虚线）。一个检测器没有触发，不代表它在工作。 门限 0.1 能拦住，是因为它已经紧到接近噪声水平（单个样本的噪声就有 0.05 m/s²），所以才有上面 4.6% 的误拦，这是个典型的权衡，不是白拿的收益。用模长检测水平线加速度本来就不是个好办法；第 6 篇我仍然沿用 0.5 的门限（保持可比），用的是同一份数据，所以它在那里同样不会触发。

60 到 70 秒的总误差，三条曲线：不设门、门限 0.5、门限 0.1。前两条完全重合

零偏估计#

1
bias error after 30 s (rad/s): max 0.02121, RMS per axis [0.00411 0.00597 0.00573]
2
final bias: true [ 0.01953 -0.01146  0.01275], EKF [ 0.02066 -0.01307  0.01142]

平静段里零偏估计非常好，在真值附近几乎重合。两次扰动之后它飘了：线加速度窗口里零偏估计跟着加速度计的错误摆动，磁干扰窗口里更厉害，偏离真值最大 0.021 rad/s（和零偏本身一个量级）。道理一样：偏航被错误的磁力计数据拉偏了，滤波器把”偏航在漂”解释成”零偏变了”。扰动结束之后，估计慢慢回到真值附近（最后 0.002 以内），但那需要几十秒。

$4\times4$ 四元数协方差#

1
final 4x4 quaternion covariance block
2
  eigenvalues: [1.412e-09 6.411e-08 7.028e-08 2.728e-07]
3
  variance along q: 1.420e-09   (the manifold only has 3 directions)

最后一步 $P_{qq}$ 的四个特征值里有一个小得多： $1.4\times10^{-9}$ ，另外三个是 $6.4\times10^{-8}$ 到 $2.7\times10^{-7}$ ，差了 45 到 190 倍，而且这个最小特征值对应的方向就是 $q$ 自己（沿着 $q$ 的方差是 $1.420\times10^{-9}$ ）。物理上，四元数的长度根本不是一个待估计的量， $P$ 在这个方向上的数值没有意义；它小，是因为测量函数是 $q$ 的二次齐次函数，滤波器以为自己”测到了”长度。这个多出来的方向是个自由度的”幽灵”，它会吃掉信息，也让 $P$ 变得接近奇异（条件数大 190 倍），这是数值上的风险。

我没有证明这个幽灵会让滤波器变差，我只是不喜欢它。下一篇的方案是干脆不要在四元数上做加减：把状态拆成”名义的四元数”和”3 维的误差向量”，协方差只对 3 维误差定义。

下一篇#

EKF 的问题已经够明确了：4 维四元数的协方差不对，磁力计整个 3 维向量都参与更新所以会污染横滚俯仰，门控检测的方式有盲区。下一篇的误差状态卡尔曼滤波（ESKF）一次处理前两个：误差用 3 维旋转向量表示，协方差是 $6\times6$ 满秩；磁力计只用来更新偏航。它还多了一个 EKF 做不到的好处，我们会用一个 90 度的初始偏航误差来测试它。

音乐

音乐

数学#

非线性系统与线性化#

EKF 丢掉了什么#

过程模型#

测量模型#

测量雅可比#

归一化和协方差#

初值#

Python 实现#

共用的测量更新#

EKF#

C、C++、Rust 实现#

共用的测量更新#

C#

C++#

Rust#

结果#

对比 Mahony#

加速度计门控：我设的门限从来没触发#

零偏估计#

$4\times4$ 四元数协方差#

下一篇#

支持与分享

评论区

音乐

扩展卡尔曼滤波：把四元数塞进 EKF

数学#

非线性系统与线性化#

EKF 丢掉了什么#

过程模型#

测量模型#

测量雅可比#

归一化和协方差#

初值#

Python 实现#

共用的测量更新#

EKF#

C、C++、Rust 实现#

共用的测量更新#

C#

C++#

Rust#

结果#

对比 Mahony#

加速度计门控：我设的门限从来没触发#

零偏估计#

4×44\times44×4 四元数协方差#

下一篇#

支持与分享

评论区

$4\times4$ 四元数协方差#