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
@@ -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