Я думаю, это не так сложно, но я застрял на этом некоторое время.
У меня есть сустав, который может вращаться в обоих направлениях. Датчик дает мне угол соединения в диапазоне -pi и + pi.
Я хотел бы преобразовать его в диапазон -infinity и + бесконечность. Это означает, что если, например, сустав вращается по часовой стрелке вечно, угол будет начинаться с 0, а затем увеличиваться до бесконечности.
В matlab функция unwrap делает это очень хорошо:
newAngle = unwrap([previousAngle newAngle]);
previousAngle = newAngle;
Примечание: предполагается, что угол не делает большого прыжка, точно ничего не превосходит PI.
Примечание: я действительно выглядел очень усердным, прежде чем спрашивать …
Спасибо !
После некоторой работы придумали это. Кажется, работает нормально.
//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));
}
Я использовал код из этого поста:
Работа с Angle Wrap в коде c ++
Следующая функция выполняет работу, предполагая, что абсолютная разница между углами ввода меньше 2 * пи:
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;
}
}
// 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);
}
Это работает.