无迹卡尔曼滤波，以及四个滤波器同台

先看一个玩具问题。把极坐标 $(r,\theta)$ 变成直角坐标 $(r\cos\theta,r\sin\theta)$ ， $r\sim\mathcal N(1,0.02^2)$ ， $\theta\sim\mathcal N(\pi/2,0.5^2)$ 。用一百万个样本算出来， $y$ 的均值是 0.8827。线性化给的答案是 1.0000，差了 0.117；无迹变换只用 5 个点，给的答案是 0.8801，差了 0.003。这就是这一篇的主角，它连一个雅可比也没求。

然后是更扫兴的部分：把它放进姿态滤波器之后，在 50 个种子的蒙特卡洛里，UKF 的总姿态误差是 0.929 度，和上一篇的 ESKF 的 0.929 度在三位小数里没有区别。我会把这件事说清楚，也会说它在哪里确实赢了。

本篇检查点

这一篇结束时你会有：make ch07 通过（它跑得比较久，大概 6 分钟，大部分时间花在 50 个种子的四滤波器蒙特卡洛上），out/ch07/ukf_{nominal,large}_{py,c,cpp,rs}.csv，三张图；C、C++、Rust 各一个误差状态 UKF（kl_ukf_att、ukf.hpp、ukf.rs）。每个语言的 app 先做一个 sigma 点的自检。前置：第 6 篇（误差状态、注入与重置、航向更新、门控）。

数学#

为什么传点而不是传函数#

EKF 的做法是把非线性函数 $f$ 换成它的一阶泰勒展开，再传播高斯分布。无迹变换（unscented transform，UT）反过来：保留真正的非线性函数，用一小撮精心选择的点来代表高斯分布，把点一个个过一遍 $f$ ，再用输出点的加权统计量近似输出分布的均值和协方差。

设 $x\sim(\mu,\Sigma)$ ， $n$ 维。选 $2n+1$ 个 sigma 点 $\mu+\chi_i$ ，权重 $W_i^m$ （均值用）和 $W_i^c$ （协方差用）。对称选法： $\chi_0 = 0$ ， $\chi_i = \pm$ 矩阵 $\sqrt{(n+\lambda)\Sigma}$ 的第 $i$ 列，使得

\sum_iW_i^m\chi_i = 0,\qquad \sum_iW_i^c\chi_i\chi_i^T\approx\Sigma

其中 $\sqrt{\cdot}$ 用 Cholesky 因子： $\sqrt{(n+\lambda)\Sigma} = L$ ， $LL^T = (n+\lambda)\Sigma$ 。

为什么它能抓到二阶项#

把 $f$ 在 $\mu$ 处展开到二阶：

f(\mu+\delta) = f(\mu) + J\delta + \tfrac12\sum_ke_k\,\delta^TH_k\delta+O(|\delta|^3)

$J$ 是雅可比， $H_k$ 是第 $k$ 个输出分量的海森矩阵， $e_k$ 是单位向量。真实的输出均值（ $\delta\sim\mathcal N(0,\Sigma)$ 对称，奇次项为零）：

E[f(x)] = f(\mu)+\tfrac12\sum_ke_k\operatorname{tr}(H_k\Sigma)+O(\text{4 阶})

线性化给 $f(\mu)$ ，漏掉了第二项。UT 的均值是

\sum_iW_i^mf(\mu+\chi_i) = f(\mu)\sum_iW^m_i + J\sum_iW_i^m\chi_i+\tfrac12\sum_ke_k\sum_iW_i^m\chi_i^TH_k\chi_i

权重和为 1，一次项因对称选点而为零，二次项用 $\sum_iW_i\chi_i\chi_i^T = \Sigma$ 得到 $\sum_iW_i\chi_i^TH_k\chi_i = \operatorname{tr}(H_k\Sigma)$ 。于是 UT 的均值和真实均值在二阶项上相同。协方差的一阶项 $J\Sigma J^T$ 也被精确抓住。四阶以后的项会有误差，参数 $\beta$ 就是用来在高斯情形下补偿四阶矩的。

玩具问题里， $y$ 的二阶项是 $\tfrac12\operatorname{tr}(H_y\Sigma) = -\tfrac12r\sin\theta\,\sigma_\theta^2 = -0.125$ ，对应线性化和真实均值的差（实测 0.1173，差别来自更高阶）。

缩放参数#

标准的缩放 UT 有三个参数 $\alpha,\beta,\kappa$ ， $\lambda = \alpha^2(n+\kappa)-n$ ：

W^m_0 = \frac{\lambda}{n+\lambda},\quad W^c_0 = W_0^m+(1-\alpha^2+\beta),\quad W^m_i = W^c_i = \frac1{2(n+\lambda)},\ i=1..2n

我取 $\alpha = 1$ ， $\beta = 2$ ， $\kappa = 0$ ，于是 $\lambda = 0$ ： $W_0^m = 0$ ， $W_0^c = 2$ ，其余 12 个点权重都是 $1/12$ ，全部是正数。常见的另一种选择是 $\alpha = 10^{-3}$ ，让 sigma 点贴近均值（局部性好）。但它的代价是 $\lambda\approx-n$ ， $n+\lambda\approx n\alpha^2$ 极小，中心点权重变成 $-n/(n\alpha^2)\approx-10^6$ ，其他点权重是 $+1/(2n\alpha^2)\approx8\times10^4$ ：几十万量级的正负权重相消，在 float32 里精度会丢得很难看。 $\alpha = 1$ 的代价是 sigma 点离均值 $\sqrt n\sigma$ 远，对于很强的非线性，这个”非局部”采样会有问题。折衷之下，我为了第 8 篇的 float32 选了 $\alpha = 1$ 。

加性噪声 UKF#

状态 $x$ ，过程 $x' = f(x)+w$ ，测量 $z = h(x)+v$ ，噪声加性。

预测： 从 $(\hat x,P)$ 生成 sigma 点 $\chi_i$ ； $y_i = f(\chi_i)$ ；

\hat x^- = \sum_iW_i^my_i,\qquad P^- = \sum_iW_i^c(y_i-\hat x^-)(y_i-\hat x^-)^T+Q

更新： 从 $(\hat x^-,P^-)$ 重新生成 sigma 点； $z_i = h(\chi_i)$ ；

\hat z = \sum_iW_i^mz_i,\quad S = \sum_iW_i^c(z_i-\hat z)(z_i-\hat z)^T+R,\quad P_{xz} = \sum_iW_i^c(\chi_i-\hat x^-)(z_i-\hat z)^T

K = P_{xz}S^{-1},\qquad\hat x = \hat x^-+K(z-\hat z),\qquad P = P^--KSK^T

最后一个等式： $KSK^T = P_{xz}S^{-1}P_{xz}^T$ 。检验： $h$ 是线性的 $h(x) = Hx$ 时， $z_i-\hat z = H(\chi_i-\hat x^-)$ ， $P_{xz} = P^-H^T$ ， $S = HP^-H^T+R$ ，UKF 恰好还原成第 2 篇的线性卡尔曼滤波。

四元数的均值#

怎么对旋转取”均值”？对四元数的四个分量直接取加权平均，不能：结果不是单位四元数，而且 $q$ 与 $-q$ 是同一个旋转， $q$ 和 $-q$ 的平均是零。解法就是第 6 篇的做法：在名义姿态的切空间里工作。

生成 sigma 点：用 6 维误差向量 $[\delta\theta_i,\delta b_i]$ 的 sigma 点，每个点对应一个姿态 $q_i = \hat q\otimes\exp(\delta\theta_i)$ 。
传播：每个点各自过一遍陀螺模型， $q_i\leftarrow q_i\otimes\exp((\omega_m-b_i)\Delta t)$ 。
取参考：把传播后的中心点 $q_{ref}$ 当参考，每个点相对参考的偏差 $\delta\theta_i = \log(q_{ref}^{-1}\otimes q_i)$ （就是第 4 篇的 att_error），这些偏差是普通向量，可以加权平均、求协方差。
回到流形： $\hat q^- = q_{ref}\otimes\exp(\bar{\delta\theta})$ 。

测量更新同样：用名义姿态周围的 sigma 点，对每个点算第 6 篇的加速度计模型（重力方向向量）和航向模型，其余是标准 UKF。注入、重置、门控、噪声参数与第 6 篇完全一样。

一个航向的小处理。 航向是个角度，有折叠问题。在 90 度初始误差下 sigma 点会分散到 $\sqrt6\cdot\pi/2 = 3.85$ rad（超过 $\pi$ ），朴素地对折叠后的角度加权平均会出错。我让每个点的航向相对中心点的航向取差（折到 $(-\pi,\pi]$ ），再加权平均，最后加回中心点的航向。

玩具问题的实现#

玩具问题只在 Python 里。

1
def toy():
2
    """Polar to Cartesian: true moments (Monte Carlo) vs linearization vs unscented transform."""
3
    from kl.plotting import L, save
4
    import matplotlib.pyplot as plt
5

6
    mu = np.array([1.0, np.pi / 2])
7
    S = np.diag([0.02**2, 0.5**2])
8
    f = lambda rt: np.column_stack([rt[:, 0] * np.cos(rt[:, 1]), rt[:, 0] * np.sin(rt[:, 1])])  # noqa: E731
9
    rng = np.random.default_rng(1)
10
    smp = rng.multivariate_normal(mu, S, 1_000_000)
11
    ys = f(smp)
12
    m_mc, c_mc = ys.mean(axis=0), np.cov(ys.T)
13
    J = np.array([[np.cos(mu[1]), -mu[0] * np.sin(mu[1])], [np.sin(mu[1]), mu[0] * np.cos(mu[1])]])
14
    m_lin, c_lin = f(mu[None])[0], J @ S @ J.T
15
    X, Wm, Wc = sigma_points(S)
16
    m_ut, c_ut = unscented_transform(f(mu + X), Wm, Wc)
17
    print("Polar to Cartesian: r ~ N(1, 0.02^2), theta ~ N(pi/2, 0.5^2)")
18
    print(f"{'':16s} {'mean x':>9s} {'mean y':>9s} {'var x':>10s} {'var y':>10s} {'cov xy':>10s}")
