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

EKF: Parameterize maximum angle for rng fusion

This commit is contained in:
CarlOlsson 2017-09-26 20:50:02 +02:00
parent 160e4d69c1
commit 0a7c3ecbc6
4 changed files with 5 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
EKF/common.h
View File

@ -266,6 +266,7 @@ struct parameters {
float max_vel_for_range_aid{1.0f}; ///< maximum ground velocity for which we allow to use the range finder as height source (if range_aid == 1) float max_vel_for_range_aid{1.0f}; ///< maximum ground velocity for which we allow to use the range finder as height source (if range_aid == 1)
int32_t range_aid{0}; ///< allow switching primary height source to range finder if certian conditions are met int32_t range_aid{0}; ///< allow switching primary height source to range finder if certian conditions are met
float range_aid_innov_gate{1.0f}; ///< gate size used for innovation consistency checks for range aid fusion float range_aid_innov_gate{1.0f}; ///< gate size used for innovation consistency checks for range aid fusion
float range_cos_max_tilt{0.7071f}; ///< cosine of the maximum tilt angle from the vertical that permits use of range finder data
// vision position fusion // vision position fusion
float ev_innov_gate{5.0f}; ///< vision estimator fusion innovation consistency gate size (STD) float ev_innov_gate{5.0f}; ///< vision estimator fusion innovation consistency gate size (STD)

2
EKF/control.cpp
View File

@ -103,7 +103,7 @@ void Ekf::controlFusionModes()
// calculate 2,2 element of rotation matrix from sensor frame to earth frame // calculate 2,2 element of rotation matrix from sensor frame to earth frame
_R_rng_to_earth_2_2 = _R_to_earth(2, 0) * _sin_tilt_rng + _R_to_earth(2, 2) * _cos_tilt_rng; _R_rng_to_earth_2_2 = _R_to_earth(2, 0) * _sin_tilt_rng + _R_to_earth(2, 2) * _cos_tilt_rng;
_range_data_ready = _range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed) _range_data_ready = _range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed)
&& (_R_rng_to_earth_2_2 > 0.7071f); && (_R_rng_to_earth_2_2 > _params.range_cos_max_tilt);
checkForStuckRange(); checkForStuckRange();

4
EKF/terrain_estimator.cpp
View File

@ -47,7 +47,7 @@ bool Ekf::initHagl()
// get most recent range measurement from buffer // get most recent range measurement from buffer
rangeSample latest_measurement = _range_buffer.get_newest(); rangeSample latest_measurement = _range_buffer.get_newest();
if ((_time_last_imu - latest_measurement.time_us) < 2e5 && _R_rng_to_earth_2_2 > 0.7071f) { if ((_time_last_imu - latest_measurement.time_us) < 2e5 && _R_rng_to_earth_2_2 > _params.range_cos_max_tilt) {
// if we have a fresh measurement, use it to initialise the terrain estimator // if we have a fresh measurement, use it to initialise the terrain estimator
_terrain_vpos = _state.pos(2) + latest_measurement.rng * _R_rng_to_earth_2_2; _terrain_vpos = _state.pos(2) + latest_measurement.rng * _R_rng_to_earth_2_2;
// initialise state variance to variance of measurement // initialise state variance to variance of measurement
@ -113,7 +113,7 @@ void Ekf::runTerrainEstimator()
void Ekf::fuseHagl() void Ekf::fuseHagl()
{ {
// If the vehicle is excessively tilted, do not try to fuse range finder observations // If the vehicle is excessively tilted, do not try to fuse range finder observations
if (_R_rng_to_earth_2_2 > 0.7071f) { if (_R_rng_to_earth_2_2 > _params.range_cos_max_tilt) {
// get a height above ground measurement from the range finder assuming a flat earth // get a height above ground measurement from the range finder assuming a flat earth
float meas_hagl = _range_sample_delayed.rng * _R_rng_to_earth_2_2; float meas_hagl = _range_sample_delayed.rng * _R_rng_to_earth_2_2;

2
EKF/vel_pos_fusion.cpp
View File

@ -179,7 +179,7 @@ void Ekf::fuseVelPosHeight()
// innovation gate size // innovation gate size
gate_size[5] = fmaxf(_params.baro_innov_gate, 1.0f); gate_size[5] = fmaxf(_params.baro_innov_gate, 1.0f);
} else if (_control_status.flags.rng_hgt && (_R_rng_to_earth_2_2 > 0.7071f)) { } else if (_control_status.flags.rng_hgt && (_R_rng_to_earth_2_2 > _params.range_cos_max_tilt)) {
fuse_map[5] = true; fuse_map[5] = true;
// use range finder with tilt correction // use range finder with tilt correction
_vel_pos_innov[5] = _state.pos(2) - (-math::max(_range_sample_delayed.rng * _R_rng_to_earth_2_2, _vel_pos_innov[5] = _state.pos(2) - (-math::max(_range_sample_delayed.rng * _R_rng_to_earth_2_2,