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

EKF2: vision attitude error filter (#21791) 

* ekf2-ev: filter q_error for frame correction
* ekf2: filter EV attitude error centrally
This commit is contained in:
Mathieu Bresciani 2023-07-03 17:13:59 +02:00 committed by GitHub
parent 24665f10f2
commit 288e3ae74a
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
5 changed files with 24 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/modules/ekf2/EKF/ekf.h
View File

@ -1003,6 +1003,7 @@ private:
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
// control fusion of external vision observations
void controlExternalVisionFusion();
void updateEvAttitudeErrorFilter(extVisionSample &ev_sample, bool ev_reset);
void controlEvHeightFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s &aid_src);
void controlEvPosFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source2d_s &aid_src);
void controlEvVelFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source3d_s &aid_src);
@ -1160,6 +1161,8 @@ private:
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
HeightBiasEstimator _ev_hgt_b_est{HeightSensor::EV, _height_sensor_ref};
PositionBiasEstimator _ev_pos_b_est{static_cast<uint8_t>(PositionSensor::EV), _position_sensor_ref};
AlphaFilter<AxisAnglef> _ev_q_error_filt{0.001f};
bool _ev_q_error_initialized{false};
#endif // CONFIG_EKF2_EXTERNAL_VISION
// Resets the main Nav EKf yaw to the estimator from the EKF-GSF yaw estimator

18
src/modules/ekf2/EKF/ev_control.cpp
View File

@ -58,6 +58,7 @@ void Ekf::controlExternalVisionFusion()
&& ((_params.ev_quality_minimum <= 0) || (_ext_vision_buffer->get_newest().quality >= _params.ev_quality_minimum)) // newest quality sufficient
&& isNewestSampleRecent(_time_last_ext_vision_buffer_push, EV_MAX_INTERVAL);
updateEvAttitudeErrorFilter(ev_sample, ev_reset);
controlEvYawFusion(ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_yaw);
controlEvVelFusion(ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_vel);
controlEvPosFusion(ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_pos);
@ -84,3 +85,20 @@ void Ekf::controlExternalVisionFusion()
ECL_WARN("vision data stopped");
}
}
void Ekf::updateEvAttitudeErrorFilter(extVisionSample &ev_sample, bool ev_reset)
{
const AxisAnglef q_error((_state.quat_nominal * ev_sample.quat.inversed()).normalized());
if (!q_error.isAllFinite()) {
return;
}
if (!_ev_q_error_initialized || ev_reset) {
_ev_q_error_filt.reset(q_error);
_ev_q_error_initialized = true;
} else {
_ev_q_error_filt.update(q_error);
}
}

3
src/modules/ekf2/EKF/ev_height_control.cpp
View File

@ -56,10 +56,9 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool com
Matrix3f pos_cov{matrix::diag(ev_sample.position_var)};
// rotate EV to the EKF reference frame unless we're operating entirely in vision frame
// TODO: only necessary if there's a roll/pitch offset between VIO and EKF
if (!(_control_status.flags.ev_yaw && _control_status.flags.ev_pos)) {
const Quatf q_error((_state.quat_nominal * ev_sample.quat.inversed()).normalized());
const Quatf q_error(_ev_q_error_filt.getState());
if (q_error.isAllFinite()) {
const Dcmf R_ev_to_ekf(q_error);

3
src/modules/ekf2/EKF/ev_pos_control.cpp
View File

@ -95,8 +95,7 @@ void Ekf::controlEvPosFusion(const extVisionSample &ev_sample, const bool common
} else {
// rotate EV to the EKF reference frame
const Quatf q_error((_state.quat_nominal * ev_sample.quat.inversed()).normalized());
const Dcmf R_ev_to_ekf = Dcmf(q_error);
const Dcmf R_ev_to_ekf = Dcmf(_ev_q_error_filt.getState());
pos = R_ev_to_ekf * ev_sample.pos - pos_offset_earth;
pos_cov = R_ev_to_ekf * matrix::diag(ev_sample.position_var) * R_ev_to_ekf.transpose();

3
src/modules/ekf2/EKF/ev_vel_control.cpp
View File

@ -80,8 +80,7 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common
} else {
// rotate EV to the EKF reference frame
const Quatf q_error((_state.quat_nominal * ev_sample.quat.inversed()).normalized());
const Dcmf R_ev_to_ekf = Dcmf(q_error);
const Dcmf R_ev_to_ekf = Dcmf(_ev_q_error_filt.getState());
vel = R_ev_to_ekf * ev_sample.vel - vel_offset_earth;
vel_cov = R_ev_to_ekf * matrix::diag(ev_sample.velocity_var) * R_ev_to_ekf.transpose();