java - 如何对角度值进行低通滤波

Question

我正在从传感器接收以弧度为单位的角度更新，并且我想通过低通滤波功能传递它们，以便由于噪音而稍微平滑它们。

我的低通滤波器函数是这样的

protected void lowPass(float alpha, double[] input, double[] output) {
    for (int i = 0; i < input.length; i++) {
        output[i] = alpha * output[i] + (1 - alpha) * input[i];
    }
}

而且它对大多数部分都非常有效。

问题是有时角度“走完整个圈”，例如从 2π 到 0、0 到 -2π 等等。当然这会导致“不正确”的输出，因为过滤器函数将 2π 和 0 简单地视为 6.28 和 0.0。

我如何实现可以正确处理这些类型的角度的过滤器函数？

Answer 1

有一种有趣的方法用于 angle averaging .

获取角度alpha的余弦和正弦(作为具有该角度的 vector 的分量)并将它们用于单独的过滤，然后获取结果 vector 的角度

 cosa = alpha * cos(output) + (1 - alpha) * cos(input);
 sina = alpha * sin(output) + (1 - alpha) * sin(input);
 output = atan2(sina, cosa)