状態方程式

説明を始める前に、この章で使用する記法について説明します。

• \( \boldsymbol{x} \) のような、太字の小文字はベクトルを表します。
• \( \boldsymbol{A} \) のような、太字の大文字は行列を表します。
• 通常の小文字は、ベクトルの要素やスカラーを表します。
• 通常の大文字は、行列の要素を表します。

初めに、カルマンフィルタの式の１つである、状態方程式について説明します。

状態方程式を用いることで、現在の状態の情報に基づいて、次のシステムの状態を予測することができます。状態方程式は、状態ベクトルを現在（時刻 \( n \) ）から未来（時刻 \( n + 1 \) ）に遷移させます。

状態方程式は、動的システムのモデルを表します。他の文献では次のようにも呼ばれます。

• Predictor Equation
• Transition Equation
• Prediction Equation
• Dynamic Model
• State Space Model

行列表現を用いた一般的な状態方程式は、次のように表されます。

Get the book

次の図に、状態方程式の概略を示します。

状態変数は、私たちが知りたいシステムのふるまいを表すことができます。

例えば、航空機の運動では、位置・速度・加速度の3つです。

どの変数が状態変数で、どの変数がシステムへの入力なのか、自分で考えてみるといいかもしれません。

• 機械システムは、位置・速度・加速度・抗力などの変数が存在します。
• システムに作用する力は外部入力、すなわち状態ベクトル（一定加速度の場合は位置と速度）に影響を及ぼす、システムへの入力と考えられます。
• ニュートンの第二法則によれば \( F = ma \) であるため、加速度はシステムへの外部入力として考えることができます。
• 位置と速度は主要な状態変数です。

例えば、バネ系では、バネに働く力 \( F(t) \) は入力 \( u(t) \) であり、バネの先端の位置 \( x(t) \) はシステムの状態変数です。

落下する物体では、入力は重力 \( F_{g} \) と抗力 \( F_{drag}(t) \) であり、物体の高さ \( h(t) \) と速度 \( v(t) \) はシステムの状態変数です。

それでは、状態方程式に関するいくつかの例を見ていきましょう。

Example - 航空機 - 制御入力無し

この例題では、等加速度を仮定した、航空機の状態方程式を定義します。

ここでは、制御入力は無いと仮定します。制御入力ありについては、次の例題で扱います。

\[ \boldsymbol{u_{n}} = 0 \]

3次元空間を等加速度で運動している航空機を考えます。このとき、状態ベクトル \( \boldsymbol{\hat{x}_{n}} \) は直行座標系 \( \left( x,y,z \right) \) における航空機の位置・速度・加速度です。

\[ \boldsymbol{\hat{x}_{n}}= \left[ \begin{matrix} \hat{x}_{n}\\ \hat{y}_{n}\\ \hat{z}_{n}\\ \hat{\dot{x}}_{n}\\ \hat{\dot{y}}_{n}\\ \hat{\dot{z}}_{n}\\ \hat{\ddot{x}}_{n}\\ \hat{\ddot{y}}_{n}\\ \hat{\ddot{z}}_{n}\\ \end{matrix} \right] \]

システム行列 \( \boldsymbol{F} \) は次のように表されます。

\[ \boldsymbol{F}= \left[ \begin{matrix} 1 & 0 & 0 & \Delta t & 0 & 0 & 0.5\Delta t^{2} & 0 & 0 \\ 0 & 1 & 0 & 0 & \Delta t & 0 & 0 & 0.5\Delta t^{2} & 0 \\ 0 & 0 & 1 & 0 & 0 & \Delta t & 0 & 0 & 0.5\Delta t^{2}\\ 0 & 0 & 0 & 1 & 0 & 0 & \Delta t & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 & 0 & \Delta t & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & \Delta t\\ 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1\\ \end{matrix} \right] \]

状態方程式は次のように表されます。

\[ \boldsymbol{\hat{x}_{n+1,n}=F\hat{x}_{n}} \]

\[ \left[ \begin{matrix} \hat{x}_{n+1,n}\\ \hat{y}_{n+1,n}\\ \hat{z}_{n+1,n}\\ \hat{\dot{x}}_{n+1,n}\\ \hat{\dot{y}}_{n+1,n}\\ \hat{\dot{z}}_{n+1,n}\\ \hat{\ddot{x}}_{n+1,n}\\ \hat{\ddot{y}}_{n+1,n}\\ \hat{\ddot{z}}_{n+1,n}\\ \end{matrix} \right] = \left[ \begin{matrix} 1 & 0 & 0 & \Delta t & 0 & 0 & 0.5\Delta t^{2} & 0 & 0 \\ 0 & 1 & 0 & 0 & \Delta t & 0 & 0 & 0.5\Delta t^{2} & 0 \\ 0 & 0 & 1 & 0 & 0 & \Delta t & 0 & 0 & 0.5\Delta t^{2}\\ 0 & 0 & 0 & 1 & 0 & 0 & \Delta t & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 & 0 & \Delta t & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & \Delta t\\ 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1\\ \end{matrix} \right] \left[ \begin{matrix} \hat{x}_{n,n}\\ \hat{y}_{n,n}\\ \hat{z}_{n,n}\\ \hat{\dot{x}}_{n,n}\\ \hat{\dot{y}}_{n,n}\\ \hat{\dot{z}}_{n,n}\\ \hat{\ddot{x}}_{n,n}\\ \hat{\ddot{y}}_{n,n}\\ \hat{\ddot{z}}_{n,n}\\ \end{matrix} \right] \]

