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

ekf2: integrate mag heading into mag 3D

This commit is contained in:
bresch 2024-03-25 11:57:27 +01:00
parent 41acb1ba7e
commit b7f62b41a6
14 changed files with 66 additions and 265 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
msg/EstimatorAidSource1d.msg
View File

@ -20,5 +20,5 @@ bool fused # true if the sample was successfully fused
# TOPICS estimator_aid_src_baro_hgt estimator_aid_src_ev_hgt estimator_aid_src_gnss_hgt estimator_aid_src_rng_hgt
# TOPICS estimator_aid_src_airspeed estimator_aid_src_sideslip
# TOPICS estimator_aid_src_fake_hgt
# TOPICS estimator_aid_src_mag_heading estimator_aid_src_gnss_yaw estimator_aid_src_ev_yaw
# TOPICS estimator_aid_src_gnss_yaw estimator_aid_src_ev_yaw
# TOPICS estimator_aid_src_terrain_range_finder

1
src/modules/ekf2/CMakeLists.txt
View File

@ -189,7 +189,6 @@ if(CONFIG_EKF2_MAGNETOMETER)
EKF/mag_3d_control.cpp
EKF/mag_control.cpp
EKF/mag_fusion.cpp
EKF/mag_heading_control.cpp
)
endif()

1
src/modules/ekf2/EKF/CMakeLists.txt
View File

@ -107,7 +107,6 @@ if(CONFIG_EKF2_MAGNETOMETER)
mag_3d_control.cpp
mag_control.cpp
mag_fusion.cpp
mag_heading_control.cpp
)
endif()

1
src/modules/ekf2/EKF/ekf.cpp
View File

@ -155,7 +155,6 @@ void Ekf::reset()
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_MAGNETOMETER)
resetEstimatorAidStatus(_aid_src_mag_heading);
resetEstimatorAidStatus(_aid_src_mag);
#endif // CONFIG_EKF2_MAGNETOMETER

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

@ -149,11 +149,7 @@ public:
float getHeadingInnov() const
{
#if defined(CONFIG_EKF2_MAGNETOMETER)
if (_control_status.flags.mag_hdg) {
return _aid_src_mag_heading.innovation;
}
if (_control_status.flags.mag_3D) {
if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) {
return Vector3f(_aid_src_mag.innovation).max();
}
#endif // CONFIG_EKF2_MAGNETOMETER
@ -176,11 +172,7 @@ public:
float getHeadingInnovVar() const
{
#if defined(CONFIG_EKF2_MAGNETOMETER)
if (_control_status.flags.mag_hdg) {
return _aid_src_mag_heading.innovation_variance;
}
if (_control_status.flags.mag_3D) {
if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) {
return Vector3f(_aid_src_mag.innovation_variance).max();
}
#endif // CONFIG_EKF2_MAGNETOMETER
@ -203,11 +195,7 @@ public:
float getHeadingInnovRatio() const
{
#if defined(CONFIG_EKF2_MAGNETOMETER)
if (_control_status.flags.mag_hdg) {
return _aid_src_mag_heading.test_ratio;
}
if (_control_status.flags.mag_3D) {
if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) {
return Vector3f(_aid_src_mag.test_ratio).max();
}
#endif // CONFIG_EKF2_MAGNETOMETER
@ -460,7 +448,6 @@ public:
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_MAGNETOMETER)
const auto &aid_src_mag_heading() const { return _aid_src_mag_heading; }
const auto &aid_src_mag() const { return _aid_src_mag; }
#endif // CONFIG_EKF2_MAGNETOMETER
@ -715,19 +702,13 @@ private:
#endif // CONFIG_EKF2_BAROMETER
#if defined(CONFIG_EKF2_MAGNETOMETER)
float _mag_heading_prev{}; ///< previous value of mag heading (rad)
float _mag_heading_pred_prev{}; ///< previous value of yaw state used by mag heading fusion (rad)
// used by magnetometer fusion mode selection
bool _yaw_angle_observable{false}; ///< true when there is enough horizontal acceleration to make yaw observable
AlphaFilter<float> _mag_heading_innov_lpf{0.1f};
float _mag_heading_last_declination{}; ///< last magnetic field declination used for heading fusion (rad)
bool _mag_decl_cov_reset{false}; ///< true after the fuseDeclination() function has been used to modify the earth field covariances after a magnetic field reset event.
uint8_t _nb_mag_heading_reset_available{0};
uint8_t _nb_mag_3d_reset_available{0};
uint32_t _min_mag_health_time_us{1'000'000}; ///< magnetometer is marked as healthy only after this amount of time
estimator_aid_source1d_s _aid_src_mag_heading{};
estimator_aid_source3d_s _aid_src_mag{};
AlphaFilter<Vector3f> _mag_lpf{0.1f}; ///< filtered magnetometer measurement for instant reset (Gauss)
@ -777,7 +758,7 @@ private:
#if defined(CONFIG_EKF2_MAGNETOMETER)
// ekf sequential fusion of magnetometer measurements
bool fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bool update_all_states = true);
bool fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bool update_all_states = true, bool update_tilt = true);
// fuse magnetometer declination measurement
// argument passed in is the declination uncertainty in radians
@ -1048,7 +1029,6 @@ private:
#if defined(CONFIG_EKF2_MAGNETOMETER)
// control fusion of magnetometer observations
void controlMagFusion();
void controlMagHeadingFusion(const magSample &mag_sample, const bool common_starting_conditions_passing, estimator_aid_source1d_s &aid_src);
void controlMag3DFusion(const magSample &mag_sample, const bool common_starting_conditions_passing, estimator_aid_source3d_s &aid_src);
bool checkHaglYawResetReq() const;
@ -1058,12 +1038,11 @@ private:
bool haglYawResetReq();
void checkYawAngleObservability();
void checkMagHeadingConsistency();
void checkMagHeadingConsistency(const magSample &mag_sample);
bool checkMagField(const Vector3f &mag);
static bool isMeasuredMatchingExpected(float measured, float expected, float gate);
void stopMagHdgFusion();
void stopMagFusion();
// calculate a synthetic value for the magnetometer Z component, given the 3D magnetomter

