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 <daniel@agar.ca>

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.dof, 1>(State::wind_vel.idx, 0);

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

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<float, State::quat_nominal.dof> q_cov;
-	sym::RotVarNedToLowerTriangularQuatCov(getStateAtFusionHorizonAsVector(), rot_var_ned, &q_cov);
+	sym::RotVarNedToLowerTriangularQuatCov(_state.vector(), rot_var_ned, &q_cov);
 	q_cov.copyLowerToUpperTriangle();
 	resetStateCovariance<State::quat_nominal>(q_cov);
 }

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;
 

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<float, State::size> getStateAtFusionHorizonAsVector() const;
+	const matrix::Vector<float, State::size> &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
 

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<float, 24> Ekf::getStateAtFusionHorizonAsVector() const
-{
-	matrix::Vector<float, 24> state;
-	state.slice<State::quat_nominal.dof, 1>(State::quat_nominal.idx, 0) = _state.quat_nominal;
-	state.slice<State::vel.dof, 1>(State::vel.idx, 0) = _state.vel;
-	state.slice<State::pos.dof, 1>(State::pos.idx, 0) = _state.pos;
-	state.slice<State::gyro_bias.dof, 1>(State::gyro_bias.idx, 0) = _state.gyro_bias;
-	state.slice<State::accel_bias.dof, 1>(State::accel_bias.idx, 0) = _state.accel_bias;
-	state.slice<State::mag_I.dof, 1>(State::mag_I.idx, 0) = _state.mag_I;
-	state.slice<State::mag_B.dof, 1>(State::mag_B.idx, 0) = _state.mag_B;
-	state.slice<State::wind_vel.dof, 1>(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;
 }
 

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

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

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());
 

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;

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:
+# The state vector is organized in an ordered dictionary
-    qw = 0
+State = Values(
-    qx = 1
+    quat_nominal = sf.V4(),
-    qy = 2
+    vel = sf.V3(),
-    qz = 3
+    pos = sf.V3(),
-    vx = 4
+    gyro_bias = sf.V3(),
-    vy = 5
+    accel_bias = sf.V3(),
-    vz = 6
+    mag_I = sf.V3(),
-    px = 7
+    mag_B = sf.V3(),
-    py = 8
+    wind_vel = sf.V2()
-    pz = 9
+)
-    gyro_bx  = 10
+
-    gyro_by  = 11
+class IdxDof():
-    gyro_bz  = 12
+    def __init__(self, idx, dof):
-    accel_bx = 13
+        self.idx = idx
-    accel_by = 14
+        self.dof = dof
-    accel_bz = 15
+
-    ix = 16
+def BuildTangentStateIndex():
-    iy = 17
+    # Build a dictionary that can be used to access elements of vectors
-    iz = 18
+    # and matrices defined in the state tangent space (e.g.: P, K and H)
-    ibx = 19
+    tangent_state_index = {}
-    iby = 20
+    idx = 0
-    ibz = 21
+    for key in State.keys_recursive():
-    wx = 22
+        dof = State[key].tangent_dim()
-    wy = 23
+        tangent_state_index[key] = IdxDof(idx, dof)
-    n_states = 24
+        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):
+class VTangent(sf.Matrix):
-    SHAPE = (State.n_states, State.n_states)
+    SHAPE = (State.tangent_dim(), 1)
 
-def state_to_quat(state: VState) -> sf.Quaternion:
+class MTangent(sf.Matrix):
-    return sf.Quaternion(sf.V3(state[State.qx], state[State.qy], state[State.qz]), state[State.qw])
+    SHAPE = (State.tangent_dim(), State.tangent_dim())
 
