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/modules/att_pos_estimator_ekf/KalmanNav.cpp | |
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/modules/att_pos_estimator_ekf/KalmanNav.cpp')
-rw-r--r-- | src/modules/att_pos_estimator_ekf/KalmanNav.cpp | 20 |
1 files changed, 14 insertions, 6 deletions
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; |