![[PukiWiki]](image/pukiwiki.png "[PukiWiki]")
カルマンフィルター とは？ †離散的な観測値を用いて、ある動的システムの状態を推定する方法である。 時々刻々と得られる観測データに基づき、実際の観測値 と直前の推定値から予測 される観測値のずれを用いて、システムの状態を補正して推定する方法である。
カルマンフィルターをベイズの定理から、最尤推定法で導出する。尤度関数の最大化問題の解が、逐次決定過程の2点境界値問題になることを明らかにし、カルマンフィルタとスムージングの最適解である漸化式を導出する。
状態方程式と観測式 †フィルタリングとは、時刻kまでの入出力の観測データを用いて、時刻kの状態を推定することを意味する。カルマンフィルタのフィルタの意味はフィルタリングの方法であることによる。またスムージングとは平滑化の意味であり、時刻kまでの入出力の観測データを用いて、それ以前の状態を推定することである。
条件付き確率密度関数と尤度：ベイズの定理 †観測｛yt},t=0～nが与えられた時、状態{xt},t=0～n の条件付き確率密度関数は、ベイズの定理より、次式で与えられる。 Px|y(x0,...,xn|yo,...yn) = py|x(y0,...,yn|x0,...,xn)・px(x0,...,xn)/py(y0,...,yn) ４式 vkの密度関数をPvk(・)とすると ２式より py|x(y0,...,yn|x0,...,xn)= Π Pvk(yk-Hkxk) 5式 Π は、k=0からnまでの積 次に、1式のマルコフ性により px(x0,x1,....,xn)=P0(x0)p1(x1|x0)p2(x2|x1)....pn(xn|xn-1) ６式 が成立する。 そして、1式より Pk(xk+1|xk)は、平均FkXk?、共分散GkQkG'k の正規分布となる。 pvk(・)もPk(・)もいずれも正規分布であるので、過去の観測値で条件づけられた状態の確率 4式もまた下記の正規分布で表わされる。 Px|y(x0,...,xn|yo,...yn) =C(y0,...,yn)・EXP｛-(1/2)Σ[yk-Hkxk]'Rk^(-1)[yk-Hkxk]-(1/2)[xo-μ0]'V0^(-1)[xo-μ0] -(1/2)Σ[xk-Fkxk]'(G'kQkGk)^(-1)[xk-Fkxk] } 7式 但し、C(y0,...,yn)は、正規化の係数。 上式は、観測値が与えられた場合の事後確率であり、これを最大化するような状態xk,k=1~n は、最尤法による最適推定値と呼ばれる。
最尤推定問題の書き換え †以上より、最尤推定問題は、下記の2次形式を最小とするような状態量である。 Jn(x0,..,xn)= (1/2)Σ[yk-Hkxk]'Rk^(-1)[yk-Hkxk]+(1/2)[xo-μ0]'V0^(-1)[xo-μ0] +(1/2)Σ[xk-Fkxk]'(G'kQkGk)^(-1)[xk-Fkxk] ８式
上記の問題は、所与の{yk},k=1~nとxk+1 = Fkxk+Gkuk の制約の下で、コスト関数 (1/2)Σ[yk-Hkxk]'Rk^(-1)[yk-Hkxk]+(1/2)[xo-μ0]'Vk^(-1)[xo-μ0]+(1/2)Σ[uk]'(Qk)^(-1)[uk]を最小にするような｛uk｝k=1~n-1を求める問題（ykに対するukを操作変数とする最適追従制御問題）の解を求めること等価である。 制約式のラグランジェ未定乗数ベクトルをλkとすれば、次のラグランジェ汎関数を最小化する{uk}k=1~n-1を求める問題になる。 Ln(u0,...un)=(1/2)[xo-μ0]'V0^(-1)[xo-μ0]+(1/2)Σ[yk-Hkxk]'Rk^(-1)[yk-Hkxk]
+Σ{(1/2)u'kQk^(-1)uk + λ'k(xk+1-Fkxk-Gkuk)｝ ９式
2点境界値問題 †上の式で、k期の観測誤差の分散を Hk(xk,uk)= (1/2)[yk-Hkxk]'Rk^(-1)[yk-Hkxk]+(1/2)u'kQk^(-1)uk - λ'k(Fkxk+Gkuk) １０式 と置くと、 Ln(u0,...un)=(1/2)[xo-μ0]'Vk^(-1)[xo-μ0]+Σ｛Hk(xk,uk)+λ'kxk-1｝
＋(1/2)[yn-Hnxn]'Rn^(-1)[yn-Hnxn]
=(1/2)[xo-μ0]'V0^(-1)[xo-μ0]+H0(x0,y0)+Σ｛Hk(xk,uk)+λ'kxk-1｝
＋(1/2)[yn-Hnxn]'Rn^(-1)[yn-Hnxn]+λ'n-1xn 11式
と表わされる。 これを最小化するukは、 Hk(xk,uk)を最小化するukを求めれば良いことになる。 １０式より、容易に u*k=QkG'λk の時最小となり、その最小値H*k は H*k=(1/2)[yn-Hnxn]'Rn^(-1)[yn-Hnxn]-λ'kFkXk-(1/2)λ'kGkQkG'kλk となる。 次に １１式を最小にするXkを X*k|nとすると、xk+1 = Fkxk+Gkuk と H*k(xk)+λ'kxk のxkでの一階偏微分が〇となる条件より、次の必要条件を得る。 x*k+1|n = Fk xk|n + GkQkG'kλk k=0,1,...,n-1 12式 λk-1= F'k λk ＋H'kR^(-1)｛yk-Hk x*k|n｝k=0,1,...,n 13式 境界条件は明らかに次式で与えられる。 λn=0 １４式 V0^(-1)｛x*0|n-μ0｝＝H'0R^(-1)｛yo-H0x*0|n｝+F'0λ0 １５式 最適推定値x*k|nは、yo,...,ynの観測データが与えられた下で、xkの平滑推定値（内挿値）であり、１２~１５式で表わされる2点境界値問題の解である。 平滑（スムージング）問題とフィルタリング問題の解 †まず、15式をx*0|nについて解く。 x*0|n = μ0+[I+V0H'0R^(-1)H0]^(-1)V0H'0R0^(-1)(y0-H0μ0) 16式 ここで P0=[I+V0H'0R0^(-1)H0]^(-1)V0 17式 と置く。 x*0|0=μ0+P0H'0R0^(-1)(y0-H0μ0) 18式 であることに注目すれば x*0|n=x*0|0 + P0F'0λ0 19式 になる。 この結果から、2点境界値問題の解は、帰納法的に次式で与えられることが証明できる。 x*k|n = x*k|k + PkF'kλk 20式
但し、Pk=[I+VkH'kRk^(-1)Hk]^(-1)Vk 21式の１
Vk+1=FkVkF'k + GkQkG'k 21式の２
x*k+1|k+1 = Fk x*k|k + Pk+1H'k+1Rk+1^(-1)[yk+1-Hk+1FkX*k|k] ２２式
以下で、帰納法で証明する。まず20式が成立すると仮定すれば21，22式を用いて、20式k+1でも成立することを示す。 20式を12式に代入する。 x*k+1|n=Fkx*k|k+[FkPkF'k+GkQkG'k]λk = Fk x*k|k+Vk+1λk
=Fk x*k|k +Pk+1λk + Vk+1H'Rk+1^(-1)Hk+1Pk+1λk 23式
13式に23式を代入する。 λk = F'k+1λk+1 + H'Rk+1^(-1)[yk+1-Hk+1FkX*k|k]
-H'k+1Rk+1^(-1)Hk+1vk+1λk 24式
24式を23式に代入して整理する。 x*k+1|n=Fk x*k|k +Pk{F'k+1λk+1 + H'Rk+1^(-1)[yk+1-Hk+1FkX*k|k]}
- Pk+1H'k+1Rk+1^(-1)Hk+1Vk+1λk + Vk+1H'k+1Rk+1^(-1)Hk+1Pk+1λk
=x*k+1|k+1 + Pk+1F'k+1λk+1
- {(PH'R^(-1)HV)k+1 - (VH'R^(-1)HP)k+1]λk 25式
一方、２１式より、Pk=[vk^(-1)+H'kRk^(-1)Hk]^(-1)であるので、Vk,Pkはk=0~nで全て対称行列であることがわかる。 また Pk+1 + (VH'R^(-1)HP)k+1 =Vk+1 26式 の両辺の転置をとり、もとの式と比較すると容易に (PH'R^(-1)HV)k+1 = (VH'R^(-1)HP)k+1 が確かめられる。 これを用いれば、25式は x*k+1|n= =x*k+1|k+1 + Pk+1F'k+1λk+1 となるので20式が k+1でも成立している。 カルマンフィルターの式の要約 †21式と22式である。再度、以下に記す。 x*k+1|k+1 = Fk x*k|k + Pk+1H'k+1Rk+1^(-1)[yk+1-Hk+1FkX*k|k] Pk=[I+VkH'kRk^(-1)Hk]^(-1)Vk Vk+1=FkVkF'k + GkQkG'k 平滑問題の式の要約 †13式と20式を用いて求められる。 x*k|n = x*k|k + PkF'kλk λk-1= F'k λk ＋H'kR^(-1)｛yk-Hk x*k|n｝k=0,1,...,n 関連文献 †
|
