fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 00:23:34 +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
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)
}
};
}