オンライン状态推定推定のためためののカルマンカルマンフィルターアルゴリズムおよびアンセンテッドフィルター - Matlab和Simulink 金宝app - Mathworks日本

オンラインオンライン状态推定ののためののカルマンカルマンフィルターアルゴリズムおよびカルマンフィルター

离散时间非非形システムオンラインの状况推定のに离散の张カルマンフィルターアルゴリズムアンセンテッド非フィルターアルゴリズム使ますでき非非形をもつのののがをもつシステム推定结果得得场合。状态推定はsimulink金宝app^®およびおよびコマンドラインで実実できます推定うううう合，最初にシステムに合并た非形のの状态关键词相关关键词

コマンドラインでこれらの关联数目使使し，目的のアルゴリズムのExtendedKalmanFilter.またはundentedkalmanfilter作者：郝晓晖预测および正确的コマンドタイムデータを使て状态状态の推定います状态コマンド実は。预测および正确的ののリファレンスページをを参照してて

金宝appSimulinkでは，これらの关键扩展卡尔曼筛选器ブロックとUnscented Kalman Filter.ブロック，关节においてプロセスノイズと测定非项性性を指定でますであるかブロック内でしはかブロック内でしはかか加でしはかか加でははかか加指定ははかか加ではははか加でははわか内でははわをのででlがソフトウェアによって决定さますます。

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

ExtendedKalmanFilter.コマンドと扩展卡尔曼筛选器ブロックは，1次离散时间カルマンフィルターを実装します。离散时空形システムの状态式と测定方程式式で，非加法性プロセスノイズおよび测定ノイズ项ゼロゼロでありおよびノイズ项ゼロゼロでありでありでありrををそれぞれももつものしし

$\begin{array}{l} X [K. + 1] = F （ X [K.] 那 W. [K.] 那你_{S.} [K.] ） \\ y [K.] = H （ X [K.] 那 V. [K.] 那你_{m} [K.] ） \\ W. [K.] 〜（ 0. 那问： [K.] ） \\ V. [K.] 〜（ 0. 那 R. [K.] ） \end{array}$

ここでfは，タイムステップ间の状态Xのの化を记述する非形のの迁移迁移关键词。绕线のの关键词非，タイムステップK.でのXを测定値yに关键词。これらの关键，你_S.と你_mで表されるををもできでき。プロセスノイズおよび测定は，それぞれW.およびV.です.qとrはユーザーがします。

预测および正确的ののリファレンスページをを参照してて预测の前に正确的コマンドを装配するととと，アルゴリズムアルゴリズムは次のようにに実れ

状态状态の初値x [0]および状态推定误差の共列列列列列列を列列ししししし

$\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.}_{一种} | {K.}_{B.}]$ はタイムステップ0,1，...，k_B.での测定値を使使たタイムステップK._一种での状态推定をますます。 $\hat{X} [0. | - 1]$ は，测定を行うの状态値値な推定推定推定ですをを推定。
タイムステップk = 0,1,2,3，......についてについて，次の手顺を実ます。
1. 测定关节のヤコビアンを计算计算，测定测定Y [K]正确的コマンドがこの更新を実行します。
  
  $\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. 那你_{m} [K.] ） \\ \hat{P.} [K. | K.] = P. [K. | K. - 1] - K. [K.] C [K.] P. [K. | K. - 1] \end{array}$
  
  ここではK.はカルマンゲインです。
