如何使用 NIS 判断观测是否可靠：从卡尔曼滤波推导到机器人实践

核心问题：考虑了预测与传感器各自的不确定性之后，本次观测与预测之间的差异，是否大得不合理？

NIS（Normalized Innovation Squared，归一化创新平方）检验的是观测与当前预测、噪声模型之间的统计一致性，不是直接判断观测是否接近真实值。它不需要真实状态，适合在线使用；但也正因为没有真实值，它不能仅凭一次检验就证明“传感器错了，预测一定是对的”。

本文从创新和协方差的定义出发，逐步推导 NIS 与卡方门限；随后用固定数字走完整个预测、门控和更新过程，再讨论相关误差、接触约束、走廊退化、多帧统计与 ESKF 实现。所有数值例子均为教学构造，不代表某一具体传感器的标定参数。

1. NIS 到底在判断什么？

机器人通过 IMU、运动模型或者上一时刻状态，得到当前状态的预测；相机、激光雷达、轮速计等传感器又提供了新的观测。两者通常不会完全相同。

最简单的想法是：

观测与预测差得越多，观测就越不可靠。

但这个判断遗漏了一个关键问题：预测本身有多准，传感器本身又有多准？

例如，同样是位置差了

0.5

0.5\\,\\mathrm m

$0.5 m$ ：

预测标准差只有 $0.01\\,\\mathrm m 0.01m，传感器标准差也只有 0.01 m 0.01\\,\\mathrm m 0.01m，这个差异非常异常。$
预测标准差已经达到 $2\\,\\mathrm m 2m，那么差了 0.5 m 0.5\\,\\mathrm m 0.5m 并不奇怪。$

因此，不能只比较原始距离，而应该比较：

差异的大小

相对于

这一差异本来应该具有的不确定性

\\text{差异的大小} \\quad\\text{相对于}\\quad \\text{这一差异本来应该具有的不确定性}

$差异的大小相对于这一差异本来应该具有的不确定性$

这正是 NIS 所做的事情。

还需要区分三个问题：

问题NIS 能否单独回答？

当前观测与预测及其协方差模型是否一致？	可以提供统计检验证据
当前观测是否接近真实值？	不能，因为没有直接使用真实值
当前观测能否显著提高所有状态方向的精度？	不能，还要看雅可比、协方差、可观性与增益

通过 NIS 门控，不等于观测精度高；NIS 超限，也不等于已经定位到传感器故障。

2. 先分清楚：状态误差、测量噪声和创新

2.1 符号约定

本文使用以下符号：

符号含义

	真实状态
$\\hat x^- x^−$	使用当前观测之前的预测状态
$P^- P−$	预测状态误差的协方差
	当前实际观测
	观测模型
	测量噪声
	测量噪声协方差
	观测模型对状态误差的一阶雅可比
$\\nu ν$	创新，即实际观测减去预测观测
	创新协方差
	创新向量的维数，在正定情形下也是 NIS 的自由度
$\\epsilon ϵ$	NIS 数值
$\\tau τ$	NIS 门限

上标 “

−

–

$-$ ” 表示当前观测更新之前；上标 “

$+$ ” 表示更新之后。

这里用

$J$ 表示观测雅可比，避免与退化分析中常见的信息矩阵

H=J^T J

$H = J^{T} J$ 混淆。

2.2 状态预测误差

假设机器人已经获得预测状态：

−

\\hat x^-

$\overset{x}{^}^{-}$

定义真实状态与预测状态之间的误差：

−

\\tilde x=x-\\hat x^-

$\tilde{x} = x - \overset{x}{^}^{-}$

假设预测无偏：

[

]

[

]

−

E[\\tilde x]=0, \\qquad E[\\tilde x\\tilde x^T]=P^-

$E [\tilde{x}] = 0, E [\tilde{x} \tilde{x}^{T}] = P^{-}$

对于 ESKF，上式中的减法应理解为与误差状态定义一致的局部差异。姿态状态通常不能简单地用四元数的四个分量直接相减；本文先用欧氏空间写法解释基本统计关系。

2.3 测量噪声

传感器模型为：

(

)

z=h(x)+n,

$z = h (x) + n,$

其中：

[

]

[

]

E[n]=0, \\qquad E[nn^T]=R

$E [n] = 0, E [n n^{T}] = R$

测量噪声是实际观测与真实状态对应的理想观测之间的差：

−

(

)

n=z-h(x)

$n = z - h (x)$

由于不知道真实状态

$x$ ，通常也无法直接得到每次观测的真实测量噪声。

2.4 创新

用预测状态计算“传感器应该测到什么”：

−

(

−

)

\\hat z^-=h(\\hat x^-)

$\overset{z}{^}^{-} = h (\overset{x}{^}^{-})$

实际观测与预测观测之间的差：

−

(

−

)

\\boxed{\\nu=z-h(\\hat x^-)}

$ν = z - h (\overset{x}{^}^{-})$

就叫做创新，innovation。

三者的区别是：

量定义在线能否直接获得？

状态误差	$\\tilde x=x-\\hat x^- x~=x−x^−$	通常不能，因为不知道真实状态
测量噪声		通常不能，因为不知道真实状态
创新	$\\nu=z-h(\\hat x^-) ν=z−h(x^−)$	可以，观测和预测都有

NIS 使用创新，正是因为创新可以在线计算。

2.5 创新中到底包含什么误差？

对观测模型做一阶展开：

(

)

(

−

)

≈

(

−

)

h(x) =h(\\hat x^-+\\tilde x) \\approx h(\\hat x^-)+J\\tilde x,

$h (x) = h (\overset{x}{^}^{-} + \tilde{x}) \approx h (\overset{x}{^}^{-}) + J \tilde{x},$

其中：

∂

∣

−

J=\\left.\\frac{\\partial h}{\\partial x}\\right|_{\\hat x^-}

$J = \frac{\partial h}{\partial x}$

x^−

代入创新：

−

(

−

)

(

)

−

(

−

)

≈

\\begin{aligned} \\nu &=z-h(\\hat x^-)\\\\ &=h(x)+n-h(\\hat x^-)\\\\ &\\approx J\\tilde x+n \\end{aligned}

$ν = z - h (\overset{x}{^}^{-}) = h (x) + n - h (\overset{x}{^}^{-}) \approx J \tilde{x} + n$

因此：

创新

≈

状态预测误差投影到观测空间

测量噪声

\\boxed{ \\text{创新} \\approx \\text{状态预测误差投影到观测空间} + \\text{测量噪声} }

$创新 \approx 状态预测误差投影到观测空间 + 测量噪声$

这说明：创新大，既可能是传感器存在问题，也可能是状态预测存在问题。

3. 推导创新协方差：为什么是 $S=JP^-J^T+R S=JP−JT+R？$

3.1 从协方差定义展开

由：

≈

\\nu\\approx J\\tilde x+n,

$ν \approx J \tilde{x} + n,$

在零均值假设下，创新协方差为：

[

]

S=E[\\nu\\nu^T]

$S = E [ν ν^{T}]$

展开得到：

≈

[

(

)

(

)

]

[

]

[

]

[

]

[

]

\\begin{aligned} S &\\approx E[(J\\tilde x+n)(J\\tilde x+n)^T]\\\\ &=JE[\\tilde x\\tilde x^T]J^T +JE[\\tilde x n^T]\\\\ &\\quad+E[n\\tilde x^T]J^T +E[nn^T] \\end{aligned}

$S \approx E [(J \tilde{x} + n) (J \tilde{x} + n)^{T}] = J E [\tilde{x} \tilde{x}^{T}] J^{T} + J E [\tilde{x} n^{T}] + E [n \tilde{x}^{T}] J^{T} + E [n n^{T}]$

如果当前测量噪声与预测误差不相关：

[

]

E[\\tilde x n^T]=0,

$E [\tilde{x} n^{T}] = 0,$

则交叉项消失：

−

\\boxed{S=JP^-J^T+R}

$S = J P^{-} J^{T} + R$

两个部分分别是：

−

⏟

预测观测的不确定性

⏟

测量噪声的不确定性

\\underbrace{JP^-J^T}_{\\text{预测观测的不确定性}} + \\underbrace{R}_{\\text{测量噪声的不确定性}}

$预测观测的不确定性$

JP−JT+测量噪声的不确定性

3.2 为什么要乘

因为

−

P^-

$P^{-}$ 描述的是状态空间里的误差，创新却在观测空间中。

假设状态有 15 维，但当前只观测三维速度，那么：

−

∈

P^-\\in\\mathbb R^{15\\times15}, \\qquad J\\in\\mathbb R^{3\\times15}, \\qquad S\\in\\mathbb R^{3\\times3}

$P^{-} \in R^{15 \times 15}, J \in R^{3 \times 15}, S \in R^{3 \times 3}$

$J$ 的作用，是将状态误差的一阶影响映射到当前观测上。因此，参与创新协方差的不是整个

−

P^-

$P^{-}$ 的简单加和，而是：

−

JP^-J^T

$J P^{-} J^{T}$

3.3 一维数值例子

