はじめに

　本エントリーでは、カルマンフィルターで用いる観測データの一貫性チェックについて説明します。実用において観測は非常にノイジーであり、時に異常値をを含んだものとなります。そのため、外れ値をそのまま統合してしまうと状態推定が不安定になってしまう恐れがあります。そこで、毎回の更新ステップ時に得られている観測を統合すべきか否かを判断し、大きく外れた観測が得られている場合はその観測を拒否することで安定したフィルター状態を保つような仕組みが一般的に採用されています。一貫性をチェックする方法は幾つか提案がされていますが、本エントリーではその中でもひろく用いられているNormalized Innovation Squared (NIS)について具体的に解説します。

カルマンフィルターについては過去にもまとめています。

観測の一貫性判定

　カルマンフィルターでは、予測モデルと観測モデルの誤差が正規分布に従うという過程の下で状態量を推定します。なので誤差がのっている観測を扱うという点ではカルマンフィルターのモデルに既に織り込み済みです。しかし実用上では、仮定している正規性に従わないような外れ値的な観測が突然得られることも多く、そのような観測を全て統合してしまうと、状態推定が不安定になってしまう恐れがあります。

　そこで、得られている観測の"一貫性"を確認し、一貫した観測出ない場合は統合を行わないという方法が良くとられます。一貫性の判定手段としては、Normalized Innovation Squared (NIS)という値を用います。

状態量:${\bf y_t}$、観測:${\bf z_t}$および制御入力：観測:${\bf u_t}$とした場合の予測モデルと観測モデルは以下で表されます。

$$ \begin{align} \bf{y_t} &= { \bf A_t y_{t-1} + B_t u_t+G_tw_t } \\ \bf{z_t} &= { \bf H_t y_t+ v_t } \\ \newline {\bf w_t} & \sim N(0,{ \bf Q}_t) \\ {\bf v_t} & \sim N(0,{ \bf R}_t) \end{align} \tag{1} $$

${\bf w_t } \in R^n$と${ \bf v_t } \in R^m$は多次元正規分布に従うノイズです。各変数の意味については過去のエントリーをご参照ください。この時の予測と更新モデルの式は以下のようになります。

予測モデル：

$$ \begin{align} {\bf \hat{y}}_{t|t-1} &={\bf A}_t {\bf \hat{y}}_{t-1|t-1} + {\bf B}_t \bf{u_t} \\\ {\bf \Sigma}_{t|t-1} &={\bf A}_t {\bf \Sigma}_{t-1|t-1}{{\bf A}_t}^T + {\bf G}_t {\bf Q}_t {{\bf G}_t}^T \end{align} \tag{2} $$

更新モデル：

$$ \begin{align} { \bf S }&={ \bf H_t \Sigma_{t|t-1} H_t^T + R_t } \\\ { \bf K_t } &= { \bf \Sigma_{t|t-1}H_t^T S^{-1} } \\\ { \bf e_t } &={ \bf z}_t - { \bf H}_t { \bf \hat{y}}_{t|t-1} \\\ { \bf \hat{y}_{t|t} } &={ \bf \hat{y}_{t|t-1} + { \bf K}_t { \bf e}_t } \\\ { \bf \Sigma_{t|t} } &={ \bf (I -K_t H_t)\Sigma_{t|t-1} } \end{align} \tag{3} $$

　式(3)の${\bf e}_t$をInnovation, Residualなどといい、これを判定に用います。 ${\bf e}_t$は図１に示したように、平均：${ \bf \hat{y}}_{t|t-1}$で分散：$S$の分布に従うことが分かります。NISは、過去の情報によって予測されているこの${\bf e}_t$の分布を基に今回時刻$t$に得られた観測がこの分布上あり得そうな観測なのかそうでないかを判断します。

NISの定義は式(4)で表されます。

$$ NIS = {\bf e}^T_t {\bf S}^{-1}{\bf e}_t \tag{4} $$

　これは図１の分布においてのマハラノビス距離(Mahalanobis' distance)に他なりません。マハラノビス距離は、過去に対する現在のデータの一貫性を確認する方法として教師なし学習での異常検知等によく用いられます。カルマンフィルターの観測においては、平均：${ \bf \hat{y}}_{t|t-1}$で分散：$S$が時刻$t-1$までの状態と時刻$t$の予測によって決まっているという点が重要で、今回時刻$t$に得られた観測データが疎の分布に適合するかをみて異常か正常かを判断します。

ここでマハラノビス距離がカイ２乗分布に従うことを利用して、判断の基準としてはカイ２乗検定の考え方を用いることが出来ます。マハラノビス距離がカイ２乗分布に従う理由は、こちらのエントリーでまとめています。

biocv.hateblo.jp

カルマンフィルターの観測においては、特定の有意水準を設定しNISが棄却域である場合は異常と判断し式(3)の更新モデルの平均と分散の更新式をスキップするということを行います。