From bcba1fb27e225b7ecb051a741e2dcef827588eb7 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Mon, 14 Mar 2016 11:28:08 -0400 Subject: [PATCH] Enabled gps delay for lpe based on x hist. --- .../BlockLocalPositionEstimator.cpp | 345 ++++++++++-------- .../BlockLocalPositionEstimator.hpp | 26 +- .../local_position_estimator/CMakeLists.txt | 2 +- .../local_position_estimator/fault_table.py | 13 + .../local_position_estimator_main.cpp | 2 +- src/modules/local_position_estimator/params.c | 59 +-- 6 files changed, 255 insertions(+), 192 deletions(-) create mode 100644 src/modules/local_position_estimator/fault_table.py diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index 26753fc755..55230373b6 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -4,6 +4,8 @@ #include #include +// required number of samples for sensor +// to initialize static const int REQ_BARO_INIT_COUNT = 100; static const int REQ_FLOW_INIT_COUNT = 20; static const int REQ_GPS_INIT_COUNT = 10; @@ -12,6 +14,7 @@ static const int REQ_SONAR_INIT_COUNT = 20; static const int REQ_VISION_INIT_COUNT = 20; static const int REQ_MOCAP_INIT_COUNT = 20; +// timeouts for sensors in microseconds static const uint32_t BARO_TIMEOUT = 1000000; // 1.0 s static const uint32_t FLOW_TIMEOUT = 500000; // 0.5 s static const uint32_t GPS_TIMEOUT = 1000000; // 1.0 s @@ -20,14 +23,23 @@ static const uint32_t VISION_TIMEOUT = 500000; // 0.5 s static const uint32_t MOCAP_TIMEOUT = 200000; // 0.2 s static const uint32_t EST_SRC_TIMEOUT = 500000; // 0.5 s -// for fault detection -// chi squared distribution, false alarm probability 0.005 -// http://sites.stat.psu.edu/~mga/401/tables/Chi-square-table.pdf -static const float BETA_TABLE[7] = {0, 7.879, 10.597, - 12.838, 14.860, 16.750, 18.548 - }; +// change this to set when +// the system will abort correcting a measurement +// given a fault has been detected +static fault_t fault_lvl_disable = FAULT_SEVERE; -using namespace std; +// for fault detection +// chi squared distribution, false alarm probability 0.0001 +// see fault_table.py +// note skip 0 index so we can use degree of freedom as index +static const float BETA_TABLE[7] = {0, + 8.82050518214, + 12.094592431, + 13.9876612368, + 16.0875642296, + 17.8797700658, + 19.6465647819, + }; BlockLocalPositionEstimator::BlockLocalPositionEstimator() : // this block has no parent, and has name LPE @@ -57,7 +69,6 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() : // publications _pub_lpos(ORB_ID(vehicle_local_position), -1, &getPublications()), _pub_gpos(ORB_ID(vehicle_global_position), -1, &getPublications()), - //_pub_filtered_flow(ORB_ID(filtered_bottom_flow), -1, &getPublications()), _pub_est_status(ORB_ID(estimator_status), -1, &getPublications()), // map projection @@ -72,6 +83,7 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() : _accel_xy_stddev(this, "ACC_XY"), _accel_z_stddev(this, "ACC_Z"), _baro_stddev(this, "BAR_Z"), + _gps_delay(this, "GPS_DELAY"), _gps_xy_stddev(this, "GPS_XY"), _gps_z_stddev(this, "GPS_Z"), _gps_vxy_stddev(this, "GPS_VXY"), @@ -86,10 +98,10 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() : //_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_power(this, "PN_P"), - _pn_v_noise_power(this, "PN_V"), - _pn_b_noise_power(this, "PN_B"), - _pn_t_noise_power(this, "PN_T"), + _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"), // flow gyro _flow_gyro_x_high_pass(this, "FGYRO_HP"), @@ -105,14 +117,13 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() : _gpsStats(this, ""), // stats - //_xDelay(this, ""), - //_PDelay(this, ""), - //_tDelay(this, ""), + _xDelay(this, ""), + _tDelay(this, ""), // misc _polls(), _timeStamp(hrt_absolute_time()), - //_time_last_hist(0), + _time_last_hist(0), _time_last_xy(0), _time_last_z(0), _time_last_tz(0), @@ -157,6 +168,7 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() : _canEstimateXY(false), _canEstimateZ(false), _canEstimateT(false), + _canEstimateGlobal(true), _xyTimeout(true), _zTimeout(true), _tzTimeout(true), @@ -282,15 +294,15 @@ void BlockLocalPositionEstimator::update() // determine if we should start estimating _canEstimateZ = - (_baroInitialized && _baroFault < FAULT_SEVERE); + (_baroInitialized && _baroFault < fault_lvl_disable); _canEstimateXY = - (_gpsInitialized && _gpsFault < FAULT_SEVERE) || - (_flowInitialized && _flowFault < FAULT_SEVERE) || - (_visionInitialized && _visionFault < FAULT_SEVERE) || - (_mocapInitialized && _mocapFault < FAULT_SEVERE); + (_gpsInitialized && _gpsFault < fault_lvl_disable) || + (_flowInitialized && _flowFault < fault_lvl_disable) || + (_visionInitialized && _visionFault < fault_lvl_disable) || + (_mocapInitialized && _mocapFault < fault_lvl_disable); _canEstimateT = - (_lidarInitialized && _lidarFault < FAULT_SEVERE) || - (_sonarInitialized && _sonarFault < FAULT_SEVERE); + (_lidarInitialized && _lidarFault < fault_lvl_disable) || + (_sonarInitialized && _sonarFault < fault_lvl_disable); if (_canEstimateXY) { _time_last_xy = _timeStamp; @@ -319,7 +331,7 @@ void BlockLocalPositionEstimator::update() // should we do a reinit // of sensors here? // don't want it to take too long - if (!isfinite(_x(i))) { + if (!PX4_ISFINITE(_x(i))) { reinit_x = true; break; } @@ -339,7 +351,7 @@ void BlockLocalPositionEstimator::update() for (int i = 0; i < n_x; i++) { for (int j = 0; j < n_x; j++) { - if (!isfinite(_P(i, j))) { + if (!PX4_ISFINITE(_P(i, j))) { reinit_P = true; break; } @@ -430,7 +442,10 @@ void BlockLocalPositionEstimator::update() // update all publications if possible publishLocalPos(); publishEstimatorStatus(); - publishGlobalPos(); + + if (_canEstimateGlobal) { + publishGlobalPos(); + } } else if (!_zTimeout && _altHomeInitialized) { // publish only Z estimate @@ -438,6 +453,17 @@ void BlockLocalPositionEstimator::update() publishEstimatorStatus(); } + // propagate delayed state, no matter what + // if state is frozen, delayed state still + // needs to be propagated with frozen state + float dt_hist = 1.0e-6f * (_timeStamp - _time_last_hist); + + if (_time_last_hist == 0 || + (dt_hist > HIST_STEP)) { + _tDelay.update(Scalar(_timeStamp)); + _xDelay.update(_x); + _time_last_hist = _timeStamp; + } } void BlockLocalPositionEstimator::checkTimeouts() @@ -449,7 +475,9 @@ void BlockLocalPositionEstimator::checkTimeouts() warnx("[lpe] xy timeout "); } - } else { + } else if (_xyTimeout) { + mavlink_log_info(_mavlink_fd, "[lpe] xy resume "); + warnx("[lpe] xy resume "); _xyTimeout = false; } @@ -460,7 +488,9 @@ void BlockLocalPositionEstimator::checkTimeouts() warnx("[lpe] z timeout "); } - } else { + } else if (_zTimeout) { + mavlink_log_info(_mavlink_fd, "[lpe] z resume "); + warnx("[lpe] z resume "); _zTimeout = false; } @@ -471,7 +501,9 @@ void BlockLocalPositionEstimator::checkTimeouts() warnx("[lpe] tz timeout "); } - } else { + } else if (_tzTimeout) { + mavlink_log_info(_mavlink_fd, "[lpe] tz resume "); + warnx("[lpe] tz resume "); _tzTimeout = false; } @@ -542,9 +574,12 @@ void BlockLocalPositionEstimator::updateHome() // like gps and baro to be off, need to allow it // to reset by resetting covariance initP(); - - mavlink_log_info(_mavlink_fd, "[lpe] home: lat %5.0f, lon %5.0f, alt %5.0f", lat, lon, double(alt)); - warnx("[lpe] home: lat %5.0f, lon %5.0f, alt %5.0f", lat, lon, double(alt)); + mavlink_log_info(_mavlink_fd, "[lpe] home " + "lat %6.2f lon %6.2f alt %5.1f m", + lat, lon, double(alt)); + warnx("[lpe] home " + "lat %6.2f lon %6.2f alt %5.1f m", + lat, lon, double(alt)); map_projection_init(&_map_ref, lat, lon); float delta_alt = alt - _altHome; _altHomeInitialized = true; @@ -564,10 +599,10 @@ void BlockLocalPositionEstimator::initBaro() if (_baroStats.getCount() > REQ_BARO_INIT_COUNT) { _baroAltHome = _baroStats.getMean()(0); mavlink_log_info(_mavlink_fd, - "[lpe] baro offs: %d m stddev %d cm", + "[lpe] baro init %d m std %d cm", (int)_baroStats.getMean()(0), (int)(100 * _baroStats.getStdDev()(0))); - warnx("[lpe] baro offs: %d m stddev %d cm", + warnx("[lpe] baro init %d m std %d cm", (int)_baroStats.getMean()(0), (int)(100 * _baroStats.getStdDev()(0))); _baroInitialized = true; @@ -608,16 +643,18 @@ void BlockLocalPositionEstimator::initGps() _gpsAltHome = _gpsStats.getMean()(2); map_projection_init(&_map_ref, _gpsLatHome, _gpsLonHome); - mavlink_log_info(_mavlink_fd, "[lpe] gps init: " - "lat %d, lon %d, alt %d m", - int(_gpsLatHome), - int(_gpsLonHome), - int(_gpsAltHome)); - warnx("[lpe] gps init: lat %d, lon %d, alt %d m", - int(_gpsLatHome), - int(_gpsLonHome), - int(_gpsAltHome)); + mavlink_log_info(_mavlink_fd, "[lpe] gps init " + "lat %6.2f lon %6.2f alt %5.1f m", + _gpsLatHome, + _gpsLonHome, + double(_gpsAltHome)); + warnx("[lpe] gps init " + "lat %6.2f lon %6.2f alt %5.1f m", + _gpsLatHome, + _gpsLonHome, + double(_gpsAltHome)); _gpsInitialized = true; + _canEstimateGlobal = true; _gpsStats.reset(); if (!_altHomeInitialized) { @@ -655,11 +692,11 @@ void BlockLocalPositionEstimator::initLidar() // not, might want to hard code this to zero mavlink_log_info(_mavlink_fd, "[lpe] lidar init: " - "mean %d cm, stddev %d cm", + "mean %d cm stddev %d cm", int(100 * _lidarStats.getMean()(0)), int(100 * _lidarStats.getStdDev()(0))); warnx("[lpe] lidar init: " - "mean %d cm, stddev %d cm", + "mean %d cm std %d cm", int(100 * _lidarStats.getMean()(0)), int(100 * _lidarStats.getStdDev()(0))); _lidarInitialized = true; @@ -694,12 +731,12 @@ void BlockLocalPositionEstimator::initSonar() } // not, might want to hard code this to zero - mavlink_log_info(_mavlink_fd, "[lpe] sonar init: " - "mean %d cm, stddev %d cm", + mavlink_log_info(_mavlink_fd, "[lpe] sonar init " + "mean %d cm std %d cm", int(100 * _sonarStats.getMean()(0)), int(100 * _sonarStats.getStdDev()(0))); - warnx("[lpe] sonar init: " - "mean %d cm, stddev %d cm", + warnx("[lpe] sonar init " + "mean %d cm std %d cm", int(100 * _sonarStats.getMean()(0)), int(100 * _sonarStats.getStdDev()(0))); _sonarInitialized = true; @@ -722,11 +759,11 @@ void BlockLocalPositionEstimator::initFlow() if (_flowQStats.getCount() > REQ_FLOW_INIT_COUNT) { mavlink_log_info(_mavlink_fd, "[lpe] flow init: " - "quality %d stddev %d", + "quality %d std %d", int(_flowQStats.getMean()(0)), int(_flowQStats.getStdDev()(0))); warnx("[lpe] flow init: " - "quality %d stddev %d", + "quality %d std %d", int(_flowQStats.getMean()(0)), int(_flowQStats.getStdDev()(0))); _flowInitialized = true; @@ -749,7 +786,7 @@ void BlockLocalPositionEstimator::initVision() if (_visionStats.getCount() > REQ_VISION_INIT_COUNT) { _visionHome = _visionStats.getMean(); mavlink_log_info(_mavlink_fd, "[lpe] vision position init: " - "%f, %f, %f m std dev. %f, %f, %f m", + "%5.2f %5.2f %5.2f m std %5.2f %5.2f %5.2f m", double(_visionStats.getMean()(0)), double(_visionStats.getMean()(1)), double(_visionStats.getMean()(2)), @@ -757,7 +794,7 @@ void BlockLocalPositionEstimator::initVision() double(_visionStats.getStdDev()(1)), double(_visionStats.getStdDev()(2))); warnx("[lpe] vision position init: " - "%f, %f, %f m std dev. %f, %f, %f m", + "%5.2f %5.2f %5.2f m std %5.2f %5.2f %5.2f m", double(_visionStats.getMean()(0)), double(_visionStats.getMean()(1)), double(_visionStats.getMean()(2)), @@ -789,7 +826,7 @@ void BlockLocalPositionEstimator::initMocap() if (_mocapStats.getCount() > REQ_MOCAP_INIT_COUNT) { _mocapHome = _mocapStats.getMean(); mavlink_log_info(_mavlink_fd, "[lpe] mocap position init: " - "%f, %f, %f m std dev. %f, %f, %f m", + "%5.2f, %5.2f, %5.2f m std %5.2f, %5.2f, %5.2f m", double(_mocapStats.getMean()(0)), double(_mocapStats.getMean()(1)), double(_mocapStats.getMean()(2)), @@ -797,7 +834,7 @@ void BlockLocalPositionEstimator::initMocap() double(_mocapStats.getStdDev()(1)), double(_mocapStats.getStdDev()(2))); warnx("[lpe] mocap position init: " - "%f, %f, %f m std dev. %f, %f, %f m", + "%5.2f %5.2f %5.2f m std %5.2f %5.2f %5.2f m", double(_mocapStats.getMean()(0)), double(_mocapStats.getMean()(1)), double(_mocapStats.getMean()(2)), @@ -817,9 +854,9 @@ void BlockLocalPositionEstimator::initMocap() void BlockLocalPositionEstimator::publishLocalPos() { // publish local position - if (isfinite(_x(X_x)) && isfinite(_x(X_y)) && isfinite(_x(X_z)) && - isfinite(_x(X_vx)) && isfinite(_x(X_vy)) - && isfinite(_x(X_vz))) { + if (PX4_ISFINITE(_x(X_x)) && PX4_ISFINITE(_x(X_y)) && PX4_ISFINITE(_x(X_z)) && + PX4_ISFINITE(_x(X_vx)) && PX4_ISFINITE(_x(X_vy)) + && PX4_ISFINITE(_x(X_vz))) { _pub_lpos.get().timestamp = _timeStamp; _pub_lpos.get().xy_valid = _canEstimateXY; _pub_lpos.get().z_valid = _canEstimateZ; @@ -850,9 +887,9 @@ void BlockLocalPositionEstimator::publishLocalPos() void BlockLocalPositionEstimator::publishEstimatorStatus() { - if (isfinite(_x(X_x)) && isfinite(_x(X_y)) && isfinite(_x(X_z)) && - isfinite(_x(X_vx)) && isfinite(_x(X_vy)) - && isfinite(_x(X_vz))) { + if (PX4_ISFINITE(_x(X_x)) && PX4_ISFINITE(_x(X_y)) && PX4_ISFINITE(_x(X_z)) && + PX4_ISFINITE(_x(X_vx)) && PX4_ISFINITE(_x(X_vy)) + && PX4_ISFINITE(_x(X_vz))) { _pub_est_status.get().timestamp = _timeStamp; for (int i = 0; i < n_x; i++) { @@ -863,13 +900,13 @@ void BlockLocalPositionEstimator::publishEstimatorStatus() _pub_est_status.get().n_states = n_x; _pub_est_status.get().nan_flags = 0; _pub_est_status.get().health_flags = - ((_baroFault > 0) << SENSOR_BARO) - + ((_gpsFault > 0) << SENSOR_GPS) - + ((_lidarFault > 0) << SENSOR_LIDAR) - + ((_flowFault > 0) << SENSOR_FLOW) - + ((_sonarFault > 0) << SENSOR_SONAR) - + ((_visionFault > 0) << SENSOR_VISION) - + ((_mocapFault > 0) << SENSOR_MOCAP); + ((_baroFault > fault_lvl_disable) << SENSOR_BARO) + + ((_gpsFault > fault_lvl_disable) << SENSOR_GPS) + + ((_lidarFault > fault_lvl_disable) << SENSOR_LIDAR) + + ((_flowFault > fault_lvl_disable) << SENSOR_FLOW) + + ((_sonarFault > fault_lvl_disable) << SENSOR_SONAR) + + ((_visionFault > fault_lvl_disable) << SENSOR_VISION) + + ((_mocapFault > fault_lvl_disable) << SENSOR_MOCAP); _pub_est_status.get().timeout_flags = (_baroInitialized << SENSOR_BARO) + (_gpsInitialized << SENSOR_GPS) @@ -890,9 +927,9 @@ void BlockLocalPositionEstimator::publishGlobalPos() map_projection_reproject(&_map_ref, _x(X_x), _x(X_y), &lat, &lon); float alt = -_x(X_z) + _altHome; - if (isfinite(lat) && isfinite(lon) && isfinite(alt) && - isfinite(_x(X_vx)) && isfinite(_x(X_vy)) && - isfinite(_x(X_vz))) { + if (PX4_ISFINITE(lat) && PX4_ISFINITE(lon) && PX4_ISFINITE(alt) && + PX4_ISFINITE(_x(X_vx)) && PX4_ISFINITE(_x(X_vy)) && + PX4_ISFINITE(_x(X_vz))) { _pub_gpos.get().timestamp = _timeStamp; _pub_gpos.get().time_utc_usec = _sub_gps.get().time_utc_usec; _pub_gpos.get().lat = lat; @@ -912,17 +949,6 @@ void BlockLocalPositionEstimator::publishGlobalPos() } } -//void BlockLocalPositionEstimator::publishFilteredFlow() -//{ -//// publish filtered flow -//if (isfinite(_pub_filtered_flow.get().sumx) && -//isfinite(_pub_filtered_flow.get().sumy) && -//isfinite(_pub_filtered_flow.get().vx) && -//isfinite(_pub_filtered_flow.get().vy)) { -//_pub_filtered_flow.update(); -//} -//} - void BlockLocalPositionEstimator::initP() { _P.setZero(); @@ -994,22 +1020,26 @@ void BlockLocalPositionEstimator::predict() // process noise power matrix Matrix Q; Q.setZero(); - Q(X_x, X_x) = _pn_p_noise_power.get(); - Q(X_y, X_y) = _pn_p_noise_power.get(); - Q(X_z, X_z) = _pn_p_noise_power.get(); - Q(X_vx, X_vx) = _pn_v_noise_power.get(); - Q(X_vy, X_vy) = _pn_v_noise_power.get(); - Q(X_vz, X_vz) = _pn_v_noise_power.get(); + float pn_p_sq = _pn_p_noise_density.get() * _pn_p_noise_density.get(); + float pn_v_sq = _pn_v_noise_density.get() * _pn_v_noise_density.get(); + Q(X_x, X_x) = pn_p_sq; + Q(X_y, X_y) = pn_p_sq; + Q(X_z, X_z) = pn_p_sq; + Q(X_vx, X_vx) = pn_v_sq; + Q(X_vy, X_vy) = pn_v_sq; + Q(X_vz, X_vz) = pn_v_sq; // technically, the noise is in the body frame, // but the components are all the same, so // ignoring for now - Q(X_bx, X_bx) = _pn_b_noise_power.get(); - Q(X_by, X_by) = _pn_b_noise_power.get(); - Q(X_bz, X_bz) = _pn_b_noise_power.get(); + float pn_b_sq = _pn_b_noise_density.get() * _pn_b_noise_density.get(); + Q(X_bx, X_bx) = pn_b_sq; + Q(X_by, X_by) = pn_b_sq; + Q(X_bz, X_bz) = pn_b_sq; // terrain random walk noise - Q(X_tz, X_tz) = _pn_t_noise_power.get(); + float pn_t_sq = _pn_t_noise_density.get() * _pn_t_noise_density.get(); + Q(X_tz, X_tz) = pn_t_sq; // continuous time kalman filter prediction Vector dx = (A * _x + B * _u) * getDt(); @@ -1032,13 +1062,6 @@ void BlockLocalPositionEstimator::predict() _x += dx; _P += (A * _P + _P * A.transpose() + B * R * B.transpose() + Q) * getDt(); - - // propagate delayed state - //if (_time_last_hist == 0 || _time_last_hist - _timeStamp > 1000) { - //_xDelay.update(_x); - //_PDelay.update(_P); - //_tDelay.update(Scalar(_timeStamp)); - //} } void BlockLocalPositionEstimator::correctFlow() @@ -1062,11 +1085,11 @@ void BlockLocalPositionEstimator::correctFlow() // calculate range to center of image for flow float d = 0; - if (_lidarInitialized && _lidarFault < FAULT_SEVERE) { + if (_lidarInitialized && _lidarFault < fault_lvl_disable) { d = _sub_lidar->get().current_distance + (_lidar_z_offset.get() - _flow_z_offset.get()); - } else if (_sonarInitialized && _sonarFault < FAULT_SEVERE) { + } else if (_sonarInitialized && _sonarFault < fault_lvl_disable) { d = _sub_sonar->get().current_distance + (_sonar_z_offset.get() - _flow_z_offset.get()); @@ -1127,13 +1150,6 @@ void BlockLocalPositionEstimator::correctFlow() _flowX += delta_n(0); _flowY += delta_n(1); - /* update filtered flow */ - //float dt_flow = _sub_flow.get().integration_timespan / 1.0e6; - //_pub_filtered_flow.get().sumx = delta_n(0); - //_pub_filtered_flow.get().sumy = delta_n(1); - //_pub_filtered_flow.get().vx = delta_n(0) / dt_flow; - //_pub_filtered_flow.get().vy = delta_n(1) / dt_flow; - // measurement Vector y; y(0) = _flowX; @@ -1162,8 +1178,7 @@ void BlockLocalPositionEstimator::correctFlow() warnx("[lpe] flow OK"); } - // kalman filter correction if no fault - if (_flowFault < FAULT_SEVERE) { + if (_flowFault < fault_lvl_disable) { Matrix K = _P * C.transpose() * S_I; _x += K * r; @@ -1187,8 +1202,6 @@ void BlockLocalPositionEstimator::correctSonar() float max_dist = _sub_sonar->get().max_distance - eps; if (d < min_dist) { - //mavlink_log_info(_mavlink_fd, "[lpe] sonar min dist"); - warnx("[lpe] sonar min dist"); // can't correct, so return return; @@ -1206,7 +1219,7 @@ void BlockLocalPositionEstimator::correctSonar() _time_last_sonar = _timeStamp; // do not use sonar if lidar is active - if (_lidarInitialized && _lidarFault < FAULT_SEVERE) { return; } + if (_lidarInitialized && _lidarFault < fault_lvl_disable) { return; } // calculate covariance float cov = _sub_sonar->get().covariance; @@ -1247,19 +1260,27 @@ void BlockLocalPositionEstimator::correctSonar() if (beta > BETA_TABLE[n_y_sonar]) { if (_sonarFault < FAULT_MINOR) { - mavlink_log_info(_mavlink_fd, "[lpe] sonar fault, beta %5.2f", double(beta)); - warnx("[lpe] sonar fault, beta %5.2f", double(beta)); + // avoid printing messages near ground + if (_x(X_tz) > 1.0f) { + mavlink_log_info(_mavlink_fd, "[lpe] sonar fault, beta %5.2f", double(beta)); + warnx("[lpe] sonar fault, beta %5.2f", double(beta)); + } + _sonarFault = FAULT_MINOR; } } else if (_sonarFault) { _sonarFault = FAULT_NONE; - mavlink_log_info(_mavlink_fd, "[lpe] sonar OK"); - warnx("[lpe] sonar OK"); + + // avoid printing messages near ground + if (_x(X_tz) > 1.0f) { + mavlink_log_info(_mavlink_fd, "[lpe] sonar OK"); + warnx("[lpe] sonar OK"); + } } // kalman filter correction if no fault - if (_sonarFault < FAULT_SEVERE) { + if (_sonarFault < fault_lvl_disable) { Matrix K = _P * C.transpose() * S_I; Vector dx = K * r; @@ -1303,14 +1324,13 @@ void BlockLocalPositionEstimator::correctBaro() if (beta > BETA_TABLE[n_y_baro]) { if (_baroFault < FAULT_MINOR) { - mavlink_log_info(_mavlink_fd, "[lpe] baro fault, beta %5.2f", double(beta)); - warnx("[lpe] baro fault, beta %5.2f", double(beta)); + mavlink_log_info(_mavlink_fd, "[lpe] baro fault, r %5.2f m, beta %5.2f", + double(r(0)), double(beta)); + warnx("[lpe] baro fault, r %5.2f m, beta %5.2f", + double(r(0)), double(beta)); _baroFault = FAULT_MINOR; } - // lower baro trust - S_I = inv((C * _P * C.transpose()) + R * 10); - } else if (_baroFault) { _baroFault = FAULT_NONE; mavlink_log_info(_mavlink_fd, "[lpe] baro OK"); @@ -1318,7 +1338,7 @@ void BlockLocalPositionEstimator::correctBaro() } // kalman filter correction if no fault - if (_baroFault < FAULT_SEVERE) { + if (_baroFault < fault_lvl_disable) { Matrix K = _P * C.transpose() * S_I; Vector dx = K * r; @@ -1343,7 +1363,11 @@ void BlockLocalPositionEstimator::correctLidar() float max_dist = _sub_lidar->get().max_distance - eps; // if out of range, this is an error - if (d < min_dist || d > max_dist) { + if (d < min_dist) { + // can't correct, so return + return; + + } else if (d > max_dist) { if (_lidarFault < FAULT_SEVERE) { mavlink_log_info(_mavlink_fd, "[lpe] lidar out of range"); warnx("[lpe] lidar out of range"); @@ -1389,19 +1413,31 @@ void BlockLocalPositionEstimator::correctLidar() if (beta > BETA_TABLE[n_y_lidar]) { if (_lidarFault < FAULT_MINOR) { - mavlink_log_info(_mavlink_fd, "[lpe] lidar fault, beta %5.2f", double(beta)); - warnx("[lpe] lidar fault, beta %5.2f", double(beta)); + // only print message if above 1 meter, avoids + // message clutter when on ground + if (_x(X_tz) > 1.0f) { + mavlink_log_info(_mavlink_fd, "[lpe] lidar fault, r %5.2f m, beta %5.2f", + double(r(0)), double(beta)); + warnx("[lpe] lidar fault, r %5.2f m, beta %5.2f", + double(r(0)), double(beta)); + } + _lidarFault = FAULT_MINOR; } } else if (_lidarFault) { // disable fault if ok _lidarFault = FAULT_NONE; - mavlink_log_info(_mavlink_fd, "[lpe] lidar OK"); - warnx("[lpe] lidar OK"); + + // only print message if above 1 meter, avoids + // message clutter when on ground + if (_x(X_tz) > 1.0f) { + mavlink_log_info(_mavlink_fd, "[lpe] lidar OK"); + warnx("[lpe] lidar OK"); + } } // kalman filter correction if no fault - if (_lidarFault < FAULT_SEVERE) { + if (_lidarFault < fault_lvl_disable) { Matrix K = _P * C.transpose() * S_I; Vector dx = K * r; @@ -1424,7 +1460,7 @@ void BlockLocalPositionEstimator::correctGps() float eph = _sub_gps.get().eph; if (nSat < 6 || eph > _gps_eph_max.get()) { - if (!_gpsFault) { + if (_gpsFault < FAULT_SEVERE) { mavlink_log_info(_mavlink_fd, "[lpe] gps fault nSat: %d eph: %5.2f", nSat, double(eph)); warnx("[lpe] gps fault nSat: %d eph: %5.2f", nSat, double(eph)); _gpsFault = FAULT_SEVERE; @@ -1488,29 +1524,36 @@ void BlockLocalPositionEstimator::correctGps() R(5, 5) = var_vz; // get delayed x and P - //uint64_t t_delay = 0; - //int i = 0; + float t_delay = 0; + int i = 0; - //for (i = 0; i < HIST_LEN; i++) { - //t_delay += _tDelay.get(i)(0, 0); + for (i = 1; i < HIST_LEN; i++) { + t_delay = 1.0e-6f * (_timeStamp - _tDelay.get(i)(0, 0)); - //if (t_delay > 2000) { - //break; - //} - //} + if (t_delay > _gps_delay.get()) { + break; + } + } - //Vector x0 = _xDelay.get(i); - //Matrix P0 = _PDelay.get(i); + // if you are 3 steps past the delay you wanted, this + // data is probably too old to use + if (t_delay > GPS_DELAY_MAX) { + warnx("[lpe] gps delayed data too old: %8.4f", double(t_delay)); + mavlink_log_info(_mavlink_fd, "[lpe] gps delayed data too old: %8.4f", double(t_delay)); + return; + } + + Vector x0 = _xDelay.get(i); // residual - Vector r = y - C * _x; + Vector r = y - C * x0; Matrix S_I = inv(C * _P * C.transpose() + R); // fault detection float beta = (r.transpose() * (S_I * r))(0, 0); if (beta > BETA_TABLE[n_y_gps]) { - if (!_gpsFault) { + if (_gpsFault < FAULT_MINOR) { mavlink_log_info(_mavlink_fd, "[lpe] gps fault, beta: %5.2f", double(beta)); warnx("[lpe] gps fault, beta: %5.2f", double(beta)); mavlink_log_info(_mavlink_fd, "[lpe] r: %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f", @@ -1535,7 +1578,7 @@ void BlockLocalPositionEstimator::correctGps() } // kalman filter correction if no hard fault - if (_gpsFault < FAULT_SEVERE) { + if (_gpsFault < fault_lvl_disable) { Matrix K = _P * C.transpose() * S_I; _x += K * r; _P -= K * C * _P; @@ -1574,15 +1617,12 @@ void BlockLocalPositionEstimator::correctVision() float beta = (r.transpose() * (S_I * r))(0, 0); if (beta > BETA_TABLE[n_y_vision]) { - if (!_visionFault) { + if (_visionFault < FAULT_MINOR) { mavlink_log_info(_mavlink_fd, "[lpe] vision position fault, beta %5.2f", double(beta)); warnx("[lpe] vision position fault, beta %5.2f", double(beta)); _visionFault = FAULT_MINOR; } - // trust less - S_I = inv((C * _P * C.transpose()) + R * 10); - } else if (_visionFault) { _visionFault = FAULT_NONE; mavlink_log_info(_mavlink_fd, "[lpe] vision position OK"); @@ -1590,7 +1630,7 @@ void BlockLocalPositionEstimator::correctVision() } // kalman filter correction if no fault - if (_visionFault < FAULT_SEVERE) { + if (_visionFault < fault_lvl_disable) { Matrix K = _P * C.transpose() * S_I; _x += K * r; _P -= K * C * _P; @@ -1631,15 +1671,12 @@ void BlockLocalPositionEstimator::correctMocap() float beta = (r.transpose() * (S_I * r))(0, 0); if (beta > BETA_TABLE[n_y_mocap]) { - if (!_mocapFault) { + if (_mocapFault < FAULT_MINOR) { mavlink_log_info(_mavlink_fd, "[lpe] mocap fault, beta %5.2f", double(beta)); warnx("[lpe] mocap fault, beta %5.2f", double(beta)); _mocapFault = FAULT_MINOR; } - // trust less - S_I = inv((C * _P * C.transpose()) + R * 10); - } else if (_mocapFault) { _mocapFault = FAULT_NONE; mavlink_log_info(_mavlink_fd, "[lpe] mocap OK"); @@ -1647,7 +1684,7 @@ void BlockLocalPositionEstimator::correctMocap() } // kalman filter correction if no fault - if (_mocapFault < FAULT_SEVERE) { + if (_mocapFault < fault_lvl_disable) { Matrix K = _P * C.transpose() * S_I; _x += K * r; _P -= K * C * _P; diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp index a77a358363..b94757cb20 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp @@ -1,7 +1,7 @@ #pragma once #include -#include +#include #include #include #include @@ -35,14 +35,15 @@ using namespace Eigen; #include #include #include -//#include #include #define CBRK_NO_VISION_KEY 328754 using namespace control; -//static const size_t HIST_LEN = 2; // each step is 100 ms, gps has 200 ms delay +static const float GPS_DELAY_MAX = 0.5; // seconds +static const float HIST_STEP = 0.05; // 20 hz +static const size_t HIST_LEN = (GPS_DELAY_MAX / HIST_STEP); enum fault_t { FAULT_NONE = 0, @@ -165,7 +166,6 @@ private: // publications void publishLocalPos(); void publishGlobalPos(); - //void publishFilteredFlow(); void publishEstimatorStatus(); // attributes @@ -192,7 +192,6 @@ private: // publications uORB::Publication _pub_lpos; uORB::Publication _pub_gpos; - //uORB::Publication _pub_filtered_flow; uORB::Publication _pub_est_status; // map projection @@ -217,6 +216,7 @@ private: BlockParamFloat _baro_stddev; // gps parameters + BlockParamFloat _gps_delay; BlockParamFloat _gps_xy_stddev; BlockParamFloat _gps_z_stddev; BlockParamFloat _gps_vxy_stddev; @@ -239,10 +239,10 @@ private: BlockParamInt _flow_min_q; // process noise - BlockParamFloat _pn_p_noise_power; - BlockParamFloat _pn_v_noise_power; - BlockParamFloat _pn_b_noise_power; - BlockParamFloat _pn_t_noise_power; + BlockParamFloat _pn_p_noise_density; + BlockParamFloat _pn_v_noise_density; + BlockParamFloat _pn_b_noise_density; + BlockParamFloat _pn_t_noise_density; // flow gyro filter BlockHighPass _flow_gyro_x_high_pass; @@ -258,14 +258,13 @@ private: BlockStats _gpsStats; // delay blocks - //BlockDelay _xDelay; - //BlockDelay _PDelay; - //BlockDelay _tDelay; + BlockDelay _xDelay; + BlockDelay _tDelay; // misc px4_pollfd_struct_t _polls[3]; uint64_t _timeStamp; - //uint64_t _time_last_hist; + uint64_t _time_last_hist; uint64_t _time_last_xy; uint64_t _time_last_z; uint64_t _time_last_tz; @@ -308,6 +307,7 @@ private: bool _canEstimateXY; bool _canEstimateZ; bool _canEstimateT; + bool _canEstimateGlobal; bool _xyTimeout; bool _zTimeout; bool _tzTimeout; diff --git a/src/modules/local_position_estimator/CMakeLists.txt b/src/modules/local_position_estimator/CMakeLists.txt index dd46f1ca59..1d7152a89c 100644 --- a/src/modules/local_position_estimator/CMakeLists.txt +++ b/src/modules/local_position_estimator/CMakeLists.txt @@ -31,7 +31,7 @@ # ############################################################################ if(${OS} STREQUAL "nuttx") - list(APPEND MODULE_CFLAGS -Wframe-larger-than=7000) + list(APPEND MODULE_CFLAGS -Wframe-larger-than=10000) elseif(${OS} STREQUAL "posix") list(APPEND MODULE_CFLAGS -Wno-error) endif() diff --git a/src/modules/local_position_estimator/fault_table.py b/src/modules/local_position_estimator/fault_table.py new file mode 100644 index 0000000000..8a9eccf84e --- /dev/null +++ b/src/modules/local_position_estimator/fault_table.py @@ -0,0 +1,13 @@ +#!/bin/bash +from __future__ import print_function +import pylab as pl +import scipy.optimize +from scipy.stats import chi2 + +for fa_rate in 1.0/pl.array([1e1, 1e2, 1e4, 1e6, 1e9]): + print(fa_rate) + for df in range(1,7): + f_eq = lambda x: ((1- fa_rate) - chi2.cdf(x, df))**2 + res = scipy.optimize.minimize(f_eq, df) + assert res['success'] + print('\t', res.x[0]) diff --git a/src/modules/local_position_estimator/local_position_estimator_main.cpp b/src/modules/local_position_estimator/local_position_estimator_main.cpp index 87e4f0497e..6928dfeb1f 100644 --- a/src/modules/local_position_estimator/local_position_estimator_main.cpp +++ b/src/modules/local_position_estimator/local_position_estimator_main.cpp @@ -112,7 +112,7 @@ int local_position_estimator_main(int argc, char *argv[]) deamon_task = px4_task_spawn_cmd("lp_estimator", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 10240, + 11264, local_position_estimator_thread_main, (argv && argc > 2) ? (char *const *) &argv[2] : (char *const *) NULL); return 0; diff --git a/src/modules/local_position_estimator/params.c b/src/modules/local_position_estimator/params.c index 0a308f324c..4b2ff0ebd6 100644 --- a/src/modules/local_position_estimator/params.c +++ b/src/modules/local_position_estimator/params.c @@ -105,7 +105,7 @@ PARAM_DEFINE_FLOAT(LPE_LDR_OFF_Z, 0.00f); * should be 0.0464 * * @group Local Position Estimator - * @unit m/s/s + * @unit m/s^2 * @min 0.00001 * @max 2 * @decimal 4 @@ -118,7 +118,7 @@ PARAM_DEFINE_FLOAT(LPE_ACC_XY, 0.0454f); * (see Accel x comments) * * @group Local Position Estimator - * @unit m/s/s + * @unit m/s^2 * @min 0.00001 * @max 2 * @decimal 4 @@ -136,6 +136,19 @@ PARAM_DEFINE_FLOAT(LPE_ACC_Z, 0.0454f); */ PARAM_DEFINE_FLOAT(LPE_BAR_Z, 1.0f); + +/** + * GPS delay compensaton + * + * @group Local Position Estimator + * @unit sec + * @min 0 + * @max 0.4 + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(LPE_GPS_DELAY, 0.25f); + + /** * GPS xy standard deviation. * @@ -153,10 +166,10 @@ PARAM_DEFINE_FLOAT(LPE_GPS_XY, 2.0f); * @group Local Position Estimator * @unit m * @min 0.01 - * @max 20 + * @max 200 * @decimal 2 */ -PARAM_DEFINE_FLOAT(LPE_GPS_Z, 10.0f); +PARAM_DEFINE_FLOAT(LPE_GPS_Z, 100.0f); /** * GPS xy velocity standard deviation. @@ -237,43 +250,43 @@ PARAM_DEFINE_INT32(LPE_NO_VISION, 0); PARAM_DEFINE_FLOAT(LPE_VIC_P, 0.05f); /** - * Position propagation process noise power (variance*sampling rate). + * Position propagation noise density * * @group Local Position Estimator - * @unit (m/s/s)-s + * @unit m/s/sqrt(Hz) * @min 0 * @max 1 * @decimal 8 */ -PARAM_DEFINE_FLOAT(LPE_PN_P, 0.0f); +PARAM_DEFINE_FLOAT(LPE_PN_P, 0.1f); /** - * Velocity propagation process noise power (variance*sampling rate). + * Velocity propagation noise density * * @group Local Position Estimator - * @unit (m/s)-s - * @min 0 - * @max 5 - * @decimal 8 - */ -PARAM_DEFINE_FLOAT(LPE_PN_V, 0.0f); - -/** - * Accel bias propagation process noise power (variance*sampling rate). - * - * @group Local Position Estimator - * @unit (m/s)-s + * @unit (m/s)/s/sqrt(Hz) * @min 0 * @max 1 * @decimal 8 */ -PARAM_DEFINE_FLOAT(LPE_PN_B, 1e-8f); +PARAM_DEFINE_FLOAT(LPE_PN_V, 0.1f); /** - * Terrain random walk noise power (variance*sampling rate). + * Accel bias propagation noise density * * @group Local Position Estimator - * @unit m-s + * @unit (m/s^2)/s/sqrt(Hz) + * @min 0 + * @max 1 + * @decimal 8 + */ +PARAM_DEFINE_FLOAT(LPE_PN_B, 1e-3f); + +/** + * Terrain random walk noise density + * + * @group Local Position Estimator + * @unit m/s/sqrt(Hz) * @min 0 * @max 1 * @decimal 3