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

LPE vision estimation fixes. (#5505)

This commit is contained in:
James Goppert 2016-10-01 09:02:12 -04:00 committed by GitHub
parent f6bed6f2d2
commit fe40e9cfae
10 changed files with 134 additions and 110 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
launch/mavros.launch
View File

@ -6,11 +6,13 @@
<arg name="gcs_url" default="udp://:14550@:14555" />
<arg name="tgt_system" default="1" />
<arg name="tgt_component" default="1" />
<arg name="pluginlists_yaml" default="$(find mavros)/launch/px4_pluginlists.yaml" />
<arg name="config_yaml" default="$(find mavros)/launch/px4_config.yaml" />
<group ns="$(arg ns)">
<include file="$(find mavros)/launch/node.launch">
<arg name="pluginlists_yaml" value="$(find mavros)/launch/px4_pluginlists.yaml" />
<arg name="config_yaml" value="$(find mavros)/launch/px4_config.yaml" />
<arg name="pluginlists_yaml" value="$(arg pluginlists_yaml)" />
<arg name="config_yaml" value="$(arg config_yaml)" />
<arg name="fcu_url" value="$(arg fcu_url)" />
<arg name="gcs_url" value="$(arg gcs_url)" />
@ -19,3 +21,5 @@
</include>
</group>
</launch>
<!-- vim: set ft=xml et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : -->

5
launch/mavros_posix_sitl.launch
View File

@ -25,6 +25,9 @@
<arg name="verbose" default="false"/>
<arg name="paused" default="false"/>
<arg name="pluginlists_yaml" default="$(find mavros)/launch/px4_pluginlists.yaml" />
<arg name="config_yaml" default="$(find mavros)/launch/px4_config.yaml" />
<include file="$(find px4)/launch/posix_sitl.launch">
<arg name="x" value="$(arg x)"/>
<arg name="y" value="$(arg y)"/>
@ -48,6 +51,8 @@
<arg name="ns" value="$(arg ns)"/>
<arg name="gcs_url" value=""/> <!-- GCS link is provided by SITL -->
<arg name="fcu_url" value="$(arg fcu_url)"/>
<arg name="pluginlists_yaml" value="$(arg pluginlists_yaml)" />
<arg name="config_yaml" value="$(arg config_yaml)" />
</include>
</launch>

3
launch/posix_sitl.launch
View File

@ -6,8 +6,7 @@
<arg name="z" default="0"/>
<arg name="R" default="0"/>
<arg name="P" default="0"/>
<arg name="Y" default="0"/>
<arg name="Y" default="0"/>
<arg name="est" default="lpe"/>
<arg name="vehicle" default="iris"/>
<arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/empty.world"/>

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

@ -57,7 +57,8 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
_map_ref(),
// block parameters
_xy_pub_thresh(this, "XY_PUB"),
_pub_agl_z(this, "PUB_AGL_Z"),
_vxy_pub_thresh(this, "VXY_PUB"),
_z_pub_thresh(this, "Z_PUB"),
_sonar_z_stddev(this, "SNR_Z"),
_sonar_z_offset(this, "SNR_OFF_Z"),
@ -78,9 +79,11 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
_vision_z_stddev(this, "VIS_Z"),
_vision_on(this, "VIS_ON"),
_mocap_p_stddev(this, "VIC_P"),
_flow_gyro_comp(this, "FLW_GYRO_CMP"),
_flow_z_offset(this, "FLW_OFF_Z"),
_flow_xy_stddev(this, "FLW_XY"),
_flow_xy_d_stddev(this, "FLW_XY_D"),
_flow_vxy_stddev(this, "FLW_VXY"),
_flow_vxy_d_stddev(this, "FLW_VXY_D"),
_flow_vxy_r_stddev(this, "FLW_VXY_R"),
//_flow_board_x_offs(NULL, "SENS_FLW_XOFF"),
//_flow_board_y_offs(NULL, "SENS_FLW_YOFF"),
_flow_min_q(this, "FLW_QMIN"),
@ -147,12 +150,6 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
_altOriginInitialized(false),
_baroAltOrigin(0),
_gpsAltOrigin(0),
_visionOrigin(),
_mocapOrigin(),
// flow integration
_flowX(0),
_flowY(0),
// status
_validXY(false),
@ -266,10 +263,6 @@ void BlockLocalPositionEstimator::update()
// _x(X_y) = 0;
// // reset Z or not? _x(X_z) = 0;
// // reset flow integral
// _flowX = 0;
// _flowY = 0;
// // we aren't moving, all velocities are zero
// _x(X_vx) = 0;
// _x(X_vy) = 0;
@ -305,17 +298,23 @@ void BlockLocalPositionEstimator::update()
}
// is xy valid?
bool xy_stddev_ok = sqrtf(math::max(_P(X_x, X_x), _P(X_y, X_y))) < _xy_pub_thresh.get();
bool vxy_stddev_ok = false;
if (math::max(_P(X_vx, X_vx), _P(X_vy, X_vy)) < _vxy_pub_thresh.get()*_vxy_pub_thresh.get()) {
vxy_stddev_ok = true;
}
if (_validXY) {
// if valid and gps has timed out, set to not valid
if (!xy_stddev_ok && !_gpsInitialized) {
if (!vxy_stddev_ok && !_gpsInitialized) {
_validXY = false;
}
} else {
if (xy_stddev_ok) {
_validXY = true;
if (vxy_stddev_ok) {
if (_flowInitialized || _gpsInitialized || _visionInitialized || _mocapInitialized) {
_validXY = true;
}
}
}
@ -380,6 +379,7 @@ void BlockLocalPositionEstimator::update()
// don't want it to take too long
if (!PX4_ISFINITE(_x(i))) {
reinit_x = true;
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] reinit x, x(%d) not finite", i);
break;
}
}
@ -684,7 +684,14 @@ void BlockLocalPositionEstimator::publishLocalPos()
_pub_lpos.get().v_z_valid = _validZ;
_pub_lpos.get().x = xLP(X_x); // north
_pub_lpos.get().y = xLP(X_y); // east
_pub_lpos.get().z = xLP(X_z); // down
if (_pub_agl_z.get()) {
_pub_lpos.get().z = -_aglLowPass.getState(); // agl
} else {
_pub_lpos.get().z = xLP(X_z); // down
}
_pub_lpos.get().vx = xLP(X_vx); // north
_pub_lpos.get().vy = xLP(X_vy); // east
_pub_lpos.get().vz = xLP(X_vz); // down

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

@ -126,7 +126,7 @@ public:
enum {U_ax = 0, U_ay, U_az, n_u};
enum {Y_baro_z = 0, n_y_baro};
enum {Y_lidar_z = 0, n_y_lidar};
enum {Y_flow_x = 0, Y_flow_y, n_y_flow};
enum {Y_flow_vx = 0, Y_flow_vy, n_y_flow};
enum {Y_sonar_z = 0, n_y_sonar};
enum {Y_gps_x = 0, Y_gps_y, Y_gps_z, Y_gps_vx, Y_gps_vy, Y_gps_vz, n_y_gps};
enum {Y_vision_x = 0, Y_vision_y, Y_vision_z, n_y_vision};
@ -244,7 +244,8 @@ private:
struct map_projection_reference_s _map_ref;
// general parameters
BlockParamFloat _xy_pub_thresh;
BlockParamInt _pub_agl_z;
BlockParamFloat _vxy_pub_thresh;
BlockParamFloat _z_pub_thresh;
// sonar parameters
@ -281,9 +282,11 @@ private:
BlockParamFloat _mocap_p_stddev;
// flow parameters
BlockParamInt _flow_gyro_comp;
BlockParamFloat _flow_z_offset;
BlockParamFloat _flow_xy_stddev;
BlockParamFloat _flow_xy_d_stddev;
BlockParamFloat _flow_vxy_stddev;
BlockParamFloat _flow_vxy_d_stddev;
BlockParamFloat _flow_vxy_r_stddev;
//BlockParamFloat _flow_board_x_offs;
//BlockParamFloat _flow_board_y_offs;
BlockParamInt _flow_min_q;
@ -351,12 +354,6 @@ private:
bool _altOriginInitialized;
float _baroAltOrigin;
float _gpsAltOrigin;
Vector3f _visionOrigin;
Vector3f _mocapOrigin;
// flow integration
float _flowX;
float _flowY;
// status
bool _validXY;

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

