複数のマルチレトセンサをも非線形システムの状態の推定

この例では次を使用します:

この例では,さまざまなサンプルレートで動作する複数のセンサーをもつシステムについて仿金宝app真软件™で非線形の状態推定を行う方法を示します。系统识别工具箱™ の Extended Kalman Filter ブロックは、GPS およびレーダーの測定値を使用してオブジェクトの位置と速度を推定するために使用されます。

はじめに

ルボックスには非線形の状態推定を行うためのSim金宝appulinkブロックが3あります。

扩展卡尔曼滤波器:1次離散時間の拡張カルマンフィルターアルゴリズムを実装します。
无味卡尔曼滤波:離散時間のアンセンテッドカルマンフィルターアルゴリズムを実装します。
粒子滤波:離散時間の粒子フィルタ，アルゴリズムを実装します。

これらのブロックは,さまざまなサンプルレートで動作する複数のセンサーを使用した状態推定をサポートします。これらのブロックの使用に関する一般的なワ，クフロ，は次のとおりです。

MATLABまたはSi金宝appmulink関数を使用してプラントとセンサ，の動作をモデル化します。
ブロックのパラメ，タ，を構成します。
フィルタ，をシミュレ，トして結果を解析し，フィルタ，の性能の信頼度を得ます。
ハ，ドウェアにフィルタ，を展開します。金宝appSimulink Coder™ソフトウェアを使用して，これらのフィルタ，のコ，ドを生成できます。

この例では,扩展卡尔曼滤波器ブロックを使用し,このワークフローの最初の2つの手順を示します。最後の2の手順にいては”次のステップの節で簡単に説明します。この例の目標は,レーダーおよびGPSセンサーから提供されたノイズを含む測定値を使用してオブジェクトの状態を推定することです。オブジェクトの状態とはその位置と速度であり,金宝app仿真软件モデル内ではxTrueで表されます。

粒子滤波ブロックに関心がある場合,“粒子滤波ブロックを使用した仿真软件でのパラメーターおよび状態の推定“の例を参金宝app照してください。