2. [预测コマンドコマンドがこの予测予测を実ししし
  
  $\begin{array}{l} 一种 [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.] = 一种 [K.] P. [K. | K.] 一种 [^{K.] T.} + G [K.] 问： [K.] G [^{K.] T.} \\ \hat{X} [K. + 1 | K.] = F （ \hat{X} [K. | K.] 那 0. 那你_{S.} [K.] ） \end{array}$
  
  关节正确的はこれらの値次のタイムステップで使使ます。数码値ををさせるため，共分类行为の平方英数分数分类ささますます。[2]を参照してください。

扩展卡尔曼筛选器ブロックは，复数の测定关键をの测定はいます。これらの测定は，サンプル时间が状态迁移のサンプルのの倍ば，异なるサンプル场场であれもつことができ。

前述の关アルゴリズムステップは，状态迁移关键相关有关部に想定さがある非想定性项项があるある场とおりとおりとおりとおりとおりとおりとおりとおりとおりとおりとおりとおり想定ノイズノイズノイズ项が想定想定项ががあるある想定想定さ项い项が想定想定さ性ノイズ项が想定想定ささ性ノイズとおりとおりとおりとおりとおりとおりとおりとおり场とおり

プロセスノイズW.が加法性である（つまり状态迁移方程式の形式が $X [K.] = F （ X [K. - 1] 那你_{S.} [K. - 1] ） + W. [K. - 1]$ である）综合，ヤコビ行列g [k]はは行。
测定ノイズV.が加法性性（つまりつまり方程式の形式が $y [K.] = H （ X [K.] 那你_{m} [K.] ） + V. [K.]$ である）综合，ヤコビ行列s [k]はは行。

[

一个次次张カルマンフィルター，非线形の状态迁移关键有关部の形近似结果て，システムの形性が着しいはアルゴリズム頼性低いはあります。フィルターアルゴリズムによって优れた结果得得られるられるられるがますます。

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

アンセンテッドカルマンフィルターアルゴリズムとUnscented Kalman Filter.ブロックは，アンセンテッドアンセンテッド変换使用し，非线关节による推定の统计の伝播を取得しますしししししししししし连これらこれらを生ポイントしこれらこれらの生シグマしますますこれらシグマポイントしますますシグマシグマポイントますますますシグマポイントポイントますますシグマポイントポイントはますシグマポイントポイントポイントポイントポイントポイントポイントポイントポイントポイントポイントポイントポイントポイントポイントの平均と共と共をしします。アルゴリズムアルゴリズム，各シグマポイントを状态迁移关键有关への入として使ててされ状态を取得ますますれ状态ををますますれ状态れた点ますます。と共分类は，状态推定および状态推定误差共共されするためされ。

M状态の离散时空非非システムの状态迁移方程式测定方程式には，ゼロ平等と共分类をもつ性のプロセスノイズおよび测定项qおよびrがそれぞれあると项ますしししますししますしますますしししますますますししますしますますますますますますしししししししますますしますししますますししますますし。

$\begin{array}{l} X [K. + 1] = F （ X [K.] 那你_{S.} [K.] ） + W. [K.] \\ y [K.] = H （ X [K.] 那你_{m} [K.] ） + V. [K.] \\ W. [K.] 〜（ 0. 那问： [K.] ） \\ V. [K.] 〜（ 0. 那 R. [K.] ） \end{array}$

Qおよびrのの初値値をアンセンテッドカルマンフィルターオブジェクトオブジェクトprocessnoise.および测量管理プロパティに指定します。

状态状态の初値x [0]および状态推定误差の共共でフィルターフィルターををます。

$\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.}_{一种} | {K.}_{B.}]$ はタイムステップ0,1，...，k_B.での测定値を使使たタイムステップK._一种での状态推定をますます。 $\hat{X} [0. | - 1]$ は，测定を行うの状态値値な推定推定推定ですをを推定。
各タイムステップKについてについて，测定测定Y [K]正确的コマンドがこの更新を実行します。
1. タイムステップKででのシグマシグマ ${\hat{X}}^{（一世）} [K. | K. - 1]$ を选択します。
  
  $\begin{array}{l} {\hat{X}}^{（ 0. ）} [K. | K. - 1] = \hat{X} [K. | K. - 1] \\ {\hat{X}}^{（一世）} [K. | K. - 1] = \hat{X} [K. | K. - 1] + δ. X^{（一世）} 一世 = 1 那 ...... 那 2 m \\ δ. X^{（一世）} = （ \sqrt{C P. [K. | K. - 1]} ） {}_{一世} 一世 = 1 那 ...... 那 m \\ δ. X^{（ m + 一世）} = - {（ \sqrt{C P. [K. | K. - 1]} ）}_{一世} 一世 = 1 那 ...... 那 m \end{array}$
  
  ここで $C = {α.}^{2} （ m + κ.. ）$ は，状态状态数とααおよびκに基极值です。パラメーターの详细について，アルファ，ベータ，カッパパラメーターの影响を参照してください。 $\sqrt{C P.}$ は， $\sqrt{C P.} {（ \sqrt{C P.} ）}^{T.} = C P.$ であり ${（ \sqrt{C P.} ）}_{一世}$ が $\sqrt{C P.}$ のi番目の列であるような，cpの行列の平方英です。
