fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: use correct AlphaFilter for 3D rotations

Browse Source
This commit is contained in:
bresch 2023-07-12 16:24:20 +02:00 committed by Mathieu Bresciani
parent 15bb8c0f2e
commit 1128ada90a
2 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1161,7 +1161,7 @@ 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};
AlphaFilter<Quatf> _ev_q_error_filt{0.001f};
bool _ev_q_error_initialized{false};
#endif // CONFIG_EKF2_EXTERNAL_VISION

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

@ -88,7 +88,7 @@ void Ekf::controlExternalVisionFusion()
void Ekf::updateEvAttitudeErrorFilter(extVisionSample &ev_sample, bool ev_reset)
{
const AxisAnglef q_error((_state.quat_nominal * ev_sample.quat.inversed()).normalized());
const Quatf q_error((_state.quat_nominal * ev_sample.quat.inversed()).normalized());
if (!q_error.isAllFinite()) {
return;