-def state_to_rot3(state: VState) -> sf.Rot3:
+def state_to_rot3(state: Values):
-    return sf.Rot3(state_to_quat(state))
+    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,
+    state: VState,
-        P: MState,
+    P: MTangent,
-        d_vel: sf.V3,
+    d_vel: sf.V3,
-        d_vel_dt: sf.Scalar,
+    d_vel_dt: sf.Scalar,
-        d_vel_var: sf.V3,
+    d_vel_var: sf.V3,
-        d_ang: sf.V3,
+    d_ang: sf.V3,
-        d_ang_dt: sf.Scalar,
+    d_ang_dt: sf.Scalar,
-        d_ang_var: 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 = state["accel_bias"] * d_vel_dt
-    d_vel_b = accel_b * 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 = state["gyro_bias"] * d_ang_dt
-    d_ang_b = gyro_b * d_ang_dt
     d_ang_true = d_ang - d_ang_b
 
-    q = state_to_quat(state)
+    q = sf.Quaternion(sf.V3(state["quat_nominal"][1], state["quat_nominal"][2], state["quat_nominal"][3]), state["quat_nominal"][0])
-    R_to_earth = sf.Rot3(q)
+    R_to_earth = state_to_rot3(state)
-    v = sf.V3(state[State.vx], state[State.vy], state[State.vz])
+    v = state["vel"]
-    p = sf.V3(state[State.px], state[State.py], state[State.pz])
+    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)
+    A = VState(state_new.to_storage()).jacobian(state, tangent_space = False)
-    G = state_new.jacobian(sf.V6.block_matrix([[d_vel], [d_ang]]))
+    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 index in range(state.storage_dim()):
-        for j in range(State.n_states):
+        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_field_earth = state["mag_I"]
-    mag_bias_body = sf.V3(state[State.ibx], state[State.iby], state[State.ibz])
+    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 * state["vel"]
-    rel_vel_sensor = R_to_body * v
 
     # 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],
+    wind = sf.V3(state["wind_vel"][0], state["wind_vel"][1], 0.0)
-                    state[State.vy] - state[State.wy],
+    vel_rel = state["vel"] - wind
-                    state[State.vz])
     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 = VTangent()
-    K[State.wx] = Ktotal[State.wx]
+    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]
-    K[State.wy] = Ktotal[State.wy]
 
     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 = VTangent()
-    K[State.wx] = Ktotal[State.wx]
+    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]
-    K[State.wy] = Ktotal[State.wy]
 
     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
-):
+) -> sf.V3:
-    J = sf.V3(state_to_rot3(state).to_tangent(epsilon=epsilon)).jacobian(state)
+    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,
+generate_px4_state(State, tangent_idx)
-                    "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})

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):
+def build_state_struct(state, T="float"):
-    print("Generate EKF state definition")
+    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<matrix::Vector<{T}, {state_size}>*>(const_cast<float*>(reinterpret_cast<const {T}*>(&{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 <matrix/math.hpp>\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()

src/modules/ekf2/EKF/python/ekf_derivation/generated/state.h

@@ -5,8 +5,40 @@
 #ifndef EKF_STATE_H
 #define EKF_STATE_H
 
+#include <matrix/math.hpp>
+
 namespace estimator
 {
+struct StateSample {
+	matrix::Quaternion<float> quat_nominal{};
+	matrix::Vector3<float> vel{};
+	matrix::Vector3<float> pos{};
+	matrix::Vector3<float> gyro_bias{};
+	matrix::Vector3<float> accel_bias{};
+	matrix::Vector3<float> mag_I{};
+	matrix::Vector3<float> mag_B{};
+	matrix::Vector2<float> wind_vel{};
+
+	matrix::Vector<float, 24> Data() const {
+		matrix::Vector<float, 24> 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<float, 24>& vector() const {
+		return *reinterpret_cast<matrix::Vector<float, 24>*>(const_cast<float*>(reinterpret_cast<const float*>(&quat_nominal)));
+	};
+
+};
+static_assert(sizeof(matrix::Vector<float, 24>) == sizeof(StateSample), "state vector doesn't match StateSample size");
+
 struct IdxDof { unsigned idx; unsigned dof; };
 namespace State {
 	static constexpr IdxDof quat_nominal{0, 4};

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.dof, 1>(State::wind_vel.idx, 0);

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);
 	}
 }