From 6b08e8ce1f9a3a96c3b378a21e23f44c42de531c Mon Sep 17 00:00:00 2001 From: James Goppert 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 &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 &y) { // check for agl @@ -66,8 +76,9 @@ int BlockLocalPositionEstimator::flowMeasure(Vector &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 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 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) {