滤波算法入门的一维卡尔曼只估计一个缓慢变化的数。如果对象持续移动，用“下一刻位置均值不变”的随机游走模型就可能产生明显滞后。一个自然的进阶是把位置和速度一起放入状态。

这一篇仍归数据分析的时间序列与滤波方向。重点是协方差如何把两个量连接起来：只有位置观测，为什么速度也会得到修正？不同采样间隔下，过程噪声为什么不能机械地加一个固定标量？

先定义状态

位置与速度有不同单位，不是两个可互换的列。

再传播协方差

转移矩阵把速度的不确定性带入位置。

最后看观测

位置创新通过两个增益分量修正联合估计。

位置速度联合预测协方差为2、1、1、1，位置观测1通过增益2/3和1/3把均值修正为2/3与1/3，更新后协方差对角为2/3、交叉项为1/3
位置与速度的误差相关，观测修正才会跨分量传播。 图使用正文第一步数值，轮廓表示相同马氏二次型水平；不是覆盖率声明，也不是实测运动轨迹。查看原图

状态与协方差各在记录什么

状态列向量 x=[p,v]ᵀ，分别表示位置和速度。估计均值 m=[m_p,m_v]ᵀ，误差协方差为：

1
2
P = [[Var(p误差),     Cov(p误差,v误差)],
     [Cov(p误差,v误差), Var(v误差)]]

若位置单位是米、时间单位是秒，三个不同量的单位分别是 m²、m²/s、m²/s²，不能把矩阵元素当成同一种单位的分数。P 必须对称且半正定：对任何方向 a，aᵀPa≥0，因为它表示该线性组合的误差方差。

二维情况下，非负对角和非负行列式给出半正定检查。代码使用归一化行列式的数值容差处理舍入误差，但不会把它当作修复错误协方差的算法。

常速度模型怎样引入位置速度耦合

一个采样间隔为 Δt 时，名义转移为：

1
2
3
F = [[1, Δt], [0, 1]]
m⁻ = F m
P⁻ = F P Fᵀ + Q

即使原来的 P 是对角矩阵，F 也会把速度不确定性传播给位置，并产生交叉协方差。例如 P=I、Δt=1、Q=0，会得到 P⁻=[[2,1],[1,1]]。把两个分量各自做独立标量滤波，会漏掉这条耦合路径。

Q 来自哪一种随机加速度假设

本文假设每个采样区间内有一个恒定随机加速度 a，区间之间独立，均值为零、方差为 σ_a²。它对位置、速度的增量为 g a，其中 g=[Δt²/2,Δt]ᵀ，所以：

1
2
3
Q = σ_a² g gᵀ
  = σ_a² [[Δt⁴/4, Δt³/2],
           [Δt³/2, Δt²]]

这和FilterPy 官方离散噪声实现的二状态定义相同。它与连续白噪声加速度按谱密度积分得到的 Δt³/3、Δt²/2、Δt 形式不是同一模型，不能只因为都叫“加速度噪声”就互换。

对当前模型，改变区间长度需要重新计算 F 和 Q。σ_a² 的单位是加速度的平方；Q 的元素则分别对应位置方差、位置速度协方差和速度方差。下面的实现没有估计 σ_a²，也没有证明某个真实设备符合这套噪声模型。

只有位置观测，为什么速度也会更新

观测方程为 y=Hx+ε，H=[1,0]，观测噪声方差 R>0。创新 e=y−m_p⁻，创新方差 S=P_pp⁻+R，增益是一个列向量：

1
2
K = P⁻ Hᵀ / S = [P_pp⁻/S, P_vp⁻/S]ᵀ
m = m⁻ + K e

速度增益来自交叉协方差 P_vp⁻。它的大小和符号取决于当前模型与协方差；不能概括成“位置增加时速度一定增加”。

