fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

mathlib delete Matrix, Quaternion, Vector

Browse Source
This commit is contained in:
Daniel Agar 2018-03-28 17:05:42 -04:00
parent 0d7b5c4a4e
commit 222a91c6be
32 changed files with 160 additions and 1746 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/drivers/camera_trigger/camera_trigger.cpp
View File

@ -49,6 +49,7 @@
#include <stdbool.h>
#include <poll.h>
#include <mathlib/mathlib.h>
#include <matrix/math.hpp>
#include <px4_workqueue.h>
#include <systemlib/systemlib.h>
#include <systemlib/err.h>

1
src/drivers/tap_esc/tap_esc.cpp
View File

@ -41,6 +41,7 @@
#include <errno.h>
#include <cmath> // NAN
#include <cstring>
#include <lib/mathlib/mathlib.h>
#include <drivers/device/device.h>

2
src/drivers/telemetry/bst/bst.cpp
View File

@ -50,7 +50,7 @@
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/vehicle_attitude.h>
#include <mathlib/math/Quaternion.hpp>
#include <matrix/math.hpp>
using namespace matrix;

1
src/drivers/vmount/output.cpp
View File

@ -49,6 +49,7 @@
#include <lib/ecl/geo/geo.h>
#include <math.h>
#include <mathlib/mathlib.h>
#include <matrix/math.hpp>
namespace vmount
{

1
src/examples/uuv_example_app/uuv_example_app.cpp
View File

@ -58,6 +58,7 @@
#include <systemlib/circuit_breaker.h>
// internal libraries
#include <lib/mathlib/mathlib.h>
#include <matrix/math.hpp>
#include <lib/ecl/geo/geo.h>
// Include uORB and the required topics for this app

1
src/lib/battery/battery.cpp
View File

@ -41,6 +41,7 @@
#include "battery.h"
#include <mathlib/mathlib.h>
#include <cstring>
Battery::Battery() :
ModuleParams(nullptr),

68
src/lib/conversion/rotation.cpp
View File

@ -40,16 +40,6 @@
#include "math.h"
#include "rotation.h"
__EXPORT void
get_rot_matrix(enum Rotation rot, math::Matrix<3, 3> *rot_matrix)
{
float roll = M_DEG_TO_RAD_F * (float)rot_lookup[rot].roll;
float pitch = M_DEG_TO_RAD_F * (float)rot_lookup[rot].pitch;
float yaw = M_DEG_TO_RAD_F * (float)rot_lookup[rot].yaw;
rot_matrix->from_euler(roll, pitch, yaw);
}
__EXPORT matrix::Dcmf
get_rot_matrix(enum Rotation rot)
{
@ -59,8 +49,6 @@ get_rot_matrix(enum Rotation rot)
math::radians((float)rot_lookup[rot].yaw)}};
}
#define HALF_SQRT_2 0.70710678118654757f
__EXPORT void
rotate_3f(enum Rotation rot, float &x, float &y, float &z)
{
@ -72,8 +60,8 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z)
return;
case ROTATION_YAW_45: {
tmp = HALF_SQRT_2 * (x - y);
y = HALF_SQRT_2 * (x + y);
tmp = M_SQRT1_2_F * (x - y);
y = M_SQRT1_2_F * (x + y);
x = tmp;
return;
}
@ -84,8 +72,8 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z)
}
case ROTATION_YAW_135: {
tmp = -HALF_SQRT_2 * (x + y);
y = HALF_SQRT_2 * (x - y);
tmp = -M_SQRT1_2_F * (x + y);
y = M_SQRT1_2_F * (x - y);
x = tmp;
return;
}
@ -95,8 +83,8 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z)
return;
case ROTATION_YAW_225: {
tmp = HALF_SQRT_2 * (y - x);
y = -HALF_SQRT_2 * (x + y);
tmp = M_SQRT1_2_F * (y - x);
y = -M_SQRT1_2_F * (x + y);
x = tmp;
return;
}
@ -107,8 +95,8 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z)
}
case ROTATION_YAW_315: {
tmp = HALF_SQRT_2 * (x + y);
y = HALF_SQRT_2 * (y - x);
tmp = M_SQRT1_2_F * (x + y);
y = M_SQRT1_2_F * (y - x);
x = tmp;
return;
}
@ -119,8 +107,8 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z)
}
case ROTATION_ROLL_180_YAW_45: {
tmp = HALF_SQRT_2 * (x + y);
y = HALF_SQRT_2 * (x - y);
tmp = M_SQRT1_2_F * (x + y);
y = M_SQRT1_2_F * (x - y);
x = tmp; z = -z;
return;
}
@ -131,8 +119,8 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z)
}
case ROTATION_ROLL_180_YAW_135: {
tmp = HALF_SQRT_2 * (y - x);
y = HALF_SQRT_2 * (y + x);
tmp = M_SQRT1_2_F * (y - x);
y = M_SQRT1_2_F * (y + x);
x = tmp; z = -z;
return;
}
@ -143,8 +131,8 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z)
}
case ROTATION_ROLL_180_YAW_225: {
tmp = -HALF_SQRT_2 * (x + y);
y = HALF_SQRT_2 * (y - x);
tmp = -M_SQRT1_2_F * (x + y);
y = M_SQRT1_2_F * (y - x);
x = tmp; z = -z;
return;
}
@ -155,8 +143,8 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z)
}
case ROTATION_ROLL_180_YAW_315: {
tmp = HALF_SQRT_2 * (x - y);
y = -HALF_SQRT_2 * (x + y);
tmp = M_SQRT1_2_F * (x - y);
y = -M_SQRT1_2_F * (x + y);
x = tmp; z = -z;
return;
}
@ -168,8 +156,8 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z)
case ROTATION_ROLL_90_YAW_45: {
tmp = z; z = y; y = -tmp;
tmp = HALF_SQRT_2 * (x - y);
y = HALF_SQRT_2 * (x + y);
tmp = M_SQRT1_2_F * (x - y);
y = M_SQRT1_2_F * (x + y);
x = tmp;
return;
}
@ -182,8 +170,8 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z)
case ROTATION_ROLL_90_YAW_135: {
tmp = z; z = y; y = -tmp;
tmp = -HALF_SQRT_2 * (x + y);
y = HALF_SQRT_2 * (x - y);
tmp = -M_SQRT1_2_F * (x + y);
y = M_SQRT1_2_F * (x - y);
x = tmp;
return;
}
@ -195,8 +183,8 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z)
case ROTATION_ROLL_270_YAW_45: {
tmp = z; z = -y; y = tmp;
tmp = HALF_SQRT_2 * (x - y);
y = HALF_SQRT_2 * (x + y);
tmp = M_SQRT1_2_F * (x - y);
y = M_SQRT1_2_F * (x + y);
x = tmp;
return;
}
@ -209,8 +197,8 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z)
case ROTATION_ROLL_270_YAW_135: {
tmp = z; z = -y; y = tmp;
tmp = -HALF_SQRT_2 * (x + y);
y = HALF_SQRT_2 * (x - y);
tmp = -M_SQRT1_2_F * (x + y);
y = M_SQRT1_2_F * (x - y);
x = tmp;
return;
}
@ -276,15 +264,15 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z)
}
case ROTATION_PITCH_45: {
tmp = HALF_SQRT_2 * x + HALF_SQRT_2 * z;
z = HALF_SQRT_2 * z - HALF_SQRT_2 * x;
tmp = M_SQRT1_2_F * x + M_SQRT1_2_F * z;
z = M_SQRT1_2_F * z - M_SQRT1_2_F * x;
x = tmp;
return;
}
case ROTATION_PITCH_315: {
tmp = HALF_SQRT_2 * x - HALF_SQRT_2 * z;
z = HALF_SQRT_2 * z + HALF_SQRT_2 * x;
tmp = M_SQRT1_2_F * x - M_SQRT1_2_F * z;
z = M_SQRT1_2_F * z + M_SQRT1_2_F * x;
x = tmp;
return;
}

2
src/lib/conversion/rotation.h
View File

@ -42,6 +42,7 @@
#include <unistd.h>
#include <mathlib/mathlib.h>
#include <matrix/math.hpp>
/**
* Enum for board and external compass rotations.
@ -133,7 +134,6 @@ const rot_lookup_t rot_lookup[] = {
/**
* Get the rotation matrix
*/
__EXPORT void get_rot_matrix(enum Rotation rot, math::Matrix<3, 3> *rot_matrix);
__EXPORT matrix::Dcmf get_rot_matrix(enum Rotation rot);

1
src/lib/drivers/device/integrator.h
View File

