- 追加された行はこの色です。
- 削除された行はこの色です。
- カルマンフィルタ へ行く。
#freeze
*カルマンフィルター とは？ [#v66a7d99]
離散的な観測値を用いて、ある動的システムの状態を推定する方法である。
時々刻々と得られる観測データに基づき、実際の観測値 と直前の推定値から予測
される観測値のずれを用いて、システムの状態を補正して推定する方法である。
-推定には、
--予測：prediction 現在(t=t0)までの情報に基づき、未来(t>t0)の状態を推定すること。
--ろ波：filtering 現在(t=t0)の情報に基づき、現在(t=t0)の状態を推定すること。
--平滑：smoothing 現在(t=t0)までの情報に基づき、過去(t<t0)の状態を推定すること。
がある。カルマンフィルターは、現在(t=t0)の情報に基づき、現在(t=t0)の状態を推定する。
--この状態推定値がわかれば、予測は状態推移式を用いて容易に可能である。
''カルマンフィルターをベイズの定理から、最尤推定法で導出する。尤度関数の最大化問題の解が、逐次決定過程の2点境界値問題になることを明らかにし、カルマンフィルタとスムージングの最適解である漸化式を導出する。''
-線形の入出力系を前提とし、その状態変数が確率的挙動を示す。
-状態変数とは、位置、速度、加速度のようなダイナミクスを示す変数である。動的システムのある時点の状態量は、初期状態値とそれまでの入力値によって表わすことができる。線形微分・差分方程式で表わされる。
-このフィルターはルドルフ・カルマンによって提唱された。カルマンの優れた業績のひとつに、状態量の導入による可制御性・可観測性など動的システムの構造に関する概念の提示による厳密な理論展開にあった。
*状態方程式と観測式 [#jab9b08f]
フィルタリングとは、時刻kまでの入出力の観測データを用いて、時刻kの状態を推定することを意味する。カルマンフィルタのフィルタの意味はフィルタリングの方法であることによる。またスムージングとは平滑化の意味であり、時刻kまでの入出力の観測データを用いて、それ以前の状態を推定することである。
-''状態推移式''とは、システムの状態の遷移を記述する状態方程式である.
xk+1 = Fkxk+Gkuk １式
xk:時刻ｋでの状態ベクトル
uk:時刻ｋでの入力(誤差を含む）
-観測値とは、状態の一部またはその線形関数で表わされるとする。
--これを''観測式''と呼ぶ。
yk = Hkxk+vk ２式
yk:観測ベクトル
vk:観測誤差ベクトル
-以下の展開では、uk,vkは互いに独立な白色正規性の確率変数とする。
--平均0、共分散は
E(uku'j)=Qkδkj、E(vkv'j)=Rkδkj、E(ukvj)=0。 ３式
Qk、Rkは正定行列とする。
--入力ukの平均を%%%uk%%%としても以下の議論は成立するので、簡単のためこのように仮定する。
--初期状態x0は、平均μ0、共分散Voの正規分布であり、uk、vkとは無相関とする。
''レーダーやコンピュータビジョンなど、工学分野のみならず、金融時系列の推定など経済・金融分野や気象予報のデーター解析、カーナビゲーションなど広く用いられている。''
*条件付き確率密度関数と尤度：ベイズの定理 [#w298a6ef]
観測｛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 は、最尤法による最適推定値と呼ばれる。''
--正規分布の場合、尤度の最大化と推定誤差二乗和の最小化は同一の解をあたえる。その意味で
カルマンフィルタとは、各ステップ毎のシステムのプロセス情報及び観測(測定)値を
用いて観測誤差の事後分布の共分散を最小にするような推定を行う手法である。
*最尤推定問題の書き換え [#x6160d3d]
以上より、最尤推定問題は、下記の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] ８式
-これを最小とする状態推定値を｛x*0,...,x*n-1,x*n｝で表わす時
--｛x*0,...,x*n-1｝は、n期までのデータに基づく最適スムージング値（内挿値）である。
--X*nは、n期までのデータに基づくn期の最適推定値(フィルタリング値)である。
''上記の問題は、所与の{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)｝ ９式
--最初のΣはk=1,nまでの和、次のΣはk=1,n-1までの和。
*2点境界値問題 [#h695bbd9]
上の式で、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点境界値問題の解である。''
*平滑（スムージング）問題とフィルタリング問題の解 [#hec1b30a]
まず、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でも成立している。
*カルマンフィルターの式の要約 [#x7a72e1d]
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
*平滑問題の式の要約 [#m455c67e]
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
*関連文献 [#p9af3809]
-[[予測と推定の計算理論的基礎.pdf>http://www.jnns.org/niss/2000/text/ishii.pdf]]
-[[R.E. Kalman , A new approach to linear filtering and prediction problems. J. Basic Eng. 82 (1960), pp. 35–45.>http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.129.6247&rep=rep1&type=pdf]]
-[[R.E. Kalman ,Contributions to the Theory of Optimal Control>http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.26.4070&rep=rep1&type=pdf]]
-[[R.E. KALMAN, “On the general theory of control systems,” in Proc.First Internat. Congress Automat. Contr., pp. 481–491, Moscow,1960.>http://www.control.utoronto.ca/~broucke/ece557f/kalman.pdf]]
-R.E. Kalman, When is a linear control system optimal?, Transactions of the
ASME, Journal of Basic Engineering, Series D, 86 (1964), 8190.
![[PukiWiki]](image/pukiwiki.png "[PukiWiki]")