一般线性更新的误差协方差为 (I−KH)P⁻(I−KH)ᵀ+KRKᵀ。把它配方，可写成最优更新协方差加上 (K−K*)S(K−K*)ᵀ；S>0，所以 K*=P⁻Hᵀ/S 在半正定意义下最小。这需要预测误差与观测噪声不相关。若还满足线性高斯假设，均值与协方差对应高斯后验；只有二阶矩条件时，不自动得到任意分布的完整后验。

代码保留上述 Joseph 形式，并对输出的两个交叉项取平均，消除浮点计算中的微小不对称；这一步不是删除交叉协方差。

三步手算：第三步没有观测

观测前初始均值 [0,0]、P=I，三步 Δt 都为 1，Q=0，R=1，观测为 [1,2,None]。每一步先预测，第一步也一样。

步骤 预测均值 [p,v] 预测协方差 [Ppp,Ppv,Pvv] 增益 [Kp,Kv] 更新均值 [p,v] 更新协方差 [Ppp,Ppv,Pvv]
1，y=1 [0,0] [2,1,1] [2/3,1/3] [2/3,1/3] [2/3,1/3,2/3]
2，y=2 [1,1/3] [2,1,2/3] [2/3,1/3] [5/3,2/3] [2/3,1/3,1/3]
3，缺失 [7/3,2/3] [5/3,2/3,1/3] [0,0]，跳过 [7/3,2/3] [5/3,2/3,1/3]

缺失时不把观测填成零，均值仍按已有速度推进。第三步的位置方差增大，即使 Q=0 也是如此：已有速度的不确定性仍会传播到位置。这里的 Q=0 不等于“状态已经确定”。

完整标准库实现

这份代码固定为两个状态、一个位置观测，矩阵函数也固定为 2×2；不是通用矩阵库。None 只表示缺失观测，时间间隔必须是有限正数。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
import json
import math


def real(x, name="number"):
    if isinstance(x, bool) or not isinstance(x, (int, float)):
        raise ValueError(name + " must be real")
    try:
        value = float(x)
    except OverflowError as error:
        raise ValueError(name + " is too large") from error
    if not math.isfinite(value):
        raise ValueError(name + " must be finite")
    return value


def pair(values):
    if not isinstance(values, (list, tuple)) or len(values) != 2:
        raise ValueError("two entries required")
    return [real(v) for v in values]


def matrix(values):
    if not isinstance(values, (list, tuple)) or len(values) != 2:
        raise ValueError("2 by 2 matrix required")
    return [pair(row) for row in values]


def total(values):
    try:
        return real(math.fsum(values))
    except (OverflowError, ValueError) as error:
        raise ValueError("invalid intermediate sum") from error


def transpose(a):
    return [[a[j][i] for j in range(2)] for i in range(2)]


def multiply(a, b):
    return [[total(a[i][k]*b[k][j] for k in range(2))
             for j in range(2)] for i in range(2)]


def add(a, b):
    return [[total((a[i][j], b[i][j])) for j in range(2)] for i in range(2)]


def covariance(values):
    p = matrix(values)
    a, b, d = p[0][0], p[0][1], p[1][1]
    if b != p[1][0] or a < 0 or d < 0:
        raise ValueError("symmetric nonnegative diagonal required")
    scale = max(a, d, abs(b))
    if ((a == 0 or d == 0) and b != 0) or (
            scale and (a/scale)*(d/scale)-(b/scale)**2 < -1e-12):
        raise ValueError("covariance is not positive semidefinite")
    return p


