カルマンフィルタ入門

カルマンフィルタの導出 †

カルマンフィルタを、簡単に説明したい。

• １．ここでは、まず

  y=ax+b

  の最も簡単な線形回帰の場合について、最小二乗法、最尤推定法、条件付き期待値について説明する。
• ２．次に、最小二乗法を使って、線形1次の離散型確率モデルのカルマンフィルタの漸化式を導出する。

  yt=hxt+vt        観測式
  Xt+1=fxt+gut+wt　状態推移式
  vt,wtは、それぞれ誤差項で平均0、分散σv^2、σw^2の白色ノイズとする。
  また、相互に独立とする。E(vtWt)=0。

  • パラメータ(f,g,h)は、定数である。また、分散も時間的に変化しないと仮定している。
  • この場合は、xtもytも、ガウス分布に従うことになり、尤度関数の計算も容易である。
  • そして、観測値(y0,...,yt)が与えられた時の、xtの条件付き期待値は、最小二乗法でも、最尤法でも求められる。そして、その逐次計算方法がカルマンフィルタリングの解法である。
• 3.非線形の場合は、

  yt=h(xt)+vt        観測式
  Xt+1=f(xt,ut)+wt　状態推移式

  であらわされるが、これは関数h(.)、f(・,・)が所与ならば、線形近似することで

  yt=htxt+vt        観測式
  Xt+1=ftxt+Gtut+wt　状態推移式

  の形の、パラメータが時間的に変化する場合の、カルマンフィルタを用いて、状態推定が可能となる。

最小二乗法：y=ax+b　の場合 †

入力xと出力yの関数関係を想定し(これをモデルと呼ぶ）観測データy1;t=(y1,...,yt)とx1;t=(x1,...,xt)から、パラメータを推定する問題を考える。 推定の規範として、下記の二乗誤差を最小にするものとしよう。そして、xとyの関係（モデル）をy*=ax+bと想定する。y*は推定値である。

SE=Σ（yt-y*t)^2 =Σ（yt-axt-b)^2

この時、二乗誤差を最小にする(a,b)の推定値(a*,b*)は、次式で求められる。

このとき誤差をri=yi-(axi+b)　i = 1, ..., tで表わすと、

SE=∑ ri^2　　--->Min(a,b)

SEをそれぞれの係数aとbで、微分すれば0となる必要がある。この条件は

Σ2ri・xi=0
Σ2ri=0
ri=yi-(axi+b)

これを正規方程式とよぶ。この線形連立式の解が求めるパラメータ(a*,b*)である。

• 2番目の式から誤差の総和はゼロであり、誤差と入力変数のデータベクトルは直交していることを正規方程式は表わしている。これが祇差二乗和の最小化の条件となっていることが理解できる。(Σri・xi=0)
• aとbの推定値は、正規方程式を解いて、次式で表わされる。

  a*=σxy/σxx
  b*=E(y)-(σxy/σxx)・E(x)
  但し、σxy=∑(xi-E(x))・(yi-E(y))/t,E(x)=∑xi/t
  tが増えると推定値も変化することに留意。

• 詳細は、ピタゴラスの定理、あるいは最小二乗法のページを参照のこと

• 以上より、ytの推定値は、以下の通りである。

  y*t=a*xt+b*

  書き換えれば

  y*t=(σxy/σxx)Xt+E(y)-(σxy/σxx)・E(x)

  この式は、平均や分散が観測値が増えると変わるが、漸化式になっていない。

最尤推定法：y=ax+u †

これは、uが確率変数であるので、確率モデルと呼ばれる。 観測値yをm次元ベクトル、入力　xをn(n>m)次元ベクトルとする。データセット｛xt, yt t= 1,..., T} から、パラメータa を求める問題を考える。ただしu は分平均0、分散σ^2のガウスノイズである。このとき、y は中心位置ax、分散σ^2の正規分布N(y|ax,σ^2) にしたがう。

P(y|x,a)=N(y|ax,σ^2)=SQRT(2πσ^2)^(-m/2)EXP[-||y-ax||^2/)2σ^2)]

中心位置が入力x に依存しているため上式で与えられるy の分布はx の条件付きである。対数尤度を取ると

LogP(y|x,a)=(-1/2σ^2)||y-ax||^2 + aに無関係な定数

