yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #4009 from jgoppert/lpe-gps-delay

Browse Source 
Enabled gps delay compensation for lpe based on x hist.
This commit is contained in:
James Goppert
2016-03-18 01:42:47 -04:00
parent ce7dad81c2 bcba1fb27e
commit 21e5c0a990
6 changed files with 255 additions and 192 deletions
Download Patch File Download Diff File Expand all files Collapse all files

331
src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
View File

@@ -4,6 +4,8 @@
#include <systemlib/err.h> #include <systemlib/err.h>
#include <matrix/math.hpp> #include <matrix/math.hpp>
// required number of samples for sensor
// to initialize
static const int REQ_BARO_INIT_COUNT = 100; static const int REQ_BARO_INIT_COUNT = 100;
static const int REQ_FLOW_INIT_COUNT = 20; static const int REQ_FLOW_INIT_COUNT = 20;
static const int REQ_GPS_INIT_COUNT = 10; 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_VISION_INIT_COUNT = 20;
static const int REQ_MOCAP_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 BARO_TIMEOUT = 1000000; // 1.0 s
static const uint32_t FLOW_TIMEOUT = 500000; // 0.5 s static const uint32_t FLOW_TIMEOUT = 500000; // 0.5 s
static const uint32_t GPS_TIMEOUT = 1000000; // 1.0 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 MOCAP_TIMEOUT = 200000; // 0.2 s
static const uint32_t EST_SRC_TIMEOUT = 500000; // 0.5 s static const uint32_t EST_SRC_TIMEOUT = 500000; // 0.5 s
// for fault detection // change this to set when
// chi squared distribution, false alarm probability 0.005 // the system will abort correcting a measurement
// http://sites.stat.psu.edu/~mga/401/tables/Chi-square-table.pdf // given a fault has been detected
static const float BETA_TABLE[7] = {0, 7.879, 10.597, static fault_t fault_lvl_disable = FAULT_SEVERE;
12.838, 14.860, 16.750, 18.548
};
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() : BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
// this block has no parent, and has name LPE // this block has no parent, and has name LPE
@@ -57,7 +69,6 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
// publications // publications
_pub_lpos(ORB_ID(vehicle_local_position), -1, &getPublications()), _pub_lpos(ORB_ID(vehicle_local_position), -1, &getPublications()),
_pub_gpos(ORB_ID(vehicle_global_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()), _pub_est_status(ORB_ID(estimator_status), -1, &getPublications()),
// map projection // map projection
@@ -72,6 +83,7 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
_accel_xy_stddev(this, "ACC_XY"), _accel_xy_stddev(this, "ACC_XY"),
_accel_z_stddev(this, "ACC_Z"), _accel_z_stddev(this, "ACC_Z"),
_baro_stddev(this, "BAR_Z"), _baro_stddev(this, "BAR_Z"),
_gps_delay(this, "GPS_DELAY"),
_gps_xy_stddev(this, "GPS_XY"), _gps_xy_stddev(this, "GPS_XY"),
_gps_z_stddev(this, "GPS_Z"), _gps_z_stddev(this, "GPS_Z"),
_gps_vxy_stddev(this, "GPS_VXY"), _gps_vxy_stddev(this, "GPS_VXY"),
@@ -86,10 +98,10 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
//_flow_board_x_offs(NULL, "SENS_FLW_XOFF"), //_flow_board_x_offs(NULL, "SENS_FLW_XOFF"),
//_flow_board_y_offs(NULL, "SENS_FLW_YOFF"), //_flow_board_y_offs(NULL, "SENS_FLW_YOFF"),
_flow_min_q(this, "FLW_QMIN"), _flow_min_q(this, "FLW_QMIN"),
_pn_p_noise_power(this, "PN_P"), _pn_p_noise_density(this, "PN_P"),
_pn_v_noise_power(this, "PN_V"), _pn_v_noise_density(this, "PN_V"),
_pn_b_noise_power(this, "PN_B"), _pn_b_noise_density(this, "PN_B"),
_pn_t_noise_power(this, "PN_T"), _pn_t_noise_density(this, "PN_T"),
// flow gyro // flow gyro
_flow_gyro_x_high_pass(this, "FGYRO_HP"), _flow_gyro_x_high_pass(this, "FGYRO_HP"),
@@ -105,14 +117,13 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
_gpsStats(this, ""), _gpsStats(this, ""),
// stats // stats
//_xDelay(this, ""), _xDelay(this, ""),
//_PDelay(this, ""), _tDelay(this, ""),
//_tDelay(this, ""),
// misc // misc
_polls(), _polls(),
_timeStamp(hrt_absolute_time()), _timeStamp(hrt_absolute_time()),
//_time_last_hist(0), _time_last_hist(0),
_time_last_xy(0), _time_last_xy(0),
_time_last_z(0), _time_last_z(0),
_time_last_tz(0), _time_last_tz(0),
@@ -157,6 +168,7 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
_canEstimateXY(false), _canEstimateXY(false),
_canEstimateZ(false), _canEstimateZ(false),
_canEstimateT(false), _canEstimateT(false),
_canEstimateGlobal(true),
_xyTimeout(true), _xyTimeout(true),
_zTimeout(true), _zTimeout(true),
_tzTimeout(true), _tzTimeout(true),
@@ -282,15 +294,15 @@ void BlockLocalPositionEstimator::update()
// determine if we should start estimating // determine if we should start estimating
_canEstimateZ = _canEstimateZ =
(_baroInitialized && _baroFault < FAULT_SEVERE); (_baroInitialized && _baroFault < fault_lvl_disable);
_canEstimateXY = _canEstimateXY =
(_gpsInitialized && _gpsFault < FAULT_SEVERE) || (_gpsInitialized && _gpsFault < fault_lvl_disable) ||
(_flowInitialized && _flowFault < FAULT_SEVERE) || (_flowInitialized && _flowFault < fault_lvl_disable) ||
(_visionInitialized && _visionFault < FAULT_SEVERE) || (_visionInitialized && _visionFault < fault_lvl_disable) ||
(_mocapInitialized && _mocapFault < FAULT_SEVERE); (_mocapInitialized && _mocapFault < fault_lvl_disable);
_canEstimateT = _canEstimateT =
(_lidarInitialized && _lidarFault < FAULT_SEVERE) || (_lidarInitialized && _lidarFault < fault_lvl_disable) ||
(_sonarInitialized && _sonarFault < FAULT_SEVERE); (_sonarInitialized && _sonarFault < fault_lvl_disable);
if (_canEstimateXY) { if (_canEstimateXY) {
_time_last_xy = _timeStamp; _time_last_xy = _timeStamp;
@@ -319,7 +331,7 @@ void BlockLocalPositionEstimator::update()
// should we do a reinit // should we do a reinit
// of sensors here? // of sensors here?
// don't want it to take too long // don't want it to take too long
if (!isfinite(_x(i))) { if (!PX4_ISFINITE(_x(i))) {
reinit_x = true; reinit_x = true;
break; break;
} }
@@ -339,7 +351,7 @@ void BlockLocalPositionEstimator::update()
for (int i = 0; i < n_x; i++) { for (int i = 0; i < n_x; i++) {
for (int j = 0; j < n_x; j++) { for (int j = 0; j < n_x; j++) {
if (!isfinite(_P(i, j))) { if (!PX4_ISFINITE(_P(i, j))) {
reinit_P = true; reinit_P = true;
break; break;
} }
@@ -430,7 +442,10 @@ void BlockLocalPositionEstimator::update()
// update all publications if possible // update all publications if possible
publishLocalPos(); publishLocalPos();
publishEstimatorStatus(); publishEstimatorStatus();
if (_canEstimateGlobal) {
publishGlobalPos(); publishGlobalPos();
}
} else if (!_zTimeout && _altHomeInitialized) { } else if (!_zTimeout && _altHomeInitialized) {
// publish only Z estimate // publish only Z estimate
@@ -438,6 +453,17 @@ void BlockLocalPositionEstimator::update()
publishEstimatorStatus(); 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<uint64_t>(_timeStamp));
_xDelay.update(_x);
_time_last_hist = _timeStamp;
}
} }
void BlockLocalPositionEstimator::checkTimeouts() void BlockLocalPositionEstimator::checkTimeouts()
@@ -449,7 +475,9 @@ void BlockLocalPositionEstimator::checkTimeouts()
warnx("[lpe] xy timeout "); warnx("[lpe] xy timeout ");
} }
} else { } else if (_xyTimeout) {
mavlink_log_info(_mavlink_fd, "[lpe] xy resume ");
warnx("[lpe] xy resume ");
_xyTimeout = false; _xyTimeout = false;
} }
@@ -460,7 +488,9 @@ void BlockLocalPositionEstimator::checkTimeouts()
warnx("[lpe] z timeout "); warnx("[lpe] z timeout ");
} }
} else { } else if (_zTimeout) {
mavlink_log_info(_mavlink_fd, "[lpe] z resume ");
warnx("[lpe] z resume ");
_zTimeout = false; _zTimeout = false;
} }
@@ -471,7 +501,9 @@ void BlockLocalPositionEstimator::checkTimeouts()
warnx("[lpe] tz timeout "); warnx("[lpe] tz timeout ");
} }
} else { } else if (_tzTimeout) {
mavlink_log_info(_mavlink_fd, "[lpe] tz resume ");
warnx("[lpe] tz resume ");
_tzTimeout = false; _tzTimeout = false;
} }
@@ -542,9 +574,12 @@ void BlockLocalPositionEstimator::updateHome()
// like gps and baro to be off, need to allow it // like gps and baro to be off, need to allow it
// to reset by resetting covariance // to reset by resetting covariance
initP(); initP();
mavlink_log_info(_mavlink_fd, "[lpe] home "
mavlink_log_info(_mavlink_fd, "[lpe] home: lat %5.0f, lon %5.0f, alt %5.0f", lat, lon, double(alt)); "lat %6.2f lon %6.2f alt %5.1f m",
warnx("[lpe] home: lat %5.0f, lon %5.0f, alt %5.0f", lat, lon, double(alt)); 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); map_projection_init(&_map_ref, lat, lon);
float delta_alt = alt - _altHome; float delta_alt = alt - _altHome;
_altHomeInitialized = true; _altHomeInitialized = true;
@@ -564,10 +599,10 @@ void BlockLocalPositionEstimator::initBaro()
if (_baroStats.getCount() > REQ_BARO_INIT_COUNT) { if (_baroStats.getCount() > REQ_BARO_INIT_COUNT) {
_baroAltHome = _baroStats.getMean()(0); _baroAltHome = _baroStats.getMean()(0);
mavlink_log_info(_mavlink_fd, 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)_baroStats.getMean()(0),
(int)(100 * _baroStats.getStdDev()(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)_baroStats.getMean()(0),
(int)(100 * _baroStats.getStdDev()(0))); (int)(100 * _baroStats.getStdDev()(0)));
_baroInitialized = true; _baroInitialized = true;
@@ -608,16 +643,18 @@ void BlockLocalPositionEstimator::initGps()
_gpsAltHome = _gpsStats.getMean()(2); _gpsAltHome = _gpsStats.getMean()(2);
map_projection_init(&_map_ref, map_projection_init(&_map_ref,
_gpsLatHome, _gpsLonHome); _gpsLatHome, _gpsLonHome);
mavlink_log_info(_mavlink_fd, "[lpe] gps init: " mavlink_log_info(_mavlink_fd, "[lpe] gps init "
"lat %d, lon %d, alt %d m", "lat %6.2f lon %6.2f alt %5.1f m",
int(_gpsLatHome), _gpsLatHome,
int(_gpsLonHome), _gpsLonHome,
int(_gpsAltHome)); double(_gpsAltHome));
warnx("[lpe] gps init: lat %d, lon %d, alt %d m", warnx("[lpe] gps init "
int(_gpsLatHome), "lat %6.2f lon %6.2f alt %5.1f m",
int(_gpsLonHome), _gpsLatHome,
int(_gpsAltHome)); _gpsLonHome,
double(_gpsAltHome));
_gpsInitialized = true; _gpsInitialized = true;
_canEstimateGlobal = true;
_gpsStats.reset(); _gpsStats.reset();
if (!_altHomeInitialized) { if (!_altHomeInitialized) {
@@ -655,11 +692,11 @@ void BlockLocalPositionEstimator::initLidar()
// not, might want to hard code this to zero // not, might want to hard code this to zero
mavlink_log_info(_mavlink_fd, "[lpe] lidar init: " 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.getMean()(0)),
int(100 * _lidarStats.getStdDev()(0))); int(100 * _lidarStats.getStdDev()(0)));
warnx("[lpe] lidar init: " warnx("[lpe] lidar init: "
"mean %d cm, stddev %d cm", "mean %d cm std %d cm",
int(100 * _lidarStats.getMean()(0)), int(100 * _lidarStats.getMean()(0)),
int(100 * _lidarStats.getStdDev()(0))); int(100 * _lidarStats.getStdDev()(0)));
_lidarInitialized = true; _lidarInitialized = true;
@@ -694,12 +731,12 @@ void BlockLocalPositionEstimator::initSonar()
} }
// not, might want to hard code this to zero // not, might want to hard code this to zero
mavlink_log_info(_mavlink_fd, "[lpe] sonar init: " mavlink_log_info(_mavlink_fd, "[lpe] sonar init "
"mean %d cm, stddev %d cm", "mean %d cm std %d cm",
int(100 * _sonarStats.getMean()(0)), int(100 * _sonarStats.getMean()(0)),
int(100 * _sonarStats.getStdDev()(0))); int(100 * _sonarStats.getStdDev()(0)));
warnx("[lpe] sonar init: " warnx("[lpe] sonar init "
"mean %d cm, stddev %d cm", "mean %d cm std %d cm",
int(100 * _sonarStats.getMean()(0)), int(100 * _sonarStats.getMean()(0)),
int(100 * _sonarStats.getStdDev()(0))); int(100 * _sonarStats.getStdDev()(0)));
_sonarInitialized = true; _sonarInitialized = true;
@@ -722,11 +759,11 @@ void BlockLocalPositionEstimator::initFlow()
if (_flowQStats.getCount() > REQ_FLOW_INIT_COUNT) { if (_flowQStats.getCount() > REQ_FLOW_INIT_COUNT) {
mavlink_log_info(_mavlink_fd, "[lpe] flow init: " mavlink_log_info(_mavlink_fd, "[lpe] flow init: "
"quality %d stddev %d", "quality %d std %d",
int(_flowQStats.getMean()(0)), int(_flowQStats.getMean()(0)),
int(_flowQStats.getStdDev()(0))); int(_flowQStats.getStdDev()(0)));
warnx("[lpe] flow init: " warnx("[lpe] flow init: "
"quality %d stddev %d", "quality %d std %d",
int(_flowQStats.getMean()(0)), int(_flowQStats.getMean()(0)),
int(_flowQStats.getStdDev()(0))); int(_flowQStats.getStdDev()(0)));
_flowInitialized = true; _flowInitialized = true;
@@ -749,7 +786,7 @@ void BlockLocalPositionEstimator::initVision()
if (_visionStats.getCount() > REQ_VISION_INIT_COUNT) { if (_visionStats.getCount() > REQ_VISION_INIT_COUNT) {
_visionHome = _visionStats.getMean(); _visionHome = _visionStats.getMean();
mavlink_log_info(_mavlink_fd, "[lpe] vision position init: " 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()(0)),
double(_visionStats.getMean()(1)), double(_visionStats.getMean()(1)),
double(_visionStats.getMean()(2)), double(_visionStats.getMean()(2)),
@@ -757,7 +794,7 @@ void BlockLocalPositionEstimator::initVision()
double(_visionStats.getStdDev()(1)), double(_visionStats.getStdDev()(1)),
double(_visionStats.getStdDev()(2))); double(_visionStats.getStdDev()(2)));
warnx("[lpe] vision position init: " 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()(0)),
double(_visionStats.getMean()(1)), double(_visionStats.getMean()(1)),
double(_visionStats.getMean()(2)), double(_visionStats.getMean()(2)),
@@ -789,7 +826,7 @@ void BlockLocalPositionEstimator::initMocap()
if (_mocapStats.getCount() > REQ_MOCAP_INIT_COUNT) { if (_mocapStats.getCount() > REQ_MOCAP_INIT_COUNT) {
_mocapHome = _mocapStats.getMean(); _mocapHome = _mocapStats.getMean();
mavlink_log_info(_mavlink_fd, "[lpe] mocap position init: " 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()(0)),
double(_mocapStats.getMean()(1)), double(_mocapStats.getMean()(1)),
double(_mocapStats.getMean()(2)), double(_mocapStats.getMean()(2)),
@@ -797,7 +834,7 @@ void BlockLocalPositionEstimator::initMocap()
double(_mocapStats.getStdDev()(1)), double(_mocapStats.getStdDev()(1)),
double(_mocapStats.getStdDev()(2))); double(_mocapStats.getStdDev()(2)));
warnx("[lpe] mocap position init: " 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()(0)),
double(_mocapStats.getMean()(1)), double(_mocapStats.getMean()(1)),
double(_mocapStats.getMean()(2)), double(_mocapStats.getMean()(2)),
@@ -817,9 +854,9 @@ void BlockLocalPositionEstimator::initMocap()
void BlockLocalPositionEstimator::publishLocalPos() void BlockLocalPositionEstimator::publishLocalPos()
{ {
// publish local position // publish local position
if (isfinite(_x(X_x)) && isfinite(_x(X_y)) && isfinite(_x(X_z)) && if (PX4_ISFINITE(_x(X_x)) && PX4_ISFINITE(_x(X_y)) && PX4_ISFINITE(_x(X_z)) &&
isfinite(_x(X_vx)) && isfinite(_x(X_vy)) PX4_ISFINITE(_x(X_vx)) && PX4_ISFINITE(_x(X_vy))
&& isfinite(_x(X_vz))) { && PX4_ISFINITE(_x(X_vz))) {
_pub_lpos.get().timestamp = _timeStamp; _pub_lpos.get().timestamp = _timeStamp;
_pub_lpos.get().xy_valid = _canEstimateXY; _pub_lpos.get().xy_valid = _canEstimateXY;
_pub_lpos.get().z_valid = _canEstimateZ; _pub_lpos.get().z_valid = _canEstimateZ;
@@ -850,9 +887,9 @@ void BlockLocalPositionEstimator::publishLocalPos()
void BlockLocalPositionEstimator::publishEstimatorStatus() void BlockLocalPositionEstimator::publishEstimatorStatus()
{ {
if (isfinite(_x(X_x)) && isfinite(_x(X_y)) && isfinite(_x(X_z)) && if (PX4_ISFINITE(_x(X_x)) && PX4_ISFINITE(_x(X_y)) && PX4_ISFINITE(_x(X_z)) &&
isfinite(_x(X_vx)) && isfinite(_x(X_vy)) PX4_ISFINITE(_x(X_vx)) && PX4_ISFINITE(_x(X_vy))
&& isfinite(_x(X_vz))) { && PX4_ISFINITE(_x(X_vz))) {
_pub_est_status.get().timestamp = _timeStamp; _pub_est_status.get().timestamp = _timeStamp;
for (int i = 0; i < n_x; i++) { 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().n_states = n_x;
_pub_est_status.get().nan_flags = 0; _pub_est_status.get().nan_flags = 0;
_pub_est_status.get().health_flags = _pub_est_status.get().health_flags =
((_baroFault > 0) << SENSOR_BARO) ((_baroFault > fault_lvl_disable) << SENSOR_BARO)
+ ((_gpsFault > 0) << SENSOR_GPS) + ((_gpsFault > fault_lvl_disable) << SENSOR_GPS)
+ ((_lidarFault > 0) << SENSOR_LIDAR) + ((_lidarFault > fault_lvl_disable) << SENSOR_LIDAR)
+ ((_flowFault > 0) << SENSOR_FLOW) + ((_flowFault > fault_lvl_disable) << SENSOR_FLOW)
+ ((_sonarFault > 0) << SENSOR_SONAR) + ((_sonarFault > fault_lvl_disable) << SENSOR_SONAR)
+ ((_visionFault > 0) << SENSOR_VISION) + ((_visionFault > fault_lvl_disable) << SENSOR_VISION)
+ ((_mocapFault > 0) << SENSOR_MOCAP); + ((_mocapFault > fault_lvl_disable) << SENSOR_MOCAP);
_pub_est_status.get().timeout_flags = _pub_est_status.get().timeout_flags =
(_baroInitialized << SENSOR_BARO) (_baroInitialized << SENSOR_BARO)
+ (_gpsInitialized << SENSOR_GPS) + (_gpsInitialized << SENSOR_GPS)
@@ -890,9 +927,9 @@ void BlockLocalPositionEstimator::publishGlobalPos()
map_projection_reproject(&_map_ref, _x(X_x), _x(X_y), &lat, &lon); map_projection_reproject(&_map_ref, _x(X_x), _x(X_y), &lat, &lon);
float alt = -_x(X_z) + _altHome; float alt = -_x(X_z) + _altHome;
if (isfinite(lat) && isfinite(lon) && isfinite(alt) && if (PX4_ISFINITE(lat) && PX4_ISFINITE(lon) && PX4_ISFINITE(alt) &&
isfinite(_x(X_vx)) && isfinite(_x(X_vy)) && PX4_ISFINITE(_x(X_vx)) && PX4_ISFINITE(_x(X_vy)) &&
isfinite(_x(X_vz))) { PX4_ISFINITE(_x(X_vz))) {
_pub_gpos.get().timestamp = _timeStamp; _pub_gpos.get().timestamp = _timeStamp;
_pub_gpos.get().time_utc_usec = _sub_gps.get().time_utc_usec; _pub_gpos.get().time_utc_usec = _sub_gps.get().time_utc_usec;
_pub_gpos.get().lat = lat; _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() void BlockLocalPositionEstimator::initP()
{ {
_P.setZero(); _P.setZero();
@@ -994,22 +1020,26 @@ void BlockLocalPositionEstimator::predict()
// process noise power matrix // process noise power matrix
Matrix<float, n_x, n_x> Q; Matrix<float, n_x, n_x> Q;
Q.setZero(); Q.setZero();
Q(X_x, X_x) = _pn_p_noise_power.get(); float pn_p_sq = _pn_p_noise_density.get() * _pn_p_noise_density.get();
Q(X_y, X_y) = _pn_p_noise_power.get(); float pn_v_sq = _pn_v_noise_density.get() * _pn_v_noise_density.get();
Q(X_z, X_z) = _pn_p_noise_power.get(); Q(X_x, X_x) = pn_p_sq;
Q(X_vx, X_vx) = _pn_v_noise_power.get(); Q(X_y, X_y) = pn_p_sq;
Q(X_vy, X_vy) = _pn_v_noise_power.get(); Q(X_z, X_z) = pn_p_sq;
Q(X_vz, X_vz) = _pn_v_noise_power.get(); 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, // technically, the noise is in the body frame,
// but the components are all the same, so // but the components are all the same, so
// ignoring for now // ignoring for now
Q(X_bx, X_bx) = _pn_b_noise_power.get(); float pn_b_sq = _pn_b_noise_density.get() * _pn_b_noise_density.get();
Q(X_by, X_by) = _pn_b_noise_power.get(); Q(X_bx, X_bx) = pn_b_sq;
Q(X_bz, X_bz) = _pn_b_noise_power.get(); Q(X_by, X_by) = pn_b_sq;
Q(X_bz, X_bz) = pn_b_sq;
// terrain random walk noise // 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 // continuous time kalman filter prediction
Vector<float, n_x> dx = (A * _x + B * _u) * getDt(); Vector<float, n_x> dx = (A * _x + B * _u) * getDt();
@@ -1032,13 +1062,6 @@ void BlockLocalPositionEstimator::predict()
_x += dx; _x += dx;
_P += (A * _P + _P * A.transpose() + _P += (A * _P + _P * A.transpose() +
B * R * B.transpose() + Q) * getDt(); 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<uint64_t>(_timeStamp));
//}
} }
void BlockLocalPositionEstimator::correctFlow() void BlockLocalPositionEstimator::correctFlow()
@@ -1062,11 +1085,11 @@ void BlockLocalPositionEstimator::correctFlow()
// calculate range to center of image for flow // calculate range to center of image for flow
float d = 0; float d = 0;
if (_lidarInitialized && _lidarFault < FAULT_SEVERE) { if (_lidarInitialized && _lidarFault < fault_lvl_disable) {
d = _sub_lidar->get().current_distance d = _sub_lidar->get().current_distance
+ (_lidar_z_offset.get() - _flow_z_offset.get()); + (_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 d = _sub_sonar->get().current_distance
+ (_sonar_z_offset.get() - _flow_z_offset.get()); + (_sonar_z_offset.get() - _flow_z_offset.get());
@@ -1127,13 +1150,6 @@ void BlockLocalPositionEstimator::correctFlow()
_flowX += delta_n(0); _flowX += delta_n(0);
_flowY += delta_n(1); _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 // measurement
Vector<float, 2> y; Vector<float, 2> y;
y(0) = _flowX; y(0) = _flowX;
@@ -1162,8 +1178,7 @@ void BlockLocalPositionEstimator::correctFlow()
warnx("[lpe] flow OK"); warnx("[lpe] flow OK");
} }
// kalman filter correction if no fault if (_flowFault < fault_lvl_disable) {
if (_flowFault < FAULT_SEVERE) {
Matrix<float, n_x, n_y_flow> K = Matrix<float, n_x, n_y_flow> K =
_P * C.transpose() * S_I; _P * C.transpose() * S_I;
_x += K * r; _x += K * r;
@@ -1187,8 +1202,6 @@ void BlockLocalPositionEstimator::correctSonar()
float max_dist = _sub_sonar->get().max_distance - eps; float max_dist = _sub_sonar->get().max_distance - eps;
if (d < min_dist) { if (d < min_dist) {
//mavlink_log_info(_mavlink_fd, "[lpe] sonar min dist");
warnx("[lpe] sonar min dist");
// can't correct, so return // can't correct, so return
return; return;
@@ -1206,7 +1219,7 @@ void BlockLocalPositionEstimator::correctSonar()
_time_last_sonar = _timeStamp; _time_last_sonar = _timeStamp;
// do not use sonar if lidar is active // do not use sonar if lidar is active
if (_lidarInitialized && _lidarFault < FAULT_SEVERE) { return; } if (_lidarInitialized && _lidarFault < fault_lvl_disable) { return; }
// calculate covariance // calculate covariance
float cov = _sub_sonar->get().covariance; float cov = _sub_sonar->get().covariance;
@@ -1247,19 +1260,27 @@ void BlockLocalPositionEstimator::correctSonar()
if (beta > BETA_TABLE[n_y_sonar]) { if (beta > BETA_TABLE[n_y_sonar]) {
if (_sonarFault < FAULT_MINOR) { if (_sonarFault < FAULT_MINOR) {
// avoid printing messages near ground
if (_x(X_tz) > 1.0f) {
mavlink_log_info(_mavlink_fd, "[lpe] sonar fault, beta %5.2f", double(beta)); mavlink_log_info(_mavlink_fd, "[lpe] sonar fault, beta %5.2f", double(beta));
warnx("[lpe] sonar fault, beta %5.2f", double(beta)); warnx("[lpe] sonar fault, beta %5.2f", double(beta));
}
_sonarFault = FAULT_MINOR; _sonarFault = FAULT_MINOR;
} }
} else if (_sonarFault) { } else if (_sonarFault) {
_sonarFault = FAULT_NONE; _sonarFault = FAULT_NONE;
// avoid printing messages near ground
if (_x(X_tz) > 1.0f) {
mavlink_log_info(_mavlink_fd, "[lpe] sonar OK"); mavlink_log_info(_mavlink_fd, "[lpe] sonar OK");
warnx("[lpe] sonar OK"); warnx("[lpe] sonar OK");
} }
}
// kalman filter correction if no fault // kalman filter correction if no fault
if (_sonarFault < FAULT_SEVERE) { if (_sonarFault < fault_lvl_disable) {
Matrix<float, n_x, n_y_sonar> K = Matrix<float, n_x, n_y_sonar> K =
_P * C.transpose() * S_I; _P * C.transpose() * S_I;
Vector<float, n_x> dx = K * r; Vector<float, n_x> dx = K * r;
@@ -1303,14 +1324,13 @@ void BlockLocalPositionEstimator::correctBaro()
if (beta > BETA_TABLE[n_y_baro]) { if (beta > BETA_TABLE[n_y_baro]) {
if (_baroFault < FAULT_MINOR) { if (_baroFault < FAULT_MINOR) {
mavlink_log_info(_mavlink_fd, "[lpe] baro fault, beta %5.2f", double(beta)); mavlink_log_info(_mavlink_fd, "[lpe] baro fault, r %5.2f m, beta %5.2f",
warnx("[lpe] baro fault, beta %5.2f", double(beta)); double(r(0)), double(beta));
warnx("[lpe] baro fault, r %5.2f m, beta %5.2f",
double(r(0)), double(beta));
_baroFault = FAULT_MINOR; _baroFault = FAULT_MINOR;
} }
// lower baro trust
S_I = inv<float, n_y_baro>((C * _P * C.transpose()) + R * 10);
} else if (_baroFault) { } else if (_baroFault) {
_baroFault = FAULT_NONE; _baroFault = FAULT_NONE;
mavlink_log_info(_mavlink_fd, "[lpe] baro OK"); mavlink_log_info(_mavlink_fd, "[lpe] baro OK");
@@ -1318,7 +1338,7 @@ void BlockLocalPositionEstimator::correctBaro()
} }
// kalman filter correction if no fault // kalman filter correction if no fault
if (_baroFault < FAULT_SEVERE) { if (_baroFault < fault_lvl_disable) {
Matrix<float, n_x, n_y_baro> K = _P * C.transpose() * S_I; Matrix<float, n_x, n_y_baro> K = _P * C.transpose() * S_I;
Vector<float, n_x> dx = K * r; Vector<float, n_x> dx = K * r;
@@ -1343,7 +1363,11 @@ void BlockLocalPositionEstimator::correctLidar()
float max_dist = _sub_lidar->get().max_distance - eps; float max_dist = _sub_lidar->get().max_distance - eps;
// if out of range, this is an error // 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) { if (_lidarFault < FAULT_SEVERE) {
mavlink_log_info(_mavlink_fd, "[lpe] lidar out of range"); mavlink_log_info(_mavlink_fd, "[lpe] lidar out of range");
warnx("[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 (beta > BETA_TABLE[n_y_lidar]) {
if (_lidarFault < FAULT_MINOR) { if (_lidarFault < FAULT_MINOR) {
mavlink_log_info(_mavlink_fd, "[lpe] lidar fault, beta %5.2f", double(beta)); // only print message if above 1 meter, avoids
warnx("[lpe] lidar fault, beta %5.2f", double(beta)); // 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; _lidarFault = FAULT_MINOR;
} }
} else if (_lidarFault) { // disable fault if ok } else if (_lidarFault) { // disable fault if ok
_lidarFault = FAULT_NONE; _lidarFault = FAULT_NONE;
// 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"); mavlink_log_info(_mavlink_fd, "[lpe] lidar OK");
warnx("[lpe] lidar OK"); warnx("[lpe] lidar OK");
} }
}
// kalman filter correction if no fault // kalman filter correction if no fault
if (_lidarFault < FAULT_SEVERE) { if (_lidarFault < fault_lvl_disable) {
Matrix<float, n_x, n_y_lidar> K = _P * C.transpose() * S_I; Matrix<float, n_x, n_y_lidar> K = _P * C.transpose() * S_I;
Vector<float, n_x> dx = K * r; Vector<float, n_x> dx = K * r;
@@ -1424,7 +1460,7 @@ void BlockLocalPositionEstimator::correctGps()
float eph = _sub_gps.get().eph; float eph = _sub_gps.get().eph;
if (nSat < 6 || eph > _gps_eph_max.get()) { 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)); 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)); warnx("[lpe] gps fault nSat: %d eph: %5.2f", nSat, double(eph));
_gpsFault = FAULT_SEVERE; _gpsFault = FAULT_SEVERE;
@@ -1488,29 +1524,36 @@ void BlockLocalPositionEstimator::correctGps()
R(5, 5) = var_vz; R(5, 5) = var_vz;
// get delayed x and P // get delayed x and P
//uint64_t t_delay = 0; float t_delay = 0;
//int i = 0; int i = 0;
//for (i = 0; i < HIST_LEN; i++) { for (i = 1; i < HIST_LEN; i++) {
//t_delay += _tDelay.get(i)(0, 0); t_delay = 1.0e-6f * (_timeStamp - _tDelay.get(i)(0, 0));
//if (t_delay > 2000) { if (t_delay > _gps_delay.get()) {
//break; break;
//} }
//} }
//Vector<float, n_x> x0 = _xDelay.get(i); // if you are 3 steps past the delay you wanted, this
//Matrix<float, n_x, n_x> P0 = _PDelay.get(i); // 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<float, n_x> x0 = _xDelay.get(i);
// residual // residual
Vector<float, n_y_gps> r = y - C * _x; Vector<float, n_y_gps> r = y - C * x0;
Matrix<float, n_y_gps, n_y_gps> S_I = inv<float, 6>(C * _P * C.transpose() + R); Matrix<float, n_y_gps, n_y_gps> S_I = inv<float, 6>(C * _P * C.transpose() + R);
// fault detection // fault detection
float beta = (r.transpose() * (S_I * r))(0, 0); float beta = (r.transpose() * (S_I * r))(0, 0);
if (beta > BETA_TABLE[n_y_gps]) { 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)); mavlink_log_info(_mavlink_fd, "[lpe] gps fault, beta: %5.2f", double(beta));
warnx("[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", 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 // kalman filter correction if no hard fault
if (_gpsFault < FAULT_SEVERE) { if (_gpsFault < fault_lvl_disable) {
Matrix<float, n_x, n_y_gps> K = _P * C.transpose() * S_I; Matrix<float, n_x, n_y_gps> K = _P * C.transpose() * S_I;
_x += K * r; _x += K * r;
_P -= K * C * _P; _P -= K * C * _P;
@@ -1574,15 +1617,12 @@ void BlockLocalPositionEstimator::correctVision()
float beta = (r.transpose() * (S_I * r))(0, 0); float beta = (r.transpose() * (S_I * r))(0, 0);
if (beta > BETA_TABLE[n_y_vision]) { 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)); mavlink_log_info(_mavlink_fd, "[lpe] vision position fault, beta %5.2f", double(beta));
warnx("[lpe] vision position fault, beta %5.2f", double(beta)); warnx("[lpe] vision position fault, beta %5.2f", double(beta));
_visionFault = FAULT_MINOR; _visionFault = FAULT_MINOR;
} }
// trust less
S_I = inv<float, n_y_vision>((C * _P * C.transpose()) + R * 10);
} else if (_visionFault) { } else if (_visionFault) {
_visionFault = FAULT_NONE; _visionFault = FAULT_NONE;
mavlink_log_info(_mavlink_fd, "[lpe] vision position OK"); mavlink_log_info(_mavlink_fd, "[lpe] vision position OK");
@@ -1590,7 +1630,7 @@ void BlockLocalPositionEstimator::correctVision()
} }
// kalman filter correction if no fault // kalman filter correction if no fault
if (_visionFault < FAULT_SEVERE) { if (_visionFault < fault_lvl_disable) {
Matrix<float, n_x, n_y_vision> K = _P * C.transpose() * S_I; Matrix<float, n_x, n_y_vision> K = _P * C.transpose() * S_I;
_x += K * r; _x += K * r;
_P -= K * C * _P; _P -= K * C * _P;
@@ -1631,15 +1671,12 @@ void BlockLocalPositionEstimator::correctMocap()
float beta = (r.transpose() * (S_I * r))(0, 0); float beta = (r.transpose() * (S_I * r))(0, 0);
if (beta > BETA_TABLE[n_y_mocap]) { 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)); mavlink_log_info(_mavlink_fd, "[lpe] mocap fault, beta %5.2f", double(beta));
warnx("[lpe] mocap fault, beta %5.2f", double(beta)); warnx("[lpe] mocap fault, beta %5.2f", double(beta));
_mocapFault = FAULT_MINOR; _mocapFault = FAULT_MINOR;
} }
// trust less
S_I = inv<float, n_y_mocap>((C * _P * C.transpose()) + R * 10);
} else if (_mocapFault) { } else if (_mocapFault) {
_mocapFault = FAULT_NONE; _mocapFault = FAULT_NONE;
mavlink_log_info(_mavlink_fd, "[lpe] mocap OK"); mavlink_log_info(_mavlink_fd, "[lpe] mocap OK");
@@ -1647,7 +1684,7 @@ void BlockLocalPositionEstimator::correctMocap()
} }
// kalman filter correction if no fault // kalman filter correction if no fault
if (_mocapFault < FAULT_SEVERE) { if (_mocapFault < fault_lvl_disable) {
Matrix<float, n_x, n_y_mocap> K = _P * C.transpose() * S_I; Matrix<float, n_x, n_y_mocap> K = _P * C.transpose() * S_I;
_x += K * r; _x += K * r;
_P -= K * C * _P; _P -= K * C * _P;

26
src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
View File

@@ -1,7 +1,7 @@
#pragma once #pragma once
#include <px4_posix.h> #include <px4_posix.h>
#include <controllib/uorb/blocks.hpp> #include <controllib/blocks.hpp>
#include <mathlib/mathlib.h> #include <mathlib/mathlib.h>
#include <systemlib/perf_counter.h> #include <systemlib/perf_counter.h>
#include <lib/geo/geo.h> #include <lib/geo/geo.h>
@@ -35,14 +35,15 @@ using namespace Eigen;
#include <uORB/Publication.hpp> #include <uORB/Publication.hpp>
#include <uORB/topics/vehicle_local_position.h> #include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_global_position.h> #include <uORB/topics/vehicle_global_position.h>
//#include <uORB/topics/filtered_bottom_flow.h>
#include <uORB/topics/estimator_status.h> #include <uORB/topics/estimator_status.h>
#define CBRK_NO_VISION_KEY 328754 #define CBRK_NO_VISION_KEY 328754
using namespace control; 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 { enum fault_t {
FAULT_NONE = 0, FAULT_NONE = 0,
@@ -165,7 +166,6 @@ private:
// publications // publications
void publishLocalPos(); void publishLocalPos();
void publishGlobalPos(); void publishGlobalPos();
//void publishFilteredFlow();
void publishEstimatorStatus(); void publishEstimatorStatus();
// attributes // attributes
@@ -192,7 +192,6 @@ private:
// publications // publications
uORB::Publication<vehicle_local_position_s> _pub_lpos; uORB::Publication<vehicle_local_position_s> _pub_lpos;
uORB::Publication<vehicle_global_position_s> _pub_gpos; uORB::Publication<vehicle_global_position_s> _pub_gpos;
//uORB::Publication<filtered_bottom_flow_s> _pub_filtered_flow;
uORB::Publication<estimator_status_s> _pub_est_status; uORB::Publication<estimator_status_s> _pub_est_status;
// map projection // map projection
@@ -217,6 +216,7 @@ private:
BlockParamFloat _baro_stddev; BlockParamFloat _baro_stddev;
// gps parameters // gps parameters
BlockParamFloat _gps_delay;
BlockParamFloat _gps_xy_stddev; BlockParamFloat _gps_xy_stddev;
BlockParamFloat _gps_z_stddev; BlockParamFloat _gps_z_stddev;
BlockParamFloat _gps_vxy_stddev; BlockParamFloat _gps_vxy_stddev;
@@ -239,10 +239,10 @@ private:
BlockParamInt _flow_min_q; BlockParamInt _flow_min_q;
// process noise // process noise
BlockParamFloat _pn_p_noise_power; BlockParamFloat _pn_p_noise_density;
BlockParamFloat _pn_v_noise_power; BlockParamFloat _pn_v_noise_density;
BlockParamFloat _pn_b_noise_power; BlockParamFloat _pn_b_noise_density;
BlockParamFloat _pn_t_noise_power; BlockParamFloat _pn_t_noise_density;
// flow gyro filter // flow gyro filter
BlockHighPass _flow_gyro_x_high_pass; BlockHighPass _flow_gyro_x_high_pass;
@@ -258,14 +258,13 @@ private:
BlockStats<double, 3> _gpsStats; BlockStats<double, 3> _gpsStats;
// delay blocks // delay blocks
//BlockDelay<float, n_x, 1, HIST_LEN> _xDelay; BlockDelay<float, n_x, 1, HIST_LEN> _xDelay;
//BlockDelay<float, n_x, n_x, HIST_LEN> _PDelay; BlockDelay<uint64_t, 1, 1, HIST_LEN> _tDelay;
//BlockDelay<uint64_t, 1, 1, HIST_LEN> _tDelay;
// misc // misc
px4_pollfd_struct_t _polls[3]; px4_pollfd_struct_t _polls[3];
uint64_t _timeStamp; uint64_t _timeStamp;
//uint64_t _time_last_hist; uint64_t _time_last_hist;
uint64_t _time_last_xy; uint64_t _time_last_xy;
uint64_t _time_last_z; uint64_t _time_last_z;
uint64_t _time_last_tz; uint64_t _time_last_tz;
@@ -308,6 +307,7 @@ private:
bool _canEstimateXY; bool _canEstimateXY;
bool _canEstimateZ; bool _canEstimateZ;
bool _canEstimateT; bool _canEstimateT;
bool _canEstimateGlobal;
bool _xyTimeout; bool _xyTimeout;
bool _zTimeout; bool _zTimeout;
bool _tzTimeout; bool _tzTimeout;

2
src/modules/local_position_estimator/CMakeLists.txt
View File

@@ -31,7 +31,7 @@
# #
############################################################################ ############################################################################
if(${OS} STREQUAL "nuttx") if(${OS} STREQUAL "nuttx")
list(APPEND MODULE_CFLAGS -Wframe-larger-than=7000) list(APPEND MODULE_CFLAGS -Wframe-larger-than=10000)
elseif(${OS} STREQUAL "posix") elseif(${OS} STREQUAL "posix")
list(APPEND MODULE_CFLAGS -Wno-error) list(APPEND MODULE_CFLAGS -Wno-error)
endif() endif()

13
src/modules/local_position_estimator/fault_table.py Normal file
View File

@@ -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])

2
src/modules/local_position_estimator/local_position_estimator_main.cpp
View File

@@ -112,7 +112,7 @@ int local_position_estimator_main(int argc, char *argv[])
deamon_task = px4_task_spawn_cmd("lp_estimator", deamon_task = px4_task_spawn_cmd("lp_estimator",
SCHED_DEFAULT, SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5, SCHED_PRIORITY_MAX - 5,
10240, 11264,
local_position_estimator_thread_main, local_position_estimator_thread_main,
(argv && argc > 2) ? (char *const *) &argv[2] : (char *const *) NULL); (argv && argc > 2) ? (char *const *) &argv[2] : (char *const *) NULL);
return 0; return 0;

59
src/modules/local_position_estimator/params.c
View File

@@ -105,7 +105,7 @@ PARAM_DEFINE_FLOAT(LPE_LDR_OFF_Z, 0.00f);
* should be 0.0464 * should be 0.0464
* *
* @group Local Position Estimator * @group Local Position Estimator
* @unit m/s/s * @unit m/s^2
* @min 0.00001 * @min 0.00001
* @max 2 * @max 2
* @decimal 4 * @decimal 4
@@ -118,7 +118,7 @@ PARAM_DEFINE_FLOAT(LPE_ACC_XY, 0.0454f);
* (see Accel x comments) * (see Accel x comments)
* *
* @group Local Position Estimator * @group Local Position Estimator
* @unit m/s/s * @unit m/s^2
* @min 0.00001 * @min 0.00001
* @max 2 * @max 2
* @decimal 4 * @decimal 4
@@ -136,6 +136,19 @@ PARAM_DEFINE_FLOAT(LPE_ACC_Z, 0.0454f);
*/ */
PARAM_DEFINE_FLOAT(LPE_BAR_Z, 1.0f); 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. * GPS xy standard deviation.
* *
@@ -153,10 +166,10 @@ PARAM_DEFINE_FLOAT(LPE_GPS_XY, 2.0f);
* @group Local Position Estimator * @group Local Position Estimator
* @unit m * @unit m
* @min 0.01 * @min 0.01
* @max 20 * @max 200
* @decimal 2 * @decimal 2
*/ */
PARAM_DEFINE_FLOAT(LPE_GPS_Z, 10.0f); PARAM_DEFINE_FLOAT(LPE_GPS_Z, 100.0f);
/** /**
* GPS xy velocity standard deviation. * GPS xy velocity standard deviation.
@@ -237,43 +250,43 @@ PARAM_DEFINE_INT32(LPE_NO_VISION, 0);
PARAM_DEFINE_FLOAT(LPE_VIC_P, 0.05f); PARAM_DEFINE_FLOAT(LPE_VIC_P, 0.05f);
/** /**
* Position propagation process noise power (variance*sampling rate). * Position propagation noise density
* *
* @group Local Position Estimator * @group Local Position Estimator
* @unit (m/s/s)-s * @unit m/s/sqrt(Hz)
* @min 0 * @min 0
* @max 1 * @max 1
* @decimal 8 * @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 * @group Local Position Estimator
* @unit (m/s)-s * @unit (m/s)/s/sqrt(Hz)
* @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
* @min 0 * @min 0
* @max 1 * @max 1
* @decimal 8 * @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 * @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 * @min 0
* @max 1 * @max 1
* @decimal 3 * @decimal 3