升到高维：多变量卡尔曼滤波的推导与实现

1
"""Model matrices and simulators for the linear examples (chapters 2 and 3)."""
2

3
from pathlib import Path
4

5
import numpy as np
6

7
from .io import write_csv
8

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

11

12
def tilt_bias_model(dt, gyro_sigma, bias_rw_sigma, meas_sigma):
13
    """x = [theta, b], u = omega_m, z = theta + v.
14

15
    gyro_sigma is the per-sample gyro noise std (rad/s), as in chapter 1, so its
16
    continuous white-noise PSD is gyro_sigma^2 * dt. bias_rw_sigma is in rad/s/sqrt(s).
17
    """
18
    F = np.array([[1.0, -dt], [0.0, 1.0]])
19
    B = np.array([[dt], [0.0]])
20
    H = np.array([[1.0, 0.0]])
21
    qg = gyro_sigma**2 * dt
22
    qb = bias_rw_sigma**2
23
    Q = np.array(
24
        [
25
            [qg * dt + qb * dt**3 / 3, -qb * dt**2 / 2],
26
            [-qb * dt**2 / 2, qb * dt],
27
        ]
28
    )
29
    R = np.array([[meas_sigma**2]])
30
    return F, B, H, Q, R
31

32

33
def cv2d_model(dt, qc, meas_sigma):
34
    """x = [px, py, vx, vy], position measurements, white-noise acceleration PSD qc."""
35
    I2, Z2 = np.eye(2), np.zeros((2, 2))
36
    F = np.block([[I2, dt * I2], [Z2, I2]])
37
    H = np.block([[I2, Z2]])
38
    Q = qc * np.block(
39
        [[dt**3 / 3 * I2, dt**2 / 2 * I2], [dt**2 / 2 * I2, dt * I2]]
40
    )
41
    R = meas_sigma**2 * I2
42
    return F, None, H, Q, R

1
def simulate_tilt_bias(seed, n=6000, dt=0.01, gyro_sigma=0.05, meas_sigma=0.05,
2
                       bias0=0.05, bias_rw_sigma=1e-3, write=True, gyro_avg=True):
3
    rng = np.random.default_rng(seed)
4
    t = np.arange(n) * dt
5
    w = 2 * np.pi * 0.25
6
    theta = 0.5 * np.sin(w * t)
7
    if gyro_avg:
8
        # the gyro reports the rate averaged over the sample interval, which is exactly
9
        # what the filter's discrete model theta_k = theta_{k-1} + dt * rate assumes
10
        omega = (theta - 0.5 * np.sin(w * (t - dt))) / dt
11
    else:
12
        omega = 0.5 * w * np.cos(w * t)  # instantaneous rate at the sample time
13
    bias = bias0 + np.concatenate(([0.0], np.cumsum(bias_rw_sigma * np.sqrt(dt) * rng.standard_normal(n - 1))))
14
    omega_m = omega + bias + gyro_sigma * rng.standard_normal(n)
15
    z = theta + meas_sigma * rng.standard_normal(n)
16
    cols = {"t": t, "omega_m": omega_m, "z": z, "theta": theta, "bias": bias}
17
    if write:
18
        write_csv(ROOT / "data" / f"ch02_tilt_bias_seed{seed}.csv", cols)
19
    return cols
20

21

22
def simulate_cv(seed, n=200, dt=0.1, qc=0.1, meas_sigma=1.0, write=True):
23
    rng = np.random.default_rng(seed)
24
    F, _, H, Q, R = cv2d_model(dt, qc, meas_sigma)
25
    L = np.linalg.cholesky(Q)
26
    x = np.array([0.0, 0.0, 1.0, 0.5])
27
    truth = np.empty((n, 4))
28
    for k in range(n):
29
        truth[k] = x
30
        x = F @ x + L @ rng.standard_normal(4)
31
    z = truth[:, :2] + meas_sigma * rng.standard_normal((n, 2))
