移植之前：float32、定长内存与每步耗时

1
"""Sequential scalar updates and a UD-factored ESKF (chapter 8). Built on kl.eskf.ESKF."""
2

3
import numpy as np
4

5
from .eskf import ESKF
6
from .imu_sim import G
7
from .quat import qexp, qmul, qnormalize, qrot, skew, wrap_pi
8
from .ud import bierman_update, thornton_predict, ud_to_P
9

10

11
def scalar_update(P, h, r, y, joseph=True):
12
    """One scalar measurement: returns (dx, P_new). joseph=False is the short form (I - K h) P."""
13
    s = h @ P @ h + r
14
    if not s > 0:
15
        raise np.linalg.LinAlgError("S is not positive")
16
    K = P @ h / s
17
    A = np.eye(len(h)) - np.outer(K, h)
18
    if joseph:
19
        Pn = A @ P @ A.T + r * np.outer(K, K)
20
        Pn = 0.5 * (Pn + Pn.T)
21
    else:
22
        Pn = A @ P
23
    return K * y, Pn
24

25

26
def update_accel_seq(f, accel, joseph=True):
27
    """Accelerometer update processing the three components one at a time (diagonal R).
28

29
    Same linearization point for all three rows; the nominal state is injected once at the end.
30
    """
31
    norm = np.sqrt(accel @ accel)
32
    if f.accel_gate is not None and abs(norm - G) > f.accel_gate:
33
        return False
34
    h = qrot(f.q).T @ np.array([0.0, 0.0, -1.0])
35
    Hm = skew(h)
36
    y = accel / norm - h
37
    r = (f.sa / G) ** 2
38
    dx = np.zeros(6)
39
    for j in range(3):
40
        hj = np.zeros(6)
41
        hj[:3] = Hm[j]
42
        dxj, f.P = scalar_update(f.P, hj, r, y[j] - hj @ dx, joseph)
43
        dx = dx + dxj
44
    f._inject(dx)
45
    return True
46

47

48
def update_mag_heading_seq(f, mag, joseph=True):
49
    norm = np.sqrt(mag @ mag)
50
    if f.mag_gate is not None and abs(norm - 1.0) > f.mag_gate:
51
        return False
52
    mw = qrot(f.q) @ mag
53
    y = wrap_pi(np.arctan2(mw[1], mw[0]) - np.arctan2(f.m_w[1], f.m_w[0]))
54
    h = np.zeros(6)
55
    h[:3] = f.heading_jacobian()
56
    r = (f.sm / np.hypot(f.m_w[0], f.m_w[1])) ** 2
57
    dx, f.P = scalar_update(f.P, h, r, y, joseph)
58
    f._inject(dx)
59
    return True

1
/* One scalar measurement h^T x + v, var(v) = r, innovation y: P in place, dx = K y.
2
 * w is scratch for 126 kl_real. Returns -1 (P unchanged) if s <= 0. */
3
static int scalar_update(kl_real *P, const kl_real *h, kl_real r, kl_real y, int joseph, kl_real *dx,
4
       kl_real *w)
5
{
6
  kl_real *Ph = w, *hP = Ph + 6, *K = hP + 6, *A = K + 6, *T = A + 36, *U = T + 36;
7
  kl_mat_mul(P, h, Ph, 6, 6, 1);
8
  kl_mat_mul(h, P, hP, 1, 6, 6);
9
  kl_real s = r;
10
  for (int i = 0; i < 6; i++)
11
    s += hP[i] * h[i];
12
  if (!(s > 0))
13
    return -1;
14
  for (int i = 0; i < 6; i++) {
15
    K[i] = Ph[i] / s;
16
    dx[i] = K[i] * y;
17
  }
18
  /* A = I - K h^T */
19
  for (int i = 0; i < 6; i++)
20
    for (int j = 0; j < 6; j++)
21
      A[i * 6 + j] = (i == j) - K[i] * h[j];
22
  kl_mat_mul(A, P, T, 6, 6, 6);
23
  if (joseph) {
24
    kl_mat_mul_bt(T, A, U, 6, 6, 6);
25
    for (int i = 0; i < 6; i++)
26
      for (int j = 0; j < 6; j++)
27
        P[i * 6 + j] = U[i * 6 + j] + r * (K[i] * K[j]);
28
    kl_mat_symmetrize(P, 6);
29
  } else {
30
    kl_mat_copy(T, P, 6, 6);
31
  }
32
  return 0;
33
}

1
"""UD (unit upper triangular x diagonal) covariance factorization, P = U D U^T.
2

3
Bierman's scalar measurement update and Thornton's time update (modified weighted
4
Gram-Schmidt). See G. J. Bierman, "Factorization Methods for Discrete Sequential
5
Estimation" (1977) and Grewal and Andrews, "Kalman Filtering: Theory and Practice".
6
"""
7

8
import numpy as np
9

10

11
def ud_factor(P):
12
    """P (symmetric positive definite) -> (U, D). Raises ValueError if a pivot is <= 0."""
