自动控制 / 系统课时
用 Joseph 形式维护协方差稳定
用 Joseph 形式维护协方差稳定
协方差出现不对称或负特征值时,估计曲线暂时正常也可能隐藏数值错误。本篇用同一份先验比较简化更新与 Joseph 形式,复现单精度计算中的方差丢失,并加入对称性、半正定性与参考值检查。
学习目标
- 将简化协方差更新替换为 Joseph 形式,正确使用同一份先验、增益和测量噪声。
- 运行
float32与float64对比,解释为什么位置方差可能被错误地算成零。 - 检查原始对称误差与最小特征值,区分结构检查通过和数值结果准确。
简化更新为何会丢失方差
设状态 ,位置用米、速度用米每秒。 是观测前的先验协方差, 是观测后的后验协方差;位置观测取 , 是测量噪声方差。
在先验误差与当前测量噪声不相关的标准模型下,新息协方差、卡尔曼增益和简化更新为:
取 、。精确计算得到位置增益 ,位置后验方差约为 。但 float32 会把 舍入成 ,位置增益变成 ,简化式便得到零方差。
这个结果仍然对称、半正定,却错误地声称位置没有不确定性。更复杂的矩阵还可能因舍入产生非对称项或负特征值,因此不能只检查对角线有没有负数。
Joseph 形式保留两条误差支路
令 ,Joseph 更新为:
第一项是更新后保留的先验误差协方差,第二项是测量噪声经过增益引入的误差协方差。若 、 对称半正定,两项在精确算术下都半正定,相加后仍半正定;它避免直接从大协方差中减去一个接近它的矩阵。
使用与同一组 一致的标准增益时,两种公式在精确算术下等价。Joseph 形式也适用于预先确定的非最优增益;有限精度计算仍可能出错,不能把它当成无条件保证。genui{"citation":{"ref":"turn8view0"}}

在前面的数值例子中,Joseph 形式的位置第一项变为 ,第二项仍保留 ,因此得到约为 的位置方差。它保存了简化式因相消而丢失的信息。
先检查结构,再判断精度
对称要求 ;半正定要求所有特征值非负,允许出现零特征值。实际检查按以下顺序进行,不能先对称化再检查原始误差:
- 用
np.isfinite(P).all()排除NaN和无穷大。 - 记录 ,判断原始矩阵是否明显不对称。
- 对 计算最小特征值,要求 ,同时要求 。
np.linalg.eigvalsh 面向实对称矩阵,默认只读取下三角。直接把非对称矩阵交给它,会遗漏另一侧的不一致,因此代码先记录对称误差,再检查对称部分。genui{"citation":{"ref":"turn3view1"}}
本例取 ,其中 是状态维数, 来自更新所用数据类型。范数使用待检矩阵本身,系数 是演示用余量,不能视为通用标准。
本例按位置 米、速度 米每秒作为固定尺度,数值无需改变。迁移到其他量纲或极小协方差时,应先按固定工程尺度归一化,并重新确定绝对容差,避免阈值掩盖小方向上的错误。
运行同一份先验的对比实验
将下面代码保存为 joseph_demo.py,在已安装 NumPy 的 Python 3 环境运行 python joseph_demo.py。输出已按 NumPy 2.3.5 验证;最后的 bad 矩阵用于检查“对角线全正但并非半正定”的情况。
增益通过 np.linalg.solve 解线性方程得到,右端保持二维形状。该接口要求系数矩阵非奇异;示例的 是正数构成的 矩阵。genui{"citation":{"ref":"turn3view2"}}
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}')预期输出:
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=Falseok=True 只表示结构检查在容差内通过。第一行虽然通过,位置方差却与解析参考值相差约 ;转成 float64 做诊断无法找回已经丢失的精度。最后一行说明:对称且对角线全正,也不足以证明半正定。
把检查接入实际滤波循环
- 保留更新前的 ,先计算 ,再一次性形成 Joseph 后验。不要先把先验覆盖成简化结果,再用它计算 Joseph;状态均值仍按 更新。
- 对原始后验记录对称误差和最小特征值。只有非对称部分处于舍入容差内时,才用
0.5 * (P + P.T)清理它;这一步不会修复显著负特征值。 - 若检查失败,阻止异常后验进入下一拍,记录 便于回放;仅当当前先验有效时才保留先验。优先检查矩阵维度、单位、数据精度,以及 、过程噪声协方差 和 的对称半正定性,不能靠对角线取绝对值掩盖错误。
本例的 float32 用于暴露问题,实际实现可优先统一使用 float64。若 正定且 半正定,则精确算术下 正定;若求解失败,应检查输入和条件数,而不是随意修改测量噪声。
对位置、速度这类两维状态,可以每拍检查。直接进行密集 Joseph 乘法和完整特征值检查,固定观测维数时计算量为 、存储量为 ;状态维数很大时,应根据预算安排完整谱检查,并用解析算例或高精度回放验证误差。