如何使用 NIS 判断观测是否可靠：从卡尔曼滤波推导到机器人实践
核心问题：考虑了预测与传感器各自的不确定性之后，本次观测与预测之间的差异，是否大得不合理？
NIS（Normalized Innovation Squared，归一化创新平方）检验的是观测与当前预测、噪声模型之间的统计一致性，不是直接判断观测是否接近真实值。它不需要真实状态，适合在线使用；但也正因为没有真实值，它不能仅凭一次检验就证明"传感器错了，预测一定是对的"。
本文从创新和协方差的定义出发，逐步推导 NIS 与卡方门限；随后用固定数字走完整个预测、门控和更新过程，再讨论相关误差、接触约束、走廊退化、多帧统计与 ESKF 实现。所有数值例子均为教学构造，不代表某一具体传感器的标定参数。
1. NIS 到底在判断什么？
机器人通过 IMU、运动模型或者上一时刻状态，得到当前状态的预测；相机、激光雷达、轮速计等传感器又提供了新的观测。两者通常不会完全相同。
最简单的想法是：
观测与预测差得越多，观测就越不可靠。
但这个判断遗漏了一个关键问题：预测本身有多准，传感器本身又有多准？
例如，同样是位置差了 0.5 m 0.5\,\mathrm m 0.5m：
- 预测标准差只有 0.01 m 0.01\,\mathrm m 0.01m，传感器标准差也只有 0.01 m 0.01\,\mathrm m 0.01m，这个差异非常异常。
- 预测标准差已经达到 2 m 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 符号约定
本文使用以下符号：
| 符号 | 含义 |
|---|---|
| x x x | 真实状态 |
| x ^ − \hat x^- x^− | 使用当前观测之前的预测状态 |
| P − P^- P− | 预测状态误差的协方差 |
| z z z | 当前实际观测 |
| h ( x ) h(x) h(x) | 观测模型 |
| n n n | 测量噪声 |
| R R R | 测量噪声协方差 |
| J J J | 观测模型对状态误差的一阶雅可比 |
| ν \nu ν | 创新，即实际观测减去预测观测 |
| S S S | 创新协方差 |
| m m m | 创新向量的维数，在正定情形下也是 NIS 的自由度 |
| ϵ \epsilon ϵ | NIS 数值 |
| τ \tau τ | NIS 门限 |
上标 " − - −" 表示当前观测更新之前；上标 " + + +" 表示更新之后。
这里用 J J J 表示观测雅可比，避免与退化分析中常见的信息矩阵 H = J T J H=J^T J H=JTJ 混淆。
2.2 状态预测误差
假设机器人已经获得预测状态：
x ^ − \hat x^- x^−
定义真实状态与预测状态之间的误差：
x ~ = x − x ^ − \tilde x=x-\hat x^- x~=x−x^−
假设预测无偏：
E x \~ = 0 , E x \~ x \~ T = P − E\\tilde x=0, \qquad E\\tilde x\\tilde x\^T=P^- Ex\~=0,Ex\~x\~T=P−
对于 ESKF，上式中的减法应理解为与误差状态定义一致的局部差异。姿态状态通常不能简单地用四元数的四个分量直接相减；本文先用欧氏空间写法解释基本统计关系。
2.3 测量噪声
传感器模型为：
z = h ( x ) + n , z=h(x)+n, z=h(x)+n,
其中：
E n = 0 , E n n T = R En=0, \qquad Enn\^T=R En=0,EnnT=R
测量噪声是实际观测与真实状态对应的理想观测之间的差：
n = z − h ( x ) n=z-h(x) n=z−h(x)
由于不知道真实状态 x x x，通常也无法直接得到每次观测的真实测量噪声。
2.4 创新
用预测状态计算"传感器应该测到什么"：
z ^ − = h ( x ^ − ) \hat z^-=h(\hat x^-) z^−=h(x^−)
实际观测与预测观测之间的差：
ν = z − h ( x ^ − ) \boxed{\nu=z-h(\hat x^-)} ν=z−h(x^−)
就叫做创新，innovation。
三者的区别是：
| 量 | 定义 | 在线能否直接获得？ |
|---|---|---|
| 状态误差 | x ~ = x − x ^ − \tilde x=x-\hat x^- x~=x−x^− | 通常不能，因为不知道真实状态 |
| 测量噪声 | n = z − h ( x ) n=z-h(x) n=z−h(x) | 通常不能，因为不知道真实状态 |
| 创新 | ν = z − h ( x ^ − ) \nu=z-h(\hat x^-) ν=z−h(x^−) | 可以，观测和预测都有 |
NIS 使用创新，正是因为创新可以在线计算。
2.5 创新中到底包含什么误差？
对观测模型做一阶展开：
h ( x ) = h ( x ^ − + x ~ ) ≈ h ( x ^ − ) + J x ~ , h(x) =h(\hat x^-+\tilde x) \approx h(\hat x^-)+J\tilde x, h(x)=h(x^−+x~)≈h(x^−)+Jx~,
其中：
J = ∂ h ∂ x ∣ x ^ − J=\left.\frac{\partial h}{\partial x}\right|_{\hat x^-} J=∂x∂h x^−
代入创新：
ν = z − h ( x ^ − ) = h ( x ) + n − h ( x ^ − ) ≈ J x ~ + n \begin{aligned} \nu &=z-h(\hat x^-)\\ &=h(x)+n-h(\hat x^-)\\ &\approx J\tilde x+n \end{aligned} ν=z−h(x^−)=h(x)+n−h(x^−)≈Jx~+n
因此：
创新 ≈ 状态预测误差投影到观测空间 + 测量噪声 \boxed{ \text{创新} \approx \text{状态预测误差投影到观测空间} + \text{测量噪声} } 创新≈状态预测误差投影到观测空间+测量噪声
这说明：创新大，既可能是传感器存在问题，也可能是状态预测存在问题。
3. 推导创新协方差：为什么是 S = J P − J T + R S=JP^-J^T+R S=JP−JT+R？
3.1 从协方差定义展开
由：
ν ≈ J x ~ + n , \nu\approx J\tilde x+n, ν≈Jx~+n,
在零均值假设下，创新协方差为：
S = E ν ν T S=E\\nu\\nu\^T S=EννT
展开得到：
S ≈ E ( J x \~ + n ) ( J x \~ + n ) T = J E x \~ x \~ T J T + J E x \~ n T + E n x \~ T J T + E n n T \begin{aligned} S &\approx E(J\\tilde x+n)(J\\tilde x+n)\^T\\ &=JE\\tilde x\\tilde x\^TJ^T +JE\\tilde x n\^T\\ &\quad+En\\tilde x\^TJ^T +Enn\^T \end{aligned} S≈E(Jx\~+n)(Jx\~+n)T=JEx\~x\~TJT+JEx\~nT+Enx\~TJT+EnnT
如果当前测量噪声与预测误差不相关：
E x \~ n T = 0 , E\\tilde x n\^T=0, Ex\~nT=0,
则交叉项消失：
S = J P − J T + R \boxed{S=JP^-J^T+R} S=JP−JT+R
两个部分分别是：
J P − J T ⏟ 预测观测的不确定性 + R ⏟ 测量噪声的不确定性 \underbrace{JP^-J^T}{\text{预测观测的不确定性}} + \underbrace{R}{\text{测量噪声的不确定性}} 预测观测的不确定性 JP−JT+测量噪声的不确定性 R
3.2 为什么要乘 J J J？
因为 P − P^- P− 描述的是状态空间 里的误差，创新却在观测空间中。
假设状态有 15 维，但当前只观测三维速度，那么：
P − ∈ R 15 × 15 , J ∈ R 3 × 15 , S ∈ R 3 × 3 P^-\in\mathbb R^{15\times15}, \qquad J\in\mathbb R^{3\times15}, \qquad S\in\mathbb R^{3\times3} P−∈R15×15,J∈R3×15,S∈R3×3
J J J 的作用，是将状态误差的一阶影响映射到当前观测上。因此，参与创新协方差的不是整个 P − P^- P− 的简单加和，而是：
J P − J T JP^-J^T JP−JT
3.3 一维数值例子
假设预测位置的标准差为：
σ p r e d = 0.2 m , \sigma_{\mathrm{pred}}=0.2\,\mathrm m, σpred=0.2m,
传感器测量位置的标准差为：
σ s e n s o r = 0.1 m \sigma_{\mathrm{sensor}}=0.1\,\mathrm m σsensor=0.1m
直接测量位置，故 J = 1 J=1 J=1。于是：
S = 0.2 2 + 0.1 2 = 0.05 m 2 S=0.2^2+0.1^2=0.05\,\mathrm{m^2} S=0.22+0.12=0.05m2
创新的标准差为：
σ ν = 0.05 ≈ 0.2236 m \boxed{ \sigma_\nu=\sqrt{0.05}\approx0.2236\,\mathrm m } σν=0.05 ≈0.2236m
解释这个数值时，可以这样理解：
预测与观测之间有一定差异是正常的。判断差异大不大，应使用双方误差合成后的 0.2236 m 0.2236\,\mathrm m 0.2236m，而不是只使用传感器的 0.1 m 0.1\,\mathrm m 0.1m。
所以，标准预测创新检验使用：
ν T S − 1 ν , \nu^TS^{-1}\nu, νTS−1ν,
而不是直接使用：
ν T R − 1 ν \nu^TR^{-1}\nu νTR−1ν
后一种写法忽略了预测状态的不确定性，除非具体问题已经满足可以忽略该项等额外条件，否则不能替代 NIS。
4. 从"几个标准差"推导出 NIS
4.1 一维情况下：标准化创新的平方
设一维创新为 ν \nu ν，方差为：
S = σ ν 2 S=\sigma_\nu^2 S=σν2
先除以标准差：
w = ν σ ν w=\frac{\nu}{\sigma_\nu} w=σνν
w w w 表示当前创新相当于多少个创新标准差。
再平方：
ϵ N I S = w 2 = ν 2 S \boxed{ \epsilon_{\mathrm{NIS}}=w^2=\frac{\nu^2}{S} } ϵNIS=w2=Sν2
例如：
ν = 0.6 m , S = 0.05 m 2 \nu=0.6\,\mathrm m, \qquad S=0.05\,\mathrm{m^2} ν=0.6m,S=0.05m2
于是：
w = 0.6 0.2236 ≈ 2.683 , w=\frac{0.6}{0.2236}\approx2.683, w=0.22360.6≈2.683,
ϵ N I S = 2.683 2 = 7.2 \epsilon_{\mathrm{NIS}}=2.683^2=7.2 ϵNIS=2.6832=7.2
NIS 为 7.2 7.2 7.2，不是偏了 7.2 σ 7.2\sigma 7.2σ；在一维情况下，它对应的是约 2.683 σ 2.683\sigma 2.683σ。
4.2 NIS 为什么没有单位？
因为分子是创新平方，分母是创新方差，两者的单位抵消。
把前面例子的米换成厘米：
ν = 60 c m , S = 500 c m 2 , \nu=60\,\mathrm{cm}, \qquad S=500\,\mathrm{cm^2}, ν=60cm,S=500cm2,
仍然得到：
60 2 500 = 7.2 \frac{60^2}{500}=7.2 500602=7.2
因此，NIS 不会因为长度单位从米改成厘米就改变。前提是创新和协方差必须一起进行正确的单位变换。
4.3 多维情况下：为什么不能只逐项除以方差？
当创新各分量相互独立、 S S S 为对角矩阵时，可以写成：
ϵ N I S = ν 1 2 S 11 + ν 2 2 S 22 + ⋯ + ν m 2 S m m \epsilon_{\mathrm{NIS}} =\frac{\nu_1^2}{S_{11}} +\frac{\nu_2^2}{S_{22}} +\cdots +\frac{\nu_m^2}{S_{mm}} ϵNIS=S11ν12+S22ν22+⋯+Smmνm2
但如果各分量存在相关性，上式会遗漏非对角项包含的信息。
正确做法是白化：既统一尺度，又去除相关性。
4.4 用 Cholesky 分解推导白化
对正定创新协方差进行分解：
S = L L T , S=LL^T, S=LLT,
其中 L L L 是下三角矩阵。
定义：
w = L − 1 ν w=L^{-1}\nu w=L−1ν
白化创新的协方差为：
Cov ( w ) = L − 1 S L − T = L − 1 L L T L − T = I \begin{aligned} \operatorname{Cov}(w) &=L^{-1}SL^{-T}\\ &=L^{-1}LL^TL^{-T}\\ &=I \end{aligned} Cov(w)=L−1SL−T=L−1LLTL−T=I
如果：
ν ∼ N ( 0 , S ) , \nu\sim\mathcal N(0,S), ν∼N(0,S),
则：
w ∼ N ( 0 , I ) w\sim\mathcal N(0,I) w∼N(0,I)
在联合高斯假设下，白化后的各分量相互独立，并且都服从标准正态分布。注意：只知道协方差为单位阵、但没有联合高斯等额外条件时，不能仅凭"不相关"就断言"独立"。
计算白化创新的平方长度：
w T w = ( L − 1 ν ) T ( L − 1 ν ) = ν T L − T L − 1 ν = ν T S − 1 ν \begin{aligned} w^Tw &=(L^{-1}\nu)^T(L^{-1}\nu)\\ &=\nu^TL^{-T}L^{-1}\nu\\ &=\nu^TS^{-1}\nu \end{aligned} wTw=(L−1ν)T(L−1ν)=νTL−TL−1ν=νTS−1ν
因此：
ϵ N I S = ν T S − 1 ν \boxed{ \epsilon_{\mathrm{NIS}}=\nu^TS^{-1}\nu } ϵNIS=νTS−1ν
NIS 就是创新的马氏距离平方 ，也就是白化创新的平方长度。
5. 为什么 NIS 要和卡方分布比较？
5.1 卡方分布从哪里来？
设创新有 m m m 维。白化后：
w = w 1 ⋯ w m T , w=\begin{bmatrix}w_1&\cdots&w_m\end{bmatrix}^T, w=w1⋯wmT,
其中各分量相互独立，并满足：
w i ∼ N ( 0 , 1 ) w_i\sim\mathcal N(0,1) wi∼N(0,1)
于是：
ϵ N I S = w 1 2 + w 2 2 + ⋯ + w m 2 \epsilon_{\mathrm{NIS}} =w_1^2+w_2^2+\cdots+w_m^2 ϵNIS=w12+w22+⋯+wm2
m m m 个独立标准正态变量的平方和，服从自由度为 m m m 的卡方分布。因此：1
ϵ N I S ∼ χ m 2 \boxed{\epsilon_{\mathrm{NIS}}\sim\chi_m^2} ϵNIS∼χm2
其均值与方差为：
E ϵ N I S = m , Var ( ϵ N I S ) = 2 m E\\epsilon_{\\mathrm{NIS}}=m, \qquad \operatorname{Var}(\epsilon_{\mathrm{NIS}})=2m EϵNIS=m,Var(ϵNIS)=2m
因此：
正常 NIS 的长期均值不是一律接近 1，而是接近观测维数 m m m。
有些代码记录的是：
ϵ N I S m , \frac{\epsilon_{\mathrm{NIS}}}{m}, mϵNIS,
这个量的期望才是 1。使用门限之前，必须确认日志里的指标有没有除以自由度。
5.2 这个结论需要哪些前提？
核心前提是：
ν ∼ N ( 0 , S ) , \nu\sim\mathcal N(0,S), ν∼N(0,S),
并且算法使用的 S S S 确实描述了创新的不确定性。
在线性高斯、正确建模、正确初始化等标准条件下，可以得到相应的卡方结论。对 EKF/ESKF，创新与协方差来自线性化，通常只能把 NIS 视为近似检验。
强非线性、错误关联、系统性偏差、错误协方差、未处理的相关性等，都会使理论门限的实际含义发生变化。
因此，门限具有统计意义的前提不是"公式写对了"这一项，而是创新分布模型也基本成立。
5.3 单帧异常门控：上尾检验
建立原假设：
H 0 : ν ∼ N ( 0 , S ) H_0:\quad\nu\sim\mathcal N(0,S) H0:ν∼N(0,S)
选择理论误报概率 α \alpha α，例如：
α = 0.01 \alpha=0.01 α=0.01
门限为：
τ = F χ m 2 − 1 ( 1 − α ) \boxed{ \tau=F_{\chi_m^2}^{-1}(1-\alpha) } τ=Fχm2−1(1−α)
其中 F − 1 F^{-1} F−1 是分位数函数。
判定规则为：
{ ϵ N I S ≤ τ , 未发现显著的不一致证据； ϵ N I S > τ , 判为与当前统计模型显著不一致 \boxed{ \begin{cases} \epsilon_{\mathrm{NIS}}\le\tau, &\text{未发现显著的不一致证据；}\\ \epsilon_{\mathrm{NIS}}>\tau, &\text{判为与当前统计模型显著不一致} \end{cases} } {ϵNIS≤τ,ϵNIS>τ,未发现显著的不一致证据；判为与当前统计模型显著不一致
因为创新已经平方，所以正方向和负方向的过大创新都会被检测；"上尾检验"不是说只检查正创新。
5.4 常用卡方门限
下表表头表示原假设成立时的理论覆盖率；数值为对应卡方分布的上分位点。2
| 观测维数 m m m | 95% | 99% | 99.9% |
|---|---|---|---|
| 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% 门限：
ϵ N I S > 11.345 ⇒ 触发异常门控 \epsilon_{\mathrm{NIS}}>11.345 \quad\Rightarrow\quad \text{触发异常门控} ϵNIS>11.345⇒触发异常门控
即使 ESKF 有 15 维状态，这里仍然使用 3 个自由度，不是 15 个。
5.5 通过 99% 门限，不等于数据有 99% 概率正确
正确的概率含义是：
P ( ϵ N I S > τ ∣ H 0 ) = 1 % P(\epsilon_{\mathrm{NIS}}>\tau\mid H_0)=1\% P(ϵNIS>τ∣H0)=1%
即：
假如预测、观测和协方差模型符合原假设，仍然约有 1% 的样本会超过这个门限。
它不是：
P ( 数据错误 ∣ ϵ N I S > τ ) = 99 % 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 = p v x=\begin{bmatrix}p\\v\end{bmatrix} x=pv
预测状态为：
x ^ − = 10 2 , \hat x^-= \begin{bmatrix} 10\\2 \end{bmatrix}, x^−=102,
即位置 10 m 10\,\mathrm m 10m、速度 2 m / s 2\,\mathrm{m/s} 2m/s。
预测协方差为：
P − = 0.04 0.015 0.015 0.09 P^-= \begin{bmatrix} 0.04&0.015\\ 0.015&0.09 \end{bmatrix} P−=0.040.0150.0150.09
各项的含义是：
- 位置方差： 0.04 m 2 0.04\,\mathrm{m^2} 0.04m2，对应标准差 0.2 m 0.2\,\mathrm m 0.2m。
- 速度方差： 0.09 ( m / s ) 2 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 m 2 / s 0.015\,\mathrm{m^2/s} 0.015m2/s，表示位置与速度误差相关。
传感器只测量位置：
z = p + n , J = 1 0 z=p+n, \qquad J=\begin{bmatrix}1&0\end{bmatrix} z=p+n,J=10
测量标准差为 0.1 m 0.1\,\mathrm m 0.1m，所以：
R = 0.01 R=0.01 R=0.01
创新协方差：
S = J P − J T + R = 1 0 0.04 0.015 0.015 0.09 1 0 + 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=JP−JT+R=100.040.0150.0150.0910+0.01=0.05
虽然状态是二维，本次创新仍然只有一维。
6.2 情况 A：观测位置为 10.2 m 10.2\,\mathrm m 10.2m
创新：
ν = 10.2 − 10 = 0.2 \nu=10.2-10=0.2 ν=10.2−10=0.2
NIS：
ϵ N I S = 0.2 2 0.05 = 0.8 \epsilon_{\mathrm{NIS}} =\frac{0.2^2}{0.05} =0.8 ϵNIS=0.050.22=0.8
采用一维 99% 门限：
0.8 < 6.635 0.8<6.635 0.8<6.635
因此，通过门控。
随后才计算卡尔曼增益并更新状态：
K = P − J T S − 1 = 0.04 0.015 1 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−JTS−1=0.040.0150.051=0.80.3
状态更新：
x ^ + = x ^ − + K ν = 10 2 + 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} x^+=x^−+Kν=102+0.80.30.2=10.162.06
后验协方差为：
P + = P − − P − J T S − 1 J P − = 0.008 0.003 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−JTS−1JP−=0.0080.0030.0030.0855
由此可以看出：
NIS 门控决定是否允许这份观测进入更新；卡尔曼增益决定允许更新之后，各状态调整多少。
虽然传感器只测量位置，但位置观测仍然改变了速度。这是因为 P − P^- P− 中位置与速度的交叉协方差不为零。
6.3 情况 B：同一份先验，观测位置为 10.6 m 10.6\,\mathrm m 10.6m
注意：这里不是接着情况 A 更新，而是重新从相同的 x ^ − , P − \hat x^-,P^- x^−,P− 出发。
创新：
ν = 10.6 − 10 = 0.6 \nu=10.6-10=0.6 ν=10.6−10=0.6
NIS：
ϵ N I S = 0.6 2 0.05 = 7.2 \epsilon_{\mathrm{NIS}} =\frac{0.6^2}{0.05} =7.2 ϵNIS=0.050.62=7.2
因为：
7.2 > 6.635 , 7.2>6.635, 7.2>6.635,
所以触发异常门控。在最简单的硬门控策略下，不用这一观测执行状态更新。
一维情况下，还可以把 NIS 门限转换回原始距离门限：
ν 2 S ≤ τ ⟺ ∣ ν ∣ ≤ τ S \frac{\nu^2}{S}\le\tau \quad\Longleftrightarrow\quad |\nu|\le\sqrt{\tau S} Sν2≤τ⟺∣ν∣≤τS
代入数据：
∣ ν ∣ ≤ 6.634897 × 0.05 ≈ 0.575973 m |\nu| \le\sqrt{6.634897\times0.05} \approx0.575973\,\mathrm m ∣ν∣≤6.634897×0.05 ≈0.575973m
因此， 0.6 m 0.6\,\mathrm m 0.6m 的创新已经越界。
它的上尾 p p p 值约为：
p u p p e r = P ( χ 1 2 ≥ 7.2 ) ≈ 0.007290 p_{\mathrm{upper}} =P(\chi_1^2\ge7.2) \approx0.007290 pupper=P(χ12≥7.2)≈0.007290
含义是：在原假设成立时，出现至少这么大 NIS 的概率约为 0.729 % 0.729\% 0.729%。
它不是"这份数据只有 0.729 % 0.729\% 0.729% 的概率正确"。
6.4 情况 C：还是差 0.6 m 0.6\,\mathrm m 0.6m，但预测更不确定
将预测位置方差改成：
P p p − = 0.25 , P_{pp}^-=0.25, Ppp−=0.25,
即预测位置标准差为 0.5 m 0.5\,\mathrm m 0.5m。
仍然使用：
R = 0.01 , ν = 0.6 R=0.01, \qquad \nu=0.6 R=0.01,ν=0.6
此时：
S = 0.25 + 0.01 = 0.26 , S=0.25+0.01=0.26, S=0.25+0.01=0.26,
ϵ N I S = 0.36 0.26 ≈ 1.385 \epsilon_{\mathrm{NIS}} =\frac{0.36}{0.26} \approx1.385 ϵNIS=0.260.36≈1.385
这次通过门控。
观测没有改变，改变的是预测对自身精度的声明。
因此，NIS 不是固定距离门限，而是根据预测与观测的不确定性调整判定尺度。
7. 二维相关性例子：同样的距离，为什么一个通过、另一个拒绝？
7.1 创新协方差
考虑二维位置创新：
S = 0.04 0.036 0.036 0.04 m 2 S= \begin{bmatrix} 0.04&0.036\\ 0.036&0.04 \end{bmatrix} \mathrm{m^2} S=0.040.0360.0360.04m2
两个坐标轴上的标准差都是：
σ x = σ y = 0.2 m \sigma_x=\sigma_y=0.2\,\mathrm m σx=σy=0.2m
相关系数为：
ρ = 0.036 0.2 × 0.2 = 0.9 \rho=\frac{0.036}{0.2\times0.2}=0.9 ρ=0.2×0.20.036=0.9
这表示两维创新具有很强的正相关性，更倾向于一起变大、一起变小。
比较两组创新：
ν A = 0.2 0.2 , ν B = 0.2 − 0.2 \nu_A=\begin{bmatrix}0.2\\0.2\end{bmatrix}, \qquad \nu_B=\begin{bmatrix}0.2\\-0.2\end{bmatrix} νA=0.20.2,νB=0.2−0.2
它们的欧氏长度相同：
∥ ν A ∥ = ∥ ν B ∥ = 0.08 ≈ 0.283 m \|\nu_A\|=\|\nu_B\| =\sqrt{0.08} \approx0.283\,\mathrm m ∥νA∥=∥νB∥=0.08 ≈0.283m
而且，每一个坐标分量都只偏了 1 σ 1\sigma 1σ。
但联合来看，这两组创新并不等价。
7.2 特征方向与特征值
S S S 的两个特征方向为：
u + = 1 2 1 1 , λ + = 0.076 , u_+=\frac1{\sqrt2}\begin{bmatrix}1\\1\end{bmatrix}, \qquad \lambda_+=0.076, u+=2 111,λ+=0.076,
u − = 1 2 1 − 1 , λ − = 0.004 u_-=\frac1{\sqrt2}\begin{bmatrix}1\\-1\end{bmatrix}, \qquad \lambda_-=0.004 u−=2 11−1,λ−=0.004
这里 u + u_+ u+ 与 u − u_- u− 是单位特征向量。
沿着"同向变化"的方向，方差较大；沿着"反向变化"的方向，方差很小。
一般地，若：
S = U Λ U T , S=U\Lambda U^T, S=UΛUT,
则：
ϵ N I S = ν T U Λ − 1 U T ν = ∑ i = 1 m ( u i T ν ) 2 λ i \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=νTUΛ−1UTν=i=1∑mλi(uiTν)2
计算过程是：
先将创新投影到协方差的主方向，再除以各方向的方差，最后求和。
7.3 分别计算两组创新
ν A \nu_A νA 完全沿着 u + u_+ u+，因此：
ϵ A = 0.08 0.076 ≈ 1.053 \epsilon_A=\frac{0.08}{0.076}\approx1.053 ϵA=0.0760.08≈1.053
ν B \nu_B νB 完全沿着 u − u_- u−，因此：
ϵ B = 0.08 0.004 = 20 \epsilon_B=\frac{0.08}{0.004}=20 ϵB=0.0040.08=20
使用二维 99% 门限：
τ = 9.210 , \tau=9.210, τ=9.210,
所以：
ν A 通过， ν B 拒绝 \boxed{\nu_A\text{ 通过，}\qquad\nu_B\text{ 拒绝}} νA 通过，νB 拒绝
| 创新 | 欧氏长度 | NIS | 二维 99% 门控 |
|---|---|---|---|
| 0.2 , 0.2 T 0.2,\\,0.2^T 0.2,0.2T | 0.283 m 0.283\,\mathrm m 0.283m | 1.053 | 通过 |
| 0.2 , − 0.2 T 0.2,\\,-0.2^T 0.2,−0.2T | 0.283 m 0.283\,\mathrm m 0.283m | 20.000 | 拒绝 |
如果错误地忽略交叉项，两者都会被算成：
0.2 2 0.04 + 0.2 2 0.04 = 2 , \frac{0.2^2}{0.04}+\frac{0.2^2}{0.04}=2, 0.040.22+0.040.22=2,
从而漏掉第二组异常。
因此：
NIS 不只是考虑"创新有多大"，还考虑"创新朝哪个方向大"。
从几何上看，门控条件：
ν T S − 1 ν ≤ τ \nu^TS^{-1}\nu\le\tau νTS−1ν≤τ
对应一个椭球。沿着特征向量 u i u_i ui 的半轴长度为：
τ λ i \sqrt{\tau\lambda_i} τλi
方差大的方向允许更大的偏差；方差小的方向对偏差更敏感。
8. 机器人应用：接触约束与退化观测
8.1 足端零速度约束是否适合融合？
假设当前认为某只脚与地面接触，并且接触点没有滑动，于是构造零速度伪观测：
z = 0 0 0 z=\begin{bmatrix}0\\0\\0\end{bmatrix} z= 000
通过预测状态和关节运动学，得到世界坐标系中的足端速度：
h ( x ^ − ) = v ^ f W h(\hat x^-)=\hat v_f^W h(x^−)=v^fW
创新为：
ν = 0 − v ^ f W \nu=0-\hat v_f^W ν=0−v^fW
这里检验的不是"某个直接测量出的足端速度是否正确"，而是：
足端零速度假设，是否与当前预测和运动学计算相容？
假设创新协方差为：
S = 0.0009 I 3 ( m / s ) 2 , S=0.0009I_3 \quad(\\mathrm{m/s})\^2, S=0.0009I3(m/s)2,
也就是每个创新分量的标准差为：
0.0009 = 0.03 m / s \sqrt{0.0009}=0.03\,\mathrm{m/s} 0.0009 =0.03m/s
注意， S S S 应由状态预测、运动学与观测噪声模型推导，而不是仅因为构造了零观测就设置为零。
第一只脚的预测速度为：
v ^ f , 1 W = 0.02 0.01 0 m / s \hat v_{f,1}^W= \begin{bmatrix}0.02\\0.01\\0\end{bmatrix} \mathrm{m/s} v^f,1W= 0.020.010 m/s
NIS 为：
ϵ 1 = 0.02 2 + 0.01 2 0.0009 ≈ 0.556 \epsilon_1 =\frac{0.02^2+0.01^2}{0.0009} \approx0.556 ϵ1=0.00090.022+0.012≈0.556
第二只脚的预测速度为：
v ^ f , 2 W = 0.12 0.01 0 m / s \hat v_{f,2}^W= \begin{bmatrix}0.12\\0.01\\0\end{bmatrix} \mathrm{m/s} v^f,2W= 0.120.010 m/s
NIS 为：
ϵ 2 = 0.12 2 + 0.01 2 0.0009 ≈ 16.111 \epsilon_2 =\frac{0.12^2+0.01^2}{0.0009} \approx16.111 ϵ2=0.00090.122+0.012≈16.111
三维 99% 门限为：
τ = 11.345 \tau=11.345 τ=11.345
因此：
第一只脚的零速约束通过，第二只脚的零速约束触发异常 \boxed{ \text{第一只脚的零速约束通过，第二只脚的零速约束触发异常} } 第一只脚的零速约束通过，第二只脚的零速约束触发异常
但是，第二只脚超限不能单独证明脚在打滑。可能的解释还包括基座速度预测错误、关节运动学参数错误、外参错误或者时间未对齐。
更准确的工程结论是：
当前不宜在未经进一步处理的情况下，强行融合第二只脚的零速度约束。
可以进一步结合接触力、关节状态、其他支撑脚、独立速度观测等信息进行故障区分。NIS 本身不负责把这些原因自动区分开。
8.2 走廊退化：NIS 很小，为什么也可能没什么约束能力？
考虑一个仅含 x , y x,y x,y 平移的教学问题。假设匹配残差已经按噪声白化，局部信息矩阵为：
I = 0.01 0 0 200 m − 2 \mathcal I= \begin{bmatrix} 0.01&0\\ 0&200 \end{bmatrix} \mathrm{m^{-2}} I=0.0100200m−2
在线性高斯、无额外先验且局部问题可逆等简化条件下，匹配结果的协方差可近似写成：
$$
R\approx\mathcal I^{-1}
\begin{bmatrix}
100&0\
0&0.005
\end{bmatrix}
\mathrm{m^2}
也就是： σ x = 10 m , σ y ≈ 0.0707 m \\sigma_x=10\\,\\mathrm m, \\qquad \\sigma_y\\approx0.0707\\,\\mathrm m σx=10m,σy≈0.0707m 这份观测在 x x x 方向很不确定，在 y y y 方向比较精确。 假设滤波器预测位置协方差为： P − = 0.04 I 2 , P\^-=0.04I_2, P−=0.04I2, 并将匹配结果视作独立的位置观测： J = I 2 J=I_2 J=I2 那么： ##
S=P^-+R
\begin{bmatrix}
100.04&0\
0&0.045
\end{bmatrix}
假设创新为：
ν = \[ 2 0.1 \] m \\nu= \\begin{bmatrix} 2\\\\0.1 \\end{bmatrix} \\mathrm m ν=\[20.1\]m
虽然 x x x 方向差了两米，但：
ϵ N I S = 2 2 100.04 + 0.1 2 0.045 ≈ 0.262 \\epsilon_{\\mathrm{NIS}} =\\frac{2\^2}{100.04} +\\frac{0.1\^2}{0.045} \\approx0.262 ϵNIS=100.0422+0.0450.12≈0.262
NIS 很小，会通过门控。
这并不奇怪，因为这份匹配结果本来就在声明：
> 我的 x x x 方向标准差有 10 米，差两米并不意外。
卡尔曼增益为：
K = P − ( P − + R ) − 1 ≈ \[ 0.000400 0 0 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≈\[0.000400000.888889\]
位置修正约为：
Δ p = K ν ≈ \[ 0.000800 0.088889 \] m \\Delta p=K\\nu \\approx \\begin{bmatrix} 0.000800\\\\0.088889 \\end{bmatrix} \\mathrm m Δp=Kν≈\[0.0008000.088889\]m
因此，滤波器几乎不使用它修正 x x x，却会明显利用它修正 y y y。
这里需要区分：
通过一致性检验 ≠ 观测很精确 ≠ 所有方向都可观 \\boxed{ \\text{通过一致性检验} \\neq \\text{观测很精确} \\neq \\text{所有方向都可观} } 通过一致性检验=观测很精确=所有方向都可观
NIS 回答"这个差异能否被不确定性解释"；信息矩阵、协方差和增益则回答"哪些方向有约束、约束有多强"。
\*\*实际系统中的额外注意：\*\*不能把任意优化问题的 Hessian 直接求逆后，当作独立传感器的 R R R。必须检查白化尺度、规范自由度、鲁棒权重、地图不确定性，以及优化结果是否已经包含同一份状态先验。若匹配结果与滤波器预测相关，还需要处理相关性，避免重复使用信息。上面例子只展示在所列简化条件下的关系。
*** ** * ** ***
### 9. NIS 偏大、偏小，分别可能意味着什么？
#### 9.1 用一个公式统一理解
设实际创新的均值与协方差为：
E \[ ν \] = b , Cov ( ν ) = Σ , E\[\\nu\]=b, \\qquad \\operatorname{Cov}(\\nu)=\\Sigma, E\[ν\]=b,Cov(ν)=Σ,
但算法用来计算 NIS 的协方差是 S S S。
由于：
E \[ ν ν T \] = Σ + b b T , E\[\\nu\\nu\^T\]=\\Sigma+bb\^T, E\[ννT\]=Σ+bbT,
因此：
E \[ ϵ N I S \] = 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 \\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\[νTS−1ν\]=tr(S−1E\[ννT\])=tr(S−1Σ)+tr(S−1bbT)=tr(S−1Σ)+bTS−1b
这里假设 S S S 是给定的；若 S S S 随历史变化，可以先在相应历史条件下理解该关系。
理想情况下：
b = 0 , Σ = S , b=0, \\qquad \\Sigma=S, b=0,Σ=S,
于是：
E \[ ϵ N I S \] = tr ( I ) = m E\[\\epsilon_{\\mathrm{NIS}}\] =\\operatorname{tr}(I)=m E\[ϵNIS\]=tr(I)=m
这个式子直接揭示了 NIS 偏大的两类不同来源：实际随机散布过大，或者存在非零偏差。
#### 9.2 长期 NIS 偏大：未必是传感器噪声变大
第一类情况，是实际创新在某些方向上的散布，比算法声明的更大。
这可能来自 R R R 偏小，也可能来自 P − P\^- P− 偏小。
而预测协方差又受过程噪声影响：
P − = F P + F T + G Q G T P\^-=FP\^+F\^T+GQG\^T P−=FP+FT+GQGT
所以，低估 Q Q Q 也可能导致 NIS 持续偏大。
第二类情况，是创新存在非零均值：
b ≠ 0 b\\neq0 b=0
时间不同步、外参错误、未建模的运动、传感器偏置等，都可能使创新持续偏离零。
因此，看到 NIS 偏大，不能马上得出：
> 应该增大 R R R。
更应该先判断：到底是测量噪声模型、预测模型、系统性偏差，还是数据关联出了问题。
#### 9.3 长期 NIS 偏小：也未必是估计很准
保持实际创新不变，只将声明的协方差放大为：
S ′ = 100 S S'=100S S′=100S
那么：
ϵ ′ = ν T ( 100 S ) − 1 ν = ϵ 100 \\epsilon' =\\nu\^T(100S)\^{-1}\\nu =\\frac{\\epsilon}{100} ϵ′=νT(100S)−1ν=100ϵ
NIS 小了 100 倍，但实际数据和真实估计误差并没有因此改善。
因此：
> 不能把"尽量降低 NIS"作为滤波器的优化目标。
长期偏小可能意味着不确定性声明过于保守，也可能来自样本筛选、更新后残差误用、相关性未建模等原因。
此外，单帧 NIS 很小本来就可能是正常随机结果。不能因为某一次观测"过于接近预测"就判为故障；检查下尾通常用于模型一致性诊断，且要结合多帧统计。
#### 9.4 NIS 无法发现所有状态错误
设某个状态误差方向为 d d d，如果：
J d = 0 , Jd=0, Jd=0,
那么在一阶近似下，该方向的误差不会改变当前预测观测。
因此，即使状态沿 d d d 发生了很大的误差，当前观测的 NIS 也可能很小。
这就是不可观方向的漂移不能靠当前创新自动发现的数学原因。
同样，如果预测和观测共享某种误差，两者可能一起错，但彼此仍然一致。
#### 9.5 与 NEES 的区别
NIS 使用的是观测空间中的创新：
ϵ N I S = ν T S − 1 ν \\epsilon_{\\mathrm{NIS}}=\\nu\^TS\^{-1}\\nu ϵNIS=νTS−1ν
与之相对，若能够获得真实状态，则可以构造状态误差的一致性指标：
ϵ N E E S = ( x − x \^ ) T P − 1 ( x − x \^ ) \\epsilon_{\\mathrm{NEES}} =(x-\\hat x)\^TP\^{-1}(x-\\hat x) ϵNEES=(x−x\^)TP−1(x−x\^)
前者关注"观测与预测是否一致"，后者关注"实际状态误差是否与状态协方差一致"。后者通常需要仿真真值或可信的参考系统，不能把两者混为一谈。对于姿态等流形状态，NEES 中的状态差同样需要使用一致的局部误差定义。
*** ** * ** ***
### 10. 连续数据怎么判断：平均 NIS、偏置与时间相关性
#### 10.1 单次超限不等于系统失效
NIS 是随机量。即使模型正确，仍会按照所选显著性水平出现一定比例的超限样本。
如果观测频率很高，那么"偶尔超限"与"持续超限"是不同性质的问题。应当同时关注每次 NIS、超限频率、连续超限情况以及系统状态，而不是看到一次超限就宣布传感器故障。
#### 10.2 推导平均 NIS 的检验区间
假设有 N N N 个相互独立的 NIS，每次观测维数都为 m m m：
ϵ k ∼ χ m 2 \\epsilon_k\\sim\\chi_m\^2 ϵk∼χm2
其和满足：
T = ∑ k = 1 N ϵ k ∼ χ N m 2 T=\\sum_{k=1}\^{N}\\epsilon_k \\sim\\chi_{Nm}\^2 T=k=1∑Nϵk∼χNm2
定义平均 NIS：
ϵ ‾ = 1 N ∑ k = 1 N ϵ k \\overline\\epsilon =\\frac1N\\sum_{k=1}\^{N}\\epsilon_k ϵ=N1k=1∑Nϵk
于是：
E \[ ϵ ‾ \] = m , Var ( ϵ ‾ ) = 2 m N E\[\\overline\\epsilon\]=m, \\qquad \\operatorname{Var}(\\overline\\epsilon)=\\frac{2m}{N} E\[ϵ\]=m,Var(ϵ)=N2m
对长期一致性，可以使用双侧检验：
F χ N m 2 − 1 ( α / 2 ) N ≤ ϵ ‾ ≤ F χ N m 2 − 1 ( 1 − α / 2 ) 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} } NFχNm2−1(α/2)≤ϵ≤NFχNm2−1(1−α/2)
这与单帧异常观测的上尾门控不是同一个用途：
| 检验方式 | 主要目的 |
|-----------|--------------------|
| 单帧上尾门控 | 检查当前创新是否异常偏大 |
| 多帧双侧一致性检验 | 检查整体 NIS 是否持续偏大或偏小 |
上下分位点的用法与卡方双侧检验一致。\[2\]
#### 10.3 数值例子：100 帧三维观测
设：
m = 3 , N = 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≤ϵ≤3.498745
因此，平均 NIS 为 3.1 3.1 3.1 不会触发这项检验；平均值为 8 8 8 或 0.5 0.5 0.5，则应检查模型、协方差或样本处理流程。
但均值通过这一项检验，并不代表创新的全部统计性质都正确。
#### 10.4 为什么还要看创新的符号和均值？
考虑一维白化创新长期满足：
w k ≈ 1 w_k\\approx1 wk≈1
此时每次都有：
ϵ k = w k 2 ≈ 1 \\epsilon_k=w_k\^2\\approx1 ϵk=wk2≈1
平均 NIS 看起来恰好等于一维的理论均值。
但创新持续为正，说明预测观测可能长期偏低。只计算平方并求平均，会丢掉符号信息。
因此，一个更完整的诊断需要同时检查：
E \[ w k \] ≈ 0 , E\[w_k\]\\approx0, E\[wk\]≈0,
Cov ( w k ) ≈ I , \\operatorname{Cov}(w_k)\\approx I, Cov(wk)≈I,
以及不同时间之间是否存在明显相关性。
例如，记录白化创新的样本均值：
w ˉ = 1 N ∑ k = 1 N w k , \\bar w=\\frac1N\\sum_{k=1}\^{N}w_k, wˉ=N1k=1∑Nwk,
并观察各分量是否长期偏离零。
#### 10.5 白化空间相关性，不等于消除时间相关性
用当前的 S k S_k Sk 白化创新，是在处理同一时刻各分量的协方差：
w k = L k − 1 ν k w_k=L_k\^{-1}\\nu_k wk=Lk−1νk
这并不自动保证：
E \[ w k w k − ℓ T \] = 0 , ℓ ≠ 0 E\[w_kw_{k-\\ell}\^T\]=0, \\qquad\\ell\\neq0 E\[wkwk−ℓT\]=0,ℓ=0
若相邻帧共享数据、观测模型遗漏动态项、滤波器模型不正确，或者样本被某种流程筛选，那么时间相关性可能仍然存在。
在理想线性高斯卡尔曼滤波的相应条件下，创新具有白噪声性质；但不能把这个理想结论直接套到任意 EKF、滑动窗口结果或经过复杂筛选的数据上。
\*\*窗口样本明显相关时，不能直接把 NIS 的和当成 χ N m 2 \\chi\^2_{Nm} χNm2。\*\*否则理论置信区间的实际覆盖率可能不正确。
#### 10.6 不要只统计通过门控的样本
假设先删除所有：
ϵ \> τ \\epsilon\>\\tau ϵ\>τ
的样本，再对剩下的 NIS 求平均。
此时统计的不是原始卡方分布，而是截断后的条件分布：
ϵ ∣ ϵ ≤ τ \\epsilon\\mid\\epsilon\\le\\tau ϵ∣ϵ≤τ
高值被删除，均值自然会下降。
以二维 NIS 为例：
ϵ ∼ χ 2 2 , E \[ ϵ \] = 2 \\epsilon\\sim\\chi_2\^2, \\qquad E\[\\epsilon\]=2 ϵ∼χ22,E\[ϵ\]=2
其概率密度为：\[1\]
f ( t ) = 1 2 e − t / 2 , t ≥ 0 f(t)=\\frac12e\^{-t/2}, \\qquad t\\ge0 f(t)=21e−t/2,t≥0
只保留 ϵ ≤ τ \\epsilon\\le\\tau ϵ≤τ 后：
E \[ ϵ ∣ ϵ ≤ τ \] = ∫ 0 τ t 1 2 e − t / 2 d t 1 − e − τ / 2 = 2 − τ e − τ / 2 1 − e − τ / 2 \\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\[ϵ∣ϵ≤τ\]=1−e−τ/2∫0τt21e−t/2dt=2−1−e−τ/2τe−τ/2
使用二维 99% 门限：
τ = 9.210340 , e − τ / 2 = 0.01 , \\tau=9.210340, \\qquad e\^{-\\tau/2}=0.01, τ=9.210340,e−τ/2=0.01,
得到：
E \[ ϵ ∣ ϵ ≤ τ \] ≈ 1.906966 \\boxed{ E\[\\epsilon\\mid\\epsilon\\le\\tau\] \\approx1.906966 } E\[ϵ∣ϵ≤τ\]≈1.906966
即便原始数据完全符合模型，门控通过样本的均值也不是 2，而是约 1.907。
数据关联中"从多个候选里挑残差最小的那个"，也会引入选择偏差。筛选后的分布不能不加区分地继续按原始卡方分布解释。
#### 10.7 应该记录哪些数据？
建议保留门控前的创新、创新协方差或其分解、NIS、自由度、门限、接受/拒绝标记，以及观测类型与时间戳。
这样才能区分"原始数据确实一致"和"因为大残差都被删了，所以剩下的数据看起来很好"。
但也要注意：\*\*门控前记录是必要的，不代表所有记录都属于原假设下的有效高斯样本。\*\*错误关联、杂波候选以及门控对后续滤波状态的影响，仍需单独考虑。
*** ** * ** ***
### 11. 实现时最容易出错的地方
#### 11.1 必须使用当前观测更新前的创新与协方差
标准预测创新 NIS 使用：
ν − = z − h ( x \^ − ) , \\nu\^-=z-h(\\hat x\^-), ν−=z−h(x\^−),
S − = J P − J T + R S\^-=JP\^-J\^T+R S−=JP−JT+R
不能先融合观测，然后直接使用：
\[ z − h ( x \^ + ) \] T ( J P + J T + R ) − 1 \[ z − h ( x \^ + ) \] \[z-h(\\hat x\^+)\]\^T (JP\^+J\^T+R)\^{-1} \[z-h(\\hat x\^+)\] \[z−h(x\^+)\]T(JP+JT+R)−1\[z−h(x\^+)\]
并把它当成同一个 NIS。
仍以前面的异常观测 z = 10.6 z=10.6 z=10.6 为例。
正确的更新前 NIS 是：
ϵ − = 7.2 \\epsilon\^-=7.2 ϵ−=7.2
如果没有门控，先用位置增益 K p = 0.8 K_p=0.8 Kp=0.8 更新：
p \^ + = 10 + 0.8 × 0.6 = 10.48 \\hat p\^+=10+0.8\\times0.6=10.48 p\^+=10+0.8×0.6=10.48
更新后残差只剩：
ν + = 10.6 − 10.48 = 0.12 \\nu\^+=10.6-10.48=0.12 ν+=10.6−10.48=0.12
若错误地使用：
P p p + + R = 0.008 + 0.01 = 0.018 , P_{pp}\^++R=0.008+0.01=0.018, Ppp++R=0.008+0.01=0.018,
会得到：
0.12 2 0.018 = 0.8 \\frac{0.12\^2}{0.018}=0.8 0.0180.122=0.8
原本异常的观测，竟然"通过了"。
问题在于：**后验状态已经使用了这份测量，所以它与该测量噪声不再独立。**
在线性情况下，后验残差满足：
ν + = ( I − J K ) ν − \\nu\^+=(I-JK)\\nu\^- ν+=(I−JK)ν−
其正确协方差为：
Cov ( ν + ) = ( I − J K ) S − ( I − J K ) T \\operatorname{Cov}(\\nu\^+) =(I-JK)S\^-(I-JK)\^T Cov(ν+)=(I−JK)S−(I−JK)T
在本例中：
Var ( ν + ) = ( 1 − 0.8 ) 2 × 0.05 = 0.002 \\operatorname{Var}(\\nu\^+) =(1-0.8)\^2\\times0.05 =0.002 Var(ν+)=(1−0.8)2×0.05=0.002
使用正确的协方差，得到：
0.12 2 0.002 = 7.2 \\frac{0.12\^2}{0.002}=7.2 0.0020.122=7.2
所以，不是"后验残差永远不能检验"，而是**不能对后验残差直接套用更新前创新的协方差公式**。
#### 11.2 预测误差与测量噪声相关时，要保留交叉项
前面默认：
E \[ x \~ n T \] = 0 E\[\\tilde x n\^T\]=0 E\[x\~nT\]=0
若定义：
C = E \[ x \~ n T \] ≠ 0 , C=E\[\\tilde x n\^T\]\\neq0, C=E\[x\~nT\]=0,
则应使用：
S = J P − J T + R + J C + C T J T \\boxed{ S=JP\^-J\^T+R+JC+C\^TJ\^T } S=JP−JT+R+JC+CTJT
这里的正负号来自本文误差和创新定义；若更换误差定义，需要一致地重新推导。
同一份原始数据既参与预测又参与构造观测时，尤其需要检查独立性。例如，某个估计结果已经融合了 IMU，却又被当作完全独立的观测回灌到使用同一 IMU 的滤波器中，就不能直接假定没有相关性。
#### 11.3 坐标系、单位与误差定义必须一致
对 ESKF，应保证创新、雅可比、协方差描述的是同一个局部误差空间。
典型问题包括：
* 创新使用世界坐标系，但协方差仍在机体坐标系。
* 创新使用角度制，协方差却使用弧度制。
* 雅可比按右扰动推导，协方差却按另一种误差定义传播。
* 姿态观测以四元数四个分量直接相减，又直接使用 4 个自由度。
例如，三维姿态通常具有三个局部自由度，应该根据实际构造的姿态创新及其协方差来决定检验维数，而不是按存储变量的个数机械决定。
#### 11.4 不要为了通过门控而反复增大 R R R
增大 R R R 会扩大 S S S，从而降低 NIS。
但这只是改变了不确定性声明，并没有证明观测变好了。
根据成像质量、特征几何、接触条件等外部质量信息构建 R R R，可以作为建模的一部分。但其统计解释依然依赖于该条件噪声模型是否合理。
如果看到当前残差过大，就不断增大 R R R 直到通过，那么这个过程不能继续解释为原先固定显著性水平的标准 NIS 检验。
同样，经过裁剪、鲁棒加权或数据依赖变换后的残差，也不能未经推导就沿用原始卡方门限。
#### 11.5 持续拒绝观测，也可能是预测已经错误且过度自信
假如预测状态已经偏离真实状态，但 P − P\^- P− 仍然很小，那么一个正确的观测也可能产生很大的 NIS。
此时若只会永久拒绝观测，滤波器可能失去恢复机会。
因此，工程流程应区分"偶发外点"和"持续模型失配"。对于持续异常，需要结合独立观测、初始化状态、运动模型和传感器诊断，决定是否重新初始化、调整建模或进入恢复模式，而不是只凭 NIS 认定传感器长期失效。
#### 11.6 不要显式计算矩阵逆
虽然公式为：
ϵ = ν T S − 1 ν , \\epsilon=\\nu\^TS\^{-1}\\nu, ϵ=νTS−1ν,
实现时更适合：
S = L L T , L w = ν , ϵ = w T w S=LL\^T, \\qquad Lw=\\nu, \\qquad \\epsilon=w\^Tw S=LLT,Lw=ν,ϵ=wTw
这样不需要显式计算 S − 1 S\^{-1} S−1。NumPy 的 Cholesky 分解可用于对称正定矩阵。\[4\]
如果分解失败，应先检查协方差构造、单位和数值条件。不能无条件加入一个很大的对角项，把模型问题掩盖掉。
#### 11.7 自由度不是状态维数，也不一定等于原始数据项数
本文默认 S S S 正定，此时自由度为创新维数 m m m。
如果存在精确冗余、 S S S 在数学上确实奇异，那么应在有效子空间内构造检验。设有效秩为 r r r，只有在创新确实落在该子空间且相应高斯模型正确等条件下，才可使用对应的 r r r 维卡方检验。
不能随便使用伪逆，再忽略创新在零方差方向上的非零分量。若模型声称某方向的方差严格为零，而创新在该方向却非零，这本身就是模型不相容的表现。
还要注意：
rank ( J ) \< m 不意味着 rank ( S ) \< m \\operatorname{rank}(J)\
\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