Skip to content

Commit

Permalink
ekf2: make mag control responsible for WMM
Browse files Browse the repository at this point in the history 
 - this further untangles mag control (which requires the WMM) from GPS
  • Loading branch information
@dagar
dagar committed May 31, 2024
1 parent 8dc431c commit dae97b0
Show file tree
Hide file tree
Showing 10 changed files with 624 additions and 596 deletions.
77 changes: 27 additions & 50 deletions src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,10 +41,6 @@

#include "ekf.h"

#if defined(CONFIG_EKF2_MAGNETOMETER)
# include <lib/world_magnetic_model/geo_mag_declination.h>
#endif // CONFIG_EKF2_MAGNETOMETER

#include <mathlib/mathlib.h>

// GPS pre-flight check bit locations
Expand All @@ -62,19 +58,17 @@ void Ekf::collect_gps(const gnssSample &gps)
{
if (_filter_initialised && !_NED_origin_initialised && _gps_checks_passed) {
// If we have good GPS data set the origin's WGS-84 position to the last gps fix
const double lat = gps.lat;
const double lon = gps.lon;

if (!_pos_ref.isInitialized()) {
_pos_ref.initReference(lat, lon, gps.time_us);

// if we are already doing aiding, correct for the change in position since the EKF started navigating
if (isHorizontalAidingActive()) {
double est_lat;
double est_lon;
_pos_ref.reproject(-_state.pos(0), -_state.pos(1), est_lat, est_lon);
_pos_ref.initReference(est_lat, est_lon, gps.time_us);
}
// force new origin if it was previously set early by rough 2d fix (_gpos_origin_eph > 0)
if (!_pos_ref.isInitialized() || (_gpos_origin_eph > 0.f)) {
_pos_ref.initReference(gps.lat, gps.lon, _time_delayed_us);
}

// if we are already doing aiding, correct for the change in position since the EKF started navigating
if (isHorizontalAidingActive()) {
double est_lat;
double est_lon;
_pos_ref.reproject(-_state.pos(0), -_state.pos(1), est_lat, est_lon);
_pos_ref.initReference(est_lat, est_lon, _time_delayed_us);
}

// Take the current GPS height and subtract the filter height above origin to estimate the GPS height of the origin
Expand All @@ -88,49 +82,32 @@ void Ekf::collect_gps(const gnssSample &gps)
_gpos_origin_eph = gps.hacc;
_gpos_origin_epv = gps.vacc;

_information_events.flags.gps_checks_passed = true;
ECL_INFO("GPS checks passed");
}
_earth_rate_NED = calcEarthRateNED(math::radians(static_cast<float>(gps.lat)));

if ((isTimedOut(_wmm_gps_time_last_checked, 1e6)) || (_wmm_gps_time_last_set == 0)) {
// a rough 2D fix is sufficient to lookup declination
const bool gps_rough_2d_fix = (gps.fix_type >= 2) && (gps.hacc < 1000);
_information_events.flags.gps_checks_passed = true;

if (gps_rough_2d_fix && (_gps_checks_passed || !_NED_origin_initialised)) {
ECL_INFO("GPS origin set to lat = %.6f, lon = %.6f",
_pos_ref.getProjectionReferenceLat(), _pos_ref.getProjectionReferenceLon());

// If we have good GPS data set the origin's WGS-84 position to the last gps fix
#if defined(CONFIG_EKF2_MAGNETOMETER)
} else {

// set the magnetic field data returned by the geo library using the current GPS position
const float mag_declination_gps = math::radians(get_mag_declination_degrees(gps.lat, gps.lon));
const float mag_inclination_gps = math::radians(get_mag_inclination_degrees(gps.lat, gps.lon));
const float mag_strength_gps = get_mag_strength_gauss(gps.lat, gps.lon);
if (!_NED_origin_initialised
&& isTimedOut(_pos_ref.getProjectionReferenceTimestamp(), 1e6)
) {
// a rough 2D fix is sufficient to lookup declination
const bool gps_rough_2d_fix = (gps.fix_type >= 2) && (gps.hacc < 1000);

if (PX4_ISFINITE(mag_declination_gps) && PX4_ISFINITE(mag_inclination_gps) && PX4_ISFINITE(mag_strength_gps)) {
if (gps_rough_2d_fix) {

const bool mag_declination_changed = (fabsf(mag_declination_gps - _mag_declination_gps) > math::radians(1.f));
const bool mag_inclination_changed = (fabsf(mag_inclination_gps - _mag_inclination_gps) > math::radians(1.f));
_pos_ref.initReference(gps.lat, gps.lon, _time_delayed_us);
_gpos_origin_eph = gps.hacc;

if ((_wmm_gps_time_last_set == 0)
|| !PX4_ISFINITE(_mag_declination_gps)
|| !PX4_ISFINITE(_mag_inclination_gps)
|| !PX4_ISFINITE(_mag_strength_gps)
|| mag_declination_changed
|| mag_inclination_changed
) {
_mag_declination_gps = mag_declination_gps;
_mag_inclination_gps = mag_inclination_gps;
_mag_strength_gps = mag_strength_gps;
_earth_rate_NED = calcEarthRateNED(math::radians(static_cast<float>(gps.lat)));

_wmm_gps_time_last_set = _time_delayed_us;
}
ECL_DEBUG("rough GPS origin reset to lat = %.6f, lon = %.6f",
_pos_ref.getProjectionReferenceLat(), _pos_ref.getProjectionReferenceLon());
}
#endif // CONFIG_EKF2_MAGNETOMETER

_earth_rate_NED = calcEarthRateNED((float)math::radians(gps.lat));
}

_wmm_gps_time_last_checked = _time_delayed_us;
}
}

Expand Down
118 changes: 98 additions & 20 deletions src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,8 @@
#include "ekf.h"
#include <mathlib/mathlib.h>

#include <lib/world_magnetic_model/geo_mag_declination.h>

#include <ekf_derivation/generated/compute_mag_innov_innov_var_and_hx.h>

void Ekf::controlMagFusion()
Expand Down Expand Up @@ -78,15 +80,42 @@ void Ekf::controlMagFusion()
_mag_counter++;
}

// check for WMM update periodically or if global origin has changed
bool wmm_updated = false;

if (global_origin().isInitialized()) {
if ((global_origin().getProjectionReferenceTimestamp() > aid_src.time_last_fuse)
|| (local_position_is_valid() && isTimedOut(_wmm_mag_time_last_checked, 10e6))
) {
if (local_position_is_valid()) {
// position of local NED origin in GPS / WGS84 frame
double latitude_deg;
double longitude_deg;
global_origin().reproject(_state.pos(0), _state.pos(1), latitude_deg, longitude_deg);

if (updateWorldMagneticModel(latitude_deg, longitude_deg)) {
wmm_updated = true;
}

} else {
// use global origin
if (updateWorldMagneticModel(global_origin().getProjectionReferenceLat(),
global_origin().getProjectionReferenceLon())
) {
wmm_updated = true;
}
}

_wmm_mag_time_last_checked = _time_delayed_us;
}
}

// if enabled, use knowledge of theoretical magnetic field vector to calculate a synthetic magnetomter Z component value.
// this is useful if there is a lot of interference on the sensor measurement.
if (_params.synthesize_mag_z && (_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL)
&& (PX4_ISFINITE(_mag_inclination_gps) && PX4_ISFINITE(_mag_declination_gps) && PX4_ISFINITE(_mag_strength_gps))
&& (_wmm_earth_field_gauss.isAllFinite() && _wmm_earth_field_gauss.longerThan(0.f))
) {
const Vector3f mag_earth_pred = Dcmf(Eulerf(0, -_mag_inclination_gps, _mag_declination_gps))
* Vector3f(_mag_strength_gps, 0, 0);

mag_sample.mag(2) = calculate_synthetic_mag_z_measurement(mag_sample.mag, mag_earth_pred);
mag_sample.mag(2) = calculate_synthetic_mag_z_measurement(mag_sample.mag, _wmm_earth_field_gauss);

_control_status.flags.synthetic_mag_z = true;

Expand Down Expand Up @@ -145,8 +174,6 @@ void Ekf::controlMagFusion()

checkMagHeadingConsistency(mag_sample);

// WMM update can occur on the last epoch, just after mag fusion
const bool wmm_updated = (_wmm_gps_time_last_set >= aid_src.time_last_fuse);
const bool using_ne_aiding = _control_status.flags.gps || _control_status.flags.aux_gpos;


Expand Down Expand Up @@ -208,7 +235,21 @@ void Ekf::controlMagFusion()
}

if (_control_status.flags.mag_dec) {
fuseDeclination(0.5f);

if ((_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL)
&& PX4_ISFINITE(_wmm_declination_rad)
) {
fuseDeclination(_wmm_declination_rad, 0.5f);

} else if ((_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL)
&& PX4_ISFINITE(_params.mag_declination_deg) && (fabsf(_params.mag_declination_deg) > 0.f)
) {

fuseDeclination(math::radians(_params.mag_declination_deg), 0.5f);

} else {
_control_status.flags.mag_dec = false;
}
}
}

Expand Down Expand Up @@ -326,24 +367,21 @@ void Ekf::resetMagStates(const Vector3f &mag, bool reset_heading)
resetMagCov();

// if world magnetic model (inclination, declination, strength) available then use it to reset mag states
if (PX4_ISFINITE(_mag_inclination_gps) && PX4_ISFINITE(_mag_declination_gps) && PX4_ISFINITE(_mag_strength_gps)) {

if (_wmm_earth_field_gauss.longerThan(0.f) && _wmm_earth_field_gauss.isAllFinite()) {
// use expected earth field to reset states
const Vector3f mag_earth_pred = Dcmf(Eulerf(0, -_mag_inclination_gps, _mag_declination_gps))
* Vector3f(_mag_strength_gps, 0, 0);

// mag_B: reset
if (!reset_heading && _control_status.flags.yaw_align) {
// mag_B: reset using WMM
const Dcmf R_to_body = quatToInverseRotMat(_state.quat_nominal);
_state.mag_B = mag - (R_to_body * mag_earth_pred);
_state.mag_B = mag - (R_to_body * _wmm_earth_field_gauss);

} else {
_state.mag_B.zero();
}

// mag_I: reset, skipped if no change in state and variance good
_state.mag_I = mag_earth_pred;
_state.mag_I = _wmm_earth_field_gauss;

if (reset_heading) {
resetMagHeading(mag);
Expand Down Expand Up @@ -458,8 +496,8 @@ bool Ekf::checkMagField(const Vector3f &mag_sample)
_mag_strength = mag_sample.length();

if (_params.mag_check & static_cast<int32_t>(MagCheckMask::STRENGTH)) {
if (PX4_ISFINITE(_mag_strength_gps)) {
if (!isMeasuredMatchingExpected(_mag_strength, _mag_strength_gps, _params.mag_check_strength_tolerance_gs)) {
if (PX4_ISFINITE(_wmm_field_strength_gauss)) {
if (!isMeasuredMatchingExpected(_mag_strength, _wmm_field_strength_gauss, _params.mag_check_strength_tolerance_gs)) {
_control_status.flags.mag_field_disturbed = true;
is_check_failing = true;
}
Expand All @@ -482,9 +520,9 @@ bool Ekf::checkMagField(const Vector3f &mag_sample)
_mag_inclination = asinf(mag_earth(2) / fmaxf(mag_earth.norm(), 1e-4f));

if (_params.mag_check & static_cast<int32_t>(MagCheckMask::INCLINATION)) {
if (PX4_ISFINITE(_mag_inclination_gps)) {
if (PX4_ISFINITE(_wmm_inclination_rad)) {
const float inc_tol_rad = radians(_params.mag_check_inclination_tolerance_deg);
const float inc_error_rad = wrap_pi(_mag_inclination - _mag_inclination_gps);
const float inc_error_rad = wrap_pi(_mag_inclination - _wmm_inclination_rad);

if (fabsf(inc_error_rad) > inc_tol_rad) {
_control_status.flags.mag_field_disturbed = true;
Expand Down Expand Up @@ -558,12 +596,52 @@ float Ekf::getMagDeclination()

} else if (_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL) {
// use parameter value until GPS is available, then use value returned by geo library
if (PX4_ISFINITE(_mag_declination_gps)) {
return _mag_declination_gps;
if (PX4_ISFINITE(_wmm_declination_rad)) {
return _wmm_declination_rad;
}
}

// otherwise use the parameter value
return math::radians(_params.mag_declination_deg);
}

bool Ekf::updateWorldMagneticModel(const double latitude_deg, const double longitude_deg)
{
// set the magnetic field data returned by the geo library using the current GPS position
const float declination_rad = math::radians(get_mag_declination_degrees(latitude_deg, longitude_deg));
const float inclination_rad = math::radians(get_mag_inclination_degrees(latitude_deg, longitude_deg));
const float strength_gauss = get_mag_strength_gauss(latitude_deg, longitude_deg);

if (PX4_ISFINITE(declination_rad) && PX4_ISFINITE(inclination_rad) && PX4_ISFINITE(strength_gauss)) {

const bool declination_changed = (fabsf(declination_rad - _wmm_declination_rad) > math::radians(1.f));
const bool inclination_changed = (fabsf(inclination_rad - _wmm_inclination_rad) > math::radians(1.f));
const bool strength_changed = (fabsf(strength_gauss - _wmm_field_strength_gauss) > 0.01f);

if (!PX4_ISFINITE(_wmm_declination_rad)
|| !PX4_ISFINITE(_wmm_inclination_rad)
|| !PX4_ISFINITE(_wmm_field_strength_gauss)
|| !_wmm_earth_field_gauss.longerThan(0.f)
|| !_wmm_earth_field_gauss.isAllFinite()
|| declination_changed
|| inclination_changed
|| strength_changed
) {

ECL_DEBUG("WMM declination updated %.3f -> %.3f deg (lat=%.6f, lon=%.6f)",
(double)math::degrees(_wmm_declination_rad), (double)math::degrees(declination_rad),
(double)latitude_deg, (double)longitude_deg
);

_wmm_declination_rad = declination_rad;
_wmm_inclination_rad = inclination_rad;
_wmm_field_strength_gauss = strength_gauss;

_wmm_earth_field_gauss = Dcmf(Eulerf(0, -inclination_rad, declination_rad)) * Vector3f(strength_gauss, 0, 0);

return true;
}
}

return false;
}
53 changes: 18 additions & 35 deletions src/modules/ekf2/EKF/aid_sources/magnetometer/mag_fusion.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -147,50 +147,33 @@ bool Ekf::fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estima
return false;
}

bool Ekf::fuseDeclination(float decl_sigma)
bool Ekf::fuseDeclination(float decl_measurement_rad, float decl_sigma)
{
float decl_measurement = NAN;
// observation variance (rad**2)
const float R_DECL = sq(decl_sigma);

if ((_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL)
&& PX4_ISFINITE(_mag_declination_gps)
) {
decl_measurement = _mag_declination_gps;
VectorState H;
float decl_pred;
float innovation_variance;

} else if ((_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL)
&& PX4_ISFINITE(_params.mag_declination_deg) && (fabsf(_params.mag_declination_deg) > 0.f)
) {
decl_measurement = math::radians(_params.mag_declination_deg);
}

if (PX4_ISFINITE(decl_measurement)) {

// observation variance (rad**2)
const float R_DECL = sq(decl_sigma);

VectorState H;
float decl_pred;
float innovation_variance;

sym::ComputeMagDeclinationPredInnovVarAndH(_state.vector(), P, R_DECL, FLT_EPSILON, &decl_pred, &innovation_variance, &H);

const float innovation = wrap_pi(decl_pred - decl_measurement);
sym::ComputeMagDeclinationPredInnovVarAndH(_state.vector(), P, R_DECL, FLT_EPSILON, &decl_pred, &innovation_variance, &H);

if (innovation_variance < R_DECL) {
// variance calculation is badly conditioned
return false;
}
const float innovation = wrap_pi(decl_pred - decl_measurement_rad);

// Calculate the Kalman gains
VectorState Kfusion = P * H / innovation_variance;
if (innovation_variance < R_DECL) {
// variance calculation is badly conditioned
_fault_status.flags.bad_mag_decl = true;
return false;
}

const bool is_fused = measurementUpdate(Kfusion, H, R_DECL, innovation);
// Calculate the Kalman gains
VectorState Kfusion = P * H / innovation_variance;

_fault_status.flags.bad_mag_decl = !is_fused;
const bool is_fused = measurementUpdate(Kfusion, H, R_DECL, innovation);

return is_fused;
}
_fault_status.flags.bad_mag_decl = !is_fused;

return false;
return is_fused;
}

float Ekf::calculate_synthetic_mag_z_measurement(const Vector3f &mag_meas, const Vector3f &mag_earth_predicted)
Expand Down
3 changes: 1 addition & 2 deletions src/modules/ekf2/EKF/ekf.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -332,8 +332,7 @@ void Ekf::predictState(const imuSample &imu_delayed)

void Ekf::resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float accuracy, uint64_t timestamp_observation)
{

if (!_pos_ref.isInitialized()) {
if (!global_origin_valid()) {
return;
}

Expand Down
Loading

0 comments on commit dae97b0

Please sign in to comment.