カルマンフィルターのソースを表示

'''【かるまんふぃるたー (Kalman filter)】'''

=== 概要 ===

(1) 信号を生成するシステムのモデル,  (2) 雑音の統計的性質,(3) 初期状態に関する事前情報の3つが与えられたとき, 観測データから状態ベクトルの最小2乗推定値を逐次的に生成するオンラインアルゴリズムである.  カルマンフィルターの応用は航空宇宙工学, 制御工学, 通信工学, OR, 土木工学など, 非常に広範な分野にわたっている.

=== 詳説 ===

<center>
<table>
<tr>
<td align=center>[[画像:sk-0098-b-h-05-1.png]]</td>
</tr>
<td align=center>図１：状態空間モデル<br></td>
</table>
</center>

<!--
\begin{figure}[hbt] \begin{center} \unitlength=10pt \begin{picture}(23,7.6)(0,0) \put(0.5,4){\vector(1,0){3}} % 2.0 \put(1.3,4.5){\makebox(0,0)[b]{$w_t$}} \put(3.5,3){\framebox(3,2){$B$}} % 4.0 \put(6.5,4){\vector(1,0){1.7}} % 1.2 \put(7.5,4.6){\makebox(0,0){\scriptsize $+$}} \put(8.5,4){\circle{0.6}} %8.5 \put(8.8,4){\vector(1,0){2.2}} \put(9.8,4.7){\makebox(0,0)[b]{$x_{t+1}$}} \put(11,3){\framebox(3,2){$z^{-1}$}} \put(14,4){\vector(1,0){3}} \put(15.5,4.5){\makebox(0,0)[b]{$x_t$}} \put(15.5,4){\circle*{0.1}} \put(17,3){\framebox(3,2){$C$}} \put(20,4){\vector(1,0){2.2}} \put(21.5,4.6){\makebox(0,0){\scriptsize $+$}} \put(22.5,4){\circle{0.6}} \put(22.8,4){\vector(1,0){3}} \put(25,4.5){\makebox(0,0)[b]{$y_t$}} \put(15.5,4){\line(0,-1){4}} \put(15.5,0){\vector(-1,0){1.5}} \put(11,-1){\framebox(3,2){$A$}} \put(11,0){\line(-1,0){2.5}} \put(8.5,0){\vector(0,1){3.7}} \put(7.9,3){\makebox(0,0){$ \scriptstyle + $}} \put(22.5,6.8){\vector(0,-1){2.5}} \put(23,5){\makebox(0,0){$ \scriptstyle + $}} \put(23.5,6.5){\makebox(0,0){$v_t$}} \put(21,0){\makebox(0,0)[b]{$z^{-1}$: {\scriptsize 遅れ作用素}}} \end{picture} \end{center} \caption{状態空間モデル} \label{B-H-05+KALMAN1} \end{figure} 
-->


　図1に示すガウス白色雑音を受ける離散時間線形確率システムを考える.


<center>
<math>
\begin{array}{rlll}
x_{t+1} &=& Ax_t+Bw_t & \qquad (1)\\
y_t &=& Cx_t+v_t,\ t=0, 1, \cdots & \qquad (2)
\end{array}
\, </math>
</center>


ただし, <math>x_t\, </math>はn次元状態ベクトル, <math>y_t\, </math>はp次元観測ベクトル,<math>w_t\, </math>, <math>v_t\, </math>は<math>m\, </math>および<math>p\, </math>次元ガウス白色雑音ベクトルで, 平均値は0, 共分散行列は


<center>
<math>
E \{ w_t w_s^{\top} \} = Q\delta_{ts}, \ \ 
E \{ v_t v_s^{\top} \} = R\delta_{ts}, \ \ E\{w_tv_s^{\top}\}=0
\, </math>
</center>