2. 非线形の测定关数を使用して，それぞれのシグマポイントについて予测される测定値を计算します。
  
  ${\hat{y}}^{（一世）} [K. | K. - 1] = H （ {\hat{X}}^{（一世）} [K. | K. - 1] 那你_{m} [K]）一世 = 0. 那 1 那 ...... 那 2 m$
3. 予测される测定値组みわせて，时间kの予测さ测定を取得します。
  
  $\begin{array}{l} \hat{y} [K.] = {σ.}_{一世 = 0.}^{2 m} {W.}_{m}^{（一世）} {\hat{y}}^{（一世）} [K. | K. - 1] \\ {W.}_{m}^{（ 0. ）} = 1 - \frac{m}{{α.}^{2} （ m + κ.. ）} \\ {W.}_{m}^{一世} = \frac{1}{2 {α.}^{2} （ m + κ.. ）} 一世 = 1 那 2 那 ...... 那 2 m \end{array}$
4. 予测される测定値の共共を推定しますをためノイズをするにr [k]を追加します。
  
  $\begin{array}{l} {P.}_{y} = {σ.}_{一世 = 0.}^{2 m} {W.}_{C}^{（一世）} （ {\hat{y}}^{（一世）} [K. | K. - 1] - \hat{y} [K.] ）（^{{\hat{y}}^{（一世）} [K. | K. - 1] - \hat{y} [K.] ） T.} + R. [K.] \\ {W.}_{C}^{（ 0. ）} = （ 2 - {α.}^{2} + β ） - \frac{m}{{α.}^{2} （ m + κ.. ）} \\ {W.}_{C}^{一世} = 1 / （ 2 {α.}^{2} （ m + κ.. ））一世 = 1 那 2 那 ...... 那 2 m \end{array}$
  
  βパラメーターパラメーターの详细について，アルファ，ベータ，カッパパラメーターの影响を参照してください。