13
    n = P.shape[0]
14
    U = np.eye(n)
15
    D = np.zeros(n)
16
    A = P.copy()
17
    for j in range(n - 1, -1, -1):
18
        D[j] = A[j, j]
19
        if not D[j] > 0:
20
            raise ValueError("matrix is not positive definite")
21
        for i in range(j):
22
            U[i, j] = A[i, j] / D[j]
23
        for i in range(j):
24
            for k in range(i + 1):
25
                A[k, i] -= U[k, j] * D[j] * U[i, j]
26
    return U, D
27

28

29
def ud_to_P(U, D):
30
    return (U * D) @ U.T
31

32

33
def bierman_update(U, D, h, r, y):
34
    """Scalar measurement update of (U, D) in place; returns the correction dx = K y.
35

36
    Measurement z = h x + v, var(v) = r, innovation y. Uses the Joseph-equivalent
37
    factored form, so P stays positive definite by construction.
38
    """
39
    n = len(D)
40
    f = U.T @ h
41
    g = D * f
42
    b = np.zeros(n)
43
    alpha = r + f[0] * g[0]
44
    D[0] = D[0] * r / alpha
45
    b[0] = g[0]
46
    for j in range(1, n):
47
        gamma = alpha
48
        alpha = alpha + f[j] * g[j]
49
        D[j] = D[j] * gamma / alpha
50
        lam = -f[j] / gamma
51
        for i in range(j):
52
            s = U[i, j]
53
            U[i, j] = s + b[i] * lam
54
            b[i] = b[i] + s * g[j]
55
        b[j] = g[j]
56
    return b / alpha * y
57

58

59
def thornton_predict(U, D, F, qdiag):
60
    """Time update P <- F P F^T + diag(qdiag) in factored form. Returns new (U, D)."""
61
    n = len(D)
62
    W = np.hstack([F @ U, np.eye(n)])  # rows are the vectors to orthogonalize
63
    wt = np.concatenate([D, qdiag])  # their weights
64
    Un, Dn = np.eye(n), np.zeros(n)
65
    for j in range(n - 1, -1, -1):
66
        c = wt * W[j]
67
        Dn[j] = W[j] @ c
68
        d = c / Dn[j]
69
        for i in range(j):
70
            Un[i, j] = W[i] @ d
71
            W[i] = W[i] - Un[i, j] * W[j]
72
    return Un, Dn

1
void kl_ud_bierman(kl_real *U, kl_real *D, const kl_real *h, kl_real r, kl_real y, int n,
2
       kl_real *dx, kl_real *work)
3
{
4
  kl_real *f = work, *g = f + n, *b = g + n;
5
  for (int j = 0; j < n; j++) {
6
    f[j] = 0;
7
    for (int i = 0; i <= j; i++)
8
      f[j] += U[i * n + j] * h[i];
9
    g[j] = D[j] * f[j];
10
  }
11
  kl_real alpha = r + f[0] * g[0];
12
  D[0] = D[0] * r / alpha;
13
  b[0] = g[0];
14
  for (int j = 1; j < n; j++) {
15
    kl_real gamma = alpha;
16
    alpha = alpha + f[j] * g[j];
17
    D[j] = D[j] * gamma / alpha;
18
    kl_real lam = -f[j] / gamma;
19
    for (int i = 0; i < j; i++) {
20
      kl_real s = U[i * n + j];
21
      U[i * n + j] = s + b[i] * lam;
22
      b[i] = b[i] + s * g[j];
23
    }
24
    b[j] = g[j];
25
  }
26
  for (int i = 0; i < n; i++)
27
    dx[i] = b[i] / alpha * y;
28
}

1
// Time update P <- F P F^T + diag(qdiag) in factored form, in place.
2
// The rows of W = [F U | I] are orthogonalized with weights [D, qdiag].
3
template <std::size_t N, class T>
4
void thornton_predict(Mat<N, N, T>& U, std::array<T, N>& D, const Mat<N, N, T>& F,
5
          const std::array<T, N>& qdiag) {
6
  const Mat<N, N, T> FU = F * U;
7
  Mat<N, 2 * N, T> W{};
8
  std::array<T, 2 * N> wt{}, c{};
9
  for (std::size_t i = 0; i < N; i++) {
10
    for (std::size_t k = 0; k < N; k++) W(i, k) = FU(i, k);
11
    W(i, N + i) = 1;
12
    wt[i] = D[i];
13
    wt[N + i] = qdiag[i];
14
  }
15
  U = Mat<N, N, T>::identity();
16
  for (std::size_t j = N; j-- > 0;) {
17
    T dj = 0;
18
    for (std::size_t k = 0; k < 2 * N; k++) {
19
      c[k] = wt[k] * W(j, k);
20
      dj += W(j, k) * c[k];
21
    }
22
    D[j] = dj;
23
    for (std::size_t i = 0; i < j; i++) {
24
      T u = 0;
25
      for (std::size_t k = 0; k < 2 * N; k++) u += W(i, k) * (c[k] / dj);
26
      U(i, j) = u;
27
      for (std::size_t k = 0; k < 2 * N; k++) W(i, k) -= u * W(j, k);
28
    }
29
  }
30
}