19
    for name, m, c in (("Monte Carlo 1e6", m_mc, c_mc), ("linearization", m_lin, c_lin), ("unscented", m_ut, c_ut)):
20
        print(f"{name:16s} {m[0]:9.4f} {m[1]:9.4f} {c[0, 0]:10.5f} {c[1, 1]:10.5f} {c[0, 1]:10.5f}")

sigma_points 和 unscented_transform 是后面姿态 UKF 共用的两个函数。

Python 实现#

1
"""Unscented transform and an error-state unscented Kalman filter for attitude."""
2

3
import numpy as np
4

5
from .imu_sim import G
6
from .quat import SMALL, qrot, skew, wrap_pi
7
from .update import joseph_update  # noqa: F401  (kept for symmetry with the other filters)
8

9

10
def sigma_points(P, alpha=1.0, beta=2.0, kappa=0.0):
11
    """Scaled sigma points around zero for N(0, P): returns (X, Wm, Wc).
12

13
    X has 2n+1 rows: the centre, then +-columns of chol((n + lambda) P).
14
    """
15
    n = P.shape[0]
16
    lam = alpha**2 * (n + kappa) - n
17
    L = np.linalg.cholesky((n + lam) * P)
18
    X = np.vstack([np.zeros(n), L.T, -L.T])
19
    Wm = np.full(2 * n + 1, 0.5 / (n + lam))
20
    Wc = Wm.copy()
21
    Wm[0] = lam / (n + lam)
22
    Wc[0] = Wm[0] + (1.0 - alpha**2 + beta)
23
    return X, Wm, Wc
24

25

26
def unscented_transform(Y, Wm, Wc, noise=None):
27
    """Weighted mean and covariance of transformed sigma points Y (rows)."""
28
    mean = Wm @ Y
29
    dev = Y - mean
30
    cov = (Wc[:, None] * dev).T @ dev
31
    return mean, cov if noise is None else cov + noise
32

33

34
# ---- row-wise quaternion helpers (same formulas as kl.quat) ----
35

36
def qexp_rows(V):
37
    th = np.sqrt(np.sum(V * V, axis=1))
38
    small = th < SMALL
39
    ths = np.where(small, 1.0, th)
40
    w = np.where(small, 1.0 - th * th / 8.0, np.cos(0.5 * th))
41
    s = np.where(small, 0.5, np.sin(0.5 * th) / ths)
42
    return np.column_stack([w, s[:, None] * V])
43

44

45
def qmul_rows(a, b):
46
    a, b = np.broadcast_arrays(np.atleast_2d(a), np.atleast_2d(b))
47
    aw, ax, ay, az = a.T
48
    bw, bx, by, bz = b.T
49
    return np.column_stack([
50
        aw * bw - ax * bx - ay * by - az * bz,
51
        aw * bx + ax * bw + ay * bz - az * by,
52
        aw * by - ax * bz + ay * bw + az * bx,
53
        aw * bz + ax * by - ay * bx + az * bw,
54
    ])
55

56

57
def qlog_rows(Q):
58
    Q = np.where((Q[:, 0] < 0)[:, None], -Q, Q)
59
    u = Q[:, 1:]
60
    n = np.sqrt(np.sum(u * u, axis=1))
61
    small = n < SMALL
62
    ns = np.where(small, 1.0, n)
63
    k = np.where(small, 2.0 / Q[:, 0], 2.0 * np.arctan2(n, Q[:, 0]) / ns)
64
    return k[:, None] * u
65

66

67
def rot_inv_rows(Q, v):
68
    """C(q_i)^T v for every row (unit q_i): (w^2 - u.u) v + 2 (u.v) u - 2 w (u x v)."""
69
    w, u = Q[:, :1], Q[:, 1:]
70
    return (w * w - np.sum(u * u, axis=1, keepdims=True)) * v + 2 * (u @ v)[:, None] * u - 2 * w * np.cross(u, v)
71

72

73
class AttitudeUKF:
74
    """Error-state UKF: same 6-dim error as the ESKF, additive noise, scaled unscented transform."""
75

76
    def __init__(self, sigma_g, sigma_bw, sigma_a, sigma_m, m_w, q0, P0,
77
                 accel_gate=0.5, mag_gate=0.1, reset=True, alpha=1.0, beta=2.0, kappa=0.0):
78
        self.sg, self.sbw, self.sa, self.sm = sigma_g, sigma_bw, sigma_a, sigma_m
79
        self.m_w = np.asarray(m_w, dtype=float)
80
        self.q = np.asarray(q0, dtype=float).copy()
81
        self.b = np.zeros(3)
82
        self.P = np.asarray(P0, dtype=float).copy()
83
        self.accel_gate, self.mag_gate, self.reset = accel_gate, mag_gate, reset
84
        self.ut = (alpha, beta, kappa)
85
        self.last_nis = np.nan
86

87
    def predict(self, gyro, dt):
88
        X, Wm, Wc = sigma_points(self.P, *self.ut)
89
        qi = qmul_rows(self.q, qexp_rows(X[:, :3]))
90
        bi = self.b + X[:, 3:]
91
        qi = qmul_rows(qi, qexp_rows((gyro - bi) * dt))  # each point through the gyro model
92
        q_ref = qi[0]  # the propagated centre point
93
        D = np.column_stack([qlog_rows(qmul_rows(q_ref * np.array([1, -1, -1, -1]), qi)), bi - self.b])
94
        mean, cov = unscented_transform(D, Wm, Wc)
95
        Q = np.diag([self.sg**2 * dt**2] * 3 + [self.sbw**2 * dt] * 3)
96
        self.q = qmul_rows(q_ref, qexp_rows(mean[None, :3]))[0]
97
        self.b = self.b + mean[3:]
98
        self.P = cov + Q
99

100
    def _inject(self, dx):
101
        from .quat import qexp, qmul, qnormalize
102

103
        dth = dx[:3]
104
        self.q = qnormalize(qmul(self.q, qexp(dth)))
105
        self.b = self.b + dx[3:]
106
        if self.reset:
107
            Gm = np.eye(6)
108
            Gm[:3, :3] -= skew(0.5 * dth)
109
            self.P = Gm @ self.P @ Gm.T
110

111
    def _correct(self, X, Wm, Wc, Z, y, R):
112
        """Standard UKF measurement update given model outputs Z at the sigma points."""
113
        zbar = Wm @ Z
114
        dz = Z - zbar
115
        S = (Wc[:, None] * dz).T @ dz + R
116
        Pxz = (Wc[:, None] * X).T @ dz
117
        L = np.linalg.cholesky(S)
118
        K = np.linalg.solve(L.T, np.linalg.solve(L, Pxz.T)).T
119
        dx = K @ y
120
        P_new = self.P - K @ S @ K.T
121
        self.P = 0.5 * (P_new + P_new.T)
122
        self.last_nis = float(y @ np.linalg.solve(S, y))
123
        self._inject(dx)
124

125
    def update_accel(self, accel):
126
        norm = np.sqrt(accel @ accel)
127
        if self.accel_gate is not None and abs(norm - G) > self.accel_gate:
128
            return False
129
        X, Wm, Wc = sigma_points(self.P, *self.ut)
130
        qi = qmul_rows(self.q, qexp_rows(X[:, :3]))
131
        Z = rot_inv_rows(qi, np.array([0.0, 0.0, -1.0]))
132
        y = accel / norm - Wm @ Z
133
        self._correct(X, Wm, Wc, Z, y, (self.sa / G) ** 2 * np.eye(3))
134
        return True
135

136
    def update_mag_heading(self, mag):
137
        norm = np.sqrt(mag @ mag)
138
        if self.mag_gate is not None and abs(norm - 1.0) > self.mag_gate:
139
            return False
140
        X, Wm, Wc = sigma_points(self.P, *self.ut)
141
        qi = qmul_rows(self.q, qexp_rows(X[:, :3]))
142
        C = qrot(self.q)
143
        mw = rot_inv_rows(qi, self.m_w) @ C.T  # C(q) C(q_i)^T m_w for every point
144
        psi_h = np.arctan2(self.m_w[1], self.m_w[0])
145
        Z0 = wrap_pi(np.arctan2(mw[:, 1], mw[:, 0]) - psi_h)
146
        Zrel = wrap_pi(Z0 - Z0[0])[:, None]  # heading relative to the centre point: no wrap trouble
147
        zbar = Z0[0] + Wm @ Zrel[:, 0]
148
        m_meas = C @ mag
149
        y = np.array([wrap_pi(wrap_pi(np.arctan2(m_meas[1], m_meas[0]) - psi_h) - zbar)])
150
        r = (self.sm / np.hypot(self.m_w[0], self.m_w[1])) ** 2
151
        self._correct(X, Wm, Wc, Zrel, y, np.array([[r]]))
152
        return True

sigma_points 返回 $13\times6$ 的点（每行一个）和两组权重。后面四个 *_rows 函数是第 4 篇四元数函数的逐行批量版本，公式一模一样，只是对 13 个点一次算完，因为 Python 里循环 13 次太慢。AttitudeUKF.predict 是上一节”四元数的均值”的四个步骤。_correct 是标准 UKF 更新，update_accel 和 update_mag_heading 只负责给出模型输出 Z 和新息 y。

C、C++、Rust 实现#

三种语言的结构和 Python 一致。区别：

C： sigma 点（ $13\times6$ ）、四元数数组、权重都是 kl_ukf_att 结构体里的定长数组，没有 malloc。kl_ukf_sigma_points 公开出来，因为 app 的自检要直接调它。
C++： SigmaPoints<N, T> 是个小模板结构体，权重在 compute 里算出来；参数 $\alpha,\beta,\kappa$ 是 constexpr 成员，编译期常量。
Rust： sigma_points(&P) 返回 Option<[[Real; 6]; 13]>，Cholesky 失败是 None，权重函数 unscented_weights() 单独暴露。

C++#

