matlab - 扩展卡尔曼滤波器 - 更新步骤中的错误

Question

我必须在实际上没有很好的数学理解的情况下实现 EKF。（太好了……不是……）到目前为止，我一直做得很好，但是自从我尝试实施预测步骤后，事情就开始出错了。

1. 使用 EKF（红色）的智能体向随机方向发射
2. 最终一些变量（pose、Sigma、S、K）变成了 NaN，模拟失败了

我的代码基于 Thrun 在页面上的“概率机器人”中的代码。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

我完全一无所知......任何想法和提示将不胜感激。谢谢你。

更新

代理射击的原因是计算两个角度之间的差异时的“错误”。这些是使用 atan2 计算的。虽然我知道问题出在哪里，但我仍然不知道如何解决它。

让我们想象一下，在为两个对象计算 atan2 之后，我得到了 a = 135 和 b = 45 的值。我计算了它们之间的差异，分别为 90 度和 270 度，但代理仍然没有按照预期的方式行事. 我以前从未真正遇到过atan2。我对计算 atan2 值之间差异的理解是否错误？这是我的理解的说明：

Answer 1

Q是过程噪音吗？您不能将过程噪声设置为

Q = randn*eye(3);

因为你可能有负协方差，这没有意义。