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 | |
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
-rw-r--r-- | src/lib/mathlib/math/Matrix.hpp | 15 | ||||
-rw-r--r-- | src/lib/mathlib/math/Vector.hpp | 7 | ||||
-rw-r--r-- | src/modules/att_pos_estimator_ekf/KalmanNav.cpp | 20 | ||||
-rw-r--r-- | src/modules/att_pos_estimator_ekf/kalman_main.cpp | 2 |
4 files changed, 34 insertions, 10 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++) diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp index 4ab41c6ce..9d3ef07f2 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp @@ -97,6 +97,14 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : { using namespace math; + F.zero(); + G.zero(); + V.zero(); + HAtt.zero(); + RAtt.zero(); + HPos.zero(); + RPos.zero(); + // initial state covariance matrix P0.identity(); P0 *= 0.01f; @@ -214,8 +222,8 @@ void KalmanNav::update() if (correctAtt() == ret_ok) _attitudeInitCounter++; if (_attitudeInitCounter > 100) { - warnx("initialized EKF attitude\n"); - warnx("phi: %8.4f, theta: %8.4f, psi: %8.4f\n", + warnx("initialized EKF attitude"); + warnx("phi: %8.4f, theta: %8.4f, psi: %8.4f", double(phi), double(theta), double(psi)); _attitudeInitialized = true; } @@ -245,8 +253,8 @@ void KalmanNav::update() // lat/lon and not have init map_projection_init(lat0, lon0); _positionInitialized = true; - warnx("initialized EKF state with GPS\n"); - warnx("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n", + warnx("initialized EKF state with GPS"); + warnx("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f", double(vN), double(vE), double(vD), lat, lon, double(alt)); } @@ -625,7 +633,7 @@ int KalmanNav::correctAtt() if (isnan(val) || isinf(val)) { // abort correction and return - warnx("numerical failure in att correction\n"); + warnx("numerical failure in att correction"); // reset P matrix to P0 P = P0; return ret_error; @@ -691,7 +699,7 @@ int KalmanNav::correctPos() if (!isfinite(val)) { // abort correction and return - warnx("numerical failure in gps correction\n"); + warnx("numerical failure in gps correction"); // fallback to GPS vN = _gps.vel_n_m_s; vE = _gps.vel_e_m_s; diff --git a/src/modules/att_pos_estimator_ekf/kalman_main.cpp b/src/modules/att_pos_estimator_ekf/kalman_main.cpp index 372b2d162..70ba628f3 100644 --- a/src/modules/att_pos_estimator_ekf/kalman_main.cpp +++ b/src/modules/att_pos_estimator_ekf/kalman_main.cpp @@ -107,7 +107,7 @@ int att_pos_estimator_ekf_main(int argc, char *argv[]) daemon_task = task_spawn_cmd("att_pos_estimator_ekf", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 30, - 4096, + 8096, kalman_demo_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); exit(0); |