dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Improvements to lpe for flow and gps. (#5401)

This commit is contained in:
James Goppert 2016-08-27 23:50:24 -04:00 committed by GitHub
parent be1417f613
commit 6b08e8ce1f
6 changed files with 94 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -79,12 +79,14 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
_mocap_p_stddev(this, "VIC_P"),
_flow_z_offset(this, "FLW_OFF_Z"),
_flow_xy_stddev(this, "FLW_XY"),
_flow_xy_d_stddev(this, "FLW_XY_D"),
//_flow_board_x_offs(NULL, "SENS_FLW_XOFF"),
//_flow_board_y_offs(NULL, "SENS_FLW_YOFF"),
_flow_min_q(this, "FLW_QMIN"),
_pn_p_noise_density(this, "PN_P"),
_pn_v_noise_density(this, "PN_V"),
_pn_b_noise_density(this, "PN_B"),
_pn_t_noise_density(this, "PN_T"),
_t_max_grade(this, "T_MAX_GRADE"),
// init origin
@ -150,7 +152,6 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
// flow integration
_flowX(0),
_flowY(0),
_flowMeanQual(0),
// status
_validXY(false),
@ -197,8 +198,6 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
// initialize A, B, P, x, u
_x.setZero();
_u.setZero();
_flowX = 0;
_flowY = 0;
initSS();
// perf counters
@ -689,7 +688,11 @@ void BlockLocalPositionEstimator::publishLocalPos()
_pub_lpos.get().dist_bottom = _aglLowPass.getState();
_pub_lpos.get().dist_bottom_rate = - xLP(X_vz);
_pub_lpos.get().surface_bottom_timestamp = _timeStamp;
_pub_lpos.get().dist_bottom_valid = _validTZ && _validZ;
// we estimate agl even when we don't have terrain info
// if you are in terrain following mode this is important
// so that if terrain estimation fails there isn't a
// sudden altitude jump
_pub_lpos.get().dist_bottom_valid = _validZ;
_pub_lpos.get().eph = sqrtf(_P(X_x, X_x) + _P(X_y, X_y));
_pub_lpos.get().epv = sqrtf(_P(X_z, X_z));
_pub_lpos.update();
@ -840,8 +843,11 @@ void BlockLocalPositionEstimator::updateSSParams()
_Q(X_bz, X_bz) = pn_b_sq;
// terrain random walk noise ((m/s)/sqrt(hz)), scales with velocity
float pn_t_stddev = (_t_max_grade.get() / 100.0f) * sqrtf(_x(X_vx) * _x(X_vx) + _x(X_vy) * _x(X_vy));
_Q(X_tz, X_tz) = pn_t_stddev * pn_t_stddev;
float pn_t_noise_density =
_pn_t_noise_density.get() +
(_t_max_grade.get() / 100.0f) * sqrtf(_x(X_vx) * _x(X_vx) + _x(X_vy) * _x(X_vy));
_Q(X_tz, X_tz) = pn_t_noise_density * pn_t_noise_density;
}
void BlockLocalPositionEstimator::predict()
@ -853,6 +859,7 @@ void BlockLocalPositionEstimator::predict()
if (integrate && _sub_att.get().R_valid) {
Matrix3f R_att(_sub_att.get().R);
Vector3f a(_sub_sensor.get().accelerometer_m_s2);
// note, bias is removed in dynamics function
_u = R_att * a;
_u(U_az) += 9.81f; // add g

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

@ -98,10 +98,10 @@ class BlockLocalPositionEstimator : public control::SuperBlock
// ax, ay, az (acceleration NED)
//
// states:
// px, py, pz , ( position NED)
// vx, vy, vz ( vel NED),
// bx, by, bz ( accel bias)
// tz (terrain altitude, ASL)
// px, py, pz , ( position NED, m)
// vx, vy, vz ( vel NED, m/s),
// bx, by, bz ( accel bias, m/s^2)
// tz (terrain altitude, ASL, m)
//
// measurements:
//
@ -184,6 +184,7 @@ private:
int flowMeasure(Vector<float, n_y_flow> &y);
void flowCorrect();
void flowInit();
void flowDeinit();
void flowCheckTimeout();
// vision
@ -282,6 +283,7 @@ private:
// flow parameters
BlockParamFloat _flow_z_offset;
BlockParamFloat _flow_xy_stddev;
BlockParamFloat _flow_xy_d_stddev;
//BlockParamFloat _flow_board_x_offs;
//BlockParamFloat _flow_board_y_offs;
BlockParamInt _flow_min_q;
@ -290,6 +292,7 @@ private:
BlockParamFloat _pn_p_noise_density;
BlockParamFloat _pn_v_noise_density;
BlockParamFloat _pn_b_noise_density;
BlockParamFloat _pn_t_noise_density;
BlockParamFloat _t_max_grade;
// init origin
@ -354,7 +357,6 @@ private:
// flow integration
float _flowX;
float _flowY;
float _flowMeanQual;
// status
bool _validXY;

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

@ -25,6 +25,17 @@ PARAM_DEFINE_FLOAT(LPE_FLW_OFF_Z, 0.0f);
*/
PARAM_DEFINE_FLOAT(LPE_FLW_XY, 0.01f);
/**
* Optical flow xy standard deviation linear factor on distance
*
* @group Local Position Estimator
* @unit m / m
* @min 0.01
* @max 1
* @decimal 3
*/
PARAM_DEFINE_FLOAT(LPE_FLW_XY_D, 0.01f);
/**
* Optical flow minimum quality threshold
*
@ -285,8 +296,20 @@ PARAM_DEFINE_FLOAT(LPE_PN_V, 0.1f);
*/
PARAM_DEFINE_FLOAT(LPE_PN_B, 1e-3f);
/**
* Terrain random walk noise density, hilly/outdoor (0.1), flat/Indoor (0.001)
*
* @group Local Position Estimator
* @unit (m/s)/(sqrt(hz))
* @min 0
* @max 1
* @decimal 3
*/
PARAM_DEFINE_FLOAT(LPE_PN_T, 0.001f);
/**
* Terrain maximum percent grade, hilly/outdoor (100 = 45 deg), flat/Indoor (0 = 0 deg)
* Used to calculate increased terrain random walk nosie due to movement.
*
* @group Local Position Estimator
* @unit %

26
src/modules/local_position_estimator/sensors/flow.cpp
View File

@ -29,11 +29,21 @@ void BlockLocalPositionEstimator::flowInit()
"quality %d std %d",
int(_flowQStats.getMean()(0)),
int(_flowQStats.getStdDev()(0)));
// set flow x, y as estimate x, y at beginning of optical
// flow tracking
_flowX = _x(X_x);
_flowY = _x(X_y);
_flowInitialized = true;
_flowFault = FAULT_NONE;
}
}
void BlockLocalPositionEstimator::flowDeinit()
{
_flowInitialized = false;
_flowQStats.reset();
}
int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y)
{
// check for agl
@ -66,8 +76,9 @@ int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y)
if (delta.norm() > 3) {
mavlink_and_console_log_info(&mavlink_log_pub,
"[lpe] flow too far from GPS, disabled");
_flowInitialized = false;
"[lpe] flow too far from GPS, resetting position");
_flowX = px;
_flowY = py;
return -1;
}
}
@ -127,10 +138,10 @@ void BlockLocalPositionEstimator::flowCorrect()
SquareMatrix<float, n_y_flow> R;
R.setZero();
R(Y_flow_x, Y_flow_x) =
_flow_xy_stddev.get() * _flow_xy_stddev.get();
R(Y_flow_y, Y_flow_y) =
_flow_xy_stddev.get() * _flow_xy_stddev.get();
float d = agl() * cosf(_sub_att.get().roll) * cosf(_sub_att.get().pitch);
float flow_xy_stddev = _flow_xy_stddev.get() + _flow_xy_d_stddev.get() * d ;
R(Y_flow_x, Y_flow_x) = flow_xy_stddev * flow_xy_stddev;
R(Y_flow_y, Y_flow_y) = R(Y_flow_x, Y_flow_x);
// residual
Vector<float, 2> r = y - C * _x;
@ -178,8 +189,7 @@ void BlockLocalPositionEstimator::flowCheckTimeout()
{
if (_timeStamp - _time_last_flow > FLOW_TIMEOUT) {
if (_flowInitialized) {
_flowInitialized = false;
_flowQStats.reset();
flowDeinit();
mavlink_and_console_log_critical(&mavlink_log_pub, "[lpe] flow timeout ");
}
}

11
src/modules/local_position_estimator/sensors/gps.cpp
View File

@ -148,6 +148,17 @@ void BlockLocalPositionEstimator::gpsCorrect()
var_z = _sub_gps.get().epv * _sub_gps.get().epv;
}
float gps_s_stddev = _sub_gps.get().s_variance_m_s;
if (gps_s_stddev > _gps_vxy_stddev.get()) {
var_vxy = gps_s_stddev * gps_s_stddev;
}
if (gps_s_stddev > _gps_vz_stddev.get()) {
var_z = gps_s_stddev * gps_s_stddev;
}
R(0, 0) = var_xy;
R(1, 1) = var_xy;
R(2, 2) = var_z;

23
src/modules/mavlink/mavlink_messages.cpp
View File

@ -3158,12 +3158,33 @@ protected:
struct vehicle_local_position_s local_pos;
updated |= _local_pos_sub->update(&_local_pos_time, &local_pos);
msg.altitude_local = (_local_pos_time > 0) ? -local_pos.z : NAN;
// publish this data if global isn't publishing
if (_global_pos_time == 0) {
if (local_pos.dist_bottom_valid) {
msg.bottom_clearance = local_pos.dist_bottom;
msg.altitude_terrain = msg.altitude_local - msg.bottom_clearance;
} else {
msg.bottom_clearance = NAN;
msg.altitude_terrain = NAN;
}
}
}
{
struct home_position_s home;
updated |= _home_sub->update(&_home_time, &home);
msg.altitude_relative = (_home_time > 0) ? (global_alt - home.alt) : NAN;
if (_global_pos_time > 0 and _home_time > 0) {
msg.altitude_relative = global_alt - home.alt;
} else if (_local_pos_time > 0 and _home_time > 0) {
msg.altitude_relative = msg.altitude_local;
} else {
msg.altitude_relative = NAN;
}
}
if (updated) {