拡張カルマン フィルター

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

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

メモ

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

状態更新モデル

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

$$x_{k+1}=f(x_k,u_k,w_k,t)$$

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

$$F^{(x)}=\frac{\partial f}{\partial x}.$$

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

$$F^{(w)}=\frac{\partial f}{\partial w}.$$

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

$$x_{k+1}=f(x_k,u_k,t)+w_k$$

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

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

測定モデル

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

$$z_k=h(x_k,v_k,t)$$

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

$$H^{(x)}=\frac{\partial h}{\partial x}.$$

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

$$H^{(v)}=\frac{\partial h}{\partial v}.$$

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

$$z_k=h(x_k,t)+v_k$$

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

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

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

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

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

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

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

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

ツールボックスには、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(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)")

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

初期状態を [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")

補助関数

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

参考

trackingKF | trackingEKF | trackingUKF | Linear Kalman Filters