目录(fullfile (matlabroot,“例子”，“识别”，“主要”）)添加示例数据open_system (“multirateEKFExample”）;

プラントのモデル化

拡張カルマンフィルター(EKF)アルゴリズムには,タイムステップ間の状態の変化を記述する状態遷移関数が必要です。このブロックは次の2の関数形式をサポトしています。

加法性のプロセスノ电子邮箱ズ:
非加法性のプロセスノ电子邮箱ズ:

ここで，f(..)は状態遷移関数，xは状態，wはプロセスノescズです。Uはオプションで，fに対する追加入力を表しています(たとえば，システム入力やパラメタ)。加法性ノesc escズは，次の状態美元$ x [k + 1] とプロセスノ@ @ズ w [k]美元が線形的に関係していることを意味します。関係が非線形である場合は，非加法性の形式を使用します。

関数f(…)はMATLAB编码器™の制限に準拠するMATLAB関数か,仿真软件功能ブロックに金宝appすることができます.f(…)の作成後,関数名と,扩展卡尔曼滤波器ブロックのプロセスノイズが加法性と非加法性のどちらであるかを指定します。

この例では，北方向および東方向の位置と，オブジェクトの2次元平面における速度を追跡しています。推定された量は次のとおりです。

$\hat{x}[k] = \left[ c \开始{数组}{}& # xA;{x} \帽子_e [k] \ \ & # xA;{x} \帽子_n”[k] \ \ & # xA;{v} \帽子_e [k] \ \ & # xA;{v} \帽子_n”[k] & # xA;结束\{数组}& # xA;\右]& # xA;\; \begin{array} {ll} \textnormal{东位置估计}\;[m] \\ \textnormal{北方位置估计}\; [m] \\ \textnormal{East velocity estimate} \; [m/s] \\ \textnormal{North velocity estimate} \; [m/s] \\ \end{array}$

ここで k美元は離散時間@ @ンデックスです。使用される状態遷移方程式は非加法性形式 $\hat{x}[k+1] = A\hat{x}[k] + Gw[k]$ であり， $\帽子{x}$ は状態ベクトル， w美元はプロセスノ@ @ズです。フィルタ，では， w美元はゼロ平均で，既知の分散 E (ww ^ T)美元をも独立の確率変数であると仮定します。行列aおよび行列gは次のとおりです。

$A = \left[ \begin{数组}{c c c c} 1 & # 38岁;0 & # 38;T_s & # 38;0 xA \ \ & #;0 & # 38;1 & # 38岁;0 & # 38;T_s \ \ & # xA; 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{array} \right] \;\;\;\;\;\;\;\; G = \left[ \begin{array}{c c} T_s/2 & 0 \\ 0 & T_s/2 \\ 1 & 0 \\ 0 & 1 \end{array} \right]$

ここで T_s美元はサンプル時間になります。一个およびgの3行目は，ランダムウォク $$ {v} \帽子_e (k + 1) = {v} \帽子_e [k] + w_1 [k]美元$ として東方向の速度をモデル化しています。実際は，位置は連続時間変数であり，経時的速度 $$ \压裂{d} {dt} {x} _e = \ \帽子帽子{v} _e美元$ の積分となります。一个およびgの1行目は，この運動学的関係 $美元(\帽子{x} _e [k + 1] - \帽子{x} _e [k]) / T_s =(\帽子{v} _e [k + 1] + \帽子{v} _e [k]) / 2美元$ の離散近似を表します。Aおよびgの2行目と4行目は，北方向の速度と位置の間の同じ関係を表します。この状態遷移モデルは線形ですが，レ，ダ，測定値のモデルは非線形です。この非線形性のため，拡張カルマンフィルタ，などの非線形の状態推定器の使用が必要になります。

この例ではS金宝appimulink命令功能ブロックを使用して状態遷移関数を実装します。そのためには以下を行います。

金宝app仿真软件的功能ブロックを金宝app模型/用户定义函数ラ@ @ブラリからモデルに追加します。
金宝appSimulink命令功能ブロックに示された名前をクリックします。必要に応じて，関数名を編集し，入力引数と出力引数を追加または削除します。この例では状態遷移関数の名前はstateTransitionFcnです。1 . .の出力引数(xNext)と;2 . .の入力引数(xとw)があります。

この例では必要ありませんが,金宝app模型函数内で仿真软件モデルの残りの部分からの任意の信号を使用できます。そのためには，金宝app模型/来源ラ@ @ブラリから轮廓尺寸ブロックを追加します。これらは，関数のシグネチャを介して設定されるArgInブロックおよびArgOutブロックとは異なります(xNext = stateTransitionFcn(x, w))。
金宝app仿真软件功能ブロックで,仿真软件ブロックを使用して関数を作成します。
入力引数と出力引数x, wおよびxNextの次元を，ArgInブロックおよびArgOutブロックの[信号属性]タブで設定します。デ，タ型と端子の次元は，扩展卡尔曼滤波器ブロックで指定した情報と同じでなければなりません。

この例では状態遷移関数の解析ヤコビアンも実装されています。ヤコビアンの指定はオプションです。ただし，これによって計算負荷が軽減され，大半の場合は状態推定の精度を高めることができます。状態遷移関数は Simulink 関数であるため、ヤコビ関数を Simulink 関数として実装します。

open_system (multirateEKFExample/S金宝appimulink函数-状态转换雅可比矩阵）;

センサのモデル化—レダ

扩展卡尔曼滤波器ブロックには,状態と測定値の関係を記述する測定関数も必要です。次の2の関数形式がサポトされます。

加法性の測定ノ电子邮箱ズ:
非加法性の測定ノ电子邮箱ズ:

ここで，h(..)は測定関数，vは測定ノescズです。Uはオプションで，hに対する追加入力を表しています(たとえば，システム入力やパラメタ)。これらの入力は，状態遷移関数の入力とは異なるものを指定できます。

この例では原点に位置するレ，ダ，がオブジェクトの範囲と角度を20 Hzで測定します。両方の測定値が約5%のノaaplズを含むと仮定します。これは次の測定方程式によってモデル化できます。

$$ y_{雷达}[k] = \left[ c \开始{数组}{}& # xA;大概{x_n \ [k] ^ 2 + x_e [k] ^ 2} \;(1 + v_1 [k]) \ \ & # xA;Atan2 (x_n[k]， x_e[k]) \;(1 + v_2 [k]) \ \ & # xA;结束\{数组}& # xA;\右]& # xA; $$

ここで v_1 [k]美元と美元$ v_2 [k] は測定ノ▪▪ズ項で，それぞれが分散0.05^2をも▪▪ます。すなわ，測定値の大部分で誤差が5%未満になります。とは測定値に単純に加算されるのではなく状態xに依存するため，測定ノescズは非加法性です。この例では,レーダーの測定方程式が仿金宝app真软件功能ブロックを使用して実装されます。

open_system (multirateekf示例/Simuli金宝appnk函数-雷达测量）;

センサのモデル化- GPS

GPSは，オブジェクトの東方向および北方向の位置を1hzで測定します。したがって，gpsセンサ，の測定方程式は次のようになります。

$y_{GPS}[k] = \left[ c \开始{数组}{}& # xA;x_e [k] \ \ & # xA;x_n [k] & # xA;结束\{数组}& # xA;\右]+ & # xA;左\ [& # xA;c \开始{数组}{}& # xA;v_1 [k] \ \ & # xA;v_2 [k] & # xA; \end{array} \right]$

ここで v_1 [k]美元と美元$ v_2 [k] は，共分散行列[10^2 0;0 10^2]をも測定ノズ項です。したがって，測定値の誤差は約 10 メートルであり、誤差は無相関です。ノイズ項が測定値 $美元y_ {GPS} $$ に線形の影響を与えるので，この測定ノ。

この関数を作成し，gpsMeasurementFcn.mという名前のファ@ @ルに保存します。測定ノaapl . exeズが加法性の場合，関数内にノaapl . exeズ項を指定することはできません。この関数名と測定ノイズ共分散を,扩展卡尔曼滤波器ブロック内で提供します。

类型gpsMeasurementFcn

function y = gpsMeasurementFcn(x) % gpsMeasurementFcn状态估计GPS测量函数% %假设状态x为:% [EastPosition;NorthPosition;EastVelocity;上面的%#codegen标签是需要的，你想使用MATLAB Coder为你的过滤器y = x([1 2]) %生成C或c++代码;%位置状态测量结束

フィルタ，構築

推定を実行する扩展卡尔曼滤波器ブロックを設定します。状態遷移と測定の関数名、初期状態および状態誤差の共分散、プロセスノイズと測定ノイズの特性をそれぞれを指定します。

ブロックダ@ @アログの[システムモデル]タブで，次のパラメ，タ，を指定します。

状態遷移

[関数]に状態遷移関数stateTransitionFcnを指定します。この関数のヤコビアンがあるので，[ヤコビアン]を選択して，ヤコビ関数stateTransitionJacobianFcnを指定します。
プロセスノ电子邮箱ズが状態に与える影響を関数で明示的に宣言したので，[プロセスノ电子邮箱ズ]ドロップダウンリストで非相加を選択します。
プロセスノesc escズの共分散を[0.2 0;0 0.2]に指定します。この例の”プラントのモデル化“の節で説明されているように,プロセスノイズ項は各方向における速度のランダムウォークを定義します。対角項は,状態遷移関数の1つのサンプル時間において可能なおおよその速度の変化量を捉えるものです。非対角項はゼロに設定されます。これは北方向と東方向の速度の変化が無相関であるという単純な仮定を表します。

初期化

[初期状態]に最良の初期状態推定を指定します。この例では[100;100;0;0]を指定します。
[初期の共分散]に状態推定の信頼度を指定します。この例では10を指定します。ソフトウェアはこの値を，真の状態値がおそらく初期推定値から $下午\ \ sqrt{10} $美元$ 以内にあるとして解釈します。最初的协方差をベクトルとして設定することで，状態ごとに個別の値を指定できます。行列として指定すると，この不確かさの相互相関を指定できます。

センサが2あるので，[測定値の追加]をクリックして2番目の測定関数を指定します。

測定 1

[関数]に，測定関数の名前radarMeasurementFcnを指定します。
プロセスノ电子邮箱ズが状態に与える影響を関数で明示的に宣言したので，[測定ノ电子邮箱ズ]ドロップダウンリストで非相加を選択します。
「センサのモデル化—レダ“の節で説明されているように，測定ノ▪▪ズの共分散を[0.05^2 0;0 0.05^2]に指定します。

測定 2

[関数]に，測定関数の名前gpsMeasurementFcnを指定します。
このセンサモデルは加法性ノズを含みます。したがって，[測定ノ电子邮箱ズ]ドロップダウンリストでGPS測定ノ电子邮箱ズを添加剂に指定します。
測定ノesc escズ共分散を[100 0;0 100]に指定します。

2 .，[マルチレ，ト]タブで次の構成を行います。

启用多速率操作を選択します。
状態遷移のサンプル時間を指定します。状態遷移のサンプル時間は最小でなければならず、すべての測定サンプル時間は状態遷移サンプル時間の整数倍でなければなりません。[状態遷移]のサンプル時間を，最速測定のサンプル時間である0.05に指定します。この例では必要ありませんが,すべての測定値よりも小さいサンプル時間を状態遷移に指定することも可能です。その場合，測定値のないサンプル時間が発生する可能性があります。これらのサンプル時間にいてはフィルタが状態遷移関数を使って状態の予測を生成します。
[測定 1]のサンプル時間(レ，ダ，)を0.05秒，[測定 2](gps)を1秒に指定します。

シミュレ，ションと結果

オブジェクトが次の操縦方法によって正方形のパターンで移動するシナリオをシミュレートして,拡張カルマンフィルターの性能をテストします。

T = 0でオブジェクトが $美元x_e (0) = 100 \;\textnormal{[m]}， x_n(0)=100 \;\textnormal{[m]}$$ から開始します。
T = 20秒までは ${x} \点_n”= 50 \;\ textnormal {[m / s]}$ で北方向に移動します。
T = 20からT = 45秒の間は ${x} \点_n”= 40 \;\ textnormal {(m / s)}$ で東方向に移動します。
T = 45からT = 85秒の間は ${x} \点_n”= -25 \ \ textnormal {[m / s]}$ で南方向に移動します。
T = 85からT = 185秒の間は $\ \点{x} _e = -10美元;\ textnormal {[m / s]} $$ で西方向に移動します。

この動きに対応する真の状態値を生成します。

Ts = 0.05;% [s]真实状态的抽样率[t, xTrue] = generateTrueStates(Ts);生成0-185秒内的位置和速度剖面

モデルのシミュレ，ションを実行します。たとえば，東方向における実際の速度と推定速度を見てみましょう。

sim卡(“multirateEKFExample”）;open_system (' multirateekf示例/范围-东速度'）;

プロットには，東方向における真の速度と拡張カルマンフィルタ，による速度の推定値が表示されます。フィルタ，は速度の変化を正しく追跡しています。フィルタ，がマルチレ，トであることは，tが20 ~ 30秒の時間範囲で最も顕著です。フィルターは毎秒(GPSサンプルレート)ごとに大幅な修正を行いますが,レーダー測定による修正は0.05秒ごとに見られます。

次のステップ

状態推定の検証:アンセンテッドまたは拡張カルマンフィルターのパフォーマンス検証は,通常は幅広いモンテカルロ法シミュレーションを使用して実行されます。詳細にいては，金宝appSimulinkでのオンラeconlン状態推定の検証を参照してください。
コードの生成:无味卡尔曼滤波ブロックと扩展卡尔曼滤波器ブロックは,仿真软件编码器™ソフトウェアを使用したCおよびc++コードの生成をサ金宝appポートします。これらのブロックに提供する関数は,MATLAB编码器™ソフトウェア(MATLAB関数を使用してシステムをモデル化している場合)および仿真软件编码器ソ金宝appフトウェア(仿真软件函数ブロックを使用してシステムをモデル化している場合)の制限に準拠しなければなりません。

まとめ

この例では,系统辨识工具箱で扩展卡尔曼滤波器ブロックを使用する方法を示しました。ここでは別々のサンプリングレートで動作する2つの異なるセンサーからオブジェクトの位置と速度を推定しました。

close_system (“multirateEKFExample”, 0);rmpath (fullfile (matlabroot,“例子”，“识别”，“主要”）)删除示例数据

参考

扩展卡尔曼滤波器|无味卡尔曼滤波器|粒子滤波