13
src/modules/ekf2/EKF/ekf_helper.cpp
View File

@ -381,11 +381,7 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
mag = 0.f;
#if defined(CONFIG_EKF2_MAGNETOMETER)
if (_control_status.flags.mag_hdg) {
mag = math::max(mag, sqrtf(_aid_src_mag_heading.test_ratio));
}
if (_control_status.flags.mag_3D) {
if (_control_status.flags.mag_hdg ||_control_status.flags.mag_3D) {
mag = math::max(mag, sqrtf(Vector3f(_aid_src_mag.test_ratio).max()));
}
#endif // CONFIG_EKF2_MAGNETOMETER
@ -523,12 +519,7 @@ void Ekf::get_ekf_soln_status(uint16_t *status) const
bool mag_innov_good = true;
#if defined(CONFIG_EKF2_MAGNETOMETER)
if (_control_status.flags.mag_hdg) {
if (_aid_src_mag_heading.test_ratio < 1.f) {
mag_innov_good = false;
}
} else if (_control_status.flags.mag_3D) {
if (_control_status.flags.mag_hdg ||_control_status.flags.mag_3D) {
if (Vector3f(_aid_src_mag.test_ratio).max() < 1.f) {
mag_innov_good = false;
}

23
src/modules/ekf2/EKF/mag_3d_control.cpp
View File

@ -67,6 +67,18 @@ void Ekf::controlMag3DFusion(const magSample &mag_sample, const bool common_star
&& !_control_status.flags.ev_yaw
&& !_control_status.flags.gps_yaw;
const bool mag_consistent_or_no_gnss = _control_status.flags.mag_heading_consistent || !_control_status.flags.gps;
_control_status.flags.mag_hdg = ((_params.mag_fusion_type == MagFuseType::HEADING)
|| (_params.mag_fusion_type == MagFuseType::AUTO && !_control_status.flags.mag_3D))
&& _control_status.flags.tilt_align
&& ((_control_status.flags.yaw_align && mag_consistent_or_no_gnss)
|| (!_control_status.flags.ev_yaw && !_control_status.flags.yaw_align))
&& !_control_status.flags.mag_fault
&& !_control_status.flags.mag_field_disturbed
&& !_control_status.flags.ev_yaw
&& !_control_status.flags.gps_yaw;
// TODO: allow clearing mag_fault if mag_3d is good?
if (_control_status.flags.mag_3D && !_control_status_prev.flags.mag_3D) {
@ -108,8 +120,9 @@ void Ekf::controlMag3DFusion(const magSample &mag_sample, const bool common_star
// The normal sequence is to fuse the magnetometer data first before fusing
// declination angle at a higher uncertainty to allow some learning of
// declination angle over time.
const bool update_all_states = _control_status.flags.mag_3D;
fuseMag(mag_sample.mag, aid_src, update_all_states);
const bool update_all_states = _control_status.flags.mag_3D || _control_status.flags.mag_hdg;
const bool update_tilt = _control_status.flags.mag_3D;
fuseMag(mag_sample.mag, aid_src, update_all_states, update_tilt);
if (_control_status.flags.mag_dec) {
fuseDeclination(0.5f);
@ -199,6 +212,12 @@ void Ekf::stopMagFusion()
_control_status.flags.mag_3D = false;
}
if (_control_status.flags.mag_hdg) {
ECL_INFO("stopping mag heading fusion");
_control_status.flags.mag_hdg = false;
_fault_status.flags.bad_hdg = false;
}
_fault_status.flags.bad_mag_x = false;
_fault_status.flags.bad_mag_y = false;
_fault_status.flags.bad_mag_z = false;

34
src/modules/ekf2/EKF/mag_control.cpp
View File

@ -48,11 +48,9 @@ void Ekf::controlMagFusion()
}
checkYawAngleObservability();
checkMagHeadingConsistency();
if (_params.mag_fusion_type == MagFuseType::NONE) {
stopMagFusion();
stopMagHdgFusion();
return;
}
@ -105,12 +103,11 @@ void Ekf::controlMagFusion()
_control_status.flags.synthetic_mag_z = false;
}
checkMagHeadingConsistency(mag_sample);
controlMag3DFusion(mag_sample, starting_conditions_passing, _aid_src_mag);
controlMagHeadingFusion(mag_sample, starting_conditions_passing, _aid_src_mag_heading);
} else if (!isNewestSampleRecent(_time_last_mag_buffer_push, 2 * MAG_MAX_INTERVAL)) {
// No data anymore. Stop until it comes back.
stopMagHdgFusion();
stopMagFusion();
}
}
@ -239,8 +236,34 @@ void Ekf::checkYawAngleObservability()
}
}
void Ekf::checkMagHeadingConsistency()
void Ekf::checkMagHeadingConsistency(const magSample &mag_sample)
{
// use mag bias if variance good
Vector3f mag_bias{0.f, 0.f, 0.f};
const Vector3f mag_bias_var = getMagBiasVariance();
if ((mag_bias_var.min() > 0.f) && (mag_bias_var.max() <= sq(_params.mag_noise))) {
mag_bias = _state.mag_B;
}
// calculate mag heading
// Rotate the measurements into earth frame using the zero yaw angle
const Dcmf R_to_earth = updateYawInRotMat(0.f, _R_to_earth);
// the angle of the projection onto the horizontal gives the yaw angle
// calculate the yaw innovation and wrap to the interval between +-pi
const Vector3f mag_earth_pred = R_to_earth * (mag_sample.mag - mag_bias);
const float declination = getMagDeclination();
const float measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + declination;
if (_control_status.flags.yaw_align) {
const float innovation = wrap_pi(getEulerYaw(_R_to_earth) - measured_hdg);
_mag_heading_innov_lpf.update(innovation);
} else {
_mag_heading_innov_lpf.reset(0.f);
}
if (fabsf(_mag_heading_innov_lpf.getState()) < _params.mag_heading_noise) {
if (_yaw_angle_observable) {
// yaw angle must be observable to consider consistency
@ -349,7 +372,6 @@ void Ekf::resetMagHeading(const Vector3f &mag)
// update quaternion states and corresponding covarainces
resetQuatStateYaw(yaw_new, yaw_new_variance);
_mag_heading_last_declination = declination;
_time_last_heading_fuse = _time_delayed_us;

7
src/modules/ekf2/EKF/mag_fusion.cpp
View File

@ -51,7 +51,7 @@
#include <mathlib/mathlib.h>
bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bool update_all_states)
bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bool update_all_states, bool update_tilt)
{
// XYZ Measurement uncertainty. Need to consider timing errors for fast rotations
const float R_MAG = math::max(sq(_params.mag_noise), sq(0.01f));
@ -191,6 +191,11 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo
Kfusion.slice<State::mag_B.dof, 1>(State::mag_B.idx, 0) = K_mag_B;
}
if (!update_tilt) {
Kfusion(State::quat_nominal.idx) = 0.f;
Kfusion(State::quat_nominal.idx + 1) = 0.f;
}
if (measurementUpdate(Kfusion, H, aid_src_mag.observation_variance[index], aid_src_mag.innovation[index])) {
fused[index] = true;
limitDeclination();

204
src/modules/ekf2/EKF/mag_heading_control.cpp
View File

@ -1,204 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2023 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file mag_heading_control.cpp
* Control functions for ekf mag heading fusion
*/
#include "ekf.h"
void Ekf::controlMagHeadingFusion(const magSample &mag_sample, const bool common_starting_conditions_passing,
estimator_aid_source1d_s &aid_src)
{
static constexpr const char *AID_SRC_NAME = "mag heading";
// use mag bias if variance good
Vector3f mag_bias{0.f, 0.f, 0.f};
const Vector3f mag_bias_var = getMagBiasVariance();
if ((mag_bias_var.min() > 0.f) && (mag_bias_var.max() <= sq(_params.mag_noise))) {
mag_bias = _state.mag_B;
}
// calculate mag heading
// Rotate the measurements into earth frame using the zero yaw angle
const Dcmf R_to_earth = updateYawInRotMat(0.f, _R_to_earth);
// the angle of the projection onto the horizontal gives the yaw angle
// calculate the yaw innovation and wrap to the interval between +-pi
const Vector3f mag_earth_pred = R_to_earth * (mag_sample.mag - mag_bias);
const float declination = getMagDeclination();
const float measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + declination;
resetEstimatorAidStatus(aid_src);
aid_src.observation = measured_hdg;
aid_src.observation_variance = math::max(sq(_params.mag_heading_noise), sq(0.01f));
if (_control_status.flags.yaw_align) {
// mag heading
aid_src.innovation = wrap_pi(getEulerYaw(_R_to_earth) - aid_src.observation);
_mag_heading_innov_lpf.update(aid_src.innovation);
} else {
// mag heading delta (logging only)
aid_src.innovation = wrap_pi(wrap_pi(getEulerYaw(_R_to_earth) - _mag_heading_pred_prev)
- wrap_pi(measured_hdg - _mag_heading_prev));
_mag_heading_innov_lpf.reset(0.f);
}
// determine if we should use mag heading aiding
const bool mag_consistent_or_no_gnss = _control_status.flags.mag_heading_consistent || !_control_status.flags.gps;
bool continuing_conditions_passing = ((_params.mag_fusion_type == MagFuseType::HEADING)
|| (_params.mag_fusion_type == MagFuseType::AUTO && !_control_status.flags.mag_3D))
&& _control_status.flags.tilt_align
&& ((_control_status.flags.yaw_align && mag_consistent_or_no_gnss)
|| (!_control_status.flags.ev_yaw && !_control_status.flags.yaw_align))
&& !_control_status.flags.mag_fault
&& !_control_status.flags.mag_field_disturbed
&& !_control_status.flags.ev_yaw
&& !_control_status.flags.gps_yaw
&& PX4_ISFINITE(aid_src.observation)
&& PX4_ISFINITE(aid_src.observation_variance);
const bool starting_conditions_passing = common_starting_conditions_passing
&& continuing_conditions_passing
&& isTimedOut(aid_src.time_last_fuse, 3e6);
if (_control_status.flags.mag_hdg) {
aid_src.timestamp_sample = mag_sample.time_us;
if (continuing_conditions_passing && _control_status.flags.yaw_align) {
const bool declination_changed = _control_status_prev.flags.mag_hdg
&& (fabsf(declination - _mag_heading_last_declination) > math::radians(3.f));
bool skip_timeout_check = false;
if (mag_sample.reset || declination_changed) {
if (declination_changed) {
ECL_INFO("reset to %s, declination changed %.5f->%.5f",
AID_SRC_NAME, (double)_mag_heading_last_declination, (double)declination);
} else {
ECL_INFO("reset to %s", AID_SRC_NAME);
}
resetMagHeading(_mag_lpf.getState());
aid_src.time_last_fuse = _time_delayed_us;
} else {
VectorState H_YAW;
computeYawInnovVarAndH(aid_src.observation_variance, aid_src.innovation_variance, H_YAW);
if ((aid_src.innovation_variance - aid_src.observation_variance) > sq(_params.mag_heading_noise / 2.f)) {
// Only fuse mag to constrain the yaw variance to a safe value
fuseYaw(aid_src, H_YAW);
} else {
// Yaw variance is low enough, stay in mag heading mode but don't fuse the data and skip the fusion timeout check
skip_timeout_check = true;
aid_src.test_ratio = 0.f; // required for preflight checks to pass
}
}
const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.reset_timeout_max) && !skip_timeout_check;
if (is_fusion_failing) {
if (_nb_mag_heading_reset_available > 0) {
// Data seems good, attempt a reset
ECL_WARN("%s fusion failing, resetting", AID_SRC_NAME);
resetMagHeading(_mag_lpf.getState());
aid_src.time_last_fuse = _time_delayed_us;
if (_control_status.flags.in_air) {
_nb_mag_heading_reset_available--;
}
} else if (starting_conditions_passing) {
// Data seems good, but previous reset did not fix the issue
// something else must be wrong, declare the sensor faulty and stop the fusion
_control_status.flags.mag_fault = true;
ECL_WARN("stopping %s fusion, starting conditions failing", AID_SRC_NAME);
stopMagHdgFusion();
} else {
// A reset did not fix the issue but all the starting checks are not passing
// This could be a temporary issue, stop the fusion without declaring the sensor faulty
ECL_WARN("stopping %s, fusion failing", AID_SRC_NAME);
stopMagHdgFusion();
}
}
} else {
// Stop fusion but do not declare it faulty
ECL_WARN("stopping %s fusion, continuing conditions failing", AID_SRC_NAME);
stopMagHdgFusion();
}
} else {
if (starting_conditions_passing) {
// activate fusion
ECL_INFO("starting %s fusion", AID_SRC_NAME);
if (!_control_status.flags.yaw_align) {
// reset heading
resetMagHeading(_mag_lpf.getState());
_control_status.flags.yaw_align = true;
}
_control_status.flags.mag_hdg = true;
aid_src.time_last_fuse = _time_delayed_us;
_nb_mag_heading_reset_available = 1;
}
}
// record corresponding mag heading and yaw state for future mag heading delta heading innovation (logging only)
_mag_heading_prev = measured_hdg;
_mag_heading_pred_prev = getEulerYaw(_state.quat_nominal);
_mag_heading_last_declination = getMagDeclination();
}
void Ekf::stopMagHdgFusion()
{
if (_control_status.flags.mag_hdg) {
ECL_INFO("stopping mag heading fusion");
resetEstimatorAidStatus(_aid_src_mag_heading);
_control_status.flags.mag_hdg = false;
_fault_status.flags.bad_hdg = false;
}
}

4
src/modules/ekf2/EKF2.cpp
View File

@ -335,7 +335,6 @@ bool EKF2::multi_init(int imu, int mag)
// mag advertise
if (_param_ekf2_mag_type.get() != MagFuseType::NONE) {
_estimator_aid_src_mag_heading_pub.advertise();
_estimator_aid_src_mag_pub.advertise();
}
@ -874,9 +873,6 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime &timestamp)
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_MAGNETOMETER)
// mag heading
PublishAidSourceStatus(_ekf.aid_src_mag_heading(), _status_mag_heading_pub_last, _estimator_aid_src_mag_heading_pub);
// mag 3d
PublishAidSourceStatus(_ekf.aid_src_mag(), _status_mag_pub_last, _estimator_aid_src_mag_pub);
#endif // CONFIG_EKF2_MAGNETOMETER

