ekf2: apply astyle formatting and enforce

Tools/astyle/files_to_check_code_style.sh

@@ -18,7 +18,8 @@ exec find boards msg src platforms test \
     -path src/lib/events/libevents -prune -o \
     -path src/lib/parameters/uthash -prune -o \
     -path src/lib/wind_estimator/python/generated -prune -o \
-    -path src/modules/ekf2/EKF -prune -o \
+    -path src/modules/ekf2/EKF/python/ekf_derivation/generated -prune -o \
+    -path src/modules/ekf2/EKF/yaw_estimator/derivation/generated -prune -o \
     -path src/modules/gyro_fft/CMSIS_5 -prune -o \
     -path src/modules/mavlink/mavlink -prune -o \
     -path test/mavsdk_tests/catch2 -prune -o \

src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp

@@ -89,7 +89,8 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
 
 		updateAirspeed(airspeed_sample, _aid_src_airspeed);
 
-		_innov_check_fail_status.flags.reject_airspeed = _aid_src_airspeed.innovation_rejected; // TODO: remove this redundant flag
+		_innov_check_fail_status.flags.reject_airspeed =
+			_aid_src_airspeed.innovation_rejected; // TODO: remove this redundant flag
 
 		const bool continuing_conditions_passing = _control_status.flags.in_air
 				&& _control_status.flags.fixed_wing
@@ -155,12 +156,12 @@ void Ekf::updateAirspeed(const airspeedSample &airspeed_sample, estimator_aid_so
 					     &innov, &innov_var);
 
 	updateAidSourceStatus(aid_src,
-				 airspeed_sample.time_us,                 // sample timestamp
-				 airspeed_sample.true_airspeed,           // observation
-				 R,                                       // observation variance
-				 innov,                                   // innovation
-				 innov_var,                               // innovation variance
-				 math::max(_params.tas_innov_gate, 1.f)); // innovation gate
+			      airspeed_sample.time_us,                 // sample timestamp
+			      airspeed_sample.true_airspeed,           // observation
+			      R,                                       // observation variance
+			      innov,                                   // innovation
+			      innov_var,                               // innovation variance
+			      math::max(_params.tas_innov_gate, 1.f)); // innovation gate
 }
 
 void Ekf::fuseAirspeed(const airspeedSample &airspeed_sample, estimator_aid_source1d_s &aid_src)

src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp

@@ -85,12 +85,12 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed
 			const Vector2f pos_obs_var(pos_var, pos_var);
 
 			ekf.updateAidSourceStatus(aid_src,
-						     sample.time_us,                                    // sample timestamp
-						     position,                                          // observation
-						     pos_obs_var,                                       // observation variance
-						     Vector2f(ekf.state().pos) - position,              // innovation
-						     Vector2f(ekf.getPositionVariance()) + pos_obs_var, // innovation variance
-						     math::max(_param_ekf2_agp_gate.get(), 1.f));       // innovation gate
+						  sample.time_us,                                    // sample timestamp
+						  position,                                          // observation
+						  pos_obs_var,                                       // observation variance
+						  Vector2f(ekf.state().pos) - position,              // innovation
+						  Vector2f(ekf.getPositionVariance()) + pos_obs_var, // innovation variance
+						  math::max(_param_ekf2_agp_gate.get(), 1.f));       // innovation gate
 		}
 
 		const bool starting_conditions = PX4_ISFINITE(sample.latitude) && PX4_ISFINITE(sample.longitude)

src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.hpp