假设预测位置的标准差为：

0.2

\\sigma_{\\mathrm{pred}}=0.2\\,\\mathrm m,

$σ_{pred} = 0.2 m,$

传感器测量位置的标准差为：

0.1

\\sigma_{\\mathrm{sensor}}=0.1\\,\\mathrm m

$σ_{sensor} = 0.1 m$

直接测量位置，故

J=1

$J = 1$ 。于是：

0.2

0.1

0.05

S=0.2^2+0.1^2=0.05\\,\\mathrm{m^2}

$S = 0. 2^{2} + 0. 1^{2} = 0.05 m^{2}$

创新的标准差为：

0.05

≈

0.2236

\\boxed{ \\sigma_\\nu=\\sqrt{0.05}\\approx0.2236\\,\\mathrm m }

$σ_{ν} = 0.05$

≈0.2236m

解释这个数值时，可以这样理解：

预测与观测之间有一定差异是正常的。判断差异大不大，应使用双方误差合成后的

0.2236

0.2236\\,\\mathrm m

$0.2236 m$ ，而不是只使用传感器的

0.1

0.1\\,\\mathrm m

$0.1 m$ 。

所以，标准预测创新检验使用：

−

\\nu^TS^{-1}\\nu,

$ν^{T} S^{- 1} ν,$

而不是直接使用：

−

\\nu^TR^{-1}\\nu

$ν^{T} R^{- 1} ν$

后一种写法忽略了预测状态的不确定性，除非具体问题已经满足可以忽略该项等额外条件，否则不能替代 NIS。

4. 从“几个标准差”推导出 NIS

4.1 一维情况下：标准化创新的平方

设一维创新为

\\nu

$ν$ ，方差为：

S=\\sigma_\\nu^2

$S = σ_{ν}^{2}$

先除以标准差：

w=\\frac{\\nu}{\\sigma_\\nu}

$w = \frac{ν}{σ _{ν}}$

$w$ 表示当前创新相当于多少个创新标准差。

再平方：

\\boxed{ \\epsilon_{\\mathrm{NIS}}=w^2=\\frac{\\nu^2}{S} }

$ϵ_{NIS} = w^{2} = \frac{ν ^{2}}{S}$

例如：

0.6

0.05

\\nu=0.6\\,\\mathrm m, \\qquad S=0.05\\,\\mathrm{m^2}

$ν = 0.6 m, S = 0.05 m^{2}$

于是：

0.6

0.2236

≈

2.683

w=\\frac{0.6}{0.2236}\\approx2.683,

$w = \frac{0.6}{0.2236} \approx 2.683,$

2.683

7.2

\\epsilon_{\\mathrm{NIS}}=2.683^2=7.2

$ϵ_{NIS} = 2.68 3^{2} = 7.2$

NIS 为

7.2

$7.2$ ，不是偏了

7.2

7.2\\sigma

$7.2 σ$ ；在一维情况下，它对应的是约

2.683

2.683\\sigma

$2.683 σ$ 。

4.2 NIS 为什么没有单位？

因为分子是创新平方，分母是创新方差，两者的单位抵消。

把前面例子的米换成厘米：

500

\\nu=60\\,\\mathrm{cm}, \\qquad S=500\\,\\mathrm{cm^2},

$ν = 60 cm, S = 500 c m^{2},$

仍然得到：

500

7.2

\\frac{60^2}{500}=7.2

$\frac{6 0 ^{2}}{500} = 7.2$

因此，NIS 不会因为长度单位从米改成厘米就改变。前提是创新和协方差必须一起进行正确的单位变换。

4.3 多维情况下：为什么不能只逐项除以方差？

当创新各分量相互独立、

$S$ 为对角矩阵时，可以写成：

⋯

\\epsilon_{\\mathrm{NIS}} =\\frac{\\nu_1^2}{S_{11}} +\\frac{\\nu_2^2}{S_{22}} +\\cdots +\\frac{\\nu_m^2}{S_{mm}}

$ϵ_{NIS} = \frac{ν _{1}^{2}}{S _{11}} + \frac{ν _{2}^{2}}{S _{22}} + \dots + \frac{ν _{m}^{2}}{S _{mm}}$

但如果各分量存在相关性，上式会遗漏非对角项包含的信息。

正确做法是白化：既统一尺度，又去除相关性。

4.4 用 Cholesky 分解推导白化

对正定创新协方差进行分解：

S=LL^T,

$S = L L^{T},$

其中

$L$ 是下三角矩阵。

定义：

−

w=L^{-1}\\nu

$w = L^{- 1} ν$

白化创新的协方差为：

Cov

⁡

(

)

−

\\begin{aligned} \\operatorname{Cov}(w) &=L^{-1}SL^{-T}\\\\ &=L^{-1}LL^TL^{-T}\\\\ &=I \\end{aligned}

$Cov (w) = L^{- 1} S L^{- T} = L^{- 1} L L^{T} L^{- T} = I$

如果：

∼

(

)

\\nu\\sim\\mathcal N(0,S),

$ν \sim N (0, S),$

则：

∼

(

)

w\\sim\\mathcal N(0,I)

$w \sim N (0, I)$

在联合高斯假设下，白化后的各分量相互独立，并且都服从标准正态分布。注意：只知道协方差为单位阵、但没有联合高斯等额外条件时，不能仅凭“不相关”就断言“独立”。

计算白化创新的平方长度：

(

−

)

(

−

)

−

\\begin{aligned} w^Tw &=(L^{-1}\\nu)^T(L^{-1}\\nu)\\\\ &=\\nu^TL^{-T}L^{-1}\\nu\\\\ &=\\nu^TS^{-1}\\nu \\end{aligned}

$w^{T} w = (L^{- 1} ν)^{T} (L^{- 1} ν) = ν^{T} L^{- T} L^{- 1} ν = ν^{T} S^{- 1} ν$

因此：

−

\\boxed{ \\epsilon_{\\mathrm{NIS}}=\\nu^TS^{-1}\\nu }

$ϵ_{NIS} = ν^{T} S^{- 1} ν$

NIS 就是创新的马氏距离平方，也就是白化创新的平方长度。

5. 为什么 NIS 要和卡方分布比较？

5.1 卡方分布从哪里来？

设创新有

$m$ 维。白化后：

[

⋯

]

w=\\begin{bmatrix}w_1&\\cdots&w_m\\end{bmatrix}^T,

$w = [w_{1} \dots w_{m}]^{T},$

其中各分量相互独立，并满足：

∼

(

)

w_i\\sim\\mathcal N(0,1)

$w_{i} \sim N (0, 1)$

于是：

⋯

\\epsilon_{\\mathrm{NIS}} =w_1^2+w_2^2+\\cdots+w_m^2

$ϵ_{NIS} = w_{1}^{2} + w_{2}^{2} + \dots + w_{m}^{2}$

$m$ 个独立标准正态变量的平方和，服从自由度为

$m$ 的卡方分布。因此：[1]

∼

\\boxed{\\epsilon_{\\mathrm{NIS}}\\sim\\chi_m^2}

$ϵ_{NIS} \sim χ_{m}^{2}$

其均值与方差为：

[

]

Var

⁡

(

)

E[\\epsilon_{\\mathrm{NIS}}]=m, \\qquad \\operatorname{Var}(\\epsilon_{\\mathrm{NIS}})=2m

$E [ϵ_{NIS}] = m, Var (ϵ_{NIS}) = 2 m$

因此：

正常 NIS 的长期均值不是一律接近 1，而是接近观测维数

$m$ 。

有些代码记录的是：

\\frac{\\epsilon_{\\mathrm{NIS}}}{m},

$\frac{ϵ _{NIS}}{m},$

这个量的期望才是 1。使用门限之前，必须确认日志里的指标有没有除以自由度。

5.2 这个结论需要哪些前提？

核心前提是：

∼

(

)

\\nu\\sim\\mathcal N(0,S),

$ν \sim N (0, S),$

并且算法使用的

$S$ 确实描述了创新的不确定性。

在线性高斯、正确建模、正确初始化等标准条件下，可以得到相应的卡方结论。对 EKF/ESKF，创新与协方差来自线性化，通常只能把 NIS 视为近似检验。

强非线性、错误关联、系统性偏差、错误协方差、未处理的相关性等，都会使理论门限的实际含义发生变化。

因此，门限具有统计意义的前提不是“公式写对了”这一项，而是创新分布模型也基本成立。

5.3 单帧异常门控：上尾检验

建立原假设：

∼

(

)

H_0:\\quad\\nu\\sim\\mathcal N(0,S)

$H_{0} : ν \sim N (0, S)$

选择理论误报概率

\\alpha

$α$ ，例如：

0.01

\\alpha=0.01

$α = 0.01$

门限为：

−

(

−

)

\\boxed{ \\tau=F_{\\chi_m^2}^{-1}(1-\\alpha) }

$τ = F_{χ_{m}^{2}}^{- 1} (1 - α)$

其中

−

F^{-1}

$F^{- 1}$ 是分位数函数。

判定规则为：

