mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
Merge pull request #4009 from jgoppert/lpe-gps-delay
Enabled gps delay compensation for lpe based on x hist.
This commit is contained in:
James Goppert
commit
21e5c0a990
@ -4,6 +4,8 @@
|
||||
#include <systemlib/err.h>
|
||||
#include <matrix/math.hpp>
|
||||
|
||||
// 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<uint64_t>(_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<float, n_x, n_x> 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<float, n_x> 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<uint64_t>(_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<float, 2> 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<float, n_x, n_y_flow> 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<float, n_x, n_y_sonar> K =
|
||||
_P * C.transpose() * S_I;
|
||||
Vector<float, n_x> 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<float, n_y_baro>((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<float, n_x, n_y_baro> K = _P * C.transpose() * S_I;
|
||||
Vector<float, n_x> 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<float, n_x, n_y_lidar> K = _P * C.transpose() * S_I;
|
||||
Vector<float, n_x> 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<float, n_x> x0 = _xDelay.get(i);
|
||||
//Matrix<float, n_x, n_x> 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<float, n_x> x0 = _xDelay.get(i);
|
||||
|
||||
// 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);
|
||||
|
||||
// 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<float, n_x, n_y_gps> 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<float, n_y_vision>((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<float, n_x, n_y_vision> 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<float, n_y_mocap>((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<float, n_x, n_y_mocap> K = _P * C.transpose() * S_I;
|
||||
_x += K * r;
|
||||
_P -= K * C * _P;
|
||||
|
||||
@ -1,7 +1,7 @@
|
||||
#pragma once
|
||||
|
||||
#include <px4_posix.h>
|
||||
#include <controllib/uorb/blocks.hpp>
|
||||
#include <controllib/blocks.hpp>
|
||||
#include <mathlib/mathlib.h>
|
||||
#include <systemlib/perf_counter.h>
|
||||
#include <lib/geo/geo.h>
|
||||
@ -35,14 +35,15 @@ using namespace Eigen;
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/topics/vehicle_local_position.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
//#include <uORB/topics/filtered_bottom_flow.h>
|
||||
#include <uORB/topics/estimator_status.h>
|
||||
|
||||
#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<vehicle_local_position_s> _pub_lpos;
|
||||
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;
|
||||
|
||||
// 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<double, 3> _gpsStats;
|
||||
|
||||
// delay blocks
|
||||
//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<float, n_x, 1, HIST_LEN> _xDelay;
|
||||
BlockDelay<uint64_t, 1, 1, HIST_LEN> _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;
|
||||
|
||||
@ -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()
|
||||
|
||||
13
src/modules/local_position_estimator/fault_table.py
Normal file
13
src/modules/local_position_estimator/fault_table.py
Normal 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])
|
||||
@ -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;
|
||||
|
||||
@ -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
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user