Main Content

このページの翻訳は最新ではありません。ここをクリックして、英語の最新版を参照してください。

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

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

スクリプトを開く

この例では、Simulink® で時変カルマン フィルターを使用して線形システムの状態を推定する方法を説明します。Control System Toolbox™ ライブラリの Kalman Filter ブロックを使用して、GPS センサーの測定値などのノイズを含む位置測定値に基づき、地上車両の位置と速度を推定します。カルマン フィルターのプラント モデルには、時変ノイズ特性があります。

はじめに

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

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

この Simulink モデルは 2 つの主要部分、車両モデルとカルマン フィルターで構成されています。これらについて、以下の節で詳しく説明します。

open_system('ctrlKalmanNavigationExample');

車両モデル

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

$$ \frac{d}{dt} \left[
\begin{array} {c}
 x_e(t) \\
 x_n(t) \\
 s(t) \\
 \theta(t)
\end{array} \right] = \left[
\begin{array} {c}
 s(t)\cos(\theta(t)) \\
 s(t)\sin(\theta(t)) \\
 (P \; \frac{u_T(t)}{s(t)} - A \; C_d \; s(t)^2) / m \\
 s(t) \tan(u_\psi(t)) / L
\end{array} \right]
$$

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

$$ \begin{array} {ll}
x_e(t) \; & \textnormal{East position} \; [m] \\
x_n(t) \; & \textnormal{North position} \; [m] \\
s(t) \; & \textnormal{Speed} \; [m/s] \\
\theta(t) \; & \textnormal{Orientation from east} \; [deg] \\
\end{array} $$

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

$$ \begin{array} {ll}
P=100000 \; & \textnormal{Peak engine power} \; [W] \\
A=1 \; & \textnormal{Frontal area} \; [m^2] \\
C_d=0.3 \; & \textnormal{Drag coefficient} \; [Unitless] \\
m=1250 \; & \textnormal{Vehicle mass} \; [kg] \\
L=2.5 \; & \textnormal{Wheelbase length} \; [m] \\
\end{array} $$

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

$$ \begin{array} {ll}
u_T(t) \; & \textnormal{Throttle position in the range of -1 and 1} \; [Unitless] \\
u_\psi(t) \; & \textnormal{Steering angle} \; [deg] \\
\end{array} $$

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

車両モデルは ctrlKalmanNavigationExample/Vehicle Model サブシステムに実装されています。Simulink モデルには、ctrlKalmanNavigationExample/Speed And Orientation Tracking サブシステムで車両の目的の方向と速度を追跡するための 2 つの PI コントローラーが含まれています。これにより、車両にさまざまな操作条件を指定して、カルマン フィルターの性能をテストできます。

カルマン フィルターの設計

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

$$ \hat{x}[n] = \left[
 \begin{array}{c}
 \hat{x}_e[n] \\
 \hat{x}_n[n] \\
 \hat{\dot{x}}_e[n] \\
 \hat{\dot{x}}_n[n]
 \end{array}
 \right]
$$

ここで

$$ \begin{array} {ll}
\hat{x}_e[n] \; & \textnormal{East position estimate} \; [m] \\
\hat{x}_n[n] \; & \textnormal{North position estimate} \; [m] \\
\hat{\dot{x}}_e[n] \; & \textnormal{East velocity estimate} \; [m/s] \\
\hat{\dot{x}}_n[n] \; & \textnormal{North velocity estimate} \; [m/s] \\
\end{array} $$

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

$$ \begin{array} {rl}
\hat{x}[n+1] &= A\hat{x}[n] + Gw[n] \\
y[n] &= C\hat{x}[n] + v[n]
\end{array} $$

ここで、$\hat{x}$ は状態ベクトル、$y$ は測定値、$w$ はプロセス ノイズ、$v$ は測定ノイズです。カルマン フィルターでは、$w$ と $v$ はゼロ平均で、既知の分散 $E[ww^T]=Q$、$E[vv^T]=R$、および $E[wv^T]=N$ をもつ独立の確率変数であると仮定します。ここでは、行列 A、G および C は次のようになります。

$$ A = \left[
 \begin{array}{c c c c}
 1 & 0 & T_s & 0 \\
 0 & 1 & 0 & T_s \\
 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]
$$

$$ C = \left[
 \begin{array}{c c c c}
 1 & 0 & 0 & 0 \\
 0 & 1 & 0 & 0
 \end{array}
 \right]
$$