32
    cols = {
33
        "t": np.arange(n) * dt,
34
        "zx": z[:, 0], "zy": z[:, 1],
35
        "px": truth[:, 0], "py": truth[:, 1], "vx": truth[:, 2], "vy": truth[:, 3],
36
    }
37
    if write:
38
        write_csv(ROOT / "data" / f"ch02_cv_seed{seed}.csv", cols)
39
    return cols

1
"""Generic linear Kalman filter (predict / update), Joseph form, Cholesky solve."""
2

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

6

7
class KalmanFilter:
8
    """x- = F x + B u,  P- = F P F' + Q;  K = P- H' S^-1,  S = H P- H' + R."""
9

10
    def __init__(self, F, B, H, Q, R, x0, P0):
11
        self.F = np.asarray(F, dtype=float)
12
        n = self.F.shape[0]
13
        self.B = np.zeros((n, 0)) if B is None else np.asarray(B, dtype=float)
14
        self.H = np.asarray(H, dtype=float)
15
        self.Q = np.asarray(Q, dtype=float)
16
        self.R = np.asarray(R, dtype=float)
17
        self.x = np.asarray(x0, dtype=float).copy()
18
        self.P = np.asarray(P0, dtype=float).copy()
19

20
    def predict(self, u=None):
21
        self.x = self.F @ self.x
22
        if u is not None and self.B.shape[1]:
23
            self.x = self.x + self.B @ np.atleast_1d(u)
24
        self.P = self.F @ self.P @ self.F.T + self.Q
25

26
    def update(self, z, joseph=True):
27
        """Returns (y, S): innovation and its covariance. Raises LinAlgError if S is not SPD."""
28
        z = np.atleast_1d(z)
29
        n = self.x.shape[0]
30
        y = z - self.H @ self.x
31
        S = self.H @ self.P @ self.H.T + self.R
32
        c = cho_factor(S, lower=True)  # raises LinAlgError when not positive definite
33
        # S K' = H P-  (P- symmetric)  =>  K' = S^-1 H P-
34
        K = cho_solve(c, self.H @ self.P).T
35
        self.x = self.x + K @ y
36
        A = np.eye(n) - K @ self.H
37
        if joseph:
38
            self.P = A @ self.P @ A.T + K @ self.R @ K.T
39
            self.P = 0.5 * (self.P + self.P.T)
40
        else:
41
            self.P = A @ self.P
42
        return y, S
43

44

45
def nis(y, S):
46
    """Normalized innovation squared y' S^-1 y via Cholesky."""
47
    c = cho_factor(np.atleast_2d(S), lower=True)
48
    return float(y @ cho_solve(c, y))

1
/* Small dense matrices: row-major kl_real arrays with explicit dimensions.
2
 * Output buffers must not alias input buffers (except for the in-place routines). */
3
#ifndef KL_MAT_H
4
#define KL_MAT_H
5

6
#include "kl/kl_real.h"
7

8
/* C = A B; A is n x m, B is m x p, C is n x p. */
9
void kl_mat_mul(const kl_real *A, const kl_real *B, kl_real *C, int n, int m, int p);
10
/* C = A B^T; A is n x m, B is p x m, C is n x p. */
11
void kl_mat_mul_bt(const kl_real *A, const kl_real *B, kl_real *C, int n, int m, int p);
12
/* C = A + B and C = A - B; all n x m. */
13
void kl_mat_add(const kl_real *A, const kl_real *B, kl_real *C, int n, int m);
14
void kl_mat_sub(const kl_real *A, const kl_real *B, kl_real *C, int n, int m);
15
/* C = s A; both n x m. */
16
void kl_mat_scale(const kl_real *A, kl_real s, kl_real *C, int n, int m);
17
/* C = A^T; A is n x m, C is m x n. */
18
void kl_mat_transpose(const kl_real *A, kl_real *C, int n, int m);
19
/* A = I (n x n). */
20
void kl_mat_identity(kl_real *A, int n);
21
/* C = A; both n x m. */
22
void kl_mat_copy(const kl_real *A, kl_real *C, int n, int m);
23
/* A = (A + A^T) / 2 in place; A is n x n. */
24
void kl_mat_symmetrize(kl_real *A, int n);
25

