c++ - C++ 中的连续角(matlab 中的 eq unwrap 函数)

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