fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-16 14:27:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Add cholesky decomp, Closes #30, and dynamic print buf

Browse Source
This commit is contained in:
James Goppert
2017-02-03 17:54:16 -05:00
parent a154e14439
commit 63aea23f9e
12 changed files with 165 additions and 64 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CMakeLists.txt
+6
View File
@@ -54,6 +54,12 @@ set(CMAKE_CXX_FLAGS
-Wconversion
-Wcast-align
-Werror
-pedantic -Wctor-dtor-privacy -Wdisabled-optimization -Wformat=2
-Winit-self -Wlogical-op -Wmissing-declarations
-Wmissing-include-dirs -Wnoexcept -Wold-style-cast
-Woverloaded-virtual -Wredundant-decls -Wshadow -Wsign-conversion
-Wsign-promo -Wstrict-null-sentinel -Wstrict-overflow=5
-Wswitch-default -Wundef -Werror -Wno-unused
)
string(REPLACE ";" " " CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}")
matrix/AxisAngle.hpp
+1 -1
View File
@@ -76,7 +76,7 @@ public:
Vector<Type, 3>()
{
AxisAngle &v = *this;
Type ang = (Type)2.0f*acosf(q(0));
Type ang = Type(2.0f)*acosf(q(0));
Type mag = sinf(ang/2.0f);
if (fabsf(mag) > 0) {
v(0) = ang*q(1)/mag;
matrix/Dcm.hpp
+1 -1
View File
@@ -164,7 +164,7 @@ public:
void renormalize()
{
/* renormalize rows */
for (int row = 0; row < 3; row++) {
for (size_t row = 0; row < 3; row++) {
matrix::Vector3f rvec(this->_data[row]);
this->setRow(row, rvec.normalized());
}
matrix/Matrix.hpp
+12 -8
View File
@@ -282,7 +282,7 @@ public:
const Matrix<Type, M, N> &self = *this;
for (size_t i = 0; i < M; i++) {
for (size_t j = 0; j < N; j++) {
snprintf(buf + strlen(buf), n - strlen(buf), "\t%g", double(self(i, j))); // directly append to the string buffer
snprintf(buf + strlen(buf), n - strlen(buf), "\t%8.2g", double(self(i, j))); // directly append to the string buffer
}
snprintf(buf + strlen(buf), n - strlen(buf), "\n");
}
@@ -290,9 +290,11 @@ public:
void print() const
{
char buf[200];
write_string(buf, 200);
static const size_t n = 10*N*M;
char * buf = new char[n];
write_string(buf, n);
printf("%s\n", buf);
delete[] buf;
}
Matrix<Type, N, M> transpose() const
@@ -499,11 +501,13 @@ bool isEqual(const Matrix<Type, M, N> &x,
if (!equal) {
char buf_x[100];
char buf_y[100];
x.write_string(buf_x, 100);
y.write_string(buf_y, 100);
printf("not equal\nx:\n%s\ny:\n%s\n", buf_x, buf_y);
static const size_t n = 10*N*M;
char * buf = new char[n];
x.write_string(buf, n);
printf("not equal\nx:\n%s\n", buf);
y.write_string(buf, n);
printf("y:\n%s\n", buf);
delete[] buf;
}
return equal;
}
matrix/Quaternion.hpp
+44 -28
View File
@@ -2,6 +2,7 @@
* @file Quaternion.hpp
*
* All rotations and axis systems follow the right-hand rule.
* The Hamilton quaternion product definition is used.
*
* In order to rotate a vector v by a righthand rotation defined by the quaternion q
* one can use the following operation:
@@ -139,12 +140,12 @@ public:
Vector<Type, 4>()
{
Quaternion &q = *this;
Type cosPhi_2 = Type(cos(euler.phi() / (Type)2.0));
Type cosTheta_2 = Type(cos(euler.theta() / (Type)2.0));
Type cosPsi_2 = Type(cos(euler.psi() / (Type)2.0));
Type sinPhi_2 = Type(sin(euler.phi() / (Type)2.0));
Type sinTheta_2 = Type(sin(euler.theta() / (Type)2.0));
Type sinPsi_2 = Type(sin(euler.psi() / (Type)2.0));
Type cosPhi_2 = Type(cos(euler.phi() / Type(2.0)));
Type cosTheta_2 = Type(cos(euler.theta() / Type(2.0)));
Type cosPsi_2 = Type(cos(euler.psi() / Type(2.0)));
Type sinPhi_2 = Type(sin(euler.phi() / Type(2.0)));
Type sinTheta_2 = Type(sin(euler.theta() / Type(2.0)));
Type sinPsi_2 = Type(sin(euler.psi() / Type(2.0)));
q(0) = cosPhi_2 * cosTheta_2 * cosPsi_2 +
sinPhi_2 * sinTheta_2 * sinPsi_2;
q(1) = sinPhi_2 * cosTheta_2 * cosPsi_2 -
@@ -166,8 +167,8 @@ public:
Quaternion &q = *this;
Type angle = aa.norm();
Vector<Type, 3> axis = aa.unit();
if (angle < (Type)1e-10) {
q(0) = (Type)1.0;
if (angle < Type(1e-10)) {
q(0) = Type(1.0);
q(1) = q(2) = q(3) = 0;
} else {
Type magnitude = sinf(angle / 2.0f);
@@ -253,11 +254,29 @@ public:
}
/**
* Computes the derivative
* Computes the derivative of q_12 when
* rotated with angular velocity expressed in frame 2
* v_2 = q_12 * v_1 * q_12^-1
* d/dt q_12 = 0.5 * q_12 * omega_12_2
*
* @param w direction
* @param w angular rate in frame 2
*/
Matrix41 derivative(const Matrix31 &w) const
Matrix41 derivative1(const Matrix31 &w) const
{
const Quaternion &q = *this;
Quaternion<Type> v(0, w(0, 0), w(1, 0), w(2, 0));
return q * v * Type(0.5);
}
/**
* Computes the derivative of q_12 when
* rotated with angular velocity expressed in frame 2
* v_2 = q_12 * v_1 * q_12^-1
* d/dt q_12 = 0.5 * omega_12_1 * q_12
*
* @param w angular rate in frame (typically reference frame)
*/
Matrix41 derivative2(const Matrix31 &w) const
{
const Quaternion &q = *this;
Quaternion<Type> v(0, w(0, 0), w(1, 0), w(2, 0));
@@ -265,14 +284,11 @@ public:
}
/**
* Invert quaternion
* Invert quaternion in place
*/
void invert()
{
Quaternion &q = *this;
q(1) *= -1;
q(2) *= -1;
q(3) *= -1;
*this = this->inversed();
}
/**
@@ -283,12 +299,12 @@ public:
Quaternion inversed()
{
Quaternion &q = *this;
Quaternion ret;
ret(0) = q(0);
ret(1) = -q(1);
ret(2) = -q(2);
ret(3) = -q(3);
return ret;
Type normSq = q.dot(q);
return Quaternion(
q(0)/normSq,
-q(1)/normSq,
-q(2)/normSq,
-q(3)/normSq);
}
/**
@@ -332,8 +348,8 @@ public:
Quaternion &q = *this;
Type theta = vec.norm();
if (theta < (Type)1e-10) {
q(0) = (Type)1.0;
if (theta < Type(1e-10)) {
q(0) = Type(1.0);
q(1) = q(2) = q(3) = 0;
return;
}
@@ -354,8 +370,8 @@ public:
{
Quaternion &q = *this;
if (theta < (Type)1e-10) {
q(0) = (Type)1.0;
if (theta < Type(1e-10)) {
q(0) = Type(1.0);
q(1) = q(2) = q(3) = 0;
}
@@ -386,9 +402,9 @@ public:
vec(1) = q(2);
vec(2) = q(3);
if (axis_magnitude >= (Type)1e-10) {
if (axis_magnitude >= Type(1e-10)) {
vec = vec / axis_magnitude;
vec = vec * wrap_pi((Type)2.0 * atan2f(axis_magnitude, q(0)));
vec = vec * wrap_pi(Type(2.0) * atan2f(axis_magnitude, q(0)));
}
return vec;
matrix/Scalar.hpp
-5
View File
@@ -44,11 +44,6 @@ public:
return _value;
}
operator Type const &() const
{
return _value;
}
operator Matrix<Type, 1, 1>() const {
Matrix<Type, 1, 1> m;
m(0, 0) = _value;
matrix/SquareMatrix.hpp
+52
View File
@@ -265,6 +265,58 @@ SquareMatrix<Type, M> inv(const SquareMatrix<Type, M> & A)
return i;
}
}
/**
* cholesky decomposition
*
* Note: A must be positive definite
*/
template<typename Type, size_t M>
SquareMatrix <Type, M> cholesky(const SquareMatrix<Type, M> & A)
{
SquareMatrix<Type, M> L;
for (size_t j = 0; j < M; j++) {
for (size_t i = j; i < M; i++) {
if (i==j) {
float sum = 0;
for (size_t k = 0; k < j; k++) {
sum += L(j, k)*L(j, k);
}
Type res = A(j, j) - sum;
if (res <= 0) {
L(j, j) = 0;
} else {
L(j, j) = sqrtf(res);
}
} else {
float sum = 0;
for (size_t k = 0; k < j; k++) {
sum += L(i, k)*L(j, k);
}
if (L(j, j) <= 0) {
L(i, j) = 0;
} else {
L(i, j) = (A(i, j) - sum)/L(j, j);
}
}
}
}
return L;
}
/**
* cholesky inverse
*
* TODO: Check if gaussian elimination jumps straight to back-substitution
* for L or we need to do it manually. Will impact speed otherwise.
*/
template<typename Type, size_t M>
SquareMatrix <Type, M> choleskyInv(const SquareMatrix<Type, M> & A)
{
SquareMatrix<Type, M> L_inv = inv(cholesky(A));
return L_inv.T()*L_inv;
}
typedef SquareMatrix<float, 3> Matrix3f;
} // namespace matrix
matrix/helper_functions.hpp
+5 -5
View File
@@ -29,13 +29,13 @@ Type wrap_pi(Type x)
return x;
}
while (x >= (Type)M_PI) {
x -= (Type)(2.0 * M_PI);
while (x >= Type(M_PI)) {
x -= Type(2.0 * M_PI);
}
while (x < (Type)(-M_PI)) {
x += (Type)(2.0 * M_PI);
while (x < Type(-M_PI)) {
x += Type(2.0 * M_PI);
}
@@ -43,4 +43,4 @@ Type wrap_pi(Type x)
}
};
}
test/attitude.cpp
+20 -10
View File
@@ -99,12 +99,12 @@ int main()
// dcm renormalize
Dcmf A = eye<float, 3>();
Dcmf R(euler_check);
for (int i = 0; i < 1000; i++) {
for (size_t i = 0; i < 1000; i++) {
A = R * A;
}
A.renormalize();
float err = 0.0f;
for (int row = 0; row < 3; row++) {
for (size_t row = 0; row < 3; row++) {
matrix::Vector3f rvec(A._data[row]);
err += fabsf(1.0f - rvec.length());
}
@@ -179,12 +179,18 @@ int main()
Quatf q_from_m(m4);
TEST(isEqual(q_from_m, m4));
// quaternion derivate
// quaternion derivative in frame 1
Quatf q1(0, 1, 0, 0);
Vector<float, 4> q1_dot = q1.derivative(Vector3f(1, 2, 3));
float data_q_dot_check[] = { -0.5f, 0.0f, -1.5f, 1.0f};
Vector<float, 4> q1_dot_check(data_q_dot_check);
TEST(isEqual(q1_dot, q1_dot_check));
Vector<float, 4> q1_dot1 = q1.derivative1(Vector3f(1, 2, 3));
float data_q_dot1_check[] = { -0.5f, 0.0f, 1.5f, -1.0f};
Vector<float, 4> q1_dot1_check(data_q_dot1_check);
TEST(isEqual(q1_dot1, q1_dot1_check));
// quaternion derivative in frame 2
Vector<float, 4> q1_dot2 = q1.derivative2(Vector3f(1, 2, 3));
float data_q_dot2_check[] = { -0.5f, 0.0f, -1.5f, 1.0f};
Vector<float, 4> q1_dot2_check(data_q_dot2_check);
TEST(isEqual(q1_dot2, q1_dot2_check));
// quaternion product
Quatf q_prod_check(
@@ -220,8 +226,12 @@ int main()
TEST(fabsf(q_check(2) + q(2)) < eps);
TEST(fabsf(q_check(3) + q(3)) < eps);
// rotate quaternion (nonzero rotation)
// non-unit quaternion invese
Quatf qI(1.0f, 0.0f, 0.0f, 0.0f);
Quatf q_nonunit(0.1f, 0.2f, 0.3f, 0.4f);
TEST(isEqual(qI, q_nonunit*q_nonunit.inversed()));
// rotate quaternion (nonzero rotation)
Vector<float, 3> rot;
rot(0) = 1.0f;
rot(1) = rot(2) = 0.0f;
@@ -270,7 +280,7 @@ int main()
float q_array[] = {0.9833f, -0.0343f, -0.1060f, -0.1436f};
Quaternion<float>q_from_array(q_array);
for (int i = 0; i < 4; i++) {
for (size_t i = 0; i < 4; i++) {
TEST(fabsf(q_from_array(i) - q_array[i]) < eps);
}
@@ -333,6 +343,6 @@ int main()
q = Quatf(0,0,0,1); // 180 degree rotation around the z axis
R = Dcmf(q);
TEST(isEqual(q, Quatf(R)));
};
}
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
test/inverse.cpp
+20
View File
@@ -106,6 +106,26 @@ int main()
TEST(isEqual(A3_I, Z3));
TEST(isEqual(A3.I(), Z3));
float data4[9] = {
1.33471626f, 0.74946721f, -0.0531679f,
0.74946721f, 1.07519593f, 0.08036323f,
-0.0531679f, 0.08036323f, 1.01618474f
};
SquareMatrix<float, 3> A4(data4);
float data4_cholesky[9] = {
1.15529921f, 0.f, 0.f,
0.6487213f , 0.80892311f, 0.f,
-0.04602089f, 0.13625271f, 0.99774847f
};
SquareMatrix<float, 3> A4_cholesky_check(data4_cholesky);
SquareMatrix<float, 3> A4_cholesky = cholesky(A4);
TEST(isEqual(A4_cholesky_check, A4_cholesky));
SquareMatrix<float, 3> I3;
I3.setIdentity();
TEST(isEqual(choleskyInv(A4)*A4, I3));
TEST(isEqual(cholesky(Z3), Z3));
return 0;
}
test/matrixAssignment.cpp
-2
View File
@@ -101,10 +101,8 @@ int main()
Scalar<float> s;
s = 1;
float const & s_float = (const Scalar<float>)s;
const Vector<float, 1> & s_vect = s;
TEST(fabs(s - 1) < 1e-5);
TEST(fabs(s_float - 1.0f) < 1e-5);
TEST(fabs(s_vect(0) - 1.0f) < 1e-5);
Matrix<float, 1, 1> m5 = s;
test/setIdentity.cpp
+4 -4
View File
@@ -10,8 +10,8 @@ int main()
Matrix3f A;
A.setIdentity();
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
for (size_t i = 0; i < 3; i++) {
for (size_t j = 0; j < 3; j++) {
if (i == j) {
TEST( fabs(A(i, j) - 1) < 1e-7);
@@ -24,8 +24,8 @@ int main()
Matrix3f B;
B.identity();
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
for (size_t i = 0; i < 3; i++) {
for (size_t j = 0; j < 3; j++) {
if (i == j) {
TEST( fabs(B(i, j) - 1) < 1e-7);