ここで、$T_s=1\;[s]$

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

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

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

$$f_{sat}(z)=min(max(z,25),625)$$

$$ Q[n] = \left[
 \begin{array}{ c c }
 \displaystyle 1+\frac{250}{f_{sat}(\hat{\dot{x}}_e^2)} & 0 \\
 0 & \displaystyle 1+\frac{250}{f_{sat}(\hat{\dot{x}}_n^2)}
 \end{array}
 \right]
$$

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

Kalman Filter ブロックの入力と設定

'Kalman Filter' ブロックは、Simulink の Control System Toolbox ライブラリにあります。また、System Identification Toolbox/Estimators ライブラリにもあります。離散時間の状態推定のブロック パラメーターを設定します。次の [フィルター設定] パラメーターを指定します。

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

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

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

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

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

[モデル パラメーター] をクリックして、プラント モデルとノイズの特性を指定します。

  • モデル ソース: 個々の行列 A、B、C、D。

  • A: A。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。プロセス ノイズは、Kalman Filter ブロックに入る測定値 y には影響しません。

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

  • R: R。これは測定ノイズ $v[n]$ の共分散です。R 行列はこの例内で既に定義されています。

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

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

結果

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

  • t = 0 において、車両は $x_e(0)=0$、$x_n(0)=0$ に位置し、静止している。

  • 東方向へ向かい、25 m/s まで加速する。t = 50s で 5 m/s に減速する。

  • t = 100s で、北に向きを変え、20 m/s まで加速する。

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

  • t = 260s で 15 m/s に減速し、一定速度で 180 度方向転換する。

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

sim('ctrlKalmanNavigationExample');

figure;
% Plot results and connect data points with a solid line.
plot(x(:,1),x(:,2),'bx',...
    y(:,1),y(:,2),'gd',...
    xhat(:,1),xhat(:,2),'ro',...
    'LineStyle','-');
title('Position');
xlabel('East [m]');
ylabel('North [m]');
legend('Actual','Measured','Kalman filter estimate','Location','Best');
axis tight;

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

% East position measurement error [m]
n_xe = y(:,1)-x(:,1);
% North position measurement error [m]
n_xn = y(:,2)-x(:,2);
% Kalman filter east position error [m]
e_xe = xhat(:,1)-x(:,1);
% Kalman filter north position error [m]
e_xn = xhat(:,2)-x(:,2);

figure;
% East Position Errors
subplot(2,1,1);
plot(t,n_xe,'g',t,e_xe,'r');
ylabel('Position Error - East [m]');
xlabel('Time [s]');
legend(sprintf('Meas: %.3f',norm(n_xe,1)/numel(n_xe)),...
    sprintf('Kalman f.: %.3f',norm(e_xe,1)/numel(e_xe)));
axis tight;
% North Position Errors
subplot(2,1,2);
plot(t,y(:,2)-x(:,2),'g',t,xhat(:,2)-x(:,2),'r');
ylabel('Position Error - North [m]');
xlabel('Time [s]');
legend(sprintf('Meas: %.3f',norm(n_xn,1)/numel(n_xn)),...
    sprintf('Kalman f: %.3f',norm(e_xn,1)/numel(e_xn)));
axis tight;

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

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

e_ve = xhat(:,3)-x(:,3); % [m/s] Kalman filter east velocity error
e_vn = xhat(:,4)-x(:,4); % [m/s] Kalman filter north velocity error
figure;
% Velocity in east direction and its estimate
subplot(2,1,1);
plot(t,x(:,3),'b',t,xhat(:,3),'r');
ylabel('Velocity - East [m/s]');
xlabel('Time [s]');
legend('Actual','Kalman filter','Location','Best');
axis tight;
subplot(2,1,2);
% Estimation error
plot(t,e_ve,'r');
ylabel('Velocity Error - East [m/s]');
xlabel('Time [s]');
legend(sprintf('Kalman filter: %.3f',norm(e_ve,1)/numel(e_ve)));
axis tight;

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

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

まとめ

Simulink で Kalman Filter ブロックを使用して車両の位置と速度を推定しました。モデルのプロセス ノイズ ダイナミクスは時変でした。さまざまな車両の操作とランダムに生成された測定ノイズをシミュレートして、フィルター性能を検証しました。カルマン フィルターにより、車両の位置の測定値と提供される速度の推定値が向上しました。

bdclose('ctrlKalmanNavigationExample');

参考

関連するトピック