@ -43,6 +43,7 @@
#pragma once
#include <mathlib/mathlib.h>
#include <matrix/math.hpp>
class Integrator
{

544
src/lib/mathlib/math/Matrix.hpp
View File

@ -1,544 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Anton Babushkin <anton.babushkin@me.com>
* Pavel Kirienko <pavel.kirienko@gmail.com>
* Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file Matrix.hpp
*
* Matrix class
*/
#ifndef MATRIX_HPP
#define MATRIX_HPP
#include <stdio.h>
#include <cmath>
#include "Vector.hpp" // Vector and eigen_matrix_instance
#include "matrix/math.hpp"
#include <platforms/px4_defines.h>
namespace math
{
template<unsigned int M, unsigned int N>
class __EXPORT Matrix;
// MxN matrix with float elements
template <unsigned int M, unsigned int N>
class __EXPORT MatrixBase
{
public:
/**
* matrix data[row][col]
*/
float data[M][N];
/**
* struct for using arm_math functions
*/
eigen_matrix_instance arm_mat;
/**
* trivial ctor
* Initializes the elements to zero.
*/
MatrixBase() :
data{},
arm_mat{M, N, &data[0][0]}
{
}
~MatrixBase() = default;
/**
* copyt ctor
*/
MatrixBase(const MatrixBase<M, N> &m) :
arm_mat{M, N, &data[0][0]}
{
memcpy(data, m.data, sizeof(data));
}
MatrixBase(const float *d) :
arm_mat{M, N, &data[0][0]}
{
memcpy(data, d, sizeof(data));
}
MatrixBase(const float d[M][N]) :
arm_mat{M, N, &data[0][0]}
{
memcpy(data, d, sizeof(data));
}
/**
* set data
*/
void set(const float *d)
{
memcpy(data, d, sizeof(data));
}
/**
* set data
*/
void set(const float d[M][N])
{
memcpy(data, d, sizeof(data));
}
/**
* set row from vector
*/
void set_row(unsigned int row, const Vector<N> v)
{
for (unsigned i = 0; i < N; i++) {
data[row][i] = v.data[i];
}
}
/**
* set column from vector
*/
void set_col(unsigned int col, const Vector<M> v)
{
for (unsigned i = 0; i < M; i++) {
data[i][col] = v.data[i];
}
}
/**
* access by index
*/
float &operator()(const unsigned int row, const unsigned int col)
{
return data[row][col];
}
/**
* access by index
*/
float operator()(const unsigned int row, const unsigned int col) const
{
return data[row][col];
}
/**
* get rows number
*/
unsigned int get_rows() const
{
return M;
}
/**
* get columns number
*/
unsigned int get_cols() const
{
return N;
}
/**
* test for equality
*/
bool operator ==(const Matrix<M, N> &m) const
{
for (unsigned int i = 0; i < M; i++) {
for (unsigned int j = 0; j < N; j++) {
if (data[i][j] != m.data[i][j]) {
return false;
}
}
}
return true;
}
/**
* test for inequality
*/
bool operator !=(const Matrix<M, N> &m) const
{
for (unsigned int i = 0; i < M; i++) {
for (unsigned int j = 0; j < N; j++) {
if (data[i][j] != m.data[i][j]) {
return true;
}
}
}
return false;
}
/**
* set to value
*/
const Matrix<M, N> &operator =(const Matrix<M, N> &m)
{
memcpy(data, m.data, sizeof(data));
return *static_cast<Matrix<M, N>*>(this);
}
/**
* negation
*/
Matrix<M, N> operator -() const
{
Matrix<M, N> res;
for (unsigned int i = 0; i < M; i++) {
for (unsigned int j = 0; j < N; j++) {
res.data[i][j] = -data[i][j];
}
}
return res;
}
/**
* addition
*/
Matrix<M, N> operator +(const Matrix<M, N> &m) const
{
Matrix<M, N> res;
for (unsigned int i = 0; i < M; i++) {
for (unsigned int j = 0; j < N; j++) {
res.data[i][j] = data[i][j] + m.data[i][j];
}
}
return res;
}
Matrix<M, N> &operator +=(const Matrix<M, N> &m)
{
for (unsigned int i = 0; i < M; i++) {
for (unsigned int j = 0; j < N; j++) {
data[i][j] += m.data[i][j];
}
}
return *static_cast<Matrix<M, N>*>(this);
}
/**
* subtraction
*/
Matrix<M, N> operator -(const Matrix<M, N> &m) const
{
Matrix<M, N> res;
for (unsigned int i = 0; i < M; i++) {
for (unsigned int j = 0; j < N; j++) {
res.data[i][j] = data[i][j] - m.data[i][j];
}
}
return res;
}
Matrix<M, N> &operator -=(const Matrix<M, N> &m)
{
for (unsigned int i = 0; i < M; i++) {
for (unsigned int j = 0; j < N; j++) {
data[i][j] -= m.data[i][j];
}
}
return *static_cast<Matrix<M, N>*>(this);
}
/**
* uniform scaling
*/
Matrix<M, N> operator *(const float num) const
{
Matrix<M, N> res;
for (unsigned int i = 0; i < M; i++) {
for (unsigned int j = 0; j < N; j++) {
res.data[i][j] = data[i][j] * num;
}
}
return res;
}
Matrix<M, N> &operator *=(const float num)
{
for (unsigned int i = 0; i < M; i++) {
for (unsigned int j = 0; j < N; j++) {
data[i][j] *= num;
}
}
return *static_cast<Matrix<M, N>*>(this);
}
Matrix<M, N> operator /(const float num) const
{
Matrix<M, N> res;
for (unsigned int i = 0; i < M; i++) {
for (unsigned int j = 0; j < N; j++) {
res.data[i][j] = data[i][j] / num;
}
}
return res;
}
Matrix<M, N> &operator /=(const float num)
{
for (unsigned int i = 0; i < M; i++) {
for (unsigned int j = 0; j < N; j++) {
data[i][j] /= num;
}
}
return *static_cast<Matrix<M, N>*>(this);
}
/**
* multiplication by another matrix
*/
template <unsigned int P>
Matrix<M, P> operator *(const Matrix<N, P> &m) const
{
matrix::Matrix<float, M, N> Me(this->arm_mat.pData);
matrix::Matrix<float, N, P> Him(m.arm_mat.pData);
matrix::Matrix<float, M, P> Product = Me * Him;
Matrix<M, P> res(Product.data());
return res;
}
/**
* transpose the matrix
*/
Matrix<N, M> transposed() const
{
matrix::Matrix<float, M, N> Me(this->arm_mat.pData);
Matrix<N, M> res(Me.transpose().data());
return res;
}
/**
* invert the matrix
*/
Matrix<M, N> inversed() const
{
matrix::SquareMatrix<float, M> Me = matrix::Matrix<float, M, N>(this->arm_mat.pData);
Matrix<M, N> res(Me.I().data());
return res;
}
/**
* set zero matrix
*/
void zero()
{
memset(data, 0, sizeof(data));
}
/**
* set identity matrix
*/
void identity()
{
memset(data, 0, sizeof(data));
unsigned int n = (M < N) ? M : N;
for (unsigned int i = 0; i < n; i++) {
data[i][i] = 1;
}
}
void print()
{
for (unsigned int i = 0; i < M; i++) {
printf("[ ");
for (unsigned int j = 0; j < N; j++) {
printf("%.3f\t", (double)data[i][j]);
}
printf(" ]\n");
}
}
};
template <unsigned int M, unsigned int N>
class __EXPORT Matrix : public MatrixBase<M, N>
{
public:
using MatrixBase<M, N>::operator *;
Matrix() : MatrixBase<M, N>() {}
Matrix(const Matrix<M, N> &m) : MatrixBase<M, N>(m) {}
Matrix(const float *d) : MatrixBase<M, N>(d) {}
Matrix(const float d[M][N]) : MatrixBase<M, N>(d) {}
/**
* set to value
*/
const Matrix<M, N> &operator =(const Matrix<M, N> &m)
{
memcpy(this->data, m.data, sizeof(this->data));
return *this;
}
/**
* multiplication by a vector
*/
Vector<M> operator *(const Vector<N> &v) const
{
matrix::Matrix<float, M, N> Me(this->arm_mat.pData);
matrix::Matrix<float, N, 1> Vec(v.arm_col.pData);
matrix::Matrix<float, M, 1> Product = Me * Vec;
Vector<M> res(Product.data());
return res;
}
};
template <>
class __EXPORT Matrix<3, 3> : public MatrixBase<3, 3>
{
public:
using MatrixBase<3, 3>::operator *;
Matrix() : MatrixBase<3, 3>() {}
Matrix(const Matrix<3, 3> &m) : MatrixBase<3, 3>(m) {}
Matrix(const float *d) : MatrixBase<3, 3>(d) {}
Matrix(const float d[3][3]) : MatrixBase<3, 3>(d) {}
/**
* set data
*/
void set(const float d[9])
{
memcpy(data, d, sizeof(data));
}
#if defined(__PX4_ROS)
/**
* set data from boost::array
*/
void set(const boost::array<float, 9ul> d)
{
set(static_cast<const float *>(d.data()));
}
#endif
/**
* set to value
*/
const Matrix<3, 3> &operator =(const Matrix<3, 3> &m)
{
memcpy(this->data, m.data, sizeof(this->data));
return *this;
}
/**
* multiplication by a vector
*/
Vector<3> operator *(const Vector<3> &v) const
{
Vector<3> res(data[0][0] * v.data[0] + data[0][1] * v.data[1] + data[0][2] * v.data[2],
data[1][0] * v.data[0] + data[1][1] * v.data[1] + data[1][2] * v.data[2],
data[2][0] * v.data[0] + data[2][1] * v.data[1] + data[2][2] * v.data[2]);
return res;
}
/**
* create a rotation matrix from given euler angles
* based on http://gentlenav.googlecode.com/files/EulerAngles.pdf
*/
void from_euler(float roll, float pitch, float yaw)
{
float cp = cosf(pitch);
float sp = sinf(pitch);
float sr = sinf(roll);
float cr = cosf(roll);
float sy = sinf(yaw);
float cy = cosf(yaw);
data[0][0] = cp * cy;
data[0][1] = (sr * sp * cy) - (cr * sy);
data[0][2] = (cr * sp * cy) + (sr * sy);
data[1][0] = cp * sy;
data[1][1] = (sr * sp * sy) + (cr * cy);
data[1][2] = (cr * sp * sy) - (sr * cy);
data[2][0] = -sp;
data[2][1] = sr * cp;
data[2][2] = cr * cp;
}
/**
* get euler angles from rotation matrix
*/
Vector<3> to_euler() const
{
Vector<3> euler;
euler.data[1] = asinf(-data[2][0]);
if (fabsf(euler.data[1] - M_PI_2_F) < 1.0e-3f) {
euler.data[0] = 0.0f;
euler.data[2] = atan2f(data[1][2] - data[0][1], data[0][2] + data[1][1]) + euler.data[0];
} else if (fabsf(euler.data[1] + M_PI_2_F) < 1.0e-3f) {
euler.data[0] = 0.0f;
euler.data[2] = atan2f(data[1][2] - data[0][1], data[0][2] + data[1][1]) - euler.data[0];
} else {
euler.data[0] = atan2f(data[2][1], data[2][2]);
euler.data[2] = atan2f(data[1][0], data[0][0]);
}
return euler;
}
};
}
#endif // MATRIX_HPP

310
src/lib/mathlib/math/Quaternion.hpp
View File

@ -1,310 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file Quaternion.hpp
*
* Quaternion class
*
* @author Anton Babushkin <anton.babushkin@me.com>
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
* @author Lorenz Meier <lorenz@px4.io>
*/
#ifndef QUATERNION_HPP
#define QUATERNION_HPP
#include <math.h>
#include "Vector.hpp"
#include "Matrix.hpp"
namespace math
{
class __EXPORT Quaternion : public Vector<4>
{
public:
/**
* trivial ctor
*/
Quaternion() : Vector<4>() {}
/**
* copy ctor
*/
Quaternion(const Quaternion &q) : Vector<4>(q) {}
/**
* casting from vector
*/
Quaternion(const Vector<4> &v) : Vector<4>(v) {}
/**
* setting ctor
*/
Quaternion(const float d[4]) : Vector<4>(d) {}
/**
* setting ctor
*/
Quaternion(const float a0, const float b0, const float c0, const float d0): Vector<4>(a0, b0, c0, d0) {}
using Vector<4>::operator *;
/**
* multiplication
*/
const Quaternion operator *(const Quaternion &q) const
{
return Quaternion(
data[0] * q.data[0] - data[1] * q.data[1] - data[2] * q.data[2] - data[3] * q.data[3],
data[0] * q.data[1] + data[1] * q.data[0] + data[2] * q.data[3] - data[3] * q.data[2],
data[0] * q.data[2] - data[1] * q.data[3] + data[2] * q.data[0] + data[3] * q.data[1],
data[0] * q.data[3] + data[1] * q.data[2] - data[2] * q.data[1] + data[3] * q.data[0]);
}
/**
* division
*/
Quaternion operator /(const Quaternion &q) const
{
float norm = q.length_squared();
return Quaternion(
(data[0] * q.data[0] + data[1] * q.data[1] + data[2] * q.data[2] + data[3] * q.data[3]) / norm,
(- data[0] * q.data[1] + data[1] * q.data[0] - data[2] * q.data[3] + data[3] * q.data[2]) / norm,
(- data[0] * q.data[2] + data[1] * q.data[3] + data[2] * q.data[0] - data[3] * q.data[1]) / norm,
(- data[0] * q.data[3] - data[1] * q.data[2] + data[2] * q.data[1] + data[3] * q.data[0]) / norm
);
}
/**
* derivative
*/
const Quaternion derivative(const Vector<3> &w)
{
float dataQ[] = {
data[0], -data[1], -data[2], -data[3],
data[1], data[0], -data[3], data[2],
data[2], data[3], data[0], -data[1],
data[3], -data[2], data[1], data[0]
};
Matrix<4, 4> Q(dataQ);
Vector<4> v(0.0f, w.data[0], w.data[1], w.data[2]);
return Q * v * 0.5f;
}
/**
* conjugate
*/
Quaternion conjugated() const
{
return Quaternion(data[0], -data[1], -data[2], -data[3]);
}
/**
* inversed
*/
Quaternion inversed() const
{
float norm = length_squared();
return Quaternion(data[0] / norm, -data[1] / norm, -data[2] / norm, -data[3] / norm);
}
/**
* conjugation
*/
Vector<3> conjugate(const Vector<3> &v) const
{
float q0q0 = data[0] * data[0];
float q1q1 = data[1] * data[1];
float q2q2 = data[2] * data[2];
float q3q3 = data[3] * data[3];
return Vector<3>(
v.data[0] * (q0q0 + q1q1 - q2q2 - q3q3) +
v.data[1] * 2.0f * (data[1] * data[2] - data[0] * data[3]) +
v.data[2] * 2.0f * (data[0] * data[2] + data[1] * data[3]),
v.data[0] * 2.0f * (data[1] * data[2] + data[0] * data[3]) +
v.data[1] * (q0q0 - q1q1 + q2q2 - q3q3) +
v.data[2] * 2.0f * (data[2] * data[3] - data[0] * data[1]),
v.data[0] * 2.0f * (data[1] * data[3] - data[0] * data[2]) +
v.data[1] * 2.0f * (data[0] * data[1] + data[2] * data[3]) +
v.data[2] * (q0q0 - q1q1 - q2q2 + q3q3)
);
}
/**
* conjugation with inversed quaternion
*/
Vector<3> conjugate_inversed(const Vector<3> &v) const
{
float q0q0 = data[0] * data[0];
float q1q1 = data[1] * data[1];
float q2q2 = data[2] * data[2];
float q3q3 = data[3] * data[3];
return Vector<3>(
v.data[0] * (q0q0 + q1q1 - q2q2 - q3q3) +
v.data[1] * 2.0f * (data[1] * data[2] + data[0] * data[3]) +
v.data[2] * 2.0f * (data[1] * data[3] - data[0] * data[2]),
v.data[0] * 2.0f * (data[1] * data[2] - data[0] * data[3]) +
v.data[1] * (q0q0 - q1q1 + q2q2 - q3q3) +
v.data[2] * 2.0f * (data[2] * data[3] + data[0] * data[1]),
v.data[0] * 2.0f * (data[1] * data[3] + data[0] * data[2]) +
v.data[1] * 2.0f * (data[2] * data[3] - data[0] * data[1]) +
v.data[2] * (q0q0 - q1q1 - q2q2 + q3q3)
);
}
/**
* imaginary part of quaternion
*/
Vector<3> imag()
{
return Vector<3>(&data[1]);
}
/**
* set quaternion to rotation defined by euler angles
*/
void from_euler(float roll, float pitch, float yaw)
{
double cosPhi_2 = cos(double(roll) / 2.0);
double sinPhi_2 = sin(double(roll) / 2.0);
double cosTheta_2 = cos(double(pitch) / 2.0);
double sinTheta_2 = sin(double(pitch) / 2.0);
double cosPsi_2 = cos(double(yaw) / 2.0);
double sinPsi_2 = sin(double(yaw) / 2.0);
/* operations executed in double to avoid loss of precision through
* consecutive multiplications. Result stored as float.
*/
data[0] = static_cast<float>(cosPhi_2 * cosTheta_2 * cosPsi_2 + sinPhi_2 * sinTheta_2 * sinPsi_2);
data[1] = static_cast<float>(sinPhi_2 * cosTheta_2 * cosPsi_2 - cosPhi_2 * sinTheta_2 * sinPsi_2);
data[2] = static_cast<float>(cosPhi_2 * sinTheta_2 * cosPsi_2 + sinPhi_2 * cosTheta_2 * sinPsi_2);
data[3] = static_cast<float>(cosPhi_2 * cosTheta_2 * sinPsi_2 - sinPhi_2 * sinTheta_2 * cosPsi_2);
}
/**
* simplified version of the above method to create quaternion representing rotation only by yaw
*/
void from_yaw(float yaw)
{
data[0] = cosf(yaw / 2.0f);
data[1] = 0.0f;
data[2] = 0.0f;
data[3] = sinf(yaw / 2.0f);
}
/**
* create Euler angles vector from the quaternion
*/
Vector<3> to_euler() const
{
return Vector<3>(
atan2f(2.0f * (data[0] * data[1] + data[2] * data[3]), 1.0f - 2.0f * (data[1] * data[1] + data[2] * data[2])),
asinf(2.0f * (data[0] * data[2] - data[3] * data[1])),
atan2f(2.0f * (data[0] * data[3] + data[1] * data[2]), 1.0f - 2.0f * (data[2] * data[2] + data[3] * data[3]))
);
}
/**
* set quaternion to rotation by DCM
* Reference: Shoemake, Quaternions, http://www.cs.ucr.edu/~vbz/resources/quatut.pdf
*/
void from_dcm(const Matrix<3, 3> &dcm)
{
float tr = dcm.data[0][0] + dcm.data[1][1] + dcm.data[2][2];
if (tr > 0.0f) {
float s = sqrtf(tr + 1.0f);
data[0] = s * 0.5f;
s = 0.5f / s;
data[1] = (dcm.data[2][1] - dcm.data[1][2]) * s;
data[2] = (dcm.data[0][2] - dcm.data[2][0]) * s;
data[3] = (dcm.data[1][0] - dcm.data[0][1]) * s;
} else {
/* Find maximum diagonal element in dcm
* store index in dcm_i */
int dcm_i = 0;
for (int i = 1; i < 3; i++) {
if (dcm.data[i][i] > dcm.data[dcm_i][dcm_i]) {
dcm_i = i;
}
}
int dcm_j = (dcm_i + 1) % 3;
int dcm_k = (dcm_i + 2) % 3;
float s = sqrtf((dcm.data[dcm_i][dcm_i] - dcm.data[dcm_j][dcm_j] -
dcm.data[dcm_k][dcm_k]) + 1.0f);
data[dcm_i + 1] = s * 0.5f;
s = 0.5f / s;
data[dcm_j + 1] = (dcm.data[dcm_i][dcm_j] + dcm.data[dcm_j][dcm_i]) * s;
data[dcm_k + 1] = (dcm.data[dcm_k][dcm_i] + dcm.data[dcm_i][dcm_k]) * s;
data[0] = (dcm.data[dcm_k][dcm_j] - dcm.data[dcm_j][dcm_k]) * s;
}
}
/**
* create rotation matrix for the quaternion
*/
Matrix<3, 3> to_dcm() const
{
Matrix<3, 3> R;
float aSq = data[0] * data[0];
float bSq = data[1] * data[1];
float cSq = data[2] * data[2];
float dSq = data[3] * data[3];
R.data[0][0] = aSq + bSq - cSq - dSq;
R.data[0][1] = 2.0f * (data[1] * data[2] - data[0] * data[3]);
R.data[0][2] = 2.0f * (data[0] * data[2] + data[1] * data[3]);
R.data[1][0] = 2.0f * (data[1] * data[2] + data[0] * data[3]);
R.data[1][1] = aSq - bSq + cSq - dSq;
R.data[1][2] = 2.0f * (data[2] * data[3] - data[0] * data[1]);
R.data[2][0] = 2.0f * (data[1] * data[3] - data[0] * data[2]);
R.data[2][1] = 2.0f * (data[0] * data[1] + data[2] * data[3]);
R.data[2][2] = aSq - bSq - cSq + dSq;
return R;
}
};
}
#endif // QUATERNION_HPP

608
src/lib/mathlib/math/Vector.hpp
View File

@ -1,608 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Anton Babushkin <anton.babushkin@me.com>
* Pavel Kirienko <pavel.kirienko@gmail.com>
* Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file Vector.hpp
*
* Vector class
*/
#ifndef VECTOR_HPP
#define VECTOR_HPP
#include <stdio.h>
#include <math.h>
#include <string.h>
#include <platforms/ros/eigen_math.h>
#include <platforms/px4_defines.h>
namespace math
{
template <unsigned int N>
class __EXPORT Vector;
template <unsigned int N>
class __EXPORT VectorBase
{
public:
/**
* vector data
*/
float data[N];
/**
* struct for using arm_math functions, represents column vector
*/
eigen_matrix_instance arm_col;
/**
* trivial ctor
* initializes elements to zero
*/
VectorBase() :
data{},
arm_col{N, 1, &data[0]}
{
}
~VectorBase() = default;
/**
* copy ctor
*/
VectorBase(const VectorBase<N> &v) :
arm_col{N, 1, &data[0]}
{
memcpy(data, v.data, sizeof(data));
}
/**
* setting ctor
*/
VectorBase(const float d[N]) :
arm_col{N, 1, &data[0]}
{
memcpy(data, d, sizeof(data));
}
/**
* set data
*/
void set(const float d[N])
{
memcpy(data, d, sizeof(data));
}
/**
* access to elements by index
*/
float &operator()(const unsigned int i)
{
return data[i];
}
/**
* access to elements by index
*/
float operator()(const unsigned int i) const
{
return data[i];
}
/**
* get vector size
*/
unsigned int get_size() const
{
return N;
}
/**
* test for equality
*/
bool operator ==(const Vector<N> &v) const
{
for (unsigned int i = 0; i < N; i++) {
if (data[i] != v.data[i]) {
return false;
}
}
return true;
}
/**
* test for inequality
*/
bool operator !=(const Vector<N> &v) const
{
for (unsigned int i = 0; i < N; i++) {
if (data[i] != v.data[i]) {
return true;
}
}
return false;
}
/**
* set to value
*/
const Vector<N> &operator =(const Vector<N> &v)
{
memcpy(data, v.data, sizeof(data));
return *static_cast<const Vector<N>*>(this);
}
/**
* negation
*/
const Vector<N> operator -() const
{
Vector<N> res;
for (unsigned int i = 0; i < N; i++) {
res.data[i] = -data[i];
}
return res;
}
/**
* addition
*/
const Vector<N> operator +(const Vector<N> &v) const
{
Vector<N> res;
for (unsigned int i = 0; i < N; i++) {
res.data[i] = data[i] + v.data[i];
}
return res;
}
/**
* subtraction
*/
const Vector<N> operator -(const Vector<N> &v) const
{
Vector<N> res;
for (unsigned int i = 0; i < N; i++) {
res.data[i] = data[i] - v.data[i];
}
return res;
}
/**
* uniform scaling
*/
const Vector<N> operator *(const float num) const
{
Vector<N> res;
for (unsigned int i = 0; i < N; i++) {
res.data[i] = data[i] * num;
}
return res;
}
/**
* uniform scaling
*/
const Vector<N> operator /(const float num) const
{
Vector<N> res;
for (unsigned int i = 0; i < N; i++) {
res.data[i] = data[i] / num;
}
return res;
}
/**
* addition
*/
const Vector<N> &operator +=(const Vector<N> &v)
{
for (unsigned int i = 0; i < N; i++) {
data[i] += v.data[i];
}
return *static_cast<const Vector<N>*>(this);
}
/**
* subtraction
*/
const Vector<N> &operator -=(const Vector<N> &v)
{
for (unsigned int i = 0; i < N; i++) {
data[i] -= v.data[i];
}
return *static_cast<const Vector<N>*>(this);
}
/**
* uniform scaling
*/
const Vector<N> &operator *=(const float num)
{
for (unsigned int i = 0; i < N; i++) {
data[i] *= num;
}
return *static_cast<const Vector<N>*>(this);
}
/**
* uniform scaling
*/
const Vector<N> &operator /=(const float num)
{
for (unsigned int i = 0; i < N; i++) {
data[i] /= num;
}
return *static_cast<const Vector<N>*>(this);
}
/**
* dot product
*/
float operator *(const Vector<N> &v) const
{
float res = 0.0f;
for (unsigned int i = 0; i < N; i++) {
res += data[i] * v.data[i];
}
return res;
}
/**
* element by element multiplication
*/
const Vector<N> emult(const Vector<N> &v) const
{
Vector<N> res;
for (unsigned int i = 0; i < N; i++) {
res.data[i] = data[i] * v.data[i];
}
return res;
}
/**
* element by element division
*/
const Vector<N> edivide(const Vector<N> &v) const
{
Vector<N> res;
for (unsigned int i = 0; i < N; i++) {
res.data[i] = data[i] / v.data[i];
}
return res;
}
/**
* gets the length of this vector squared
*/
float length_squared() const
{
float res = 0.0f;
for (unsigned int i = 0; i < N; i++) {
res += data[i] * data[i];
}
return res;
}
/**
* gets the length of this vector
*/
float length() const
{
float res = 0.0f;
for (unsigned int i = 0; i < N; i++) {
res += data[i] * data[i];
}
return sqrtf(res);
}
/**
* normalizes this vector
*/
void normalize()
{
*this /= length();
}
/**
* returns the normalized version of this vector
*/
Vector<N> normalized() const
{
return *this / length();
}
/**
* set zero vector
*/
void zero()
{
memset(data, 0, sizeof(data));
}
void print()
{
printf("[ ");
for (unsigned int i = 0; i < N; i++) {
printf("%.3f\t", (double)data[i]);
}
printf("]\n");
}
};
template <unsigned int N>
class __EXPORT Vector : public VectorBase<N>
{
public:
Vector() : VectorBase<N>() {}
Vector(const Vector<N> &v) : VectorBase<N>(v) {}
Vector(const float d[N]) : VectorBase<N>(d) {}
/**
* set to value
*/
const Vector<N> &operator =(const Vector<N> &v)
{
memcpy(this->data, v.data, sizeof(this->data));
return *this;
}
};
template <>
class __EXPORT Vector<2> : public VectorBase<2>
{
public:
Vector() : VectorBase<2>() {}
// simple copy is 1.6 times faster than memcpy
Vector(const Vector<2> &v) : VectorBase<2>()
{
data[0] = v.data[0];
data[1] = v.data[1];
}
Vector(const float d[2]) : VectorBase<2>()
{
data[0] = d[0];
data[1] = d[1];
}
Vector(const float x, const float y) : VectorBase<2>()
{
data[0] = x;
data[1] = y;
}
/**
* set data
*/
void set(const float d[2])
{
data[0] = d[0];
data[1] = d[1];
}
#if defined(__PX4_ROS)
/**
* set data from boost::array
*/
void set(const boost::array<float, 2ul> d)
{
set(static_cast<const float *>(d.data()));
}
#endif
/**
* set to value
*/
const Vector<2> &operator =(const Vector<2> &v)
{
data[0] = v.data[0];
data[1] = v.data[1];
return *this;
}
float operator %(const Vector<2> &v) const
{
return data[0] * v.data[1] - data[1] * v.data[0];
}
};
template <>
class __EXPORT Vector<3> : public VectorBase<3>
{
public:
Vector() : VectorBase<3>() {}
// simple copy is 1.6 times faster than memcpy
Vector(const Vector<3> &v) : VectorBase<3>()
{
for (unsigned int i = 0; i < 3; i++) {
data[i] = v.data[i];
}
}
Vector(const float d[3]) : VectorBase<3>()
{
for (unsigned int i = 0; i < 3; i++) {
data[i] = d[i];
}
}
Vector(const float x, const float y, const float z) : VectorBase<3>()
{
data[0] = x;
data[1] = y;
data[2] = z;
}
#if defined(__PX4_ROS)
/**
* set data from boost::array
*/
void set(const boost::array<float, 3ul> d)
{
set(static_cast<const float *>(d.data()));
}
#endif
/**
* set data
*/
void set(const float d[3])
{
for (unsigned int i = 0; i < 3; i++) {
data[i] = d[i];
}
}
/**
* set to value
*/
const Vector<3> &operator =(const Vector<3> &v)
{
for (unsigned int i = 0; i < 3; i++) {
data[i] = v.data[i];
}
return *this;
}
Vector<3> operator %(const Vector<3> &v) const
{
return Vector<3>(
data[1] * v.data[2] - data[2] * v.data[1],
data[2] * v.data[0] - data[0] * v.data[2],
data[0] * v.data[1] - data[1] * v.data[0]
);
}
};
template <>
class __EXPORT Vector<4> : public VectorBase<4>
{
public:
Vector() : VectorBase() {}
Vector(const Vector<4> &v) : VectorBase<4>()
{
for (unsigned int i = 0; i < 4; i++) {
data[i] = v.data[i];
}
}
Vector(const float d[4]) : VectorBase<4>()
{
for (unsigned int i = 0; i < 4; i++) {
data[i] = d[i];
}
}
Vector(const float x0, const float x1, const float x2, const float x3) : VectorBase()
{
data[0] = x0;
data[1] = x1;
data[2] = x2;
data[3] = x3;
}
#if defined(__PX4_ROS)
/**
* set data from boost::array
*/
void set(const boost::array<float, 4ul> d)
{
set(static_cast<const float *>(d.data()));
}
#endif
/**
* set data
*/
void set(const float d[4])
{
for (unsigned int i = 0; i < 4; i++) {
data[i] = d[i];
}
}
/**
* set to value
*/
const Vector<4> &operator =(const Vector<4> &v)
{
for (unsigned int i = 0; i < 4; i++) {
data[i] = v.data[i];
}
return *this;
}
};
}
#endif // VECTOR_HPP

BIN
src/lib/mathlib/math/nasa_rotation_def.pdf
View File

Binary file not shown.

63
src/lib/mathlib/math/test_math.sce
View File

@ -1,63 +0,0 @@
clc
clear
function out = float_truncate(in, digits)
out = round(in*10^digits)
out = out/10^digits
endfunction
phi = 0.1
theta = 0.2
psi = 0.3
cosPhi = cos(phi)
cosPhi_2 = cos(phi/2)
sinPhi = sin(phi)
sinPhi_2 = sin(phi/2)
cosTheta = cos(theta)
cosTheta_2 = cos(theta/2)
sinTheta = sin(theta)
sinTheta_2 = sin(theta/2)
cosPsi = cos(psi)
cosPsi_2 = cos(psi/2)
sinPsi = sin(psi)
sinPsi_2 = sin(psi/2)
C_nb = [cosTheta*cosPsi, -cosPhi*sinPsi + sinPhi*sinTheta*cosPsi, sinPhi*sinPsi + cosPhi*sinTheta*cosPsi;
cosTheta*sinPsi, cosPhi*cosPsi + sinPhi*sinTheta*sinPsi, -sinPhi*cosPsi + cosPhi*sinTheta*sinPsi;
-sinTheta, sinPhi*cosTheta, cosPhi*cosTheta]
disp(C_nb)
//C_nb = float_truncate(C_nb,3)
//disp(C_nb)
theta = asin(-C_nb(3,1))
phi = atan(C_nb(3,2), C_nb(3,3))
psi = atan(C_nb(2,1), C_nb(1,1))
printf('phi %f\n', phi)
printf('theta %f\n', theta)
printf('psi %f\n', psi)
q = [cosPhi_2*cosTheta_2*cosPsi_2 + sinPhi_2*sinTheta_2*sinPsi_2;
sinPhi_2*cosTheta_2*cosPsi_2 - cosPhi_2*sinTheta_2*sinPsi_2;
cosPhi_2*sinTheta_2*cosPsi_2 + sinPhi_2*cosTheta_2*sinPsi_2;
cosPhi_2*cosTheta_2*sinPsi_2 - sinPhi_2*sinTheta_2*cosPsi_2]
//q = float_truncate(q,3)
a = q(1)
b = q(2)
c = q(3)
d = q(4)
printf('q: %f %f %f %f\n', a, b, c, d)
a2 = a*a
b2 = b*b
c2 = c*c
d2 = d*d
C2_nb = [a2 + b2 - c2 - d2, 2*(b*c - a*d), 2*(b*d + a*c);
2*(b*c + a*d), a2 - b2 + c2 - d2, 2*(c*d - a*b);
2*(b*d - a*c), 2*(c*d + a*b), a2 - b2 - c2 + d2]
disp(C2_nb)

3
src/lib/mathlib/mathlib.h
View File

@ -41,9 +41,6 @@
#ifdef __cplusplus
#include "math/Vector.hpp"
#include "math/Matrix.hpp"
#include "math/Quaternion.hpp"
#include "math/Limits.hpp"
#include "math/Functions.hpp"
#include "math/matrix_alg.h"

1
src/lib/mixer/mixer_multirotor.cpp
View File

@ -40,6 +40,7 @@
#include "mixer.h"
#include <cfloat>
#include <cstring>
#include <mathlib/mathlib.h>

2
src/lib/terrain_estimation/terrain_estimator.h
View File

@ -38,7 +38,7 @@
#pragma once
#include <lib/mathlib/mathlib.h>
#include "matrix/Matrix.hpp"
#include <matrix/math.hpp>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_attitude.h>

1
src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
View File

@ -45,6 +45,7 @@
#include <lib/ecl/geo_lookup/geo_mag_declination.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp>
#include <mathlib/mathlib.h>
#include <matrix/math.hpp>
#include <px4_config.h>
#include <px4_posix.h>
#include <px4_tasks.h>

40
src/modules/commander/calibration_routines.cpp
View File

@ -51,6 +51,7 @@
#include <lib/ecl/geo/geo.h>
#include <string.h>
#include <mathlib/mathlib.h>
#include <matrix/math.hpp>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/sensor_combined.h>
@ -270,13 +271,10 @@ int run_lm_sphere_fit(const float x[], const float y[], const float z[], float &
float fitness = _fitness;
float fit1 = 0.0f, fit2 = 0.0f;
float JTJ[16];
float JTJ2[16];
float JTFI[4];
matrix::SquareMatrix<float, 4> JTJ;
matrix::SquareMatrix<float, 4> JTJ2;
float JTFI[4] = {};
float residual = 0.0f;
memset(JTJ, 0, sizeof(JTJ));
memset(JTJ2, 0, sizeof(JTJ2));
memset(JTFI, 0, sizeof(JTFI));
// Gauss Newton Part common for all kind of extensions including LM
for (uint16_t k = 0; k < _samples_collected; k++) {
@ -299,8 +297,8 @@ int run_lm_sphere_fit(const float x[], const float y[], const float z[], float &
for (uint8_t i = 0; i < 4; i++) {
// compute JTJ
for (uint8_t j = 0; j < 4; j++) {
JTJ[i * 4 + j] += sphere_jacob[i] * sphere_jacob[j];
JTJ2[i * 4 + j] += sphere_jacob[i] * sphere_jacob[j]; //a backup JTJ for LM
JTJ(i, j) += sphere_jacob[i] * sphere_jacob[j];
JTJ2(i, j) += sphere_jacob[i] * sphere_jacob[j]; //a backup JTJ for LM
}
JTFI[i] += sphere_jacob[i] * residual;
@ -315,22 +313,22 @@ int run_lm_sphere_fit(const float x[], const float y[], const float z[], float &
memcpy(fit2_params, fit1_params, sizeof(fit1_params));
for (uint8_t i = 0; i < 4; i++) {
JTJ[i * 4 + i] += _sphere_lambda;
JTJ2[i * 4 + i] += _sphere_lambda / lma_damping;
JTJ(i, i) += _sphere_lambda;
JTJ2(i, i) += _sphere_lambda / lma_damping;
}
if (!inverse4x4(JTJ, JTJ)) {
if (!JTJ.I(JTJ)) {
return -1;
}
if (!inverse4x4(JTJ2, JTJ2)) {
if (!JTJ2.I(JTJ2)) {
return -1;
}
for (uint8_t row = 0; row < 4; row++) {
for (uint8_t col = 0; col < 4; col++) {
fit1_params[row] -= JTFI[col] * JTJ[row * 4 + col];
fit2_params[row] -= JTFI[col] * JTJ2[row * 4 + col];
fit1_params[row] -= JTFI[col] * JTJ(row, col);
fit2_params[row] -= JTFI[col] * JTJ2(row, col);
}
}
@ -395,15 +393,13 @@ int run_lm_ellipsoid_fit(const float x[], const float y[], const float z[], floa
const float lma_damping = 10.0f;
float _samples_collected = size;
float fitness = _fitness;
float fit1 = 0.0f, fit2 = 0.0f;
float fit1 = 0.0f;
float fit2 = 0.0f;
float JTJ[81];
float JTJ2[81];
float JTFI[9];
float JTJ[81] = {};
float JTJ2[81] = {};
float JTFI[9] = {};
float residual = 0.0f;
memset(JTJ, 0, sizeof(JTJ));
memset(JTJ2, 0, sizeof(JTJ2));
memset(JTFI, 0, sizeof(JTFI));
float ellipsoid_jacob[9];
// Gauss Newton Part common for all kind of extensions including LM
@ -461,8 +457,6 @@ int run_lm_ellipsoid_fit(const float x[], const float y[], const float z[], floa
return -1;
}
for (uint8_t row = 0; row < 9; row++) {
for (uint8_t col = 0; col < 9; col++) {
fit1_params[row] -= JTFI[col] * JTJ[row * 9 + col];

1
src/modules/fw_att_control/FixedwingAttitudeControl.hpp
View File

@ -39,6 +39,7 @@
#include <ecl/attitude_fw/ecl_yaw_controller.h>
#include <lib/ecl/geo/geo.h>
#include <mathlib/mathlib.h>
#include <matrix/math.hpp>
#include <px4_config.h>
#include <px4_defines.h>
#include <px4_posix.h>

1
src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h
View File

@ -49,6 +49,7 @@
#include <px4_module_params.h>
#include <systemlib/mavlink_log.h>
#include <mathlib/mathlib.h>
#include <matrix/math.hpp>
namespace runwaytakeoff
{

1
src/modules/gnd_att_control/CMakeLists.txt
View File

@ -37,5 +37,4 @@ px4_add_module(
COMPILE_FLAGS
SRCS
GroundRoverAttitudeControl.cpp
DEPENDS
)

1
src/modules/gnd_att_control/GroundRoverAttitudeControl.hpp
View File

@ -48,6 +48,7 @@
#include <drivers/drv_hrt.h>
#include <mathlib/mathlib.h>
#include <matrix/math.hpp>
#include <parameters/param.h>
#include <systemlib/pid/pid.h>
#include <perf/perf_counter.h>

1
src/modules/logger/log_writer_mavlink.cpp
View File

@ -38,6 +38,7 @@
#include <mathlib/mathlib.h>
#include <px4_log.h>
#include <px4_posix.h>
#include <cstring>
namespace px4
{

1
src/modules/mavlink/mavlink_messages.cpp
View File

@ -55,6 +55,7 @@
#include <drivers/drv_rc_input.h>
#include <lib/ecl/geo/geo.h>
#include <mathlib/mathlib.h>
#include <matrix/math.hpp>
#include <px4_time.h>
#include <systemlib/err.h>
#include <systemlib/mavlink_log.h>

1
src/modules/mc_pos_control/mc_pos_control_tests/mc_pos_control_tests.cpp
View File

@ -42,6 +42,7 @@
#include <systemlib/err.h>
#include <unit_test.h>
#include <mathlib/mathlib.h>
#include <matrix/math.hpp>
extern "C" __EXPORT int mc_pos_control_tests_main(int argc, char *argv[]);

5
src/modules/navigator/follow_target.h
View File

@ -40,11 +40,12 @@
#pragma once
#include <mathlib/mathlib.h>
#include "navigator_mode.h"
#include "mission_block.h"
#include <mathlib/mathlib.h>
#include <matrix/math.hpp>
#include <px4_module_params.h>
#include <uORB/topics/follow_target.h>

1
src/modules/sensors/temperature_compensation.h
View File

@ -44,6 +44,7 @@
#include <parameters/param.h>
#include <mathlib/mathlib.h>
#include <matrix/math.hpp>
#include "common.h"

6
src/modules/vtol_att_control/tailsitter.cpp
View File

@ -242,9 +242,9 @@ void Tailsitter::update_transition_state()
_v_att_sp->roll_body = 0.0f;
_v_att_sp->yaw_body = _yaw_transition;
math::Quaternion q_sp;
q_sp.from_euler(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body);
memcpy(&_v_att_sp->q_d[0], &q_sp.data[0], sizeof(_v_att_sp->q_d));
matrix::Quatf q_sp = matrix::Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body);
q_sp.copyTo(_v_att_sp->q_d);
_v_att_sp->q_d_valid = true;
}
void Tailsitter::waiting_on_tecs()

3
src/modules/vtol_att_control/vtol_att_control_main.h
View File

@ -58,7 +58,8 @@
#include <drivers/drv_hrt.h>
#include <drivers/drv_pwm_output.h>
#include <lib/ecl/geo/geo.h>
#include <lib/mathlib/mathlib.h>
#include <mathlib/mathlib.h>
#include <matrix/math.hpp>
#include <parameters/param.h>
#include <uORB/topics/actuator_controls.h>

2
src/modules/wind_estimator/wind_estimator_main.cpp
View File

@ -32,8 +32,8 @@
****************************************************************************/
#include <drivers/drv_hrt.h>
#include <matrix/math.hpp>
#include <ecl/airdata/WindEstimator.hpp>
#include <matrix/matrix/math.hpp>
#include <px4_module.h>
#include <px4_module_params.h>
#include <px4_workqueue.h>

232
src/systemcmds/tests/test_mathlib.cpp
View File

@ -53,6 +53,7 @@
#include <mathlib/mathlib.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
#include <matrix/math.hpp>
#include "tests_main.h"
@ -64,7 +65,6 @@ public:
private:
bool testVector2();
bool testVector3();
bool testVector4();
bool testVector10();
bool testMatrix3x3();
bool testMatrix10x10();
@ -82,21 +82,21 @@ using namespace math;
bool MathlibTest::testVector2()
{
{
Vector<2> v;
Vector<2> v1(1.0f, 2.0f);
Vector<2> v2(1.0f, -1.0f);
matrix::Vector2f v;
matrix::Vector2f v1(1.0f, 2.0f);
matrix::Vector2f v2(1.0f, -1.0f);
float data[2] = {1.0f, 2.0f};
TEST_OP("Constructor Vector<2>()", Vector<2> v3);
TEST_OP("Constructor Vector<2>(Vector<2>)", Vector<2> v3(v1); ut_assert_true(v3 == v1); v3.zero());
TEST_OP("Constructor Vector<2>(float[])", Vector<2> v3(data));
TEST_OP("Constructor Vector<2>(float, float)", Vector<2> v3(1.0f, 2.0f));
TEST_OP("Vector<2> = Vector<2>", v = v1);
TEST_OP("Vector<2> + Vector<2>", v + v1);
TEST_OP("Vector<2> - Vector<2>", v - v1);
TEST_OP("Vector<2> += Vector<2>", v += v1);
TEST_OP("Vector<2> -= Vector<2>", v -= v1);
TEST_OP("Vector<2> * Vector<2>", v * v1);
TEST_OP("Vector<2> %% Vector<2>", v1 % v2);
TEST_OP("Constructor matrix::Vector2f()", matrix::Vector2f v3);
TEST_OP("Constructor matrix::Vector2f(matrix::Vector2f)", matrix::Vector2f v3(v1); ut_assert_true(v3 == v1); v3.zero());
TEST_OP("Constructor matrix::Vector2f(float[])", matrix::Vector2f v3(data));
TEST_OP("Constructor matrix::Vector2f(float, float)", matrix::Vector2f v3(1.0f, 2.0f));
TEST_OP("matrix::Vector2f = matrix::Vector2f", v = v1);
TEST_OP("matrix::Vector2f + matrix::Vector2f", v + v1);
TEST_OP("matrix::Vector2f - matrix::Vector2f", v - v1);
TEST_OP("matrix::Vector2f += matrix::Vector2f", v += v1);
TEST_OP("matrix::Vector2f -= matrix::Vector2f", v -= v1);
TEST_OP("matrix::Vector2f * matrix::Vector2f", v * v1);
TEST_OP("matrix::Vector2f %% matrix::Vector2f", v1 % v2);
}
return true;
}
@ -105,70 +105,33 @@ bool MathlibTest::testVector3()
{
{
Vector<3> v;
Vector<3> v1(1.0f, 2.0f, 0.0f);
Vector<3> v2(1.0f, -1.0f, 2.0f);
matrix::Vector3f v;
matrix::Vector3f v1(1.0f, 2.0f, 0.0f);
matrix::Vector3f v2(1.0f, -1.0f, 2.0f);
float data[3] = {1.0f, 2.0f, 3.0f};
TEST_OP("Constructor Vector<3>()", Vector<3> v3);
TEST_OP("Constructor Vector<3>(Vector<3>)", Vector<3> v3(v1); ut_assert_true(v3 == v1); v3.zero());
TEST_OP("Constructor Vector<3>(float[])", Vector<3> v3(data));
TEST_OP("Constructor Vector<3>(float, float, float)", Vector<3> v3(1.0f, 2.0f, 3.0f));
TEST_OP("Vector<3> = Vector<3>", v = v1);
TEST_OP("Vector<3> + Vector<3>", v + v1);
TEST_OP("Vector<3> - Vector<3>", v - v1);
TEST_OP("Vector<3> += Vector<3>", v += v1);
TEST_OP("Vector<3> -= Vector<3>", v -= v1);
TEST_OP("Vector<3> * float", v1 * 2.0f);
TEST_OP("Vector<3> / float", v1 / 2.0f);
TEST_OP("Vector<3> *= float", v1 *= 2.0f);
TEST_OP("Vector<3> /= float", v1 /= 2.0f);
TEST_OP("Vector<3> * Vector<3>", v * v1);
TEST_OP("Vector<3> %% Vector<3>", v1 % v2);
TEST_OP("Vector<3> length", v1.length());
TEST_OP("Vector<3> length squared", v1.length_squared());
TEST_OP("Constructor matrix::Vector3f()", matrix::Vector3f v3);
TEST_OP("Constructor matrix::Vector3f(matrix::Vector3f)", matrix::Vector3f v3(v1); ut_assert_true(v3 == v1); v3.zero());
TEST_OP("Constructor matrix::Vector3f(float[])", matrix::Vector3f v3(data));
TEST_OP("Constructor matrix::Vector3f(float, float, float)", matrix::Vector3f v3(1.0f, 2.0f, 3.0f));
TEST_OP("matrix::Vector3f = matrix::Vector3f", v = v1);
TEST_OP("matrix::Vector3f + matrix::Vector3f", v + v1);
TEST_OP("matrix::Vector3f - matrix::Vector3f", v - v1);
TEST_OP("matrix::Vector3f += matrix::Vector3f", v += v1);
TEST_OP("matrix::Vector3f -= matrix::Vector3f", v -= v1);
TEST_OP("matrix::Vector3f * float", v1 * 2.0f);
TEST_OP("matrix::Vector3f / float", v1 / 2.0f);
TEST_OP("matrix::Vector3f *= float", v1 *= 2.0f);
TEST_OP("matrix::Vector3f /= float", v1 /= 2.0f);
TEST_OP("matrix::Vector3f * matrix::Vector3f", v * v1);
TEST_OP("matrix::Vector3f %% matrix::Vector3f", v1 % v2);
TEST_OP("matrix::Vector3f length", v1.length());
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-variable"
// Need pragma here instead of moving variable out of TEST_OP and just reference because
// TEST_OP measures performance of vector operations.
TEST_OP("Vector<3> element read", volatile float a = v1(0));
TEST_OP("Vector<3> element read direct", volatile float a = v1.data[0]);
TEST_OP("matrix::Vector3f element read", volatile float a = v1(0));
#pragma GCC diagnostic pop
TEST_OP("Vector<3> element write", v1(0) = 1.0f);
TEST_OP("Vector<3> element write direct", v1.data[0] = 1.0f);
}
return true;
}
bool MathlibTest::testVector4()
{
{
Vector<4> v;
Vector<4> v1(1.0f, 2.0f, 0.0f, -1.0f);
Vector<4> v2(1.0f, -1.0f, 2.0f, 0.0f);
float data[4] = {1.0f, 2.0f, 3.0f, 4.0f};
TEST_OP("Constructor Vector<4>()", Vector<4> v3);
TEST_OP("Constructor Vector<4>(Vector<4>)", Vector<4> v3(v1); ut_assert_true(v3 == v1); v3.zero());
TEST_OP("Constructor Vector<4>(float[])", Vector<4> v3(data));
TEST_OP("Constructor Vector<4>(float, float, float, float)", Vector<4> v3(1.0f, 2.0f, 3.0f, 4.0f));
TEST_OP("Vector<4> = Vector<4>", v = v1);
TEST_OP("Vector<4> + Vector<4>", v + v1);
TEST_OP("Vector<4> - Vector<4>", v - v1);
TEST_OP("Vector<4> += Vector<4>", v += v1);
TEST_OP("Vector<4> -= Vector<4>", v -= v1);
TEST_OP("Vector<4> * Vector<4>", v * v1);
}
return true;
}
bool MathlibTest::testVector10()
{
{
Vector<10> v1;
v1.zero();
float data[10];
TEST_OP("Constructor Vector<10>()", Vector<10> v3);
TEST_OP("Constructor Vector<10>(Vector<10>)", Vector<10> v3(v1); ut_assert_true(v3 == v1); v3.zero());
TEST_OP("Constructor Vector<10>(float[])", Vector<10> v3(data));
TEST_OP("matrix::Vector3f element write", v1(0) = 1.0f);
}
return true;
}
@ -176,30 +139,14 @@ bool MathlibTest::testVector10()
bool MathlibTest::testMatrix3x3()
{
{
Matrix<3, 3> m1;
matrix::Matrix3f m1;
m1.identity();
Matrix<3, 3> m2;
matrix::Matrix3f m2;
m2.identity();
Vector<3> v1(1.0f, 2.0f, 0.0f);
TEST_OP("Matrix<3, 3> * Vector<3>", m1 * v1);
TEST_OP("Matrix<3, 3> + Matrix<3, 3>", m1 + m2);
TEST_OP("Matrix<3, 3> * Matrix<3, 3>", m1 * m2);
}
return true;
}
bool MathlibTest::testMatrix10x10()
{
{
Matrix<10, 10> m1;
m1.identity();
Matrix<10, 10> m2;
m2.identity();
Vector<10> v1;
v1.zero();
TEST_OP("Matrix<10, 10> * Vector<10>", m1 * v1);
TEST_OP("Matrix<10, 10> + Matrix<10, 10>", m1 + m2);
TEST_OP("Matrix<10, 10> * Matrix<10, 10>", m1 * m2);
matrix::Vector3f v1(1.0f, 2.0f, 0.0f);
TEST_OP("matrix::Matrix3f * matrix::Vector3f", m1 * v1);
TEST_OP("matrix::Matrix3f + matrix::Matrix3f", m1 + m2);
TEST_OP("matrix::Matrix3f * matrix::Matrix3f", m1 * m2);
}
return true;
}
@ -215,12 +162,12 @@ bool MathlibTest::testMatrixNonsymmetric()
float data2[2][3] = {{2, 4, 6}, {8, 10, 12}};
float data3[2][3] = {{3, 6, 9}, {12, 15, 18}};
Matrix<2, 3> m1(data1);
Matrix<2, 3> m2(data2);
Matrix<2, 3> m3(data3);
matrix::Matrix<float, 2, 3> m1(data1);
matrix::Matrix<float, 2, 3> m2(data2);
matrix::Matrix<float, 2, 3> m3(data3);
if (m1 + m2 != m3) {
PX4_ERR("Matrix<2, 3> + Matrix<2, 3> failed!");
PX4_ERR("matrix::Matrix<float, 2, 3> + matrix::Matrix<float, 2, 3> failed!");
(m1 + m2).print();
printf("!=\n");
m3.print();
@ -230,7 +177,7 @@ bool MathlibTest::testMatrixNonsymmetric()
ut_assert("m1 + m2 == m3", m1 + m2 == m3);
if (m3 - m2 != m1) {
PX4_ERR("Matrix<2, 3> - Matrix<2, 3> failed!");
PX4_ERR("matrix::Matrix<float, 2, 3> - matrix::Matrix<float, 2, 3> failed!");
(m3 - m2).print();
printf("!=\n");
m1.print();
@ -242,7 +189,7 @@ bool MathlibTest::testMatrixNonsymmetric()
m1 += m2;
if (m1 != m3) {
PX4_ERR("Matrix<2, 3> += Matrix<2, 3> failed!");
PX4_ERR("matrix::Matrix<float, 2, 3> += matrix::Matrix<float, 2, 3> failed!");
m1.print();
printf("!=\n");
m3.print();
@ -252,10 +199,10 @@ bool MathlibTest::testMatrixNonsymmetric()
ut_assert("m1 == m3", m1 == m3);
m1 -= m2;
Matrix<2, 3> m1_orig(data1);
matrix::Matrix<float, 2, 3> m1_orig(data1);
if (m1 != m1_orig) {
PX4_ERR("Matrix<2, 3> -= Matrix<2, 3> failed!");
PX4_ERR("matrix::Matrix<float, 2, 3> -= matrix::Matrix<float, 2, 3> failed!");
m1.print();
printf("!=\n");
m1_orig.print();
@ -271,9 +218,9 @@ bool MathlibTest::testMatrixNonsymmetric()
bool MathlibTest::testRotationMatrixQuaternion()
{
// test conversion rotation matrix to quaternion and back
math::Matrix<3, 3> R_orig;
math::Matrix<3, 3> R;
math::Quaternion q;
matrix::Dcmf R_orig;
matrix::Dcmf R;
matrix::Quatf q;
float diff = 0.2f;
float tol = 0.00001f;
@ -282,13 +229,13 @@ bool MathlibTest::testRotationMatrixQuaternion()
for (float roll = -M_PI_F; roll <= M_PI_F; roll += diff) {
for (float pitch = -M_PI_2_F; pitch <= M_PI_2_F; pitch += diff) {
for (float yaw = -M_PI_F; yaw <= M_PI_F; yaw += diff) {
R_orig.from_euler(roll, pitch, yaw);
q.from_dcm(R_orig);
R = q.to_dcm();
R_orig = matrix::Eulerf(roll, pitch, yaw);
q = R_orig;
R = q;
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
ut_assert("Quaternion method 'from_dcm' or 'to_dcm' outside tolerance!", fabsf(R_orig.data[i][j] - R.data[i][j]) < tol);
ut_assert("matrix::Quatf method 'from_dcm' or 'to_dcm' outside tolerance!", fabsf(R_orig(i, j) - R(i, j)) < tol);
}
}
}
@ -303,16 +250,16 @@ bool MathlibTest::testQuaternionfrom_dcm()
{
// test against some known values
float tol = 0.0001f;
math::Quaternion q_true = {1.0f, 0.0f, 0.0f, 0.0f};
matrix::Quatf q_true = {1.0f, 0.0f, 0.0f, 0.0f};
math::Matrix<3, 3> R_orig;
matrix::Matrix3f R_orig;
R_orig.identity();
math::Quaternion q;
matrix::Quatf q;
q.from_dcm(R_orig);
for (unsigned i = 0; i < 4; i++) {
ut_assert("Quaternion method 'from_dcm()' outside tolerance!", fabsf(q.data[i] - q_true.data[i]) < tol);
ut_assert("matrix::Quatf method 'from_dcm()' outside tolerance!", fabsf(q(i) - q_true(i)) < tol);
}
return true;
@ -321,33 +268,33 @@ bool MathlibTest::testQuaternionfrom_dcm()
bool MathlibTest::testQuaternionfrom_euler()
{
float tol = 0.0001f;
math::Quaternion q_true = {1.0f, 0.0f, 0.0f, 0.0f};
matrix::Quatf q_true = {1.0f, 0.0f, 0.0f, 0.0f};
math::Matrix<3, 3> R_orig;
matrix::Matrix3f R_orig;
R_orig.identity();
math::Quaternion q;
matrix::Quatf q;
q.from_dcm(R_orig);
q_true.from_euler(0.3f, 0.2f, 0.1f);
q_true = matrix::Eulerf(0.3f, 0.2f, 0.1f);
q = {0.9833f, 0.1436f, 0.1060f, 0.0343f};
for (unsigned i = 0; i < 4; i++) {
ut_assert("Quaternion method 'from_euler()' outside tolerance!", fabsf(q.data[i] - q_true.data[i]) < tol);
ut_assert("matrix::Quatf method 'from_euler()' outside tolerance!", fabsf(q(i) - q_true(i)) < tol);
}
q_true.from_euler(-1.5f, -0.2f, 0.5f);
q_true = matrix::Eulerf(-1.5f, -0.2f, 0.5f);
q = {0.7222f, -0.6391f, -0.2386f, 0.1142f};
for (unsigned i = 0; i < 4; i++) {
ut_assert("Quaternion method 'from_euler()' outside tolerance!", fabsf(q.data[i] - q_true.data[i]) < tol);
ut_assert("matrix::Quatf method 'from_euler()' outside tolerance!", fabsf(q(i) - q_true(i)) < tol);
}
q_true.from_euler(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3);
q_true = matrix::Eulerf(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3);
q = {0.6830f, 0.1830f, -0.6830f, 0.1830f};
for (unsigned i = 0; i < 4; i++) {
ut_assert("Quaternion method 'from_euler()' outside tolerance!", fabsf(q.data[i] - q_true.data[i]) < tol);
ut_assert("matrix::Quatf method 'from_euler()' outside tolerance!", fabsf(q(i) - q_true(i)) < tol);
}
return true;
@ -356,26 +303,26 @@ bool MathlibTest::testQuaternionfrom_euler()
bool MathlibTest::testQuaternionRotate()
{
// test quaternion method "rotate" (rotate vector by quaternion)
Vector<3> vector = {1.0f, 1.0f, 1.0f};
Vector<3> vector_q;
Vector<3> vector_r;
Quaternion q;
Matrix<3, 3> R;
matrix::Vector3f vector = {1.0f, 1.0f, 1.0f};
matrix::Vector3f vector_q;
matrix::Vector3f vector_r;
matrix::Quatf q;
matrix::Dcmf R;
float diff = 0.2f;
float tol = 0.00001f;
//PX4_INFO("Quaternion vector rotation method test.");
//PX4_INFO("matrix::Quatf vector rotation method test.");
for (float roll = -M_PI_F; roll <= M_PI_F; roll += diff) {
for (float pitch = -M_PI_2_F; pitch <= M_PI_2_F; pitch += diff) {
for (float yaw = -M_PI_F; yaw <= M_PI_F; yaw += diff) {
R.from_euler(roll, pitch, yaw);
q.from_euler(roll, pitch, yaw);
R = matrix::Eulerf(roll, pitch, yaw);
q = matrix::Eulerf(roll, pitch, yaw);
vector_r = R * vector;
vector_q = q.conjugate(vector);
for (int i = 0; i < 3; i++) {
ut_assert("Quaternion method 'rotate' outside tolerance", fabsf(vector_r(i) - vector_q(i)) < tol);
ut_assert("matrix::Quatf method 'rotate' outside tolerance", fabsf(vector_r(i) - vector_q(i)) < tol);
}
}
}
@ -384,36 +331,36 @@ bool MathlibTest::testQuaternionRotate()
// test some values calculated with matlab
tol = 0.0001f;
q.from_euler(M_PI_2_F, 0.0f, 0.0f);
q = matrix::Eulerf(M_PI_2_F, 0.0f, 0.0f);
vector_q = q.conjugate(vector);
Vector<3> vector_true = {1.00f, -1.00f, 1.00f};
matrix::Vector3f vector_true = {1.00f, -1.00f, 1.00f};
for (unsigned i = 0; i < 3; i++) {
ut_assert("Quaternion method 'rotate' outside tolerance", fabsf(vector_true(i) - vector_q(i)) < tol);
ut_assert("matrix::Quatf method 'rotate' outside tolerance", fabsf(vector_true(i) - vector_q(i)) < tol);
}
q.from_euler(0.3f, 0.2f, 0.1f);
q = matrix::Eulerf(0.3f, 0.2f, 0.1f);
vector_q = q.conjugate(vector);
vector_true = {1.1566, 0.7792, 1.0273};
for (unsigned i = 0; i < 3; i++) {
ut_assert("Quaternion method 'rotate' outside tolerance", fabsf(vector_true(i) - vector_q(i)) < tol);
ut_assert("matrix::Quatf method 'rotate' outside tolerance", fabsf(vector_true(i) - vector_q(i)) < tol);
}
q.from_euler(-1.5f, -0.2f, 0.5f);
q = matrix::Eulerf(-1.5f, -0.2f, 0.5f);
vector_q = q.conjugate(vector);
vector_true = {0.5095, 1.4956, -0.7096};
for (unsigned i = 0; i < 3; i++) {
ut_assert("Quaternion method 'rotate' outside tolerance", fabsf(vector_true(i) - vector_q(i)) < tol);
ut_assert("matrix::Quatf method 'rotate' outside tolerance", fabsf(vector_true(i) - vector_q(i)) < tol);
}
q.from_euler(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3.0f);
q = matrix::Eulerf(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3.0f);
vector_q = q.conjugate(vector);
vector_true = { -1.3660, 0.3660, 1.0000};
for (unsigned i = 0; i < 3; i++) {
ut_assert("Quaternion method 'rotate' outside tolerance", fabsf(vector_true(i) - vector_q(i)) < tol);
ut_assert("matrix::Quatf method 'rotate' outside tolerance", fabsf(vector_true(i) - vector_q(i)) < tol);
}
return true;
@ -423,10 +370,7 @@ bool MathlibTest::run_tests()
{
ut_run_test(testVector2);
ut_run_test(testVector3);
ut_run_test(testVector4);
ut_run_test(testVector10);
ut_run_test(testMatrix3x3);
ut_run_test(testMatrix10x10);
ut_run_test(testMatrixNonsymmetric);
ut_run_test(testRotationMatrixQuaternion);
ut_run_test(testQuaternionfrom_dcm);