オンライン状態推定のための拡張カルマンフィルターアルゴリズムおよびアンセンテッドカルマンフィルターアルゴリズム

離散時間非線形システムのオンラインの状況推定のために離散時間の拡張カルマンフィルターアルゴリズムおよびアンセンテッドカルマンフィルターアルゴリズムを使用できます。著しい非線形性をもつシステムの場合、アンセンテッドカルマンフィルターアルゴリズムによって優れた推定結果が得られる場合があります。状態推定は Simulink^® およびコマンドラインで実行できます。状態推定を行う場合、最初にシステムに合わせた非線形の状態遷移関数と測定関数を作成します。

コマンドラインでこれらの関数を使用して、目的のアルゴリズムの extendedKalmanFilter または unscentedKalmanFilter オブジェクトを作成し、関数のプロセスノイズ項と測定ノイズ項が加法性であるか非加法性であるかを指定します。オブジェクトの作成後、predict および correct コマンドを使用して、リアルタイムデータを使用した状態の推定を行います。これらのコマンドを実行する順序の詳細については、predict および correct のリファレンスページを参照してください。

Simulink では、これらの関数を Extended Kalman Filter ブロックと Unscented Kalman Filter ブロックで指定します。また、関数においてプロセスノイズ項と測定ノイズ項が加法性であるか非加法性であるかを指定します。これらのブロック内では、状態推定の予測と補正が行われる順序がソフトウェアによって決定されます。

拡張カルマンフィルターアルゴリズム

extendedKalmanFilter コマンドと Extended Kalman Filter ブロックは、1 次離散時間カルマンフィルターアルゴリズムを実装します。離散時間非線形システムの状態遷移方程式と測定方程式では、非加法性のプロセスノイズ項および測定ノイズ項はゼロ平均であり、共分散行列 Q と R をそれぞれもつものとします。

$\begin{array}{l} x [k + 1] = f (x [k], w [k], u_{s} [k]) \\ y [k] = h (x [k], v [k], u_{m} [k]) \\ w [k] ~ (0, Q [k]) \\ v [k] ~ (0, R [k]) \end{array}$

ここで f は、タイムステップ間の状態 x の変化を記述する非線形の状態遷移関数です。非線形の測定関数 h は、タイムステップ k での x を測定値 y に関連付けます。これらの関数は、u_s と u_m で表される追加の入力引数をもつこともできます。プロセスノイズおよび測定ノイズは、それぞれ w および v です。Q と R はユーザーが指定します。

ブロック内では、状態推定の予測と補正の順序がソフトウェアによって決定されます。コマンドラインでは、ユーザーが順序を決定します。これらのコマンドを実行する順序の詳細については、predict および correct のリファレンスページを参照してください。predict の前に correct コマンドを実装すると仮定すると、アルゴリズムは次のように実装されます。

