最新のリリースでは、このページがまだ翻訳されていません。 このページの最新版は英語でご覧になれます。

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

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

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

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

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

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

x[k+1]=f(x[k],w[k],us[k])y[k]=h(x[k],v[k],um[k])w[k]~(0,Q[k])v[k]~(0,R[k])

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

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

  1. 状態の初期値 x[0] および状態推定誤差の共分散行列 P でフィルター オブジェクトを初期化します。

    x^[0|1]=E(x[0])P[0|1]=E(x[0]x^[0|1])(x[0]x^[0|1])T

    ここで x^ は状態推定であり、x^[ka|kb] はタイム ステップ 0,1,...,kb での測定値を使用したタイム ステップ ka での状態推定を表します。x^[0|1] は、測定を行う前の状態値の最適な推定です。フィルターを作成するときに、この値を指定します。

  2. タイム ステップ k = 0,1,2,3,... については、次の手順を実行します。

    1. 測定関数のヤコビアンを計算し、測定データ y[k] を使用して状態および状態推定誤差の共分散を更新します。コマンド ラインで、correct コマンドがこの更新を実行します。

      C[k]=hx|x^[k|k1]S[k]=hv|x^[k|k1]

      解析ヤコビアンを指定しない限り、これらのヤコビ行列は数値的に計算されます。

      K[k]=P[k|k1]C[k]T(C[k]P[k|k1]C[k]T+S[k]R[k]S[k]T)1x^[k|k]=x^[k|k1]+K[k](y[k]h(x^[k|k1],0,um[k])P^[k|k]=P[k|k-1]K[k] C[k] P[k|k-1]

      ここでは K はカルマン ゲインです。

    2. 状態遷移関数のヤコビアンを計算して、次のタイム ステップでの状態および状態推定誤差の共分散を予測します。ソフトウェアでは、predict コマンドがこの予測を実行します。

      A[k]=fx|x^[k|k]G[k]=fw|x^[k|k]

      解析ヤコビアンを指定しない限り、これらのヤコビ行列は数値的に計算されます。この数値計算によって処理時間が長くなり、状態推定の数値が不正確になる場合があります。

      P[k+1|k]=A[k]P[k|k]A[k]T+G[k]Q[k]G[k]Tx^[k+1|k]=f(x^[k|k],0,us[k])

      これらの値は、次のタイム ステップで correct コマンドによって使用されます。

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

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

  • プロセス ノイズ w が加法性である (つまり状態遷移方程式の形式が x[k]=f(x[k1],us[k1])+w[k1] である) 場合、ヤコビ行列 G[k] は単位行列。

  • 測定ノイズ v が加法性である (つまり測定方程式の形式が y[k]=h(x[k],um[k])+v[k] である) 場合、ヤコビ行列 S[k] は単位行列。

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

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

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

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

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

x[k+1]=f(x[k],us[k])+w[k]y[k]=h(x[k],um[k])+v[k]w[k]~(0,Q[k])v[k]~(0,R[k])

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

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

  1. 状態の初期値 x[0] および状態推定誤差の共分散 P でフィルター オブジェクトを初期化します。

    x^[0|1]=E(x[0])P[0|1]=E(x[0]x^[0|1])(x[0]x^[0|1])T

    ここで x^ は状態推定であり、x^[ka|kb] はタイム ステップ 0,1,...,kb での測定値を使用したタイム ステップ ka での状態推定を表します。x^[0|1] は、測定を行う前の状態値の最適な推定です。フィルターを作成するときに、この値を指定します。

  2. 各タイム ステップ k については、測定データ y[k] を使用して状態および状態推定誤差の共分散を更新します。ソフトウェアでは、correct コマンドがこの更新を実行します。

    1. タイム ステップ k でのシグマ ポイント x^(i)[k|k1] を選択します。

      x^(0)[k|k1]= x^[k|k1]x^(i)[k|k1]   =  x^[k|k1]+Δx(i)        i=1,...,2MΔx(i)     =  (cP[k|k1]) i   i=1,...,MΔx(M+i)= (cP[k|k1])i   i=1,...,M

      ここで c=α2(M+κ) は、状態の数 M とパラメーター α および κ に基づくスケーリング係数です。パラメーターの詳細については、アルファ、ベータ、カッパ パラメーターの影響を参照してください。cP は、cP (cP)T=cP であり (cP)icP の i 番目の列であるような、cP の行列の平方根です。

    2. 非線形の測定関数を使用して、それぞれのシグマ ポイントについて予測される測定値を計算します。

      y^(i)[k|k1]=h(x^(i)[k|k1],um[k])        i=0,1,...,2M

    3. 予測される測定値を組み合わせて、時間 k の予測される測定値を取得します。

      y^[k]=i=02MWM(i)y^(i)[k|k1]WM(0)=1Mα2(M+κ)WMi=12α2(M+κ)       i=1,2,...,2M

    4. 予測される測定値の共分散を推定します。加法性の測定ノイズを考慮するために R[k] を追加します。

      Py=i=02MWc(i)(y^(i)[k|k1]y^[k])(y^(i)[k|k1]y^[k])T+R[k]Wc(0)=(2α2+β)Mα2(M+κ)Wci=1/(2α2(M+κ))       i=1,2,...,2M

      β パラメーターの詳細については、アルファ、ベータ、カッパ パラメーターの影響を参照してください。

    5. x^[k|k1]y^[k] の間の相互共分散を推定します。

      Pxy=12α2(m+κ)i=12M(x^(i)[k|k1]x^[k|k1])(y^(i)[k|k1]y^[k])T

      x^(0)[k|k1]x^[k|k1]=0 であるため、総和計算は i = 1 から開始されます。

    6. タイム ステップ k での推定された状態および状態推定誤差の共分散を取得します。

      K=PxyPy1x^[k|k]=x^[k|k1]+K(y[k]y^[k])P[k|k]=P[k|k1]KPyKkT

      ここでは K はカルマン ゲインです。

  3. 次のタイム ステップでの状態と状態推定誤差の共分散を予測します。ソフトウェアでは、predict コマンドがこの予測を実行します。

    1. タイム ステップ k でのシグマ ポイント x^(i)[k|k] を選択します。

      x^(0)[k|k] =  x^[k|k]x^(i)[k|k]     =  x^[k|k]+Δx(i)        i=1,...,2MΔx(i)     =  (cP[k|k]) i   i=1,...,MΔx(M+i)= (cP[k|k])i   i=1,...,M

    2. 非線形の状態遷移関数を使用して、それぞれのシグマ ポイントについて予測される状態を計算します。

      x^(i)[k+1|k]=f(x^(i)[k|k],us[k]) 

    3. 予測される状態を組み合わせて、時間 k+1 の予測される状態を取得します。これらの値は、次のタイム ステップで correct コマンドによって使用されます。

      x^[k+1|k]=i=02MWM(i)x^(i)[k+1|k]WM(0)=1Mα2(M+κ)WMi=12α2(M+κ)     i=1,2,...,2M

    4. 予測される状態の共分散を計算します。加法性のプロセス ノイズを考慮するために Q[k] を追加します。これらの値は、次のタイム ステップで correct コマンドによって使用されます。

      P[k+1|k]=i=02MWc(i)(x^(i)[k+1|k]x^[k+1|k])(x^(i)[k+1|k]x^[k+1|k])T+Q[k]Wc(0)=(2α2+β)Mα2(M+κ)Wci=1/(2α2(M+κ))   i=1,2,...,2M

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

以前のアルゴリズムは加法性ノイズ項を考慮して状態遷移方程式と測定方程式に実装されます。ノイズ項は非加法性であり、アルゴリズムの主な変更は次のとおりです。

  • 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 が最適です。

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

参照

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

参考

関数

ブロック