diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-07-07 18:49:36 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-07-07 18:49:36 +0200 |
commit | 6a8990b3baac40cf0b4ef7a8ea3bdda73d3513e6 (patch) | |
tree | 30b4387f17e1ac99396fcb7bc9fb848cb930a40b /src/modules/ekf_att_pos_estimator/estimator_utilities.cpp | |
parent | cd9f4f33a593e8eea8a0314b776d1ad3ef847462 (diff) | |
parent | be73ad0bdb932e35cf891e8f5ffc1b89fdd683dc (diff) | |
download | px4-firmware-6a8990b3baac40cf0b4ef7a8ea3bdda73d3513e6.tar.gz px4-firmware-6a8990b3baac40cf0b4ef7a8ea3bdda73d3513e6.tar.bz2 px4-firmware-6a8990b3baac40cf0b4ef7a8ea3bdda73d3513e6.zip |
Merged master
Diffstat (limited to 'src/modules/ekf_att_pos_estimator/estimator_utilities.cpp')
-rw-r--r-- | src/modules/ekf_att_pos_estimator/estimator_utilities.cpp | 20 |
1 files changed, 20 insertions, 0 deletions
diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp index b4767a0d3..29a8c8d1e 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp @@ -8,6 +8,7 @@ #ifdef EKF_DEBUG #include <stdio.h> +#include <stdarg.h> static void ekf_debug_print(const char *fmt, va_list args) @@ -101,6 +102,25 @@ Vector3f operator*( Mat3f matIn, Vector3f vecIn) return vecOut; } +// overload * operator to provide a matrix product +Mat3f operator*( Mat3f matIn1, Mat3f matIn2) +{ + Mat3f matOut; + matOut.x.x = matIn1.x.x*matIn2.x.x + matIn1.x.y*matIn2.y.x + matIn1.x.z*matIn2.z.x; + matOut.x.y = matIn1.x.x*matIn2.x.y + matIn1.x.y*matIn2.y.y + matIn1.x.z*matIn2.z.y; + matOut.x.z = matIn1.x.x*matIn2.x.z + matIn1.x.y*matIn2.y.z + matIn1.x.z*matIn2.z.z; + + matOut.y.x = matIn1.y.x*matIn2.x.x + matIn1.y.y*matIn2.y.x + matIn1.y.z*matIn2.z.x; + matOut.y.y = matIn1.y.x*matIn2.x.y + matIn1.y.y*matIn2.y.y + matIn1.y.z*matIn2.z.y; + matOut.y.z = matIn1.y.x*matIn2.x.z + matIn1.y.y*matIn2.y.z + matIn1.y.z*matIn2.z.z; + + matOut.z.x = matIn1.z.x*matIn2.x.x + matIn1.z.y*matIn2.y.x + matIn1.z.z*matIn2.z.x; + matOut.z.y = matIn1.z.x*matIn2.x.y + matIn1.z.y*matIn2.y.y + matIn1.z.z*matIn2.z.y; + matOut.z.z = matIn1.z.x*matIn2.x.z + matIn1.z.y*matIn2.y.z + matIn1.z.z*matIn2.z.z; + + return matOut; +} + // overload % operator to provide a vector cross product Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2) { |