26
/* In-place lower Cholesky A = L L^T for symmetric positive definite A (n x n).
27
 * Only the lower triangle of A is read; on success it holds L and the strict upper
28
 * triangle is zeroed. Returns 0 on success, -1 if a pivot is <= 0 (A is then garbage). */
29
int kl_chol(kl_real *A, int n);
30
/* Solves (L L^T) X = B in place. L is the n x n factor from kl_chol (lower triangle used),
31
 * B is n x ncols and is overwritten with X. */
32
void kl_chol_solve(const kl_real *L, kl_real *B, int n, int ncols);
33

34
#endif

1
/* Generic linear Kalman filter. The caller owns every buffer (the library never allocates).
2
 * All matrices are row-major: x n, P n x n, F n x n, B n x p, H m x n, Q n x n, R m x m. */
3
#ifndef KL_KF_H
4
#define KL_KF_H
5

6
#include "kl/kl_real.h"
7

8
/* Workspace (kl_real count) used by kl_kf_update, laid out in this order:
9
 *   y m | S m*m | L m*m | HP m*n | Kt m*n | K n*m | A n*n | T n*n | U n*n | KR n*m
10
 * (kl_kf_predict only needs a prefix of the same buffer). */
11
#define KL_KF_WORK_LEN(n, m) ((m) + 2 * (m) * (m) + 4 * (m) * (n) + 3 * (n) * (n))
12

13
typedef struct {
14
  int n, m, p; /* state, measurement, control dimensions (p may be 0) */
15
  kl_real *x, *P, *F, *B, *H, *Q, *R;
16
  kl_real *work; /* KL_KF_WORK_LEN(n, m) entries */
17
} kl_kf;
18

19
/* x = F x + B u, P = F P F^T + Q. u may be NULL when p == 0. */
20
void kl_kf_predict(kl_kf *kf, const kl_real *u);
21

22
/* Joseph-form update. y = z - H x, S = H P H^T + R, K^T = S^-1 (H P) by Cholesky,
23
 * x += K y, P = A P A^T + K R K^T with A = I - K H, then P is symmetrized.
24
 * Returns -1 if S is not positive definite (x and P are left unchanged), else 0.
25
 * y_out (m) and S_out (m x m) receive the innovation and its covariance; either may be NULL. */
26
int kl_kf_update(kl_kf *kf, const kl_real *z, kl_real *y_out, kl_real *S_out);
27

28
/* Same as kl_kf_update but with the textbook short form P = (I - K H) P:
29
 * no Joseph terms and no symmetrization. Same failure behavior and workspace. */
30
int kl_kf_update_short(kl_kf *kf, const kl_real *z, kl_real *y_out, kl_real *S_out);
31

32
/* Workspace (kl_real count) for kl_kf_steady_gain:
33
 *   P n*n | T n*n | U n*n | V n*n | S m*m | L m*m | HP m*n | Kt m*n | K n*m */
34
#define KL_STEADY_WORK_LEN(n, m) (4 * (n) * (n) + 2 * (m) * (m) + 3 * (m) * (n))
35

36
/* Iterates the Riccati recursion P <- F (P - K H P) F^T + Q from P = Q until
37
 * max|dP| <= tol * max|P|, then returns the gain K (n x m) and prior covariance P_out (n x n,
38
 * may be NULL) at convergence. Returns -1 if it does not converge or S is not SPD. */
39
int kl_kf_steady_gain(const kl_real *F, const kl_real *H, const kl_real *Q, const kl_real *R, int n,
40
          int m, kl_real tol, int max_iter, kl_real *K_out, kl_real *P_out,
41
          kl_real *work);
42

43
#endif

