自动控制 / 系统课时

用 Joseph 形式维护协方差稳定

用 Joseph 形式维护协方差稳定

协方差出现不对称或负特征值时,估计曲线暂时正常也可能隐藏数值错误。本篇用同一份先验比较简化更新与 Joseph 形式,复现单精度计算中的方差丢失,并加入对称性、半正定性与参考值检查。

学习目标

  • 将简化协方差更新替换为 Joseph 形式,正确使用同一份先验、增益和测量噪声。
  • 运行 float32 与 float64 对比,解释为什么位置方差可能被错误地算成零。
  • 检查原始对称误差与最小特征值,区分结构检查通过和数值结果准确。

简化更新为何会丢失方差

设状态 x=[p,v]Tx=[p,v]^T,位置用米、速度用米每秒。P−P^- 是观测前的先验协方差,P+P^+ 是观测后的后验协方差;位置观测取 H=[1  0]H=[1\;0],RR 是测量噪声方差。

在先验误差与当前测量噪声不相关的标准模型下,新息协方差、卡尔曼增益和简化更新为:

S=HP−HT+R,K=P−HTS−1,Psimple+=(I−KH)P−.S=HP^-H^T+R,\qquad K=P^-H^TS^{-1},\qquad P^+_{\mathrm{simple}}=(I-KH)P^-.

取 P−=diag⁡(108,1)P^-=\operatorname{diag}(10^8,1)、R=[1]R=[1]。精确计算得到位置增益 108/(108+1)10^8/(10^8+1),位置后验方差约为 0.999999990.99999999。但 float32 会把 108+110^8+1 舍入成 10810^8,位置增益变成 11,简化式便得到零方差。

这个结果仍然对称、半正定,却错误地声称位置没有不确定性。更复杂的矩阵还可能因舍入产生非对称项或负特征值,因此不能只检查对角线有没有负数。

Joseph 形式保留两条误差支路

令 A=I−KHA=I-KH,Joseph 更新为:

PJoseph+=AP−AT+KRKT.P^+_{\mathrm{Joseph}}=AP^-A^T+KRK^T.

第一项是更新后保留的先验误差协方差,第二项是测量噪声经过增益引入的误差协方差。若 P−P^-、RR 对称半正定,两项在精确算术下都半正定,相加后仍半正定;它避免直接从大协方差中减去一个接近它的矩阵。

使用与同一组 P−,H,RP^-,H,R 一致的标准增益时,两种公式在精确算术下等价。Joseph 形式也适用于预先确定的非最优增益;有限精度计算仍可能出错,不能把它当成无条件保证。genui{"citation":{"ref":"turn8view0"}}

先验误差与测量噪声分别传播,相加形成后验协方差

在前面的数值例子中,Joseph 形式的位置第一项变为 00,第二项仍保留 11,因此得到约为 11 的位置方差。它保存了简化式因相消而丢失的信息。

先检查结构,再判断精度

对称要求 P=PTP=P^T;半正定要求所有特征值非负,允许出现零特征值。实际检查按以下顺序进行,不能先对称化再检查原始误差:

  1. 用 np.isfinite(P).all() 排除 NaN 和无穷大。
  2. 记录 δ=∥P−PT∥∞\delta=\lVert P-P^T\rVert_\infty,判断原始矩阵是否明显不对称。
  3. 对 Ps=(P+PT)/2P_s=(P+P^T)/2 计算最小特征值,要求 λmin⁡(Ps)≥−τ\lambda_{\min}(P_s)\geq-\tau,同时要求 δ≤τ\delta\leq\tau。

np.linalg.eigvalsh 面向实对称矩阵,默认只读取下三角。直接把非对称矩阵交给它,会遗漏另一侧的不一致,因此代码先记录对称误差,再检查对称部分。genui{"citation":{"ref":"turn3view1"}}

本例取 τ=100nϵmax⁡(1,∥P∥∞)\tau=100n\epsilon\max(1,\lVert P\rVert_\infty),其中 nn 是状态维数,ϵ\epsilon 来自更新所用数据类型。范数使用待检矩阵本身,系数 100100 是演示用余量,不能视为通用标准。

本例按位置 11 米、速度 11 米每秒作为固定尺度,数值无需改变。迁移到其他量纲或极小协方差时,应先按固定工程尺度归一化,并重新确定绝对容差,避免阈值掩盖小方向上的错误。

运行同一份先验的对比实验

将下面代码保存为 joseph_demo.py,在已安装 NumPy 的 Python 3 环境运行 python joseph_demo.py。输出已按 NumPy 2.3.5 验证;最后的 bad 矩阵用于检查“对角线全正但并非半正定”的情况。

