多次元カルマンフィルタ
「1次元カルマンフィルタ」の章を読んで、カルマンフィルタの概念について理解できたかと思います。この章では、多次元カルマンフィルタの式を導出します。
これまでは、液体の温度を推定するような、1次元のモデルについて扱ってきました。しかしながら、多くの動的モデルは、2次元、3次元、あるいはそれ以上の次元を持っています。
例えば、空間上の航空機の位置を表す状態ベクトルは3次元で表されます。
\[ \left[ \begin{matrix} x\\ y\\ z\\ \end{matrix} \right] \]
航空気の速度を加えると、状態ベクトルは6次元になります。
\[ \left[ \begin{matrix} x\\ y\\ z\\ \dot{x}\\ \dot{y}\\ \dot{z}\\ \end{matrix} \right] \]
さらに、加速度も加えるとすると、状態ベクトルは9次元になります。
\[ \left[ \begin{matrix} x\\ y\\ z\\ \dot{x}\\ \dot{y}\\ \dot{z}\\ \ddot{x}\\ \ddot{y}\\ \ddot{z}\\ \end{matrix} \right] \]
加速度は一定であると仮定すると、時刻 \( n \) における航空機の状態遷移式は次の9つの式で表されます。
\[ \begin{cases} x_{n} = x_{n-1} + \dot{x}_{n-1} \Delta t+ \frac{1}{2}\ddot{x}_{n-1} \Delta t^{2}\\ y_{n} = y_{n-1} + \dot{y}_{n-1} \Delta t+ \frac{1}{2}\ddot{y}_{n-1} \Delta t^{2}\\ z_{n} = z_{n-1} + \dot{z}_{n-1} \Delta t+ \frac{1}{2}\ddot{z}_{n-1} \Delta t^{2}\\ \dot{x}_{n} = \dot{x}_{n-1} + \ddot{x}_{n-1} \Delta t\\ \dot{y}_{n} = \dot{y}_{n-1} + \ddot{y}_{n-1} \Delta t\\ \dot{z}_{n} = \dot{z}_{n-1} + \ddot{z}_{n-1} \Delta t\\ \ddot{x}_{n} = \ddot{x}_{n-1}\\ \ddot{y}_{n} = \ddot{y}_{n-1}\\ \ddot{z}_{n} = \ddot{z}_{n-1}\\ \end{cases} \]
このような多次元モデルの式は、行列表現がよく用いられます。
その理由としては、第一に、これらの方程式を書くことは非常に大変であるからです。行列表現を用いれば、これらの式をより短く、エレガントに記述することができます。
また、コンピュータは行列計算を非常に効率的に行うことができます。カルマンフィルタを行列表現で実装することで、速い計算速度を実現できます。
次の章では、カルマンフィルタの式を行列形式で説明します。そしてもちろん、数値例も紹介します。