fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-13 23:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: enable separate monitoring of aux velocity innovations

Browse Source
This commit is contained in:
Paul Riseborough
2017-11-21 11:18:09 +11:00
parent 2a57fd858d
commit b0ad8269a5
5 changed files with 38 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+4 -4
View File
@@ -1368,10 +1368,10 @@ void Ekf::controlAuxVelFusion()
bool primary_aiding = _control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow;
if (data_ready && primary_aiding) {
_fuse_vert_vel = _fuse_pos = _fuse_height = false;
_fuse_hor_vel = true;
_vel_pos_innov[0] = _state.vel(0) - _auxvel_sample_delayed.velNE(0);
_vel_pos_innov[1] = _state.vel(1) - _auxvel_sample_delayed.velNE(1);
_fuse_hor_vel = _fuse_vert_vel = _fuse_pos = _fuse_height = false;
_fuse_hor_vel_aux = true;
_aux_vel_innov[0] = _state.vel(0) - _auxvel_sample_delayed.velNE(0);
_aux_vel_innov[1] = _state.vel(1) - _auxvel_sample_delayed.velNE(1);
_velObsVarNE = _auxvel_sample_delayed.velVarNE;
_hvelInnovGate = _params.auxvel_gate;
fuseVelPosHeight();
EKF/ekf.h
+6 -1
View File
@@ -60,6 +60,9 @@ public:
// 0-2 vel, 3-5 pos
void get_vel_pos_innov(float vel_pos_innov[6]);
// gets the innovations for of the NE auxiliary velocity measurement
void get_aux_vel_innov(float aux_vel_innov[2]);
// gets the innovations of the earth magnetic field measurements
void get_mag_innov(float mag_innov[3]);
@@ -260,6 +263,7 @@ private:
bool _fuse_pos{false}; ///< true when gps position data should be fused
bool _fuse_hor_vel{false}; ///< true when gps horizontal velocity measurement should be fused
bool _fuse_vert_vel{false}; ///< true when gps vertical velocity measurement should be fused
bool _fuse_hor_vel_aux{false}; ///< true when auxiliary horizontal velocity measurement should be fused
float _posObsNoiseNE; ///< 1-STD observtion noise used for the fusion of NE position data (m)
float _posInnovGateNE; ///< Number of standard deviations used for the NE position fusion innovation consistency check
@@ -319,8 +323,9 @@ private:
float P[_k_num_states][_k_num_states] {}; ///< state covariance matrix
float _vel_pos_innov[6] {}; ///< NED velocity and position innovations: 0-2 vel (m/sec), 3-5 pos (m**2)
float _vel_pos_innov[6] {}; ///< NED velocity and position innovations: 0-2 vel (m/sec), 3-5 pos (m)
float _vel_pos_innov_var[6] {}; ///< NED velocity and position innovation variances: 0-2 vel ((m/sec)**2), 3-5 pos (m**2)
float _aux_vel_innov[2] {}; ///< NE auxiliary velocity innovations: (m/sec)
float _mag_innov[3] {}; ///< earth magnetic field innovations (Gauss)
float _mag_innov_var[3] {}; ///< earth magnetic field innovation variance (Gauss**2)
EKF/ekf_helper.cpp
+6
View File
@@ -756,6 +756,12 @@ void Ekf::get_vel_pos_innov(float vel_pos_innov[6])
memcpy(vel_pos_innov, _vel_pos_innov, sizeof(float) * 6);
}
// gets the innovations of the earth magnetic field measurements
void Ekf::get_aux_vel_innov(float aux_vel_innov[2])
{
memcpy(aux_vel_innov, _aux_vel_innov, sizeof(float) * 2);
}
// writes the innovations of the earth magnetic field measurements
void Ekf::get_mag_innov(float mag_innov[3])
{
EKF/estimator_interface.h
+3
View File
@@ -61,6 +61,9 @@ public:
// 0-2 vel, 3-5 pos
virtual void get_vel_pos_innov(float vel_pos_innov[6]) = 0;
// gets the innovations for of the NE auxiliary velocity measurement
virtual void get_aux_vel_innov(float aux_vel_innov[2]) = 0;
// gets the innovations of the earth magnetic field measurements
virtual void get_mag_innov(float mag_innov[3]) = 0;
EKF/vel_pos_fusion.cpp
+19 -10
View File
@@ -51,22 +51,31 @@ void Ekf::fuseVelPosHeight()
float R[6] = {}; // observation variances for [VN,VE,VD,PN,PE,PD]
float gate_size[6] = {}; // innovation consistency check gate sizes for [VN,VE,VD,PN,PE,PD] observations
float Kfusion[24] = {}; // Kalman gain vector for any single observation - sequential fusion is used
float innovation[6]; // local copy of innovations for [VN,VE,VD,PN,PE,PD]
memcpy(innovation, _vel_pos_innov, sizeof(_vel_pos_innov));
// calculate innovations, innovations gate sizes and observation variances
if (_fuse_hor_vel) {
if (_fuse_hor_vel || _fuse_hor_vel_aux) {
// enable fusion for NE velocity axes
fuse_map[0] = fuse_map[1] = true;
// handle special case where we are getting velocity observations from an auxiliary source
if (!_fuse_hor_vel) {
innovation[0] = _aux_vel_innov[0];
innovation[1] = _aux_vel_innov[1];
}
// Set observation noise variance and innovation consistency check gate size for the NE position observations
R[0] = _velObsVarNE(0);
R[1] = _velObsVarNE(1);
gate_size[1] = gate_size[0] = _hvelInnovGate;
}
if (_fuse_vert_vel) {
fuse_map[2] = true;
// vertical velocity innovation
_vel_pos_innov[2] = _state.vel(2) - _gps_sample_delayed.vel(2);
innovation[2] = _state.vel(2) - _gps_sample_delayed.vel(2);
// observation variance - use receiver reported accuracy with parameter setting the minimum value
R[2] = fmaxf(_params.gps_vel_noise, 0.01f);
// use scaled horizontal speed accuracy assuming typical ratio of VDOP/HDOP
@@ -90,7 +99,7 @@ void Ekf::fuseVelPosHeight()
if (_control_status.flags.baro_hgt) {
fuse_map[5] = true;
// vertical position innovation - baro measurement has opposite sign to earth z axis
_vel_pos_innov[5] = _state.pos(2) + _baro_sample_delayed.hgt - _baro_hgt_offset - _hgt_sensor_offset;
innovation[5] = _state.pos(2) + _baro_sample_delayed.hgt - _baro_hgt_offset - _hgt_sensor_offset;
// observation variance - user parameter defined
R[5] = fmaxf(_params.baro_noise, 0.01f);
R[5] = R[5] * R[5];
@@ -114,7 +123,7 @@ void Ekf::fuseVelPosHeight()
} else if (_control_status.flags.gps_hgt) {
fuse_map[5] = true;
// vertical position innovation - gps measurement has opposite sign to earth z axis
_vel_pos_innov[5] = _state.pos(2) + _gps_sample_delayed.hgt - _gps_alt_ref - _hgt_sensor_offset;
innovation[5] = _state.pos(2) + _gps_sample_delayed.hgt - _gps_alt_ref - _hgt_sensor_offset;
// observation variance - receiver defined and parameter limited
// use scaled horizontal position accuracy assuming typical ratio of VDOP/HDOP
float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f);
@@ -127,7 +136,7 @@ void Ekf::fuseVelPosHeight()
} else if (_control_status.flags.rng_hgt && (_R_rng_to_earth_2_2 > _params.range_cos_max_tilt)) {
fuse_map[5] = true;
// 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,
innovation[5] = _state.pos(2) - (-math::max(_range_sample_delayed.rng * _R_rng_to_earth_2_2,
_params.rng_gnd_clearance)) - _hgt_sensor_offset;
// observation variance - user parameter defined
R[5] = fmaxf((sq(_params.range_noise) + sq(_params.range_noise_scaler * _range_sample_delayed.rng)) * sq(_R_rng_to_earth_2_2), 0.01f);
@@ -136,7 +145,7 @@ void Ekf::fuseVelPosHeight()
} else if (_control_status.flags.ev_hgt) {
fuse_map[5] = true;
// calculate the innovation assuming the external vision observaton is in local NED frame
_vel_pos_innov[5] = _state.pos(2) - _ev_sample_delayed.posNED(2);
innovation[5] = _state.pos(2) - _ev_sample_delayed.posNED(2);
// observation variance - defined externally
R[5] = fmaxf(_ev_sample_delayed.posErr, 0.01f);
R[5] = R[5] * R[5];
@@ -153,7 +162,7 @@ void Ekf::fuseVelPosHeight()
unsigned state_index = obs_index + 4; // we start with vx and this is the 4. state
_vel_pos_innov_var[obs_index] = P[state_index][state_index] + R[obs_index];
// Compute the ratio of innovation to gate size
_vel_pos_test_ratio[obs_index] = sq(_vel_pos_innov[obs_index]) / (sq(gate_size[obs_index]) *
_vel_pos_test_ratio[obs_index] = sq(innovation[obs_index]) / (sq(gate_size[obs_index]) *
_vel_pos_innov_var[obs_index]);
}
}
@@ -170,13 +179,13 @@ void Ekf::fuseVelPosHeight()
innov_check_pass_map[5] = (_vel_pos_test_ratio[5] <= 1.0f) || !_control_status.flags.tilt_align;
// record the successful velocity fusion event
if (vel_check_pass && _fuse_hor_vel) {
if ((_fuse_hor_vel || _fuse_hor_vel_aux) && vel_check_pass) {
_time_last_vel_fuse = _time_last_imu;
_innov_check_fail_status.flags.reject_vel_NED = false;
} else if (!vel_check_pass) {
_innov_check_fail_status.flags.reject_vel_NED = true;
}
_fuse_hor_vel = false;
_fuse_hor_vel = _fuse_hor_vel_aux = false;
// record the successful position fusion event
if (pos_check_pass && _fuse_pos) {
@@ -278,7 +287,7 @@ void Ekf::fuseVelPosHeight()
fixCovarianceErrors();
// apply the state corrections
fuse(Kfusion, _vel_pos_innov[obs_index]);
fuse(Kfusion, innovation[obs_index]);
}
}
}