fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-01 05:51:36 +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-17 13:43:18 +02:00
committed by Mathieu Bresciani
parent 15bb8c0f2e
commit 1128ada90a
2 changed files with 2 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ekf.h
+1 -1
View File
@@ -1161,7 +1161,7 @@ private:
#if defined(CONFIG_EKF2_EXTERNAL_VISION) #if defined(CONFIG_EKF2_EXTERNAL_VISION)
HeightBiasEstimator _ev_hgt_b_est{HeightSensor::EV, _height_sensor_ref}; HeightBiasEstimator _ev_hgt_b_est{HeightSensor::EV, _height_sensor_ref};
PositionBiasEstimator _ev_pos_b_est{static_cast<uint8_t>(PositionSensor::EV), _position_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}; bool _ev_q_error_initialized{false};
#endif // CONFIG_EKF2_EXTERNAL_VISION #endif // CONFIG_EKF2_EXTERNAL_VISION
src/modules/ekf2/EKF/ev_control.cpp
+1 -1
View File
@@ -88,7 +88,7 @@ void Ekf::controlExternalVisionFusion()
void Ekf::updateEvAttitudeErrorFilter(extVisionSample &ev_sample, bool ev_reset) 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()) { if (!q_error.isAllFinite()) {
return; return;