Main Content

このページの内容は最新ではありません。最新版の英語を参照するには、ここをクリックします。

拡張カルマン フィルター

フィルターを使用してオブジェクトを追跡する場合、一連の検出値または測定値を使用し、オブジェクトの運動モデルに基づいてオブジェクトの "状態" を推定します。運動モデルでの状態とは、オブジェクトの状態を表す数量 (位置、速度、加速度など) の集合です。拡張カルマン フィルター (trackingEKF) は、オブジェクトの運動が非線形状態方程式に従う場合、または測定が状態の非線形関数である場合に使用します。たとえば、オブジェクトの測定値が球面座標 (方位角、高度、範囲など) で表されるが、ターゲットの状態が直交座標で表される場合に拡張カルマン フィルターの使用を検討します。

拡張カルマンの定式化は、状態方程式および測定方程式の線形化に基づきます。線形化により、状態と状態の共分散をほぼ線形の形式で伝播できます。また、線形化には、状態方程式と測定方程式のヤコビアンが必要です。

メモ

推定システムが線形の場合は、線形カルマン フィルター (trackingKF) または拡張カルマン フィルター (trackingEKF) を使用してターゲットの状態を推定できます。システムが非線形の場合は、拡張カルマン フィルターまたはアンセンテッド カルマン フィルター (trackingUKF) などの非線形フィルターを使用する必要があります。

状態更新モデル

予測される状態の閉形式表現を、前の状態 xk、コントロール uk、ノイズ wk、および時間 t の関数と仮定します。

xk+1=f(xk,uk,wk,t)

前の状態から予測される状態のヤコビアンは、次のように偏微分により得られます。

F(x)=∂f∂x.

ノイズに関して予測される状態のヤコビアンは次のとおりです。

F(w)=∂f∂w.

状態更新方程式でノイズが加法的である場合、これらの関数はよりシンプルな形式になります。

xk+1=f(xk,uk,t)+wk

この場合、F(w) は単位行列です。

trackingEKF オブジェクトの StateTransitionJacobianFcn プロパティを使用して、状態のヤコビ行列を指定できます。このプロパティを指定しない場合、オブジェクトは数値の差分を使用してヤコビアンを計算しますが、精度が若干低くなり、計算時間が増加することがあります。

測定モデル

拡張カルマン フィルターでは、測定値も状態と測定ノイズの非線形関数にすることができます。

zk=h(xk,vk,t)

状態に対する測定のヤコビアンは次のとおりです。

H(x)=∂h∂x.

測定ノイズに対する測定のヤコビアンは次のとおりです。

H(v)=∂h∂v.

測定方程式でノイズが加法的である場合、これらの関数はよりシンプルな形式になります。

zk=h(xk,t)+vk

この場合、H(v) は単位行列です。

trackingEKF では、MeasurementJacobianFcn プロパティを使用して、測定のヤコビ行列を指定できます。このプロパティを指定しない場合、オブジェクトは数値の差分を使用してヤコビアンを計算しますが、精度が若干低くなり、計算時間が増加することがあります。

拡張カルマン フィルター ループ

拡張カルマン フィルター ループは、Linear Kalman Filtersのループとほぼ同じですが、以下が異なります。

  • フィルターは可能な限り、厳密に非線形の状態更新関数と測定関数を使用する。

  • 状態遷移マトリックスが状態のヤコビアンに置き換えられる。

  • 測定マトリックスが測定のヤコビアンに置き換えられる。

Extended Kalman Filter Equations

事前定義された拡張カルマン フィルター関数

ツールボックスには、trackingEKF で使用するために、事前定義された状態更新関数と測定関数が用意されています。

運動モデル関数名関数の目的状態表現
等速constvel等速状態更新モデル

  • 1-D — [x;vx]

  • 2-D — [x;vx;y;vy]

  • 3-D — [x;vx;y;vy;z;vz]

ここで、

  • x、y、z は、それぞれ x 方向、y 方向、z 方向の位置を表します。

  • vx、vy、vz は、それぞれ x 方向、y 方向、z 方向の速度を表します。

constveljac等速状態更新ヤコビ
cvmeas等速測定モデル
cvmeasjac等速測定ヤコビ
等加速度constacc等加速度状態更新モデル

  • 1-D — [x;vx;ax]

  • 2-D — [x;vx;ax;y;vy;ay]

  • 3-D — [x;vx;ax;y;vy;ay;z;vz;az]

ここで、

  • ax、ay、az は、それぞれ x 方向、y 方向、z 方向の加速度を表します。

constaccjac等加速度状態更新ヤコビ
cameas等加速度測定モデル
cameasjac等加速度測定ヤコビ
等角速度constturn等角速度状態更新モデル

  • 2-D — [x;vx;y;vy;omega]

  • 3-D — [x;vx;y;vy;omega;z;vz]

