predict

状態と状態誤差の共分散の予測

R2021b 以降

構文

predict(slamObj,controlInput)

predict(slamObj,controlInput,varargin)

説明

predict(slamObj,controlInput) は、状態と状態誤差の共分散を予測します。predict は、ekfSLAM オブジェクト slamObj の StateTransitionFcn プロパティとコントローラー入力 controlInput を使用して状態を予測します。

predict(slamObj,controlInput,varargin) は、varargin で指定されたすべての追加の引数を slamObj の基となる StateTransitionFcn プロパティに渡します。

StateTransitionFcn への最初の入力は前のタイム ステップからの姿勢で、その後に varargin のすべてのユーザー定義引数が続きます。

例

拡張カルマン フィルターを使用したランドマーク 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")

入力引数

slamObj — EKF SLAM オブジェクト

ekfSLAM オブジェクト

EKF SLAM オブジェクト。ekfSLAM オブジェクトとして指定します。

controlInput — 状態の伝播に必要なコントローラー入力

N 要素ベクトル

状態の初期値から最終値への伝播に必要なコントローラー入力。N 要素ベクトルとして指定します。

メモ

プロセス ノイズの次元は、controlInput の要素数と等しい必要があります。

データ型: single | double

varargin — 可変長の入力引数のリスト

コンマ区切りリスト

可変長の入力引数のリスト。コンマ区切りリストとして指定します。この入力は、状態を変化させるために slamObj の StateTransitionFcn プロパティに直接渡されます。次を呼び出した場合、

predict(slamObj,controlInput,arg1,arg2)

MATLAB^® は実質的に stateTransitionFcn を次のように呼び出していることになります。

stateTransitionFcn(pose(k-1),controlInput,arg1,arg2)

拡張機能

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

バージョン履歴

R2021b で導入

参考

オブジェクト

• ekfSLAM

関数

• correct | landmarkInfo | poseHistory | removeLandmark | reset