fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-01 23:24:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: Clean up use of magnetometer declination. Before the innovation was not zero in fuseDeclination()

Browse Source 
Signed-off-by: CarlOlsson <carlolsson.co@gmail.com>
This commit is contained in:
CarlOlsson 2019-03-12 15:08:45 +01:00 committed by Paul Riseborough
parent 1d91785a8e
commit 2b17ced405
5 changed files with 15 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
EKF/control.cpp
View File

@ -48,9 +48,6 @@ void Ekf::controlFusionModes()
// Store the status to enable change detection
_control_status_prev.value = _control_status.value;
// Get the magnetic declination
calcMagDeclination();
// monitor the tilt alignment
if (!_control_status.flags.tilt_align) {
// whilst we are aligning the tilt, monitor the variances

2
EKF/ekf.cpp
View File

@ -254,8 +254,6 @@ bool Ekf::initialiseFilter()
_R_to_earth = quat_to_invrotmat(_state.quat_nominal);
// calculate the initial magnetic field and yaw alignment
// Get the magnetic declination
calcMagDeclination();
_control_status.flags.yaw_align = resetMagHeading(_mag_filt_state, false, false);
// initialise the state covariance matrix

4
EKF/ekf.h
View File

@ -389,8 +389,6 @@ private:
bool _inhibit_flow_use{false}; ///< true when use of optical flow and range finder is being inhibited
Vector2f _flowRadXYcomp; ///< measured delta angle of the image about the X and Y body axes after removal of body rotation (rad), RH rotation is positive
float _mag_declination{0.0f}; ///< magnetic declination used by reset and fusion functions (rad)
// output predictor states
Vector3f _delta_angle_corr; ///< delta angle correction vector (rad)
imuSample _imu_down_sampled{}; ///< down sampled imu data (sensor rate -> filter update rate)
@ -553,7 +551,7 @@ private:
bool realignYawGPS();
// calculate the magnetic declination to be used by the alignment and fusion processing
void calcMagDeclination();
float getMagDeclination();
// reset position states of the ekf (only horizontal position)
bool resetPosition();

16
EKF/ekf_helper.cpp
View File

@ -610,7 +610,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init, bool increase_yaw_var, bool update
// rotate the magnetometer measurements into earth frame using a zero yaw angle
Vector3f mag_earth_pred = R_to_earth * mag_init;
// the angle of the projection onto the horizontal gives the yaw angle
euler321(2) = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + _mag_declination;
euler321(2) = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();
} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_INDOOR && _mag_use_inhibit) {
// we are operating without knowing the earth frame yaw angle
@ -668,7 +668,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init, bool increase_yaw_var, bool update
// rotate the magnetometer measurements into earth frame using a zero yaw angle
Vector3f mag_earth_pred = R_to_earth * mag_init;
// the angle of the projection onto the horizontal gives the yaw angle
euler312(0) = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + _mag_declination;
euler312(0) = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();
} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_INDOOR && _mag_use_inhibit) {
// we are operating without knowing the earth frame yaw angle
@ -767,28 +767,28 @@ bool Ekf::resetMagHeading(Vector3f &mag_init, bool increase_yaw_var, bool update
}
// Calculate the magnetic declination to be used by the alignment and fusion processing
void Ekf::calcMagDeclination()
float Ekf::getMagDeclination()
{
// set source of magnetic declination for internal use
if (_control_status.flags.mag_align_complete) {
// Use value consistent with earth field state
_mag_declination = atan2f(_state.mag_I(1), _state.mag_I(0));
return atan2f(_state.mag_I(1), _state.mag_I(0));
} else if (_params.mag_declination_source & MASK_USE_GEO_DECL) {
// use parameter value until GPS is available, then use value returned by geo library
if (_NED_origin_initialised) {
_mag_declination = _mag_declination_gps;
_mag_declination_to_save_deg = math::degrees(_mag_declination);
_mag_declination_to_save_deg = math::degrees(_mag_declination_gps);
return _mag_declination_gps;
} else {
_mag_declination = math::radians(_params.mag_declination_deg);
_mag_declination_to_save_deg = _params.mag_declination_deg;
return math::radians(_params.mag_declination_deg);
}
} else {
// always use the parameter value
_mag_declination = math::radians(_params.mag_declination_deg);
_mag_declination_to_save_deg = _params.mag_declination_deg;
return math::radians(_params.mag_declination_deg);
}
}

11
EKF/mag_fusion.cpp
View File

@ -491,7 +491,7 @@ void Ekf::fuseHeading()
}
// the angle of the projection onto the horizontal gives the yaw angle
measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + _mag_declination;
measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();
} else if (_control_status.flags.ev_yaw) {
// calculate the yaw angle for a 321 sequence
@ -584,7 +584,7 @@ void Ekf::fuseHeading()
}
// the angle of the projection onto the horizontal gives the yaw angle
measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + _mag_declination;
measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();
} else if (_control_status.flags.ev_yaw) {
// calculate the yaw angle for a 312 sequence
@ -873,7 +873,7 @@ void Ekf::fuseDeclination(float decl_sigma)
Kfusion[23] = -t4*t13*(P[23][16]*magE-P[23][17]*magN);
// calculate innovation and constrain
float innovation = atan2f(magE, magN) - _mag_declination;
float innovation = atan2f(magE, magN) - getMagDeclination();
innovation = math::constrain(innovation, -0.5f, 0.5f);
// apply covariance correction via P_new = (I -K*H)*P
@ -963,8 +963,9 @@ void Ekf::limitDeclination()
_state.mag_I(1) *= h_scaler;
} else {
// too small to scale radially so set to expected value
_state.mag_I(0) = 2.0f * h_field_min * cosf(_mag_declination);
_state.mag_I(1) = 2.0f * h_field_min * sinf(_mag_declination);
float mag_declination = getMagDeclination();
_state.mag_I(0) = 2.0f * h_field_min * cosf(mag_declination);
_state.mag_I(1) = 2.0f * h_field_min * sinf(mag_declination);
}
h_field = h_field_min;
}