fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-01 09:11:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

LPE vision estimation fixes. (#5505)

Browse Source
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
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/local_position_estimator/sensors/flow.cpp
+35 -46
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);
}
}
src/modules/local_position_estimator/sensors/lidar.cpp
+4 -7
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();
src/modules/local_position_estimator/sensors/mocap.cpp
+1 -5
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();
src/modules/local_position_estimator/sensors/vision.cpp
+8 -7
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();