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

ekf2: cleanup zero innovation heading fusion

Browse Source
This commit is contained in:
Daniel Agar
2022-04-01 11:09:44 -04:00
parent 1a5ff0bae4
commit becffa3441
9 changed files with 203 additions and 248 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/CMakeLists.txt
+1
View File
@@ -71,6 +71,7 @@ px4_add_module(
EKF/terrain_estimator.cpp
EKF/utils.cpp
EKF/vel_pos_fusion.cpp
EKF/zero_innovation_heading_update.cpp
EKF/zero_velocity_update.cpp
EKF2.cpp
src/modules/ekf2/EKF/CMakeLists.txt
+1
View File
@@ -56,6 +56,7 @@ add_library(ecl_EKF
terrain_estimator.cpp
utils.cpp
vel_pos_fusion.cpp
zero_innovation_heading_update.cpp
zero_velocity_update.cpp
)
src/modules/ekf2/EKF/control.cpp
+6 -7
View File
@@ -181,6 +181,8 @@ void Ekf::controlFusionModes()
// Additional horizontal velocity data from an auxiliary sensor can be fused
controlAuxVelFusion();
controlZeroInnovationHeadingUpdate();
controlZeroVelocityUpdate();
// Fake position measurement for constraining drift when no other velocity or position measurements
@@ -357,14 +359,11 @@ void Ekf::controlExternalVisionFusion()
resetYawToEv();
}
if (shouldUse321RotationSequence(_R_to_earth)) {
float measured_hdg = getEuler321Yaw(_ev_sample_delayed.quat);
fuseYaw321(measured_hdg, _ev_sample_delayed.angVar);
float measured_hdg = shouldUse321RotationSequence(_R_to_earth) ? getEuler321Yaw(_ev_sample_delayed.quat) : getEuler312Yaw(_ev_sample_delayed.quat);
} else {
float measured_hdg = getEuler312Yaw(_ev_sample_delayed.quat);
fuseYaw312(measured_hdg, _ev_sample_delayed.angVar);
}
float innovation = wrap_pi(getEulerYaw(_R_to_earth)) - wrap_pi(measured_hdg);
float obs_var = fmaxf(_ev_sample_delayed.angVar, 1.e-4f);
updateQuaternion(innovation, obs_var);
}
// record observation and estimate for use next time
src/modules/ekf2/EKF/ekf.h
+4 -20
View File
@@ -587,28 +587,10 @@ private:
// ekf sequential fusion of magnetometer measurements
bool fuseMag(const Vector3f &mag, bool update_all_states = true);
// fuse the first euler angle from either a 321 or 312 rotation sequence as the observation (currently measures yaw using the magnetometer)
bool 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
bool 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
bool 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
// update quaternion states and covariances using a yaw innovation and yaw observation variance
// innovation : prediction - measurement
// variance : observaton variance
// gate_sigma : innovation consistency check gate size (Sigma)
// jacobian : 4x1 vector of partial derivatives of observation wrt each quaternion state
bool updateQuaternion(const float innovation, const float variance, const float gate_sigma,
const Vector4f &yaw_jacobian);
bool updateQuaternion(const float innovation, const float variance);
// fuse the yaw angle obtained from a dual antenna GPS unit
void fuseGpsYaw();
@@ -882,6 +864,8 @@ private:
void controlZeroVelocityUpdate();
void controlZeroInnovationHeadingUpdate();
// control fusion of auxiliary velocity observations
void controlAuxVelFusion();
src/modules/ekf2/EKF/ekf_helper.cpp
+2 -1
View File
@@ -1211,7 +1211,6 @@ void Ekf::stopMag3DFusion()
if (_control_status.flags.mag_3D) {
saveMagCovData();
_control_status.flags.mag_3D = false;
_control_status.flags.mag_dec = false;
@@ -1747,6 +1746,8 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance, bool update_buffer)
_output_new.quat_nominal = _state_reset_status.quat_change * _output_new.quat_nominal;
}
_last_static_yaw = NAN;
// capture the reset event
_state_reset_status.quat_counter++;
src/modules/ekf2/EKF/gps_yaw_fusion.cpp
+3 -2
View File
@@ -156,8 +156,8 @@ void Ekf::fuseGpsYaw()
_yaw_signed_test_ratio_lpf.update(matrix::sign(_heading_innov) * _yaw_test_ratio);
if (!_control_status.flags.in_air
&& fabsf(_yaw_signed_test_ratio_lpf.getState()) > 0.2f) {
if ((fabsf(_yaw_signed_test_ratio_lpf.getState()) > 0.2f)
&& !_control_status.flags.in_air && isTimedOut(_time_last_heading_fuse, (uint64_t)1e6)) {
// A constant large signed test ratio is a sign of wrong gyro bias
// Reset the yaw gyro variance to converge faster and avoid
@@ -189,6 +189,7 @@ void Ekf::fuseGpsYaw()
if (is_fused) {
_time_last_gps_yaw_fuse = _time_last_imu;
_time_last_heading_fuse = _time_last_imu;
}
}
src/modules/ekf2/EKF/mag_control.cpp
+7 -15
View File
@@ -83,25 +83,11 @@ void Ekf::controlMagFusion()
_time_last_mov_3d_mag_suitable = _imu_sample_delayed.time_us;
}
// When operating without a magnetometer and no other source of yaw aiding is active,
// yaw fusion is run selectively to enable yaw gyro bias learning when stationary on
// ground and to prevent uncontrolled yaw variance growth
// Also fuse zero heading innovation during the leveling fine alignment step to keep the yaw variance low
if (_params.mag_fusion_type >= MAG_FUSE_TYPE_NONE
|| _control_status.flags.mag_fault
|| !_control_status.flags.tilt_align) {
stopMagFusion();
if (!_control_status.flags.ev_yaw && !_control_status.flags.gps_yaw) {
// TODO: setting _is_yaw_fusion_inhibited to true is required to tell
// fuseHeading to perform a "zero innovation heading fusion"
// We should refactor it to avoid using this flag here
_is_yaw_fusion_inhibited = true;
fuseHeading();
_is_yaw_fusion_inhibited = false;
}
return;
}
@@ -340,9 +326,15 @@ void Ekf::runMagAndMagDeclFusions(const Vector3f &mag)
Vector3f mag_earth_pred = R_to_earth * (mag - _state.mag_B);
// the angle of the projection onto the horizontal gives the yaw angle
// calculate the yaw innovation and wrap to the interval between +-pi
float measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();
if (fuseHeading(measured_hdg, sq(_params.mag_heading_noise))) {
float innovation = wrap_pi(getEulerYaw(_R_to_earth)) - wrap_pi(measured_hdg);
float obs_var = fmaxf(sq(_params.mag_heading_noise), 1.e-4f);
// Update the quaternion states and covariance matrix
if (updateQuaternion(innovation, obs_var)) {
_time_last_mag_heading_fuse = _time_last_imu;
}
}
src/modules/ekf2/EKF/mag_fusion.cpp
+98 -203
View File
@@ -433,91 +433,8 @@ bool Ekf::fuseMag(const Vector3f &mag, bool update_all_states)
return false;
}
bool Ekf::fuseYaw321(float yaw, float yaw_variance, bool zero_innovation)
{
// assign intermediate state variables
const float &q0 = _state.quat_nominal(0);
const float &q1 = _state.quat_nominal(1);
const float &q2 = _state.quat_nominal(2);
const float &q3 = _state.quat_nominal(3);
const float R_YAW = fmaxf(yaw_variance, 1.0e-4f);
const float measurement = wrap_pi(yaw);
// calculate 321 yaw observation matrix
// choose A or B computational paths to avoid singularity in derivation at +-90 degrees yaw
bool canUseA = false;
const float SA0 = 2*q3;
const float SA1 = 2*q2;
const float SA2 = SA0*q0 + SA1*q1;
const float SA3 = sq(q0) + sq(q1) - sq(q2) - sq(q3);
float SA4, SA5_inv;
if (sq(SA3) > 1e-6f) {
SA4 = 1.0F/sq(SA3);
SA5_inv = sq(SA2)*SA4 + 1;
canUseA = fabsf(SA5_inv) > 1e-6f;
}
bool canUseB = false;
const float SB0 = 2*q0;
const float SB1 = 2*q1;
const float SB2 = SB0*q3 + SB1*q2;
const float SB4 = sq(q0) + sq(q1) - sq(q2) - sq(q3);
float SB3, SB5_inv;
if (sq(SB2) > 1e-6f) {
SB3 = 1.0F/sq(SB2);
SB5_inv = SB3*sq(SB4) + 1;
canUseB = fabsf(SB5_inv) > 1e-6f;
}
Vector4f H_YAW;
if (canUseA && (!canUseB || fabsf(SA5_inv) >= fabsf(SB5_inv))) {
const float SA5 = 1.0F/SA5_inv;
const float SA6 = 1.0F/SA3;
const float SA7 = SA2*SA4;
const float SA8 = 2*SA7;
const float SA9 = 2*SA6;
H_YAW(0) = SA5*(SA0*SA6 - SA8*q0);
H_YAW(1) = SA5*(SA1*SA6 - SA8*q1);
H_YAW(2) = SA5*(SA1*SA7 + SA9*q1);
H_YAW(3) = SA5*(SA0*SA7 + SA9*q0);
} else if (canUseB && (!canUseA || fabsf(SB5_inv) > fabsf(SA5_inv))) {
const float SB5 = 1.0F/SB5_inv;
const float SB6 = 1.0F/SB2;
const float SB7 = SB3*SB4;
const float SB8 = 2*SB7;
const float SB9 = 2*SB6;
H_YAW(0) = -SB5*(SB0*SB6 - SB8*q3);
H_YAW(1) = -SB5*(SB1*SB6 - SB8*q2);
H_YAW(2) = -SB5*(-SB1*SB7 - SB9*q2);
H_YAW(3) = -SB5*(-SB0*SB7 - SB9*q3);
} else {
return false;
}
// calculate the yaw innovation and wrap to the interval between +-pi
float innovation;
if (zero_innovation) {
innovation = 0.0f;
} else {
innovation = wrap_pi(atan2f(_R_to_earth(1, 0), _R_to_earth(0, 0)) - measurement);
}
// define the innovation gate size
float innov_gate = math::max(_params.heading_innov_gate, 1.0f);
// Update the quaternion states and covariance matrix
return updateQuaternion(innovation, R_YAW, innov_gate, H_YAW);
}
bool Ekf::fuseYaw312(float yaw, float yaw_variance, bool zero_innovation)
// update quaternion states and covariances using the yaw innovation and yaw observation variance
bool Ekf::updateQuaternion(const float innovation, const float variance)
{
// assign intermediate state variables
const float q0 = _state.quat_nominal(0);
@@ -525,86 +442,113 @@ bool Ekf::fuseYaw312(float yaw, float yaw_variance, bool zero_innovation)
const float q2 = _state.quat_nominal(2);
const float q3 = _state.quat_nominal(3);
const float R_YAW = fmaxf(yaw_variance, 1.0e-4f);
const float measurement = wrap_pi(yaw);
// calculate 312 yaw observation matrix
// choose A or B computational paths to avoid singularity in derivation at +-90 degrees yaw
bool canUseA = false;
const float SA0 = 2*q3;
const float SA1 = 2*q2;
const float SA2 = SA0*q0 - SA1*q1;
const float SA3 = sq(q0) - sq(q1) + sq(q2) - sq(q3);
float SA4, SA5_inv;
bool canUseB = false;
float SA0;
float SA1;
float SA2;
float SA3;
float SA4;
float SA5_inv;
float SB0;
float SB1;
float SB2;
float SB3;
float SB4;
float SB5_inv;
const bool fuse_yaw_321 = shouldUse321RotationSequence(_R_to_earth);
if (fuse_yaw_321) {
// calculate 321 yaw observation matrix
SA0 = 2*q3;
SA1 = 2*q2;
SA2 = SA0*q0 + SA1*q1;
SA3 = sq(q0) + sq(q1) - sq(q2) - sq(q3);
SB0 = 2*q0;
SB1 = 2*q1;
SB2 = SB0*q3 + SB1*q2;
SB4 = sq(q0) + sq(q1) - sq(q2) - sq(q3);
} else {
// calculate 312 yaw observation matrix
SA0 = 2*q3;
SA1 = 2*q2;
SA2 = SA0*q0 - SA1*q1;
SA3 = sq(q0) - sq(q1) + sq(q2) - sq(q3);
SB0 = 2*q0;
SB1 = 2*q1;
SB2 = -SB0*q3 + SB1*q2;
SB4 = -sq(q0) + sq(q1) - sq(q2) + sq(q3);
}
if (sq(SA3) > 1e-6f) {
SA4 = 1.0F/sq(SA3);
SA5_inv = sq(SA2)*SA4 + 1;
SA4 = 1.f/sq(SA3);
SA5_inv = sq(SA2)*SA4 + 1.f;
canUseA = fabsf(SA5_inv) > 1e-6f;
}
bool canUseB = false;
const float SB0 = 2*q0;
const float SB1 = 2*q1;
const float SB2 = -SB0*q3 + SB1*q2;
const float SB4 = -sq(q0) + sq(q1) - sq(q2) + sq(q3);
float SB3, SB5_inv;
if (sq(SB2) > 1e-6f) {
SB3 = 1.0F/sq(SB2);
SB5_inv = SB3*sq(SB4) + 1;
SB3 = 1.f/sq(SB2);
SB5_inv = SB3*sq(SB4) + 1.f;
canUseB = fabsf(SB5_inv) > 1e-6f;
}
Vector4f H_YAW;
if (canUseA && (!canUseB || fabsf(SA5_inv) >= fabsf(SB5_inv))) {
const float SA5 = 1.0F/SA5_inv;
const float SA6 = 1.0F/SA3;
const float SA5 = 1.f/SA5_inv;
const float SA6 = 1.f/SA3;
const float SA7 = SA2*SA4;
const float SA8 = 2*SA7;
const float SA9 = 2*SA6;
H_YAW(0) = SA5*(SA0*SA6 - SA8*q0);
H_YAW(1) = SA5*(-SA1*SA6 + SA8*q1);
H_YAW(2) = SA5*(-SA1*SA7 - SA9*q1);
H_YAW(3) = SA5*(SA0*SA7 + SA9*q0);
if (fuse_yaw_321) {
// calculate 321 yaw observation matrix
H_YAW(0) = SA5*(SA0*SA6 - SA8*q0);
H_YAW(1) = SA5*(SA1*SA6 - SA8*q1);
H_YAW(2) = SA5*(SA1*SA7 + SA9*q1);
H_YAW(3) = SA5*(SA0*SA7 + SA9*q0);
} else {
// calculate 312 yaw observation matrix
H_YAW(0) = SA5*(SA0*SA6 - SA8*q0);
H_YAW(1) = SA5*(-SA1*SA6 + SA8*q1);
H_YAW(2) = SA5*(-SA1*SA7 - SA9*q1);
H_YAW(3) = SA5*(SA0*SA7 + SA9*q0);
}
} else if (canUseB && (!canUseA || fabsf(SB5_inv) > fabsf(SA5_inv))) {
const float SB5 = 1.0F/SB5_inv;
const float SB6 = 1.0F/SB2;
const float SB5 = 1.f/SB5_inv;
const float SB6 = 1.f/SB2;
const float SB7 = SB3*SB4;
const float SB8 = 2*SB7;
const float SB9 = 2*SB6;
H_YAW(0) = -SB5*(-SB0*SB6 + SB8*q3);
H_YAW(1) = -SB5*(SB1*SB6 - SB8*q2);
H_YAW(2) = -SB5*(-SB1*SB7 - SB9*q2);
H_YAW(3) = -SB5*(SB0*SB7 + SB9*q3);
if (fuse_yaw_321) {
// calculate 321 yaw observation matrix
H_YAW(0) = -SB5*(SB0*SB6 - SB8*q3);
H_YAW(1) = -SB5*(SB1*SB6 - SB8*q2);
H_YAW(2) = -SB5*(-SB1*SB7 - SB9*q2);
H_YAW(3) = -SB5*(-SB0*SB7 - SB9*q3);
} else {
// calculate 312 yaw observation matrix
H_YAW(0) = -SB5*(-SB0*SB6 + SB8*q3);
H_YAW(1) = -SB5*(SB1*SB6 - SB8*q2);
H_YAW(2) = -SB5*(-SB1*SB7 - SB9*q2);
H_YAW(3) = -SB5*(SB0*SB7 + SB9*q3);
}
} else {
return false;
}
float innovation;
if (zero_innovation) {
innovation = 0.0f;
} else {
// calculate the the innovation and wrap to the interval between +-pi
innovation = wrap_pi(atan2f(-_R_to_earth(0, 1), _R_to_earth(1, 1)) - measurement);
}
// define the innovation gate size
float innov_gate = math::max(_params.heading_innov_gate, 1.0f);
// Update the quaternion states and covariance matrix
return updateQuaternion(innovation, R_YAW, innov_gate, H_YAW);
}
// update quaternion states and covariances using the yaw innovation, yaw observation variance and yaw Jacobian
bool Ekf::updateQuaternion(const float innovation, const float variance, const float gate_sigma,
const Vector4f &yaw_jacobian)
{
// Calculate innovation variance and Kalman gains, taking advantage of the fact that only the first 4 elements in H are non zero
// calculate the innovation variance
_heading_innov_var = variance;
@@ -613,10 +557,10 @@ bool Ekf::updateQuaternion(const float innovation, const float variance, const f
float tmp = 0.0f;
for (uint8_t col = 0; col <= 3; col++) {
tmp += P(row, col) * yaw_jacobian(col);
tmp += P(row, col) * H_YAW(col);
}
_heading_innov_var += yaw_jacobian(row) * tmp;
_heading_innov_var += H_YAW(row) * tmp;
}
float heading_innov_var_inv;
@@ -633,7 +577,7 @@ bool Ekf::updateQuaternion(const float innovation, const float variance, const f
// we reinitialise the covariance matrix and abort this fusion step
initialiseCovariance();
ECL_ERR("mag yaw fusion numerical error - covariance reset");
ECL_ERR("yaw fusion numerical error - covariance reset");
return false;
}
@@ -643,7 +587,7 @@ bool Ekf::updateQuaternion(const float innovation, const float variance, const f
for (uint8_t row = 0; row <= 15; row++) {
for (uint8_t col = 0; col <= 3; col++) {
Kfusion(row) += P(row, col) * yaw_jacobian(col);
Kfusion(row) += P(row, col) * H_YAW(col);
}
Kfusion(row) *= heading_innov_var_inv;
@@ -652,13 +596,16 @@ bool Ekf::updateQuaternion(const float innovation, const float variance, const f
if (_control_status.flags.wind) {
for (uint8_t row = 22; row <= 23; row++) {
for (uint8_t col = 0; col <= 3; col++) {
Kfusion(row) += P(row, col) * yaw_jacobian(col);
Kfusion(row) += P(row, col) * H_YAW(col);
}
Kfusion(row) *= heading_innov_var_inv;
}
}
// define the innovation gate size
float gate_sigma = math::max(_params.heading_innov_gate, 1.f);
// innovation test ratio
_yaw_test_ratio = sq(innovation) / (sq(gate_sigma) * _heading_innov_var);
@@ -669,7 +616,10 @@ bool Ekf::updateQuaternion(const float innovation, const float variance, const f
// if we are in air we don't want to fuse the measurement
// we allow to use it when on the ground because the large innovation could be caused
// by interference or a large initial gyro bias
if (!_control_status.flags.in_air && isTimedOut(_time_last_in_air, (uint64_t)5e6)) {
if (!_control_status.flags.in_air
&& isTimedOut(_time_last_in_air, (uint64_t)5e6)
&& isTimedOut(_time_last_heading_fuse, (uint64_t)1e6)
) {
// constrain the innovation to the maximum set by the gate
// we need to delay this forced fusion to avoid starting it
// immediately after touchdown, when the drone is still armed
@@ -697,10 +647,10 @@ bool Ekf::updateQuaternion(const float innovation, const float variance, const f
for (unsigned row = 0; row < _k_num_states; row++) {
KH[0] = Kfusion(row) * yaw_jacobian(0);
KH[1] = Kfusion(row) * yaw_jacobian(1);
KH[2] = Kfusion(row) * yaw_jacobian(2);
KH[3] = Kfusion(row) * yaw_jacobian(3);
KH[0] = Kfusion(row) * H_YAW(0);
KH[1] = Kfusion(row) * H_YAW(1);
KH[2] = Kfusion(row) * H_YAW(2);
KH[3] = Kfusion(row) * H_YAW(3);
for (unsigned column = 0; column < _k_num_states; column++) {
float tmp = KH[0] * P(0, column);
@@ -732,61 +682,6 @@ bool Ekf::updateQuaternion(const float innovation, const float variance, const f
return false;
}
bool Ekf::fuseHeading(float measured_hdg, float obs_var)
{
// 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
const float predicted_hdg = getEulerYaw(_R_to_earth);
if (!PX4_ISFINITE(measured_hdg)) {
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;
}
} else {
_last_static_yaw = predicted_hdg;
}
if (shouldUse321RotationSequence(_R_to_earth)) {
return fuseYaw321(measured_hdg, R_YAW, fuse_zero_innov);
} else {
return fuseYaw312(measured_hdg, R_YAW, fuse_zero_innov);
}
}
bool Ekf::fuseDeclination(float decl_sigma)
{
// assign intermediate state variables
src/modules/ekf2/EKF/zero_innovation_heading_update.cpp
+81
View File
@@ -0,0 +1,81 @@
/****************************************************************************
*
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file zero_innovation_heading_update.cpp
* Control function for ekf heading update when at rest or no other heading source available
*/
#include "ekf.h"
void Ekf::controlZeroInnovationHeadingUpdate()
{
// When at rest or no source of yaw aiding is active yaw fusion is run selectively to enable yaw gyro
// bias learning when stationary on ground and to prevent uncontrolled yaw variance growth
if (_control_status.flags.vehicle_at_rest && _control_status.flags.tilt_align) {
const float euler_yaw = wrap_pi(getEulerYaw(_R_to_earth));
// record static yaw when transitioning to at rest
if (!PX4_ISFINITE(_last_static_yaw)) {
_last_static_yaw = euler_yaw;
}
// fuse last static yaw at a limited rate (every 200 milliseconds)
if (isTimedOut(_time_last_heading_fuse, (uint64_t)200'000)) {
float innovation = euler_yaw - _last_static_yaw;
float obs_var = 0.001f;
updateQuaternion(innovation, obs_var);
}
} else {
// vehicle moving or tilt alignment not yet completed
const float sumQuatVar = P(0, 0) + P(1, 1) + P(2, 2) + P(3, 3);
if (sumQuatVar > _params.quat_max_variance) {
// if necessary fuse zero innovation to prevent unconstrained quaternion variance growth
float innovation = 0.f;
float obs_var = 0.25f;
updateQuaternion(innovation, obs_var);
} else if (!_control_status.flags.tilt_align) {
// fuse zero heading innovation during the leveling fine alignment step to keep the yaw variance low
float innovation = 0.f;
float obs_var = 0.01f;
updateQuaternion(innovation, obs_var);
}
_last_static_yaw = wrap_pi(getEulerYaw(_R_to_earth));
}
}