mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-02 05:04:08 +08:00
EKF: Enable use of rotated external nav estimates
This commit is contained in:
Paul Riseborough
parent
f921b2de5c
commit
01d68ef67c
@ -174,6 +174,7 @@ struct dragSample {
|
||||
#define MASK_USE_EVPOS (1<<3) ///< set to true to use external vision NED position data
|
||||
#define MASK_USE_EVYAW (1<<4) ///< set to true to use exernal vision quaternion data for yaw
|
||||
#define MASK_USE_DRAG (1<<5) ///< set to true to use the multi-rotor drag model to estimate wind
|
||||
#define MASK_ROTATE_EV (1<<6) ///< set to true to if the EV observations are in a non NED reference frame and need to be rotated before being used
|
||||
|
||||
// Integer definitions for mag_fusion_type
|
||||
#define MAG_FUSE_TYPE_AUTO 0 ///< The selection of either heading or 3D magnetometer fusion will be automatic
|
||||
|
||||
103
EKF/control.cpp
103
EKF/control.cpp
@ -133,6 +133,7 @@ void Ekf::controlFusionModes()
|
||||
|
||||
// report dead reckoning if we are no longer fusing measurements that directly constrain velocity drift
|
||||
_is_dead_reckoning = (_time_last_imu - _time_last_pos_fuse > _params.no_aid_timeout_max)
|
||||
&& (_time_last_imu - _time_last_delpos_fuse > _params.no_aid_timeout_max)
|
||||
&& (_time_last_imu - _time_last_vel_fuse > _params.no_aid_timeout_max)
|
||||
&& (_time_last_imu - _time_last_of_fuse > _params.no_aid_timeout_max);
|
||||
|
||||
@ -143,6 +144,13 @@ void Ekf::controlExternalVisionFusion()
|
||||
// Check for new exernal vision data
|
||||
if (_ev_data_ready) {
|
||||
|
||||
// if the ev data is not in a NED reference frame, then the transformation between EV and EKF navigation frames
|
||||
// needs to be calculated and the observations rotated into the EKF frame of reference
|
||||
if ((_params.fusion_mode & MASK_ROTATE_EV) && (_params.fusion_mode & MASK_USE_EVPOS) && !(_params.fusion_mode & MASK_USE_EVYAW)) {
|
||||
// rotate EV measurements into the EKF Navigation frame
|
||||
calcExtVisRotMat();
|
||||
}
|
||||
|
||||
// external vision position aiding selection logic
|
||||
if ((_params.fusion_mode & MASK_USE_EVPOS) && !_control_status.flags.ev_pos && _control_status.flags.tilt_align && _control_status.flags.yaw_align) {
|
||||
// check for a exernal vision measurement that has fallen behind the fusion time horizon
|
||||
@ -154,12 +162,17 @@ void Ekf::controlExternalVisionFusion()
|
||||
// reset the position if we are not already aiding using GPS, else use a relative position
|
||||
// method for fusing the position data
|
||||
if (_control_status.flags.gps) {
|
||||
_hpos_odometry = true;
|
||||
_fuse_hpos_as_odom = true;
|
||||
|
||||
} else {
|
||||
resetPosition();
|
||||
resetVelocity();
|
||||
_hpos_odometry = false;
|
||||
// we cannot use an absolue position from a rotating reference frame
|
||||
if (_params.fusion_mode & MASK_ROTATE_EV) {
|
||||
_fuse_hpos_as_odom = true;
|
||||
} else {
|
||||
_fuse_hpos_as_odom = false;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
@ -243,16 +256,57 @@ void Ekf::controlExternalVisionFusion()
|
||||
_ev_sample_delayed.posNED(1) -= pos_offset_earth(1);
|
||||
_ev_sample_delayed.posNED(2) -= pos_offset_earth(2);
|
||||
|
||||
// if GPS data is being used, then use an incremental position fusion method for EV data
|
||||
if (_control_status.flags.gps) {
|
||||
_hpos_odometry = true;
|
||||
// Use an incremental position fusion method for EV data if using GPS or if the observations are not in NED
|
||||
if (_control_status.flags.gps || (_params.fusion_mode & MASK_ROTATE_EV)) {
|
||||
_fuse_hpos_as_odom = true;
|
||||
} else {
|
||||
_fuse_hpos_as_odom = false;
|
||||
}
|
||||
|
||||
if(_fuse_hpos_as_odom) {
|
||||
if(!_hpos_prev_available) {
|
||||
// no previous observation available to calculate position change
|
||||
_fuse_pos = false;
|
||||
_hpos_prev_available = true;
|
||||
|
||||
} else {
|
||||
// calculate the change in position since the last measurement
|
||||
Vector3f ev_delta_pos = _ev_sample_delayed.posNED - _pos_meas_prev;
|
||||
|
||||
// rotate measurement into body frame if required
|
||||
if (_params.fusion_mode & MASK_ROTATE_EV) {
|
||||
ev_delta_pos = _ev_rot_mat * ev_delta_pos;
|
||||
}
|
||||
|
||||
// use the change in position since the last measurement
|
||||
_vel_pos_innov[3] = _state.pos(0) - _hpos_pred_prev(0) - ev_delta_pos(0);
|
||||
_vel_pos_innov[4] = _state.pos(1) - _hpos_pred_prev(1) - ev_delta_pos(1);
|
||||
|
||||
}
|
||||
|
||||
// record observation and estimate for use next time
|
||||
_pos_meas_prev = _ev_sample_delayed.posNED;
|
||||
_hpos_pred_prev(0) = _state.pos(0);
|
||||
_hpos_pred_prev(1) = _state.pos(1);
|
||||
|
||||
} else {
|
||||
// use the absolute position
|
||||
_vel_pos_innov[3] = _state.pos(0) - _ev_sample_delayed.posNED(0);
|
||||
_vel_pos_innov[4] = _state.pos(1) - _ev_sample_delayed.posNED(1);
|
||||
}
|
||||
|
||||
// observation 1-STD error
|
||||
_posObsNoiseNE = fmaxf(_ev_sample_delayed.posErr, 0.01f);
|
||||
|
||||
// innovation gate size
|
||||
_posInnovGateNE = fmaxf(_params.ev_innov_gate, 1.0f);
|
||||
}
|
||||
|
||||
// Fuse available NED position data into the main filter
|
||||
if (_fuse_height || _fuse_pos) {
|
||||
fuseVelPosHeight();
|
||||
_fuse_pos = _fuse_height = false;
|
||||
_fuse_hpos_as_odom = false;
|
||||
|
||||
}
|
||||
|
||||
@ -423,12 +477,16 @@ void Ekf::controlGpsFusion()
|
||||
}
|
||||
|
||||
// handle the case when we now have GPS, but have not been using it for an extended period
|
||||
if (_control_status.flags.gps && !_control_status.flags.opt_flow) {
|
||||
// We are relying on GPS aiding to constrain attitude drift so after 7 seconds without aiding we need to do something
|
||||
bool do_reset = (_time_last_imu - _time_last_pos_fuse > _params.no_gps_timeout_max) && (_time_last_imu - _time_last_vel_fuse > _params.no_gps_timeout_max);
|
||||
if (_control_status.flags.gps) {
|
||||
// We are relying on aiding to constrain drift so after a specified time
|
||||
// with no aiding we need to do something
|
||||
bool do_reset = (_time_last_imu - _time_last_pos_fuse > _params.no_gps_timeout_max)
|
||||
&& (_time_last_imu - _time_last_delpos_fuse > _params.no_gps_timeout_max)
|
||||
&& (_time_last_imu - _time_last_vel_fuse > _params.no_gps_timeout_max)
|
||||
&& (_time_last_imu - _time_last_of_fuse > _params.no_gps_timeout_max);
|
||||
|
||||
// Our position measurments have been rejected for more than 14 seconds
|
||||
do_reset |= _time_last_imu - _time_last_pos_fuse > 2 * _params.no_gps_timeout_max;
|
||||
// We haven't had an absolute position fix for a longer time so need to do something
|
||||
do_reset = do_reset || (_time_last_imu - _time_last_pos_fuse > 2 * _params.no_gps_timeout_max);
|
||||
|
||||
if (do_reset) {
|
||||
// Reset states to the last GPS measurement
|
||||
@ -469,6 +527,19 @@ void Ekf::controlGpsFusion()
|
||||
_gps_sample_delayed.pos(1) -= pos_offset_earth(1);
|
||||
_gps_sample_delayed.hgt += pos_offset_earth(2);
|
||||
|
||||
// calculate observation process noise
|
||||
float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f);
|
||||
float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit);
|
||||
_posObsNoiseNE = math::constrain(_gps_sample_delayed.hacc, lower_limit, upper_limit);
|
||||
|
||||
// calculate innovations
|
||||
_vel_pos_innov[3] = _state.pos(0) - _gps_sample_delayed.pos(0);
|
||||
_vel_pos_innov[4] = _state.pos(1) - _gps_sample_delayed.pos(1);
|
||||
|
||||
// set innovation gate size
|
||||
_posInnovGateNE = fmaxf(_params.posNE_innov_gate, 1.0f);
|
||||
|
||||
|
||||
}
|
||||
|
||||
} else if (_control_status.flags.gps && (_time_last_imu - _time_last_gps > 10e6)) {
|
||||
@ -1251,6 +1322,7 @@ void Ekf::controlVelPosFusion()
|
||||
_last_known_posNE(0) = _state.pos(0);
|
||||
_last_known_posNE(1) = _state.pos(1);
|
||||
_state.vel.setZero();
|
||||
_fuse_hpos_as_odom = false;
|
||||
ECL_WARN("EKF stopping navigation");
|
||||
|
||||
}
|
||||
@ -1258,6 +1330,17 @@ void Ekf::controlVelPosFusion()
|
||||
_fuse_pos = true;
|
||||
_time_last_fake_gps = _time_last_imu;
|
||||
|
||||
if (_control_status.flags.in_air && _control_status.flags.tilt_align) {
|
||||
_posObsNoiseNE = fmaxf(_params.pos_noaid_noise, _params.gps_pos_noise);
|
||||
} else {
|
||||
_posObsNoiseNE = 0.5f;
|
||||
}
|
||||
_vel_pos_innov[3] = _state.pos(0) - _last_known_posNE(0);
|
||||
_vel_pos_innov[4] = _state.pos(1) - _last_known_posNE(1);
|
||||
|
||||
// glitch protection is not required so set gate to a large value
|
||||
_posInnovGateNE = 100.0f;
|
||||
|
||||
}
|
||||
|
||||
// Fuse available NED velocity and position data into the main filter
|
||||
|
||||
@ -273,6 +273,11 @@ bool Ekf::initialiseFilter()
|
||||
// calculate the initial magnetic field and yaw alignment
|
||||
_control_status.flags.yaw_align = resetMagHeading(mag_init);
|
||||
|
||||
// initialise the rotation from EV to EKF navigation frame if required
|
||||
if ((_params.fusion_mode & MASK_ROTATE_EV) && (_params.fusion_mode & MASK_USE_EVPOS) && !(_params.fusion_mode & MASK_USE_EVYAW)) {
|
||||
resetExtVisRotMat();
|
||||
}
|
||||
|
||||
if (_control_status.flags.rng_hgt) {
|
||||
// if we are using the range finder as the primary source, then calculate the baro height at origin so we can use baro as a backup
|
||||
// so it can be used as a backup ad set the initial height using the range finder
|
||||
@ -297,6 +302,7 @@ bool Ekf::initialiseFilter()
|
||||
// reset the essential fusion timeout counters
|
||||
_time_last_hgt_fuse = _time_last_imu;
|
||||
_time_last_pos_fuse = _time_last_imu;
|
||||
_time_last_delpos_fuse = _time_last_imu;
|
||||
_time_last_vel_fuse = _time_last_imu;
|
||||
_time_last_hagl_fuse = _time_last_imu;
|
||||
_time_last_of_fuse = _time_last_imu;
|
||||
|
||||
23
EKF/ekf.h
23
EKF/ekf.h
@ -226,6 +226,9 @@ public:
|
||||
// return a bitmask integer that describes which state estimates can be used for flight control
|
||||
void get_ekf_soln_status(uint16_t *status);
|
||||
|
||||
// return the quaternion defining the rotation from the EKF to the External Vision reference frame
|
||||
void get_ekf2ev_quaternion(float *quat);
|
||||
|
||||
private:
|
||||
|
||||
static constexpr uint8_t _k_num_states{24}; ///< number of EKF states
|
||||
@ -258,11 +261,17 @@ private:
|
||||
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
|
||||
|
||||
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
|
||||
|
||||
// variables used when position data is being fused using a relative position odometry model
|
||||
bool _hpos_odometry{false}; ///< true when the NE position data is being fused using an odometry assumption
|
||||
Vector2f _hpos_meas_prev; ///< previous value of NE position measurement fused using odometry assumption (m)
|
||||
bool _fuse_hpos_as_odom{false}; ///< true when the NE position data is being fused using an odometry assumption
|
||||
Vector3f _pos_meas_prev; ///< previous value of NED position measurement fused using odometry assumption (m)
|
||||
Vector2f _hpos_pred_prev; ///< previous value of NE position state used by odometry fusion (m)
|
||||
bool _hpos_prev_available{false}; ///< true when previous values of the estimate and measurement are available for use
|
||||
Vector3f _ev_rot_vec_filt; ///< filtered rotation vector defining the rotation from EKF to EV reference (rad)
|
||||
Dcmf _ev_rot_mat; ///< transformation matrix that rotates observations from the EV to the EKF navigation frame
|
||||
uint64_t _ev_rot_last_time_us{0}; ///< previous time that the calculation of the ekf to ev rotation matrix was updated (uSec)
|
||||
|
||||
// booleans true when fresh sensor data is available at the fusion time horizon
|
||||
bool _gps_data_ready{false}; ///< true when new GPS data has fallen behind the fusion time horizon and is available to be fused
|
||||
@ -276,6 +285,7 @@ private:
|
||||
uint64_t _time_last_fake_gps{0}; ///< last time we faked GPS position measurements to constrain tilt errors during operation without external aiding (uSec)
|
||||
|
||||
uint64_t _time_last_pos_fuse{0}; ///< time the last fusion of horizontal position measurements was performed (uSec)
|
||||
uint64_t _time_last_delpos_fuse{0}; ///< time the last fusion of incremental horizontal position measurements was performed (uSec)
|
||||
uint64_t _time_last_vel_fuse{0}; ///< time the last fusion of velocity measurements was performed (uSec)
|
||||
uint64_t _time_last_hgt_fuse{0}; ///< time the last fusion of height measurements was performed (uSec)
|
||||
uint64_t _time_last_of_fuse{0}; ///< time the last fusion of optical flow measurements were performed (uSec)
|
||||
@ -487,6 +497,15 @@ private:
|
||||
// modify output filter to match the the EKF state at the fusion time horizon
|
||||
void alignOutputFilter();
|
||||
|
||||
// update the estimated angular misalignment vector between the EV naigration frame and the EKF navigation frame
|
||||
// and update the rotation matrix which transforms EV navigation frame measurements into NED
|
||||
void calcExtVisRotMat();
|
||||
|
||||
|
||||
// reset the estimated angular misalignment vector between the EV naigration frame and the EKF navigation frame
|
||||
// and reset the rotation matrix which transforms EV navigation frame measurements into NED
|
||||
void resetExtVisRotMat();
|
||||
|
||||
// limit the diagonal of the covariance matrix
|
||||
void fixCovarianceErrors();
|
||||
|
||||
|
||||
@ -607,6 +607,11 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
|
||||
// update transformation matrix from body to world frame using the current estimate
|
||||
_R_to_earth = quat_to_invrotmat(_state.quat_nominal);
|
||||
|
||||
// reset the rotation from the EV to EKF frame of reference if it is being used
|
||||
if ((_params.fusion_mode & MASK_ROTATE_EV) && (_params.fusion_mode & MASK_USE_EVPOS) && !(_params.fusion_mode & MASK_USE_EVYAW)) {
|
||||
resetExtVisRotMat();
|
||||
}
|
||||
|
||||
// update the yaw angle variance using the variance of the measurement
|
||||
if (_params.fusion_mode & MASK_USE_EVYAW) {
|
||||
// using error estimate from external vision data
|
||||
@ -1182,7 +1187,9 @@ bool Ekf::global_position_is_valid()
|
||||
bool Ekf::inertial_dead_reckoning()
|
||||
{
|
||||
bool velPosAiding = (_control_status.flags.gps || _control_status.flags.ev_pos)
|
||||
&& ((_time_last_imu - _time_last_pos_fuse <= _params.no_aid_timeout_max) || (_time_last_imu - _time_last_vel_fuse <= _params.no_aid_timeout_max));
|
||||
&& ((_time_last_imu - _time_last_pos_fuse <= _params.no_aid_timeout_max)
|
||||
|| (_time_last_imu - _time_last_vel_fuse <= _params.no_aid_timeout_max)
|
||||
|| (_time_last_imu - _time_last_delpos_fuse <= _params.no_aid_timeout_max));
|
||||
bool optFlowAiding = _control_status.flags.opt_flow && (_time_last_imu - _time_last_of_fuse <= _params.no_aid_timeout_max);
|
||||
bool airDataAiding = _control_status.flags.wind && (_time_last_imu - _time_last_arsp_fuse <= _params.no_aid_timeout_max) && (_time_last_imu - _time_last_beta_fuse <= _params.no_aid_timeout_max);
|
||||
|
||||
@ -1422,3 +1429,111 @@ void Ekf::setControlEVHeight()
|
||||
_control_status.flags.gps_hgt = false;
|
||||
_control_status.flags.rng_hgt = false;
|
||||
}
|
||||
|
||||
// update the estimated misalignment between the EV navigation frame and the EKF navigation frame
|
||||
// and calculate a rotation matrix which rotates EV measurements into the EKF's navigatin frame
|
||||
void Ekf::calcExtVisRotMat()
|
||||
{
|
||||
// calculate the quaternion delta between the EKF and EV reference frames at the EKF fusion time horizon
|
||||
Quatf quat_inv = _ev_sample_delayed.quat.inversed();
|
||||
Quatf q_error = quat_inv * _state.quat_nominal;
|
||||
q_error.normalize();
|
||||
|
||||
// convert to a delta angle and apply a spike and low pass filter
|
||||
Vector3f rot_vec;
|
||||
float delta;
|
||||
float scaler;
|
||||
if (q_error(0) >= 0.0f) {
|
||||
delta = 2 * acosf(q_error(0));
|
||||
if (delta > 1e-6f) {
|
||||
scaler = 1.0f / sinf(delta/2);
|
||||
} else {
|
||||
// The rotation is too small to calculate a vector accurately
|
||||
// Make the rotation vector zero
|
||||
scaler = 0.0f;
|
||||
}
|
||||
} else {
|
||||
delta = 2 * acosf(-q_error(0));
|
||||
if (delta > 1e-6f) {
|
||||
scaler = -1.0f / sinf(delta/2);
|
||||
} else {
|
||||
// The rotation is too small to calculate a vector accurately
|
||||
// Make the rotation vector zero
|
||||
scaler = 0.0f;
|
||||
}
|
||||
}
|
||||
rot_vec(0) = q_error(2) * scaler;
|
||||
rot_vec(1) = q_error(3) * scaler;
|
||||
rot_vec(2) = q_error(4) * scaler;
|
||||
|
||||
float rot_vec_norm = rot_vec.norm();
|
||||
if (rot_vec_norm > 1e-6f) {
|
||||
// ensure magnitude of rotation matches the quaternion
|
||||
rot_vec = rot_vec * (delta / rot_vec_norm);
|
||||
|
||||
// apply an input limiter to protect from spikes
|
||||
Vector3f _input_delta_vec = rot_vec - _ev_rot_vec_filt;
|
||||
float input_delta_mag = _input_delta_vec.norm();
|
||||
if (input_delta_mag > 0.1f) {
|
||||
rot_vec = _ev_rot_vec_filt + rot_vec * (0.1f / input_delta_mag);
|
||||
}
|
||||
|
||||
// Apply a first order IIR low pass filter
|
||||
const float omega_lpf_us = 0.2e-6f; // cutoff frequency in rad/uSec
|
||||
float alpha = math::constrain(omega_lpf_us * (float)(_time_last_imu - _ev_rot_last_time_us) , 0.0f , 1.0f);
|
||||
_ev_rot_last_time_us = _time_last_imu;
|
||||
_ev_rot_vec_filt = _ev_rot_vec_filt * (1.0f - alpha) + rot_vec * alpha;
|
||||
|
||||
}
|
||||
|
||||
// convert filtered vector to a quaternion and then to a rotation matrix
|
||||
q_error.from_axis_angle(_ev_rot_vec_filt);
|
||||
_ev_rot_mat = quat_to_invrotmat(q_error);
|
||||
|
||||
}
|
||||
|
||||
// reset the estimated misalignment between the EV navigation frame and the EKF navigation frame
|
||||
// and update the rotation matrix which rotates EV measurements into the EKF's navigation frame
|
||||
void Ekf::resetExtVisRotMat()
|
||||
{
|
||||
// calculate the quaternion delta between the EKF and EV reference frames at the EKF fusion time horizon
|
||||
Quatf quat_inv = _ev_sample_delayed.quat.inversed();
|
||||
Quatf q_error = quat_inv * _state.quat_nominal;
|
||||
q_error.normalize();
|
||||
|
||||
// convert to a delta angle and reset
|
||||
Vector3f rot_vec;
|
||||
float delta;
|
||||
if (q_error(0) >= 0.0f) {
|
||||
delta = 2 * acosf(q_error(0));
|
||||
rot_vec(0) = q_error(1) / sinf(delta/2);
|
||||
rot_vec(1) = q_error(2) / sinf(delta/2);
|
||||
rot_vec(2) = q_error(3) / sinf(delta/2);
|
||||
} else {
|
||||
delta = 2 * acosf(-q_error(1));
|
||||
rot_vec(0) = -q_error(2) / sinf(delta/2);
|
||||
rot_vec(1) = -q_error(3) / sinf(delta/2);
|
||||
rot_vec(2) = -q_error(4) / sinf(delta/2);
|
||||
}
|
||||
|
||||
float rot_vec_norm = rot_vec.norm();
|
||||
if (rot_vec_norm > 1e-9f) {
|
||||
_ev_rot_vec_filt = rot_vec * delta / rot_vec_norm;
|
||||
} else {
|
||||
_ev_rot_vec_filt.zero();
|
||||
}
|
||||
|
||||
// reset the rotation matrix
|
||||
_ev_rot_mat = quat_to_invrotmat(q_error);
|
||||
}
|
||||
|
||||
// return the quaternions for the rotation from the EKF to the External Vision system frame of reference
|
||||
void Ekf::get_ekf2ev_quaternion(float *quat)
|
||||
{
|
||||
Quatf quat_ekf2ev;
|
||||
quat_ekf2ev.from_axis_angle(_ev_rot_vec_filt);
|
||||
for (unsigned i = 0; i < 4; i++) {
|
||||
quat[i] = quat_ekf2ev(i);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -236,6 +236,9 @@ public:
|
||||
}
|
||||
}
|
||||
|
||||
// return the quaternion defining the rotation from the EKF to the External Vision reference frame
|
||||
virtual void get_ekf2ev_quaternion(float *quat) = 0;
|
||||
|
||||
// get the velocity of the body frame origin in local NED earth frame
|
||||
void get_velocity(float *vel)
|
||||
{
|
||||
|
||||
@ -82,76 +82,12 @@ void Ekf::fuseVelPosHeight()
|
||||
}
|
||||
|
||||
if (_fuse_pos) {
|
||||
// enable fusion for the NE position axes
|
||||
fuse_map[3] = fuse_map[4] = true;
|
||||
|
||||
// Calculate innovations and observation variance depending on type of observations
|
||||
// being used
|
||||
if (_control_status.flags.gps) {
|
||||
// we are using GPS measurements
|
||||
float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f);
|
||||
float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit);
|
||||
R[3] = math::constrain(_gps_sample_delayed.hacc, lower_limit, upper_limit);
|
||||
_vel_pos_innov[3] = _state.pos(0) - _gps_sample_delayed.pos(0);
|
||||
_vel_pos_innov[4] = _state.pos(1) - _gps_sample_delayed.pos(1);
|
||||
|
||||
// innovation gate size
|
||||
gate_size[3] = fmaxf(_params.posNE_innov_gate, 1.0f);
|
||||
|
||||
|
||||
} else if (_control_status.flags.ev_pos) {
|
||||
// calculate innovations
|
||||
if(_hpos_odometry) {
|
||||
if(!_hpos_prev_available) {
|
||||
// no previous observation available to calculate position change
|
||||
fuse_map[3] = fuse_map[4] = false;
|
||||
_hpos_prev_available = true;
|
||||
|
||||
} else {
|
||||
// use the change in position since the last measurement
|
||||
_vel_pos_innov[3] = _state.pos(0) - _hpos_pred_prev(0) - _ev_sample_delayed.posNED(0) + _hpos_meas_prev(0);
|
||||
_vel_pos_innov[4] = _state.pos(1) - _hpos_pred_prev(1) - _ev_sample_delayed.posNED(1) + _hpos_meas_prev(1);
|
||||
|
||||
}
|
||||
|
||||
// record observation and estimate for use next time
|
||||
_hpos_meas_prev(0) = _ev_sample_delayed.posNED(0);
|
||||
_hpos_meas_prev(1) = _ev_sample_delayed.posNED(1);
|
||||
_hpos_pred_prev(0) = _state.pos(0);
|
||||
_hpos_pred_prev(1) = _state.pos(1);
|
||||
|
||||
} else {
|
||||
// use the absolute position
|
||||
_vel_pos_innov[3] = _state.pos(0) - _ev_sample_delayed.posNED(0);
|
||||
_vel_pos_innov[4] = _state.pos(1) - _ev_sample_delayed.posNED(1);
|
||||
}
|
||||
|
||||
// observation 1-STD error
|
||||
R[3] = fmaxf(_ev_sample_delayed.posErr, 0.01f);
|
||||
|
||||
// innovation gate size
|
||||
gate_size[3] = fmaxf(_params.ev_innov_gate, 1.0f);
|
||||
|
||||
} else {
|
||||
// No observations - use a static position to constrain drift
|
||||
if (_control_status.flags.in_air && _control_status.flags.tilt_align) {
|
||||
R[3] = fmaxf(_params.pos_noaid_noise, _params.gps_pos_noise);
|
||||
} else {
|
||||
R[3] = 0.5f;
|
||||
}
|
||||
_vel_pos_innov[3] = _state.pos(0) - _last_known_posNE(0);
|
||||
_vel_pos_innov[4] = _state.pos(1) - _last_known_posNE(1);
|
||||
|
||||
// glitch protection is not required so set gate to a large value
|
||||
gate_size[3] = 100.0f;
|
||||
|
||||
}
|
||||
|
||||
// convert North position noise to variance
|
||||
R[3] = R[3] * R[3];
|
||||
|
||||
// copy North axis values to East axis
|
||||
R[4] = R[3];
|
||||
gate_size[4] = gate_size[3];
|
||||
// Set observation noise variance and innovation consistency check gate size for the NE position observations
|
||||
R[4] = R[3] = sq(_posObsNoiseNE);
|
||||
gate_size[4] = gate_size[3] = _posInnovGateNE;
|
||||
|
||||
}
|
||||
|
||||
@ -234,7 +170,11 @@ void Ekf::fuseVelPosHeight()
|
||||
|
||||
// record the successful position fusion event
|
||||
if (pos_check_pass && _fuse_pos) {
|
||||
_time_last_pos_fuse = _time_last_imu;
|
||||
if (!_fuse_hpos_as_odom) {
|
||||
_time_last_pos_fuse = _time_last_imu;
|
||||
} else {
|
||||
_time_last_delpos_fuse = _time_last_imu;
|
||||
}
|
||||
_innov_check_fail_status.flags.reject_pos_NE = false;
|
||||
} else if (!pos_check_pass) {
|
||||
_innov_check_fail_status.flags.reject_pos_NE = true;
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user