我很难估计接近指南针“枢轴”点时的航向，并且可能会使用一些输入来解决它。在测试过程中，我将角度设置为0-360度，但从现在开始将使用弧度（-pi，pi）。和一个磁罗盘。

互补滤波器用于融合陀螺仪和odo测量以估计航向，然后使用电磁罗盘用卡尔曼滤波器对其进行校正。

当机器人的航向接近-pi / pi。

即使机器人甚至没有移动，估计的航向也没有用。

我认为这肯定是一个非常普遍的问题可能有比我想出的更好的解决方案，它要么是在过零时重新初始化积分器，每当误差较大时加180度，或者如果误差太大则忽略罗盘... br />
这是我的第一个Kalman过滤器，所以如果这不是常见问题，我可能执行不佳...

编辑：trudesagen的解决方案解决了我的问题。

#1 楼

在为四旋翼飞机制作扩展卡尔曼滤波器时，我自己遇到了这个问题。

您必须检查估计值是高于还是低于+/- pi，然后进行校正。

您可以使用简单的if语句执行此操作：

if (angle > pi)
    angle = angle - 2*pi;
else if (angle < -pi)
    angle = angle + 2*pi;


如果值变得大于由于某些原因，由于尖峰等原因，导致while看起来像这样：

while (abs(angle) > pi) {    
    if (angle > pi)
        angle = angle - 2*pi;
    else if (angle< -pi)
        angle = angle + 2*pi;
}


请记住，在进行预测以及进行测量之间的差异后，请进行此检查指南针角度和预测，用于计算校正后的估计值。

之后，还要检查校正后的估计值是否相同，这应该可以解决您的问题。

评论

$ \ begingroup $
这是解决该问题的创可贴。尽管卡尔曼滤波器的角速度分量应迅速纠正，但问题仍然存在。更好的解决方案是使用在滤波器中不像欧拉角（四元数或旋转矩阵）那样受到万向节锁定的方向参数。
$ \ endgroup $
–高达
16年5月29日在1:58
$ \ begingroup $
我认为您对这个问题有误解。这样可以将航向保持在+/- 180度以内。而且，它是一个差动的两轮机器人，因此您永远不会遇到万向节锁定，而且如果这样做（90度俯仰或滚动，取决于欧拉角度，则您手上还会遇到其他问题）。 “问题仍然存在于角速度中”是什么意思？角速度可以根据需要设置为高或低
$ \ endgroup $
– trudesagen
16年5月29日在15:27
$ \ begingroup $
@Gouda我认为您在这里误解了这个问题。它不是创可贴，这是将航向计算保持在+/- pi（180）度以内的方式。当控制机器人时，这是必需的，因此，如果机器人的航向为5 * 2pi，则当您要旋转90度并且控制器从当前航向中减去该角度时，就不必旋转5次才能到达那里。对于平面两轮机器人，万向节锁定永远不会成为问题，仅针对俯仰或滚动，这取决于您的欧拉表示。同样，角速度也不是问题，角速度是冲压和下一个冲压之间的差异。
$ \ endgroup $
– trudesagen
16年5月29日在15:32
$ \ begingroup $
必须发表两次评论，因为我的连接在评论中断开，失去了5分钟的编辑窗口
$ \ endgroup $
– trudesagen
16年5月29日在15:34
$ \ begingroup $
@trudesagen，我同意。也许我要说的是，角速度由航向残差确定，而航向残差很容易错误地计算为预测角度与测量角度之间的差。例如，在估算1度然后测量359度的情况下，角速度非常大，而实际上（可能）非常小且为负数。尽管在2D情况下也很容易解决：)。我提到四元数的原因是因为它将同时处理角度包角和角速度问题。
$ \ endgroup $
–高达
16 Jun 2'在14:23