1
void kl_kf_predict(kl_kf *kf, const kl_real *u)
2
{
3
  int n = kf->n;
4
  kl_real *fx = kf->work;      /* n   */
5
  kl_real *fp = fx + n;        /* n*n */
6
  kl_real *fpft = fp + n * n;  /* n*n */
7

8
  kl_mat_mul(kf->F, kf->x, fx, n, n, 1);
9
  if (kf->p > 0) {
10
    kl_real *bu = fpft + n * n; /* n */
11
    kl_mat_mul(kf->B, u, bu, n, kf->p, 1);
12
    kl_mat_add(fx, bu, kf->x, n, 1);
13
  } else {
14
    kl_mat_copy(fx, kf->x, n, 1);
15
  }
16

17
  kl_mat_mul(kf->F, kf->P, fp, n, n, n);
18
  kl_mat_mul_bt(fp, kf->F, fpft, n, n, n);
19
  kl_mat_add(fpft, kf->Q, kf->P, n, n);
20
}
21

22
/* Shared by both update forms: y, S, K^T = S^-1 H P by Cholesky, then x += K y.
23
 * Returns -1 before touching x on failure. K ends up at the workspace offset used below. */
24
static int kf_gain_and_state(kl_kf *kf, const kl_real *z, kl_real *y_out, kl_real *S_out)
25
{
26
  int n = kf->n, m = kf->m;
27
  kl_real *y = kf->work;
28
  kl_real *S = y + m;
29
  kl_real *L = S + m * m;
30
  kl_real *HP = L + m * m;
31
  kl_real *Kt = HP + m * n;
32
  kl_real *K = Kt + m * n;
33
  kl_real *T = K + n * m + n * n; /* skips A */
34
  kl_real *U = T + n * n;
35

36
  /* y = z - H x */
37
  kl_mat_mul(kf->H, kf->x, S, m, n, 1); /* S is scratch here */
38
  kl_mat_sub(z, S, y, m, 1);
39

40
  /* S = H P H^T + R */
41
  kl_mat_mul(kf->H, kf->P, HP, m, n, n);
42
  kl_mat_mul_bt(HP, kf->H, L, m, n, m); /* L is scratch until the copy below */
43
  kl_mat_add(L, kf->R, S, m, m);
44

45
  /* K^T = S^-1 H P, so K never needs an explicit inverse */
46
  kl_mat_copy(S, L, m, m);
47
  if (kl_chol(L, m) != 0)
48
    return -1;
49
  kl_mat_copy(HP, Kt, m, n);
50
  kl_chol_solve(L, Kt, m, n);
51
  kl_mat_transpose(Kt, K, m, n);
52

53
  if (y_out)
54
    kl_mat_copy(y, y_out, m, 1);
55
  if (S_out)
56
    kl_mat_copy(S, S_out, m, m);
57

58
  /* x += K y */
59
  kl_mat_mul(K, y, T, n, m, 1);
60
  kl_mat_add(kf->x, T, U, n, 1);
61
  kl_mat_copy(U, kf->x, n, 1);
62
  return 0;
63
}
64

65
int kl_kf_update(kl_kf *kf, const kl_real *z, kl_real *y_out, kl_real *S_out)
66
{
67
  int n = kf->n, m = kf->m;
68
  if (kf_gain_and_state(kf, z, y_out, S_out) != 0)
69
    return -1;
70
  kl_real *K = kf->work + m + 2 * m * m + 2 * m * n;
71
  kl_real *A = K + n * m;
72
  kl_real *T = A + n * n;
73
  kl_real *U = T + n * n;
74
  kl_real *KR = U + n * n;
75

76
  /* Joseph form: P = A P A^T + K R K^T, A = I - K H */
77
  kl_mat_mul(K, kf->H, T, n, m, n);
78
  kl_mat_identity(U, n);
79
  kl_mat_sub(U, T, A, n, n);
80
  kl_mat_mul(A, kf->P, T, n, n, n);
81
  kl_mat_mul_bt(T, A, U, n, n, n);
82
  kl_mat_mul(K, kf->R, KR, n, m, m);
83
  kl_mat_mul_bt(KR, K, T, n, m, n);
84
  kl_mat_add(U, T, kf->P, n, n);
85
  kl_mat_symmetrize(kf->P, n);
86
  return 0;
87
}