fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-09 07:51:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2-ev: filter q_error for frame correction

Browse Source
This commit is contained in:
bresch
2023-06-29 16:37:30 +02:00
parent 1c49812b17
commit 7531adab3e
2 changed files with 12 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ekf.h
+2
View File
@@ -1155,6 +1155,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_init{false};
#endif // CONFIG_EKF2_EXTERNAL_VISION
// Resets the main Nav EKf yaw to the estimator from the EKF-GSF yaw estimator
src/modules/ekf2/EKF/ev_pos_control.cpp
+10 -2
View File
@@ -95,8 +95,16 @@ 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 AxisAnglef q_error((_state.quat_nominal * ev_sample.quat.inversed()).normalized());
if (_ev_q_error_init) {
_ev_q_error_filt.update(q_error);
} else {
_ev_q_error_filt.reset(q_error);
_ev_q_error_init = true;
}
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();