1
// Sigma points of N(0, P): the centre, then +-columns of chol((n + lambda) P).
2
template <std::size_t N, class T = double>
3
struct SigmaPoints {
4
  static constexpr std::size_t COUNT = 2 * N + 1;
5
  static constexpr T ALPHA = 1, BETA = 2, KAPPA = 0;
6
  static constexpr T LAMBDA = ALPHA * ALPHA * (N + KAPPA) - N;
7

8
  Mat<COUNT, N, T> X;  // one point per row
9
  std::array<T, COUNT> Wm, Wc;
10

11
  // Returns false if P is not positive definite.
12
  bool compute(const Mat<N, N, T>& P) {
13
    Mat<N, N, T> L = P * (N + LAMBDA);
14
    if (!cholesky(L)) return false;
15
    X = {};
16
    for (std::size_t j = 0; j < N; j++)
17
      for (std::size_t k = j; k < N; k++) {  // L is lower triangular
18
        X(1 + j, k) = L(k, j);
19
        X(1 + N + j, k) = -L(k, j);
20
      }
21
    Wm.fill(T(0.5) / (N + LAMBDA));
22
    Wc = Wm;
23
    Wm[0] = LAMBDA / (N + LAMBDA);
24
    Wc[0] = Wm[0] + (1 - ALPHA * ALPHA + BETA);
25
    return true;
26
  }
27
};

1
  // Returns false, leaving the state unchanged, if P is not positive definite.
2
  bool predict(const Vec3<T>& gyro, T dt) {
3
    SigmaPoints<N, T> sp;
4
    if (!sp.compute(P)) return false;
5
    std::array<Quat<T>, K> qi;
6
    std::array<Vec3<T>, K> bi;
7
    for (std::size_t i = 0; i < K; i++) {
8
      qi[i] = q * Quat<T>::exp(rows3(sp.X, i, 0));
9
      bi[i] = b + rows3(sp.X, i, 3);
10
      qi[i] = qi[i] * Quat<T>::exp((gyro - bi[i]) * dt);  // each point through the gyro model
11
    }
12
    const Quat<T> q_ref = qi[0];  // the propagated centre point
13
    Mat<K, 6, T> D;
14
    for (std::size_t i = 0; i < K; i++) {
15
      const Vec3<T> th = (q_ref.conj() * qi[i]).log(), db = bi[i] - b;
16
      for (std::size_t j = 0; j < 3; j++) {
17
        D(i, j) = th(j, 0);
18
        D(i, 3 + j) = db(j, 0);
19
      }
20
    }
21
    Mat<1, 6, T> mean{};
22
    for (std::size_t i = 0; i < K; i++)
23
      for (std::size_t j = 0; j < 6; j++) mean(0, j) += sp.Wm[i] * D(i, j);
24
    Mat<6, 6, T> cov{};
25
    for (std::size_t i = 0; i < K; i++)
26
      for (std::size_t r = 0; r < 6; r++)
27
        for (std::size_t c = 0; c < 6; c++)
28
          cov(r, c) += sp.Wc[i] * (D(i, r) - mean(0, r)) * (D(i, c) - mean(0, c));
29
    for (std::size_t i = 0; i < 3; i++) {
30
      cov(i, i) += sg * sg * dt * dt;
31
      cov(3 + i, 3 + i) += sbw * sbw * dt;
32
    }
33
    q = q_ref * Quat<T>::exp(Vec3<T>{{mean(0, 0), mean(0, 1), mean(0, 2)}});  // not normalized here
34
    b = b + Vec3<T>{{mean(0, 3), mean(0, 4), mean(0, 5)}};
35
    P = cov;
36
    return true;
37
  }

1
  // Heading-only magnetometer update. Returns false when gated out or on Cholesky failure.
2
  bool update_mag_heading(const Vec3<T>& mag) {
3
    const T norm = std::sqrt(dot(mag, mag));
4
    if (mag_gate >= 0 && std::fabs(norm - 1) > mag_gate) return false;
5
    SigmaPoints<N, T> sp;
6
    if (!sp.compute(P)) return false;
7
    const Mat<3, 3, T> C = q.dcm();
8
    const T psi_h = std::atan2(m_w(1, 0), m_w(0, 0));
9
    std::array<T, K> z0;
10
    for (std::size_t i = 0; i < K; i++) {
11
      const Vec3<T> mw = C * rot_inv(q * Quat<T>::exp(rows3(sp.X, i, 0)), m_w);
12
      z0[i] = wrap_pi(std::atan2(mw(1, 0), mw(0, 0)) - psi_h);
13
    }
14
    Mat<K, 1, T> Zrel;  // heading relative to the centre point: no wrap trouble
15
    T zbar = 0;
16
    for (std::size_t i = 0; i < K; i++) {
17
      Zrel(i, 0) = wrap_pi(z0[i] - z0[0]);
18
      zbar += sp.Wm[i] * Zrel(i, 0);
19
    }
20
    zbar += z0[0];
21
    const Vec3<T> m_meas = C * mag;
22
    const Mat<1, 1, T> y{{wrap_pi(wrap_pi(std::atan2(m_meas(1, 0), m_meas(0, 0)) - psi_h) - zbar)}};
23
    const T r = sm / std::hypot(m_w(0, 0), m_w(1, 0));
24
    return correct(sp, Zrel, y, Mat<1, 1, T>{{r * r}});
25
  }
26

27
private:
28
  template <std::size_t R, std::size_t C>
29
  static Vec3<T> rows3(const Mat<R, C, T>& X, std::size_t i, std::size_t c0) {
30
    return Vec3<T>{{X(i, c0), X(i, c0 + 1), X(i, c0 + 2)}};
31
  }
32

33
  // Standard UKF measurement update given the model outputs Z at the sigma points.
34
  // y is the innovation, already formed by the caller.
35
  template <std::size_t M>
36
  bool correct(const SigmaPoints<N, T>& sp, const Mat<K, M, T>& Z, const Mat<M, 1, T>& y,
37
         const Mat<M, M, T>& R) {
38
    Mat<1, M, T> zbar{};
39
    for (std::size_t i = 0; i < K; i++)
40
      for (std::size_t j = 0; j < M; j++) zbar(0, j) += sp.Wm[i] * Z(i, j);
41
    Mat<M, M, T> S = R;
42
    Mat<N, M, T> Pxz{};
43
    for (std::size_t i = 0; i < K; i++)
44
      for (std::size_t r = 0; r < M; r++) {
45
        const T dr = Z(i, r) - zbar(0, r);
46
        for (std::size_t c = 0; c < M; c++) S(r, c) += sp.Wc[i] * dr * (Z(i, c) - zbar(0, c));
47
        for (std::size_t k = 0; k < N; k++) Pxz(k, r) += sp.Wc[i] * sp.X(i, k) * dr;
48
      }
49
    Mat<M, M, T> L = S;
50
    if (!cholesky(L)) return false;
51
    const Mat<N, M, T> Kg = chol_solve(L, Pxz.transpose()).transpose();
52
    const Mat<N, 1, T> dx = Kg * y;
53
    P = (P - Kg * S * Kg.transpose()).symmetrized();
54
    inject(dx);
55
    return true;
56
  }

predict 里 q_ref.conj() * qi[i] 再 .log() 就是 $\delta\theta_i = \log(q_{ref}^{-1}\otimes q_i)$ ，是第 4 篇的四元数运算直接复用。correct 是个模板函数，M 由 Z 的列数推断：加速度计 $M=3$ ，航向 $M=1$ ，同一份代码。

C#

1
int kl_ukf_att_predict(kl_ukf_att *f, const kl_real gyro[3], kl_real dt)
2
{
3
  kl_real *bi = f->work, *D = bi + 3 * NP, *cov = D + N * NP;
4
  kl_real mean[N], dev[N], q_ref[4], qc[4], t[4], dq[4], qn[4];
5
  if (kl_ukf_sigma_points(f->P, f->X, f->Wm, f->Wc) != 0)
6
    return -1;
7
  point_attitudes(f);
8
  /* each point through the gyro model */
9
  for (int i = 0; i < NP; i++) {
10
    kl_real w[3];
11
    for (int k = 0; k < 3; k++) {
12
      bi[i * 3 + k] = f->b[k] + f->X[i * N + 3 + k];
13
      w[k] = (gyro[k] - bi[i * 3 + k]) * dt;
14
    }
15
    kl_quat_exp(w, dq);
16
    kl_quat_mul(&f->qi[i * 4], dq, t);
17
    kl_mat_copy(t, &f->qi[i * 4], 4, 1);
18
  }
19
  /* deviations from the propagated centre point */
20
  kl_mat_copy(f->qi, q_ref, 4, 1);
21
  kl_quat_conj(q_ref, qc);
22
  for (int i = 0; i < NP; i++) {
23
    kl_quat_mul(qc, &f->qi[i * 4], t);
24
    kl_quat_log(t, &D[i * N]);
25
    for (int k = 0; k < 3; k++)
26
      D[i * N + 3 + k] = bi[i * 3 + k] - f->b[k];
27
  }
28
  for (int k = 0; k < N; k++)
29
    mean[k] = 0;
30
  for (int i = 0; i < NP; i++)
31
    for (int k = 0; k < N; k++)
32
      mean[k] += f->Wm[i] * D[i * N + k];
33
  for (int i = 0; i < N * N; i++)
34
    cov[i] = 0;
35
  for (int i = 0; i < NP; i++) {
36
    for (int k = 0; k < N; k++)
37
      dev[k] = D[i * N + k] - mean[k];
38
    for (int a = 0; a < N; a++)
39
      for (int c = 0; c < N; c++)
40
        cov[a * N + c] += f->Wc[i] * dev[a] * dev[c];
41
  }
42
  kl_quat_exp(mean, dq);
43
  kl_quat_mul(q_ref, dq, qn);
44
  kl_mat_copy(qn, f->q, 4, 1);
45
  for (int k = 0; k < 3; k++) {
46
    f->b[k] += mean[3 + k];
47
    cov[k * 7] += f->sg * f->sg * dt * dt;
48
    cov[(k + 3) * 7] += f->sbw * f->sbw * dt;
49
  }
50
  kl_mat_copy(cov, f->P, N, N);
51
  return 0;
52
}
53

54
/* Move the error estimate into the nominal state, then reset the error to zero. */

