Kalman Filter

動的システムの状態の予測または推定

ライブラリ

Filtering/Adaptive Filters

dspadpt3

説明

Kalman Filter ブロックを使用して、不完全でノイズを含む一連の測定を使用して動的システムの状態を予測または推定します。次の方程式で定義される、ノイズを含む線形システムがあると仮定します。

$\begin{array}{l} x_{k} = A x_{k - 1} + w_{k - 1} \\ z_{k} = H x_{k} + v_{k} \end{array}$

このブロックは、以前に推定した状態 ${\hat{x}}_{k - 1}$ を使用して、k, $x_{k}^{-}$ 時点の現在の状態を以下の方程式で示すように予測できます。

$\begin{array}{l} x_{k}^{-} = A {\hat{x}}_{k - 1} \\ P_{k}^{-} = A {\hat{P}}_{k - 1} A^{T} + Q \end{array}$

また、より正確な近似となるように、現在の測定 $z_{k}$ および予測される状態 $x_{k}^{-}$ を使用して、k, ${\hat{x}}_{k}$ 時点の現在の状態値を推定することもできます。

$\begin{array}{l} K_{k} = P_{k}^{-} H^{T} {(H P_{k}^{-} H^{T} + R)}^{- 1} \\ {\hat{x}}_{k} = x_{k}^{-} + K_{k} (z_{k} - H x_{k}^{-}) \\ {\hat{P}}_{k} = (I - K_{k} H) P_{k}^{-} \end{array}$

以前の方程式の変数を次の表に定義します。

変数	定義	既定値または初期条件
x	状態	N/A
$\hat{x}$	推定される状態	`zeros([6, 1])`
$x^{-}$	予測される状態	N/A
A	状態遷移マトリックス	$[\begin{matrix} 1 & 0 & 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \end{matrix}]$
w	プロセスノイズ	N/A
z	測定	N/A
H	測定マトリックス	$[\begin{matrix} 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \end{matrix}]$
v	測定ノイズ	N/A
$\hat{P}$	推定誤差の共分散	`10*eye(6)`
P^-	予測誤差の共分散	N/A
Q	プロセスノイズの共分散	`0.05*eye(6)`
K	カルマンゲイン	N/A
R	測定ノイズの共分散	`eye(4)`
I	単位行列	N/A

前述の方程式で、z は測定値のベクトルです。多くの場合、ブロックは Z、M 行 N 列の行列を処理します。ここで M は測定値の数で、N はフィルターの数です。

[フィルター数] パラメーターを使用して、現在の値を予測または推定するために使用するフィルターの数を指定します。

[フィルターの有効化] パラメーターを使用して、各タイムステップで有効または無効にするフィルターを指定します。Always を選択すると、フィルターは常に有効になります。Specify via input port <Enable> を選択すると、ブロックに Enable 端子が表示されます。この端子への入力は、長さがフィルターの数と等しい、1 と 0 の行ベクトルでなければなりません。たとえば、3 つのフィルターがあり、Enable 端子への入力が [1 0 1] である場合、このタイムステップでは 1 番目と 3 番目のフィルターのみが有効になります。[フィルターが無効な場合は推定された状態と推定誤差の共分散をリセット] チェックボックスをオンにすると、無効にされたフィルターに対応する推定および予想された状態と推定誤差の共分散は初期値にリセットされます。

メモ

すべてのフィルターの状態遷移マトリックス、測定マトリックス、初期条件およびノイズ共分散は同じですが、状態、測定、有効および MSE 信号は一意です。状態、測定、有効および MSE 信号では、各列がフィルターに対応します。

[測定マトリックスのソース] パラメーターを使用して、測定マトリックス値を入力する方法を指定します。Specify via dialog を選択すると、[測定マトリックス] パラメーターがダイアログボックスに表示されます。Input port <H> を選択すると、ブロックに H 端子が表示されます。この端子を使用して測定マトリックスを指定します。

