ssk tech blog

自律移動ロボットの話を書きます

【論文】Unscented Kalman Filterのパラメータを逐次的にチューニングする論文を読む【2018】

目次

はじめに

こんにちは.ササキ(@saitosasaki)です.
今回はある技術ブログのある記事で最後のほうに紹介されていた「アンセンテッドカルマンフィルタのパラメータをオートチューニングする」論文が気になったので読んでみました.

論文
[A Robust Adaptive Unscented Kalman Filter for Nonlinear Estimation with Uncertain Noise Covariance](https://www.researchgate.net/publication/323621421_A_Robust_Adaptive_Unscented_Kalman_Filter_for_Nonlinear_Estimation_with_Uncertain_Noise_Covariance}

UKF(Unscented Kalman Filter)について

 以下の東工大の山北先生の解説論文が6ページと短く、わかりやすくておすすめです。アンセンテッドの由来も書いてあって読み物としても面白いです。

UKF (Unscented Kalman Filter)って何?
ちなみにUnsecentedの由来は以下.

そこに込められている意味は手垢のついていない, まったく新しい発想に基づく手法であると いう意味らしい ,足立先生はこれらのことを考慮してUnscented を「無香料」と訳されている.

 「UKFの Unscented変換って結局なんなの?」という人は以下の記事を読むと良いと思います.
参考文献の片山先生の『非線形カルマンフィルタの基礎』を辿れば確認できるんですが,Gauss-Hermite求積由来です.
https://t.co/cpeAeeHV6D

上の記事消されてますね・・・悲しい.

カルマンフィルタの派生形についてのまとめ

論文を読む前に、有名なカルマンフィルタの派生形について整理しました。
基本『確率ロボティクス』のまとめです。ここには書いてないですけど,Ensemble Kalman Filter(EnKF)も有名ですね.
以前まとめたんですが、書く機会がなかったんでとりあえずここにまとめときます。

  • EKF(Extended Kalman Filter)はモデルを線形近似すれば、後はカルマンフィルタのアルゴリズムと同じ。
  • UKFは非線形なモデルのままで、事前分布の平均値の他に分布からシグマ点を数個とって、モデルに通した点から事後分布(正規分布)の平均値と分散を求める。
    分布からいくつかの点をサンプルするという意味ではUKFとPF(Particle Filter)は似ていて、違いはサンプルする点が決定論的かランダムか。

  • EKFとUKFの比較
    ・EKFの方がわずかにUKFより計算は速い.
    ・UKFの方が推定の正確性が上だが、非線形性や事前の状態の不確かさによってあまり差がないことも多い。
    ・UKFの場合微分が不要.
    ・UKFはハイパーパラメータが増えるので面倒.

論文

概要

  • 通常のUKFの問題点
    ユーザが事前に仮定した雑音分布と実際の非線形システムにおける実際のものとの間の不一致により、性能が劣化または発散する.
  • 提案手法
    本論文では、不確実な雑音の共分散による状態推定の精度とロバスト性を改善するためのRobust Adaptive UKF(RAUKF)を提案する。
  • アルゴリズム
    1.各タイムステップで、標準のUKFが最初に実行され、新しく取得された測定データを使用して状態推定値を取得する。
    2.次に、現在の雑音共分散を更新する必要があるかどうかを判断するためにオンラインで故障を検出する機構を使用する。
     必要であれば、革新ベースの方法と残差ベースの方法を使用して、それぞれプロセスと測定の現在のノイズ共分散の推定値を計算する。 重み係数を利用することによって、フィルタは最後の雑音共分散行列を新しい雑音共分散行列としての推定と組み合わせる。
    3.最後に、新しい雑音共分散行列と以前の状態推定に従って状態推定を修正する。
  • 結果
    通常のUKFおよび他の適応UKFアルゴリズムと比較して、RAUKFは実際の雑音共分散に速く収束し、  
    ロバスト性、精度、および不確かな雑音共分散による非線形推定のための計算に関してより良い性能を達成した。

Standard UKF(SUKF)による通常の状態推定

  • 動作モデル

x_k = f( x_{k-1}) + w_{k-1} (1)

  • 誤差共分散

P_{k}^{xx} = E((x_k - \hat{x}_{k \vert k})(x_k - \hat{x}_{k \vert k})^T)(2)
ここで、Eは平均を表す。

  • 観測モデル

z_k=h (x_k) + v_k(3)

f:id:ssk0109:20190502143349p:plain f:id:ssk0109:20190502143422p:plain
f:id:ssk0109:20190502143628p:plain

Robust Adaptive Unscented Kalman Filter(RAUKF)

  • 故障検出機構
    RAUKFは、現在の理論的推定値とシステムに障害が発生した場合の最後のデータとの重み付けの組み合わせに基づいて、QおよびRを適応的に調整する。したがって、雑音共分散行列を修正する必要があるかどうかを判断するには、障害検出メカニズムを採用する必要がある。
    本論文で採用されている障害検出メカニズムは、研究 [19]で使用されているものと類似している。

\phi_k =\mu_{k}^{T} (\overline{P}_{k \vert k-1}^{zz})^{-1} \mu_k(17)

kはs自由度のχ2分布をもち、sはµkの次元である。 1-σの信頼性レベルを有する故障を検出したいと仮定する。ここで、σは選択されたパラメータである。 それから、

P(\phi_k > \xi^{2}_{\sigma、s} )= \sigma (18)

のように、ϕkの\xi^{2}_{\sigma、s}分布によって決定される閾値\xi^{2}_{\sigma、s}がある。 したがって、予め選択されたσの値を使用して、システム障害が1-σの信頼性レベルで検出され得るように、式(18)から対応する障害検出の閾値\chi^{2}_{\sigma、s}を定義する。

  • プロセスノイズQのAdaptive Adjustment
    イノベーションµkは、新しい測定値の結果としてフィルタに利用可能な追加情報を表す。 したがって、µkはフィルタ適応に最も関連のある情報と考えられ、雑音共分散を推定するために使用できる[23]。 式(1)によれば、プロセスノイズは、w_{k-1} = x_k - f(x_{k-1})として表すことができる。 さらに、SUKFの方程式から、プロセスノイズは次のようになる。

f:id:ssk0109:20190503105909p:plain

上記の導出で使用される線形化による2つの近似には、\overline{x}_{k \vert k-1}\approx f(\hat{x}_{k \vert k-1}) および\overline{z}_{k \vert k-1}\approx h(\hat{z}_{k \vert k-1})がある。 ここでは、時間k-1での平均の前方の展開は前方に展開したシグマ点から計算された平均によって近似される; それぞれ、平均値の観測値はシグマ点の観測値を介して計算された観測値の平均によって近似される。
 したがって、プロセスノイズ共分散行列Qk-1の推定は、次のように推定できる。

f:id:ssk0109:20190503105945p:plain

ここで、cov(∗)は共分散、 E(∗)は期待値である。
上記の式を実装するために、E(\mu_k \mu_k^{T}) は通常、windowing methodを使用して\mu_k \mu_k^{T}を時間平均することによって近似される。 moving window methodsを使用する代わりに(研究[15,19,23,29]のように)、本論文は、最後の雑音共分散値と現在の推定値とのバランスをとるために重み係数λを利用することによってQを適応的に調整する。 重み係数λは、更新強度を保証するために下限λ0∈(0、1)で設定され、φkが事前設定された閾値を超える場合、φkの上昇につれて増加する。 したがって、システムプロセスノイズ共分散行列は次のように更新される。

Q_{k-1} = (1 - \lambda)Q_{k-1} + \lambda ( K_k \mu_k \mu_k^{T}K_k^{T} )(21)
\lambda = max(\lambda_0,(\phi_k - a \times \chi_{\sigma,s}^{2})/\phi_k)(22)

ここで、a(a> 0)は実際の環境に応じた調整パラメータであり、aが大きいほどデフォルトのλ0を使用する可能性が高いことを意味する。 特に、λ0とaの間のトレードオフが、共分散の更新が新しいイノベーションに対してどれほど敏感で あるかを決定する。

  • 観測ノイズRのAdaptive Adjustment
    時間ステップkにおける測定雑音共分散行列Rはまた、以下のようにイノベーションに基づくアプローチによって推定できる。

f:id:ssk0109:20190503111005p:plain
ここで、

f:id:ssk0109:20190503111044p:plain

である。(導出プロセスについての詳細は、参考文献[30]を参照。)一般的に、R_{k}は共分散行列であるため、R_{k}は正定値であるべきである。しかしながら、式(23)からのその推定値\hat{R}_{k}は、2つの正定値行列を減算することによって得られるので、正定値行列であることを保証できない。それから、ノイズ共分散行列の主対角に負のパラメータがあると、適応フィルタは突然発散する[22]。正定行列\hat{R}_{k}を取得するには、残差ベースのアプローチを使用する。式(3)から、時間ステップkにおける測定雑音は、v_k = x_k -h(x_k)として導出することができる。次に、タイムステップkにおける残差ベクトルは、

\varepsilon_k = z_k - h(\hat{x}_{k \vert k})(24)

で与えられる。
さらに、非特許文献2によれば、残差ベクトルεkに基づく測定雑音共分散の推定は、次式で与えられる。

f:id:ssk0109:20190503110148p:plain

ここで、

f:id:ssk0109:20190503110126p:plain

前のセクションで使用された方法と同様に、Rkは以下のように下限値δ0∈(0,1)で設定された重み係数δを利用して更新される。

R_k = (1 - \delta)R_k + \delta (  \varepsilon_k \varepsilon_k^T+ \hat{S}_{k \vert k-1}^{zz} )(29)
\delta = max(\delta_0,(\phi_k - b \times \chi_{\sigma,s}^{2})/\phi_k)(30)

ここで、b(b>0)も実際の環境に依存する調整パラメータである。 bおよびδ0の選択は、aおよびλ0の選択と同じである。

  • 状態推定の修正

f:id:ssk0109:20190502143015p:plain

  • 疑似コード

f:id:ssk0109:20190502140259p:plain:w400

シミュレーション結果

  • システムモデル

f:id:ssk0109:20190503114326p:plain

f:id:ssk0109:20190503114420p:plain

未知のノイズのある環境下でのシミュレーション結果

f:id:ssk0109:20190502140042p:plain:w400
f:id:ssk0109:20190502140127p:plain:w500

動的に変化するノイズのある環境下でのシミュレーション結果

プロセスノイズの共分散の条件
f:id:ssk0109:20190502135914p:plain
実験結果
f:id:ssk0109:20190502135508p:plain:w500
f:id:ssk0109:20190502135533p:plain:w500

感想

紹介されていたブログによると「パラメータをオートチューニングする論文」ということだったので、読む前はUKF固有のハイパーパラメータを実験したデータを基に求める論文なのかなと思ったんですが、違いました。下記の論文が自分が思ってたものに近いのかな?今後、読むべき課題とします。

Complete offline tuning of the unscented Kalman filter

後、論文をブログにまとめるのは「そこそこ文章が必要で大変(Google翻訳で楽しようと思ったら修正が結局大変)」かつ「制約が無いので長くなりがち」なので、発表形式のスライドにまとめた方が良いという気付きを得ました。