上記の状態方程式は、次の方程式を行列表現したものです。

\[ \begin{cases} \hat{x}_{n+1,n} = \hat{x}_{n,n} + \hat{\dot{x}}_{n,n} \Delta t+ \frac{1}{2}\hat{\ddot{x}}_{n,n} \Delta t^{2}\\ \hat{y}_{n+1,n} = \hat{y}_{n,n} + \hat{\dot{y}}_{n,n} \Delta t+ \frac{1}{2}\hat{\ddot{y}}_{n,n} \Delta t^{2}\\ \hat{z}_{n+1,n} = \hat{z}_{n,n} + \hat{\dot{z}}_{n,n} \Delta t+ \frac{1}{2}\hat{\ddot{z}}_{n,n} \Delta t^{2}\\ \hat{\dot{x}}_{n+1,n} = \hat{\dot{x}}_{n,n} + \hat{\ddot{x}}_{n,n} \Delta t\\ \hat{\dot{y}}_{n+1,n} = \hat{\dot{y}}_{n,n} + \hat{\ddot{y}}_{n,n} \Delta t\\ \hat{\dot{z}}_{n+1,n} = \hat{\dot{z}}_{n,n} + \hat{\ddot{z}}_{n,n} \Delta t\\ \hat{\ddot{x}}_{n+1,n} = \hat{\ddot{x}}_{n,n}\\ \hat{\ddot{y}}_{n+1,n} = \hat{\ddot{y}}_{n,n}\\ \hat{\ddot{z}}_{n+1,n} = \hat{\ddot{z}}_{n,n}\\ \end{cases} \]

Example - 航空機 - 制御入力あり

この例題は前回の例題と似ていますが、航空機に加速度センサを取り付け、パイロットの操舵コマンドに基づいた航空機の加速度を観測できるようにしています。

状態ベクトル \( \boldsymbol{\hat{x}_{n}} \) は直交座標系 \( \left( x,y,z \right) \) における航空機の位置・速度の推定値であり、次のように表されます。

\[ \boldsymbol{\hat{x}_{n}}= \left[ \begin{matrix} \hat{x}_{n}\\ \hat{y}_{n}\\ \hat{z}_{n}\\ \hat{\dot{x}}_{n}\\ \hat{\dot{y}}_{n}\\ \hat{\dot{z}}_{n}\\ \end{matrix} \right] \]

入力ベクトル \( \boldsymbol{u_{n}} \) は直交座標系 \( \left( x,y,z \right) \) における航空機の加速度の観測値であり、次のように表されます。

\[ \boldsymbol{u_{n}}= \left[ \begin{matrix} \ddot{x}_{n}\\ \ddot{y}_{n}\\ \ddot{z}_{n}\\ \end{matrix} \right] \]

システム行列 \( \boldsymbol{F} \) は次のように表されます。

\[ \boldsymbol{F}= \left[ \begin{matrix} 1 & 0 & 0 & \Delta t & 0 & 0\\ 0 & 1 & 0 & 0 & \Delta t & 0\\ 0 & 0 & 1 & 0 & 0 & \Delta t\\ 0 & 0 & 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 0 & 0 & 1\\ \end{matrix} \right] \]

入力係数行列 \( \boldsymbol{G} \) は次のように表されます。

\[ \boldsymbol{G}= \left[ \begin{matrix} 0.5\Delta t^{2} & 0 & 0 \\ 0 & 0.5\Delta t^{2} & 0 \\ 0 & 0 & 0.5\Delta t^{2} \\ \Delta t & 0 & 0 \\ 0 & \Delta t & 0 \\ 0 & 0 & \Delta t \\ \end{matrix} \right] \]

状態方程式は次のように表されます。

\[ \boldsymbol{\hat{x}_{n+1,n}=F\hat{x}_{n,n}+Gu_{n,n}} \]

