一貫性(Consistency)

精度は推定器がどれだけ間違っているかを教えてくれるが、**一貫性(consistency)**は推定器が自分がどれだけ間違っているかを知っているかを教えてくれる。SLAMの推定器は、状態推定値 $\hat{\mathbf{x}}$ (姿勢、ランドマーク)と、その確信度を表す共分散 $P$ の両方を出力する。実際の推定誤差が、報告された共分散と統計的に整合している場合、その推定器は一貫している——つまり、その推定器は過信(共分散が小さすぎる)でもなく、過小評価(共分散が大きすぎる)でもない。

NEES: 標準的な一貫性検定

一貫性を検査するための標準的な手段は**NEES(Normalized Estimation Error Squared、正規化推定誤差二乗)**である。

\epsilon_k = (\mathbf{x}_k - \hat{\mathbf{x}}_k)^T P_k^{-1} (\mathbf{x}_k - \hat{\mathbf{x}}_k)

ここで $\mathbf{x}_k$ は時刻 $k$ における真の状態である。推定器が一貫していて誤差がガウス分布に従う場合、NEESは自由度 $n = \dim(\mathbf{x})$ のカイ二乗分布に従うため、その期待値は状態の次元に等しくなる。

\mathbb{E}[\epsilon_k] = n

1回の実行のNEESはノイズを含む確率変数であるため、検定はモンテカルロ実験として実行される。すなわち、同じシナリオを独立したノイズの実現値で $M$ 回繰り返し、平均を取る。

\bar{\epsilon}_k = \frac{1}{M} \sum_{i=1}^{M} \epsilon_k^{(i)}, \qquad M\,\bar{\epsilon}_k \sim \chi^2_{Mn},

そして、 $\bar{\epsilon}_k$ が $\chi^2_{Mn}$ の分位点(を $M$ で割った値)から導かれる信頼区間の中に収まっているかを確認する。解釈は以下の通り。

平均NEES ≈ 状態次元 — 一貫している。
平均NEESがはるかに大きい — 過信している: フィルタが自分自身を過度に信頼しており、最終的には発散を引き起こす(観測の重みが下げられたり、拒否されたりする)。
平均NEESがはるかに小さい — 保守的: 危険ではないが、推定器は情報を無駄にしている。

状態の位置ブロックと姿勢(向き)ブロックを別々に評価することが一般的であり、これにより、片方の正直なブロックが、もう片方の過信したブロックを隠してしまうことを防げる。

NIS: 真値なしでの検定

NEESには真値が必要だが、実際のロボットにはそれがほとんど存在しない。オンラインで機能する対となる検定が**NIS(Normalized Innovation Squared、正規化イノベーション二乗)**である。イノベーション $\boldsymbol{\nu} = \mathbf{z} - h(\hat{\mathbf{x}})$ とイノベーション共分散 $S$ を持つフィルタに対して、

\text{NIS} = \boldsymbol{\nu}^T S^{-1} \boldsymbol{\nu} \sim \chi^2_{\dim(\mathbf{z})}

が一貫性のもとで成り立つ。このアイデアは、その名前を意識せずとも常に使われている。カイ二乗ゲーティング(NISが分位点の閾値を超える観測を拒否すること)は、観測ごとの一貫性検定であり——これに供される共分散が正直である場合にのみ機能する。

SLAM推定器が非一貫になる理由

一貫性は単なる理論上の細かい話ではなく、SLAMにおける実際的で現実的な問題である。

変化する推定値での線形化。 フィルタベースやスライディングウィンドウの推定器は非線形モデルを線形化するが、同じ変数を時間をかけて異なる推定値で線形化すると、本来観測不能であるべき方向(例: 視覚-慣性オドメトリにおける全体位置とヨー角)に沿って偽の情報が注入される。その結果、実際には情報が存在しない方向に沿って共分散が縮小し——過信が生じる。
凍結されたマージナリゼーションの事前分布。 古い状態をマージナライズした後、残った事前分布は古い推定値で線形化されたままで再線形化できない。それを新しい推定値で評価された最新のヤコビアンと混ぜ合わせることは、典型的な「FEJ問題」である(Marginalizationを参照)。
ゲージの自由度。 フルSLAMでは、解は大域的な座標系(および単眼の場合はスケール)を除いて一意に決まらない。共分散はゲージ方向に沿って特異になるため、状態全体に対する単純なNEESは定義不能になる——ゲージを固定する(例: 最初の姿勢を固定する)か、ゲージ不変な形で誤差を評価すること。

対処法

より後のレベルで出会う対処法は以下の通り。

First-Estimate Jacobians (FEJ) — 各変数のヤコビアンを、その変数の最初の推定値で評価し、すべての情報が整合した方向に沿って入るようにする。
観測可能性制約付き設計 — 偽の情報を観測不能な部分空間から明示的に取り除く(OpenVINS系のフィルタのように)。
凍結された事前分布の再線形化を避けるマージナリゼーション手法、あるいは凍結された線形事前分布の代わりに非線形の因子を復元する手法。
平方根形式/情報形式での実装: 数値的な健全性のため、共分散が正定値性を失わないようにする。

実践的なレシピ

既知の真値と現実的なノイズパラメータを持つ、あなたのセンサー構成のシミュレーションを構築する。
ノイズシードのみが異なる、数十回のモンテカルロ試行を実行する。
位置ブロックと姿勢ブロックを別々に、平均NEESの時間変化をカイ二乗の境界に対してプロットする。
実データでは、NISとカイ二乗ゲートによって拒否される観測の割合を監視する——拒否率の上昇は、過信した推定器の現場での症状である。

SLAMにおける意義

ATE/RPEのような軌跡メトリクスは平均誤差しか測らないため、同じATEを持つ2つのシステムでも下流で全く異なる振る舞いをすることがある。非一貫なシステムは、過信した共分散をループクロージングのゲーティング、センサー融合、あるいはプランナーに送り込んでしまい、悪い判断を招く。確率的なSLAMバックエンド——特にEKFやマージナリゼーションを伴う固定ラグスムーザー——を評価または設計する際は、シミュレーションデータ上でNEESを確認することが、推定器が自身の不確実性について正直かどうかを検証する標準的な方法である。