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

【问题标题】：Extended Kalman Filter - error in update step扩展卡尔曼滤波器 - 更新步骤中的错误
【发布时间】：2015-03-20 13:45:25
【问题描述】：

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

使用 EKF（红色）的代理向随机方向发射
最终某些变量（姿势、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 值之间差异的理解是否错误？这是我的理解的说明：

【问题讨论】：

当变量变为 NaN 时，您的“q”值是多少？ H 有一个除以 q 和 sqrt(q)，可以是 0。
在某些时候“q”变成了 Inf... 之后变量变成了 NaN。我不明白为什么它会变成 Inf。
查看q定义中的参数。什么是fx、fy、x、y等。这段代码我跑不了，所以也不能调试。
fx,fy 是特征的坐标，x,y 是代理认为它所在位置的坐标。 q 正在计算从代理到对象（图像中的绿星）的欧几里得距离。我正在尝试调试，但这非常耗时。设置条件点，例如q == Inf 不起作用。
q 是欧式距离 -> 如果 q = Inf，则参数 fx,fy,x,y 之一也是 Inf。哪一个？

标签： matlab prediction robotics kalman-filter atan2

【解决方案1】：

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

Q = randn*eye(3);

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

【讨论】：

老实说，我实际上不知道将 Q 设置为什么，我也不明白这本书是怎么说的......我想是因为它应该是噪音零均值因此 randn 是一个不错的选择。
Q 是过程噪声的协方差，它必须是正矩阵。您可以将其设置为 eye(3) 以尝试是否是问题所在。
不幸的是它没有任何区别。
@Vetaxili：noise 是零均值，方差由 Q 给出。噪声是一个随机过程。 Q 是对它的描述。