で表わされる。観測データy1;t=(y1，y2,...yt)　x1;t=(x1,x2,...,xt)の生起確率は、独立なガウス分布に従うことから、y1;tの正規確率は上記の確率密度関数ので表わされるので、対数尤度関数は和の形になり

P(y1,y2,...,yt|a)=P(y1|a)P(y2|a)....P(yt|a)

であることから、観測データ所与のもとでの尤度関数は

L(a)=(-1/2σ^2)　Σ||yt-axt||^2 + aに無関係な定数

このことから、L(a) をa について最大化することと、二乗誤差E=Σ||yt-axt||^2を最小化することは等価である。したがってこの場合の最尤推定法は最小二乗法と等価である。

線形1次離散型確率モデルのカルマンフィルタ †

目標は、状態xtの推定について、推定誤差が最小分散を与える不偏推定量として、カルマンフィルタの式を求めることにある。

• 観測値は、出力ytのみ
• 線形推定かつ漸化式による状態推定を行う。すなわち前期の推定値と今期の観測値の線形和で、今期の状態を推定する。

X*t+1=Lｔx*t＋Ktyt+1

の形で毎期の状態を推定する。

• 入力utと出力ytのモデルに、下記の確率差分方程式を考える。

  yt=hxt+vt        観測式
  Xt+1=fxt+wt　状態推移式

  次の仮定を置く。

  E(X0)=μ(x0)
  E(vt)=0,E(vkvj)=σvvδkj
  E(wt)=0,E(wkwj)=σwwδkj
  Wk,Vj,x0は互いに無相関。共分散が0。

  xtのフィルタリング値をX*tで表わし、この確率変数の推定誤差をεXtで表わす。

  εXt=X*t-xt

不偏推定量の条件：カルマンゲインに基づく修正（update式） †

不偏性の条件は、

E(εXt)=E(X*t-xt)=0

である。 この条件から、LとKはどのような条件を満たすべきかを調べる。

εXt+1=X*t+1-xt+1=Ltx*t＋Ktyt+1 -xt+1

である。上式に観測式を代入し、Ltxtを加えて引く

εXt+1=X*t+1-xt+1=Ltx*t＋Kt(hxt+1+vt+1) -xt+1 -Ltxt+Ltxt

Xt+1にシステム推移式を代入

εXt+1=Lt(x*t-xt)+Kt{h(fxt+wt)+vt+1}-(fxt+wt)+Ltxt

Xt,ut,vt,wtについて整理

εXt+1=Lt(x*t-xt)+(Kthf-f+L)xt+(Kth-1)wt+Ktvt+1

誤差項のVt,Wtの期待値は0である。 不偏性の条件は上式の期待値が0であるので

E(εXt+1)=(Kthf-f+L)E(xt)=0

以上より、不偏性の条件式は、次式で表わされる。

Lt=(1-Kth)f :不偏性の条件式

これを、推定式に代入すれば

X*t+1=Ltx*t＋Ktyt+1
     =fx*t-Kthfx*t+Ktyt+1
     =fx*t+Kt(yt+1-hfx*t)

不偏性から、次期の状態推定は、1期の予測値fX*tを、観測誤差yt-hfxtにあるゲイン定数Kを掛けて修正することが導かれた。 ここで、求められたupdateの式を整理しておく。

X*t+1=X~t+1+Kt(yt+1-y~t+1)  ;状態のupdate式
X~t+1=fx*t    :状態の1期予測式
y~t+1=hx~t+1  :出力の1期予測式

• カルマンゲインは、出力の観測誤差を状態の推定誤差に変換する役割を果たしている。
• 1期予測式は、元の観測式と状態推移式の両辺の期待値から自然に理解できるでしょう。

最小分散推定：カルマンゲインの漸化式 †

いまだカルマンゲインをどのように決めるかが判っていないので、調べてみよう。 カルマンゲインは、予測誤差を最小になるように、最小分散を与える最適化を行うことで求められる。 そのために、まず予測誤差X~t+1-xt+1の分散を計算してみる。

X~t+1-xt+1=fx*t-(fxt+wt)=f(x*t-xt)-wt

x*t-xtの分散がPt、wtの分散がσwwであるので、左辺の予測誤差の分散は次式で与えられる。

