3

我正在尝试使用 3 轴加速度计和 3 轴陀螺仪作为传感器来实现卡尔曼滤波器以获得物体的方向。

为这个过滤器的预测阶段选择动态模型是直截了当的，它是：

new_angle = angle + angular_velocity * time
new_angular_velocity = angular_velocity

但是我手头没有浮点支持，我需要每一位精度来模拟角度。因此，我的计划是将角度表示为 32 位整数数据，将一整圈表示2 pi为 2^32 小步。因此，整数溢出会免费处理换行（2 pi与 的方向相同0）。

但这也给过滤器带来了一个问题：如果估计的角度是359°，而我的测量结果是0°，那么过滤器会假设一个巨大的创新，导致不确定性和奇数值。

有没有办法让过滤器知道这种可能的包装？1°仅在上述情况下给予创新？

为了规避这个问题，我考虑过使用角度差异而不是角度，但我找不到合适的模型。

4

1 回答 1

2

我今天和类似的KF也有同样的问题……但我没有你那么拘束。

这是我所做的：

  1. 在计算出 predict_angle 值之后：

    // 避免在 0 到 360 之间跳跃
    if (measured_angle < pi/2 && predict_angle > 3*pi/2) predicted_angle = predicted_angle - 2*pi;
    if (measured_angle > 3*pi/2 &&estimate_aAngle < pi/2) predict_angle = predict_angle + 2*pi;

  2. 计算后对estimated_angle 进行归一化。

如果您不介意牺牲一点精度，您可以切换到有符号整数 [-2*Pi,+2*Pi) 范围并执行相同操作。

PS 如果每个样本的最大角度变化很小，我认为您可以使用 KF 中的内部角度偏移仅牺牲一小部分精度。偏移量必须足够大于该值。您将拥有 2^32 = 2*Pi + 2*OFFSET 范围，而不是 2*pi。在 KF 中，将此 OFFSET 添加到局部角度变量并返回estimated_angle = normalize(offset_estimated_angle - OFFSET, 0-2*pi)。

高温高压

于 2014-08-28T19:48:38.610 回答