1
static int correct(kl_ukf_att *f, int m, const kl_real *y, const kl_real *R)
2
{
3
  kl_real *dz = f->work, *S = dz + NP * m, *Pxz = S + m * m, *Kt = Pxz + N * m;
4
  kl_real *K = Kt + m * N, *KS = K + N * m, *KSKt = KS + N * m;
5
  kl_real zbar[3], dx[N];
6
  for (int k = 0; k < m; k++)
7
    zbar[k] = 0;
8
  for (int i = 0; i < NP; i++)
9
    for (int k = 0; k < m; k++)
10
      zbar[k] += f->Wm[i] * f->Z[i * m + k];
11
  for (int i = 0; i < m * m; i++)
12
    S[i] = R[i];
13
  for (int i = 0; i < N * m; i++)
14
    Pxz[i] = 0;
15
  for (int i = 0; i < NP; i++) {
16
    for (int k = 0; k < m; k++)
17
      dz[i * m + k] = f->Z[i * m + k] - zbar[k];
18
    for (int a = 0; a < m; a++)
19
      for (int c = 0; c < m; c++)
20
        S[a * m + c] += f->Wc[i] * dz[i * m + a] * dz[i * m + c];
21
    for (int a = 0; a < N; a++)
22
      for (int c = 0; c < m; c++)
23
        Pxz[a * m + c] += f->Wc[i] * f->X[i * N + a] * dz[i * m + c];
24
  }
25
  /* K^T = S^-1 Pxz^T by Cholesky; the factor goes in KS, which is free until below */
26
  kl_mat_copy(S, KS, m, m);
27
  if (kl_chol(KS, m) != 0)
28
    return -1;
29
  kl_mat_transpose(Pxz, Kt, N, m);
30
  kl_chol_solve(KS, Kt, m, N);
31
  kl_mat_transpose(Kt, K, m, N);
32
  kl_mat_mul(K, y, dx, N, m, 1);
33
  kl_mat_mul(K, S, KS, N, m, m);
34
  kl_mat_mul_bt(KS, K, KSKt, N, m, N);
35
  kl_mat_sub(f->P, KSKt, f->P, N, N);
36
  kl_mat_symmetrize(f->P, N);
37
  inject(f, dx);
38
  return 1;
39
}

C 版里 inject 是文件内静态函数的又一份拷贝，没有和 ESKF 的共用：ESKF 的 inject 是 static，我没有为了去重去改前一章已经定稿的文件。三个语言的 UKF 都各自有一份几行的 inject。

Rust#

1
    pub fn predict(&mut self, gyro: [Real; 3], dt: Real) -> bool {
2
        let Some(x) = sigma_points(&self.p) else {
3
            return false;
4
        };
5
        let (wm, wc) = weights();
6
        let mut qi = self.point_attitudes(&x);
7
        let mut bi = [[0.0; 3]; NP];
8
        for i in 0..NP {
9
            for k in 0..3 {
10
                bi[i][k] = self.b[k] + x[i][3 + k];
11
            }
12
            let w = [
13
                (gyro[0] - bi[i][0]) * dt,
14
                (gyro[1] - bi[i][1]) * dt,
15
                (gyro[2] - bi[i][2]) * dt,
16
            ];
17
            qi[i] = qi[i] * Quat::exp(w);
18
        }
19
        // Deviations from the propagated centre point, in its tangent space.
20
        let q_ref = qi[0];
21
        let mut d = [[0.0; N]; NP];
22
        for i in 0..NP {
23
            let th = (q_ref.conj() * qi[i]).log();
24
            d[i][..3].copy_from_slice(&th);
25
            for k in 0..3 {
26
                d[i][3 + k] = bi[i][k] - self.b[k];
27
            }
28
        }
29
        let mut mean = [0.0; N];
30
        for i in 0..NP {
31
            for k in 0..N {
32
                mean[k] += wm[i] * d[i][k];
33
            }
34
        }
35
        let mut cov = Mat::<N, N>::zeros();
36
        for i in 0..NP {
37
            for r in 0..N {
38
                for c in 0..N {
39
                    cov.m[r][c] += wc[i] * (d[i][r] - mean[r]) * (d[i][c] - mean[c]);
40
                }
41
            }
42
        }
43
        for k in 0..3 {
44
            cov.m[k][k] += self.sg * self.sg * (dt * dt);
45
            cov.m[3 + k][3 + k] += self.sbw * self.sbw * dt;
46
        }
47
        self.q = q_ref * Quat::exp([mean[0], mean[1], mean[2]]);
48
        for k in 0..3 {
49
            self.b[k] += mean[3 + k];
50
        }
51
        self.p = cov;
52
        true
53
    }

1
    fn correct<const M: usize>(
2
        &mut self,
3
        x: &Points,
4
        z: &[[Real; M]; NP],
5
        y: &Mat<M, 1>,
6
        r: &Mat<M, M>,
7
    ) -> bool {
8
        let (wm, wc) = weights();
9
        let mut zbar = [0.0; M];
10
        for i in 0..NP {
11
            for k in 0..M {
12
                zbar[k] += wm[i] * z[i][k];
13
            }
14
        }
15
        let mut s = *r;
16
        let mut pxz = Mat::<N, M>::zeros();
17
        for i in 0..NP {
18
            for a in 0..M {
19
                for b in 0..M {
20
                    s.m[a][b] += wc[i] * (z[i][a] - zbar[a]) * (z[i][b] - zbar[b]);
21
                }
22
                for a2 in 0..N {
23
                    pxz.m[a2][a] += wc[i] * x[i][a2] * (z[i][a] - zbar[a]);
24
                }
25
            }
26
        }
27
        let Some(l) = s.cholesky() else {
28
            return false;
29
        };
30
        let k = chol_solve(&l, &pxz.transpose()).transpose();
31
        let dx = k * *y;
32
        self.p = (self.p - k * s * k.transpose()).symmetrized();
33
        self.inject(&dx);
34
        true
35
    }
36

37
    /// Gravity-direction update. False when gated out or on Cholesky failure.

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

1
/* Error-state unscented Kalman filter for attitude: same 6-dim error [dtheta, db] as the ESKF,
2
 * additive noise, scaled unscented transform with alpha = 1, beta = 2, kappa = 0
3
 * (lambda = 0, so Wm[0] = 0, Wc[0] = 2 and every other weight is 1/12).
4
 * Quaternion conventions: see kl_quat.h. All storage is inline; nothing is allocated. */
5
#ifndef KL_UKF_ATT_H
6
#define KL_UKF_ATT_H
7

8
#include "kl/kl_real.h"
9

10
#define KL_UKF_N 6
11
#define KL_UKF_NPTS (2 * KL_UKF_N + 1)
12

13
/* Sigma points of N(0, P): X (13 x 6, row-major) is the centre (zero) followed by
14
 * +-columns of chol((n + lambda) P); Wm and Wc are the mean and covariance weights.
15
 * Returns -1 if P is not positive definite. */
16
int kl_ukf_sigma_points(const kl_real P[36], kl_real X[78], kl_real Wm[13], kl_real Wc[13]);
17

18
typedef struct {
19
  kl_real q[4], b[3], P[36];
20
  kl_real sg, sbw, sa, sm; /* gyro, bias random walk, accel, mag noise */
21
  kl_real m_w[3];
22
  kl_real accel_gate, mag_gate; /* negative disables the gate */
23
  int reset;                    /* apply the reset Jacobian after injection */
24
  kl_real X[78], Wm[13], Wc[13], qi[52], Z[39]; /* sigma points, weights, attitudes, outputs */
25
  kl_real work[256];
26
} kl_ukf_att;
27

28
/* p0_diag: initial diagonal of P (6). The bias starts at zero. */
29
void kl_ukf_att_init(kl_ukf_att *f, const kl_real q0[4], const kl_real p0_diag[6], 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, kl_real mag_gate, int reset);
32
/* Returns 0 on success, -1 if P is not positive definite (state unchanged). */
33
int kl_ukf_att_predict(kl_ukf_att *f, const kl_real gyro[3], kl_real dt);
34
/* Return 1 if applied, 0 if gated out, -1 on Cholesky failure (state unchanged). */
35
int kl_ukf_att_update_accel(kl_ukf_att *f, const kl_real accel[3]);
36
int kl_ukf_att_update_mag_heading(kl_ukf_att *f, const kl_real mag[3]);
37

38
#endif

1
#include "kl/kl_ukf_att.h"
2

3
#include <stddef.h>
4

5
#include "kl/kl_ekf_att.h"
6
#include "kl/kl_eskf.h"
7
#include "kl/kl_mat.h"
8
#include "kl/kl_quat.h"
9

10
#define N KL_UKF_N
11
#define NP KL_UKF_NPTS
12
#define ALPHA ((kl_real)1)
13
#define BETA ((kl_real)2)
14
#define KAPPA ((kl_real)0)
15

16
int kl_ukf_sigma_points(const kl_real P[36], kl_real X[78], kl_real Wm[13], kl_real Wc[13])
17
{
18
  const kl_real lam = ALPHA * ALPHA * (N + KAPPA) - N;
19
  kl_real L[N * N];
20
  kl_mat_scale(P, N + lam, L, N, N);
21
  if (kl_chol(L, N) != 0)
22
    return -1;
23
  for (int j = 0; j < N; j++)
24
    X[j] = 0;
25
  for (int j = 0; j < N; j++)
26
    for (int i = 0; i < N; i++) {
27
      X[(1 + j) * N + i] = L[i * N + j];
28
      X[(1 + N + j) * N + i] = -L[i * N + j];
29
    }
30
  for (int i = 0; i < NP; i++)
31
    Wm[i] = Wc[i] = (kl_real)0.5 / (N + lam);
32
  Wm[0] = lam / (N + lam);
33
  Wc[0] = Wm[0] + (1 - ALPHA * ALPHA + BETA);
34
  return 0;
35
}
36

37
void kl_ukf_att_init(kl_ukf_att *f, const kl_real q0[4], const kl_real p0_diag[6], kl_real sigma_g,
38
         kl_real sigma_bw, kl_real sigma_a, kl_real sigma_m, const kl_real m_w[3],
39
         kl_real accel_gate, kl_real mag_gate, int reset)
