時変のカルマンフィルタ，を使用した状態推定

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

この例では,金宝app仿真软件で時変カルマンフィルターを使用して線形システムの状態を推定する方法を説明します。控制系统工具箱ライブラリの Kalman Filter ブロックを使用して、GPS センサーの測定値などのノイズを含む位置測定に基づき、地上車両の位置と速度を推定します。カルマンフィルターのプラントモデルには、時変ノイズ特性があります。

はじめに

北方向と東方向における地上車両の位置と速度を推測します。車両は，制約なしで2次元空間を自由に移動できます。車両だけではなく任意の目的で使用できる多目的のナビゲーションおよびトラッキングシステムを設計します。

とは，出発地からの車両の東方向および北方向の位置， $\θ(t)美元$ は東方向を基準とした車両の向き， $u_ \ psi (t)美元$ は車両のステアリング角度を表します。は連続時間変数です。

この仿金宝app真软件モデルは2つの主要部分,車両モデルとカルマンフィルターで構成されています。これらにいて，以下の節で詳しく説明します。

open_system (“ctrlKalmanNavigationExample”）;

車両モデル

追跡される車両は，次のようなシンプルな質点モデルで表されます。

$$ \压裂{d} {dt} \离开[& # xA; \开始{数组}{c} & # xA;x_e (t) \ \ & # xA;x_n (t) \ \ & # xA;s (t) \ \ & # xA;\θ(t) & # xA; \{数组}\]= \左右结束[& # xA; \开始{数组}{c} & # xA;s (t) \ cosθ(t)) (\ \ \ & # xA;s (t) \ sinθ(t)) (\ \ \ & # xA;(P \;\frac{u_T(t)}{s(t)} - A \;重金属镉\; s(t)^2) / m \\ s(t) \tan(u_\psi(t)) / L \end{array} \right] $$

この車両の状態は次のようになります。

$\begin{array} {ll} x_e(t) \;& # 38;\textnormal{东位置}\;[m] \\ x_n(t) \;& # 38;\textnormal{北面位置}\;[m] \\ s(t) \;& # 38;\ textnormal{速度}\;[m/s] \\ \theta(t) \; & \textnormal{Orientation from east} \; [deg] \\ \end{array}$

車両パラメ，タ，は次のようになります。

$\begin{array} {ll} P=100000 \;& # 38;\textnormal{引擎峰值功率}\;[W] A=1 \;& # 38;\textnormal{前额区域}\;[m^2] \\ C_d=0.3 \;& # 38;\textnormal{阻力系数}\;[无单位]\\ m=1250 \; & \textnormal{Vehicle mass} \; [kg] \\ L=2.5 \; & \textnormal{Wheelbase length} \; [m] \\ \end{array}$

制御入力は次のようになります。

$\begin{array} {ll} u_T(t) \;& # 38;\textnormal{节流阀的位置在-1和1}\;[无单位]\\ u_\psi(t) \;& # 38;\textnormal{转向角度}\;[deg] \\ \end{array}$

モデルの前後方向のダaapl . exeナミクスでは，タaapl . exeヤの転がり抵抗が無視されます。モデルの横方向のダイナミクスでは,目的のステアリング角度を瞬時に達成することができ,慣性のヨーモーメントは無視されるものとします。

車両モデルはctrlKalmanNavigationExample /车辆模型サブシステムに実装されています。金宝appSimulinkモデルには，ctrlKalmanNavigationExample/速度和方向跟踪サブシステムで車両の目的の方向と速度を追跡するための2つのπコントローラーが含まれています。これにより，車両にさまざまな操作条件を指定して，カルマンフィルタ，の性能をテストできます。

カルマンフィルタ，設計

カルマンフィルタ，は，線形モデルに基づいて対象の未知の変数を推定するアルゴリズムです。この線形モデルは,モデルの初期条件と既知および未知のモデル入力に応じて経時的に推定された変数の変化を表します。この例では，次のパラメ，タ，/変数を推定します。

$$ \hat{x}[n] = \left[ c \开始{数组}{}& # xA;{x} \帽子_e [n] \ \ & # xA;{x} \帽子_n”[n] \ \ & # xA;{\ \帽子点{x}} _e [n] \ \ & # xA;{\ \帽子点{x}} _n”[n] & # xA;结束\{数组}& # xA;\右]& # xA; $$

ここで，

$\begin{array} {ll} \hat{x}_e[n] \;& # 38;\textnormal{东位置估计}\;[m] \\ \hat{x}_n[n] \;& # 38;\textnormal{北方位置估计}\;[m] \\ \hat{\dot{x}}_e[n] \;& # 38;\textnormal{东速度估计}\;[m/s] \\ \hat{\dot{x}}_n[n] \; & \textnormal{North velocity estimate} \; [m/s] \\ \end{array}$

$\点{x}$ 項は微分演算子ではなく，速度を表します。は離散時間@ @ンデックスです。カルマンフィルタ，で使用されるモデルは，次の形式になります。

$数组$ $ \开始{}{rl} & # xA; \帽子{x} (n + 1) & # 38; = \帽子{x} [n] + Gw [n] \ \ & # xA; y [n] & # 38; = C \帽子{x} [n] + v [n] & # xA; \{数组}$ $$

ここで， $\帽子{x}$ は状態ベクトル，は測定値，はプロセスノ电子邮箱ズ，は測定ノ@ @ズです。カルマンフィルタ，では，とはゼロ平均で，既知の分散、，およびをも独立の確率変数であると仮定します。ここでは，行列a, gおよびcは次のようになります。

$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[ 开始\{数组}{c c} & # xA;T_s / 2 & # 38;0 xA \ \ & #;0 & # 38;T_s / 2 \ \ & # xA;1 & # 38岁;0 xA \ \ & #;0 & # 38;1 & # xA; \end{array} \right]$

$C = \left[ \begin{数组}{c c c c} 1 & # 38岁;0 & # 38;0 & # 38;0 xA \ \ & #;0 & # 38;1 & # 38岁;0 & # 38;0 xA & #; \end{array} \right]$

ここで， $美元$ T_s = 1 \; [s]$

一个およびgの3行目は，ランダムウォク $$ \帽子{\点{x}} _e (n + 1) ={\点{x}} \帽子_e [n] + w_1美元[n]$ として東方向の速度をモデル化しています。実際は，位置は連続時間変数であり，経時的速度 $$ \压裂{d} {dt} {x} _e = \ \帽子帽子{\点{x}} _e美元$ の積分となります。一个およびGの 1 行目は、この運動学的関係の離散近似 $美元(\帽子{x} _e (n + 1) - \帽子{x} _e [n]) / Ts =(\帽子{\点{x}} _e (n + 1) + \帽子{\点{x}} _e [n]) / 2美元$ を表します。一个およびGの 2 行目と 4 行目は、北方向の速度と位置の間の同じ関係を表します。

C行列は，位置の測定値のみが使用できることを表します。GPS などの位置センサーは、サンプルレート 1Hz でこれらの測定値を提供します。測定ノイズの分散(r行列)は，として指定されます。Rはスカラーとして指定されるため、Kalman Filter ブロックでは、R は対角で、その対角要素は 50 とし、y と互換性のある次元をもつ行列とします。測定ノイズがガウスである場合、R=50 は、東方向および北方向の実際の位置に対して、位置の測定値の 68% が $下午\ \ sqrt{50} \美元;m美元$ 以内となることに相当します。ただし，この仮定はカルマンフィルタ，には不要です。

の要素は，単一のサンプル時間Tsで車両の速度がどの程度変化し得るかを捉えます。プロセスノesc esc esc esc esc esc esc esc esc esc escこれは，速度が大きくなるとの通常の値は小さくなるという直感的概念を捉えます。たとえば0 m / sから10 m / sにすることは,10 m / sから20 m / sにするより簡単です。具体的には，推定された北方向および東方向の速度と飽和関数を使用してQ[n]を作成します。

$$ Q[n] = \left[ \begin{数组}{c c} \ displaystyle 1 + \压裂{250}{f{坐在}(\帽子{\点{x}} _e ^ 2)} & # 38;0 xA \ \ & #;0 & # 38;\ displaystyle 1 + \压裂{250}{f{坐在}({\点{x}} \帽子_n”^ 2)}& # xA;结束\{数组}& # xA;\右]& # xA; $$

Qの対角要素は，wの分散をモデル化します。飽和関数は，qが大きくなりすぎたり，小さくなりすぎたりするのを防ぎます。は係数250,一般的な車両の0 ~ 5,5 ~ 10,10 ~ 15日,15 ~ 20,20 ~ 25米/秒の加速度時のデータに対する最小二乗適合から取得したものです。対角问の選択は,北方向と東方向の速度の変化が無相関であるという単純な仮定を表すことに注意してください。

卡尔曼滤波器ブロックの入力と設定

“卡尔曼滤波器”ブロックは，Simulink金宝appの控制系统工具箱ラ@ @ブラリにあります。また，系统识别工具箱/估计器ラ@ @ブラリにもあります。離散時間の状態推定のブロックパラメ，タ，を設定します。次の[フィルタ，設定]パラメ，タ，を指定します。

時間領域:離散時間。離散時間の状態を推定するには，このオプションを選択します。

[現在の測定値y[n]を使用してxhat[n]を改善]チェックボックスをオンにします。これにより，離散時間のカルマンフィルタ，の“現在の推定器”バリアントが実装されます。このオプションにより，推定の精度が向上し，低速なサンプル時間の場合は特に役立ます。ただし，計算コストが増加します。さらに,このカルマンフィルターバリアントには直達があり,遅延を含まないフィードバックループでカルマンフィルターを使用すると,代数ループになります(フィードバックループそのものにも直達があります)。代数ル，プは，シミュレ，ションの速度にさらに影響する場合があります。

[オプション]タブをクリックし，ブロックの入力端子および出力端子オプションを設定します。

[入力端子uの追加]チェックボックスはオフにします。プラントモデルに既知の入力はありません。

[状態推定誤差の共分散zの出力]チェックボックスをオンにします。Z行列は，状態推定におけるフィルタ，の信頼性に関する情報を提供します。

[モデルパラメ，タ，]をクリックして，プラントモデルとノ。

モデルソ，ス:個々の行列a, b, c, d。

一个: a。一个行列は、この例内で既に定義されています。

C: c。C行列はこの例内で既に定義されています。

初期推定のソ，ス:ダ化学键アログ。

初期状態x[0]：0。これは，t=0sにおける位置と速度の推定値に対する0の初期推定を表します。

状態推定誤差の共分散p [0]：10。初期推定x[0]とその実際の値との間の誤差が標準偏差 $\ sqrt {10}$ をも確率変数であると仮定します。

既定以外のg行列を指定するには，[g行列とh行列を使用(既定値g = i, h =0)]チェックボックスをオンにします。

G: g。G行列はこの例内で既に定義されています。

H：0。プロセスノaapl . aapl .ズは，卡尔曼滤波ブロックに入る測定値yには影響しません。

[時不変q]チェックボックスをオフにします。Q行列は時変で，ブロック入力端子Qにより提供されます。この設定により，ブロックでは時変カルマンフィルタ，が使用されます。このオプションは，時不変カルマンフィルタ，を使用する場合に選択できます。時不変カルマンフィルターは,この問題では若干性能が低下しますが,設計が簡単で,計算コストが削減されます。

R: r。これは測定ノ@ @ズの共分散です。R行列はこの例内で既に定義されています。

N：0。プロセスと測定ノ@ @ズとの間には相関がないものと仮定します。

サンプル時間(継承は-1):この例内で既に定義されているTs。

結果

車両を次のように操作するシナリオをシミュレ，トして，カルマンフィルタ，の性能をテストします。

T = 0において，車両は、に位置し，静止している。

東方向へ向かい，25 m/sまで加速する。T = 50米で5米/秒に減速する。

T = 100秒で，北に向きを変え，20米/秒まで加速する。

T = 200sで，今度は西に向きを変える。25 m/sまで加速する。

T = 260秒で15米/秒に減速し，一定速度で180度方向転換する。

金宝appSimulinkモデルをシミュレ，ションします。車両の実際の位置，測定位置およびカルマンフィルタ，推定値をプロットします。

sim卡(“ctrlKalmanNavigationExample”）;图;绘制结果并用实线连接数据点。情节(x (: 1) x (:, 2),“软”，.．.(: 1), y (:, 2),“gd”，.．.xhat (: 1), xhat (:, 2),“罗”，.．.“线型”，“- - -”）;标题(“位置”）;包含(“东[m]”）;ylabel (“北[m]”）;传奇(“实际”，“测量”，“卡尔曼滤波估计”，“位置”，“最佳”）;轴紧；

測定位置と実際の位置との誤差およびカルマンフィルター推定値と実際の位置との誤差は次のようになります。

%东位测量误差[m]N_xe = y(:，1)-x(:，1);%北位测量误差[m]n = y(:，2)-x(:，2);%卡尔曼滤波东位置误差[m]E_xe = xhat(:，1)-x(:，1);%卡尔曼滤波北位置误差[m]E_xn = xhat(:，2)-x(:，2);图;%东位置误差次要情节(2,1,1);情节(t, n_xe‘g’t e_xe“r”）;ylabel (位置误差-东[m]）;包含(“时间[s]”）;传奇(sprintf (“量:% .3f”规范(n_xe 1) /元素个数(n_xe)), sprintf (“卡尔曼f.: %.3f”规范(e_xe 1) /元素个数(e_xe)));轴紧；%北位置误差次要情节(2,1,2);情节(t、y (:, 2) - x (:, 2),‘g’t xhat (:, 2) - x (:, 2),“r”）;ylabel (位置误差-北[m]）;包含(“时间[s]”）;传奇(sprintf (“量:% .3f”规范(n_xn 1) /元素个数(n_xn)), sprintf (“卡尔曼f: %.3f”规范(e_xn 1) /元素个数(e_xn)));轴紧；

プロットの凡例には、デ、タポ、ント数で正規化された位置の測定値と推定誤差( $$ | | x_e - \帽子{x} _e | | _1美元$ および $$ | | x_n - \帽子{x} _n”| | _1美元$ )が示されています。カルマンフィルタ，推定値は，生の測定値に比べ誤差の割合が約25%低くなっています。

以下の上部のプロットには,東方向における実際の速度とそのカルマンフィルター推定値が表示されています。下部のプロットには，推定誤差が示されています。

E_ve = xhat(:，3)-x(:，3);% [m/s]卡尔曼滤波东速度误差E_vn = xhat(:，4)-x(:，4);% [m/s]卡尔曼滤波北速度误差图;%东方向速度及其估计次要情节(2,1,1);情节(t) x (:, 3),“b”t xhat (:, 3),“r”）;ylabel (“速度-东[m]”）;包含(“时间[s]”）;传奇(“实际”，卡尔曼滤波器的，“位置”，“最佳”）;轴紧；次要情节(2,1,2);估计误差%情节(t, e_ve“r”）;ylabel (速度误差-东[m]）;包含(“时间[s]”）;传奇(sprintf ('卡尔曼滤波器:%.3f'规范(e_ve 1) /元素个数(e_ve)));轴紧；

誤差プロットの凡例には，デ，タポ，ント数で正規化された東方向の速度推定誤差 $$ | | \点{x} _e - \帽子{\点{x}} _e | | _1美元$ が示されています。

カルマンフィルタ，速度推定では，実際の速度のトレンドが正しく追跡されています。車両が高速で移動しているときは，ノ。これは，q行列の設計と一致しています。2 .の大きいスパ▪▪クがt=50sとt=200sにあります。これらはそれぞれ，車両が突然減速した時点と急旋回した時点を表しています。これらの時点における速度の変化は,问行列入力に基づいているカルマンフィルターの予測値よりも大幅に大きくなっています。いく。

まとめ

金宝app仿真软件で卡尔曼滤波器ブロックを使用して車両の位置と速度を推定しました。モデルのプロセスノ▪▪ズダ▪▪ナミクスは時変でした。さまざまな車両の操作とランダムに生成された測定ノイズをシミュレートして,フィルター性能を検証しました。カルマンフィルタ，により，車両の位置の測定値と提供される速度の推定値が向上しました。

bdclose (“ctrlKalmanNavigationExample”）;

時変のカルマンフィルタ，を使用した状態推定

はじめに

車両モデル

カルマンフィルタ，設計

卡尔曼滤波器ブロックの入力と設定

結果

まとめ

関連するトピック

控制系统工具箱ドキュメンテ，ション

サポト

学习如何自动整定PID控制器增益