\( \alpha -\beta -\gamma \) フィルタ

Example 1 – 金の質量

最初の簡単な例題に取りかかる準備ができました。この例では、静的なシステムの状態を推定します。静的なシステムとは、一定の期間にわたって状態が変化しないシステムのことです。例えば、塔は静的な系であり、塔の高さがその状態になります。このようなモデルを、持続予測モデルと呼びます。

ここで扱う例題では、金の延べ棒の質量推定を行います。ここで、測定にあたって系統誤差は存在しないと仮定していますが、測定値は偶然誤差(ランダムノイズ)を含んでいます。

Gold bars

ここでは、システムは金の延べ棒であり、システムの状態量は金の延べ棒の質量です。このシステムは静的なシステムであり、質量は時間で変化しないと仮定します。

システムの状態量(すなわち質量)を推定するために、複数回の測定を行い、それらの平均値を計算します。

Measurements vs. True value

\( n \)回目において、推定値\( \hat{x}_{n,n} \)はそれまでの測定の平均値を表します。

\[ \hat{x}_{n,n}= \frac{1}{n} \left( z_{1}+ z_{2}+ \ldots + z_{n-1}+ z_{n} \right) = \frac{1}{n} \sum _{i=1}^{n} \left( z_{i} \right) \]
Example Notation:
\( x \) 真の質量
\( z_{n} \) \( n \)回目における質量の測定値
\( \hat{x}_{n,n} \) \( n \)回目における\( x \)の推定値 (測定値\( z_{n} \)が得られた後に計算された推定値)
\( \hat{x}_{n,n-1} \) (\( n-1 \))回目において得られた\( x \)の推定値 (測定値\( z_{n-1} \)が得られた後に計算された推定値)
\( \hat{x}_{n+1,n} \) (\( n+1 \) )回目の\( x \)の推定値。この推定は\( n \)回目の測定後に行われる。つまり、\( \hat{x}_{n+1,n} \)は予測値である。
注:いくつかの文献では、変数の上に書かれる記号(キャレットやハットと呼ばれる)は推定値を示します。

この例題のモデルは持続予測モデルです。したがって、\( \hat{x}_{n+1,n}= \hat{x}_{n,n} \)です。

\( \hat{x}_{n,n} \)を推定するためには、これまでの全ての測定値を記録しておく必要があります。 仮に、測定値を記録するための紙とペンがなかったとします。また、過去の測定値をすべて記憶しておくという記憶力も頼りにできません。そこで、前回の推定値 \( \hat{x}_{N,N-1} \) を使って、少し調整を加えるだけで\( \hat{x}_{N,N} \)を推定したい(実際のアプリケーションでは、コンピュータのメモリを節約したい)とします。これは、ちょっとした数学的なトリックを使用すれば実現できます。