パラメーター

フィルター数: 現在の値を予測または推定するために使用するフィルターの数を指定します。
フィルターの有効化: 各タイムステップで有効または無効にするフィルターを指定します。Always を選択すると、フィルターは常に有効になります。Specify via input port <Enable> を選択すると、ブロックに Enable 端子が表示されます。
フィルターが無効な場合は推定された状態と推定誤差の共分散をリセット: このチェックボックスをオンにすると、無効にされたフィルターに対応する推定および予測される状態と推定誤差の共分散は初期値にリセットされます。このパラメーターが表示されるのは [フィルターの有効化] パラメーターで Specify via input port <Enable> を選択した場合です。
Initial condition for estimated state: 推定される状態の初期条件を入力します。
推定誤差の共分散の初期状態: 推定誤差の共分散の初期条件を入力します。
状態遷移マトリックス: 状態遷移マトリックスを入力します。
プロセスノイズの共分散: プロセスノイズの共分散を入力します。
測定マトリックスのソース: 測定マトリックス値を入力する方法を指定します。Specify via dialog を選択すると、[測定マトリックス] パラメーターがダイアログボックスに表示されます。Input port <H> を選択すると、ブロックに H 端子が表示されます。
測定マトリックス: 測定マトリックス値を入力します。このパラメーターが表示されるのは [測定マトリックスのソース] パラメーターで Specify via dialog を選択した場合です。
測定ノイズの共分散: 測定ノイズの共分散を入力します。
Output estimated measurement <Z_est>: 推定される測定値を出力するには、このチェックボックスをオンにします。
Output estimated state <X_est>: 推定される状態を出力するには、このチェックボックスをオンにします。
Output MSE of estimated state <MSE_est>: 推定される状態の平均二乗誤差を出力するには、このチェックボックスをオンにします。
予測された測定値の出力 <Z_prd>: 予測される測定値を出力するには、このチェックボックスをオンにします。
予測された状態の出力 <X_prd>: 予測される状態を出力するには、このチェックボックスをオンにします。
Output MSE of predicted state <MSE_prb>: 予測される状態の平均二乗誤差を出力するには、このチェックボックスをオンにします。

参考文献

[1] Haykin, Simon. Adaptive Filter Theory. Upper Saddle River, NJ: Prentice Hall, 1996.

[2] Welch, Greg and Gary Bishop, “An Introduction to the Kalman Filter,” TR 95–041, Department of Computer Science, University of North Carolina.

サポートされているデータ型

端子	入力/出力	サポートされているデータ型
Z	M 行 N 列の測定。M は測定ベクトルの長さで N はフィルターの数です。	倍精度浮動小数点単精度浮動小数点
Enable	1 と 0 の 1 行 N 列のベクトル。N はフィルターの数です。	倍精度浮動小数点単精度浮動小数点 boolean
H	M 行 P 列の測定。M は測定ベクトルの長さで P はフィルター状態ベクトルの長さです。	Z 端子と同じ
Z_est	M 行 N 列の推定される測定マトリックス。M は測定ベクトルの長さで N はフィルターの数です。	Z 端子と同じ
X_est	P 行 N 列の推定される状態マトリックス。P はフィルター状態ベクトルの長さで N はフィルターの数です。	Z 端子と同じ
MSE_est	推定される状態の平均二乗誤差を表す 1 行 N 列のベクトル。N はフィルターの数です。	Z 端子と同じ
Z_prd	M 行 N 列の予測される測定マトリックス。M は測定ベクトルの長さで N はフィルターの数です。	Z 端子と同じ
X_prd	P 行 N 列の予測される状態マトリックス。P はフィルター状態ベクトルの長さで N はフィルターの数です。	Z 端子と同じ
MSE_prd	予測される状態の平均二乗誤差を表す 1 行 N 列のベクトル。N はフィルターの数です。	Z 端子と同じ