diff --git a/src/lib/parameters/px4params/srcparser.py b/src/lib/parameters/px4params/srcparser.py index 0e2c0b0ef9..efac384818 100644 --- a/src/lib/parameters/px4params/srcparser.py +++ b/src/lib/parameters/px4params/srcparser.py @@ -358,7 +358,7 @@ class SourceParser(object): 'deg', 'deg*1e7', 'deg/s', 'deg/s^2', 'celcius', 'gauss', 'gauss/s', 'gauss^2', 'hPa', 'kg', 'kg/m^2', 'kg m^2', 'kg/m^3', - 'mm', 'm', 'm/s', 'm^2', 'm/s^2', 'm/s^3', 'm/s^2/sqrt(Hz)', '1/s/sqrt(Hz)', 'm/s/rad', + 'mm', 'm', 'm/s', 'm^2', 'm/s^2', 'm/s^3', 'm/s^2/sqrt(Hz)', '1/s/sqrt(Hz)', 'm/s/rad', 'g', 'Ohm', 'V', 'A', 'us', 'ms', 's', 'S', 'A/%', '(m/s^2)^2', 'm/m', 'tan(rad)^2', '(m/s)^2', 'm/rad', diff --git a/src/modules/ekf2/EKF/gravity_fusion.cpp b/src/modules/ekf2/EKF/gravity_fusion.cpp index ccccbd451f..7e2cf7f471 100644 --- a/src/modules/ekf2/EKF/gravity_fusion.cpp +++ b/src/modules/ekf2/EKF/gravity_fusion.cpp @@ -40,14 +40,16 @@ */ #include "ekf.h" -#include +#include +#include +#include #include void Ekf::controlGravityFusion(const imuSample &imu) { // get raw accelerometer reading at delayed horizon and expected measurement noise (gaussian) - const Vector3f measurement = imu.delta_vel / imu.delta_vel_dt - _state.accel_bias; + const Vector3f measurement = Vector3f(imu.delta_vel / imu.delta_vel_dt - _state.accel_bias).unit(); const float measurement_var = math::max(sq(_params.gravity_noise), sq(0.01f)); const float upper_accel_limit = CONSTANTS_ONE_G * 1.1f; @@ -60,12 +62,11 @@ void Ekf::controlGravityFusion(const imuSample &imu) && !isHorizontalAidingActive(); // calculate kalman gains and innovation variances - Vector3f innovation; // innovation of the last gravity fusion observation (m/s**2) + Vector3f innovation = _state.quat_nominal.rotateVectorInverse(Vector3f(0.f, 0.f, -1.f)) - measurement; Vector3f innovation_variance; - VectorState Kx, Ky, Kz; // Kalman gain vectors - sym::ComputeGravityInnovVarAndKAndH( - _state.vector(), P, measurement, measurement_var, FLT_EPSILON, - &innovation, &innovation_variance, &Kx, &Ky, &Kz); + const auto state_vector = _state.vector(); + VectorState H; + sym::ComputeGravityXyzInnovVarAndHx(state_vector, P, measurement_var, &innovation_variance, &H); // fill estimator aid source status resetEstimatorAidStatus(_aid_src_gravity); @@ -82,16 +83,40 @@ void Ekf::controlGravityFusion(const imuSample &imu) float innovation_gate = 0.25f; setEstimatorAidStatusTestRatio(_aid_src_gravity, innovation_gate); - const bool accel_clipping = imu.delta_vel_clipping[0] || imu.delta_vel_clipping[1] || imu.delta_vel_clipping[2]; + bool fused[3] {false, false, false}; - if (_control_status.flags.gravity_vector && !_aid_src_gravity.innovation_rejected && !accel_clipping) { - // 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)); + // update the states and covariance using sequential fusion + for (uint8_t index = 0; index <= 2; index++) { + // Calculate Kalman gains and observation jacobians + if (index == 0) { + // everything was already computed above - if (_aid_src_gravity.fused) { - _aid_src_gravity.time_last_fuse = imu.time_us; + } 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::ComputeGravityYInnovVarAndH(state_vector, P, measurement_var, &_aid_src_gravity.innovation_variance[index], &H); + + // recalculate innovation using the updated state + _aid_src_gravity.innovation[index] = _state.quat_nominal.rotateVectorInverse(Vector3f(0.f, 0.f, -1.f))(index) - measurement(index); + + } else if (index == 2) { + // recalculate innovation variance because state covariances have changed due to previous fusion (linearise using the same initial state for all axes) + sym::ComputeGravityZInnovVarAndH(state_vector, P, measurement_var, &_aid_src_gravity.innovation_variance[index], &H); + + // recalculate innovation using the updated state + _aid_src_gravity.innovation[index] = _state.quat_nominal.rotateVectorInverse(Vector3f(0.f, 0.f, -1.f))(index) - measurement(index); + } + + VectorState K = P * H / _aid_src_gravity.innovation_variance[index]; + + const bool accel_clipping = imu.delta_vel_clipping[0] || imu.delta_vel_clipping[1] || imu.delta_vel_clipping[2]; + + if (_control_status.flags.gravity_vector && !_aid_src_gravity.innovation_rejected && !accel_clipping) { + fused[index] = measurementUpdate(K, _aid_src_gravity.innovation_variance[index], _aid_src_gravity.innovation[index]); } } + + if (fused[0] && fused[1] && fused[2]) { + _aid_src_gravity.fused = true; + _aid_src_gravity.time_last_fuse = imu.time_us; + } } diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py index 641e7f290d..d651eda279 100755 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py @@ -607,35 +607,64 @@ def compute_drag_y_innov_var_and_k( return (innov_var, K) -def compute_gravity_innov_var_and_k_and_h( - state: VState, - P: MTangent, - meas: sf.V3, - R: sf.Scalar, - epsilon: sf.Scalar -) -> (sf.V3, sf.V3, VTangent, VTangent, VTangent): - - state = vstate_to_state(state) +def predict_gravity_direction(state: State): # get transform from earth to body frame R_to_body = state["quat_nominal"].inverse() # 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) + return R_to_body * sf.Matrix([0,0,-1]) + +def compute_gravity_xyz_innov_var_and_hx( + state: VState, + P: MTangent, + R: sf.Scalar +) -> (sf.V3, VTangent): + + state = vstate_to_state(state) + meas_pred = predict_gravity_direction(state) # initialize outputs innov_var = sf.V3() - K = [None] * 3 + H = [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] + H[i] = sf.V1(meas_pred[i]).jacobian(state) + innov_var[i] = (H[i] * P * H[i].T + R)[0,0] - return (innov, innov_var, K[0], K[1], K[2]) + return (innov_var, H[0].T) + +def compute_gravity_y_innov_var_and_h( + state: VState, + P: MTangent, + R: sf.Scalar +) -> (sf.V3, VTangent, VTangent, VTangent): + + state = vstate_to_state(state) + meas_pred = predict_gravity_direction(state) + + # calculate observation jacobian (H), kalman gain (K), and innovation variance (S) + H = sf.V1(meas_pred[1]).jacobian(state) + innov_var = (H * P * H.T + R)[0,0] + + return (innov_var, H.T) + +def compute_gravity_z_innov_var_and_h( + state: VState, + P: MTangent, + R: sf.Scalar +) -> (sf.V3, VTangent, VTangent, VTangent): + + state = vstate_to_state(state) + meas_pred = predict_gravity_direction(state) + + # calculate observation jacobian (H), kalman gain (K), and innovation variance (S) + H = sf.V1(meas_pred[2]).jacobian(state) + innov_var = (H * P * H.T + R)[0,0] + + return (innov_var, H.T) print("Derive EKF2 equations...") generate_px4_function(predict_covariance, output_names=None) @@ -662,6 +691,8 @@ generate_px4_function(compute_yaw_321_innov_var_and_h_alternate, output_names=[" generate_px4_function(compute_flow_xy_innov_var_and_hx, output_names=["innov_var", "H"]) generate_px4_function(compute_flow_y_innov_var_and_h, output_names=["innov_var", "H"]) generate_px4_function(compute_gnss_yaw_pred_innov_var_and_h, output_names=["meas_pred", "innov_var", "H"]) -generate_px4_function(compute_gravity_innov_var_and_k_and_h, output_names=["innov", "innov_var", "Kx", "Ky", "Kz"]) +generate_px4_function(compute_gravity_xyz_innov_var_and_hx, output_names=["innov_var", "Hx"]) +generate_px4_function(compute_gravity_y_innov_var_and_h, output_names=["innov_var", "Hy"]) +generate_px4_function(compute_gravity_z_innov_var_and_h, output_names=["innov_var", "Hz"]) generate_px4_state(State, tangent_idx) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gravity_innov_var_and_k_and_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gravity_innov_var_and_k_and_h.h deleted file mode 100644 index 2ac6d93aa3..0000000000 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gravity_innov_var_and_k_and_h.h +++ /dev/null @@ -1,191 +0,0 @@ -// ----------------------------------------------------------------------------- -// This file was autogenerated by symforce from template: -// function/FUNCTION.h.jinja -// Do NOT modify by hand. -// ----------------------------------------------------------------------------- - -#pragma once - -#include - -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: Matrix23_23 - * meas: Matrix31 - * R: Scalar - * epsilon: Scalar - * - * Outputs: - * innov: Matrix31 - * innov_var: Matrix31 - * Kx: Matrix23_1 - * Ky: Matrix23_1 - * Kz: Matrix23_1 - */ -template -void ComputeGravityInnovVarAndKAndH(const matrix::Matrix& state, - const matrix::Matrix& P, - const matrix::Matrix& meas, const Scalar R, - const Scalar epsilon, - matrix::Matrix* const innov = nullptr, - matrix::Matrix* const innov_var = nullptr, - matrix::Matrix* const Kx = nullptr, - matrix::Matrix* const Ky = nullptr, - matrix::Matrix* const Kz = nullptr) { - // Total ops: 361 - - // Input arrays - - // Intermediate terms (31) - const Scalar _tmp0 = 2 * state(2, 0); - const Scalar _tmp1 = _tmp0 * state(0, 0); - const Scalar _tmp2 = 2 * state(1, 0); - const Scalar _tmp3 = _tmp2 * state(3, 0); - const Scalar _tmp4 = -_tmp1 + _tmp3; - const Scalar _tmp5 = - 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 _tmp6 = _tmp0 * state(3, 0); - const Scalar _tmp7 = _tmp2 * state(0, 0); - const Scalar _tmp8 = _tmp6 + _tmp7; - const Scalar _tmp9 = std::pow(state(2, 0), Scalar(2)); - const Scalar _tmp10 = std::pow(state(1, 0), Scalar(2)); - const Scalar _tmp11 = std::pow(state(3, 0), Scalar(2)); - const Scalar _tmp12 = std::pow(state(0, 0), Scalar(2)); - const Scalar _tmp13 = -Scalar(9.8066499999999994) * _tmp10 + Scalar(9.8066499999999994) * _tmp11 + - Scalar(9.8066499999999994) * _tmp12 - Scalar(9.8066499999999994) * _tmp9; - const Scalar _tmp14 = -Scalar(9.8066499999999994) * _tmp8; - const Scalar _tmp15 = P(2, 2) * _tmp14; - const Scalar _tmp16 = P(1, 1) * _tmp13; - const Scalar _tmp17 = - R + _tmp13 * (P(2, 1) * _tmp14 + _tmp16) + _tmp14 * (P(1, 2) * _tmp13 + _tmp15); - const Scalar _tmp18 = Scalar(9.8066499999999994) * _tmp10 - Scalar(9.8066499999999994) * _tmp11 - - Scalar(9.8066499999999994) * _tmp12 + Scalar(9.8066499999999994) * _tmp9; - const Scalar _tmp19 = -Scalar(9.8066499999999994) * _tmp1 + Scalar(9.8066499999999994) * _tmp3; - const Scalar _tmp20 = P(2, 2) * _tmp19; - const Scalar _tmp21 = P(0, 0) * _tmp18; - const Scalar _tmp22 = - R + _tmp18 * (P(2, 0) * _tmp19 + _tmp21) + _tmp19 * (P(0, 2) * _tmp18 + _tmp20); - const Scalar _tmp23 = Scalar(9.8066499999999994) * _tmp6 + Scalar(9.8066499999999994) * _tmp7; - const Scalar _tmp24 = -Scalar(9.8066499999999994) * _tmp4; - const Scalar _tmp25 = P(1, 1) * _tmp24; - const Scalar _tmp26 = P(0, 0) * _tmp23; - const Scalar _tmp27 = - R + _tmp23 * (P(1, 0) * _tmp24 + _tmp26) + _tmp24 * (P(0, 1) * _tmp23 + _tmp25); - const Scalar _tmp28 = Scalar(1.0) / (_tmp17); - const Scalar _tmp29 = Scalar(1.0) / (_tmp22); - const Scalar _tmp30 = Scalar(1.0) / (_tmp27); - - // Output terms (5) - if (innov != nullptr) { - matrix::Matrix& _innov = (*innov); - - _innov(0, 0) = -Scalar(9.8066499999999994) * _tmp4 - _tmp5 * meas(0, 0); - _innov(1, 0) = -_tmp5 * meas(1, 0) - Scalar(9.8066499999999994) * _tmp8; - _innov(2, 0) = Scalar(19.613299999999999) * _tmp10 - _tmp5 * meas(2, 0) + - Scalar(19.613299999999999) * _tmp9 + Scalar(-9.8066499999999994); - } - - if (innov_var != nullptr) { - matrix::Matrix& _innov_var = (*innov_var); - - _innov_var(0, 0) = _tmp17; - _innov_var(1, 0) = _tmp22; - _innov_var(2, 0) = _tmp27; - } - - if (Kx != nullptr) { - matrix::Matrix& _kx = (*Kx); - - _kx(0, 0) = _tmp28 * (P(0, 1) * _tmp13 + P(0, 2) * _tmp14); - _kx(1, 0) = _tmp28 * (P(1, 2) * _tmp14 + _tmp16); - _kx(2, 0) = _tmp28 * (P(2, 1) * _tmp13 + _tmp15); - _kx(3, 0) = _tmp28 * (P(3, 1) * _tmp13 + P(3, 2) * _tmp14); - _kx(4, 0) = _tmp28 * (P(4, 1) * _tmp13 + P(4, 2) * _tmp14); - _kx(5, 0) = _tmp28 * (P(5, 1) * _tmp13 + P(5, 2) * _tmp14); - _kx(6, 0) = _tmp28 * (P(6, 1) * _tmp13 + P(6, 2) * _tmp14); - _kx(7, 0) = _tmp28 * (P(7, 1) * _tmp13 + P(7, 2) * _tmp14); - _kx(8, 0) = _tmp28 * (P(8, 1) * _tmp13 + P(8, 2) * _tmp14); - _kx(9, 0) = _tmp28 * (P(9, 1) * _tmp13 + P(9, 2) * _tmp14); - _kx(10, 0) = _tmp28 * (P(10, 1) * _tmp13 + P(10, 2) * _tmp14); - _kx(11, 0) = _tmp28 * (P(11, 1) * _tmp13 + P(11, 2) * _tmp14); - _kx(12, 0) = _tmp28 * (P(12, 1) * _tmp13 + P(12, 2) * _tmp14); - _kx(13, 0) = _tmp28 * (P(13, 1) * _tmp13 + P(13, 2) * _tmp14); - _kx(14, 0) = _tmp28 * (P(14, 1) * _tmp13 + P(14, 2) * _tmp14); - _kx(15, 0) = _tmp28 * (P(15, 1) * _tmp13 + P(15, 2) * _tmp14); - _kx(16, 0) = _tmp28 * (P(16, 1) * _tmp13 + P(16, 2) * _tmp14); - _kx(17, 0) = _tmp28 * (P(17, 1) * _tmp13 + P(17, 2) * _tmp14); - _kx(18, 0) = _tmp28 * (P(18, 1) * _tmp13 + P(18, 2) * _tmp14); - _kx(19, 0) = _tmp28 * (P(19, 1) * _tmp13 + P(19, 2) * _tmp14); - _kx(20, 0) = _tmp28 * (P(20, 1) * _tmp13 + P(20, 2) * _tmp14); - _kx(21, 0) = _tmp28 * (P(21, 1) * _tmp13 + P(21, 2) * _tmp14); - _kx(22, 0) = _tmp28 * (P(22, 1) * _tmp13 + P(22, 2) * _tmp14); - } - - if (Ky != nullptr) { - matrix::Matrix& _ky = (*Ky); - - _ky(0, 0) = _tmp29 * (P(0, 2) * _tmp19 + _tmp21); - _ky(1, 0) = _tmp29 * (P(1, 0) * _tmp18 + P(1, 2) * _tmp19); - _ky(2, 0) = _tmp29 * (P(2, 0) * _tmp18 + _tmp20); - _ky(3, 0) = _tmp29 * (P(3, 0) * _tmp18 + P(3, 2) * _tmp19); - _ky(4, 0) = _tmp29 * (P(4, 0) * _tmp18 + P(4, 2) * _tmp19); - _ky(5, 0) = _tmp29 * (P(5, 0) * _tmp18 + P(5, 2) * _tmp19); - _ky(6, 0) = _tmp29 * (P(6, 0) * _tmp18 + P(6, 2) * _tmp19); - _ky(7, 0) = _tmp29 * (P(7, 0) * _tmp18 + P(7, 2) * _tmp19); - _ky(8, 0) = _tmp29 * (P(8, 0) * _tmp18 + P(8, 2) * _tmp19); - _ky(9, 0) = _tmp29 * (P(9, 0) * _tmp18 + P(9, 2) * _tmp19); - _ky(10, 0) = _tmp29 * (P(10, 0) * _tmp18 + P(10, 2) * _tmp19); - _ky(11, 0) = _tmp29 * (P(11, 0) * _tmp18 + P(11, 2) * _tmp19); - _ky(12, 0) = _tmp29 * (P(12, 0) * _tmp18 + P(12, 2) * _tmp19); - _ky(13, 0) = _tmp29 * (P(13, 0) * _tmp18 + P(13, 2) * _tmp19); - _ky(14, 0) = _tmp29 * (P(14, 0) * _tmp18 + P(14, 2) * _tmp19); - _ky(15, 0) = _tmp29 * (P(15, 0) * _tmp18 + P(15, 2) * _tmp19); - _ky(16, 0) = _tmp29 * (P(16, 0) * _tmp18 + P(16, 2) * _tmp19); - _ky(17, 0) = _tmp29 * (P(17, 0) * _tmp18 + P(17, 2) * _tmp19); - _ky(18, 0) = _tmp29 * (P(18, 0) * _tmp18 + P(18, 2) * _tmp19); - _ky(19, 0) = _tmp29 * (P(19, 0) * _tmp18 + P(19, 2) * _tmp19); - _ky(20, 0) = _tmp29 * (P(20, 0) * _tmp18 + P(20, 2) * _tmp19); - _ky(21, 0) = _tmp29 * (P(21, 0) * _tmp18 + P(21, 2) * _tmp19); - _ky(22, 0) = _tmp29 * (P(22, 0) * _tmp18 + P(22, 2) * _tmp19); - } - - if (Kz != nullptr) { - matrix::Matrix& _kz = (*Kz); - - _kz(0, 0) = _tmp30 * (P(0, 1) * _tmp24 + _tmp26); - _kz(1, 0) = _tmp30 * (P(1, 0) * _tmp23 + _tmp25); - _kz(2, 0) = _tmp30 * (P(2, 0) * _tmp23 + P(2, 1) * _tmp24); - _kz(3, 0) = _tmp30 * (P(3, 0) * _tmp23 + P(3, 1) * _tmp24); - _kz(4, 0) = _tmp30 * (P(4, 0) * _tmp23 + P(4, 1) * _tmp24); - _kz(5, 0) = _tmp30 * (P(5, 0) * _tmp23 + P(5, 1) * _tmp24); - _kz(6, 0) = _tmp30 * (P(6, 0) * _tmp23 + P(6, 1) * _tmp24); - _kz(7, 0) = _tmp30 * (P(7, 0) * _tmp23 + P(7, 1) * _tmp24); - _kz(8, 0) = _tmp30 * (P(8, 0) * _tmp23 + P(8, 1) * _tmp24); - _kz(9, 0) = _tmp30 * (P(9, 0) * _tmp23 + P(9, 1) * _tmp24); - _kz(10, 0) = _tmp30 * (P(10, 0) * _tmp23 + P(10, 1) * _tmp24); - _kz(11, 0) = _tmp30 * (P(11, 0) * _tmp23 + P(11, 1) * _tmp24); - _kz(12, 0) = _tmp30 * (P(12, 0) * _tmp23 + P(12, 1) * _tmp24); - _kz(13, 0) = _tmp30 * (P(13, 0) * _tmp23 + P(13, 1) * _tmp24); - _kz(14, 0) = _tmp30 * (P(14, 0) * _tmp23 + P(14, 1) * _tmp24); - _kz(15, 0) = _tmp30 * (P(15, 0) * _tmp23 + P(15, 1) * _tmp24); - _kz(16, 0) = _tmp30 * (P(16, 0) * _tmp23 + P(16, 1) * _tmp24); - _kz(17, 0) = _tmp30 * (P(17, 0) * _tmp23 + P(17, 1) * _tmp24); - _kz(18, 0) = _tmp30 * (P(18, 0) * _tmp23 + P(18, 1) * _tmp24); - _kz(19, 0) = _tmp30 * (P(19, 0) * _tmp23 + P(19, 1) * _tmp24); - _kz(20, 0) = _tmp30 * (P(20, 0) * _tmp23 + P(20, 1) * _tmp24); - _kz(21, 0) = _tmp30 * (P(21, 0) * _tmp23 + P(21, 1) * _tmp24); - _kz(22, 0) = _tmp30 * (P(22, 0) * _tmp23 + P(22, 1) * _tmp24); - } -} // NOLINT(readability/fn_size) - -// NOLINTNEXTLINE(readability/fn_size) -} // namespace sym diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gravity_xyz_innov_var_and_hx.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gravity_xyz_innov_var_and_hx.h new file mode 100644 index 0000000000..f3f074e82d --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gravity_xyz_innov_var_and_hx.h @@ -0,0 +1,77 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +namespace sym { + +/** + * This function was autogenerated from a symbolic function. Do not modify by hand. + * + * Symbolic function: compute_gravity_xyz_innov_var_and_hx + * + * Args: + * state: Matrix24_1 + * P: Matrix23_23 + * R: Scalar + * + * Outputs: + * innov_var: Matrix31 + * Hx: Matrix23_1 + */ +template +void ComputeGravityXyzInnovVarAndHx(const matrix::Matrix& state, + const matrix::Matrix& P, const Scalar R, + matrix::Matrix* const innov_var = nullptr, + matrix::Matrix* const Hx = nullptr) { + // Total ops: 51 + + // Input arrays + + // Intermediate terms (16) + const Scalar _tmp0 = std::pow(state(3, 0), Scalar(2)); + const Scalar _tmp1 = std::pow(state(0, 0), Scalar(2)); + const Scalar _tmp2 = std::pow(state(2, 0), Scalar(2)); + const Scalar _tmp3 = std::pow(state(1, 0), Scalar(2)); + const Scalar _tmp4 = _tmp0 + _tmp1 - _tmp2 - _tmp3; + const Scalar _tmp5 = 2 * state(2, 0); + const Scalar _tmp6 = _tmp5 * state(3, 0); + const Scalar _tmp7 = 2 * state(1, 0); + const Scalar _tmp8 = _tmp7 * state(0, 0); + const Scalar _tmp9 = -_tmp6 - _tmp8; + const Scalar _tmp10 = -_tmp0 - _tmp1 + _tmp2 + _tmp3; + const Scalar _tmp11 = _tmp5 * state(0, 0); + const Scalar _tmp12 = _tmp7 * state(3, 0); + const Scalar _tmp13 = -_tmp11 + _tmp12; + const Scalar _tmp14 = _tmp6 + _tmp8; + const Scalar _tmp15 = _tmp11 - _tmp12; + + // Output terms (2) + if (innov_var != nullptr) { + matrix::Matrix& _innov_var = (*innov_var); + + _innov_var(0, 0) = R + _tmp4 * (P(1, 1) * _tmp4 + P(2, 1) * _tmp9) + + _tmp9 * (P(1, 2) * _tmp4 + P(2, 2) * _tmp9); + _innov_var(1, 0) = R + _tmp10 * (P(0, 0) * _tmp10 + P(2, 0) * _tmp13) + + _tmp13 * (P(0, 2) * _tmp10 + P(2, 2) * _tmp13); + _innov_var(2, 0) = R + _tmp14 * (P(0, 0) * _tmp14 + P(1, 0) * _tmp15) + + _tmp15 * (P(0, 1) * _tmp14 + P(1, 1) * _tmp15); + } + + if (Hx != nullptr) { + matrix::Matrix& _hx = (*Hx); + + _hx.setZero(); + + _hx(1, 0) = _tmp4; + _hx(2, 0) = _tmp9; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gravity_y_innov_var_and_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gravity_y_innov_var_and_h.h new file mode 100644 index 0000000000..2c3a11b682 --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gravity_y_innov_var_and_h.h @@ -0,0 +1,60 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +namespace sym { + +/** + * This function was autogenerated from a symbolic function. Do not modify by hand. + * + * Symbolic function: compute_gravity_y_innov_var_and_h + * + * Args: + * state: Matrix24_1 + * P: Matrix23_23 + * R: Scalar + * + * Outputs: + * innov_var: Scalar + * Hy: Matrix23_1 + */ +template +void ComputeGravityYInnovVarAndH(const matrix::Matrix& state, + const matrix::Matrix& P, const Scalar R, + Scalar* const innov_var = nullptr, + matrix::Matrix* const Hy = nullptr) { + // Total ops: 22 + + // Input arrays + + // Intermediate terms (2) + const Scalar _tmp0 = -std::pow(state(0, 0), Scalar(2)) + std::pow(state(1, 0), Scalar(2)) + + std::pow(state(2, 0), Scalar(2)) - std::pow(state(3, 0), Scalar(2)); + const Scalar _tmp1 = -2 * state(0, 0) * state(2, 0) + 2 * state(1, 0) * state(3, 0); + + // Output terms (2) + if (innov_var != nullptr) { + Scalar& _innov_var = (*innov_var); + + _innov_var = R + _tmp0 * (P(0, 0) * _tmp0 + P(2, 0) * _tmp1) + + _tmp1 * (P(0, 2) * _tmp0 + P(2, 2) * _tmp1); + } + + if (Hy != nullptr) { + matrix::Matrix& _hy = (*Hy); + + _hy.setZero(); + + _hy(0, 0) = _tmp0; + _hy(2, 0) = _tmp1; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gravity_z_innov_var_and_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gravity_z_innov_var_and_h.h new file mode 100644 index 0000000000..43d48e1283 --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gravity_z_innov_var_and_h.h @@ -0,0 +1,61 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +namespace sym { + +/** + * This function was autogenerated from a symbolic function. Do not modify by hand. + * + * Symbolic function: compute_gravity_z_innov_var_and_h + * + * Args: + * state: Matrix24_1 + * P: Matrix23_23 + * R: Scalar + * + * Outputs: + * innov_var: Scalar + * Hz: Matrix23_1 + */ +template +void ComputeGravityZInnovVarAndH(const matrix::Matrix& state, + const matrix::Matrix& P, const Scalar R, + Scalar* const innov_var = nullptr, + matrix::Matrix* const Hz = nullptr) { + // Total ops: 18 + + // Input arrays + + // Intermediate terms (4) + const Scalar _tmp0 = 2 * state(2, 0); + const Scalar _tmp1 = 2 * state(1, 0); + const Scalar _tmp2 = _tmp0 * state(3, 0) + _tmp1 * state(0, 0); + const Scalar _tmp3 = _tmp0 * state(0, 0) - _tmp1 * state(3, 0); + + // Output terms (2) + if (innov_var != nullptr) { + Scalar& _innov_var = (*innov_var); + + _innov_var = R + _tmp2 * (P(0, 0) * _tmp2 + P(1, 0) * _tmp3) + + _tmp3 * (P(0, 1) * _tmp2 + P(1, 1) * _tmp3); + } + + if (Hz != nullptr) { + matrix::Matrix& _hz = (*Hz); + + _hz.setZero(); + + _hz(0, 0) = _tmp2; + _hz(1, 0) = _tmp3; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index 4b797bafdc..5c5980583d 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -826,10 +826,10 @@ PARAM_DEFINE_FLOAT(EKF2_EVA_NOISE, 0.1f); * @group EKF2 * @min 0.1 * @max 10.0 - * @unit m/s^2 + * @unit g * @decimal 2 */ -PARAM_DEFINE_FLOAT(EKF2_GRAV_NOISE, 5.0f); +PARAM_DEFINE_FLOAT(EKF2_GRAV_NOISE, 1.0f); /** * Optical flow aiding