{

≤

未发现显著的不一致证据；

判为与当前统计模型显著不一致

\\boxed{ \\begin{cases} \\epsilon_{\\mathrm{NIS}}\\le\\tau, &\\text{未发现显著的不一致证据；}\\\\ \\epsilon_{\\mathrm{NIS}}>\\tau, &\\text{判为与当前统计模型显著不一致} \\end{cases} }

${ϵ_{NIS} \leq τ, ϵ_{NIS} > τ, 未发现显著的不一致证据；判为与当前统计模型显著不一致$

因为创新已经平方，所以正方向和负方向的过大创新都会被检测；“上尾检验”不是说只检查正创新。

5.4 常用卡方门限

下表表头表示原假设成立时的理论覆盖率；数值为对应卡方分布的上分位点。[2]

观测维数

1	3.841	6.635	10.828
2	5.991	9.210	13.816
3	7.815	11.345	16.266
6	12.592	16.812	22.458

例如，三维速度观测使用 99% 门限：

11.345

⇒

触发异常门控

\\epsilon_{\\mathrm{NIS}}>11.345 \\quad\\Rightarrow\\quad \\text{触发异常门控}

$ϵ_{NIS} > 11.345 \Rightarrow 触发异常门控$

即使 ESKF 有 15 维状态，这里仍然使用 3 个自由度，不是 15 个。

5.5 通过 99% 门限，不等于数据有 99% 概率正确

正确的概率含义是：

(

∣

)

P(\\epsilon_{\\mathrm{NIS}}>\\tau\\mid H_0)=1\\%

$P (ϵ_{NIS} > τ ∣ H_{0}) = 1%$

即：

假如预测、观测和协方差模型符合原假设，仍然约有 1% 的样本会超过这个门限。

它不是：

(

数据错误

∣

)

P(\\text{数据错误}\\mid\\epsilon_{\\mathrm{NIS}}>\\tau)=99\\%

$P (数据错误 ∣ ϵ_{NIS} > τ) = 99%$

这两个条件概率完全不同。要判断“给定检验结果后，数据错误的概率”，还需要故障先验和故障情况下的创新分布等额外信息。

同理，通过门控只能表述为“没有足够证据拒绝原假设”，而不是“已经证明观测正确”。

5.6 99.9% 门限反而比 99% 更宽松

覆盖率越高，门限越大：

$$ F_{\\chi_m^2}{-1}(0.999)

F_{\\chi_m^2}{-1}(0.99) $$

因此，更大的创新也可能通过。

它通常意味着在原假设下误拒正常观测更少，但也可能放过更多异常观测。不能因为数字“99.9%”更大，就认为异常筛选更严格。

6. 完整数值例子：从预测到门控，再到卡尔曼更新

本节始终使用同一组位置—速度状态，逐项计算所有中间量。

6.1 状态、预测和观测模型

状态为：

[

]

x=\\begin{bmatrix}p\\\\v\\end{bmatrix}

$x = [p v]$

预测状态为：

−

[

]

\\hat x^-= \\begin{bmatrix} 10\\\\2 \\end{bmatrix},

$\overset{x}{^}^{-} = [102],$

即位置

10\\,\\mathrm m

$10 m$ 、速度

2\\,\\mathrm{m/s}

$2 m/s$ 。

预测协方差为：

−

[

0.04

0.015

0.09

]

P^-= \\begin{bmatrix} 0.04&0.015\\\\ 0.015&0.09 \\end{bmatrix}

$P^{-} = [0.04 0.015 0.015 0.09]$

各项的含义是：

位置方差： $0.04\\,\\mathrm{m^2} 0.04m2，对应标准差 0.2 m 0.2\\,\\mathrm m 0.2m。$
速度方差： $0.09\\,(\\mathrm{m/s})^2 0.09(m/s)2，对应标准差 0.3 m / s 0.3\\,\\mathrm{m/s} 0.3m/s。$
交叉协方差： $0.015\\,\\mathrm{m^2/s} 0.015m2/s，表示位置与速度误差相关。$

传感器只测量位置：

[

]

z=p+n, \\qquad J=\\begin{bmatrix}1&0\\end{bmatrix}

$z = p + n, J = [10]$

测量标准差为

0.1

0.1\\,\\mathrm m

$0.1 m$ ，所以：

0.01

R=0.01

$R = 0.01$

创新协方差：

−

[

]

[

0.04

0.015

0.09

]

[

]

0.01

0.05

\\begin{aligned} S &=JP^-J^T+R\\\\ &= \\begin{bmatrix}1&0\\end{bmatrix} \\begin{bmatrix} 0.04&0.015\\\\ 0.015&0.09 \\end{bmatrix} \\begin{bmatrix}1\\\\0\\end{bmatrix} +0.01\\\\ &=0.05 \\end{aligned}

$S = J P^{-} J^{T} + R = [10] [0.04 0.015 0.015 0.09] [10] + 0.01 = 0.05$

虽然状态是二维，本次创新仍然只有一维。

6.2 情况 A：观测位置为 $10.2\\,\\mathrm m 10.2m$

创新：

10.2

−

0.2

\\nu=10.2-10=0.2

$ν = 10.2 - 10 = 0.2$

NIS：

0.2

0.05

0.8

\\epsilon_{\\mathrm{NIS}} =\\frac{0.2^2}{0.05} =0.8

$ϵ_{NIS} = \frac{0. 2 ^{2}}{0.05} = 0.8$

采用一维 99% 门限：

0.8

6.635

0.8<6.635

$0.8 < 6.635$

因此，通过门控。

随后才计算卡尔曼增益并更新状态：

−

[

0.04

0.015

]

0.05

[

0.8

0.3

]

\\begin{aligned} K &=P^-J^TS^{-1}\\\\ &=\\begin{bmatrix}0.04\\\\0.015\\end{bmatrix}\\frac1{0.05}\\\\ &=\\begin{bmatrix}0.8\\\\0.3\\end{bmatrix} \\end{aligned}

$K = P^{-} J^{T} S^{- 1} = [0.04 0.015] \frac{1}{0.05} = [0.8 0.3]$

状态更新：

−

[

]

[

0.8

0.3

]

0.2

[

10.16

2.06

]

\\begin{aligned} \\hat x^+ &=\\hat x^-+K\\nu\\\\ &=\\begin{bmatrix}10\\\\2\\end{bmatrix} +\\begin{bmatrix}0.8\\\\0.3\\end{bmatrix}0.2\\\\ &=\\boxed{\\begin{bmatrix}10.16\\\\2.06\\end{bmatrix}} \\end{aligned}

$\overset{x}{^}^{+} = \overset{x}{^}^{-} + K ν = [102] + [0.8 0.3] 0.2 = [10.16 2.06]$

后验协方差为：

−

[

0.008

0.003

0.0855

]

\\begin{aligned} P^+ &=P^–P^-J^TS^{-1}JP^-\\\\ &=\\begin{bmatrix} 0.008&0.003\\\\ 0.003&0.0855 \\end{bmatrix} \\end{aligned}

$P^{+} = P^{-} - P^{-} J^{T} S^{- 1} J P^{-} = [0.008 0.003 0.003 0.0855]$

由此可以看出：

NIS 门控决定是否允许这份观测进入更新；卡尔曼增益决定允许更新之后，各状态调整多少。

虽然传感器只测量位置，但位置观测仍然改变了速度。这是因为

−

P^-

$P^{-}$ 中位置与速度的交叉协方差不为零。

6.3 情况 B：同一份先验，观测位置为 $10.6\\,\\mathrm m 10.6m$

注意：这里不是接着情况 A 更新，而是重新从相同的

−

\\hat x^-,P^-

$\overset{x}{^}^{-}, P^{-}$ 出发。

创新：

10.6

−

0.6

\\nu=10.6-10=0.6

$ν = 10.6 - 10 = 0.6$

NIS：

0.6

0.05

7.2

\\epsilon_{\\mathrm{NIS}} =\\frac{0.6^2}{0.05} =7.2

$ϵ_{NIS} = \frac{0. 6 ^{2}}{0.05} = 7.2$

因为：

7.2

6.635

7.2>6.635,

$7.2 > 6.635,$

所以触发异常门控。在最简单的硬门控策略下，不用这一观测执行状态更新。

一维情况下，还可以把 NIS 门限转换回原始距离门限：

≤

⟺

∣

≤

\\frac{\\nu^2}{S}\\le\\tau \\quad\\Longleftrightarrow\\quad |\\nu|\\le\\sqrt{\\tau S}

$\frac{ν ^{2}}{S} \leq τ ⟺ ∣ ν ∣ \leq τ S$

代入数据：

∣

≤

6.634897

0.05

≈

0.575973

|\\nu| \\le\\sqrt{6.634897\\times0.05} \\approx0.575973\\,\\mathrm m

$∣ ν ∣ \leq 6.634897 \times 0.05$

≈0.575973m

因此，

0.6

0.6\\,\\mathrm m

$0.6 m$ 的创新已经越界。

它的上尾

$p$ 值约为：

(

≥

7.2

)

≈

0.007290

p_{\\mathrm{upper}} =P(\\chi_1^2\\ge7.2) \\approx0.007290

$p_{upper} = P (χ_{1}^{2} \geq 7.2) \approx 0.007290$

含义是：在原假设成立时，出现至少这么大 NIS 的概率约为

0.729

0.729\\%

$0.729%$ 。

它不是“这份数据只有

0.729

0.729\\%

$0.729%$ 的概率正确”。

6.4 情况 C：还是差 $0.6\\,\\mathrm m 0.6m，但预测更不确定$

将预测位置方差改成：

−

0.25

P_{pp}^-=0.25,

$P_{pp}^{-} = 0.25,$

即预测位置标准差为

0.5

0.5\\,\\mathrm m

$0.5 m$ 。

仍然使用：

0.01

0.6

R=0.01, \\qquad \\nu=0.6

$R = 0.01, ν = 0.6$

此时：

0.25

0.01

0.26

S=0.25+0.01=0.26,

$S = 0.25 + 0.01 = 0.26,$

0.36

0.26

≈

1.385

\\epsilon_{\\mathrm{NIS}} =\\frac{0.36}{0.26} \\approx1.385

$ϵ_{NIS} = \frac{0.36}{0.26} \approx 1.385$

这次通过门控。

观测没有改变，改变的是预测对自身精度的声明。

因此，NIS 不是固定距离门限，而是根据预测与观测的不确定性调整判定尺度。

7. 二维相关性例子：同样的距离，为什么一个通过、另一个拒绝？

7.1 创新协方差

考虑二维位置创新：

[

0.04

0.036

0.04

]

S= \\begin{bmatrix} 0.04&0.036\\\\ 0.036&0.04 \\end{bmatrix} \\mathrm{m^2}

$S = [0.04 0.036 0.036 0.04] m^{2}$

两个坐标轴上的标准差都是：

0.2

\\sigma_x=\\sigma_y=0.2\\,\\mathrm m

$σ_{x} = σ_{y} = 0.2 m$

7.2 特征方向与特征值

$S$ 的两个特征方向为：

[

]

0.076

u_+=\\frac1{\\sqrt2}\\begin{bmatrix}1\\\\1\\end{bmatrix}, \\qquad \\lambda_+=0.076,

$u_{+} = 2$

1[11],λ+=0.076,

−

[

−

]

−

0.004

u_-=\\frac1{\\sqrt2}\\begin{bmatrix}1\\\\-1\\end{bmatrix}, \\qquad \\lambda_-=0.004

$u_{-} = 2$

1[1−1],λ−=0.004

这里

u_+

$u_{+}$ 与

−

u_-

$u_{-}$ 是单位特征向量。

沿着“同向变化”的方向，方差较大；沿着“反向变化”的方向，方差很小。

一般地，若：

S=U\\Lambda U^T,

$S = U Λ U^{T},$

则：

−

∑

(

)

\\begin{aligned} \\epsilon_{\\mathrm{NIS}} &=\\nu^TU\\Lambda^{-1}U^T\\nu\\\\ &=\\boxed{\\sum_{i=1}^{m}\\frac{(u_i^T\\nu)^2}{\\lambda_i}} \\end{aligned}

$ϵ_{NIS} = ν^{T} U Λ^{- 1} U^{T} ν = i = 1 \sum m \frac{( u _{i}^{T} ν ) ^{2}}{λ _{i}}$

计算过程是：

先将创新投影到协方差的主方向，再除以各方向的方差，最后求和。

7.3 分别计算两组创新

\\nu_A

$ν_{A}$ 完全沿着

u_+

$u_{+}$ ，因此：

0.08

0.076

≈

1.053

\\epsilon_A=\\frac{0.08}{0.076}\\approx1.053

$ϵ_{A} = \frac{0.08}{0.076} \approx 1.053$

\\nu_B

$ν_{B}$ 完全沿着

−

u_-

$u_{-}$ ，因此：

0.08

0.004

\\epsilon_B=\\frac{0.08}{0.004}=20

$ϵ_{B} = \frac{0.08}{0.004} = 20$

使用二维 99% 门限：

9.210

\\tau=9.210,

$τ = 9.210,$

所以：

通过，

拒绝

\\boxed{\\nu_A\\text{ 通过，}\\qquad\\nu_B\\text{ 拒绝}}

$ν_{A} 通过， ν_{B} 拒绝$

创新欧氏长度NIS二维 99% 门控

$[0.2,\\,0.2]^T [0.2,0.2]T$	$0.283\\,\\mathrm m 0.283m$	1.053	通过
$[0.2,\\,-0.2]^T [0.2,−0.2]T$	$0.283\\,\\mathrm m 0.283m$	20.000	拒绝

如果错误地忽略交叉项，两者都会被算成：

0.2

0.04

0.2

0.04

\\frac{0.2^2}{0.04}+\\frac{0.2^2}{0.04}=2,

$\frac{0. 2 ^{2}}{0.04} + \frac{0. 2 ^{2}}{0.04} = 2,$

从而漏掉第二组异常。

因此：

NIS 不只是考虑“创新有多大”，还考虑“创新朝哪个方向大”。

从几何上看，门控条件：

−

≤

\\nu^TS^{-1}\\nu\\le\\tau

$ν^{T} S^{- 1} ν \leq τ$

对应一个椭球。沿着特征向量

u_i

$u_{i}$ 的半轴长度为：

\\sqrt{\\tau\\lambda_i}

$τ λ_{i}$

方差大的方向允许更大的偏差；方差小的方向对偏差更敏感。

8. 机器人应用：接触约束与退化观测

8.1 足端零速度约束是否适合融合？

假设当前认为某只脚与地面接触，并且接触点没有滑动，于是构造零速度伪观测：

[

]

z=\\begin{bmatrix}0\\\\0\\\\0\\end{bmatrix}

$z =$

000

通过预测状态和关节运动学，得到世界坐标系中的足端速度：

(

−

)

h(\\hat x^-)=\\hat v_f^W

$h (\overset{x}{^}^{-}) = \overset{v}{^}_{f}^{W}$

创新为：

−

\\nu=0-\\hat v_f^W

$ν = 0 - \overset{v}{^}_{f}^{W}$

这里检验的不是“某个直接测量出的足端速度是否正确”，而是：

足端零速度假设，是否与当前预测和运动学计算相容？

假设创新协方差为：

0.0009

[

(

)

]

S=0.0009I_3 \\quad[(\\mathrm{m/s})^2],

$S = 0.0009 I_{3} [(m/s)^{2}],$

也就是每个创新分量的标准差为：

0.0009

0.03

\\sqrt{0.0009}=0.03\\,\\mathrm{m/s}

$0.0009$

=0.03m/s

注意，

$S$ 应由状态预测、运动学与观测噪声模型推导，而不是仅因为构造了零观测就设置为零。

第一只脚的预测速度为：

[

0.02

0.01

]

\\hat v_{f,1}^W= \\begin{bmatrix}0.02\\\\0.01\\\\0\\end{bmatrix} \\mathrm{m/s}

$\overset{v}{^}_{f, 1}^{W} =$

0.020.010

m/s

NIS 为：

0.02

0.01

0.0009

≈

0.556

\\epsilon_1 =\\frac{0.02^2+0.01^2}{0.0009} \\approx0.556

$ϵ_{1} = \frac{0.0 2 ^{2} + 0.0 1 ^{2}}{0.0009} \approx 0.556$

第二只脚的预测速度为：

[

0.12

0.01

]

\\hat v_{f,2}^W= \\begin{bmatrix}0.12\\\\0.01\\\\0\\end{bmatrix} \\mathrm{m/s}

$\overset{v}{^}_{f, 2}^{W} =$

0.120.010

m/s

NIS 为：

0.12

0.01

0.0009

≈

16.111

\\epsilon_2 =\\frac{0.12^2+0.01^2}{0.0009} \\approx16.111

$ϵ_{2} = \frac{0.1 2 ^{2} + 0.0 1 ^{2}}{0.0009} \approx 16.111$

三维 99% 门限为：

11.345

\\tau=11.345

$τ = 11.345$

因此：

第一只脚的零速约束通过，第二只脚的零速约束触发异常

\\boxed{ \\text{第一只脚的零速约束通过，第二只脚的零速约束触发异常} }

$第一只脚的零速约束通过，第二只脚的零速约束触发异常$

但是，第二只脚超限不能单独证明脚在打滑。可能的解释还包括基座速度预测错误、关节运动学参数错误、外参错误或者时间未对齐。

更准确的工程结论是：

当前不宜在未经进一步处理的情况下，强行融合第二只脚的零速度约束。

可以进一步结合接触力、关节状态、其他支撑脚、独立速度观测等信息进行故障区分。NIS 本身不负责把这些原因自动区分开。

8.2 走廊退化：NIS 很小，为什么也可能没什么约束能力？

考虑一个仅含

x,y

$x, y$ 平移的教学问题。假设匹配残差已经按噪声白化，局部信息矩阵为：

[

0.01

200

]

−

\\mathcal I= \\begin{bmatrix} 0.01&0\\\\ 0&200 \\end{bmatrix} \\mathrm{m^{-2}}

$I = [0.01 0 0200] m^{- 2}$

在线性高斯、无额外先验且局部问题可逆等简化条件下，匹配结果的协方差可近似写成：

$$ R\\approx\\mathcal I^{-1}

\\begin{bmatrix} 100&0\\ 0&0.005 \\end{bmatrix} \\mathrm{m^2} $$

也就是：

≈

0.0707

\\sigma_x=10\\,\\mathrm m, \\qquad \\sigma_y\\approx0.0707\\,\\mathrm m

$σ_{x} = 10 m, σ_{y} \approx 0.0707 m$

这份观测在

$x$ 方向很不确定，在

$y$ 方向比较精确。

假设滤波器预测位置协方差为：

−

0.04

P^-=0.04I_2,

$P^{-} = 0.04 I_{2},$

并将匹配结果视作独立的位置观测：

J=I_2

$J = I_{2}$

那么：

$$ S=P^-+R

\\begin{bmatrix} 100.04&0\\ 0&0.045 \\end{bmatrix} $$

假设创新为：

[

0.1

]

\\nu= \\begin{bmatrix} 2\\\\0.1 \\end{bmatrix} \\mathrm m

$ν = [2 0.1] m$

虽然

$x$ 方向差了两米，但：

100.04

0.1

0.045

≈

0.262

\\epsilon_{\\mathrm{NIS}} =\\frac{2^2}{100.04} +\\frac{0.1^2}{0.045} \\approx0.262

$ϵ_{NIS} = \frac{2 ^{2}}{100.04} + \frac{0. 1 ^{2}}{0.045} \approx 0.262$

NIS 很小，会通过门控。

这并不奇怪，因为这份匹配结果本来就在声明：

我的

$x$ 方向标准差有 10 米，差两米并不意外。

卡尔曼增益为：

−

(

−

)

−

≈

[

0.000400

0.888889

]

\\begin{aligned} K &=P^-(P^-+R)^{-1}\\\\ &\\approx \\begin{bmatrix} 0.000400&0\\\\ 0&0.888889 \\end{bmatrix} \\end{aligned}

$K = P^{-} (P^{-} + R)^{- 1} \approx [0.000400 0 0 0.888889]$

位置修正约为：

≈

[

0.000800

0.088889

]

\\Delta p=K\\nu \\approx \\begin{bmatrix} 0.000800\\\\0.088889 \\end{bmatrix} \\mathrm m

$Δ p = K ν \approx [0.000800 0.088889] m$

因此，滤波器几乎不使用它修正

$x$ ，却会明显利用它修正

$y$ 。

这里需要区分：

通过一致性检验

≠

观测很精确

≠

所有方向都可观

\\boxed{ \\text{通过一致性检验} \\neq \\text{观测很精确} \\neq \\text{所有方向都可观} }

$通过一致性检验 \neq = 观测很精确 \neq = 所有方向都可观$

NIS 回答“这个差异能否被不确定性解释”；信息矩阵、协方差和增益则回答“哪些方向有约束、约束有多强”。

**实际系统中的额外注意：**不能把任意优化问题的 Hessian 直接求逆后，当作独立传感器的

$R$ 。必须检查白化尺度、规范自由度、鲁棒权重、地图不确定性，以及优化结果是否已经包含同一份状态先验。若匹配结果与滤波器预测相关，还需要处理相关性，避免重复使用信息。上面例子只展示在所列简化条件下的关系。

9. NIS 偏大、偏小，分别可能意味着什么？

9.1 用一个公式统一理解

设实际创新的均值与协方差为：

[

]

Cov

⁡

(

)

E[\\nu]=b, \\qquad \\operatorname{Cov}(\\nu)=\\Sigma,

$E [ν] = b, Cov (ν) = Σ,$

但算法用来计算 NIS 的协方差是

$S$ 。

由于：

[

]

E[\\nu\\nu^T]=\\Sigma+bb^T,

$E [ν ν^{T}] = Σ + b b^{T},$

因此：

[

]

[

−

]

⁡

(

−

[

]

)

⁡

(

−

)

⁡

(

−

)

⁡

(

−

)

−

\\begin{aligned} E[\\epsilon_{\\mathrm{NIS}}] &=E[\\nu^TS^{-1}\\nu]\\\\ &=\\operatorname{tr}\\left(S^{-1}E[\\nu\\nu^T]\\right)\\\\ &=\\operatorname{tr}(S^{-1}\\Sigma) +\\operatorname{tr}(S^{-1}bb^T)\\\\ &=\\boxed{ \\operatorname{tr}(S^{-1}\\Sigma) +b^TS^{-1}b } \\end{aligned}

$E [ϵ_{NIS}] = E [ν^{T} S^{- 1} ν] = tr (S^{- 1} E [ν ν^{T}]) = tr (S^{- 1} Σ) + tr (S^{- 1} b b^{T}) = tr (S^{- 1} Σ) + b^{T} S^{- 1} b$

这里假设

$S$ 是给定的；若

$S$ 随历史变化，可以先在相应历史条件下理解该关系。

理想情况下：

b=0, \\qquad \\Sigma=S,

$b = 0, Σ = S,$

于是：

[

]

⁡

(

)

E[\\epsilon_{\\mathrm{NIS}}] =\\operatorname{tr}(I)=m

$E [ϵ_{NIS}] = tr (I) = m$

这个式子直接揭示了 NIS 偏大的两类不同来源：实际随机散布过大，或者存在非零偏差。

9.2 长期 NIS 偏大：未必是传感器噪声变大

第一类情况，是实际创新在某些方向上的散布，比算法声明的更大。

这可能来自

$R$ 偏小，也可能来自

−

P^-

$P^{-}$ 偏小。

而预测协方差又受过程噪声影响：

−

P^-=FP^+F^T+GQG^T

$P^{-} = F P^{+} F^{T} + GQ G^{T}$

所以，低估

$Q$ 也可能导致 NIS 持续偏大。

第二类情况，是创新存在非零均值：

≠

b\\neq0

$b \neq = 0$

时间不同步、外参错误、未建模的运动、传感器偏置等，都可能使创新持续偏离零。

因此，看到 NIS 偏大，不能马上得出：

应该增大

$R$ 。

更应该先判断：到底是测量噪声模型、预测模型、系统性偏差，还是数据关联出了问题。

9.3 长期 NIS 偏小：也未必是估计很准

保持实际创新不变，只将声明的协方差放大为：

′

100

S'=100S

$S^{'} = 100 S$

那么：

′

(

100

)

−

100

\\epsilon' =\\nu^T(100S)^{-1}\\nu =\\frac{\\epsilon}{100}

$ϵ^{'} = ν^{T} (100 S)^{- 1} ν = \frac{ϵ}{100}$

NIS 小了 100 倍，但实际数据和真实估计误差并没有因此改善。

因此：

不能把“尽量降低 NIS”作为滤波器的优化目标。

长期偏小可能意味着不确定性声明过于保守，也可能来自样本筛选、更新后残差误用、相关性未建模等原因。

此外，单帧 NIS 很小本来就可能是正常随机结果。不能因为某一次观测“过于接近预测”就判为故障；检查下尾通常用于模型一致性诊断，且要结合多帧统计。

9.4 NIS 无法发现所有状态错误

设某个状态误差方向为

$d$ ，如果：

Jd=0,

$J d = 0,$

那么在一阶近似下，该方向的误差不会改变当前预测观测。

因此，即使状态沿

$d$ 发生了很大的误差，当前观测的 NIS 也可能很小。

这就是不可观方向的漂移不能靠当前创新自动发现的数学原因。

同样，如果预测和观测共享某种误差，两者可能一起错，但彼此仍然一致。

9.5 与 NEES 的区别

NIS 使用的是观测空间中的创新：

−

\\epsilon_{\\mathrm{NIS}}=\\nu^TS^{-1}\\nu

$ϵ_{NIS} = ν^{T} S^{- 1} ν$

与之相对，若能够获得真实状态，则可以构造状态误差的一致性指标：

(

−

)

−

(

−

)

\\epsilon_{\\mathrm{NEES}} =(x-\\hat x)^TP^{-1}(x-\\hat x)

$ϵ_{NEES} = (x - \overset{x}{^})^{T} P^{- 1} (x - \overset{x}{^})$

前者关注“观测与预测是否一致”，后者关注“实际状态误差是否与状态协方差一致”。后者通常需要仿真真值或可信的参考系统，不能把两者混为一谈。对于姿态等流形状态，NEES 中的状态差同样需要使用一致的局部误差定义。

10. 连续数据怎么判断：平均 NIS、偏置与时间相关性

10.1 单次超限不等于系统失效

NIS 是随机量。即使模型正确，仍会按照所选显著性水平出现一定比例的超限样本。

如果观测频率很高，那么“偶尔超限”与“持续超限”是不同性质的问题。应当同时关注每次 NIS、超限频率、连续超限情况以及系统状态，而不是看到一次超限就宣布传感器故障。

10.2 推导平均 NIS 的检验区间

假设有

$N$ 个相互独立的 NIS，每次观测维数都为

$m$ ：

∼

\\epsilon_k\\sim\\chi_m^2

$ϵ_{k} \sim χ_{m}^{2}$

其和满足：

∑

∼

T=\\sum_{k=1}^{N}\\epsilon_k \\sim\\chi_{Nm}^2

$T = k = 1 \sum N ϵ_{k} \sim χ_{N m}^{2}$

定义平均 NIS：

‾

∑

\\overline\\epsilon =\\frac1N\\sum_{k=1}^{N}\\epsilon_k

$\overline{ϵ} = \frac{1}{N} k = 1 \sum N ϵ_{k}$

于是：

[

‾

]

Var

⁡

(

‾

)

E[\\overline\\epsilon]=m, \\qquad \\operatorname{Var}(\\overline\\epsilon)=\\frac{2m}{N}

$E [\overline{ϵ}] = m, Var (\overline{ϵ}) = \frac{2 m}{N}$

对长期一致性，可以使用双侧检验：

−

(

)

≤

‾

≤

−

(

−

)

\\boxed{ \\frac{F_{\\chi_{Nm}^2}^{-1}(\\alpha/2)}{N} \\le\\overline\\epsilon\\le \\frac{F_{\\chi_{Nm}^2}^{-1}(1-\\alpha/2)}{N} }

$\frac{F _{χ_{N m}^{2}}^{- 1} ( α /2 )}{N} \leq \overline{ϵ} \leq \frac{F _{χ_{N m}^{2}}^{- 1} ( 1 - α /2 )}{N}$

这与单帧异常观测的上尾门控不是同一个用途：

检验方式主要目的

单帧上尾门控	检查当前创新是否异常偏大
多帧双侧一致性检验	检查整体 NIS 是否持续偏大或偏小

上下分位点的用法与卡方双侧检验一致。[2]

10.3 数值例子：100 帧三维观测

设：

100

m=3, \\qquad N=100

$m = 3, N = 100$

在独立性等假设成立时，使用 95% 双侧区间，即：

0.05

\\alpha=0.05

$α = 0.05$

计算得到：

2.539123

≤

‾

≤

3.498745

\\boxed{ 2.539123 \\le\\overline\\epsilon\\le 3.498745 }

$2.539123 \leq \overline{ϵ} \leq 3.498745$

因此，平均 NIS 为

3.1

$3.1$ 不会触发这项检验；平均值为

$8$ 或

0.5

$0.5$ ，则应检查模型、协方差或样本处理流程。

但均值通过这一项检验，并不代表创新的全部统计性质都正确。

10.4 为什么还要看创新的符号和均值？

考虑一维白化创新长期满足：

≈

w_k\\approx1

$w_{k} \approx 1$

此时每次都有：

≈

\\epsilon_k=w_k^2\\approx1

$ϵ_{k} = w_{k}^{2} \approx 1$

平均 NIS 看起来恰好等于一维的理论均值。

但创新持续为正，说明预测观测可能长期偏低。只计算平方并求平均，会丢掉符号信息。

因此，一个更完整的诊断需要同时检查：

[

]

≈

E[w_k]\\approx0,

$E [w_{k}] \approx 0,$

Cov

⁡

(

)

≈

\\operatorname{Cov}(w_k)\\approx I,

$Cov (w_{k}) \approx I,$

以及不同时间之间是否存在明显相关性。

例如，记录白化创新的样本均值：

∑

\\bar w=\\frac1N\\sum_{k=1}^{N}w_k,

$\overset{w}{ˉ} = \frac{1}{N} k = 1 \sum N w_{k},$

并观察各分量是否长期偏离零。

10.5 白化空间相关性，不等于消除时间相关性

用当前的

S_k

$S_{k}$ 白化创新，是在处理同一时刻各分量的协方差：

−

w_k=L_k^{-1}\\nu_k

$w_{k} = L_{k}^{- 1} ν_{k}$

这并不自动保证：

[

−

ℓ

]

ℓ

≠

E[w_kw_{k-\\ell}^T]=0, \\qquad\\ell\\neq0

$E [w_{k} w_{k - ℓ}^{T}] = 0, ℓ \neq = 0$

若相邻帧共享数据、观测模型遗漏动态项、滤波器模型不正确，或者样本被某种流程筛选，那么时间相关性可能仍然存在。

在理想线性高斯卡尔曼滤波的相应条件下，创新具有白噪声性质；但不能把这个理想结论直接套到任意 EKF、滑动窗口结果或经过复杂筛选的数据上。

**窗口样本明显相关时，不能直接把 NIS 的和当成

\\chi^2_{Nm}

$χ_{N m}^{2}$ 。**否则理论置信区间的实际覆盖率可能不正确。

10.6 不要只统计通过门控的样本

假设先删除所有：

\\epsilon>\\tau

$ϵ > τ$

的样本，再对剩下的 NIS 求平均。

此时统计的不是原始卡方分布，而是截断后的条件分布：

∣

≤

\\epsilon\\mid\\epsilon\\le\\tau

$ϵ ∣ ϵ \leq τ$

高值被删除，均值自然会下降。

以二维 NIS 为例：

∼

[

]

\\epsilon\\sim\\chi_2^2, \\qquad E[\\epsilon]=2

$ϵ \sim χ_{2}^{2}, E [ϵ] = 2$

其概率密度为：[1]

(

)

−

≥

f(t)=\\frac12e^{-t/2}, \\qquad t\\ge0

$f (t) = \frac{1}{2} e^{- t /2}, t \geq 0$

只保留

≤

\\epsilon\\le\\tau

$ϵ \leq τ$ 后：

[

∣

≤

]

∫

−

\\begin{aligned} E[\\epsilon\\mid\\epsilon\\le\\tau] &=\\frac{\\int_0^\\tau t\\frac12e^{-t/2}\\,dt}{1-e^{-\\tau/2}}\\\\ &=2-\\frac{\\tau e^{-\\tau/2}}{1-e^{-\\tau/2}} \\end{aligned}

$E [ϵ ∣ ϵ \leq τ] = \frac{\int _{0}^{τ} t \frac{1}{2} e ^{- t /2} d t}{1 - e ^{- τ /2}} = 2 - \frac{τ e ^{- τ /2}}{1 - e ^{- τ /2}}$

使用二维 99% 门限：

9.210340

−

0.01

\\tau=9.210340, \\qquad e^{-\\tau/2}=0.01,

$τ = 9.210340, e^{- τ /2} = 0.01,$

得到：

[

∣

≤

]

≈

1.906966

\\boxed{ E[\\epsilon\\mid\\epsilon\\le\\tau] \\approx1.906966 }

$E [ϵ ∣ ϵ \leq τ] \approx 1.906966$

即便原始数据完全符合模型，门控通过样本的均值也不是 2，而是约 1.907。

数据关联中“从多个候选里挑残差最小的那个”，也会引入选择偏差。筛选后的分布不能不加区分地继续按原始卡方分布解释。

10.7 应该记录哪些数据？

建议保留门控前的创新、创新协方差或其分解、NIS、自由度、门限、接受/拒绝标记，以及观测类型与时间戳。

这样才能区分“原始数据确实一致”和“因为大残差都被删了，所以剩下的数据看起来很好”。

但也要注意：**门控前记录是必要的，不代表所有记录都属于原假设下的有效高斯样本。**错误关联、杂波候选以及门控对后续滤波状态的影响，仍需单独考虑。

11. 实现时最容易出错的地方

11.1 必须使用当前观测更新前的创新与协方差

标准预测创新 NIS 使用：

−

(

−

)

\\nu^-=z-h(\\hat x^-),

$ν^{-} = z - h (\overset{x}{^}^{-}),$

−

S^-=JP^-J^T+R

$S^{-} = J P^{-} J^{T} + R$

不能先融合观测，然后直接使用：

[

−

(

)

]

(

)

−

[

−

(

)

]

[z-h(\\hat x^+)]^T (JP^+J^T+R)^{-1} [z-h(\\hat x^+)]

$[z - h (\overset{x}{^}^{+})]^{T} (J P^{+} J^{T} + R)^{- 1} [z - h (\overset{x}{^}^{+})]$

并把它当成同一个 NIS。

仍以前面的异常观测

10.6

z=10.6

$z = 10.6$ 为例。

正确的更新前 NIS 是：

−

7.2

\\epsilon^-=7.2

$ϵ^{-} = 7.2$

如果没有门控，先用位置增益

0.8

K_p=0.8

$K_{p} = 0.8$ 更新：

0.8

0.6

10.48

\\hat p^+=10+0.8\\times0.6=10.48

$\overset{p}{^}^{+} = 10 + 0.8 \times 0.6 = 10.48$

更新后残差只剩：

10.6

−

10.48

0.12

\\nu^+=10.6-10.48=0.12

$ν^{+} = 10.6 - 10.48 = 0.12$

若错误地使用：

0.008

0.01

0.018

P_{pp}^++R=0.008+0.01=0.018,

$P_{pp}^{+} + R = 0.008 + 0.01 = 0.018,$

会得到：

0.12

0.018

0.8

\\frac{0.12^2}{0.018}=0.8

$\frac{0.1 2 ^{2}}{0.018} = 0.8$

原本异常的观测，竟然“通过了”。

问题在于：后验状态已经使用了这份测量，所以它与该测量噪声不再独立。

在线性情况下，后验残差满足：

(

−

)

−

\\nu^+=(I-JK)\\nu^-

$ν^{+} = (I - J K) ν^{-}$

其正确协方差为：

Cov

⁡

(

)

(

−

)

−

(

−

)

\\operatorname{Cov}(\\nu^+) =(I-JK)S^-(I-JK)^T

$Cov (ν^{+}) = (I - J K) S^{-} (I - J K)^{T}$

在本例中：

Var

⁡

(

)

(

−

0.8

)

0.05

0.002

\\operatorname{Var}(\\nu^+) =(1-0.8)^2\\times0.05 =0.002

$Var (ν^{+}) = (1 - 0.8)^{2} \times 0.05 = 0.002$

使用正确的协方差，得到：

0.12

0.002

7.2

\\frac{0.12^2}{0.002}=7.2

$\frac{0.1 2 ^{2}}{0.002} = 7.2$

所以，不是“后验残差永远不能检验”，而是不能对后验残差直接套用更新前创新的协方差公式。

11.2 预测误差与测量噪声相关时，要保留交叉项

前面默认：

[

]

E[\\tilde x n^T]=0

$E [\tilde{x} n^{T}] = 0$

若定义：

[

]

≠

C=E[\\tilde x n^T]\\neq0,

$C = E [\tilde{x} n^{T}] \neq = 0,$

则应使用：

−

\\boxed{ S=JP^-J^T+R+JC+C^TJ^T }

$S = J P^{-} J^{T} + R + J C + C^{T} J^{T}$

这里的正负号来自本文误差和创新定义；若更换误差定义，需要一致地重新推导。

同一份原始数据既参与预测又参与构造观测时，尤其需要检查独立性。例如，某个估计结果已经融合了 IMU，却又被当作完全独立的观测回灌到使用同一 IMU 的滤波器中，就不能直接假定没有相关性。

11.3 坐标系、单位与误差定义必须一致

对 ESKF，应保证创新、雅可比、协方差描述的是同一个局部误差空间。

典型问题包括：

创新使用世界坐标系，但协方差仍在机体坐标系。
创新使用角度制，协方差却使用弧度制。
雅可比按右扰动推导，协方差却按另一种误差定义传播。
姿态观测以四元数四个分量直接相减，又直接使用 4 个自由度。

例如，三维姿态通常具有三个局部自由度，应该根据实际构造的姿态创新及其协方差来决定检验维数，而不是按存储变量的个数机械决定。

11.4 不要为了通过门控而反复增大

增大

$R$ 会扩大

$S$ ，从而降低 NIS。

但这只是改变了不确定性声明，并没有证明观测变好了。

根据成像质量、特征几何、接触条件等外部质量信息构建

$R$ ，可以作为建模的一部分。但其统计解释依然依赖于该条件噪声模型是否合理。

如果看到当前残差过大，就不断增大

$R$ 直到通过，那么这个过程不能继续解释为原先固定显著性水平的标准 NIS 检验。

同样，经过裁剪、鲁棒加权或数据依赖变换后的残差，也不能未经推导就沿用原始卡方门限。

11.5 持续拒绝观测，也可能是预测已经错误且过度自信

假如预测状态已经偏离真实状态，但

−

P^-

$P^{-}$ 仍然很小，那么一个正确的观测也可能产生很大的 NIS。

此时若只会永久拒绝观测，滤波器可能失去恢复机会。

因此，工程流程应区分“偶发外点”和“持续模型失配”。对于持续异常，需要结合独立观测、初始化状态、运动模型和传感器诊断，决定是否重新初始化、调整建模或进入恢复模式，而不是只凭 NIS 认定传感器长期失效。

11.6 不要显式计算矩阵逆

虽然公式为：

−

\\epsilon=\\nu^TS^{-1}\\nu,

$ϵ = ν^{T} S^{- 1} ν,$

实现时更适合：

S=LL^T, \\qquad Lw=\\nu, \\qquad \\epsilon=w^Tw

$S = L L^{T}, L w = ν, ϵ = w^{T} w$

这样不需要显式计算

−

S^{-1}

$S^{- 1}$ 。NumPy 的 Cholesky 分解可用于对称正定矩阵。[4]

如果分解失败，应先检查协方差构造、单位和数值条件。不能无条件加入一个很大的对角项，把模型问题掩盖掉。

11.7 自由度不是状态维数，也不一定等于原始数据项数

本文默认

$S$ 正定，此时自由度为创新维数

$m$ 。

如果存在精确冗余、

$S$ 在数学上确实奇异，那么应在有效子空间内构造检验。设有效秩为

$r$ ，只有在创新确实落在该子空间且相应高斯模型正确等条件下，才可使用对应的

$r$ 维卡方检验。

不能随便使用伪逆，再忽略创新在零方差方向上的非零分量。若模型声称某方向的方差严格为零，而创新在该方向却非零，这本身就是模型不相容的表现。

还要注意：

rank

⁡

(

)

不意味着

rank

⁡

(

)

\\operatorname{rank}(J)<m \\quad\\text{不意味着}\\quad \\operatorname{rank}(S)<m

$rank (J) < m 不意味着 rank (S) < m$

只要

$R$ 正定，即使

$J$ 秩不足，

−

S=JP^-J^T+R

$S = J P^{-} J^{T} + R$ 仍然可以正定满秩。

12. 可直接运行的 Python 实现

下面的函数只负责计算 NIS 和门控，不在函数内部修改状态。

输入必须是当前观测更新前的创新及其正确协方差。代码使用 Cholesky 分解白化创新，不显式求逆。

chi2.ppf 用于计算卡方分位点，chi2.sf 用于计算上尾概率。在尾部概率很小时，直接使用生存函数 sf 比自己计算 1 – cdf 更合适。[3]

依赖安装：

pip install numpy scipy

完整示例：

import numpy as np
from scipy.linalg import solve_triangular
from scipy.stats import chi2

def nis_gate(
innovation: np.ndarray,
S: np.ndarray,
confidence: float = 0.99,
) –> dict:
"""计算更新前创新的 NIS，并进行上尾门控。

Parameters
———-
innovation : np.ndarray
一维创新向量，shape=(m,)。
S : np.ndarray
创新协方差，shape=(m, m)，要求对称正定。
confidence : float
原假设成立时的理论覆盖率，例如 0.99。

Returns
——-
dict
NIS、自由度、门限、上尾概率、门控结果和白化创新。
accepted=True 只表示通过一致性门控，不代表观测一定正确。
"""
nu = np.asarray(innovation, dtype=float)
S = np.asarray(S, dtype=float)

if not 0.0 < confidence < 1.0:
raise ValueError("confidence 必须位于 (0, 1)。")

if nu.ndim != 1 or nu.size == 0:
raise ValueError("innovation 必须是非空一维向量。")

if S.shape != (nu.size, nu.size):
raise ValueError("S 的尺寸必须等于观测维数 × 观测维数。")

if not np.isfinite(nu).all() or not np.isfinite(S).all():
raise ValueError("创新或协方差包含 NaN / Inf。")

if not np.allclose(S, S.T, rtol=1e-7, atol=1e-12):
raise ValueError("S 明显不对称，请检查协方差计算。")

# 仅消除很小的浮点非对称，不用于修复错误模型。
S = 0.5 * (S + S.T)

try:
L = np.linalg.cholesky(S)
except np.linalg.LinAlgError as exc:
raise ValueError(
"S 不是正定矩阵，不能进行此 NIS 检验。"
) from exc

# 白化：解 L @ w = nu，不显式计算 S 的逆矩阵。
whitened = solve_triangular(L, nu, lower=True)
value = float(whitened @ whitened)

if not np.isfinite(value):
raise ValueError("NIS 数值溢出，请检查量纲和协方差尺度。")

dof = int(nu.size)
threshold = float(chi2.ppf(confidence, df=dof))

return {
"nis": value,
"dof": dof,
"threshold": threshold,
"p_upper": float(chi2.sf(value, df=dof)),
"accepted": bool(value <= threshold),
"whitened_innovation": whitened,
}

def mean_nis_interval(
dof: int,
count: int,
confidence: float = 0.95,
) –> tuple:
"""相互独立、固定维数的 NIS 样本均值的双侧接受区间。

不能直接用于仅保留门控通过样本的数据，或明显时间相关的数据。
此函数只计算理论区间，不验证输入样本是否满足统计假设。
"""
for name, value in (("dof", dof), ("count", count)):
if (
isinstance(value, bool)
or not isinstance(value, (int, np.integer))
or value <= 0
):
raise ValueError(f"{name} 必须是正整数。")

if not 0.0 < confidence < 1.0:
raise ValueError("confidence 必须位于 (0, 1)。")

alpha = 1.0 – confidence
total_dof = int(dof) * int(count)
lower = chi2.ppf(alpha / 2.0, df=total_dof) / count
upper = chi2.ppf(1.0 – alpha / 2.0, df=total_dof) / count
return float(lower), float(upper)

def main() –> None:
# 1. 位置—速度状态，传感器只测量位置。
x_pred = np.array([10.0, 2.0])
P_pred = np.array([
[0.04, 0.015],
[0.015, 0.09],
])
J = np.array([[1.0, 0.0]])
R = np.array([[0.01]])
S = J @ P_pred @ J.T + R

# 两个观测分别使用同一份先验，不是依次更新。
for z in (10.2, 10.6):
innovation = np.array([z]) – J @ x_pred
result = nis_gate(innovation, S, confidence=0.99)
print(
f"z={z:.1f}, NIS={result['nis']:.3f}, "
f"门限={result['threshold']:.3f}, "
f"通过={result['accepted']}, "
f"p_upper={result['p_upper']:.6f}"
)

if result["accepted"]:
# 解 S @ K.T = J @ P_pred，避免显式求逆。
K = np.linalg.solve(S, J @ P_pred).T
x_post = x_pred + K @ innovation

# Joseph 形式；此处是欧氏空间的线性 KF 示例。
A = np.eye(x_pred.size) – K @ J
P_post = A @ P_pred @ A.T + K @ R @ K.T
P_post = 0.5 * (P_post + P_post.T)

print(" 更新后的状态:", np.round(x_post, 6))
print(" 更新后的协方差:")
print(P_post)

# 2. 二维强相关创新：长度相同，但方向不同。
S_correlated = np.array([
[0.04, 0.036],
[0.036, 0.04],
])
cases = (
("同向创新", np.array([0.2, 0.2])),
("反向创新", np.array([0.2, –0.2])),
)
for name, innovation in cases:
result = nis_gate(innovation, S_correlated)
print(
f"{name}: NIS={result['nis']:.3f}, "
f"通过={result['accepted']}"
)

# 3. 两只脚分别进行零速度约束门控。
S_foot = 0.0009 * np.eye(3)
foot_velocities = (
np.array([0.02, 0.01, 0.0]),
np.array([0.12, 0.01, 0.0]),
)
for index, velocity in enumerate(foot_velocities, start=1):
innovation = –velocity # 观测为零，减去预测足端速度。
result = nis_gate(innovation, S_foot)
print(
f"足端{index}: NIS={result['nis']:.3f}, "
f"通过={result['accepted']}"
)

# 4. 100 个独立三维 NIS 样本的均值接受区间。
lower, upper = mean_nis_interval(dof=3, count=100)
print(f"平均 NIS 的 95% 双侧区间: [{lower:.6f}, {upper:.6f}]")

if __name__ == "__main__":
main()

预期输出：

z=10.2, NIS=0.800, 门限=6.635, 通过=True, p_upper=0.371093
更新后的状态: [10.16 2.06]
更新后的协方差:
[[0.008 0.003 ]
[0.003 0.0855]]
z=10.6, NIS=7.200, 门限=6.635, 通过=False, p_upper=0.007290
同向创新: NIS=1.053, 通过=True
反向创新: NIS=20.000, 通过=False
足端1: NIS=0.556, 通过=True
足端2: NIS=16.111, 通过=False
平均 NIS 的 95% 双侧区间: [2.539123, 3.498745]

这份代码中，门控与更新是分开的。被拒绝的观测不会更新状态；用于比较的两个位置观测也不会互相改变先验。

Joseph 形式为：

(

−

)

−

(

−

)

P^+=(I-KJ)P^-(I-KJ)^T+KRK^T

$P^{+} = (I - K J) P^{-} (I - K J)^{T} + K R K^{T}$

它在本文不相关噪声的线性模型下与前面给出的协方差更新式等价。若存在预测—观测噪声相关性，卡尔曼增益和协方差更新也需要相应修改，不能只改

$S$ 而其他公式保持不变。

对 ESKF，还需要将估计的误差注入名义状态，并按照采用的误差定义执行协方差重置。上面的欧氏空间状态更新示例不能原封不动替代这些步骤。

13. 接入 ESKF 的推荐流程

下面是一条便于理解和调试的处理顺序：

IMU / 运动模型预测
得到名义状态 x⁻、误差协方差 P⁻
↓
在当前观测时刻构造观测模型
计算预测观测、创新 ν、雅可比 J、噪声协方差 R
↓
构造创新协方差 S
检查坐标系、单位、正定性及相关性假设
↓
计算白化创新与门控前 NIS
记录 ν、S、NIS、自由度、门限与时间戳
↓
执行 NIS 门控，并结合独立的观测质量与几何检查
↓
通过：计算增益，估计误差，注入名义状态，更新并重置协方差
拒绝：保留当前预测，记录异常；必要时进入故障诊断或恢复逻辑

这里的“拒绝后保留预测”对应最简单的硬门控。降权、多模型判断或重新初始化属于更完整的估计系统设计；使用这些机制时，应重新考虑原有统计门限是否仍然适用。

也不应将 NIS 通过作为唯一的融合条件。例如，几何上完全退化的匹配，即使通过一致性检验，也不意味着可以在所有方向上给予很高权重。

14. 公式速查与最终总结

14.1 公式速查

内容公式或结论

更新前创新	$\\nu=z-h(\\hat x^-) ν=z−h(x^−)$
创新一阶模型	$\\nu\\approx J\\tilde x+n ν≈Jx~+n$
无交叉相关时的创新协方差	$S=JP^-J^T+R S=JP−JT+R$
有交叉相关时的创新协方差	$S=JP^-J^T+R+JC+C^TJ^T S=JP−JT+R+JC+CTJT$
NIS	$\\epsilon=\\nu^TS^{-1}\\nu ϵ=νTS−1ν$
白化计算	$S=LL^T,\\ Lw=\\nu,\\ \\epsilon=w^Tw S=LLT, Lw=ν, ϵ=wTw$
理想分布	$\\epsilon\\sim\\chi_m^2 ϵ∼χm2$
理想均值	$E[\\epsilon]=m E[ϵ]=m$
单帧上尾门限	$\\tau=F^{-1}_{\\chi_m^2}(1-\\alpha) τ=Fχm2−1(1−α)$
上尾概率	$p_{\\mathrm{upper}}=P(\\chi_m^2\\ge\\epsilon) pupper=P(χm2≥ϵ)$
一维原始创新门限	$\\lvert\\nu\\rvert\\le\\sqrt{\\tau S} ∣ν∣≤τS $
独立样本求和	$\\sum_{k=1}^{N}\\epsilon_k\\sim\\chi_{Nm}^2 ∑k=1Nϵk∼χNm2$
一般创新均值下的期望	$E[\\epsilon]=\\operatorname{tr}(S^{-1}\\Sigma)+b^TS^{-1}b E[ϵ]=tr(S−1Σ)+bTS−1b$

上表中的分布和门限结论，都以文中说明的高斯性、协方差正确性、维数和独立性条件为前提。

14.2 最容易混淆的四句话

“NIS 小，所以传感器准确。”

不一定。也可能是

−

P^-

$P^{-}$ 或

$R$ 声明得很大，或者预测与观测共享同一种误差。

“NIS 大，所以应该增大

$R$ 。”

不一定。问题可能出在

$Q$ 、

−

P^-

$P^{-}$ 、外参、时间同步、运动模型或数据关联。

“通过 99% 门控，所以观测有 99% 的概率正确。”

不是。99% 是原假设成立时的理论覆盖率，不是观测正确的后验概率。

“NIS 通过，所以所有状态都受到有效约束。”

不是。NIS 与可观性、信息强弱回答的是不同问题。

14.3 用一句话记住 NIS

$$ \\boxed{ \\text{NIS}

\\text{考虑完整协方差之后，创新的异常程度} } $$

它不是问：

观测与预测相差多少米？

而是问：

按照预测和传感器各自声明的不确定性，这么大的差异是否合理？

正确使用 NIS 的关键是：用更新前创新，使用正确的创新协方差，按实际创新自由度选择卡方门限，并将“统计不一致”与“传感器故障”“精度不足”“状态不可观”区分开。

参考资料

以下资料对应文中的基础分布、门限与函数接口；教学例子的具体数值由文中给出的参数计算得到。

[1] NIST/SEMATECH, Chi-Square Distribution：卡方分布定义、密度、均值和方差。 https://www.itl.nist.gov/div898/handbook/eda/section3/eda3666.htm

[2] NIST/SEMATECH, Critical Values of the Chi-Square Distribution：卡方临界值与分位点。 https://www.itl.nist.gov/div898/handbook/eda/section3/eda3674.htm

[3] SciPy 官方文档，scipy.stats.chi2：ppf、cdf 和 sf 等函数接口。 https://docs.scipy.org/doc/scipy/reference/generated/scipy.stats.chi2.html

[4] NumPy 官方文档，numpy.linalg.cholesky：对称正定矩阵的 Cholesky 分解。 https://numpy.org/doc/stable/reference/generated/numpy.linalg.cholesky.html