P~t+1=fPtf+σww　　　：予測誤差分散の推移式

これが、予測誤差の分散の漸化式である。

次に、状態推定誤差を計算する。yとX*の項をupdateの式を使って消去していく。

εXt=x*t-xt
   =X~t+Kt(yt-y~t)-Xt
   =(X~t-xt)+Kt(hxt+vt-hx~t)
   =(1-Kth)(x~t-xt)+Ktvt

この推定誤差は

εXt=(1-Kth)εX~t+Ktvt

で表わされるので、この状態推定誤差分散Ptを1期予測の分散P~tとVtの分散σvvを使って表わすことができる。

Pt=(1-Kth)P~t(1-Kth)+KtσvvKt

これが、分散のupdate式である。

最終ステップは、Kt(カルマンゲイン）の決め方である。カルマンゲインは、推定誤差共分散Pkを最小にするように決めるのが最適である。

そこで、上式をKtで微分して0とおく。

-2P~th(1-Kth)+2σvvKt=0
-P~th + P~tKth+σvvKt=0

整理すれば、最適ゲインの式が求められる。

Kt=P~th/[σvv+hPth]　　：カルマンゲインの計算式

このKtをPtの式に代入すれば

Pt=(1-Kth)P~t　　　　　：状態推定誤差のupdate式

を得られる。これが分散のupdate式である。

Kalman Filterのアルゴリズム：以上の整理 †

• 予測ステップ：t期の状態を適当に与えて、状態と分散の予測を行う。

  X~t+1=fx*t    :状態の1期予測式
  y~t+1=hx~t+1  :出力の1期予測式
  P~t+1=fPtf+σww　　　：予測誤差分散の推移式

• 修正ステップ：観測値に基ずいて、updateする。

  X*t+1=X~t+1+Kt(yt+1-y~t+1)  ;状態のupdate式
  Kt=P~th/[σvv+hPth]　　：カルマンゲインの計算式
  Pt=(1-Kth)P~t　　　　　：状態推定誤差のupdate式

  • カルマンゲインは、出力の観測誤差と状態の推定誤差の比を適切に与えていることに留意しましょう。

Kalman Filterのアルゴリズム：以上の整理 †

• 予測ステップ：t期の状態を適当に与えて、状態と分散の予測を行う。

  X~t+1=fx*t    :状態の1期予測式
  y~t+1=hx~t+1  :出力の1期予測式
  P~t+1=fPtf+σww　　　：予測誤差分散の推移式

• 修正ステップ：観測値に基ずいて、updateする。

  X*t+1=X~t+1+Kt(yt+1-y~t+1)  ;状態のupdate式
  Kt=P~th/[σvv+hPth]　　：カルマンゲインの計算式
  Pt=(1-Kth)P~t　　　　　：状態推定誤差のupdate式

• 時間を進めて、予測ステップに戻って繰り返す。
  • カルマンゲインは、出力の観測誤差と状態の推定誤差の比を適切に与えていることに留意しましょう。

多入力多出力多状態の場合：一般の確率差分モデル †

下記の確率差分方程式を考える。

yt=Hxt+vt        観測式
Xt+1=Fxt+Gut+wt　状態推移式

（F,G,H)は、次元に対応したサイズのマトリックスである。次の仮定を置く。

E(X0)=μ(x0)
E(vt)=0,E(vkvj)=Qδkj
E(wt)=0,E(wkwj)=Rδkj
Wk,Vj,x0は互いに無相関。共分散が0。

• この場合も、導出の手順も、アルゴリズムも自然に拡張できる。
• ただし、カルマンゲインは、マトリックスKtであり、誤差共分散行列PtのtraceをKtで偏微分してカルマンゲインを求めることになる。これは各状態の推定誤差の二乗和を最小にすることとみなせる。

εXt=x*t-xt
Pt=Cov(εXt)

Pk=|σ1^2        |
   |　　　・       |
   |　　　　・     |
   |　　　　　σn^2|

tracePk=σ1^2+.....+σn^2

• 導出は省略し、推定式を以下に紹介する。

一般のKalman Filterのアルゴリズム：以上の整理 †

