Question

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

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

new_angle = angle + angular_velocity * time
new_angular_velocity = angular_velocity

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

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

有没有办法让过滤器知道这种可能的换行？在上述情况下仅提供1°的创新？

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

Answer 1

今天我和类似的KF有同样的问题......但我并没有像你那样受到限制。

这是我做的：

在计算predict_angle值之后：

//避免在0到360之间跳跃 if（measured_angle＆lt; pi / 2＆amp;＆amp; predict_angle＆gt; 3 * pi / 2）predict_angle = predict_angle - 2 * pi;
if（measured_angle＆gt; 3 * pi / 2＆amp;＆amp; estimate_aAngle＆lt; pi / 2）predict_angle = predict_angle + 2 * pi;
计算后估算estimated_angle。

如果你不介意牺牲一点精度，你可以切换到有符号整数[-2 * Pi，+ 2 * Pi]范围并做同样的事情。

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

HTH

卡尔曼滤波器的溢出感知实现

1 个答案: