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

Remove filtering on external vision alignment

This commit is contained in:
kamilritz 2020-01-23 15:36:48 +01:00 committed by Lorenz Meier
parent 2fe16c4ffe
commit 4b0e7b2cff
4 changed files with 3 additions and 76 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
EKF/control.cpp
View File

@ -205,14 +205,6 @@ void Ekf::controlExternalVisionFusion()
resetVelocity();
ECL_INFO_TIMESTAMPED("commencing external vision velocity fusion");
}
if ((_params.fusion_mode & MASK_ROTATE_EV) && !(_params.fusion_mode & MASK_USE_EVYAW)
&& !_R_ev_to_ekf_initialised) {
// Reset transformation between EV and EKF navigation frames when starting fusion
resetExtVisRotMat();
_R_ev_to_ekf_initialised = true;
ECL_INFO_TIMESTAMPED("external vision aligned");
}
}
}

11
EKF/ekf.h
View File

@ -319,10 +319,7 @@ private:
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
AxisAnglef _ev_rot_vec_filt; ///< filtered rotation vector defining the rotation EV to EKF reference, initiliazied to zero rotation (rad)
Dcmf _R_ev_to_ekf; ///< transformation matrix that rotates observations from the EV to the EKF navigation frame, initialized with Identity
uint64_t _ev_rot_last_time_us{0}; ///< previous time that the calculation of the EV to EKF rotation matrix was updated (uSec)
bool _R_ev_to_ekf_initialised{0}; ///< _R_ev_to_ekf should only be initialised once in the beginning through the reset function
// 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
@ -615,15 +612,9 @@ 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
// 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
// force symmetry when the argument is true
void fixCovarianceErrors(bool force_symmetry);

55
EKF/ekf_helper.cpp
View File

@ -696,11 +696,6 @@ bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool
// record the state change
_state_reset_status.quat_change = q_error;
// reset the rotation from the EV to EKF frame of reference if it is being used
if ((_params.fusion_mode & MASK_ROTATE_EV) && !_control_status.flags.ev_yaw) {
resetExtVisRotMat();
}
if (increase_yaw_var) {
// update the yaw angle variance using the variance of the measurement
if (_control_status.flags.ev_yaw) {
@ -1657,64 +1652,18 @@ void Ekf::startMag3DFusion()
}
}
// 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 navigation frame
// update the rotation matrix which rotates EV measurements into the EKF's navigation frame
void Ekf::calcExtVisRotMat()
{
// Calculate the quaternion delta that rotates from the EV to the EKF reference frame at the EKF fusion time horizon.
const Quatf q_error((_state.quat_nominal * _ev_sample_delayed.quat.inversed()).normalized());
// convert to a delta angle and apply a spike and low pass filter
AxisAnglef rot_vec(q_error);
if (rot_vec.norm() > 1e-6f) {
// apply an input limiter to protect from spikes
const Vector3f _input_delta_vec = rot_vec - _ev_rot_vec_filt;
const float input_delta_len = _input_delta_vec.norm();
if (input_delta_len > 0.1f) {
rot_vec = _ev_rot_vec_filt + _input_delta_vec * (0.1f / input_delta_len);
}
// 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;
}
_R_ev_to_ekf = Dcmf(_ev_rot_vec_filt);
}
// 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 that rotates from the EV to the EKF reference frame at the EKF fusion time horizon.
Quatf q_error = _state.quat_nominal * _ev_sample_delayed.quat.inversed();
q_error.normalize();
AxisAnglef rot_vec(q_error);
float rot_vec_norm = rot_vec.norm();
if (rot_vec_norm > 1e-9f) {
_ev_rot_vec_filt = rot_vec;
} else {
_ev_rot_vec_filt.zero();
}
_R_ev_to_ekf = Dcmf(q_error);
}
// return the quaternions for the rotation from External Vision system reference frame to the EKF reference frame
void Ekf::get_ev2ekf_quaternion(float *quat)
{
const Quatf quat_ev2ekf(_ev_rot_vec_filt);
const Quatf quat_ev2ekf(_R_ev_to_ekf);
for (unsigned i = 0; i < 4; i++) {
quat[i] = quat_ev2ekf(i);

5
EKF/gps_yaw_fusion.cpp
View File

@ -393,11 +393,6 @@ bool Ekf::resetGpsAntYaw()
// update transformation matrix from body to world frame using the current estimate
_R_to_earth = Dcmf(_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)) {
resetExtVisRotMat();
}
// update the yaw angle variance using the variance of the measurement
increaseQuatYawErrVariance(sq(fmaxf(_params.mag_heading_noise, 1.0e-2f)));