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

Question

我必须实施EKF，而不是对它有很好的数学理解。 （太棒了......不......）到目前为止，我一直表现不错，但是自从我尝试实施预测步骤后，事情就开始出错了。

1. 使用EKF（红色）的代理人以随机方向射击
2. 最终，一些变量（姿势，Sigma，S，K）变为NaN，模拟失败



我的代码基于Thrun＆＃34; Probabilistic Robotics＆＃34;的代码。在页面上。 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

我完全无能为力......任何想法和提示都将受到赞赏。谢谢。

更新

代理人拍摄的原因是错误＆＃39;计算两个角度之间的差异时。这些是使用atan2计算的。虽然我知道问题是什么，但我仍然无法弄清楚如何修复它。

让我们想象一下，在为两个物体计算atan2之后，我得到了a = 135和b = 45的值。我计算了它们之间的差异，对于90度和270度两种可能性，但是代理人仍然没有＆＃39 ; t表现出应有的方式。我之前从未真正遇到过atan2。我对计算atan2值之间差异的理解是错误的吗？以下是我理解的例证：

Answer 1

Q是过程噪音？ 您无法将过程噪音设置为

Q = randn*eye(3);

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