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

Initial commit.

Browse Source
This commit is contained in:
jgoppert
2015-11-03 14:46:54 -05:00
commit 76e86cf937
22 changed files with 1093 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
matrix/Dcm.hpp
+64
View File
@@ -0,0 +1,64 @@
/**
* @file Dcm.hpp
*
* A direction cosine matrix class.
*
* @author James Goppert <james.goppert@gmail.com>
*/
#pragma once
#include <Matrix.hpp>
#include <Quaternion.hpp>
#include <Euler.hpp>
namespace matrix
{
template<typename Type>
class Quaternion;
template<typename Type>
class Dcm : public Matrix<Type, 3, 3>
{
public:
virtual ~Dcm() {};
typedef Matrix<Type, 3, 1> Vector3;
Dcm() : Matrix<Type, 3, 3>()
{
}
Dcm(const Quaternion<Type> & q) {
// TODO
Dcm &c = *this;
c(0, 0) = 0;
c(0, 1) = 0;
c(0, 2) = 0;
c(1, 0) = 0;
c(1, 1) = 0;
c(1, 2) = 0;
c(2, 0) = 0;
c(2, 1) = 0;
c(2, 2) = 0;
}
Dcm(const Euler<Type> & e) {
// TODO
Dcm &c = *this;
c(0, 0) = 0;
c(0, 1) = 0;
c(0, 2) = 0;
c(1, 0) = 0;
c(1, 1) = 0;
c(1, 2) = 0;
c(2, 0) = 0;
c(2, 1) = 0;
c(2, 2) = 0;
}
};
typedef Dcm<float> Dcmf;
}; // namespace matrix
matrix/Euler.hpp
+61
View File
@@ -0,0 +1,61 @@
/**
* @file Euler.hpp
*
* Euler angle tait-bryan body 3-2-1
*
* @author James Goppert <james.goppert@gmail.com>
*/
#pragma once
#include <Vector.hpp>
#include <Dcm.hpp>
#include <Quaternion.hpp>
namespace matrix
{
template <typename Type>
class Dcm;
template <typename Type>
class Quaternion;
template<typename Type>
class Euler : public Vector<Type, 3>
{
public:
virtual ~Euler() {};
Euler() : Vector<Type, 3>()
{
}
Euler(Type roll, Type pitch, Type yaw) : Vector<Type, 3>()
{
Euler &v(*this);
v(0) = roll;
v(1) = pitch;
v(2) = yaw;
}
Euler(const Dcm<Type> & dcm) {
// TODO
Euler &e = *this;
e(0) = 0;
e(1) = 0;
e(2) = 0;
}
Euler(const Quaternion<Type> & q) {
// TODO
Euler &e = *this;
e(0) = 0;
e(1) = 0;
e(2) = 0;
}
};
typedef Euler<float> Eulerf;
}; // namespace matrix
matrix/Matrix.hpp
+472
View File
@@ -0,0 +1,472 @@
/**
* @file Matrix.hpp
*
* A simple matrix template library.
*
* @author James Goppert <james.goppert@gmail.com>
*/
#pragma once
#include <stdio.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
namespace matrix
{
template<typename Type, size_t M, size_t N>
class Matrix
{
protected:
Type _data[M][N];
size_t _rows;
size_t _cols;
public:
virtual ~Matrix() {};
Matrix() :
_data(),
_rows(M),
_cols(N)
{
}
Matrix(const Type *data) :
_data(),
_rows(M),
_cols(N)
{
memcpy(_data, data, sizeof(_data));
}
Matrix(const Matrix &other) :
_data(),
_rows(M),
_cols(N)
{
memcpy(_data, other._data, sizeof(_data));
}
/**
* Accessors/ Assignment etc.
*/
Type *data()
{
return _data[0];
}
inline size_t rows() const
{
return _rows;
}
inline size_t cols() const
{
return _rows;
}
inline Type operator()(size_t i, size_t j) const
{
return _data[i][j];
}
inline Type &operator()(size_t i, size_t j)
{
return _data[i][j];
}
/**
* Matrix Operations
*/
// this might use a lot of programming memory
// since it instantiates a class for every
// required mult pair, but it provides
// compile time size_t checking
template<size_t P>
Matrix<Type, M, P> operator*(const Matrix<Type, N, P> &other) const
{
const Matrix<Type, M, N> &self = *this;
Matrix<Type, M, P> res;
res.setZero();
for (size_t i = 0; i < M; i++) {
for (size_t k = 0; k < P; k++) {
for (size_t j = 0; j < N; j++) {
res(i, k) += self(i, j) * other(j, k);
}
}
}
return res;
}
Matrix<Type, M, N> operator+(const Matrix<Type, M, N> &other) const
{
Matrix<Type, M, N> res;
const Matrix<Type, M, N> &self = *this;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
res(i , j) = self(i, j) + other(i, j);
}
}
return res;
}
bool operator==(const Matrix<Type, M, N> &other) const
{
Matrix<Type, M, N> res;
const Matrix<Type, M, N> &self = *this;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
if (self(i , j) != other(i, j)) {
return false;
}
}
}
return true;
}
operator Type();
Matrix<Type, M, N> operator-(const Matrix<Type, M, N> &other) const
{
Matrix<Type, M, N> res;
const Matrix<Type, M, N> &self = *this;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
res(i , j) = self(i, j) - other(i, j);
}
}
return res;
}
void operator+=(const Matrix<Type, M, N> &other)
{
Matrix<Type, M, N> &self = *this;
self = self + other;
}
void operator-=(const Matrix<Type, M, N> &other)
{
Matrix<Type, M, N> &self = *this;
self = self - other;
}
void operator*=(const Matrix<Type, M, N> &other)
{
Matrix<Type, M, N> &self = *this;
self = self * other;
}
/**
* Scalar Operations
*/
Matrix<Type, M, N> operator*(Type scalar) const
{
Matrix<Type, M, N> res;
const Matrix<Type, M, N> &self = *this;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
res(i , j) = self(i, j) * scalar;
}
}
return res;
}
Matrix<Type, M, N> operator+(Type scalar) const
{
Matrix<Type, M, N> res;
Matrix<Type, M, N> &self = *this;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
res(i , j) = self(i, j) + scalar;
}
}
return res;
}
void operator*=(Type scalar)
{
Matrix<Type, M, N> &self = *this;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
self(i, j) = self(i, j) * scalar;
}
}
}
void operator/=(Type scalar)
{
Matrix<Type, M, N> &self = *this;
self = self * (1.0f / scalar);
}
/**
* Misc. Functions
*/
void print()
{
Matrix<Type, M, N> &self = *this;
printf("\n");
for (size_t i = 0; i < M; i++) {
printf("[");
for (size_t j = 0; j < N; j++) {
printf("%10g\t", double(self(i, j)));
}
printf("]\n");
}
}
Matrix<Type, N, M> transpose() const
{
Matrix<Type, N, M> res;
const Matrix<Type, M, N> &self = *this;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
res(j, i) = self(i, j);
}
}
return res;
}
// tranpose alias
inline Matrix<Type, N, M> T() const
{
return transpose();
}
Matrix<Type, M, M> expm(float dt, size_t n) const
{
Matrix<float, M, M> res;
res.setIdentity();
Matrix<float, M, M> A_pow = *this;
float k_fact = 1;
size_t k = 1;
while (k < n) {
res += A_pow * (float(pow(dt, k)) / k_fact);
if (k == n) { break; }
A_pow *= A_pow;
k_fact *= k;
k++;
}
return res;
}
Matrix<Type, M, 1> diagonal() const
{
Matrix<Type, M, 1> res;
// force square for now
const Matrix<Type, M, M> &self = *this;
for (size_t i = 0; i < M; i++) {
res(i) = self(i, i);
}
return res;
}
void setZero()
{
memset(_data, 0, sizeof(_data));
}
void setIdentity()
{
setZero();
Matrix<Type, M, N> &self = *this;
for (size_t i = 0; i < M and i < N; i++) {
self(i, i) = 1;
}
}
inline void swapRows(size_t a, size_t b)
{
if (a == b) { return; }
Matrix<Type, M, N> &self = *this;
for (size_t j = 0; j < cols(); j++) {
Type tmp = self(a, j);
self(a, j) = self(b, j);
self(b, j) = tmp;
}
}
inline void swapCols(size_t a, size_t b)
{
if (a == b) { return; }
Matrix<Type, M, N> &self = *this;
for (size_t i = 0; i < rows(); i++) {
Type tmp = self(i, a);
self(i, a) = self(i, b);
self(i, b) = tmp;
}
}
/**
* inverse based on LU factorization with partial pivotting
*/
Matrix <Type, M, M> inverse() const
{
Matrix<Type, M, M> L;
L.setIdentity();
const Matrix<Type, M, M> &A = (*this);
Matrix<Type, M, M> U = A;
Matrix<Type, M, M> P;
P.setIdentity();
//printf("A:\n"); A.print();
// for all diagonal elements
for (size_t n = 0; n < N; n++) {
// if diagonal is zero, swap with row below
if (fabsf(U(n, n)) < 1e-8f) {
//printf("trying pivot for row %d\n",n);
for (size_t i = 0; i < N; i++) {
if (i == n) { continue; }
//printf("\ttrying row %d\n",i);
if (fabsf(U(i, n)) > 1e-8f) {
//printf("swapped %d\n",i);
U.swapRows(i, n);
P.swapRows(i, n);
}
}
}
#ifdef MATRIX_ASSERT
//printf("A:\n"); A.print();
//printf("U:\n"); U.print();
//printf("P:\n"); P.print();
//fflush(stdout);
ASSERT(fabsf(U(n, n)) > 1e-8f);
#endif
// failsafe, return zero matrix
if (fabsf(U(n, n)) < 1e-8f) {
Matrix<Type, M, M> zero;
zero.setZero();
return zero;
}
// for all rows below diagonal
for (size_t i = (n + 1); i < N; i++) {
L(i, n) = U(i, n) / U(n, n);
// add i-th row and n-th row
// multiplied by: -a(i,n)/a(n,n)
for (size_t k = n; k < N; k++) {
U(i, k) -= L(i, n) * U(n, k);
}
}
}
//printf("L:\n"); L.print();
//printf("U:\n"); U.print();
// solve LY=P*I for Y by forward subst
Matrix<Type, M, M> Y = P;
// for all columns of Y
for (size_t c = 0; c < N; c++) {
// for all rows of L
for (size_t i = 0; i < N; i++) {
// for all columns of L
for (size_t j = 0; j < i; j++) {
// for all existing y
// subtract the component they
// contribute to the solution
Y(i, c) -= L(i, j) * Y(j, c);
}
// divide by the factor
// on current
// term to be solved
// Y(i,c) /= L(i,i);
// but L(i,i) = 1.0
}
}
//printf("Y:\n"); Y.print();
// solve Ux=y for x by back subst
Matrix<Type, M, M> X = Y;
// for all columns of X
for (size_t c = 0; c < N; c++) {
// for all rows of U
for (size_t k = 0; k < N; k++) {
// have to go in reverse order
size_t i = N - 1 - k;
// for all columns of U
for (size_t j = i + 1; j < N; j++) {
// for all existing x
// subtract the component they
// contribute to the solution
X(i, c) -= U(i, j) * X(j, c);
}
// divide by the factor
// on current
// term to be solved
X(i, c) /= U(i, i);
}
}
//printf("X:\n"); X.print();
return X;
}
// inverse alias
inline Matrix<Type, N, M> I() const
{
return inverse();
}
};
template <>
Matrix<float,1,1>::operator float()
{
return (*this)(0,0);
}
typedef Matrix<float, 3,3> Matrix3f;
}; // namespace matrix
matrix/Quaternion.hpp
+89
View File
@@ -0,0 +1,89 @@
/**
* @file Matrix.hpp
*
* A quaternion class.
*
* @author James Goppert <james.goppert@gmail.com>
*/
#pragma once
#include <Vector.hpp>
#include <Dcm.hpp>
#include <Euler.hpp>
namespace matrix
{
template <typename Type>
class Dcm;
template <typename Type>
class Euler;
template<typename Type>
class Quaternion : public Vector<Type, 4>
{
public:
virtual ~Quaternion() {};
Quaternion() : Vector<Type, 4>()
{
// TODO
Quaternion &q = *this;
q(0) = 1;
q(1) = 0;
q(2) = 0;
q(3) = 0;
}
Quaternion(const Dcm<Type> & dcm) {
// TODO
Quaternion &q = *this;
q(0) = 0;
q(1) = 0;
q(2) = 0;
q(3) = 0;
}
Quaternion(const Euler<Type> & e) {
// TODO
Quaternion &q = *this;
q(0) = 0;
q(1) = 0;
q(2) = 0;
q(3) = 0;
}
Quaternion(Type a, Type b, Type c, Type d) : Vector<Type, 4>()
{
// TODO
Quaternion &q = *this;
q(0) = a;
q(1) = b;
q(2) = c;
q(3) = d;
}
Quaternion operator*(const Quaternion &q) const
{
// TODO
const Quaternion &p = *this;
Quaternion r;
r(0) = 0;
r(1) = 0;
r(2) = 0;
r(3) = 0;
return r;
}
Matrix<Type, 4, 1> derivative() const {
// TODO
Matrix<Type, 4, 1> d;
return d;
}
};
typedef Quaternion<float> Quatf;
}; // namespace matrix
matrix/Vector.hpp
+52
View File
@@ -0,0 +1,52 @@
/**
* @file Vector.hpp
*
* Vector class.
*
* @author James Goppert <james.goppert@gmail.com>
*/
#pragma once
#include <Matrix.hpp>
namespace matrix
{
template<typename Type, size_t N>
class Vector : public Matrix<Type, N, 1>
{
public:
virtual ~Vector() {};
Vector() : Matrix<Type, N, 1>()
{
}
inline Type operator()(size_t i) const
{
const Matrix<Type, N, 1> &v = *this;
return v(i, 0);
}
inline Type &operator()(size_t i)
{
Matrix<Type, N, 1> &v = *this;
return v(i, 0);
}
Type dot(const Vector & b) {
Vector &a(*this);
Type r = 0;
for (int i = 0; i<3; i++) {
r += a(i)*b(i);
}
return r;
}
Type norm() {
Vector &a(*this);
return sqrt(a.dot(a));
}
};
}; // namespace matrix
matrix/Vector3.hpp
+46
View File
@@ -0,0 +1,46 @@
/**
* @file Vector3.hpp
*
* 3D vector class.
*
* @author James Goppert <james.goppert@gmail.com>
*/
#pragma once
#include <Vector.hpp>
namespace matrix
{
template<typename Type>
class Vector3 : public Vector<Type, 3>
{
public:
virtual ~Vector3() {};
Vector3() : Vector<Type, 3>()
{
}
Vector3(Type x, Type y, Type z) : Vector<Type, 3>()
{
Vector3 &v(*this);
v(0) = x;
v(1) = y;
v(2) = z;
}
Vector3 cross(const Vector3 & b) {
// TODO
Vector3 &a(*this);
Vector3 c;
c(0) = 0;
c(1) = 0;
c(2) = 0;
return c;
}
};
typedef Vector3<float> Vector3f;
}; // namespace matrix