EKF 拡張カルマンフィルター

 はじめに

カルマンフィルタといったら一般的に線形カルマンフィルタのことを指す。線形カルマンフィルタは線形システムのモデルにしか適用できない。
そこで、この線形カルマンフィルタを非線形モデルに適用できるよう拡張されたのが拡張カルマンフィルタ(Extended Kalman Filter, EKF)となります。

拡張カルマンフィルタ(Extended Kalman Filter)

線形カルマンフィルタの場合、状態空間モデルは以下の式で表されていました。

  • 状態空間モデル(線形)
x(k+1)y(k)=Ax(k)+Bu(k)+v(k)=Cx(k)+w(k)(1)(2)(1)x(k+1)=Ax(k)+Bu(k)+v(k)(2)y(k)=Cx(k)+w(k)

拡張カルマンフィルタの場合、状態空間モデルは以下の式で表わされます。

  • 状態空間モデル(非線形)
x(k+1)y(k)=f(x(k),u(k))+v(k)=h(x(k))+w(k)(3)(4)(3)x(k+1)=f(x(k),u(k))+v(k)(4)y(k)=h(x(k))+w(k)

但し、
f()f(⋅)x(k)x(k)及び、u(k)u(k)の非線形関数
h()h(⋅)x(k)x(k)の非線形関数

f()f(⋅)h()h(⋅)は、どちらの非線形関数であるため、直接共分散行列を計算することができません。しかし、これらの関数を線形近似することによって線形カルマンフィルタと同じ要領で解くことができます。線形システムと非線形システムは対立する関係ではなく、非線形システムの特殊な場合が線形システムであるととらえることが重要です。
拡張カルマンフィルタでは非線形の状態方程式と観測方程式の偏微分行列(ヤコビアン)を用います。

A(k)C(k)=f(x,u)x∣∣∣x=x^(k),u=u(k)=h(x)x∣∣∣x=x^(k)(5)(6)(5)A(k)=∂f(x,u)∂x|x=x^(k),u=u(k)(6)C(k)=∂h(x)∂x|x=x^(k)

この場合のカルマンフィルタの時間更新式は次のようになります。

  • 予測ステップ
 x^(k)=f(x^(k1),u(k1)) A(k1)=f(x,u)x∣∣∣x=x^(k1),u=u(k1)  C(k)=h(x)x∣∣∣x=x^(k) P(k)=A(k1)P(k1)AT(k1)+Q(k1)(7)(8)(9)(10)(7)事前状態推定値: x^−(k)=f(x^(k−1),u(k−1))(8)線形近似: A(k−1)=∂f(x,u)∂x|x=x^(k−1),u=u(k−1)(9)  C(k)=∂h(x)∂x|x=x^(k)(10)事前誤差共分散行列: P−(k)=A(k−1)P(k−1)AT(k−1)+Q(k−1)
  • フィルタリングステップ
 G(k)=P(k)CT(k)(C(k)P(k)CT(k)+R(k))1 x^(k)=x^(k)+G(k)(y(k)h(x^(k)) P(k)=(IG(k)C)P(k)(11)(12)(13)(11)カルマンゲイン行列: G(k)=P−(k)CT(k)(C(k)P−(k)CT(k)+R(k))−1(12)状態推定値: x^(k)=x^−(k)+G(k)(y(k)−h(x^−(k))(13)事後誤差共分散行列: P(k)=(I−G(k)C)P−(k)

x(k)=⎡⎣⎢x(k)y(k)θ(k)⎤⎦⎥(14)(14)x(k)=[x(k)y(k)θ(k)]

よって状態方程式は以下のようになります。

x^(k+1)=f(x^(k),u(k))=⎡⎣⎢100010001⎤⎦⎥x^(k)+⎡⎣⎢v000v000ω⎤⎦⎥u(k)(15)(15)x^−(k+1)=f(x^(k),u(k))=[100010001]x^−(k)+[v000v000ω]u(k)

但し、

u(k)=⎡⎣⎢Δtcosθ(k)Δtsinθ(k)Δt⎤⎦⎥(16)(16)u(k)=[Δt⋅cosθ(k)Δt⋅sinθ(k)Δt]

するとヤコビアンは以下のように求められます。

f(x,u)x=⎡⎣⎢100010vΔtcosθ(k)vΔtsinθ(k)1⎤⎦⎥(17)(17)∂f(x,u)∂x=[10v⋅Δt⋅cosθ(k)01v⋅Δt⋅sinθ(k)001]

次は観測値y(k)y(k)を定義します。
まず、観測ベクトルy(k)y(k)を以下のように定義します。

y(k)=[x(k)y(k)](18)(18)y(k)=[x(k)y(k)]

x,yx,yはそれぞれ、観測されたロボットのXX座標、YY座標を表します。
次に、観測値y(k)y(k)は「真の位置xgt(k)xgt(k)+観測雑音w(k)w(k)」で与えられるものとします。そうすると、観測値は以下の式で算出されます。

y(k)=[100100]xgt(k)+w(k)(19)(19)y(k)=[100010]xgt(k)+w(k)

例題の説明は以上です。
上記例題を実装したスクリプトはGitHubで公開しています。
GitHub - extended_kalman_filter.py

また、スクリプトを実行した結果は以下のようになります。
(画像をクリックするとYouTubeへジャンプします。)

青い点が拡張カルマンフィルタによる推定値になるのですが、その回りに見える黄色の薄い楕円は誤差楕円(Error Ellipse)となっています。


先駆者に感謝