@@ -96,8 +96,8 @@ class AuxGlobalPosition : public ModuleParams
 	uint64_t _time_last_buffer_push{0};
 
 	enum class Ctrl : uint8_t {
-		HPOS  = (1<<0),
-		VPOS  = (1<<1)
+		HPOS  = (1 << 0),
+		VPOS  = (1 << 1)
 	};
 
 	enum class State {

src/modules/ekf2/EKF/aid_sources/auxvel/auxvel_fusion.cpp

@@ -41,12 +41,12 @@ void Ekf::controlAuxVelFusion()
 		if (_auxvel_buffer->pop_first_older_than(_time_delayed_us, &sample)) {
 
 			updateAidSourceStatus(_aid_src_aux_vel,
-						 sample.time_us,                                           // sample timestamp
-						 sample.vel,                                               // observation
-						 sample.velVar,                                            // observation variance
-						 Vector2f(_state.vel.xy()) - sample.vel,                   // innovation
-						 Vector2f(getStateVariance<State::vel>()) + sample.velVar, // innovation variance
-						 math::max(_params.auxvel_gate, 1.f));                     // innovation gate
+					      sample.time_us,                                           // sample timestamp
+					      sample.vel,                                               // observation
+					      sample.velVar,                                            // observation variance
+					      Vector2f(_state.vel.xy()) - sample.vel,                   // innovation
+					      Vector2f(getStateVariance<State::vel>()) + sample.velVar, // innovation variance
+					      math::max(_params.auxvel_gate, 1.f));                     // innovation gate
 
 			if (isHorizontalAidingActive()) {
 				fuseHorizontalVelocity(_aid_src_aux_vel);

src/modules/ekf2/EKF/aid_sources/drag/drag_fusion.cpp

@@ -169,12 +169,12 @@ void Ekf::fuseDrag(const dragSample &drag_sample)
 	}
 
 	updateAidSourceStatus(_aid_src_drag,
-				 drag_sample.time_us,  // sample timestamp
-				 observation,          // observation
-				 observation_variance, // observation variance
-				 innovation,           // innovation
-				 innovation_variance,  // innovation variance
-				 innov_gate);          // innovation gate
+			      drag_sample.time_us,  // sample timestamp
+			      observation,          // observation
+			      observation_variance, // observation variance
+			      innovation,           // innovation
+			      innovation_variance,  // innovation variance
+			      innov_gate);          // innovation gate
 
 	if (fused[0] && fused[1]) {
 		_aid_src_drag.fused = true;

src/modules/ekf2/EKF/aid_sources/external_vision/ev_control.cpp

@@ -55,8 +55,10 @@ void Ekf::controlExternalVisionFusion()
 
 		const bool starting_conditions_passing = quality_sufficient
 				&& ((ev_sample.time_us - _ev_sample_prev.time_us) < EV_MAX_INTERVAL)
-				&& ((_params.ev_quality_minimum <= 0) || (_ev_sample_prev.quality >= _params.ev_quality_minimum)) // previous quality sufficient
-				&& ((_params.ev_quality_minimum <= 0) || (_ext_vision_buffer->get_newest().quality >= _params.ev_quality_minimum)) // newest quality sufficient
+				&& ((_params.ev_quality_minimum <= 0)
+				    || (_ev_sample_prev.quality >= _params.ev_quality_minimum)) // previous quality sufficient
+				&& ((_params.ev_quality_minimum <= 0)
+				    || (_ext_vision_buffer->get_newest().quality >= _params.ev_quality_minimum)) // newest quality sufficient
 				&& isNewestSampleRecent(_time_last_ext_vision_buffer_push, EV_MAX_INTERVAL);
 
 		updateEvAttitudeErrorFilter(ev_sample, ev_reset);

src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp

@@ -77,10 +77,12 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool com
 	float measurement_var = math::max(pos_cov(2, 2), sq(_params.ev_pos_noise), sq(0.01f));
 
 #if defined(CONFIG_EKF2_GNSS)
+
 	// increase minimum variance if GPS active
 	if (_control_status.flags.gps_hgt) {
 		measurement_var = math::max(measurement_var, sq(_params.gps_pos_noise));
 	}
+
 #endif // CONFIG_EKF2_GNSS
 
 	const bool measurement_valid = PX4_ISFINITE(measurement) && PX4_ISFINITE(measurement_var);

src/modules/ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp

@@ -161,12 +161,12 @@ void Ekf::controlEvPosFusion(const extVisionSample &ev_sample, const bool common
 	const Vector2f pos_obs_var = measurement_var + _ev_pos_b_est.getBiasVar();
 
 	updateAidSourceStatus(aid_src,
-				 ev_sample.time_us,                                      // sample timestamp
-				 position,                                               // observation
-				 pos_obs_var,                                            // observation variance
-				 Vector2f(_state.pos) - position,                        // innovation
-				 Vector2f(getStateVariance<State::pos>()) + pos_obs_var, // innovation variance
-				 math::max(_params.ev_pos_innov_gate, 1.f));             // innovation gate
+			      ev_sample.time_us,                                      // sample timestamp
+			      position,                                               // observation
+			      pos_obs_var,                                            // observation variance
+			      Vector2f(_state.pos) - position,                        // innovation
+			      Vector2f(getStateVariance<State::pos>()) + pos_obs_var, // innovation variance
+			      math::max(_params.ev_pos_innov_gate, 1.f));             // innovation gate
 
 	// update the bias estimator before updating the main filter but after
 	// using its current state to compute the vertical position innovation
@@ -205,7 +205,8 @@ void Ekf::controlEvPosFusion(const extVisionSample &ev_sample, const bool common
 	}
 }
 