2
src/modules/ekf2/EKF2.hpp
View File

@ -307,11 +307,9 @@ private:
Vector3f _last_mag_bias_published{};
hrt_abstime _status_mag_pub_last{0};
hrt_abstime _status_mag_heading_pub_last{0};
uORB::Subscription _magnetometer_sub{ORB_ID(vehicle_magnetometer)};
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_mag_heading_pub{ORB_ID(estimator_aid_src_mag_heading)};
uORB::PublicationMulti<estimator_aid_source3d_s> _estimator_aid_src_mag_pub{ORB_ID(estimator_aid_src_mag)};
#endif // CONFIG_EKF2_MAGNETOMETER

6
src/modules/ekf2/test/test_EKF_flow.cpp
View File

@ -259,7 +259,6 @@ TEST_F(EkfFlowTest, yawMotionCorrectionWithAutopilotGyroData)
// THEN: the flow due to the yaw rotation and the offsets is canceled
// and the velocity estimate stays 0
// FIXME: the estimate isn't perfect 0 mainly because the mag simulated measurement isn't rotating
// and this creates an incorrect Z gyro bias which is used to compensate for the apparent flow
const Vector2f estimated_horz_velocity = Vector2f(_ekf->getVelocity());
EXPECT_NEAR(estimated_horz_velocity(0), 0.f, 0.02f)
<< "estimated vel = " << estimated_horz_velocity(0);
@ -298,10 +297,11 @@ TEST_F(EkfFlowTest, yawMotionCorrectionWithFlowGyroData)
// THEN: the flow due to the yaw rotation and the offsets is canceled
// and the velocity estimate stays 0
// FIXME: the estimate isn't perfect 0 mainly because the mag simulated measurement isn't rotating
const Vector2f estimated_horz_velocity = Vector2f(_ekf->getVelocity());
EXPECT_NEAR(estimated_horz_velocity(0), 0.f, 0.01f)
EXPECT_NEAR(estimated_horz_velocity(0), 0.f, 0.02f)
<< "estimated vel = " << estimated_horz_velocity(0);
EXPECT_NEAR(estimated_horz_velocity(1), 0.f, 0.01f)
EXPECT_NEAR(estimated_horz_velocity(1), 0.f, 0.02f)
<< "estimated vel = " << estimated_horz_velocity(1);
_ekf->state().vector().print();
_ekf->covariances().print();