40
{
41
  kl_mat_copy(q0, f->q, 4, 1);
42
  kl_mat_copy(m_w, f->m_w, 3, 1);
43
  for (int i = 0; i < 3; i++)
44
    f->b[i] = 0;
45
  for (int i = 0; i < 36; i++)
46
    f->P[i] = 0;
47
  for (int i = 0; i < N; i++)
48
    f->P[i * 7] = p0_diag[i];
49
  f->sg = sigma_g;
50
  f->sbw = sigma_bw;
51
  f->sa = sigma_a;
52
  f->sm = sigma_m;
53
  f->accel_gate = accel_gate;
54
  f->mag_gate = mag_gate;
55
  f->reset = reset;
56
}
57

58
/* qi[i] = q (x) exp(X[i, :3]) for every sigma point */
59
static void point_attitudes(kl_ukf_att *f)
60
{
61
  for (int i = 0; i < NP; i++) {
62
    kl_real dq[4];
63
    kl_quat_exp(&f->X[i * N], dq);
64
    kl_quat_mul(f->q, dq, &f->qi[i * 4]);
65
  }
66
}
67

68
int kl_ukf_att_predict(kl_ukf_att *f, const kl_real gyro[3], kl_real dt)
69
{
70
  kl_real *bi = f->work, *D = bi + 3 * NP, *cov = D + N * NP;
71
  kl_real mean[N], dev[N], q_ref[4], qc[4], t[4], dq[4], qn[4];
72
  if (kl_ukf_sigma_points(f->P, f->X, f->Wm, f->Wc) != 0)
73
    return -1;
74
  point_attitudes(f);
75
  /* each point through the gyro model */
76
  for (int i = 0; i < NP; i++) {
77
    kl_real w[3];
78
    for (int k = 0; k < 3; k++) {
79
      bi[i * 3 + k] = f->b[k] + f->X[i * N + 3 + k];
80
      w[k] = (gyro[k] - bi[i * 3 + k]) * dt;
81
    }
82
    kl_quat_exp(w, dq);
83
    kl_quat_mul(&f->qi[i * 4], dq, t);
84
    kl_mat_copy(t, &f->qi[i * 4], 4, 1);
85
  }
86
  /* deviations from the propagated centre point */
87
  kl_mat_copy(f->qi, q_ref, 4, 1);
88
  kl_quat_conj(q_ref, qc);
89
  for (int i = 0; i < NP; i++) {
90
    kl_quat_mul(qc, &f->qi[i * 4], t);
91
    kl_quat_log(t, &D[i * N]);
92
    for (int k = 0; k < 3; k++)
93
      D[i * N + 3 + k] = bi[i * 3 + k] - f->b[k];
94
  }
95
  for (int k = 0; k < N; k++)
96
    mean[k] = 0;
97
  for (int i = 0; i < NP; i++)
98
    for (int k = 0; k < N; k++)
99
      mean[k] += f->Wm[i] * D[i * N + k];
100
  for (int i = 0; i < N * N; i++)
101
    cov[i] = 0;
102
  for (int i = 0; i < NP; i++) {
103
    for (int k = 0; k < N; k++)
104
      dev[k] = D[i * N + k] - mean[k];
105
    for (int a = 0; a < N; a++)
106
      for (int c = 0; c < N; c++)
107
        cov[a * N + c] += f->Wc[i] * dev[a] * dev[c];
108
  }
109
  kl_quat_exp(mean, dq);
110
  kl_quat_mul(q_ref, dq, qn);
111
  kl_mat_copy(qn, f->q, 4, 1);
112
  for (int k = 0; k < 3; k++) {
113
    f->b[k] += mean[3 + k];
114
    cov[k * 7] += f->sg * f->sg * dt * dt;
115
    cov[(k + 3) * 7] += f->sbw * f->sbw * dt;
116
  }
117
  kl_mat_copy(cov, f->P, N, N);
118
  return 0;
119
}
120

121
/* Move the error estimate into the nominal state, then reset the error to zero. */
122
static void inject(kl_ukf_att *f, const kl_real dx[6])
123
{
124
  kl_real *G = f->work, *T = G + 36, *U = T + 36;
125
  kl_real dq[4], qn[4];
126
  kl_quat_exp(dx, dq);
127
  kl_quat_mul(f->q, dq, qn);
128
  kl_quat_normalize(qn);
129
  kl_mat_copy(qn, f->q, 4, 1);
130
  for (int i = 0; i < 3; i++)
131
    f->b[i] += dx[3 + i];
132
  if (f->reset) {
133
    kl_real half[3] = {(kl_real)0.5 * dx[0], (kl_real)0.5 * dx[1], (kl_real)0.5 * dx[2]};
134
    kl_real S[9];
135
    kl_skew(half, S);
136
    kl_mat_identity(G, 6);
137
    for (int i = 0; i < 3; i++)
138
      for (int j = 0; j < 3; j++)
139
        G[i * 6 + j] -= S[i * 3 + j];
140
    kl_mat_mul(G, f->P, T, 6, 6, 6);
141
    kl_mat_mul_bt(T, G, U, 6, 6, 6);
142
    kl_mat_copy(U, f->P, 6, 6);
143
  }
144
}
145

146
/* Standard UKF measurement update given the model outputs Z (13 x m) at the sigma points X. */
147
static int correct(kl_ukf_att *f, int m, const kl_real *y, const kl_real *R)
148
{
149
  kl_real *dz = f->work, *S = dz + NP * m, *Pxz = S + m * m, *Kt = Pxz + N * m;
150
  kl_real *K = Kt + m * N, *KS = K + N * m, *KSKt = KS + N * m;
151
  kl_real zbar[3], dx[N];
152
  for (int k = 0; k < m; k++)
153
    zbar[k] = 0;
154
  for (int i = 0; i < NP; i++)
155
    for (int k = 0; k < m; k++)
156
      zbar[k] += f->Wm[i] * f->Z[i * m + k];
157
  for (int i = 0; i < m * m; i++)
158
    S[i] = R[i];
159
  for (int i = 0; i < N * m; i++)
160
    Pxz[i] = 0;
161
  for (int i = 0; i < NP; i++) {
162
    for (int k = 0; k < m; k++)
163
      dz[i * m + k] = f->Z[i * m + k] - zbar[k];
164
    for (int a = 0; a < m; a++)
165
      for (int c = 0; c < m; c++)
166
        S[a * m + c] += f->Wc[i] * dz[i * m + a] * dz[i * m + c];
167
    for (int a = 0; a < N; a++)
168
      for (int c = 0; c < m; c++)
169
        Pxz[a * m + c] += f->Wc[i] * f->X[i * N + a] * dz[i * m + c];
170
  }
171
  /* K^T = S^-1 Pxz^T by Cholesky; the factor goes in KS, which is free until below */
172
  kl_mat_copy(S, KS, m, m);
173
  if (kl_chol(KS, m) != 0)
174
    return -1;
175
  kl_mat_transpose(Pxz, Kt, N, m);
176
  kl_chol_solve(KS, Kt, m, N);
177
  kl_mat_transpose(Kt, K, m, N);
178
  kl_mat_mul(K, y, dx, N, m, 1);
179
  kl_mat_mul(K, S, KS, N, m, m);
180
  kl_mat_mul_bt(KS, K, KSKt, N, m, N);
181
  kl_mat_sub(f->P, KSKt, f->P, N, N);
182
  kl_mat_symmetrize(f->P, N);
183
  inject(f, dx);
184
  return 1;
185
}
186

187
int kl_ukf_att_update_accel(kl_ukf_att *f, const kl_real accel[3])
188
{
189
  kl_real norm = KL_SQRT(accel[0] * accel[0] + accel[1] * accel[1] + accel[2] * accel[2]);
190
  if (f->accel_gate >= 0 && KL_FABS(norm - (kl_real)KL_GRAVITY) > f->accel_gate)
191
    return 0;
192
  const kl_real down[3] = {0, 0, -1};
193
  kl_real y[3], R[9], zbar[3] = {0, 0, 0};
194
  if (kl_ukf_sigma_points(f->P, f->X, f->Wm, f->Wc) != 0)
195
    return -1;
196
  point_attitudes(f);
197
  for (int i = 0; i < NP; i++) {
198
    kl_ekf_att_rot_inv(&f->qi[i * 4], down, &f->Z[i * 3]);
199
    for (int k = 0; k < 3; k++)
200
      zbar[k] += f->Wm[i] * f->Z[i * 3 + k];
201
  }
202
  kl_real r = (f->sa / (kl_real)KL_GRAVITY) * (f->sa / (kl_real)KL_GRAVITY);
203
  for (int i = 0; i < 3; i++) {
204
    y[i] = accel[i] / norm - zbar[i];
205
    for (int j = 0; j < 3; j++)
206
      R[i * 3 + j] = (i == j) ? r : 0;
207
  }
208
  return correct(f, 3, y, R);
209
}
210

211
int kl_ukf_att_update_mag_heading(kl_ukf_att *f, const kl_real mag[3])
212
{
213
  kl_real norm = KL_SQRT(mag[0] * mag[0] + mag[1] * mag[1] + mag[2] * mag[2]);
214
  if (f->mag_gate >= 0 && KL_FABS(norm - 1) > f->mag_gate)
215
    return 0;
216
  kl_real C[9], h[3], mw[3], z0[NP], zbar, y, R;
217
  if (kl_ukf_sigma_points(f->P, f->X, f->Wm, f->Wc) != 0)
218
    return -1;
219
  point_attitudes(f);
220
  kl_quat_to_dcm(f->q, C);
221
  const kl_real psi_h = KL_ATAN2(f->m_w[1], f->m_w[0]);
222
  for (int i = 0; i < NP; i++) {
223
    kl_ekf_att_rot_inv(&f->qi[i * 4], f->m_w, h); /* C(q) C(q_i)^T m_w */
224
    kl_mat_mul(C, h, mw, 3, 3, 1);
225
    z0[i] = kl_wrap_pi(KL_ATAN2(mw[1], mw[0]) - psi_h);
226
  }
227
  /* heading relative to the centre point: no wrap trouble */
228
  zbar = 0;
229
  for (int i = 0; i < NP; i++) {
230
    f->Z[i] = kl_wrap_pi(z0[i] - z0[0]);
231
    zbar += f->Wm[i] * f->Z[i];
232
  }
233
  zbar += z0[0];
234
  kl_mat_mul(C, mag, mw, 3, 3, 1);
235
  y = kl_wrap_pi(kl_wrap_pi(KL_ATAN2(mw[1], mw[0]) - psi_h) - zbar);
236
  R = (f->sm / KL_SQRT(f->m_w[0] * f->m_w[0] + f->m_w[1] * f->m_w[1]));
237
  R = R * R;
238
  return correct(f, 1, &y, &R);
239
}

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