ここで、omega は角速度を表します。

constturnjac等角速度状態更新ヤコビ
ctmeas等角速度測定モデル
ctmeasjac等角速度測定ヤコビ

例: trackingEKF で角度と範囲の測定値を使用した 2-D ターゲットの状態推定

ライブ スクリプトを開く

推定モデルの初期化

次の初期位置と初期速度をもつターゲットが 2D で運動すると仮定します。シミュレーションはサンプル時間 0.2 秒で 20 秒間継続します。

rng(2022);    % For repeatable results
dt = 0.2;     % seconds
simTime = 20; % seconds
tspan = 0:dt:simTime;
trueInitialState = [30; 1; 40; 1]; % [x;vx;y;vy]
initialCovariance = diag([100,1e3,100,1e3]);
processNoise = diag([0; .01; 0; .01]); % Process noise matrix

測定値は正の x 方向を基準とした方位角であり、範囲は原点からターゲットまでと仮定します。測定ノイズの共分散行列は次のとおりです。

measureNoise = diag([2e-6;1]); % Measurement noise matrix. Units are m^2 and rad^2.

結果の保存先の変数を事前割り当てします。

numSteps = length(tspan);
trueStates = NaN(4,numSteps);
trueStates(:,1) = trueInitialState;
estimateStates = NaN(size(trueStates));
measurements = NaN(2,numSteps);

実際の状態と測定値の取得

等速モデルを伝播して、ノイズを含む測定値を生成します。 

for i = 2:length(tspan)
    if i ~= 1
        trueStates(:,i) = stateModel(trueStates(:,i-1),dt) + sqrt(processNoise)*randn(4,1);  
    end
    measurements(:,i) = measureModel(trueStates(:,i)) + sqrt(measureNoise)*randn(2,1);
end

実際の軌跡と測定値をプロットします。

figure(1)
plot(trueStates(1,1),trueStates(3,1),"r*",DisplayName="Initial Truth")
hold on
plot(trueStates(1,:),trueStates(3,:),"r",DisplayName="True Trajectory")
xlabel("x (m)")
ylabel("y (m)")
title("True Trajectory")
axis square

Figure contains an axes object. The axes object with title True Trajectory, xlabel x (m), ylabel y (m) contains 2 objects of type line. One or more of the lines displays its values using only markers These objects represent Initial Truth, True Trajectory.

figure(2)
subplot(2,1,1)
plot(tspan,measurements(1,:)*180/pi)
xlabel("time (s)")
ylabel("angle (deg)")
title("Angle and Range")
subplot(2,1,2)
plot(tspan,measurements(2,:))
xlabel("time (s)")
ylabel("range (m)")

Figure contains 2 axes objects. Axes object 1 with title Angle and Range, xlabel time (s), ylabel angle (deg) contains an object of type line. Axes object 2 with xlabel time (s), ylabel range (m) contains an object of type line.

拡張カルマン フィルターの初期化

初期状態を [35; 0; 45; 0] と推定して、フィルターを初期化します。

filter = trackingEKF(State=[35; 0; 45; 0],StateCovariance=initialCovariance, ...
    StateTransitionFcn=@stateModel,ProcessNoise=processNoise, ...
    MeasurementFcn=@measureModel,MeasurementNoise=measureNoise);
estimateStates(:,1) = filter.State;

拡張カルマン フィルターの実行と結果の表示

オブジェクト関数 predict および correct を再帰的に呼び出して、フィルターを実行します。 

for i=2:length(tspan)
    predict(filter,dt);
    estimateStates(:,i) = correct(filter,measurements(:,i));
end
figure(1)
plot(estimateStates(1,1),estimateStates(3,1),"g*",DisplayName="Initial Estimate")
plot(estimateStates(1,:),estimateStates(3,:),"g",DisplayName="Estimated Trajectory")
legend(Location="northwest")
title("True Trajectory vs Estimated Trajectory")

Figure contains an axes object. The axes object with title True Trajectory vs Estimated Trajectory, xlabel x (m), ylabel y (m) contains 4 objects of type line. One or more of the lines displays its values using only markers These objects represent Initial Truth, True Trajectory, Initial Estimate, Estimated Trajectory.

補助関数

stateModel は、プロセス ノイズなしの等速運動をモデル化したものです。

function stateNext = stateModel(state,dt)
    F = [1 dt 0  0; 
         0  1 0  0;
         0  0 1 dt;
         0  0 0  1];
    stateNext = F*state;
end

meausreModel は、ノイズなしの範囲と方位角の測定値をモデル化します。

function z = measureModel(state)
    angle = atan(state(3)/state(1));
    range = norm([state(1) state(3)]);
    z = [angle;range];
end

参考

| | |