Files
PX4-Autopilot/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_fusion.cpp
T

225 lines
8.0 KiB
C++

/****************************************************************************
*
* Copyright (c) 2015-2023 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 mag_fusion.cpp
* Magnetometer fusion methods.
* Equations generated using EKF/python/ekf_derivation/main.py
*
* @author Roman Bast <bapstroman@gmail.com>
* @author Paul Riseborough <p_riseborough@live.com.au>
*
*/
#include "ekf.h"
#include <ekf_derivation/generated/compute_mag_y_innov_var_and_h.h>
#include <ekf_derivation/generated/compute_mag_z_innov_var_and_h.h>
#include <ekf_derivation/generated/compute_mag_declination_pred_innov_var_and_h.h>
#include <mathlib/mathlib.h>
bool Ekf::fuseMag(const float R_MAG, VectorState &H, estimator_aid_source3d_s &aid_src,
bool update_all_states, bool update_tilt)
{
// if any axis failed, abort the mag fusion
if (aid_src.innovation_rejected) {
return false;
}
const auto state_vector = _state.vector();
const float dt_heading = update_all_states
? math::constrain((float)(_time_delayed_us - _time_last_heading_fuse) * 1e-6f, 1e-4f, 0.2f)
: 0.f;
float delta_heading_max = math::radians(1.f) * dt_heading;
VectorState state_correction;
// update the states and covariance using sequential fusion of the magnetometer components
for (uint8_t index = 0; index <= 2; index++) {
// Calculate Kalman gains and observation jacobians
if (index == 0) {
// everything was already computed
} else if (index == 1) {
// recalculate innovation variance because state covariances have changed due to previous fusion (linearise using the same initial state for all axes)
sym::ComputeMagYInnovVarAndH(state_vector, P, R_MAG, FLT_EPSILON, &aid_src.innovation_variance[index], &H);
} else if (index == 2) {
// we do not fuse synthesized magnetomter measurements when doing 3D fusion
if (_control_status.flags.synthetic_mag_z) {
continue;
}
// recalculate innovation variance because state covariances have changed due to previous fusion (linearise using the same initial state for all axes)
sym::ComputeMagZInnovVarAndH(state_vector, P, R_MAG, FLT_EPSILON, &aid_src.innovation_variance[index], &H);
}
if (aid_src.innovation_variance[index] < R_MAG) {
ECL_ERR("mag numerical error covariance reset");
// keep the covariance and state consistent by applying the correction accumulated so far
applyStateCorrection(state_correction);
// we need to re-initialise covariances and abort this fusion step
if (update_all_states) {
resetQuatCov(_params.ekf2_head_noise);
}
resetMagEarthCov();
resetMagBiasCov();
return false;
}
VectorState Kfusion = computeKalmanGain(H, aid_src.innovation_variance[index]);
if (update_all_states) {
if (!update_tilt) {
Kfusion(State::quat_nominal.idx + 0) = 0.f;
Kfusion(State::quat_nominal.idx + 1) = 0.f;
// Also avoid corruption of the XY gyro biases
Kfusion(State::gyro_bias.idx + 0) = 0.f;
Kfusion(State::gyro_bias.idx + 1) = 0.f;
}
} else {
// zero non-mag Kalman gains if not updating all states
// copy mag_I and mag_B Kalman gains
const Vector3f K_mag_I = Kfusion.slice<State::mag_I.dof, 1>(State::mag_I.idx, 0);
const Vector3f K_mag_B = Kfusion.slice<State::mag_B.dof, 1>(State::mag_B.idx, 0);
// zero all Kalman gains, then restore mag
Kfusion.setZero();
Kfusion.slice<State::mag_I.dof, 1>(State::mag_I.idx, 0) = K_mag_I;
Kfusion.slice<State::mag_B.dof, 1>(State::mag_B.idx, 0) = K_mag_B;
}
// limit total heading change rate to prevent rapid wrong convergence when heading variance is high
const float innov_corrected = aid_src.innovation[index] + H.dot(state_correction);
const float delta_heading = Kfusion(State::quat_nominal.idx + 2) * innov_corrected;
const float delta_heading_abs = fabsf(delta_heading);
if (delta_heading_abs > delta_heading_max) {
Kfusion *= delta_heading_max / math::max(delta_heading_abs, FLT_EPSILON);
delta_heading_max = 0.f;
} else {
delta_heading_max -= delta_heading_abs;
}
accumulateMeasurementUpdate(Kfusion, H, aid_src.observation_variance[index], aid_src.innovation[index], state_correction);
}
applyStateCorrection(state_correction);
_fault_status.flags.bad_mag_x = false;
_fault_status.flags.bad_mag_y = false;
_fault_status.flags.bad_mag_z = false;
aid_src.fused = true;
aid_src.time_last_fuse = _time_delayed_us;
if (update_all_states) {
_time_last_heading_fuse = _time_delayed_us;
}
return true;
}
bool Ekf::fuseDeclination(float decl_measurement_rad, float R, bool update_all_states, bool update_tilt)
{
VectorState H;
float decl_pred;
float innovation_variance;
sym::ComputeMagDeclinationPredInnovVarAndH(_state.vector(), P, R, FLT_EPSILON,
&decl_pred, &innovation_variance, &H);
const float innovation = wrap_pi(decl_pred - decl_measurement_rad);
if (innovation_variance < R) {
// variance calculation is badly conditioned
_fault_status.flags.bad_mag_decl = true;
return false;
}
// Calculate the Kalman gains
VectorState Kfusion = computeKalmanGain(H, innovation_variance);
if (update_all_states) {
if (!update_tilt) {
Kfusion(State::quat_nominal.idx + 0) = 0.f;
Kfusion(State::quat_nominal.idx + 1) = 0.f;
// Also avoid corruption of the XY gyro biases
Kfusion(State::gyro_bias.idx + 0) = 0.f;
Kfusion(State::gyro_bias.idx + 1) = 0.f;
}
} else {
// zero non-mag Kalman gains if not updating all states
// copy mag_I and mag_B Kalman gains
const Vector3f K_mag_I = Kfusion.slice<State::mag_I.dof, 1>(State::mag_I.idx, 0);
const Vector3f K_mag_B = Kfusion.slice<State::mag_B.dof, 1>(State::mag_B.idx, 0);
// zero all Kalman gains, then restore mag
Kfusion.setZero();
Kfusion.slice<State::mag_I.dof, 1>(State::mag_I.idx, 0) = K_mag_I;
Kfusion.slice<State::mag_B.dof, 1>(State::mag_B.idx, 0) = K_mag_B;
}
measurementUpdate(Kfusion, H, R, innovation);
_fault_status.flags.bad_mag_decl = false;
return true;
}
float Ekf::calculate_synthetic_mag_z_measurement(const Vector3f &mag_meas, const Vector3f &mag_earth_predicted)
{
// theoretical magnitude of the magnetometer Z component value given X and Y sensor measurement and our knowledge
// of the earth magnetic field vector at the current location
const float mag_z_abs = sqrtf(math::max(sq(mag_earth_predicted.length()) - sq(mag_meas(0)) - sq(mag_meas(1)), 0.0f));
// calculate sign of synthetic magnetomter Z component based on the sign of the predicted magnetometer Z component
const float mag_z_body_pred = mag_earth_predicted.dot(_R_to_earth.col(2));
return (mag_z_body_pred < 0) ? -mag_z_abs : mag_z_abs;
}