forked from Archive/PX4-Autopilot
Compare commits
1 Commits
main
...
pr-ekf2-ma
Author | SHA1 | Date |
---|---|---|
bresch | 6b626ce71b |
|
@ -762,7 +762,7 @@ private:
|
||||||
|
|
||||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||||
// ekf sequential fusion of magnetometer measurements
|
// 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
|
// fuse magnetometer declination measurement
|
||||||
// argument passed in is the declination uncertainty in radians
|
// argument passed in is the declination uncertainty in radians
|
||||||
|
|
|
@ -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_3D = (_params.mag_fusion_type == MagFuseType::AUTO)
|
||||||
&& _control_status.flags.mag
|
&& _control_status.flags.mag
|
||||||
&& _control_status.flags.mag_aligned_in_flight
|
&& _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.mag_fault
|
||||||
&& !_control_status.flags.ev_yaw
|
&& !_control_status.flags.ev_yaw
|
||||||
&& !_control_status.flags.gps_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.
|
// states for the first few observations.
|
||||||
fuseDeclination(0.02f);
|
fuseDeclination(0.02f);
|
||||||
_mag_decl_cov_reset = true;
|
_mag_decl_cov_reset = true;
|
||||||
fuseMag(mag_sample.mag, aid_src, false);
|
fuseMag(mag_sample.mag, aid_src, false, false);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
// The normal sequence is to fuse the magnetometer data first before fusing
|
// 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 at a higher uncertainty to allow some learning of
|
||||||
// declination angle over time.
|
// declination angle over time.
|
||||||
const bool update_all_states = _control_status.flags.mag_3D;
|
const bool update_all_states = _control_status.flags.mag_3D || _control_status.flags.mag_hdg;
|
||||||
fuseMag(mag_sample.mag, aid_src, update_all_states);
|
fuseMag(mag_sample.mag, aid_src, update_all_states, _control_status.flags.mag_hdg);
|
||||||
|
|
||||||
if (_control_status.flags.mag_dec) {
|
if (_control_status.flags.mag_dec) {
|
||||||
fuseDeclination(0.5f);
|
fuseDeclination(0.5f);
|
||||||
|
@ -176,7 +180,7 @@ void Ekf::controlMag3DFusion(const magSample &mag_sample, const bool common_star
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
ECL_INFO("starting %s fusion", AID_SRC_NAME);
|
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;
|
aid_src.time_last_fuse = _time_delayed_us;
|
||||||
|
|
|
@ -47,9 +47,6 @@ void Ekf::controlMagFusion()
|
||||||
_control_status.flags.mag_aligned_in_flight = false;
|
_control_status.flags.mag_aligned_in_flight = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
checkYawAngleObservability();
|
|
||||||
checkMagHeadingConsistency();
|
|
||||||
|
|
||||||
if (_params.mag_fusion_type == MagFuseType::NONE) {
|
if (_params.mag_fusion_type == MagFuseType::NONE) {
|
||||||
stopMagFusion();
|
stopMagFusion();
|
||||||
stopMagHdgFusion();
|
stopMagHdgFusion();
|
||||||
|
@ -106,7 +103,7 @@ void Ekf::controlMagFusion()
|
||||||
}
|
}
|
||||||
|
|
||||||
controlMag3DFusion(mag_sample, starting_conditions_passing, _aid_src_mag);
|
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)) {
|
} else if (!isNewestSampleRecent(_time_last_mag_buffer_push, 2 * MAG_MAX_INTERVAL)) {
|
||||||
// No data anymore. Stop until it comes back.
|
// 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)
|
bool Ekf::checkMagField(const Vector3f &mag_sample)
|
||||||
{
|
{
|
||||||
_control_status.flags.mag_field_disturbed = false;
|
_control_status.flags.mag_field_disturbed = false;
|
||||||
|
|
|
@ -51,7 +51,7 @@
|
||||||
|
|
||||||
#include <mathlib/mathlib.h>
|
#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
|
// XYZ Measurement uncertainty. Need to consider timing errors for fast rotations
|
||||||
const float R_MAG = math::max(sq(_params.mag_noise), sq(0.01f));
|
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];
|
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) {
|
if (!update_all_states) {
|
||||||
// zero non-mag Kalman gains if not updating all states
|
// zero non-mag Kalman gains if not updating all states
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue