mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-23 06:36:45 +08:00
ekf2: new gravity observation (#21038)
Signed-off-by: Daniel M. Sahu <danielmohansahu@gmail.com>
This commit is contained in:
Daniel Sahu
GitHub
@@ -18,6 +18,4 @@ bool fusion_enabled # true when measurements are being fused
|
||||
bool innovation_rejected # true if the observation has been rejected
|
||||
bool fused # true if the sample was successfully fused
|
||||
|
||||
# TOPICS estimator_aid_src_ev_vel
|
||||
# TOPICS estimator_aid_src_gnss_vel
|
||||
# TOPICS estimator_aid_src_mag
|
||||
# TOPICS estimator_aid_src_ev_vel estimator_aid_src_gnss_vel estimator_aid_src_gravity estimator_aid_src_mag
|
||||
|
||||
@@ -28,6 +28,7 @@ float32[2] terr_flow # flow innvoation (rad/sec) and innovation variance compute
|
||||
# Various
|
||||
float32 heading # heading innovation (rad) and innovation variance (rad**2)
|
||||
float32[3] mag_field # earth magnetic field innovation (Gauss) and innovation variance (Gauss**2)
|
||||
float32[3] gravity # gravity innovation from accelerometerr vector (m/s**2)
|
||||
float32[2] drag # drag specific force innovation (m/sec**2) and innovation variance ((m/sec)**2)
|
||||
float32 airspeed # airspeed innovation (m/sec) and innovation variance ((m/sec)**2)
|
||||
float32 beta # synthetic sideslip innovation (rad) and innovation variance (rad**2)
|
||||
|
||||
@@ -38,6 +38,7 @@ bool cs_wind_dead_reckoning # 30 - true if we are navigationg reliant on win
|
||||
bool cs_rng_kin_consistent # 31 - true when the range finder kinematic consistency check is passing
|
||||
bool cs_fake_pos # 32 - true when fake position measurements are being fused
|
||||
bool cs_fake_hgt # 33 - true when fake height measurements are being fused
|
||||
bool cs_gravity_vector # 34 - true when gravity vector measurements are being fused
|
||||
|
||||
# fault status
|
||||
uint32 fault_status_changes # number of filter fault status (fs) changes
|
||||
|
||||
@@ -102,6 +102,7 @@ px4_add_module(
|
||||
EKF/gps_checks.cpp
|
||||
EKF/gps_control.cpp
|
||||
EKF/gps_yaw_fusion.cpp
|
||||
EKF/gravity_fusion.cpp
|
||||
EKF/height_control.cpp
|
||||
EKF/imu_down_sampler.cpp
|
||||
EKF/mag_control.cpp
|
||||
|
||||
@@ -53,6 +53,7 @@ add_library(ecl_EKF
|
||||
gps_checks.cpp
|
||||
gps_control.cpp
|
||||
gps_yaw_fusion.cpp
|
||||
gravity_fusion.cpp
|
||||
height_control.cpp
|
||||
imu_down_sampler.cpp
|
||||
mag_control.cpp
|
||||
|
||||
@@ -127,8 +127,9 @@ enum class PositionSensor : uint8_t {
|
||||
};
|
||||
|
||||
enum class ImuCtrl : uint8_t {
|
||||
GyroBias = (1<<0),
|
||||
AccelBias = (1<<1),
|
||||
GyroBias = (1<<0),
|
||||
AccelBias = (1<<1),
|
||||
GravityVector = (1<<2),
|
||||
};
|
||||
|
||||
enum GnssCtrl : uint8_t {
|
||||
@@ -393,6 +394,9 @@ struct parameters {
|
||||
float ev_pos_innov_gate{5.0f}; ///< vision position fusion innovation consistency gate size (STD)
|
||||
float ev_hgt_bias_nsd{0.13f}; ///< process noise for vision height bias estimation (m/s/sqrt(Hz))
|
||||
|
||||
// gravity fusion
|
||||
float gravity_noise{1.0f}; ///< accelerometer measurement gaussian noise (m/s**2)
|
||||
|
||||
// optical flow fusion
|
||||
float flow_noise{0.15f}; ///< observation noise for optical flow LOS rate measurements (rad/sec)
|
||||
float flow_noise_qual_min{0.5f}; ///< observation noise for optical flow LOS rate measurements when flow sensor quality is at the minimum useable (rad/sec)
|
||||
@@ -564,6 +568,7 @@ union filter_control_status_u {
|
||||
uint64_t rng_kin_consistent : 1; ///< 31 - true when the range finder kinematic consistency check is passing
|
||||
uint64_t fake_pos : 1; ///< 32 - true when fake position measurements are being fused
|
||||
uint64_t fake_hgt : 1; ///< 33 - true when fake height measurements are being fused
|
||||
uint64_t gravity_vector : 1; ///< 34 - true when gravity vector measurements are being fused
|
||||
} flags;
|
||||
uint64_t value;
|
||||
};
|
||||
|
||||
@@ -198,6 +198,7 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
|
||||
controlBetaFusion(imu_delayed);
|
||||
controlDragFusion();
|
||||
controlHeightFusion(imu_delayed);
|
||||
controlGravityFusion(imu_delayed);
|
||||
|
||||
// Additional data odometry data from an external estimator can be fused.
|
||||
controlExternalVisionFusion();
|
||||
|
||||
@@ -191,6 +191,10 @@ public:
|
||||
void getHaglRateInnovVar(float &hagl_rate_innov_var) const { hagl_rate_innov_var = _rng_consistency_check.getInnovVar(); }
|
||||
void getHaglRateInnovRatio(float &hagl_rate_innov_ratio) const { hagl_rate_innov_ratio = _rng_consistency_check.getSignedTestRatioLpf(); }
|
||||
|
||||
void getGravityInnov(float grav_innov[3]) const { memcpy(grav_innov, _aid_src_gravity.innovation, sizeof(_aid_src_gravity.innovation)); }
|
||||
void getGravityInnovVar(float grav_innov_var[3]) const { memcpy(grav_innov_var, _aid_src_gravity.innovation_variance, sizeof(_aid_src_gravity.innovation_variance)); }
|
||||
void getGravityInnovRatio(float &grav_innov_ratio) const { grav_innov_ratio = Vector3f(_aid_src_gravity.test_ratio).max(); }
|
||||
|
||||
// get the state vector at the delayed time horizon
|
||||
matrix::Vector<float, 24> getStateAtFusionHorizonAsVector() const;
|
||||
|
||||
@@ -433,6 +437,8 @@ public:
|
||||
const auto &aid_src_mag_heading() const { return _aid_src_mag_heading; }
|
||||
const auto &aid_src_mag() const { return _aid_src_mag; }
|
||||
|
||||
const auto &aid_src_gravity() const { return _aid_src_gravity; }
|
||||
|
||||
const auto &aid_src_aux_vel() const { return _aid_src_aux_vel; }
|
||||
|
||||
const auto &aid_src_optical_flow() const { return _aid_src_optical_flow; }
|
||||
@@ -573,6 +579,8 @@ private:
|
||||
estimator_aid_source1d_s _aid_src_mag_heading{};
|
||||
estimator_aid_source3d_s _aid_src_mag{};
|
||||
|
||||
estimator_aid_source3d_s _aid_src_gravity{};
|
||||
|
||||
estimator_aid_source2d_s _aid_src_aux_vel{};
|
||||
|
||||
estimator_aid_source2d_s _aid_src_optical_flow{};
|
||||
@@ -927,6 +935,9 @@ private:
|
||||
|
||||
void updateGroundEffect();
|
||||
|
||||
// gravity fusion: heuristically enable / disable gravity fusion
|
||||
void controlGravityFusion(const imuSample &imu_delayed);
|
||||
|
||||
// calculate the measurement variance for the optical flow sensor
|
||||
float calcOptFlowMeasVar(const flowSample &flow_sample);
|
||||
|
||||
|
||||
@@ -0,0 +1,95 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 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 ECL 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 gravity_fusion.cpp
|
||||
* Fuse observations from the gravity vector to constrain roll
|
||||
* and pitch (a la complementary filter).
|
||||
*
|
||||
* @author Daniel M. Sahu <danielmohansahu@gmail.com>
|
||||
*/
|
||||
|
||||
#include "ekf.h"
|
||||
#include "python/ekf_derivation/generated/compute_gravity_innov_var_and_k_and_h.h"
|
||||
|
||||
#include <mathlib/mathlib.h>
|
||||
|
||||
void Ekf::controlGravityFusion(const imuSample &imu)
|
||||
{
|
||||
// fuse gravity observation if our overall acceleration isn't too big
|
||||
const float gravity_scale = _accel_vec_filt.norm() / CONSTANTS_ONE_G;
|
||||
|
||||
_control_status.flags.gravity_vector = (_params.imu_ctrl & static_cast<int32_t>(ImuCtrl::GravityVector))
|
||||
&& (((gravity_scale >= 0.9f && gravity_scale <= 1.1f)) || _control_status.flags.vehicle_at_rest)
|
||||
&& !isHorizontalAidingActive();
|
||||
|
||||
// get raw accelerometer reading at delayed horizon and expected measurement noise (gaussian)
|
||||
const Vector3f measurement = imu.delta_vel / imu.delta_vel_dt - getAccelBias();
|
||||
const float measurement_var = sq(_params.gravity_noise);
|
||||
|
||||
// calculate kalman gains and innovation variances
|
||||
Vector3f innovation; // innovation of the last gravity fusion observation (m/s**2)
|
||||
Vector3f innovation_variance;
|
||||
Vector24f Kx, Ky, Kz; // Kalman gain vectors
|
||||
sym::ComputeGravityInnovVarAndKAndH(
|
||||
getStateAtFusionHorizonAsVector(), P, measurement, measurement_var, FLT_EPSILON,
|
||||
&innovation, &innovation_variance, &Kx, &Ky, &Kz);
|
||||
|
||||
// fill estimator aid source status
|
||||
resetEstimatorAidStatus(_aid_src_gravity);
|
||||
_aid_src_gravity.timestamp_sample = imu.time_us;
|
||||
measurement.copyTo(_aid_src_gravity.observation);
|
||||
|
||||
for (auto &var : _aid_src_gravity.observation_variance) {
|
||||
var = measurement_var;
|
||||
}
|
||||
|
||||
innovation.copyTo(_aid_src_gravity.innovation);
|
||||
innovation_variance.copyTo(_aid_src_gravity.innovation_variance);
|
||||
|
||||
float innovation_gate = 1.f;
|
||||
setEstimatorAidStatusTestRatio(_aid_src_gravity, innovation_gate);
|
||||
|
||||
_aid_src_gravity.fusion_enabled = _control_status.flags.gravity_vector;
|
||||
|
||||
if (_aid_src_gravity.fusion_enabled && !_aid_src_gravity.innovation_rejected) {
|
||||
// perform fusion for each axis
|
||||
_aid_src_gravity.fused = measurementUpdate(Kx, innovation_variance(0), innovation(0))
|
||||
&& measurementUpdate(Ky, innovation_variance(1), innovation(1))
|
||||
&& measurementUpdate(Kz, innovation_variance(2), innovation(2));
|
||||
|
||||
if (_aid_src_gravity.fused) {
|
||||
_aid_src_gravity.time_last_fuse = imu.time_us;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -477,6 +477,36 @@ def compute_drag_y_innov_var_and_k(
|
||||
|
||||
return (innov_var, K)
|
||||
|
||||
def compute_gravity_innov_var_and_k_and_h(
|
||||
state: VState,
|
||||
P: MState,
|
||||
meas: sf.V3,
|
||||
R: sf.Scalar,
|
||||
epsilon: sf.Scalar
|
||||
) -> (sf.V3, sf.V3, VState, VState, VState):
|
||||
|
||||
# get transform from earth to body frame
|
||||
q_att = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz])
|
||||
R_to_body = quat_to_rot(q_att).T
|
||||
|
||||
# the innovation is the error between measured acceleration
|
||||
# and predicted (body frame), assuming no body acceleration
|
||||
meas_pred = R_to_body * sf.Matrix([0,0,-9.80665])
|
||||
innov = meas_pred - 9.80665 * meas.normalized(epsilon=epsilon)
|
||||
|
||||
# initialize outputs
|
||||
innov_var = sf.V3()
|
||||
K = [None] * 3
|
||||
|
||||
# calculate observation jacobian (H), kalman gain (K), and innovation variance (S)
|
||||
# for each axis
|
||||
for i in range(3):
|
||||
H = sf.V1(meas_pred[i]).jacobian(state)
|
||||
innov_var[i] = (H * P * H.T + R)[0,0]
|
||||
K[i] = P * H.T / innov_var[i]
|
||||
|
||||
return (innov, innov_var, K[0], K[1], K[2])
|
||||
|
||||
print("Derive EKF2 equations...")
|
||||
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"])
|
||||
@@ -497,3 +527,4 @@ generate_px4_function(compute_flow_y_innov_var_and_h, output_names=["innov_var",
|
||||
generate_px4_function(compute_gnss_yaw_innon_innov_var_and_h, output_names=["innov", "innov_var", "H"])
|
||||
generate_px4_function(compute_drag_x_innov_var_and_k, output_names=["innov_var", "K"])
|
||||
generate_px4_function(compute_drag_y_innov_var_and_k, output_names=["innov_var", "K"])
|
||||
generate_px4_function(compute_gravity_innov_var_and_k_and_h, output_names=["innov", "innov_var", "Kx", "Ky", "Kz"])
|
||||
|
||||
+276
@@ -0,0 +1,276 @@
|
||||
// -----------------------------------------------------------------------------
|
||||
// This file was autogenerated by symforce from template:
|
||||
// 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_gravity_innov_var_and_k_and_h
|
||||
*
|
||||
* Args:
|
||||
* state: Matrix24_1
|
||||
* P: Matrix24_24
|
||||
* meas: Matrix31
|
||||
* R: Scalar
|
||||
* epsilon: Scalar
|
||||
*
|
||||
* Outputs:
|
||||
* innov: Matrix31
|
||||
* innov_var: Matrix31
|
||||
* Kx: Matrix24_1
|
||||
* Ky: Matrix24_1
|
||||
* Kz: Matrix24_1
|
||||
*/
|
||||
template <typename Scalar>
|
||||
void ComputeGravityInnovVarAndKAndH(const matrix::Matrix<Scalar, 24, 1>& state,
|
||||
const matrix::Matrix<Scalar, 24, 24>& P,
|
||||
const matrix::Matrix<Scalar, 3, 1>& meas, const Scalar R,
|
||||
const Scalar epsilon,
|
||||
matrix::Matrix<Scalar, 3, 1>* const innov = nullptr,
|
||||
matrix::Matrix<Scalar, 3, 1>* const innov_var = nullptr,
|
||||
matrix::Matrix<Scalar, 24, 1>* const Kx = nullptr,
|
||||
matrix::Matrix<Scalar, 24, 1>* const Ky = nullptr,
|
||||
matrix::Matrix<Scalar, 24, 1>* const Kz = nullptr) {
|
||||
// Total ops: 736
|
||||
|
||||
// Input arrays
|
||||
|
||||
// Intermediate terms (54)
|
||||
const Scalar _tmp0 =
|
||||
Scalar(9.8066499999999994) /
|
||||
std::sqrt(Scalar(epsilon + std::pow(meas(0, 0), Scalar(2)) + std::pow(meas(1, 0), Scalar(2)) +
|
||||
std::pow(meas(2, 0), Scalar(2))));
|
||||
const Scalar _tmp1 = Scalar(19.613299999999999) * state(1, 0);
|
||||
const Scalar _tmp2 = -P(3, 0) * _tmp1;
|
||||
const Scalar _tmp3 = Scalar(19.613299999999999) * state(2, 0);
|
||||
const Scalar _tmp4 = P(0, 0) * _tmp3;
|
||||
const Scalar _tmp5 = Scalar(19.613299999999999) * state(0, 0);
|
||||
const Scalar _tmp6 = P(2, 0) * _tmp5;
|
||||
const Scalar _tmp7 = Scalar(19.613299999999999) * state(3, 0);
|
||||
const Scalar _tmp8 = P(3, 1) * _tmp1;
|
||||
const Scalar _tmp9 = P(2, 1) * _tmp5;
|
||||
const Scalar _tmp10 = -P(1, 1) * _tmp7;
|
||||
const Scalar _tmp11 = P(0, 2) * _tmp3;
|
||||
const Scalar _tmp12 = P(2, 2) * _tmp5;
|
||||
const Scalar _tmp13 = -P(1, 2) * _tmp7;
|
||||
const Scalar _tmp14 = -P(3, 3) * _tmp1;
|
||||
const Scalar _tmp15 = P(0, 3) * _tmp3;
|
||||
const Scalar _tmp16 = -P(1, 3) * _tmp7;
|
||||
const Scalar _tmp17 = R - _tmp1 * (P(2, 3) * _tmp5 + _tmp14 + _tmp15 + _tmp16) +
|
||||
_tmp3 * (-P(1, 0) * _tmp7 + _tmp2 + _tmp4 + _tmp6) +
|
||||
_tmp5 * (-P(3, 2) * _tmp1 + _tmp11 + _tmp12 + _tmp13) -
|
||||
_tmp7 * (P(0, 1) * _tmp3 + _tmp10 - _tmp8 + _tmp9);
|
||||
const Scalar _tmp18 = P(3, 0) * _tmp3;
|
||||
const Scalar _tmp19 = -P(0, 0) * _tmp1;
|
||||
const Scalar _tmp20 = -P(1, 0) * _tmp5;
|
||||
const Scalar _tmp21 = P(3, 2) * _tmp3;
|
||||
const Scalar _tmp22 = -P(2, 2) * _tmp7;
|
||||
const Scalar _tmp23 = P(1, 2) * _tmp5;
|
||||
const Scalar _tmp24 = P(0, 1) * _tmp1;
|
||||
const Scalar _tmp25 = -P(2, 1) * _tmp7;
|
||||
const Scalar _tmp26 = -P(1, 1) * _tmp5;
|
||||
const Scalar _tmp27 = -P(3, 3) * _tmp3;
|
||||
const Scalar _tmp28 = -P(0, 3) * _tmp1;
|
||||
const Scalar _tmp29 = -P(2, 3) * _tmp7;
|
||||
const Scalar _tmp30 = R - _tmp1 * (-P(2, 0) * _tmp7 - _tmp18 + _tmp19 + _tmp20) -
|
||||
_tmp3 * (-P(1, 3) * _tmp5 + _tmp27 + _tmp28 + _tmp29) -
|
||||
_tmp5 * (-P(3, 1) * _tmp3 - _tmp24 + _tmp25 + _tmp26) -
|
||||
_tmp7 * (-P(0, 2) * _tmp1 - _tmp21 + _tmp22 - _tmp23);
|
||||
const Scalar _tmp31 = -P(0, 0) * _tmp5;
|
||||
const Scalar _tmp32 = P(2, 0) * _tmp3;
|
||||
const Scalar _tmp33 = P(1, 0) * _tmp1;
|
||||
const Scalar _tmp34 = -P(3, 2) * _tmp7;
|
||||
const Scalar _tmp35 = P(0, 2) * _tmp5;
|
||||
const Scalar _tmp36 = P(2, 2) * _tmp3;
|
||||
const Scalar _tmp37 = -P(3, 1) * _tmp7;
|
||||
const Scalar _tmp38 = -P(0, 1) * _tmp5;
|
||||
const Scalar _tmp39 = P(1, 1) * _tmp1;
|
||||
const Scalar _tmp40 = -P(3, 3) * _tmp7;
|
||||
const Scalar _tmp41 = P(2, 3) * _tmp3;
|
||||
const Scalar _tmp42 = P(1, 3) * _tmp1;
|
||||
const Scalar _tmp43 = R + _tmp1 * (P(2, 1) * _tmp3 + _tmp37 + _tmp38 + _tmp39) +
|
||||
_tmp3 * (P(1, 2) * _tmp1 + _tmp34 - _tmp35 + _tmp36) -
|
||||
_tmp5 * (-P(3, 0) * _tmp7 + _tmp31 + _tmp32 + _tmp33) -
|
||||
_tmp7 * (-P(0, 3) * _tmp5 + _tmp40 + _tmp41 + _tmp42);
|
||||
const Scalar _tmp44 = Scalar(1.0) / (_tmp17);
|
||||
const Scalar _tmp45 = Scalar(19.613299999999999) * P(8, 3);
|
||||
const Scalar _tmp46 = Scalar(19.613299999999999) * P(8, 0);
|
||||
const Scalar _tmp47 = Scalar(19.613299999999999) * P(8, 2);
|
||||
const Scalar _tmp48 = Scalar(19.613299999999999) * P(9, 3);
|
||||
const Scalar _tmp49 = Scalar(19.613299999999999) * P(9, 2);
|
||||
const Scalar _tmp50 = Scalar(19.613299999999999) * P(9, 0);
|
||||
const Scalar _tmp51 = Scalar(1.0) / (_tmp30);
|
||||
const Scalar _tmp52 = Scalar(19.613299999999999) * P(4, 0);
|
||||
const Scalar _tmp53 = Scalar(1.0) / (_tmp43);
|
||||
|
||||
// Output terms (5)
|
||||
if (innov != nullptr) {
|
||||
matrix::Matrix<Scalar, 3, 1>& _innov = (*innov);
|
||||
|
||||
_innov(0, 0) = -_tmp0 * meas(0, 0) + Scalar(19.613299999999999) * state(0, 0) * state(2, 0) -
|
||||
Scalar(19.613299999999999) * state(1, 0) * state(3, 0);
|
||||
_innov(1, 0) = -_tmp0 * meas(1, 0) - Scalar(19.613299999999999) * state(0, 0) * state(1, 0) -
|
||||
Scalar(19.613299999999999) * state(2, 0) * state(3, 0);
|
||||
_innov(2, 0) = -_tmp0 * meas(2, 0) -
|
||||
Scalar(9.8066499999999994) * std::pow(state(0, 0), Scalar(2)) +
|
||||
Scalar(9.8066499999999994) * std::pow(state(1, 0), Scalar(2)) +
|
||||
Scalar(9.8066499999999994) * std::pow(state(2, 0), Scalar(2)) -
|
||||
Scalar(9.8066499999999994) * std::pow(state(3, 0), Scalar(2));
|
||||
}
|
||||
|
||||
if (innov_var != nullptr) {
|
||||
matrix::Matrix<Scalar, 3, 1>& _innov_var = (*innov_var);
|
||||
|
||||
_innov_var(0, 0) = _tmp17;
|
||||
_innov_var(1, 0) = _tmp30;
|
||||
_innov_var(2, 0) = _tmp43;
|
||||
}
|
||||
|
||||
if (Kx != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _kx = (*Kx);
|
||||
|
||||
_kx(0, 0) = _tmp44 * (-P(0, 1) * _tmp7 + _tmp28 + _tmp35 + _tmp4);
|
||||
_kx(1, 0) = _tmp44 * (P(1, 0) * _tmp3 + _tmp10 + _tmp23 - _tmp42);
|
||||
_kx(2, 0) = _tmp44 * (-P(2, 3) * _tmp1 + _tmp12 + _tmp25 + _tmp32);
|
||||
_kx(3, 0) = _tmp44 * (P(3, 2) * _tmp5 + _tmp14 + _tmp18 + _tmp37);
|
||||
_kx(4, 0) = _tmp44 * (P(4, 0) * _tmp3 - P(4, 1) * _tmp7 + P(4, 2) * _tmp5 - P(4, 3) * _tmp1);
|
||||
_kx(5, 0) = _tmp44 * (P(5, 0) * _tmp3 - P(5, 1) * _tmp7 + P(5, 2) * _tmp5 - P(5, 3) * _tmp1);
|
||||
_kx(6, 0) = _tmp44 * (P(6, 0) * _tmp3 - P(6, 1) * _tmp7 + P(6, 2) * _tmp5 - P(6, 3) * _tmp1);
|
||||
_kx(7, 0) = _tmp44 * (P(7, 0) * _tmp3 - P(7, 1) * _tmp7 + P(7, 2) * _tmp5 - P(7, 3) * _tmp1);
|
||||
_kx(8, 0) = _tmp44 * (-P(8, 1) * _tmp7 - _tmp45 * state(1, 0) + _tmp46 * state(2, 0) +
|
||||
_tmp47 * state(0, 0));
|
||||
_kx(9, 0) = _tmp44 * (-P(9, 1) * _tmp7 - _tmp48 * state(1, 0) + _tmp49 * state(0, 0) +
|
||||
_tmp50 * state(2, 0));
|
||||
_kx(10, 0) =
|
||||
_tmp44 * (P(10, 0) * _tmp3 - P(10, 1) * _tmp7 + P(10, 2) * _tmp5 - P(10, 3) * _tmp1);
|
||||
_kx(11, 0) =
|
||||
_tmp44 * (P(11, 0) * _tmp3 - P(11, 1) * _tmp7 + P(11, 2) * _tmp5 - P(11, 3) * _tmp1);
|
||||
_kx(12, 0) =
|
||||
_tmp44 * (P(12, 0) * _tmp3 - P(12, 1) * _tmp7 + P(12, 2) * _tmp5 - P(12, 3) * _tmp1);
|
||||
_kx(13, 0) =
|
||||
_tmp44 * (P(13, 0) * _tmp3 - P(13, 1) * _tmp7 + P(13, 2) * _tmp5 - P(13, 3) * _tmp1);
|
||||
_kx(14, 0) =
|
||||
_tmp44 * (P(14, 0) * _tmp3 - P(14, 1) * _tmp7 + P(14, 2) * _tmp5 - P(14, 3) * _tmp1);
|
||||
_kx(15, 0) =
|
||||
_tmp44 * (P(15, 0) * _tmp3 - P(15, 1) * _tmp7 + P(15, 2) * _tmp5 - P(15, 3) * _tmp1);
|
||||
_kx(16, 0) =
|
||||
_tmp44 * (P(16, 0) * _tmp3 - P(16, 1) * _tmp7 + P(16, 2) * _tmp5 - P(16, 3) * _tmp1);
|
||||
_kx(17, 0) =
|
||||
_tmp44 * (P(17, 0) * _tmp3 - P(17, 1) * _tmp7 + P(17, 2) * _tmp5 - P(17, 3) * _tmp1);
|
||||
_kx(18, 0) =
|
||||
_tmp44 * (P(18, 0) * _tmp3 - P(18, 1) * _tmp7 + P(18, 2) * _tmp5 - P(18, 3) * _tmp1);
|
||||
_kx(19, 0) =
|
||||
_tmp44 * (P(19, 0) * _tmp3 - P(19, 1) * _tmp7 + P(19, 2) * _tmp5 - P(19, 3) * _tmp1);
|
||||
_kx(20, 0) =
|
||||
_tmp44 * (P(20, 0) * _tmp3 - P(20, 1) * _tmp7 + P(20, 2) * _tmp5 - P(20, 3) * _tmp1);
|
||||
_kx(21, 0) =
|
||||
_tmp44 * (P(21, 0) * _tmp3 - P(21, 1) * _tmp7 + P(21, 2) * _tmp5 - P(21, 3) * _tmp1);
|
||||
_kx(22, 0) =
|
||||
_tmp44 * (P(22, 0) * _tmp3 - P(22, 1) * _tmp7 + P(22, 2) * _tmp5 - P(22, 3) * _tmp1);
|
||||
_kx(23, 0) =
|
||||
_tmp44 * (P(23, 0) * _tmp3 - P(23, 1) * _tmp7 + P(23, 2) * _tmp5 - P(23, 3) * _tmp1);
|
||||
}
|
||||
|
||||
if (Ky != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _ky = (*Ky);
|
||||
|
||||
_ky(0, 0) = _tmp51 * (-P(0, 2) * _tmp7 - _tmp15 + _tmp19 + _tmp38);
|
||||
_ky(1, 0) = _tmp51 * (-P(1, 3) * _tmp3 + _tmp13 + _tmp26 - _tmp33);
|
||||
_ky(2, 0) = _tmp51 * (-P(2, 0) * _tmp1 + _tmp22 - _tmp41 - _tmp9);
|
||||
_ky(3, 0) = _tmp51 * (-P(3, 1) * _tmp5 + _tmp2 + _tmp27 + _tmp34);
|
||||
_ky(4, 0) =
|
||||
_tmp51 * (-P(4, 1) * _tmp5 - P(4, 2) * _tmp7 - P(4, 3) * _tmp3 - _tmp52 * state(1, 0));
|
||||
_ky(5, 0) = _tmp51 * (-P(5, 0) * _tmp1 - P(5, 1) * _tmp5 - P(5, 2) * _tmp7 - P(5, 3) * _tmp3);
|
||||
_ky(6, 0) = _tmp51 * (-P(6, 0) * _tmp1 - P(6, 1) * _tmp5 - P(6, 2) * _tmp7 - P(6, 3) * _tmp3);
|
||||
_ky(7, 0) = _tmp51 * (-P(7, 0) * _tmp1 - P(7, 1) * _tmp5 - P(7, 2) * _tmp7 - P(7, 3) * _tmp3);
|
||||
_ky(8, 0) =
|
||||
_tmp51 * (-P(8, 1) * _tmp5 - P(8, 2) * _tmp7 - _tmp45 * state(2, 0) - _tmp46 * state(1, 0));
|
||||
_ky(9, 0) =
|
||||
_tmp51 * (-P(9, 1) * _tmp5 - P(9, 2) * _tmp7 - _tmp48 * state(2, 0) - _tmp50 * state(1, 0));
|
||||
_ky(10, 0) =
|
||||
_tmp51 * (-P(10, 0) * _tmp1 - P(10, 1) * _tmp5 - P(10, 2) * _tmp7 - P(10, 3) * _tmp3);
|
||||
_ky(11, 0) =
|
||||
_tmp51 * (-P(11, 0) * _tmp1 - P(11, 1) * _tmp5 - P(11, 2) * _tmp7 - P(11, 3) * _tmp3);
|
||||
_ky(12, 0) =
|
||||
_tmp51 * (-P(12, 0) * _tmp1 - P(12, 1) * _tmp5 - P(12, 2) * _tmp7 - P(12, 3) * _tmp3);
|
||||
_ky(13, 0) =
|
||||
_tmp51 * (-P(13, 0) * _tmp1 - P(13, 1) * _tmp5 - P(13, 2) * _tmp7 - P(13, 3) * _tmp3);
|
||||
_ky(14, 0) =
|
||||
_tmp51 * (-P(14, 0) * _tmp1 - P(14, 1) * _tmp5 - P(14, 2) * _tmp7 - P(14, 3) * _tmp3);
|
||||
_ky(15, 0) =
|
||||
_tmp51 * (-P(15, 0) * _tmp1 - P(15, 1) * _tmp5 - P(15, 2) * _tmp7 - P(15, 3) * _tmp3);
|
||||
_ky(16, 0) =
|
||||
_tmp51 * (-P(16, 0) * _tmp1 - P(16, 1) * _tmp5 - P(16, 2) * _tmp7 - P(16, 3) * _tmp3);
|
||||
_ky(17, 0) =
|
||||
_tmp51 * (-P(17, 0) * _tmp1 - P(17, 1) * _tmp5 - P(17, 2) * _tmp7 - P(17, 3) * _tmp3);
|
||||
_ky(18, 0) =
|
||||
_tmp51 * (-P(18, 0) * _tmp1 - P(18, 1) * _tmp5 - P(18, 2) * _tmp7 - P(18, 3) * _tmp3);
|
||||
_ky(19, 0) =
|
||||
_tmp51 * (-P(19, 0) * _tmp1 - P(19, 1) * _tmp5 - P(19, 2) * _tmp7 - P(19, 3) * _tmp3);
|
||||
_ky(20, 0) =
|
||||
_tmp51 * (-P(20, 0) * _tmp1 - P(20, 1) * _tmp5 - P(20, 2) * _tmp7 - P(20, 3) * _tmp3);
|
||||
_ky(21, 0) =
|
||||
_tmp51 * (-P(21, 0) * _tmp1 - P(21, 1) * _tmp5 - P(21, 2) * _tmp7 - P(21, 3) * _tmp3);
|
||||
_ky(22, 0) =
|
||||
_tmp51 * (-P(22, 0) * _tmp1 - P(22, 1) * _tmp5 - P(22, 2) * _tmp7 - P(22, 3) * _tmp3);
|
||||
_ky(23, 0) =
|
||||
_tmp51 * (-P(23, 0) * _tmp1 - P(23, 1) * _tmp5 - P(23, 2) * _tmp7 - P(23, 3) * _tmp3);
|
||||
}
|
||||
|
||||
if (Kz != nullptr) {
|
||||
matrix::Matrix<Scalar, 24, 1>& _kz = (*Kz);
|
||||
|
||||
_kz(0, 0) = _tmp53 * (-P(0, 3) * _tmp7 + _tmp11 + _tmp24 + _tmp31);
|
||||
_kz(1, 0) = _tmp53 * (P(1, 2) * _tmp3 + _tmp16 + _tmp20 + _tmp39);
|
||||
_kz(2, 0) = _tmp53 * (P(2, 1) * _tmp1 + _tmp29 + _tmp36 - _tmp6);
|
||||
_kz(3, 0) = _tmp53 * (-P(3, 0) * _tmp5 + _tmp21 + _tmp40 + _tmp8);
|
||||
_kz(4, 0) =
|
||||
_tmp53 * (P(4, 1) * _tmp1 + P(4, 2) * _tmp3 - P(4, 3) * _tmp7 - _tmp52 * state(0, 0));
|
||||
_kz(5, 0) = _tmp53 * (-P(5, 0) * _tmp5 + P(5, 1) * _tmp1 + P(5, 2) * _tmp3 - P(5, 3) * _tmp7);
|
||||
_kz(6, 0) = _tmp53 * (-P(6, 0) * _tmp5 + P(6, 1) * _tmp1 + P(6, 2) * _tmp3 - P(6, 3) * _tmp7);
|
||||
_kz(7, 0) = _tmp53 * (-P(7, 0) * _tmp5 + P(7, 1) * _tmp1 + P(7, 2) * _tmp3 - P(7, 3) * _tmp7);
|
||||
_kz(8, 0) =
|
||||
_tmp53 * (P(8, 1) * _tmp1 - P(8, 3) * _tmp7 - _tmp46 * state(0, 0) + _tmp47 * state(2, 0));
|
||||
_kz(9, 0) =
|
||||
_tmp53 * (P(9, 1) * _tmp1 - P(9, 3) * _tmp7 + _tmp49 * state(2, 0) - _tmp50 * state(0, 0));
|
||||
_kz(10, 0) =
|
||||
_tmp53 * (-P(10, 0) * _tmp5 + P(10, 1) * _tmp1 + P(10, 2) * _tmp3 - P(10, 3) * _tmp7);
|
||||
_kz(11, 0) =
|
||||
_tmp53 * (-P(11, 0) * _tmp5 + P(11, 1) * _tmp1 + P(11, 2) * _tmp3 - P(11, 3) * _tmp7);
|
||||
_kz(12, 0) =
|
||||
_tmp53 * (-P(12, 0) * _tmp5 + P(12, 1) * _tmp1 + P(12, 2) * _tmp3 - P(12, 3) * _tmp7);
|
||||
_kz(13, 0) =
|
||||
_tmp53 * (-P(13, 0) * _tmp5 + P(13, 1) * _tmp1 + P(13, 2) * _tmp3 - P(13, 3) * _tmp7);
|
||||
_kz(14, 0) =
|
||||
_tmp53 * (-P(14, 0) * _tmp5 + P(14, 1) * _tmp1 + P(14, 2) * _tmp3 - P(14, 3) * _tmp7);
|
||||
_kz(15, 0) =
|
||||
_tmp53 * (-P(15, 0) * _tmp5 + P(15, 1) * _tmp1 + P(15, 2) * _tmp3 - P(15, 3) * _tmp7);
|
||||
_kz(16, 0) =
|
||||
_tmp53 * (-P(16, 0) * _tmp5 + P(16, 1) * _tmp1 + P(16, 2) * _tmp3 - P(16, 3) * _tmp7);
|
||||
_kz(17, 0) =
|
||||
_tmp53 * (-P(17, 0) * _tmp5 + P(17, 1) * _tmp1 + P(17, 2) * _tmp3 - P(17, 3) * _tmp7);
|
||||
_kz(18, 0) =
|
||||
_tmp53 * (-P(18, 0) * _tmp5 + P(18, 1) * _tmp1 + P(18, 2) * _tmp3 - P(18, 3) * _tmp7);
|
||||
_kz(19, 0) =
|
||||
_tmp53 * (-P(19, 0) * _tmp5 + P(19, 1) * _tmp1 + P(19, 2) * _tmp3 - P(19, 3) * _tmp7);
|
||||
_kz(20, 0) =
|
||||
_tmp53 * (-P(20, 0) * _tmp5 + P(20, 1) * _tmp1 + P(20, 2) * _tmp3 - P(20, 3) * _tmp7);
|
||||
_kz(21, 0) =
|
||||
_tmp53 * (-P(21, 0) * _tmp5 + P(21, 1) * _tmp1 + P(21, 2) * _tmp3 - P(21, 3) * _tmp7);
|
||||
_kz(22, 0) =
|
||||
_tmp53 * (-P(22, 0) * _tmp5 + P(22, 1) * _tmp1 + P(22, 2) * _tmp3 - P(22, 3) * _tmp7);
|
||||
_kz(23, 0) =
|
||||
_tmp53 * (-P(23, 0) * _tmp5 + P(23, 1) * _tmp1 + P(23, 2) * _tmp3 - P(23, 3) * _tmp7);
|
||||
}
|
||||
} // NOLINT(readability/fn_size)
|
||||
|
||||
// NOLINTNEXTLINE(readability/fn_size)
|
||||
} // namespace sym
|
||||
@@ -131,6 +131,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
|
||||
_param_ekf2_eva_noise(_params->ev_att_noise),
|
||||
_param_ekf2_evv_gate(_params->ev_vel_innov_gate),
|
||||
_param_ekf2_evp_gate(_params->ev_pos_innov_gate),
|
||||
_param_ekf2_grav_noise(_params->gravity_noise),
|
||||
_param_ekf2_of_n_min(_params->flow_noise),
|
||||
_param_ekf2_of_n_max(_params->flow_noise_qual_min),
|
||||
_param_ekf2_of_qmin(_params->flow_qual_min),
|
||||
@@ -804,6 +805,9 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime ×tamp)
|
||||
// mag 3d
|
||||
PublishAidSourceStatus(_ekf.aid_src_mag(), _status_mag_pub_last, _estimator_aid_src_mag_pub);
|
||||
|
||||
// gravity
|
||||
PublishAidSourceStatus(_ekf.aid_src_gravity(), _status_gravity_pub_last, _estimator_aid_src_gravity_pub);
|
||||
|
||||
// aux velocity
|
||||
PublishAidSourceStatus(_ekf.aid_src_aux_vel(), _status_aux_vel_pub_last, _estimator_aid_src_aux_vel_pub);
|
||||
|
||||
@@ -1098,6 +1102,7 @@ void EKF2::PublishInnovations(const hrt_abstime ×tamp)
|
||||
_ekf.getHaglInnov(innovations.hagl);
|
||||
_ekf.getHaglRateInnov(innovations.hagl_rate);
|
||||
_ekf.getTerrainFlowInnov(innovations.terr_flow);
|
||||
_ekf.getGravityInnov(innovations.gravity);
|
||||
// Not yet supported
|
||||
innovations.aux_vvel = NAN;
|
||||
|
||||
@@ -1148,6 +1153,7 @@ void EKF2::PublishInnovationTestRatios(const hrt_abstime ×tamp)
|
||||
_ekf.getHaglInnovRatio(test_ratios.hagl);
|
||||
_ekf.getHaglRateInnovRatio(test_ratios.hagl_rate);
|
||||
_ekf.getTerrainFlowInnovRatio(test_ratios.terr_flow[0]);
|
||||
_ekf.getGravityInnovRatio(test_ratios.gravity[0]);
|
||||
// Not yet supported
|
||||
test_ratios.aux_vvel = NAN;
|
||||
|
||||
@@ -1174,6 +1180,7 @@ void EKF2::PublishInnovationVariances(const hrt_abstime ×tamp)
|
||||
_ekf.getHaglInnovVar(variances.hagl);
|
||||
_ekf.getHaglRateInnovVar(variances.hagl_rate);
|
||||
_ekf.getTerrainFlowInnovVar(variances.terr_flow);
|
||||
_ekf.getGravityInnovVar(variances.gravity);
|
||||
// Not yet supported
|
||||
variances.aux_vvel = NAN;
|
||||
|
||||
@@ -1507,6 +1514,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime ×tamp)
|
||||
status_flags.cs_rng_kin_consistent = _ekf.control_status_flags().rng_kin_consistent;
|
||||
status_flags.cs_fake_pos = _ekf.control_status_flags().fake_pos;
|
||||
status_flags.cs_fake_hgt = _ekf.control_status_flags().fake_hgt;
|
||||
status_flags.cs_gravity_vector = _ekf.control_status_flags().gravity_vector;
|
||||
|
||||
status_flags.fault_status_changes = _filter_fault_status_changes;
|
||||
status_flags.fs_bad_mag_x = _ekf.fault_status_flags().bad_mag_x;
|
||||
|
||||
@@ -285,6 +285,8 @@ private:
|
||||
hrt_abstime _status_mag_pub_last{0};
|
||||
hrt_abstime _status_mag_heading_pub_last{0};
|
||||
|
||||
hrt_abstime _status_gravity_pub_last{0};
|
||||
|
||||
hrt_abstime _status_aux_vel_pub_last{0};
|
||||
|
||||
hrt_abstime _status_optical_flow_pub_last{0};
|
||||
@@ -376,6 +378,8 @@ private:
|
||||
uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_mag_heading_pub{ORB_ID(estimator_aid_src_mag_heading)};
|
||||
uORB::PublicationMulti<estimator_aid_source3d_s> _estimator_aid_src_mag_pub{ORB_ID(estimator_aid_src_mag)};
|
||||
|
||||
uORB::PublicationMulti<estimator_aid_source3d_s> _estimator_aid_src_gravity_pub{ORB_ID(estimator_aid_src_gravity)};
|
||||
|
||||
uORB::PublicationMulti<estimator_aid_source2d_s> _estimator_aid_src_aux_vel_pub{ORB_ID(estimator_aid_src_aux_vel)};
|
||||
|
||||
uORB::PublicationMulti<estimator_aid_source2d_s> _estimator_aid_src_optical_flow_pub{ORB_ID(estimator_aid_src_optical_flow)};
|
||||
@@ -540,6 +544,9 @@ private:
|
||||
(ParamExtFloat<px4::params::EKF2_EVP_GATE>)
|
||||
_param_ekf2_evp_gate, ///< external vision position innovation consistency gate size (STD)
|
||||
|
||||
(ParamExtFloat<px4::params::EKF2_GRAV_NOISE>)
|
||||
_param_ekf2_grav_noise, ///< default accelerometer noise for gravity fusion measurements (m/s**2)
|
||||
|
||||
// optical flow fusion
|
||||
(ParamExtFloat<px4::params::EKF2_OF_N_MIN>)
|
||||
_param_ekf2_of_n_min, ///< best quality observation noise for optical flow LOS rate measurements (rad/sec)
|
||||
|
||||
@@ -57,11 +57,12 @@ PARAM_DEFINE_INT32(EKF2_PREDICT_US, 10000);
|
||||
*
|
||||
* @group EKF2
|
||||
* @min 0
|
||||
* @max 3
|
||||
* @max 7
|
||||
* @bit 0 Gyro Bias
|
||||
* @bit 1 Accel Bias
|
||||
* @bit 2 Gravity vector fusion
|
||||
*/
|
||||
PARAM_DEFINE_INT32(EKF2_IMU_CTRL, 3);
|
||||
PARAM_DEFINE_INT32(EKF2_IMU_CTRL, 7);
|
||||
|
||||
/**
|
||||
* Magnetometer measurement delay relative to IMU measurements
|
||||
@@ -854,6 +855,17 @@ PARAM_DEFINE_FLOAT(EKF2_EVV_NOISE, 0.1f);
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(EKF2_EVA_NOISE, 0.1f);
|
||||
|
||||
/**
|
||||
* Accelerometer measurement noise for gravity based observations.
|
||||
*
|
||||
* @group EKF2
|
||||
* @min 0.1
|
||||
* @max 10.0
|
||||
* @unit m/s^2
|
||||
* @decimal 2
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(EKF2_GRAV_NOISE, 1.0f);
|
||||
|
||||
/**
|
||||
* Measurement noise for the optical flow sensor when it's reported quality metric is at the maximum
|
||||
*
|
||||
|
||||
@@ -187,6 +187,7 @@ void LoggedTopics::add_default_topics()
|
||||
// add_optional_topic_multi("estimator_aid_src_ev_pos", 100, MAX_ESTIMATOR_INSTANCES);
|
||||
// add_optional_topic_multi("estimator_aid_src_ev_vel", 100, MAX_ESTIMATOR_INSTANCES);
|
||||
// add_optional_topic_multi("estimator_aid_src_ev_yaw", 100, MAX_ESTIMATOR_INSTANCES);
|
||||
// add_optional_topic_multi("estimator_aid_src_gravity", 100, MAX_ESTIMATOR_INSTANCES);
|
||||
// add_optional_topic_multi("estimator_aid_src_rng_hgt", 100, MAX_ESTIMATOR_INSTANCES);
|
||||
// add_optional_topic_multi("estimator_aid_src_fake_hgt", 100, MAX_ESTIMATOR_INSTANCES);
|
||||
// add_optional_topic_multi("estimator_aid_src_fake_pos", 100, MAX_ESTIMATOR_INSTANCES);
|
||||
|
||||
Reference in New Issue
Block a user