-void Ekf::startEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, estimator_aid_source2d_s &aid_src)
+void Ekf::startEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var,
+			   estimator_aid_source2d_s &aid_src)
 {
 	// activate fusion
 	// TODO:  (_params.position_sensor_ref == PositionSensor::EV)
@@ -229,7 +230,8 @@ void Ekf::startEvPosFusion(const Vector2f &measurement, const Vector2f &measurem
 	_control_status.flags.ev_pos = true;
 }
 
-void Ekf::updateEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, bool quality_sufficient, bool reset, estimator_aid_source2d_s &aid_src)
+void Ekf::updateEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, bool quality_sufficient,
+			    bool reset, estimator_aid_source2d_s &aid_src)
 {
 	if (reset) {
 

src/modules/ekf2/EKF/aid_sources/external_vision/ev_yaw_control.cpp

@@ -53,12 +53,12 @@ void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common
 	computeYawInnovVarAndH(obs_var, innov_var, H_YAW);
 
 	updateAidSourceStatus(aid_src,
-		ev_sample.time_us,                           // sample timestamp
-		obs,                                         // observation
-		obs_var,                                     // observation variance
-		innov,                                       // innovation
-		innov_var,                                   // innovation variance
-		math::max(_params.heading_innov_gate, 1.f)); // innovation gate
+			      ev_sample.time_us,                           // sample timestamp
+			      obs,                                         // observation
+			      obs_var,                                     // observation variance
+			      innov,                                       // innovation
+			      innov_var,                                   // innovation variance
+			      math::max(_params.heading_innov_gate, 1.f)); // innovation gate
 
 	if (ev_reset) {
 		_control_status.flags.ev_yaw_fault = false;
@@ -191,9 +191,11 @@ void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common
 void Ekf::stopEvYawFusion()
 {
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
+
 	if (_control_status.flags.ev_yaw) {
 
 		_control_status.flags.ev_yaw = false;
 	}
+
 #endif // CONFIG_EKF2_EXTERNAL_VISION
 }

src/modules/ekf2/EKF/aid_sources/fake_pos_control.cpp

@@ -68,17 +68,17 @@ void Ekf::controlFakePosFusion()
 		const float innov_gate = 3.f;
 
 		updateAidSourceStatus(aid_src,
-					 _time_delayed_us,
-					 position,                                           // observation
-					 obs_var,                                            // observation variance
-					 Vector2f(_state.pos) - position,                    // innovation
-					 Vector2f(getStateVariance<State::pos>()) + obs_var, // innovation variance
-					 innov_gate);                                        // innovation gate
+				      _time_delayed_us,
+				      position,                                           // observation
+				      obs_var,                                            // observation variance
+				      Vector2f(_state.pos) - position,                    // innovation
+				      Vector2f(getStateVariance<State::pos>()) + obs_var, // innovation variance
+				      innov_gate);                                        // innovation gate
 
 		const bool enable_conditions_passing = !isHorizontalAidingActive()
-				&& ((getTiltVariance() > sq(math::radians(3.f))) || _control_status.flags.vehicle_at_rest)
-				&& (!(_params.imu_ctrl & static_cast<int32_t>(ImuCtrl::GravityVector)) || _control_status.flags.vehicle_at_rest)
-				&& _horizontal_deadreckon_time_exceeded;
+						       && ((getTiltVariance() > sq(math::radians(3.f))) || _control_status.flags.vehicle_at_rest)
+						       && (!(_params.imu_ctrl & static_cast<int32_t>(ImuCtrl::GravityVector)) || _control_status.flags.vehicle_at_rest)
+						       && _horizontal_deadreckon_time_exceeded;
 
 		if (_control_status.flags.fake_pos) {
 			if (enable_conditions_passing) {

src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp

@@ -126,6 +126,7 @@ void Ekf::collect_gps(const gnssSample &gps)
 					_wmm_gps_time_last_set = _time_delayed_us;
 				}
 			}
+
 #endif // CONFIG_EKF2_MAGNETOMETER
 
 			_earth_rate_NED = calcEarthRateNED((float)math::radians(gps.lat));
@@ -157,7 +158,8 @@ bool Ekf::runGnssChecks(const gnssSample &gps)
 
 	// Calculate time lapsed since last update, limit to prevent numerical errors and calculate a lowpass filter coefficient
 	constexpr float filt_time_const = 10.0f;
-	const float dt = math::constrain(float(int64_t(gps.time_us) - int64_t(_gps_pos_prev.getProjectionReferenceTimestamp())) * 1e-6f, 0.001f, filt_time_const);
+	const float dt = math::constrain(float(int64_t(gps.time_us) - int64_t(_gps_pos_prev.getProjectionReferenceTimestamp()))
+					 * 1e-6f, 0.001f, filt_time_const);
 	const float filter_coef = dt / filt_time_const;
 
 	// The following checks are only valid when the vehicle is at rest

src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp

@@ -190,12 +190,12 @@ void Ekf::updateGnssVel(const gnssSample &gnss_sample, estimator_aid_source3d_s
 	const float innovation_gate = math::max(_params.gps_vel_innov_gate, 1.f);
 
 	updateAidSourceStatus(aid_src,
-				 gnss_sample.time_us,                  // sample timestamp
-				 velocity,                             // observation
-				 vel_obs_var,                          // observation variance
-				 _state.vel - velocity,                // innovation
-				 getVelocityVariance() + vel_obs_var,  // innovation variance
-				 innovation_gate);                     // innovation gate
+			      gnss_sample.time_us,                  // sample timestamp
+			      velocity,                             // observation
+			      vel_obs_var,                          // observation variance
+			      _state.vel - velocity,                // innovation
+			      getVelocityVariance() + vel_obs_var,  // innovation variance
+			      innovation_gate);                     // innovation gate
 
 	// vz special case if there is bad vertical acceleration data, then don't reject measurement if GNSS reports velocity accuracy is acceptable,
 	// but limit innovation to prevent spikes that could destabilise the filter
@@ -228,12 +228,12 @@ void Ekf::updateGnssPos(const gnssSample &gnss_sample, estimator_aid_source2d_s
 	const Vector2f pos_obs_var(pos_var, pos_var);
 
 	updateAidSourceStatus(aid_src,
-				 gnss_sample.time_us,                                    // sample timestamp
-				 position,                                               // observation
-				 pos_obs_var,                                            // observation variance
-				 Vector2f(_state.pos) - position,                        // innovation
-				 Vector2f(getStateVariance<State::pos>()) + pos_obs_var, // innovation variance
-				 math::max(_params.gps_pos_innov_gate, 1.f));            // innovation gate
+			      gnss_sample.time_us,                                    // sample timestamp
+			      position,                                               // observation
+			      pos_obs_var,                                            // observation variance
+			      Vector2f(_state.pos) - position,                        // innovation
+			      Vector2f(getStateVariance<State::pos>()) + pos_obs_var, // innovation variance
+			      math::max(_params.gps_pos_innov_gate, 1.f));            // innovation gate
 }
 
 void Ekf::controlGnssYawEstimator(estimator_aid_source3d_s &aid_src_vel)

src/modules/ekf2/EKF/aid_sources/gnss/gps_yaw_fusion.cpp

@@ -62,12 +62,12 @@ void Ekf::updateGpsYaw(const gnssSample &gps_sample)
 					    &heading_pred, &heading_innov_var, &H);
 
 	updateAidSourceStatus(_aid_src_gnss_yaw,
-				 gps_sample.time_us,                          // sample timestamp
-				 measured_hdg,                                // observation
-				 R_YAW,                                       // observation variance
-				 wrap_pi(heading_pred - measured_hdg),        // innovation
-				 heading_innov_var,                           // innovation variance
-				 math::max(_params.heading_innov_gate, 1.f)); // innovation gate
+			      gps_sample.time_us,                          // sample timestamp
+			      measured_hdg,                                // observation
+			      R_YAW,                                       // observation variance
+			      wrap_pi(heading_pred - measured_hdg),        // innovation
+			      heading_innov_var,                           // innovation variance
+			      math::max(_params.heading_innov_gate, 1.f)); // innovation gate
 }
 
 void Ekf::fuseGpsYaw(float antenna_yaw_offset)