C++の連続アングル（matlabのeラップ解除関数）

Question

それは難しいとは思えませんが、私はしばらくそれに固執しています。

私は両方向に回転できるジョイントを持っています。センサーが私に関節の角度を-piと+ piの範囲で与えます。

-∞と+∞の範囲で変換したいと思います。例えば関節が時計回りに永遠に回転する場合、角度は0から始まり、無限に増加することを意味します。 MATLABでは は、アンラップ関数は、非常によく行われます。

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

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

これは動作します。

Answer 3

次の関数は、入力間の絶対差が角度と仮定すると、以下の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; 
    } 
}