From 05fd8c59761bf96f5dd4d39ece4cbab21ebdfdce Mon Sep 17 00:00:00 2001 From: Mathieu Bresciani Date: Fri, 6 Oct 2023 16:28:21 +0200 Subject: [PATCH] EKF2: centralized auto-generated state (#22183) * ekf2_derivation: use single source of state definition The state is defined as an ordered dictionary of group elements and everything else is generated using that state definition * ekf2: generated state sample add const reference getter --------- Co-authored-by: bresch <[brescianimathieu@gmail.com](mailto:brescianimathieu@gmail.com)> Co-authored-by: Daniel Agar --- src/modules/ekf2/EKF/airspeed_fusion.cpp | 4 +- src/modules/ekf2/EKF/common.h | 11 - src/modules/ekf2/EKF/covariance.cpp | 4 +- src/modules/ekf2/EKF/drag_fusion.cpp | 2 +- src/modules/ekf2/EKF/ekf.h | 4 +- src/modules/ekf2/EKF/ekf_helper.cpp | 17 +- src/modules/ekf2/EKF/gps_yaw_fusion.cpp | 4 +- src/modules/ekf2/EKF/gravity_fusion.cpp | 2 +- src/modules/ekf2/EKF/mag_fusion.cpp | 4 +- src/modules/ekf2/EKF/optflow_fusion.cpp | 6 +- .../EKF/python/ekf_derivation/derivation.py | 311 ++++++++++-------- .../python/ekf_derivation/derivation_utils.py | 76 ++++- .../python/ekf_derivation/generated/state.h | 32 ++ src/modules/ekf2/EKF/sideslip_fusion.cpp | 4 +- src/modules/ekf2/EKF/yaw_fusion.cpp | 4 +- 15 files changed, 279 insertions(+), 206 deletions(-) diff --git a/src/modules/ekf2/EKF/airspeed_fusion.cpp b/src/modules/ekf2/EKF/airspeed_fusion.cpp index 5ad89e76cb..fcdc9c095c 100644 --- a/src/modules/ekf2/EKF/airspeed_fusion.cpp +++ b/src/modules/ekf2/EKF/airspeed_fusion.cpp @@ -144,7 +144,7 @@ void Ekf::updateAirspeed(const airspeedSample &airspeed_sample, estimator_aid_so float innov = 0.f; float innov_var = 0.f; - sym::ComputeAirspeedInnovAndInnovVar(getStateAtFusionHorizonAsVector(), P, airspeed_sample.true_airspeed, R, FLT_EPSILON, &innov, &innov_var); + sym::ComputeAirspeedInnovAndInnovVar(_state.vector(), P, airspeed_sample.true_airspeed, R, FLT_EPSILON, &innov, &innov_var); aid_src.observation = airspeed_sample.true_airspeed; aid_src.observation_variance = R; @@ -195,7 +195,7 @@ void Ekf::fuseAirspeed(const airspeedSample &airspeed_sample, estimator_aid_sour VectorState H; // Observation jacobian VectorState K; // Kalman gain vector - sym::ComputeAirspeedHAndK(getStateAtFusionHorizonAsVector(), P, innov_var, FLT_EPSILON, &H, &K); + sym::ComputeAirspeedHAndK(_state.vector(), P, innov_var, FLT_EPSILON, &H, &K); if (update_wind_only) { const Vector2f K_wind = K.slice(State::wind_vel.idx, 0); diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 90c678ae7f..1c53a8a961 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -269,17 +269,6 @@ struct systemFlagUpdate { bool gnd_effect{false}; }; -struct stateSample { - Quatf quat_nominal{}; ///< quaternion defining the rotation from body to earth frame - Vector3f vel{}; ///< NED velocity in earth frame in m/s - Vector3f pos{}; ///< NED position in earth frame in m - Vector3f gyro_bias{}; ///< gyro bias estimate in rad/s - Vector3f accel_bias{}; ///< accel bias estimate in m/s^2 - Vector3f mag_I{}; ///< NED earth magnetic field in gauss - Vector3f mag_B{}; ///< magnetometer bias estimate in body frame in gauss - Vector2f wind_vel{}; ///< horizontal wind velocity in earth frame in m/s -}; - struct parameters { int32_t filter_update_interval_us{10000}; ///< filter update interval in microseconds diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index bbd7d3bec9..f6363e392a 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -230,7 +230,7 @@ void Ekf::predictCovariance(const imuSample &imu_delayed) SquareMatrixState nextP; // calculate variances and upper diagonal covariances for quaternion, velocity, position and gyro bias states - sym::PredictCovariance(getStateAtFusionHorizonAsVector(), P, + sym::PredictCovariance(_state.vector(), P, imu_delayed.delta_vel, imu_delayed.delta_vel_dt, d_vel_var, imu_delayed.delta_ang, imu_delayed.delta_ang_dt, d_ang_var, &nextP); @@ -542,7 +542,7 @@ void Ekf::resetQuatCov(const float yaw_noise) void Ekf::resetQuatCov(const Vector3f &rot_var_ned) { matrix::SquareMatrix q_cov; - sym::RotVarNedToLowerTriangularQuatCov(getStateAtFusionHorizonAsVector(), rot_var_ned, &q_cov); + sym::RotVarNedToLowerTriangularQuatCov(_state.vector(), rot_var_ned, &q_cov); q_cov.copyLowerToUpperTriangle(); resetStateCovariance(q_cov); } diff --git a/src/modules/ekf2/EKF/drag_fusion.cpp b/src/modules/ekf2/EKF/drag_fusion.cpp index a11a599be7..0d523dc21a 100644 --- a/src/modules/ekf2/EKF/drag_fusion.cpp +++ b/src/modules/ekf2/EKF/drag_fusion.cpp @@ -85,7 +85,7 @@ void Ekf::fuseDrag(const dragSample &drag_sample) _state.vel(2)); const Vector3f rel_wind_body = _state.quat_nominal.rotateVectorInverse(rel_wind_earth); const float rel_wind_speed = rel_wind_body.norm(); - const VectorState state_vector_prev = getStateAtFusionHorizonAsVector(); + const VectorState state_vector_prev = _state.vector(); Vector2f bcoef_inv; diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 1896a8ea8a..f69b1ece3c 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -304,7 +304,7 @@ public: 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 getStateAtFusionHorizonAsVector() const; + const matrix::Vector &getStateAtFusionHorizonAsVector() const { return _state.vector(); } // get the wind velocity in m/s const Vector2f &getWindVelocity() const { return _state.wind_vel; }; @@ -585,7 +585,7 @@ private: Vector3f _ang_rate_delayed_raw{}; ///< uncorrected angular rate vector at fusion time horizon (rad/sec) - stateSample _state{}; ///< state struct of the ekf running at the delayed time horizon + StateSample _state{}; ///< state struct of the ekf running at the delayed time horizon bool _filter_initialised{false}; ///< true when the EKF sttes and covariances been initialised diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index fa27b5620c..92bee563f0 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -369,21 +369,6 @@ void Ekf::getAuxVelInnovVar(float aux_vel_innov_var[2]) const } #endif // CONFIG_EKF2_AUXVEL -// get the state vector at the delayed time horizon -matrix::Vector Ekf::getStateAtFusionHorizonAsVector() const -{ - matrix::Vector state; - state.slice(State::quat_nominal.idx, 0) = _state.quat_nominal; - state.slice(State::vel.idx, 0) = _state.vel; - state.slice(State::pos.idx, 0) = _state.pos; - state.slice(State::gyro_bias.idx, 0) = _state.gyro_bias; - state.slice(State::accel_bias.idx, 0) = _state.accel_bias; - state.slice(State::mag_I.idx, 0) = _state.mag_I; - state.slice(State::mag_B.idx, 0) = _state.mag_B; - state.slice(State::wind_vel.idx, 0) = _state.wind_vel; - return state; -} - bool Ekf::getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &longitude, float &origin_alt) const { origin_time = _pos_ref.getProjectionReferenceTimestamp(); @@ -907,7 +892,7 @@ void Ekf::updateVerticalDeadReckoningStatus() Vector3f Ekf::calcRotVecVariances() const { Vector3f rot_var; - sym::QuatVarToRotVar(getStateAtFusionHorizonAsVector(), P, FLT_EPSILON, &rot_var); + sym::QuatVarToRotVar(_state.vector(), P, FLT_EPSILON, &rot_var); return rot_var; } diff --git a/src/modules/ekf2/EKF/gps_yaw_fusion.cpp b/src/modules/ekf2/EKF/gps_yaw_fusion.cpp index 22bdfb4847..cd2a3efd14 100644 --- a/src/modules/ekf2/EKF/gps_yaw_fusion.cpp +++ b/src/modules/ekf2/EKF/gps_yaw_fusion.cpp @@ -65,7 +65,7 @@ void Ekf::updateGpsYaw(const gpsSample &gps_sample) { VectorState H; - sym::ComputeGnssYawPredInnovVarAndH(getStateAtFusionHorizonAsVector(), P, _gps_yaw_offset, R_YAW, FLT_EPSILON, &heading_pred, &heading_innov_var, &H); + sym::ComputeGnssYawPredInnovVarAndH(_state.vector(), P, _gps_yaw_offset, R_YAW, FLT_EPSILON, &heading_pred, &heading_innov_var, &H); } gnss_yaw.observation = measured_hdg; @@ -97,7 +97,7 @@ void Ekf::fuseGpsYaw() // Note: we recompute innov and innov_var because it doesn't cost much more than just computing H // making a separate function just for H uses more flash space without reducing CPU load significantly - sym::ComputeGnssYawPredInnovVarAndH(getStateAtFusionHorizonAsVector(), P, _gps_yaw_offset, gnss_yaw.observation_variance, FLT_EPSILON, &heading_pred, &heading_innov_var, &H); + sym::ComputeGnssYawPredInnovVarAndH(_state.vector(), P, _gps_yaw_offset, gnss_yaw.observation_variance, FLT_EPSILON, &heading_pred, &heading_innov_var, &H); } // check if the innovation variance calculation is badly conditioned diff --git a/src/modules/ekf2/EKF/gravity_fusion.cpp b/src/modules/ekf2/EKF/gravity_fusion.cpp index d004a9b3ec..b685a92ea7 100644 --- a/src/modules/ekf2/EKF/gravity_fusion.cpp +++ b/src/modules/ekf2/EKF/gravity_fusion.cpp @@ -62,7 +62,7 @@ void Ekf::controlGravityFusion(const imuSample &imu) Vector3f innovation_variance; VectorState Kx, Ky, Kz; // Kalman gain vectors sym::ComputeGravityInnovVarAndKAndH( - getStateAtFusionHorizonAsVector(), P, measurement, measurement_var, FLT_EPSILON, + _state.vector(), P, measurement, measurement_var, FLT_EPSILON, &innovation, &innovation_variance, &Kx, &Ky, &Kz); // fill estimator aid source status diff --git a/src/modules/ekf2/EKF/mag_fusion.cpp b/src/modules/ekf2/EKF/mag_fusion.cpp index f033750d11..c87022c511 100644 --- a/src/modules/ekf2/EKF/mag_fusion.cpp +++ b/src/modules/ekf2/EKF/mag_fusion.cpp @@ -63,7 +63,7 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo // Observation jacobian and Kalman gain vectors VectorState H; - const VectorState state_vector = getStateAtFusionHorizonAsVector(); + const VectorState state_vector = _state.vector(); sym::ComputeMagInnovInnovVarAndHx(state_vector, P, mag, R_MAG, FLT_EPSILON, &mag_innov, &innov_var, &H); innov_var.copyTo(aid_src_mag.innovation_variance); @@ -262,7 +262,7 @@ bool Ekf::fuseDeclination(float decl_sigma) float innovation_variance; // TODO: review getMagDeclination() usage, use mag_I, _mag_declination_gps, or parameter? - sym::ComputeMagDeclinationPredInnovVarAndH(getStateAtFusionHorizonAsVector(), P, R_DECL, FLT_EPSILON, &decl_pred, &innovation_variance, &H); + sym::ComputeMagDeclinationPredInnovVarAndH(_state.vector(), P, R_DECL, FLT_EPSILON, &decl_pred, &innovation_variance, &H); const float innovation = wrap_pi(decl_pred - getMagDeclination()); diff --git a/src/modules/ekf2/EKF/optflow_fusion.cpp b/src/modules/ekf2/EKF/optflow_fusion.cpp index a2d69624e6..235a44254c 100644 --- a/src/modules/ekf2/EKF/optflow_fusion.cpp +++ b/src/modules/ekf2/EKF/optflow_fusion.cpp @@ -77,11 +77,9 @@ void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src) aid_src.observation_variance[0] = R_LOS; aid_src.observation_variance[1] = R_LOS; - const VectorState state_vector = getStateAtFusionHorizonAsVector(); - Vector2f innov_var; VectorState H; - sym::ComputeFlowXyInnovVarAndHx(state_vector, P, range, R_LOS, FLT_EPSILON, &innov_var, &H); + sym::ComputeFlowXyInnovVarAndHx(_state.vector(), P, range, R_LOS, FLT_EPSILON, &innov_var, &H); innov_var.copyTo(aid_src.innovation_variance); // run the innovation consistency check and record result @@ -96,7 +94,7 @@ void Ekf::fuseOptFlow() // a positive offset in earth frame leads to a smaller height above the ground. float range = predictFlowRange(); - const VectorState state_vector = getStateAtFusionHorizonAsVector(); + const VectorState state_vector = _state.vector(); Vector2f innov_var; VectorState H; diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py index d23b9a33e4..1a17dda154 100755 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 # -*- coding: utf-8 -*- """ - Copyright (c) 2022 PX4 Development Team + Copyright (c) 2022-2023 PX4 Development Team Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: @@ -37,82 +37,92 @@ import symforce symforce.set_epsilon_to_symbol() import symforce.symbolic as sf +from symforce import typing as T +from symforce import ops +from symforce.values import Values from derivation_utils import * -class State: - qw = 0 - qx = 1 - qy = 2 - qz = 3 - vx = 4 - vy = 5 - vz = 6 - px = 7 - py = 8 - pz = 9 - gyro_bx = 10 - gyro_by = 11 - gyro_bz = 12 - accel_bx = 13 - accel_by = 14 - accel_bz = 15 - ix = 16 - iy = 17 - iz = 18 - ibx = 19 - iby = 20 - ibz = 21 - wx = 22 - wy = 23 - n_states = 24 +# The state vector is organized in an ordered dictionary +State = Values( + quat_nominal = sf.V4(), + vel = sf.V3(), + pos = sf.V3(), + gyro_bias = sf.V3(), + accel_bias = sf.V3(), + mag_I = sf.V3(), + mag_B = sf.V3(), + wind_vel = sf.V2() +) + +class IdxDof(): + def __init__(self, idx, dof): + self.idx = idx + self.dof = dof + +def BuildTangentStateIndex(): + # Build a dictionary that can be used to access elements of vectors + # and matrices defined in the state tangent space (e.g.: P, K and H) + tangent_state_index = {} + idx = 0 + for key in State.keys_recursive(): + dof = State[key].tangent_dim() + tangent_state_index[key] = IdxDof(idx, dof) + idx += dof + return tangent_state_index + +tangent_idx = BuildTangentStateIndex() class VState(sf.Matrix): - SHAPE = (State.n_states, 1) + SHAPE = (State.storage_dim(), 1) -class MState(sf.Matrix): - SHAPE = (State.n_states, State.n_states) +class VTangent(sf.Matrix): + SHAPE = (State.tangent_dim(), 1) -def state_to_quat(state: VState) -> sf.Quaternion: - return sf.Quaternion(sf.V3(state[State.qx], state[State.qy], state[State.qz]), state[State.qw]) +class MTangent(sf.Matrix): + SHAPE = (State.tangent_dim(), State.tangent_dim()) -def state_to_rot3(state: VState) -> sf.Rot3: - return sf.Rot3(state_to_quat(state)) +def state_to_rot3(state: Values): + q = sf.Quaternion(sf.V3(state["quat_nominal"][1], state["quat_nominal"][2], state["quat_nominal"][3]), state["quat_nominal"][0]) + return sf.Rot3(q) def predict_covariance( - state: VState, - P: MState, - d_vel: sf.V3, - d_vel_dt: sf.Scalar, - d_vel_var: sf.V3, - d_ang: sf.V3, - d_ang_dt: sf.Scalar, - d_ang_var: sf.Scalar -): + state: VState, + P: MTangent, + d_vel: sf.V3, + d_vel_dt: sf.Scalar, + d_vel_var: sf.V3, + d_ang: sf.V3, + d_ang_dt: sf.Scalar, + d_ang_var: sf.Scalar +) -> MTangent: + + state = State.from_storage(state) g = sf.Symbol("g") # does not appear in the jacobians - accel_b = sf.V3(state[State.accel_bx], state[State.accel_by], state[State.accel_bz]) - d_vel_b = accel_b * d_vel_dt + d_vel_b = state["accel_bias"] * d_vel_dt d_vel_true = d_vel - d_vel_b - gyro_b = sf.V3(state[State.gyro_bx], state[State.gyro_by], state[State.gyro_bz]) - d_ang_b = gyro_b * d_ang_dt + d_ang_b = state["gyro_bias"] * d_ang_dt d_ang_true = d_ang - d_ang_b - q = state_to_quat(state) - R_to_earth = sf.Rot3(q) - v = sf.V3(state[State.vx], state[State.vy], state[State.vz]) - p = sf.V3(state[State.px], state[State.py], state[State.pz]) + q = sf.Quaternion(sf.V3(state["quat_nominal"][1], state["quat_nominal"][2], state["quat_nominal"][3]), state["quat_nominal"][0]) + R_to_earth = state_to_rot3(state) + v = state["vel"] + p = state["pos"] q_new = q * sf.Quaternion(sf.V3(0.5 * d_ang_true[0], 0.5 * d_ang_true[1], 0.5 * d_ang_true[2]), 1) v_new = v + R_to_earth * d_vel_true + sf.V3(0, 0, g) * d_vel_dt p_new = p + v * d_vel_dt # Predicted state vector at time t + dt - state_new = VState.block_matrix([[sf.V4(q_new.w, q_new.x, q_new.y, q_new.z)], [v_new], [p_new], [sf.Matrix(state[State.gyro_bx:State.n_states])]]) + state_new = state.copy() + state_new["quat_nominal"] = sf.V4(q_new.w, q_new.x, q_new.y, q_new.z), # convert to Hamiltonian form + state_new["vel"] = v_new, + state_new["pos"] = p_new, # State propagation jacobian - A = state_new.jacobian(state) - G = state_new.jacobian(sf.V6.block_matrix([[d_vel], [d_ang]])) + A = VState(state_new.to_storage()).jacobian(state, tangent_space = False) + G = VState(state_new.to_storage()).jacobian(sf.V6.block_matrix([[d_vel], [d_ang]]), tangent_space = False) # Covariance propagation var_u = sf.Matrix.diag([d_vel_var[0], d_vel_var[1], d_vel_var[2], d_ang_var, d_ang_var, d_ang_var]) @@ -121,8 +131,8 @@ def predict_covariance( # Generate the equations for the upper triangular matrix and the diagonal only # Since the matrix is symmetric, the lower triangle does not need to be derived # and can simply be copied in the implementation - for index in range(State.n_states): - for j in range(State.n_states): + for index in range(state.storage_dim()): + for j in range(state.storage_dim()): if index > j: P_new[index,j] = 0 @@ -130,43 +140,48 @@ def predict_covariance( def compute_airspeed_innov_and_innov_var( state: VState, - P: MState, + P: MTangent, airspeed: sf.Scalar, R: sf.Scalar, epsilon: sf.Scalar ) -> (sf.Scalar, sf.Scalar): - vel_rel = sf.V3(state[State.vx] - state[State.wx], state[State.vy] - state[State.wy], state[State.vz]) + state = State.from_storage(state) + wind = sf.V3(state["wind_vel"][0], state["wind_vel"][1], 0.0) + vel_rel = state["vel"] - wind airspeed_pred = vel_rel.norm(epsilon=epsilon) innov = airspeed_pred - airspeed - H = sf.V1(airspeed_pred).jacobian(state) + H = sf.V1(airspeed_pred).jacobian(state, tangent_space=False) innov_var = (H * P * H.T + R)[0,0] return (innov, innov_var) def compute_airspeed_h_and_k( state: VState, - P: MState, + P: MTangent, innov_var: sf.Scalar, epsilon: sf.Scalar -) -> (VState, VState): +) -> (VTangent, VTangent): - vel_rel = sf.V3(state[State.vx] - state[State.wx], state[State.vy] - state[State.wy], state[State.vz]) + state = State.from_storage(state) + wind = sf.V3(state["wind_vel"][0], state["wind_vel"][1], 0.0) + vel_rel = state["vel"] - wind airspeed_pred = vel_rel.norm(epsilon=epsilon) - H = sf.V1(airspeed_pred).jacobian(state) + H = sf.V1(airspeed_pred).jacobian(state, tangent_space=False) K = P * H.T / sf.Max(innov_var, epsilon) return (H.T, K) def predict_sideslip( - state: VState, + state: State, epsilon: sf.Scalar ) -> (sf.Scalar): - vel_rel = sf.V3(state[State.vx] - state[State.wx], state[State.vy] - state[State.wy], state[State.vz]) + wind = sf.V3(state["wind_vel"][0], state["wind_vel"][1], 0.0) + vel_rel = state["vel"] - wind relative_wind_body = state_to_rot3(state).inverse() * vel_rel # Small angle approximation of side slip model @@ -177,166 +192,176 @@ def predict_sideslip( def compute_sideslip_innov_and_innov_var( state: VState, - P: MState, + P: MTangent, R: sf.Scalar, epsilon: sf.Scalar ) -> (sf.Scalar, sf.Scalar, sf.Scalar): + state = State.from_storage(state) sideslip_pred = predict_sideslip(state, epsilon); innov = sideslip_pred - 0.0 - H = sf.V1(sideslip_pred).jacobian(state) + H = sf.V1(sideslip_pred).jacobian(state, tangent_space=False) innov_var = (H * P * H.T + R)[0,0] return (innov, innov_var) def compute_sideslip_h_and_k( state: VState, - P: MState, + P: MTangent, innov_var: sf.Scalar, epsilon: sf.Scalar -) -> (VState, VState): +) -> (VTangent, VTangent): + state = State.from_storage(state) sideslip_pred = predict_sideslip(state, epsilon); - H = sf.V1(sideslip_pred).jacobian(state) + H = sf.V1(sideslip_pred).jacobian(state, tangent_space=False) K = P * H.T / sf.Max(innov_var, epsilon) return (H.T, K) def predict_mag_body(state) -> sf.V3: - mag_field_earth = sf.V3(state[State.ix], state[State.iy], state[State.iz]) - mag_bias_body = sf.V3(state[State.ibx], state[State.iby], state[State.ibz]) + mag_field_earth = state["mag_I"] + mag_bias_body = state["mag_B"] mag_body = state_to_rot3(state).inverse() * mag_field_earth + mag_bias_body return mag_body def compute_mag_innov_innov_var_and_hx( state: VState, - P: MState, + P: MTangent, meas: sf.V3, R: sf.Scalar, epsilon: sf.Scalar -) -> (sf.V3, sf.V3, VState): +) -> (sf.V3, sf.V3, VTangent): + state = State.from_storage(state) meas_pred = predict_mag_body(state); innov = meas_pred - meas innov_var = sf.V3() - Hx = sf.V1(meas_pred[0]).jacobian(state) + Hx = sf.V1(meas_pred[0]).jacobian(state, tangent_space=False) innov_var[0] = (Hx * P * Hx.T + R)[0,0] - Hy = sf.V1(meas_pred[1]).jacobian(state) + Hy = sf.V1(meas_pred[1]).jacobian(state, tangent_space=False) innov_var[1] = (Hy * P * Hy.T + R)[0,0] - Hz = sf.V1(meas_pred[2]).jacobian(state) + Hz = sf.V1(meas_pred[2]).jacobian(state, tangent_space=False) innov_var[2] = (Hz * P * Hz.T + R)[0,0] return (innov, innov_var, Hx.T) def compute_mag_y_innov_var_and_h( state: VState, - P: MState, + P: MTangent, R: sf.Scalar, epsilon: sf.Scalar -) -> (sf.Scalar, VState): +) -> (sf.Scalar, VTangent): + state = State.from_storage(state) meas_pred = predict_mag_body(state); - H = sf.V1(meas_pred[1]).jacobian(state) + H = sf.V1(meas_pred[1]).jacobian(state, tangent_space=False) innov_var = (H * P * H.T + R)[0,0] return (innov_var, H.T) def compute_mag_z_innov_var_and_h( state: VState, - P: MState, + P: MTangent, R: sf.Scalar, epsilon: sf.Scalar -) -> (sf.Scalar, VState): +) -> (sf.Scalar, VTangent): + state = State.from_storage(state) meas_pred = predict_mag_body(state); - H = sf.V1(meas_pred[2]).jacobian(state) + H = sf.V1(meas_pred[2]).jacobian(state, tangent_space=False) innov_var = (H * P * H.T + R)[0,0] return (innov_var, H.T) def compute_yaw_321_innov_var_and_h( state: VState, - P: MState, + P: MTangent, R: sf.Scalar, epsilon: sf.Scalar -) -> (sf.Scalar, VState): +) -> (sf.Scalar, VTangent): + state = State.from_storage(state) R_to_earth = state_to_rot3(state).to_rotation_matrix() # Fix the singularity at pi/2 by inserting epsilon meas_pred = sf.atan2(R_to_earth[1,0], R_to_earth[0,0], epsilon=epsilon) - H = sf.V1(meas_pred).jacobian(state) + H = sf.V1(meas_pred).jacobian(state, tangent_space=False) innov_var = (H * P * H.T + R)[0,0] return (innov_var, H.T) def compute_yaw_321_innov_var_and_h_alternate( state: VState, - P: MState, + P: MTangent, R: sf.Scalar, epsilon: sf.Scalar -) -> (sf.Scalar, VState): +) -> (sf.Scalar, VTangent): + state = State.from_storage(state) R_to_earth = state_to_rot3(state).to_rotation_matrix() # Alternate form that has a singularity at yaw 0 instead of pi/2 meas_pred = sf.pi/2 - sf.atan2(R_to_earth[0,0], R_to_earth[1,0], epsilon=epsilon) - H = sf.V1(meas_pred).jacobian(state) + H = sf.V1(meas_pred).jacobian(state, tangent_space=False) innov_var = (H * P * H.T + R)[0,0] return (innov_var, H.T) def compute_yaw_312_innov_var_and_h( state: VState, - P: MState, + P: MTangent, R: sf.Scalar, epsilon: sf.Scalar -) -> (sf.Scalar, VState): +) -> (sf.Scalar, VTangent): + state = State.from_storage(state) R_to_earth = state_to_rot3(state).to_rotation_matrix() # Alternate form to be used when close to pitch +-pi/2 meas_pred = sf.atan2(-R_to_earth[0,1], R_to_earth[1,1], epsilon=epsilon) - H = sf.V1(meas_pred).jacobian(state) + H = sf.V1(meas_pred).jacobian(state, tangent_space=False) innov_var = (H * P * H.T + R)[0,0] return (innov_var, H.T) def compute_yaw_312_innov_var_and_h_alternate( state: VState, - P: MState, + P: MTangent, R: sf.Scalar, epsilon: sf.Scalar -) -> (sf.Scalar, VState): +) -> (sf.Scalar, VTangent): + state = State.from_storage(state) R_to_earth = state_to_rot3(state).to_rotation_matrix() # Alternate form to be used when close to pitch +-pi/2 meas_pred = sf.pi/2 - sf.atan2(-R_to_earth[1,1], R_to_earth[0,1], epsilon=epsilon) - H = sf.V1(meas_pred).jacobian(state) + H = sf.V1(meas_pred).jacobian(state, tangent_space=False) innov_var = (H * P * H.T + R)[0,0] return (innov_var, H.T) def compute_mag_declination_pred_innov_var_and_h( state: VState, - P: MState, + P: MTangent, R: sf.Scalar, epsilon: sf.Scalar -) -> (sf.Scalar, sf.Scalar, VState): +) -> (sf.Scalar, sf.Scalar, VTangent): - meas_pred = sf.atan2(state[State.iy], state[State.ix], epsilon=epsilon) + state = State.from_storage(state) + meas_pred = sf.atan2(state["mag_I"][1], state["mag_I"][0], epsilon=epsilon) - H = sf.V1(meas_pred).jacobian(state) + H = sf.V1(meas_pred).jacobian(state, tangent_space=False) innov_var = (H * P * H.T + R)[0,0] return (meas_pred, innov_var, H.T) @@ -345,8 +370,7 @@ def predict_opt_flow(state, distance, epsilon): R_to_body = state_to_rot3(state).inverse() # Calculate earth relative velocity in a non-rotating sensor frame - v = sf.V3(state[State.vx], state[State.vy], state[State.vz]) - rel_vel_sensor = R_to_body * v + rel_vel_sensor = R_to_body * state["vel"] # Divide by range to get predicted angular LOS rates relative to X and Y # axes. Note these are rates in a non-rotating sensor frame @@ -360,43 +384,46 @@ def predict_opt_flow(state, distance, epsilon): def compute_flow_xy_innov_var_and_hx( state: VState, - P: MState, + P: MTangent, distance: sf.Scalar, R: sf.Scalar, epsilon: sf.Scalar -) -> (sf.V2, VState): +) -> (sf.V2, VTangent): + state = State.from_storage(state) meas_pred = predict_opt_flow(state, distance, epsilon); innov_var = sf.V2() - Hx = sf.V1(meas_pred[0]).jacobian(state) + Hx = sf.V1(meas_pred[0]).jacobian(state, tangent_space=False) innov_var[0] = (Hx * P * Hx.T + R)[0,0] - Hy = sf.V1(meas_pred[1]).jacobian(state) + Hy = sf.V1(meas_pred[1]).jacobian(state, tangent_space=False) innov_var[1] = (Hy * P * Hy.T + R)[0,0] return (innov_var, Hx.T) def compute_flow_y_innov_var_and_h( state: VState, - P: MState, + P: MTangent, distance: sf.Scalar, R: sf.Scalar, epsilon: sf.Scalar -) -> (sf.Scalar, VState): +) -> (sf.Scalar, VTangent): + state = State.from_storage(state) meas_pred = predict_opt_flow(state, distance, epsilon); - Hy = sf.V1(meas_pred[1]).jacobian(state) + Hy = sf.V1(meas_pred[1]).jacobian(state, tangent_space=False) innov_var = (Hy * P * Hy.T + R)[0,0] return (innov_var, Hy.T) def compute_gnss_yaw_pred_innov_var_and_h( state: VState, - P: MState, + P: MTangent, antenna_yaw_offset: sf.Scalar, R: sf.Scalar, epsilon: sf.Scalar -) -> (sf.Scalar, sf.Scalar, VState): +) -> (sf.Scalar, sf.Scalar, VTangent): + state = State.from_storage(state) R_to_earth = state_to_rot3(state) # define antenna vector in body frame @@ -408,13 +435,13 @@ def compute_gnss_yaw_pred_innov_var_and_h( # Calculate the yaw angle from the projection meas_pred = sf.atan2(ant_vec_ef[1], ant_vec_ef[0], epsilon=epsilon) - H = sf.V1(meas_pred).jacobian(state) + H = sf.V1(meas_pred).jacobian(state, tangent_space=False) innov_var = (H * P * H.T + R)[0,0] return (meas_pred, innov_var, H.T) def predict_drag( - state: VState, + state: State, rho: sf.Scalar, cd: sf.Scalar, cm: sf.Scalar, @@ -422,9 +449,8 @@ def predict_drag( ) -> (sf.Scalar): R_to_body = state_to_rot3(state).inverse() - vel_rel = sf.V3(state[State.vx] - state[State.wx], - state[State.vy] - state[State.wy], - state[State.vz]) + wind = sf.V3(state["wind_vel"][0], state["wind_vel"][1], 0.0) + vel_rel = state["vel"] - wind vel_rel_body = R_to_body * vel_rel vel_rel_body_xy = sf.V2(vel_rel_body[0], vel_rel_body[1]) @@ -436,52 +462,53 @@ def predict_drag( def compute_drag_x_innov_var_and_k( state: VState, - P: MState, + P: MTangent, rho: sf.Scalar, cd: sf.Scalar, cm: sf.Scalar, R: sf.Scalar, epsilon: sf.Scalar -) -> (sf.Scalar, VState): +) -> (sf.Scalar, VTangent): + state = State.from_storage(state) meas_pred = predict_drag(state, rho, cd, cm, epsilon) - Hx = sf.V1(meas_pred[0]).jacobian(state) + Hx = sf.V1(meas_pred[0]).jacobian(state, tangent_space=False) innov_var = (Hx * P * Hx.T + R)[0,0] Ktotal = P * Hx.T / sf.Max(innov_var, epsilon) - K = VState() - K[State.wx] = Ktotal[State.wx] - K[State.wy] = Ktotal[State.wy] + K = VTangent() + K[tangent_idx["wind_vel"].idx: tangent_idx["wind_vel"].idx + tangent_idx["wind_vel"].dof] = Ktotal[tangent_idx["wind_vel"].idx: tangent_idx["wind_vel"].idx + tangent_idx["wind_vel"].dof] return (innov_var, K) def compute_drag_y_innov_var_and_k( state: VState, - P: MState, + P: MTangent, rho: sf.Scalar, cd: sf.Scalar, cm: sf.Scalar, R: sf.Scalar, epsilon: sf.Scalar -) -> (sf.Scalar, VState): +) -> (sf.Scalar, VTangent): + state = State.from_storage(state) meas_pred = predict_drag(state, rho, cd, cm, epsilon) - Hy = sf.V1(meas_pred[1]).jacobian(state) + Hy = sf.V1(meas_pred[1]).jacobian(state, tangent_space=False) innov_var = (Hy * P * Hy.T + R)[0,0] Ktotal = P * Hy.T / sf.Max(innov_var, epsilon) - K = VState() - K[State.wx] = Ktotal[State.wx] - K[State.wy] = Ktotal[State.wy] + K = VTangent() + K[tangent_idx["wind_vel"].idx: tangent_idx["wind_vel"].idx + tangent_idx["wind_vel"].dof] = Ktotal[tangent_idx["wind_vel"].idx: tangent_idx["wind_vel"].idx + tangent_idx["wind_vel"].dof] return (innov_var, K) def compute_gravity_innov_var_and_k_and_h( state: VState, - P: MState, + P: MTangent, meas: sf.V3, R: sf.Scalar, epsilon: sf.Scalar -) -> (sf.V3, sf.V3, VState, VState, VState): +) -> (sf.V3, sf.V3, VTangent, VTangent, VTangent): + state = State.from_storage(state) # get transform from earth to body frame R_to_body = state_to_rot3(state).inverse() @@ -497,7 +524,7 @@ def compute_gravity_innov_var_and_k_and_h( # 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) + H = sf.V1(meas_pred[i]).jacobian(state, tangent_space=False) innov_var[i] = (H * P * H.T + R)[0,0] K[i] = P * H.T / innov_var[i] @@ -505,22 +532,24 @@ def compute_gravity_innov_var_and_k_and_h( def quat_var_to_rot_var( state: VState, - P: MState, + P: MTangent, epsilon: sf.Scalar -): - J = sf.V3(state_to_rot3(state).to_tangent(epsilon=epsilon)).jacobian(state) +) -> sf.V3: + state = State.from_storage(state) + J = sf.V3(state_to_rot3(state).to_tangent(epsilon=epsilon)).jacobian(state, tangent_space=False) rot_cov = J * P * J.T return sf.V3(rot_cov[0, 0], rot_cov[1, 1], rot_cov[2, 2]) def rot_var_ned_to_lower_triangular_quat_cov( state: VState, rot_var_ned: sf.V3 -): +) -> sf.M44: # This function converts an attitude variance defined by a 3D vector in NED frame # into a 4x4 covariance matrix representing the uncertainty on each of the 4 quaternion parameters # Note: the resulting quaternion uncertainty is defined as a perturbation # at the tip of the quaternion (i.e.:body-frame uncertainty) - q = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz]) + state = State.from_storage(state) + q = state["quat_nominal"] attitude = state_to_rot3(state) J = q.jacobian(attitude) @@ -536,12 +565,11 @@ def rot_var_ned_to_lower_triangular_quat_cov( return q_var.lower_triangle() print("Derive EKF2 equations...") +generate_px4_function(predict_covariance, output_names=["P_new"]) 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"]) -generate_px4_function(predict_covariance, output_names=["P_new"]) generate_px4_function(compute_mag_innov_innov_var_and_hx, output_names=["innov", "innov_var", "Hx"]) generate_px4_function(compute_mag_y_innov_var_and_h, output_names=["innov_var", "H"]) generate_px4_function(compute_mag_z_innov_var_and_h, output_names=["innov_var", "H"]) @@ -559,11 +587,4 @@ generate_px4_function(compute_gravity_innov_var_and_k_and_h, output_names=["inno generate_px4_function(quat_var_to_rot_var, output_names=["rot_var"]) generate_px4_function(rot_var_ned_to_lower_triangular_quat_cov, output_names=["q_cov_lower_triangle"]) -generate_px4_state({"quat_nominal": sf.V4, - "vel": sf.V3, - "pos": sf.V3, - "gyro_bias": sf.V3, - "accel_bias": sf.V3, - "mag_I": sf.V3, - "mag_B": sf.V3, - "wind_vel": sf.V2}) +generate_px4_state(State, tangent_idx) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation_utils.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation_utils.py index f2b00ac0da..bd91c3ba61 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation_utils.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation_utils.py @@ -1,7 +1,7 @@ #!/usr/bin/env python # -*- coding: utf-8 -*- """ - Copyright (c) 2022 PX4 Development Team + Copyright (c) 2022-2023 PX4 Development Team Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: @@ -88,8 +88,63 @@ def generate_python_function(function_name, output_names): output_dir="generated", skip_directory_nesting=True) -def generate_px4_state(states): - print("Generate EKF state definition") +def build_state_struct(state, T="float"): + out = "struct StateSample {\n" + + def TypeFromLength(len): + if len == 1: + return f"{T}" + elif len == 2: + return f"matrix::Vector2<{T}>" + elif len == 3: + return f"matrix::Vector3<{T}>" + elif len == 4: + return f"matrix::Quaternion<{T}>" + else: + raise NotImplementedError + + for key, val in state.items(): + out += f"\t{TypeFromLength(len(val))} {key}{{}};\n" + + state_size = state.storage_dim() + out += f"\n\tmatrix::Vector<{T}, {state_size}> Data() const {{\n" \ + + f"\t\tmatrix::Vector<{T}, {state_size}> state;\n" + + index = state.index() + for key in index: + out += f"\t\tstate.slice<{index[key].storage_dim}, 1>({index[key].offset}, 0) = {key};\n" + + out += "\t\treturn state;\n" + out += "\t};\n" # Data + + # const ref vector access + first_field = next(iter(state)) + + out += f"\n\tconst matrix::Vector<{T}, {state_size}>& vector() const {{\n" \ + + f"\t\treturn *reinterpret_cast*>(const_cast(reinterpret_cast(&{first_field})));\n" \ + + f"\t}};\n\n" + + out += "};\n" # StateSample + + out += f"static_assert(sizeof(matrix::Vector<{T}, {state_size}>) == sizeof(StateSample), \"state vector doesn't match StateSample size\");\n" + + return out + +def build_tangent_state_struct(state, tangent_state_index): + out = "struct IdxDof { unsigned idx; unsigned dof; };\n" + + out += "namespace State {\n" + + start_index = 0 + for key in tangent_state_index.keys(): + out += f"\tstatic constexpr IdxDof {key}{{{tangent_state_index[key].idx}, {tangent_state_index[key].dof}}};\n" + + out += f"\tstatic constexpr uint8_t size{{{state.tangent_dim()}}};\n" + out += "};\n" # namespace State + return out + +def generate_px4_state(state, tangent_state_index): + print("Generate EKF tangent state definition") filename = "state.h" f = open(f"./generated/{filename}", "w") header = ["// --------------------------------------------------\n", @@ -97,21 +152,14 @@ def generate_px4_state(states): "// --------------------------------------------------\n", "\n#ifndef EKF_STATE_H", "\n#define EKF_STATE_H\n\n", + "#include \n\n", "namespace estimator\n{\n"] f.writelines(header) - f.write("struct IdxDof { unsigned idx; unsigned dof; };\n"); + f.write(build_state_struct(state)) + f.write("\n") + f.write(build_tangent_state_struct(state, tangent_state_index)) - f.write("namespace State {\n"); - - start_index = 0 - for (state_name, state_type) in states.items(): - tangent_dim = state_type.tangent_dim() - f.write(f"\tstatic constexpr IdxDof {state_name}{{{start_index}, {tangent_dim}}};\n") - start_index += tangent_dim - - f.write(f"\tstatic constexpr uint8_t size{{{start_index}}};\n") - f.write("};\n") # namespace State f.write("}\n") # namespace estimator f.write("#endif // !EKF_STATE_H\n") f.close() diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/state.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/state.h index 964fbcd71f..200af9c0e4 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/state.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/state.h @@ -5,8 +5,40 @@ #ifndef EKF_STATE_H #define EKF_STATE_H +#include + namespace estimator { +struct StateSample { + matrix::Quaternion quat_nominal{}; + matrix::Vector3 vel{}; + matrix::Vector3 pos{}; + matrix::Vector3 gyro_bias{}; + matrix::Vector3 accel_bias{}; + matrix::Vector3 mag_I{}; + matrix::Vector3 mag_B{}; + matrix::Vector2 wind_vel{}; + + matrix::Vector Data() const { + matrix::Vector state; + state.slice<4, 1>(0, 0) = quat_nominal; + state.slice<3, 1>(4, 0) = vel; + state.slice<3, 1>(7, 0) = pos; + state.slice<3, 1>(10, 0) = gyro_bias; + state.slice<3, 1>(13, 0) = accel_bias; + state.slice<3, 1>(16, 0) = mag_I; + state.slice<3, 1>(19, 0) = mag_B; + state.slice<2, 1>(22, 0) = wind_vel; + return state; + }; + + const matrix::Vector& vector() const { + return *reinterpret_cast*>(const_cast(reinterpret_cast(&quat_nominal))); + }; + +}; +static_assert(sizeof(matrix::Vector) == sizeof(StateSample), "state vector doesn't match StateSample size"); + struct IdxDof { unsigned idx; unsigned dof; }; namespace State { static constexpr IdxDof quat_nominal{0, 4}; diff --git a/src/modules/ekf2/EKF/sideslip_fusion.cpp b/src/modules/ekf2/EKF/sideslip_fusion.cpp index 3ccdf2a7b6..bea266d81e 100644 --- a/src/modules/ekf2/EKF/sideslip_fusion.cpp +++ b/src/modules/ekf2/EKF/sideslip_fusion.cpp @@ -87,7 +87,7 @@ void Ekf::updateSideslip(estimator_aid_source1d_s &sideslip) const float innov = 0.f; float innov_var = 0.f; - sym::ComputeSideslipInnovAndInnovVar(getStateAtFusionHorizonAsVector(), P, R, FLT_EPSILON, &innov, &innov_var); + sym::ComputeSideslipInnovAndInnovVar(_state.vector(), P, R, FLT_EPSILON, &innov, &innov_var); sideslip.observation = 0.f; sideslip.observation_variance = R; @@ -136,7 +136,7 @@ void Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip) VectorState H; // Observation jacobian VectorState K; // Kalman gain vector - sym::ComputeSideslipHAndK(getStateAtFusionHorizonAsVector(), P, sideslip.innovation_variance, FLT_EPSILON, &H, &K); + sym::ComputeSideslipHAndK(_state.vector(), P, sideslip.innovation_variance, FLT_EPSILON, &H, &K); if (update_wind_only) { const Vector2f K_wind = K.slice(State::wind_vel.idx, 0); diff --git a/src/modules/ekf2/EKF/yaw_fusion.cpp b/src/modules/ekf2/EKF/yaw_fusion.cpp index 05615585ce..770a786338 100644 --- a/src/modules/ekf2/EKF/yaw_fusion.cpp +++ b/src/modules/ekf2/EKF/yaw_fusion.cpp @@ -136,9 +136,9 @@ bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H void Ekf::computeYawInnovVarAndH(float variance, float &innovation_variance, VectorState &H_YAW) const { if (shouldUse321RotationSequence(_R_to_earth)) { - sym::ComputeYaw321InnovVarAndH(getStateAtFusionHorizonAsVector(), P, variance, FLT_EPSILON, &innovation_variance, &H_YAW); + sym::ComputeYaw321InnovVarAndH(_state.vector(), P, variance, FLT_EPSILON, &innovation_variance, &H_YAW); } else { - sym::ComputeYaw312InnovVarAndH(getStateAtFusionHorizonAsVector(), P, variance, FLT_EPSILON, &innovation_variance, &H_YAW); + sym::ComputeYaw312InnovVarAndH(_state.vector(), P, variance, FLT_EPSILON, &innovation_variance, &H_YAW); } }