ekfSLAM

拡張カルマン フィルターを使用した自己位置推定と環境地図作成の同時実行

R2021b 以降

説明

ekfSLAM オブジェクトは、拡張カルマン フィルター (EKF) を使用して自己位置推定と環境地図作成の同時実行 (SLAM) を行います。環境から観測されたランドマークを取り込んで既知のランドマークと比較し、関連付けと新しいランドマークを検出します。関連付けを使用して状態と状態の共分散を補正します。新しいランドマークは状態ベクトル内で拡張されます。

作成

構文

slamObj = ekfSLAM

slamObj = ekfSLAM(Name,Value)

slamObj = ekfSLAM('MaxNumLandmark',N,Name,Value)

slamObj = ekfSLAM('MaxNumLandmark',N,‘MaxNumPoseStored’,M,Name,Value)

説明

slamObj = ekfSLAM は、既定のプロパティを使用して EKF SLAM オブジェクトを作成します。

slamObj = ekfSLAM(Name,Value) は、前述した構文の入力引数の任意の組み合わせに加え、1 つ以上の名前と値のペアの引数を使用してプロパティを設定します。指定されていないプロパティはすべて既定値になります。

slamObj = ekfSLAM('MaxNumLandmark',N,Name,Value) は、コード生成時に状態ベクトルで受け入れ可能なランドマーク数の上限 N を指定します。このランドマーク数の制限は、コード生成時にのみ適用されます。

slamObj = ekfSLAM('MaxNumLandmark',N,‘MaxNumPoseStored’,M,Name,Value) は、コード生成時に状態ベクトルに含まれるランドマークの最大数 N と共に、姿勢履歴の最大サイズ M を指定します。これらの制限は、コード生成時にのみ適用されます。

プロパティ

オブジェクトを作成した後にプロパティ State、StateCovariance、StateTransitionFcn、および MaxNumLandmark の値を変更することはできません。これらのプロパティ値を既定値として設定するか、またはオブジェクトの作成時に設定します。

State — 状態ベクトル

[0; 0; 0] (既定値) | M 要素の列ベクトル

状態ベクトル。M 要素の列ベクトルとして指定します。

データ型: single | double

StateCovariance — 状態推定誤差の共分散

eye(3) (既定値) | M 行 M 列の行列

状態推定誤差の共分散。M 行 M 列の行列として指定します。M は状態ベクトル内の状態数です。

データ型: single | double

StateTransitionFcn — 状態遷移関数

nav.algs.velocityMotionModel (既定値) | 関数ハンドル

状態遷移関数。関数ハンドルとして指定します。この関数は、タイム ステップ k-1 における状態ベクトルからタイム ステップ k における状態ベクトルを計算します。この関数は、制御入力やタイム ステップ サイズなどの追加の入力パラメーターを取ることができます。

また、この関数は現在の姿勢とコントローラー入力に対するヤコビアンを計算します。指定しない場合、ヤコビアンは関数 predict の呼び出しごとに数値の差分を使用して計算されます。この計算によって処理時間が長くなり、数値が不正確になる場合があります。

この関数では非加法性のプロセス ノイズが考慮され、次のシグネチャが必要です。

[pose(k),jacPose,jacControl] = StateTransitionFcn(pose(k-1),controlInput,parameters)

• pose(k) は、時間 k における推定姿勢です。

• jacPose は、pose(k-1) に対する StateTransitionFcn のヤコビアンです。

• jacControl は、controlInput に対する StateTransitionFcn のヤコビアンです。

• controlInput は、状態を伝播するための入力です。

• parameters は、状態遷移関数に必要な追加の引数です。

データ型: function_handle

MeasurementFcn — 測定関数

nav.algs.rangeBearingMeasurement (既定値) | 関数ハンドル

測定関数。関数ハンドルとして指定します。この関数は、M 要素状態ベクトルに対する N 要素測定ベクトルを計算します。

また、この関数は現在の姿勢とランドマークの位置に対するヤコビアンを計算します。指定しない場合、ヤコビアンは関数 correct の呼び出しごとに数値の差分を使用して計算されます。この計算によって処理時間が長くなり、数値が不正確になる場合があります。

この関数では加法性の測定ノイズが考慮され、次のシグネチャが必要です。

[measurements(k),jacPose,jacLandmarks] = MeasurementFcn(pose(k),landmarks)

• pose(k) は、時間 k における推定姿勢です。

• measurements(k) は、時間 k における推定測定値です。

• landmarks は、ランドマークの位置です。

• jacPose は、pose(k) に対する MeasurementFcn のヤコビアンです。

• jacLandmarks は、landmarks に対する MeasurementFcn のヤコビアンです。

データ型: function_handle

InverseMeasurementFcn — 逆測定関数

nav.algs.rangeBearingInverseMeasurement (既定値) | 関数ハンドル

逆測定関数。関数ハンドルとして指定します。この関数は、ランドマークの位置を N 要素測定ベクトルに対する M 要素状態ベクトルとして計算します。

また、この関数は現在の姿勢と測定値に対するヤコビアンを計算します。指定しない場合、ヤコビアンは関数 correct の呼び出しごとに数値の差分を使用して計算されます。この計算によって処理時間が長くなり、数値が不正確になる場合があります。

