diff options
| author | Anton Babushkin <anton.babushkin@me.com> | 2013-12-23 22:34:11 +0400 |
|---|---|---|
| committer | Anton Babushkin <anton.babushkin@me.com> | 2013-12-23 22:34:11 +0400 |
| commit | a26e46bede186c36b475e0b5a9cf3ef758cc1c9b (patch) | |
| tree | c959c0d5e43352048c4d39f9a39624811e45827d /src/lib/mathlib | |
| parent | 5a2da77758867914e3e8b55267df56a6f95913ed (diff) | |
| download | px4-firmware-a26e46bede186c36b475e0b5a9cf3ef758cc1c9b.tar.gz px4-firmware-a26e46bede186c36b475e0b5a9cf3ef758cc1c9b.tar.bz2 px4-firmware-a26e46bede186c36b475e0b5a9cf3ef758cc1c9b.zip | |
att_pos_estimator_ekf: fixes, mathlib: minor changes
Diffstat (limited to 'src/lib/mathlib')
| -rw-r--r-- | src/lib/mathlib/math/Matrix.hpp | 15 | ||||
| -rw-r--r-- | src/lib/mathlib/math/Vector.hpp | 7 |
2 files changed, 19 insertions, 3 deletions
diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index 31b03b54b..7ed8879a7 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -241,19 +241,28 @@ public: } /** - * setup the identity matrix + * set zero matrix + */ + void zero(void) { + memset(data, 0, sizeof(data)); + } + + /** + * set identity matrix */ void identity(void) { memset(data, 0, sizeof(data)); - for (unsigned int i = 0; i < M; i++) + unsigned int n = (M < N) ? M : N; + for (unsigned int i = 0; i < n; i++) data[i][i] = 1; } void print(void) { for (unsigned int i = 0; i < M; i++) { + printf("[ "); for (unsigned int j = 0; j < N; j++) printf("%.3f\t", data[i][j]); - printf("\n"); + printf(" ]\n"); } } }; diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp index c865bab42..744402e21 100644 --- a/src/lib/mathlib/math/Vector.hpp +++ b/src/lib/mathlib/math/Vector.hpp @@ -259,6 +259,13 @@ public: return *this / length(); } + /** + * set zero vector + */ + void zero(void) { + memset(data, 0, sizeof(data)); + } + void print(void) { printf("[ "); for (unsigned int i = 0; i < N; i++) |