ekf2: access state covariance using enum

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

@@ -558,3 +558,12 @@ generate_px4_function(compute_drag_y_innov_var_and_k, output_names=["innov_var",
 generate_px4_function(compute_gravity_innov_var_and_k_and_h, output_names=["innov", "innov_var", "Kx", "Ky", "Kz"])
 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})

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

@@ -87,3 +87,31 @@ def generate_python_function(function_name, output_names):
     metadata = codegen.generate_function(
             output_dir="generated",
             skip_directory_nesting=True)
+
+def generate_px4_state(states):
+    print("Generate EKF state definition")
+    filename = "state.h"
+    f = open(f"./generated/{filename}", "w")
+    header = ["// --------------------------------------------------\n",
+              "// This file was autogenerated, do NOT modify by hand\n",
+              "// --------------------------------------------------\n",
+              "\n#ifndef EKF_STATE_H",
+              "\n#define EKF_STATE_H\n\n",
+              "namespace estimator\n{\n"]
+    f.writelines(header)
+
+    f.write("struct IdxDof { unsigned idx; unsigned dof; };\n");
+
+    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("};\n") # namespace State
+    f.write("}\n") # namespace estimator
+    f.write("#endif // !EKF_STATE_H\n")
+    f.close()
+    print(f"  |- {filename}")

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

@@ -0,0 +1,22 @@
+// --------------------------------------------------
+// This file was autogenerated, do NOT modify by hand
+// --------------------------------------------------
+
+#ifndef EKF_STATE_H
+#define EKF_STATE_H
+
+namespace estimator
+{
+struct IdxDof { unsigned idx; unsigned dof; };
+namespace State {
+	static constexpr IdxDof quat_nominal{0, 4};
+	static constexpr IdxDof vel{4, 3};
+	static constexpr IdxDof pos{7, 3};
+	static constexpr IdxDof gyro_bias{10, 3};
+	static constexpr IdxDof accel_bias{13, 3};
+	static constexpr IdxDof mag_I{16, 3};
+	static constexpr IdxDof mag_B{19, 3};
+	static constexpr IdxDof wind_vel{22, 2};
+};
+}
+#endif // !EKF_STATE_H