def filter_cv(observations, dts, r=1.0, accel_variance=0.0,
              mean=(0.0, 0.0), initial_cov=((1.0, 0.0), (0.0, 1.0))):
    if (not isinstance(observations, (list, tuple)) or not observations
            or not isinstance(dts, (list, tuple)) or len(dts) != len(observations)):
        raise ValueError("matching nonempty sequences required")
    obs = [None if z is None else real(z, "observation") for z in observations]
    dts = [real(dt, "dt") for dt in dts]
    r, av = real(r, "R"), real(accel_variance, "acceleration variance")
    if r <= 0 or av < 0 or any(dt <= 0 for dt in dts):
        raise ValueError("require R>0, acceleration variance>=0 and dt>0")
    mean, p = pair(mean), covariance(initial_cov)
    time, out = 0.0, []
    for z, dt in zip(obs, dts):
        time = total((time, dt))
        f = [[1.0, dt], [0.0, 1.0]]
        g = [real(0.5*dt*dt), dt]
        q = matrix([[av*g[i]*g[j] for j in range(2)] for i in range(2)])
        prediction = [total((mean[0], dt*mean[1])), mean[1]]
        prior = add(multiply(multiply(f, p), transpose(f)), q)
        cross = total((prior[0][1]/2, prior[1][0]/2))
        prior[0][1] = prior[1][0] = cross
        covariance(prior)
        gain, innovation = [0.0, 0.0], None
        mean, p = prediction[:], [row[:] for row in prior]
        if z is not None:
            s = total((prior[0][0], r))
            gain = [prior[0][0]/s, prior[1][0]/s]
            innovation = total((z, -prediction[0]))
            mean = [total((prediction[i], gain[i]*innovation)) for i in range(2)]
            a = [[1-gain[0], 0.0], [-gain[1], 1.0]]
            noise = [[gain[i]*r*gain[j] for j in range(2)] for i in range(2)]
            p = add(multiply(multiply(a, prior), transpose(a)), noise)
            cross = total((p[0][1]/2, p[1][0]/2))
            p[0][1] = p[1][0] = cross
            covariance(p)
        out.append({"time": time, "prediction": prediction,
                    "prior_covariance": prior, "gain": gain,
                    "innovation": innovation, "mean": mean[:],
                    "covariance": [row[:] for row in p]})
    return out


if __name__ == "__main__":
    print(json.dumps({
        "hand": filter_cv([1, 2, None], [1, 1, 1]),
        "irregular": filter_cv([0.8, 2.2, None, 4.1, 5.2],
                               [1, 1, 0.5, 1, 1.5], r=0.25,
                               accel_variance=0.1, mean=(0, 1))
    }))

固定二维状态时，每步工作为常数，总时间 O(T)、递推状态空间 O(1)；输入副本及返回轨迹另占 O(T)。不能把这个复杂度直接推广成任意状态维度的常数成本。代码做有限性检查，但不是高条件数问题的数值线性代数保障。

验证程序 tests/verify-kalman-cv-article.cjs 对 240 组随机模型，用初态与各区间随机加速度的联合批量高斯消元，独立核对 881 次后验均值和协方差；另检查 Q=0、缺失、因果前缀、位置平移、确定初态及 21 类非法输入。图中轮廓也按实际协方差的二次型核验，不是任意画出的椭圆。

失败边界与验收

现象 应检查什么
位置看似合理，速度乱跳 时间单位、交叉协方差、初始速度方差和 R
丢帧后仍使用固定时间间隔 F、Q 是否按实际区间重算
不确定性越来越小但误差很大 噪声参数或运动模型可能失配，P 不是自动校准的真实误差
忽略所有非对角元素 可能切断观测对其他状态的修正路径
更换加速度噪声定义后复用原 Q 区间常量噪声与连续白噪声积分不能混用
异常观测把状态拉偏 这里没有鲁棒门控或异常分布模型
想使用未来数据改善历史估计 那涉及平滑器，不能作为在线滤波同一时刻的结果
自检：没有速度传感器，速度增益为什么仍可能非零？

预测使位置误差与速度误差相关。位置创新通过 P_vp⁻/S 进入速度更新；这是模型和协方差共同产生的推断，不是凭空增加一条速度测量。

用一维滤波的三步手算对照这里的矩阵计算，再用图表表达同时画出位置、速度及其不确定性，能比只看一条“更平滑”的位置曲线发现更多问题。