@ -2,6 +2,13 @@
// 16 is max name length
/**
* Publish AGL as Z
*
* @group Local Position Estimator
* @boolean
*/
PARAM_DEFINE_FLOAT(LPE_PUB_AGL_Z, 0);
/**
* Optical flow z offset from center
@ -15,7 +22,18 @@
PARAM_DEFINE_FLOAT(LPE_FLW_OFF_Z, 0.0f);
/**
* Optical flow xy standard deviation.
* Optical flow gyro compensation
*
* @group Local Position Estimator
* @unit m
* @min -1
* @max 1
* @decimal 3
*/
PARAM_DEFINE_INT32(LPE_FLW_GYRO_CMP, 1);
/**
* Optical flow xy velocity standard deviation.
*
* @group Local Position Estimator
* @unit m
@ -23,10 +41,10 @@ PARAM_DEFINE_FLOAT(LPE_FLW_OFF_Z, 0.0f);
* @max 1
* @decimal 3
*/
PARAM_DEFINE_FLOAT(LPE_FLW_XY, 0.01f);
PARAM_DEFINE_FLOAT(LPE_FLW_VXY, 0.04f);
/**
* Optical flow xy standard deviation linear factor on distance
* Optical flow xy velocity standard deviation linear factor on distance
*
* @group Local Position Estimator
* @unit m / m
@ -34,7 +52,18 @@ PARAM_DEFINE_FLOAT(LPE_FLW_XY, 0.01f);
* @max 1
* @decimal 3
*/
PARAM_DEFINE_FLOAT(LPE_FLW_XY_D, 0.01f);
PARAM_DEFINE_FLOAT(LPE_FLW_VXY_D, 0.04f);
/**
* Optical flow xy velocity standard deviation linear factor on rotation rate
*
* @group Local Position Estimator
* @unit m / m
* @min 0.01
* @max 1
* @decimal 3
*/
PARAM_DEFINE_FLOAT(LPE_FLW_VXY_R, 1.0f);
/**
* Optical flow minimum quality threshold
@ -44,7 +73,7 @@ PARAM_DEFINE_FLOAT(LPE_FLW_XY_D, 0.01f);
* @max 255
* @decimal 0
*/
PARAM_DEFINE_INT32(LPE_FLW_QMIN, 75);
PARAM_DEFINE_INT32(LPE_FLW_QMIN, 150);
/**
* Sonar z standard deviation.
@ -225,7 +254,7 @@ PARAM_DEFINE_FLOAT(LPE_EPV_MAX, 5.0f);
* @max 1
* @decimal 3
*/
PARAM_DEFINE_FLOAT(LPE_VIS_XY, 0.5f);
PARAM_DEFINE_FLOAT(LPE_VIS_XY, 0.1f);
/**
* Vision z standard deviation.
@ -328,7 +357,7 @@ PARAM_DEFINE_FLOAT(LPE_T_MAX_GRADE, 1.0f);
* @max 2
* @decimal 3
*/
PARAM_DEFINE_FLOAT(LPE_FGYRO_HP, 0.1f);
PARAM_DEFINE_FLOAT(LPE_FGYRO_HP, 0.001f);
/**
* Local origin latitude for nav w/o GPS
@ -364,15 +393,15 @@ PARAM_DEFINE_FLOAT(LPE_LON, -86.929);
PARAM_DEFINE_FLOAT(LPE_X_LP, 5.0f);
/**
* Required xy standard deviation to publish position
* Required velocity xy standard deviation to publish position
*
* @group Local Position Estimator
* @unit m
* @min 0.3
* @max 5.0
* @decimal 1
* @unit m/s
* @min 0.01
* @max 1.0
* @decimal 3
*/
PARAM_DEFINE_FLOAT(LPE_XY_PUB, 1.0f);
PARAM_DEFINE_FLOAT(LPE_VXY_PUB, 0.1f);
/**
* Required z standard deviation to publish altitude/ terrain

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

@ -29,10 +29,6 @@ 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;
}
@ -46,6 +42,11 @@ void BlockLocalPositionEstimator::flowDeinit()
int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y)
{
// check for sane pitch/roll
if (_sub_att.get().roll > 0.5f || _sub_att.get().pitch > 0.5f) {
return -1;
}
// check for agl
if (agl() < flow_min_agl) {
return -1;
@ -65,33 +66,25 @@ int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y)
float d = agl() * cosf(_sub_att.get().roll) * cosf(_sub_att.get().pitch);
// check for global accuracy
if (_gpsInitialized) {
double lat = _sub_gps.get().lat * 1.0e-7;
double lon = _sub_gps.get().lon * 1.0e-7;
float px = 0;
float py = 0;
map_projection_project(&_map_ref, lat, lon, &px, &py);
Vector2f delta(px - _flowX, py - _flowY);
if (delta.norm() > 3) {
mavlink_and_console_log_info(&mavlink_log_pub,
"[lpe] flow too far from GPS, resetting position");
_flowX = px;
_flowY = py;
return -1;
}
}
// optical flow in x, y axis
float flow_x_rad = _sub_flow.get().pixel_flow_x_integral;
float flow_y_rad = _sub_flow.get().pixel_flow_y_integral;
float dt_flow = _sub_flow.get().integration_timespan / 1.0e6f;
if (dt_flow > 0.5f || dt_flow < 1.0e-6f) {
return -1;
}
// angular rotation in x, y axis
float gyro_x_rad = _flow_gyro_x_high_pass.update(
_sub_flow.get().gyro_x_rate_integral);
float gyro_y_rad = _flow_gyro_y_high_pass.update(
_sub_flow.get().gyro_y_rate_integral);
float gyro_x_rad = 0;
float gyro_y_rad = 0;
if (_flow_gyro_comp.get()) {
gyro_x_rad = _flow_gyro_x_high_pass.update(
_sub_flow.get().gyro_x_rate_integral);
gyro_y_rad = _flow_gyro_y_high_pass.update(
_sub_flow.get().gyro_y_rate_integral);
}
//warnx("flow x: %10.4f y: %10.4f gyro_x: %10.4f gyro_y: %10.4f d: %10.4f",
//double(flow_x_rad), double(flow_y_rad), double(gyro_x_rad), double(gyro_y_rad), double(d));
@ -107,19 +100,15 @@ int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y)
Matrix3f R_nb(_sub_att.get().R);
Vector3f delta_n = R_nb * delta_b;
// flow integration
_flowX += delta_n(0);
_flowY += delta_n(1);
// measurement
y(Y_flow_x) = _flowX;
y(Y_flow_y) = _flowY;
_flowQStats.update(Scalarf(_sub_flow.get().quality));
// imporant to timestamp flow even if distance is bad
_time_last_flow = _timeStamp;
// measurement
y(Y_flow_vx) = delta_n(0) / dt_flow;
y(Y_flow_vy) = delta_n(1) / dt_flow;
_flowQStats.update(Scalarf(_sub_flow.get().quality));
return OK;
}
@ -133,15 +122,20 @@ void BlockLocalPositionEstimator::flowCorrect()
// flow measurement matrix and noise matrix
Matrix<float, n_y_flow, n_x> C;
C.setZero();
C(Y_flow_x, X_x) = 1;
C(Y_flow_y, X_y) = 1;
C(Y_flow_vx, X_vx) = 1;
C(Y_flow_vy, X_vy) = 1;
SquareMatrix<float, n_y_flow> R;
R.setZero();
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);
float rot_rate_norm = sqrtf(_sub_att.get().rollspeed * _sub_att.get().rollspeed
+ _sub_att.get().pitchspeed * _sub_att.get().pitchspeed
+ _sub_att.get().yawspeed * _sub_att.get().yawspeed);
float flow_vxy_stddev = _flow_vxy_stddev.get()
+ _flow_vxy_d_stddev.get() * d
+ _flow_vxy_r_stddev.get() * rot_rate_norm;
R(Y_flow_vx, Y_flow_vx) = flow_vxy_stddev * flow_vxy_stddev;
R(Y_flow_vy, Y_flow_vy) = R(Y_flow_vx, Y_flow_vx);
// residual
Vector<float, 2> r = y - C * _x;
@ -176,11 +170,6 @@ void BlockLocalPositionEstimator::flowCorrect()
_x += dx;
_P -= K * C * _P;
} else {
// reset flow integral to current estimate of position
// if a fault occurred
_flowX = _x(X_x);
_flowY = _x(X_y);
}
}

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

@ -49,10 +49,12 @@ int BlockLocalPositionEstimator::lidarMeasure(Vector<float, n_y_lidar> &y)
}
// update stats
_lidarStats.update(Scalarf(d + _lidar_z_offset.get()));
_lidarStats.update(Scalarf(d));
_time_last_lidar = _timeStamp;
y.setZero();
y(0) = d;
y(0) = (d + _lidar_z_offset.get()) *
cosf(_sub_att.get().roll) *
cosf(_sub_att.get().pitch);
return OK;
}
@ -63,11 +65,6 @@ void BlockLocalPositionEstimator::lidarCorrect()
if (lidarMeasure(y) != OK) { return; }
// account for leaning
y(0) = y(0) *
cosf(_sub_att.get().roll) *
cosf(_sub_att.get().pitch);
// measurement matrix
Matrix<float, n_y_lidar, n_x> C;
C.setZero();

6
src/modules/local_position_estimator/sensors/mocap.cpp
View File

@ -21,7 +21,6 @@ void BlockLocalPositionEstimator::mocapInit()
// if finished
if (_mocapStats.getCount() > REQ_MOCAP_INIT_COUNT) {
_mocapOrigin = _mocapStats.getMean();
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] mocap position init: "
"%5.2f, %5.2f, %5.2f m std %5.2f, %5.2f, %5.2f m",
double(_mocapStats.getMean()(0)),
@ -35,7 +34,7 @@ void BlockLocalPositionEstimator::mocapInit()
if (!_altOriginInitialized) {
_altOriginInitialized = true;
_altOrigin = _mocapOrigin(2);
_altOrigin = 0;
}
}
}
@ -58,9 +57,6 @@ void BlockLocalPositionEstimator::mocapCorrect()
if (mocapMeasure(y) != OK) { return; }
// make measurement relative to origin
y -= _mocapOrigin;
// mocap measurement matrix, measures position
Matrix<float, n_y_mocap, n_x> C;
C.setZero();

15
src/modules/local_position_estimator/sensors/vision.cpp
View File

@ -6,8 +6,13 @@ extern orb_advert_t mavlink_log_pub;
// required number of samples for sensor
// to initialize
static const uint32_t REQ_VISION_INIT_COUNT = 20;
static const uint32_t VISION_TIMEOUT = 500000; // 0.5 s
// this is a vision based position measurement so we assume as soon as we get one
// measurement it is initialized, we also don't want to deinitialize it because
// this will throw away a correction before it starts using the data so we
// set the timeout to 10 seconds
static const uint32_t REQ_VISION_INIT_COUNT = 1;
static const uint32_t VISION_TIMEOUT = 10000000; // 10 s
void BlockLocalPositionEstimator::visionInit()
{
@ -21,7 +26,6 @@ void BlockLocalPositionEstimator::visionInit()
// increament sums for mean
if (_visionStats.getCount() > REQ_VISION_INIT_COUNT) {
_visionOrigin = _visionStats.getMean();
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] vision position init: "
"%5.2f %5.2f %5.2f m std %5.2f %5.2f %5.2f m",
double(_visionStats.getMean()(0)),
@ -35,7 +39,7 @@ void BlockLocalPositionEstimator::visionInit()
if (!_altOriginInitialized) {
_altOriginInitialized = true;
_altOrigin = _visionOrigin(2);
_altOrigin = 0;
}
}
}
@ -58,9 +62,6 @@ void BlockLocalPositionEstimator::visionCorrect()
if (visionMeasure(y) != OK) { return; }
// make measurement relative to origin
y -= _visionOrigin;
// vision measurement matrix, measures position
Matrix<float, n_y_vision, n_x> C;
C.setZero();