mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-11-22 03:43:19 +08:00
Improvements to lpe for flow and gps. (#5401)
This commit is contained in:
@@ -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
|
||||
|
||||
|
||||
Reference in New Issue
Block a user