1
// Error-state unscented Kalman filter for attitude: the same 6-dim error
2
// [dtheta, db] as the ESKF, additive noise, scaled unscented transform with
3
// alpha = 1, beta = 2, kappa = 0 (so lambda = 0, W0 = 0 and all weights positive).
4
#pragma once
5
#include "kl/eskf.hpp"
6

7
namespace kl {
8

9
// Sigma points of N(0, P): the centre, then +-columns of chol((n + lambda) P).
10
template <std::size_t N, class T = double>
11
struct SigmaPoints {
12
  static constexpr std::size_t COUNT = 2 * N + 1;
13
  static constexpr T ALPHA = 1, BETA = 2, KAPPA = 0;
14
  static constexpr T LAMBDA = ALPHA * ALPHA * (N + KAPPA) - N;
15

16
  Mat<COUNT, N, T> X;  // one point per row
17
  std::array<T, COUNT> Wm, Wc;
18

19
  // Returns false if P is not positive definite.
20
  bool compute(const Mat<N, N, T>& P) {
21
    Mat<N, N, T> L = P * (N + LAMBDA);
22
    if (!cholesky(L)) return false;
23
    X = {};
24
    for (std::size_t j = 0; j < N; j++)
25
      for (std::size_t k = j; k < N; k++) {  // L is lower triangular
26
        X(1 + j, k) = L(k, j);
27
        X(1 + N + j, k) = -L(k, j);
28
      }
29
    Wm.fill(T(0.5) / (N + LAMBDA));
30
    Wc = Wm;
31
    Wm[0] = LAMBDA / (N + LAMBDA);
32
    Wc[0] = Wm[0] + (1 - ALPHA * ALPHA + BETA);
33
    return true;
34
  }
35
};
36

37
template <class T = double>
38
class AttitudeUKF {
39
  static constexpr std::size_t N = 6, K = SigmaPoints<N, T>::COUNT;
40

41
public:
42
  Quat<T> q;
43
  Vec3<T> b{};
44
  Mat<6, 6, T> P{};
45
  T sg, sbw, sa, sm;
46
  Vec3<T> m_w;
47
  T accel_gate, mag_gate;  // negative disables the gate
48
  bool reset;
49

50
  AttitudeUKF(T sigma_g, T sigma_bw, T sigma_a, T sigma_m, const Vec3<T>& m_w, const Quat<T>& q0,
51
        const Mat<6, 6, T>& P0, T accel_gate = T(0.5), T mag_gate = T(0.1), bool reset = true)
52
      : q(q0), P(P0), sg(sigma_g), sbw(sigma_bw), sa(sigma_a), sm(sigma_m), m_w(m_w),
53
        accel_gate(accel_gate), mag_gate(mag_gate), reset(reset) {}
54

55
  // Returns false, leaving the state unchanged, if P is not positive definite.
56
  bool predict(const Vec3<T>& gyro, T dt) {
57
    SigmaPoints<N, T> sp;
58
    if (!sp.compute(P)) return false;
59
    std::array<Quat<T>, K> qi;
60
    std::array<Vec3<T>, K> bi;
61
    for (std::size_t i = 0; i < K; i++) {
62
      qi[i] = q * Quat<T>::exp(rows3(sp.X, i, 0));
63
      bi[i] = b + rows3(sp.X, i, 3);
64
      qi[i] = qi[i] * Quat<T>::exp((gyro - bi[i]) * dt);  // each point through the gyro model
65
    }
66
    const Quat<T> q_ref = qi[0];  // the propagated centre point
67
    Mat<K, 6, T> D;
68
    for (std::size_t i = 0; i < K; i++) {
69
      const Vec3<T> th = (q_ref.conj() * qi[i]).log(), db = bi[i] - b;
70
      for (std::size_t j = 0; j < 3; j++) {
71
        D(i, j) = th(j, 0);
72
        D(i, 3 + j) = db(j, 0);
73
      }
74
    }
75
    Mat<1, 6, T> mean{};
76
    for (std::size_t i = 0; i < K; i++)
77
      for (std::size_t j = 0; j < 6; j++) mean(0, j) += sp.Wm[i] * D(i, j);
78
    Mat<6, 6, T> cov{};
79
    for (std::size_t i = 0; i < K; i++)
80
      for (std::size_t r = 0; r < 6; r++)
81
        for (std::size_t c = 0; c < 6; c++)
82
          cov(r, c) += sp.Wc[i] * (D(i, r) - mean(0, r)) * (D(i, c) - mean(0, c));
83
    for (std::size_t i = 0; i < 3; i++) {
84
      cov(i, i) += sg * sg * dt * dt;
85
      cov(3 + i, 3 + i) += sbw * sbw * dt;
86
    }
87
    q = q_ref * Quat<T>::exp(Vec3<T>{{mean(0, 0), mean(0, 1), mean(0, 2)}});  // not normalized here
88
    b = b + Vec3<T>{{mean(0, 3), mean(0, 4), mean(0, 5)}};
89
    P = cov;
90
    return true;
91
  }
92

93
  // Gravity-direction update. Returns false when gated out or on Cholesky failure.
94
  bool update_accel(const Vec3<T>& accel) {
95
    const T norm = std::sqrt(dot(accel, accel));
96
    if (accel_gate >= 0 && std::fabs(norm - T(GRAVITY)) > accel_gate) return false;
97
    SigmaPoints<N, T> sp;
98
    if (!sp.compute(P)) return false;
99
    Mat<K, 3, T> Z;
100
    Vec3<T> zbar{};
101
    for (std::size_t i = 0; i < K; i++) {
102
      const Vec3<T> z = rot_inv(q * Quat<T>::exp(rows3(sp.X, i, 0)), Vec3<T>{{0, 0, -1}});
103
      for (std::size_t j = 0; j < 3; j++) {
104
        Z(i, j) = z(j, 0);
105
        zbar(j, 0) += sp.Wm[i] * z(j, 0);
106
      }
107
    }
108
    const T r = (sa / T(GRAVITY)) * (sa / T(GRAVITY));
109
    return correct(sp, Z, accel * (1 / norm) - zbar, Mat<3, 3, T>::identity() * r);
110
  }
111

112
  // Heading-only magnetometer update. Returns false when gated out or on Cholesky failure.
113
  bool update_mag_heading(const Vec3<T>& mag) {
114
    const T norm = std::sqrt(dot(mag, mag));
115
    if (mag_gate >= 0 && std::fabs(norm - 1) > mag_gate) return false;
116
    SigmaPoints<N, T> sp;
117
    if (!sp.compute(P)) return false;
118
    const Mat<3, 3, T> C = q.dcm();
119
    const T psi_h = std::atan2(m_w(1, 0), m_w(0, 0));
120
    std::array<T, K> z0;
121
    for (std::size_t i = 0; i < K; i++) {
122
      const Vec3<T> mw = C * rot_inv(q * Quat<T>::exp(rows3(sp.X, i, 0)), m_w);
123
      z0[i] = wrap_pi(std::atan2(mw(1, 0), mw(0, 0)) - psi_h);
124
    }
125
    Mat<K, 1, T> Zrel;  // heading relative to the centre point: no wrap trouble
126
    T zbar = 0;
127
    for (std::size_t i = 0; i < K; i++) {
128
      Zrel(i, 0) = wrap_pi(z0[i] - z0[0]);
129
      zbar += sp.Wm[i] * Zrel(i, 0);
130
    }
131
    zbar += z0[0];
132
    const Vec3<T> m_meas = C * mag;
133
    const Mat<1, 1, T> y{{wrap_pi(wrap_pi(std::atan2(m_meas(1, 0), m_meas(0, 0)) - psi_h) - zbar)}};
134
    const T r = sm / std::hypot(m_w(0, 0), m_w(1, 0));
135
    return correct(sp, Zrel, y, Mat<1, 1, T>{{r * r}});
136
  }
137

138
private:
139
  template <std::size_t R, std::size_t C>
140
  static Vec3<T> rows3(const Mat<R, C, T>& X, std::size_t i, std::size_t c0) {
141
    return Vec3<T>{{X(i, c0), X(i, c0 + 1), X(i, c0 + 2)}};
142
  }
143

144
  // Standard UKF measurement update given the model outputs Z at the sigma points.
145
  // y is the innovation, already formed by the caller.
146
  template <std::size_t M>
147
  bool correct(const SigmaPoints<N, T>& sp, const Mat<K, M, T>& Z, const Mat<M, 1, T>& y,
148
         const Mat<M, M, T>& R) {
149
    Mat<1, M, T> zbar{};
150
    for (std::size_t i = 0; i < K; i++)
151
      for (std::size_t j = 0; j < M; j++) zbar(0, j) += sp.Wm[i] * Z(i, j);
152
    Mat<M, M, T> S = R;
153
    Mat<N, M, T> Pxz{};
154
    for (std::size_t i = 0; i < K; i++)
155
      for (std::size_t r = 0; r < M; r++) {
156
        const T dr = Z(i, r) - zbar(0, r);
157
        for (std::size_t c = 0; c < M; c++) S(r, c) += sp.Wc[i] * dr * (Z(i, c) - zbar(0, c));
158
        for (std::size_t k = 0; k < N; k++) Pxz(k, r) += sp.Wc[i] * sp.X(i, k) * dr;
159
      }
160
    Mat<M, M, T> L = S;
161
    if (!cholesky(L)) return false;
162
    const Mat<N, M, T> Kg = chol_solve(L, Pxz.transpose()).transpose();
163
    const Mat<N, 1, T> dx = Kg * y;
164
    P = (P - Kg * S * Kg.transpose()).symmetrized();
165
    inject(dx);
166
    return true;
167
  }
168

169
  // q <- q (x) exp(dtheta), b <- b + db, then reset P <- G P G^T.
170
  void inject(const Mat<6, 1, T>& dx) {
171
    const Vec3<T> dth{{dx(0, 0), dx(1, 0), dx(2, 0)}};
172
    q = (q * Quat<T>::exp(dth)).normalized();
173
    b = b + Vec3<T>{{dx(3, 0), dx(4, 0), dx(5, 0)}};
174
    if (reset) {
175
      Mat<6, 6, T> G = Mat<6, 6, T>::identity();
176
      const Mat<3, 3, T> S = skew(dth * T(0.5));
177
      for (std::size_t i = 0; i < 3; i++)
178
        for (std::size_t j = 0; j < 3; j++) G(i, j) -= S(i, j);
179
      P = G * P * G.transpose();
180
    }
181
  }
182
};
183

184
}  // namespace kl

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

