EKF2: Added gravity observation EKF fusion calculation.

Signed-off-by: Daniel M. Sahu <danielmohansahu@gmail.com>

src/modules/ekf2/EKF/python/ekf_derivation/derivation.py

@@ -477,6 +477,33 @@ def compute_drag_y_innov_var_and_k(
 
     return (innov_var, K)
 
+def computy_gravity_innov_var_and_k_and_h(
+        state: VState,
+        P: MState,
+        meas: sf.V3,
+        R: sf.Scalar,
+        epsilon: sf.Scalar
+) -> (sf.V3, sf.V3, VState, VState, VState, VState, VState, VState):
+
+    # the innovation is the error between measured acceleration
+    #  and predicted (body frame), assuming no body acceleration
+    meas_pred = R * sf.Matrix([0,0,-9.80665])
+    innov = meas_pred - meas.norm(epsilon=epsilon)
+
+    # initialize outputs
+    innov_var = sf.V3()
+    H = [None] * 3
+    K = [None] * 3
+
+    # calculate observation jacobian (H), kalman gain (K), and innovation variance (S)
+    #  for each axis
+    for i in range(3):
+        H[i] = sf.V1(meas_pred[i]).jacobian(state)
+        innov_var[i] = (H[i] * P * H[i].T + R)[0,0]
+        K[i] = P * H[i].T / innov_var[i]
+
+    return (innov, innov_var, K[0], K[1], K[2], H[0], H[1], H[2])
+
 print("Derive EKF2 equations...")
 generate_px4_function(compute_airspeed_innov_and_innov_var, output_names=["innov", "innov_var"])
 generate_px4_function(compute_airspeed_h_and_k, output_names=["H", "K"])
@@ -497,3 +524,4 @@ generate_px4_function(compute_flow_y_innov_var_and_h, output_names=["innov_var",
 generate_px4_function(compute_gnss_yaw_innon_innov_var_and_h, output_names=["innov", "innov_var", "H"])
 generate_px4_function(compute_drag_x_innov_var_and_k, output_names=["innov_var", "K"])
 generate_px4_function(compute_drag_y_innov_var_and_k, output_names=["innov_var", "K"])
+generate_px4_function(computy_gravity_innov_var_and_k_and_h, output_names=["innov", "innov_var", "Kx", "Ky", "Kz", "Hx", "Hy", "Hz"])