0

実際に数学的によく理解していなくても、EKF を実装する必要があります。(素晴らしい... 違います...) これまでのところ、私はうまくやっていますが、予測ステップを実装しようとしてから、うまくいきませんでした。

  1. EKF (赤) を使用するエージェントは、ランダムな方向に発射します
  2. 最終的に一部の変数 (ポーズ、シグマ、S、K) が NaN になり、シミュレーションが失敗する

ここに画像の説明を入力

コードは、Thrun の「Probabilistic Robotics」ページのコードに基づいています。204. これは、物事を台無しにしているように見えるコードの一部です。

% Get variables
[x,y,th] = getPose(mu_bar);
numoffeatures = size(map,2);

for f = 1:numoffeatures
    j = c(f);
    [fx,fy] = getFeatures(map,f);

    q = (fx-x).^2 + (fy-y).^2;  

    z_hat = [sqrt(q);
             atan2(fy-y,fx-x)-th;
             j];

    H = [(-fx-x)/sqrt(q) (-fy-y)/sqrt(q) 0;
          (fy-y)/q       (-fx-x)/q      -1;
                0               0        0];

    S = H*Sigma_bar*H'+Q;
    K = Sigma_bar*H'/inv(S);

    mu_bar    = mu_bar+K*(z(:,j)-z_hat);
    Sigma_bar = (eye(3)-K*H)*Sigma_bar; 
end

私はまったく無知です...アイデアやヒントをいただければ幸いです。ありがとうございました。

アップデート

エージェントが発砲する理由は、2 つの角度の差を計算するときの「エラー」です。これらは atan2 を使用して計算されます。問題が何であるかはわかっていますが、まだそれを修正する方法がわかりません。

2 つのオブジェクトの atan2 を計算した後、a = 135 と b = 45 という値が得られたとします。90 度と 270 度の両方の可能性についてそれらの差を計算しましたが、エージェントはまだ想定どおりに動作しません。 . 私はこれまでatan2に実際に遭遇したことはありません。atan2 値の差の計算に関する私の理解は間違っていますか? これが私の理解の実例です：

ここに画像の説明を入力

4

1 に答える 1