fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

LPE flow improvements.

Browse Source
This commit is contained in:
James Goppert 2016-10-12 01:46:54 -04:00 committed by James Goppert
parent db44129ec0
commit cf658638f4
5 changed files with 52 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -82,9 +82,7 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
_mocap_p_stddev(this, "VIC_P"),
_flow_gyro_comp(this, "FLW_GYRO_CMP"),
_flow_z_offset(this, "FLW_OFF_Z"),
_flow_vxy_stddev(this, "FLW_VXY"),
_flow_vxy_d_stddev(this, "FLW_VXY_D"),
_flow_vxy_r_stddev(this, "FLW_VXY_R"),
_flow_scale(this, "FLW_SCALE"),
//_flow_board_x_offs(NULL, "SENS_FLW_XOFF"),
//_flow_board_y_offs(NULL, "SENS_FLW_YOFF"),
_flow_min_q(this, "FLW_QMIN"),

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

@ -286,9 +286,7 @@ private:
// flow parameters
BlockParamInt _flow_gyro_comp;
BlockParamFloat _flow_z_offset;
BlockParamFloat _flow_vxy_stddev;
BlockParamFloat _flow_vxy_d_stddev;
BlockParamFloat _flow_vxy_r_stddev;
BlockParamFloat _flow_scale;
//BlockParamFloat _flow_board_x_offs;
//BlockParamFloat _flow_board_y_offs;
BlockParamInt _flow_min_q;

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

@ -21,6 +21,17 @@ PARAM_DEFINE_FLOAT(LPE_PUB_AGL_Z, 0);
*/
PARAM_DEFINE_FLOAT(LPE_FLW_OFF_Z, 0.0f);
/**
* Optical flow scale
*
* @group Local Position Estimator
* @unit m
* @min 0.1
* @max 10.0
* @decimal 3
*/
PARAM_DEFINE_FLOAT(LPE_FLW_SCALE, 1.3f);
/**
* Optical flow gyro compensation
*
@ -32,39 +43,6 @@ PARAM_DEFINE_FLOAT(LPE_FLW_OFF_Z, 0.0f);
*/
PARAM_DEFINE_INT32(LPE_FLW_GYRO_CMP, 1);
/**
* Optical flow xy velocity standard deviation.
*
* @group Local Position Estimator
* @unit m
* @min 0.01
* @max 1
* @decimal 3
*/
PARAM_DEFINE_FLOAT(LPE_FLW_VXY, 0.04f);
/**
* Optical flow xy velocity 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_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
*
@ -132,7 +110,7 @@ PARAM_DEFINE_FLOAT(LPE_LDR_OFF_Z, 0.00f);
* @max 2
* @decimal 4
*/
PARAM_DEFINE_FLOAT(LPE_ACC_XY, 0.0015f);
PARAM_DEFINE_FLOAT(LPE_ACC_XY, 0.012f);
/**
* Accelerometer z noise density
@ -145,7 +123,7 @@ PARAM_DEFINE_FLOAT(LPE_ACC_XY, 0.0015f);
* @max 2
* @decimal 4
*/
PARAM_DEFINE_FLOAT(LPE_ACC_Z, 0.0015f);
PARAM_DEFINE_FLOAT(LPE_ACC_Z, 0.02f);
/**
* Barometric presssure altitude z standard deviation.
@ -412,7 +390,7 @@ PARAM_DEFINE_FLOAT(LPE_X_LP, 5.0f);
* @max 1.0
* @decimal 3
*/
PARAM_DEFINE_FLOAT(LPE_VXY_PUB, 0.1f);
PARAM_DEFINE_FLOAT(LPE_VXY_PUB, 0.3f);
/**
* Required z standard deviation to publish altitude/ terrain

BIN
src/modules/local_position_estimator/sensors/Flow+Noise+Modelling.pdf Normal file
View File

Binary file not shown.

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

@ -8,7 +8,7 @@ extern orb_advert_t mavlink_log_pub;
// required number of samples for sensor
// to initialize
static const uint32_t REQ_FLOW_INIT_COUNT = 10;
static const uint32_t FLOW_TIMEOUT = 1000000; // 1 s
static const uint32_t FLOW_TIMEOUT = 1000000; // 1 s
// minimum flow altitude
static const float flow_min_agl = 0.05;
@ -67,8 +67,9 @@ int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y)
float d = agl() * cosf(_sub_att.get().roll) * cosf(_sub_att.get().pitch);
// 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;
// TODO consider making flow scale a states of the kalman filter
float flow_x_rad = _sub_flow.get().pixel_flow_x_integral * _flow_scale.get();
float flow_y_rad = _sub_flow.get().pixel_flow_y_integral * _flow_scale.get();
float dt_flow = _sub_flow.get().integration_timespan / 1.0e6f;
if (dt_flow > 0.5f || dt_flow < 1.0e-6f) {
@ -127,13 +128,38 @@ void BlockLocalPositionEstimator::flowCorrect()
SquareMatrix<float, n_y_flow> R;
R.setZero();
float d = agl() * cosf(_sub_att.get().roll) * cosf(_sub_att.get().pitch);
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;
// polynomial noise model, found using least squares fit
// h, h**2, v, v*h, v*h**2
const float p[5] = {0.04005232f, -0.00656446f, -0.26265873f, 0.13686658f, -0.00397357f};
// prevent extrapolation past end of polynomial fit by bounding independent variables
float h = agl();
float v = y.norm();
const float h_min = 2.0f;
const float h_max = 8.0f;
const float v_min = 0.5f;
const float v_max = 1.0f;
if (h > h_max) {
h = h_max;
}
if (h < h_min) {
h = h_min;
}
if (v > v_max) {
v = v_max;
}
if (v < v_min) {
v = v_min;
}
// compute polynomial value
float flow_vxy_stddev = p[0] * h + p[1] * h * h + p[2] * v + p[3] * v * h + p[4] * v * h * h;
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);