AP_Math: added quaternion helper functions and a test suite

This commit is contained in:
Andrew Tridgell 2012-03-05 14:31:47 +11:00
parent f70dfe440d
commit c7d5f06b21
5 changed files with 233 additions and 0 deletions

View File

@ -67,3 +67,33 @@ void calculate_euler_angles(Matrix3f &m, float *roll, float *pitch, float *yaw)
*yaw = atan2(m.b.x, m.a.x);
}
}
// create a quaternion from Euler angles
void quaternion_from_euler(Quaternion &q, float roll, float pitch, float yaw)
{
float cr2 = cos(roll/2);
float cp2 = cos(pitch/2);
float cy2 = cos(yaw/2);
// the sign reversal here is due to the different conventions
// in the madgwick quaternion code and the rest of APM
float sr2 = -sin(roll/2);
float sp2 = -sin(pitch/2);
float sy2 = sin(yaw/2);
q.q1 = cr2*cp2*cy2 + sr2*sp2*sy2;
q.q2 = sr2*cp2*cy2 - cr2*sp2*sy2;
q.q3 = cr2*sp2*cy2 + sr2*cp2*sy2;
q.q4 = cr2*cp2*sy2 - sr2*sp2*cy2;
}
// create eulers from a quaternion
void euler_from_quaternion(Quaternion &q, float *roll, float *pitch, float *yaw)
{
*roll = -(atan2(2.0*(q.q1*q.q2 + q.q3*q.q4),
1 - 2.0*(q.q2*q.q2 + q.q3*q.q3)));
// we let safe_asin() handle the singularities near 90/-90 in pitch
*pitch = -safe_asin(2.0*(q.q1*q.q3 - q.q4*q.q2));
*yaw = atan2(2.0*(q.q1*q.q4 + q.q2*q.q3),
1 - 2.0*(q.q3*q.q3 + q.q4*q.q4));
}

View File

@ -7,6 +7,7 @@
#include "vector2.h"
#include "vector3.h"
#include "matrix3.h"
#include "quaternion.h"
#include "polygon.h"
// define AP_Param types AP_Vector3f and Ap_Matrix3f
@ -24,3 +25,9 @@ void rotation_matrix_from_euler(Matrix3f &m, float roll, float pitch, float yaw)
// calculate euler angles from a rotation matrix
void calculate_euler_angles(Matrix3f &m, float *roll, float *pitch, float *yaw);
// create a quaternion from Euler angles
void quaternion_from_euler(Quaternion &q, float roll, float pitch, float yaw);
// create eulers from a quaternion
void euler_from_quaternion(Quaternion &q, float *roll, float *pitch, float *yaw);

View File

@ -0,0 +1 @@
include ../../../AP_Common/Arduino.mk

View File

