Continous angles in C++ (eq unwrap function in matlab)

Question

I guess it is not that hard, but I have been stuck on that one for a while.

I have a joint that can rotate both direction. A sensor gives me the angle of the joint in the range -pi and +pi.

I would like to convert it in the range -infinity and +infinity. Meaning that if for example the joint rotate clockwise forever, the angle would start at 0 and then increase to infinity. In matlab, the unwrap function does that very well:

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

Note: it is assumed the angle does not make big jump, nothing superior to PI for sure.

Note: I really looked hard before asking ...

Thanks !

score 4 · Accepted Answer · edited May 23 '17 at 12:01

4

After some work, came up with this. Seems to be working fine.

//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));
}

I used code from this post: Dealing with Angle Wrap in c++ code

edited May 23 '17 at 12:01

Community

1
1

answered Mar 27 '13 at 03:32

Vince

3,251
8
28
57

In angleConv() function, angle has been constrained to [-180, 180], so it is obviously in [-360, 360]. I could not see the necessity. – kevin Jun 09 '15 at 00:05
I actually don't think this returns the desired result. First, the angleConv function maps from [0,2*pi], not [-2*pi,2*pi]. Also in testing this code it doesn't return the correct result for negative angles – jlack Dec 31 '19 at 14:43

roeeb · Answer 2 · 2016-04-08T18:22:19.017

The following function does the job, assuming the absolute difference between the input angles is less than 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;
}

If you need to unwrap an array, you can use this routine:

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;
    }
}

score 3 · Answer 3 · answered Feb 29 '16 at 11:13

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);
}

This works.

answered Feb 29 '16 at 11:13

MujjinGun

430
3
9

Since -pi and pi map to the same value, we need to handle those separately. Before the fmod, you can add: if (fabs(x) == M_PI) return x; – Jeffrey Faust Sep 23 '17 at 21:41

Continous angles in C++ (eq unwrap function in matlab)

3 Answers3