• 予測ステップ：t期の状態を適当に与えて、状態と分散の予測を行う。

  X~t+1=Fx*t+Gut    :状態の1期予測式
  y~t+1=Hx~t+1  :出力の1期予測式
  P~t+1=FPtF'+Q　　　：予測誤差分散の推移式

• 修正ステップ：観測値に基ずいて、updateする。

  X*t+1=X~t+1+Kt(yt+1-y~t+1)  ;状態のupdate式
  Kt=P~tH'[R+HPtH']^(-1)　　：カルマンゲインの計算式
  Pt=(I-KtH)P~t　　　　　：状態推定誤差のupdate式

  • 1次元の場合と同様のアルゴリズムになっていることを比較して理解しましょう。

一般型の場合のカルマンゲインの求め方 †

1次元と同様に次の推定誤差ベクトルεxtが得られるまでは容易に進められます。

εxt=X*t-Xt=(I-KtH)εx~t+Ktvt

そこで、誤差共分散行列は

Pt=Cov(εxt)=E(εxtεxt')
  =[I-KtH]P~t[I-KtH]'+KtRKt'

ここで、面倒なのでサフィックスtを省略して表示し、P~t=Pとすれば

Pt=[I-KH]P[I-KH]'+KRK'
  =P-KHP-PH'K'+KHPH'K'+KRK'

tracePtをtrPtで表示して

trPt=trP-2trKHP+trK(HPH')K'+trKRK'

ここで、次の恒等式を使う。

(PH'K')'=KHP
tr(PH'K')=tr(KHP)
∂trABA'/∂A=2AB where B is symmetric
∂trAC/∂A=C'

これらを使うと、PtのトレースをカルマンゲインKで偏微分して0となるKを求める。

∂trPt/∂K=-2PH'+2KHPH'+2K=0

よって、カルマンゲインKの計算式は

K=PH'(HPH'+R)^(-1)

で与えられる。　（QED) きちんとサフィックスを付けて表示すれば

 Kt=PtHt'(HtP~tHt'+R)^(-1)

KtをPtの次の表示式に代入して整理すると

Pt=[I-KtH]P~t[I-KtH]'+KtRKt'

次のPtのupdate式を得る。

Pt=(I-KtHt)P~t

もう一つのカルマンゲインの表現 †

Pt=[(p~t)^(-1)+H'R^(-1)H]^(-1)
Kt=PtH'R^(-1)

• この場合は、状態次元の正方対称行列の逆行列計算が必要になる。一般に、前の表示のほうが次元が少ないので標準的によく用いられる。逆に観測の次元が状態の次元より多い場合はこちらの方が簡単かもしれない。

• 上のPt式の証明は、逆行列の補題から容易に可能である。

  Pt=(I-KtHt)P~t=P~t-P~tH'(HP~tH'+R)^(-1)HP~t

  逆行列補題より

    =[(p~t)^(-1)+H'R^(-1)H]^(-1)

• またKtの式は、ゲイン表現の恒等式から導ける。

  Kt=P~tH'(HP~tH'+R]^(-1)

  恒等式から

  =[(p~t)~(-1)+H'R^(-1)H]^(-1)H'R^(-1)
  =P~tH'R^(-1)

• 逆行列補題の証明

  [p^(-1)+H'R^(-1)]^(-1)=[p^(-1)+H'R^(-1)]^(-1)[I+H'R^(-1)HP-H'R^(-1)HP]
  =[p^(-1)+H'R^(-1)]^(-1)=[p^(-1)+H'R^(-1)]P -H'R^(-1)HP
  =P-[p^(-1)+H'R^(-1)]^(-1)H'R(-1)HP

• ゲイン表現の恒等式の証明

  H'=H'
  H'R(-1)HPH'+H'=H'+H'R(-1)HPH'
  H'R^(-1)[HPH'+R]=[p(-1)+H'R(-1)H]PH'
  [p(-1)+H'R(-1)H]^(-1)H'R^(-1)=PH'[HPH'+R]^(-1)

非線形系への拡張カルマンフィルタ †

非線形の場合も、線形化を行って、時変パラメータ行列を持つ線形差分確率モデルとみなすことで拡張できる。詳細は、参考文献を参照のこと。 力学系の多くは、この方法で状態推定が可能である。

参考 †

• G.Welch and G.Bishop;An Introduction to the Kalman Filter