From 6906dc4edac85113c3217b250c84e89ed1fe26a4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 12 May 2014 11:23:48 +0200 Subject: [PATCH] Minor improvements to estimator --- src/modules/ekf_att_pos_estimator/estimator.cpp | 11 ++++++++--- src/modules/ekf_att_pos_estimator/estimator.h | 1 + 2 files changed, 9 insertions(+), 3 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 9d60f5c0e5..d05506396f 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -1,5 +1,6 @@ #include "estimator.h" #include +#include // Define EKF_DEBUG here to enable the debug print calls // if the macro is not set, these will be completely @@ -2119,9 +2120,7 @@ void AttPosEKF::ConstrainStates() } // Constrain delta velocity bias - ekf_debug("pre delta vel"); states[13] = ConstrainFloat(states[13], -1.0f * dtIMU, 1.0f * dtIMU); - ekf_debug("post delta vel"); // Wind velocity limits - assume 120 m/s max velocity for (unsigned i = 14; i <= 15; i++) { @@ -2483,12 +2482,18 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination) { - //store initial lat,long and height + // store initial lat,long and height latRef = referenceLat; lonRef = referenceLon; hgtRef = referenceHgt; refSet = true; + // we are at reference altitude, so measurement must be zero + hgtMea = 0.0f; + + // the baro offset must be this difference now + baroHgtOffset = baroHgt - referenceHgt; + memset(&last_ekf_error, 0, sizeof(last_ekf_error)); InitializeDynamic(initvelNED, declination); diff --git a/src/modules/ekf_att_pos_estimator/estimator.h b/src/modules/ekf_att_pos_estimator/estimator.h index 378107b698..e821089f23 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.h +++ b/src/modules/ekf_att_pos_estimator/estimator.h @@ -198,6 +198,7 @@ public: float velNED[3]; // North, East, Down velocity obs (m/s) float posNE[2]; // North, East position obs (m) float hgtMea; // measured height (m) + float baroHgtOffset; ///< the baro (weather) offset from normalized altitude float rngMea; // Ground distance float posNED[3]; // North, East Down position (m)