@ -0,0 +1,159 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
//
// Unit tests for the AP_Math euler code
//
#include <FastSerial.h>
#include <AP_Common.h>
#include <AP_Math.h>
FastSerialPort(Serial, 0);
static float rad_diff(float rad1, float rad2)
{
float diff = rad1 - rad2;
if (diff > PI) {
diff -= 2*PI;
}
if (diff < -PI) {
diff += 2*PI;
}
return fabs(diff);
}
static void test_euler(float roll, float pitch, float yaw)
{
Matrix3f m;
float roll2, pitch2, yaw2;
rotation_matrix_from_euler(m, roll, pitch, yaw);
calculate_euler_angles(m, &roll2, &pitch2, &yaw2);
if (m.is_nan()) {
Serial.printf("NAN matrix roll=%f pitch=%f yaw=%f\n",
roll, pitch, yaw);
}
if (isnan(roll2) ||
isnan(pitch2) ||
isnan(yaw2)) {
Serial.printf("NAN eulers roll=%f pitch=%f yaw=%f\n",
roll, pitch, yaw);
}
if (rad_diff(roll2,roll) > 0.01 ||
rad_diff(pitch2, pitch) > 0.01 ||
rad_diff(yaw2, yaw) > 0.01) {
Serial.printf("incorrect eulers roll=%f/%f pitch=%f/%f yaw=%f/%f\n",
roll, roll2, pitch, pitch2, yaw, yaw2);
}
}
#define ARRAY_LENGTH(x) (sizeof((x))/sizeof((x)[0]))
static const float angles[] = { 0, PI/8, PI/4, PI/2, PI,
-PI/8, -PI/4, -PI/2, -PI};
void test_matrix_eulers(void)
{
uint8_t i, j, k;
uint8_t N = ARRAY_LENGTH(angles);
Serial.println("rotation matrix unit tests\n");
for (i=0; i<N; i++)
for (j=0; j<N; j++)
for (k=0; k<N; k++)
test_euler(angles[i], angles[j], angles[k]);
Serial.println("tests done\n");
}
static void test_quaternion(float roll, float pitch, float yaw)
{
Quaternion q;
float roll2, pitch2, yaw2;
quaternion_from_euler(q, roll, pitch, yaw);
euler_from_quaternion(q, &roll2, &pitch2, &yaw2);
if (q.is_nan()) {
Serial.printf("NAN quaternion roll=%f pitch=%f yaw=%f\n",
roll, pitch, yaw);
}
if (isnan(roll2) ||
isnan(pitch2) ||
isnan(yaw2)) {
Serial.printf("NAN eulers roll=%f pitch=%f yaw=%f\n",
roll, pitch, yaw);
}
if (rad_diff(roll2,roll) > ToRad(179)) {
// reverse all 3
roll2 += fmod(roll2+PI, 2*PI);
pitch2 += fmod(pitch2+PI, 2*PI);
yaw2 += fmod(yaw2+PI, 2*PI);
}
if (rad_diff(roll2,roll) > 0.01 ||
rad_diff(pitch2, pitch) > 0.01 ||
rad_diff(yaw2, yaw) > 0.01) {
if (ToDeg(rad_diff(pitch, PI/2)) < 1 ||
ToDeg(rad_diff(pitch, -PI/2)) < 1) {
// we expect breakdown at these poles
Serial.printf("breakdown eulers roll=%f/%f pitch=%f/%f yaw=%f/%f\n",
ToDeg(roll), ToDeg(roll2), ToDeg(pitch), ToDeg(pitch2), ToDeg(yaw), ToDeg(yaw2));
} else {
Serial.printf("incorrect eulers roll=%f/%f pitch=%f/%f yaw=%f/%f\n",
ToDeg(roll), ToDeg(roll2), ToDeg(pitch), ToDeg(pitch2), ToDeg(yaw), ToDeg(yaw2));
}
} else {
Serial.printf("correct eulers roll=%f/%f pitch=%f/%f yaw=%f/%f\n",
ToDeg(roll), ToDeg(roll2), ToDeg(pitch), ToDeg(pitch2), ToDeg(yaw), ToDeg(yaw2));
}
}
void test_quaternion_eulers(void)
{
uint8_t i, j, k;
uint8_t N = ARRAY_LENGTH(angles);
Serial.println("quaternion unit tests\n");
test_quaternion(PI/4, 0, 0);
test_quaternion(0, PI/4, 0);
test_quaternion(0, 0, PI/4);
test_quaternion(-PI/4, 0, 0);
test_quaternion(0, -PI/4, 0);
test_quaternion(0, 0, -PI/4);
test_quaternion(-PI/4, 1, 1);
test_quaternion(1, -PI/4, 1);
test_quaternion(1, 1, -PI/4);
test_quaternion(ToRad(89), 0, 0.1);
test_quaternion(0, ToRad(89), 0.1);
test_quaternion(0.1, 0, ToRad(89));
test_quaternion(ToRad(91), 0, 0.1);
test_quaternion(0, ToRad(91), 0.1);
test_quaternion(0.1, 0, ToRad(91));
for (i=0; i<N; i++)
for (j=0; j<N; j++)
for (k=0; k<N; k++)
test_quaternion(angles[i], angles[j], angles[k]);
Serial.println("tests done\n");
}
/*
euler angle tests
*/
void setup(void)
{
Serial.begin(115200);
Serial.println("euler unit tests\n");
test_quaternion_eulers();
//test_matrix_eulers();
}
void
loop(void)
{
}

View File

@ -0,0 +1,36 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
// Copyright 2012 Andrew Tridgell, all rights reserved.
// This library is free software; you can redistribute it and / or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.
#ifndef QUATERNION_H
#define QUATERNION_H
#include <math.h>
class Quaternion
{
public:
float q1, q2, q3, q4;
// constructor
Quaternion() { q1 = 1; q2 = q3 = q4 = 0; }
// setting constructor
Quaternion(const float _q1, const float _q2, const float _q3, const float _q4):
q1(_q1), q2(_q2), q3(_q3), q4(_q4) {}
// function call operator
void operator ()(const float _q1, const float _q2, const float _q3, const float _q4)
{ q1 = _q1; q2 = _q2; q3 = _q3; q4 = _q4; }
// check if any elements are NAN
bool is_nan(void)
{ return isnan(q1) || isnan(q2) || isnan(q3) || isnan(q4); }
};
#endif // QUATERNION_H