From 6b08e8ce1f9a3a96c3b378a21e23f44c42de531c Mon Sep 17 00:00:00 2001
From: James Goppert <jgoppert@users.noreply.github.com>
Date: Sat, 27 Aug 2016 23:50:24 -0400
Subject: [PATCH] Improvements to lpe for flow and gps. (#5401)
---
.../BlockLocalPositionEstimator.cpp | 19 +++++++++-----
.../BlockLocalPositionEstimator.hpp | 12 +++++----
src/modules/local_position_estimator/params.c | 23 ++++++++++++++++
.../local_position_estimator/sensors/flow.cpp | 26 +++++++++++++------
.../local_position_estimator/sensors/gps.cpp | 11 ++++++++
src/modules/mavlink/mavlink_messages.cpp | 23 +++++++++++++++-
6 files changed, 94 insertions(+), 20 deletions(-)
diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
index 5d08da6f4e..9eb2d3a5f5 100644
--- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
+++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
@@ -79,12 +79,14 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
_mocap_p_stddev(this, "VIC_P"),
_flow_z_offset(this, "FLW_OFF_Z"),
_flow_xy_stddev(this, "FLW_XY"),
+ _flow_xy_d_stddev(this, "FLW_XY_D"),
//_flow_board_x_offs(NULL, "SENS_FLW_XOFF"),
//_flow_board_y_offs(NULL, "SENS_FLW_YOFF"),
_flow_min_q(this, "FLW_QMIN"),
_pn_p_noise_density(this, "PN_P"),
_pn_v_noise_density(this, "PN_V"),
_pn_b_noise_density(this, "PN_B"),
+ _pn_t_noise_density(this, "PN_T"),
_t_max_grade(this, "T_MAX_GRADE"),
// init origin
@@ -150,7 +152,6 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
// flow integration
_flowX(0),
_flowY(0),
- _flowMeanQual(0),
// status
_validXY(false),
@@ -197,8 +198,6 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
// initialize A, B, P, x, u
_x.setZero();
_u.setZero();
- _flowX = 0;
- _flowY = 0;
initSS();
// perf counters
@@ -689,7 +688,11 @@ void BlockLocalPositionEstimator::publishLocalPos()
_pub_lpos.get().dist_bottom = _aglLowPass.getState();
_pub_lpos.get().dist_bottom_rate = - xLP(X_vz);
_pub_lpos.get().surface_bottom_timestamp = _timeStamp;
- _pub_lpos.get().dist_bottom_valid = _validTZ && _validZ;
+ // we estimate agl even when we don't have terrain info
+ // if you are in terrain following mode this is important
+ // so that if terrain estimation fails there isn't a
+ // sudden altitude jump
+ _pub_lpos.get().dist_bottom_valid = _validZ;
_pub_lpos.get().eph = sqrtf(_P(X_x, X_x) + _P(X_y, X_y));
_pub_lpos.get().epv = sqrtf(_P(X_z, X_z));
_pub_lpos.update();
@@ -840,8 +843,11 @@ void BlockLocalPositionEstimator::updateSSParams()
_Q(X_bz, X_bz) = pn_b_sq;
// terrain random walk noise ((m/s)/sqrt(hz)), scales with velocity
- float pn_t_stddev = (_t_max_grade.get() / 100.0f) * sqrtf(_x(X_vx) * _x(X_vx) + _x(X_vy) * _x(X_vy));
- _Q(X_tz, X_tz) = pn_t_stddev * pn_t_stddev;
+ float pn_t_noise_density =
+ _pn_t_noise_density.get() +
+ (_t_max_grade.get() / 100.0f) * sqrtf(_x(X_vx) * _x(X_vx) + _x(X_vy) * _x(X_vy));
+ _Q(X_tz, X_tz) = pn_t_noise_density * pn_t_noise_density;
+
}
void BlockLocalPositionEstimator::predict()
@@ -853,6 +859,7 @@ void BlockLocalPositionEstimator::predict()
if (integrate && _sub_att.get().R_valid) {
Matrix3f R_att(_sub_att.get().R);
Vector3f a(_sub_sensor.get().accelerometer_m_s2);
+ // note, bias is removed in dynamics function
_u = R_att * a;
_u(U_az) += 9.81f; // add g
diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
index 7f3ddc29b5..163235bd64 100644
--- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
+++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
@@ -98,10 +98,10 @@ class BlockLocalPositionEstimator : public control::SuperBlock
// ax, ay, az (acceleration NED)
//
// states:
-// px, py, pz , ( position NED)
-// vx, vy, vz ( vel NED),
-// bx, by, bz ( accel bias)
-// tz (terrain altitude, ASL)
+// px, py, pz , ( position NED, m)
+// vx, vy, vz ( vel NED, m/s),
+// bx, by, bz ( accel bias, m/s^2)
+// tz (terrain altitude, ASL, m)
//
// measurements:
//
@@ -184,6 +184,7 @@ private:
int flowMeasure(Vector<float, n_y_flow> &y);
void flowCorrect();
void flowInit();
+ void flowDeinit();
void flowCheckTimeout();
// vision
@@ -282,6 +283,7 @@ private:
// flow parameters
BlockParamFloat _flow_z_offset;
BlockParamFloat _flow_xy_stddev;
+ BlockParamFloat _flow_xy_d_stddev;
//BlockParamFloat _flow_board_x_offs;
//BlockParamFloat _flow_board_y_offs;
BlockParamInt _flow_min_q;
@@ -290,6 +292,7 @@ private:
BlockParamFloat _pn_p_noise_density;
BlockParamFloat _pn_v_noise_density;
BlockParamFloat _pn_b_noise_density;
+ BlockParamFloat _pn_t_noise_density;
BlockParamFloat _t_max_grade;
// init origin
@@ -354,7 +357,6 @@ private:
// flow integration
float _flowX;
float _flowY;
- float _flowMeanQual;
// status
bool _validXY;
diff --git a/src/modules/local_position_estimator/params.c b/src/modules/local_position_estimator/params.c
index 3543377a10..455f53d142 100644
--- a/src/modules/local_position_estimator/params.c
+++ b/src/modules/local_position_estimator/params.c
@@ -25,6 +25,17 @@ PARAM_DEFINE_FLOAT(LPE_FLW_OFF_Z, 0.0f);
*/
PARAM_DEFINE_FLOAT(LPE_FLW_XY, 0.01f);
+/**
+ * Optical flow xy standard deviation linear factor on distance
+ *
+ * @group Local Position Estimator
+ * @unit m / m
+ * @min 0.01
+ * @max 1
+ * @decimal 3
+ */
+PARAM_DEFINE_FLOAT(LPE_FLW_XY_D, 0.01f);
+
/**
* Optical flow minimum quality threshold
*
@@ -285,8 +296,20 @@ PARAM_DEFINE_FLOAT(LPE_PN_V, 0.1f);
*/
PARAM_DEFINE_FLOAT(LPE_PN_B, 1e-3f);
+/**
+ * Terrain random walk noise density, hilly/outdoor (0.1), flat/Indoor (0.001)
+ *
+ * @group Local Position Estimator
+ * @unit (m/s)/(sqrt(hz))
+ * @min 0
+ * @max 1
+ * @decimal 3
+ */
+PARAM_DEFINE_FLOAT(LPE_PN_T, 0.001f);
+
/**
* Terrain maximum percent grade, hilly/outdoor (100 = 45 deg), flat/Indoor (0 = 0 deg)
+ * Used to calculate increased terrain random walk nosie due to movement.
*
* @group Local Position Estimator
* @unit %
diff --git a/src/modules/local_position_estimator/sensors/flow.cpp b/src/modules/local_position_estimator/sensors/flow.cpp
index bdfcb3fb7e..85e53b8082 100644
--- a/src/modules/local_position_estimator/sensors/flow.cpp
+++ b/src/modules/local_position_estimator/sensors/flow.cpp
@@ -29,11 +29,21 @@ void BlockLocalPositionEstimator::flowInit()
"quality %d std %d",
int(_flowQStats.getMean()(0)),
int(_flowQStats.getStdDev()(0)));
+ // set flow x, y as estimate x, y at beginning of optical
+ // flow tracking
+ _flowX = _x(X_x);
+ _flowY = _x(X_y);
_flowInitialized = true;
_flowFault = FAULT_NONE;
}
}
+void BlockLocalPositionEstimator::flowDeinit()
+{
+ _flowInitialized = false;
+ _flowQStats.reset();
+}
+
int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y)
{
// check for agl
@@ -66,8 +76,9 @@ int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y)
if (delta.norm() > 3) {
mavlink_and_console_log_info(&mavlink_log_pub,
- "[lpe] flow too far from GPS, disabled");
- _flowInitialized = false;
+ "[lpe] flow too far from GPS, resetting position");
+ _flowX = px;
+ _flowY = py;
return -1;
}
}
@@ -127,10 +138,10 @@ void BlockLocalPositionEstimator::flowCorrect()
SquareMatrix<float, n_y_flow> R;
R.setZero();
- R(Y_flow_x, Y_flow_x) =
- _flow_xy_stddev.get() * _flow_xy_stddev.get();
- R(Y_flow_y, Y_flow_y) =
- _flow_xy_stddev.get() * _flow_xy_stddev.get();
+ float d = agl() * cosf(_sub_att.get().roll) * cosf(_sub_att.get().pitch);
+ float flow_xy_stddev = _flow_xy_stddev.get() + _flow_xy_d_stddev.get() * d ;
+ R(Y_flow_x, Y_flow_x) = flow_xy_stddev * flow_xy_stddev;
+ R(Y_flow_y, Y_flow_y) = R(Y_flow_x, Y_flow_x);
// residual
Vector<float, 2> r = y - C * _x;
@@ -178,8 +189,7 @@ void BlockLocalPositionEstimator::flowCheckTimeout()
{
if (_timeStamp - _time_last_flow > FLOW_TIMEOUT) {
if (_flowInitialized) {
- _flowInitialized = false;
- _flowQStats.reset();
+ flowDeinit();
mavlink_and_console_log_critical(&mavlink_log_pub, "[lpe] flow timeout ");
}
}
diff --git a/src/modules/local_position_estimator/sensors/gps.cpp b/src/modules/local_position_estimator/sensors/gps.cpp
index 740ee0b3c2..118cf0b46b 100644
--- a/src/modules/local_position_estimator/sensors/gps.cpp
+++ b/src/modules/local_position_estimator/sensors/gps.cpp
@@ -148,6 +148,17 @@ void BlockLocalPositionEstimator::gpsCorrect()
var_z = _sub_gps.get().epv * _sub_gps.get().epv;
}
+ float gps_s_stddev = _sub_gps.get().s_variance_m_s;
+
+ if (gps_s_stddev > _gps_vxy_stddev.get()) {
+ var_vxy = gps_s_stddev * gps_s_stddev;
+ }
+
+ if (gps_s_stddev > _gps_vz_stddev.get()) {
+ var_z = gps_s_stddev * gps_s_stddev;
+ }
+
+
R(0, 0) = var_xy;
R(1, 1) = var_xy;
R(2, 2) = var_z;
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 16f856e55d..917abf2594 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -3158,12 +3158,33 @@ protected:
struct vehicle_local_position_s local_pos;
updated |= _local_pos_sub->update(&_local_pos_time, &local_pos);
msg.altitude_local = (_local_pos_time > 0) ? -local_pos.z : NAN;
+
+ // publish this data if global isn't publishing
+ if (_global_pos_time == 0) {
+ if (local_pos.dist_bottom_valid) {
+ msg.bottom_clearance = local_pos.dist_bottom;
+ msg.altitude_terrain = msg.altitude_local - msg.bottom_clearance;
+
+ } else {
+ msg.bottom_clearance = NAN;
+ msg.altitude_terrain = NAN;
+ }
+ }
}
{
struct home_position_s home;
updated |= _home_sub->update(&_home_time, &home);
- msg.altitude_relative = (_home_time > 0) ? (global_alt - home.alt) : NAN;
+
+ if (_global_pos_time > 0 and _home_time > 0) {
+ msg.altitude_relative = global_alt - home.alt;
+
+ } else if (_local_pos_time > 0 and _home_time > 0) {
+ msg.altitude_relative = msg.altitude_local;
+
+ } else {
+ msg.altitude_relative = NAN;
+ }
}
if (updated) {
--
GitLab