状態の初期値 x[0] および状態推定誤差の共分散行列 P でフィルターオブジェクトを初期化します。
$\begin{array}{l} \hat{x} [0 | - 1] = E (x [0]) \\ P [0 | - 1] = E (x [0] - \hat{x} [0 | - 1]) (^{x [0] - \hat{x} [0 | - 1]) T} \end{array}$
ここで $\hat{x}$ は状態推定であり、 $\hat{x} [k_{a} | k_{b}] $ はタイムステップ 0,1,...,k_b での測定値を使用したタイムステップ k_a での状態推定を表します。 $\hat{x} [0 | - 1]$ は、測定を行う前の状態値の最適な推定です。フィルターを作成するときに、この値を指定します。
タイムステップ k = 0,1,2,3,... については、次の手順を実行します。
1. 測定関数のヤコビアンを計算し、測定データ y[k] を使用して状態および状態推定誤差の共分散を更新します。コマンドラインで、correct コマンドがこの更新を実行します。
  $\begin{array}{l} C [k] = {\frac{\partial h}{\partial x} |}_{\hat{x} [k | k - 1]} \\ S [k] = {\frac{\partial h}{\partial v} |}_{\hat{x} [k | k - 1]} \end{array}$
  ヤコビアンに独自の関数ハンドルを指定しない限り、これらのヤコビ行列はソフトウェアで数値的に計算されます。
  $\begin{array}{l} K [k] = P [k | k - 1] C [^{k] T} (^{C [k] P [k | k - 1] C [^{k] T} + S [k] R [k] S [^{k] T}) - 1} \\ \hat{x} [k | k] = \hat{x} [k | k - 1] + K [k] (y [k] - h (\hat{x} [k | k - 1], 0, u_{m} [k]) \\ \hat{P} [k | k] = P [k | k - 1] - K [k] C [k] P [k | k - 1] \end{array}$
  ここでは K はカルマンゲインです。
2. 状態遷移関数のヤコビアンを計算して、次のタイムステップでの状態および状態推定誤差の共分散を予測します。ソフトウェアでは、predict コマンドがこの予測を実行します。
  $\begin{array}{l} A [k] = {\frac{\partial f}{\partial x} |}_{\hat{x} [k | k]} \\ G [k] = {\frac{\partial f}{\partial w} |}_{\hat{x} [k | k]} \end{array}$
  解析ヤコビアンを指定しない限り、これらのヤコビ行列は数値的に計算されます。この数値計算によって処理時間が長くなり、状態推定の数値が不正確になる場合があります。
  $\begin{array}{l} P [k + 1 | k] = A [k] P [k | k] A [^{k] T} + G [k] Q [k] G [^{k] T} \\ \hat{x} [k + 1 | k] = f (\hat{x} [k | k], 0, u_{s} [k]) \end{array}$
  関数 correct はこれらの値を次のタイムステップで使用します。数値パフォーマンスを向上させるために、共分散行列の平方根の因数分解が使用されます。この因数分解の詳細については、[2]を参照してください。

Extended Kalman Filter ブロックは、複数の測定関数をサポートしています。これらの測定値は、サンプル時間が状態遷移のサンプル時間の整数倍であれば、異なるサンプル時間をもつことができます。この場合、各測定関数からの測定値に応じて個別の補正ステップが実行されます。

前述のアルゴリズムのステップでは、状態遷移関数と測定関数に非加法性ノイズ項があることが想定されています。関数に加法性ノイズ項がある場合、このアルゴリズムの変更は次のとおりです。

プロセスノイズ w が加法性である (つまり状態遷移方程式の形式が $x [k] = f (x [k - 1], u_{s} [k - 1]) + w [k - 1]$ である) 場合、ヤコビ行列 G[k] は単位行列。
測定ノイズ v が加法性である (つまり測定方程式の形式が $y [k] = h (x [k], u_{m} [k]) + v [k]$ である) 場合、ヤコビ行列 S[k] は単位行列。

ステート関数および遷移関数の加法性ノイズ項により、処理時間が短縮されます。

一次の拡張カルマンフィルターは、非線形の状態遷移関数と測定関数の線形近似を使用します。結果として、システムの非線形性が著しい場合はアルゴリズムの信頼性が低いことがあります。この場合、アンセンテッドカルマンフィルターアルゴリズムによって優れた結果が得られる場合があります。

アンセンテッドカルマンフィルターアルゴリズム

アンセンテッドカルマンフィルターアルゴリズムと Unscented Kalman Filter ブロックは、アンセンテッド変換を使用して、非線形関数による状態推定の統計プロパティの伝播を取得します。アルゴリズムはまず、シグマポイントという一連の状態値を生成します。これらのシグマポイントは、状態推定の平均と共分散を取得します。アルゴリズムは、各シグマポイントを状態遷移関数と測定関数への入力として使用して、新しい一連の変換された状態点を取得します。変換された点の平均と共分散は、状態推定および状態推定誤差の共分散を取得するために使用されます。

M 状態の離散時間非線形システムの状態遷移方程式と測定方程式には、ゼロ平均と共分散をもつ加法性のプロセスノイズ項および測定ノイズ項 Q および R がそれぞれあるとします。

$\begin{array}{l} x [k + 1] = f (x [k], u_{s} [k]) + w [k] \\ y [k] = h (x [k], u_{m} [k]) + v [k] \\ w [k] ~ (0, Q [k]) \\ v [k] ~ (0, R [k]) \end{array}$

Q および R の初期値をアンセンテッドカルマンフィルターオブジェクトの ProcessNoise および MeasurementNoise プロパティに指定します。

状態の初期値 x[0] および状態推定誤差の共分散 P でフィルターオブジェクトを初期化します。
$\begin{array}{l} \hat{x} [0 | - 1] = E (x [0]) \\ P [0 | - 1] = E (x [0] - \hat{x} [0 | - 1]) (^{x [0] - \hat{x} [0 | - 1]) T} \end{array}$
ここで $\hat{x}$ は状態推定であり、 $\hat{x} [k_{a} | k_{b}] $ はタイムステップ 0,1,...,k_b での測定値を使用したタイムステップ k_a での状態推定を表します。 $\hat{x} [0 | - 1]$ は、測定を行う前の状態値の最適な推定です。フィルターを作成するときに、この値を指定します。
各タイムステップ k については、測定データ y[k] を使用して状態および状態推定誤差の共分散を更新します。ソフトウェアでは、correct コマンドがこの更新を実行します。
1. タイムステップ k でのシグマポイント ${\hat{x}}^{(i)} [k | k - 1]$ を選択します。
  $\begin{array}{l} {\hat{x}}^{(0)} [k | k - 1] = \hat{x} [k | k - 1] \\ {\hat{x}}^{(i)} [k | k - 1] = \hat{x} [k | k - 1] + Δ x^{(i)} i = 1, ..., 2 M \\ Δ x^{(i)} = (\sqrt{c P [k | k - 1]}) {}_{i} i = 1, ..., M \\ Δ x^{(M + i)} = - {(\sqrt{c P [k | k - 1]})}_{i} i = 1, ..., M \end{array}$
  ここで $c = α^{2} (M + κ)$ は、状態の数 M とパラメーター α および κ に基づくスケーリング係数です。パラメーターの詳細については、アルファ、ベータ、カッパパラメーターの影響を参照してください。 $\sqrt{c P}$ は、 $\sqrt{c P} {(\sqrt{c P})}^{T} = c P$ であり ${(\sqrt{c P})}_{i}$ が $\sqrt{c P}$ の i 番目の列であるような、cP の行列の平方根です。
2. 非線形の測定関数を使用して、それぞれのシグマポイントについて予測される測定値を計算します。
  ${\hat{y}}^{(i)} [k | k - 1] = h ({\hat{x}}^{(i)} [k | k - 1], u_{m} [k]) i = 0, 1, ..., 2 M$
3. 予測される測定値を組み合わせて、時間 k の予測される測定値を取得します。
  $\begin{array}{l} \hat{y} [k] = \sum_{i = 0}^{2 M} W_{M}^{(i)} {\hat{y}}^{(i)} [k | k - 1] \\ W_{M}^{(0)} = 1 - \frac{M}{α^{2} (M + κ)} \\ W_{M}^{i} = \frac{1}{2 α^{2} (M + κ)} i = 1, 2, ..., 2 M \end{array}$
4. 予測される測定値の共分散を推定します。加法性の測定ノイズを考慮するために R[k] を追加します。
  $\begin{array}{l} P_{y} = \sum_{i = 0}^{2 M} W_{c}^{(i)} ({\hat{y}}^{(i)} [k | k - 1] - \hat{y} [k]) (^{{\hat{y}}^{(i)} [k | k - 1] - \hat{y} [k]) T} + R [k] \\ W_{c}^{(0)} = (2 - α^{2} + β) - \frac{M}{α^{2} (M + κ)} \\ W_{c}^{i} = 1 / (2 α^{2} (M + κ)) i = 1, 2, ..., 2 M \end{array}$
  β パラメーターの詳細については、アルファ、ベータ、カッパパラメーターの影響を参照してください。
5. $\hat{x} [k | k - 1]$ と $\hat{y} [k]$ の間の相互共分散を推定します。
  $P_{x y} = \frac{1}{2 α^{2} (m + κ)} \sum_{i = 1}^{2 M} ({\hat{x}}^{(i)} [k | k - 1] - \hat{x} [k | k - 1]) {({\hat{y}}^{(i)} [k | k - 1] - \hat{y} [k])}^{T}$
  ${\hat{x}}^{(0)} [k | k - 1] - \hat{x} [k | k - 1] = 0$ であるため、総和計算は i = 1 から開始されます。
6. タイムステップ k での推定された状態および状態推定誤差の共分散を取得します。
  $\begin{array}{l} K = P_{x y} P_{y}^{- 1} \\ \hat{x} [k | k] = \hat{x} [k | k - 1] + K (y [k] - \hat{y} [k]) \\ P [k | k] = P [k | k - 1] - K P_{y} K_{k}^{T} \end{array}$
  ここでは K はカルマンゲインです。
次のタイムステップでの状態と状態推定誤差の共分散を予測します。ソフトウェアでは、predict コマンドがこの予測を実行します。
1. タイムステップ k でのシグマポイント ${\hat{x}}^{(i)} [k | k]$ を選択します。
  $\begin{array}{l} {\hat{x}}^{(0)} [k | k] = \hat{x} [k | k] \\ {\hat{x}}^{(i)} [k | k] = \hat{x} [k | k] + Δ x^{(i)} i = 1, ..., 2 M \\ Δ x^{(i)} = (\sqrt{c P [k | k]}) {}_{i} i = 1, ..., M \\ Δ x^{(M + i)} = - {(\sqrt{c P [k | k]})}_{i} i = 1, ..., M \end{array}$
2. 非線形の状態遷移関数を使用して、それぞれのシグマポイントについて予測される状態を計算します。
  ${\hat{x}}^{(i)} [k + 1 | k] = f ({\hat{x}}^{(i)} [k | k], u_{s} [k])$
3. 予測される状態を組み合わせて、時間 k+1 の予測される状態を取得します。これらの値は、次のタイムステップで correct コマンドによって使用されます。
  $\begin{array}{l} \hat{x} [k + 1 | k] = \sum_{i = 0}^{2 M} W_{M}^{(i)} {\hat{x}}^{(i)} [k + 1 | k] \\ W_{M}^{(0)} = 1 - \frac{M}{α^{2} (M + κ)} \\ W_{M}^{i} = \frac{1}{2 α^{2} (M + κ)} i = 1, 2, ..., 2 M \end{array}$
4. 予測される状態の共分散を計算します。加法性のプロセスノイズを考慮するために Q[k] を追加します。これらの値は、次のタイムステップで correct コマンドによって使用されます。
  
  $\begin{array}{l} P [k + 1 | k] = \sum_{i = 0}^{2 M} W_{c}^{(i)} ({\hat{x}}^{(i)} [k + 1 | k] - \hat{x} [k + 1 | k]) {({\hat{x}}^{(i)} [k + 1 | k] - \hat{x} [k + 1 | k])}^{T} + Q [k] \\ W_{c}^{(0)} = (2 - α^{2} + β) - \frac{M}{α^{2} (M + κ)} \\ W_{c}^{i} = 1 / (2 α^{2} (M + κ)) i = 1, 2, ..., 2 M \end{array}$

Unscented Kalman Filter ブロックは、複数の測定関数をサポートしています。これらの測定値は、サンプル時間が状態遷移のサンプル時間の整数倍であれば、異なるサンプル時間をもつことができます。この場合、各測定関数からの測定値に応じて個別の補正ステップが実行されます。

以前のアルゴリズムは加法性ノイズ項を考慮して状態遷移方程式と測定方程式に実装されます。数値パフォーマンスを向上させるために、共分散行列の平方根の因数分解が使用されます。この因数分解の詳細については、[2]を参照してください。

ノイズ項は非加法性であり、アルゴリズムの主な変更は次のとおりです。

correct コマンドは、P[k|k-1] および R[k] を使用して 2*(M+V)+1 シグマポイントを生成します。ここでは、V は測定ノイズ v[k] の要素の数です。追加のシグマポイントが Py の測定ノイズの影響を取得するため、R[k] 項がアルゴリズムのステップ 2(d) に追加されることはありません。
predict コマンドは P[k|k] および Q[k] を使用して 2*(M+W)+1 シグマポイントを生成します。ここで W は、プロセスノイズ w[k] 内の要素の数です。追加のシグマポイントが P[k+1|k] のプロセスノイズの影響を取得するため、Q[k] 項がアルゴリズムのステップ 3(d) に追加されることはありません。

アルファ、ベータ、カッパパラメーターの影響

次のタイムステップでの状態およびその統計プロパティを計算するために、アンセンテッドカルマンフィルターアルゴリズムが平均の状態値の周りに分布する一連の状態値を生成します。アルゴリズムは、各シグマポイントを状態遷移関数と測定関数への入力として使用して、新しい一連の変換された状態ポイントを取得します。変換された点の平均と共分散は、状態推定および状態推定誤差の共分散を取得するために使用されます。

平均の状態値の周りのシグマポイントの広がりは、2 つのパラメーター α および κ によって制御されます。3 番目のパラメーター β は、状態および測定共分散の計算中に変換された点の重みに影響します。

α — 平均の状態値の周りのシグマポイントの広がりを決定します。これは通常、小さい正の数値です。シグマポイントの広がりは α に比例します。値が小さいほど、シグマポイントは平均の状態に近くなります。
κ — 2 番目のスケーリングパラメーター。通常は 0 に設定されます。値が小さいほど、シグマポイントは平均の状態に近くなります。広がりは κ の平方根に比例します。
β — 状態の分布の事前情報を組み込みます。ガウス分布の場合、β = 2 が最適です。

これらのパラメーターをアンセンテッドカルマンフィルターの Alpha、Kappa、Beta プロパティに指定します。状態および状態の共分散の分布がわかっている場合は、これらのパラメーターを調整して分布の高次モーメントの変換を取得できます。アルゴリズムが追跡できるのは、状態の確率分布の単一のピークのみです。システムの状態分布に複数のピークがある場合、これらのパラメーターを調整してシグマポイントが単一のピークの周りにとどまるようにすることができます。たとえば、小さい Alpha を選択して、平均の状態値に近いシグマポイントを生成します。

参照

[1] Simon, Dan. Optimal State Estimation: Kalman, H Infinity, and Nonlinear Approaches. Hoboken, NJ: John Wiley and Sons, 2006.

[2] Van der Merwe, Rudolph, and Eric A. Wan. “The Square-Root Unscented Kalman Filter for State and Parameter-Estimation.” 2001 IEEE International Conference on Acoustics, Speech, and Signal Processing. Proceedings (Cat. No.01CH37221), 6:3461–64. Salt Lake City, UT, USA: IEEE, 2001. https://doi.org/10.1109/ICASSP.2001.940586.