5. $\hat{X} [K. | K. - 1]$ と $\hat{y} [K.]$ の间の相互共共をします。
  
  ${P.}_{X y} = \frac{1}{2 {α.}^{2} （ m + κ.. ）} {σ.}_{一世 = 1}^{2 m} （ {\hat{X}}^{（一世）} [K. | K. - 1] - \hat{X} [K. | K. - 1] ） {（ {\hat{y}}^{（一世）} [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.はカルマンゲインです。
，预测コマンドコマンドがこの予测予测を実ししし
1. タイムステップKででのシグマシグマ ${\hat{X}}^{（一世）} [K. | K.]$ を选択します。
  
  $\begin{array}{l} {\hat{X}}^{（ 0. ）} [K. | K.] = \hat{X} [K. | K.] \\ {\hat{X}}^{（一世）} [K. | K.] = \hat{X} [K. | K.] + δ. X^{（一世）} 一世 = 1 那 ...... 那 2 m \\ δ. X^{（一世）} = （ \sqrt{C P. [K. | K.]} ） {}_{一世} 一世 = 1 那 ...... 那 m \\ δ. X^{（ m + 一世）} = - {（ \sqrt{C P. [K. | K.]} ）}_{一世} 一世 = 1 那 ...... 那 m \end{array}$
2. 非线形の状态迁移关数を使用して，それぞれのシグマポイントについて予测される状态を计算します。
  
  ${\hat{X}}^{（一世）} [K. + 1 | K.] = F （ {\hat{X}}^{（一世）} [K. | K.] 那你_{S.} [K]）$
3. 予测される状态ををわせ，时间k + 1のされる状态取得し。正确的コマンドによって使使れます。
  
  $\begin{array}{l} \hat{X} [K. + 1 | K.] = {σ.}_{一世 = 0.}^{2 m} {W.}_{m}^{（一世）} {\hat{X}}^{（一世）} [K. + 1 | K.] \\ {W.}_{m}^{（ 0. ）} = 1 - \frac{m}{{α.}^{2} （ m + κ.. ）} \\ {W.}_{m}^{一世} = \frac{1}{2 {α.}^{2} （ m + κ.. ）} 一世 = 1 那 2 那 ...... 那 2 m \end{array}$
4. 予测される状态の共共をします。加法性プロセスノイズを考虑ためにq [k]を追加します。これらこれら値は，次のタイムステップで正确的コマンドによって使使れます。
  
  $\begin{array}{l} P. [K. + 1 | K.] = {σ.}_{一世 = 0.}^{2 m} {W.}_{C}^{（一世）} （ {\hat{X}}^{（一世）} [K. + 1 | K.] - \hat{X} [K. + 1 | K.] ） {（ {\hat{X}}^{（一世）} [K. + 1 | K.] - \hat{X} [K. + 1 | K.] ）}^{T.} + 问： [K.] \\ {W.}_{C}^{（ 0. ）} = （ 2 - {α.}^{2} + β ） - \frac{m}{{α.}^{2} （ m + κ.. ）} \\ {W.}_{C}^{一世} = 1 / （ 2 {α.}^{2} （ m + κ.. ））一世 = 1 那 2 那 ...... 那 2 m \end{array}$

Unscented Kalman Filter.ブロックは，复数の测定关键をの测定はいます。これらの测定は，サンプル时间が状态迁移のサンプルのの倍ば，异なるサンプル场场であれもつことができ。

以前のアルゴリズムは加法性ノイズ项を考虑てて状态方程式と测定ととにに装されれ。数码値ををさために，共分类行为の平方英阶段分类分类は，[2]を参照してください。

〖图库“

正确的コマンドは，p [k | k-1]およびr [k]をを用して2 *（m + v）+1シグマポイントを生成します。ここでは，V.は测定ノイズv [k]の要素の数号。附加のシグマポイントがPYの测定ノイズの影响影响を取得ため，r [k]项がアルゴリズムのステップ2（d）に追加されることはません。
预测コマンドはp [k | k]およびq [k]をを用して2 *（m + w）+1シグマポイントを生成しし。ここここW.は，プロセスノイズw [k]内内要素の数号。附加のシグマポイントがp [k + 1 | k]のプロセスノイズの影响影响を取得ため，q [k]项项アルゴリズムのステップ3（d）に追加されることありません。

アルファ，ベータ，カッパパラメーターの影响

.关键と关有关部へののとしてとしてて，新闻一般の変换さた状态ポイントを取得しますと共さた点のしますと共はははしと共はは，状态推定および状态误差のの共共使用されます。

平均のの状态値の周りシグマシグマの広がりはは制御のパラメーターおよびκκ制御されます33制御のれますます3制御のれますは3および测定共共散の计算に测定共れた点の変换されたた点変换変换れたた点影响し。

α - 平台のの状态値周りののシグマポイントのを决定ししこれ决定决定しますこれをさいの値値ですです比例の広がりはαししますししますますしししますししししますますししますししししますしししししししししししますししししししししししししししさいますますに近くなります。
κ - 2番目のスケーリングパラメーター。値値小さいほど，シグマポイントは平衡のにΚの平方英にκκますしκます。
β - 状态の分布のの情组み込みます。β= 2がです。

これらこれらのパラメーターををアンセンテッドフィルターフィルターΑ那kappa.那bet.分裂の単一ののみですシステム状态分布ににのピークあるある场てポイントのパラメーターを调整してポイントが単のピークの周りとどまるようにすることができことができたとえばたとえばようにすることができことができたとえばたとえばようにすることができことができたとえばたとえばようにすることができことができますたとえばたとえばようにすることができますたとえばたとえばようにするすることができますたとえばたとえばΑを选択して，平均の状态値に近いシグマポイントを生成します。

参照

[1]西蒙，丹。最优状态估计：卡尔曼，H无限远和非线性方法。Hoboken，NJ：John Wiley和Sons，2006。

[2] van der Merwe，Rudolph和Eric A. Wan。“Square-Root Uncented Kalman滤波器用于状态和参数估计。”2001 IEEE声学，语音和信号处理国际会议。诉讼程序（猫。No.01CH37221），6：3461-64。盐湖城，US，USA：IEEE，2001. HTTPS://Doi.org/10.1109/ICASSP.2001.940586。