diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2015-04-12 01:47:48 +0200 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2015-04-12 01:47:48 +0200 |
commit | dc2bb2a85ea4d2a840306cafa6aa4e217ee4dbe9 (patch) | |
tree | 965e2902540f20668500e166010d9a725ac946bb /src/lib/mathlib | |
parent | d4ae721bc0a96509cfbdb004bbe302694e143ee7 (diff) | |
download | px4-firmware-dc2bb2a85ea4d2a840306cafa6aa4e217ee4dbe9.tar.gz px4-firmware-dc2bb2a85ea4d2a840306cafa6aa4e217ee4dbe9.tar.bz2 px4-firmware-dc2bb2a85ea4d2a840306cafa6aa4e217ee4dbe9.zip |
attitude_estimator_q added
Diffstat (limited to 'src/lib/mathlib')
-rw-r--r-- | src/lib/mathlib/math/Matrix.hpp | 18 | ||||
-rw-r--r-- | src/lib/mathlib/math/Quaternion.hpp | 106 |
2 files changed, 100 insertions, 24 deletions
diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index f6f4fc5ea..2a7b61238 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -136,6 +136,24 @@ public: #endif /** + * 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) { diff --git a/src/lib/mathlib/math/Quaternion.hpp b/src/lib/mathlib/math/Quaternion.hpp index d28966fca..0603f0049 100644 --- a/src/lib/mathlib/math/Quaternion.hpp +++ b/src/lib/mathlib/math/Quaternion.hpp @@ -94,6 +94,19 @@ public: } /** + * 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) { @@ -109,39 +122,73 @@ public: } /** - * imaginary part of quaternion + * conjugate */ - Vector<3> imag(void) { - return Vector<3>(&data[1]); + Quaternion conjugated() const { + return Quaternion(data[0], -data[1], -data[2], -data[3]); } /** - * inverse of quaternion + * inversed */ - math::Quaternion inverse() { - Quaternion res; - memcpy(res.data,data,sizeof(res.data)); - res.data[1] = -res.data[1]; - res.data[2] = -res.data[2]; - res.data[3] = -res.data[3]; - return res; + 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) + ); + } /** - * rotate vector by quaternion - */ - Vector<3> rotate(const Vector<3> &w) { - Quaternion q_w; // extend vector to quaternion - Quaternion q = {data[0],data[1],data[2],data[3]}; - Quaternion q_rotated; // quaternion representation of rotated vector - q_w(0) = 0; - q_w(1) = w.data[0]; - q_w(2) = w.data[1]; - q_w(3) = w.data[2]; - q_rotated = q*q_w*q.inverse(); - Vector<3> res = {q_rotated.data[1],q_rotated.data[2],q_rotated.data[3]}; - return res; + * 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(void) { + return Vector<3>(&data[1]); } /** @@ -165,6 +212,17 @@ public: } /** + * create Euler angles vector from the quaternion + */ + Vector<3> to_euler() const { + return Vector<3>( + atan2(2.0f * (data[0] * data[1] + data[2] * data[3]), 1.0f - 2.0f * (data[1] * data[1] + data[2] * data[2])), + asin(2.0f * (data[0] * data[2] - data[3] * data[1])), + atan2(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 */ |