2
src/modules/logger/logged_topics.cpp
View File

@ -204,7 +204,6 @@ void LoggedTopics::add_default_topics()
// add_optional_topic_multi("estimator_aid_src_gnss_yaw", 100, MAX_ESTIMATOR_INSTANCES);
// add_optional_topic_multi("estimator_aid_src_gnss_vel", 100, MAX_ESTIMATOR_INSTANCES);
// add_optional_topic_multi("estimator_aid_src_gnss_pos", 100, MAX_ESTIMATOR_INSTANCES);
// add_optional_topic_multi("estimator_aid_src_mag_heading", 100, MAX_ESTIMATOR_INSTANCES);
// add_optional_topic_multi("estimator_aid_src_mag", 100, MAX_ESTIMATOR_INSTANCES);
// add_optional_topic_multi("estimator_aid_src_optical_flow", 100, MAX_ESTIMATOR_INSTANCES);
// add_optional_topic_multi("estimator_aid_src_terrain_optical_flow", 100, MAX_ESTIMATOR_INSTANCES);
@ -297,7 +296,6 @@ void LoggedTopics::add_default_topics()
add_optional_topic_multi("estimator_aid_src_gnss_vel", 0, MAX_ESTIMATOR_INSTANCES);
add_optional_topic_multi("estimator_aid_src_gnss_yaw", 0, MAX_ESTIMATOR_INSTANCES);
add_optional_topic_multi("estimator_aid_src_gravity", 0, MAX_ESTIMATOR_INSTANCES);
add_optional_topic_multi("estimator_aid_src_mag_heading", 0, MAX_ESTIMATOR_INSTANCES);
add_optional_topic_multi("estimator_aid_src_mag", 0, MAX_ESTIMATOR_INSTANCES);
add_optional_topic_multi("estimator_aid_src_optical_flow", 0, MAX_ESTIMATOR_INSTANCES);
add_optional_topic_multi("estimator_aid_src_terrain_optical_flow", 0, MAX_ESTIMATOR_INSTANCES);