reacting to comments

PX4 · Jun 14, 2024 · 35c65d7 · 35c65d7
1 parent 658a249
commit 35c65d7
Show file tree

Hide file tree

Showing 7 changed files with 62 additions and 64 deletions.
diff --git a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel_control.cpp b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel_control.cpp
@@ -85,8 +85,7 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common
 			const Dcmf R_ev_to_ekf = Dcmf(_ev_q_error_filt.getState());
 
 			measurement = R_ev_to_ekf * ev_sample.vel - vel_offset_earth;
-			measurement_var = matrix::SquareMatrix3f(R_ev_to_ekf * matrix::diag(ev_sample.velocity_var) *
-					  R_ev_to_ekf.transpose()).diag();
+			measurement_var = rotateVarianceToEkf(ev_sample.velocity_var);
 			minimum_variance = math::max(minimum_variance, ev_sample.orientation_var.max());
 		}
 
@@ -124,10 +123,10 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common
 	continuing_conditions_passing &= measurement.isAllFinite() && measurement_var.isAllFinite();
 
 	if (ev_sample.vel_frame == VelocityFrame::BODY_FRAME_FRD) {
+		const Vector3f measurement_var_ekf_frame = rotateVarianceToEkf(measurement_var);
 		updateVelocityAidSrcStatus(ev_sample.time_us,
 					   _R_to_earth * measurement,
-					   matrix::SquareMatrix3f(_R_to_earth * matrix::diag(measurement_var) *
-							   _R_to_earth.transpose()).diag(),
+					   measurement_var_ekf_frame,
 					   math::max(_params.ev_vel_innov_gate, 1.f),
 					   aid_src);
 		measurement.copyTo(aid_src.observation);
@@ -242,15 +241,21 @@ void Ekf::stopEvVelFusion()
 	}
 }
 
-void Ekf::resetVelocityToEV(Vector3f &measurement, Vector3f &measurement_var, const VelocityFrame &vel_frame)
+void Ekf::resetVelocityToEV(const Vector3f &measurement, const Vector3f &measurement_var, const VelocityFrame &vel_frame)
 {
 	if (vel_frame == VelocityFrame::BODY_FRAME_FRD) {
-		resetVelocityTo(_R_to_earth * measurement,
-				matrix::SquareMatrix3f(_R_to_earth * matrix::diag(measurement_var) *
-						       _R_to_earth.transpose()).diag());
+		const Vector3f measurement_var_ekf_frame = rotateVarianceToEkf(measurement_var);
+		resetVelocityTo(_R_to_earth * measurement, measurement_var_ekf_frame);
 
 	} else {
 		resetVelocityTo(measurement, measurement_var);
 	}
 
 }
+
+Vector3f Ekf::rotateVarianceToEkf(const Vector3f &measurement_var)
+{
+	// rotate the covariance matrix into the EKF frame
+	const matrix::SquareMatrix<float, 3> R_cov = _R_to_earth * matrix::diag(measurement_var) * _R_to_earth.transpose();
+	return R_cov.diag();
+}
diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h
@@ -958,7 +958,8 @@ class Ekf final : public EstimatorInterface
 	void controlEvPosFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source2d_s &aid_src);
 	void controlEvVelFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source3d_s &aid_src);
 	void controlEvYawFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s &aid_src);
-	void resetVelocityToEV(Vector3f &measurement, Vector3f &measurement_var, const VelocityFrame &vel_frame);
+	void resetVelocityToEV(const Vector3f &measurement, const Vector3f &measurement_var, const VelocityFrame &vel_frame);
+	Vector3f rotateVarianceToEkf(const Vector3f &measurement_var);
 
 	void startEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, estimator_aid_source2d_s &aid_src);
 	void updateEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, bool quality_sufficient, bool reset, estimator_aid_source2d_s &aid_src);

diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py
@@ -360,59 +360,58 @@ def compute_sideslip_h_and_k(
 
     return (H.T, K)
 
+def predict_vel_body(
+        state: VState
+) -> (sf.V3):
+    vel = state["vel"]
+    R_to_body = state["quat_nominal"].inverse()
+    return R_to_body * vel
+
 def compute_ev_body_vel_var_and_hx(
         state: VState,
         P: MTangent,
 ) -> (VTangent):
 
     state = vstate_to_state(state)
-
-    vel = state["vel"]
-    R_to_body = state["quat_nominal"].inverse()
-    meas_pred = R_to_body * vel
-
+    meas_pred = predict_vel_body(state)
     Hx = jacobian_chain_rule(meas_pred[0], state)
     Hy = jacobian_chain_rule(meas_pred[1], state)
     Hz = jacobian_chain_rule(meas_pred[2], state)
     Hx[:3] = [0,0,0]
     Hy[:3] = [0,0,0]
     Hz[:3] = [0,0,0]
-    incomp_innov_var = sf.V3()
+    body_vel_var = sf.V3()
     # attention: +R needs to be added to the variance in the calling function
-    incomp_innov_var[0] = (Hx * P * Hx.T)[0,0]
-    incomp_innov_var[1] = (Hy * P * Hy.T)[0,0]
-    incomp_innov_var[2] = (Hz * P * Hz.T)[0,0]
-    return (incomp_innov_var, Hx)
+    body_vel_var[0] = (Hx * P * Hx.T)[0,0]
+    body_vel_var[1] = (Hy * P * Hy.T)[0,0]
+    body_vel_var[2] = (Hz * P * Hz.T)[0,0]
+    return (body_vel_var, Hx.T)
 
 def compute_ev_body_vel_var_and_hy(
         state: VState,
         P: MTangent,
 ) -> (VTangent):
 
     state = vstate_to_state(state)
-    vel = state["vel"]
-    R_to_body = state["quat_nominal"].inverse()
-    meas_pred = (R_to_body * vel)[1]
+    meas_pred = predict_vel_body(state)[1]
     Hy = jacobian_chain_rule(meas_pred, state)
     Hy[:3] = [0,0,0]
     # attention: +R needs to be added to the variance in the calling function
-    incomp_innov_var = (Hy * P * Hy.T)[0,0]
-    return (incomp_innov_var, Hy)
+    body_vel_var = (Hy * P * Hy.T)[0,0]
+    return (body_vel_var, Hy.T)
 
 def compute_ev_body_vel_var_and_hz(
         state: VState,
         P: MTangent,
 ) -> (VTangent):
 
     state = vstate_to_state(state)
-    vel = state["vel"]
-    R_to_body = state["quat_nominal"].inverse()
-    meas_pred = (R_to_body * vel)[2]
+    meas_pred = predict_vel_body(state)[2]
     Hz = jacobian_chain_rule(meas_pred, state)
     Hz[:3] = [0,0,0]
     # attention: +R needs to be added to the variance in the calling function
-    incomp_innov_var = (Hz * P * Hz.T)[0,0]
-    return (incomp_innov_var, Hz)
+    body_vel_var = (Hz * P * Hz.T)[0,0]
+    return (body_vel_var, Hz.T)
 
 def predict_mag_body(state) -> sf.V3:
     mag_field_earth = state["mag_I"]
@@ -722,12 +721,6 @@ def compute_gravity_z_innov_var_and_h(
 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_function(compute_body_vel_innov_innov_var_and_h, output_names=["innov", "H"])
-
-# generate_px4_function(compute_body_vel_innov_innov_var_and_hx, output_names=["innov", "innov_var", "H"])
-# generate_px4_function(compute_body_vel_innov_innov_var_and_hy, output_names=["innov", "innov_var", "H"])
-# generate_px4_function(compute_body_vel_innov_innov_var_and_hz, output_names=["innov", "innov_var", "H"])
-
 generate_px4_function(compute_ev_body_vel_var_and_hx, output_names=["incomp_innov_var", "H"])
 generate_px4_function(compute_ev_body_vel_var_and_hy, output_names=["incomp_innov_var", "H"])
 generate_px4_function(compute_ev_body_vel_var_and_hz, output_names=["incomp_innov_var", "H"])

diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_ev_body_vel_var_and_hx.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_ev_body_vel_var_and_hx.h
@@ -21,13 +21,13 @@ namespace sym {
  *
  * Outputs:
  *     incomp_innov_var: Matrix31
- *     H: Matrix1_23
+ *     H: Matrix23_1
  */
 template <typename Scalar>
 void ComputeEvBodyVelVarAndHx(const matrix::Matrix<Scalar, 24, 1>& state,
                               const matrix::Matrix<Scalar, 23, 23>& P,
                               matrix::Matrix<Scalar, 3, 1>* const incomp_innov_var = nullptr,
-                              matrix::Matrix<Scalar, 1, 23>* const H = nullptr) {
+                              matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
   // Total ops: 88
 
   // Input arrays
@@ -70,13 +70,13 @@ void ComputeEvBodyVelVarAndHx(const matrix::Matrix<Scalar, 24, 1>& state,
   }
 
   if (H != nullptr) {
-    matrix::Matrix<Scalar, 1, 23>& _h = (*H);
+    matrix::Matrix<Scalar, 23, 1>& _h = (*H);
 
     _h.setZero();
 
-    _h(0, 3) = _tmp2;
-    _h(0, 4) = _tmp10;
-    _h(0, 5) = _tmp6;
+    _h(3, 0) = _tmp2;
+    _h(4, 0) = _tmp10;
+    _h(5, 0) = _tmp6;
   }
 }  // NOLINT(readability/fn_size)
 

diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_ev_body_vel_var_and_hy.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_ev_body_vel_var_and_hy.h
@@ -21,13 +21,13 @@ namespace sym {
  *
  * Outputs:
  *     incomp_innov_var: Scalar
- *     H: Matrix1_23
+ *     H: Matrix23_1
  */
 template <typename Scalar>
 void ComputeEvBodyVelVarAndHy(const matrix::Matrix<Scalar, 24, 1>& state,
                               const matrix::Matrix<Scalar, 23, 23>& P,
                               Scalar* const incomp_innov_var = nullptr,
-                              matrix::Matrix<Scalar, 1, 23>* const H = nullptr) {
+                              matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
   // Total ops: 34
 
   // Input arrays
@@ -50,13 +50,13 @@ void ComputeEvBodyVelVarAndHy(const matrix::Matrix<Scalar, 24, 1>& state,
   }
 
   if (H != nullptr) {
-    matrix::Matrix<Scalar, 1, 23>& _h = (*H);
+    matrix::Matrix<Scalar, 23, 1>& _h = (*H);
 
     _h.setZero();
 
-    _h(0, 3) = _tmp4;
-    _h(0, 4) = _tmp0;
-    _h(0, 5) = _tmp3;
+    _h(3, 0) = _tmp4;
+    _h(4, 0) = _tmp0;
+    _h(5, 0) = _tmp3;
   }
 }  // NOLINT(readability/fn_size)
 

diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_ev_body_vel_var_and_hz.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_ev_body_vel_var_and_hz.h
@@ -21,13 +21,13 @@ namespace sym {
  *
  * Outputs:
  *     incomp_innov_var: Scalar
- *     H: Matrix1_23
+ *     H: Matrix23_1
  */
 template <typename Scalar>
 void ComputeEvBodyVelVarAndHz(const matrix::Matrix<Scalar, 24, 1>& state,
                               const matrix::Matrix<Scalar, 23, 23>& P,
                               Scalar* const incomp_innov_var = nullptr,
-                              matrix::Matrix<Scalar, 1, 23>* const H = nullptr) {
+                              matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
   // Total ops: 34
 
   // Input arrays
@@ -50,13 +50,13 @@ void ComputeEvBodyVelVarAndHz(const matrix::Matrix<Scalar, 24, 1>& state,
   }
 
   if (H != nullptr) {
-    matrix::Matrix<Scalar, 1, 23>& _h = (*H);
+    matrix::Matrix<Scalar, 23, 1>& _h = (*H);
 
     _h.setZero();
 
-    _h(0, 3) = _tmp2;
-    _h(0, 4) = _tmp3;
-    _h(0, 5) = _tmp4;
+    _h(3, 0) = _tmp2;
+    _h(4, 0) = _tmp3;
+    _h(5, 0) = _tmp4;
   }
 }  // NOLINT(readability/fn_size)
 

diff --git a/src/modules/ekf2/EKF/velocity_fusion.cpp b/src/modules/ekf2/EKF/velocity_fusion.cpp
@@ -84,17 +84,16 @@ void Ekf::updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector3f &ob
 
 void Ekf::fuseBodyVelocity(estimator_aid_source3d_s &aid_src)
 {
-	Vector3f innov;
-	Vector3f innov_var;
-	matrix::Matrix<float, 1, 23UL> H;
+	Vector3f body_vel_var;
+	VectorState H;
 	bool fused[3] = {false, false, false};
 	const auto state_vector = _state.vector();
 	Vector3f R(aid_src.observation_variance);
 
-	sym::ComputeEvBodyVelVarAndHx(state_vector, P, &innov_var, &H);
-	innov = _R_to_earth.transpose() * _state.vel - Vector3f(aid_src.observation);
+	sym::ComputeEvBodyVelVarAndHx(state_vector, P, &body_vel_var, &H);
+	const Vector3f innov = _R_to_earth.transpose() * _state.vel - Vector3f(aid_src.observation);
 	innov.copyTo(aid_src.innovation);
-	(innov_var + R).copyTo(aid_src.innovation_variance);
+	(body_vel_var + R).copyTo(aid_src.innovation_variance);
 	const float innov_gate = math::max(_params.ev_vel_innov_gate, 1.f);
 	setEstimatorAidStatusTestRatio(aid_src, innov_gate);
 
@@ -104,19 +103,19 @@ void Ekf::fuseBodyVelocity(estimator_aid_source3d_s &aid_src)
 
 	for (uint8_t index = 0; index <= 2; index++) {
 		if (index == 1) {
-			sym::ComputeEvBodyVelVarAndHy(state_vector, P, &(innov_var(index)), &H);
+			sym::ComputeEvBodyVelVarAndHy(state_vector, P, &body_vel_var(index), &H);
 
 		} else if (index == 2) {
-			sym::ComputeEvBodyVelVarAndHz(state_vector, P, &(innov_var(index)), &H);
+			sym::ComputeEvBodyVelVarAndHz(state_vector, P, &body_vel_var(index), &H);
 		}
 
 		Vector3f meas_pred = _R_to_earth.transpose() * _state.vel;
-		innov_var(index) += aid_src.observation_variance[index];
+		const float innov_var = body_vel_var(index) + aid_src.observation_variance[index];
 		aid_src.innovation[index] = meas_pred(index) - aid_src.observation[index];
 
-		VectorState Kfusion = P * H.transpose() / innov_var(index);
+		VectorState Kfusion = P * H / innov_var;
 
-		if (measurementUpdate(Kfusion, H.transpose(), aid_src.observation_variance[index], aid_src.innovation[index])) {
+		if (measurementUpdate(Kfusion, H, aid_src.observation_variance[index], aid_src.innovation[index])) {
 			fused[index] = true;
 		}
 	}