\[ \left[ \begin{matrix} \hat{x}_{n+1,n}\\ \hat{y}_{n+1,n}\\ \hat{z}_{n+1,n}\\ \hat{\dot{x}}_{n+1,n}\\ \hat{\dot{y}}_{n+1,n}\\ \hat{\dot{z}}_{n+1,n}\\ \end{matrix} \right] = \left[ \begin{matrix} 1 & 0 & 0 & \Delta t & 0 & 0\\ 0 & 1 & 0 & 0 & \Delta t & 0\\ 0 & 0 & 1 & 0 & 0 & \Delta t\\ 0 & 0 & 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 0 & 0 & 1\\ \end{matrix} \right] \left[ \begin{matrix} \hat{x}_{n,n}\\ \hat{y}_{n,n}\\ \hat{z}_{n,n}\\ \hat{\dot{x}}_{n,n}\\ \hat{\dot{y}}_{n,n}\\ \hat{\dot{z}}_{n,n}\\ \end{matrix} \right] + \left[ \begin{matrix} 0.5\Delta t^{2} & 0 & 0 \\ 0 & 0.5\Delta t^{2} & 0 \\ 0 & 0 & 0.5\Delta t^{2} \\ \Delta t & 0 & 0 \\ 0 & \Delta t & 0 \\ 0 & 0 & \Delta t \\ \end{matrix} \right] \left[ \begin{matrix} \ddot{x}_{n}\\ \ddot{y}_{n}\\ \ddot{z}_{n}\\ \end{matrix} \right] \]

Example – 落下する物体

自由落下する物体を考えます。状態ベクトルは物体の高さ \( h \) と速度 \( \dot{h} \) から構成されます。

\[ \boldsymbol{\hat{x}_{n}}= \left[ \begin{matrix} \hat{h}_{n}\\ \hat{\dot{h}}_{n}\\ \end{matrix} \right] \]

システム行列 \( \boldsymbol{F} \) は次のように表されます。

\[ \boldsymbol{F}= \left[ \begin{matrix} 1 & \Delta t \\ 0 & 1 \\ \end{matrix} \right] \]

入力係数行列 \( \boldsymbol{G} \) は次のように表されます。

\[ \boldsymbol{G}= \left[ \begin{matrix} 0.5\Delta t^{2} \\ \Delta t \\ \end{matrix} \right] \]

入力変数は次のように表されます。

\[ \boldsymbol{u_{n}}= \left[ \begin{matrix} g \end{matrix} \right] \]

ここで、 \( g \) は重力加速度です。

私たちは、加速度を観測するセンサを持っていません。しかし、自由落下する物体の加速度は \( g \) と等しくなります。

状態方程式は次のように表されます。

\[ \left[ \begin{matrix} \hat{h}_{n+1,n}\\ \hat{\dot{h}}_{n+1,n}\\ \end{matrix} \right] = \left[ \begin{matrix} 1 & \Delta t \\ 0 & 1 \\ \end{matrix} \right] \left[ \begin{matrix} \hat{h}_{n,n}\\ \hat{\dot{h}}_{n,n}\\ \end{matrix} \right] + \left[ \begin{matrix} 0.5\Delta t^{2} \\ \Delta t \\ \end{matrix} \right] \left[ \begin{matrix} g \end{matrix} \right] \]

上記の状態方程式は、次の方程式を行列表現したものです。

\[ \begin{cases} \hat{h}_{n+1,n} = \hat{h}_{n,n} + \hat{\dot{h}}_{n,n} \Delta t + 0.5 \Delta t^{2} g\\ \hat{\dot{h}}_{n+1,n} = \hat{\dot{h}}_{n,n} + \Delta t g\\ \end{cases} \]

状態方程式の次元

次の表に、状態方程式の変数の次元をまとめました。

線形時不変システム

このセクションでは、LTI (：Linear Time-Invariant、線形時不変 ) システムを扱います。時変システムに対するカルマンフィルタや非線形システムに対するカルマンフィルタ（拡張カルマンフィルタ）については後に扱いたいと思います。

では、線形や時不変とはどのようなことなのでしょうか？

線形システムは、変数同士が掛け合わされることなく、変数に定数のみが掛け合わされ、それらが足し合わされた方程式です。線形システムは、変数間の静的および動的な関係を記述するために使用されます。

線形システムでは、関数 \( \mathcal{F} \) について次が成り立ちます。

\[ \mathcal{F} \left( a \times g \left( t \right) +b \times h \left( t \right) \right) = a \times \mathcal{F} \left( g \left( t \right) \right) + b \times \mathcal{F} \left( h \left( t \right) \right) \]

ここで、

\( a \) と \( b \) は定数です。

\( g \) と \( h \) は独立変数 \( t \) で表される任意の関数です。

線形システムには、2つの基本的な性質があります。

Get the book

1. 定数係数（上記の \( a \) と \( b \) ）は関数の外へ出すことができます。
2. 複数の入力に対するシステムの応答は、それぞれの入力をシステムに加えたときの応答の重ね合わせで表現できます。

時不変システムは時間に関与しないシステム関数を持ちます。

ゲイン G = 10 のアンプの例を考えます。

このシステムは時不変です。システムの出力は時間によって変化しますが、システム関数は時間に依存しません。

時不変システムは、入力に時間的な遅れ（またはシフト）があると、システムの出力にも同等の時間的な遅れが生じるシステムです。