1
/// Time update P <- F P F^T + diag(qdiag) in factored form, in place.
2
/// Orthogonalizes the rows of W = [F U | I] with weights [D, qdiag].
3
pub fn thornton_predict<const N: usize>(
4
    u: &mut Mat<N, N>,
5
    d: &mut [Real; N],
6
    f: &Mat<N, N>,
7
    qdiag: &[Real; N],
8
) {
9
    let mut wa = *f * *u;
10
    let mut wb = Mat::<N, N>::identity();
11
    let mut un = Mat::<N, N>::identity();
12
    let mut dn = [0.0; N];
13
    for j in (0..N).rev() {
14
        let ca: [Real; N] = core::array::from_fn(|k| d[k] * wa.m[j][k]);
15
        let cb: [Real; N] = core::array::from_fn(|k| qdiag[k] * wb.m[j][k]);
16
        let mut dj = 0.0;
17
        for k in 0..N {
18
            dj += wa.m[j][k] * ca[k];
19
        }
20
        for k in 0..N {
21
            dj += wb.m[j][k] * cb[k];
22
        }
23
        dn[j] = dj;
24
        for i in 0..j {
25
            let mut s = 0.0;
26
            for k in 0..N {
27
                s += wa.m[i][k] * (ca[k] / dj);
28
            }
29
            for k in 0..N {
30
                s += wb.m[i][k] * (cb[k] / dj);
31
            }
32
            un.m[i][j] = s;
33
            for k in 0..N {
34
                wa.m[i][k] -= s * wa.m[j][k];
35
                wb.m[i][k] -= s * wb.m[j][k];
36
            }
37
        }
38
    }
39
    *u = un;
40
    *d = dn;
41
}

1
class EskfUD:
2
    """ESKF whose covariance lives in UD form: Thornton predict, Bierman scalar updates.
3

4
    No reset Jacobian (it would not preserve the factored form); compare with ESKF(reset=False).
5
    """
6

7
    def __init__(self, sigma_g, sigma_bw, sigma_a, sigma_m, m_w, q0, P0, accel_gate=0.5, mag_gate=0.1):
8
        self.ref = ESKF(sigma_g, sigma_bw, sigma_a, sigma_m, m_w, q0, P0, accel_gate, mag_gate, reset=False)
9
        d = np.diag(P0).copy()
10
        assert np.allclose(np.diag(d), P0), "initial covariance must be diagonal"
11
        self.U, self.D = np.eye(6), d
12

13
    q = property(lambda s: s.ref.q)
14
    b = property(lambda s: s.ref.b)
15
    P = property(lambda s: ud_to_P(s.U, s.D))
16

17
    def predict(self, gyro, dt):
18
        f = self.ref
19
        dq = qexp((gyro - f.b) * dt)
20
        F = np.eye(6)
21
        F[:3, :3] = qrot(dq).T
22
        F[:3, 3:] = -dt * np.eye(3)
23
        qd = np.array([f.sg**2 * dt**2] * 3 + [f.sbw**2 * dt] * 3)
24
        f.q = qmul(f.q, dq)
25
        self.U, self.D = thornton_predict(self.U, self.D, F, qd)
26

27
    def _inject(self, dx):
28
        f = self.ref
29
        f.q = qnormalize(qmul(f.q, qexp(dx[:3])))
30
        f.b = f.b + dx[3:]
31

32
    def update_accel(self, accel):
33
        f = self.ref
34
        norm = np.sqrt(accel @ accel)
35
        if f.accel_gate is not None and abs(norm - G) > f.accel_gate:
36
            return False
37
        h = qrot(f.q).T @ np.array([0.0, 0.0, -1.0])
38
        Hm = skew(h)
39
        y = accel / norm - h
40
        r = (f.sa / G) ** 2
41
        dx = np.zeros(6)
42
        for j in range(3):
43
            hj = np.zeros(6)
44
            hj[:3] = Hm[j]
45
            dx = dx + bierman_update(self.U, self.D, hj, r, y[j] - hj @ dx)
46
        self._inject(dx)
47
        return True
48

49
    def update_mag_heading(self, mag):
50
        f = self.ref
51
        norm = np.sqrt(mag @ mag)
52
        if f.mag_gate is not None and abs(norm - 1.0) > f.mag_gate:
53
            return False
54
        mw = qrot(f.q) @ mag
55
        y = wrap_pi(np.arctan2(mw[1], mw[0]) - np.arctan2(f.m_w[1], f.m_w[0]))
56
        h = np.zeros(6)
57
        h[:3] = f.heading_jacobian()
58
        r = (f.sm / np.hypot(f.m_w[0], f.m_w[1])) ** 2
59
        self._inject(bierman_update(self.U, self.D, h, r, y))
60
        return True