であるとする. ただし, <math>Q\, </math>, <math>R\, </math>は非負定値対称行列, <math>\delta_{ts}\, </math>はクロネッカーの記号である. <math>A,\;B,\;C,\;Q,\;R\, </math>は一般に時間の関数であってもよいが,簡単のために添字<math>t\, </math>は省略している. また初期値<math>x_0\, </math>は平均値<math>\bar{x}_0\, </math>,共分散行列<math>\Sigma_0\, </math>のガウス確率ベクトルであり, 雑音とは無相関であるとする.

　カルマンフィルタは, 観測データ<math>Y_{0}^{\top}:=\{y_0, y_1, \cdots, y_t\}\, </math>に基づいて,状態<math>x_{t+m}\, </math>の最小分散推定値(すなわち<math>x_{t+m}\, </math>の条件付き期待値)


<center>
<math>
\hat{x}_{t+m|t}=E\{x_{t+m}|Y_{0}^{\top}\}, \ t=0, 1, \cdots
\, </math>
</center>


を逐次的に計算するアルゴリズムである. <math>m>0, m=0, m<0</math>にしたがってそれぞれ, 予測, 濾波, 平滑という.以下では, 状態ベクトルの予測推定値, 濾波推定値を<math>\hat{x}_{t/t-1}, \hat{x}_{t/t}</math>と表し, それぞれの推定誤差共分散行列を次のようにおく.


<center>
<math>
P_{t/t-1}=E\{[x_t-\hat{x}_{t/t-1}][x_t-\hat{x}_{t/t-1}]^{\top}\}, \quad
P_{t/t}=E\{[x_t-\hat{x}_{t/t}][x_t-\hat{x}_{t/t}]^{\top}\}
\, </math>
</center>


　1960-61年にカルマン(R. E. Kalman)と ビュシー(R. S. Bucy)は式(1), (2)の状態空間モデルに対して, カルマンフィルターと呼ばれる以下のようなアルゴリズムを提案した [1, 2, 3, 4, 5, 6].


(i) フィルタ方程式

<center>
<math>
\begin{array}{rlll}
\hat{x}_{t+1/t}&=&A\hat{x}_{t/t}, \quad \hat{x}_0=\bar{x}_0  & \qquad (3)\\
\hat{x}_{t/t}&=&\hat{x}_{t/t-1}+K_t[y_t-C\hat{x}_{t/t-1}] & \qquad (4)
\end{array}
\, </math>
</center>

(ii) カルマンゲイン

<center>
<math>
K_t=P_{t/t-1}C^{\top}[CP_{t/t-1}C^{\top}+R]^{-1}   \qquad (5)
\, </math>
</center>


(iii) 推定誤差共分散行列

<center>
<math>
\begin{array}{rlll}
P_{t+1/t} &=& AP_{t/t}A^{\top} + BQB^{\top}  & \qquad (6)\\ 
P_{t/t} &=& P_{t/t-1}-P_{t/t-1}C^{\top}[CP_{t/t-1}C^{\top}+R]^{-1}CP_{t/t-1}, 
\quad P_{0/-1}=\Sigma_0 & \qquad (7)
\end{array}
\, </math>
</center>


<center>
<table>
<tr><td align=center>[[画像:sk-0098-b-h-05-2.png]]</td></tr>
<td align=center>図２：カルマンフィルターのブロック線図<br>
</td>
</table>
</center>