この関数には次のシグネチャが必要です。

[landmarks(k),jacPose,jacMeasurements] = InverseMeasurementFcn(pose(k),measurements)

• pose(k) は、時間 k における推定姿勢です。

• landmarks(k) は、時間 k におけるランドマークの位置です。

• measurements は、時間 k における観測されたランドマークです。

• jacPose は、pose(k) に対する InverseMeasurementFcn のヤコビアンです。

• jacMeasurements は、measurements に対する InverseMeasurementFcn のヤコビアンです。

データ型: function_handle

DataAssociationFcn — データ関連付け関数

nav.algs.associateMaxLikelihood (既定値) | 関数ハンドル

データ関連付け関数。関数ハンドルとして指定します。この関数は、状態ベクトルで既に利用できるランドマークに測定値を関連付けます。この関数は追加の入力パラメーターを取ることができます。

この関数には次のシグネチャが必要です。

[associations,newLandmarks] = DataAssociationFcn(knownLandmarks,knownLandmarksCovariance,observedLandmarks,observedLandmarksCovariance,parameters)

• knownLandmarks は、マップ内の既知のランドマークです。

• knownLandmarksCovariance は、knownLandmarks の共分散です。

• observedLandmarks は、環境内で観測されたランドマークです。

• observedLandmarksCovariance は、observedLandmarks の共分散です。

• parameters は、必要な追加の引数です。

• associations は、knownLandmarks から observedLandmarks への関連付けのリストです。

• newLandmarks は、新しいランドマークと見なす observedLandmarks のインデックスです。

データ型: function_handle

ProcessNoise — プロセス ノイズの共分散

eye(2) (既定値) | W 行 W 列の行列

プロセス ノイズの共分散。W 行 W 列の行列として指定します。W はプロセス ノイズ項の数です。

データ型: single | double

MaxAssociationRange — 関連付けをチェックするランドマークの最大範囲

inf (既定値) | 正の整数

関連付けをチェックするランドマークの最大範囲。正の整数として指定します。

データ型: single | double

MaxNumLandmark — 状態ベクトル内のランドマークの最大数

inf (既定値) | 正の整数

状態ベクトル内のランドマークの最大数。正の整数として指定します。

データ型: single | double

MaxNumPoseStored — 姿勢履歴の最大サイズ

inf (既定値) | 正の整数

姿勢履歴の最大サイズ。正の整数として指定します。

データ型: single | double

オブジェクト関数

copy	Create deep copy of EKF SLAM object
correct	Correct state and state error covariance
landmarkInfo	Retrieve landmark information
poseHistory	Retrieve corrected and predicted pose history
predict	状態と状態誤差の共分散の予測
removeLandmark	Remove landmark from state vector
reset	Reset state and state estimation error covariance

例

拡張カルマン フィルターを使用したランドマーク SLAM の実行

ビークルの初期状態、ビークルの初期状態の共分散、プロセス ノイズの共分散、制御入力、タイム ステップ サイズ、測定値、測定共分散、および検証ゲートの値を含むレース トラックのデータ セットを読み込みます。

load("racetrackDataset.mat","initialState","initialStateCovariance", ...
     "processNoise","controllerInputs","timeStep", ...
     "measurements","measCovar","validationGate");

初期状態、初期状態の共分散、およびプロセス ノイズを指定して ekfSLAM オブジェクトを作成します。

ekfSlamObj = ekfSLAM("State",initialState, ...
                     "StateCovariance",initialStateCovariance, ...
                     "ProcessNoise",processNoise);

姿勢を格納する変数を初期化します。

storedPose = nan(size(controllerInputs,1)+1,3);
storedPose(1,:) = ekfSlamObj.State(1:3);

状態遷移関数の制御入力とタイム ステップ サイズを使用して状態を予測します。次に、データ関連付け関数の観測されたランドマーク、測定共分散、および検証ゲートのデータを使用して状態を補正します。

for count = 1:size(controllerInputs,1)
    % Predict the state
    predict(ekfSlamObj,controllerInputs(count,:),timeStep);
 
    % Get the landmarks in the environment
    observedLandmarks = measurements{count};
 
    % Correct the state
    if ~isempty(observedLandmarks)
        correct(ekfSlamObj,observedLandmarks, ...
                measCovar,validationGate);
    end
  
    % Log the estimated pose
    storedPose(count+1,:) = ekfSlamObj.State(1:3);
end

作成したマップを可視化します。

fig = figure;
figAx = axes(fig);
axis equal
grid minor
hold on
plot(figAx,storedPose(:,1),storedPose(:,2),"g.-")
landmarks = reshape(ekfSlamObj.State(4:end),2,[])';
plot(figAx,landmarks(:,1),landmarks(:,2),"m+")
plot(figAx,storedPose(1,1),storedPose(1,2),"k*")
plot(figAx,storedPose(end,1),storedPose(end,2),"rd")
legend("Robot trajectory","Landmarks","Start","End")

拡張機能

C/C++ コード生成
MATLAB® Coder™ を使用して C および C++ コードを生成します。

バージョン履歴

R2021b で導入

参考

オブジェクト

• lidarSLAM

関数

• copy | correct | landmarkInfo | poseHistory | predict | removeLandmark | reset

トピック

• EKF ベースのランドマーク SLAM