Notes
\( \hat{x}_{n,n}= \frac{1}{n} \sum _{i=1}^{n} \left( z_{i} \right) = \) 平均値の計算:\( n \)回までの合計値を\( n \)で除算
\( = \frac{1}{n} \left( \sum _{i=1}^{n-1} \left( z_{i} \right) + z_{n} \right) = \) ((\( n - 1 \))回までの合計値+最後の測定値)を\( n \)で除算
\( = \frac{1}{n} \sum _{i=1}^{n-1} \left( z_{i} \right) + \frac{1}{n} z_{n} = \) 展開
\( = \frac{1}{n}\frac{n-1}{n-1} \sum _{i=1}^{n-1} \left( z_{i} \right) + \frac{1}{n} z_{n} = \) \( n - 1 \)を分子・分母に乗じる
\( \frac{n-1}{n}\color{#FF8C00}{\frac{1}{n-1} \sum _{i=1}^{n-1} \left( z_{i} \right)} + \frac{1}{n} z_{n} = \) 整理
オレンジの項は前回の推定値を表す
\( = \frac{n-1}{n}\color{#FF8C00}{\hat{x}_{n-1,n-1}} + \frac{1}{n} z_{n} = \) 置き換え
\( = \hat{x}_{n-1,n-1}- \frac{1}{n}\hat{x}_{n-1,n-1}+ \frac{1}{n} z_{n} = \) \( \frac{n-1}{n} \)の項を変形
\( = \hat{x}_{n-1,n-1}+ \frac{1}{n} \left( z_{n}- \hat{x}_{n-1,n-1} \right) \) 整理

\( \hat{x}_{n-1,n-1} \) は、\( n - 1\) 時点の観測値に基づいた、\( n - 1\) 時点における状態量 \( x \) の推定値です。

\( \hat{x}_{n-1,n-1} \) (\( n-1 \) 時点における推定値)に基づいて、 ( \( n \) 時点における状態量 \( x \) の予測値) を求めてみましょう。すなわち、\( \hat{x}_{n-1,n-1} \)をN時点に遷移させます。

この例題で扱っているような、静的なモデル(金の延べ棒の質量は時間的に変化しない)では、状態量 \( x \) の推定値と予測値は等しくなります。つまり、\( \hat{x}_{n,n-1} = \hat{x}_{n-1,n-1} \) です。

したがって、現時点での状態量 \( \hat{x}_{n,n} \) の推定値は次のように表されます。

\[ \hat{x}_{n,n} = \hat{x}_{n,n-1} + \frac{1}{n} \left( z_{n} - \hat{x}_{n,n-1} \right) \]

上式は、カルマンフィルタの5つの式のうちの1つです。これは、状態更新式と呼ばれるものです。それは次のような意味です。

State update

ここで、\( \frac{1}{n} \)はこの例題固有の値です。この係数の重要な役割については後ほど説明します。ここで、カルマンフィルタにおいて、この係数はカルマンゲインと呼ばれています。カルマンゲインは\( K_{n} \)で表され、添え字\( n \)はカルマンゲインが反復計算の中で変化することを示しています。

このカルマンゲイン\( K_{n} \)の発見は、ルドルフ・カルマンの主な貢献の1つでした。

カルマンフィルタの仕組みに入る前に、ここでは\( K_{n} \)に代わってギリシャ文字\( \alpha _{n} \)を使用します。

したがって、状態の更新式は次のようになります。

\[ \hat{x}_{n,n}= \hat{x}_{n,n-1}+ \alpha _{n} \left( z_{n}-\hat{x}_{n,n-1} \right) \]

\( \hat{x}_{n,n} \) は事後推定値と呼ばれます。 \( \left( z_{n}- x_{n,n-1} \right) \)の項は、測定値と事前推定値との残差であり、イノベーション(または、予測誤差)と呼ばれます。イノベーションは新しい情報を含んでいます。

この例では、\( N \)が増えるにつれて\( \frac{1}{n} \)は減少します。これは、最初は現在の状態について十分な情報がないため、測定値に基づいて推定が行われることを意味します。測定が続いていくと、連続する測定値の重みは小さくなり、つまり\( \frac{1}{n} \)は減少していくので、推定プロセスにおける重みは小さくなります。ある時点で、新しい測定値の寄与は無視できるようになります。

この例を続けましょう。最初の測定を行う前に、金の延べ棒の刻印を読めば、金の延べ棒の重さを推測(概算)することができます。これは初期推定値と呼ばれ、私たちの最初の推定値となります。

後で見るように、カルマンフィルターはあらかじめ初期推定が必要で、それは非常に大まかなものになる可能性があります。

推定アルゴリズム

次のチャートは、この例題で使用される推定アルゴリズムを示しています。

Estimation algorithm

これで、測定・推定プロセスをスタートする準備が整いました。

数値例

反復開始前

初期化

金塊の重さの初期推定値は1000gとします。この初期値はフィルタの開始時に一度だけ使われるもので、次の反復処理では必要ありません。

\[ \hat{x}_{0,0}=1000g \]

Prediction

金の延べ棒の重さは変わらないものと考えています。したがって、このシステムは静的なシステム(持続予測モデル)です。なので、次の状態の推定値(予測値)は初期値に等しくなります。

\[ \hat{x}_{1,0} = \hat{x}_{0,0}=1000g \]

反復1回目

Step 1

金の重量を測定します。

\[ z_{1}= 1030g \]

Step 2

ゲインを計算します。この例題では、ゲインは\( \alpha _{n}= \frac{1}{n} \)です。したがって、

\[ \alpha _{1}= \frac{1}{1}=1 \]

状態更新式を使用して、現在の状態の推定値(事後推定値)を計算します。

\[ \hat{x}_{1,1}= \hat{x}_{1,0}+ \alpha _{1} \left( z_{1}- \hat{x}_{1,0} \right) =1000+1 \left( 1030-1000 \right) =1030g \]

注:この具体例では、初期推定値は任意の数値でもよいです。\( \alpha _{1}= 1 \)なので、最初の反復で初期推定値は除去されます。
Step 3

静的なシステムであるため、金の延べ棒の重さは変化しないはずです。つまり、次時点の状態の推定値(予測値)は現時点の状態の推定値と等しくなります。

\[ \hat{x}_{2,1}= \hat{x}_{1,1}=1030g \]

反復2回目

単位時間後、前の反復での予測推定値 (predicted estimate) は、現時点での反復計算では、事前推定値となります。

\[ \hat{x}_{2,1}=1030g \]

Step 1

2回目の質量の測定を行います。

\[ z_{2}= 989g \]

Step 2

ゲインの計算をします。

\[ \alpha _{2}= \frac{1}{2} \]

現在の状態の推定値(事後推定値)を計算します。

\[ \hat{x}_{2,2}= \hat{x}_{2,1}+ \alpha _{2} \left( z_{2}- \hat{x}_{2,1} \right) =1030+\frac{1}{2} \left( 989-1030 \right) =1009.5g \]

Step 3

\[ \hat{x}_{3,2}= \hat{x}_{2,2}=1009.5g \]

反復3回目

\[ z_{3}= 1017g~~~~~~~~~~~~~~~~~~~~~~~~~~ \alpha _{3}= \frac{1}{3} \] \[ \hat{x}_{3,3}=~ 1009.5+\frac{1}{3} \left( 1017-1009.5 \right) =1012g \] \[ \hat{x}_{4,3}=1012g \]

反復4回目

\[ z_{4}= 1009g~~~~~~~~~~~~~~~~~~~~~~~~~~ \alpha _{4}= \frac{1}{4} \] \[ \hat{x}_{4,4}= 1012+\frac{1}{4} \left( 1009-1012 \right) =1011.25g \] \[ \hat{x}_{5,4}=1011.25g \]

反復5回目

\[ z_{5}= 1013g~~~~~~~~~~~~~~~~~~~~~~~~~~ \alpha _{5}= \frac{1}{5} \] \[ \hat{x}_{5,5}= 1011.25+\frac{1}{5} \left( 1013-1011.25 \right) =1011.6g \] \[ \hat{x}_{6,5}=1011.6g \]

反復6回目

\[ z_{6}= 979g~~~~~~~~~~~~~~~~~~~~~~~~~~ \alpha _{6}= \frac{1}{6} \] \[ \hat{x}_{6,6}= 1011.6+\frac{1}{6} \left( 979-1011.6 \right) =1006.17g \] \[ \hat{x}_{7,6}=1006.17g \]

反復7回目

\[ z_{7}=1008g~~~~~~~~~~~~~~~~~~~~~~~~~~ \alpha _{7}= \frac{1}{7} \] \[ \hat{x}_{7,7}= 1006.17+\frac{1}{7} \left( 1008-1006.17 \right) =1006.43g \] \[ \hat{x}_{8,7}=1006.43g \]

反復8回目

\[ z_{8}=1042g~~~~~~~~~~~~~~~~~~~~~~~~~~ \alpha _{8}= \frac{1}{8} \] \[ \hat{x}_{8,8}= 1006.43+\frac{1}{8} \left( 1042-1006.43 \right) =1010.87g \] \[ \hat{x}_{9,8}=1010.87g \]

反復9回目

\[ z_{9}=1012g~~~~~~~~~~~~~~~~~~~~~~~~~~ \alpha _{9}= \frac{1}{9} \] \[ \hat{x}_{9,9}= 1010.87+\frac{1}{9} \left( 1012-1010.87 \right) =1011g \] \[ \hat{x}_{10,9}=1011g \]

反復10回目

\[ z_{10}=1011g~~~~~~~~~~~~~~~~~~~~~~~~~~ \alpha _{10}= \frac{1}{10} \] \[ \hat{x}_{10,10}= 1011+\frac{1}{10} \left( 1011-1011 \right) =1011g \] \[ \hat{x}_{11,10}=1011g \]

ここで止めます。測定するたびにゲインは減少します。したがって、連続した各測定値の寄与は、前の測定値の寄与より低くなります。これで、1010gという本当の重さにかなり近づきました。もし、もっとたくさん測定していたら、もっと真の値に近づくでしょう。

以下の表は測定値と推定値をまとめたものです。グラフは測定値・推定値・真値を比較したものです。

\( n \) 1 2 3 4 5 6 7 8 9 10
\( \alpha _{n} \) \( 1 \) \( \frac{1}{2} \) \( \frac{1}{3} \) \( \frac{1}{4} \) \( \frac{1}{5} \) \( \frac{1}{6} \) \( \frac{1}{7} \) \( \frac{1}{8} \) \( \frac{1}{9} \) \( \frac{1}{10} \)
\( z_{n} \) 1030 989 1017 1009 1013 979 1008 1042 1012 1011
\( \hat{x}_{n,n} \) 1030 1009.5 1012 1011.25 1011.6 1006.17 1006.43 1010.87 1011 1011
\( \hat{x}_{n+1,n} \) 1030 1009.5 1012 1011.25 1011.6 1006.17 1006.43 1010.87 1011 1011
Measurements vs. True value vs. Estimates

推定アルゴリズムが測定値を平滑化し、真値に向かって収束していることがわかります。

本例題のまとめ

この例題では、静的なシステムに対する簡単な推定アルゴリズムを検証しました。また、カルマンフィルタの5つの方程式のうちの1つである状態更新式を導きました。

Example 2 – 1次元における等速運動をする航空機の追尾

今度は時間と共に状態が変化する力学系を調べてみましょう。この例では、α-β フィルタを用いて、等速の飛行機を1次元で追尾します。

1次元の世界を想定してみます。レーダから半径方向に離れて(あるいは向かって)移動する航空機を想定します。1次元の世界では、下図のようにレーダに対する角度は一定で、航空機の高度も一定です。

1D scenario

\( x_{n} \) は、時刻 \( n \) における航空機までの距離を表します。航空機の速度は、時間に対する距離の変化率として定義され、速度は距離の微分で与えられます。

\[ \dot{x}= v= \frac{dx}{dt} \]

レーダはターゲットの方向に一定の速度で追跡ビームを照射します。追尾間隔は\( \Delta t \)です。

速度が一定であると仮定すると、システムのシステムモデルは2つの運動方程式で記述することができます。

\[ x_{n+1}= x_{n}+ \Delta t\dot{x}_{n} \] \[ \dot{x}_{n+1}= \dot{x}_{n} \]

この式から、次時点での航続距離は、現時点での航続距離に、ターゲットの速度と追尾間隔をかけたものを加えた値に等しいことがわかります。この例題では航空機の速度は一定と仮定しているので、次時点での速度は現時点での速度と等しくなります。

上式は状態方程式と呼ばれ、カルマンフィルタの5つの方程式のうちの1つでもあります。この方程式は、現在の状態から次の状態を予測します。

すでに前回の例題では、次時点での質量は現時点での質量と等しいと仮定した状態方程式(静的なモデル)を使用しました。

この状態方程式には行列表記による一般的な形式がありますが、それは後ほど紹介します。

この例題では、このケースに特化した、上記の一連の方程式を使用することにします。

注:カルマンフィルタの5つの方程式のうち2つは既に学習済みです。
  • 状態更新式
  • 状態方程式

ここで、この例題のために状態更新式を修正します。

The \( \alpha - \beta \) filter

レーダの追尾間隔(\( \Delta t \))を5秒とします。時刻 \( n - 1 \) におけるUAV(Unmanned Aerial Vehicle)の位置の推定範囲は30,000 m、UAVの推定速度は40 m/sであると仮定します。

状態方程式を用いると、時刻 \( n \) におけるUAVの位置が予測できます。

\[ \hat{x}_{n,n-1}= \hat{x}_{n-1,n-1}+ \Delta t\hat{\dot{x}}_{n-1,n-1}=30000+5\ast40=30200m \]

時刻 \( n \)におけるUAVの速度は次のように求められます。

\[ \hat{\dot{x}}_{n,n-1}= \hat{\dot{x}}_{n-1,n-1}=40m/s \]

しかしながら、時刻 \( n \) にレーダが測定した距離(\( z_{n} \))は30,110 mであったとします。これは、予測された30,200 mとは異なります。予測された距離と測定された距離の間には、90 m の誤差があります。この誤差の原因は2つ考えられます。

  • レーダによる測定は正確ではない。
  • UAVの速度が、 \( \frac{30,110-30,000}{5}=22m/s \) に変化した。

これら2つの原因のうち、正しいのはどちらでしょうか?

ここで、速度の状態更新式を書き出してみます。

\[ \hat{\dot{x}}_{n,n}= \hat{\dot{x}}_{n,n-1}+ \beta \left( \frac{z_{n}-\hat{x}_{n,n-1}}{ \Delta t} \right) \]

係数\( \beta \)の値は、レーダの精度に依存します。仮にレーダの精度が\( 1 \sigma \)で20 mとすると、予測距離と実測距離の間に90 mのずれがあるのは、機体の速度が変化したためである可能性が高いと考えられます。この場合、\( \beta \)の値を高く設定する(測定値を信用するようにする)必要があります。仮に\( \beta \)=0.9とすると、UAVの推定速度は次のようになります。

\[ \hat{\dot{x}}_{n,n}= \hat{\dot{x}}_{n,n-1}+ \beta \left( \frac{z_{n}-\hat{x}_{n,n-1}}{ \Delta t} \right) =40+0.9 \left( \frac{30110-30200}{5} \right) =23.8m/s \]

一方、レーダの \( 1 \sigma \) の精度が150 mであるとします。この場合、90 mのずれはレーダの測定誤差に起因すると考えられます。この場合、\( \beta \)の値を低く設定する(予測値を信用するようにする)必要があります。仮に\( \beta \)=0.1とすると、UAVの推定速度は次のようになります。

\[ \hat{\dot{x}}_{n,n}= \hat{\dot{x}}_{n,n-1}+ \beta \left( \frac{z_{n}-\hat{x}_{n,n-1}}{ \Delta t} \right) =40+0.1 \left( \frac{30110-30200}{5} \right) =38.2m/s \]

UAVの位置に関する状態更新式は、前回の例で導き出された式と同様です。

\[ \hat{x}_{n,n}=~ \hat{x}_{n,n-1}+ \alpha \left( z_{n}- \hat{x}_{n,n-1} \right) \]

前回の例では、\( \alpha \) の値が繰り返し計算された ( \( \alpha _{n}= \frac{1}{N} \) ) のに対し、この例では、 \( \alpha \) の値は一定です。

\( \alpha \) の大きさは、レーダの測定精度に依存します。高精度のレーダでは、 \( \alpha \) の値を大きくして、測定値に大きい重みを持たせるようにします。もし \( \alpha \) =1であれば、推定位置は測定位置と等しくなります。

\[ \hat{x}_{n,n}=~ \hat{x}_{n,n-1}+ 1 \left( z_{n}- \hat{x}_{n,n-1} \right) = z_{n} \]

\( \alpha =0 \) とすると、測定値は無視されます。

\[ \hat{x}_{n,n}=~ \hat{x}_{n,n-1}+ 0 \left( z_{n}- \hat{x}_{n,n-1} \right) = x_{n,n-1} \]

ここでは、レーダトラッカの状態更新式を構成する方程式系を導き出しました。これらは、\( \alpha - \beta \) track update equations\( \alpha - \beta \) track filtering equations と呼ばれます。

位置の更新式:
\[ \hat{x}_{n,n}=~ \hat{x}_{n,n-1}+ \alpha \left( z_{n}- \hat{x}_{n,n-1} \right) \]

速度の更新式:
\[ \hat{\dot{x}}_{n,n}= \hat{\dot{x}}_{n,n-1}+ \beta \left( \frac{z_{n}-\hat{x}_{n,n-1}}{ \Delta t} \right) \]
注:書籍によっては、 \( \alpha - \beta \) フィルタを g-h フィルタと呼ぶものもあります。ギリシャ文字の \( \alpha \) はアルファベットの g に、ギリシャ文字の \( \beta \) はアルファベットの h に置き換わっています。
注:この例では、距離測定値 ( \( \dot{x}= \frac{ \Delta x}{ \Delta t} \) ) から航空機の速度を導き出しています。現代のレーダは、ドップラー効果を利用して半径方向の速度を直接測定することができます。しかし、私の目的はカルマンフィルタの説明であり、レーダの操作ではありません。そこで、一般性のために、この例では位置測定値から速度を導き出すことにしています。

推定アルゴリズム

この例で使用されている推定アルゴリズムを次の図に示します。

Estimation Algorithm

前回の例とは異なり、この例ではゲイン値 \( \alpha \) と \( \beta \) が与えられています。カルマンフィルタでは \( \alpha \) と \( \beta \) はカルマンゲインに置き換えられ、各反復で計算されますが、それは後ほど扱います。

次から、実際に数値例を用いて解説します。

数値例

1次元の世界で、レーダーから放射状に遠ざかる(もしくは向かう)航空機を考えます。

\( \alpha - \beta \) フィルタのパラメータは次の通りです。

  • \( \alpha =0.2 \)
  • \( \beta =0.1 \)

また、追尾間隔は5秒とします。

注:この例では、より良いグラフィック表現のために、非常に不正確なレーダーと低速のターゲット(UAV)を使用します。現実では、レーダはもっと正確で、ターゲットはもっと速いことが多いです。

反復開始前

初期化

時刻 \( n=0 \) における初期条件は次にように与えられます。

\[ \hat{x}_{0,0}=30000m \] \[ \hat{\dot{x}}_{0,0}=40m/s \]

注:初期条件の取得方法については、後ほど説明する非常に重要なトピックです。今は \( \alpha - \beta \) フィルタの基本的な動作を理解することが目的なので、初期条件は誰かから与えられたと仮定します。
予測

初期推定値は、状態方程式を用いて第1周期 ( \( n=1 \) ) まで計算するものとします。

\[ \hat{x}_{n+1,n}= \hat{x}_{n,n}+ \Delta t\hat{\dot{x}}_{n,n} \rightarrow \hat{x}_{1,0}= \hat{x}_{0,0}+ \Delta t\hat{\dot{x}}_{0,0} =30000+5 \times 40=30200m \] \[ \hat{\dot{x}}_{n+1,n}= \hat{\dot{x}}_{n,n} \rightarrow \hat{\dot{x}}_{1,0}= \hat{\dot{x}}_{0,0} =40m/s \]

反復1回目

最初のサイクル ( \( n=1 \) )では、初期推定値は前回の推定値(事前推定値)です。

\[ \hat{x}_{n,n-1}=~ \hat{x}_{1,0}=30200m \] \[ \hat{\dot{x}}_{n,n-1}=~ \hat{\dot{x}}_{1,0}=40m/s \]

Step 1

レーダは航空機の距離を測定します。

\[ z_{1}= 30110m \]

Step 2

状態更新式を用いて事後推定値を計算します

\[ \hat{x}_{1,1}=~ \hat{x}_{1,0}+ \alpha \left( z_{1}- \hat{x}_{1,0} \right) =30200+0.2 \left( 30110-30200 \right) =30182m \] \[ \hat{\dot{x}}_{1,1}= \hat{\dot{x}}_{1,0}+ \beta \left( \frac{z_{1}-\hat{x}_{1,0}}{ \Delta t} \right) = 40+0.1 \left( \frac{30110-30200}{5} \right) =38.2m/s \]

Step 3

状態方程式を用いて、次の時刻の状態を推定します。

\[ \hat{x}_{2,1}= \hat{x}_{1,1}+ \Delta t\hat{\dot{x}}_{1,1} =30182+5 \times 38.2=30373m \] \[ \hat{\dot{x}}_{2,1}= \hat{\dot{x}}_{1,1} =38.2m/s \]

反復2回目

単位時間の遅延の後、前の反復で予測された推定値は、この反復では事前推定値として用いられます。

\[ \hat{x}_{2,1}=30373m \] \[ \hat{\dot{x}}_{2,1}= 38.2m/s \]

Step 1

レーダは航空機の距離を測定します。

\[ z_{2}= 30265m \]

Step 2

状態更新式を用いて事後推定値を計算します。

\[ \hat{x}_{2,2}=~ \hat{x}_{2,1}+ \alpha \left( z_{2}- \hat{x}_{2,1} \right) =30373+0.2 \left( 30265-30373 \right) =30351.4m \] \[ \hat{\dot{x}}_{2,2}= \hat{\dot{x}}_{2,1}+ \beta \left( \frac{z_{2}-\hat{x}_{2,1}}{ \Delta t} \right) = 38.2+0.1 \left( \frac{30265-30373}{5} \right) =36m/s \]

Step 3

状態方程式を用いて、次の時刻の状態を推定します。

\[ \hat{x}_{3,2}= \hat{x}_{2,2}+ \Delta t\hat{\dot{x}}_{2,2} =30351.4+5 \times 36=30531.6m \] \[ \hat{\dot{x}}_{3,2}= \hat{\dot{x}}_{2,2} =36m/s \]

反復3回目

\[ z_{3}= 30740 \] \[ \hat{x}_{3,3}=~ 30531.6+0.2 \left( 30740~ -30531.6 \right) =30573.3m \] \[ \hat{\dot{x}}_{3,3}= 36+0.1 \left( \frac{30740~ -30531.6}{5} \right) =40.2m/s \] \[ \hat{x}_{4,3}= 30573.3+5 \times 40.2=30774.3m \] \[ \hat{\dot{x}}_{4,3}= 40.2m/s \]

反復4回目

\[ z_{4}= 30750 \] \[ \hat{x}_{4,4}=~ 30774.3+0.2 \left( 30750-30774.3 \right) =30769.5m \] \[ \hat{\dot{x}}_{4,4}= 40.2+0.1 \left( \frac{30750-30774.3}{5} \right) =39.7m/s \] \[ \hat{x}_{5,4}= 30769.5+5 \times 39.7=30968.1m \] \[ \hat{\dot{x}}_{5,4}= 39.7m/s \]

反復5回目

\[ z_{5}= 31135 \] \[ \hat{x}_{5,5}=~ 30968.1+0.2 \left( 31135 -30968.1 \right) =31001.5m \] \[ \hat{\dot{x}}_{5,5}= 39.7+0.1 \left( \frac{31135 -30968.1}{5} \right) =43.1m/s \] \[ \hat{x}_{6,5}= 31001.5+5 \times 43.1=31216.8m \] \[ \hat{\dot{x}}_{6,5}= 43.1m/s \]

反復6回目

\[ z_{6}= 31015 \] \[ \hat{x}_{6,6}=~ 31216.8+0.2 \left( 31015 -31216.8 \right) =31176.4m \] \[ \hat{\dot{x}}_{6,6}= 43.1+0.1 \left( \frac{31015 -31216.8}{5} \right) =39m/s \] \[ \hat{x}_{7,6}= 31176.4+5 \times 39=31371.5m \] \[ \hat{\dot{x}}_{7,6}= 39m/s \]

反復7回目

\[ z_{7}= 31180 \] \[ \hat{x}_{7,7}=~ 31371.5+0.2 \left( 31180 -31371.5 \right) =31333.2m \] \[ \hat{\dot{x}}_{7,7}= 39+0.1 \left( \frac{31180 -31371.5}{5} \right) =35.2m/s \] \[ \hat{x}_{8,7}= 31333.2+5 \times 35.2=31509.2m \] \[ \hat{\dot{x}}_{8,7}= 35.2m/s \]

反復8回目

\[ z_{8}= 31610 \] \[ \hat{x}_{8,8}=~ 31509.2+0.2 \left( 31610 -31509.2 \right) =31529.4m \] \[ \hat{\dot{x}}_{8,8}= 35.2+0.1 \left( \frac{31610 -31509.2}{5} \right) =37.2m/s \] \[ \hat{x}_{9,8}= 31529.4+5 \times 37.2=31715.4m \] \[ \hat{\dot{x}}_{9,8}= 37.2m/s \]

反復9回目

\[ z_{9}= 31960 \] \[ \hat{x}_{9,9}=~ 31715.4+0.2 \left( 31960 -31715.4 \right) =31764.3m \] \[ \hat{\dot{x}}_{9,9}= 37.2+0.1 \left( \frac{31960 -31715.4}{5} \right) =42.1m/s \] \[ \hat{x}_{10,9}= 31764.3+5 \times 42.1=31974.8m \] \[ \hat{\dot{x}}_{10,9}=42.1m/s \]

反復10回目

\[ z_{10}= 31865 \] \[ \hat{x}_{10,10}=~ 31974.8+0.2 \left( 31865 -31974.8 \right) =31952.9m \] \[ \hat{\dot{x}}_{10,10}= 42.1+0.1 \left( \frac{31865 -31974.8}{5} \right) =39.9m/s \] \[ \hat{x}_{11,10}= 31952.9+5 \times 39.9=32152.4m \] \[ \hat{\dot{x}}_{11,10}= 39.9m/s \]

以下の表は、私たちの測定値および推定値をまとめたものです。

\( n \) 1 2 3 4 5 6 7 8 9 10
\( z_{n} \) 30110 30265 30740 30750 31135 31015 31180 31610 31960 31865
\( \hat{x}_{n,n} \) 30182 30351.4 30573.3 30769.5 31001.5 31176.4 31333.2 31529.4 31764.3 31952.9
\( \dot{\hat{x}}_{n,n} \) 38.2 36 40.2 39.7 43.1 39 35.2 37.2 42.1 39.9
\( \hat{x}_{n+1,n} \) 30373 30531.6 30774.3 30968.1 31216.8 31371.5 31509.2 31715.4 31974.8 32152.4
\( \dot{\hat{x}}_{n+1,n} \) 38.2 36.0 40.2 39.7 43.1 39 35.2 37.2 42.1 39.9

次の図は、真値、測定値、推定値を比較したものです。

Measurements vs. True value vs. Estimates - low Alpha and Beta

我々の推定アルゴリズムが測定値を平滑化し、真値に向かって収束していることがわかります。

Using high \( \alpha \) and \( \beta \)

次の図は、\( \alpha = 0.8 \) と \( \beta = 0.5 \) における推定値と真値、実測値を表したものです。

Measurements vs. True value vs. Estimates - high Alpha and Beta

このフィルタの平滑化の効果はかなり低いことがわかります。推定値は測定値に非常に近く、予測される推定値の誤差はかなり大きいことがわかります。

では、\( \alpha \) と \( \beta \) は常に低い値を選べばよいのでしょうか?

答えは"NO"です。なぜなら、\( \alpha \) と \( \beta \) の値は測定精度に依存するものと考えられるためです。レーザレーダのような非常に精密な機器を使用する場合は、測定値に追従するよう、\( \alpha \) と \( \beta \) の値を大きくしておくことが望ましいです。この場合、ターゲットの速度変化にフィルターが素早く反応することになります。一方、測定精度が低い場合は、\( \alpha \) と \( \beta \) を低くすることが望ましいです。この場合、フィルタは測定値の不確かさ(誤差)を平滑化するように振る舞います。しかし、ターゲットの速度変化に対するフィルタの反応はかなり鈍くなります。

\( \alpha \) と \( \beta \) の計算は重要なテーマなので、後ほど詳しく説明します。

本例題のまとめ

この例では、\( \alpha - \beta \) フィルタの状態更新式を導きました。また、状態方程式を学習した。\( \alpha - \beta \) フィルタに基づく1次元系の推定アルゴリズムを開発し、等速運動する航空機に対する数値例を考えました。

Example 3 – 1次元における加速度運動をする航空機の追尾

この例では、すでに説明した \( \alpha - \beta \) フィルタを用いて、一定の加速度で移動する航空機を追尾してみます。

先ほどの例では、40 m/sの等速で移動するUAVを追尾していました。次のグラフは、横軸を時間として、ターゲットとの距離および速度の時間発展を表したものです。

Constant Velocity Movement

見てわかるように、距離の値は線形に変化しています。

では、戦闘機を考えてみましょう。この航空機は、50 m/sの一定の速度で15秒間飛行します。その後、さらに35秒間、8 m/s2の一定の加速度で加速します。

次の図は、ターゲットとの距離、速度、加速度と時間の関係を表したものです。

Accelerated Movement

グラフからわかるように、機体速度は最初の15秒は一定で、その後は線形に変化しています。距離は最初の15秒間は線形に変化し、その後二次関数的に変化しています。

では、前回の例題で使用した \( \alpha - \beta \) フィルタを用いて、この航空機を追尾してみます。

数値例

1次元の世界で、レーダーから放射状に遠ざかる(もしくは向かう)航空機を考えます。

\( \alpha - \beta \) フィルタのパラメータは次の通りです。

  • \( \alpha =0.2 \)
  • \( \beta =0.1 \)

また、追尾間隔は5秒とします。

反復開始前

初期化

時刻 \( n=0 \) における初期条件は次にように与えられます。

\[ \hat{x}_{0,0}=30000m \] \[ \hat{\dot{x}}_{0,0}=50m/s \]

注:初期条件の取得方法については、後ほど説明する非常に重要なトピックです。今は \( \alpha - \beta \) フィルタの基本的な動作を理解することが目的なので、初期条件は誰かから与えられたと仮定します。
予測

初期推定値は、状態方程式を用いて第1周期 ( \( n=1 \) ) まで計算するものとします。

\[ \hat{x}_{n+1,n}= \hat{x}_{n,n}+ \Delta t\hat{\dot{x}}_{n,n} \rightarrow \hat{x}_{1,0}= \hat{x}_{0,0}+ \Delta t\hat{\dot{x}}_{0,0} =30000+5 \times 50=30250m \] \[ \hat{\dot{x}}_{n+1,n}= \hat{\dot{x}}_{n,n} \rightarrow \hat{\dot{x}}_{1,0}= \hat{\dot{x}}_{0,0} =50m/s \]

すべての反復計算を、次の表にまとめます。

\( n \) \( z_{n} \) 現時点の状態推定値 ( \( \hat{x}_{n,n} \), \( \hat{\dot{x}}_{n,n} \) ) 予測値 ( \( \hat{x}_{n+1,n} \), \( \hat{\dot{x}}_{n+1,n} \) )
1 \( 30160m \) \[ \hat{x}_{1,1}=~ 30250+0.2 \left( 30160- 30250 \right) =30232m \] \[ \hat{\dot{x}}_{1,1}= 50+0.1 \left( \frac{30160 -30250}{5} \right) =48.2m/s \] \[ \hat{x}_{2,1}= 30232+5 \times 48.2=30473m \] \[ \hat{\dot{x}}_{2,1}= 48.2m/s \]
2 \( 30365m \) \[ \hat{x}_{2,2}=~ 30473+0.2 \left( 30365 -30473 \right) =30451.1m \] \[ \hat{\dot{x}}_{2,2}= 48.2+0.1 \left( \frac{30365 -30473}{5} \right) =46m/s \] \[ \hat{x}_{3,2}= 30451.1+5 \times 46=30681.6m \] \[ \hat{\dot{x}}_{3,2}= 46m/s \]
3 \( 30890m \) \[ \hat{x}_{3,3}=~ 30681.6+0.2 \left( 30890 -30681.6 \right) =30723.3m \] \[ \hat{\dot{x}}_{3,3}= 46+0.1 \left( \frac{30890 -30681.6}{5} \right) =50.2m/s \] \[ \hat{x}_{4,3}= 30723.3+5 \times 50.2=30974.3m \] \[ \hat{\dot{x}}_{4,3}= 50.2m/s \]
4 \( 31050m \) \[ \hat{x}_{4,4}=~ 30974.3+0.2 \left( 31050 -30974.3 \right) =30989.5m \] \[ \hat{\dot{x}}_{4,4}= 50.2+0.1 \left( \frac{31050 -30974.3}{5} \right) =51.7m/s \] \[ \hat{x}_{5,4}= 30989.5+5 \times 51.7=31248.1m \] \[ \hat{\dot{x}}_{5,4}= 51.7m/s \]
5 \( 31785m \) \[ \hat{x}_{5,5}=~ 31248.1+0.2 \left( 31785 -31248.1 \right) =31355.5m \] \[ \hat{\dot{x}}_{5,5}= 51.7+0.1 \left( \frac{31785 -31248.1}{5} \right) =62.5m/s \] \[ \hat{x}_{6,5}= 31355.5+5 \times 62.5=31667.8m \] \[ \hat{\dot{x}}_{6,5}= 62.5m/s \]
6 \( 32215m \) \[ \hat{x}_{6,6}=~ 31667.8+0.2 \left( 32215 -31667.8 \right) =31777.2m \] \[ \hat{\dot{x}}_{6,6}= 62.5+0.1 \left( \frac{32215 -31667.8}{5} \right) =73.4m/s \] \[ \hat{x}_{7,6}= 31777.2+5 \times 73.4=32144.2m \] \[ \hat{\dot{x}}_{7,6}= 73.4m/s \]
7 \( 33130m \) \[ \hat{x}_{7,7}=~ 32144.2+0.2 \left( 33130 -32144.2 \right) =32341.4m \] \[ \hat{\dot{x}}_{7,7}= 73.4+0.1 \left( \frac{33130 -32144.2}{5} \right) =93.1m/s \] \[ \hat{x}_{8,7}= 32341.4+5 \times 93.1=32807m \] \[ \hat{\dot{x}}_{8,7}= 93.1m/s \]
8 \( 34510m \) \[ \hat{x}_{8,8}=~ 32807+0.2 \left( 34510 -32807 \right) =33147.6m \] \[ \hat{\dot{x}}_{8,8}= 93.1+0.1 \left( \frac{34510 -32807}{5} \right) =127.2m/s \] \[ \hat{x}_{9,8}= 33147.6+5 \times 127.2=33783.5m \] \[ \hat{\dot{x}}_{9,8}= 127.2m/s \]
9 \( 36010m \) \[ \hat{x}_{9,9}=~ 33783.5+0.2 \left( 36010 -33783.5 \right) =34228.8m \] \[ \hat{\dot{x}}_{9,9}= 127.2+0.1 \left( \frac{36010 -33783.5}{5} \right) =171.7m/s \] \[ \hat{x}_{10,9}= 34228.8+5 \times 171.7=35087.4m \] \[ \hat{\dot{x}}_{10,9}= 171.7m/s \]
10 \( 37265m \) \[ \hat{x}_{10,10}=~ 35087.4+0.2 \left( 37265 -35087.4 \right) =35522.9m \] \[ \hat{\dot{x}}_{10,10}= 171.7+0.1 \left( \frac{37265 -35087.4}{5} \right) =215.3m/s \] \[ \hat{x}_{11,10}= 35522.9+5 \times 215.3=36559.2m \] \[ \hat{\dot{x}}_{11,10}= 215.3m/s \]

以下のグラフは、最初の75秒間のターゲットとの距離と速度の真値、測定値、推定値を比較したものです。

Range vs. Time
Velocity vs. Time

真値または測定値と推定値との間に一定の誤差が存在することがわかるかと思います。この誤差をラグエラー (lag error) と呼びます。ラグエラーの他の一般的な名称は以下の通りです

  • Dynamic error
  • Systematic error
  • Bias error
  • Truncation error

本例題のまとめ

この例では、一定の加速度によって発生するラグエラーを調べました。

Example 4 – \( \alpha -\beta -\gamma \) フィルタによる加速度運動をする航空機の追尾

この例では、一定の加速度で移動する航空機を \( \alpha -\beta -\gamma \) フィルタで追尾することにします。

\( \alpha -\beta -\gamma \) フィルタ

\( \alpha -\beta -\gamma \) フィルタg-h-k フィルタとも呼ばれる)は、ターゲットの加速度を考慮します。したがって、状態方程式は次のようになります。

\[ \hat{x}_{n+1,n}= \hat{x}_{n,n}+ \hat{\dot{x}}_{n,n} \Delta t+ \hat{\ddot{x}}_{n,n}\frac{ \Delta t^{2}}{2} \] \[ \hat{\dot{x}}_{n+1,n}= \hat{\dot{x}}_{n,n}+ \hat{\ddot{x}}_{n,n} \Delta t \] \[ \hat{\ddot{x}}_{n+1,n}= \hat{\ddot{x}}_{n,n} \]
ここで、\( \ddot{x}_{n} \) は加速度 (\( x \) の2階微分)

状態更新式は次のようになります。

\[ \hat{x}_{n,n}=~ \hat{x}_{n,n-1}+ \alpha \left( z_{n}- \hat{x}_{n,n-1} \right) \] \[ \hat{\dot{x}}_{n,n}= \hat{\dot{x}}_{n,n-1}+ \beta \left( \frac{z_{n}-\hat{x}_{n,n-1}}{ \Delta t} \right) \] \[ \hat{\ddot{x}}_{n,n}= \hat{\ddot{x}}_{n,n-1}+ \gamma \left( \frac{z_{n}-\hat{x}_{n,n-1}}{0.5 \Delta t^{2}} \right) \]

数値例

先ほどの例のように、15秒間50 m/sの一定速度で飛行し、さらに35秒間8 m/s2の等加速度で加速する航空機の運動を考えてみましょう。

\( \alpha -\beta -\gamma \) フィルタのパラメータは次の通りです。

  • \( \alpha =0.5 \)
  • \( \beta =0.4 \)
  • \( \gamma =0.1 \)

また、追尾間隔は5秒とします。

注:この例では、より良いグラフィック表現のために、非常に不正確なレーダーと低速のターゲット(UAV)を使用します。現実では、レーダはもっと正確で、ターゲットはもっと速いことが多いです。

反復開始前

初期化

時刻 \( n=0 \) における初期条件は次にように与えられます。

\[ \hat{x}_{0,0}=30000m \] \[ \hat{\dot{x}}_{0,0}=50m/s \] \[ \hat{\ddot{x}}_{0,0}=0m/s^{2} \]

Prediction

初期推定値は、状態方程式を用いて第1周期 ( \( n=1 \) ) まで計算するものとします。

\[ \hat{x}_{n+1,n}= \hat{x}_{n,n}+ \hat{\dot{x}}_{n,n} \Delta t+ \hat{\ddot{x}}_{n,n}\frac{ \Delta t^{2}}{2}~~ \rightarrow \hat{x}_{1,0}= \hat{x}_{0,0}+ \hat{\dot{x}}_{0,0} \Delta t+ \hat{\ddot{x}}_{0,0}\frac{ \Delta t^{2}}{2}= 30000+ 50 \times 5+ 0 \times \frac{5^{2}}{2}=30250m \] \[ \hat{\dot{x}}_{n+1,n}= \hat{\dot{x}}_{n,n}+ \hat{\ddot{x}}_{n,n} \Delta t~ \rightarrow \hat{\dot{x}}_{1,0}= \hat{\dot{x}}_{0,0} + \hat{\ddot{x}}_{0,0} \Delta t =50+0 \times 5=50m/s \] \[ \hat{\ddot{x}}_{n+1,n}= \hat{\ddot{x}}_{n,n} \rightarrow ~ \hat{\ddot{x}}_{1,0}= \hat{\ddot{x}}_{0,0}= 0m/s^{2} \]

反復1回目

最初のサイクル ( \( n=1 \) ) では、初期推定値は前回の推定値です。

\[ \hat{x}_{n,n-1}=~ \hat{x}_{1,0}=30250m \] \[ \hat{\dot{x}}_{n,n-1}=~ \hat{\dot{x}}_{1,0}=50m/s \] \[ \hat{\ddot{x}}_{n,n-1}= \hat{\ddot{x}}_{1,0}= 0m/s^{2} \]

Step 1

レーダは航空機の距離を測定します。

\[ z_{1}= 30160m \]

Step 2

状態更新式を用いて、事後推定値を計算します

\[ \hat{x}_{1,1}=~ \hat{x}_{1,0}+ \alpha \left( z_{1}- \hat{x}_{1,0} \right) =30250+0.5 \left( 30160-30250 \right) =30205m \] \[ \hat{\dot{x}}_{1,1}= \hat{\dot{x}}_{1,0}+ \beta \left( \frac{z_{1}-\hat{x}_{1,0}}{ \Delta t} \right) = 50+0.4 \left( \frac{30160-30250}{5} \right) =42.8m/s \] \[ \hat{\ddot{x}}_{1,1}= \hat{\ddot{x}}_{1,0}+ \gamma \left( \frac{z_{1}-\hat{x}_{1,0}}{0.5 \Delta t^{2}} \right) = 0+0.1 \left( \frac{30160-30250}{0.5 \times 5^{2}} \right) =-0.7m/s^{2} \]

Step 3

状態方程式を用いて、次の時刻の状態を推定します。

\[ \hat{x}_{2,1}= \hat{x}_{1,1}+ \hat{\dot{x}}_{1,1} \Delta t+ \hat{\ddot{x}}_{1,1}\frac{ \Delta t^{2}}{2}= 30205+ 42.8 \times 5+ \left( -0.7 \right) \times \frac{5^{2}}{2}=30410m \] \[ \hat{\dot{x}}_{2,1}=\hat{\dot{x}}_{1,1} + \hat{\ddot{x}}_{1,1} \Delta t =42.8+ \left( -0.7 \right) \times 5=39.2m/s \] \[ \hat{\ddot{x}}_{2,1}= \hat{\ddot{x}}_{1,1}= -0.7m/s^{2} \]

反復2回目

単位時間の遅延の後、前の反復で予測された推定値は、この反復では事前推定値として用いられます。

\[ \hat{x}_{2,1}=30410m \] \[ \hat{\dot{x}}_{2,1}= 39.2m/s \] \[ \hat{\ddot{x}}_{2,1}= -0.7m/s^{2} \]

Step 1

レーダは航空機の距離を測定します。

\[ z_{2}= 30365m \]

Step 2

状態更新式を用いて、事後推定値を計算します。

\[ \hat{x}_{2,2}=~ \hat{x}_{2,1}+ \alpha \left( z_{2}- \hat{x}_{2,1} \right) =30410+0.5 \left( 30365-30410 \right) =30387.5m \] \[ \hat{\dot{x}}_{2,2}= \hat{\dot{x}}_{2,1}+ \beta \left( \frac{z_{2}-\hat{x}_{2,1}}{ \Delta t} \right) = 39.2+0.4 \left( \frac{30365-30410}{5} \right) =35.6m/s \] \[ \hat{\ddot{x}}_{2,2}= \hat{\ddot{x}}_{2,1}+ \gamma \left( \frac{z_{2}-\hat{x}_{2,1}}{0.5 \Delta t^{2}} \right) = -0.7+0.1 \left( \frac{30365-30410}{0.5 \times 5^{2}} \right) =-1.1m/s^{2} \]

Step 3

状態方程式を用いて、次の時刻の状態を推定します。

\[ \hat{x}_{3,2}= \hat{x}_{2,2}+ \hat{\dot{x}}_{2,2} \Delta t+ \hat{\ddot{x}}_{2,2}\frac{ \Delta t^{2}}{2}= 30387.5+ 35.6 \times 5+ \left( -1.1 \right) \times \frac{5^{2}}{2}=30552m \] \[ \hat{\dot{x}}_{3,2}=\hat{\dot{x}}_{2,2} + \hat{\ddot{x}}_{2,2} \Delta t =35.6+ \left( -1.1 \right) \times 5=30.2m/s \] \[ \hat{\ddot{x}}_{3,2}= \hat{\ddot{x}}_{2,2}= -1.1m/s^{2} \]

反復3-10回目

以降の反復計算の結果を次の表にまとめます。

\( n \) \( z_{n} \) 事後推定値 ( \( \hat{x}_{n,n} \), \( \hat{\dot{x}}_{n,n} \), \( \hat{\ddot{x}}_{n,n} \) ) 予測値 ( \( \hat{x}_{n+1,n} \), \( \hat{\dot{x}}_{n+1,n} \), \( \hat{\ddot{x}}_{n+1,n} \) )
3 \( 30890m \) \[ \hat{x}_{3,3}=~ 30552+0.5 \left( 30890 -30552 \right) =30721m \] \[ \hat{\dot{x}}_{3,3}= 35.6+0.4 \left( \frac{30890 -30552}{5} \right) =57.2m/s \] \[ \hat{\ddot{x}}_{3,3}=~ -1.1+0.1 \left( \frac{30890 -30552}{0.5 \times 5^{2}} \right) =1.6m/s^{2} \] \[ \hat{x}_{4,3}= 30721+5 \times 57.2 + 1.6 \times \frac{5^{2}}{2}=31027.5m \] \[ \hat{\dot{x}}_{4,3}= 57.2 + 1.6 \times 5=65.4m/s \] \[ \hat{\ddot{x}}_{4,3}= 1.6m/s^{2} \]
4 \( 31050m \) \[ \hat{x}_{4,4}=~ 31027.5+0.5 \left( 31050 -31027.5 \right) = 31038.8m \] \[ \hat{\dot{x}}_{4,4}= 65.4+0.4 \left( \frac{31050 -31027.5}{5} \right) = 67.2m/s \] \[ \hat{\ddot{x}}_{4,4}=~ 1.6+0.1 \left( \frac{31050 -31027.5}{0.5 \times 5^{2}} \right) = 1.8m/s^{2} \] \[ \hat{x}_{5,4}= 31038.8+5 \times 67.2 + 1.8 \times \frac{5^{2}}{2}=31397.1m \] \[ \hat{\dot{x}}_{5,4}= 67.2 + 1.8 \times 5=76.2m/s \] \[ \hat{\ddot{x}}_{4,3}= 1.8m/s^{2} \]
5 \( 31785m \) \[ \hat{x}_{5,5}=~ 31397.1+0.5 \left( 31785 -31397.1 \right) = 31591.1m \] \[ \hat{\dot{x}}_{5,5}= 76.2+0.4 \left( \frac{31785 -31397.1}{5} \right) = 107.2m/s \] \[ \hat{\ddot{x}}_{5,5}=~ 1.8+0.1 \left( \frac{31785 -31397.1}{0.5 \times 5^{2}} \right) = 4.9m/s^{2} \] \[ \hat{x}_{6,5}= 31591.1+5 \times 107.2 + 4.9 \times \frac{5^{2}}{2} = 32188.5m \] \[ \hat{\dot{x}}_{6,5}= 107.2 + 4.9 \times 5 = 131.7m/s \] \[ \hat{\ddot{x}}_{6,5}= 4.9m/s^{2} \]
6 \( 32215m \) \[ \hat{x}_{6,6}=~ 32188.5+0.5 \left( 32215 -32188.5 \right) =32201.7m \] \[ \hat{\dot{x}}_{6,6}= 131.7+0.4 \left( \frac{32215 -32188.5}{5} \right) = 133.9m/s \] \[ \hat{\ddot{x}}_{6,6}=~ 4.9+0.1 \left( \frac{32215 -32188.5}{0.5 \times 5^{2}} \right) =5.1m/s^{2} \] \[ \hat{x}_{7,6}= 32201.7+5 \times 133.9 + 5.1 \times \frac{5^{2}}{2}=32935.1m \] \[ \hat{\dot{x}}_{7,6}= 133.9 + 5.1 \times 5=159.5m/s \] \[ \hat{\ddot{x}}_{7,6}= 5.1m/s^{2} \]
7 \( 33130m \) \[ \hat{x}_{7,7}=~ 32935.1+0.5 \left( 33130 -32935.1 \right) =33032.5m \] \[ \hat{\dot{x}}_{7,7}= 159.5+0.4 \left( \frac{33130 -32935.1}{5} \right) =175.1m/s \] \[ \hat{\ddot{x}}_{7,7}=~ 5.1+0.1 \left( \frac{33130 -32935.1}{0.5 \times 5^{2}} \right) =6.7m/s^{2} \] \[ \hat{x}_{7,6}= 33032.5m+5 \times 175.1 + 6.7 \times \frac{5^{2}}{2}=33991.3m \] \[ \hat{\dot{x}}_{7,6}= 175.1 + 6.7 \times 5=208.5m/s \] \[ \hat{\ddot{x}}_{7,6}= 6.7m/s^{2} \]
8 \( 34510m \) \[ \hat{x}_{8,8}= 33991.3+0.5 \left( 34510 -33991.3 \right) =34250.7m \] \[ \hat{\dot{x}}_{8,8}=208.5+0.4 \left( \frac{34510 -33991.3}{5} \right) =250m/s \] \[ \hat{\ddot{x}}_{8,8}=~ 6.7+0.1 \left( \frac{34510 -33991.3}{0.5 \times 5^{2}} \right) =10.8m/s^{2} \] \[ \hat{x}_{9,8}= 34250.7+5 \times 250 + 10.8 \times \frac{5^{2}}{2}=35635.8m \] \[ \hat{\dot{x}}_{9,8}= 250 + 10.8 \times 5=304.1m/s \] \[ \hat{\ddot{x}}_{9,8}= 10.8m/s^{2} \]
9 \( 36010m \) \[ \hat{x}_{9,9}=~ 35635.8+0.5 \left( 36010 -35635.8 \right) =35822.9m \] \[ \hat{\dot{x}}_{9,9}= 304.1+0.4 \left( \frac{36010 -35635.8}{5} \right) =334m/s \] \[ \hat{\ddot{x}}_{9,9}=~ 10.8+0.1 \left( \frac{36010 -35635.8}{0.5 \times 5^{2}} \right) =13.8m/s^{2} \] \[ \hat{x}_{10,9}= 35822.9+5 \times 334+ 13.8 \times \frac{5^{2}}{2}=37665.8m \] \[ \hat{\dot{x}}_{10,9}= 334 + 13.8 \times 5=403.1m/s \] \[ \hat{\ddot{x}}_{10,9}= 13.8m/s^{2} \]
10 \( 37265m \) \[ \hat{x}_{10,10}=~ 37665.8+0.5 \left( 37265 -37665.8 \right) =37465.4m \] \[ \hat{\dot{x}}_{10,10}= 403.1+0.4 \left( \frac{37265 -37665.8}{5} \right) =371.1m/s \] \[ \hat{\ddot{x}}_{10,10}=~ 13.8+0.1 \left( \frac{37265 -37665.8}{0.5 \times 5^{2}} \right) =10.6m/s^{2} \] \[ \hat{x}_{11,10}= 37465.4+5 \times 371.1 +10.6 \times \frac{5^{2}}{2}=39453.5m \] \[ \hat{\dot{x}}_{11,10}= 371.1 + 10.6 \times 5=424.2m/s \] \[ \hat{\ddot{x}}_{11,10}= 10.6m/s^{2} \]

次の図は、最初の50秒間のターゲットの距離、速度、加速度について、真値、測定値、推定値を比較したものです。

Range vs. Time
Velocity vs. Time
Acceleration vs. Time

このように、加速度を含むシステムモデルを用いた \( \alpha -\beta -\gamma \) フィルタは、一定の加速度で目標に追従し、ラグエラーを解消することができます。

しかし、マニューバを行うようなターゲットの場合はどうなるでしょうか。ターゲットは突然旋回することで飛行方向を変えることができます。また、真のターゲットのシステムモデルには、ジャーク(加速度の変化)が含まれることもあります。このような場合、\( \alpha -\beta -\gamma \) の係数が一定の \( \alpha -\beta -\gamma \) フィルタは推定誤差を生じ、場合によってはターゲットを失うことになります。

カルマンフィルタはシステムモデルの不確実性を扱うことができますが、これは次のトピックで扱います。

\( \alpha -\beta -\gamma \) フィルタのまとめ

\( \alpha -\beta -(\gamma) \) フィルターには多くの種類があり、それらは同じ原理で構成されています。

  • "現時点の状態"の推定は、状態更新式に基づいて行われる。
  • "次時点の状態"の推定(予測)は、システムモデル(状態方程式)に基づいて行われる。

これらのフィルタの主な違いは、パラメータ \( \alpha -\beta -(\gamma) \)の値です。フィルタの種類によっては、一定の重み係数を使用するものもあれば、フィルタの反復(サイクル)ごとに重み係数を計算するものもあります。

\( \alpha \)や\( \beta \)、\( \gamma \) の値は、推定アルゴリズムが適切に機能するために極めて重要です。もう一つの重要な問題は、フィルタの開始、すなわち、状態推定の初期値です。

人気のある \( \alpha -\beta -(\gamma) \) フィルタをリストにして示します。

  • Wiener Filter
  • Bayes Filter
  • Fading-memory polynomial Filter
  • Expanding-memory (or growing-memory) polynomial Filter
  • Least-squares Filter
  • Benedict–Bordner Filter
  • Lumped Filter
  • Discounted least-squares \( \alpha -\beta \) Filter
  • Critically damped \( \alpha -\beta \) Filter
  • Growing-memory Filter
  • Kalman Filter
  • Extended Kalman Filter
  • Unscented Kalman Filter
  • Extended Complex Kalman Filter
  • Gauss-Hermite Kalman Filter
  • Cubature Kalman Filter
  • Particle Filter

将来的には、これらのフィルタのいくつかについてチュートリアルを書きたいと思っていますが、このチュートリアルはカルマンフィルタについてのものなので、次のトピックではカルマンフィルタについて解説します。