c++ - C++ 中的连续角（matlab 中的 eq 展开函数）

Question

我想这并不难，但我已经坚持了一段时间。

我有一个可以双向旋转的关节。传感器为我提供了 -pi 和 +pi 范围内的关节角度。

我想在 -infinity 和 +infinity 范围内转换它。这意味着如果例如关节永远顺时针旋转，角度将从 0 开始，然后增加到无穷大。在 matlab 中， unwrap 函数做得很好：

newAngle = unwrap([previousAngle newAngle]);
previousAngle = newAngle;

注意：假设角度没有大跳，肯定没有比 PI 更好。

注：问之前我真的仔细看了看……

谢谢 ！

Answer 1

经过一番工作，想出了这个。似乎工作正常。

//Normalize to [-180,180):
inline double constrainAngle(double x){
    x = fmod(x + M_PI,M_2PI);
    if (x < 0)
        x += M_2PI;
    return x - M_PI;
}
// convert to [-360,360]
inline double angleConv(double angle){
    return fmod(constrainAngle(angle),M_2PI);
}
inline double angleDiff(double a,double b){
    double dif = fmod(b - a + M_PI,M_2PI);
    if (dif < 0)
        dif += M_2PI;
    return dif - M_PI;
}
inline double unwrap(double previousAngle,double newAngle){
    return previousAngle - angleDiff(newAngle,angleConv(previousAngle));
}

我使用了这篇文章中的代码： Dealing with Angle Wrap in c++ code

Answer 2

假设输入角度之间的绝对差小于 2*pi，则以下函数可以完成这项工作：

float unwrap(float previous_angle, float new_angle) {
    float d = new_angle - previous_angle;
    d = d > M_PI ? d - 2 * M_PI : (d < -M_PI ? d + 2 * M_PI : d);
    return previous_angle + d;
}

如果你需要解开一个数组，你可以使用这个例程：

void unwrap_array(float *in, float *out, int len) {
    out[0] = in[0];
    for (int i = 1; i < len; i++) {
        float d = in[i] - in[i-1];
        d = d > M_PI ? d - 2 * M_PI : (d < -M_PI ? d + 2 * M_PI : d);
        out[i] = out[i-1] + d;
    }
}

Answer 3

// wrap to [-pi,pi]
inline double angle_norm(double x)
{
    x = fmod(x + M_PI, M_2PI);
    if (x < 0)
        x += M_2PI;
    return x - M_PI;
}

double phase_unwrap(double prev, double now)
{
    return prev + angle_norm(now - prev);
}

这行得通。