<!--
\begin{figure}[hbt] \begin{center} \unitlength=10pt \begin{picture}(23,7.6)(0,0) \put(0,4){\vector(1,0){2.5}} \put(0.5,4.5){\makebox(0,0)[b]{$y_t$}} \put(2,4.6){\makebox(0,0){$ \scriptstyle + $}} \put(4,4.4){\makebox(0,0)[b]{$\nu_t$}} \put(2.8,4){\circle{0.6}} \put(2.8,0){\vector(0,1){3.7}} \put(2.2,3.1){\makebox(0,0){$ \scriptstyle - $}} \put(3.1,4){\vector(1,0){2.3}} \put(5.4,3){\framebox(3,2){$K_t$}} \put(8.4,4){\vector(1,0){2.1}} \put(10.0,4.6){\makebox(0,0){$ \scriptstyle + $}} \put(10.8,4){\circle{0.6}} \put(12.5,4.5){\makebox(0,0)[b]{$\hat{x}_{t/t}$}} \put(11.2,4){\vector(1,0){2.6}} \put(13.8,3){\framebox(3,2){$z^{-1}$}} \put(16.8,4){\vector(1,0){5}} \put(19.5,4.5){\makebox(0,0)[b]{$\hat{x}_{t-1/t-1}$}} \put(19.6,4){\circle*{0.1}} \put(19.6,4){\line(0,-1){4}} \put(19.6,0){\vector(-1,0){2.8}} \put(13.8,-1){\framebox(3,2){$A$}} \put(10.8,0){\circle*{0.1}} \put(10.8,0){\vector(0,1){3.7}} \put(10,3.1){\makebox(0,0){$ \scriptstyle + $}} \put(5.4,-1){\framebox(3,2){$C$}} \put(2.8,0){\line(1,0){2.6}} \put(13.8,0){\vector(-1,0){5.4}} \put(12,-1){\makebox(0,0)[b]{$\hat{x}_{t/t-1}$}} \end{picture} \end{center} \caption{カルマンフィルターのブロック線図} \label{B-H-05+KALMAN2} \end{figure} 
-->



　図2にカルマンフィルタのブロック線図を示す.カルマンフィルタは観測値<math>y_t\, </math>を入力とし, 推定値<math>\hat{x}_{t/t-1}\, </math>, <math>\hat{x}_{t/t}\, </math>を逐次的に出力する線形動的システムであり,<math>\nu_t=y_t-C\hat{x}_{t/t-1}\, </math>はイノベーションと呼ばれている.

　<math>w_t,\;v_t,\;x_0\, </math>がガウス分布でない場合には, 上のアルゴリズムは状態ベクトルおよび信号の線形最小分散推定値を与えるという意味で, 最適なフィルタである.

　式(6), (7)から<math>P_{t/t}\, </math>を消去して,<math>P_t:=P_{t/t-1}\, </math>とおくと, 離散時間リッカチ方程式


<center>
<math>
P_{t+1}=A(P_{t}-P_{t}C^{\top}[CP_{t}C^{\top}+R]^{-1}CP_{t})A^{\top} + BQB^{\top} \qquad (8)
\, </math>
</center>


を得る. また上式の<math>t\to \infty\, </math>における極限を代数リッカチ方程式という. 制御理論の分野には(代数)リッカチ方程式に関する膨大な研究がある [7].

　白色雑音を受ける非線形確率システムに対しても,その線形化モデルにカルマンフィルタを適用することができるので, カルマンフィルタは航空宇宙工学の分野において飛翔体の軌道推定に威力を発揮した. また状態ベクトルだけてなく, モデルに含まれる未知パラメータを同時に推定する拡張カルマンフィルタも提案されており, カルマンフィルタの応用は時系列の推定を始めとして非常に多くの分野に見られる.



----
'''参考文献'''

[1] R. E. Kalman, "A New Approach to Linear Filtering and Prediction Problem," ''Transactions of American Society of Mechanical Engineers, Journal of Basic Engineering'', '''82D''' (1960), 34-45.

[2] R. E. Kalman and R. S. Bucy, "New Results in  Linear Filtering and Prediction Theory," ''Transactions of  American Society of Mechanical Engineers, Journal of Basic Engineering'', '''83D''' (1961), 95-108.

[3] B. D. O. Anderson and J. B. Moore, ''Optimal Filtering'', Prentice-Hall, 1979.

[4] M. S. Grewal and A. P. Andrews, ''Kalman Filtering - Theory and Practice'', Prentice-Hall, 1993.

[5] 有本 卓, 『カルマンフィルター』, 産業図書, 1977.

[6] 片山  徹, 『新版応用カルマンフィルタ』, 朝倉書店, 2000.

[7] S. Bittanti, A. J. Laub and J. C. Willems (Eds.), ''The Riccati Equation'', Springer, 1991.

[[category:予測|かるまんふぃるたー]]