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

HACK: create mag hdg fusion by modifying 3D fusion

This commit is contained in:
bresch 2024-02-28 17:15:42 +01:00
parent 28db3e1c8c
commit 6b626ce71b
4 changed files with 23 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -762,7 +762,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 mag_heading = false);
// fuse magnetometer declination measurement
// argument passed in is the declination uncertainty in radians

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

@ -62,7 +62,11 @@ void Ekf::controlMag3DFusion(const magSample &mag_sample, const bool common_star
_control_status.flags.mag_3D = (_params.mag_fusion_type == MagFuseType::AUTO)
&& _control_status.flags.mag
&& _control_status.flags.mag_aligned_in_flight
&& (_control_status.flags.mag_heading_consistent || !_control_status.flags.gps)
&& !_control_status.flags.mag_fault
&& !_control_status.flags.ev_yaw
&& !_control_status.flags.gps_yaw;
_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.mag
&& !_control_status.flags.mag_fault
&& !_control_status.flags.ev_yaw
&& !_control_status.flags.gps_yaw;
@ -102,14 +106,14 @@ void Ekf::controlMag3DFusion(const magSample &mag_sample, const bool common_star
// states for the first few observations.
fuseDeclination(0.02f);
_mag_decl_cov_reset = true;
fuseMag(mag_sample.mag, aid_src, false);
fuseMag(mag_sample.mag, aid_src, false, false);
} else {
// 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;
fuseMag(mag_sample.mag, aid_src, update_all_states, _control_status.flags.mag_hdg);
if (_control_status.flags.mag_dec) {
fuseDeclination(0.5f);
@ -176,7 +180,7 @@ void Ekf::controlMag3DFusion(const magSample &mag_sample, const bool common_star
} else {
ECL_INFO("starting %s fusion", AID_SRC_NAME);
fuseMag(mag_sample.mag, aid_src, false);
fuseMag(mag_sample.mag, aid_src, false, false);
}
aid_src.time_last_fuse = _time_delayed_us;

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

@ -47,9 +47,6 @@ void Ekf::controlMagFusion()
_control_status.flags.mag_aligned_in_flight = false;
}
checkYawAngleObservability();
checkMagHeadingConsistency();
if (_params.mag_fusion_type == MagFuseType::NONE) {
stopMagFusion();
stopMagHdgFusion();
@ -106,7 +103,7 @@ void Ekf::controlMagFusion()
}
controlMag3DFusion(mag_sample, starting_conditions_passing, _aid_src_mag);
controlMagHeadingFusion(mag_sample, starting_conditions_passing, _aid_src_mag_heading);
// 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.
@ -222,36 +219,6 @@ void Ekf::resetMagStates(const Vector3f &mag, bool reset_heading)
}
}
void Ekf::checkYawAngleObservability()
{
if (_control_status.flags.gps) {
// Check if there has been enough change in horizontal velocity to make yaw observable
// Apply hysteresis to check to avoid rapid toggling
if (_yaw_angle_observable) {
_yaw_angle_observable = _accel_lpf_NE.norm() > _params.mag_acc_gate;
} else {
_yaw_angle_observable = _accel_lpf_NE.norm() > _params.mag_acc_gate * 2.f;
}
} else {
_yaw_angle_observable = false;
}
}
void Ekf::checkMagHeadingConsistency()
{
if (fabsf(_mag_heading_innov_lpf.getState()) < _params.mag_heading_noise) {
if (_yaw_angle_observable) {
// yaw angle must be observable to consider consistency
_control_status.flags.mag_heading_consistent = true;
}
} else {
_control_status.flags.mag_heading_consistent = false;
}
}
bool Ekf::checkMagField(const Vector3f &mag_sample)
{
_control_status.flags.mag_field_disturbed = false;

13
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 mag_heading)
{
// XYZ Measurement uncertainty. Need to consider timing errors for fast rotations
const float R_MAG = math::max(sq(_params.mag_noise), sq(0.01f));
@ -178,6 +178,17 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo
VectorState Kfusion = P * H / aid_src_mag.innovation_variance[index];
if (mag_heading) {
// Modify the Kalman gain to zero the delta quaternion changes perpendicular to the down NED axis
// To do this, transform K from body to NED, zero the xy components and transform back to body frame.
// Transformation from body to NED of a delta quaternion in the tangent space is done using the adjoint
// at the current attitude (i.e.: the current rotation matrix)
Vector3f K_rot = Kfusion.slice<3, 1>(State::quat_nominal.idx, State::quat_nominal.idx);
Vector3f row = _R_to_earth.row(2);
K_rot = row * row.dot(K_rot);
Kfusion.slice<3, 1>(State::quat_nominal.idx, State::quat_nominal.idx) = K_rot;
}
if (!update_all_states) {
// zero non-mag Kalman gains if not updating all states