mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
ekf2: migrate sideslip fusion to SymForce
- split fusion into update (compute innov and innov_var) and actual fusion to the state vector - use estimator_aid_source_1d struct to group the data
This commit is contained in:
bresch
Daniel Agar
parent
c09be0f0ac
commit
5f54f6fcda
@ -20,5 +20,5 @@ bool fused # true if the sample was successfully fused
|
||||
|
||||
# TOPICS estimator_aid_source_1d
|
||||
# TOPICS estimator_aid_src_baro_hgt estimator_aid_src_rng_hgt estimator_aid_src_airspeed
|
||||
# TOPICS estimator_aid_src_fake_hgt
|
||||
# TOPICS estimator_aid_src_fake_hgt estimator_aid_src_sideslip
|
||||
# TOPICS estimator_aid_src_mag_heading estimator_aid_src_gnss_yaw estimator_aid_src_ev_yaw
|
||||
|
||||
@ -46,6 +46,8 @@ if(${PYTHON_SYMFORCE_EXIT_CODE} EQUAL 0)
|
||||
set(EKF_GENERATED_SRC_FILES
|
||||
${EKF_DERIVATION_DIR}/generated/compute_airspeed_h_and_k.h
|
||||
${EKF_DERIVATION_DIR}/generated/compute_airspeed_innov_and_innov_var.h
|
||||
${EKF_DERIVATION_DIR}/generated/compute_sideslip_h_and_k.h
|
||||
${EKF_DERIVATION_DIR}/generated/compute_sideslip_innov_and_innov_var.h
|
||||
)
|
||||
|
||||
add_custom_command(
|
||||
|
||||
@ -646,7 +646,7 @@ void Ekf::controlAirDataFusion()
|
||||
|
||||
// If both airspeed and sideslip fusion have timed out and we are not using a drag observation model then we no longer have valid wind estimates
|
||||
const bool airspeed_timed_out = isTimedOut(_aid_src_airspeed.time_last_fuse, (uint64_t)10e6);
|
||||
const bool sideslip_timed_out = isTimedOut(_time_last_beta_fuse, (uint64_t)10e6);
|
||||
const bool sideslip_timed_out = isTimedOut(_aid_src_airspeed.time_last_fuse, (uint64_t)10e6);
|
||||
|
||||
if (_control_status.flags.fake_pos || (airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & SensorFusionMask::USE_DRAG))) {
|
||||
_control_status.flags.wind = false;
|
||||
@ -701,21 +701,26 @@ void Ekf::controlBetaFusion()
|
||||
}
|
||||
|
||||
// Perform synthetic sideslip fusion at regular intervals when in-air and sideslip fuson had been enabled externally:
|
||||
const bool beta_fusion_time_triggered = isTimedOut(_time_last_beta_fuse, _params.beta_avg_ft_us);
|
||||
const bool beta_fusion_time_triggered = isTimedOut(_aid_src_sideslip.time_last_fuse, _params.beta_avg_ft_us);
|
||||
|
||||
if (beta_fusion_time_triggered &&
|
||||
_control_status.flags.fuse_beta &&
|
||||
_control_status.flags.in_air) {
|
||||
updateSideslip(_aid_src_sideslip);
|
||||
_innov_check_fail_status.flags.reject_sideslip = _aid_src_sideslip.innovation_rejected;
|
||||
|
||||
// If starting wind state estimation, reset the wind states and covariances before fusing any data
|
||||
if (!_control_status.flags.wind) {
|
||||
// activate the wind states
|
||||
_control_status.flags.wind = true;
|
||||
// reset the timeout timers to prevent repeated resets
|
||||
_time_last_beta_fuse = _imu_sample_delayed.time_us;
|
||||
_aid_src_airspeed.time_last_fuse = _imu_sample_delayed.time_us;
|
||||
resetWind();
|
||||
}
|
||||
|
||||
fuseSideslip();
|
||||
if (Vector2f(Vector2f(_state.vel) - _state.wind_vel).longerThan(7.f)) {
|
||||
fuseSideslip(_aid_src_sideslip);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -171,9 +171,9 @@ public:
|
||||
void getAirspeedInnovVar(float &airspeed_innov_var) const { airspeed_innov_var = _aid_src_airspeed.innovation_variance; }
|
||||
void getAirspeedInnovRatio(float &airspeed_innov_ratio) const { airspeed_innov_ratio = _aid_src_airspeed.test_ratio; }
|
||||
|
||||
void getBetaInnov(float &beta_innov) const { beta_innov = _beta_innov; }
|
||||
void getBetaInnovVar(float &beta_innov_var) const { beta_innov_var = _beta_innov_var; }
|
||||
void getBetaInnovRatio(float &beta_innov_ratio) const { beta_innov_ratio = _beta_test_ratio; }
|
||||
void getBetaInnov(float &beta_innov) const { beta_innov = _aid_src_sideslip.innovation; }
|
||||
void getBetaInnovVar(float &beta_innov_var) const { beta_innov_var = _aid_src_sideslip.innovation_variance; }
|
||||
void getBetaInnovRatio(float &beta_innov_ratio) const { beta_innov_ratio = _aid_src_sideslip.test_ratio; }
|
||||
|
||||
void getHaglInnov(float &hagl_innov) const { hagl_innov = _hagl_innov; }
|
||||
void getHaglInnovVar(float &hagl_innov_var) const { hagl_innov_var = _hagl_innov_var; }
|
||||
@ -414,6 +414,7 @@ public:
|
||||
const BiasEstimator::status &getEvHgtBiasEstimatorStatus() const { return _ev_hgt_b_est.getStatus(); }
|
||||
|
||||
const auto &aid_src_airspeed() const { return _aid_src_airspeed; }
|
||||
const auto &aid_src_sideslip() const { return _aid_src_sideslip; }
|
||||
|
||||
const auto &aid_src_baro_hgt() const { return _aid_src_baro_hgt; }
|
||||
const auto &aid_src_rng_hgt() const { return _aid_src_rng_hgt; }
|
||||
@ -494,7 +495,6 @@ private:
|
||||
|
||||
uint64_t _time_last_of_fuse{0}; ///< time the last fusion of optical flow measurements were performed (uSec)
|
||||
uint64_t _time_last_flow_terrain_fuse{0}; ///< time the last fusion of optical flow measurements for terrain estimation were performed (uSec)
|
||||
uint64_t _time_last_beta_fuse{0}; ///< time the last fusion of synthetic sideslip measurements were performed (uSec)
|
||||
uint64_t _time_last_zero_velocity_fuse{0}; ///< last time of zero velocity update (uSec)
|
||||
uint64_t _time_last_healthy_rng_data{0};
|
||||
uint8_t _nb_gps_yaw_reset_available{0}; ///< remaining number of resets allowed before switching to another aiding source
|
||||
@ -543,9 +543,6 @@ private:
|
||||
Vector2f _drag_innov{}; ///< multirotor drag measurement innovation (m/sec**2)
|
||||
Vector2f _drag_innov_var{}; ///< multirotor drag measurement innovation variance ((m/sec**2)**2)
|
||||
|
||||
float _beta_innov{0.0f}; ///< synthetic sideslip measurement innovation (rad)
|
||||
float _beta_innov_var{0.0f}; ///< synthetic sideslip measurement innovation variance (rad**2)
|
||||
|
||||
float _hagl_innov{0.0f}; ///< innovation of the last height above terrain measurement (m)
|
||||
float _hagl_innov_var{0.0f}; ///< innovation variance for the last height above terrain measurement (m**2)
|
||||
|
||||
@ -566,6 +563,7 @@ private:
|
||||
estimator_aid_source_1d_s _aid_src_baro_hgt{};
|
||||
estimator_aid_source_1d_s _aid_src_rng_hgt{};
|
||||
estimator_aid_source_1d_s _aid_src_airspeed{};
|
||||
estimator_aid_source_1d_s _aid_src_sideslip{};
|
||||
|
||||
estimator_aid_source_2d_s _aid_src_fake_pos{};
|
||||
estimator_aid_source_1d_s _aid_src_fake_hgt{};
|
||||
@ -692,7 +690,8 @@ private:
|
||||
void fuseAirspeed(estimator_aid_source_1d_s &airspeed);
|
||||
|
||||
// fuse synthetic zero sideslip measurement
|
||||
void fuseSideslip();
|
||||
void updateSideslip(estimator_aid_source_1d_s &_aid_src_sideslip) const;
|
||||
void fuseSideslip(estimator_aid_source_1d_s &_aid_src_sideslip);
|
||||
|
||||
// fuse body frame drag specific forces for multi-rotor wind estimation
|
||||
void fuseDrag(const dragSample &drag_sample);
|
||||
|
||||
@ -957,7 +957,7 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
|
||||
hagl = sqrtf(_hagl_test_ratio);
|
||||
|
||||
// return the synthetic sideslip innovation test ratio
|
||||
beta = sqrtf(_beta_test_ratio);
|
||||
beta = sqrtf(_aid_src_sideslip.test_ratio);
|
||||
}
|
||||
|
||||
// return a bitmask integer that describes which state estimates are valid
|
||||
@ -1032,7 +1032,7 @@ void Ekf::updateHorizontalDeadReckoningstatus()
|
||||
|
||||
const bool airDataAiding = _control_status.flags.wind &&
|
||||
isRecent(_aid_src_airspeed.time_last_fuse, _params.no_aid_timeout_max) &&
|
||||
isRecent(_time_last_beta_fuse, _params.no_aid_timeout_max);
|
||||
isRecent(_aid_src_sideslip.time_last_fuse, _params.no_aid_timeout_max);
|
||||
|
||||
_control_status.flags.wind_dead_reckoning = !velPosAiding && !optFlowAiding && airDataAiding;
|
||||
_control_status.flags.inertial_dead_reckoning = !velPosAiding && !optFlowAiding && !airDataAiding;
|
||||
|
||||
@ -345,7 +345,6 @@ protected:
|
||||
Vector2f _ev_pos_test_ratio{}; // EV position innovation consistency check ratios
|
||||
float _optflow_test_ratio{}; // Optical flow innovation consistency check ratio
|
||||
float _hagl_test_ratio{}; // height above terrain measurement innovation consistency check ratio
|
||||
float _beta_test_ratio{}; // sideslip innovation consistency check ratio
|
||||
Vector2f _drag_test_ratio{}; // drag innovation consistency check ratio
|
||||
innovation_fault_status_u _innov_check_fail_status{};
|
||||
|
||||
|
||||
@ -98,9 +98,58 @@ def compute_airspeed_h_and_k(
|
||||
airspeed_pred = vel_rel.norm(epsilon=epsilon)
|
||||
H = sf.V1(airspeed_pred).jacobian(state)
|
||||
|
||||
K = P * H.T / sm.Max(innov_var, epsilon)
|
||||
K = P * H.T / sf.Max(innov_var, epsilon)
|
||||
|
||||
return (H.T, K)
|
||||
|
||||
def predict_sideslip(
|
||||
state: VState,
|
||||
epsilon: sf.Scalar
|
||||
) -> (sf.Scalar):
|
||||
|
||||
vel_rel = sf.V3(state[State.vx] - state[State.wx], state[State.vy] - state[State.wy], state[State.vz])
|
||||
q_att = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz])
|
||||
relative_wind_body = quat_to_rot(q_att).T * vel_rel
|
||||
|
||||
# Small angle approximation of side slip model
|
||||
# Protect division by zero using epsilon
|
||||
sideslip_pred = add_epsilon_sign(relative_wind_body[1] / relative_wind_body[0], relative_wind_body[0], epsilon)
|
||||
|
||||
return sideslip_pred
|
||||
|
||||
def compute_sideslip_innov_and_innov_var(
|
||||
state: VState,
|
||||
P: MState,
|
||||
R: sf.Scalar,
|
||||
epsilon: sf.Scalar
|
||||
) -> (sf.Scalar, sf.Scalar, sf.Scalar):
|
||||
|
||||
sideslip_pred = predict_sideslip(state, epsilon);
|
||||
|
||||
innov = 0.0 - sideslip_pred
|
||||
|
||||
H = sf.V1(sideslip_pred).jacobian(state)
|
||||
innov_var = (H * P * H.T + R)[0,0]
|
||||
|
||||
return (innov, innov_var)
|
||||
|
||||
def compute_sideslip_h_and_k(
|
||||
state: VState,
|
||||
P: MState,
|
||||
innov_var: sf.Scalar,
|
||||
epsilon: sf.Scalar
|
||||
) -> (VState, VState):
|
||||
|
||||
sideslip_pred = predict_sideslip(state, epsilon);
|
||||
|
||||
H = sf.V1(sideslip_pred).jacobian(state)
|
||||
|
||||
K = P * H.T / sf.Max(innov_var, epsilon)
|
||||
|
||||
return (H.T, K)
|
||||
|
||||
generate_px4_function(compute_airspeed_innov_and_innov_var, output_names=["innov", "innov_var"])
|
||||
generate_px4_function(compute_airspeed_h_and_k, output_names=["H", "K"])
|
||||
|
||||
generate_px4_function(compute_sideslip_innov_and_innov_var, output_names=["innov", "innov_var"])
|
||||
generate_px4_function(compute_sideslip_h_and_k, output_names=["H", "K"])
|
||||
|
||||
@ -0,0 +1,181 @@
|
||||
// -----------------------------------------------------------------------------
|
||||
// This file was autogenerated by symforce from template:
|
||||
// backends/cpp/templates/function/FUNCTION.h.jinja
|
||||
// Do NOT modify by hand.
|
||||
// -----------------------------------------------------------------------------
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <matrix/math.hpp>
|
||||
|
||||
namespace sym {
|
||||
|
||||
/**
|
||||
* This function was autogenerated from a symbolic function. Do not modify by hand.
|
||||
*
|
||||
* Symbolic function: compute_sideslip_h_and_k
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix24_1
|
||||
* P: Matrix24_24
|
||||
* innov_var: Scalar
|
||||
* epsilon: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* H: Matrix24_1
|
||||
* K: Matrix24_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeSideslipHAndK(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar innov_var,
|
||||
const Scalar epsilon, matrix::Matrix<Scalar, 24, 1>* const H = nullptr,
|
||||
matrix::Matrix<Scalar, 24, 1>* const K = nullptr) {
|
||||
// Total ops: 539
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (44)
|
||||
const Scalar _tmp0 = std::pow(state(2, 0), Scalar(2));
|
||||
const Scalar _tmp1 = std::pow(state(1, 0), Scalar(2));
|
||||
const Scalar _tmp2 = std::pow(state(0, 0), Scalar(2)) - std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp3 = -_tmp0 + _tmp1 + _tmp2;
|
||||
const Scalar _tmp4 = -state(22, 0) + state(4, 0);
|
||||
const Scalar _tmp5 = state(0, 0) * state(3, 0);
|
||||
const Scalar _tmp6 = state(1, 0) * state(2, 0);
|
||||
const Scalar _tmp7 = _tmp5 + _tmp6;
|
||||
const Scalar _tmp8 = -state(23, 0) + state(5, 0);
|
||||
const Scalar _tmp9 = 2 * _tmp8;
|
||||
const Scalar _tmp10 = state(0, 0) * state(2, 0);
|
||||
const Scalar _tmp11 = state(1, 0) * state(3, 0);
|
||||
const Scalar _tmp12 = 2 * state(6, 0);
|
||||
const Scalar _tmp13 = _tmp12 * (-_tmp10 + _tmp11) + _tmp3 * _tmp4 + _tmp7 * _tmp9;
|
||||
const Scalar _tmp14 =
|
||||
_tmp13 + epsilon * (2 * math::min<Scalar>(0, (((_tmp13) > 0) - ((_tmp13) < 0))) + 1);
|
||||
const Scalar _tmp15 = Scalar(1.0) / (_tmp14);
|
||||
const Scalar _tmp16 = 2 * _tmp4;
|
||||
const Scalar _tmp17 = _tmp12 * state(1, 0) - _tmp16 * state(3, 0) + _tmp9 * state(0, 0);
|
||||
const Scalar _tmp18 = _tmp16 * state(0, 0);
|
||||
const Scalar _tmp19 = _tmp9 * state(3, 0);
|
||||
const Scalar _tmp20 = _tmp12 * state(2, 0);
|
||||
const Scalar _tmp21 = _tmp0 - _tmp1 + _tmp2;
|
||||
const Scalar _tmp22 = -2 * _tmp5 + 2 * _tmp6;
|
||||
const Scalar _tmp23 = state(2, 0) * state(3, 0);
|
||||
const Scalar _tmp24 = state(0, 0) * state(1, 0);
|
||||
const Scalar _tmp25 =
|
||||
(_tmp12 * (_tmp23 + _tmp24) + _tmp21 * _tmp8 + _tmp22 * _tmp4) / std::pow(_tmp14, Scalar(2));
|
||||
const Scalar _tmp26 = _tmp15 * _tmp17 - _tmp25 * (_tmp18 + _tmp19 - _tmp20);
|
||||
const Scalar _tmp27 = _tmp9 * state(1, 0);
|
||||
const Scalar _tmp28 = _tmp16 * state(2, 0);
|
||||
const Scalar _tmp29 = _tmp12 * state(0, 0);
|
||||
const Scalar _tmp30 = _tmp12 * state(3, 0) + _tmp16 * state(1, 0) + _tmp9 * state(2, 0);
|
||||
const Scalar _tmp31 = _tmp15 * (-_tmp27 + _tmp28 + _tmp29) - _tmp25 * _tmp30;
|
||||
const Scalar _tmp32 = _tmp15 * _tmp30 - _tmp25 * (_tmp27 - _tmp28 - _tmp29);
|
||||
const Scalar _tmp33 = _tmp15 * (-_tmp18 - _tmp19 + _tmp20) - _tmp17 * _tmp25;
|
||||
const Scalar _tmp34 = _tmp25 * _tmp3;
|
||||
const Scalar _tmp35 = 2 * _tmp5;
|
||||
const Scalar _tmp36 = 2 * _tmp6;
|
||||
const Scalar _tmp37 = _tmp15 * (-_tmp35 + _tmp36) - _tmp34;
|
||||
const Scalar _tmp38 = _tmp15 * _tmp21;
|
||||
const Scalar _tmp39 = -_tmp25 * (_tmp35 + _tmp36) + _tmp38;
|
||||
const Scalar _tmp40 = _tmp15 * (2 * _tmp23 + 2 * _tmp24) - _tmp25 * (-2 * _tmp10 + 2 * _tmp11);
|
||||
const Scalar _tmp41 = -_tmp15 * _tmp22 + _tmp34;
|
||||
const Scalar _tmp42 = 2 * _tmp25 * _tmp7 - _tmp38;
|
||||
const Scalar _tmp43 = Scalar(1.0) / (math::max<Scalar>(epsilon, innov_var));
|
||||
|
||||
// Output terms (2)
|
||||
if (H != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _H = (*H);
|
||||
|
||||
_H.setZero();
|
||||
|
||||
_H(0, 0) = _tmp26;
|
||||
_H(1, 0) = _tmp31;
|
||||
_H(2, 0) = _tmp32;
|
||||
_H(3, 0) = _tmp33;
|
||||
_H(4, 0) = _tmp37;
|
||||
_H(5, 0) = _tmp39;
|
||||
_H(6, 0) = _tmp40;
|
||||
_H(22, 0) = _tmp41;
|
||||
_H(23, 0) = _tmp42;
|
||||
}
|
||||
|
||||
if (K != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _K = (*K);
|
||||
|
||||
_K(0, 0) = _tmp43 * (P(0, 0) * _tmp26 + P(0, 1) * _tmp31 + P(0, 2) * _tmp32 +
|
||||
P(0, 22) * _tmp41 + P(0, 23) * _tmp42 + P(0, 3) * _tmp33 +
|
||||
P(0, 4) * _tmp37 + P(0, 5) * _tmp39 + P(0, 6) * _tmp40);
|
||||
_K(1, 0) = _tmp43 * (P(1, 0) * _tmp26 + P(1, 1) * _tmp31 + P(1, 2) * _tmp32 +
|
||||
P(1, 22) * _tmp41 + P(1, 23) * _tmp42 + P(1, 3) * _tmp33 +
|
||||
P(1, 4) * _tmp37 + P(1, 5) * _tmp39 + P(1, 6) * _tmp40);
|
||||
_K(2, 0) = _tmp43 * (P(2, 0) * _tmp26 + P(2, 1) * _tmp31 + P(2, 2) * _tmp32 +
|
||||
P(2, 22) * _tmp41 + P(2, 23) * _tmp42 + P(2, 3) * _tmp33 +
|
||||
P(2, 4) * _tmp37 + P(2, 5) * _tmp39 + P(2, 6) * _tmp40);
|
||||
_K(3, 0) = _tmp43 * (P(3, 0) * _tmp26 + P(3, 1) * _tmp31 + P(3, 2) * _tmp32 +
|
||||
P(3, 22) * _tmp41 + P(3, 23) * _tmp42 + P(3, 3) * _tmp33 +
|
||||
P(3, 4) * _tmp37 + P(3, 5) * _tmp39 + P(3, 6) * _tmp40);
|
||||
_K(4, 0) = _tmp43 * (P(4, 0) * _tmp26 + P(4, 1) * _tmp31 + P(4, 2) * _tmp32 +
|
||||
P(4, 22) * _tmp41 + P(4, 23) * _tmp42 + P(4, 3) * _tmp33 +
|
||||
P(4, 4) * _tmp37 + P(4, 5) * _tmp39 + P(4, 6) * _tmp40);
|
||||
_K(5, 0) = _tmp43 * (P(5, 0) * _tmp26 + P(5, 1) * _tmp31 + P(5, 2) * _tmp32 +
|
||||
P(5, 22) * _tmp41 + P(5, 23) * _tmp42 + P(5, 3) * _tmp33 +
|
||||
P(5, 4) * _tmp37 + P(5, 5) * _tmp39 + P(5, 6) * _tmp40);
|
||||
_K(6, 0) = _tmp43 * (P(6, 0) * _tmp26 + P(6, 1) * _tmp31 + P(6, 2) * _tmp32 +
|
||||
P(6, 22) * _tmp41 + P(6, 23) * _tmp42 + P(6, 3) * _tmp33 +
|
||||
P(6, 4) * _tmp37 + P(6, 5) * _tmp39 + P(6, 6) * _tmp40);
|
||||
_K(7, 0) = _tmp43 * (P(7, 0) * _tmp26 + P(7, 1) * _tmp31 + P(7, 2) * _tmp32 +
|
||||
P(7, 22) * _tmp41 + P(7, 23) * _tmp42 + P(7, 3) * _tmp33 +
|
||||
P(7, 4) * _tmp37 + P(7, 5) * _tmp39 + P(7, 6) * _tmp40);
|
||||
_K(8, 0) = _tmp43 * (P(8, 0) * _tmp26 + P(8, 1) * _tmp31 + P(8, 2) * _tmp32 +
|
||||
P(8, 22) * _tmp41 + P(8, 23) * _tmp42 + P(8, 3) * _tmp33 +
|
||||
P(8, 4) * _tmp37 + P(8, 5) * _tmp39 + P(8, 6) * _tmp40);
|
||||
_K(9, 0) = _tmp43 * (P(9, 0) * _tmp26 + P(9, 1) * _tmp31 + P(9, 2) * _tmp32 +
|
||||
P(9, 22) * _tmp41 + P(9, 23) * _tmp42 + P(9, 3) * _tmp33 +
|
||||
P(9, 4) * _tmp37 + P(9, 5) * _tmp39 + P(9, 6) * _tmp40);
|
||||
_K(10, 0) = _tmp43 * (P(10, 0) * _tmp26 + P(10, 1) * _tmp31 + P(10, 2) * _tmp32 +
|
||||
P(10, 22) * _tmp41 + P(10, 23) * _tmp42 + P(10, 3) * _tmp33 +
|
||||
P(10, 4) * _tmp37 + P(10, 5) * _tmp39 + P(10, 6) * _tmp40);
|
||||
_K(11, 0) = _tmp43 * (P(11, 0) * _tmp26 + P(11, 1) * _tmp31 + P(11, 2) * _tmp32 +
|
||||
P(11, 22) * _tmp41 + P(11, 23) * _tmp42 + P(11, 3) * _tmp33 +
|
||||
P(11, 4) * _tmp37 + P(11, 5) * _tmp39 + P(11, 6) * _tmp40);
|
||||
_K(12, 0) = _tmp43 * (P(12, 0) * _tmp26 + P(12, 1) * _tmp31 + P(12, 2) * _tmp32 +
|
||||
P(12, 22) * _tmp41 + P(12, 23) * _tmp42 + P(12, 3) * _tmp33 +
|
||||
P(12, 4) * _tmp37 + P(12, 5) * _tmp39 + P(12, 6) * _tmp40);
|
||||
_K(13, 0) = _tmp43 * (P(13, 0) * _tmp26 + P(13, 1) * _tmp31 + P(13, 2) * _tmp32 +
|
||||
P(13, 22) * _tmp41 + P(13, 23) * _tmp42 + P(13, 3) * _tmp33 +
|
||||
P(13, 4) * _tmp37 + P(13, 5) * _tmp39 + P(13, 6) * _tmp40);
|
||||
_K(14, 0) = _tmp43 * (P(14, 0) * _tmp26 + P(14, 1) * _tmp31 + P(14, 2) * _tmp32 +
|
||||
P(14, 22) * _tmp41 + P(14, 23) * _tmp42 + P(14, 3) * _tmp33 +
|
||||
P(14, 4) * _tmp37 + P(14, 5) * _tmp39 + P(14, 6) * _tmp40);
|
||||
_K(15, 0) = _tmp43 * (P(15, 0) * _tmp26 + P(15, 1) * _tmp31 + P(15, 2) * _tmp32 +
|
||||
P(15, 22) * _tmp41 + P(15, 23) * _tmp42 + P(15, 3) * _tmp33 +
|
||||
P(15, 4) * _tmp37 + P(15, 5) * _tmp39 + P(15, 6) * _tmp40);
|
||||
_K(16, 0) = _tmp43 * (P(16, 0) * _tmp26 + P(16, 1) * _tmp31 + P(16, 2) * _tmp32 +
|
||||
P(16, 22) * _tmp41 + P(16, 23) * _tmp42 + P(16, 3) * _tmp33 +
|
||||
P(16, 4) * _tmp37 + P(16, 5) * _tmp39 + P(16, 6) * _tmp40);
|
||||
_K(17, 0) = _tmp43 * (P(17, 0) * _tmp26 + P(17, 1) * _tmp31 + P(17, 2) * _tmp32 +
|
||||
P(17, 22) * _tmp41 + P(17, 23) * _tmp42 + P(17, 3) * _tmp33 +
|
||||
P(17, 4) * _tmp37 + P(17, 5) * _tmp39 + P(17, 6) * _tmp40);
|
||||
_K(18, 0) = _tmp43 * (P(18, 0) * _tmp26 + P(18, 1) * _tmp31 + P(18, 2) * _tmp32 +
|
||||
P(18, 22) * _tmp41 + P(18, 23) * _tmp42 + P(18, 3) * _tmp33 +
|
||||
P(18, 4) * _tmp37 + P(18, 5) * _tmp39 + P(18, 6) * _tmp40);
|
||||
_K(19, 0) = _tmp43 * (P(19, 0) * _tmp26 + P(19, 1) * _tmp31 + P(19, 2) * _tmp32 +
|
||||
P(19, 22) * _tmp41 + P(19, 23) * _tmp42 + P(19, 3) * _tmp33 +
|
||||
P(19, 4) * _tmp37 + P(19, 5) * _tmp39 + P(19, 6) * _tmp40);
|
||||
_K(20, 0) = _tmp43 * (P(20, 0) * _tmp26 + P(20, 1) * _tmp31 + P(20, 2) * _tmp32 +
|
||||
P(20, 22) * _tmp41 + P(20, 23) * _tmp42 + P(20, 3) * _tmp33 +
|
||||
P(20, 4) * _tmp37 + P(20, 5) * _tmp39 + P(20, 6) * _tmp40);
|
||||
_K(21, 0) = _tmp43 * (P(21, 0) * _tmp26 + P(21, 1) * _tmp31 + P(21, 2) * _tmp32 +
|
||||
P(21, 22) * _tmp41 + P(21, 23) * _tmp42 + P(21, 3) * _tmp33 +
|
||||
P(21, 4) * _tmp37 + P(21, 5) * _tmp39 + P(21, 6) * _tmp40);
|
||||
_K(22, 0) = _tmp43 * (P(22, 0) * _tmp26 + P(22, 1) * _tmp31 + P(22, 2) * _tmp32 +
|
||||
P(22, 22) * _tmp41 + P(22, 23) * _tmp42 + P(22, 3) * _tmp33 +
|
||||
P(22, 4) * _tmp37 + P(22, 5) * _tmp39 + P(22, 6) * _tmp40);
|
||||
_K(23, 0) = _tmp43 * (P(23, 0) * _tmp26 + P(23, 1) * _tmp31 + P(23, 2) * _tmp32 +
|
||||
P(23, 22) * _tmp41 + P(23, 23) * _tmp42 + P(23, 3) * _tmp33 +
|
||||
P(23, 4) * _tmp37 + P(23, 5) * _tmp39 + P(23, 6) * _tmp40);
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
// NOLINTNEXTLINE(readability/fn_size)
|
||||
} // namespace sym
|
||||
@ -0,0 +1,126 @@
|
||||
// -----------------------------------------------------------------------------
|
||||
// This file was autogenerated by symforce from template:
|
||||
// backends/cpp/templates/function/FUNCTION.h.jinja
|
||||
// Do NOT modify by hand.
|
||||
// -----------------------------------------------------------------------------
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <matrix/math.hpp>
|
||||
|
||||
namespace sym {
|
||||
|
||||
/**
|
||||
* This function was autogenerated from a symbolic function. Do not modify by hand.
|
||||
*
|
||||
* Symbolic function: compute_sideslip_innov_and_innov_var
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix24_1
|
||||
* P: Matrix24_24
|
||||
* R: Scalar
|
||||
* epsilon: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* innov: Scalar
|
||||
* innov_var: Scalar
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeSideslipInnovAndInnovVar(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
|
||||
const Scalar epsilon, Scalar* const innov = nullptr,
|
||||
Scalar* const innov_var = nullptr) {
|
||||
// Total ops: 277
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (44)
|
||||
const Scalar _tmp0 = std::pow(state(2, 0), Scalar(2));
|
||||
const Scalar _tmp1 = std::pow(state(1, 0), Scalar(2));
|
||||
const Scalar _tmp2 = std::pow(state(0, 0), Scalar(2)) - std::pow(state(3, 0), Scalar(2));
|
||||
const Scalar _tmp3 = -_tmp0 + _tmp1 + _tmp2;
|
||||
const Scalar _tmp4 = -state(22, 0) + state(4, 0);
|
||||
const Scalar _tmp5 = state(0, 0) * state(3, 0);
|
||||
const Scalar _tmp6 = state(1, 0) * state(2, 0);
|
||||
const Scalar _tmp7 = _tmp5 + _tmp6;
|
||||
const Scalar _tmp8 = -state(23, 0) + state(5, 0);
|
||||
const Scalar _tmp9 = 2 * _tmp8;
|
||||
const Scalar _tmp10 = state(0, 0) * state(2, 0);
|
||||
const Scalar _tmp11 = state(1, 0) * state(3, 0);
|
||||
const Scalar _tmp12 = 2 * state(6, 0);
|
||||
const Scalar _tmp13 = _tmp12 * (-_tmp10 + _tmp11) + _tmp3 * _tmp4 + _tmp7 * _tmp9;
|
||||
const Scalar _tmp14 =
|
||||
_tmp13 + epsilon * (2 * math::min<Scalar>(0, (((_tmp13) > 0) - ((_tmp13) < 0))) + 1);
|
||||
const Scalar _tmp15 = Scalar(1.0) / (_tmp14);
|
||||
const Scalar _tmp16 = _tmp0 - _tmp1 + _tmp2;
|
||||
const Scalar _tmp17 = -_tmp5 + _tmp6;
|
||||
const Scalar _tmp18 = 2 * _tmp4;
|
||||
const Scalar _tmp19 = state(2, 0) * state(3, 0);
|
||||
const Scalar _tmp20 = state(0, 0) * state(1, 0);
|
||||
const Scalar _tmp21 = _tmp12 * (_tmp19 + _tmp20) + _tmp16 * _tmp8 + _tmp17 * _tmp18;
|
||||
const Scalar _tmp22 = _tmp9 * state(1, 0);
|
||||
const Scalar _tmp23 = _tmp18 * state(2, 0);
|
||||
const Scalar _tmp24 = _tmp12 * state(0, 0);
|
||||
const Scalar _tmp25 = _tmp12 * state(3, 0) + _tmp18 * state(1, 0) + _tmp9 * state(2, 0);
|
||||
const Scalar _tmp26 = _tmp21 / std::pow(_tmp14, Scalar(2));
|
||||
const Scalar _tmp27 = _tmp15 * (-_tmp22 + _tmp23 + _tmp24) - _tmp25 * _tmp26;
|
||||
const Scalar _tmp28 = _tmp15 * _tmp16;
|
||||
const Scalar _tmp29 = 2 * _tmp5;
|
||||
const Scalar _tmp30 = 2 * _tmp6;
|
||||
const Scalar _tmp31 = -_tmp26 * (_tmp29 + _tmp30) + _tmp28;
|
||||
const Scalar _tmp32 = _tmp12 * state(1, 0) - _tmp18 * state(3, 0) + _tmp9 * state(0, 0);
|
||||
const Scalar _tmp33 = _tmp18 * state(0, 0);
|
||||
const Scalar _tmp34 = _tmp9 * state(3, 0);
|
||||
const Scalar _tmp35 = _tmp12 * state(2, 0);
|
||||
const Scalar _tmp36 = _tmp15 * _tmp32 - _tmp26 * (_tmp33 + _tmp34 - _tmp35);
|
||||
const Scalar _tmp37 = 2 * _tmp26 * _tmp7 - _tmp28;
|
||||
const Scalar _tmp38 = _tmp26 * _tmp3;
|
||||
const Scalar _tmp39 = -2 * _tmp15 * _tmp17 + _tmp38;
|
||||
const Scalar _tmp40 = _tmp15 * _tmp25 - _tmp26 * (_tmp22 - _tmp23 - _tmp24);
|
||||
const Scalar _tmp41 = _tmp15 * (-_tmp33 - _tmp34 + _tmp35) - _tmp26 * _tmp32;
|
||||
const Scalar _tmp42 = _tmp15 * (2 * _tmp19 + 2 * _tmp20) - _tmp26 * (-2 * _tmp10 + 2 * _tmp11);
|
||||
const Scalar _tmp43 = _tmp15 * (-_tmp29 + _tmp30) - _tmp38;
|
||||
|
||||
// Output terms (2)
|
||||
if (innov != nullptr) {
|
||||
Scalar& _innov = (*innov);
|
||||
|
||||
_innov = -_tmp15 * _tmp21;
|
||||
}
|
||||
|
||||
if (innov_var != nullptr) {
|
||||
Scalar& _innov_var = (*innov_var);
|
||||
|
||||
_innov_var = R +
|
||||
_tmp27 * (P(0, 1) * _tmp36 + P(1, 1) * _tmp27 + P(2, 1) * _tmp40 +
|
||||
P(22, 1) * _tmp39 + P(23, 1) * _tmp37 + P(3, 1) * _tmp41 +
|
||||
P(4, 1) * _tmp43 + P(5, 1) * _tmp31 + P(6, 1) * _tmp42) +
|
||||
_tmp31 * (P(0, 5) * _tmp36 + P(1, 5) * _tmp27 + P(2, 5) * _tmp40 +
|
||||
P(22, 5) * _tmp39 + P(23, 5) * _tmp37 + P(3, 5) * _tmp41 +
|
||||
P(4, 5) * _tmp43 + P(5, 5) * _tmp31 + P(6, 5) * _tmp42) +
|
||||
_tmp36 * (P(0, 0) * _tmp36 + P(1, 0) * _tmp27 + P(2, 0) * _tmp40 +
|
||||
P(22, 0) * _tmp39 + P(23, 0) * _tmp37 + P(3, 0) * _tmp41 +
|
||||
P(4, 0) * _tmp43 + P(5, 0) * _tmp31 + P(6, 0) * _tmp42) +
|
||||
_tmp37 * (P(0, 23) * _tmp36 + P(1, 23) * _tmp27 + P(2, 23) * _tmp40 +
|
||||
P(22, 23) * _tmp39 + P(23, 23) * _tmp37 + P(3, 23) * _tmp41 +
|
||||
P(4, 23) * _tmp43 + P(5, 23) * _tmp31 + P(6, 23) * _tmp42) +
|
||||
_tmp39 * (P(0, 22) * _tmp36 + P(1, 22) * _tmp27 + P(2, 22) * _tmp40 +
|
||||
P(22, 22) * _tmp39 + P(23, 22) * _tmp37 + P(3, 22) * _tmp41 +
|
||||
P(4, 22) * _tmp43 + P(5, 22) * _tmp31 + P(6, 22) * _tmp42) +
|
||||
_tmp40 * (P(0, 2) * _tmp36 + P(1, 2) * _tmp27 + P(2, 2) * _tmp40 +
|
||||
P(22, 2) * _tmp39 + P(23, 2) * _tmp37 + P(3, 2) * _tmp41 +
|
||||
P(4, 2) * _tmp43 + P(5, 2) * _tmp31 + P(6, 2) * _tmp42) +
|
||||
_tmp41 * (P(0, 3) * _tmp36 + P(1, 3) * _tmp27 + P(2, 3) * _tmp40 +
|
||||
P(22, 3) * _tmp39 + P(23, 3) * _tmp37 + P(3, 3) * _tmp41 +
|
||||
P(4, 3) * _tmp43 + P(5, 3) * _tmp31 + P(6, 3) * _tmp42) +
|
||||
_tmp42 * (P(0, 6) * _tmp36 + P(1, 6) * _tmp27 + P(2, 6) * _tmp40 +
|
||||
P(22, 6) * _tmp39 + P(23, 6) * _tmp37 + P(3, 6) * _tmp41 +
|
||||
P(4, 6) * _tmp43 + P(5, 6) * _tmp31 + P(6, 6) * _tmp42) +
|
||||
_tmp43 * (P(0, 4) * _tmp36 + P(1, 4) * _tmp27 + P(2, 4) * _tmp40 +
|
||||
P(22, 4) * _tmp39 + P(23, 4) * _tmp37 + P(3, 4) * _tmp41 +
|
||||
P(4, 4) * _tmp43 + P(5, 4) * _tmp31 + P(6, 4) * _tmp42);
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
// NOLINTNEXTLINE(readability/fn_size)
|
||||
} // namespace sym
|
||||
@ -42,176 +42,87 @@
|
||||
*/
|
||||
|
||||
#include "ekf.h"
|
||||
#include "python/ekf_derivation/generated/compute_sideslip_innov_and_innov_var.h"
|
||||
#include "python/ekf_derivation/generated/compute_sideslip_h_and_k.h"
|
||||
|
||||
#include <mathlib/mathlib.h>
|
||||
|
||||
void Ekf::fuseSideslip()
|
||||
void Ekf::updateSideslip(estimator_aid_source_1d_s &sideslip) const
|
||||
{
|
||||
// get latest estimated orientation
|
||||
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);
|
||||
// reset flags
|
||||
resetEstimatorAidStatusFlags(sideslip);
|
||||
|
||||
// get latest velocity in earth frame
|
||||
const float vn = _state.vel(0);
|
||||
const float ve = _state.vel(1);
|
||||
const float vd = _state.vel(2);
|
||||
const float R = sq(_params.beta_noise); // observation noise variance
|
||||
|
||||
// get latest wind velocity in earth frame
|
||||
const float vwn = _state.wind_vel(0);
|
||||
const float vwe = _state.wind_vel(1);
|
||||
float innov = 0.f;
|
||||
float innov_var = 0.f;
|
||||
sym::ComputeSideslipInnovAndInnovVar(getStateAtFusionHorizonAsVector(), P, R, FLT_EPSILON, &innov, &innov_var);
|
||||
|
||||
// calculate relative wind velocity in earth frame and rotate into body frame
|
||||
const Vector3f rel_wind_earth(vn - vwn, ve - vwe, vd);
|
||||
const Dcmf earth_to_body = quatToInverseRotMat(_state.quat_nominal);
|
||||
const Vector3f rel_wind_body = earth_to_body * rel_wind_earth;
|
||||
sideslip.observation = 0.f;
|
||||
sideslip.observation_variance = R;
|
||||
sideslip.innovation = innov;
|
||||
sideslip.innovation_variance = innov_var;
|
||||
|
||||
// perform fusion of assumed sideslip = 0
|
||||
if (rel_wind_body.norm() > 7.0f) {
|
||||
const float R_BETA = sq(_params.beta_noise); // observation noise variance
|
||||
sideslip.fusion_enabled = _control_status.flags.fuse_aspd;
|
||||
|
||||
// determine if we need the sideslip fusion to correct states other than wind
|
||||
bool update_wind_only = !_control_status.flags.wind_dead_reckoning;
|
||||
sideslip.timestamp_sample = _imu_sample_delayed.time_us;
|
||||
|
||||
// Intermediate Values
|
||||
const float HK0 = vn - vwn;
|
||||
const float HK1 = ve - vwe;
|
||||
const float HK2 = HK0*q0 + HK1*q3 - q2*vd;
|
||||
const float HK3 = q0*q2 - q1*q3;
|
||||
const float HK4 = 2*vd;
|
||||
const float HK5 = q0*q3;
|
||||
const float HK6 = q1*q2;
|
||||
const float HK7 = 2*HK5 + 2*HK6;
|
||||
const float HK8 = ecl::powf(q0, 2);
|
||||
const float HK9 = ecl::powf(q3, 2);
|
||||
const float HK10 = HK8 - HK9;
|
||||
const float HK11 = ecl::powf(q1, 2);
|
||||
const float HK12 = ecl::powf(q2, 2);
|
||||
const float HK13 = HK11 - HK12;
|
||||
const float HK14 = HK10 + HK13;
|
||||
const float HK15 = HK0*HK14 + HK1*HK7 - HK3*HK4;
|
||||
const float HK16 = 1.0F/HK15;
|
||||
const float HK17 = q0*q1 + q2*q3;
|
||||
const float HK18 = HK10 - HK11 + HK12;
|
||||
const float HK19 = HK16*(-2*HK0*(HK5 - HK6) + HK1*HK18 + HK17*HK4);
|
||||
const float HK20 = -HK0*q3 + HK1*q0 + q1*vd;
|
||||
const float HK21 = -HK19*HK2 + HK20;
|
||||
const float HK22 = 2*HK16;
|
||||
const float HK23 = HK0*q1 + HK1*q2 + q3*vd;
|
||||
const float HK24 = HK0*q2 - HK1*q1 + q0*vd;
|
||||
const float HK25 = -HK19*HK23 + HK24;
|
||||
const float HK26 = HK19*HK24 + HK23;
|
||||
const float HK27 = HK19*HK20 + HK2;
|
||||
const float HK28 = HK14*HK19 + 2*HK5 - 2*HK6;
|
||||
const float HK29 = HK16*HK28;
|
||||
const float HK30 = HK19*HK7;
|
||||
const float HK31 = HK17 + HK19*HK3;
|
||||
const float HK32 = HK13 + HK30 - HK8 + HK9;
|
||||
const float HK33 = 2*HK31;
|
||||
const float HK34 = 2*HK26;
|
||||
const float HK35 = 2*HK25;
|
||||
const float HK36 = 2*HK27;
|
||||
const float HK37 = 2*HK21;
|
||||
const float HK38 = HK28*P(0,22) - HK28*P(0,4) + HK32*P(0,23) - HK32*P(0,5) + HK33*P(0,6) + HK34*P(0,2) + HK35*P(0,1) - HK36*P(0,3) + HK37*P(0,0);
|
||||
const float HK39 = ecl::powf(HK15, -2);
|
||||
const float HK40 = -HK28*P(4,6) + HK28*P(6,22) - HK32*P(5,6) + HK32*P(6,23) + HK33*P(6,6) + HK34*P(2,6) + HK35*P(1,6) - HK36*P(3,6) + HK37*P(0,6);
|
||||
const float HK41 = HK32*P(5,23);
|
||||
const float HK42 = HK28*P(22,23) - HK28*P(4,23) + HK32*P(23,23) + HK33*P(6,23) + HK34*P(2,23) + HK35*P(1,23) - HK36*P(3,23) + HK37*P(0,23) - HK41;
|
||||
const float HK43 = HK32*HK39;
|
||||
const float HK44 = HK28*P(4,22);
|
||||
const float HK45 = HK28*P(22,22) + HK32*P(22,23) - HK32*P(5,22) + HK33*P(6,22) + HK34*P(2,22) + HK35*P(1,22) - HK36*P(3,22) + HK37*P(0,22) - HK44;
|
||||
const float HK46 = HK28*HK39;
|
||||
const float HK47 = -HK28*P(4,5) + HK28*P(5,22) - HK32*P(5,5) + HK33*P(5,6) + HK34*P(2,5) + HK35*P(1,5) - HK36*P(3,5) + HK37*P(0,5) + HK41;
|
||||
const float HK48 = -HK28*P(4,4) + HK32*P(4,23) - HK32*P(4,5) + HK33*P(4,6) + HK34*P(2,4) + HK35*P(1,4) - HK36*P(3,4) + HK37*P(0,4) + HK44;
|
||||
const float HK49 = HK28*P(2,22) - HK28*P(2,4) + HK32*P(2,23) - HK32*P(2,5) + HK33*P(2,6) + HK34*P(2,2) + HK35*P(1,2) - HK36*P(2,3) + HK37*P(0,2);
|
||||
const float HK50 = HK28*P(1,22) - HK28*P(1,4) + HK32*P(1,23) - HK32*P(1,5) + HK33*P(1,6) + HK34*P(1,2) + HK35*P(1,1) - HK36*P(1,3) + HK37*P(0,1);
|
||||
const float HK51 = HK28*P(3,22) - HK28*P(3,4) + HK32*P(3,23) - HK32*P(3,5) + HK33*P(3,6) + HK34*P(2,3) + HK35*P(1,3) - HK36*P(3,3) + HK37*P(0,3);
|
||||
//const float HK52 = HK16/(HK33*HK39*HK40 + HK34*HK39*HK49 + HK35*HK39*HK50 - HK36*HK39*HK51 + HK37*HK38*HK39 + HK42*HK43 - HK43*HK47 + HK45*HK46 - HK46*HK48 + R_BETA);
|
||||
const float innov_gate = fmaxf(_params.beta_innov_gate, 1.f);
|
||||
setEstimatorAidStatusTestRatio(sideslip, innov_gate);
|
||||
}
|
||||
|
||||
// innovation variance
|
||||
_beta_innov_var = (HK33*HK39*HK40 + HK34*HK39*HK49 + HK35*HK39*HK50 - HK36*HK39*HK51 + HK37*HK38*HK39 + HK42*HK43 - HK43*HK47 + HK45*HK46 - HK46*HK48 + R_BETA);
|
||||
void Ekf::fuseSideslip(estimator_aid_source_1d_s &sideslip)
|
||||
{
|
||||
if (sideslip.innovation_rejected) {
|
||||
return;
|
||||
}
|
||||
// determine if we need the sideslip fusion to correct states other than wind
|
||||
bool update_wind_only = !_control_status.flags.wind_dead_reckoning;
|
||||
|
||||
// Reset covariance and states if the calculation is badly conditioned
|
||||
if (_beta_innov_var < R_BETA) {
|
||||
_fault_status.flags.bad_sideslip = true;
|
||||
// Reset covariance and states if the calculation is badly conditioned
|
||||
if ((sideslip.innovation_variance < sideslip.observation_variance)
|
||||
|| (sideslip.innovation_variance < FLT_EPSILON)) {
|
||||
_fault_status.flags.bad_sideslip = true;
|
||||
|
||||
// if we are getting aiding from other sources, warn and reset the wind states and covariances only
|
||||
const char *action_string = nullptr;
|
||||
// if we are getting aiding from other sources, warn and reset the wind states and covariances only
|
||||
const char *action_string = nullptr;
|
||||
|
||||
if (update_wind_only) {
|
||||
resetWind();
|
||||
action_string = "wind";
|
||||
|
||||
} else {
|
||||
initialiseCovariance();
|
||||
_state.wind_vel.setZero();
|
||||
action_string = "full";
|
||||
}
|
||||
|
||||
ECL_ERR("sideslip badly conditioned - %s covariance reset", action_string);
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
_fault_status.flags.bad_sideslip = false;
|
||||
const float HK52 = HK16 / _beta_innov_var;
|
||||
|
||||
// Calculate predicted sideslip angle and innovation using small angle approximation
|
||||
_beta_innov = rel_wind_body(1) / rel_wind_body(0);
|
||||
|
||||
// Compute the ratio of innovation to gate size
|
||||
_beta_test_ratio = sq(_beta_innov) / (sq(fmaxf(_params.beta_innov_gate, 1.0f)) * _beta_innov_var);
|
||||
|
||||
// if the innovation consistency check fails then don't fuse the sample and indicate bad beta health
|
||||
if (_beta_test_ratio > 1.0f) {
|
||||
_innov_check_fail_status.flags.reject_sideslip = true;
|
||||
return;
|
||||
if (update_wind_only) {
|
||||
resetWind();
|
||||
action_string = "wind";
|
||||
|
||||
} else {
|
||||
_innov_check_fail_status.flags.reject_sideslip = false;
|
||||
initialiseCovariance();
|
||||
_state.wind_vel.setZero();
|
||||
action_string = "full";
|
||||
}
|
||||
|
||||
// Observation Jacobians
|
||||
SparseVector24f<0,1,2,3,4,5,6,22,23> Hfusion;
|
||||
Hfusion.at<0>() = HK21*HK22;
|
||||
Hfusion.at<1>() = HK22*HK25;
|
||||
Hfusion.at<2>() = HK22*HK26;
|
||||
Hfusion.at<3>() = -HK22*HK27;
|
||||
Hfusion.at<4>() = -HK29;
|
||||
Hfusion.at<5>() = HK16*(HK18 - HK30);
|
||||
Hfusion.at<6>() = HK22*HK31;
|
||||
Hfusion.at<22>() = HK29;
|
||||
Hfusion.at<23>() = HK16*HK32;
|
||||
ECL_ERR("sideslip badly conditioned - %s covariance reset", action_string);
|
||||
|
||||
// Calculate Kalman gains
|
||||
Vector24f Kfusion;
|
||||
return;
|
||||
}
|
||||
|
||||
if (!update_wind_only) {
|
||||
_fault_status.flags.bad_sideslip = false;
|
||||
|
||||
Kfusion(0) = HK38*HK52;
|
||||
Kfusion(1) = HK50*HK52;
|
||||
Kfusion(2) = HK49*HK52;
|
||||
Kfusion(3) = HK51*HK52;
|
||||
Kfusion(4) = HK48*HK52;
|
||||
Kfusion(5) = HK47*HK52;
|
||||
Kfusion(6) = HK40*HK52;
|
||||
Vector24f H; // Observation jacobian
|
||||
Vector24f K; // Kalman gain vector
|
||||
|
||||
for (unsigned row = 7; row <= 21; row++) {
|
||||
Kfusion(row) = HK52*(HK28*P(row,22) - HK28*P(4,row) + HK32*P(row,23) - HK32*P(5,row) + HK33*P(6,row) + HK34*P(2,row) + HK35*P(1,row) - HK36*P(3,row) + HK37*P(0,row));
|
||||
}
|
||||
sym::ComputeSideslipHAndK(getStateAtFusionHorizonAsVector(), P, sideslip.innovation_variance, FLT_EPSILON, &H, &K);
|
||||
|
||||
}
|
||||
SparseVector24f<0,1,2,3,4,5,6,22,23> H_sparse(H);
|
||||
|
||||
Kfusion(22) = HK45*HK52;
|
||||
Kfusion(23) = HK42*HK52;
|
||||
|
||||
const bool is_fused = measurementUpdate(Kfusion, Hfusion, _beta_innov);
|
||||
|
||||
_fault_status.flags.bad_sideslip = !is_fused;
|
||||
|
||||
if (is_fused) {
|
||||
_time_last_beta_fuse = _imu_sample_delayed.time_us;
|
||||
if (update_wind_only) {
|
||||
for (unsigned row = 0; row <= 21; row++) {
|
||||
K(row) = 0.f;
|
||||
}
|
||||
}
|
||||
|
||||
const bool is_fused = measurementUpdate(K, H_sparse, sideslip.innovation);
|
||||
|
||||
sideslip.fused = is_fused;
|
||||
_fault_status.flags.bad_sideslip = !is_fused;
|
||||
|
||||
if (is_fused) {
|
||||
sideslip.time_last_fuse = _imu_sample_delayed.time_us;
|
||||
}
|
||||
}
|
||||
|
||||
@ -714,6 +714,9 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime ×tamp)
|
||||
// airspeed
|
||||
PublishAidSourceStatus(_ekf.aid_src_airspeed(), _status_airspeed_pub_last, _estimator_aid_src_airspeed_pub);
|
||||
|
||||
// sideslip
|
||||
PublishAidSourceStatus(_ekf.aid_src_sideslip(), _status_sideslip_pub_last, _estimator_aid_src_sideslip_pub);
|
||||
|
||||
// baro height
|
||||
PublishAidSourceStatus(_ekf.aid_src_baro_hgt(), _status_baro_hgt_pub_last, _estimator_aid_src_baro_hgt_pub);
|
||||
|
||||
|
||||
@ -262,6 +262,7 @@ private:
|
||||
hrt_abstime _last_gps_status_published{0};
|
||||
|
||||
hrt_abstime _status_airspeed_pub_last{0};
|
||||
hrt_abstime _status_sideslip_pub_last{0};
|
||||
|
||||
hrt_abstime _status_baro_hgt_pub_last{0};
|
||||
hrt_abstime _status_rng_hgt_pub_last{0};
|
||||
@ -349,6 +350,7 @@ private:
|
||||
uORB::PublicationMulti<estimator_aid_source_1d_s> _estimator_aid_src_airspeed_pub{ORB_ID(estimator_aid_src_airspeed)};
|
||||
uORB::PublicationMulti<estimator_aid_source_1d_s> _estimator_aid_src_baro_hgt_pub{ORB_ID(estimator_aid_src_baro_hgt)};
|
||||
uORB::PublicationMulti<estimator_aid_source_1d_s> _estimator_aid_src_rng_hgt_pub{ORB_ID(estimator_aid_src_rng_hgt)};
|
||||
uORB::PublicationMulti<estimator_aid_source_1d_s> _estimator_aid_src_sideslip_pub{ORB_ID(estimator_aid_src_sideslip)};
|
||||
|
||||
uORB::PublicationMulti<estimator_aid_source_1d_s> _estimator_aid_src_fake_hgt_pub{ORB_ID(estimator_aid_src_fake_hgt)};
|
||||
uORB::PublicationMulti<estimator_aid_source_2d_s> _estimator_aid_src_fake_pos_pub{ORB_ID(estimator_aid_src_fake_pos)};
|
||||
|
||||
@ -273,6 +273,7 @@ void LoggedTopics::add_default_topics()
|
||||
add_optional_topic_multi("estimator_aid_src_mag_heading", 0, MAX_ESTIMATOR_INSTANCES);
|
||||
add_optional_topic_multi("estimator_aid_src_mag", 0, MAX_ESTIMATOR_INSTANCES);
|
||||
add_optional_topic_multi("estimator_aid_src_ev_yaw", 0, MAX_ESTIMATOR_INSTANCES);
|
||||
add_optional_topic_multi("estimator_aid_src_sideslip", 0, MAX_ESTIMATOR_INSTANCES);
|
||||
|
||||
#endif /* CONFIG_ARCH_BOARD_PX4_SITL */
|
||||
}
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user