mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
ekf2: refactor mag control
- remove class _mag_sample_delayed - update mag fusion call graph to use mag sample delayed functionally - Ekf::resetMagHeading() - use low pass mag directly, but check if valid (magnitude) - MAG_FUSE_TYPE_INDOOR treat like auto if heading required
This commit is contained in:
Daniel Agar
GitHub
parent
ad447ab223
commit
999737ddd5
@ -110,31 +110,6 @@ void Ekf::controlFusionModes()
|
||||
_gps_data_ready = _gps_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed);
|
||||
}
|
||||
|
||||
|
||||
if (_mag_buffer) {
|
||||
_mag_data_ready = _mag_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed);
|
||||
|
||||
if (_mag_data_ready) {
|
||||
_mag_lpf.update(_mag_sample_delayed.mag);
|
||||
|
||||
// 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 & MASK_USE_GEO_DECL)
|
||||
&& (_NED_origin_initialised || PX4_ISFINITE(_mag_declination_gps))
|
||||
) {
|
||||
|
||||
const Vector3f mag_earth_pred = Dcmf(Eulerf(0, -_mag_inclination_gps, _mag_declination_gps)) * Vector3f(_mag_strength_gps, 0, 0);
|
||||
_mag_sample_delayed.mag(2) = calculate_synthetic_mag_z_measurement(_mag_sample_delayed.mag, mag_earth_pred);
|
||||
_control_status.flags.synthetic_mag_z = true;
|
||||
|
||||
} else {
|
||||
_control_status.flags.synthetic_mag_z = false;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
if (_range_buffer) {
|
||||
// Get range data from buffer and check validity
|
||||
bool is_rng_data_ready = _range_buffer->pop_first_older_than(_imu_sample_delayed.time_us, _range_sensor.getSampleAddress());
|
||||
@ -371,7 +346,14 @@ void Ekf::controlExternalVisionFusion()
|
||||
resetYawToEv();
|
||||
}
|
||||
|
||||
fuseHeading();
|
||||
if (shouldUse321RotationSequence(_R_to_earth)) {
|
||||
float measured_hdg = getEuler321Yaw(_ev_sample_delayed.quat);
|
||||
fuseYaw321(measured_hdg, _ev_sample_delayed.angVar);
|
||||
|
||||
} else {
|
||||
float measured_hdg = getEuler312Yaw(_ev_sample_delayed.quat);
|
||||
fuseYaw312(measured_hdg, _ev_sample_delayed.angVar);
|
||||
}
|
||||
}
|
||||
|
||||
// record observation and estimate for use next time
|
||||
|
||||
@ -143,16 +143,20 @@ bool Ekf::initialiseFilter()
|
||||
}
|
||||
|
||||
// Sum the magnetometer measurements
|
||||
if (_mag_buffer && _mag_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) {
|
||||
if (_mag_sample_delayed.time_us != 0) {
|
||||
if (_mag_counter == 0) {
|
||||
_mag_lpf.reset(_mag_sample_delayed.mag);
|
||||
if (_mag_buffer) {
|
||||
magSample mag_sample;
|
||||
|
||||
} else {
|
||||
_mag_lpf.update(_mag_sample_delayed.mag);
|
||||
if (_mag_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &mag_sample)) {
|
||||
if (mag_sample.time_us != 0) {
|
||||
if (_mag_counter == 0) {
|
||||
_mag_lpf.reset(mag_sample.mag);
|
||||
|
||||
} else {
|
||||
_mag_lpf.update(mag_sample.mag);
|
||||
}
|
||||
|
||||
_mag_counter++;
|
||||
}
|
||||
|
||||
_mag_counter++;
|
||||
}
|
||||
}
|
||||
|
||||
@ -192,7 +196,7 @@ bool Ekf::initialiseFilter()
|
||||
// calculate the initial magnetic field and yaw alignment
|
||||
// but do not mark the yaw alignement complete as it needs to be
|
||||
// reset once the leveling phase is done
|
||||
resetMagHeading(_mag_lpf.getState(), false, false);
|
||||
resetMagHeading(false, false);
|
||||
|
||||
// initialise the state covariance matrix now we have starting values for all the states
|
||||
initialiseCovariance();
|
||||
|
||||
@ -364,7 +364,6 @@ private:
|
||||
|
||||
// booleans true when fresh sensor data is available at the fusion time horizon
|
||||
bool _gps_data_ready{false}; ///< true when new GPS data has fallen behind the fusion time horizon and is available to be fused
|
||||
bool _mag_data_ready{false}; ///< true when new magnetometer data has fallen behind the fusion time horizon and is available to be fused
|
||||
bool _baro_data_ready{false}; ///< true when new baro height data has fallen behind the fusion time horizon and is available to be fused
|
||||
bool _flow_data_ready{false}; ///< true when the leading edge of the optical flow integration period has fallen behind the fusion time horizon
|
||||
bool _ev_data_ready{false}; ///< true when new external vision system data has fallen behind the fusion time horizon and is available to be fused
|
||||
@ -577,22 +576,22 @@ private:
|
||||
void predictCovariance();
|
||||
|
||||
// ekf sequential fusion of magnetometer measurements
|
||||
void fuseMag();
|
||||
void fuseMag(const Vector3f &mag);
|
||||
|
||||
// fuse the first euler angle from either a 321 or 312 rotation sequence as the observation (currently measures yaw using the magnetometer)
|
||||
void fuseHeading();
|
||||
void fuseHeading(float measured_hdg = NAN, float obs_var = NAN);
|
||||
|
||||
// fuse the yaw angle defined as the first rotation in a 321 Tait-Bryan rotation sequence
|
||||
// yaw : angle observation defined as the first rotation in a 321 Tait-Bryan rotation sequence (rad)
|
||||
// yaw_variance : variance of the yaw angle observation (rad^2)
|
||||
// zero_innovation : Fuse data with innovation set to zero
|
||||
void fuseYaw321(const float yaw, const float yaw_variance, bool zero_innovation);
|
||||
void fuseYaw321(const float yaw, const float yaw_variance, bool zero_innovation = false);
|
||||
|
||||
// fuse the yaw angle defined as the first rotation in a 312 Tait-Bryan rotation sequence
|
||||
// yaw : angle observation defined as the first rotation in a 312 Tait-Bryan rotation sequence (rad)
|
||||
// yaw_variance : variance of the yaw angle observation (rad^2)
|
||||
// zero_innovation : Fuse data with innovation set to zero
|
||||
void fuseYaw312(const float yaw, const float yaw_variance, bool zero_innovation);
|
||||
void fuseYaw312(const float yaw, const float yaw_variance, bool zero_innovation = false);
|
||||
|
||||
// update quaternion states and covariances using an innovation, observation variance and Jacobian vector
|
||||
// innovation : prediction - measurement
|
||||
@ -700,7 +699,7 @@ private:
|
||||
|
||||
// reset the heading and magnetic field states using the declination and magnetometer measurements
|
||||
// return true if successful
|
||||
bool resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var = true, bool update_buffer = true);
|
||||
bool resetMagHeading(bool increase_yaw_var = true, bool update_buffer = true);
|
||||
|
||||
// reset the heading using the external vision measurements
|
||||
// return true if successful
|
||||
@ -708,7 +707,7 @@ private:
|
||||
|
||||
// Do a forced re-alignment of the yaw angle to align with the horizontal velocity vector from the GPS.
|
||||
// It is used to align the yaw angle after launch or takeoff for fixed wing vehicle.
|
||||
bool realignYawGPS();
|
||||
bool realignYawGPS(const Vector3f &mag);
|
||||
|
||||
// Return the magnetic declination in radians to be used by the alignment and fusion processing
|
||||
float getMagDeclination();
|
||||
@ -839,7 +838,7 @@ private:
|
||||
void runOnGroundYawReset();
|
||||
bool isYawResetAuthorized() const { return !_is_yaw_fusion_inhibited; }
|
||||
bool canResetMagHeading() const;
|
||||
void runInAirYawReset();
|
||||
void runInAirYawReset(const Vector3f &mag);
|
||||
bool canRealignYawUsingGps() const { return _control_status.flags.fixed_wing; }
|
||||
void runVelPosReset();
|
||||
|
||||
@ -854,11 +853,11 @@ private:
|
||||
void checkMagDeclRequired();
|
||||
void checkMagInhibition();
|
||||
bool shouldInhibitMag() const;
|
||||
void checkMagFieldStrength();
|
||||
void checkMagFieldStrength(const Vector3f &mag);
|
||||
bool isStrongMagneticDisturbance() const { return _control_status.flags.mag_field_disturbed; }
|
||||
static bool isMeasuredMatchingExpected(float measured, float expected, float gate);
|
||||
void runMagAndMagDeclFusions();
|
||||
void run3DMagAndDeclFusions();
|
||||
void runMagAndMagDeclFusions(const Vector3f &mag);
|
||||
void run3DMagAndDeclFusions(const Vector3f &mag);
|
||||
|
||||
// control fusion of range finder observations
|
||||
void controlRangeFinderFusion();
|
||||
|
||||
@ -376,7 +376,7 @@ void Ekf::alignOutputFilter()
|
||||
|
||||
// Do a forced re-alignment of the yaw angle to align with the horizontal velocity vector from the GPS.
|
||||
// It is used to align the yaw angle after launch or takeoff for fixed wing vehicle only.
|
||||
bool Ekf::realignYawGPS()
|
||||
bool Ekf::realignYawGPS(const Vector3f &mag)
|
||||
{
|
||||
const float gpsSpeed = sqrtf(sq(_gps_sample_delayed.vel(0)) + sq(_gps_sample_delayed.vel(1)));
|
||||
|
||||
@ -386,7 +386,7 @@ bool Ekf::realignYawGPS()
|
||||
|
||||
if (!gps_yaw_alignment_possible) {
|
||||
// attempt a normal alignment using the magnetometer
|
||||
return resetMagHeading(_mag_lpf.getState());
|
||||
return resetMagHeading();
|
||||
}
|
||||
|
||||
// check for excessive horizontal GPS velocity innovations
|
||||
@ -455,7 +455,7 @@ bool Ekf::realignYawGPS()
|
||||
// Use the last magnetometer measurements to reset the field states
|
||||
_state.mag_B.zero();
|
||||
_R_to_earth = Dcmf(_state.quat_nominal);
|
||||
_state.mag_I = _R_to_earth * _mag_sample_delayed.mag;
|
||||
_state.mag_I = _R_to_earth * mag;
|
||||
|
||||
resetMagCov();
|
||||
|
||||
@ -471,7 +471,7 @@ bool Ekf::realignYawGPS()
|
||||
// align mag states only
|
||||
|
||||
// calculate initial earth magnetic field states
|
||||
_state.mag_I = _R_to_earth * _mag_sample_delayed.mag;
|
||||
_state.mag_I = _R_to_earth * mag;
|
||||
|
||||
resetMagCov();
|
||||
|
||||
@ -483,18 +483,28 @@ bool Ekf::realignYawGPS()
|
||||
}
|
||||
|
||||
// Reset heading and magnetic field states
|
||||
bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool update_buffer)
|
||||
bool Ekf::resetMagHeading(bool increase_yaw_var, bool update_buffer)
|
||||
{
|
||||
// prevent a reset being performed more than once on the same frame
|
||||
if (_imu_sample_delayed.time_us == _flt_mag_align_start_time) {
|
||||
return true;
|
||||
}
|
||||
|
||||
// low pass filtered mag required
|
||||
if (_mag_counter == 0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
const Vector3f mag_init = _mag_lpf.getState();
|
||||
|
||||
// calculate the observed yaw angle and yaw variance
|
||||
float yaw_new;
|
||||
float yaw_new_variance = 0.0f;
|
||||
|
||||
if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
|
||||
const bool heading_required_for_navigation = _control_status.flags.gps || _control_status.flags.ev_pos;
|
||||
|
||||
if ((_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) || ((_params.mag_fusion_type == MAG_FUSE_TYPE_INDOOR) && heading_required_for_navigation)) {
|
||||
|
||||
// rotate the magnetometer measurements into earth frame using a zero yaw angle
|
||||
const Dcmf R_to_earth = updateYawInRotMat(0.f, _R_to_earth);
|
||||
|
||||
|
||||
@ -299,7 +299,6 @@ protected:
|
||||
imuSample _imu_sample_delayed{}; // captures the imu sample on the delayed time horizon
|
||||
|
||||
// measurement samples capturing measurements on the delayed time horizon
|
||||
magSample _mag_sample_delayed{};
|
||||
baroSample _baro_sample_delayed{};
|
||||
gpsSample _gps_sample_delayed{};
|
||||
sensor::SensorRangeFinder _range_sensor{};
|
||||
|
||||
@ -70,7 +70,7 @@ void Ekf::controlGpsFusion()
|
||||
|
||||
fuseGpsVelPos();
|
||||
|
||||
if (shouldResetGpsFusion()){
|
||||
if (shouldResetGpsFusion()) {
|
||||
const bool was_gps_signal_lost = isTimedOut(_time_prev_gps_us, 1000000);
|
||||
|
||||
/* A reset is not performed when getting GPS back after a significant period of no data
|
||||
@ -88,7 +88,7 @@ void Ekf::controlGpsFusion()
|
||||
// use GPS velocity data to check and correct yaw angle if a FW vehicle
|
||||
if (_control_status.flags.fixed_wing && _control_status.flags.in_air) {
|
||||
// if flying a fixed wing aircraft, do a complete reset that includes yaw
|
||||
_control_status.flags.mag_aligned_in_flight = realignYawGPS();
|
||||
_mag_yaw_reset_req = true;
|
||||
}
|
||||
|
||||
_warning_events.flags.gps_fusion_timout = true;
|
||||
@ -131,8 +131,7 @@ void Ekf::controlGpsFusion()
|
||||
startGpsFusion();
|
||||
}
|
||||
|
||||
} else if(!_control_status.flags.yaw_align
|
||||
&& (_params.mag_fusion_type == MAG_FUSE_TYPE_NONE)) {
|
||||
} else if (!_control_status.flags.yaw_align && (_params.mag_fusion_type == MAG_FUSE_TYPE_NONE)) {
|
||||
// If no mag is used, align using the yaw estimator
|
||||
_do_ekfgsf_yaw_reset = true;
|
||||
}
|
||||
@ -208,7 +207,7 @@ void Ekf::processYawEstimatorResetRequest()
|
||||
* to improve its estimate if the previous reset was not successful.
|
||||
*/
|
||||
if (_do_ekfgsf_yaw_reset
|
||||
&& isTimedOut(_ekfgsf_yaw_reset_time, 5000000)){
|
||||
&& isTimedOut(_ekfgsf_yaw_reset_time, 5000000)) {
|
||||
if (resetYawToEKFGSF()) {
|
||||
_ekfgsf_yaw_reset_time = _time_last_imu;
|
||||
_time_last_hor_pos_fuse = _time_last_imu;
|
||||
|
||||
@ -41,7 +41,35 @@
|
||||
|
||||
void Ekf::controlMagFusion()
|
||||
{
|
||||
checkMagFieldStrength();
|
||||
bool mag_data_ready = false;
|
||||
|
||||
magSample mag_sample;
|
||||
|
||||
if (_mag_buffer) {
|
||||
mag_data_ready = _mag_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &mag_sample);
|
||||
|
||||
if (mag_data_ready) {
|
||||
_mag_lpf.update(mag_sample.mag);
|
||||
_mag_counter++;
|
||||
|
||||
// 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 & MASK_USE_GEO_DECL)
|
||||
&& (_NED_origin_initialised || PX4_ISFINITE(_mag_declination_gps))
|
||||
) {
|
||||
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);
|
||||
_control_status.flags.synthetic_mag_z = true;
|
||||
|
||||
} else {
|
||||
_control_status.flags.synthetic_mag_z = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (mag_data_ready) {
|
||||
checkMagFieldStrength(mag_sample.mag);
|
||||
}
|
||||
|
||||
// If we are on ground, reset the flight alignment flag so that the mag fields will be
|
||||
// re-initialised next time we achieve flight altitude
|
||||
@ -76,7 +104,7 @@ void Ekf::controlMagFusion()
|
||||
_mag_yaw_reset_req |= !_control_status.flags.yaw_align;
|
||||
_mag_yaw_reset_req |= _mag_inhibit_yaw_reset_req;
|
||||
|
||||
if (noOtherYawAidingThanMag() && _mag_data_ready) {
|
||||
if (noOtherYawAidingThanMag() && mag_data_ready) {
|
||||
// Determine if we should use simple magnetic heading fusion which works better when
|
||||
// there are large external disturbances or the more accurate 3-axis fusion
|
||||
switch (_params.mag_fusion_type) {
|
||||
@ -101,7 +129,7 @@ void Ekf::controlMagFusion()
|
||||
|
||||
if (_control_status.flags.in_air) {
|
||||
checkHaglYawResetReq();
|
||||
runInAirYawReset();
|
||||
runInAirYawReset(mag_sample.mag);
|
||||
runVelPosReset(); // TODO: review this; a vel/pos reset can be requested from COG reset (for fixedwing) only
|
||||
|
||||
} else {
|
||||
@ -116,7 +144,7 @@ void Ekf::controlMagFusion()
|
||||
checkMagDeclRequired();
|
||||
checkMagInhibition();
|
||||
|
||||
runMagAndMagDeclFusions();
|
||||
runMagAndMagDeclFusions(mag_sample.mag);
|
||||
}
|
||||
}
|
||||
|
||||
@ -142,9 +170,7 @@ void Ekf::checkHaglYawResetReq()
|
||||
void Ekf::runOnGroundYawReset()
|
||||
{
|
||||
if (_mag_yaw_reset_req && isYawResetAuthorized()) {
|
||||
const bool has_realigned_yaw = canResetMagHeading()
|
||||
? resetMagHeading(_mag_lpf.getState())
|
||||
: false;
|
||||
const bool has_realigned_yaw = canResetMagHeading() ? resetMagHeading() : false;
|
||||
|
||||
if (has_realigned_yaw) {
|
||||
_mag_yaw_reset_req = false;
|
||||
@ -166,16 +192,16 @@ bool Ekf::canResetMagHeading() const
|
||||
return !isStrongMagneticDisturbance() && (_params.mag_fusion_type != MAG_FUSE_TYPE_NONE);
|
||||
}
|
||||
|
||||
void Ekf::runInAirYawReset()
|
||||
void Ekf::runInAirYawReset(const Vector3f &mag_sample)
|
||||
{
|
||||
if (_mag_yaw_reset_req && isYawResetAuthorized()) {
|
||||
bool has_realigned_yaw = false;
|
||||
|
||||
if (canRealignYawUsingGps()) {
|
||||
has_realigned_yaw = realignYawGPS();
|
||||
has_realigned_yaw = realignYawGPS(mag_sample);
|
||||
|
||||
} else if (canResetMagHeading()) {
|
||||
has_realigned_yaw = resetMagHeading(_mag_lpf.getState());
|
||||
has_realigned_yaw = resetMagHeading();
|
||||
}
|
||||
|
||||
if (has_realigned_yaw) {
|
||||
@ -300,25 +326,23 @@ bool Ekf::shouldInhibitMag() const
|
||||
|| isStrongMagneticDisturbance();
|
||||
}
|
||||
|
||||
void Ekf::checkMagFieldStrength()
|
||||
void Ekf::checkMagFieldStrength(const Vector3f &mag_sample)
|
||||
{
|
||||
if (_mag_data_ready) {
|
||||
if (_params.check_mag_strength
|
||||
&& ((_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) || (_params.mag_fusion_type == MAG_FUSE_TYPE_INDOOR && _control_status.flags.gps))) {
|
||||
if (_params.check_mag_strength
|
||||
&& ((_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) || (_params.mag_fusion_type == MAG_FUSE_TYPE_INDOOR && _control_status.flags.gps))) {
|
||||
|
||||
if (PX4_ISFINITE(_mag_strength_gps)) {
|
||||
constexpr float wmm_gate_size = 0.2f; // +/- Gauss
|
||||
_control_status.flags.mag_field_disturbed = !isMeasuredMatchingExpected(_mag_sample_delayed.mag.length(), _mag_strength_gps, wmm_gate_size);
|
||||
|
||||
} else {
|
||||
constexpr float average_earth_mag_field_strength = 0.45f; // Gauss
|
||||
constexpr float average_earth_mag_gate_size = 0.40f; // +/- Gauss
|
||||
_control_status.flags.mag_field_disturbed = !isMeasuredMatchingExpected(_mag_sample_delayed.mag.length(), average_earth_mag_field_strength, average_earth_mag_gate_size);
|
||||
}
|
||||
if (PX4_ISFINITE(_mag_strength_gps)) {
|
||||
constexpr float wmm_gate_size = 0.2f; // +/- Gauss
|
||||
_control_status.flags.mag_field_disturbed = !isMeasuredMatchingExpected(mag_sample.length(), _mag_strength_gps, wmm_gate_size);
|
||||
|
||||
} else {
|
||||
_control_status.flags.mag_field_disturbed = false;
|
||||
constexpr float average_earth_mag_field_strength = 0.45f; // Gauss
|
||||
constexpr float average_earth_mag_gate_size = 0.40f; // +/- Gauss
|
||||
_control_status.flags.mag_field_disturbed = !isMeasuredMatchingExpected(mag_sample.length(), average_earth_mag_field_strength, average_earth_mag_gate_size);
|
||||
}
|
||||
|
||||
} else {
|
||||
_control_status.flags.mag_field_disturbed = false;
|
||||
}
|
||||
}
|
||||
|
||||
@ -328,17 +352,25 @@ bool Ekf::isMeasuredMatchingExpected(const float measured, const float expected,
|
||||
&& (measured <= expected + gate);
|
||||
}
|
||||
|
||||
void Ekf::runMagAndMagDeclFusions()
|
||||
void Ekf::runMagAndMagDeclFusions(const Vector3f &mag)
|
||||
{
|
||||
if (_control_status.flags.mag_3D) {
|
||||
run3DMagAndDeclFusions();
|
||||
run3DMagAndDeclFusions(mag);
|
||||
|
||||
} else if (_control_status.flags.mag_hdg) {
|
||||
fuseHeading();
|
||||
// Rotate the measurements into earth frame using the zero yaw angle
|
||||
Dcmf R_to_earth = shouldUse321RotationSequence(_R_to_earth) ? updateEuler321YawInRotMat(0.f, _R_to_earth) : updateEuler312YawInRotMat(0.f, _R_to_earth);
|
||||
|
||||
Vector3f mag_earth_pred = R_to_earth * (mag - _state.mag_B);
|
||||
|
||||
// the angle of the projection onto the horizontal gives the yaw angle
|
||||
float measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();
|
||||
|
||||
fuseHeading(measured_hdg, sq(_params.mag_heading_noise));
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::run3DMagAndDeclFusions()
|
||||
void Ekf::run3DMagAndDeclFusions(const Vector3f &mag)
|
||||
{
|
||||
if (!_mag_decl_cov_reset) {
|
||||
// After any magnetic field covariance reset event the earth field state
|
||||
@ -347,13 +379,13 @@ void Ekf::run3DMagAndDeclFusions()
|
||||
// states for the first few observations.
|
||||
fuseDeclination(0.02f);
|
||||
_mag_decl_cov_reset = true;
|
||||
fuseMag();
|
||||
fuseMag(mag);
|
||||
|
||||
} 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.
|
||||
fuseMag();
|
||||
fuseMag(mag);
|
||||
|
||||
if (_control_status.flags.mag_dec) {
|
||||
fuseDeclination(0.5f);
|
||||
|
||||
@ -45,7 +45,7 @@
|
||||
|
||||
#include <mathlib/mathlib.h>
|
||||
|
||||
void Ekf::fuseMag()
|
||||
void Ekf::fuseMag(const Vector3f &mag)
|
||||
{
|
||||
// assign intermediate variables
|
||||
const float &q0 = _state.quat_nominal(0);
|
||||
@ -161,7 +161,7 @@ void Ekf::fuseMag()
|
||||
const Vector3f mag_I_rot = R_to_body * _state.mag_I;
|
||||
|
||||
// compute magnetometer innovations
|
||||
_mag_innov = mag_I_rot + _state.mag_B - _mag_sample_delayed.mag;
|
||||
_mag_innov = mag_I_rot + _state.mag_B - mag;
|
||||
|
||||
// do not use the synthesized measurement for the magnetomter Z component for 3D fusion
|
||||
if (_control_status.flags.synthetic_mag_z) {
|
||||
@ -714,149 +714,62 @@ void Ekf::updateQuaternion(const float innovation, const float variance, const f
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::fuseHeading()
|
||||
void Ekf::fuseHeading(float measured_hdg, float obs_var)
|
||||
{
|
||||
Vector3f mag_earth_pred;
|
||||
float measured_hdg;
|
||||
|
||||
// Calculate the observation variance
|
||||
float R_YAW;
|
||||
|
||||
if (_control_status.flags.mag_hdg) {
|
||||
// using magnetic heading tuning parameter
|
||||
R_YAW = sq(_params.mag_heading_noise);
|
||||
|
||||
} else if (_control_status.flags.ev_yaw) {
|
||||
// using error estimate from external vision data
|
||||
R_YAW = _ev_sample_delayed.angVar;
|
||||
|
||||
} else {
|
||||
// default value
|
||||
R_YAW = 0.01f;
|
||||
}
|
||||
// observation variance
|
||||
float R_YAW = PX4_ISFINITE(obs_var) ? obs_var : 0.01f;
|
||||
|
||||
// update transformation matrix from body to world frame using the current state estimate
|
||||
_R_to_earth = Dcmf(_state.quat_nominal);
|
||||
|
||||
if (shouldUse321RotationSequence(_R_to_earth)) {
|
||||
const float predicted_hdg = getEuler321Yaw(_R_to_earth);
|
||||
const bool use_321_rotation_seq = shouldUse321RotationSequence(_R_to_earth);
|
||||
|
||||
if (_control_status.flags.mag_hdg) {
|
||||
// Rotate the measurements into earth frame using the zero yaw angle
|
||||
const Dcmf R_to_earth = updateEuler321YawInRotMat(0.f, _R_to_earth);
|
||||
mag_earth_pred = R_to_earth * (_mag_sample_delayed.mag - _state.mag_B);
|
||||
const float predicted_hdg = use_321_rotation_seq ? getEuler321Yaw(_R_to_earth) : getEuler312Yaw(_R_to_earth);
|
||||
|
||||
// the angle of the projection onto the horizontal gives the yaw angle
|
||||
measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();
|
||||
if (!PX4_ISFINITE(measured_hdg)) {
|
||||
measured_hdg = predicted_hdg;
|
||||
}
|
||||
|
||||
} else if (_control_status.flags.ev_yaw) {
|
||||
measured_hdg = getEuler321Yaw(_ev_sample_delayed.quat);
|
||||
// handle special case where yaw measurement is unavailable
|
||||
bool fuse_zero_innov = false;
|
||||
|
||||
} else {
|
||||
measured_hdg = predicted_hdg;
|
||||
}
|
||||
|
||||
// handle special case where yaw measurement is unavailable
|
||||
bool fuse_zero_innov = false;
|
||||
|
||||
if (_is_yaw_fusion_inhibited) {
|
||||
// The yaw measurement cannot be trusted but we need to fuse something to prevent a badly
|
||||
// conditioned covariance matrix developing over time.
|
||||
if (!_control_status.flags.vehicle_at_rest) {
|
||||
// Vehicle is not at rest so fuse a zero innovation if necessary to prevent
|
||||
// unconstrained quaternion variance growth and record the predicted heading
|
||||
// to use as an observation when movement ceases.
|
||||
// TODO a better way of determining when this is necessary
|
||||
const float sumQuatVar = P(0, 0) + P(1, 1) + P(2, 2) + P(3, 3);
|
||||
|
||||
if (sumQuatVar > _params.quat_max_variance) {
|
||||
fuse_zero_innov = true;
|
||||
R_YAW = 0.25f;
|
||||
|
||||
}
|
||||
|
||||
_last_static_yaw = predicted_hdg;
|
||||
|
||||
} else {
|
||||
// Vehicle is at rest so use the last moving prediction as an observation
|
||||
// to prevent the heading from drifting and to enable yaw gyro bias learning
|
||||
// before takeoff.
|
||||
|
||||
if (!PX4_ISFINITE(_last_static_yaw)) {
|
||||
_last_static_yaw = predicted_hdg;
|
||||
}
|
||||
|
||||
measured_hdg = _last_static_yaw;
|
||||
if (_is_yaw_fusion_inhibited) {
|
||||
// The yaw measurement cannot be trusted but we need to fuse something to prevent a badly
|
||||
// conditioned covariance matrix developing over time.
|
||||
if (!_control_status.flags.vehicle_at_rest) {
|
||||
// Vehicle is not at rest so fuse a zero innovation if necessary to prevent
|
||||
// unconstrained quaternion variance growth and record the predicted heading
|
||||
// to use as an observation when movement ceases.
|
||||
// TODO a better way of determining when this is necessary
|
||||
const float sumQuatVar = P(0, 0) + P(1, 1) + P(2, 2) + P(3, 3);
|
||||
|
||||
if (sumQuatVar > _params.quat_max_variance) {
|
||||
fuse_zero_innov = true;
|
||||
R_YAW = 0.25f;
|
||||
}
|
||||
|
||||
} else {
|
||||
_last_static_yaw = predicted_hdg;
|
||||
|
||||
} else {
|
||||
// Vehicle is at rest so use the last moving prediction as an observation
|
||||
// to prevent the heading from drifting and to enable yaw gyro bias learning
|
||||
// before takeoff.
|
||||
if (!PX4_ISFINITE(_last_static_yaw)) {
|
||||
_last_static_yaw = predicted_hdg;
|
||||
}
|
||||
|
||||
measured_hdg = _last_static_yaw;
|
||||
}
|
||||
|
||||
} else {
|
||||
_last_static_yaw = predicted_hdg;
|
||||
}
|
||||
|
||||
if (use_321_rotation_seq) {
|
||||
fuseYaw321(measured_hdg, R_YAW, fuse_zero_innov);
|
||||
|
||||
} else {
|
||||
const float predicted_hdg = getEuler312Yaw(_R_to_earth);
|
||||
|
||||
if (_control_status.flags.mag_hdg) {
|
||||
|
||||
// rotate the magnetometer measurements into earth frame using a zero yaw angle
|
||||
const Dcmf R_to_earth = updateEuler312YawInRotMat(0.f, _R_to_earth);
|
||||
mag_earth_pred = R_to_earth * (_mag_sample_delayed.mag - _state.mag_B);
|
||||
|
||||
// the angle of the projection onto the horizontal gives the yaw angle
|
||||
measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();
|
||||
|
||||
} else if (_control_status.flags.ev_yaw) {
|
||||
measured_hdg = getEuler312Yaw(_ev_sample_delayed.quat);
|
||||
|
||||
} else {
|
||||
measured_hdg = predicted_hdg;
|
||||
}
|
||||
|
||||
// handle special case where yaw measurement is unavailable
|
||||
bool fuse_zero_innov = false;
|
||||
|
||||
if (_is_yaw_fusion_inhibited) {
|
||||
// The yaw measurement cannot be trusted but we need to fuse something to prevent a badly
|
||||
// conditioned covariance matrix developing over time.
|
||||
if (!_control_status.flags.vehicle_at_rest) {
|
||||
// Vehicle is not at rest so fuse a zero innovation if necessary to prevent
|
||||
// unconstrained quaterniion variance growth and record the predicted heading
|
||||
// to use as an observation when movement ceases.
|
||||
// TODO a better way of determining when this is necessary
|
||||
const float sumQuatVar = P(0, 0) + P(1, 1) + P(2, 2) + P(3, 3);
|
||||
|
||||
if (sumQuatVar > _params.quat_max_variance) {
|
||||
fuse_zero_innov = true;
|
||||
R_YAW = 0.25f;
|
||||
|
||||
}
|
||||
|
||||
_last_static_yaw = predicted_hdg;
|
||||
|
||||
} else {
|
||||
// Vehicle is at rest so use the last moving prediction as an observation
|
||||
// to prevent the heading from drifting and to enable yaw gyro bias learning
|
||||
// before takeoff.
|
||||
|
||||
if (!PX4_ISFINITE(_last_static_yaw)) {
|
||||
_last_static_yaw = predicted_hdg;
|
||||
}
|
||||
|
||||
measured_hdg = _last_static_yaw;
|
||||
|
||||
}
|
||||
|
||||
} else {
|
||||
_last_static_yaw = predicted_hdg;
|
||||
|
||||
}
|
||||
|
||||
fuseYaw312(measured_hdg, R_YAW, fuse_zero_innov);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user