1
//! Error-state unscented Kalman filter for attitude: the same 6-dim error as the
2
//! ESKF (dtheta, db), additive noise, scaled unscented transform.
3
#![allow(clippy::needless_range_loop)]
4

5
use crate::ekf_att::{G, rot_inv};
6
use crate::eskf::wrap_pi;
7
use crate::mat::{Mat, chol_solve};
8
use crate::quat::{Quat, skew};
9
use crate::real::{Real, atan2, fabs, sqrt};
10

11
const N: usize = 6;
12
const NP: usize = 2 * N + 1;
13
/// Scaled UT with alpha = 1, beta = 2, kappa = 0: lambda = 0, so the centre
14
/// point has mean weight 0 and the other 2n points share the weight 1/(2n).
15
const LAMBDA: Real = 0.0;
16
const BETA: Real = 2.0;
17
const ALPHA2: Real = 1.0;
18

19
pub type Points = [[Real; N]; NP];
20

21
fn weights() -> ([Real; NP], [Real; NP]) {
22
    let n = N as Real;
23
    let mut wm = [0.5 / (n + LAMBDA); NP];
24
    let mut wc = wm;
25
    wm[0] = LAMBDA / (n + LAMBDA);
26
    wc[0] = wm[0] + (1.0 - ALPHA2 + BETA);
27
    (wm, wc)
28
}
29

30
/// Sigma points around zero for N(0, P): the centre, then +-columns of
31
/// chol((n + lambda) P). Returns `None` if P is not positive definite.
32
pub fn sigma_points(p: &Mat<N, N>) -> Option<Points> {
33
    let l = (*p * (N as Real + LAMBDA)).cholesky()?;
34
    let mut x = [[0.0; N]; NP];
35
    for j in 0..N {
36
        for i in 0..N {
37
            x[1 + j][i] = l.m[i][j];
38
            x[1 + N + j][i] = -l.m[i][j];
39
        }
40
    }
41
    Some(x)
42
}
43

44
fn dot(a: [Real; 3], b: [Real; 3]) -> Real {
45
    a[0] * b[0] + a[1] * b[1] + a[2] * b[2]
46
}
47

48
pub struct AttitudeUkf {
49
    pub q: Quat,
50
    pub b: [Real; 3],
51
    pub p: Mat<N, N>,
52
    sg: Real,
53
    sbw: Real,
54
    sa: Real,
55
    sm: Real,
56
    m_w: [Real; 3],
57
    /// Gates on | |f| - g | (m/s^2) and | |m| - 1 |; negative disables gating.
58
    accel_gate: Real,
59
    mag_gate: Real,
60
    reset: bool,
61
}
62

63
impl AttitudeUkf {
64
    #[allow(clippy::too_many_arguments)]
65
    pub fn new(
66
        q0: Quat,
67
        p0_diag: [Real; N],
68
        sigma_g: Real,
69
        sigma_bw: Real,
70
        sigma_a: Real,
71
        sigma_m: Real,
72
        m_w: [Real; 3],
73
        accel_gate: Real,
74
        mag_gate: Real,
75
        reset: bool,
76
    ) -> Self {
77
        let mut p = Mat::zeros();
78
        for (i, d) in p0_diag.iter().enumerate() {
79
            p.m[i][i] = *d;
80
        }
81
        Self {
82
            q: q0,
83
            b: [0.0; 3],
84
            p,
85
            sg: sigma_g,
86
            sbw: sigma_bw,
87
            sa: sigma_a,
88
            sm: sigma_m,
89
            m_w,
90
            accel_gate,
91
            mag_gate,
92
            reset,
93
        }
94
    }
95

96
    /// Sigma-point attitudes q (x) exp(dtheta_i).
97
    fn point_attitudes(&self, x: &Points) -> [Quat; NP] {
98
        x.map(|r| self.q * Quat::exp([r[0], r[1], r[2]]))
99
    }
100

101
    /// Propagate every sigma point through the gyro model. False (state
102
    /// unchanged) if P is not positive definite.
103
    pub fn predict(&mut self, gyro: [Real; 3], dt: Real) -> bool {
104
        let Some(x) = sigma_points(&self.p) else {
105
            return false;
106
        };
107
        let (wm, wc) = weights();
108
        let mut qi = self.point_attitudes(&x);
109
        let mut bi = [[0.0; 3]; NP];
110
        for i in 0..NP {
111
            for k in 0..3 {
112
                bi[i][k] = self.b[k] + x[i][3 + k];
113
            }
114
            let w = [
115
                (gyro[0] - bi[i][0]) * dt,
116
                (gyro[1] - bi[i][1]) * dt,
117
                (gyro[2] - bi[i][2]) * dt,
118
            ];
119
            qi[i] = qi[i] * Quat::exp(w);
120
        }
121
        // Deviations from the propagated centre point, in its tangent space.
122
        let q_ref = qi[0];
123
        let mut d = [[0.0; N]; NP];
124
        for i in 0..NP {
125
            let th = (q_ref.conj() * qi[i]).log();
126
            d[i][..3].copy_from_slice(&th);
127
            for k in 0..3 {
128
                d[i][3 + k] = bi[i][k] - self.b[k];
129
            }
130
        }
131
        let mut mean = [0.0; N];
132
        for i in 0..NP {
133
            for k in 0..N {
134
                mean[k] += wm[i] * d[i][k];
135
            }
136
        }
137
        let mut cov = Mat::<N, N>::zeros();
138
        for i in 0..NP {
139
            for r in 0..N {
140
                for c in 0..N {
141
                    cov.m[r][c] += wc[i] * (d[i][r] - mean[r]) * (d[i][c] - mean[c]);
142
                }
143
            }
144
        }
145
        for k in 0..3 {
146
            cov.m[k][k] += self.sg * self.sg * (dt * dt);
147
            cov.m[3 + k][3 + k] += self.sbw * self.sbw * dt;
148
        }
149
        self.q = q_ref * Quat::exp([mean[0], mean[1], mean[2]]);
150
        for k in 0..3 {
151
            self.b[k] += mean[3 + k];
152
        }
153
        self.p = cov;
154
        true
155
    }
156

157
    fn inject(&mut self, dx: &Mat<N, 1>) {
158
        let dth = [dx.m[0][0], dx.m[1][0], dx.m[2][0]];
159
        self.q = (self.q * Quat::exp(dth)).normalized();
160
        for i in 0..3 {
161
            self.b[i] += dx.m[3 + i][0];
162
        }
163
        if self.reset {
164
            let s = skew([0.5 * dth[0], 0.5 * dth[1], 0.5 * dth[2]]);
165
            let mut g = Mat::<N, N>::identity();
166
            for i in 0..3 {
167
                for j in 0..3 {
168
                    g.m[i][j] -= s.m[i][j];
169
                }
170
            }
171
            self.p = g * self.p * g.transpose();
172
        }
173
    }
174

175
    /// Standard UKF correction given model outputs `z` at the sigma points `x`.
176
    /// False (state unchanged) if S is not positive definite.
177
    fn correct<const M: usize>(
178
        &mut self,
179
        x: &Points,
180
        z: &[[Real; M]; NP],
181
        y: &Mat<M, 1>,
182
        r: &Mat<M, M>,
183
    ) -> bool {
184
        let (wm, wc) = weights();
185
        let mut zbar = [0.0; M];
186
        for i in 0..NP {
187
            for k in 0..M {
188
                zbar[k] += wm[i] * z[i][k];
189
            }
190
        }
191
        let mut s = *r;
192
        let mut pxz = Mat::<N, M>::zeros();
193
        for i in 0..NP {
194
            for a in 0..M {
195
                for b in 0..M {
196
                    s.m[a][b] += wc[i] * (z[i][a] - zbar[a]) * (z[i][b] - zbar[b]);
197
                }
198
                for a2 in 0..N {
199
                    pxz.m[a2][a] += wc[i] * x[i][a2] * (z[i][a] - zbar[a]);
200
                }
201
            }
202
        }
203
        let Some(l) = s.cholesky() else {
204
            return false;
205
        };
206
        let k = chol_solve(&l, &pxz.transpose()).transpose();
207
        let dx = k * *y;
208
        self.p = (self.p - k * s * k.transpose()).symmetrized();
209
        self.inject(&dx);
210
        true
211
    }
212

213
    /// Gravity-direction update. False when gated out or on Cholesky failure.
214
    pub fn update_accel(&mut self, accel: [Real; 3]) -> bool {
215
        let norm = sqrt(dot(accel, accel));
216
        if self.accel_gate >= 0.0 && fabs(norm - G) > self.accel_gate {
217
            return false;
218
        }
219
        let Some(x) = sigma_points(&self.p) else {
220
            return false;
221
        };
222
        let (wm, _) = weights();
223
        let qi = self.point_attitudes(&x);
224
        let z = qi.map(|q| rot_inv(q, [0.0, 0.0, -1.0]));
225
        let mut y = Mat::<3, 1> {
226
            m: [[accel[0] / norm], [accel[1] / norm], [accel[2] / norm]],
227
        };
228
        for i in 0..NP {
229
            for k in 0..3 {
230
                y.m[k][0] -= wm[i] * z[i][k];
231
            }
232
        }
233
        let r = Mat::<3, 3>::identity() * ((self.sa / G) * (self.sa / G));
234
        self.correct(&x, &z, &y, &r)
235
    }
236

237
    /// Heading-only magnetometer update. False when gated out or on Cholesky failure.
238
    pub fn update_mag_heading(&mut self, mag: [Real; 3]) -> bool {
239
        let norm = sqrt(dot(mag, mag));
240
        if self.mag_gate >= 0.0 && fabs(norm - 1.0) > self.mag_gate {
241
            return false;
242
        }
243
        let Some(x) = sigma_points(&self.p) else {
244
            return false;
245
        };
246
        let (wm, _) = weights();
247
        let qi = self.point_attitudes(&x);
248
        let c = self.q.dcm();
249
        let rot = |v: [Real; 3]| -> [Real; 3] { [dot(c.m[0], v), dot(c.m[1], v), dot(c.m[2], v)] };
250
        let psi_h = atan2(self.m_w[1], self.m_w[0]);
251
        let z0 = qi.map(|q| {
252
            let mw = rot(rot_inv(q, self.m_w));
253
            wrap_pi(atan2(mw[1], mw[0]) - psi_h)
254
        });
255
        // Headings relative to the centre point: no wrap trouble near +-pi.
256
        let zrel = z0.map(|v| [wrap_pi(v - z0[0])]);
257
        let mut zbar = z0[0];
258
        for i in 0..NP {
259
            zbar += wm[i] * zrel[i][0];
260
        }
261
        let mm = rot(mag);
262
        let y = wrap_pi(wrap_pi(atan2(mm[1], mm[0]) - psi_h) - zbar);
263
        let mh = sqrt(self.m_w[0] * self.m_w[0] + self.m_w[1] * self.m_w[1]);
264
        let r = Mat {
265
            m: [[(self.sm / mh) * (self.sm / mh)]],
266
        };
267
        self.correct(&x, &zrel, &Mat { m: [[y]] }, &r)
268
    }
269
}
270

271
/// Weights, exposed for the self-check.
272
pub fn unscented_weights() -> ([Real; NP], [Real; NP]) {
273
    weights()
274
}

三个 app（c/apps/ch07.c、cpp/apps/ch07.cpp、rust/src/bin/ch07.rs）先做 sigma 点自检：对 $P = \operatorname{diag}(1,2,3,4,5,6)$ ， $\sum_iW^m_i\chi_i = 0$ 、 $\sum_iW^c_i\chi_i\chi_i^T = P$ 都要在 $10^{-12}$ 内成立（float 构建是 $10^{-4}$ ）。然后处理第 6 篇的两个初始条件，写 out/ch07/ukf_{nominal,large}_<lang>.csv。

结果#

1
$ make -s ch07
2
sigma point check: worst error 8.882e-16
3
ch07 self-check passed (worst 8.88e-16)
4
ok   ukf_large_c.csv: max diff 1.037e-13 <= 1.0e-09
5
ok   ukf_large_cpp.csv: max diff 7.694e-14 <= 1.0e-09
6
ok   ukf_large_rs.csv: max diff 7.622e-14 <= 1.0e-09
7
ok   ukf_nominal_c.csv: max diff 8.227e-14 <= 1.0e-09
8
ok   ukf_nominal_cpp.csv: max diff 6.395e-14 <= 1.0e-09
9
ok   ukf_nominal_rs.csv: max diff 6.761e-14 <= 1.0e-09

Python 参考的 UKF 和三种语言的输出最大差异在 $10^{-13}$ 量级（比第 6 篇 ESKF 的 $10^{-15}$ 大了两个数量级）：13 个点的加权求和和 Cholesky 里累积的舍入多了。

玩具问题#

1
Polar to Cartesian: r ~ N(1, 0.02^2), theta ~ N(pi/2, 0.5^2)
2
                    mean x    mean y      var x      var y     cov xy
3
Monte Carlo 1e6    -0.0006    0.8827    0.19641    0.02486    0.00017
4
linearization       0.0000    1.0000    0.25000    0.00040   -0.00000
5
unscented           0.0000    0.8801    0.21101    0.04351   -0.00000

极坐标到直角坐标：蒙特卡洛样本（灰点）、真实一倍标准差椭圆（黑）、线性化（红）、无迹变换（蓝）

均值：蒙特卡洛 $(−0.0006,\ 0.8827)$ ，线性化 $(0,\ 1.0000)$ ，UT $(0,\ 0.8801)$ 。UT 抓到了二阶项，这是上面推导的实证。方差： $y$ 方向真实是 0.02486，线性化给了 0.00040，小了 62 倍；UT 给了 0.04351，偏大 1.75 倍。所以 UT 不是”更准”，它是”没有系统性漏掉一个量级”：它的方差偏大，在滤波里偏保守，这个方向相对安全。图里蓝色椭圆比黑色大，红色椭圆扁成了一条线。

四个滤波器同台#

1
Four filters, seeds 1..50, 120 s each (deg RMS pooled over seeds and time)
2
method       yaw   pitch    roll   total  calm tot  mean NEES  calm NEES  in bounds
3
mahony     4.391   1.488   2.243   4.880     0.733          -          -          -
4
ekf        4.504   0.685   1.120   4.721     0.074    9557.93      6.048      96.5%
5
eskf       0.599   0.592   0.866   0.929     0.075     895.58      6.048      96.0%
6
ukf        0.599   0.592   0.866   0.929     0.075     895.47      6.048      96.0%
7
(NEES 6 dof, mean of 50 runs; 95% bounds [5.078, 6.997]; 'in bounds' = fraction of calm [20,60) steps)

读这张表的几个地方：

UKF 与 ESKF 在三位小数内没有区别：总误差 0.929 对 0.929，平静段 0.075 对 0.075，NEES 895.47 对 895.58，落界比例都是 96.0%。这个问题里，误差是度级甚至零点几度的小量，线性化几乎是精确的，UT 没有可以补的东西。它花了 13 倍的传播和 26 次 $h$ 求值（下面的清单），换来的是和 ESKF 一样的结果。
EKF 的整段 RMSE 和平均 NEES 是 4.721 度和 9558，被磁干扰拖累；平静段（ $[20,60)$ ）的总误差是 0.074 度，和 ESKF、UKF 的 0.075 没有区别，NEES 都是 6.048。这三者在没有干扰时，等价于同一个滤波器的三种写法。
Mahony 的平静段误差 0.733 度，是卡尔曼三兄弟的 10 倍：卡尔曼滤波器的好处主要体现在这里，增益自己算。
没有哪个滤波器的整段数字好看，因为线加速度窗口（60 到 70 秒）对所有人都是未建模的扰动。

模型评估次数（每个滤波步）：

Mahony：2 个预测方向，没有协方差。
EKF：1 次传播，2 次 $h$ 求值加 2 个解析的 $3\times4$ 雅可比， $7\times7$ 协方差。
ESKF：1 次传播，2 次 $h$ 求值加 2 个解析雅可比， $6\times6$ 协方差。
UKF：13 次传播（ $2n+1$ 个 sigma 点）， $13+13 = 26$ 次 $h$ 求值，没有雅可比， $6\times6$ 协方差。

每步的耗时放到第 8 篇一起测。

50 个种子的总姿态误差均方根随时间的变化。卡尔曼三兄弟在平静段几乎重合；磁干扰段 EKF 被拉开

大初始误差：UKF 赢了#

1
Large initial error, seed 1: time until the total error stays below 2 deg for 5 s (error at 10 s)
2
  90 deg yaw                   | mahony  43.28 s (3.96) | ekf   1.18 s (0.05) | eskf   0.02 s (0.04) | ukf   0.01 s (0.04)
3
  120 deg about world [1,1,0]  | mahony  15.21 s (7.00) | ekf   0.01 s (0.04) | eskf   0.86 s (0.18) | ukf   0.04 s (0.04)

括号里是 10 秒时的误差（度）。第 6 篇我用的两个大误差初始条件：

90 度偏航误差：UKF 0.01 秒，ESKF 0.02 秒，EKF 1.18 秒，Mahony 43.28 秒。
120 度、绕世界 $[1,1,0]$ ：UKF 0.04 秒，EKF 0.01 秒，ESKF 0.86 秒，Mahony 15.21 秒。

只有 UKF 在两种情形里都在几十毫秒内收敛：EKF 和 ESKF 各有一个情形慢了一个数量级。它不是赢了精度，是赢了鲁棒性。我猜道理是 sigma 点把大范围的不确定度（ $\sqrt6\cdot\pi/2$ 弧度的散布）真的当成了非线性函数的输入，而 EKF 和 ESKF 只在一个点上线性化；但我没有做更多的实验来证实这个解释，所以这只是一个猜测。Mahony 在两种情形里都需要十几到几十秒，我猜是因为它的增益 $k_p = 1.0$ 是固定的，对大误差来说是个慢滤波。

粒子滤波，只给个路标#

UKF 用 13 个点表示一个高斯。如果后验分布明显不是高斯（比如多峰），点数再多也没用，需要粒子滤波：用成百上千个带权样本直接表示任意分布，靠重采样避免权重退化。它的代价是每步要传播几百到几千个粒子，不适合我们这个 100 Hz 的姿态估计。我没有实现它。要读的话，Arulampalam 等人的 “A tutorial on particle filters for online nonlinear/non-Gaussian Bayesian tracking”（IEEE TSP 2002）是一份很清晰的入门，Doucet、de Freitas、Gordon 编的 Sequential Monte Carlo Methods in Practice（2001）是更完整的参考。UKF 的原始论文是 Julier 和 Uhlmann 的 “A new extension of the Kalman filter to nonlinear systems”（1997），缩放参数的版本来自 Wan 和 van der Merwe 的 “The unscented Kalman filter for nonlinear estimation”（2000）。

下一篇#

三个卡尔曼滤波器在这个问题上，精度几乎没有区别，真正的区别是：每步花多少算力，数字格式（float32）能不能撑住，内存是不是固定。下一篇是整个系列的最后一篇，我们把这些全部量一量：在 float32 下把 KF、ESKF、UKF 跑一遍，看和 double 参考差多少；实现 UD 分解，看协方差的数值健康；打印每个滤波器结构体的字节数；检验库里没有堆分配；把 C 的滤波器编译到 Cortex-M4 上看代码大小；测每步的耗时，包括 Python 慢多少。

音乐

音乐

数学#