ekf2-gravity: nomalize gravity fusion and proper sequential fusion

src/lib/parameters/px4params/srcparser.py

@@ -358,7 +358,7 @@ def Validate(self):
                                 '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',

src/modules/ekf2/EKF/gravity_fusion.cpp

@@ -40,14 +40,16 @@
  */
 
 #include "ekf.h"
-#include <ekf_derivation/generated/compute_gravity_innov_var_and_k_and_h.h>
+#include <ekf_derivation/generated/compute_gravity_xyz_innov_var_and_hx.h>
+#include <ekf_derivation/generated/compute_gravity_y_innov_var_and_h.h>
+#include <ekf_derivation/generated/compute_gravity_z_innov_var_and_h.h>
 
 #include <mathlib/mathlib.h>
 
 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;
 	}
 }

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_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, innov_var, K[0], K[1], K[2])
+    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 @@ def compute_gravity_innov_var_and_k_and_h(
 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)