增益通过 np.linalg.solve 解线性方程得到,右端保持二维形状。该接口要求系数矩阵非奇异;示例的 SS 是正数构成的 1×11\times1 矩阵。genui{"citation":{"ref":"turn3view2"}}

python
import numpy as np

def inspect_cov(P):
    # 容差按原始运算精度确定;转成 float64 只用于诊断。
    eps = float(np.finfo(P.dtype).eps)
    M = P.astype(np.float64)
    if not np.isfinite(M).all():
        raise ValueError('non-finite covariance')
    scale = max(1.0, float(np.linalg.norm(M, ord=np.inf)))
    tol = 100 * P.shape[0] * eps * scale
    asym = float(np.linalg.norm(M - M.T, ord=np.inf))
    # 先保留非对称误差,再检查对称部分,避免只读一侧三角。
    Ps = 0.5 * (M + M.T)
    low = float(np.linalg.eigvalsh(Ps)[0])
    return asym, low, bool(asym <= tol and low >= -tol)

def compare(dtype):
    # 所有计算数组使用同一精度,两种更新共享同一份先验。
    P = np.array([[1e8, 0], [0, 1]], dtype=dtype)
    H = np.array([[1, 0]], dtype=dtype)
    R = np.array([[1]], dtype=dtype)
    PHt = P @ H.T
    S = H @ PHt + R
    # 解 S.T @ K.T = PHt.T,避免显式计算 S 的逆。
    K = np.linalg.solve(S.T, PHt.T).T
    A = np.eye(2, dtype=dtype) - K @ H
    simple = A @ P
    joseph = A @ P @ A.T + K @ R @ K.T
    for name, post in [('simple', simple), ('Joseph', joseph)]:
        asym, low, ok = inspect_cov(post)
        print(f'{dtype.__name__} {name}: P11={post[0, 0]:.8f}')
        print(f'  asym={asym:.1e}, min_eig={low:.8f}, ok={ok}')

if __name__ == '__main__':
    compare(np.float32)
    compare(np.float64)
    # 反例:对角线全正且对称,仍可能存在负特征值。
    bad = np.array([[1, 2], [2, 1]], dtype=np.float64)
    asym, low, ok = inspect_cov(bad)
    print(f'bad: min_eig={low:.8f}, ok={ok}')

预期输出:

text
float32 simple: P11=0.00000000
  asym=0.0e+00, min_eig=0.00000000, ok=True
float32 Joseph: P11=1.00000000
  asym=0.0e+00, min_eig=1.00000000, ok=True
float64 simple: P11=0.99999999
  asym=0.0e+00, min_eig=0.99999999, ok=True
float64 Joseph: P11=0.99999999
  asym=0.0e+00, min_eig=0.99999999, ok=True
bad: min_eig=-1.00000000, ok=False

ok=True 只表示结构检查在容差内通过。第一行虽然通过,位置方差却与解析参考值相差约 11;转成 float64 做诊断无法找回已经丢失的精度。最后一行说明:对称且对角线全正,也不足以证明半正定。

把检查接入实际滤波循环

  1. 保留更新前的 P−P^-,先计算 S,K,AS,K,A,再一次性形成 Joseph 后验。不要先把先验覆盖成简化结果,再用它计算 Joseph;状态均值仍按 x^+=x^−+K(z−Hx^−)\hat x^+=\hat x^-+K(z-H\hat x^-) 更新。
  2. 对原始后验记录对称误差和最小特征值。只有非对称部分处于舍入容差内时,才用 0.5 * (P + P.T) 清理它;这一步不会修复显著负特征值。
  3. 若检查失败,阻止异常后验进入下一拍,记录 P−,H,RP^-,H,R 便于回放;仅当当前先验有效时才保留先验。优先检查矩阵维度、单位、数据精度,以及 P−P^-、过程噪声协方差 QQ 和 RR 的对称半正定性,不能靠对角线取绝对值掩盖错误。

本例的 float32 用于暴露问题,实际实现可优先统一使用 float64。若 RR 正定且 P−P^- 半正定,则精确算术下 SS 正定;若求解失败,应检查输入和条件数,而不是随意修改测量噪声。

对位置、速度这类两维状态,可以每拍检查。直接进行密集 Joseph 乘法和完整特征值检查,固定观测维数时计算量为 O(n3)O(n^3)、存储量为 O(n2)O(n^2);状态维数很大时,应根据预算安排完整谱检查,并用解析算例或高精度回放验证误差。

按自己的节奏,确认真正理解。

目录与阅读设置

阅读设置

17

记录一个想法

用 Joseph 形式维护协方差稳定

0 / 10000 · 笔记保存到账号。未提交的修改会在关闭、离开或刷新后丢失。

登录哪都通