mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-07-08 08:50:35 +08:00
Work on testing.
This commit is contained in:
jgoppert
@@ -1,2 +1,3 @@
|
||||
build*/
|
||||
*.orig
|
||||
*.swp
|
||||
|
||||
@@ -31,6 +31,14 @@ public:
|
||||
Matrix<Type, 3, 3>::setIdentity();
|
||||
}
|
||||
|
||||
Dcm(const Type *data) : Matrix<Type, 3, 3>(data)
|
||||
{
|
||||
}
|
||||
|
||||
Dcm(const Matrix<Type, 3, 3> &other) : Matrix<Type, 3, 3>(other)
|
||||
{
|
||||
}
|
||||
|
||||
Dcm(const Quaternion<Type> & q) {
|
||||
Dcm &dcm = *this;
|
||||
Type a = q(0);
|
||||
|
||||
+2
-2
@@ -42,11 +42,11 @@ public:
|
||||
Type phi = 0;
|
||||
Type psi = 0;
|
||||
|
||||
if (abs(theta - M_PI_2) < 1.0e-3) {
|
||||
if (fabs(theta - M_PI_2) < 1.0e-3) {
|
||||
psi = atan2(dcm(1, 2) - dcm(0, 1),
|
||||
dcm(0, 2) + dcm(1, 1));
|
||||
|
||||
} else if (abs(theta + M_PI_2) < 1.0e-3) {
|
||||
} else if (fabs(theta + M_PI_2) < 1.0e-3) {
|
||||
psi = atan2(dcm(1, 2) - dcm(0, 1),
|
||||
dcm(0, 2) + dcm(1, 1));
|
||||
|
||||
|
||||
@@ -293,6 +293,45 @@ public:
|
||||
}
|
||||
}
|
||||
|
||||
Matrix<Type, M, N> abs()
|
||||
{
|
||||
Matrix<Type, M, N> r;
|
||||
for (int i=0; i<M; i++) {
|
||||
for (int j=0; j<M; j++) {
|
||||
r(i,j) = fabs((*this)(i,j));
|
||||
}
|
||||
}
|
||||
return r;
|
||||
}
|
||||
|
||||
Type max()
|
||||
{
|
||||
Type max = (*this)(0,0);
|
||||
for (int i=0; i<M; i++) {
|
||||
for (int j=0; j<M; j++) {
|
||||
Type val = (*this)(i,j);
|
||||
if (val > max) {
|
||||
max = val;
|
||||
}
|
||||
}
|
||||
}
|
||||
return max;
|
||||
}
|
||||
|
||||
Type min()
|
||||
{
|
||||
Type min = (*this)(0,0);
|
||||
for (int i=0; i<M; i++) {
|
||||
for (int j=0; j<M; j++) {
|
||||
Type val = (*this)(i,j);
|
||||
if (val < min) {
|
||||
min = val;
|
||||
}
|
||||
}
|
||||
}
|
||||
return min;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
template<typename Type, size_t M, size_t N>
|
||||
|
||||
+14
-5
@@ -47,20 +47,24 @@ public:
|
||||
return v(i, 0);
|
||||
}
|
||||
|
||||
Type dot(const Vector & b) {
|
||||
Vector &a(*this);
|
||||
Type dot(const Vector & b) const {
|
||||
const Vector &a(*this);
|
||||
Type r = 0;
|
||||
for (int i = 0; i<3; i++) {
|
||||
for (int i = 0; i<M; i++) {
|
||||
r += a(i)*b(i);
|
||||
}
|
||||
return r;
|
||||
}
|
||||
|
||||
Type norm() {
|
||||
Vector &a(*this);
|
||||
Type norm() const {
|
||||
const Vector &a(*this);
|
||||
return sqrt(a.dot(a));
|
||||
}
|
||||
|
||||
inline void normalize() {
|
||||
(*this) /= norm();
|
||||
}
|
||||
|
||||
/**
|
||||
* Vector Operations
|
||||
*/
|
||||
@@ -131,6 +135,11 @@ public:
|
||||
return res;
|
||||
}
|
||||
|
||||
Vector<Type, M> operator/(Type scalar) const
|
||||
{
|
||||
return (*this)*(1.0/scalar);
|
||||
}
|
||||
|
||||
Vector<Type, M> operator+(Type scalar) const
|
||||
{
|
||||
Vector<Type, M> res;
|
||||
|
||||
+1
-3
@@ -6,11 +6,9 @@ set(tests
|
||||
matrixAssignment
|
||||
matrixScalarMult
|
||||
transpose
|
||||
quaternion
|
||||
dcm
|
||||
vector
|
||||
vector3
|
||||
euler
|
||||
attitude
|
||||
filter
|
||||
)
|
||||
|
||||
|
||||
@@ -0,0 +1,73 @@
|
||||
#include <cassert>
|
||||
#include <cstdio>
|
||||
|
||||
#include "Quaternion.hpp"
|
||||
#include "Vector3.hpp"
|
||||
#include "matrix.hpp"
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
template class Quaternion<float>;
|
||||
template class Euler<float>;
|
||||
template class Dcm<float>;
|
||||
|
||||
int main()
|
||||
{
|
||||
double eps = 1e-6;
|
||||
|
||||
// check data
|
||||
Eulerf euler_check(0.1, 0.2, 0.3);
|
||||
Quatf q_check(0.98334744, 0.0342708, 0.10602051, .14357218);
|
||||
float dcm_data[] = {
|
||||
0.93629336, -0.27509585, 0.21835066,
|
||||
0.28962948, 0.95642509, -0.03695701,
|
||||
-0.19866933, 0.0978434 , 0.97517033};
|
||||
Dcmf dcm_check(dcm_data);
|
||||
|
||||
// euler ctor
|
||||
euler_check.T().print();
|
||||
assert((euler_check - Vector3f(0.1, 0.2, 0.3)).norm() < eps);
|
||||
|
||||
// quaternion ctor
|
||||
Quatf q(1, 2, 3, 4);
|
||||
assert(q(0) == 1);
|
||||
assert(q(1) == 2);
|
||||
assert(q(2) == 3);
|
||||
assert(q(3) == 4);
|
||||
|
||||
q.T().print();
|
||||
q.normalize();
|
||||
q.T().print();
|
||||
assert((q - Quatf(
|
||||
0.18257419, 0.36514837, 0.54772256, 0.73029674)
|
||||
).norm() < eps);
|
||||
|
||||
// euler to quaternion
|
||||
q = Quatf(euler_check);
|
||||
q.T().print();
|
||||
assert((q - q_check).norm() < eps);
|
||||
|
||||
// euler to dcm
|
||||
Dcmf dcm(euler_check);
|
||||
dcm.print();
|
||||
assert((dcm - dcm_check).abs().max() < eps);
|
||||
|
||||
// quaternion to euler
|
||||
Eulerf e1(q_check);
|
||||
assert((e1 - euler_check).norm() < eps);
|
||||
|
||||
// quaternion to dcm
|
||||
Dcmf dcm1(q_check);
|
||||
assert((dcm1 - dcm_check).abs().max() < eps);
|
||||
|
||||
// dcm to euler
|
||||
Eulerf e2(dcm_check);
|
||||
assert((e2 - euler_check).norm() < eps);
|
||||
|
||||
// dcm to quaterion
|
||||
Quatf q2(dcm_check);
|
||||
assert((q2 - q_check).norm() < eps);
|
||||
|
||||
}
|
||||
|
||||
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|
||||
@@ -1,19 +0,0 @@
|
||||
#include <assert.h>
|
||||
#include <stdio.h>
|
||||
|
||||
#include "matrix.hpp"
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
int main()
|
||||
{
|
||||
Dcmf dcm;
|
||||
Quatf q(1,0,0,0);
|
||||
dcm = Dcmf(q);
|
||||
Matrix3f I = eye<float, 3>();
|
||||
dcm = Dcmf(q);
|
||||
Eulerf e = Eulerf(dcm);
|
||||
return 0;
|
||||
};
|
||||
|
||||
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|
||||
@@ -1,20 +0,0 @@
|
||||
#include "Euler.hpp"
|
||||
#include "Scalar.hpp"
|
||||
#include <assert.h>
|
||||
#include <stdio.h>
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
int main()
|
||||
{
|
||||
Eulerf e;
|
||||
float dp = Scalarf(e.T()*e);
|
||||
(void)dp;
|
||||
Dcmf dcm = Dcmf(e);
|
||||
Quatf q = Quatf(e);
|
||||
float n = e.norm();
|
||||
(void)n;
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|
||||
@@ -1,50 +0,0 @@
|
||||
#include "Quaternion.hpp"
|
||||
#include <cassert>
|
||||
#include <cstdio>
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
// instantiate so coverage works
|
||||
template class Quaternion<float>;
|
||||
template class Euler<float>;
|
||||
template class Dcm<float>;
|
||||
|
||||
int main()
|
||||
{
|
||||
// test default ctor
|
||||
Quatf q;
|
||||
assert(q(0) == 1);
|
||||
assert(q(1) == 0);
|
||||
assert(q(2) == 0);
|
||||
assert(q(3) == 0);
|
||||
|
||||
q = Quatf(1,2,3,4);
|
||||
assert(q(0) == 1);
|
||||
assert(q(1) == 2);
|
||||
assert(q(2) == 3);
|
||||
assert(q(3) == 4);
|
||||
|
||||
q = Quatf(0.1825742f, 0.3651484f, 0.5477226f, 0.7302967f);
|
||||
assert(q(0) == 0.1825742f);
|
||||
assert(q(1) == 0.3651484f);
|
||||
assert(q(2) == 0.5477226f);
|
||||
assert(q(3) == 0.7302967f);
|
||||
|
||||
// test euler ctor
|
||||
q = Quatf(Eulerf(0.1f, 0.2f, 0.3f));
|
||||
assert((q - Quatf(0.983347f, 0.034271f, 0.106021f, 0.143572f)).norm() < 1e-5);
|
||||
|
||||
// test dcm ctor
|
||||
q = Quatf(Dcmf());
|
||||
assert(q == Quatf(1.0f, 0.0f, 0.0f, 0.0f));
|
||||
// TODO test derivative
|
||||
// test accessors
|
||||
q(0) = 0.1f;
|
||||
q(1) = 0.2f;
|
||||
q(2) = 0.3f;
|
||||
q(3) = 0.4f;
|
||||
assert(q == Quatf(0.1f, 0.2f, 0.3f, 0.4f));
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
|
||||
@@ -0,0 +1,104 @@
|
||||
from __future__ import print_function
|
||||
from pylab import *
|
||||
from pprint import pprint
|
||||
|
||||
# test cases, derived from doc/nasa_rotation_def.pdf
|
||||
|
||||
#pylint: disable=all
|
||||
|
||||
def euler_to_quat(phi, theta, psi):
|
||||
"Quaternion from (body 3(psi)-2(theta)-1(phi) euler angles"
|
||||
s1 = sin(psi/2)
|
||||
c1 = cos(psi/2)
|
||||
s2 = sin(theta/2)
|
||||
c2 = cos(theta/2)
|
||||
s3 = sin(phi/2)
|
||||
c3 = cos(phi/2)
|
||||
return array([
|
||||
s1*s2*s3 + c1*c2*c3,
|
||||
-s1*s2*c3 + s3*c1*c2,
|
||||
s1*s3*c2 + s2*c1*c3,
|
||||
s1*c2*c3 - s2*s3*c1
|
||||
])
|
||||
|
||||
def euler_to_dcm(phi, theta, psi):
|
||||
s1 = sin(psi)
|
||||
c1 = cos(psi)
|
||||
s2 = sin(theta)
|
||||
c2 = cos(theta)
|
||||
s3 = sin(phi)
|
||||
c3 = cos(phi)
|
||||
return array([
|
||||
[c1*c2, c1*s2*s3 - s1*c3, c1*s2*c3 + s1*s3],
|
||||
[s1*c2, s1*s2*s3 + c1*c3, s1*s2*c3 - c1*s3],
|
||||
[-s2, c2*s3, c2*c3],
|
||||
])
|
||||
|
||||
def quat_prod(q, r):
|
||||
"Quaternion product"
|
||||
return array([
|
||||
r[0]*q[0] - r[1]*q[1] - r[2]*q[2] - r[3]*q[3],
|
||||
r[0]*q[1] + r[1]*q[0] - r[2]*q[3] + r[3]*q[2],
|
||||
r[0]*q[2] + r[1]*q[3] + r[2]*q[0] - r[3]*q[1],
|
||||
r[0]*q[3] - r[1]*q[2] + r[2]*q[1] + r[3]*q[0]
|
||||
])
|
||||
|
||||
def dcm_to_euler(dcm):
|
||||
return array([
|
||||
arctan(dcm[2,1]/ dcm[2,2]),
|
||||
arctan(-dcm[2,0]/ sqrt(1 - dcm[2,0]**2)),
|
||||
arctan(dcm[1,0]/ dcm[0,0]),
|
||||
])
|
||||
|
||||
def dcm_from_quat(q):
|
||||
q1 = q[0]
|
||||
q2 = q[1]
|
||||
q3 = q[2]
|
||||
q4 = q[3]
|
||||
return array([
|
||||
[q1*q1 + q2*q2 - q3*q3 - q4*q4, 2*(q2*q3 - q1*q4), 2*(q2*q4 + q1*q3)],
|
||||
[2*(q2*q3 + q1*q4), q1*q1 - q2*q2 + q3*q3 - q4*q4, 2*(q3*q4 - q1*q2)],
|
||||
[2*(q2*q4 - q1*q3), 2*(q3*q4 + q1*q2), q1*q1 - q2*q2 - q3*q3 + q4*q4]
|
||||
])
|
||||
|
||||
def quat_to_euler(q):
|
||||
"Quaternion to (body 3(psi)-2(theta)-1(phi) euler angles"
|
||||
return dcm_to_euler(dcm_from_quat(q))
|
||||
|
||||
def quat_to_dcm(q):
|
||||
return euler_to_dcm(quat_to_euler(q))
|
||||
|
||||
phi = 0.1
|
||||
theta = 0.2
|
||||
psi = 0.3
|
||||
print('euler', phi, theta, psi)
|
||||
|
||||
q = euler_to_quat(phi, theta, psi)
|
||||
assert(abs(norm(q) - 1) < 1e-10)
|
||||
assert(abs(norm(q) - 1) < 1e-10)
|
||||
assert(norm(array(quat_to_euler(q)) - array([phi, theta, psi])) < 1e-10)
|
||||
print('\nq:')
|
||||
pprint(q)
|
||||
|
||||
dcm = euler_to_dcm(phi, theta, psi)
|
||||
assert(norm(dcm[:,0]) == 1)
|
||||
assert(norm(dcm[:,1]) == 1)
|
||||
assert(norm(dcm[:,2]) == 1)
|
||||
assert(abs(dcm[:,0].dot(dcm[:,1])) < 1e-10)
|
||||
assert(abs(dcm[:,0].dot(dcm[:,2])) < 1e-10)
|
||||
print('\ndcm:')
|
||||
pprint(dcm)
|
||||
|
||||
|
||||
q2 = quat_prod(q, q)
|
||||
pprint(q2)
|
||||
print(norm(q2))
|
||||
|
||||
print('\nq3:')
|
||||
q3 = array([1,2,3,4])
|
||||
pprint(q3)
|
||||
print('\nq3_norm:')
|
||||
q3_norm =q3 / norm(q3)
|
||||
pprint(q3_norm)
|
||||
|
||||
# vim: set et ft=python fenc=utf-8 ff=unix sts=4 sw=4 ts=8 :
|
||||
@@ -1,66 +0,0 @@
|
||||
from __future__ import print_function
|
||||
from pylab import *
|
||||
from pprint import pprint
|
||||
|
||||
#pylint: disable=all
|
||||
|
||||
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 = array([
|
||||
[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]])
|
||||
|
||||
print('\nC_nb')
|
||||
pprint(C_nb)
|
||||
|
||||
theta = arcsin(-C_nb[2,0])
|
||||
phi = arctan(C_nb[2,1]/ C_nb[2,2])
|
||||
psi = arctan(C_nb[1,0]/ C_nb[0,0])
|
||||
print('\nphi {:f}, theta {:f}, psi {:f}\n'.format(phi, theta, psi))
|
||||
|
||||
q = array([
|
||||
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])
|
||||
|
||||
a = q[0]
|
||||
b = q[1]
|
||||
c = q[2]
|
||||
d = q[3]
|
||||
|
||||
print('\nq')
|
||||
pprint(q.T)
|
||||
|
||||
a2 = a*a
|
||||
b2 = b*b
|
||||
c2 = c*c
|
||||
d2 = d*d
|
||||
|
||||
C2_nb = array([
|
||||
[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]])
|
||||
|
||||
print('\nC2_nb')
|
||||
pprint(C2_nb)
|
||||
|
||||
# vim: set et ft=python fenc=utf-8 ff=unix sts=4 sw=4 ts=8 :
|
||||
Reference in New Issue
Block a user