diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-07-07 14:09:29 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-07-07 14:09:29 +0200 |
commit | 77ca6fa8a3d3990bf75a02da70b25c7a0c6ae91e (patch) | |
tree | b09cc86c699ad66e58b416307fb947397c4a6ffb /src/modules/ekf_att_pos_estimator/estimator_utilities.cpp | |
parent | 509180abf6ff9e6a291e34bfb7dbdd105454feb5 (diff) | |
parent | 2ec635dd1c33ef453818bd9396e3cec42694a30f (diff) | |
download | px4-firmware-77ca6fa8a3d3990bf75a02da70b25c7a0c6ae91e.tar.gz px4-firmware-77ca6fa8a3d3990bf75a02da70b25c7a0c6ae91e.tar.bz2 px4-firmware-77ca6fa8a3d3990bf75a02da70b25c7a0c6ae91e.zip |
Merge pull request #1142 from PX4/simon_test
EKF filter update
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) { |