カルマンフィルタ入門
をテンプレートにして作成
[
トップ
] [
新規
|
一覧
|
単語検索
|
最終更新
|
ヘルプ
]
開始行:
*カルマンフィルタの導出 [#f13493d6]
カルマンフィルタを、簡単に説明したい。
-1.ここでは、まず
y=ax+b
の最も簡単な線形回帰の場合について、最小二乗法、最尤推定...
-2.次に、最小二乗法を使って、線形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 の場合 [#od8e5513]
入力xと出力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)
これを正規方程式とよぶ。この線形連立式の解が求めるパラメ...
--2番目の式から誤差の総和はゼロであり、誤差と入力変数のデ...
--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 [#z9cc70cf]
これは、uが確率変数であるので、確率モデルと呼ばれる。
観測値yをm次元ベクトル、入力 xをn(n>m)次元ベクトルとする...
P(y|x,a)=N(y|ax,σ^2)=SQRT(2πσ^2)^(-m/2)EXP[-||y-ax||^2/)...
中心位置が入力x に依存しているため上式で与えられるy の分...
LogP(y|x,a)=(-1/2σ^2)||y-ax||^2 + aに無関係な定数
で表わされる。観測データy1;t=(y1,y2,...yt) x1;t=(x1,x2,...
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 について最大化することと、二乗誤差...
*線形1次離散型確率モデルのカルマンフィルタ [#e573b06f]
''目標は、状態xtの推定について、推定誤差が最小分散を与え...
--観測値は、出力ytのみ
--線形推定かつ漸化式による状態推定を行う。すなわち前期の...
X*t+1=Ltx*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=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を、観測誤差...
ここで、求められた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期予測式は、元の観測式と状態推移式の両辺の期待値から自...
*最小分散推定:カルマンゲインの漸化式 [#r98585d5]
いまだカルマンゲインをどのように決めるかが判っていないの...
カルマンゲインは、予測誤差を最小になるように、最小分散を...
そのために、まず予測誤差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~...
Pt=(1-Kth)P~t(1-Kth)+KtσvvKt
これが、分散のupdate式である。
最終ステップは、Kt(カルマンゲイン)の決め方である。カルマ...
そこで、上式を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のアルゴリズム:以上の整理 [#s4b1b141]
-予測ステップ: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のアルゴリズム:以上の整理 [#s4b1b141]
-予測ステップ: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式
-時間を進めて、予測ステップに戻って繰り返す。
--カルマンゲインは、出力の観測誤差と状態の推定誤差の比を...
#ref(K-filter.JPG)
*多入力多出力多状態の場合:一般の確率差分モデル [#bf70221e]
下記の確率差分方程式を考える。
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であり、誤差共分...
εXt=x*t-xt
Pt=Cov(εXt)
Pk=|σ1^2 |
| ・ |
| ・ |
| σn^2|
tracePk=σ1^2+.....+σn^2
-導出は省略し、推定式を以下に紹介する。
*一般のKalman Filterのアルゴリズム:以上の整理 [#s4b1b141]
-予測ステップ: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次元の場合と同様のアルゴリズムになっていることを比較し...
*一般型の場合のカルマンゲインの求め方 [#t79688f3]
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で偏微分して...
∂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
*もう一つのカルマンゲインの表現 [#x4a13f30]
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)...
=[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)
*非線形系への拡張カルマンフィルタ [#v7dbf9ad]
非線形の場合も、線形化を行って、時変パラメータ行列を持つ...
力学系の多くは、この方法で状態推定が可能である。
*参考 [#p119c078]
-[[G.Welch and G.Bishop;An Introduction to the Kalman Fil...
終了行:
*カルマンフィルタの導出 [#f13493d6]
カルマンフィルタを、簡単に説明したい。
-1.ここでは、まず
y=ax+b
の最も簡単な線形回帰の場合について、最小二乗法、最尤推定...
-2.次に、最小二乗法を使って、線形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 の場合 [#od8e5513]
入力xと出力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)
これを正規方程式とよぶ。この線形連立式の解が求めるパラメ...
--2番目の式から誤差の総和はゼロであり、誤差と入力変数のデ...
--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 [#z9cc70cf]
これは、uが確率変数であるので、確率モデルと呼ばれる。
観測値yをm次元ベクトル、入力 xをn(n>m)次元ベクトルとする...
P(y|x,a)=N(y|ax,σ^2)=SQRT(2πσ^2)^(-m/2)EXP[-||y-ax||^2/)...
中心位置が入力x に依存しているため上式で与えられるy の分...
LogP(y|x,a)=(-1/2σ^2)||y-ax||^2 + aに無関係な定数
で表わされる。観測データy1;t=(y1,y2,...yt) x1;t=(x1,x2,...
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 について最大化することと、二乗誤差...
*線形1次離散型確率モデルのカルマンフィルタ [#e573b06f]
''目標は、状態xtの推定について、推定誤差が最小分散を与え...
--観測値は、出力ytのみ
--線形推定かつ漸化式による状態推定を行う。すなわち前期の...
X*t+1=Ltx*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=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を、観測誤差...
ここで、求められた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期予測式は、元の観測式と状態推移式の両辺の期待値から自...
*最小分散推定:カルマンゲインの漸化式 [#r98585d5]
いまだカルマンゲインをどのように決めるかが判っていないの...
カルマンゲインは、予測誤差を最小になるように、最小分散を...
そのために、まず予測誤差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~...
Pt=(1-Kth)P~t(1-Kth)+KtσvvKt
これが、分散のupdate式である。
最終ステップは、Kt(カルマンゲイン)の決め方である。カルマ...
そこで、上式を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のアルゴリズム:以上の整理 [#s4b1b141]
-予測ステップ: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のアルゴリズム:以上の整理 [#s4b1b141]
-予測ステップ: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式
-時間を進めて、予測ステップに戻って繰り返す。
--カルマンゲインは、出力の観測誤差と状態の推定誤差の比を...
#ref(K-filter.JPG)
*多入力多出力多状態の場合:一般の確率差分モデル [#bf70221e]
下記の確率差分方程式を考える。
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であり、誤差共分...
εXt=x*t-xt
Pt=Cov(εXt)
Pk=|σ1^2 |
| ・ |
| ・ |
| σn^2|
tracePk=σ1^2+.....+σn^2
-導出は省略し、推定式を以下に紹介する。
*一般のKalman Filterのアルゴリズム:以上の整理 [#s4b1b141]
-予測ステップ: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次元の場合と同様のアルゴリズムになっていることを比較し...
*一般型の場合のカルマンゲインの求め方 [#t79688f3]
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で偏微分して...
∂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
*もう一つのカルマンゲインの表現 [#x4a13f30]
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)...
=[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)
*非線形系への拡張カルマンフィルタ [#v7dbf9ad]
非線形の場合も、線形化を行って、時変パラメータ行列を持つ...
力学系の多くは、この方法で状態推定が可能である。
*参考 [#p119c078]
-[[G.Welch and G.Bishop;An Introduction to the Kalman Fil...
ページ名: