このページの内容は最新ではありません。最新版の英語を参照するには、ここをクリックします。
拡張カルマン フィルター
フィルターを使用してオブジェクトを追跡する場合、一連の検出値または測定値を使用し、オブジェクトの運動モデルに基づいてオブジェクトの "状態" を推定します。運動モデルでの状態とは、オブジェクトの状態を表す数量 (位置、速度、加速度など) の集合です。拡張カルマン フィルター (trackingEKF
) は、オブジェクトの運動が非線形状態方程式に従う場合、または測定が状態の非線形関数である場合に使用します。たとえば、オブジェクトの測定値が球面座標 (方位角、高度、範囲など) で表されるが、ターゲットの状態が直交座標で表される場合に拡張カルマン フィルターの使用を検討します。
拡張カルマンの定式化は、状態方程式および測定方程式の線形化に基づきます。線形化により、状態と状態の共分散をほぼ線形の形式で伝播できます。また、線形化には、状態方程式と測定方程式のヤコビアンが必要です。
メモ
推定システムが線形の場合は、線形カルマン フィルター (trackingKF
) または拡張カルマン フィルター (trackingEKF
) を使用してターゲットの状態を推定できます。システムが非線形の場合は、拡張カルマン フィルターまたはアンセンテッド カルマン フィルター (trackingUKF
) などの非線形フィルターを使用する必要があります。
状態更新モデル
予測される状態の閉形式表現を、前の状態 xk、コントロール uk、ノイズ wk、および時間 t の関数と仮定します。
前の状態から予測される状態のヤコビアンは、次のように偏微分により得られます。
ノイズに関して予測される状態のヤコビアンは次のとおりです。
状態更新方程式でノイズが加法的である場合、これらの関数はよりシンプルな形式になります。
この場合、F(w) は単位行列です。
trackingEKF
オブジェクトの StateTransitionJacobianFcn
プロパティを使用して、状態のヤコビ行列を指定できます。このプロパティを指定しない場合、オブジェクトは数値の差分を使用してヤコビアンを計算しますが、精度が若干低くなり、計算時間が増加することがあります。
測定モデル
拡張カルマン フィルターでは、測定値も状態と測定ノイズの非線形関数にすることができます。
状態に対する測定のヤコビアンは次のとおりです。
測定ノイズに対する測定のヤコビアンは次のとおりです。
測定方程式でノイズが加法的である場合、これらの関数はよりシンプルな形式になります。
この場合、H(v) は単位行列です。
trackingEKF
では、MeasurementJacobianFcn
プロパティを使用して、測定のヤコビ行列を指定できます。このプロパティを指定しない場合、オブジェクトは数値の差分を使用してヤコビアンを計算しますが、精度が若干低くなり、計算時間が増加することがあります。
拡張カルマン フィルター ループ
拡張カルマン フィルター ループは、Linear Kalman Filtersのループとほぼ同じですが、以下が異なります。
フィルターは可能な限り、厳密に非線形の状態更新関数と測定関数を使用する。
状態遷移マトリックスが状態のヤコビアンに置き換えられる。
測定マトリックスが測定のヤコビアンに置き換えられる。
事前定義された拡張カルマン フィルター関数
ツールボックスには、trackingEKF
で使用するために、事前定義された状態更新関数と測定関数が用意されています。
運動モデル | 関数名 | 関数の目的 | 状態表現 |
---|---|---|---|
等速 | constvel | 等速状態更新モデル |
ここで、
|
constveljac | 等速状態更新ヤコビ | ||
cvmeas | 等速測定モデル | ||
cvmeasjac | 等速測定ヤコビ | ||
等加速度 | constacc | 等加速度状態更新モデル |
ここで、
|
constaccjac | 等加速度状態更新ヤコビ | ||
cameas | 等加速度測定モデル | ||
cameasjac | 等加速度測定ヤコビ | ||
等角速度 | constturn | 等角速度状態更新モデル |
ここで、 |
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