adjustments for wind fusion

only allow VEHICLE_CMD_EXTERNAL_WIND_ESTIMATE during wind dead-reckoning
and increase horizontal velocity variance to allow velocity states to move towards solution that is aligned with the newly set wind Signed-off-by: RomanBapst <bapstroman@gmail.com>
2026-06-03 06:40:04 +08:00 · 2024-07-11 09:44:19 +02:00 · 2024-07-03 18:36:44 +02:00 · 2024-07-03 18:36:44 +02:00 · 2024-07-03 18:36:44 +02:00 · 2024-07-03 18:36:44 +02:00
121 changed files with 3319 additions and 3053 deletions
@@ -157,6 +157,7 @@ param set-default CBRK_SUPPLY_CHK 894281

 # disable check, no CPU load reported on posix yet
 param set-default COM_CPU_MAX -1
+param set-default COM_RAM_MAX -1

 # Don't require RC calibration and configuration
 param set-default COM_RC_IN_MODE 1
@@ -36,7 +36,6 @@ param set-default COM_RC_LOSS_T 3


 # ekf2
-param set-default EKF2_TERR_MASK 1
 param set-default EKF2_BARO_NOISE 2

 param set-default EKF2_BCOEF_X 31.5
@@ -25,7 +25,6 @@
 param set-default BAT1_CAPACITY 4000
 param set-default BAT1_N_CELLS 6
 param set-default BAT1_V_EMPTY 3.3
-param set-default BAT1_V_LOAD_DROP 0.5
 param set-default BAT_AVRG_CURRENT 13

 # Square quadrotor X PX4 numbering
@@ -217,25 +217,31 @@ else
 	fi
 	unset BOARD_RC_DEFAULTS

-	#
-	# Set parameters and env variables for selected SYS_AUTOSTART.
-	#
-	set AUTOSTART_PATH etc/init.d/rc.autostart
+	# Load airframe configuration based on SYS_AUTOSTART parameter
 	if ! param compare SYS_AUTOSTART 0
 	then
-		if param greater SYS_AUTOSTART 1000000
+		# rc.autostart directly run the right airframe script which sets the VEHICLE_TYPE
+		# Look for airframe in ROMFS
+		. ${R}etc/init.d/rc.autostart
+
+		if [ ${VEHICLE_TYPE} == none ]
 		then
-			# Use external startup file
+			# Look for airframe on SD card
 			if [ $SDCARD_AVAILABLE = yes ]
 			then
-				set AUTOSTART_PATH etc/init.d/rc.autostart_ext
+				. ${R}etc/init.d/rc.autostart_ext
 			else
-				echo "ERROR [init] SD card not mounted - trying to load airframe from ROMFS"
+				echo "ERROR [init] SD card not mounted - can't load external airframe"
 			fi
 		fi
-		. ${R}$AUTOSTART_PATH
+
+		if [ ${VEHICLE_TYPE} == none ]
+		then
+			echo "ERROR [init] No airframe file found for SYS_AUTOSTART value"
+			param set SYS_AUTOSTART 0
+			tune_control play error
+		fi
 	fi
-	unset AUTOSTART_PATH

 	# Check parameter version and reset upon airframe configuration version mismatch.
 	# Reboot required because "param reset_all" would reset all "param set" lines from airframe.
@@ -223,7 +223,7 @@ def create_pdf_report(ulog: ULog, multi_instance: int, output_plot_filename: str
        data_plot.save()
        data_plot.close()

-        # plot innovation_check_flags summary
+        # plot innovation flags summary
        data_plot = CheckFlagsPlot(
            status_flags_time, estimator_status_flags, [['reject_hor_vel', 'reject_hor_pos'], ['reject_ver_vel', 'reject_ver_pos',
                                                                               'reject_hagl'],
@@ -79,12 +79,6 @@ class RCOutput():
        result += "then\n"
        result += "\techo \"Loading airframe: /etc/init.d/airframes/${AIRFRAME}\"\n"
        result += "\t. /etc/init.d/airframes/${AIRFRAME}\n"
-        if not post_start:
-            result += "else\n"
-            result += "\techo \"ERROR [init] No file matches SYS_AUTOSTART value found in : /etc/init.d/airframes\"\n"
-            # Reset the configuration
-            result += "\tparam set SYS_AUTOSTART 0\n"
-            result += "\ttone_alarm ${TUNE_ERR}\n"
        result += "fi\n"
        result += "unset AIRFRAME"
        self.output = result
@@ -7,7 +7,7 @@ jinja2>=2.8
 jsonschema
 kconfiglib
 lxml
-matplotlib>=3.0.*
+matplotlib>=3.0
 numpy>=1.13
 nunavut>=1.1.0
 packaging
@@ -10,6 +10,7 @@ CONFIG_DRIVERS_BAROMETER_BMP388=y
 CONFIG_DRIVERS_CDCACM_AUTOSTART=y
 CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
 CONFIG_DRIVERS_DISTANCE_SENSOR_BROADCOM_AFBRS50=y
+CONFIG_DRIVERS_DISTANCE_SENSOR_VL53L0X=y
 CONFIG_DRIVERS_DSHOT=y
 CONFIG_DRIVERS_GPS=y
 CONFIG_DRIVERS_HEATER=y
@@ -1148,7 +1148,6 @@ handle_message_hil_sensor_dsp(mavlink_message_t *msg)

 		hil_battery_status.timestamp = gyro_accel_time;
 		hil_battery_status.voltage_v = 16.0f;
-		hil_battery_status.voltage_filtered_v = 16.0f;
 		hil_battery_status.current_a = 10.0f;
 		hil_battery_status.discharged_mah = -1.0f;
 		hil_battery_status.connected = true;
@@ -55,7 +55,7 @@ CONFIG_MODULES_FW_POS_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
-CONFIG_MODULES_GYRO_FFT=y
+CONFIG_MODULES_GYRO_FFT=n
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
@@ -154,7 +154,6 @@
 *(.text._ZN3Ekf20updateIMUBiasInhibitERKN9estimator9imuSampleE)
 *(.text._ZN9Commander13dataLinkCheckEv)
 *(.text._ZN17FlightModeManager10switchTaskE15FlightTaskIndex)
-*(.text._ZNK3Ekf26get_innovation_test_statusERtRfS1_S1_S1_S1_S1_S1_)
 *(.text._ZN12PX4Gyroscope9set_scaleEf)
 *(.text._ZN12FailsafeBase6updateERKyRKNS_5StateEbbRK16failsafe_flags_s)
 *(.text._ZN18MavlinkStreamDebug4sendEv)
@@ -1,9 +1,7 @@
 uint64 timestamp			# time since system start (microseconds)
 bool connected				# Whether or not a battery is connected, based on a voltage threshold
 float32 voltage_v			# Battery voltage in volts, 0 if unknown
-float32 voltage_filtered_v	# Battery voltage in volts, filtered, 0 if unknown
 float32 current_a			# Battery current in amperes, -1 if unknown
-float32 current_filtered_a	# Battery current in amperes, filtered, 0 if unknown
 float32 current_average_a	# Battery current average in amperes (for FW average in level flight), -1 if unknown
 float32 discharged_mah		# Discharged amount in mAh, -1 if unknown
 float32 remaining			# From 1 to 0, -1 if unknown
@@ -76,3 +74,11 @@ float32 design_capacity             # The design capacity of the battery
 uint16 average_time_to_full         # The predicted remaining time until the battery reaches full charge, in minutes
 uint16 over_discharge_count         # Number of battery overdischarge
 float32 nominal_voltage             # Nominal voltage of the battery pack
+
+float32 internal_resistance_estimate            # [Ohm] Internal resistance per cell estimate
+float32 ocv_estimate                            # [V] Open circuit voltage estimate
+float32 ocv_estimate_filtered			# [V] Filtered open circuit voltage estimate
+float32 volt_based_soc_estimate			# [0, 1] Normalized volt based state of charge estimate
+float32 voltage_prediction			# [V] Predicted voltage
+float32 prediction_error                        # [V] Prediction error
+float32 estimation_covariance_norm		# Norm of the covariance matrix
@@ -25,4 +25,3 @@ bool fused                   # true if the sample was successfully fused
 # TOPICS estimator_aid_src_airspeed estimator_aid_src_sideslip
 # TOPICS estimator_aid_src_fake_hgt
 # TOPICS estimator_aid_src_gnss_yaw estimator_aid_src_ev_yaw
-# TOPICS estimator_aid_src_terrain_range_finder
@@ -22,5 +22,5 @@ bool innovation_rejected        # true if the observation has been rejected
 bool fused                      # true if the sample was successfully fused

 # TOPICS estimator_aid_src_ev_pos estimator_aid_src_fake_pos estimator_aid_src_gnss_pos estimator_aid_src_aux_global_position
-# TOPICS estimator_aid_src_aux_vel estimator_aid_src_optical_flow estimator_aid_src_terrain_optical_flow
+# TOPICS estimator_aid_src_aux_vel estimator_aid_src_optical_flow
 # TOPICS estimator_aid_src_drag
@@ -9,4 +9,4 @@ float32 innov			# innovation of the last measurement fusion (m)
 float32 innov_var		# innovation variance of the last measurement fusion (m^2)
 float32 innov_test_ratio	# normalized innovation squared test ratio

-# TOPICS estimator_baro_bias estimator_gnss_hgt_bias estimator_rng_hgt_bias
+# TOPICS estimator_baro_bias estimator_gnss_hgt_bias
@@ -22,7 +22,6 @@ float32[2] aux_hvel	# horizontal auxiliary velocity innovation from landing targ

 # Optical flow
 float32[2] flow		# flow innvoation (rad/sec) and innovation variance ((rad/sec)**2)
-float32[2] terr_flow	# flow innvoation (rad/sec) and innovation variance computed by the terrain estimator ((rad/sec)**2)

 # Various
 float32 heading		# heading innovation (rad) and innovation variance (rad**2)
@@ -1,7 +1,7 @@
 uint64 timestamp		# time since system start (microseconds)
 uint64 timestamp_sample         # the timestamp of the raw data (microseconds)

-float32[24] states		# Internal filter states
+float32[25] states		# Internal filter states
 uint8 n_states		# Number of states effectively used

-float32[23] covariances	# Diagonal Elements of Covariance Matrix
+float32[24] covariances	# Diagonal Elements of Covariance Matrix
@@ -69,27 +69,14 @@ uint32 filter_fault_flags	# Bitmask to indicate EKF internal faults

 float32 pos_horiz_accuracy # 1-Sigma estimated horizontal position accuracy relative to the estimators origin (m)
 float32 pos_vert_accuracy # 1-Sigma estimated vertical position accuracy relative to the estimators origin (m)
-uint16 innovation_check_flags # Bitmask to indicate pass/fail status of innovation consistency checks
-# 0 - true if velocity observations have been rejected
-# 1 - true if horizontal position observations have been rejected
-# 2 - true if true if vertical position observations have been rejected
-# 3 - true if the X magnetometer observation has been rejected
-# 4 - true if the Y magnetometer observation has been rejected
-# 5 - true if the Z magnetometer observation has been rejected
-# 6 - true if the yaw observation has been rejected
-# 7 - true if the airspeed observation has been rejected
-# 8 - true if the synthetic sideslip observation has been rejected
-# 9 - true if the height above ground observation has been rejected
-# 10 - true if the X optical flow observation has been rejected
-# 11 - true if the Y optical flow observation has been rejected

-float32 mag_test_ratio # ratio of the largest magnetometer innovation component to the innovation test limit
-float32 vel_test_ratio # ratio of the largest velocity innovation component to the innovation test limit
-float32 pos_test_ratio # ratio of the largest horizontal position innovation component to the innovation test limit
-float32 hgt_test_ratio # ratio of the vertical position innovation to the innovation test limit
-float32 tas_test_ratio # ratio of the true airspeed innovation to the innovation test limit
-float32 hagl_test_ratio # ratio of the height above ground innovation to the innovation test limit
-float32 beta_test_ratio # ratio of the synthetic sideslip innovation to the innovation test limit
+float32 mag_test_ratio # low-pass filtered ratio of the largest magnetometer innovation component to the innovation test limit
+float32 vel_test_ratio # low-pass filtered ratio of the largest velocity innovation component to the innovation test limit
+float32 pos_test_ratio # low-pass filtered ratio of the largest horizontal position innovation component to the innovation test limit
+float32 hgt_test_ratio # low-pass filtered ratio of the vertical position innovation to the innovation test limit
+float32 tas_test_ratio # low-pass filtered ratio of the true airspeed innovation to the innovation test limit
+float32 hagl_test_ratio # low-pass filtered ratio of the height above ground innovation to the innovation test limit
+float32 beta_test_ratio # low-pass filtered ratio of the synthetic sideslip innovation to the innovation test limit

 uint16 solution_status_flags # Bitmask indicating which filter kinematic state outputs are valid for flight control use.
 # 0 - True if the attitude estimate is good
@@ -102,6 +102,7 @@ uint16 VEHICLE_CMD_FIXED_MAG_CAL_YAW = 42006            # Magnetometer calibrati
 uint16 VEHICLE_CMD_DO_WINCH = 42600			# Command to operate winch.

 uint16 VEHICLE_CMD_EXTERNAL_POSITION_ESTIMATE = 43003 # external reset of estimator global position when deadreckoning
+uint16 VEHICLE_CMD_EXTERNAL_WIND_ESTIMATE = 43004

 # PX4 vehicle commands (beyond 16 bit mavlink commands)
 uint32 VEHICLE_CMD_PX4_INTERNAL_START    = 65537        # start of PX4 internal only vehicle commands (> UINT16_MAX)
@@ -116,13 +116,11 @@ void BATT_SMBUS::RunImpl()

 	// Convert millivolts to volts.
 	new_report.voltage_v = ((float)result) / 1000.0f;
-	new_report.voltage_filtered_v = new_report.voltage_v;

 	// Read current.
 	ret |= _interface->read_word(BATT_SMBUS_CURRENT, result);

 	new_report.current_a = (-1.0f * ((float)(*(int16_t *)&result)) / 1000.0f) * _c_mult;
-	new_report.current_filtered_a = new_report.current_a;

 	// Read average current.
 	ret |= _interface->read_word(BATT_SMBUS_AVERAGE_CURRENT, result);
@@ -76,8 +76,6 @@ public:

 		battery_status_s bat_status {0};
 		bat_status.timestamp = hrt_absolute_time();
-		bat_status.voltage_filtered_v = bat_info.voltage;
-		bat_status.current_filtered_a = bat_info.current;
 		bat_status.current_average_a = bat_info.average_power_10sec;
 		bat_status.remaining = bat_info.state_of_charge_pct / 100.0f;
 		bat_status.scale = -1;
@@ -56,7 +56,7 @@
 #endif

 #ifdef SEP_LOG_ERROR
-#define SEP_ERR(...)            {PX4_WARN(__VA_ARGS__);}
+#define SEP_ERR(...)            {PX4_ERR(__VA_ARGS__);}
 #else
 #define SEP_ERR(...)            {}
 #endif
@@ -71,7 +71,7 @@ parameters:
        type: float
        decimal: 3
        default: 0
-        min: 0
+        min: -360
        max: 360
        unit: deg
        reboot_required: true
@@ -104,7 +104,8 @@ parameters:
        description:
          short: Log GPS communication data
          long: |
-            Dump raw communication data from and to the connected receiver to the log file.
+            Log raw communication between the driver and connected receivers.
+            For example, "To receiver" will log all commands and corrections sent by the driver to the receiver.
        type: enum
        default: 0
        min: 0
@@ -60,8 +60,8 @@ constexpr uint32_t k_dnu_u4_value {4294967295};
 constexpr uint32_t k_dnu_u4_bitfield {0};
 constexpr uint16_t k_dnu_u2_value {65535};
 constexpr uint16_t k_dnu_u2_bitfield {0};
-constexpr float k_dnu_f4_value {-2 * 10000000000};
-constexpr double k_dnu_f8_value {-2 * 10000000000};
+constexpr float k_dnu_f4_value {-2.0f * 10000000000.0f};
+constexpr double k_dnu_f8_value {-2.0 * 10000000000.0};

 /// Maximum size of all expected messages.
 /// This needs to be bigger than the maximum size of all declared SBF blocks so that `memcpy()` can safely copy from the decoding buffer using this value into messages.
@@ -55,6 +55,7 @@
 #include <px4_platform_common/defines.h>
 #include <px4_platform_common/log.h>
 #include <px4_platform_common/time.h>
+#include <lib/systemlib/mavlink_log.h>
 #include <uORB/topics/gps_inject_data.h>
 #include <uORB/topics/sensor_gps.h>

@@ -86,6 +87,11 @@ constexpr int k_max_receiver_read_timeout = 50;
 */
 constexpr size_t k_min_receiver_read_bytes = 32;

+/**
+ * The baud rate of Septentrio receivers with factory default configuration.
+*/
+constexpr uint32_t k_septentrio_receiver_default_baud_rate = 115200;
+
 constexpr uint8_t k_max_command_size            = 120;
 constexpr uint16_t k_timeout_5hz                = 500;
 constexpr uint32_t k_read_buffer_size           = 150;
@@ -134,18 +140,19 @@ constexpr const char *k_frequency_25_0hz = "msec40";
 constexpr const char *k_frequency_50_0hz = "msec20";

 px4::atomic<SeptentrioDriver *> SeptentrioDriver::_secondary_instance {nullptr};
+uint32_t SeptentrioDriver::k_supported_baud_rates[] {0, 38400, 57600, 115200, 230400, 460800, 500000, 576000, 921600, 1000000, 1500000};
+uint32_t SeptentrioDriver::k_default_baud_rate {230400};
+orb_advert_t SeptentrioDriver::k_mavlink_log_pub {nullptr};

 SeptentrioDriver::SeptentrioDriver(const char *device_path, Instance instance, uint32_t baud_rate) :
 	Device(MODULE_NAME),
 	_instance(instance),
-	_baud_rate(baud_rate)
+	_chosen_baud_rate(baud_rate)
 {
 	strncpy(_port, device_path, sizeof(_port) - 1);
 	// Enforce null termination.
 	_port[sizeof(_port) - 1] = '\0';

-	reset_gps_state_message();
-
 	int32_t enable_sat_info {0};
 	get_parameter("SEP_SAT_INFO", &enable_sat_info);

@@ -171,6 +178,10 @@ SeptentrioDriver::SeptentrioDriver(const char *device_path, Instance instance, u
 	get_parameter("SEP_STREAM_LOG", &receiver_stream_log);
 	_receiver_stream_log = receiver_stream_log;

+	if (_receiver_stream_log == _receiver_stream_main) {
+		mavlink_log_warning(&k_mavlink_log_pub, "Septentrio: Logging stream should be different from main stream");
+	}
+
 	int32_t automatic_configuration {true};
 	get_parameter("SEP_AUTO_CONFIG", &automatic_configuration);
 	_automatic_configuration = static_cast<bool>(automatic_configuration);
@@ -198,6 +209,8 @@ SeptentrioDriver::SeptentrioDriver(const char *device_path, Instance instance, u
 	}

 	set_device_type(DRV_GPS_DEVTYPE_SBF);
+
+	reset_gps_state_message();
 }

 SeptentrioDriver::~SeptentrioDriver()
@@ -240,15 +253,13 @@ int SeptentrioDriver::print_status()
 		break;
 	}

-	PX4_INFO("health: %s, port: %s, baud rate: %lu", is_healthy() ? "OK" : "NOT OK", _port, _baud_rate);
+	PX4_INFO("health: %s, port: %s, baud rate: %lu", is_healthy() ? "OK" : "NOT OK", _port, _uart.getBaudrate());
 	PX4_INFO("controller -> receiver data rate: %lu B/s", output_data_rate());
 	PX4_INFO("receiver -> controller data rate: %lu B/s", input_data_rate());
 	PX4_INFO("sat info: %s", (_message_satellite_info != nullptr) ? "enabled" : "disabled");

-	if (_message_gps_state.timestamp != 0) {
-
+	if (first_gps_uorb_message_created() && _state == State::ReceivingData) {
 		PX4_INFO("rate RTCM injection: %6.2f Hz", static_cast<double>(rtcm_injection_frequency()));
-
 		print_message(ORB_ID(sensor_gps), _message_gps_state);
 	}

@@ -267,7 +278,7 @@ void SeptentrioDriver::run()
 				_uart.setPort(_port);

 				if (_uart.open()) {
-					_state = State::ConfiguringDevice;
+					_state = State::DetectingBaudRate;

 				} else {
 					// Failed to open port, so wait a bit before trying again.
@@ -277,14 +288,42 @@ void SeptentrioDriver::run()
 				break;
 			}

+		case State::DetectingBaudRate: {
+				static uint32_t expected_baud_rates[] = {k_septentrio_receiver_default_baud_rate, 115200, 230400, 57600, 460800, 500000, 576000, 38400, 921600, 1000000, 1500000};
+				expected_baud_rates[0] = _chosen_baud_rate != 0 ? _chosen_baud_rate : k_septentrio_receiver_default_baud_rate;
+
+				if (detect_receiver_baud_rate(expected_baud_rates[_current_baud_rate_index], true)) {
+					if (set_baudrate(expected_baud_rates[_current_baud_rate_index]) == PX4_OK) {
+						_state = State::ConfiguringDevice;
+
+					} else {
+						SEP_ERR("Setting local baud rate failed");
+					}
+
+				} else {
+					_current_baud_rate_index++;
+
+					if (_current_baud_rate_index == sizeof(expected_baud_rates) / sizeof(expected_baud_rates[0])) {
+						_current_baud_rate_index = 0;
+					}
+				}
+
+				break;
+			}
+
 		case State::ConfiguringDevice: {
-				if (configure() == PX4_OK) {
-					SEP_INFO("Automatic configuration successful");
+				ConfigureResult result = configure();
+
+				if (!(static_cast<int32_t>(result) & static_cast<int32_t>(ConfigureResult::FailedCompletely))) {
+					if (static_cast<int32_t>(result) & static_cast<int32_t>(ConfigureResult::NoLogging)) {
+						mavlink_log_warning(&k_mavlink_log_pub, "Septentrio: Failed to configure receiver internal logging");
+					}
+
+					SEP_INFO("Automatic configuration finished");
 					_state = State::ReceivingData;

 				} else {
-					// Failed to configure device, so wait a bit before trying again.
-					px4_usleep(200000);
+					_state = State::DetectingBaudRate;
 				}

 				break;
@@ -296,7 +335,7 @@ void SeptentrioDriver::run()
 				receive_result = receive(k_timeout_5hz);

 				if (receive_result == -1) {
-					_state = State::ConfiguringDevice;
+					_state = State::DetectingBaudRate;
 				}

 				if (_message_satellite_info && (receive_result & 2)) {
@@ -385,19 +424,51 @@ SeptentrioDriver *SeptentrioDriver::instantiate(int argc, char *argv[])

 SeptentrioDriver *SeptentrioDriver::instantiate(int argc, char *argv[], Instance instance)
 {
-	ModuleArguments arguments{};
-	SeptentrioDriver *gps{nullptr};
+	ModuleArguments arguments {};
+	SeptentrioDriver *gps {nullptr};

 	if (parse_cli_arguments(argc, argv, arguments) == PX4_ERROR) {
 		return nullptr;
 	}

+	if (arguments.device_path_main && arguments.device_path_secondary
+	    && strcmp(arguments.device_path_main, arguments.device_path_secondary) == 0) {
+		mavlink_log_critical(&k_mavlink_log_pub, "Septentrio: Device paths must be different");
+		return nullptr;
+	}
+
+	bool valid_chosen_baud_rate {false};
+
+	for (uint8_t i = 0; i < sizeof(k_supported_baud_rates) / sizeof(k_supported_baud_rates[0]); i++) {
+		switch (instance) {
+		case Instance::Main:
+			if (arguments.baud_rate_main == static_cast<int>(k_supported_baud_rates[i])) {
+				valid_chosen_baud_rate = true;
+			}
+
+			break;
+
+		case Instance::Secondary:
+			if (arguments.baud_rate_secondary == static_cast<int>(k_supported_baud_rates[i])) {
+				valid_chosen_baud_rate = true;
+			}
+
+			break;
+		}
+	}
+
+	if (!valid_chosen_baud_rate) {
+		mavlink_log_critical(&k_mavlink_log_pub, "Septentrio: Baud rate %d is unsupported, falling back to default %lu",
+				     instance == Instance::Main ? arguments.baud_rate_main : arguments.baud_rate_secondary, k_default_baud_rate);
+	}
+
 	if (instance == Instance::Main) {
 		if (Serial::validatePort(arguments.device_path_main)) {
-			gps = new SeptentrioDriver(arguments.device_path_main, instance, arguments.baud_rate_main);
+			gps = new SeptentrioDriver(arguments.device_path_main, instance,
+						   valid_chosen_baud_rate ? arguments.baud_rate_main : k_default_baud_rate);

 		} else {
-			PX4_ERR("invalid device (-d) %s", arguments.device_path_main ? arguments.device_path_main : "");
+			PX4_ERR("Invalid device (-d) %s", arguments.device_path_main ? arguments.device_path_main : "");
 		}

 		if (gps && arguments.device_path_secondary) {
@@ -410,7 +481,8 @@ SeptentrioDriver *SeptentrioDriver::instantiate(int argc, char *argv[], Instance

 	} else {
 		if (Serial::validatePort(arguments.device_path_secondary)) {
-			gps = new SeptentrioDriver(arguments.device_path_secondary, instance, arguments.baud_rate_secondary);
+			gps = new SeptentrioDriver(arguments.device_path_secondary, instance,
+						   valid_chosen_baud_rate ? arguments.baud_rate_secondary : k_default_baud_rate);

 		} else {
 			PX4_ERR("Invalid secondary device (-e) %s", arguments.device_path_secondary ? arguments.device_path_secondary : "");
@@ -425,6 +497,7 @@ SeptentrioDriver *SeptentrioDriver::instantiate(int argc, char *argv[], Instance
 int SeptentrioDriver::custom_command(int argc, char *argv[])
 {
 	bool handled = false;
+	const char *failure_reason {"unknown command"};
 	SeptentrioDriver *driver_instance;

 	if (!is_running()) {
@@ -448,7 +521,7 @@ int SeptentrioDriver::custom_command(int argc, char *argv[])
 				type = ReceiverResetType::Cold;

 			} else {
-				print_usage("incorrect reset type");
+				failure_reason = "unknown reset type";
 			}

 			if (type != ReceiverResetType::None) {
@@ -457,11 +530,11 @@ int SeptentrioDriver::custom_command(int argc, char *argv[])
 			}

 		} else {
-			print_usage("incorrect usage of reset command");
+			failure_reason = "incorrect usage of reset command";
 		}
 	}

-	return (handled) ? 0 : print_usage("unknown command");
+	return handled ? 0 : print_usage(failure_reason);
 }

 int SeptentrioDriver::print_usage(const char *reason)
@@ -473,25 +546,30 @@ int SeptentrioDriver::print_usage(const char *reason)
 	PRINT_MODULE_DESCRIPTION(
 		R"DESCR_STR(
 ### Description
-GPS driver module that handles the communication with Septentrio devices and publishes the position via uORB.
-
-The module supports a secondary GPS device, specified via `-e` parameter. The position will be published on
-the second uORB topic instance. It can be used for logging and heading computation.
+Driver for Septentrio GNSS receivers.
+It can automatically configure them and make their output available for the rest of the system.
+A secondary receiver is supported for redundancy, logging and dual-receiver heading.
+Septentrio receiver baud rates from 57600 to 1500000 are supported.
+If others are used, the driver will use 230400 and give a warning.

 ### Examples

-Starting 2 GPS devices (main one on /dev/ttyS3, secondary on /dev/ttyS4)
+Use one receiver on port `/dev/ttyS0` and automatically configure it to use baud rate 230400:
+$ septentrio start -d /dev/ttyS0 -b 230400
+
+Use two receivers, the primary on port `/dev/ttyS3` and the secondary on `/dev/ttyS4`,
+detect baud rate automatically and preserve them:
 $ septentrio start -d /dev/ttyS3 -e /dev/ttyS4

-Initiate warm restart of GPS device
+Perform warm reset of the receivers:
 $ gps reset warm
 )DESCR_STR");
 	PRINT_MODULE_USAGE_NAME("septentrio", "driver");
 	PRINT_MODULE_USAGE_COMMAND("start");
-	PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, "<file:dev>", "Primary Septentrio receiver", false);
-	PRINT_MODULE_USAGE_PARAM_INT('b', 0, 57600, 1500000, "Primary baud rate", true);
-	PRINT_MODULE_USAGE_PARAM_STRING('e', nullptr, "<file:dev>", "Secondary Septentrio receiver", true);
-	PRINT_MODULE_USAGE_PARAM_INT('g', 0, 57600, 1500000, "Secondary baud rate", true);
+	PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, "<file:dev>", "Primary receiver port", false);
+	PRINT_MODULE_USAGE_PARAM_INT('b', 0, 57600, 1500000, "Primary receiver baud rate", true);
+	PRINT_MODULE_USAGE_PARAM_STRING('e', nullptr, "<file:dev>", "Secondary receiver port", true);
+	PRINT_MODULE_USAGE_PARAM_INT('g', 0, 57600, 1500000, "Secondary receiver baud rate", true);

 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 	PRINT_MODULE_USAGE_COMMAND_DESCR("reset", "Reset connected receiver");
@@ -508,15 +586,15 @@ int SeptentrioDriver::reset(ReceiverResetType type)

 	switch (type) {
 	case ReceiverResetType::Hot:
-		res = send_message_and_wait_for_ack(k_command_reset_hot, k_receiver_ack_timeout);
+		res = send_message_and_wait_for_ack(k_command_reset_hot, k_receiver_ack_timeout_fast);
 		break;

 	case ReceiverResetType::Warm:
-		res = send_message_and_wait_for_ack(k_command_reset_warm, k_receiver_ack_timeout);
+		res = send_message_and_wait_for_ack(k_command_reset_warm, k_receiver_ack_timeout_fast);
 		break;

 	case ReceiverResetType::Cold:
-		res = send_message_and_wait_for_ack(k_command_reset_cold, k_receiver_ack_timeout);
+		res = send_message_and_wait_for_ack(k_command_reset_cold, k_receiver_ack_timeout_fast);
 		break;

 	default:
@@ -553,13 +631,13 @@ int SeptentrioDriver::parse_cli_arguments(int argc, char *argv[], ModuleArgument
 		switch (ch) {
 		case 'b':
 			if (px4_get_parameter_value(myoptarg, arguments.baud_rate_main) != 0) {
-				PX4_ERR("baud rate parsing failed");
+				PX4_ERR("Baud rate parsing failed");
 				return PX4_ERROR;
 			}
 			break;
 		case 'g':
 			if (px4_get_parameter_value(myoptarg, arguments.baud_rate_secondary) != 0) {
-				PX4_ERR("baud rate parsing failed");
+				PX4_ERR("Baud rate parsing failed");
 				return PX4_ERROR;
 			}
 			break;
@@ -637,42 +715,31 @@ void SeptentrioDriver::schedule_reset(ReceiverResetType reset_type)
 	}
 }

-uint32_t SeptentrioDriver::detect_receiver_baud_rate(bool forced_input) {
-	// So we can restore the port to its original state.
-	const uint32_t original_baud_rate = _uart.getBaudrate();
-
-	// Baud rates we expect the receiver to be running at.
-	uint32_t expected_baud_rates[] = {115200, 115200, 230400, 57600, 460800, 500000, 576000, 38400, 921600, 1000000, 1500000};
-	if (_baud_rate != 0) {
-		expected_baud_rates[0] = _baud_rate;
+bool SeptentrioDriver::detect_receiver_baud_rate(const uint32_t &baud_rate, bool forced_input) {
+	if (set_baudrate(baud_rate) != PX4_OK) {
+		return false;
 	}

-	for (uint i = 0; i < sizeof(expected_baud_rates)/sizeof(expected_baud_rates[0]); i++) {
-		if (set_baudrate(expected_baud_rates[i]) != PX4_OK) {
-			set_baudrate(original_baud_rate);
-			return 0;
-		}
-
-		if (forced_input) {
-			force_input();
-		}
-
-		if (send_message_and_wait_for_ack(k_command_ping, k_receiver_ack_timeout)) {
-			SEP_INFO("Detected baud rate: %lu", expected_baud_rates[i]);
-			set_baudrate(original_baud_rate);
-			return expected_baud_rates[i];
-		}
+	if (forced_input) {
+		force_input();
 	}

-	set_baudrate(original_baud_rate);
-	return 0;
+	// Make sure that any weird data is "flushed" in the receiver.
+	(void)send_message("\n");
+
+	if (send_message_and_wait_for_ack(k_command_ping, k_receiver_ack_timeout_fast)) {
+		SEP_INFO("Detected baud rate: %lu", baud_rate);
+		return true;
+	}
+
+	return false;
 }

 int SeptentrioDriver::detect_serial_port(char* const port_name) {
 	// Read buffer to get the COM port
 	char buf[k_read_buffer_size];
 	size_t buffer_offset = 0;   // The offset into the string where the next data should be read to.
-	hrt_abstime timeout_time = hrt_absolute_time() + 5 * 1000 * k_receiver_ack_timeout;
+	hrt_abstime timeout_time = hrt_absolute_time() + 5 * 1000 * k_receiver_ack_timeout_fast;
 	bool response_detected = false;

 	// Receiver prints prompt after a message.
@@ -682,7 +749,7 @@ int SeptentrioDriver::detect_serial_port(char* const port_name) {

 	do {
 		// Read at most the amount of available bytes in the buffer after the current offset, -1 because we need '\0' at the end for a valid string.
-		int read_result = read(reinterpret_cast<uint8_t *>(buf) + buffer_offset, sizeof(buf) - buffer_offset - 1, k_receiver_ack_timeout);
+		int read_result = read(reinterpret_cast<uint8_t *>(buf) + buffer_offset, sizeof(buf) - buffer_offset - 1, k_receiver_ack_timeout_fast);

 		if (read_result < 0) {
 			SEP_WARN("SBF read error");
@@ -734,132 +801,81 @@ int SeptentrioDriver::detect_serial_port(char* const port_name) {
 	}
 }

-int SeptentrioDriver::configure()
+SeptentrioDriver::ConfigureResult SeptentrioDriver::configure()
 {
 	char msg[k_max_command_size] {};
-
-	// Passively detect receiver baud rate.
-	uint32_t detected_receiver_baud_rate = detect_receiver_baud_rate(true);
-
-	if (detected_receiver_baud_rate == 0) {
-		SEP_INFO("CONFIG: failed baud detection");
-		return PX4_ERROR;
-	}
-
-	// Set same baud rate on our end.
-	if (set_baudrate(detected_receiver_baud_rate) != PX4_OK) {
-		SEP_INFO("CONFIG: failed local baud rate setting");
-		return PX4_ERROR;
-	}
-
 	char com_port[5] {};
+	ConfigureResult result {ConfigureResult::OK};

 	// Passively detect receiver port.
 	if (detect_serial_port(com_port) != PX4_OK) {
-		SEP_INFO("CONFIG: failed port detection");
-		return PX4_ERROR;
+		SEP_WARN("CONFIG: failed port detection");
+		return ConfigureResult::FailedCompletely;
 	}

 	// We should definitely match baud rates and detect used port, but don't do other configuration if not requested.
 	// This will force input on the receiver. That shouldn't be a problem as it's on our own connection.
 	if (!_automatic_configuration) {
-		return PX4_OK;
+		return ConfigureResult::OK;
 	}

 	// If user requested specific baud rate, set it now. Otherwise keep detected baud rate.
-	if (strstr(com_port, "COM") != nullptr && _baud_rate != 0) {
-		snprintf(msg, sizeof(msg), k_command_set_baud_rate, com_port, _baud_rate);
+	if (strstr(com_port, "COM") != nullptr && _chosen_baud_rate != 0) {
+		snprintf(msg, sizeof(msg), k_command_set_baud_rate, com_port, _chosen_baud_rate);

 		if (!send_message(msg)) {
-			SEP_INFO("CONFIG: baud rate command write error");
-			return PX4_ERROR;
+			SEP_WARN("CONFIG: baud rate command write error");
+			return ConfigureResult::FailedCompletely;
 		}

 		// When sending a command and setting the baud rate right after, the controller could send the command at the new baud rate.
 		// From testing this could take some time.
-		px4_usleep(1000000);
+		px4_usleep(2000000);

-		if (set_baudrate(_baud_rate) != PX4_OK) {
-			SEP_INFO("CONFIG: failed local baud rate setting");
-			return PX4_ERROR;
+		if (set_baudrate(_chosen_baud_rate) != PX4_OK) {
+			SEP_WARN("CONFIG: failed local baud rate setting");
+			return ConfigureResult::FailedCompletely;
 		}

-		if (!send_message_and_wait_for_ack(k_command_ping, k_receiver_ack_timeout)) {
-			SEP_INFO("CONFIG: failed remote baud rate setting");
-			return PX4_ERROR;
+		if (!send_message_and_wait_for_ack(k_command_ping, k_receiver_ack_timeout_fast)) {
+			SEP_WARN("CONFIG: failed remote baud rate setting");
+			return ConfigureResult::FailedCompletely;
 		}
-	} else {
-		_baud_rate = detected_receiver_baud_rate;
 	}

 	// Delete all sbf outputs on current COM port to remove clutter data
 	snprintf(msg, sizeof(msg), k_command_clear_sbf, _receiver_stream_main, com_port);

-	if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout)) {
-		SEP_INFO("CONFIG: failed delete output");
-		return PX4_ERROR; // connection and/or baudrate detection failed
+	if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_fast)) {
+		SEP_WARN("CONFIG: failed delete output");
+		return ConfigureResult::FailedCompletely; // connection and/or baudrate detection failed
 	}

-	// Define/inquire the type of data that the receiver should accept/send on a given connection descriptor
-	snprintf(msg, sizeof(msg), k_command_set_data_io, com_port, "SBF");
-
-	if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout)) {
-		return PX4_ERROR;
-	}
-
-	// Specify the offsets that the receiver applies to the computed attitude angles.
-	snprintf(msg, sizeof(msg), k_command_set_attitude_offset, (double)(_heading_offset * 180 / M_PI_F), (double)_pitch_offset);
-
-	if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout)) {
-		return PX4_ERROR;
-	}
-
-	snprintf(msg, sizeof(msg), k_command_set_dynamics, "high");
-	if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout)) {
-		return PX4_ERROR;
-	}
-
-	const char *sbf_frequency {k_frequency_10_0hz};
-	switch (_sbf_output_frequency) {
-	case SBFOutputFrequency::Hz5_0:
-		sbf_frequency = k_frequency_5_0hz;
-		break;
-	case SBFOutputFrequency::Hz10_0:
-		sbf_frequency = k_frequency_10_0hz;
-		break;
-	case SBFOutputFrequency::Hz20_0:
-		sbf_frequency = k_frequency_20_0hz;
-		break;
-	case SBFOutputFrequency::Hz25_0:
-		sbf_frequency = k_frequency_25_0hz;
-		break;
-	}
-
-	// Output a set of SBF blocks on a given connection at a regular interval.
-	snprintf(msg, sizeof(msg), k_command_sbf_output_pvt, _receiver_stream_main, com_port, sbf_frequency);
-	if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout)) {
-		SEP_INFO("Failed to configure SBF");
-		return PX4_ERROR;
-	}
-
-	if (_receiver_setup == ReceiverSetup::MovingBase) {
-		if (_instance == Instance::Secondary) {
-			snprintf(msg, sizeof(msg), k_command_set_data_io, com_port, "+RTCMv3");
-			if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout)) {
-				SEP_INFO("Failed to configure RTCM output");
-			}
-		} else {
-			snprintf(msg, sizeof(msg), k_command_set_gnss_attitude, k_gnss_attitude_source_moving_base);
-			if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout)) {
-				SEP_INFO("Failed to configure attitude source");
-			}
+	// Set up the satellites used in PVT computation.
+	if (_receiver_constellation_usage != static_cast<int32_t>(SatelliteUsage::Default)) {
+		char requested_satellites[40] {};
+		if (_receiver_constellation_usage & static_cast<int32_t>(SatelliteUsage::GPS)) {
+			strcat(requested_satellites, "GPS+");
 		}
-	} else {
-		snprintf(msg, sizeof(msg), k_command_set_gnss_attitude, k_gnss_attitude_source_multi_antenna);
-		// This fails on single-antenna receivers, which is fine. Therefore don't check for acknowledgement.
-		if (!send_message(msg)) {
-			SEP_INFO("Failed to configure attitude source");
-			return PX4_ERROR;
+		if (_receiver_constellation_usage & static_cast<int32_t>(SatelliteUsage::GLONASS)) {
+			strcat(requested_satellites, "GLONASS+");
+		}
+		if (_receiver_constellation_usage & static_cast<int32_t>(SatelliteUsage::Galileo)) {
+			strcat(requested_satellites, "GALILEO+");
+		}
+		if (_receiver_constellation_usage & static_cast<int32_t>(SatelliteUsage::SBAS)) {
+			strcat(requested_satellites, "SBAS+");
+		}
+		if (_receiver_constellation_usage & static_cast<int32_t>(SatelliteUsage::BeiDou)) {
+			strcat(requested_satellites, "BEIDOU+");
+		}
+		// Make sure to remove the trailing '+' if any.
+		requested_satellites[math::max(static_cast<int>(strlen(requested_satellites)) - 1, 0)] = '\0';
+		snprintf(msg, sizeof(msg), k_command_set_satellite_usage, requested_satellites);
+		// Use a longer timeout as the `setSatelliteUsage` command acknowledges a bit slower on mosaic-H-based receivers.
+		if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_slow)) {
+			SEP_WARN("CONFIG: Failed to configure constellation usage");
+			return ConfigureResult::FailedCompletely;
 		}
 	}

@@ -919,42 +935,77 @@ int SeptentrioDriver::configure()
 		}

 		snprintf(msg, sizeof(msg), k_command_set_sbf_output, _receiver_stream_log, "DSK1", _receiver_logging_overwrite ? "" : "+", level, frequency);
-		if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout)) {
-			SEP_ERR("Failed to configure logging");
-			return PX4_ERROR;
+		if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_fast)) {
+			result = static_cast<ConfigureResult>(static_cast<int32_t>(result) | static_cast<int32_t>(ConfigureResult::NoLogging));
 		}
 	} else if (_receiver_stream_log == _receiver_stream_main) {
-		SEP_WARN("Skipping logging setup: logging stream can't equal main stream");
+		result = static_cast<ConfigureResult>(static_cast<int32_t>(result) | static_cast<int32_t>(ConfigureResult::NoLogging));
 	}

-	// Set up the satellites used in PVT computation.
-	if (_receiver_constellation_usage != static_cast<int32_t>(SatelliteUsage::Default)) {
-		char requested_satellites[40] {};
-		if (_receiver_constellation_usage & static_cast<int32_t>(SatelliteUsage::GPS)) {
-			strcat(requested_satellites, "GPS+");
+	// Define/inquire the type of data that the receiver should accept/send on a given connection descriptor
+	snprintf(msg, sizeof(msg), k_command_set_data_io, com_port, "SBF");
+
+	if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_fast)) {
+		return ConfigureResult::FailedCompletely;
+	}
+
+	// Specify the offsets that the receiver applies to the computed attitude angles.
+	snprintf(msg, sizeof(msg), k_command_set_attitude_offset, static_cast<double>(_heading_offset), static_cast<double>(_pitch_offset));
+
+	if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_fast)) {
+		return ConfigureResult::FailedCompletely;
+	}
+
+	snprintf(msg, sizeof(msg), k_command_set_dynamics, "high");
+	if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_fast)) {
+		return ConfigureResult::FailedCompletely;
+	}
+
+	const char *sbf_frequency {k_frequency_10_0hz};
+	switch (_sbf_output_frequency) {
+	case SBFOutputFrequency::Hz5_0:
+		sbf_frequency = k_frequency_5_0hz;
+		break;
+	case SBFOutputFrequency::Hz10_0:
+		sbf_frequency = k_frequency_10_0hz;
+		break;
+	case SBFOutputFrequency::Hz20_0:
+		sbf_frequency = k_frequency_20_0hz;
+		break;
+	case SBFOutputFrequency::Hz25_0:
+		sbf_frequency = k_frequency_25_0hz;
+		break;
+	}
+
+	// Output a set of SBF blocks on a given connection at a regular interval.
+	snprintf(msg, sizeof(msg), k_command_sbf_output_pvt, _receiver_stream_main, com_port, sbf_frequency);
+	if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_fast)) {
+		SEP_WARN("CONFIG: Failed to configure SBF");
+		return ConfigureResult::FailedCompletely;
+	}
+
+	if (_receiver_setup == ReceiverSetup::MovingBase) {
+		if (_instance == Instance::Secondary) {
+			snprintf(msg, sizeof(msg), k_command_set_data_io, com_port, "+RTCMv3");
+			if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_fast)) {
+				SEP_WARN("CONFIG: Failed to configure RTCM output");
+			}
+		} else {
+			snprintf(msg, sizeof(msg), k_command_set_gnss_attitude, k_gnss_attitude_source_moving_base);
+			if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_fast)) {
+				SEP_WARN("CONFIG: Failed to configure attitude source");
+			}
 		}
-		if (_receiver_constellation_usage & static_cast<int32_t>(SatelliteUsage::GLONASS)) {
-			strcat(requested_satellites, "GLONASS+");
-		}
-		if (_receiver_constellation_usage & static_cast<int32_t>(SatelliteUsage::Galileo)) {
-			strcat(requested_satellites, "GALILEO+");
-		}
-		if (_receiver_constellation_usage & static_cast<int32_t>(SatelliteUsage::SBAS)) {
-			strcat(requested_satellites, "SBAS+");
-		}
-		if (_receiver_constellation_usage & static_cast<int32_t>(SatelliteUsage::BeiDou)) {
-			strcat(requested_satellites, "BEIDOU+");
-		}
-		// Make sure to remove the trailing '+' if any.
-		requested_satellites[math::max(static_cast<int>(strlen(requested_satellites)) - 1, 0)] = '\0';
-		snprintf(msg, sizeof(msg), k_command_set_satellite_usage, requested_satellites);
-		if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout)) {
-			SEP_ERR("Failed to configure constellation usage");
-			return PX4_ERROR;
+	} else {
+		snprintf(msg, sizeof(msg), k_command_set_gnss_attitude, k_gnss_attitude_source_multi_antenna);
+		// This fails on single-antenna receivers, which is fine. Therefore don't check for acknowledgement.
+		if (!send_message(msg)) {
+			SEP_WARN("CONFIG: Failed to configure attitude source");
+			return ConfigureResult::FailedCompletely;
 		}
 	}

-	return PX4_OK;
+	return result;
 }

 int SeptentrioDriver::parse_char(const uint8_t byte)
@@ -1035,36 +1086,37 @@ int SeptentrioDriver::process_message()
 			PVTGeodetic pvt_geodetic;

 			if (_sbf_decoder.parse(&header) == PX4_OK && _sbf_decoder.parse(&pvt_geodetic) == PX4_OK) {
-				if (pvt_geodetic.mode_type < ModeType::StandAlonePVT) {
+				switch (pvt_geodetic.mode_type) {
+				case ModeType::NoPVT:
 					_message_gps_state.fix_type = sensor_gps_s::FIX_TYPE_NONE;
-
-				} else if (pvt_geodetic.mode_type == ModeType::PVTWithSBAS) {
+					break;
+				case ModeType::PVTWithSBAS:
 					_message_gps_state.fix_type = sensor_gps_s::FIX_TYPE_RTCM_CODE_DIFFERENTIAL;
-
-				} else if (pvt_geodetic.mode_type == ModeType::RTKFloat || pvt_geodetic.mode_type == ModeType::MovingBaseRTKFloat) {
+					break;
+				case ModeType::RTKFloat:
+				case ModeType::MovingBaseRTKFloat:
 					_message_gps_state.fix_type = sensor_gps_s::FIX_TYPE_RTK_FLOAT;
-
-				} else if (pvt_geodetic.mode_type == ModeType::RTKFixed || pvt_geodetic.mode_type == ModeType::MovingBaseRTKFixed) {
+					break;
+				case ModeType::RTKFixed:
+				case ModeType::MovingBaseRTKFixed:
 					_message_gps_state.fix_type = sensor_gps_s::FIX_TYPE_RTK_FIXED;
-
-				} else {
+					break;
+				default:
 					_message_gps_state.fix_type = sensor_gps_s::FIX_TYPE_3D;
+					break;
 				}

-				// Check fix and error code
-				_message_gps_state.vel_ned_valid = _message_gps_state.fix_type > sensor_gps_s::FIX_TYPE_NONE && pvt_geodetic.error == Error::None;
-
 				// Check boundaries and invalidate GPS velocities
 				if (pvt_geodetic.vn <= k_dnu_f4_value || pvt_geodetic.ve <= k_dnu_f4_value || pvt_geodetic.vu <= k_dnu_f4_value) {
 					_message_gps_state.vel_ned_valid = false;
 				}

-				// Check boundaries and invalidate position
-				// We're not just checking for the do-not-use value but for any value beyond the specified max values
-				if (pvt_geodetic.latitude <= k_dnu_f8_value ||
-				    pvt_geodetic.longitude <= k_dnu_f8_value ||
-				    pvt_geodetic.height <= k_dnu_f8_value ||
-				    pvt_geodetic.undulation <= k_dnu_f4_value) {
+				if (pvt_geodetic.latitude > k_dnu_f8_value && pvt_geodetic.longitude > k_dnu_f8_value && pvt_geodetic.height > k_dnu_f8_value && pvt_geodetic.undulation > k_dnu_f4_value) {
+					_message_gps_state.latitude_deg = pvt_geodetic.latitude * M_RAD_TO_DEG;
+					_message_gps_state.longitude_deg = pvt_geodetic.longitude * M_RAD_TO_DEG;
+					_message_gps_state.altitude_msl_m = pvt_geodetic.height - static_cast<double>(pvt_geodetic.undulation);
+					_message_gps_state.altitude_ellipsoid_m = pvt_geodetic.height;
+				} else {
 					_message_gps_state.fix_type = sensor_gps_s::FIX_TYPE_NONE;
 				}

@@ -1082,23 +1134,22 @@ int SeptentrioDriver::process_message()
 					_message_gps_state.satellites_used = 0;
 				}

-				_message_gps_state.latitude_deg = pvt_geodetic.latitude * M_RAD_TO_DEG;
-				_message_gps_state.longitude_deg = pvt_geodetic.longitude * M_RAD_TO_DEG;
-				_message_gps_state.altitude_msl_m = pvt_geodetic.height - static_cast<double>(pvt_geodetic.undulation);
-				_message_gps_state.altitude_ellipsoid_m = pvt_geodetic.height;
-
 				/* H and V accuracy are reported in 2DRMS, but based off the u-blox reporting we expect RMS.
 				 * Divide by 100 from cm to m and in addition divide by 2 to get RMS. */
 				_message_gps_state.eph = static_cast<float>(pvt_geodetic.h_accuracy) / 200.0f;
 				_message_gps_state.epv = static_cast<float>(pvt_geodetic.v_accuracy) / 200.0f;

+				// Check fix and error code
+				_message_gps_state.vel_ned_valid = _message_gps_state.fix_type > sensor_gps_s::FIX_TYPE_NONE && pvt_geodetic.error == Error::None;
 				_message_gps_state.vel_n_m_s = pvt_geodetic.vn;
 				_message_gps_state.vel_e_m_s = pvt_geodetic.ve;
 				_message_gps_state.vel_d_m_s = -1.0f * pvt_geodetic.vu;
 				_message_gps_state.vel_m_s = sqrtf(_message_gps_state.vel_n_m_s * _message_gps_state.vel_n_m_s +
 							_message_gps_state.vel_e_m_s * _message_gps_state.vel_e_m_s);

-				_message_gps_state.cog_rad = pvt_geodetic.cog * M_DEG_TO_RAD_F;
+				if (pvt_geodetic.cog > k_dnu_f4_value) {
+					_message_gps_state.cog_rad = pvt_geodetic.cog * M_DEG_TO_RAD_F;
+				}
 				_message_gps_state.c_variance_rad = M_DEG_TO_RAD_F;

 				_message_gps_state.time_utc_usec = 0;
@@ -1178,14 +1229,8 @@ int SeptentrioDriver::process_message()
 			VelCovGeodetic vel_cov_geodetic;

 			if (_sbf_decoder.parse(&vel_cov_geodetic) == PX4_OK) {
-				_message_gps_state.s_variance_m_s = vel_cov_geodetic.cov_ve_ve;
-
-				if (_message_gps_state.s_variance_m_s < vel_cov_geodetic.cov_vn_vn) {
-					_message_gps_state.s_variance_m_s = vel_cov_geodetic.cov_vn_vn;
-				}
-
-				if (_message_gps_state.s_variance_m_s < vel_cov_geodetic.cov_vu_vu) {
-					_message_gps_state.s_variance_m_s = vel_cov_geodetic.cov_vu_vu;
+				if (vel_cov_geodetic.cov_ve_ve > k_dnu_f4_value && vel_cov_geodetic.cov_vn_vn > k_dnu_f4_value && vel_cov_geodetic.cov_vu_vu > k_dnu_f4_value) {
+					_message_gps_state.s_variance_m_s = math::max(math::max(vel_cov_geodetic.cov_ve_ve, vel_cov_geodetic.cov_vn_vn), vel_cov_geodetic.cov_vu_vu);
 				}
 			}

@@ -1203,10 +1248,11 @@ int SeptentrioDriver::process_message()

 			AttEuler att_euler;

-			if (_sbf_decoder.parse(&att_euler) &&
+			if (_sbf_decoder.parse(&att_euler) == PX4_OK &&
 			    !att_euler.error_not_requested &&
 			    att_euler.error_aux1 == Error::None &&
-			    att_euler.error_aux2 == Error::None) {
+			    att_euler.error_aux2 == Error::None &&
+			    att_euler.heading > k_dnu_f4_value) {
 				float heading = att_euler.heading * M_PI_F / 180.0f; // Range of degrees to range of radians in [0, 2PI].

 				// Ensure range is in [-PI, PI].
@@ -1230,7 +1276,8 @@ int SeptentrioDriver::process_message()
 			if (_sbf_decoder.parse(&att_cov_euler) == PX4_OK &&
 			    !att_cov_euler.error_not_requested &&
 			    att_cov_euler.error_aux1 == Error::None &&
-			    att_cov_euler.error_aux2 == Error::None) {
+			    att_cov_euler.error_aux2 == Error::None &&
+			    att_cov_euler.cov_headhead > k_dnu_f4_value) {
 				_message_gps_state.heading_accuracy = att_cov_euler.cov_headhead * M_PI_F / 180.0f; // Convert range of degrees to range of radians in [0, 2PI]
 			}

@@ -1289,13 +1336,16 @@ bool SeptentrioDriver::send_message_and_wait_for_ack(const char *msg, const int
 				return true;
 			} else if (expected_response[response_check_character] == buf[i]) {
 				++response_check_character;
+			} else if (buf[i] == '$') {
+				// Special case makes sure we don't miss start of new response if that happened to be the character we weren't expecting next (e.g., `$R: ge$R: gecm`)
+				response_check_character = 1;
 			} else {
 				response_check_character = 0;
 			}
 		}
 	} while (timeout_time > hrt_absolute_time());

-	PX4_DEBUG("Response: timeout");
+	SEP_WARN("Response: timeout");
 	return false;
 }

@@ -1492,10 +1542,6 @@ void SeptentrioDriver::publish()

 	_sensor_gps_pub.publish(_message_gps_state);

-	// Heading/yaw data can be updated at a lower rate than the other navigation data.
-	// The uORB message definition requires this data to be set to a NAN if no new valid data is available.
-	_message_gps_state.heading = NAN;
-
 	if (_message_gps_state.spoofing_state != _spoofing_state) {

 		if (_message_gps_state.spoofing_state > sensor_gps_s::SPOOFING_STATE_NONE) {
@@ -1523,6 +1569,11 @@ void SeptentrioDriver::publish_satellite_info()
 	}
 }

+bool SeptentrioDriver::first_gps_uorb_message_created() const
+{
+	return _message_gps_state.timestamp != 0;
+}
+
 void SeptentrioDriver::publish_rtcm_corrections(uint8_t *data, size_t len)
 {
 	gps_inject_data_s gps_inject_data{};
@@ -1668,13 +1719,11 @@ void SeptentrioDriver::set_clock(timespec rtc_gps_time)

 bool SeptentrioDriver::is_healthy() const
 {
-	if (_state == State::ReceivingData) {
-		if (!receiver_configuration_healthy()) {
-			return false;
-		}
+	if (_state == State::ReceivingData && receiver_configuration_healthy()) {
+		return true;
 	}

-	return true;
+	return false;
 }

 void SeptentrioDriver::reset_gps_state_message()
@@ -47,6 +47,7 @@
 #include <px4_platform_common/getopt.h>
 #include <px4_platform_common/module.h>
 #include <px4_platform_common/Serial.hpp>
+#include <uORB/uORB.h>
 #include <uORB/Publication.hpp>
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/SubscriptionMultiArray.hpp>
@@ -271,9 +272,20 @@ public:
 	 * @return `PX4_OK` on success, `PX4_ERROR` otherwise
 	 */
 	int force_input();
+
+	/**
+	 * Standard baud rates the driver can be started with. `0` means the driver matches baud rates but does not change them.
+	 */
+	static uint32_t k_supported_baud_rates[];
+
+	/**
+	 * Default baud rate, used when the user requested an invalid baud rate.
+	 */
+	static uint32_t k_default_baud_rate;
 private:
 	enum class State {
 		OpeningSerialPort,
+		DetectingBaudRate,
 		ConfiguringDevice,
 		ReceivingData,
 	};
@@ -295,9 +307,24 @@ private:
 	};

 	/**
-	 * Maximum timeout to wait for command acknowledgement by the receiver.
+	 * The result of trying to configure the receiver.
+	 */
+	enum class ConfigureResult : int32_t {
+		OK               = 0,
+		FailedCompletely = 1 << 0,
+		NoLogging        = 1 << 1,
+	};
+
+	/**
+	 * Maximum timeout to wait for fast command acknowledgement by the receiver.
 	*/
-	static constexpr uint16_t k_receiver_ack_timeout = 200;
+	static constexpr uint16_t k_receiver_ack_timeout_fast = 200;
+
+	/**
+	 * Maximum timeout to wait for slow command acknowledgement by the receiver.
+	 * Might be the case for commands that send more output back as reply.
+	 */
+	static constexpr uint16_t k_receiver_ack_timeout_slow = 400;

 	/**
 	 * Duration of one update monitoring interval in us.
@@ -306,6 +333,11 @@ private:
 	*/
 	static constexpr hrt_abstime k_update_monitoring_interval_duration = 5_s;

+	/**
+	 * uORB type to send messages to ground control stations.
+	 */
+	static orb_advert_t k_mavlink_log_pub;
+
 	/**
 	 * The default stream for output of PVT data.
 	*/
@@ -347,13 +379,15 @@ private:
 	void schedule_reset(ReceiverResetType type);

 	/**
-	 * @brief Detect the current baud rate used by the receiver on the connected port.
+	 * @brief Detect whether the receiver is running at the given baud rate.
+	 * Does not preserve local baud rate!
 	 *
-	 * @param force_input Choose whether the receiver forces input on the port
+	 * @param baud_rate The baud rate to check.
+	 * @param force_input Choose whether the receiver forces input on the port.
 	 *
-	 * @return The detected baud rate on success, or `0` on error
+	 * @return `true` if running at the baud rate, or `false` on error.
 	 */
-	uint32_t detect_receiver_baud_rate(bool forced_input);
+	bool detect_receiver_baud_rate(const uint32_t &baud_rate, bool forced_input);

 	/**
 	 * @brief Try to detect the serial port used on the receiver side.
@@ -369,9 +403,9 @@ private:
 	 *
 	 * If the user has disabled automatic configuration, only execute the steps that do not touch the receiver (e.g., baud rate detection, port detection...).
 	 *
-	 * @return `PX4_OK` on success, `PX4_ERROR` otherwise.
+	 * @return `ConfigureResult::OK` if configured, or error.
 	 */
-	int configure();
+	ConfigureResult configure();

 	/**
 	 * @brief Parse the next byte of a received message from the receiver.
@@ -505,6 +539,13 @@ private:
 	 */
 	void publish_satellite_info();

+	/**
+	 * @brief Check whether the driver has created its first complete `SensorGPS` uORB message.
+	 *
+	 * @return `true` if the driver has created its first complete `SensorGPS` uORB message, `false` if still waiting.
+	 */
+	bool first_gps_uorb_message_created() const;
+
 	/**
 	 * @brief Publish RTCM corrections.
 	 *
@@ -579,6 +620,9 @@ private:
 	/**
 	 * @brief Check whether the current receiver configuration is likely healthy.
 	 *
+	 * This is checked by passively monitoring output from the receiver and validating whether it is what is
+	 * expected.
+	 *
 	 * @return `true` if the receiver is operating correctly, `false` if it needs to be reconfigured.
 	*/
 	bool receiver_configuration_healthy() const;
@@ -611,6 +655,9 @@ private:
 	/**
 	 * @brief Check whether the driver is operating correctly.
 	 *
+	 * The driver is operating correctly when it has fully configured the receiver and is actively receiving all the
+	 * expected data.
+	 *
 	 * @return `true` if the driver is working as expected, `false` otherwise.
 	*/
 	bool is_healthy() const;
@@ -666,7 +713,7 @@ private:
 	uint8_t                                _spoofing_state {0};                                          ///< Receiver spoofing state
 	uint8_t                                _jamming_state {0};                                           ///< Receiver jamming state
 	const Instance                         _instance {Instance::Main};                                   ///< The receiver that this instance of the driver controls
-	uint32_t                               _baud_rate {0};
+	uint32_t                               _chosen_baud_rate {0};                                        ///< The baud rate requested by the user
 	static px4::atomic<SeptentrioDriver *> _secondary_instance;
 	hrt_abstime                            _sleep_end {0};                                               ///< End time for sleeping
 	State                                  _resume_state {State::OpeningSerialPort};                     ///< Resume state after sleep
@@ -683,6 +730,7 @@ private:
 	bool                                   _automatic_configuration {true};                              ///< Automatic configuration of the receiver given by the `SEP_AUTO_CONFIG` parameter
 	ReceiverSetup                          _receiver_setup {ReceiverSetup::Default};                     ///< Purpose of the receivers, given by the `SEP_HARDW_SETUP` parameter
 	int32_t                                _receiver_constellation_usage {0};                            ///< Constellation usage in PVT computation given by the `SEP_CONST_USAGE` parameter
+	uint8_t                                _current_baud_rate_index {0};                                 ///< Index of the current baud rate to check

 	// Decoding and parsing
 	DecodingStatus                                 _active_decoder {DecodingStatus::Searching}; ///< Currently active decoder that new data has to be fed into
@@ -241,7 +241,7 @@ OSDatxxxx::add_battery_info(uint8_t pos_x, uint8_t pos_y)
 	int ret = PX4_OK;

 	// TODO: show battery symbol based on battery fill level
-	snprintf(buf, sizeof(buf), "%c%5.2f", OSD_SYMBOL_BATT_3, (double)_battery_voltage_filtered_v);
+	snprintf(buf, sizeof(buf), "%c%5.2f", OSD_SYMBOL_BATT_3, (double)_battery_voltage_v);
 	buf[sizeof(buf) - 1] = '\0';

 	for (int i = 0; buf[i] != '\0'; i++) {
@@ -330,7 +330,7 @@ OSDatxxxx::update_topics()
 		_battery_sub.copy(&battery);

 		if (battery.connected) {
-			_battery_voltage_filtered_v = battery.voltage_filtered_v;
+			_battery_voltage_v = battery.voltage_v;
 			_battery_discharge_mah = battery.discharged_mah;
 			_battery_valid = true;

@@ -111,7 +111,7 @@ private:
 	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};

 	// battery
-	float _battery_voltage_filtered_v{0.f};
+	float _battery_voltage_v{0.f};
 	float _battery_discharge_mah{0.f};
 	bool _battery_valid{false};

@@ -224,8 +224,8 @@ void CrsfRc::Run()
 				battery_status_s battery_status;

 				if (_battery_status_sub.update(&battery_status)) {
-					uint16_t voltage = battery_status.voltage_filtered_v * 10;
-					uint16_t current = battery_status.current_filtered_a * 10;
+					uint16_t voltage = battery_status.voltage_v * 10;
+					uint16_t current = battery_status.current_a * 10;
 					int fuel = battery_status.discharged_mah;
 					uint8_t remaining = battery_status.remaining * 100;
 					this->SendTelemetryBattery(voltage, current, fuel, remaining);
@@ -241,7 +241,7 @@ void CrsfRc::Run()
 					int32_t longitude = static_cast<int32_t>(round(sensor_gps.longitude_deg * 1e7));
 					uint16_t groundspeed = sensor_gps.vel_d_m_s / 3.6f * 10.f;
 					uint16_t gps_heading = math::degrees(sensor_gps.cog_rad) * 100.f;
-					uint16_t altitude = static_cast<int16_t>(sensor_gps.altitude_msl_m * 1e3) + 1000;
+					uint16_t altitude = static_cast<int16_t>(sensor_gps.altitude_msl_m) + 1000;
 					uint8_t num_satellites = sensor_gps.satellites_used;
 					this->SendTelemetryGps(latitude, longitude, groundspeed, gps_heading, altitude, num_satellites);
 				}
@@ -81,8 +81,8 @@ bool CRSFTelemetry::send_battery()
 		return false;
 	}

-	uint16_t voltage = battery_status.voltage_filtered_v * 10;
-	uint16_t current = battery_status.current_filtered_a * 10;
+	uint16_t voltage = battery_status.voltage_v * 10;
+	uint16_t current = battery_status.current_a * 10;
 	int fuel = battery_status.discharged_mah;
 	uint8_t remaining = battery_status.remaining * 100;
 	return crsf_send_telemetry_battery(_uart_fd, voltage, current, fuel, remaining);
@@ -90,8 +90,8 @@ bool GHSTTelemetry::send_battery_status()
 	battery_status_s battery_status;

 	if (_battery_status_sub.update(&battery_status)) {
-		voltage_in_10mV = battery_status.voltage_filtered_v * FACTOR_VOLTS_TO_10MV;
-		current_in_10mA = battery_status.current_filtered_a * FACTOR_AMPS_TO_10MA;
+		voltage_in_10mV = battery_status.voltage_v * FACTOR_VOLTS_TO_10MV;
+		current_in_10mA = battery_status.current_a * FACTOR_AMPS_TO_10MA;
 		fuel_in_10mAh = battery_status.discharged_mah * FACTOR_MAH_TO_10MAH;
 		success = ghst_send_telemetry_battery_status(_uart_fd,
 				static_cast<uint16_t>(voltage_in_10mV),
@@ -146,13 +146,11 @@ void Batmon::RunImpl()

 	// Convert millivolts to volts.
 	new_report.voltage_v = ((float)result) / 1000.0f;
-	new_report.voltage_filtered_v = new_report.voltage_v;

 	// Read current.
 	ret |= _interface->read_word(BATT_SMBUS_CURRENT, result);

 	new_report.current_a = (-1.0f * ((float)(*(int16_t *)&result)) / 1000.0f);
-	new_report.current_filtered_a = new_report.current_a;

 	// Read average current.
 	ret |= _interface->read_word(BATT_SMBUS_AVERAGE_CURRENT, result);
@@ -115,9 +115,7 @@ void TattuCan::Run()
 		battery_status.state_of_health = static_cast<uint16_t>(tattu_message.health_status);

 		battery_status.voltage_v = static_cast<float>(tattu_message.voltage) / 1000.0f;
-		battery_status.voltage_filtered_v = static_cast<float>(tattu_message.voltage) / 1000.0f;
 		battery_status.current_a = static_cast<float>(tattu_message.current) / 1000.0f;
-		battery_status.current_filtered_a = static_cast<float>(tattu_message.current) / 1000.0f;
 		battery_status.remaining = static_cast<float>(tattu_message.remaining_percent) / 100.0f;
 		battery_status.temperature = static_cast<float>(tattu_message.temperature);
 		battery_status.capacity = tattu_message.standard_capacity;
@@ -104,9 +104,7 @@ UavcanBatteryBridge::battery_sub_cb(const uavcan::ReceivedDataStructure<uavcan::

 	_battery_status[instance].timestamp = hrt_absolute_time();
 	_battery_status[instance].voltage_v = msg.voltage;
-	_battery_status[instance].voltage_filtered_v = msg.voltage;
 	_battery_status[instance].current_a = msg.current;
-	_battery_status[instance].current_filtered_a = msg.current;
 	_battery_status[instance].current_average_a = msg.current;

 	if (_batt_update_mod[instance] == BatteryDataType::Raw) {
@@ -436,7 +436,7 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure<FixType>
 	}

 	// If we haven't already done so, set the system clock using GPS data
-	if (valid_pos_cov && !_system_clock_set) {
+	if ((fix_type >= sensor_gps_s::FIX_TYPE_2D) && !_system_clock_set) {
 		timespec ts{};

 		// get the whole microseconds
@@ -46,6 +46,7 @@
 #include <px4_platform_common/defines.h>

 using namespace time_literals;
+using namespace matrix;

 Battery::Battery(int index, ModuleParams *parent, const int sample_interval_us, const uint8_t source) :
 	ModuleParams(parent),
@@ -53,10 +54,9 @@ Battery::Battery(int index, ModuleParams *parent, const int sample_interval_us,
 	_source(source)
 {
 	const float expected_filter_dt = static_cast<float>(sample_interval_us) / 1_s;
-	_voltage_filter_v.setParameters(expected_filter_dt, 1.f);
-	_current_filter_a.setParameters(expected_filter_dt, .5f);
 	_current_average_filter_a.setParameters(expected_filter_dt, 50.f);
-	_throttle_filter.setParameters(expected_filter_dt, 1.f);
+	_ocv_filter_v.setParameters(expected_filter_dt, 1.f);
+	_cell_voltage_filter_v.setParameters(expected_filter_dt, 1.f);

 	if (index > 9 || index < 1) {
 		PX4_ERR("Battery index must be between 1 and 9 (inclusive). Received %d. Defaulting to 1.", index);
@@ -81,9 +81,6 @@ Battery::Battery(int index, ModuleParams *parent, const int sample_interval_us,
 	snprintf(param_name, sizeof(param_name), "BAT%d_CAPACITY", _index);
 	_param_handles.capacity = param_find(param_name);

-	snprintf(param_name, sizeof(param_name), "BAT%d_V_LOAD_DROP", _index);
-	_param_handles.v_load_drop = param_find(param_name);
-
 	snprintf(param_name, sizeof(param_name), "BAT%d_R_INTERNAL", _index);
 	_param_handles.r_internal = param_find(param_name);

@@ -97,29 +94,36 @@ Battery::Battery(int index, ModuleParams *parent, const int sample_interval_us,
 	_param_handles.bat_avrg_current = param_find("BAT_AVRG_CURRENT");

 	updateParams();
+
+	// Internal resistance estimation initializations
+	_RLS_est(0) = OCV_DEFAULT * _params.n_cells;
+	_RLS_est(1) = R_DEFAULT * _params.n_cells;
+	_estimation_covariance(0, 0) = OCV_COVARIANCE * _params.n_cells;
+	_estimation_covariance(0, 1) = 0.f;
+	_estimation_covariance(1, 0) = 0.f;
+	_estimation_covariance(1, 1) = R_COVARIANCE * _params.n_cells;
+	_estimation_covariance_norm = sqrtf(powf(_estimation_covariance(0, 0), 2.f) + 2.f * powf(_estimation_covariance(1, 0),
+					    2.f) + powf(_estimation_covariance(1, 1), 2.f));
 }

 void Battery::updateVoltage(const float voltage_v)
 {
 	_voltage_v = voltage_v;
-	_voltage_filter_v.update(voltage_v);
 }

 void Battery::updateCurrent(const float current_a)
 {
 	_current_a = current_a;
-	_current_filter_a.update(current_a);
 }

 void Battery::updateBatteryStatus(const hrt_abstime &timestamp)
 {
 	if (!_battery_initialized) {
-		_voltage_filter_v.reset(_voltage_v);
-		_current_filter_a.reset(_current_a);
+		resetInternalResistanceEstimation(_voltage_v, _current_a);
 	}

 	// Require minimum voltage otherwise override connected status
-	if (_voltage_filter_v.getState() < LITHIUM_BATTERY_RECOGNITION_VOLTAGE) {
+	if (_voltage_v < LITHIUM_BATTERY_RECOGNITION_VOLTAGE) {
 		_connected = false;
 	}

@@ -132,7 +136,7 @@ void Battery::updateBatteryStatus(const hrt_abstime &timestamp)

 	sumDischarged(timestamp, _current_a);
 	_state_of_charge_volt_based =
-		calculateStateOfChargeVoltageBased(_voltage_filter_v.getState(), _current_filter_a.getState());
+		calculateStateOfChargeVoltageBased(_voltage_v, _current_a);

 	if (!_external_state_of_charge) {
 		estimateStateOfCharge();
@@ -149,9 +153,7 @@ battery_status_s Battery::getBatteryStatus()
 {
 	battery_status_s battery_status{};
 	battery_status.voltage_v = _voltage_v;
-	battery_status.voltage_filtered_v = _voltage_filter_v.getState();
 	battery_status.current_a = _current_a;
-	battery_status.current_filtered_a = _current_filter_a.getState();
 	battery_status.current_average_a = _current_average_filter_a.getState();
 	battery_status.discharged_mah = _discharged_mah;
 	battery_status.remaining = _state_of_charge;
@@ -167,6 +169,14 @@ battery_status_s Battery::getBatteryStatus()
 	battery_status.warning = _warning;
 	battery_status.timestamp = hrt_absolute_time();
 	battery_status.faults = determineFaults();
+	battery_status.internal_resistance_estimate = _internal_resistance_estimate;
+	battery_status.ocv_estimate = _voltage_v + _internal_resistance_estimate * _params.n_cells * _current_a;
+	battery_status.ocv_estimate_filtered = _ocv_filter_v.getState();
+	battery_status.volt_based_soc_estimate = math::interpolate(_ocv_filter_v.getState() / _params.n_cells,
+			_params.v_empty, _params.v_charged, 0.f, 1.f);
+	battery_status.voltage_prediction = _voltage_prediction;
+	battery_status.prediction_error = _prediction_error;
+	battery_status.estimation_covariance_norm = _estimation_covariance_norm;
 	return battery_status;
 }

@@ -213,27 +223,69 @@ float Battery::calculateStateOfChargeVoltageBased(const float voltage_v, const f
 	// remaining battery capacity based on voltage
 	float cell_voltage = voltage_v / _params.n_cells;

-	// correct battery voltage locally for load drop to avoid estimation fluctuations
-	if (_params.r_internal >= 0.f && current_a > FLT_EPSILON) {
-		cell_voltage += _params.r_internal * current_a;
+	// correct battery voltage locally for load drop according to internal resistance and current
+	if (current_a > FLT_EPSILON) {
+		updateInternalResistanceEstimation(voltage_v, current_a);

-	} else {
-		vehicle_thrust_setpoint_s vehicle_thrust_setpoint{};
-		_vehicle_thrust_setpoint_0_sub.copy(&vehicle_thrust_setpoint);
-		const matrix::Vector3f thrust_setpoint = matrix::Vector3f(vehicle_thrust_setpoint.xyz);
-		const float throttle = thrust_setpoint.length();
+		if (_params.r_internal >= 0.f) { // Use user specified internal resistance value
+			cell_voltage += _params.r_internal * current_a;

-		_throttle_filter.update(throttle);
-
-		if (!_battery_initialized) {
-			_throttle_filter.reset(throttle);
+		} else { // Use estimated internal resistance value
+			cell_voltage += _internal_resistance_estimate * current_a;
 		}

-		// assume linear relation between throttle and voltage drop
-		cell_voltage += throttle * _params.v_load_drop;
 	}

-	return math::interpolate(cell_voltage, _params.v_empty, _params.v_charged, 0.f, 1.f);
+	_cell_voltage_filter_v.update(cell_voltage);
+	return math::interpolate(_cell_voltage_filter_v.getState(), _params.v_empty, _params.v_charged, 0.f, 1.f);
+}
+
+void Battery::updateInternalResistanceEstimation(const float voltage_v, const float current_a)
+{
+	Vector2f x{1, -current_a};
+	_voltage_prediction = (x.transpose() * _RLS_est)(0, 0);
+	_prediction_error = voltage_v - _voltage_prediction;
+	const Vector2f gamma = _estimation_covariance * x / (LAMBDA + (x.transpose() * _estimation_covariance * x)(0, 0));
+	const Vector2f RSL_est_temp = _RLS_est + gamma * _prediction_error;
+	const Matrix2f estimation_covariance_temp = (_estimation_covariance
+			- Matrix<float, 2, 1>(gamma) * (x.transpose() * _estimation_covariance)) / LAMBDA;
+	const float estimation_covariance_temp_norm =
+		sqrtf(powf(estimation_covariance_temp(0, 0), 2.f)
+		      + 2.f * powf(estimation_covariance_temp(1, 0), 2.f)
+		      + powf(estimation_covariance_temp(1, 1), 2.f));
+
+	if (estimation_covariance_temp_norm < _estimation_covariance_norm) { // Only update if estimation improves
+		_RLS_est = RSL_est_temp;
+		_estimation_covariance = estimation_covariance_temp;
+		_estimation_covariance_norm = estimation_covariance_temp_norm;
+		_internal_resistance_estimate =
+			math::max(_RLS_est(1) / _params.n_cells, 0.f); // Only use positive values
+
+	} else { // Update OCV estimate with IR estimate
+		_RLS_est(0) = voltage_v + _RLS_est(1) * current_a;
+	}
+
+	_ocv_filter_v.update(voltage_v + _internal_resistance_estimate * _params.n_cells * current_a);
+}
+
+void Battery::resetInternalResistanceEstimation(const float voltage_v, const float current_a)
+{
+	_RLS_est(0) = voltage_v;
+	_RLS_est(1) = R_DEFAULT * _params.n_cells;
+	_estimation_covariance.setZero();
+	_estimation_covariance(0, 0) = OCV_COVARIANCE * _params.n_cells;
+	_estimation_covariance(1, 1) = R_COVARIANCE * _params.n_cells;
+	_estimation_covariance_norm = sqrtf(powf(_estimation_covariance(0, 0), 2.f) + 2.f * powf(_estimation_covariance(1, 0),
+					    2.f) + powf(_estimation_covariance(1, 1), 2.f));
+	_internal_resistance_estimate = R_DEFAULT;
+	_ocv_filter_v.reset(voltage_v + _internal_resistance_estimate * _params.n_cells * current_a);
+
+	if (_params.r_internal >= 0.f) { // Use user specified internal resistance value
+		_cell_voltage_filter_v.reset(voltage_v / _params.n_cells + _params.r_internal * current_a);
+
+	} else { // Use estimated internal resistance value
+		_cell_voltage_filter_v.reset(voltage_v / _params.n_cells + _internal_resistance_estimate * current_a);
+	}
 }

 void Battery::estimateStateOfCharge()
@@ -354,7 +406,6 @@ void Battery::updateParams()
 	param_get(_param_handles.v_charged, &_params.v_charged);
 	param_get(_param_handles.n_cells, &_params.n_cells);
 	param_get(_param_handles.capacity, &_params.capacity);
-	param_get(_param_handles.v_load_drop, &_params.v_load_drop);
 	param_get(_param_handles.r_internal, &_params.r_internal);
 	param_get(_param_handles.source, &_params.source);
 	param_get(_param_handles.low_thr, &_params.low_thr);
@@ -58,7 +58,6 @@
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/flight_phase_estimation.h>
 #include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/vehicle_thrust_setpoint.h>

 /**
 * BatteryBase is a base class for any type of battery.
@@ -118,7 +117,6 @@ protected:
 		param_t v_charged;
 		param_t n_cells;
 		param_t capacity;
-		param_t v_load_drop;
 		param_t r_internal;
 		param_t low_thr;
 		param_t crit_thr;
@@ -132,7 +130,6 @@ protected:
 		float v_charged;
 		int32_t  n_cells;
 		float capacity;
-		float v_load_drop;
 		float r_internal;
 		float low_thr;
 		float crit_thr;
@@ -155,7 +152,6 @@ private:
 	void computeScale();
 	float computeRemainingTime(float current_a);

-	uORB::Subscription _vehicle_thrust_setpoint_0_sub{ORB_ID(vehicle_thrust_setpoint)};
 	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
 	uORB::SubscriptionData<flight_phase_estimation_s> _flight_phase_estimation_sub{ORB_ID(flight_phase_estimation)};
 	uORB::PublicationMulti<battery_status_s> _battery_status_pub{ORB_ID(battery_status)};
@@ -167,12 +163,11 @@ private:
 	uint8_t _priority{0};
 	bool _battery_initialized{false};
 	float _voltage_v{0.f};
-	AlphaFilter<float> _voltage_filter_v;
+	AlphaFilter<float> _ocv_filter_v;
+	AlphaFilter<float> _cell_voltage_filter_v;
 	float _current_a{-1};
-	AlphaFilter<float> _current_filter_a;
 	AlphaFilter<float>
 	_current_average_filter_a; ///< averaging filter for current. For FW, it is the current in level flight.
-	AlphaFilter<float> _throttle_filter;
 	float _discharged_mah{0.f};
 	float _discharged_mah_loop{0.f};
 	float _state_of_charge_volt_based{-1.f}; // [0,1]
@@ -183,4 +178,19 @@ private:
 	bool _armed{false};
 	bool _vehicle_status_is_fw{false};
 	hrt_abstime _last_unconnected_timestamp{0};
+
+	// Internal Resistance estimation
+	void updateInternalResistanceEstimation(const float voltage_v, const float current_a);
+	void resetInternalResistanceEstimation(const float voltage_v, const float current_a);
+	matrix::Vector2f _RLS_est; // [Open circuit voltage estimate [V], Total internal resistance estimate [Ohm]]^T
+	matrix::Matrix2f _estimation_covariance;
+	float _estimation_covariance_norm{0.f};
+	float _internal_resistance_estimate{0.005f}; // [Ohm] Per cell estimate of the internal resistance
+	float _voltage_prediction{0.f}; // [V] Predicted voltage of the estimator
+	float _prediction_error{0.f}; // [V] Error between the predicted and measured voltage
+	static constexpr float LAMBDA = 0.95f; 	// [0, 1] Forgetting factor (Tuning parameter for the RLS algorithm)
+	static constexpr float R_DEFAULT = 0.005f; // [Ohm] Initial per cell estimate of the internal resistance
+	static constexpr float OCV_DEFAULT = 4.2f; // [V] Initial per cell estimate of the open circuit voltage
+	static constexpr float R_COVARIANCE = 0.1f; // Initial per cell covariance of the internal resistance
+	static constexpr float OCV_COVARIANCE = 1.5f; // Initial per cell covariance of the open circuit voltage
 };
@@ -0,0 +1,176 @@
+# Test internal resistance estimator on flight logs
+# run with:
+# python3 int_res_est_replay.py -f <pathToLogFile> -c <#batteryCells>
+#   -u <(optional)fullCellVoltage> -e <(optional)emptyCellVoltage> -l <(optional)forgettingFactor> -d <(optional)filterMeasurements>
+# Note: Can lead to slightly different results than the online estimation due to the fact that
+# the log frequency of the voltage and current are not the same as the online frequency.
+
+from pyulog import ULog
+import matplotlib.pyplot as plt
+import numpy as np
+import argparse
+
+def getData(log, topic_name, variable_name, instance=0):
+    for elem in log.data_list:
+        if elem.name == topic_name and instance == elem.multi_id:
+            return elem.data[variable_name]
+    return np.array([])
+
+def us2s(time_us):
+    return time_us * 1e-6
+
+def rls_update(theta, P, x, V, I, lam):
+    gamma = P @ x / (lam + x.T @ P @ x)
+    error = V - x.T @ theta
+    data_cov = x.T @ P @ x
+    theta_temp = theta + gamma * error
+    P_temp = (P - gamma @ x.T @ P) / lam
+    if (abs(np.linalg.norm(P)) < abs(np.linalg.norm(P_temp))):
+        theta_corr = np.array([V + theta[1] * I, theta[1]]) # Correct OCV estimation
+        P_corr = P
+        return theta_corr, P_corr, error, data_cov, 0, 0
+    return theta_temp, P_temp, error, data_cov, gamma[0], gamma[1]
+
+def main(log_name, n_cells, full_cell, empty_cell, lam):
+    log = ULog(log_name)
+    timestamps = us2s(getData(log, 'battery_status', 'timestamp'))
+    I = getData(log, 'battery_status', 'current_a')
+    V = getData(log, 'battery_status', 'voltage_v')
+    remaining = getData(log, 'battery_status', 'remaining')
+
+    if not timestamps.size or not I.size or not V.size or not remaining.size:
+        print("Error: Incomplete data.")
+        return
+
+    # Initializations
+    theta = np.array([[V[0] + 0.005 * n_cells * I[0]], [0.005 * n_cells]])  # Initial VOC and R
+    P = np.diag([1.2 * n_cells, 0.1 * n_cells]) # Initial covariance
+    error = 0
+
+    # For plotting
+    VOC_est = np.zeros_like(I)
+    R_est = np.zeros_like(I)
+    error_hist = np.zeros_like(I)
+    v_est = np.zeros_like(I)
+    internal_resistance_stable = np.zeros_like(I)
+    internal_resistance_stable[-1] = 0.005
+    cov_norm = np.zeros_like(I)
+    r_cov = np.zeros_like(I)
+    ocv_cov = np.zeros_like(I)
+    mixed_cov = np.zeros_like(I)
+    data_cov_hist = np.zeros_like(I)
+    gamma_voc_hist = np.zeros_like(I)
+    gamma_r_hist = np.zeros_like(I)
+
+    for index in range(len(I)):
+        # RLS algorithm
+        x = np.array([[1.0], [-I[index]]]) # Input vector
+        theta, P, error, data_cov, gamma_voc_hist[index], gamma_r_hist[index] = rls_update(theta, P, x, V[index], I[index], lam) # Run RLS
+
+        # For plotting
+        VOC_est[index] = theta[0][0]
+        R_est[index] = theta[1][0]
+        error_hist[index] = error
+        v_est[index] = x.T @ theta
+        cov_norm[index] = abs(np.linalg.norm(P))
+        ocv_cov[index] = P[0][0]
+        r_cov[index] = P[1][1]
+        mixed_cov[index] = P[0][1]
+        data_cov_hist[index] = data_cov
+        internal_resistance_stable[index] = max(R_est[index]/n_cells, 0.001)
+
+    ## Plot data
+    print("Internal Resistance mean (per cell): ", np.mean(R_est) / n_cells)
+
+    # Summary plot
+    sumFig = plt.figure("Battery Estimation with RLS")
+
+    volt = plt.subplot(2, 3, 1)
+    volt.plot(timestamps, V, label='Measured voltage')
+    volt.plot(timestamps, v_est, label='Estimated voltage')
+    volt.plot(timestamps, np.array(V) + np.array(internal_resistance_stable) * np.array(I) * n_cells, label='OCV estimate')
+    ocv_smoothed = np.convolve(np.array(V) + np.array(internal_resistance_stable) * np.array(I) * n_cells, np.ones(30)/30, mode='full')[0:np.size(timestamps)]
+    ocv_smoothed[0:30] = ocv_smoothed[31]
+    volt.plot(timestamps, ocv_smoothed, label='OCV estimate smoothed')
+    volt.plot(timestamps, np.full_like(I, full_cell * n_cells), label='100% SOC')
+    volt.set_title("Measured Voltage vs Estimated voltage vs Estimated Open circuit voltage [V]")
+    volt.legend()
+
+    intR = plt.subplot(2, 3, 2)
+    intR.plot(timestamps, np.array(R_est) * 1000 / n_cells, label='Internal resistance estimate')
+    intR.set_title("Internal resistance estimate (per cell) [mOhm]")
+    intR.legend()
+
+    soc = plt.subplot(2, 3, 3)
+    soc.plot(timestamps, remaining, label='SoC logged')
+    soc.plot(timestamps, np.interp((np.array(V) + np.array(internal_resistance_stable) * n_cells * np.array(I)) / n_cells, [empty_cell, full_cell], [0, 1]), label='SoC with estimator')
+    soc.plot(timestamps, np.interp(ocv_smoothed / n_cells, [empty_cell, full_cell], [0, 1]), label='SoC with estimator smoothed')
+    soc.set_title("State of charge")
+    soc.legend()
+
+    curr = plt.subplot(2, 3, 4)
+    curr.plot(timestamps, I, label='Measured current')
+    curr.set_title("Measured Current [A]")
+    curr.legend()
+
+    err = plt.subplot(2, 3, 5)
+    err.plot(timestamps, error_hist, label='$Error$')
+    err.set_title("Voltage estimation error [V]")
+    err.legend()
+
+    cov = plt.subplot(2, 3, 6)
+    cov.plot(timestamps, cov_norm, label = 'Covariance norm')
+    cov.set_title("Covariance norm")
+    cov.legend()
+
+    # # SoC estimation plots
+    # socFig = plt.figure("SoC estimation")
+    # plt.plot(timestamps, np.interp((np.array(V) + np.array(I) * 0.005 * n_cells) / n_cells, [empty_cell, full_cell], [0, 1]), label='SoC with default $R_{int}$')
+    # plt.plot(timestamps, remaining, label='SoC logged')
+    # plt.plot(timestamps, np.interp((np.array(V) + np.array(internal_resistance_stable) * n_cells * np.array(I)) / n_cells, [empty_cell, full_cell], [0, 1]), label='SoC with estimator')
+    # # plt.plot(timestamps, np.convolve(np.interp((np.array(V) + np.array(internal_resistance_stable) * n_cells * np.array(I)) / n_cells, [empty_cell, full_cell], [0, 1]), np.ones(500)/500, mode='full')[0:np.size(timestamps)], label='SoC with estimator smoothed')
+    # # plt.plot(timestamps, np.interp((np.array(V) + np.array(I) * 0.0009 * n_cells) / n_cells, [empty_cell, full_cell], [0, 1]), label='SoC with $R_{int}$ measured beforehand')
+    # # plt.plot(timestamps, np.interp(VOC_est/n_cells, [empty_cell, full_cell], [0, 1]), label='SoC with VOC estimate')
+    # plt.legend()
+
+    # # Covariance plots
+    # covFig = plt.figure("Covariance plots")
+    # covR = plt.subplot(2, 2, 1)
+    # covR.plot(timestamps, r_cov, label = 'r_cov')
+    # covR.set_title("Internal resistance covariance")
+    # covR.legend()
+    # covVOC = plt.subplot(2, 2, 2)
+    # covVOC.plot(timestamps, ocv_cov, label = 'ocv_cov')
+    # covVOC.set_title("Open circuit covariance")
+    # covVOC.legend()
+    # covM = plt.subplot(2, 2, 3)
+    # covM.plot(timestamps, mixed_cov, label = 'mixed_cov')
+    # covM.set_title("Mixed covariance")
+    # covM.legend()
+    # covM = plt.subplot(2, 2, 4)
+    # covM.plot(timestamps, cov_norm, label = 'cov_norm')
+    # covM.set_title("Covariance norm")
+    # covM.legend()
+
+    # # Gain plots
+    # gainFig = plt.figure("Gain plots")
+    # gainVoc = plt.subplot(1, 2, 1)
+    # gainVoc.plot(timestamps, gamma_voc_hist, label = 'gain_voc')
+    # gainVoc.set_title("Gain VOC")
+    # gainVoc.legend()
+    # gainR = plt.subplot(1, 2, 2)
+    # gainR.plot(timestamps, gamma_r_hist, label = 'gain_r')
+    # gainR.set_title("Gain R")
+    # gainR.legend()
+
+    plt.show()
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser(description='Estimate battery parameters from ulog file.')
+    parser.add_argument('-f', type = str,   required = True,  help = 'Full path to ulog file')
+    parser.add_argument('-c', type = float, required = True,  help = 'Number of cells in battery')
+    parser.add_argument('-u', type = float, required = False, default = 4.05, help = 'Full cell voltage')
+    parser.add_argument('-e', type = float, required = False, default = 3.6,  help = 'Empty cell voltage')
+    parser.add_argument('-l', type = float, required = False, default = 0.99, help = 'Forgetting factor')
+    args = parser.parse_args()
+    main(args.f, args.c, args.u, args.e, args.l)
@@ -39,33 +39,11 @@ parameters:
            instance_start: 1
            default: [4.05, 4.05, 4.05]

-        BAT${i}_V_LOAD_DROP:
-            description:
-                short: Voltage drop per cell on full throttle
-                long: |
-                    This implicitly defines the internal resistance
-                    to maximum current ratio for the battery and assumes linearity.
-                    A good value to use is the difference between the
-                    5C and 20-25C load. Not used if BAT${i}_R_INTERNAL is
-                    set.
-
-            type: float
-            unit: V
-            min: 0.07
-            max: 0.5
-            decimal: 2
-            increment: 0.01
-            reboot_required: true
-            num_instances: *max_num_config_instances
-            instance_start: 1
-            default: [0.1, 0.1, 0.1]
-
        BAT${i}_R_INTERNAL:
            description:
                short: Explicitly defines the per cell internal resistance for battery ${i}
                long: |
-                    If non-negative, then this will be used in place of
-                    BAT${i}_V_LOAD_DROP for all calculations.
+                    If non-negative, then this will be used instead of the online estimated internal resistance.

            type: float
            unit: Ohm
@@ -76,7 +54,7 @@ parameters:
            reboot_required: true
            num_instances: *max_num_config_instances
            instance_start: 1
-            default: [0.005, 0.005, 0.005]
+            default: [-1.0, -1.0, -1.0]

        BAT${i}_N_CELLS:
            description:
@@ -261,13 +261,11 @@ int SMBUS_SBS_BaseClass<T>::populate_smbus_data(battery_status_s &data)

 	// Convert millivolts to volts.
 	data.voltage_v = ((float)result) * 0.001f;
-	data.voltage_filtered_v = data.voltage_v;

 	// Read current.
 	ret |= _interface->read_word(BATT_SMBUS_CURRENT, result);

 	data.current_a = (-1.0f * ((float)(*(int16_t *)&result)) * 0.001f);
-	data.current_filtered_a = data.current_a;

 	// Read remaining capacity.
 	ret |= _interface->read_word(BATT_SMBUS_RELATIVE_SOC, result);
@@ -618,6 +618,7 @@ SquareMatrix <Type, M> choleskyInv(const SquareMatrix<Type, M> &A)
 	return L_inv.T() * L_inv;
 }

+using Matrix2f = SquareMatrix<float, 2>;
 using Matrix3f = SquareMatrix<float, 3>;
 using Matrix3d = SquareMatrix<double, 3>;

@@ -43,7 +43,10 @@ CpuResourceChecks::CpuResourceChecks()

 void CpuResourceChecks::checkAndReport(const Context &context, Report &reporter)
 {
-	if (_param_com_cpu_max.get() < FLT_EPSILON) {
+	const bool cpu_load_check_enabled = _param_com_cpu_max.get() > FLT_EPSILON;
+	const bool ram_usage_check_enabled = _param_com_ram_max.get() > FLT_EPSILON;
+
+	if (!cpu_load_check_enabled && !ram_usage_check_enabled) {
 		return;
 	}

@@ -54,15 +57,15 @@ void CpuResourceChecks::checkAndReport(const Context &context, Report &reporter)
 		/* EVENT
 		 * @description
 		 * <profile name="dev">
-		 * If the system does not provide any CPU load information, use the parameter <param>COM_CPU_MAX</param>
-		 * to disable the check.
+		 * If the system does not provide any CPU and RAM load information, use the parameters <param>COM_CPU_MAX</param>
+		 * and <param>COM_RAM_MAX</param> to disable the checks.
 		 * </profile>
 		 */
 		reporter.healthFailure(NavModes::All, health_component_t::system, events::ID("check_missing_cpuload"),
-				       events::Log::Error, "No CPU load information");
+				       events::Log::Error, "No CPU and RAM load information");

 		if (reporter.mavlink_log_pub()) {
-			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: No CPU load information");
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: No CPU and RAM load information");
 		}

 	} else {
@@ -71,7 +74,7 @@ void CpuResourceChecks::checkAndReport(const Context &context, Report &reporter)
 		_high_cpu_load_hysteresis.set_state_and_update(high_cpu_load, hrt_absolute_time());

 		// fail check if CPU load is above the threshold for 2 seconds
-		if (_high_cpu_load_hysteresis.get_state()) {
+		if (cpu_load_check_enabled && _high_cpu_load_hysteresis.get_state()) {
 			/* EVENT
 			 * @description
 			 * The CPU load can be reduced for example by disabling unused modules (e.g. mavlink instances) or reducing the gyro update
@@ -88,5 +91,26 @@ void CpuResourceChecks::checkAndReport(const Context &context, Report &reporter)
 				mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: CPU load too high: %3.1f%%", (double)cpuload_percent);
 			}
 		}
+
+		const float ram_usage_percent = cpuload.ram_usage * 100.f;
+		const bool high_ram_usage = ram_usage_percent > _param_com_ram_max.get();
+
+		if (ram_usage_check_enabled && high_ram_usage) {
+			/* EVENT
+			 * @description
+			 * The RAM usage can be reduced for example by disabling unused modules (e.g. mavlink instances).
+			 *
+			 * <profile name="dev">
+			 * The threshold can be adjusted via <param>COM_RAM_MAX</param> parameter.
+			 * </profile>
+			 */
+			reporter.healthFailure<float>(NavModes::All, health_component_t::system, events::ID("check_ram_usage_too_high"),
+						      events::Log::Error, "RAM usage too high: {1:.1}%", ram_usage_percent);
+
+			if (reporter.mavlink_log_pub()) {
+				mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: RAM usage too high: %3.1f%%",
+						     (double)ram_usage_percent);
+			}
+		}
 	}
 }
@@ -54,6 +54,7 @@ private:
 	systemlib::Hysteresis _high_cpu_load_hysteresis{false};

 	DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
-					(ParamFloat<px4::params::COM_CPU_MAX>) _param_com_cpu_max
+					(ParamFloat<px4::params::COM_CPU_MAX>) _param_com_cpu_max,
+					(ParamFloat<px4::params::COM_RAM_MAX>) _param_com_ram_max
 				       )
 };
@@ -802,6 +802,21 @@ PARAM_DEFINE_FLOAT(COM_KILL_DISARM, 5.0f);
 */
 PARAM_DEFINE_FLOAT(COM_CPU_MAX, 95.0f);

+/**
+ * Maximum allowed RAM usage to pass checks
+ *
+ * The check fails if the RAM usage is above this threshold.
+ *
+ * A negative value disables the check.
+ *
+ * @group Commander
+ * @unit %
+ * @min -1
+ * @max 100
+ * @increment 1
+ */
+PARAM_DEFINE_FLOAT(COM_RAM_MAX, 95.0f);
+
 /**
 * Required number of redundant power modules
 *
@@ -203,6 +203,7 @@ if(CONFIG_EKF2_RANGE_FINDER)
 	list(APPEND EKF_SRCS
 		EKF/aid_sources/range_finder/range_finder_consistency_check.cpp
 		EKF/aid_sources/range_finder/range_height_control.cpp
+		EKF/aid_sources/range_finder/range_height_fusion.cpp
 		EKF/aid_sources/range_finder/sensor_range_finder.cpp
 	)
 endif()
@@ -212,9 +213,13 @@ if(CONFIG_EKF2_SIDESLIP)
 endif()

 if(CONFIG_EKF2_TERRAIN)
-	list(APPEND EKF_SRCS EKF/terrain_estimator/terrain_estimator.cpp)
+	list(APPEND EKF_SRCS EKF/terrain_control.cpp)
 endif()

+if(CONFIG_EKF2_WIND)
+	list(APPEND EKF_SRCS EKF/wind.cpp)
+endif ()
+
 add_subdirectory(EKF)

 px4_add_module(
@@ -125,6 +125,7 @@ if(CONFIG_EKF2_RANGE_FINDER)
 	list(APPEND EKF_SRCS
 		aid_sources/range_finder/range_finder_consistency_check.cpp
 		aid_sources/range_finder/range_height_control.cpp
+		aid_sources/range_finder/range_height_fusion.cpp
 		aid_sources/range_finder/sensor_range_finder.cpp
 	)
 endif()
@@ -134,9 +135,13 @@ if(CONFIG_EKF2_SIDESLIP)
 endif()

 if(CONFIG_EKF2_TERRAIN)
-	list(APPEND EKF_SRCS terrain_estimator/terrain_estimator.cpp)
+	list(APPEND EKF_SRCS terrain_control.cpp)
 endif()

+if(CONFIG_EKF2_WIND)
+	list(APPEND EKF_SRCS wind.cpp)
+endif ()
+
 include_directories(${CMAKE_CURRENT_SOURCE_DIR})
 add_library(ecl_EKF
 	${EKF_SRCS}
@@ -127,7 +127,7 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
 			if (_control_status.flags.inertial_dead_reckoning && !is_airspeed_consistent) {
 				resetVelUsingAirspeed(airspeed_sample);

-			} else if (!_control_status.flags.wind || getWindVelocityVariance().longerThan(_params.initial_wind_uncertainty)) {
+			} else if (!_control_status.flags.wind) {
 				// If starting wind state estimation, reset the wind states and covariances before fusing any data
 				// Also catch the case where sideslip fusion enabled wind estimation recently and didn't converge yet.
 				resetWindUsingAirspeed(airspeed_sample);
@@ -151,6 +151,8 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed
 #if defined(MODULE_NAME)
 		aid_src.timestamp = hrt_absolute_time();
 		_estimator_aid_src_aux_global_position_pub.publish(aid_src);
+
+		_test_ratio_filtered = math::max(fabsf(aid_src.test_ratio_filtered[0]), fabsf(aid_src.test_ratio_filtered[1]));
 #endif // MODULE_NAME

 	} else if ((_state != State::stopped) && isTimedOut(_time_last_buffer_push, imu_delayed.time_us, (uint64_t)5e6)) {
@@ -74,6 +74,8 @@ public:
 		updateParams();
 	}

+	float test_ratio_filtered() const { return _test_ratio_filtered; }
+
 private:
 	bool isTimedOut(uint64_t last_sensor_timestamp, uint64_t time_delayed_us, uint64_t timeout_period) const
 	{
@@ -106,6 +108,8 @@ private:

 	State _state{State::stopped};

+	float _test_ratio_filtered{INFINITY};
+
 #if defined(MODULE_NAME)
 	struct reset_counters_s {
 		uint8_t lat_lon{};
@@ -57,6 +57,10 @@ void Ekf::controlDragFusion(const imuSample &imu_delayed)

 		if (_drag_buffer->pop_first_older_than(imu_delayed.time_us, &drag_sample)) {
 			fuseDrag(drag_sample);
+
+			if (!_aid_src_drag.fused && !_control_status.flags.fuse_aspd && !_control_status.flags.fuse_beta) {
+				resetWind();
+			}
 		}
 	}
 }
@@ -63,7 +63,8 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
 	if (_gps_data_ready) {
 		const gnssSample &gnss_sample = _gps_sample_delayed;

-		if (runGnssChecks(gnss_sample) && isTimedOut(_last_gps_fail_us, (uint64_t)_min_gps_health_time_us / 2)) {
+		if (runGnssChecks(gnss_sample)
+		    && isTimedOut(_last_gps_fail_us, max((uint64_t)1e6, (uint64_t)_min_gps_health_time_us / 10))) {
 			if (isTimedOut(_last_gps_fail_us, (uint64_t)_min_gps_health_time_us)) {
 				// First time checks are passing, latching.
 				_gps_checks_passed = true;
@@ -245,6 +245,7 @@ void Ekf::controlMagFusion()

 					if (reset_heading) {
 						_control_status.flags.yaw_align = true;
+						resetAidSourceStatusZeroInnovation(aid_src);
 					}

 					_control_status.flags.mag = true;
@@ -53,15 +53,17 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
 		const bool is_quality_good = (_flow_sample_delayed.quality >= min_quality);
 		const bool is_magnitude_good = _flow_sample_delayed.flow_rate.isAllFinite()
 					       && !_flow_sample_delayed.flow_rate.longerThan(_flow_max_rate);
-		const bool is_tilt_good = (_R_to_earth(2, 2) > _params.range_cos_max_tilt);

-		// don't enforce this condition if terrain estimate is not valid as we have logic below to coast through bad range finder data
-		const bool is_within_max_sensor_dist = isTerrainEstimateValid() ? (_terrain_vpos - _state.pos(2) <= _flow_max_distance) : true;
+		bool is_tilt_good = true;
+
+#if defined(CONFIG_EKF2_RANGE_FINDER)
+		is_tilt_good = (_R_to_earth(2, 2) > _params.range_cos_max_tilt);
+
+#endif // CONFIG_EKF2_RANGE_FINDER

 		if (is_quality_good
 		    && is_magnitude_good
-		    && is_tilt_good
-		    && is_within_max_sensor_dist) {
+		    && is_tilt_good) {
 			// compensate for body motion to give a LOS rate
 			calcOptFlowBodyRateComp(imu_delayed);
 			_flow_rate_compensated = _flow_sample_delayed.flow_rate - _flow_sample_delayed.gyro_rate.xy();
@@ -79,22 +81,23 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
 		// Check if we are in-air and require optical flow to control position drift
 		bool is_flow_required = _control_status.flags.in_air
 					&& (_control_status.flags.inertial_dead_reckoning // is doing inertial dead-reckoning so must constrain drift urgently
-					|| isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow));
+					    || isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow));

-		// Fuse optical flow LOS rate observations into the main filter only if height above ground has been updated recently
-		// use a relaxed time criteria to enable it to coast through bad range finder data
-		const bool terrain_available = isTerrainEstimateValid() || isRecent(_aid_src_terrain_range_finder.time_last_fuse, (uint64_t)10e6);
+		const bool is_within_max_sensor_dist = getHagl() <= _flow_max_distance;

 		const bool continuing_conditions_passing = (_params.flow_ctrl == 1)
-							   && _control_status.flags.tilt_align
-							   && (terrain_available || is_flow_required);
+				&& _control_status.flags.tilt_align
+				&& is_within_max_sensor_dist;

 		const bool starting_conditions_passing = continuing_conditions_passing
-							 && isTimedOut(_aid_src_optical_flow.time_last_fuse, (uint64_t)2e6); // Prevent rapid switching
+				&& isTimedOut(_aid_src_optical_flow.time_last_fuse, (uint64_t)2e6); // Prevent rapid switching
+
+		// If the height is relative to the ground, terrain height cannot be observed.
+		_hagl_sensor_status.flags.flow = _control_status.flags.opt_flow && !(_height_sensor_ref == HeightSensor::RANGE);

 		if (_control_status.flags.opt_flow) {
 			if (continuing_conditions_passing) {
-				fuseOptFlow();
+				fuseOptFlow(_hagl_sensor_status.flags.flow);

 				// handle the case when we have optical flow, are reliant on it, but have not been using it for an extended period
 				if (isTimedOut(_aid_src_optical_flow.time_last_fuse, _params.no_aid_timeout_max)) {
@@ -125,19 +128,35 @@ void Ekf::startFlowFusion()
 {
 	ECL_INFO("starting optical flow fusion");

-	if (!_aid_src_optical_flow.innovation_rejected && isHorizontalAidingActive()) {
-		// Consistent with the current velocity state, simply fuse the data without reset
-		fuseOptFlow();
-		_control_status.flags.opt_flow = true;
+	if (_height_sensor_ref != HeightSensor::RANGE) {
+		// If the height is relative to the ground, terrain height cannot be observed.
+		_hagl_sensor_status.flags.flow = true;
+	}

-	} else if (!isHorizontalAidingActive()) {
-		resetFlowFusion();
-		_control_status.flags.opt_flow = true;
+	if (isHorizontalAidingActive()) {
+		if (!_aid_src_optical_flow.innovation_rejected) {
+			ECL_INFO("starting optical flow no reset");
+			fuseOptFlow(_hagl_sensor_status.flags.flow);
+
+		} else if (_hagl_sensor_status.flags.flow && !_hagl_sensor_status.flags.range_finder) {
+			resetTerrainToFlow();
+
+		} else {
+			ECL_INFO("optical flow fusion failed to start");
+			_control_status.flags.opt_flow = false;
+			_hagl_sensor_status.flags.flow = false;
+		}

 	} else {
-		ECL_INFO("optical flow fusion failed to start");
-		_control_status.flags.opt_flow = false;
+		if (isTerrainEstimateValid() || (_height_sensor_ref == HeightSensor::RANGE)) {
+			resetFlowFusion();
+
+		} else if (_hagl_sensor_status.flags.flow) {
+			resetTerrainToFlow();
+		}
 	}
+
+	_control_status.flags.opt_flow = true; // needs to be here because of isHorizontalAidingActive
 }

 void Ekf::resetFlowFusion()
@@ -159,18 +178,34 @@ void Ekf::resetFlowFusion()
 	_aid_src_optical_flow.time_last_fuse = _time_delayed_us;
 }

+void Ekf::resetTerrainToFlow()
+{
+	ECL_INFO("reset hagl to flow");
+	// TODO: use the flow data
+	_state.terrain = fmaxf(0.0f, _state.pos(2));
+	P.uncorrelateCovarianceSetVariance<State::terrain.dof>(State::terrain.idx, 100.f);
+	_terrain_vpos_reset_counter++;
+
+	_innov_check_fail_status.flags.reject_optflow_X = false;
+	_innov_check_fail_status.flags.reject_optflow_Y = false;
+
+	_aid_src_optical_flow.time_last_fuse = _time_delayed_us;
+}
+
 void Ekf::stopFlowFusion()
 {
 	if (_control_status.flags.opt_flow) {
 		ECL_INFO("stopping optical flow fusion");
 		_control_status.flags.opt_flow = false;
+		_hagl_sensor_status.flags.flow = false;
 	}
 }

 void Ekf::calcOptFlowBodyRateComp(const imuSample &imu_delayed)
 {
 	if (imu_delayed.delta_ang_dt > FLT_EPSILON) {
-		_ref_body_rate = -imu_delayed.delta_ang / imu_delayed.delta_ang_dt - getGyroBias(); // flow gyro has opposite sign convention
+		_ref_body_rate = -imu_delayed.delta_ang / imu_delayed.delta_ang_dt -
+				 getGyroBias(); // flow gyro has opposite sign convention

 	} else {
 		_ref_body_rate.zero();
@@ -185,5 +220,6 @@ void Ekf::calcOptFlowBodyRateComp(const imuSample &imu_delayed)
 	}

 	// calculate the bias estimate using  a combined LPF and spike filter
-	_flow_gyro_bias = _flow_gyro_bias * 0.99f + matrix::constrain(_flow_sample_delayed.gyro_rate - _ref_body_rate, -0.1f, 0.1f) * 0.01f;
+	_flow_gyro_bias = _flow_gyro_bias * 0.99f + matrix::constrain(_flow_sample_delayed.gyro_rate - _ref_body_rate, -0.1f,
+			  0.1f) * 0.01f;
 }
@@ -33,12 +33,6 @@

 /**
 * @file optflow_fusion.cpp
- * Function for fusing gps and baro measurements/
- * equations generated using EKF/python/ekf_derivation/main.py
- *
- * @author Paul Riseborough <p_riseborough@live.com.au>
- * @author Siddharth Bharat Purohit <siddharthbharatpurohit@gmail.com>
- *
 */

 #include "ekf.h"
@@ -73,7 +67,7 @@ void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src)

 	Vector2f innov_var;
 	VectorState H;
-	sym::ComputeFlowXyInnovVarAndHx(_state.vector(), P, range, R_LOS, FLT_EPSILON, &innov_var, &H);
+	sym::ComputeFlowXyInnovVarAndHx(_state.vector(), P, R_LOS, FLT_EPSILON, &innov_var, &H);

 	// run the innovation consistency check and record result
 	updateAidSourceStatus(aid_src,
@@ -85,7 +79,7 @@ void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src)
 		math::max(_params.flow_innov_gate, 1.f));      // innovation gate
 }

-void Ekf::fuseOptFlow()
+void Ekf::fuseOptFlow(const bool update_terrain)
 {
 	const float R_LOS = _aid_src_optical_flow.observation_variance[0];

@@ -97,7 +91,7 @@ void Ekf::fuseOptFlow()

 	Vector2f innov_var;
 	VectorState H;
-	sym::ComputeFlowXyInnovVarAndHx(state_vector, P, range, R_LOS, FLT_EPSILON, &innov_var, &H);
+	sym::ComputeFlowXyInnovVarAndHx(state_vector, P, R_LOS, FLT_EPSILON, &innov_var, &H);
 	innov_var.copyTo(_aid_src_optical_flow.innovation_variance);

 	if ((innov_var(0) < R_LOS) || (innov_var(1) < R_LOS)) {
@@ -124,7 +118,7 @@ void Ekf::fuseOptFlow()

 		} 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::ComputeFlowYInnovVarAndH(state_vector, P, range, R_LOS, FLT_EPSILON, &_aid_src_optical_flow.innovation_variance[1], &H);
+			sym::ComputeFlowYInnovVarAndH(state_vector, P, R_LOS, FLT_EPSILON, &_aid_src_optical_flow.innovation_variance[1], &H);

 			// recalculate the innovation using the updated state
 			const Vector2f vel_body = predictFlowVelBody();
@@ -141,6 +135,10 @@ void Ekf::fuseOptFlow()

 		VectorState Kfusion = P * H / _aid_src_optical_flow.innovation_variance[index];

+		if (!update_terrain) {
+			Kfusion(State::terrain.idx) = 0.f;
+		}
+
 		if (measurementUpdate(Kfusion, H, _aid_src_optical_flow.observation_variance[index], _aid_src_optical_flow.innovation[index])) {
 			fused[index] = true;
 		}
@@ -165,10 +163,17 @@ float Ekf::predictFlowRange()

 	// calculate the height above the ground of the optical flow camera. Since earth frame is NED
 	// a positive offset in earth frame leads to a smaller height above the ground.
-	const float height_above_gnd_est = math::max(_terrain_vpos - _state.pos(2) - pos_offset_earth(2), fmaxf(_params.rng_gnd_clearance, 0.01f));
+	const float height_above_gnd_est = getHagl() - pos_offset_earth(2);

 	// calculate range from focal point to centre of image
-	return height_above_gnd_est / _R_to_earth(2, 2); // absolute distance to the frame region in view
+	float flow_range = height_above_gnd_est / _R_to_earth(2, 2); // absolute distance to the frame region in view
+
+	// avoid the flow prediction singularity at range = 0
+	if (fabsf(flow_range) < FLT_EPSILON) {
+		flow_range = signNoZero(flow_range) * FLT_EPSILON;
+	}
+
+	return flow_range;
 }

 Vector2f Ekf::predictFlowVelBody()
@@ -37,8 +37,9 @@
 */

 #include "ekf.h"
+#include "ekf_derivation/generated/compute_hagl_innov_var.h"

-void Ekf::controlRangeHeightFusion()
+void Ekf::controlRangeHaglFusion()
 {
 	static constexpr const char *HGT_SRC_NAME = "RNG";

@@ -93,58 +94,91 @@ void Ekf::controlRangeHeightFusion()
 	}

 	auto &aid_src = _aid_src_rng_hgt;
-	HeightBiasEstimator &bias_est = _rng_hgt_b_est;
-
-	bias_est.predict(_dt_ekf_avg);

 	if (rng_data_ready && _range_sensor.getSampleAddress()) {

-		const float measurement = math::max(_range_sensor.getDistBottom(), _params.rng_gnd_clearance);
-		const float measurement_var = sq(_params.range_noise) + sq(_params.range_noise_scaler * _range_sensor.getDistBottom());
+		updateRangeHagl(aid_src);
+		const bool measurement_valid = PX4_ISFINITE(aid_src.observation) && PX4_ISFINITE(aid_src.observation_variance);

-		const bool measurement_valid = PX4_ISFINITE(measurement) && PX4_ISFINITE(measurement_var);
-
-		// vertical position innovation - baro measurement has opposite sign to earth z axis
-		updateVerticalPositionAidStatus(aid_src,
-						_range_sensor.getSampleAddress()->time_us, // sample timestamp
-						-(measurement - bias_est.getBias()),       // observation
-						measurement_var + bias_est.getBiasVar(),   // observation variance
-						math::max(_params.range_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
-		if (measurement_valid && _range_sensor.isDataHealthy()) {
-			bias_est.setMaxStateNoise(sqrtf(measurement_var));
-			bias_est.setProcessNoiseSpectralDensity(_params.rng_hgt_bias_nsd);
-			bias_est.fuseBias(measurement - (-_state.pos(2)), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2));
-		}
-
-		// determine if we should use height aiding
-		const bool do_conditional_range_aid = (_params.rng_ctrl == static_cast<int32_t>(RngCtrl::CONDITIONAL))
-						      && isConditionalRangeAidSuitable();
-
-		const bool continuing_conditions_passing = ((_params.rng_ctrl == static_cast<int32_t>(RngCtrl::ENABLED)) || do_conditional_range_aid)
-				&& measurement_valid
-				&& _range_sensor.isDataHealthy();
+		const bool continuing_conditions_passing = ((_params.rng_ctrl == static_cast<int32_t>(RngCtrl::ENABLED))
+							    || (_params.rng_ctrl == static_cast<int32_t>(RngCtrl::CONDITIONAL)))
+							   && _control_status.flags.tilt_align
+							   && measurement_valid
+							   && _range_sensor.isDataHealthy()
+						           && _rng_consistency_check.isKinematicallyConsistent();

 		const bool starting_conditions_passing = continuing_conditions_passing
 				&& isNewestSampleRecent(_time_last_range_buffer_push, 2 * estimator::sensor::RNG_MAX_INTERVAL)
 				&& _range_sensor.isRegularlySendingData();

+
+		const bool do_conditional_range_aid = (_hagl_sensor_status.flags.range_finder || _control_status.flags.rng_hgt)
+						      && (_params.rng_ctrl == static_cast<int32_t>(RngCtrl::CONDITIONAL))
+						      && isConditionalRangeAidSuitable();
+
+		const bool do_range_aid = (_hagl_sensor_status.flags.range_finder || _control_status.flags.rng_hgt)
+					  && (_params.rng_ctrl == static_cast<int32_t>(RngCtrl::ENABLED));
+
 		if (_control_status.flags.rng_hgt) {
+			if (!(do_conditional_range_aid || do_range_aid)) {
+				ECL_INFO("stopping %s fusion", HGT_SRC_NAME);
+				stopRngHgtFusion();
+			}
+
+		} else {
+			if (_params.height_sensor_ref == static_cast<int32_t>(HeightSensor::RANGE)) {
+				if (do_conditional_range_aid) {
+					// Range finder is used while hovering to stabilize the height estimate. Don't reset but use it as height reference.
+					ECL_INFO("starting conditional %s height fusion", HGT_SRC_NAME);
+					_height_sensor_ref = HeightSensor::RANGE;
+
+					_control_status.flags.rng_hgt = true;
+					stopRngTerrFusion();
+
+					if (!_hagl_sensor_status.flags.flow && aid_src.innovation_rejected) {
+						resetTerrainToRng(aid_src);
+					}
+
+				} else if (do_range_aid) {
+					// Range finder is the primary height source, the ground is now the datum used
+					// to compute the local vertical position
+					ECL_INFO("starting %s height fusion, resetting height", HGT_SRC_NAME);
+					_height_sensor_ref = HeightSensor::RANGE;
+
+					_information_events.flags.reset_hgt_to_rng = true;
+					resetVerticalPositionTo(-aid_src.observation, aid_src.observation_variance);
+					_state.terrain = 0.f;
+					_control_status.flags.rng_hgt = true;
+					stopRngTerrFusion();
+
+					aid_src.time_last_fuse = _time_delayed_us;
+				}
+
+			} else {
+				if (do_conditional_range_aid || do_range_aid) {
+					ECL_INFO("starting %s height fusion", HGT_SRC_NAME);
+					_control_status.flags.rng_hgt = true;
+
+					if (!_hagl_sensor_status.flags.flow && aid_src.innovation_rejected) {
+						resetTerrainToRng(aid_src);
+					}
+				}
+			}
+		}
+
+		if (_control_status.flags.rng_hgt || _hagl_sensor_status.flags.range_finder) {
 			if (continuing_conditions_passing) {

-				fuseVerticalPosition(aid_src);
+				fuseHaglRng(aid_src, _control_status.flags.rng_hgt, _hagl_sensor_status.flags.range_finder);

 				const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.hgt_fusion_timeout_max);

-				if (isHeightResetRequired()) {
+				if (isHeightResetRequired() && _control_status.flags.rng_hgt) {
 					// All height sources are failing
 					ECL_WARN("%s height fusion reset required, all height sources failing", HGT_SRC_NAME);

 					_information_events.flags.reset_hgt_to_rng = true;
-					resetVerticalPositionTo(-(measurement - bias_est.getBias()));
-					bias_est.setBias(_state.pos(2) + measurement);
+					resetVerticalPositionTo(-(aid_src.observation - _state.terrain));

 					// reset vertical velocity
 					resetVerticalVelocityToZero();
@@ -153,99 +187,120 @@ void Ekf::controlRangeHeightFusion()

 				} else if (is_fusion_failing) {
 					// Some other height source is still working
-					ECL_WARN("stopping %s height fusion, fusion failing", HGT_SRC_NAME);
-					stopRngHgtFusion();
-					_control_status.flags.rng_fault = true;
-					_range_sensor.setFaulty();
+					if (_hagl_sensor_status.flags.flow) {
+						ECL_WARN("stopping %s fusion, fusion failing", HGT_SRC_NAME);
+						stopRngHgtFusion();
+						stopRngTerrFusion();
+
+					} else {
+						resetTerrainToRng(aid_src);
+					}
 				}

 			} else {
-				ECL_WARN("stopping %s height fusion, continuing conditions failing", HGT_SRC_NAME);
+				ECL_WARN("stopping %s fusion, continuing conditions failing", HGT_SRC_NAME);
 				stopRngHgtFusion();
+				stopRngTerrFusion();
 			}

 		} else {
 			if (starting_conditions_passing) {
-				if ((_params.height_sensor_ref == static_cast<int32_t>(HeightSensor::RANGE))
-				    && (_params.rng_ctrl == static_cast<int32_t>(RngCtrl::CONDITIONAL))
-				) {
-					// Range finder is used while hovering to stabilize the height estimate. Don't reset but use it as height reference.
-					ECL_INFO("starting conditional %s height fusion", HGT_SRC_NAME);
-					_height_sensor_ref = HeightSensor::RANGE;
-					bias_est.setBias(_state.pos(2) + measurement);
-
-				} else if ((_params.height_sensor_ref == static_cast<int32_t>(HeightSensor::RANGE))
-					   && (_params.rng_ctrl != static_cast<int32_t>(RngCtrl::CONDITIONAL))
-				) {
-					// Range finder is the primary height source, the ground is now the datum used
-					// to compute the local vertical position
-					ECL_INFO("starting %s height fusion, resetting height", HGT_SRC_NAME);
-					_height_sensor_ref = HeightSensor::RANGE;
-
-					_information_events.flags.reset_hgt_to_rng = true;
-					resetVerticalPositionTo(-measurement, measurement_var);
-					bias_est.reset();
+				if (_hagl_sensor_status.flags.flow) {
+					if (!aid_src.innovation_rejected) {
+						_hagl_sensor_status.flags.range_finder = true;
+						fuseHaglRng(aid_src, _control_status.flags.rng_hgt, _hagl_sensor_status.flags.range_finder);
+					}

 				} else {
-					ECL_INFO("starting %s height fusion", HGT_SRC_NAME);
-					bias_est.setBias(_state.pos(2) + measurement);
-				}
+					if (aid_src.innovation_rejected) {
+						resetTerrainToRng(aid_src);
+					}

-				aid_src.time_last_fuse = _time_delayed_us;
-				bias_est.setFusionActive();
-				_control_status.flags.rng_hgt = true;
+					_hagl_sensor_status.flags.range_finder = true;
+				}
 			}
 		}

-	} else if (_control_status.flags.rng_hgt
+	} else if ((_control_status.flags.rng_hgt || _hagl_sensor_status.flags.range_finder)
 		   && !isNewestSampleRecent(_time_last_range_buffer_push, 2 * estimator::sensor::RNG_MAX_INTERVAL)) {
 		// No data anymore. Stop until it comes back.
-		ECL_WARN("stopping %s height fusion, no data", HGT_SRC_NAME);
+		ECL_WARN("stopping %s fusion, no data", HGT_SRC_NAME);
 		stopRngHgtFusion();
+		stopRngTerrFusion();
 	}
 }

+void Ekf::updateRangeHagl(estimator_aid_source1d_s &aid_src)
+{
+	aid_src.observation = math::max(_range_sensor.getDistBottom(), _params.rng_gnd_clearance);
+	aid_src.innovation = getHagl() - aid_src.observation;
+
+	const float observation_variance = getRngVar();
+	float innovation_variance;
+	sym::ComputeHaglInnovVar(P, observation_variance, &innovation_variance);
+
+	const float innov_gate = math::max(_params.range_innov_gate, 1.f);
+	updateAidSourceStatus(aid_src,
+			      _range_sensor.getSampleAddress()->time_us,                           // sample timestamp
+			      math::max(_range_sensor.getDistBottom(), _params.rng_gnd_clearance), // observation
+			      observation_variance,                                                // observation variance
+			      getHagl() - aid_src.observation,                                     // innovation
+			      innovation_variance,                                                 // innovation variance
+			      math::max(_params.range_innov_gate, 1.f));                            // innovation gate
+
+	// z special case if there is bad vertical acceleration data, then don't reject measurement,
+	// but limit innovation to prevent spikes that could destabilise the filter
+	if (_fault_status.flags.bad_acc_vertical && aid_src.innovation_rejected) {
+		const float innov_limit = innov_gate * sqrtf(aid_src.innovation_variance);
+		aid_src.innovation = math::constrain(aid_src.innovation, -innov_limit, innov_limit);
+		aid_src.innovation_rejected = false;
+	}
+}
+
+float Ekf::getRngVar() const
+{
+	return fmaxf(
+		     P(State::pos.idx + 2, State::pos.idx + 2)
+		     + sq(_params.range_noise)
+		     + sq(_params.range_noise_scaler * _range_sensor.getRange()),
+	       0.f);
+}
+
+void Ekf::resetTerrainToRng(estimator_aid_source1d_s &aid_src)
+{
+	_state.terrain = _state.pos(2) + aid_src.observation;
+	P.uncorrelateCovarianceSetVariance<State::terrain.dof>(State::terrain.idx, aid_src.observation_variance);
+	_terrain_vpos_reset_counter++;
+
+	aid_src.time_last_fuse = _time_delayed_us;
+}
+
 bool Ekf::isConditionalRangeAidSuitable()
 {
-#if defined(CONFIG_EKF2_TERRAIN)
+	// check if we can use range finder measurements to estimate height, use hysteresis to avoid rapid switching
+	// Note that the 0.7 coefficients and the innovation check are arbitrary values but work well in practice
+	float range_hagl_max = _params.max_hagl_for_range_aid;
+	float max_vel_xy = _params.max_vel_for_range_aid;

-	if (_control_status.flags.in_air
-	    && _range_sensor.isHealthy()
-	    && isTerrainEstimateValid()) {
-		// check if we can use range finder measurements to estimate height, use hysteresis to avoid rapid switching
-		// Note that the 0.7 coefficients and the innovation check are arbitrary values but work well in practice
-		float range_hagl_max = _params.max_hagl_for_range_aid;
-		float max_vel_xy = _params.max_vel_for_range_aid;
+	const float hagl_test_ratio = _aid_src_rng_hgt.test_ratio;

-		const float hagl_innov = _aid_src_terrain_range_finder.innovation;
-		const float hagl_innov_var = _aid_src_terrain_range_finder.innovation_variance;
+	bool is_hagl_stable = (hagl_test_ratio < 1.f);

-		const float hagl_test_ratio = (hagl_innov * hagl_innov / (sq(_params.range_aid_innov_gate) * hagl_innov_var));
-
-		bool is_hagl_stable = (hagl_test_ratio < 1.f);
-
-		if (!_control_status.flags.rng_hgt) {
-			range_hagl_max = 0.7f * _params.max_hagl_for_range_aid;
-			max_vel_xy = 0.7f * _params.max_vel_for_range_aid;
-			is_hagl_stable = (hagl_test_ratio < 0.01f);
-		}
-
-		const float range_hagl = _terrain_vpos - _state.pos(2);
-
-		const bool is_in_range = (range_hagl < range_hagl_max);
-
-		bool is_below_max_speed = true;
-
-		if (isHorizontalAidingActive()) {
-			is_below_max_speed = !_state.vel.xy().longerThan(max_vel_xy);
-		}
-
-		return is_in_range && is_hagl_stable && is_below_max_speed;
+	if (!_control_status.flags.rng_hgt) {
+		range_hagl_max = 0.7f * _params.max_hagl_for_range_aid;
+		max_vel_xy = 0.7f * _params.max_vel_for_range_aid;
+		is_hagl_stable = (hagl_test_ratio < 0.01f);
 	}

-#endif // CONFIG_EKF2_TERRAIN
+	const bool is_in_range = (getHagl() < range_hagl_max);

-	return false;
+	bool is_below_max_speed = true;
+
+	if (isHorizontalAidingActive()) {
+		is_below_max_speed = !_state.vel.xy().longerThan(max_vel_xy);
+	}
+
+	return is_in_range && is_hagl_stable && is_below_max_speed;
 }

 void Ekf::stopRngHgtFusion()
@@ -256,8 +311,11 @@ void Ekf::stopRngHgtFusion()
 			_height_sensor_ref = HeightSensor::UNKNOWN;
 		}

-		_rng_hgt_b_est.setFusionInactive();
-
 		_control_status.flags.rng_hgt = false;
 	}
 }
+
+void Ekf::stopRngTerrFusion()
+{
+	_hagl_sensor_status.flags.range_finder = false;
+}
@@ -0,0 +1,70 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2024 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "ekf.h"
+#include "ekf_derivation/generated/compute_hagl_h.h"
+
+bool Ekf::fuseHaglRng(estimator_aid_source1d_s &aid_src, bool update_height, bool update_terrain)
+{
+	if (aid_src.innovation_rejected) {
+		_innov_check_fail_status.flags.reject_hagl = true;
+		return false;
+	}
+
+	VectorState H;
+
+	sym::ComputeHaglH(&H);
+
+	// calculate the Kalman gain
+	VectorState K = P * H / aid_src.innovation_variance;
+
+	if (!update_terrain) {
+		K(State::terrain.idx) = 0.f;
+	}
+
+	if (!update_height) {
+		const float k_terrain = K(State::terrain.idx);
+		K.zero();
+		K(State::terrain.idx) = k_terrain;
+	}
+
+	measurementUpdate(K, H, aid_src.observation_variance, aid_src.innovation);
+
+	// record last successful fusion event
+	_innov_check_fail_status.flags.reject_hagl = false;
+
+	aid_src.time_last_fuse = _time_delayed_us;
+	aid_src.fused = true;
+
+	return true;
+}
@@ -64,16 +64,10 @@ void Ekf::controlBetaFusion(const imuSample &imu_delayed)
 			updateSideslip(_aid_src_sideslip);
 			_innov_check_fail_status.flags.reject_sideslip = _aid_src_sideslip.innovation_rejected;

-			// If starting wind state estimation, reset the wind states and covariances before fusing any data
-			if (!_control_status.flags.wind) {
-				// activate the wind states
-				_control_status.flags.wind = true;
-				// reset the timeout timers to prevent repeated resets
-				_aid_src_sideslip.time_last_fuse = imu_delayed.time_us;
+			if (!fuseSideslip(_aid_src_sideslip) && !_control_status.flags.wind) {
 				resetWindToZero();
 			}
-
-			fuseSideslip(_aid_src_sideslip);
+			_control_status.flags.wind = true;
 		}
 	}
 }
@@ -96,10 +90,10 @@ void Ekf::updateSideslip(estimator_aid_source1d_s &aid_src) const
 				 math::max(_params.beta_innov_gate, 1.f)); // innovation gate
 }

-void Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip)
+bool Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip)
 {
 	if (sideslip.innovation_rejected) {
-		return;
+		return false;
 	}

 	// determine if we need the sideslip fusion to correct states other than wind
@@ -125,7 +119,7 @@ void Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip)

 		ECL_ERR("sideslip badly conditioned - %s covariance reset", action_string);

-		return;
+		return false;
 	}

 	_fault_status.flags.bad_sideslip = false;
@@ -149,4 +143,5 @@ void Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip)
 	if (is_fused) {
 		sideslip.time_last_fuse = _time_delayed_us;
 	}
+	return is_fused;
 }
@@ -107,7 +107,7 @@ enum MagFuseType : uint8_t {
 #endif // CONFIG_EKF2_MAGNETOMETER

 #if defined(CONFIG_EKF2_TERRAIN)
-enum TerrainFusionMask : uint8_t {
+enum class TerrainFusionMask : uint8_t {
 	TerrainFuseRangeFinder = (1 << 0),
 	TerrainFuseOpticalFlow = (1 << 1)
 };
@@ -382,9 +382,6 @@ struct parameters {
 #endif // CONFIG_EKF2_SIDESLIP

 #if defined(CONFIG_EKF2_TERRAIN)
-	int32_t terrain_fusion_mode{TerrainFusionMask::TerrainFuseRangeFinder |
-				    TerrainFusionMask::TerrainFuseOpticalFlow}; ///< aiding source(s) selection bitmask for the terrain estimator
-
 	float terrain_p_noise{5.0f};            ///< process noise for terrain offset (m/sec)
 	float terrain_gradient{0.5f};           ///< gradient of terrain used to estimate process noise due to changing position (m/m)
 	const float terrain_timeout{10.f};      ///< maximum time for invalid bottom distance measurements before resetting terrain estimate (s)
@@ -401,10 +398,8 @@ struct parameters {
 	float range_delay_ms{5.0f};             ///< range finder measurement delay relative to the IMU (mSec)
 	float range_noise{0.1f};                ///< observation noise for range finder measurements (m)
 	float range_innov_gate{5.0f};           ///< range finder fusion innovation consistency gate size (STD)
-	float rng_hgt_bias_nsd{0.13f};          ///< process noise for range height bias estimation (m/s/sqrt(Hz))
 	float rng_sens_pitch{0.0f};             ///< Pitch offset of the range sensor (rad). Sensor points out along Z axis when offset is zero. Positive rotation is RH about Y axis.
 	float range_noise_scaler{0.0f};         ///< scaling from range measurement to noise (m/m)
-	const float vehicle_variance_scaler{0.0f};      ///< gain applied to vehicle height variance used in calculation of height above ground observation variance
 	float max_hagl_for_range_aid{5.0f};     ///< maximum height above ground for which we allow to use the range finder as height source (if rng_control == 1)
 	float max_vel_for_range_aid{1.0f};      ///< maximum ground velocity for which we allow to use the range finder as height source (if rng_control == 1)
 	float range_aid_innov_gate{1.0f};       ///< gate size used for innovation consistency checks for range aid fusion
@@ -521,7 +516,7 @@ union innovation_fault_status_u {
 		bool reject_yaw       : 1; ///< 7 - true if the yaw observation has been rejected
 		bool reject_airspeed  : 1; ///< 8 - true if the airspeed observation has been rejected
 		bool reject_sideslip  : 1; ///< 9 - true if the synthetic sideslip observation has been rejected
-		bool reject_hagl      : 1; ///< 10 - true if the height above ground observation has been rejected
+		bool reject_hagl      : 1; ///< 10 - unused
 		bool reject_optflow_X : 1; ///< 11 - true if the X optical flow observation has been rejected
 		bool reject_optflow_Y : 1; ///< 12 - true if the Y optical flow observation has been rejected
 	} flags;
@@ -642,6 +637,7 @@ union information_event_status_u {
 		bool reset_hgt_to_rng           : 1; ///< 15 - true when the vertical position state is reset to the rng measurement
 		bool reset_hgt_to_ev            : 1; ///< 16 - true when the vertical position state is reset to the ev measurement
 		bool reset_pos_to_ext_obs       : 1; ///< 17 - true when horizontal position was reset to an external observation while deadreckoning
+		bool reset_wind_to_ext_obs 	: 1; ///< 18 - true when wind states were reset to an external observation
 	} flags;
 	uint32_t value;
 };
@@ -144,6 +144,10 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
 	// Additional horizontal velocity data from an auxiliary sensor can be fused
 	controlAuxVelFusion();
 #endif // CONFIG_EKF2_AUXVEL
+	//
+#if defined(CONFIG_EKF2_TERRAIN)
+	controlTerrainFakeFusion();
+#endif // CONFIG_EKF2_TERRAIN

 	controlZeroInnovationHeadingUpdate();

@@ -99,6 +99,11 @@ void Ekf::initialiseCovariance()
 #if defined(CONFIG_EKF2_WIND)
 	resetWindCov();
 #endif // CONFIG_EKF2_WIND
+
+#if defined(CONFIG_EKF2_TERRAIN)
+	// use the ground clearance value as our uncertainty
+	P.uncorrelateCovarianceSetVariance<State::terrain.dof>(State::terrain.idx, sq(_params.rng_gnd_clearance));
+#endif // CONFIG_EKF2_TERRAIN
 }

 void Ekf::predictCovariance(const imuSample &imu_delayed)
@@ -190,18 +195,28 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)


 #if defined(CONFIG_EKF2_WIND)
-	if (_control_status.flags.wind) {
-		// wind vel: add process noise
-		float wind_vel_nsd_scaled = math::constrain(_params.wind_vel_nsd, 0.f, 1.f) * (1.f + _params.wind_vel_nsd_scaler * fabsf(_height_rate_lpf));
-		float wind_vel_process_noise = sq(wind_vel_nsd_scaled) * dt;
+	// wind vel: add process noise
+	float wind_vel_nsd_scaled = math::constrain(_params.wind_vel_nsd, 0.f,
+				    1.f) * (1.f + _params.wind_vel_nsd_scaler * fabsf(_height_rate_lpf));
+	float wind_vel_process_noise = sq(wind_vel_nsd_scaled) * dt;

-		for (unsigned index = 0; index < State::wind_vel.dof; index++) {
-			const unsigned i = State::wind_vel.idx + index;
-			P(i, i) += wind_vel_process_noise;
-		}
+	for (unsigned index = 0; index < State::wind_vel.dof; index++) {
+		const unsigned i = State::wind_vel.idx + index;
+		P(i, i) = fminf(P(i, i) + wind_vel_process_noise, _params.initial_wind_uncertainty);
 	}
 #endif // CONFIG_EKF2_WIND

+#if defined(CONFIG_EKF2_TERRAIN)
+	if (_height_sensor_ref != HeightSensor::RANGE) {
+		// predict the state variance growth where the state is the vertical position of the terrain underneath the vehicle
+		// process noise due to errors in vehicle height estimate
+		float terrain_process_noise = sq(imu_delayed.delta_vel_dt * _params.terrain_p_noise);
+
+		// process noise due to terrain gradient
+		terrain_process_noise += sq(imu_delayed.delta_vel_dt * _params.terrain_gradient) * (sq(_state.vel(0)) + sq(_state.vel(1)));
+		P(State::terrain.idx, State::terrain.idx) += terrain_process_noise;
+	}
+#endif // CONFIG_EKF2_TERRAIN

 	// covariance matrix is symmetrical, so copy upper half to lower half
 	for (unsigned row = 0; row < State::size; row++) {
@@ -239,6 +254,10 @@ void Ekf::constrainStateVariances()
 		constrainStateVarLimitRatio(State::wind_vel, 1e-6f, 1e6f);
 	}
 #endif // CONFIG_EKF2_WIND
+
+#if defined(CONFIG_EKF2_TERRAIN)
+	constrainStateVarLimitRatio(State::terrain, 0.f, 1e4f);
+#endif // CONFIG_EKF2_TERRAIN
 }

 void Ekf::constrainStateVar(const IdxDof &state, float min, float max)
@@ -314,11 +333,3 @@ void Ekf::resetMagCov()
 	P.uncorrelateCovarianceSetVariance<State::mag_B.dof>(State::mag_B.idx, sq(_params.mag_noise));
 }
 #endif // CONFIG_EKF2_MAGNETOMETER
-
-#if defined(CONFIG_EKF2_WIND)
-void Ekf::resetWindCov()
-{
-	// start with a small initial uncertainty to improve the initial estimate
-	P.uncorrelateCovarianceSetVariance<State::wind_vel.dof>(State::wind_vel.idx, sq(_params.initial_wind_uncertainty));
-}
-#endif // CONFIG_EKF2_WIND
@@ -71,6 +71,11 @@ void Ekf::reset()
 #if defined(CONFIG_EKF2_WIND)
 	_state.wind_vel.setZero();
 #endif // CONFIG_EKF2_WIND
+	//
+#if defined(CONFIG_EKF2_TERRAIN)
+	// assume a ground clearance
+	_state.terrain = _state.pos(2) + _params.rng_gnd_clearance;
+#endif // CONFIG_EKF2_TERRAIN

 #if defined(CONFIG_EKF2_RANGE_FINDER)
 	_range_sensor.setPitchOffset(_params.rng_sens_pitch);
@@ -122,7 +127,10 @@ void Ekf::reset()

 	_time_bad_vert_accel = 0;
 	_time_good_vert_accel = 0;
-	_clip_counter = 0;
+
+	for (auto &clip_count : _clip_counter) {
+		clip_count = 0;
+	}

 	_zero_velocity_update.reset();
 }
@@ -160,11 +168,6 @@ bool Ekf::update()
 		// control fusion of observation data
 		controlFusionModes(imu_sample_delayed);

-#if defined(CONFIG_EKF2_TERRAIN)
-		// run a separate filter for terrain estimation
-		runTerrainEstimator(imu_sample_delayed);
-#endif // CONFIG_EKF2_TERRAIN
-
 		_output_predictor.correctOutputStates(imu_sample_delayed.time_us, _state.quat_nominal, _state.vel, _state.pos, _state.gyro_bias, _state.accel_bias);

 		return true;
@@ -200,11 +203,6 @@ bool Ekf::initialiseFilter()
 	// initialise the state covariance matrix now we have starting values for all the states
 	initialiseCovariance();

-#if defined(CONFIG_EKF2_TERRAIN)
-	// Initialise the terrain estimator
-	initHagl();
-#endif // CONFIG_EKF2_TERRAIN
-
 	// reset the output predictor state history to match the EKF initial values
 	_output_predictor.alignOutputFilter(_state.quat_nominal, _state.vel, _state.pos);

@@ -377,6 +375,14 @@ void Ekf::print_status()
 	      );
 #endif // CONFIG_EKF2_WIND

+#if defined(CONFIG_EKF2_TERRAIN)
+	printf("Terrain position (%d): %.3f var: %.1e\n",
+	       State::terrain.idx,
+	       (double)_state.terrain,
+	       (double)getStateVariance<State::terrain>()(0)
+	      );
+#endif // CONFIG_EKF2_TERRAIN
+
 	printf("\nP:\n");
 	P.print();

@@ -73,7 +73,6 @@ class Ekf final : public EstimatorInterface
 public:
 	typedef matrix::Vector<float, State::size> VectorState;
 	typedef matrix::SquareMatrix<float, State::size> SquareMatrixState;
-	typedef matrix::SquareMatrix<float, 2> Matrix2f;

 	Ekf()
 	{
@@ -104,27 +103,19 @@ public:
 	uint8_t getTerrainEstimateSensorBitfield() const { return _hagl_sensor_status.value; }

 	// get the estimated terrain vertical position relative to the NED origin
-	float getTerrainVertPos() const { return _terrain_vpos; };
+	float getTerrainVertPos() const { return _state.terrain; };
+	float getHagl() const { return _state.terrain - _state.pos(2); }

 	// get the number of times the vertical terrain position has been reset
 	uint8_t getTerrainVertPosResetCounter() const { return _terrain_vpos_reset_counter; };

 	// get the terrain variance
-	float get_terrain_var() const { return _terrain_var; }
-
-# if defined(CONFIG_EKF2_RANGE_FINDER)
-	const auto &aid_src_terrain_range_finder() const { return _aid_src_terrain_range_finder; }
-# endif // CONFIG_EKF2_RANGE_FINDER
-
-# if defined(CONFIG_EKF2_OPTICAL_FLOW)
-	const auto &aid_src_terrain_optical_flow() const { return _aid_src_terrain_optical_flow; }
-# endif // CONFIG_EKF2_OPTICAL_FLOW
+	float getTerrainVariance() const { return P(State::terrain.idx, State::terrain.idx); }

 #endif // CONFIG_EKF2_TERRAIN

 #if defined(CONFIG_EKF2_RANGE_FINDER)
 	// range height
-	const BiasEstimator::status &getRngHgtBiasEstimatorStatus() const { return _rng_hgt_b_est.getStatus(); }
 	const auto &aid_src_rng_hgt() const { return _aid_src_rng_hgt; }

 	float getHaglRateInnov() const { return _rng_consistency_check.getInnov(); }
@@ -383,13 +374,17 @@ public:
 		*counter = _state_reset_status.reset_count.quat;
 	}

-	// get EKF innovation consistency check status information comprising of:
-	// status - a bitmask integer containing the pass/fail status for each EKF measurement innovation consistency check
-	// Innovation Test Ratios - these are the ratio of the innovation to the acceptance threshold.
-	// A value > 1 indicates that the sensor measurement has exceeded the maximum acceptable level and has been rejected by the EKF
-	// Where a measurement type is a vector quantity, eg magnetometer, GPS position, etc, the maximum value is returned.
-	void get_innovation_test_status(uint16_t &status, float &mag, float &vel, float &pos, float &hgt, float &tas,
-					float &hagl, float &beta) const;
+	float getHeadingInnovationTestRatio() const;
+
+	float getVelocityInnovationTestRatio() const;
+
+	float getHorizontalPositionInnovationTestRatio() const;
+	float getVerticalPositionInnovationTestRatio() const;
+
+	float getAirspeedInnovationTestRatio() const;
+	float getSyntheticSideslipInnovationTestRatio() const;
+
+	float getHeightAboveGroundInnovationTestRatio() const;

 	// return a bitmask integer that describes which state estimates are valid
 	void get_ekf_soln_status(uint16_t *status) const;
@@ -508,6 +503,16 @@ public:

 	void resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float accuracy, uint64_t timestamp_observation);

+	/**
+	* @brief Resets the wind states to an external observation
+	*
+	* @param wind_speed The wind speed in m/s
+	* @param wind_direction The azimuth (from true north) from where the wind is flowing from in degrees (0 to 360)
+	* @param wind_speed_accuracy The 1 sigma accuracy of the wind speed estimate in m/s
+	* @param wind_direction_accuracy The 1 sigma accuracy of the wind direction estimate in degrees
+	*/
+	void resetWindToExternalObservation(float wind_speed, float wind_direction, float wind_speed_accuracy, float wind_direction_accuracy);
+
 	void updateParameters();

 	friend class AuxGlobalPosition;
@@ -588,27 +593,14 @@ private:

 #if defined(CONFIG_EKF2_TERRAIN)
 	// Terrain height state estimation
-	float _terrain_vpos{0.0f};		///< estimated vertical position of the terrain underneath the vehicle in local NED frame (m)
-	float _terrain_var{1e4f};		///< variance of terrain position estimate (m**2)
 	uint8_t _terrain_vpos_reset_counter{0};	///< number of times _terrain_vpos has been reset

 	terrain_fusion_status_u _hagl_sensor_status{}; ///< Struct indicating type of sensor used to estimate height above ground
 	float _last_on_ground_posD{0.0f};	///< last vertical position when the in_air status was false (m)
-
-# if defined(CONFIG_EKF2_RANGE_FINDER)
-	estimator_aid_source1d_s _aid_src_terrain_range_finder{};
-	uint64_t _time_last_healthy_rng_data{0};
-# endif // CONFIG_EKF2_RANGE_FINDER
-
-# if defined(CONFIG_EKF2_OPTICAL_FLOW)
-	estimator_aid_source2d_s _aid_src_terrain_optical_flow{};
-# endif // CONFIG_EKF2_OPTICAL_FLOW
-
 #endif // CONFIG_EKF2_TERRAIN

 #if defined(CONFIG_EKF2_RANGE_FINDER)
 	estimator_aid_source1d_s _aid_src_rng_hgt{};
-	HeightBiasEstimator _rng_hgt_b_est{HeightSensor::RANGE, _height_sensor_ref};
 #endif // CONFIG_EKF2_RANGE_FINDER

 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
@@ -723,7 +715,7 @@ private:
 	// imu fault status
 	uint64_t _time_bad_vert_accel{0};	///< last time a bad vertical accel was detected (uSec)
 	uint64_t _time_good_vert_accel{0};	///< last time a good vertical accel was detected (uSec)
-	uint16_t _clip_counter{0};		///< counter that increments when clipping ad decrements when not
+	uint16_t _clip_counter[3];		///< counter per axis that increments when clipping ad decrements when not

 	// initialise filter states of both the delayed ekf and the real time complementary filter
 	bool initialiseFilter(void);
@@ -780,7 +772,7 @@ private:

 	// fuse synthetic zero sideslip measurement
 	void updateSideslip(estimator_aid_source1d_s &_aid_src_sideslip) const;
-	void fuseSideslip(estimator_aid_source1d_s &_aid_src_sideslip);
+	bool fuseSideslip(estimator_aid_source1d_s &_aid_src_sideslip);
 #endif // CONFIG_EKF2_SIDESLIP

 #if defined(CONFIG_EKF2_DRAG_FUSION)
@@ -824,41 +816,30 @@ private:
 	bool fuseVelocity(estimator_aid_source3d_s &vel_aid_src);

 #if defined(CONFIG_EKF2_TERRAIN)
-	// terrain vertical position estimator
-	void initHagl();
-	void runTerrainEstimator(const imuSample &imu_delayed);
-	void predictHagl(const imuSample &imu_delayed);
-
-	float getTerrainVPos() const { return isTerrainEstimateValid() ? _terrain_vpos : _last_on_ground_posD; }
-
-	void controlHaglFakeFusion();
-	void terrainHandleVerticalPositionReset(float delta_z);
+	void initTerrain();
+	float getTerrainVPos() const { return isTerrainEstimateValid() ? _state.terrain : _last_on_ground_posD; }
+	void controlTerrainFakeFusion();

 # if defined(CONFIG_EKF2_RANGE_FINDER)
 	// update the terrain vertical position estimate using a height above ground measurement from the range finder
-	void controlHaglRngFusion();
-	void updateHaglRng(estimator_aid_source1d_s &aid_src) const;
-	void fuseHaglRng(estimator_aid_source1d_s &aid_src);
-	void resetHaglRng();
-	void stopHaglRngFusion();
+	bool fuseHaglRng(estimator_aid_source1d_s &aid_src, bool update_height, bool update_terrain);
+	void updateRangeHagl(estimator_aid_source1d_s &aid_src);
+	void resetTerrainToRng(estimator_aid_source1d_s &aid_src);
 	float getRngVar() const;
 # endif // CONFIG_EKF2_RANGE_FINDER

 # if defined(CONFIG_EKF2_OPTICAL_FLOW)
-	// update the terrain vertical position estimate using an optical flow measurement
-	void controlHaglFlowFusion();
-	void resetHaglFlow();
-	void stopHaglFlowFusion();
-	void fuseFlowForTerrain(estimator_aid_source2d_s &flow);
+	void resetTerrainToFlow();
 # endif // CONFIG_EKF2_OPTICAL_FLOW

 #endif // CONFIG_EKF2_TERRAIN

 #if defined(CONFIG_EKF2_RANGE_FINDER)
 	// range height
-	void controlRangeHeightFusion();
+	void controlRangeHaglFusion();
 	bool isConditionalRangeAidSuitable();
 	void stopRngHgtFusion();
+	void stopRngTerrFusion();
 #endif // CONFIG_EKF2_RANGE_FINDER

 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
@@ -879,7 +860,7 @@ private:

 	// fuse optical flow line of sight rate measurements
 	void updateOptFlow(estimator_aid_source2d_s &aid_src);
-	void fuseOptFlow();
+	void fuseOptFlow(bool update_terrain);
 	float predictFlowRange();
 	Vector2f predictFlowVelBody();
 #endif // CONFIG_EKF2_OPTICAL_FLOW
@@ -211,7 +211,7 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
 		if (_control_status.flags.opt_flow) {
 			float gndclearance = math::max(_params.rng_gnd_clearance, 0.1f);
-			vel_err_conservative = math::max((_terrain_vpos - _state.pos(2)), gndclearance) * Vector2f(_aid_src_optical_flow.innovation).norm();
+			vel_err_conservative = math::max(getHagl(), gndclearance) * Vector2f(_aid_src_optical_flow.innovation).norm();
 		}
 #endif // CONFIG_EKF2_OPTICAL_FLOW

@@ -271,7 +271,7 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl
 		const float flow_hagl_min = fmaxf(rangefinder_hagl_min, _flow_min_distance);
 		const float flow_hagl_max = fminf(rangefinder_hagl_max, _flow_max_distance);

-		const float flow_constrained_height = math::constrain(_terrain_vpos - _state.pos(2), flow_hagl_min, flow_hagl_max);
+		const float flow_constrained_height = math::constrain(getHagl(), flow_hagl_min, flow_hagl_max);

 		// Allow ground relative velocity to use 50% of available flow sensor range to allow for angular motion
 		const float flow_vxy_max = 0.5f * _flow_max_rate * flow_constrained_height;
@@ -301,131 +301,187 @@ void Ekf::resetAccelBias()
 	resetAccelBiasCov();
 }

-void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, float &pos, float &hgt, float &tas,
-				     float &hagl, float &beta) const
+float Ekf::getHeadingInnovationTestRatio() const
 {
-	// return the integer bitmask containing the consistency check pass/fail status
-	status = _innov_check_fail_status.value;
-
-	// return the largest magnetometer innovation test ratio
-	mag = 0.f;
+	// return the largest heading innovation test ratio
+	float test_ratio = 0.f;

 #if defined(CONFIG_EKF2_MAGNETOMETER)
 	if (_control_status.flags.mag_hdg ||_control_status.flags.mag_3D) {
-		mag = math::max(mag, sqrtf(Vector3f(_aid_src_mag.test_ratio).max()));
+		for (auto &test_ratio_filtered : _aid_src_mag.test_ratio_filtered) {
+			test_ratio = math::max(test_ratio, fabsf(test_ratio_filtered));
+		}
 	}
 #endif // CONFIG_EKF2_MAGNETOMETER

 #if defined(CONFIG_EKF2_GNSS_YAW)
 	if (_control_status.flags.gps_yaw) {
-		mag = math::max(mag, sqrtf(_aid_src_gnss_yaw.test_ratio));
+		test_ratio = math::max(test_ratio, fabsf(_aid_src_gnss_yaw.test_ratio_filtered));
 	}
 #endif // CONFIG_EKF2_GNSS_YAW

 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	if (_control_status.flags.ev_yaw) {
-		mag = math::max(mag, sqrtf(_aid_src_ev_yaw.test_ratio));
+		test_ratio = math::max(test_ratio, fabsf(_aid_src_ev_yaw.test_ratio_filtered));
 	}
 #endif // CONFIG_EKF2_EXTERNAL_VISION

-	// return the largest velocity and position innovation test ratio
-	vel = NAN;
-	pos = NAN;
+	return sqrtf(test_ratio);
+}
+
+float Ekf::getVelocityInnovationTestRatio() const
+{
+	// return the largest velocity innovation test ratio
+	float test_ratio = -1.f;

 #if defined(CONFIG_EKF2_GNSS)
 	if (_control_status.flags.gps) {
-		float gps_vel = sqrtf(Vector3f(_aid_src_gnss_vel.test_ratio).max());
-		vel = math::max(gps_vel, FLT_MIN);
-
-		float gps_pos = sqrtf(Vector2f(_aid_src_gnss_pos.test_ratio).max());
-		pos = math::max(gps_pos, FLT_MIN);
+		for (int i = 0; i < 3; i++) {
+			test_ratio = math::max(test_ratio, fabsf(_aid_src_gnss_vel.test_ratio_filtered[i]));
+		}
 	}
 #endif // CONFIG_EKF2_GNSS

 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	if (_control_status.flags.ev_vel) {
-		float ev_vel = sqrtf(Vector3f(_aid_src_ev_vel.test_ratio).max());
-		vel = math::max(vel, ev_vel, FLT_MIN);
-	}
-
-	if (_control_status.flags.ev_pos) {
-		float ev_pos = sqrtf(Vector2f(_aid_src_ev_pos.test_ratio).max());
-		pos = math::max(pos, ev_pos, FLT_MIN);
+		for (int i = 0; i < 3; i++) {
+			test_ratio = math::max(test_ratio, fabsf(_aid_src_ev_vel.test_ratio_filtered[i]));
+		}
 	}
 #endif // CONFIG_EKF2_EXTERNAL_VISION

 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
 	if (isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow)) {
-		float of_vel = sqrtf(Vector2f(_aid_src_optical_flow.test_ratio).max());
-		vel = math::max(of_vel, FLT_MIN);
+		for (auto &test_ratio_filtered : _aid_src_optical_flow.test_ratio_filtered) {
+			test_ratio = math::max(test_ratio, fabsf(test_ratio_filtered));
+		}
 	}
 #endif // CONFIG_EKF2_OPTICAL_FLOW

+	if (PX4_ISFINITE(test_ratio) && (test_ratio >= 0.f)) {
+		return sqrtf(test_ratio);
+	}
+
+	return NAN;
+}
+
+float Ekf::getHorizontalPositionInnovationTestRatio() const
+{
+	// return the largest position innovation test ratio
+	float test_ratio = -1.f;
+
+#if defined(CONFIG_EKF2_GNSS)
+	if (_control_status.flags.gps) {
+		for (auto &test_ratio_filtered : _aid_src_gnss_pos.test_ratio_filtered) {
+			test_ratio = math::max(test_ratio, fabsf(test_ratio_filtered));
+		}
+	}
+#endif // CONFIG_EKF2_GNSS
+
+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
+	if (_control_status.flags.ev_pos) {
+		for (auto &test_ratio_filtered : _aid_src_ev_pos.test_ratio_filtered) {
+			test_ratio = math::max(test_ratio, fabsf(test_ratio_filtered));
+		}
+	}
+#endif // CONFIG_EKF2_EXTERNAL_VISION
+
+#if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME)
+	if (_control_status.flags.aux_gpos) {
+		test_ratio = math::max(test_ratio, fabsf(_aux_global_position.test_ratio_filtered()));
+	}
+#endif // CONFIG_EKF2_AUX_GLOBAL_POSITION
+
+	if (PX4_ISFINITE(test_ratio) && (test_ratio >= 0.f)) {
+		return sqrtf(test_ratio);
+	}
+
+	return NAN;
+}
+
+float Ekf::getVerticalPositionInnovationTestRatio() const
+{
 	// return the combined vertical position innovation test ratio
 	float hgt_sum = 0.f;
 	int n_hgt_sources = 0;

 #if defined(CONFIG_EKF2_BAROMETER)
 	if (_control_status.flags.baro_hgt) {
-		hgt_sum += sqrtf(_aid_src_baro_hgt.test_ratio);
+		hgt_sum += sqrtf(fabsf(_aid_src_baro_hgt.test_ratio_filtered));
 		n_hgt_sources++;
 	}
 #endif // CONFIG_EKF2_BAROMETER

 #if defined(CONFIG_EKF2_GNSS)
 	if (_control_status.flags.gps_hgt) {
-		hgt_sum += sqrtf(_aid_src_gnss_hgt.test_ratio);
+		hgt_sum += sqrtf(fabsf(_aid_src_gnss_hgt.test_ratio_filtered));
 		n_hgt_sources++;
 	}
 #endif // CONFIG_EKF2_GNSS

 #if defined(CONFIG_EKF2_RANGE_FINDER)
 	if (_control_status.flags.rng_hgt) {
-		hgt_sum += sqrtf(_aid_src_rng_hgt.test_ratio);
+		hgt_sum += sqrtf(fabsf(_aid_src_rng_hgt.test_ratio_filtered));
 		n_hgt_sources++;
 	}
 #endif // CONFIG_EKF2_RANGE_FINDER

 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	if (_control_status.flags.ev_hgt) {
-		hgt_sum += sqrtf(_aid_src_ev_hgt.test_ratio);
+		hgt_sum += sqrtf(fabsf(_aid_src_ev_hgt.test_ratio_filtered));
 		n_hgt_sources++;
 	}
 #endif // CONFIG_EKF2_EXTERNAL_VISION

 	if (n_hgt_sources > 0) {
-		hgt = math::max(hgt_sum / static_cast<float>(n_hgt_sources), FLT_MIN);
-
-	} else {
-		hgt = NAN;
+		return math::max(hgt_sum / static_cast<float>(n_hgt_sources), FLT_MIN);
 	}

+	return NAN;
+}
+
+float Ekf::getAirspeedInnovationTestRatio() const
+{
 #if defined(CONFIG_EKF2_AIRSPEED)
-	// return the airspeed fusion innovation test ratio
-	tas = sqrtf(_aid_src_airspeed.test_ratio);
+	if (_control_status.flags.fuse_aspd) {
+		// return the airspeed fusion innovation test ratio
+		return sqrtf(fabsf(_aid_src_airspeed.test_ratio_filtered));
+	}
 #endif // CONFIG_EKF2_AIRSPEED

-	hagl = NAN;
+	return NAN;
+}
+
+float Ekf::getSyntheticSideslipInnovationTestRatio() const
+{
+#if defined(CONFIG_EKF2_SIDESLIP)
+	if (_control_status.flags.fuse_beta) {
+		// return the synthetic sideslip innovation test ratio
+		return sqrtf(fabsf(_aid_src_sideslip.test_ratio_filtered));
+	}
+#endif // CONFIG_EKF2_SIDESLIP
+
+	return NAN;
+}
+
+float Ekf::getHeightAboveGroundInnovationTestRatio() const
+{
+	float test_ratio = -1.f;
+
 #if defined(CONFIG_EKF2_TERRAIN)
 # if defined(CONFIG_EKF2_RANGE_FINDER)
 	if (_hagl_sensor_status.flags.range_finder) {
 		// return the terrain height innovation test ratio
-		hagl = sqrtf(_aid_src_terrain_range_finder.test_ratio);
+		test_ratio = math::max(test_ratio, fabsf(_aid_src_rng_hgt.test_ratio_filtered));
 	}
-#endif // CONFIG_EKF2_RANGE_FINDER
-
-# if defined(CONFIG_EKF2_OPTICAL_FLOW)
-	if (_hagl_sensor_status.flags.flow) {
-		// return the terrain height innovation test ratio
-		hagl = sqrtf(math::max(_aid_src_terrain_optical_flow.test_ratio[0], _aid_src_terrain_optical_flow.test_ratio[1]));
-	}
-# endif // CONFIG_EKF2_OPTICAL_FLOW
+# endif // CONFIG_EKF2_RANGE_FINDER
 #endif // CONFIG_EKF2_TERRAIN

-#if defined(CONFIG_EKF2_SIDESLIP)
-	// return the synthetic sideslip innovation test ratio
-	beta = sqrtf(_aid_src_sideslip.test_ratio);
-#endif // CONFIG_EKF2_SIDESLIP
+	if (PX4_ISFINITE(test_ratio) && (test_ratio >= 0.f)) {
+		return sqrtf(test_ratio);
+	}
+
+	return NAN;
 }

 void Ekf::get_ekf_soln_status(uint16_t *status) const
@@ -504,6 +560,10 @@ void Ekf::fuse(const VectorState &K, float innovation)
 		_state.wind_vel = matrix::constrain(_state.wind_vel - K.slice<State::wind_vel.dof, 1>(State::wind_vel.idx, 0) * innovation, -1.e2f, 1.e2f);
 	}
 #endif // CONFIG_EKF2_WIND
+
+#if defined(CONFIG_EKF2_TERRAIN)
+	_state.terrain = math::constrain(_state.terrain - K(State::terrain.idx) * innovation, -1e4f, 1e4f);
+#endif // CONFIG_EKF2_TERRAIN
 }

 void Ekf::updateDeadReckoningStatus()
@@ -605,7 +665,7 @@ void Ekf::updateGroundEffect()
 #if defined(CONFIG_EKF2_TERRAIN)
 		if (isTerrainEstimateValid()) {
 			// automatically set ground effect if terrain is valid
-			float height = _terrain_vpos - _state.pos(2);
+			float height = getHagl();
 			_control_status.flags.gnd_effect = (height < _params.gnd_effect_max_hgt);

 		} else
@@ -623,30 +683,6 @@ void Ekf::updateGroundEffect()
 }
 #endif // CONFIG_EKF2_BAROMETER

-#if defined(CONFIG_EKF2_WIND)
-void Ekf::resetWind()
-{
-#if defined(CONFIG_EKF2_AIRSPEED)
-	if (_control_status.flags.fuse_aspd && isRecent(_airspeed_sample_delayed.time_us, 1e6)) {
-		resetWindUsingAirspeed(_airspeed_sample_delayed);
-		return;
-	}
-#endif // CONFIG_EKF2_AIRSPEED
-
-	resetWindToZero();
-}
-
-void Ekf::resetWindToZero()
-{
-	ECL_INFO("reset wind to zero");
-
-	// If we don't have an airspeed measurement, then assume the wind is zero
-	_state.wind_vel.setZero();
-
-	resetWindCov();
-}
-
-#endif // CONFIG_EKF2_WIND

 void Ekf::updateIMUBiasInhibit(const imuSample &imu_delayed)
 {
@@ -53,7 +53,7 @@ void Ekf::controlHeightFusion(const imuSample &imu_delayed)
 #endif // CONFIG_EKF2_GNSS

 #if defined(CONFIG_EKF2_RANGE_FINDER)
-	controlRangeHeightFusion();
+	controlRangeHaglFusion();
 #endif // CONFIG_EKF2_RANGE_FINDER

 	checkHeightSensorRefFallback();
@@ -230,22 +230,32 @@ void Ekf::checkVerticalAccelerationHealth(const imuSample &imu_delayed)

 	Likelihood inertial_nav_falling_likelihood = estimateInertialNavFallingLikelihood();

-	// Check for more than 50% clipping affected IMU samples within the past 1 second
-	const uint16_t clip_count_limit = 1.f / _dt_ekf_avg;
-	const bool is_clipping = imu_delayed.delta_vel_clipping[0] ||
-				 imu_delayed.delta_vel_clipping[1] ||
-				 imu_delayed.delta_vel_clipping[2];
+	const uint16_t kClipCountLimit = 1.f / _dt_ekf_avg;

-	if (is_clipping && _clip_counter < clip_count_limit) {
-		_clip_counter++;
+	bool acc_clip_warning[3] {};
+	bool acc_clip_critical[3] {};

-	} else if (_clip_counter > 0) {
-		_clip_counter--;
+	for (int axis = 0; axis < 3; axis++) {
+		if (imu_delayed.delta_vel_clipping[axis] && (_clip_counter[axis] < kClipCountLimit)) {
+			_clip_counter[axis]++;
+
+		} else if (_clip_counter[axis] > 0) {
+			_clip_counter[axis]--;
+		}
+
+		// warning if more than 50% clipping affected IMU samples within the past 1 second
+		acc_clip_warning[axis] = _clip_counter[axis] >= kClipCountLimit / 2;
+		acc_clip_critical[axis] = _clip_counter[axis] >= kClipCountLimit;
 	}

-	_fault_status.flags.bad_acc_clipping = _clip_counter > clip_count_limit / 2;
+	// bad_acc_clipping if ALL axes are reporting warning or if ANY axis is critical
+	const bool all_axis_warning = (acc_clip_warning[0] && acc_clip_warning[1] && acc_clip_warning[2]);
+	const bool any_axis_critical = (acc_clip_critical[0] || acc_clip_critical[1] || acc_clip_critical[2]);

-	const bool is_clipping_frequently = _clip_counter > 0;
+	_fault_status.flags.bad_acc_clipping = all_axis_warning || any_axis_critical;
+
+	// if Z axis is warning or any other axis critical
+	const bool is_clipping_frequently = acc_clip_warning[2] || _fault_status.flags.bad_acc_clipping;

 	// Do not require evidence of clipping if the likelihood of having the INS falling is high
 	const bool bad_vert_accel = (is_clipping_frequently && (inertial_nav_falling_likelihood == Likelihood::MEDIUM))
@@ -301,7 +311,8 @@ Likelihood Ekf::estimateInertialNavFallingLikelihood() const

 #if defined(CONFIG_EKF2_RANGE_FINDER)
 	if (_control_status.flags.rng_hgt) {
-		checks[3] = {ReferenceType::GROUND, _aid_src_rng_hgt.innovation, _aid_src_rng_hgt.innovation_variance};
+		// Range is a distance to ground measurement, not a direct height observation and has an opposite sign
+		checks[3] = {ReferenceType::GROUND, -_aid_src_rng_hgt.innovation, _aid_src_rng_hgt.innovation_variance};
 	}
 #endif // CONFIG_EKF2_RANGE_FINDER

@@ -161,12 +161,9 @@ void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_v
 #if defined(CONFIG_EKF2_GNSS)
 	_gps_hgt_b_est.setBias(_gps_hgt_b_est.getBias() + delta_z);
 #endif // CONFIG_EKF2_GNSS
-#if defined(CONFIG_EKF2_RANGE_FINDER)
-	_rng_hgt_b_est.setBias(_rng_hgt_b_est.getBias() + delta_z);
-#endif // CONFIG_EKF2_RANGE_FINDER

 #if defined(CONFIG_EKF2_TERRAIN)
-	terrainHandleVerticalPositionReset(delta_z);
+	_state.terrain += delta_z;
 #endif

 	// Reset the timout timer
@@ -68,7 +68,8 @@ State = Values(
    accel_bias = sf.V3(),
    mag_I = sf.V3(),
    mag_B = sf.V3(),
-    wind_vel = sf.V2()
+    wind_vel = sf.V2(),
+    terrain = sf.V1()
 )

 if args.disable_mag:
@@ -132,7 +133,8 @@ def predict_covariance(
        accel_bias = sf.V3.symbolic("delta_a_b"),
        mag_I = sf.V3.symbolic("mag_I"),
        mag_B = sf.V3.symbolic("mag_B"),
-        wind_vel = sf.V2.symbolic("wind_vel")
+        wind_vel = sf.V2.symbolic("wind_vel"),
+        terrain = sf.V1.symbolic("terrain")
    )

    if args.disable_mag:
@@ -296,16 +298,10 @@ def compute_wind_init_and_cov_from_airspeed(
    # Initialise wind states assuming horizontal flight
    sideslip = sf.Symbol("beta")
    wind = sf.V2(v_local[0] - airspeed * sf.cos(heading + sideslip), v_local[1] - airspeed * sf.sin(heading + sideslip))
-    J = wind.jacobian([v_local[0], v_local[1], heading, sideslip, airspeed])
+    H = wind.jacobian([v_local[0], v_local[1], heading, sideslip, airspeed])

-    R = sf.M55()
-    R[0,0] = v_var[0]
-    R[1,1] = v_var[1]
-    R[2,2] = heading_var
-    R[3,3] = sideslip_var
-    R[4,4] = airspeed_var
-
-    P = J * R * J.T
+    P = sf.Matrix.diag([v_var[0], v_var[1], heading_var, sideslip_var, airspeed_var])
+    P = H * P * H.T

    # Assume zero sideslip
    P = P.subs({sideslip: 0.0})
@@ -456,7 +452,10 @@ def compute_mag_declination_pred_innov_var_and_h(

    return (meas_pred, innov_var, H.T)

-def predict_opt_flow(state, distance, epsilon):
+def predict_hagl(state):
+    return state["terrain"][0] - state["pos"][2]
+
+def predict_opt_flow(state, epsilon):
    R_to_body = state["quat_nominal"].inverse()

    # Calculate earth relative velocity in a non-rotating sensor frame
@@ -464,23 +463,23 @@ def predict_opt_flow(state, distance, epsilon):

    # Divide by range to get predicted angular LOS rates relative to X and Y
    # axes. Note these are rates in a non-rotating sensor frame
+    hagl = predict_hagl(state)
+    hagl = add_epsilon_sign(hagl, hagl, epsilon)
+    R_to_earth = state["quat_nominal"].to_rotation_matrix()
    flow_pred = sf.V2()
-    flow_pred[0] =  rel_vel_sensor[1] / distance
-    flow_pred[1] = -rel_vel_sensor[0] / distance
-    flow_pred = add_epsilon_sign(flow_pred, distance, epsilon)
+    flow_pred[0] =  rel_vel_sensor[1] / hagl * R_to_earth[2, 2]
+    flow_pred[1] = -rel_vel_sensor[0] / hagl * R_to_earth[2, 2]

    return flow_pred

-
 def compute_flow_xy_innov_var_and_hx(
        state: VState,
        P: MTangent,
-        distance: sf.Scalar,
        R: sf.Scalar,
        epsilon: sf.Scalar
 ) -> (sf.V2, VTangent):
    state = vstate_to_state(state)
-    meas_pred = predict_opt_flow(state, distance, epsilon);
+    meas_pred = predict_opt_flow(state, epsilon)

    innov_var = sf.V2()
    Hx = jacobian_chain_rule(meas_pred[0], state)
@@ -493,18 +492,40 @@ def compute_flow_xy_innov_var_and_hx(
 def compute_flow_y_innov_var_and_h(
        state: VState,
        P: MTangent,
-        distance: sf.Scalar,
        R: sf.Scalar,
        epsilon: sf.Scalar
 ) -> (sf.Scalar, VTangent):
    state = vstate_to_state(state)
-    meas_pred = predict_opt_flow(state, distance, epsilon);
+    meas_pred = predict_opt_flow(state, epsilon)

    Hy = jacobian_chain_rule(meas_pred[1], state)
    innov_var = (Hy * P * Hy.T + R)[0,0]

    return (innov_var, Hy.T)

+def compute_hagl_innov_var(
+        P: MTangent,
+        R: sf.Scalar,
+) -> (sf.Scalar):
+    state = VState.symbolic("state")
+    state = vstate_to_state(state)
+    meas_pred = predict_hagl(state)
+
+    H = jacobian_chain_rule(meas_pred, state)
+    innov_var = (H * P * H.T + R)[0,0]
+
+    return (innov_var)
+
+def compute_hagl_h(
+) -> (VTangent):
+    state = VState.symbolic("state")
+    state = vstate_to_state(state)
+    meas_pred = predict_hagl(state)
+
+    H = jacobian_chain_rule(meas_pred, state)
+
+    return (H.T)
+
 def compute_gnss_yaw_pred_innov_var_and_h(
        state: VState,
        P: MTangent,
@@ -664,6 +685,8 @@ if not args.disable_wind:
 generate_px4_function(compute_yaw_innov_var_and_h, output_names=["innov_var", "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_hagl_innov_var, output_names=["innov_var"])
+generate_px4_function(compute_hagl_h, output_names=["H"])
 generate_px4_function(compute_gnss_yaw_pred_innov_var_and_h, output_names=["meas_pred", "innov_var", "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"])
@@ -16,21 +16,21 @@ namespace sym {
 * Symbolic function: compute_airspeed_h_and_k
 *
 * Args:
- *     state: Matrix24_1
- *     P: Matrix23_23
+ *     state: Matrix25_1
+ *     P: Matrix24_24
 *     innov_var: Scalar
 *     epsilon: Scalar
 *
 * Outputs:
- *     H: Matrix23_1
- *     K: Matrix23_1
+ *     H: Matrix24_1
+ *     K: Matrix24_1
 */
 template <typename Scalar>
-void ComputeAirspeedHAndK(const matrix::Matrix<Scalar, 24, 1>& state,
-                          const matrix::Matrix<Scalar, 23, 23>& P, const Scalar innov_var,
-                          const Scalar epsilon, matrix::Matrix<Scalar, 23, 1>* const H = nullptr,
-                          matrix::Matrix<Scalar, 23, 1>* const K = nullptr) {
-  // Total ops: 246
+void ComputeAirspeedHAndK(const matrix::Matrix<Scalar, 25, 1>& state,
+                          const matrix::Matrix<Scalar, 24, 24>& P, const Scalar innov_var,
+                          const Scalar epsilon, matrix::Matrix<Scalar, 24, 1>* const H = nullptr,
+                          matrix::Matrix<Scalar, 24, 1>* const K = nullptr) {
+  // Total ops: 256

  // Input arrays

@@ -47,7 +47,7 @@ void ComputeAirspeedHAndK(const matrix::Matrix<Scalar, 24, 1>& state,

  // Output terms (2)
  if (H != nullptr) {
-    matrix::Matrix<Scalar, 23, 1>& _h = (*H);
+    matrix::Matrix<Scalar, 24, 1>& _h = (*H);

    _h.setZero();

@@ -59,7 +59,7 @@ void ComputeAirspeedHAndK(const matrix::Matrix<Scalar, 24, 1>& state,
  }

  if (K != nullptr) {
-    matrix::Matrix<Scalar, 23, 1>& _k = (*K);
+    matrix::Matrix<Scalar, 24, 1>& _k = (*K);

    _k(0, 0) = _tmp6 * (-P(0, 21) * _tmp3 - P(0, 22) * _tmp4 + P(0, 3) * _tmp3 + P(0, 4) * _tmp4 +
                        P(0, 5) * _tmp5);
@@ -107,6 +107,8 @@ void ComputeAirspeedHAndK(const matrix::Matrix<Scalar, 24, 1>& state,
                         P(21, 4) * _tmp4 + P(21, 5) * _tmp5);
    _k(22, 0) = _tmp6 * (-P(22, 21) * _tmp3 - P(22, 22) * _tmp4 + P(22, 3) * _tmp3 +
                         P(22, 4) * _tmp4 + P(22, 5) * _tmp5);
+    _k(23, 0) = _tmp6 * (-P(23, 21) * _tmp3 - P(23, 22) * _tmp4 + P(23, 3) * _tmp3 +
+                         P(23, 4) * _tmp4 + P(23, 5) * _tmp5);
  }
 }  // NOLINT(readability/fn_size)

@@ -16,8 +16,8 @@ namespace sym {
 * Symbolic function: compute_airspeed_innov_and_innov_var
 *
 * Args:
- *     state: Matrix24_1
- *     P: Matrix23_23
+ *     state: Matrix25_1
+ *     P: Matrix24_24
 *     airspeed: Scalar
 *     R: Scalar
 *     epsilon: Scalar
@@ -27,8 +27,8 @@ namespace sym {
 *     innov_var: Scalar
 */
 template <typename Scalar>
-void ComputeAirspeedInnovAndInnovVar(const matrix::Matrix<Scalar, 24, 1>& state,
-                                     const matrix::Matrix<Scalar, 23, 23>& P, const Scalar airspeed,
+void ComputeAirspeedInnovAndInnovVar(const matrix::Matrix<Scalar, 25, 1>& state,
+                                     const matrix::Matrix<Scalar, 24, 24>& P, const Scalar airspeed,
                                     const Scalar R, const Scalar epsilon,
                                     Scalar* const innov = nullptr,
                                     Scalar* const innov_var = nullptr) {
@@ -16,8 +16,8 @@ namespace sym {
 * Symbolic function: compute_drag_x_innov_var_and_h
 *
 * Args:
- *     state: Matrix24_1
- *     P: Matrix23_23
+ *     state: Matrix25_1
+ *     P: Matrix24_24
 *     rho: Scalar
 *     cd: Scalar
 *     cm: Scalar
@@ -26,14 +26,14 @@ namespace sym {
 *
 * Outputs:
 *     innov_var: Scalar
- *     Hx: Matrix23_1
+ *     Hx: Matrix24_1
 */
 template <typename Scalar>
-void ComputeDragXInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
-                              const matrix::Matrix<Scalar, 23, 23>& P, const Scalar rho,
+void ComputeDragXInnovVarAndH(const matrix::Matrix<Scalar, 25, 1>& state,
+                              const matrix::Matrix<Scalar, 24, 24>& P, const Scalar rho,
                              const Scalar cd, const Scalar cm, const Scalar R,
                              const Scalar epsilon, Scalar* const innov_var = nullptr,
-                              matrix::Matrix<Scalar, 23, 1>* const Hx = nullptr) {
+                              matrix::Matrix<Scalar, 24, 1>* const Hx = nullptr) {
  // Total ops: 357

  // Input arrays
@@ -162,7 +162,7 @@ void ComputeDragXInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
  }

  if (Hx != nullptr) {
-    matrix::Matrix<Scalar, 23, 1>& _hx = (*Hx);
+    matrix::Matrix<Scalar, 24, 1>& _hx = (*Hx);

    _hx.setZero();

@@ -16,8 +16,8 @@ namespace sym {
 * Symbolic function: compute_drag_y_innov_var_and_h
 *
 * Args:
- *     state: Matrix24_1
- *     P: Matrix23_23
+ *     state: Matrix25_1
+ *     P: Matrix24_24
 *     rho: Scalar
 *     cd: Scalar
 *     cm: Scalar
@@ -26,14 +26,14 @@ namespace sym {
 *
 * Outputs:
 *     innov_var: Scalar
- *     Hy: Matrix23_1
+ *     Hy: Matrix24_1
 */
 template <typename Scalar>
-void ComputeDragYInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
-                              const matrix::Matrix<Scalar, 23, 23>& P, const Scalar rho,
+void ComputeDragYInnovVarAndH(const matrix::Matrix<Scalar, 25, 1>& state,
+                              const matrix::Matrix<Scalar, 24, 24>& P, const Scalar rho,
                              const Scalar cd, const Scalar cm, const Scalar R,
                              const Scalar epsilon, Scalar* const innov_var = nullptr,
-                              matrix::Matrix<Scalar, 23, 1>* const Hy = nullptr) {
+                              matrix::Matrix<Scalar, 24, 1>* const Hy = nullptr) {
  // Total ops: 360

  // Input arrays
@@ -162,7 +162,7 @@ void ComputeDragYInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
  }

  if (Hy != nullptr) {
-    matrix::Matrix<Scalar, 23, 1>& _hy = (*Hy);
+    matrix::Matrix<Scalar, 24, 1>& _hy = (*Hy);

    _hy.setZero();

@@ -16,117 +16,153 @@ namespace sym {
 * Symbolic function: compute_flow_xy_innov_var_and_hx
 *
 * Args:
- *     state: Matrix24_1
- *     P: Matrix23_23
- *     distance: Scalar
+ *     state: Matrix25_1
+ *     P: Matrix24_24
 *     R: Scalar
 *     epsilon: Scalar
 *
 * Outputs:
 *     innov_var: Matrix21
- *     H: Matrix23_1
+ *     H: Matrix24_1
 */
 template <typename Scalar>
-void ComputeFlowXyInnovVarAndHx(const matrix::Matrix<Scalar, 24, 1>& state,
-                                const matrix::Matrix<Scalar, 23, 23>& P, const Scalar distance,
-                                const Scalar R, const Scalar epsilon,
+void ComputeFlowXyInnovVarAndHx(const matrix::Matrix<Scalar, 25, 1>& state,
+                                const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
+                                const Scalar epsilon,
                                matrix::Matrix<Scalar, 2, 1>* const innov_var = nullptr,
-                                matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
-  // Total ops: 275
+                                matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
+  // Total ops: 431

  // Input arrays

-  // Intermediate terms (42)
-  const Scalar _tmp0 = 2 * state(4, 0);
-  const Scalar _tmp1 = 2 * state(1, 0);
-  const Scalar _tmp2 = _tmp1 * state(6, 0);
-  const Scalar _tmp3 = -_tmp0 * state(3, 0) + _tmp2;
-  const Scalar _tmp4 =
-      Scalar(1.0) /
-      (distance + epsilon * (2 * math::min<Scalar>(0, (((distance) > 0) - ((distance) < 0))) + 1));
-  const Scalar _tmp5 = (Scalar(1) / Scalar(2)) * _tmp4;
-  const Scalar _tmp6 = _tmp5 * state(2, 0);
-  const Scalar _tmp7 = 2 * state(3, 0) * state(6, 0);
-  const Scalar _tmp8 = _tmp5 * (_tmp0 * state(1, 0) + _tmp7);
-  const Scalar _tmp9 = 2 * state(0, 0);
-  const Scalar _tmp10 = state(3, 0) * state(5, 0);
-  const Scalar _tmp11 = 2 * state(2, 0);
-  const Scalar _tmp12 = _tmp11 * state(6, 0);
-  const Scalar _tmp13 = -4 * _tmp10 + _tmp12 - _tmp9 * state(4, 0);
-  const Scalar _tmp14 = _tmp5 * state(1, 0);
-  const Scalar _tmp15 = _tmp9 * state(6, 0);
-  const Scalar _tmp16 = _tmp0 * state(2, 0) + _tmp15 - 4 * state(1, 0) * state(5, 0);
-  const Scalar _tmp17 = _tmp5 * state(3, 0);
-  const Scalar _tmp18 = -_tmp13 * _tmp14 + _tmp16 * _tmp17 - _tmp3 * _tmp6 + _tmp8 * state(0, 0);
-  const Scalar _tmp19 = 1 - 2 * std::pow(state(3, 0), Scalar(2));
-  const Scalar _tmp20 = _tmp4 * (_tmp19 - 2 * std::pow(state(1, 0), Scalar(2)));
-  const Scalar _tmp21 = _tmp3 * _tmp5;
-  const Scalar _tmp22 = _tmp5 * state(0, 0);
-  const Scalar _tmp23 =
-      _tmp13 * _tmp6 + _tmp16 * _tmp22 - _tmp21 * state(1, 0) - _tmp8 * state(3, 0);
-  const Scalar _tmp24 =
-      _tmp13 * _tmp22 - _tmp16 * _tmp6 - _tmp21 * state(3, 0) + _tmp8 * state(1, 0);
-  const Scalar _tmp25 = _tmp9 * state(3, 0);
-  const Scalar _tmp26 = _tmp1 * state(2, 0);
-  const Scalar _tmp27 = _tmp4 * (-_tmp25 + _tmp26);
-  const Scalar _tmp28 = _tmp4 * (_tmp11 * state(3, 0) + _tmp9 * state(1, 0));
-  const Scalar _tmp29 = _tmp4 * (_tmp19 - 2 * std::pow(state(2, 0), Scalar(2)));
-  const Scalar _tmp30 = 4 * state(4, 0);
-  const Scalar _tmp31 = _tmp2 - _tmp30 * state(3, 0) + _tmp9 * state(5, 0);
-  const Scalar _tmp32 = 2 * state(5, 0);
-  const Scalar _tmp33 = -_tmp15 - _tmp30 * state(2, 0) + _tmp32 * state(1, 0);
-  const Scalar _tmp34 = _tmp32 * state(2, 0) + _tmp7;
-  const Scalar _tmp35 = 2 * _tmp10 - _tmp12;
-  const Scalar _tmp36 = _tmp35 * _tmp5;
-  const Scalar _tmp37 = _tmp17 * _tmp33 - _tmp22 * _tmp34 - _tmp31 * _tmp6 + _tmp36 * state(1, 0);
-  const Scalar _tmp38 = -_tmp14 * _tmp33 - _tmp22 * _tmp31 + _tmp34 * _tmp6 + _tmp36 * state(3, 0);
-  const Scalar _tmp39 = _tmp14 * _tmp31 - _tmp17 * _tmp34 - _tmp22 * _tmp33 + _tmp35 * _tmp6;
-  const Scalar _tmp40 = _tmp4 * (_tmp25 + _tmp26);
-  const Scalar _tmp41 = _tmp4 * (_tmp1 * state(3, 0) - _tmp9 * state(2, 0));
+  // Intermediate terms (66)
+  const Scalar _tmp0 = state(2, 0) * state(4, 0);
+  const Scalar _tmp1 = state(1, 0) * state(5, 0);
+  const Scalar _tmp2 = 2 * state(6, 0);
+  const Scalar _tmp3 = _tmp2 * state(0, 0);
+  const Scalar _tmp4 = -2 * std::pow(state(2, 0), Scalar(2));
+  const Scalar _tmp5 = 1 - 2 * std::pow(state(1, 0), Scalar(2));
+  const Scalar _tmp6 = _tmp4 + _tmp5;
+  const Scalar _tmp7 = state(24, 0) - state(9, 0);
+  const Scalar _tmp8 =
+      _tmp7 + epsilon * (2 * math::min<Scalar>(0, (((_tmp7) > 0) - ((_tmp7) < 0))) + 1);
+  const Scalar _tmp9 = Scalar(1.0) / (_tmp8);
+  const Scalar _tmp10 = _tmp6 * _tmp9;
+  const Scalar _tmp11 = 2 * state(0, 0) * state(3, 0);
+  const Scalar _tmp12 = 2 * state(1, 0);
+  const Scalar _tmp13 = _tmp12 * state(2, 0);
+  const Scalar _tmp14 = -_tmp11 + _tmp13;
+  const Scalar _tmp15 = 2 * state(2, 0);
+  const Scalar _tmp16 = _tmp12 * state(0, 0) + _tmp15 * state(3, 0);
+  const Scalar _tmp17 = -2 * std::pow(state(3, 0), Scalar(2));
+  const Scalar _tmp18 = _tmp17 + _tmp5;
+  const Scalar _tmp19 = _tmp14 * state(4, 0) + _tmp16 * state(6, 0) + _tmp18 * state(5, 0);
+  const Scalar _tmp20 = 4 * _tmp9;
+  const Scalar _tmp21 = _tmp19 * _tmp20;
+  const Scalar _tmp22 = _tmp10 * (2 * _tmp0 - 4 * _tmp1 + _tmp3) - _tmp21 * state(1, 0);
+  const Scalar _tmp23 = (Scalar(1) / Scalar(2)) * _tmp22;
+  const Scalar _tmp24 = 2 * state(4, 0);
+  const Scalar _tmp25 = 4 * state(3, 0);
+  const Scalar _tmp26 = _tmp2 * state(2, 0);
+  const Scalar _tmp27 = -_tmp24 * state(0, 0) - _tmp25 * state(5, 0) + _tmp26;
+  const Scalar _tmp28 = (Scalar(1) / Scalar(2)) * _tmp10;
+  const Scalar _tmp29 = _tmp28 * state(0, 0);
+  const Scalar _tmp30 = (Scalar(1) / Scalar(2)) * state(3, 0);
+  const Scalar _tmp31 = _tmp2 * state(1, 0);
+  const Scalar _tmp32 = _tmp10 * (-_tmp24 * state(3, 0) + _tmp31);
+  const Scalar _tmp33 = _tmp2 * state(3, 0);
+  const Scalar _tmp34 = _tmp10 * (_tmp24 * state(1, 0) + _tmp33) - _tmp21 * state(2, 0);
+  const Scalar _tmp35 = (Scalar(1) / Scalar(2)) * _tmp34;
+  const Scalar _tmp36 =
+      -_tmp23 * state(2, 0) + _tmp27 * _tmp29 - _tmp30 * _tmp32 + _tmp35 * state(1, 0);
+  const Scalar _tmp37 = _tmp10 * _tmp14;
+  const Scalar _tmp38 = _tmp28 * state(2, 0);
+  const Scalar _tmp39 = (Scalar(1) / Scalar(2)) * _tmp32;
+  const Scalar _tmp40 =
+      _tmp23 * state(0, 0) + _tmp27 * _tmp38 - _tmp30 * _tmp34 - _tmp39 * state(1, 0);
+  const Scalar _tmp41 = _tmp28 * state(1, 0);
+  const Scalar _tmp42 =
+      _tmp22 * _tmp30 - _tmp27 * _tmp41 + _tmp35 * state(0, 0) - _tmp39 * state(2, 0);
+  const Scalar _tmp43 = _tmp10 * _tmp18;
+  const Scalar _tmp44 = _tmp6 / std::pow(_tmp8, Scalar(2));
+  const Scalar _tmp45 = _tmp19 * _tmp44;
+  const Scalar _tmp46 = _tmp10 * _tmp16;
+  const Scalar _tmp47 = _tmp12 * state(3, 0) - _tmp15 * state(0, 0);
+  const Scalar _tmp48 = _tmp10 * _tmp47;
+  const Scalar _tmp49 = _tmp11 + _tmp13;
+  const Scalar _tmp50 = _tmp10 * _tmp49;
+  const Scalar _tmp51 = _tmp17 + _tmp4 + 1;
+  const Scalar _tmp52 = _tmp10 * _tmp51;
+  const Scalar _tmp53 = _tmp47 * state(6, 0) + _tmp49 * state(5, 0) + _tmp51 * state(4, 0);
+  const Scalar _tmp54 = _tmp20 * _tmp53;
+  const Scalar _tmp55 = 2 * state(5, 0);
+  const Scalar _tmp56 = -_tmp10 * (_tmp33 + _tmp55 * state(2, 0)) + _tmp54 * state(1, 0);
+  const Scalar _tmp57 = -_tmp10 * (-4 * _tmp0 + 2 * _tmp1 - _tmp3) + _tmp54 * state(2, 0);
+  const Scalar _tmp58 = (Scalar(1) / Scalar(2)) * _tmp57;
+  const Scalar _tmp59 = -_tmp25 * state(4, 0) + _tmp31 + _tmp55 * state(0, 0);
+  const Scalar _tmp60 = -_tmp26 + _tmp55 * state(3, 0);
+  const Scalar _tmp61 = _tmp30 * _tmp56 + _tmp38 * _tmp60 + _tmp41 * _tmp59 + _tmp58 * state(0, 0);
+  const Scalar _tmp62 = (Scalar(1) / Scalar(2)) * _tmp56;
+  const Scalar _tmp63 = -_tmp30 * _tmp57 - _tmp38 * _tmp59 + _tmp41 * _tmp60 + _tmp62 * state(0, 0);
+  const Scalar _tmp64 =
+      _tmp10 * _tmp30 * _tmp60 - _tmp29 * _tmp59 + _tmp58 * state(1, 0) - _tmp62 * state(2, 0);
+  const Scalar _tmp65 = _tmp44 * _tmp53;

  // Output terms (2)
  if (innov_var != nullptr) {
    matrix::Matrix<Scalar, 2, 1>& _innov_var = (*innov_var);

-    _innov_var(0, 0) = R +
-                       _tmp18 * (P(0, 1) * _tmp23 + P(1, 1) * _tmp18 + P(2, 1) * _tmp24 +
-                                 P(3, 1) * _tmp27 + P(4, 1) * _tmp20 + P(5, 1) * _tmp28) +
-                       _tmp20 * (P(0, 4) * _tmp23 + P(1, 4) * _tmp18 + P(2, 4) * _tmp24 +
-                                 P(3, 4) * _tmp27 + P(4, 4) * _tmp20 + P(5, 4) * _tmp28) +
-                       _tmp23 * (P(0, 0) * _tmp23 + P(1, 0) * _tmp18 + P(2, 0) * _tmp24 +
-                                 P(3, 0) * _tmp27 + P(4, 0) * _tmp20 + P(5, 0) * _tmp28) +
-                       _tmp24 * (P(0, 2) * _tmp23 + P(1, 2) * _tmp18 + P(2, 2) * _tmp24 +
-                                 P(3, 2) * _tmp27 + P(4, 2) * _tmp20 + P(5, 2) * _tmp28) +
-                       _tmp27 * (P(0, 3) * _tmp23 + P(1, 3) * _tmp18 + P(2, 3) * _tmp24 +
-                                 P(3, 3) * _tmp27 + P(4, 3) * _tmp20 + P(5, 3) * _tmp28) +
-                       _tmp28 * (P(0, 5) * _tmp23 + P(1, 5) * _tmp18 + P(2, 5) * _tmp24 +
-                                 P(3, 5) * _tmp27 + P(4, 5) * _tmp20 + P(5, 5) * _tmp28);
-    _innov_var(1, 0) = R -
-                       _tmp29 * (P(0, 3) * _tmp37 + P(1, 3) * _tmp39 + P(2, 3) * _tmp38 -
-                                 P(3, 3) * _tmp29 - P(4, 3) * _tmp40 - P(5, 3) * _tmp41) +
-                       _tmp37 * (P(0, 0) * _tmp37 + P(1, 0) * _tmp39 + P(2, 0) * _tmp38 -
-                                 P(3, 0) * _tmp29 - P(4, 0) * _tmp40 - P(5, 0) * _tmp41) +
-                       _tmp38 * (P(0, 2) * _tmp37 + P(1, 2) * _tmp39 + P(2, 2) * _tmp38 -
-                                 P(3, 2) * _tmp29 - P(4, 2) * _tmp40 - P(5, 2) * _tmp41) +
-                       _tmp39 * (P(0, 1) * _tmp37 + P(1, 1) * _tmp39 + P(2, 1) * _tmp38 -
-                                 P(3, 1) * _tmp29 - P(4, 1) * _tmp40 - P(5, 1) * _tmp41) -
-                       _tmp40 * (P(0, 4) * _tmp37 + P(1, 4) * _tmp39 + P(2, 4) * _tmp38 -
-                                 P(3, 4) * _tmp29 - P(4, 4) * _tmp40 - P(5, 4) * _tmp41) -
-                       _tmp41 * (P(0, 5) * _tmp37 + P(1, 5) * _tmp39 + P(2, 5) * _tmp38 -
-                                 P(3, 5) * _tmp29 - P(4, 5) * _tmp40 - P(5, 5) * _tmp41);
+    _innov_var(0, 0) =
+        R +
+        _tmp36 * (P(0, 2) * _tmp40 + P(1, 2) * _tmp42 + P(2, 2) * _tmp36 - P(23, 2) * _tmp45 +
+                  P(3, 2) * _tmp37 + P(4, 2) * _tmp43 + P(5, 2) * _tmp46 + P(8, 2) * _tmp45) +
+        _tmp37 * (P(0, 3) * _tmp40 + P(1, 3) * _tmp42 + P(2, 3) * _tmp36 - P(23, 3) * _tmp45 +
+                  P(3, 3) * _tmp37 + P(4, 3) * _tmp43 + P(5, 3) * _tmp46 + P(8, 3) * _tmp45) +
+        _tmp40 * (P(0, 0) * _tmp40 + P(1, 0) * _tmp42 + P(2, 0) * _tmp36 - P(23, 0) * _tmp45 +
+                  P(3, 0) * _tmp37 + P(4, 0) * _tmp43 + P(5, 0) * _tmp46 + P(8, 0) * _tmp45) +
+        _tmp42 * (P(0, 1) * _tmp40 + P(1, 1) * _tmp42 + P(2, 1) * _tmp36 - P(23, 1) * _tmp45 +
+                  P(3, 1) * _tmp37 + P(4, 1) * _tmp43 + P(5, 1) * _tmp46 + P(8, 1) * _tmp45) +
+        _tmp43 * (P(0, 4) * _tmp40 + P(1, 4) * _tmp42 + P(2, 4) * _tmp36 - P(23, 4) * _tmp45 +
+                  P(3, 4) * _tmp37 + P(4, 4) * _tmp43 + P(5, 4) * _tmp46 + P(8, 4) * _tmp45) -
+        _tmp45 * (P(0, 23) * _tmp40 + P(1, 23) * _tmp42 + P(2, 23) * _tmp36 - P(23, 23) * _tmp45 +
+                  P(3, 23) * _tmp37 + P(4, 23) * _tmp43 + P(5, 23) * _tmp46 + P(8, 23) * _tmp45) +
+        _tmp45 * (P(0, 8) * _tmp40 + P(1, 8) * _tmp42 + P(2, 8) * _tmp36 - P(23, 8) * _tmp45 +
+                  P(3, 8) * _tmp37 + P(4, 8) * _tmp43 + P(5, 8) * _tmp46 + P(8, 8) * _tmp45) +
+        _tmp46 * (P(0, 5) * _tmp40 + P(1, 5) * _tmp42 + P(2, 5) * _tmp36 - P(23, 5) * _tmp45 +
+                  P(3, 5) * _tmp37 + P(4, 5) * _tmp43 + P(5, 5) * _tmp46 + P(8, 5) * _tmp45);
+    _innov_var(1, 0) =
+        R -
+        _tmp48 * (P(0, 5) * _tmp63 + P(1, 5) * _tmp61 + P(2, 5) * _tmp64 + P(23, 5) * _tmp65 -
+                  P(3, 5) * _tmp52 - P(4, 5) * _tmp50 - P(5, 5) * _tmp48 - P(8, 5) * _tmp65) -
+        _tmp50 * (P(0, 4) * _tmp63 + P(1, 4) * _tmp61 + P(2, 4) * _tmp64 + P(23, 4) * _tmp65 -
+                  P(3, 4) * _tmp52 - P(4, 4) * _tmp50 - P(5, 4) * _tmp48 - P(8, 4) * _tmp65) -
+        _tmp52 * (P(0, 3) * _tmp63 + P(1, 3) * _tmp61 + P(2, 3) * _tmp64 + P(23, 3) * _tmp65 -
+                  P(3, 3) * _tmp52 - P(4, 3) * _tmp50 - P(5, 3) * _tmp48 - P(8, 3) * _tmp65) +
+        _tmp61 * (P(0, 1) * _tmp63 + P(1, 1) * _tmp61 + P(2, 1) * _tmp64 + P(23, 1) * _tmp65 -
+                  P(3, 1) * _tmp52 - P(4, 1) * _tmp50 - P(5, 1) * _tmp48 - P(8, 1) * _tmp65) +
+        _tmp63 * (P(0, 0) * _tmp63 + P(1, 0) * _tmp61 + P(2, 0) * _tmp64 + P(23, 0) * _tmp65 -
+                  P(3, 0) * _tmp52 - P(4, 0) * _tmp50 - P(5, 0) * _tmp48 - P(8, 0) * _tmp65) +
+        _tmp64 * (P(0, 2) * _tmp63 + P(1, 2) * _tmp61 + P(2, 2) * _tmp64 + P(23, 2) * _tmp65 -
+                  P(3, 2) * _tmp52 - P(4, 2) * _tmp50 - P(5, 2) * _tmp48 - P(8, 2) * _tmp65) +
+        _tmp65 * (P(0, 23) * _tmp63 + P(1, 23) * _tmp61 + P(2, 23) * _tmp64 + P(23, 23) * _tmp65 -
+                  P(3, 23) * _tmp52 - P(4, 23) * _tmp50 - P(5, 23) * _tmp48 - P(8, 23) * _tmp65) -
+        _tmp65 * (P(0, 8) * _tmp63 + P(1, 8) * _tmp61 + P(2, 8) * _tmp64 + P(23, 8) * _tmp65 -
+                  P(3, 8) * _tmp52 - P(4, 8) * _tmp50 - P(5, 8) * _tmp48 - P(8, 8) * _tmp65);
  }

  if (H != nullptr) {
-    matrix::Matrix<Scalar, 23, 1>& _h = (*H);
+    matrix::Matrix<Scalar, 24, 1>& _h = (*H);

    _h.setZero();

-    _h(0, 0) = _tmp23;
-    _h(1, 0) = _tmp18;
-    _h(2, 0) = _tmp24;
-    _h(3, 0) = _tmp27;
-    _h(4, 0) = _tmp20;
-    _h(5, 0) = _tmp28;
+    _h(0, 0) = _tmp40;
+    _h(1, 0) = _tmp42;
+    _h(2, 0) = _tmp36;
+    _h(3, 0) = _tmp37;
+    _h(4, 0) = _tmp43;
+    _h(5, 0) = _tmp46;
+    _h(8, 0) = _tmp45;
+    _h(23, 0) = -_tmp45;
  }
 }  // NOLINT(readability/fn_size)

@@ -16,85 +16,100 @@ namespace sym {
 * Symbolic function: compute_flow_y_innov_var_and_h
 *
 * Args:
- *     state: Matrix24_1
- *     P: Matrix23_23
- *     distance: Scalar
+ *     state: Matrix25_1
+ *     P: Matrix24_24
 *     R: Scalar
 *     epsilon: Scalar
 *
 * Outputs:
 *     innov_var: Scalar
- *     H: Matrix23_1
+ *     H: Matrix24_1
 */
 template <typename Scalar>
-void ComputeFlowYInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
-                              const matrix::Matrix<Scalar, 23, 23>& P, const Scalar distance,
-                              const Scalar R, const Scalar epsilon,
-                              Scalar* const innov_var = nullptr,
-                              matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
-  // Total ops: 151
+void ComputeFlowYInnovVarAndH(const matrix::Matrix<Scalar, 25, 1>& state,
+                              const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
+                              const Scalar epsilon, Scalar* const innov_var = nullptr,
+                              matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
+  // Total ops: 232

  // Input arrays

-  // Intermediate terms (21)
-  const Scalar _tmp0 =
-      Scalar(1.0) /
-      (distance + epsilon * (2 * math::min<Scalar>(0, (((distance) > 0) - ((distance) < 0))) + 1));
-  const Scalar _tmp1 =
-      _tmp0 * (-2 * std::pow(state(2, 0), Scalar(2)) - 2 * std::pow(state(3, 0), Scalar(2)) + 1);
-  const Scalar _tmp2 = 4 * state(4, 0);
-  const Scalar _tmp3 = 2 * state(0, 0);
-  const Scalar _tmp4 = 2 * state(6, 0);
-  const Scalar _tmp5 = -_tmp2 * state(3, 0) + _tmp3 * state(5, 0) + _tmp4 * state(1, 0);
-  const Scalar _tmp6 = (Scalar(1) / Scalar(2)) * _tmp0;
-  const Scalar _tmp7 = _tmp5 * _tmp6;
-  const Scalar _tmp8 = 2 * state(1, 0);
-  const Scalar _tmp9 = -_tmp2 * state(2, 0) - _tmp4 * state(0, 0) + _tmp8 * state(5, 0);
-  const Scalar _tmp10 = _tmp6 * _tmp9;
-  const Scalar _tmp11 = 2 * state(5, 0);
-  const Scalar _tmp12 = _tmp6 * (_tmp11 * state(2, 0) + _tmp4 * state(3, 0));
-  const Scalar _tmp13 = _tmp11 * state(3, 0) - _tmp4 * state(2, 0);
-  const Scalar _tmp14 = _tmp6 * state(1, 0);
-  const Scalar _tmp15 =
-      _tmp10 * state(3, 0) - _tmp12 * state(0, 0) + _tmp13 * _tmp14 - _tmp7 * state(2, 0);
-  const Scalar _tmp16 = _tmp13 * _tmp6;
-  const Scalar _tmp17 =
-      _tmp12 * state(2, 0) - _tmp14 * _tmp9 + _tmp16 * state(3, 0) - _tmp7 * state(0, 0);
+  // Intermediate terms (28)
+  const Scalar _tmp0 = 2 * state(0, 0);
+  const Scalar _tmp1 = 2 * state(1, 0);
+  const Scalar _tmp2 = -_tmp0 * state(2, 0) + _tmp1 * state(3, 0);
+  const Scalar _tmp3 = 1 - 2 * std::pow(state(2, 0), Scalar(2));
+  const Scalar _tmp4 = _tmp3 - 2 * std::pow(state(1, 0), Scalar(2));
+  const Scalar _tmp5 = state(24, 0) - state(9, 0);
+  const Scalar _tmp6 =
+      _tmp5 + epsilon * (2 * math::min<Scalar>(0, (((_tmp5) > 0) - ((_tmp5) < 0))) + 1);
+  const Scalar _tmp7 = Scalar(1.0) / (_tmp6);
+  const Scalar _tmp8 = _tmp4 * _tmp7;
+  const Scalar _tmp9 = _tmp2 * _tmp8;
+  const Scalar _tmp10 = _tmp0 * state(3, 0) + _tmp1 * state(2, 0);
+  const Scalar _tmp11 = _tmp10 * _tmp8;
+  const Scalar _tmp12 = _tmp3 - 2 * std::pow(state(3, 0), Scalar(2));
+  const Scalar _tmp13 = _tmp12 * _tmp8;
+  const Scalar _tmp14 = _tmp10 * state(5, 0) + _tmp12 * state(4, 0) + _tmp2 * state(6, 0);
+  const Scalar _tmp15 = 4 * _tmp14 * _tmp7;
+  const Scalar _tmp16 = 2 * state(5, 0);
+  const Scalar _tmp17 = 2 * state(6, 0);
  const Scalar _tmp18 =
-      -_tmp10 * state(0, 0) - _tmp12 * state(3, 0) + _tmp14 * _tmp5 + _tmp16 * state(2, 0);
-  const Scalar _tmp19 = _tmp0 * (_tmp3 * state(3, 0) + _tmp8 * state(2, 0));
-  const Scalar _tmp20 = _tmp0 * (-_tmp3 * state(2, 0) + _tmp8 * state(3, 0));
+      (Scalar(1) / Scalar(2)) * _tmp15 * state(1, 0) -
+      Scalar(1) / Scalar(2) * _tmp8 * (_tmp16 * state(2, 0) + _tmp17 * state(3, 0));
+  const Scalar _tmp19 = 4 * state(4, 0);
+  const Scalar _tmp20 = (Scalar(1) / Scalar(2)) * _tmp15 * state(2, 0) -
+                        Scalar(1) / Scalar(2) * _tmp8 *
+                            (_tmp16 * state(1, 0) - _tmp17 * state(0, 0) - _tmp19 * state(2, 0));
+  const Scalar _tmp21 = (Scalar(1) / Scalar(2)) * _tmp8;
+  const Scalar _tmp22 =
+      _tmp21 * (_tmp16 * state(0, 0) + _tmp17 * state(1, 0) - _tmp19 * state(3, 0));
+  const Scalar _tmp23 = _tmp21 * (_tmp16 * state(3, 0) - _tmp17 * state(2, 0));
+  const Scalar _tmp24 =
+      _tmp18 * state(3, 0) + _tmp20 * state(0, 0) + _tmp22 * state(1, 0) + _tmp23 * state(2, 0);
+  const Scalar _tmp25 =
+      _tmp18 * state(0, 0) - _tmp20 * state(3, 0) - _tmp22 * state(2, 0) + _tmp23 * state(1, 0);
+  const Scalar _tmp26 =
+      -_tmp18 * state(2, 0) + _tmp20 * state(1, 0) - _tmp22 * state(0, 0) + _tmp23 * state(3, 0);
+  const Scalar _tmp27 = _tmp14 * _tmp4 / std::pow(_tmp6, Scalar(2));

  // Output terms (2)
  if (innov_var != nullptr) {
    Scalar& _innov_var = (*innov_var);

-    _innov_var = R -
-                 _tmp1 * (P(0, 3) * _tmp15 + P(1, 3) * _tmp18 + P(2, 3) * _tmp17 - P(3, 3) * _tmp1 -
-                          P(4, 3) * _tmp19 - P(5, 3) * _tmp20) +
-                 _tmp15 * (P(0, 0) * _tmp15 + P(1, 0) * _tmp18 + P(2, 0) * _tmp17 -
-                           P(3, 0) * _tmp1 - P(4, 0) * _tmp19 - P(5, 0) * _tmp20) +
-                 _tmp17 * (P(0, 2) * _tmp15 + P(1, 2) * _tmp18 + P(2, 2) * _tmp17 -
-                           P(3, 2) * _tmp1 - P(4, 2) * _tmp19 - P(5, 2) * _tmp20) +
-                 _tmp18 * (P(0, 1) * _tmp15 + P(1, 1) * _tmp18 + P(2, 1) * _tmp17 -
-                           P(3, 1) * _tmp1 - P(4, 1) * _tmp19 - P(5, 1) * _tmp20) -
-                 _tmp19 * (P(0, 4) * _tmp15 + P(1, 4) * _tmp18 + P(2, 4) * _tmp17 -
-                           P(3, 4) * _tmp1 - P(4, 4) * _tmp19 - P(5, 4) * _tmp20) -
-                 _tmp20 * (P(0, 5) * _tmp15 + P(1, 5) * _tmp18 + P(2, 5) * _tmp17 -
-                           P(3, 5) * _tmp1 - P(4, 5) * _tmp19 - P(5, 5) * _tmp20);
+    _innov_var =
+        R -
+        _tmp11 * (P(0, 4) * _tmp25 + P(1, 4) * _tmp24 + P(2, 4) * _tmp26 + P(23, 4) * _tmp27 -
+                  P(3, 4) * _tmp13 - P(4, 4) * _tmp11 - P(5, 4) * _tmp9 - P(8, 4) * _tmp27) -
+        _tmp13 * (P(0, 3) * _tmp25 + P(1, 3) * _tmp24 + P(2, 3) * _tmp26 + P(23, 3) * _tmp27 -
+                  P(3, 3) * _tmp13 - P(4, 3) * _tmp11 - P(5, 3) * _tmp9 - P(8, 3) * _tmp27) +
+        _tmp24 * (P(0, 1) * _tmp25 + P(1, 1) * _tmp24 + P(2, 1) * _tmp26 + P(23, 1) * _tmp27 -
+                  P(3, 1) * _tmp13 - P(4, 1) * _tmp11 - P(5, 1) * _tmp9 - P(8, 1) * _tmp27) +
+        _tmp25 * (P(0, 0) * _tmp25 + P(1, 0) * _tmp24 + P(2, 0) * _tmp26 + P(23, 0) * _tmp27 -
+                  P(3, 0) * _tmp13 - P(4, 0) * _tmp11 - P(5, 0) * _tmp9 - P(8, 0) * _tmp27) +
+        _tmp26 * (P(0, 2) * _tmp25 + P(1, 2) * _tmp24 + P(2, 2) * _tmp26 + P(23, 2) * _tmp27 -
+                  P(3, 2) * _tmp13 - P(4, 2) * _tmp11 - P(5, 2) * _tmp9 - P(8, 2) * _tmp27) +
+        _tmp27 * (P(0, 23) * _tmp25 + P(1, 23) * _tmp24 + P(2, 23) * _tmp26 + P(23, 23) * _tmp27 -
+                  P(3, 23) * _tmp13 - P(4, 23) * _tmp11 - P(5, 23) * _tmp9 - P(8, 23) * _tmp27) -
+        _tmp27 * (P(0, 8) * _tmp25 + P(1, 8) * _tmp24 + P(2, 8) * _tmp26 + P(23, 8) * _tmp27 -
+                  P(3, 8) * _tmp13 - P(4, 8) * _tmp11 - P(5, 8) * _tmp9 - P(8, 8) * _tmp27) -
+        _tmp9 * (P(0, 5) * _tmp25 + P(1, 5) * _tmp24 + P(2, 5) * _tmp26 + P(23, 5) * _tmp27 -
+                 P(3, 5) * _tmp13 - P(4, 5) * _tmp11 - P(5, 5) * _tmp9 - P(8, 5) * _tmp27);
  }

  if (H != nullptr) {
-    matrix::Matrix<Scalar, 23, 1>& _h = (*H);
+    matrix::Matrix<Scalar, 24, 1>& _h = (*H);

    _h.setZero();

-    _h(0, 0) = _tmp15;
-    _h(1, 0) = _tmp18;
-    _h(2, 0) = _tmp17;
-    _h(3, 0) = -_tmp1;
-    _h(4, 0) = -_tmp19;
-    _h(5, 0) = -_tmp20;
+    _h(0, 0) = _tmp25;
+    _h(1, 0) = _tmp24;
+    _h(2, 0) = _tmp26;
+    _h(3, 0) = -_tmp13;
+    _h(4, 0) = -_tmp11;
+    _h(5, 0) = -_tmp9;
+    _h(8, 0) = -_tmp27;
+    _h(23, 0) = _tmp27;
  }
 }  // NOLINT(readability/fn_size)

@@ -16,8 +16,8 @@ namespace sym {
 * Symbolic function: compute_gnss_yaw_pred_innov_var_and_h
 *
 * Args:
- *     state: Matrix24_1
- *     P: Matrix23_23
+ *     state: Matrix25_1
+ *     P: Matrix24_24
 *     antenna_yaw_offset: Scalar
 *     R: Scalar
 *     epsilon: Scalar
@@ -25,15 +25,15 @@ namespace sym {
 * Outputs:
 *     meas_pred: Scalar
 *     innov_var: Scalar
- *     H: Matrix23_1
+ *     H: Matrix24_1
 */
 template <typename Scalar>
-void ComputeGnssYawPredInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
-                                    const matrix::Matrix<Scalar, 23, 23>& P,
+void ComputeGnssYawPredInnovVarAndH(const matrix::Matrix<Scalar, 25, 1>& state,
+                                    const matrix::Matrix<Scalar, 24, 24>& P,
                                    const Scalar antenna_yaw_offset, const Scalar R,
                                    const Scalar epsilon, Scalar* const meas_pred = nullptr,
                                    Scalar* const innov_var = nullptr,
-                                    matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
+                                    matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
  // Total ops: 114

  // Input arrays
@@ -93,7 +93,7 @@ void ComputeGnssYawPredInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
  }

  if (H != nullptr) {
-    matrix::Matrix<Scalar, 23, 1>& _h = (*H);
+    matrix::Matrix<Scalar, 24, 1>& _h = (*H);

    _h.setZero();

@@ -16,19 +16,19 @@ namespace sym {
 * Symbolic function: compute_gravity_xyz_innov_var_and_hx
 *
 * Args:
- *     state: Matrix24_1
- *     P: Matrix23_23
+ *     state: Matrix25_1
+ *     P: Matrix24_24
 *     R: Scalar
 *
 * Outputs:
 *     innov_var: Matrix31
- *     Hx: Matrix23_1
+ *     Hx: Matrix24_1
 */
 template <typename Scalar>
-void ComputeGravityXyzInnovVarAndHx(const matrix::Matrix<Scalar, 24, 1>& state,
-                                    const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
+void ComputeGravityXyzInnovVarAndHx(const matrix::Matrix<Scalar, 25, 1>& state,
+                                    const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
                                    matrix::Matrix<Scalar, 3, 1>* const innov_var = nullptr,
-                                    matrix::Matrix<Scalar, 23, 1>* const Hx = nullptr) {
+                                    matrix::Matrix<Scalar, 24, 1>* const Hx = nullptr) {
  // Total ops: 53

  // Input arrays
@@ -61,7 +61,7 @@ void ComputeGravityXyzInnovVarAndHx(const matrix::Matrix<Scalar, 24, 1>& state,
  }

  if (Hx != nullptr) {
-    matrix::Matrix<Scalar, 23, 1>& _hx = (*Hx);
+    matrix::Matrix<Scalar, 24, 1>& _hx = (*Hx);

    _hx.setZero();

@@ -16,19 +16,19 @@ namespace sym {
 * Symbolic function: compute_gravity_y_innov_var_and_h
 *
 * Args:
- *     state: Matrix24_1
- *     P: Matrix23_23
+ *     state: Matrix25_1
+ *     P: Matrix24_24
 *     R: Scalar
 *
 * Outputs:
 *     innov_var: Scalar
- *     Hy: Matrix23_1
+ *     Hy: Matrix24_1
 */
 template <typename Scalar>
-void ComputeGravityYInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
-                                 const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
+void ComputeGravityYInnovVarAndH(const matrix::Matrix<Scalar, 25, 1>& state,
+                                 const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
                                 Scalar* const innov_var = nullptr,
-                                 matrix::Matrix<Scalar, 23, 1>* const Hy = nullptr) {
+                                 matrix::Matrix<Scalar, 24, 1>* const Hy = nullptr) {
  // Total ops: 22

  // Input arrays
@@ -47,7 +47,7 @@ void ComputeGravityYInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
  }

  if (Hy != nullptr) {
-    matrix::Matrix<Scalar, 23, 1>& _hy = (*Hy);
+    matrix::Matrix<Scalar, 24, 1>& _hy = (*Hy);

    _hy.setZero();

@@ -16,19 +16,19 @@ namespace sym {
 * Symbolic function: compute_gravity_z_innov_var_and_h
 *
 * Args:
- *     state: Matrix24_1
- *     P: Matrix23_23
+ *     state: Matrix25_1
+ *     P: Matrix24_24
 *     R: Scalar
 *
 * Outputs:
 *     innov_var: Scalar
- *     Hz: Matrix23_1
+ *     Hz: Matrix24_1
 */
 template <typename Scalar>
-void ComputeGravityZInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
-                                 const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
+void ComputeGravityZInnovVarAndH(const matrix::Matrix<Scalar, 25, 1>& state,
+                                 const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
                                 Scalar* const innov_var = nullptr,
-                                 matrix::Matrix<Scalar, 23, 1>* const Hz = nullptr) {
+                                 matrix::Matrix<Scalar, 24, 1>* const Hz = nullptr) {
  // Total ops: 18

  // Input arrays
@@ -48,7 +48,7 @@ void ComputeGravityZInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
  }

  if (Hz != nullptr) {
-    matrix::Matrix<Scalar, 23, 1>& _hz = (*Hz);
+    matrix::Matrix<Scalar, 24, 1>& _hz = (*Hz);

    _hz.setZero();

@@ -0,0 +1,43 @@
+// -----------------------------------------------------------------------------
+// This file was autogenerated by symforce from template:
+//     function/FUNCTION.h.jinja
+// Do NOT modify by hand.
+// -----------------------------------------------------------------------------
+
+#pragma once
+
+#include <matrix/math.hpp>
+
+namespace sym {
+
+/**
+ * This function was autogenerated from a symbolic function. Do not modify by hand.
+ *
+ * Symbolic function: compute_hagl_h
+ *
+ * Args:
+ *
+ * Outputs:
+ *     H: Matrix24_1
+ */
+template <typename Scalar>
+void ComputeHaglH(matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
+  // Total ops: 0
+
+  // Input arrays
+
+  // Intermediate terms (0)
+
+  // Output terms (1)
+  if (H != nullptr) {
+    matrix::Matrix<Scalar, 24, 1>& _h = (*H);
+
+    _h.setZero();
+
+    _h(8, 0) = -1;
+    _h(23, 0) = 1;
+  }
+}  // NOLINT(readability/fn_size)
+
+// NOLINTNEXTLINE(readability/fn_size)
+}  // namespace sym
@@ -0,0 +1,43 @@
+// -----------------------------------------------------------------------------
+// This file was autogenerated by symforce from template:
+//     function/FUNCTION.h.jinja
+// Do NOT modify by hand.
+// -----------------------------------------------------------------------------
+
+#pragma once
+
+#include <matrix/math.hpp>
+
+namespace sym {
+
+/**
+ * This function was autogenerated from a symbolic function. Do not modify by hand.
+ *
+ * Symbolic function: compute_hagl_innov_var
+ *
+ * Args:
+ *     P: Matrix24_24
+ *     R: Scalar
+ *
+ * Outputs:
+ *     innov_var: Scalar
+ */
+template <typename Scalar>
+void ComputeHaglInnovVar(const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
+                         Scalar* const innov_var = nullptr) {
+  // Total ops: 4
+
+  // Input arrays
+
+  // Intermediate terms (0)
+
+  // Output terms (1)
+  if (innov_var != nullptr) {
+    Scalar& _innov_var = (*innov_var);
+
+    _innov_var = P(23, 23) - P(23, 8) - P(8, 23) + P(8, 8) + R;
+  }
+}  // NOLINT(readability/fn_size)
+
+// NOLINTNEXTLINE(readability/fn_size)
+}  // namespace sym
@@ -16,22 +16,22 @@ namespace sym {
 * Symbolic function: compute_mag_declination_pred_innov_var_and_h
 *
 * Args:
- *     state: Matrix24_1
- *     P: Matrix23_23
+ *     state: Matrix25_1
+ *     P: Matrix24_24
 *     R: Scalar
 *     epsilon: Scalar
 *
 * Outputs:
 *     pred: Scalar
 *     innov_var: Scalar
- *     H: Matrix23_1
+ *     H: Matrix24_1
 */
 template <typename Scalar>
-void ComputeMagDeclinationPredInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
-                                           const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
+void ComputeMagDeclinationPredInnovVarAndH(const matrix::Matrix<Scalar, 25, 1>& state,
+                                           const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
                                           const Scalar epsilon, Scalar* const pred = nullptr,
                                           Scalar* const innov_var = nullptr,
-                                           matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
+                                           matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
  // Total ops: 22

  // Input arrays
@@ -59,7 +59,7 @@ void ComputeMagDeclinationPredInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>&
  }

  if (H != nullptr) {
-    matrix::Matrix<Scalar, 23, 1>& _h = (*H);
+    matrix::Matrix<Scalar, 24, 1>& _h = (*H);

    _h.setZero();

@@ -16,8 +16,8 @@ namespace sym {
 * Symbolic function: compute_mag_innov_innov_var_and_hx
 *
 * Args:
- *     state: Matrix24_1
- *     P: Matrix23_23
+ *     state: Matrix25_1
+ *     P: Matrix24_24
 *     meas: Matrix31
 *     R: Scalar
 *     epsilon: Scalar
@@ -25,16 +25,16 @@ namespace sym {
 * Outputs:
 *     innov: Matrix31
 *     innov_var: Matrix31
- *     Hx: Matrix23_1
+ *     Hx: Matrix24_1
 */
 template <typename Scalar>
-void ComputeMagInnovInnovVarAndHx(const matrix::Matrix<Scalar, 24, 1>& state,
-                                  const matrix::Matrix<Scalar, 23, 23>& P,
+void ComputeMagInnovInnovVarAndHx(const matrix::Matrix<Scalar, 25, 1>& state,
+                                  const matrix::Matrix<Scalar, 24, 24>& P,
                                  const matrix::Matrix<Scalar, 3, 1>& meas, const Scalar R,
                                  const Scalar epsilon,
                                  matrix::Matrix<Scalar, 3, 1>* const innov = nullptr,
                                  matrix::Matrix<Scalar, 3, 1>* const innov_var = nullptr,
-                                  matrix::Matrix<Scalar, 23, 1>* const Hx = nullptr) {
+                                  matrix::Matrix<Scalar, 24, 1>* const Hx = nullptr) {
  // Total ops: 461

  // Unused inputs
@@ -185,7 +185,7 @@ void ComputeMagInnovInnovVarAndHx(const matrix::Matrix<Scalar, 24, 1>& state,
  }

  if (Hx != nullptr) {
-    matrix::Matrix<Scalar, 23, 1>& _hx = (*Hx);
+    matrix::Matrix<Scalar, 24, 1>& _hx = (*Hx);

    _hx.setZero();

@@ -16,20 +16,20 @@ namespace sym {
 * Symbolic function: compute_mag_y_innov_var_and_h
 *
 * Args:
- *     state: Matrix24_1
- *     P: Matrix23_23
+ *     state: Matrix25_1
+ *     P: Matrix24_24
 *     R: Scalar
 *     epsilon: Scalar
 *
 * Outputs:
 *     innov_var: Scalar
- *     H: Matrix23_1
+ *     H: Matrix24_1
 */
 template <typename Scalar>
-void ComputeMagYInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
-                             const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
+void ComputeMagYInnovVarAndH(const matrix::Matrix<Scalar, 25, 1>& state,
+                             const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
                             const Scalar epsilon, Scalar* const innov_var = nullptr,
-                             matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
+                             matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
  // Total ops: 159

  // Unused inputs
@@ -85,7 +85,7 @@ void ComputeMagYInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
  }

  if (H != nullptr) {
-    matrix::Matrix<Scalar, 23, 1>& _h = (*H);
+    matrix::Matrix<Scalar, 24, 1>& _h = (*H);

    _h.setZero();

@@ -16,20 +16,20 @@ namespace sym {
 * Symbolic function: compute_mag_z_innov_var_and_h
 *
 * Args:
- *     state: Matrix24_1
- *     P: Matrix23_23
+ *     state: Matrix25_1
+ *     P: Matrix24_24
 *     R: Scalar
 *     epsilon: Scalar
 *
 * Outputs:
 *     innov_var: Scalar
- *     H: Matrix23_1
+ *     H: Matrix24_1
 */
 template <typename Scalar>
-void ComputeMagZInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
-                             const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
+void ComputeMagZInnovVarAndH(const matrix::Matrix<Scalar, 25, 1>& state,
+                             const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
                             const Scalar epsilon, Scalar* const innov_var = nullptr,
-                             matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
+                             matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
  // Total ops: 161

  // Unused inputs
@@ -85,7 +85,7 @@ void ComputeMagZInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
  }

  if (H != nullptr) {
-    matrix::Matrix<Scalar, 23, 1>& _h = (*H);
+    matrix::Matrix<Scalar, 24, 1>& _h = (*H);

    _h.setZero();

@@ -16,169 +16,172 @@ namespace sym {
 * Symbolic function: compute_sideslip_h_and_k
 *
 * Args:
- *     state: Matrix24_1
- *     P: Matrix23_23
+ *     state: Matrix25_1
+ *     P: Matrix24_24
 *     innov_var: Scalar
 *     epsilon: Scalar
 *
 * Outputs:
- *     H: Matrix23_1
- *     K: Matrix23_1
+ *     H: Matrix24_1
+ *     K: Matrix24_1
 */
 template <typename Scalar>
-void ComputeSideslipHAndK(const matrix::Matrix<Scalar, 24, 1>& state,
-                          const matrix::Matrix<Scalar, 23, 23>& P, const Scalar innov_var,
-                          const Scalar epsilon, matrix::Matrix<Scalar, 23, 1>* const H = nullptr,
-                          matrix::Matrix<Scalar, 23, 1>* const K = nullptr) {
-  // Total ops: 497
+void ComputeSideslipHAndK(const matrix::Matrix<Scalar, 25, 1>& state,
+                          const matrix::Matrix<Scalar, 24, 24>& P, const Scalar innov_var,
+                          const Scalar epsilon, matrix::Matrix<Scalar, 24, 1>* const H = nullptr,
+                          matrix::Matrix<Scalar, 24, 1>* const K = nullptr) {
+  // Total ops: 513

  // Input arrays

-  // Intermediate terms (45)
+  // Intermediate terms (43)
  const Scalar _tmp0 = -state(23, 0) + state(5, 0);
  const Scalar _tmp1 = 2 * state(1, 0);
  const Scalar _tmp2 = -state(22, 0) + state(4, 0);
-  const Scalar _tmp3 = 2 * state(6, 0);
-  const Scalar _tmp4 = _tmp3 * state(0, 0);
-  const Scalar _tmp5 = 1 - 2 * std::pow(state(3, 0), Scalar(2));
-  const Scalar _tmp6 = _tmp5 - 2 * std::pow(state(2, 0), Scalar(2));
-  const Scalar _tmp7 = 2 * state(0, 0);
-  const Scalar _tmp8 = _tmp7 * state(3, 0);
-  const Scalar _tmp9 = 2 * state(2, 0);
-  const Scalar _tmp10 = _tmp9 * state(1, 0);
-  const Scalar _tmp11 = _tmp10 + _tmp8;
-  const Scalar _tmp12 = _tmp1 * state(3, 0) - _tmp9 * state(0, 0);
-  const Scalar _tmp13 = _tmp0 * _tmp11 + _tmp12 * state(6, 0) + _tmp2 * _tmp6;
-  const Scalar _tmp14 =
-      _tmp13 + epsilon * (2 * math::min<Scalar>(0, (((_tmp13) > 0) - ((_tmp13) < 0))) + 1);
-  const Scalar _tmp15 = _tmp5 - 2 * std::pow(state(1, 0), Scalar(2));
-  const Scalar _tmp16 = _tmp10 - _tmp8;
-  const Scalar _tmp17 = _tmp7 * state(1, 0) + _tmp9 * state(3, 0);
-  const Scalar _tmp18 =
-      (_tmp0 * _tmp15 + _tmp16 * _tmp2 + _tmp17 * state(6, 0)) / std::pow(_tmp14, Scalar(2));
-  const Scalar _tmp19 = _tmp3 * state(3, 0);
-  const Scalar _tmp20 = Scalar(1.0) / (_tmp14);
-  const Scalar _tmp21 = -_tmp18 * (_tmp0 * _tmp1 - 4 * _tmp2 * state(2, 0) - _tmp4) +
-                        _tmp20 * (_tmp1 * _tmp2 + _tmp19);
-  const Scalar _tmp22 = (Scalar(1) / Scalar(2)) * state(3, 0);
-  const Scalar _tmp23 =
-      -Scalar(1) / Scalar(2) * _tmp18 * (_tmp0 * _tmp9 + _tmp19) +
-      (Scalar(1) / Scalar(2)) * _tmp20 * (-4 * _tmp0 * state(1, 0) + _tmp2 * _tmp9 + _tmp4);
-  const Scalar _tmp24 = 2 * state(3, 0);
-  const Scalar _tmp25 = _tmp3 * state(2, 0);
-  const Scalar _tmp26 = _tmp3 * state(1, 0);
-  const Scalar _tmp27 = -_tmp18 * (_tmp0 * _tmp24 - _tmp25) + _tmp20 * (-_tmp2 * _tmp24 + _tmp26);
-  const Scalar _tmp28 = (Scalar(1) / Scalar(2)) * _tmp27;
-  const Scalar _tmp29 = 4 * state(3, 0);
+  const Scalar _tmp3 = 4 * _tmp2;
+  const Scalar _tmp4 = 2 * state(6, 0);
+  const Scalar _tmp5 = _tmp4 * state(0, 0);
+  const Scalar _tmp6 = 1 - 2 * std::pow(state(3, 0), Scalar(2));
+  const Scalar _tmp7 = _tmp6 - 2 * std::pow(state(2, 0), Scalar(2));
+  const Scalar _tmp8 = 2 * state(0, 0);
+  const Scalar _tmp9 = _tmp8 * state(3, 0);
+  const Scalar _tmp10 = 2 * state(2, 0);
+  const Scalar _tmp11 = _tmp10 * state(1, 0);
+  const Scalar _tmp12 = _tmp11 + _tmp9;
+  const Scalar _tmp13 = _tmp1 * state(3, 0) - _tmp10 * state(0, 0);
+  const Scalar _tmp14 = _tmp0 * _tmp12 + _tmp13 * state(6, 0) + _tmp2 * _tmp7;
+  const Scalar _tmp15 =
+      _tmp14 + epsilon * (2 * math::min<Scalar>(0, (((_tmp14) > 0) - ((_tmp14) < 0))) + 1);
+  const Scalar _tmp16 = _tmp6 - 2 * std::pow(state(1, 0), Scalar(2));
+  const Scalar _tmp17 = _tmp11 - _tmp9;
+  const Scalar _tmp18 = _tmp10 * state(3, 0) + _tmp8 * state(1, 0);
+  const Scalar _tmp19 =
+      (_tmp0 * _tmp16 + _tmp17 * _tmp2 + _tmp18 * state(6, 0)) / std::pow(_tmp15, Scalar(2));
+  const Scalar _tmp20 = _tmp4 * state(3, 0);
+  const Scalar _tmp21 = Scalar(1.0) / (_tmp15);
+  const Scalar _tmp22 =
+      -Scalar(1) / Scalar(2) * _tmp19 * (_tmp0 * _tmp1 - _tmp3 * state(2, 0) - _tmp5) +
+      (Scalar(1) / Scalar(2)) * _tmp21 * (_tmp1 * _tmp2 + _tmp20);
+  const Scalar _tmp23 = 4 * _tmp0;
+  const Scalar _tmp24 =
+      -Scalar(1) / Scalar(2) * _tmp19 * (_tmp0 * _tmp10 + _tmp20) +
+      (Scalar(1) / Scalar(2)) * _tmp21 * (_tmp10 * _tmp2 - _tmp23 * state(1, 0) + _tmp5);
+  const Scalar _tmp25 = 2 * state(3, 0);
+  const Scalar _tmp26 = _tmp4 * state(2, 0);
+  const Scalar _tmp27 = _tmp4 * state(1, 0);
+  const Scalar _tmp28 = -Scalar(1) / Scalar(2) * _tmp19 * (_tmp0 * _tmp25 - _tmp26) +
+                        (Scalar(1) / Scalar(2)) * _tmp21 * (-_tmp2 * _tmp25 + _tmp27);
+  const Scalar _tmp29 =
+      -Scalar(1) / Scalar(2) * _tmp19 * (_tmp0 * _tmp8 + _tmp27 - _tmp3 * state(3, 0)) +
+      (Scalar(1) / Scalar(2)) * _tmp21 * (-_tmp2 * _tmp8 - _tmp23 * state(3, 0) + _tmp26);
  const Scalar _tmp30 =
-      -Scalar(1) / Scalar(2) * _tmp18 * (_tmp0 * _tmp7 - _tmp2 * _tmp29 + _tmp26) +
-      (Scalar(1) / Scalar(2)) * _tmp20 * (-_tmp0 * _tmp29 - _tmp2 * _tmp7 + _tmp25);
+      -_tmp22 * state(3, 0) + _tmp24 * state(0, 0) - _tmp28 * state(1, 0) + _tmp29 * state(2, 0);
  const Scalar _tmp31 =
-      -_tmp21 * _tmp22 + _tmp23 * state(0, 0) - _tmp28 * state(1, 0) + _tmp30 * state(2, 0);
-  const Scalar _tmp32 = (Scalar(1) / Scalar(2)) * _tmp21;
-  const Scalar _tmp33 =
-      _tmp23 * state(3, 0) - _tmp28 * state(2, 0) - _tmp30 * state(1, 0) + _tmp32 * state(0, 0);
-  const Scalar _tmp34 =
-      -_tmp22 * _tmp27 - _tmp23 * state(2, 0) + _tmp30 * state(0, 0) + _tmp32 * state(1, 0);
-  const Scalar _tmp35 = _tmp18 * _tmp6;
-  const Scalar _tmp36 = _tmp16 * _tmp20;
-  const Scalar _tmp37 = -_tmp35 + _tmp36;
-  const Scalar _tmp38 = _tmp11 * _tmp18;
-  const Scalar _tmp39 = _tmp15 * _tmp20;
-  const Scalar _tmp40 = -_tmp38 + _tmp39;
-  const Scalar _tmp41 = -_tmp12 * _tmp18 + _tmp17 * _tmp20;
-  const Scalar _tmp42 = _tmp35 - _tmp36;
-  const Scalar _tmp43 = _tmp38 - _tmp39;
-  const Scalar _tmp44 = Scalar(1.0) / (math::max<Scalar>(epsilon, innov_var));
+      _tmp22 * state(0, 0) + _tmp24 * state(3, 0) - _tmp28 * state(2, 0) - _tmp29 * state(1, 0);
+  const Scalar _tmp32 =
+      _tmp22 * state(1, 0) - _tmp24 * state(2, 0) - _tmp28 * state(3, 0) + _tmp29 * state(0, 0);
+  const Scalar _tmp33 = _tmp19 * _tmp7;
+  const Scalar _tmp34 = _tmp17 * _tmp21;
+  const Scalar _tmp35 = -_tmp33 + _tmp34;
+  const Scalar _tmp36 = _tmp12 * _tmp19;
+  const Scalar _tmp37 = _tmp16 * _tmp21;
+  const Scalar _tmp38 = -_tmp36 + _tmp37;
+  const Scalar _tmp39 = -_tmp13 * _tmp19 + _tmp18 * _tmp21;
+  const Scalar _tmp40 = _tmp33 - _tmp34;
+  const Scalar _tmp41 = _tmp36 - _tmp37;
+  const Scalar _tmp42 = Scalar(1.0) / (math::max<Scalar>(epsilon, innov_var));

  // Output terms (2)
  if (H != nullptr) {
-    matrix::Matrix<Scalar, 23, 1>& _h = (*H);
+    matrix::Matrix<Scalar, 24, 1>& _h = (*H);

    _h.setZero();

-    _h(0, 0) = _tmp31;
-    _h(1, 0) = _tmp33;
-    _h(2, 0) = _tmp34;
-    _h(3, 0) = _tmp37;
-    _h(4, 0) = _tmp40;
-    _h(5, 0) = _tmp41;
-    _h(21, 0) = _tmp42;
-    _h(22, 0) = _tmp43;
+    _h(0, 0) = _tmp30;
+    _h(1, 0) = _tmp31;
+    _h(2, 0) = _tmp32;
+    _h(3, 0) = _tmp35;
+    _h(4, 0) = _tmp38;
+    _h(5, 0) = _tmp39;
+    _h(21, 0) = _tmp40;
+    _h(22, 0) = _tmp41;
  }

  if (K != nullptr) {
-    matrix::Matrix<Scalar, 23, 1>& _k = (*K);
+    matrix::Matrix<Scalar, 24, 1>& _k = (*K);

    _k(0, 0) =
-        _tmp44 * (P(0, 0) * _tmp31 + P(0, 1) * _tmp33 + P(0, 2) * _tmp34 + P(0, 21) * _tmp42 +
-                  P(0, 22) * _tmp43 + P(0, 3) * _tmp37 + P(0, 4) * _tmp40 + P(0, 5) * _tmp41);
+        _tmp42 * (P(0, 0) * _tmp30 + P(0, 1) * _tmp31 + P(0, 2) * _tmp32 + P(0, 21) * _tmp40 +
+                  P(0, 22) * _tmp41 + P(0, 3) * _tmp35 + P(0, 4) * _tmp38 + P(0, 5) * _tmp39);
    _k(1, 0) =
-        _tmp44 * (P(1, 0) * _tmp31 + P(1, 1) * _tmp33 + P(1, 2) * _tmp34 + P(1, 21) * _tmp42 +
-                  P(1, 22) * _tmp43 + P(1, 3) * _tmp37 + P(1, 4) * _tmp40 + P(1, 5) * _tmp41);
+        _tmp42 * (P(1, 0) * _tmp30 + P(1, 1) * _tmp31 + P(1, 2) * _tmp32 + P(1, 21) * _tmp40 +
+                  P(1, 22) * _tmp41 + P(1, 3) * _tmp35 + P(1, 4) * _tmp38 + P(1, 5) * _tmp39);
    _k(2, 0) =
-        _tmp44 * (P(2, 0) * _tmp31 + P(2, 1) * _tmp33 + P(2, 2) * _tmp34 + P(2, 21) * _tmp42 +
-                  P(2, 22) * _tmp43 + P(2, 3) * _tmp37 + P(2, 4) * _tmp40 + P(2, 5) * _tmp41);
+        _tmp42 * (P(2, 0) * _tmp30 + P(2, 1) * _tmp31 + P(2, 2) * _tmp32 + P(2, 21) * _tmp40 +
+                  P(2, 22) * _tmp41 + P(2, 3) * _tmp35 + P(2, 4) * _tmp38 + P(2, 5) * _tmp39);
    _k(3, 0) =
-        _tmp44 * (P(3, 0) * _tmp31 + P(3, 1) * _tmp33 + P(3, 2) * _tmp34 + P(3, 21) * _tmp42 +
-                  P(3, 22) * _tmp43 + P(3, 3) * _tmp37 + P(3, 4) * _tmp40 + P(3, 5) * _tmp41);
+        _tmp42 * (P(3, 0) * _tmp30 + P(3, 1) * _tmp31 + P(3, 2) * _tmp32 + P(3, 21) * _tmp40 +
+                  P(3, 22) * _tmp41 + P(3, 3) * _tmp35 + P(3, 4) * _tmp38 + P(3, 5) * _tmp39);
    _k(4, 0) =
-        _tmp44 * (P(4, 0) * _tmp31 + P(4, 1) * _tmp33 + P(4, 2) * _tmp34 + P(4, 21) * _tmp42 +
-                  P(4, 22) * _tmp43 + P(4, 3) * _tmp37 + P(4, 4) * _tmp40 + P(4, 5) * _tmp41);
+        _tmp42 * (P(4, 0) * _tmp30 + P(4, 1) * _tmp31 + P(4, 2) * _tmp32 + P(4, 21) * _tmp40 +
+                  P(4, 22) * _tmp41 + P(4, 3) * _tmp35 + P(4, 4) * _tmp38 + P(4, 5) * _tmp39);
    _k(5, 0) =
-        _tmp44 * (P(5, 0) * _tmp31 + P(5, 1) * _tmp33 + P(5, 2) * _tmp34 + P(5, 21) * _tmp42 +
-                  P(5, 22) * _tmp43 + P(5, 3) * _tmp37 + P(5, 4) * _tmp40 + P(5, 5) * _tmp41);
+        _tmp42 * (P(5, 0) * _tmp30 + P(5, 1) * _tmp31 + P(5, 2) * _tmp32 + P(5, 21) * _tmp40 +
+                  P(5, 22) * _tmp41 + P(5, 3) * _tmp35 + P(5, 4) * _tmp38 + P(5, 5) * _tmp39);
    _k(6, 0) =
-        _tmp44 * (P(6, 0) * _tmp31 + P(6, 1) * _tmp33 + P(6, 2) * _tmp34 + P(6, 21) * _tmp42 +
-                  P(6, 22) * _tmp43 + P(6, 3) * _tmp37 + P(6, 4) * _tmp40 + P(6, 5) * _tmp41);
+        _tmp42 * (P(6, 0) * _tmp30 + P(6, 1) * _tmp31 + P(6, 2) * _tmp32 + P(6, 21) * _tmp40 +
+                  P(6, 22) * _tmp41 + P(6, 3) * _tmp35 + P(6, 4) * _tmp38 + P(6, 5) * _tmp39);
    _k(7, 0) =
-        _tmp44 * (P(7, 0) * _tmp31 + P(7, 1) * _tmp33 + P(7, 2) * _tmp34 + P(7, 21) * _tmp42 +
-                  P(7, 22) * _tmp43 + P(7, 3) * _tmp37 + P(7, 4) * _tmp40 + P(7, 5) * _tmp41);
+        _tmp42 * (P(7, 0) * _tmp30 + P(7, 1) * _tmp31 + P(7, 2) * _tmp32 + P(7, 21) * _tmp40 +
+                  P(7, 22) * _tmp41 + P(7, 3) * _tmp35 + P(7, 4) * _tmp38 + P(7, 5) * _tmp39);
    _k(8, 0) =
-        _tmp44 * (P(8, 0) * _tmp31 + P(8, 1) * _tmp33 + P(8, 2) * _tmp34 + P(8, 21) * _tmp42 +
-                  P(8, 22) * _tmp43 + P(8, 3) * _tmp37 + P(8, 4) * _tmp40 + P(8, 5) * _tmp41);
+        _tmp42 * (P(8, 0) * _tmp30 + P(8, 1) * _tmp31 + P(8, 2) * _tmp32 + P(8, 21) * _tmp40 +
+                  P(8, 22) * _tmp41 + P(8, 3) * _tmp35 + P(8, 4) * _tmp38 + P(8, 5) * _tmp39);
    _k(9, 0) =
-        _tmp44 * (P(9, 0) * _tmp31 + P(9, 1) * _tmp33 + P(9, 2) * _tmp34 + P(9, 21) * _tmp42 +
-                  P(9, 22) * _tmp43 + P(9, 3) * _tmp37 + P(9, 4) * _tmp40 + P(9, 5) * _tmp41);
+        _tmp42 * (P(9, 0) * _tmp30 + P(9, 1) * _tmp31 + P(9, 2) * _tmp32 + P(9, 21) * _tmp40 +
+                  P(9, 22) * _tmp41 + P(9, 3) * _tmp35 + P(9, 4) * _tmp38 + P(9, 5) * _tmp39);
    _k(10, 0) =
-        _tmp44 * (P(10, 0) * _tmp31 + P(10, 1) * _tmp33 + P(10, 2) * _tmp34 + P(10, 21) * _tmp42 +
-                  P(10, 22) * _tmp43 + P(10, 3) * _tmp37 + P(10, 4) * _tmp40 + P(10, 5) * _tmp41);
+        _tmp42 * (P(10, 0) * _tmp30 + P(10, 1) * _tmp31 + P(10, 2) * _tmp32 + P(10, 21) * _tmp40 +
+                  P(10, 22) * _tmp41 + P(10, 3) * _tmp35 + P(10, 4) * _tmp38 + P(10, 5) * _tmp39);
    _k(11, 0) =
-        _tmp44 * (P(11, 0) * _tmp31 + P(11, 1) * _tmp33 + P(11, 2) * _tmp34 + P(11, 21) * _tmp42 +
-                  P(11, 22) * _tmp43 + P(11, 3) * _tmp37 + P(11, 4) * _tmp40 + P(11, 5) * _tmp41);
+        _tmp42 * (P(11, 0) * _tmp30 + P(11, 1) * _tmp31 + P(11, 2) * _tmp32 + P(11, 21) * _tmp40 +
+                  P(11, 22) * _tmp41 + P(11, 3) * _tmp35 + P(11, 4) * _tmp38 + P(11, 5) * _tmp39);
    _k(12, 0) =
-        _tmp44 * (P(12, 0) * _tmp31 + P(12, 1) * _tmp33 + P(12, 2) * _tmp34 + P(12, 21) * _tmp42 +
-                  P(12, 22) * _tmp43 + P(12, 3) * _tmp37 + P(12, 4) * _tmp40 + P(12, 5) * _tmp41);
+        _tmp42 * (P(12, 0) * _tmp30 + P(12, 1) * _tmp31 + P(12, 2) * _tmp32 + P(12, 21) * _tmp40 +
+                  P(12, 22) * _tmp41 + P(12, 3) * _tmp35 + P(12, 4) * _tmp38 + P(12, 5) * _tmp39);
    _k(13, 0) =
-        _tmp44 * (P(13, 0) * _tmp31 + P(13, 1) * _tmp33 + P(13, 2) * _tmp34 + P(13, 21) * _tmp42 +
-                  P(13, 22) * _tmp43 + P(13, 3) * _tmp37 + P(13, 4) * _tmp40 + P(13, 5) * _tmp41);
+        _tmp42 * (P(13, 0) * _tmp30 + P(13, 1) * _tmp31 + P(13, 2) * _tmp32 + P(13, 21) * _tmp40 +
+                  P(13, 22) * _tmp41 + P(13, 3) * _tmp35 + P(13, 4) * _tmp38 + P(13, 5) * _tmp39);
    _k(14, 0) =
-        _tmp44 * (P(14, 0) * _tmp31 + P(14, 1) * _tmp33 + P(14, 2) * _tmp34 + P(14, 21) * _tmp42 +
-                  P(14, 22) * _tmp43 + P(14, 3) * _tmp37 + P(14, 4) * _tmp40 + P(14, 5) * _tmp41);
+        _tmp42 * (P(14, 0) * _tmp30 + P(14, 1) * _tmp31 + P(14, 2) * _tmp32 + P(14, 21) * _tmp40 +
+                  P(14, 22) * _tmp41 + P(14, 3) * _tmp35 + P(14, 4) * _tmp38 + P(14, 5) * _tmp39);
    _k(15, 0) =
-        _tmp44 * (P(15, 0) * _tmp31 + P(15, 1) * _tmp33 + P(15, 2) * _tmp34 + P(15, 21) * _tmp42 +
-                  P(15, 22) * _tmp43 + P(15, 3) * _tmp37 + P(15, 4) * _tmp40 + P(15, 5) * _tmp41);
+        _tmp42 * (P(15, 0) * _tmp30 + P(15, 1) * _tmp31 + P(15, 2) * _tmp32 + P(15, 21) * _tmp40 +
+                  P(15, 22) * _tmp41 + P(15, 3) * _tmp35 + P(15, 4) * _tmp38 + P(15, 5) * _tmp39);
    _k(16, 0) =
-        _tmp44 * (P(16, 0) * _tmp31 + P(16, 1) * _tmp33 + P(16, 2) * _tmp34 + P(16, 21) * _tmp42 +
-                  P(16, 22) * _tmp43 + P(16, 3) * _tmp37 + P(16, 4) * _tmp40 + P(16, 5) * _tmp41);
+        _tmp42 * (P(16, 0) * _tmp30 + P(16, 1) * _tmp31 + P(16, 2) * _tmp32 + P(16, 21) * _tmp40 +
+                  P(16, 22) * _tmp41 + P(16, 3) * _tmp35 + P(16, 4) * _tmp38 + P(16, 5) * _tmp39);
    _k(17, 0) =
-        _tmp44 * (P(17, 0) * _tmp31 + P(17, 1) * _tmp33 + P(17, 2) * _tmp34 + P(17, 21) * _tmp42 +
-                  P(17, 22) * _tmp43 + P(17, 3) * _tmp37 + P(17, 4) * _tmp40 + P(17, 5) * _tmp41);
+        _tmp42 * (P(17, 0) * _tmp30 + P(17, 1) * _tmp31 + P(17, 2) * _tmp32 + P(17, 21) * _tmp40 +
+                  P(17, 22) * _tmp41 + P(17, 3) * _tmp35 + P(17, 4) * _tmp38 + P(17, 5) * _tmp39);
    _k(18, 0) =
-        _tmp44 * (P(18, 0) * _tmp31 + P(18, 1) * _tmp33 + P(18, 2) * _tmp34 + P(18, 21) * _tmp42 +
-                  P(18, 22) * _tmp43 + P(18, 3) * _tmp37 + P(18, 4) * _tmp40 + P(18, 5) * _tmp41);
+        _tmp42 * (P(18, 0) * _tmp30 + P(18, 1) * _tmp31 + P(18, 2) * _tmp32 + P(18, 21) * _tmp40 +
+                  P(18, 22) * _tmp41 + P(18, 3) * _tmp35 + P(18, 4) * _tmp38 + P(18, 5) * _tmp39);
    _k(19, 0) =
-        _tmp44 * (P(19, 0) * _tmp31 + P(19, 1) * _tmp33 + P(19, 2) * _tmp34 + P(19, 21) * _tmp42 +
-                  P(19, 22) * _tmp43 + P(19, 3) * _tmp37 + P(19, 4) * _tmp40 + P(19, 5) * _tmp41);
+        _tmp42 * (P(19, 0) * _tmp30 + P(19, 1) * _tmp31 + P(19, 2) * _tmp32 + P(19, 21) * _tmp40 +
+                  P(19, 22) * _tmp41 + P(19, 3) * _tmp35 + P(19, 4) * _tmp38 + P(19, 5) * _tmp39);
    _k(20, 0) =
-        _tmp44 * (P(20, 0) * _tmp31 + P(20, 1) * _tmp33 + P(20, 2) * _tmp34 + P(20, 21) * _tmp42 +
-                  P(20, 22) * _tmp43 + P(20, 3) * _tmp37 + P(20, 4) * _tmp40 + P(20, 5) * _tmp41);
+        _tmp42 * (P(20, 0) * _tmp30 + P(20, 1) * _tmp31 + P(20, 2) * _tmp32 + P(20, 21) * _tmp40 +
+                  P(20, 22) * _tmp41 + P(20, 3) * _tmp35 + P(20, 4) * _tmp38 + P(20, 5) * _tmp39);
    _k(21, 0) =
-        _tmp44 * (P(21, 0) * _tmp31 + P(21, 1) * _tmp33 + P(21, 2) * _tmp34 + P(21, 21) * _tmp42 +
-                  P(21, 22) * _tmp43 + P(21, 3) * _tmp37 + P(21, 4) * _tmp40 + P(21, 5) * _tmp41);
+        _tmp42 * (P(21, 0) * _tmp30 + P(21, 1) * _tmp31 + P(21, 2) * _tmp32 + P(21, 21) * _tmp40 +
+                  P(21, 22) * _tmp41 + P(21, 3) * _tmp35 + P(21, 4) * _tmp38 + P(21, 5) * _tmp39);
    _k(22, 0) =
-        _tmp44 * (P(22, 0) * _tmp31 + P(22, 1) * _tmp33 + P(22, 2) * _tmp34 + P(22, 21) * _tmp42 +
-                  P(22, 22) * _tmp43 + P(22, 3) * _tmp37 + P(22, 4) * _tmp40 + P(22, 5) * _tmp41);
+        _tmp42 * (P(22, 0) * _tmp30 + P(22, 1) * _tmp31 + P(22, 2) * _tmp32 + P(22, 21) * _tmp40 +
+                  P(22, 22) * _tmp41 + P(22, 3) * _tmp35 + P(22, 4) * _tmp38 + P(22, 5) * _tmp39);
+    _k(23, 0) =
+        _tmp42 * (P(23, 0) * _tmp30 + P(23, 1) * _tmp31 + P(23, 2) * _tmp32 + P(23, 21) * _tmp40 +
+                  P(23, 22) * _tmp41 + P(23, 3) * _tmp35 + P(23, 4) * _tmp38 + P(23, 5) * _tmp39);
  }
 }  // NOLINT(readability/fn_size)

@@ -16,8 +16,8 @@ namespace sym {
 * Symbolic function: compute_sideslip_innov_and_innov_var
 *
 * Args:
- *     state: Matrix24_1
- *     P: Matrix23_23
+ *     state: Matrix25_1
+ *     P: Matrix24_24
 *     R: Scalar
 *     epsilon: Scalar
 *
@@ -26,8 +26,8 @@ namespace sym {
 *     innov_var: Scalar
 */
 template <typename Scalar>
-void ComputeSideslipInnovAndInnovVar(const matrix::Matrix<Scalar, 24, 1>& state,
-                                     const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
+void ComputeSideslipInnovAndInnovVar(const matrix::Matrix<Scalar, 25, 1>& state,
+                                     const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
                                     const Scalar epsilon, Scalar* const innov = nullptr,
                                     Scalar* const innov_var = nullptr) {
  // Total ops: 265
@@ -16,19 +16,19 @@ namespace sym {
 * Symbolic function: compute_yaw_innov_var_and_h
 *
 * Args:
- *     state: Matrix24_1
- *     P: Matrix23_23
+ *     state: Matrix25_1
+ *     P: Matrix24_24
 *     R: Scalar
 *
 * Outputs:
 *     innov_var: Scalar
- *     H: Matrix23_1
+ *     H: Matrix24_1
 */
 template <typename Scalar>
-void ComputeYawInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
-                            const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
+void ComputeYawInnovVarAndH(const matrix::Matrix<Scalar, 25, 1>& state,
+                            const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
                            Scalar* const innov_var = nullptr,
-                            matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
+                            matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
  // Total ops: 1

  // Unused inputs
@@ -46,7 +46,7 @@ void ComputeYawInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
  }

  if (H != nullptr) {
-    matrix::Matrix<Scalar, 23, 1>& _h = (*H);
+    matrix::Matrix<Scalar, 24, 1>& _h = (*H);

    _h.setZero();

@@ -16,8 +16,8 @@ namespace sym {
 * Symbolic function: predict_covariance
 *
 * Args:
- *     state: Matrix24_1
- *     P: Matrix23_23
+ *     state: Matrix25_1
+ *     P: Matrix24_24
 *     accel: Matrix31
 *     accel_var: Matrix31
 *     gyro: Matrix31
@@ -25,23 +25,23 @@ namespace sym {
 *     dt: Scalar
 *
 * Outputs:
- *     res: Matrix23_23
+ *     res: Matrix24_24
 */
 template <typename Scalar>
-matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24, 1>& state,
-                                                const matrix::Matrix<Scalar, 23, 23>& P,
+matrix::Matrix<Scalar, 24, 24> PredictCovariance(const matrix::Matrix<Scalar, 25, 1>& state,
+                                                const matrix::Matrix<Scalar, 24, 24>& P,
                                                const matrix::Matrix<Scalar, 3, 1>& accel,
                                                const matrix::Matrix<Scalar, 3, 1>& accel_var,
                                                const matrix::Matrix<Scalar, 3, 1>& gyro,
                                                const Scalar gyro_var, const Scalar dt) {
-  // Total ops: 1754
+  // Total ops: 1810

  // Unused inputs
  (void)gyro;

  // Input arrays

-  // Intermediate terms (147)
+  // Intermediate terms (144)
  const Scalar _tmp0 = 2 * state(1, 0);
  const Scalar _tmp1 = _tmp0 * state(3, 0);
  const Scalar _tmp2 = -_tmp1 * dt;
@@ -177,64 +177,62 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
  const Scalar _tmp110 = _tmp109 * dt;
  const Scalar _tmp111 = -_tmp1;
  const Scalar _tmp112 = _tmp11 + _tmp77;
-  const Scalar _tmp113 =
-      _tmp67 * (_tmp111 + _tmp4) + _tmp72 * (_tmp104 + _tmp112) + _tmp74 * (_tmp106 + _tmp78);
-  const Scalar _tmp114 = _tmp26 * dt;
-  const Scalar _tmp115 = P(0, 4) * _tmp18 + P(10, 4) * _tmp23 + P(11, 4) * _tmp6 + P(9, 4) * _tmp15;
-  const Scalar _tmp116 =
+  const Scalar _tmp113 = dt * (_tmp67 * (_tmp111 + _tmp4) + _tmp72 * (_tmp104 + _tmp112) +
+                               _tmp74 * (_tmp106 + _tmp78));
+  const Scalar _tmp114 = P(0, 4) * _tmp18 + P(10, 4) * _tmp23 + P(11, 4) * _tmp6 + P(9, 4) * _tmp15;
+  const Scalar _tmp115 =
      P(1, 0) * _tmp18 + P(10, 0) * _tmp29 + P(11, 0) * _tmp34 + P(9, 0) * _tmp36;
-  const Scalar _tmp117 = _tmp113 * dt;
-  const Scalar _tmp118 =
+  const Scalar _tmp116 =
      P(1, 4) * _tmp18 + P(10, 4) * _tmp29 + P(11, 4) * _tmp34 + P(9, 4) * _tmp36;
-  const Scalar _tmp119 =
+  const Scalar _tmp117 =
      P(10, 0) * _tmp45 + P(11, 0) * _tmp44 + P(2, 0) * _tmp18 + P(9, 0) * _tmp47;
-  const Scalar _tmp120 =
+  const Scalar _tmp118 =
      P(10, 4) * _tmp45 + P(11, 4) * _tmp44 + P(2, 4) * _tmp18 + P(9, 4) * _tmp47;
-  const Scalar _tmp121 = _tmp56 * _tmp96;
-  const Scalar _tmp122 = _tmp60 * _tmp97;
-  const Scalar _tmp123 = P(1, 0) * _tmp75 - P(12, 0) * _tmp57 - P(13, 0) * _tmp61 -
+  const Scalar _tmp119 = _tmp56 * _tmp96;
+  const Scalar _tmp120 = _tmp60 * _tmp97;
+  const Scalar _tmp121 = P(1, 0) * _tmp75 - P(12, 0) * _tmp57 - P(13, 0) * _tmp61 -
                         P(14, 0) * _tmp64 + P(2, 0) * _tmp79 + P(3, 0);
-  const Scalar _tmp124 = _tmp63 * _tmp98;
-  const Scalar _tmp125 = P(1, 4) * _tmp75 - P(12, 4) * _tmp57 - P(13, 4) * _tmp61 -
+  const Scalar _tmp122 = _tmp63 * _tmp98;
+  const Scalar _tmp123 = P(1, 4) * _tmp75 - P(12, 4) * _tmp57 - P(13, 4) * _tmp61 -
                         P(14, 4) * _tmp64 + P(2, 4) * _tmp79 + P(3, 4);
-  const Scalar _tmp126 = P(0, 0) * _tmp117 - P(12, 0) * _tmp110 - P(13, 0) * _tmp102 -
+  const Scalar _tmp124 = P(0, 0) * _tmp113 - P(12, 0) * _tmp110 - P(13, 0) * _tmp102 -
                         P(14, 0) * _tmp108 + P(2, 0) * _tmp105 + P(4, 0);
-  const Scalar _tmp127 = P(0, 13) * _tmp117 - P(12, 13) * _tmp110 - P(13, 13) * _tmp102 -
+  const Scalar _tmp125 = P(0, 13) * _tmp113 - P(12, 13) * _tmp110 - P(13, 13) * _tmp102 -
                         P(14, 13) * _tmp108 + P(2, 13) * _tmp105 + P(4, 13);
-  const Scalar _tmp128 = P(0, 14) * _tmp117 - P(12, 14) * _tmp110 - P(13, 14) * _tmp102 -
+  const Scalar _tmp126 = P(0, 14) * _tmp113 - P(12, 14) * _tmp110 - P(13, 14) * _tmp102 -
                         P(14, 14) * _tmp108 + P(2, 14) * _tmp105 + P(4, 14);
-  const Scalar _tmp129 = P(0, 12) * _tmp117 - P(12, 12) * _tmp110 - P(13, 12) * _tmp102 -
+  const Scalar _tmp127 = P(0, 12) * _tmp113 - P(12, 12) * _tmp110 - P(13, 12) * _tmp102 -
                         P(14, 12) * _tmp108 + P(2, 12) * _tmp105 + P(4, 12);
-  const Scalar _tmp130 = P(0, 4) * _tmp117 - P(12, 4) * _tmp110 - P(13, 4) * _tmp102 -
+  const Scalar _tmp128 = P(0, 4) * _tmp113 - P(12, 4) * _tmp110 - P(13, 4) * _tmp102 -
                         P(14, 4) * _tmp108 + P(2, 4) * _tmp105 + P(4, 4);
-  const Scalar _tmp131 = _tmp100 + _tmp55;
-  const Scalar _tmp132 = _tmp131 * dt;
-  const Scalar _tmp133 =
+  const Scalar _tmp129 = _tmp100 + _tmp55;
+  const Scalar _tmp130 = _tmp129 * dt;
+  const Scalar _tmp131 =
      dt * (_tmp67 * (_tmp112 + _tmp69) + _tmp72 * (_tmp111 + _tmp65) + _tmp74 * (_tmp21 + _tmp76));
-  const Scalar _tmp134 = _tmp73 * dt;
-  const Scalar _tmp135 = _tmp66 * dt;
-  const Scalar _tmp136 = _tmp107 * _tmp72 + _tmp109 * _tmp67 + _tmp74 * (_tmp103 + _tmp17 + _tmp68);
-  const Scalar _tmp137 = P(0, 5) * _tmp18 + P(10, 5) * _tmp23 + P(11, 5) * _tmp6 + P(9, 5) * _tmp15;
-  const Scalar _tmp138 = _tmp136 * dt;
-  const Scalar _tmp139 =
+  const Scalar _tmp132 = _tmp73 * dt;
+  const Scalar _tmp133 = _tmp66 * dt;
+  const Scalar _tmp134 =
+      dt * (_tmp107 * _tmp72 + _tmp109 * _tmp67 + _tmp74 * (_tmp103 + _tmp17 + _tmp68));
+  const Scalar _tmp135 = P(0, 5) * _tmp18 + P(10, 5) * _tmp23 + P(11, 5) * _tmp6 + P(9, 5) * _tmp15;
+  const Scalar _tmp136 =
      P(1, 5) * _tmp18 + P(10, 5) * _tmp29 + P(11, 5) * _tmp34 + P(9, 5) * _tmp36;
-  const Scalar _tmp140 =
+  const Scalar _tmp137 =
      P(10, 5) * _tmp45 + P(11, 5) * _tmp44 + P(2, 5) * _tmp18 + P(9, 5) * _tmp47;
-  const Scalar _tmp141 = P(1, 5) * _tmp75 - P(12, 5) * _tmp57 - P(13, 5) * _tmp61 -
+  const Scalar _tmp138 = P(1, 5) * _tmp75 - P(12, 5) * _tmp57 - P(13, 5) * _tmp61 -
                         P(14, 5) * _tmp64 + P(2, 5) * _tmp79 + P(3, 5);
-  const Scalar _tmp142 = P(0, 5) * _tmp117 - P(12, 5) * _tmp110 - P(13, 5) * _tmp102 -
+  const Scalar _tmp139 = P(0, 5) * _tmp113 - P(12, 5) * _tmp110 - P(13, 5) * _tmp102 -
                         P(14, 5) * _tmp108 + P(2, 5) * _tmp105 + P(4, 5);
-  const Scalar _tmp143 = P(0, 14) * _tmp138 + P(1, 14) * _tmp133 - P(12, 14) * _tmp135 -
-                         P(13, 14) * _tmp134 - P(14, 14) * _tmp132 + P(5, 14);
-  const Scalar _tmp144 = P(0, 13) * _tmp138 + P(1, 13) * _tmp133 - P(12, 13) * _tmp135 -
-                         P(13, 13) * _tmp134 - P(14, 13) * _tmp132 + P(5, 13);
-  const Scalar _tmp145 = P(0, 12) * _tmp138 + P(1, 12) * _tmp133 - P(12, 12) * _tmp135 -
-                         P(13, 12) * _tmp134 - P(14, 12) * _tmp132 + P(5, 12);
-  const Scalar _tmp146 = P(0, 5) * _tmp138 + P(1, 5) * _tmp133 - P(12, 5) * _tmp135 -
-                         P(13, 5) * _tmp134 - P(14, 5) * _tmp132 + P(5, 5);
+  const Scalar _tmp140 = P(0, 14) * _tmp134 + P(1, 14) * _tmp131 - P(12, 14) * _tmp133 -
+                         P(13, 14) * _tmp132 - P(14, 14) * _tmp130 + P(5, 14);
+  const Scalar _tmp141 = P(0, 13) * _tmp134 + P(1, 13) * _tmp131 - P(12, 13) * _tmp133 -
+                         P(13, 13) * _tmp132 - P(14, 13) * _tmp130 + P(5, 13);
+  const Scalar _tmp142 = P(0, 12) * _tmp134 + P(1, 12) * _tmp131 - P(12, 12) * _tmp133 -
+                         P(13, 12) * _tmp132 - P(14, 12) * _tmp130 + P(5, 12);
+  const Scalar _tmp143 = P(0, 5) * _tmp134 + P(1, 5) * _tmp131 - P(12, 5) * _tmp133 -
+                         P(13, 5) * _tmp132 - P(14, 5) * _tmp130 + P(5, 5);

  // Output terms (1)
-  matrix::Matrix<Scalar, 23, 23> _res;
+  matrix::Matrix<Scalar, 24, 24> _res;

  _res.setZero();

@@ -264,40 +262,40 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
               std::pow(_tmp63, Scalar(2)) * _tmp98 - _tmp64 * _tmp94 + _tmp75 * _tmp91 +
               _tmp79 * _tmp92 + _tmp99;
  _res(0, 4) = -_tmp102 * _tmp58 + _tmp105 * _tmp46 - _tmp108 * _tmp62 - _tmp110 * _tmp53 +
-               _tmp113 * _tmp114 + _tmp115;
+               _tmp113 * _tmp26 + _tmp114;
  _res(1, 4) = -_tmp102 * _tmp82 + _tmp105 * _tmp48 - _tmp108 * _tmp83 - _tmp110 * _tmp81 +
-               _tmp116 * _tmp117 + _tmp118;
+               _tmp113 * _tmp115 + _tmp116;
  _res(2, 4) = -_tmp102 * _tmp87 + _tmp105 * _tmp51 - _tmp108 * _tmp88 - _tmp110 * _tmp85 +
-               _tmp117 * _tmp119 + _tmp120;
-  _res(3, 4) = _tmp101 * _tmp122 - _tmp102 * _tmp93 + _tmp105 * _tmp92 + _tmp107 * _tmp124 -
-               _tmp108 * _tmp94 + _tmp109 * _tmp121 - _tmp110 * _tmp90 + _tmp117 * _tmp123 +
-               _tmp125;
-  _res(4, 4) = std::pow(_tmp101, Scalar(2)) * _tmp97 - _tmp102 * _tmp127 +
-               _tmp105 * (P(0, 2) * _tmp117 - P(12, 2) * _tmp110 - P(13, 2) * _tmp102 -
+               _tmp113 * _tmp117 + _tmp118;
+  _res(3, 4) = _tmp101 * _tmp120 - _tmp102 * _tmp93 + _tmp105 * _tmp92 + _tmp107 * _tmp122 -
+               _tmp108 * _tmp94 + _tmp109 * _tmp119 - _tmp110 * _tmp90 + _tmp113 * _tmp121 +
+               _tmp123;
+  _res(4, 4) = std::pow(_tmp101, Scalar(2)) * _tmp97 - _tmp102 * _tmp125 +
+               _tmp105 * (P(0, 2) * _tmp113 - P(12, 2) * _tmp110 - P(13, 2) * _tmp102 -
                          P(14, 2) * _tmp108 + P(2, 2) * _tmp105 + P(4, 2)) +
-               std::pow(_tmp107, Scalar(2)) * _tmp98 - _tmp108 * _tmp128 +
-               std::pow(_tmp109, Scalar(2)) * _tmp96 - _tmp110 * _tmp129 + _tmp117 * _tmp126 +
-               _tmp130;
-  _res(0, 5) = _tmp114 * _tmp136 - _tmp132 * _tmp62 + _tmp133 * _tmp35 - _tmp134 * _tmp58 -
-               _tmp135 * _tmp53 + _tmp137;
-  _res(1, 5) = _tmp116 * _tmp138 - _tmp132 * _tmp83 + _tmp133 * _tmp43 - _tmp134 * _tmp82 -
-               _tmp135 * _tmp81 + _tmp139;
-  _res(2, 5) = _tmp119 * _tmp138 - _tmp132 * _tmp88 + _tmp133 * _tmp86 - _tmp134 * _tmp87 -
-               _tmp135 * _tmp85 + _tmp140;
-  _res(3, 5) = _tmp121 * _tmp66 + _tmp122 * _tmp73 + _tmp123 * _tmp138 + _tmp124 * _tmp131 -
-               _tmp132 * _tmp94 + _tmp133 * _tmp91 - _tmp134 * _tmp93 - _tmp135 * _tmp90 + _tmp141;
-  _res(4, 5) = _tmp101 * _tmp73 * _tmp97 + _tmp107 * _tmp131 * _tmp98 + _tmp109 * _tmp66 * _tmp96 +
-               _tmp126 * _tmp138 - _tmp127 * _tmp134 - _tmp128 * _tmp132 - _tmp129 * _tmp135 +
-               _tmp133 * (P(0, 1) * _tmp117 - P(12, 1) * _tmp110 - P(13, 1) * _tmp102 -
+               std::pow(_tmp107, Scalar(2)) * _tmp98 - _tmp108 * _tmp126 +
+               std::pow(_tmp109, Scalar(2)) * _tmp96 - _tmp110 * _tmp127 + _tmp113 * _tmp124 +
+               _tmp128;
+  _res(0, 5) = -_tmp130 * _tmp62 + _tmp131 * _tmp35 - _tmp132 * _tmp58 - _tmp133 * _tmp53 +
+               _tmp134 * _tmp26 + _tmp135;
+  _res(1, 5) = _tmp115 * _tmp134 - _tmp130 * _tmp83 + _tmp131 * _tmp43 - _tmp132 * _tmp82 -
+               _tmp133 * _tmp81 + _tmp136;
+  _res(2, 5) = _tmp117 * _tmp134 - _tmp130 * _tmp88 + _tmp131 * _tmp86 - _tmp132 * _tmp87 -
+               _tmp133 * _tmp85 + _tmp137;
+  _res(3, 5) = _tmp119 * _tmp66 + _tmp120 * _tmp73 + _tmp121 * _tmp134 + _tmp122 * _tmp129 -
+               _tmp130 * _tmp94 + _tmp131 * _tmp91 - _tmp132 * _tmp93 - _tmp133 * _tmp90 + _tmp138;
+  _res(4, 5) = _tmp101 * _tmp73 * _tmp97 + _tmp107 * _tmp129 * _tmp98 + _tmp109 * _tmp66 * _tmp96 +
+               _tmp124 * _tmp134 - _tmp125 * _tmp132 - _tmp126 * _tmp130 - _tmp127 * _tmp133 +
+               _tmp131 * (P(0, 1) * _tmp113 - P(12, 1) * _tmp110 - P(13, 1) * _tmp102 -
                          P(14, 1) * _tmp108 + P(2, 1) * _tmp105 + P(4, 1)) +
-               _tmp142;
-  _res(5, 5) = std::pow(_tmp131, Scalar(2)) * _tmp98 - _tmp132 * _tmp143 +
-               _tmp133 * (P(0, 1) * _tmp138 + P(1, 1) * _tmp133 - P(12, 1) * _tmp135 -
-                          P(13, 1) * _tmp134 - P(14, 1) * _tmp132 + P(5, 1)) -
-               _tmp134 * _tmp144 - _tmp135 * _tmp145 +
-               _tmp138 * (P(0, 0) * _tmp138 + P(1, 0) * _tmp133 - P(12, 0) * _tmp135 -
-                          P(13, 0) * _tmp134 - P(14, 0) * _tmp132 + P(5, 0)) +
-               _tmp146 + std::pow(_tmp66, Scalar(2)) * _tmp96 +
+               _tmp139;
+  _res(5, 5) = std::pow(_tmp129, Scalar(2)) * _tmp98 - _tmp130 * _tmp140 +
+               _tmp131 * (P(0, 1) * _tmp134 + P(1, 1) * _tmp131 - P(12, 1) * _tmp133 -
+                          P(13, 1) * _tmp132 - P(14, 1) * _tmp130 + P(5, 1)) -
+               _tmp132 * _tmp141 - _tmp133 * _tmp142 +
+               _tmp134 * (P(0, 0) * _tmp134 + P(1, 0) * _tmp131 - P(12, 0) * _tmp133 -
+                          P(13, 0) * _tmp132 - P(14, 0) * _tmp130 + P(5, 0)) +
+               _tmp143 + std::pow(_tmp66, Scalar(2)) * _tmp96 +
               std::pow(_tmp73, Scalar(2)) * _tmp97;
  _res(0, 6) =
      P(0, 6) * _tmp18 + P(10, 6) * _tmp23 + P(11, 6) * _tmp6 + P(9, 6) * _tmp15 + _tmp80 * dt;
@@ -307,43 +305,43 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
      P(10, 6) * _tmp45 + P(11, 6) * _tmp44 + P(2, 6) * _tmp18 + P(9, 6) * _tmp47 + _tmp89 * dt;
  _res(3, 6) = P(1, 6) * _tmp75 - P(12, 6) * _tmp57 - P(13, 6) * _tmp61 - P(14, 6) * _tmp64 +
               P(2, 6) * _tmp79 + P(3, 6) + _tmp99 * dt;
-  _res(4, 6) = P(0, 6) * _tmp117 - P(12, 6) * _tmp110 - P(13, 6) * _tmp102 - P(14, 6) * _tmp108 +
+  _res(4, 6) = P(0, 6) * _tmp113 - P(12, 6) * _tmp110 - P(13, 6) * _tmp102 - P(14, 6) * _tmp108 +
               P(2, 6) * _tmp105 + P(4, 6) +
-               dt * (P(0, 3) * _tmp117 - P(12, 3) * _tmp110 - P(13, 3) * _tmp102 -
+               dt * (P(0, 3) * _tmp113 - P(12, 3) * _tmp110 - P(13, 3) * _tmp102 -
                     P(14, 3) * _tmp108 + P(2, 3) * _tmp105 + P(4, 3));
-  _res(5, 6) = P(0, 6) * _tmp138 + P(1, 6) * _tmp133 - P(12, 6) * _tmp135 - P(13, 6) * _tmp134 -
-               P(14, 6) * _tmp132 + P(5, 6) +
-               dt * (P(0, 3) * _tmp138 + P(1, 3) * _tmp133 - P(12, 3) * _tmp135 -
-                     P(13, 3) * _tmp134 - P(14, 3) * _tmp132 + P(5, 3));
+  _res(5, 6) = P(0, 6) * _tmp134 + P(1, 6) * _tmp131 - P(12, 6) * _tmp133 - P(13, 6) * _tmp132 -
+               P(14, 6) * _tmp130 + P(5, 6) +
+               dt * (P(0, 3) * _tmp134 + P(1, 3) * _tmp131 - P(12, 3) * _tmp133 -
+                     P(13, 3) * _tmp132 - P(14, 3) * _tmp130 + P(5, 3));
  _res(6, 6) = P(3, 6) * dt + P(6, 6) + dt * (P(3, 3) * dt + P(6, 3));
  _res(0, 7) =
-      P(0, 7) * _tmp18 + P(10, 7) * _tmp23 + P(11, 7) * _tmp6 + P(9, 7) * _tmp15 + _tmp115 * dt;
+      P(0, 7) * _tmp18 + P(10, 7) * _tmp23 + P(11, 7) * _tmp6 + P(9, 7) * _tmp15 + _tmp114 * dt;
  _res(1, 7) =
-      P(1, 7) * _tmp18 + P(10, 7) * _tmp29 + P(11, 7) * _tmp34 + P(9, 7) * _tmp36 + _tmp118 * dt;
+      P(1, 7) * _tmp18 + P(10, 7) * _tmp29 + P(11, 7) * _tmp34 + P(9, 7) * _tmp36 + _tmp116 * dt;
  _res(2, 7) =
-      P(10, 7) * _tmp45 + P(11, 7) * _tmp44 + P(2, 7) * _tmp18 + P(9, 7) * _tmp47 + _tmp120 * dt;
+      P(10, 7) * _tmp45 + P(11, 7) * _tmp44 + P(2, 7) * _tmp18 + P(9, 7) * _tmp47 + _tmp118 * dt;
  _res(3, 7) = P(1, 7) * _tmp75 - P(12, 7) * _tmp57 - P(13, 7) * _tmp61 - P(14, 7) * _tmp64 +
-               P(2, 7) * _tmp79 + P(3, 7) + _tmp125 * dt;
-  _res(4, 7) = P(0, 7) * _tmp117 - P(12, 7) * _tmp110 - P(13, 7) * _tmp102 - P(14, 7) * _tmp108 +
-               P(2, 7) * _tmp105 + P(4, 7) + _tmp130 * dt;
-  _res(5, 7) = P(0, 7) * _tmp138 + P(1, 7) * _tmp133 - P(12, 7) * _tmp135 - P(13, 7) * _tmp134 -
-               P(14, 7) * _tmp132 + P(5, 7) +
-               dt * (P(0, 4) * _tmp138 + P(1, 4) * _tmp133 - P(12, 4) * _tmp135 -
-                     P(13, 4) * _tmp134 - P(14, 4) * _tmp132 + P(5, 4));
+               P(2, 7) * _tmp79 + P(3, 7) + _tmp123 * dt;
+  _res(4, 7) = P(0, 7) * _tmp113 - P(12, 7) * _tmp110 - P(13, 7) * _tmp102 - P(14, 7) * _tmp108 +
+               P(2, 7) * _tmp105 + P(4, 7) + _tmp128 * dt;
+  _res(5, 7) = P(0, 7) * _tmp134 + P(1, 7) * _tmp131 - P(12, 7) * _tmp133 - P(13, 7) * _tmp132 -
+               P(14, 7) * _tmp130 + P(5, 7) +
+               dt * (P(0, 4) * _tmp134 + P(1, 4) * _tmp131 - P(12, 4) * _tmp133 -
+                     P(13, 4) * _tmp132 - P(14, 4) * _tmp130 + P(5, 4));
  _res(6, 7) = P(3, 7) * dt + P(6, 7) + dt * (P(3, 4) * dt + P(6, 4));
  _res(7, 7) = P(4, 7) * dt + P(7, 7) + dt * (P(4, 4) * dt + P(7, 4));
  _res(0, 8) =
-      P(0, 8) * _tmp18 + P(10, 8) * _tmp23 + P(11, 8) * _tmp6 + P(9, 8) * _tmp15 + _tmp137 * dt;
+      P(0, 8) * _tmp18 + P(10, 8) * _tmp23 + P(11, 8) * _tmp6 + P(9, 8) * _tmp15 + _tmp135 * dt;
  _res(1, 8) =
-      P(1, 8) * _tmp18 + P(10, 8) * _tmp29 + P(11, 8) * _tmp34 + P(9, 8) * _tmp36 + _tmp139 * dt;
+      P(1, 8) * _tmp18 + P(10, 8) * _tmp29 + P(11, 8) * _tmp34 + P(9, 8) * _tmp36 + _tmp136 * dt;
  _res(2, 8) =
-      P(10, 8) * _tmp45 + P(11, 8) * _tmp44 + P(2, 8) * _tmp18 + P(9, 8) * _tmp47 + _tmp140 * dt;
+      P(10, 8) * _tmp45 + P(11, 8) * _tmp44 + P(2, 8) * _tmp18 + P(9, 8) * _tmp47 + _tmp137 * dt;
  _res(3, 8) = P(1, 8) * _tmp75 - P(12, 8) * _tmp57 - P(13, 8) * _tmp61 - P(14, 8) * _tmp64 +
-               P(2, 8) * _tmp79 + P(3, 8) + _tmp141 * dt;
-  _res(4, 8) = P(0, 8) * _tmp117 - P(12, 8) * _tmp110 - P(13, 8) * _tmp102 - P(14, 8) * _tmp108 +
-               P(2, 8) * _tmp105 + P(4, 8) + _tmp142 * dt;
-  _res(5, 8) = P(0, 8) * _tmp138 + P(1, 8) * _tmp133 - P(12, 8) * _tmp135 - P(13, 8) * _tmp134 -
-               P(14, 8) * _tmp132 + P(5, 8) + _tmp146 * dt;
+               P(2, 8) * _tmp79 + P(3, 8) + _tmp138 * dt;
+  _res(4, 8) = P(0, 8) * _tmp113 - P(12, 8) * _tmp110 - P(13, 8) * _tmp102 - P(14, 8) * _tmp108 +
+               P(2, 8) * _tmp105 + P(4, 8) + _tmp139 * dt;
+  _res(5, 8) = P(0, 8) * _tmp134 + P(1, 8) * _tmp131 - P(12, 8) * _tmp133 - P(13, 8) * _tmp132 -
+               P(14, 8) * _tmp130 + P(5, 8) + _tmp143 * dt;
  _res(6, 8) = P(3, 8) * dt + P(6, 8) + dt * (P(3, 5) * dt + P(6, 5));
  _res(7, 8) = P(4, 8) * dt + P(7, 8) + dt * (P(4, 5) * dt + P(7, 5));
  _res(8, 8) = P(5, 8) * dt + P(8, 8) + dt * (P(5, 5) * dt + P(8, 5));
@@ -352,10 +350,10 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
  _res(2, 9) = _tmp52;
  _res(3, 9) = P(1, 9) * _tmp75 - P(12, 9) * _tmp57 - P(13, 9) * _tmp61 - P(14, 9) * _tmp64 +
               P(2, 9) * _tmp79 + P(3, 9);
-  _res(4, 9) = P(0, 9) * _tmp117 - P(12, 9) * _tmp110 - P(13, 9) * _tmp102 - P(14, 9) * _tmp108 +
+  _res(4, 9) = P(0, 9) * _tmp113 - P(12, 9) * _tmp110 - P(13, 9) * _tmp102 - P(14, 9) * _tmp108 +
               P(2, 9) * _tmp105 + P(4, 9);
-  _res(5, 9) = P(0, 9) * _tmp138 + P(1, 9) * _tmp133 - P(12, 9) * _tmp135 - P(13, 9) * _tmp134 -
-               P(14, 9) * _tmp132 + P(5, 9);
+  _res(5, 9) = P(0, 9) * _tmp134 + P(1, 9) * _tmp131 - P(12, 9) * _tmp133 - P(13, 9) * _tmp132 -
+               P(14, 9) * _tmp130 + P(5, 9);
  _res(6, 9) = P(3, 9) * dt + P(6, 9);
  _res(7, 9) = P(4, 9) * dt + P(7, 9);
  _res(8, 9) = P(5, 9) * dt + P(8, 9);
@@ -365,10 +363,10 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
  _res(2, 10) = _tmp49;
  _res(3, 10) = P(1, 10) * _tmp75 - P(12, 10) * _tmp57 - P(13, 10) * _tmp61 - P(14, 10) * _tmp64 +
                P(2, 10) * _tmp79 + P(3, 10);
-  _res(4, 10) = P(0, 10) * _tmp117 - P(12, 10) * _tmp110 - P(13, 10) * _tmp102 -
+  _res(4, 10) = P(0, 10) * _tmp113 - P(12, 10) * _tmp110 - P(13, 10) * _tmp102 -
                P(14, 10) * _tmp108 + P(2, 10) * _tmp105 + P(4, 10);
-  _res(5, 10) = P(0, 10) * _tmp138 + P(1, 10) * _tmp133 - P(12, 10) * _tmp135 -
-                P(13, 10) * _tmp134 - P(14, 10) * _tmp132 + P(5, 10);
+  _res(5, 10) = P(0, 10) * _tmp134 + P(1, 10) * _tmp131 - P(12, 10) * _tmp133 -
+                P(13, 10) * _tmp132 - P(14, 10) * _tmp130 + P(5, 10);
  _res(6, 10) = P(3, 10) * dt + P(6, 10);
  _res(7, 10) = P(4, 10) * dt + P(7, 10);
  _res(8, 10) = P(5, 10) * dt + P(8, 10);
@@ -379,10 +377,10 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
  _res(2, 11) = _tmp50;
  _res(3, 11) = P(1, 11) * _tmp75 - P(12, 11) * _tmp57 - P(13, 11) * _tmp61 - P(14, 11) * _tmp64 +
                P(2, 11) * _tmp79 + P(3, 11);
-  _res(4, 11) = P(0, 11) * _tmp117 - P(12, 11) * _tmp110 - P(13, 11) * _tmp102 -
+  _res(4, 11) = P(0, 11) * _tmp113 - P(12, 11) * _tmp110 - P(13, 11) * _tmp102 -
                P(14, 11) * _tmp108 + P(2, 11) * _tmp105 + P(4, 11);
-  _res(5, 11) = P(0, 11) * _tmp138 + P(1, 11) * _tmp133 - P(12, 11) * _tmp135 -
-                P(13, 11) * _tmp134 - P(14, 11) * _tmp132 + P(5, 11);
+  _res(5, 11) = P(0, 11) * _tmp134 + P(1, 11) * _tmp131 - P(12, 11) * _tmp133 -
+                P(13, 11) * _tmp132 - P(14, 11) * _tmp130 + P(5, 11);
  _res(6, 11) = P(3, 11) * dt + P(6, 11);
  _res(7, 11) = P(4, 11) * dt + P(7, 11);
  _res(8, 11) = P(5, 11) * dt + P(8, 11);
@@ -393,8 +391,8 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
  _res(1, 12) = _tmp81;
  _res(2, 12) = _tmp85;
  _res(3, 12) = _tmp90;
-  _res(4, 12) = _tmp129;
-  _res(5, 12) = _tmp145;
+  _res(4, 12) = _tmp127;
+  _res(5, 12) = _tmp142;
  _res(6, 12) = P(3, 12) * dt + P(6, 12);
  _res(7, 12) = P(4, 12) * dt + P(7, 12);
  _res(8, 12) = P(5, 12) * dt + P(8, 12);
@@ -406,8 +404,8 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
  _res(1, 13) = _tmp82;
  _res(2, 13) = _tmp87;
  _res(3, 13) = _tmp93;
-  _res(4, 13) = _tmp127;
-  _res(5, 13) = _tmp144;
+  _res(4, 13) = _tmp125;
+  _res(5, 13) = _tmp141;
  _res(6, 13) = P(3, 13) * dt + P(6, 13);
  _res(7, 13) = P(4, 13) * dt + P(7, 13);
  _res(8, 13) = P(5, 13) * dt + P(8, 13);
@@ -420,8 +418,8 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
  _res(1, 14) = _tmp83;
  _res(2, 14) = _tmp88;
  _res(3, 14) = _tmp94;
-  _res(4, 14) = _tmp128;
-  _res(5, 14) = _tmp143;
+  _res(4, 14) = _tmp126;
+  _res(5, 14) = _tmp140;
  _res(6, 14) = P(3, 14) * dt + P(6, 14);
  _res(7, 14) = P(4, 14) * dt + P(7, 14);
  _res(8, 14) = P(5, 14) * dt + P(8, 14);
@@ -436,10 +434,10 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
  _res(2, 15) = P(10, 15) * _tmp45 + P(11, 15) * _tmp44 + P(2, 15) * _tmp18 + P(9, 15) * _tmp47;
  _res(3, 15) = P(1, 15) * _tmp75 - P(12, 15) * _tmp57 - P(13, 15) * _tmp61 - P(14, 15) * _tmp64 +
                P(2, 15) * _tmp79 + P(3, 15);
-  _res(4, 15) = P(0, 15) * _tmp117 - P(12, 15) * _tmp110 - P(13, 15) * _tmp102 -
+  _res(4, 15) = P(0, 15) * _tmp113 - P(12, 15) * _tmp110 - P(13, 15) * _tmp102 -
                P(14, 15) * _tmp108 + P(2, 15) * _tmp105 + P(4, 15);
-  _res(5, 15) = P(0, 15) * _tmp138 + P(1, 15) * _tmp133 - P(12, 15) * _tmp135 -
-                P(13, 15) * _tmp134 - P(14, 15) * _tmp132 + P(5, 15);
+  _res(5, 15) = P(0, 15) * _tmp134 + P(1, 15) * _tmp131 - P(12, 15) * _tmp133 -
+                P(13, 15) * _tmp132 - P(14, 15) * _tmp130 + P(5, 15);
  _res(6, 15) = P(3, 15) * dt + P(6, 15);
  _res(7, 15) = P(4, 15) * dt + P(7, 15);
  _res(8, 15) = P(5, 15) * dt + P(8, 15);
@@ -455,10 +453,10 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
  _res(2, 16) = P(10, 16) * _tmp45 + P(11, 16) * _tmp44 + P(2, 16) * _tmp18 + P(9, 16) * _tmp47;
  _res(3, 16) = P(1, 16) * _tmp75 - P(12, 16) * _tmp57 - P(13, 16) * _tmp61 - P(14, 16) * _tmp64 +
                P(2, 16) * _tmp79 + P(3, 16);
-  _res(4, 16) = P(0, 16) * _tmp117 - P(12, 16) * _tmp110 - P(13, 16) * _tmp102 -
+  _res(4, 16) = P(0, 16) * _tmp113 - P(12, 16) * _tmp110 - P(13, 16) * _tmp102 -
                P(14, 16) * _tmp108 + P(2, 16) * _tmp105 + P(4, 16);
-  _res(5, 16) = P(0, 16) * _tmp138 + P(1, 16) * _tmp133 - P(12, 16) * _tmp135 -
-                P(13, 16) * _tmp134 - P(14, 16) * _tmp132 + P(5, 16);
+  _res(5, 16) = P(0, 16) * _tmp134 + P(1, 16) * _tmp131 - P(12, 16) * _tmp133 -
+                P(13, 16) * _tmp132 - P(14, 16) * _tmp130 + P(5, 16);
  _res(6, 16) = P(3, 16) * dt + P(6, 16);
  _res(7, 16) = P(4, 16) * dt + P(7, 16);
  _res(8, 16) = P(5, 16) * dt + P(8, 16);
@@ -475,10 +473,10 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
  _res(2, 17) = P(10, 17) * _tmp45 + P(11, 17) * _tmp44 + P(2, 17) * _tmp18 + P(9, 17) * _tmp47;
  _res(3, 17) = P(1, 17) * _tmp75 - P(12, 17) * _tmp57 - P(13, 17) * _tmp61 - P(14, 17) * _tmp64 +
                P(2, 17) * _tmp79 + P(3, 17);
-  _res(4, 17) = P(0, 17) * _tmp117 - P(12, 17) * _tmp110 - P(13, 17) * _tmp102 -
+  _res(4, 17) = P(0, 17) * _tmp113 - P(12, 17) * _tmp110 - P(13, 17) * _tmp102 -
                P(14, 17) * _tmp108 + P(2, 17) * _tmp105 + P(4, 17);
-  _res(5, 17) = P(0, 17) * _tmp138 + P(1, 17) * _tmp133 - P(12, 17) * _tmp135 -
-                P(13, 17) * _tmp134 - P(14, 17) * _tmp132 + P(5, 17);
+  _res(5, 17) = P(0, 17) * _tmp134 + P(1, 17) * _tmp131 - P(12, 17) * _tmp133 -
+                P(13, 17) * _tmp132 - P(14, 17) * _tmp130 + P(5, 17);
  _res(6, 17) = P(3, 17) * dt + P(6, 17);
  _res(7, 17) = P(4, 17) * dt + P(7, 17);
  _res(8, 17) = P(5, 17) * dt + P(8, 17);
@@ -496,10 +494,10 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
  _res(2, 18) = P(10, 18) * _tmp45 + P(11, 18) * _tmp44 + P(2, 18) * _tmp18 + P(9, 18) * _tmp47;
  _res(3, 18) = P(1, 18) * _tmp75 - P(12, 18) * _tmp57 - P(13, 18) * _tmp61 - P(14, 18) * _tmp64 +
                P(2, 18) * _tmp79 + P(3, 18);
-  _res(4, 18) = P(0, 18) * _tmp117 - P(12, 18) * _tmp110 - P(13, 18) * _tmp102 -
+  _res(4, 18) = P(0, 18) * _tmp113 - P(12, 18) * _tmp110 - P(13, 18) * _tmp102 -
                P(14, 18) * _tmp108 + P(2, 18) * _tmp105 + P(4, 18);
-  _res(5, 18) = P(0, 18) * _tmp138 + P(1, 18) * _tmp133 - P(12, 18) * _tmp135 -
-                P(13, 18) * _tmp134 - P(14, 18) * _tmp132 + P(5, 18);
+  _res(5, 18) = P(0, 18) * _tmp134 + P(1, 18) * _tmp131 - P(12, 18) * _tmp133 -
+                P(13, 18) * _tmp132 - P(14, 18) * _tmp130 + P(5, 18);
  _res(6, 18) = P(3, 18) * dt + P(6, 18);
  _res(7, 18) = P(4, 18) * dt + P(7, 18);
  _res(8, 18) = P(5, 18) * dt + P(8, 18);
@@ -518,10 +516,10 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
  _res(2, 19) = P(10, 19) * _tmp45 + P(11, 19) * _tmp44 + P(2, 19) * _tmp18 + P(9, 19) * _tmp47;
  _res(3, 19) = P(1, 19) * _tmp75 - P(12, 19) * _tmp57 - P(13, 19) * _tmp61 - P(14, 19) * _tmp64 +
                P(2, 19) * _tmp79 + P(3, 19);
-  _res(4, 19) = P(0, 19) * _tmp117 - P(12, 19) * _tmp110 - P(13, 19) * _tmp102 -
+  _res(4, 19) = P(0, 19) * _tmp113 - P(12, 19) * _tmp110 - P(13, 19) * _tmp102 -
                P(14, 19) * _tmp108 + P(2, 19) * _tmp105 + P(4, 19);
-  _res(5, 19) = P(0, 19) * _tmp138 + P(1, 19) * _tmp133 - P(12, 19) * _tmp135 -
-                P(13, 19) * _tmp134 - P(14, 19) * _tmp132 + P(5, 19);
+  _res(5, 19) = P(0, 19) * _tmp134 + P(1, 19) * _tmp131 - P(12, 19) * _tmp133 -
+                P(13, 19) * _tmp132 - P(14, 19) * _tmp130 + P(5, 19);
  _res(6, 19) = P(3, 19) * dt + P(6, 19);
  _res(7, 19) = P(4, 19) * dt + P(7, 19);
  _res(8, 19) = P(5, 19) * dt + P(8, 19);
@@ -541,10 +539,10 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
  _res(2, 20) = P(10, 20) * _tmp45 + P(11, 20) * _tmp44 + P(2, 20) * _tmp18 + P(9, 20) * _tmp47;
  _res(3, 20) = P(1, 20) * _tmp75 - P(12, 20) * _tmp57 - P(13, 20) * _tmp61 - P(14, 20) * _tmp64 +
                P(2, 20) * _tmp79 + P(3, 20);
-  _res(4, 20) = P(0, 20) * _tmp117 - P(12, 20) * _tmp110 - P(13, 20) * _tmp102 -
+  _res(4, 20) = P(0, 20) * _tmp113 - P(12, 20) * _tmp110 - P(13, 20) * _tmp102 -
                P(14, 20) * _tmp108 + P(2, 20) * _tmp105 + P(4, 20);
-  _res(5, 20) = P(0, 20) * _tmp138 + P(1, 20) * _tmp133 - P(12, 20) * _tmp135 -
-                P(13, 20) * _tmp134 - P(14, 20) * _tmp132 + P(5, 20);
+  _res(5, 20) = P(0, 20) * _tmp134 + P(1, 20) * _tmp131 - P(12, 20) * _tmp133 -
+                P(13, 20) * _tmp132 - P(14, 20) * _tmp130 + P(5, 20);
  _res(6, 20) = P(3, 20) * dt + P(6, 20);
  _res(7, 20) = P(4, 20) * dt + P(7, 20);
  _res(8, 20) = P(5, 20) * dt + P(8, 20);
@@ -565,10 +563,10 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
  _res(2, 21) = P(10, 21) * _tmp45 + P(11, 21) * _tmp44 + P(2, 21) * _tmp18 + P(9, 21) * _tmp47;
  _res(3, 21) = P(1, 21) * _tmp75 - P(12, 21) * _tmp57 - P(13, 21) * _tmp61 - P(14, 21) * _tmp64 +
                P(2, 21) * _tmp79 + P(3, 21);
-  _res(4, 21) = P(0, 21) * _tmp117 - P(12, 21) * _tmp110 - P(13, 21) * _tmp102 -
+  _res(4, 21) = P(0, 21) * _tmp113 - P(12, 21) * _tmp110 - P(13, 21) * _tmp102 -
                P(14, 21) * _tmp108 + P(2, 21) * _tmp105 + P(4, 21);
-  _res(5, 21) = P(0, 21) * _tmp138 + P(1, 21) * _tmp133 - P(12, 21) * _tmp135 -
-                P(13, 21) * _tmp134 - P(14, 21) * _tmp132 + P(5, 21);
+  _res(5, 21) = P(0, 21) * _tmp134 + P(1, 21) * _tmp131 - P(12, 21) * _tmp133 -
+                P(13, 21) * _tmp132 - P(14, 21) * _tmp130 + P(5, 21);
  _res(6, 21) = P(3, 21) * dt + P(6, 21);
  _res(7, 21) = P(4, 21) * dt + P(7, 21);
  _res(8, 21) = P(5, 21) * dt + P(8, 21);
@@ -590,10 +588,10 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
  _res(2, 22) = P(10, 22) * _tmp45 + P(11, 22) * _tmp44 + P(2, 22) * _tmp18 + P(9, 22) * _tmp47;
  _res(3, 22) = P(1, 22) * _tmp75 - P(12, 22) * _tmp57 - P(13, 22) * _tmp61 - P(14, 22) * _tmp64 +
                P(2, 22) * _tmp79 + P(3, 22);
-  _res(4, 22) = P(0, 22) * _tmp117 - P(12, 22) * _tmp110 - P(13, 22) * _tmp102 -
+  _res(4, 22) = P(0, 22) * _tmp113 - P(12, 22) * _tmp110 - P(13, 22) * _tmp102 -
                P(14, 22) * _tmp108 + P(2, 22) * _tmp105 + P(4, 22);
-  _res(5, 22) = P(0, 22) * _tmp138 + P(1, 22) * _tmp133 - P(12, 22) * _tmp135 -
-                P(13, 22) * _tmp134 - P(14, 22) * _tmp132 + P(5, 22);
+  _res(5, 22) = P(0, 22) * _tmp134 + P(1, 22) * _tmp131 - P(12, 22) * _tmp133 -
+                P(13, 22) * _tmp132 - P(14, 22) * _tmp130 + P(5, 22);
  _res(6, 22) = P(3, 22) * dt + P(6, 22);
  _res(7, 22) = P(4, 22) * dt + P(7, 22);
  _res(8, 22) = P(5, 22) * dt + P(8, 22);
@@ -611,6 +609,33 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
  _res(20, 22) = P(20, 22);
  _res(21, 22) = P(21, 22);
  _res(22, 22) = P(22, 22);
+  _res(0, 23) = P(0, 23) * _tmp18 + P(10, 23) * _tmp23 + P(11, 23) * _tmp6 + P(9, 23) * _tmp15;
+  _res(1, 23) = P(1, 23) * _tmp18 + P(10, 23) * _tmp29 + P(11, 23) * _tmp34 + P(9, 23) * _tmp36;
+  _res(2, 23) = P(10, 23) * _tmp45 + P(11, 23) * _tmp44 + P(2, 23) * _tmp18 + P(9, 23) * _tmp47;
+  _res(3, 23) = P(1, 23) * _tmp75 - P(12, 23) * _tmp57 - P(13, 23) * _tmp61 - P(14, 23) * _tmp64 +
+                P(2, 23) * _tmp79 + P(3, 23);
+  _res(4, 23) = P(0, 23) * _tmp113 - P(12, 23) * _tmp110 - P(13, 23) * _tmp102 -
+                P(14, 23) * _tmp108 + P(2, 23) * _tmp105 + P(4, 23);
+  _res(5, 23) = P(0, 23) * _tmp134 + P(1, 23) * _tmp131 - P(12, 23) * _tmp133 -
+                P(13, 23) * _tmp132 - P(14, 23) * _tmp130 + P(5, 23);
+  _res(6, 23) = P(3, 23) * dt + P(6, 23);
+  _res(7, 23) = P(4, 23) * dt + P(7, 23);
+  _res(8, 23) = P(5, 23) * dt + P(8, 23);
+  _res(9, 23) = P(9, 23);
+  _res(10, 23) = P(10, 23);
+  _res(11, 23) = P(11, 23);
+  _res(12, 23) = P(12, 23);
+  _res(13, 23) = P(13, 23);
+  _res(14, 23) = P(14, 23);
+  _res(15, 23) = P(15, 23);
+  _res(16, 23) = P(16, 23);
+  _res(17, 23) = P(17, 23);
+  _res(18, 23) = P(18, 23);
+  _res(19, 23) = P(19, 23);
+  _res(20, 23) = P(20, 23);
+  _res(21, 23) = P(21, 23);
+  _res(22, 23) = P(22, 23);
+  _res(23, 23) = P(23, 23);

  return _res;
 }  // NOLINT(readability/fn_size)
@@ -18,9 +18,10 @@ struct StateSample {
 	matrix::Vector3<float> mag_I{};
 	matrix::Vector3<float> mag_B{};
 	matrix::Vector2<float> wind_vel{};
+	float terrain{};

-	matrix::Vector<float, 24> Data() const {
-		matrix::Vector<float, 24> state;
+	matrix::Vector<float, 25> Data() const {
+		matrix::Vector<float, 25> state;
 		state.slice<4, 1>(0, 0) = quat_nominal;
 		state.slice<3, 1>(4, 0) = vel;
 		state.slice<3, 1>(7, 0) = pos;
@@ -29,15 +30,16 @@ struct StateSample {
 		state.slice<3, 1>(16, 0) = mag_I;
 		state.slice<3, 1>(19, 0) = mag_B;
 		state.slice<2, 1>(22, 0) = wind_vel;
+		state.slice<1, 1>(24, 0) = terrain;
 		return state;
 	};

-	const matrix::Vector<float, 24>& vector() const {
-		return *reinterpret_cast<matrix::Vector<float, 24>*>(const_cast<float*>(reinterpret_cast<const float*>(&quat_nominal)));
+	const matrix::Vector<float, 25>& vector() const {
+		return *reinterpret_cast<matrix::Vector<float, 25>*>(const_cast<float*>(reinterpret_cast<const float*>(&quat_nominal)));
 	};

 };
-static_assert(sizeof(matrix::Vector<float, 24>) == sizeof(StateSample), "state vector doesn't match StateSample size");
+static_assert(sizeof(matrix::Vector<float, 25>) == sizeof(StateSample), "state vector doesn't match StateSample size");

 struct IdxDof { unsigned idx; unsigned dof; };
 namespace State {
@@ -49,7 +51,8 @@ namespace State {
 	static constexpr IdxDof mag_I{15, 3};
 	static constexpr IdxDof mag_B{18, 3};
 	static constexpr IdxDof wind_vel{21, 2};
-	static constexpr uint8_t size{23};
+	static constexpr IdxDof terrain{23, 1};
+	static constexpr uint8_t size{24};
 };
 }
 #endif // !EKF_STATE_H
@@ -0,0 +1,103 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015-2023 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file terrain_control.cpp
+ */
+
+#include "ekf.h"
+#include "ekf_derivation/generated/compute_hagl_innov_var.h"
+
+#include <mathlib/mathlib.h>
+
+void Ekf::initTerrain()
+{
+	// assume a ground clearance
+	_state.terrain = _state.pos(2) + _params.rng_gnd_clearance;
+
+	// use the ground clearance value as our uncertainty
+	P.uncorrelateCovarianceSetVariance<State::terrain.dof>(State::terrain.idx, sq(_params.rng_gnd_clearance));
+}
+
+void Ekf::controlTerrainFakeFusion()
+{
+	// If we are on ground, store the local position and time to use as a reference
+	if (!_control_status.flags.in_air) {
+		_last_on_ground_posD = _state.pos(2);
+		_control_status.flags.rng_fault = false;
+
+	} else if (!_control_status_prev.flags.in_air) {
+		// Let the estimator run freely before arming for bench testing purposes, but reset on takeoff
+		// because when using optical flow measurements, it is safer to start with a small distance to ground
+		// as an overestimated distance leads to an overestimated velocity, causing a dangerous behavior.
+		initTerrain();
+	}
+
+	if (!_control_status.flags.in_air
+	    && !_hagl_sensor_status.flags.range_finder
+	    && !_hagl_sensor_status.flags.flow) {
+
+		bool recent_terrain_aiding = false;
+
+#if defined(CONFIG_EKF2_RANGE_FINDER)
+		recent_terrain_aiding |= isRecent(_aid_src_rng_hgt.time_last_fuse, (uint64_t)1e6);
+#endif // CONFIG_EKF2_RANGE_FINDER
+
+#if defined(CONFIG_EKF2_OPTICAL_FLOW)
+		recent_terrain_aiding |= isRecent(_aid_src_optical_flow.time_last_fuse, (uint64_t)1e6);
+#endif // CONFIG_EKF2_OPTICAL_FLOW
+
+		if (_control_status.flags.vehicle_at_rest || !recent_terrain_aiding) {
+			initTerrain();
+		}
+	}
+}
+
+bool Ekf::isTerrainEstimateValid() const
+{
+	// Assume being valid when the uncertainty is small compared to the height above ground
+	float hagl_var = INFINITY;
+	sym::ComputeHaglInnovVar(P, 0.f, &hagl_var);
+	bool valid = hagl_var < fmaxf(sq(0.1f * getHagl()), 0.2f);
+
+#if defined(CONFIG_EKF2_RANGE_FINDER)
+
+	// Assume that the terrain estimate is always valid when direct observations are fused
+	if (_hagl_sensor_status.flags.range_finder && isRecent(_aid_src_rng_hgt.time_last_fuse, (uint64_t)5e6)) {
+		valid = true;
+	}
+
+#endif // CONFIG_EKF2_RANGE_FINDER
+
+	return valid;
+}
@@ -1,99 +0,0 @@
-#!/usr/bin/env python3
-# -*- coding: utf-8 -*-
-"""
-    Copyright (c) 2023-2024 PX4 Development Team
-    Redistribution and use in source and binary forms, with or without
-    modification, are permitted provided that the following conditions
-    are met:
-
-    1. Redistributions of source code must retain the above copyright
-    notice, this list of conditions and the following disclaimer.
-    2. Redistributions in binary form must reproduce the above copyright
-    notice, this list of conditions and the following disclaimer in
-    the documentation and/or other materials provided with the
-    distribution.
-    3. Neither the name PX4 nor the names of its contributors may be
-    used to endorse or promote products derived from this software
-    without specific prior written permission.
-
-    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-    COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-    OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-    AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-    POSSIBILITY OF SUCH DAMAGE.
-
-File: derivation_terrain_estimator.py
-Description:
-"""
-
-import symforce
-symforce.set_epsilon_to_symbol()
-
-import symforce.symbolic as sf
-
-# generate_px4_function from derivation_utils in EKF/ekf_derivation/utils
-import os, sys
-derivation_utils_dir = os.path.dirname(os.path.abspath(__file__)) + "/../../python/ekf_derivation/utils"
-sys.path.append(derivation_utils_dir)
-import derivation_utils
-
-def predict_opt_flow(
-        terrain_vpos: sf.Scalar,
-        q_att: sf.V4,
-        v: sf.V3,
-        pos_z: sf.Scalar,
-        epsilon : sf.Scalar
-):
-    R_to_earth = sf.Rot3(sf.Quaternion(xyz=q_att[1:4], w=q_att[0])).to_rotation_matrix()
-    flow_pred = sf.V2()
-    dist = - (pos_z - terrain_vpos)
-    dist = derivation_utils.add_epsilon_sign(dist, dist, epsilon)
-    flow_pred[0] = -v[1] / dist * R_to_earth[2, 2]
-    flow_pred[1] = v[0] / dist * R_to_earth[2, 2]
-    return flow_pred
-
-def terr_est_compute_flow_xy_innov_var_and_hx(
-        terrain_vpos: sf.Scalar,
-        P: sf.Scalar,
-        q_att: sf.V4,
-        v: sf.V3,
-        pos_z: sf.Scalar,
-        R: sf.Scalar,
-        epsilon : sf.Scalar
-):
-    flow_pred = predict_opt_flow(terrain_vpos, q_att, v, pos_z, epsilon)
-    Hx = sf.V1(flow_pred[0]).jacobian(terrain_vpos)
-    Hy = sf.V1(flow_pred[1]).jacobian(terrain_vpos)
-
-    innov_var = sf.V2()
-    innov_var[0] = (Hx * P * Hx.T + R)[0,0]
-    innov_var[1] = (Hy * P * Hy.T + R)[0,0]
-
-    return (innov_var, Hx[0, 0])
-
-def terr_est_compute_flow_y_innov_var_and_h(
-        terrain_vpos: sf.Scalar,
-        P: sf.Scalar,
-        q_att: sf.V4,
-        v: sf.V3,
-        pos_z: sf.Scalar,
-        R: sf.Scalar,
-        epsilon : sf.Scalar
-):
-    flow_pred = predict_opt_flow(terrain_vpos, q_att, v, pos_z, epsilon)
-    Hy = sf.V1(flow_pred[1]).jacobian(terrain_vpos)
-
-    innov_var = (Hy * P * Hy.T + R)[0,0]
-
-    return (innov_var, Hy[0, 0])
-
-print("Derive terrain estimator equations...")
-derivation_utils.generate_px4_function(terr_est_compute_flow_xy_innov_var_and_hx, output_names=["innov_var", "H"])
-derivation_utils.generate_px4_function(terr_est_compute_flow_y_innov_var_and_h, output_names=["innov_var", "H"])
@@ -1,66 +0,0 @@
-// -----------------------------------------------------------------------------
-// This file was autogenerated by symforce from template:
-//     function/FUNCTION.h.jinja
-// Do NOT modify by hand.
-// -----------------------------------------------------------------------------
-
-#pragma once
-
-#include <matrix/math.hpp>
-
-namespace sym {
-
-/**
- * This function was autogenerated from a symbolic function. Do not modify by hand.
- *
- * Symbolic function: terr_est_compute_flow_xy_innov_var_and_hx
- *
- * Args:
- *     terrain_vpos: Scalar
- *     P: Scalar
- *     q_att: Matrix41
- *     v: Matrix31
- *     pos_z: Scalar
- *     R: Scalar
- *     epsilon: Scalar
- *
- * Outputs:
- *     innov_var: Matrix21
- *     H: Scalar
- */
-template <typename Scalar>
-void TerrEstComputeFlowXyInnovVarAndHx(const Scalar terrain_vpos, const Scalar P,
-                                       const matrix::Matrix<Scalar, 4, 1>& q_att,
-                                       const matrix::Matrix<Scalar, 3, 1>& v, const Scalar pos_z,
-                                       const Scalar R, const Scalar epsilon,
-                                       matrix::Matrix<Scalar, 2, 1>* const innov_var = nullptr,
-                                       Scalar* const H = nullptr) {
-  // Total ops: 27
-
-  // Input arrays
-
-  // Intermediate terms (4)
-  const Scalar _tmp0 =
-      -2 * std::pow(q_att(1, 0), Scalar(2)) - 2 * std::pow(q_att(2, 0), Scalar(2)) + 1;
-  const Scalar _tmp1 = pos_z - terrain_vpos;
-  const Scalar _tmp2 =
-      -_tmp1 + epsilon * (2 * math::min<Scalar>(0, -(((_tmp1) > 0) - ((_tmp1) < 0))) + 1);
-  const Scalar _tmp3 = P * std::pow(_tmp0, Scalar(2)) / std::pow(_tmp2, Scalar(4));
-
-  // Output terms (2)
-  if (innov_var != nullptr) {
-    matrix::Matrix<Scalar, 2, 1>& _innov_var = (*innov_var);
-
-    _innov_var(0, 0) = R + _tmp3 * std::pow(v(1, 0), Scalar(2));
-    _innov_var(1, 0) = R + _tmp3 * std::pow(v(0, 0), Scalar(2));
-  }
-
-  if (H != nullptr) {
-    Scalar& _h = (*H);
-
-    _h = _tmp0 * v(1, 0) / std::pow(_tmp2, Scalar(2));
-  }
-}  // NOLINT(readability/fn_size)
-
-// NOLINTNEXTLINE(readability/fn_size)
-}  // namespace sym
@@ -1,65 +0,0 @@
-// -----------------------------------------------------------------------------
-// This file was autogenerated by symforce from template:
-//     function/FUNCTION.h.jinja
-// Do NOT modify by hand.
-// -----------------------------------------------------------------------------
-
-#pragma once
-
-#include <matrix/math.hpp>
-
-namespace sym {
-
-/**
- * This function was autogenerated from a symbolic function. Do not modify by hand.
- *
- * Symbolic function: terr_est_compute_flow_y_innov_var_and_h
- *
- * Args:
- *     terrain_vpos: Scalar
- *     P: Scalar
- *     q_att: Matrix41
- *     v: Matrix31
- *     pos_z: Scalar
- *     R: Scalar
- *     epsilon: Scalar
- *
- * Outputs:
- *     innov_var: Scalar
- *     H: Scalar
- */
-template <typename Scalar>
-void TerrEstComputeFlowYInnovVarAndH(const Scalar terrain_vpos, const Scalar P,
-                                     const matrix::Matrix<Scalar, 4, 1>& q_att,
-                                     const matrix::Matrix<Scalar, 3, 1>& v, const Scalar pos_z,
-                                     const Scalar R, const Scalar epsilon,
-                                     Scalar* const innov_var = nullptr, Scalar* const H = nullptr) {
-  // Total ops: 25
-
-  // Input arrays
-
-  // Intermediate terms (3)
-  const Scalar _tmp0 =
-      -2 * std::pow(q_att(1, 0), Scalar(2)) - 2 * std::pow(q_att(2, 0), Scalar(2)) + 1;
-  const Scalar _tmp1 = pos_z - terrain_vpos;
-  const Scalar _tmp2 =
-      -_tmp1 + epsilon * (2 * math::min<Scalar>(0, -(((_tmp1) > 0) - ((_tmp1) < 0))) + 1);
-
-  // Output terms (2)
-  if (innov_var != nullptr) {
-    Scalar& _innov_var = (*innov_var);
-
-    _innov_var =
-        P * std::pow(_tmp0, Scalar(2)) * std::pow(v(0, 0), Scalar(2)) / std::pow(_tmp2, Scalar(4)) +
-        R;
-  }
-
-  if (H != nullptr) {
-    Scalar& _h = (*H);
-
-    _h = -_tmp0 * v(0, 0) / std::pow(_tmp2, Scalar(2));
-  }
-}  // NOLINT(readability/fn_size)
-
-// NOLINTNEXTLINE(readability/fn_size)
-}  // namespace sym
@@ -1,456 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2015-2023 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file terrain_estimator.cpp
- * Function for fusing rangefinder and optical flow measurements
- * to estimate terrain vertical position
- */
-
-#include "ekf.h"
-#include "terrain_estimator/derivation/generated/terr_est_compute_flow_xy_innov_var_and_hx.h"
-#include "terrain_estimator/derivation/generated/terr_est_compute_flow_y_innov_var_and_h.h"
-
-#include <mathlib/mathlib.h>
-
-void Ekf::initHagl()
-{
-	// assume a ground clearance
-	_terrain_vpos = _state.pos(2) + _params.rng_gnd_clearance;
-
-	// use the ground clearance value as our uncertainty
-	_terrain_var = sq(_params.rng_gnd_clearance);
-
-#if defined(CONFIG_EKF2_RANGE_FINDER)
-	_aid_src_terrain_range_finder.time_last_fuse = _time_delayed_us;
-#endif // CONFIG_EKF2_RANGE_FINDER
-
-#if defined(CONFIG_EKF2_OPTICAL_FLOW)
-	_aid_src_terrain_optical_flow.time_last_fuse = _time_delayed_us;
-#endif // CONFIG_EKF2_OPTICAL_FLOW
-}
-
-void Ekf::runTerrainEstimator(const imuSample &imu_delayed)
-{
-	// If we are on ground, store the local position and time to use as a reference
-	if (!_control_status.flags.in_air) {
-		_last_on_ground_posD = _state.pos(2);
-		_control_status.flags.rng_fault = false;
-
-	} else if (!_control_status_prev.flags.in_air) {
-		// Let the estimator run freely before arming for bench testing purposes, but reset on takeoff
-		// because when using optical flow measurements, it is safer to start with a small distance to ground
-		// as an overestimated distance leads to an overestimated velocity, causing a dangerous behavior.
-		initHagl();
-	}
-
-	predictHagl(imu_delayed);
-
-#if defined(CONFIG_EKF2_RANGE_FINDER)
-	controlHaglRngFusion();
-#endif // CONFIG_EKF2_RANGE_FINDER
-
-#if defined(CONFIG_EKF2_OPTICAL_FLOW)
-	controlHaglFlowFusion();
-#endif // CONFIG_EKF2_OPTICAL_FLOW
-
-	controlHaglFakeFusion();
-
-	// constrain _terrain_vpos to be a minimum of _params.rng_gnd_clearance larger than _state.pos(2)
-	if (_terrain_vpos - _state.pos(2) < _params.rng_gnd_clearance) {
-		_terrain_vpos = _params.rng_gnd_clearance + _state.pos(2);
-	}
-}
-
-void Ekf::predictHagl(const imuSample &imu_delayed)
-{
-	// predict the state variance growth where the state is the vertical position of the terrain underneath the vehicle
-
-	// process noise due to errors in vehicle height estimate
-	_terrain_var += sq(imu_delayed.delta_vel_dt * _params.terrain_p_noise);
-
-	// process noise due to terrain gradient
-	_terrain_var += sq(imu_delayed.delta_vel_dt * _params.terrain_gradient)
-			* (sq(_state.vel(0)) + sq(_state.vel(1)));
-
-	// limit the variance to prevent it becoming badly conditioned
-	_terrain_var = math::constrain(_terrain_var, 0.0f, 1e4f);
-}
-
-#if defined(CONFIG_EKF2_RANGE_FINDER)
-void Ekf::controlHaglRngFusion()
-{
-	if (!(_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseRangeFinder)
-	    || _control_status.flags.rng_fault) {
-
-		stopHaglRngFusion();
-		return;
-	}
-
-	if (_range_sensor.isDataReady()) {
-		updateHaglRng(_aid_src_terrain_range_finder);
-	}
-
-	if (_range_sensor.isDataHealthy()) {
-
-		const bool continuing_conditions_passing = _rng_consistency_check.isKinematicallyConsistent();
-				//&& !_control_status.flags.rng_hgt // TODO: should not be fused when using range height
-
-		const bool starting_conditions_passing = continuing_conditions_passing
-				&& _range_sensor.isRegularlySendingData()
-				&& (_rng_consistency_check.getTestRatio() < 1.f);
-
-		_time_last_healthy_rng_data = _time_delayed_us;
-
-		if (_hagl_sensor_status.flags.range_finder) {
-			if (continuing_conditions_passing) {
-				fuseHaglRng(_aid_src_terrain_range_finder);
-
-				// We have been rejecting range data for too long
-				const uint64_t timeout = static_cast<uint64_t>(_params.terrain_timeout * 1e6f);
-				const bool is_fusion_failing = isTimedOut(_aid_src_terrain_range_finder.time_last_fuse, timeout);
-
-				if (is_fusion_failing) {
-					if (_range_sensor.getDistBottom() > 2.f * _params.rng_gnd_clearance) {
-						// Data seems good, attempt a reset
-						resetHaglRng();
-
-					} else if (starting_conditions_passing) {
-						// The sensor can probably not detect the ground properly
-						// declare the sensor faulty and stop the fusion
-						_control_status.flags.rng_fault = true;
-						_range_sensor.setFaulty(true);
-						stopHaglRngFusion();
-
-					} else {
-						// This could be a temporary issue, stop the fusion without declaring the sensor faulty
-						stopHaglRngFusion();
-					}
-				}
-
-			} else {
-				stopHaglRngFusion();
-			}
-
-		} else {
-			if (starting_conditions_passing) {
-				_hagl_sensor_status.flags.range_finder = true;
-
-				// Reset the state to the measurement only if the test ratio is large,
-				// otherwise let it converge through the fusion
-				if (_hagl_sensor_status.flags.flow && (_aid_src_terrain_range_finder.test_ratio < 0.2f)) {
-					fuseHaglRng(_aid_src_terrain_range_finder);
-
-				} else {
-					resetHaglRng();
-				}
-			}
-		}
-
-	} else if (_hagl_sensor_status.flags.range_finder && isTimedOut(_time_last_healthy_rng_data, _params.reset_timeout_max)) {
-		// No data anymore. Stop until it comes back.
-		stopHaglRngFusion();
-	}
-}
-
-float Ekf::getRngVar() const
-{
-	return fmaxf(P(State::pos.idx + 2, State::pos.idx + 2) * _params.vehicle_variance_scaler, 0.0f)
-	       + sq(_params.range_noise)
-	       + sq(_params.range_noise_scaler * _range_sensor.getRange());
-}
-
-void Ekf::resetHaglRng()
-{
-	_terrain_vpos = _state.pos(2) + _range_sensor.getDistBottom();
-	_terrain_var = getRngVar();
-	_terrain_vpos_reset_counter++;
-
-	_aid_src_terrain_range_finder.time_last_fuse = _time_delayed_us;
-}
-
-void Ekf::stopHaglRngFusion()
-{
-	if (_hagl_sensor_status.flags.range_finder) {
-		ECL_INFO("stopping HAGL range fusion");
-
-		// reset flags
-		_innov_check_fail_status.flags.reject_hagl = false;
-
-		_hagl_sensor_status.flags.range_finder = false;
-	}
-}
-
-void Ekf::updateHaglRng(estimator_aid_source1d_s &aid_src) const
-{
-	// get a height above ground measurement from the range finder assuming a flat earth
-	const float meas_hagl = _range_sensor.getDistBottom();
-
-	// predict the hagl from the vehicle position and terrain height
-	const float pred_hagl = _terrain_vpos - _state.pos(2);
-
-	// calculate the observation variance adding the variance of the vehicles own height uncertainty
-	const float obs_variance = getRngVar();
-
-	// calculate the innovation variance - limiting it to prevent a badly conditioned fusion
-	const float hagl_innov_var = fmaxf(_terrain_var + obs_variance, obs_variance);
-
-	updateAidSourceStatus(aid_src,
-		_time_delayed_us,                          // sample timestamp
-		meas_hagl,                                 // observation
-		obs_variance,                              // observation variance
-		pred_hagl - meas_hagl,                     // innovation
-		hagl_innov_var,                            // innovation variance
-		math::max(_params.range_innov_gate, 1.f)); // innovation gate
-}
-
-void Ekf::fuseHaglRng(estimator_aid_source1d_s &aid_src)
-{
-	if (!aid_src.innovation_rejected) {
-		// calculate the Kalman gain
-		const float gain = _terrain_var / aid_src.innovation_variance;
-
-		// correct the state
-		_terrain_vpos -= gain * aid_src.innovation;
-
-		// correct the variance
-		_terrain_var = fmaxf(_terrain_var * (1.0f - gain), 0.0f);
-
-		// record last successful fusion event
-		_innov_check_fail_status.flags.reject_hagl = false;
-
-		aid_src.time_last_fuse = _time_delayed_us;
-		aid_src.fused = true;
-
-	} else {
-		_innov_check_fail_status.flags.reject_hagl = true;
-		aid_src.fused = false;
-	}
-}
-#endif // CONFIG_EKF2_RANGE_FINDER
-
-#if defined(CONFIG_EKF2_OPTICAL_FLOW)
-void Ekf::controlHaglFlowFusion()
-{
-	if (!(_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseOpticalFlow)) {
-		stopHaglFlowFusion();
-		return;
-	}
-
-	if (_flow_data_ready) {
-		updateOptFlow(_aid_src_terrain_optical_flow);
-
-		const bool continuing_conditions_passing = _control_status.flags.in_air
-				&& !_control_status.flags.opt_flow
-				&& _control_status.flags.gps
-				&& !_hagl_sensor_status.flags.range_finder;
-
-		const bool starting_conditions_passing = continuing_conditions_passing;
-
-		if (_hagl_sensor_status.flags.flow) {
-			if (continuing_conditions_passing) {
-
-				// TODO: wait until the midpoint of the flow sample has fallen behind the fusion time horizon
-				fuseFlowForTerrain(_aid_src_terrain_optical_flow);
-
-				// TODO: do something when failing continuously the innovation check
-				/* const bool is_fusion_failing = isTimedOut(_aid_src_optical_flow.time_last_fuse, _params.reset_timeout_max); */
-
-				/* if (is_fusion_failing) { */
-				/* 	resetHaglFlow(); */
-				/* } */
-
-			} else {
-				stopHaglFlowFusion();
-			}
-
-		} else {
-			if (starting_conditions_passing) {
-				_hagl_sensor_status.flags.flow = true;
-				// TODO: do a reset instead of trying to fuse the data?
-				fuseFlowForTerrain(_aid_src_terrain_optical_flow);
-			}
-		}
-
-		_flow_data_ready = false;
-
-	} else if (_hagl_sensor_status.flags.flow && (_time_delayed_us > _flow_sample_delayed.time_us + (uint64_t)5e6)) {
-		// No data anymore. Stop until it comes back.
-		stopHaglFlowFusion();
-	}
-}
-
-void Ekf::stopHaglFlowFusion()
-{
-	if (_hagl_sensor_status.flags.flow) {
-		ECL_INFO("stopping HAGL flow fusion");
-
-		_hagl_sensor_status.flags.flow = false;
-	}
-}
-
-void Ekf::resetHaglFlow()
-{
-	// TODO: use the flow data
-	_terrain_vpos = fmaxf(0.0f, _state.pos(2));
-	_terrain_var = 100.0f;
-	_terrain_vpos_reset_counter++;
-
-	_aid_src_terrain_optical_flow.time_last_fuse = _time_delayed_us;
-}
-
-void Ekf::fuseFlowForTerrain(estimator_aid_source2d_s &flow)
-{
-	flow.fused = true;
-
-	const float R_LOS = flow.observation_variance[0];
-
-	// calculate the height above the ground of the optical flow camera. Since earth frame is NED
-	// a positive offset in earth frame leads to a smaller height above the ground.
-	float range = predictFlowRange();
-
-	const float state = _terrain_vpos; // linearize both axes using the same state value
-	Vector2f innov_var;
-	float H;
-	sym::TerrEstComputeFlowXyInnovVarAndHx(state, _terrain_var, _state.quat_nominal, _state.vel, _state.pos(2), R_LOS, FLT_EPSILON, &innov_var, &H);
-	innov_var.copyTo(flow.innovation_variance);
-
-	if ((flow.innovation_variance[0] < R_LOS)
-	    || (flow.innovation_variance[1] < R_LOS)) {
-		// we need to reinitialise the covariance matrix and abort this fusion step
-		ECL_ERR("Opt flow error - covariance reset");
-		_terrain_var = 100.0f;
-		return;
-	}
-
-	_innov_check_fail_status.flags.reject_optflow_X = (flow.test_ratio[0] > 1.f);
-	_innov_check_fail_status.flags.reject_optflow_Y = (flow.test_ratio[1] > 1.f);
-
-	// if either axis fails we abort the fusion
-	if (flow.innovation_rejected) {
-		return;
-	}
-
-	// fuse observation axes sequentially
-	for (uint8_t index = 0; index <= 1; index++) {
-		if (index == 0) {
-			// everything was already computed above
-
-		} 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::TerrEstComputeFlowYInnovVarAndH(state, _terrain_var, _state.quat_nominal, _state.vel, _state.pos(2), R_LOS, FLT_EPSILON, &flow.innovation_variance[1], &H);
-
-			// recalculate the innovation using the updated state
-			const Vector2f vel_body = predictFlowVelBody();
-			range = predictFlowRange();
-			flow.innovation[1] = (-vel_body(0) / range) - flow.observation[1];
-
-			if (flow.innovation_variance[1] < R_LOS) {
-				// we need to reinitialise the covariance matrix and abort this fusion step
-				ECL_ERR("Opt flow error - covariance reset");
-				_terrain_var = 100.0f;
-				return;
-			}
-		}
-
-		float Kfusion = _terrain_var * H / flow.innovation_variance[index];
-
-		_terrain_vpos += Kfusion * flow.innovation[0];
-		// constrain terrain to minimum allowed value and predict height above ground
-		_terrain_vpos = fmaxf(_terrain_vpos, _params.rng_gnd_clearance + _state.pos(2));
-
-		// guard against negative variance
-		_terrain_var = fmaxf(_terrain_var - Kfusion * H * _terrain_var, sq(0.01f));
-	}
-
-	_fault_status.flags.bad_optflow_X = false;
-	_fault_status.flags.bad_optflow_Y = false;
-
-	flow.time_last_fuse = _time_delayed_us;
-	flow.fused = true;
-}
-#endif // CONFIG_EKF2_OPTICAL_FLOW
-
-void Ekf::controlHaglFakeFusion()
-{
-	if (!_control_status.flags.in_air
-	    && !_hagl_sensor_status.flags.range_finder
-	    && !_hagl_sensor_status.flags.flow) {
-
-		bool recent_terrain_aiding = false;
-
-#if defined(CONFIG_EKF2_RANGE_FINDER)
-		recent_terrain_aiding |= isRecent(_aid_src_terrain_range_finder.time_last_fuse, (uint64_t)1e6);
-#endif // CONFIG_EKF2_RANGE_FINDER
-
-#if defined(CONFIG_EKF2_OPTICAL_FLOW)
-		recent_terrain_aiding |= isRecent(_aid_src_terrain_optical_flow.time_last_fuse, (uint64_t)1e6);
-#endif // CONFIG_EKF2_OPTICAL_FLOW
-
-		if (_control_status.flags.vehicle_at_rest || !recent_terrain_aiding) {
-			initHagl();
-		}
-	}
-}
-
-bool Ekf::isTerrainEstimateValid() const
-{
-	bool valid = false;
-
-#if defined(CONFIG_EKF2_RANGE_FINDER)
-
-	// we have been fusing range finder measurements in the last 5 seconds
-	if (isRecent(_aid_src_terrain_range_finder.time_last_fuse, (uint64_t)5e6)) {
-		if (_hagl_sensor_status.flags.range_finder || !_control_status.flags.in_air) {
-			valid = true;
-		}
-	}
-
-#endif // CONFIG_EKF2_RANGE_FINDER
-
-#if defined(CONFIG_EKF2_OPTICAL_FLOW)
-
-	// we have been fusing optical flow measurements for terrain estimation within the last 5 seconds
-	// this can only be the case if the main filter does not fuse optical flow
-	if (_hagl_sensor_status.flags.flow && isRecent(_aid_src_terrain_optical_flow.time_last_fuse, (uint64_t)5e6)) {
-		valid = true;
-	}
-
-#endif // CONFIG_EKF2_OPTICAL_FLOW
-
-	return valid;
-}
-
-void Ekf::terrainHandleVerticalPositionReset(const float delta_z) {
-	_terrain_vpos += delta_z;
-}
@@ -0,0 +1,106 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2024 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file wind.cpp
+ * Helper functions for wind states
+ */
+
+#include "ekf.h"
+
+void Ekf::resetWindToExternalObservation(float wind_speed, float wind_direction, float wind_speed_accuracy, float wind_direction_accuracy)
+{
+	const float wind_speed_constrained = math::max(wind_speed, 0.0f);
+
+	// wind direction is given as azimuth where wind blows FROM, we need direction where wind blows TO
+	const float wind_direction_rad = wrap_pi(math::radians(wind_direction) + M_PI_F);
+	Vector2f wind = wind_speed_constrained * Vector2f(cosf(wind_direction_rad), sinf(wind_direction_rad));
+
+	ECL_INFO("reset wind states to external observation");
+	_information_events.flags.reset_wind_to_ext_obs = true;
+
+	Vector2f innov = _state.wind_vel - wind;
+	float R = sq(0.1f);
+	Vector2f innov_var{P(State::wind_vel.idx, State::wind_vel.idx) + R, P(State::wind_vel.idx + 1, State::wind_vel.idx + 1) + R};
+	const bool control_status_wind_prev = _control_status.flags.wind;
+	_control_status.flags.wind = true;
+	fuseDirectStateMeasurement(innov(0), innov_var(0), R, State::wind_vel.idx);
+	fuseDirectStateMeasurement(innov(1), innov_var(1), R, State::wind_vel.idx + 1);
+	_control_status.flags.wind = control_status_wind_prev;
+
+	// reset the horizontal velocity variances to allow the velocity states to be pulled towards
+	// a solution that is aligned with the newly set wind estimates
+	static constexpr float hor_vel_var = 25.0f;
+	P.uncorrelateCovarianceSetVariance<2>(State::vel.idx, hor_vel_var);
+}
+
+void Ekf::resetWindTo(const Vector2f &wind, const Vector2f &wind_var)
+{
+	_state.wind_vel = wind;
+
+	if (PX4_ISFINITE(wind_var(0))) {
+		P.uncorrelateCovarianceSetVariance<1>(State::wind_vel.idx, math::max(sq(_params.initial_wind_uncertainty), wind_var(0)));
+	}
+
+	if (PX4_ISFINITE(wind_var(1))) {
+		P.uncorrelateCovarianceSetVariance<1>(State::wind_vel.idx + 1, math::max(sq(_params.initial_wind_uncertainty), wind_var(1)));
+	}
+}
+
+void Ekf::resetWindCov()
+{
+	// start with a small initial uncertainty to improve the initial estimate
+	P.uncorrelateCovarianceSetVariance<State::wind_vel.dof>(State::wind_vel.idx, sq(_params.initial_wind_uncertainty));
+}
+
+void Ekf::resetWind()
+{
+#if defined(CONFIG_EKF2_AIRSPEED)
+	if (_control_status.flags.fuse_aspd && isRecent(_airspeed_sample_delayed.time_us, 1e6)) {
+		resetWindUsingAirspeed(_airspeed_sample_delayed);
+		return;
+	}
+#endif // CONFIG_EKF2_AIRSPEED
+
+	resetWindToZero();
+}
+
+void Ekf::resetWindToZero()
+{
+	ECL_INFO("reset wind to zero");
+
+	// If we don't have an airspeed measurement, then assume the wind is zero
+	_state.wind_vel.setZero();
+
+	resetWindCov();
+}
@@ -147,7 +147,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 	_param_ekf2_min_rng(_params->rng_gnd_clearance),
 #endif // CONFIG_EKF2_TERRAIN || CONFIG_EKF2_OPTICAL_FLOW || CONFIG_EKF2_RANGE_FINDER
 #if defined(CONFIG_EKF2_TERRAIN)
-	_param_ekf2_terr_mask(_params->terrain_fusion_mode),
 	_param_ekf2_terr_noise(_params->terrain_p_noise),
 	_param_ekf2_terr_grad(_params->terrain_gradient),
 #endif // CONFIG_EKF2_TERRAIN
@@ -214,18 +213,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 	_param_ekf2_abl_tau(_params->acc_bias_learn_tc),
 	_param_ekf2_gyr_b_lim(_params->gyro_bias_lim)
 {
-	// advertise expected minimal topic set immediately to ensure logging
-	_attitude_pub.advertise();
-	_local_position_pub.advertise();
-
-	_estimator_event_flags_pub.advertise();
-	_estimator_innovation_test_ratios_pub.advertise();
-	_estimator_innovation_variances_pub.advertise();
-	_estimator_innovations_pub.advertise();
-	_estimator_sensor_bias_pub.advertise();
-	_estimator_states_pub.advertise();
-	_estimator_status_flags_pub.advertise();
-	_estimator_status_pub.advertise();
+	AdvertiseTopics();
 }

 EKF2::~EKF2()
@@ -234,121 +222,164 @@ EKF2::~EKF2()
 	perf_free(_msg_missed_imu_perf);
 }

-#if defined(CONFIG_EKF2_MULTI_INSTANCE)
-bool EKF2::multi_init(int imu, int mag)
+void EKF2::AdvertiseTopics()
 {
-	// advertise all topics to ensure consistent uORB instance numbering
+	// advertise expected minimal topic set immediately for logging
+	_attitude_pub.advertise();
+	_local_position_pub.advertise();
 	_estimator_event_flags_pub.advertise();
-	_estimator_innovation_test_ratios_pub.advertise();
-	_estimator_innovation_variances_pub.advertise();
-	_estimator_innovations_pub.advertise();
-#if defined(CONFIG_EKF2_OPTICAL_FLOW)
-	_estimator_optical_flow_vel_pub.advertise();
-#endif // CONFIG_EKF2_OPTICAL_FLOW
 	_estimator_sensor_bias_pub.advertise();
-	_estimator_states_pub.advertise();
-	_estimator_status_flags_pub.advertise();
 	_estimator_status_pub.advertise();
+	_estimator_status_flags_pub.advertise();
+
+	if (_multi_mode) {
+		// only force advertise these in multi mode to ensure consistent uORB instance numbering
+		_global_position_pub.advertise();
+		_odometry_pub.advertise();
+
+#if defined(CONFIG_EKF2_WIND)
+		_wind_pub.advertise();
+#endif // CONFIG_EKF2_WIND
+	}

 #if defined(CONFIG_EKF2_GNSS)
-	_yaw_est_pub.advertise();
+
+	if (_param_ekf2_gps_ctrl.get()) {
+		_estimator_gps_status_pub.advertise();
+		_yaw_est_pub.advertise();
+	}
+
 #endif // CONFIG_EKF2_GNSS

+	// verbose logging
+	if (_param_ekf2_log_verbose.get()) {
+		_estimator_innovation_test_ratios_pub.advertise();
+		_estimator_innovation_variances_pub.advertise();
+		_estimator_innovations_pub.advertise();
+		_estimator_states_pub.advertise();
+
+#if defined(CONFIG_EKF2_AIRSPEED)
+
+		if (_param_ekf2_arsp_thr.get() > 0.f) {
+			_estimator_aid_src_airspeed_pub.advertise();
+		}
+
+#endif // CONFIG_EKF2_AIRSPEED
+
 #if defined(CONFIG_EKF2_BAROMETER)

-	// baro advertise
-	if (_param_ekf2_baro_ctrl.get()) {
-		_estimator_aid_src_baro_hgt_pub.advertise();
-		_estimator_baro_bias_pub.advertise();
-	}
+		if (_param_ekf2_baro_ctrl.get()) {
+			_estimator_aid_src_baro_hgt_pub.advertise();
+			_estimator_baro_bias_pub.advertise();
+		}

 #endif // CONFIG_EKF2_BAROMETER

+#if defined(CONFIG_EKF2_DRAG_FUSION)
+
+		if (_param_ekf2_drag_ctrl.get()) {
+			_estimator_aid_src_drag_pub.advertise();
+		}
+
+#endif // CONFIG_EKF2_DRAG_FUSION
+
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)

-	// EV advertise
-	if (_param_ekf2_ev_ctrl.get() & static_cast<int32_t>(EvCtrl::VPOS)) {
-		_estimator_aid_src_ev_hgt_pub.advertise();
-		_estimator_ev_pos_bias_pub.advertise();
-	}
+		if (_param_ekf2_ev_ctrl.get() & static_cast<int32_t>(EvCtrl::VPOS)) {
+			_estimator_aid_src_ev_hgt_pub.advertise();
+			_estimator_ev_pos_bias_pub.advertise();
+		}

-	if (_param_ekf2_ev_ctrl.get() & static_cast<int32_t>(EvCtrl::HPOS)) {
-		_estimator_aid_src_ev_pos_pub.advertise();
-		_estimator_ev_pos_bias_pub.advertise();
-	}
+		if (_param_ekf2_ev_ctrl.get() & static_cast<int32_t>(EvCtrl::HPOS)) {
+			_estimator_aid_src_ev_pos_pub.advertise();
+			_estimator_ev_pos_bias_pub.advertise();
+		}

-	if (_param_ekf2_ev_ctrl.get() & static_cast<int32_t>(EvCtrl::VEL)) {
-		_estimator_aid_src_ev_vel_pub.advertise();
-	}
+		if (_param_ekf2_ev_ctrl.get() & static_cast<int32_t>(EvCtrl::VEL)) {
+			_estimator_aid_src_ev_vel_pub.advertise();
+		}

-	if (_param_ekf2_ev_ctrl.get() & static_cast<int32_t>(EvCtrl::YAW)) {
-		_estimator_aid_src_ev_yaw_pub.advertise();
-	}
+		if (_param_ekf2_ev_ctrl.get() & static_cast<int32_t>(EvCtrl::YAW)) {
+			_estimator_aid_src_ev_yaw_pub.advertise();
+		}

 #endif // CONFIG_EKF2_EXTERNAL_VISION

 #if defined(CONFIG_EKF2_GNSS)

-	// GNSS advertise
-	if (_param_ekf2_gps_ctrl.get() & static_cast<int32_t>(GnssCtrl::VPOS)) {
-		_estimator_aid_src_gnss_hgt_pub.advertise();
-		_estimator_gnss_hgt_bias_pub.advertise();
-	}
+		if (_param_ekf2_gps_ctrl.get()) {
+			if (_param_ekf2_gps_ctrl.get() & static_cast<int32_t>(GnssCtrl::VPOS)) {
+				_estimator_aid_src_gnss_hgt_pub.advertise();
+				_estimator_gnss_hgt_bias_pub.advertise();
+			}

-	if (_param_ekf2_gps_ctrl.get() & static_cast<int32_t>(GnssCtrl::HPOS)) {
-		_estimator_aid_src_gnss_pos_pub.advertise();
-		_estimator_gps_status_pub.advertise();
-	}
+			if (_param_ekf2_gps_ctrl.get() & static_cast<int32_t>(GnssCtrl::HPOS)) {
+				_estimator_aid_src_gnss_pos_pub.advertise();
+			}

-	if (_param_ekf2_gps_ctrl.get() & static_cast<int32_t>(GnssCtrl::VEL)) {
-		_estimator_aid_src_gnss_vel_pub.advertise();
-	}
+			if (_param_ekf2_gps_ctrl.get() & static_cast<int32_t>(GnssCtrl::VEL)) {
+				_estimator_aid_src_gnss_vel_pub.advertise();
+			}

 # if defined(CONFIG_EKF2_GNSS_YAW)

-	if (_param_ekf2_gps_ctrl.get() & static_cast<int32_t>(GnssCtrl::YAW)) {
-		_estimator_aid_src_gnss_yaw_pub.advertise();
-	}
+			if (_param_ekf2_gps_ctrl.get() & static_cast<int32_t>(GnssCtrl::YAW)) {
+				_estimator_aid_src_gnss_yaw_pub.advertise();
+			}

 # endif // CONFIG_EKF2_GNSS_YAW
+		}
+
 #endif // CONFIG_EKF2_GNSS

 #if defined(CONFIG_EKF2_GRAVITY_FUSION)

-	if (_param_ekf2_imu_ctrl.get() & static_cast<int32_t>(ImuCtrl::GravityVector)) {
-		_estimator_aid_src_gravity_pub.advertise();
-	}
+		if (_param_ekf2_imu_ctrl.get() & static_cast<int32_t>(ImuCtrl::GravityVector)) {
+			_estimator_aid_src_gravity_pub.advertise();
+		}

 #endif // CONFIG_EKF2_GRAVITY_FUSION

-#if defined(CONFIG_EKF2_RANGE_FINDER)
-
-	// RNG advertise
-	if (_param_ekf2_rng_ctrl.get()) {
-		_estimator_aid_src_rng_hgt_pub.advertise();
-		_estimator_rng_hgt_bias_pub.advertise();
-	}
-
-#endif // CONFIG_EKF2_RANGE_FINDER
-
 #if defined(CONFIG_EKF2_MAGNETOMETER)

-	// mag advertise
-	if (_param_ekf2_mag_type.get() != MagFuseType::NONE) {
-		_estimator_aid_src_mag_pub.advertise();
-	}
+		if (_param_ekf2_mag_type.get() != MagFuseType::NONE) {
+			_estimator_aid_src_mag_pub.advertise();
+		}

 #endif // CONFIG_EKF2_MAGNETOMETER

-	_attitude_pub.advertise();
-	_local_position_pub.advertise();
-	_global_position_pub.advertise();
-	_odometry_pub.advertise();
+#if defined(CONFIG_EKF2_OPTICAL_FLOW)

-#if defined(CONFIG_EKF2_WIND)
-	_wind_pub.advertise();
-#endif // CONFIG_EKF2_WIND
+		if (_param_ekf2_of_ctrl.get()) {
+			_estimator_optical_flow_vel_pub.advertise();
+			_estimator_aid_src_optical_flow_pub.advertise();
+		}

+#endif // CONFIG_EKF2_OPTICAL_FLOW
+
+#if defined(CONFIG_EKF2_RANGE_FINDER)
+
+		// RNG advertise
+		if (_param_ekf2_rng_ctrl.get()) {
+			_estimator_aid_src_rng_hgt_pub.advertise();
+		}
+
+#endif // CONFIG_EKF2_RANGE_FINDER
+
+#if defined(CONFIG_EKF2_SIDESLIP)
+
+		if (_param_ekf2_fuse_beta.get()) {
+			_estimator_aid_src_sideslip_pub.advertise();
+		}
+
+#endif // CONFIG_EKF2_SIDESLIP
+
+	} // end verbose logging
+}
+
+#if defined(CONFIG_EKF2_MULTI_INSTANCE)
+bool EKF2::multi_init(int imu, int mag)
+{
 	bool changed_instance = _vehicle_imu_sub.ChangeInstance(imu);

 #if defined(CONFIG_EKF2_MAGNETOMETER)
@@ -417,6 +448,9 @@ void EKF2::Run()

 		VerifyParams();

+		// force advertise topics immediately for logging (EKF2_LOG_VERBOSE, per aid source control)
+		AdvertiseTopics();
+
 #if defined(CONFIG_EKF2_GNSS)
 		_ekf.set_min_required_gps_health_time(_param_ekf2_req_gps_h.get() * 1_s);
 #endif // CONFIG_EKF2_GNSS
@@ -500,6 +534,13 @@ void EKF2::Run()
 							accuracy, timestamp_observation);
 				}
 			}
+
+			if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_EXTERNAL_WIND_ESTIMATE) {
+				if (_ekf.control_status_flags().wind_dead_reckoning || !_ekf.control_status_flags().in_air) {
+					_ekf.resetWindToExternalObservation(vehicle_command.param1, vehicle_command.param3, vehicle_command.param2,
+									    vehicle_command.param4);
+				}
+			}
 		}
 	}

@@ -716,28 +757,31 @@ void EKF2::Run()

 			// publish status/logging messages
 			PublishEventFlags(now);
-			PublishInnovations(now);
-			PublishInnovationTestRatios(now);
-			PublishInnovationVariances(now);
-			PublishStates(now);
 			PublishStatus(now);
 			PublishStatusFlags(now);
-			PublishAidSourceStatus(now);
+
+			if (_param_ekf2_log_verbose.get()) {
+				PublishAidSourceStatus(now);
+				PublishInnovations(now);
+				PublishInnovationTestRatios(now);
+				PublishInnovationVariances(now);
+				PublishStates(now);

 #if defined(CONFIG_EKF2_BAROMETER)
-			PublishBaroBias(now);
+				PublishBaroBias(now);
 #endif // CONFIG_EKF2_BAROMETER

-#if defined(CONFIG_EKF2_RANGE_FINDER)
-			PublishRngHgtBias(now);
-#endif // CONFIG_EKF2_RANGE_FINDER
-
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
-			PublishEvPosBias(now);
+				PublishEvPosBias(now);
 #endif // CONFIG_EKF2_EXTERNAL_VISION

 #if defined(CONFIG_EKF2_GNSS)
-			PublishGnssHgtBias(now);
+				PublishGnssHgtBias(now);
+#endif // CONFIG_EKF2_GNSS
+
+			}
+
+#if defined(CONFIG_EKF2_GNSS)
 			PublishGpsStatus(now);
 			PublishYawEstimatorStatus(now);
 #endif // CONFIG_EKF2_GNSS
@@ -928,21 +972,6 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime &timestamp)
 	// optical flow
 	PublishAidSourceStatus(_ekf.aid_src_optical_flow(), _status_optical_flow_pub_last, _estimator_aid_src_optical_flow_pub);
 #endif // CONFIG_EKF2_OPTICAL_FLOW
-
-#if defined(CONFIG_EKF2_TERRAIN)
-# if defined(CONFIG_EKF2_RANGE_FINDER)
-	// range finder
-	PublishAidSourceStatus(_ekf.aid_src_terrain_range_finder(), _status_terrain_range_finder_pub_last,
-			       _estimator_aid_src_terrain_range_finder_pub);
-#endif // CONFIG_EKF2_RANGE_FINDER
-
-# if defined(CONFIG_EKF2_OPTICAL_FLOW)
-	// optical flow
-	PublishAidSourceStatus(_ekf.aid_src_terrain_optical_flow(), _status_terrain_optical_flow_pub_last,
-			       _estimator_aid_src_terrain_optical_flow_pub);
-# endif // CONFIG_EKF2_OPTICAL_FLOW
-
-#endif // CONFIG_EKF2_TERRAIN
 }

 void EKF2::PublishAttitude(const hrt_abstime &timestamp)
@@ -996,21 +1025,6 @@ void EKF2::PublishGnssHgtBias(const hrt_abstime &timestamp)
 }
 #endif // CONFIG_EKF2_GNSS

-#if defined(CONFIG_EKF2_RANGE_FINDER)
-void EKF2::PublishRngHgtBias(const hrt_abstime &timestamp)
-{
-	if (_ekf.get_rng_sample_delayed().time_us != 0) {
-		const BiasEstimator::status &status = _ekf.getRngHgtBiasEstimatorStatus();
-
-		if (fabsf(status.bias - _last_rng_hgt_bias_published) > 0.001f) {
-			_estimator_rng_hgt_bias_pub.publish(fillEstimatorBiasMsg(status, _ekf.get_rng_sample_delayed().time_us, timestamp));
-
-			_last_rng_hgt_bias_published = status.bias;
-		}
-	}
-}
-#endif // CONFIG_EKF2_RANGE_FINDER
-
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
 void EKF2::PublishEvPosBias(const hrt_abstime &timestamp)
 {
@@ -1272,10 +1286,6 @@ void EKF2::PublishInnovations(const hrt_abstime &timestamp)
 	// Optical flow
 	innovations.flow[0] = _ekf.aid_src_optical_flow().innovation[0];
 	innovations.flow[1] = _ekf.aid_src_optical_flow().innovation[1];
-# if defined(CONFIG_EKF2_TERRAIN)
-	innovations.terr_flow[0] = _ekf.aid_src_terrain_optical_flow().innovation[0];
-	innovations.terr_flow[1] = _ekf.aid_src_terrain_optical_flow().innovation[1];
-# endif // CONFIG_EKF2_TERRAIN
 #endif // CONFIG_EKF2_OPTICAL_FLOW

 	// heading
@@ -1313,7 +1323,7 @@ void EKF2::PublishInnovations(const hrt_abstime &timestamp)

 #if defined(CONFIG_EKF2_TERRAIN) && defined(CONFIG_EKF2_RANGE_FINDER)
 	// hagl
-	innovations.hagl = _ekf.aid_src_terrain_range_finder().innovation;
+	innovations.hagl = _ekf.aid_src_rng_hgt().innovation;
 #endif // CONFIG_EKF2_TERRAIN && CONFIG_EKF2_RANGE_FINDER

 #if defined(CONFIG_EKF2_RANGE_FINDER)
@@ -1339,7 +1349,7 @@ void EKF2::PublishInnovations(const hrt_abstime &timestamp)

 #if defined(CONFIG_EKF2_TERRAIN) && defined(CONFIG_EKF2_OPTICAL_FLOW)
 		// set dist bottom to scale flow innovation
-		const float dist_bottom = _ekf.getTerrainVertPos() - _ekf.getPosition()(2);
+		const float dist_bottom = _ekf.getHagl();
 		_preflt_checker.setDistBottom(dist_bottom);
 #endif // CONFIG_EKF2_TERRAIN && CONFIG_EKF2_OPTICAL_FLOW

@@ -1406,10 +1416,6 @@ void EKF2::PublishInnovationTestRatios(const hrt_abstime &timestamp)
 	// Optical flow
 	test_ratios.flow[0] = _ekf.aid_src_optical_flow().test_ratio[0];
 	test_ratios.flow[1] = _ekf.aid_src_optical_flow().test_ratio[1];
-# if defined(CONFIG_EKF2_TERRAIN)
-	test_ratios.terr_flow[0] = _ekf.aid_src_terrain_optical_flow().test_ratio[0];
-	test_ratios.terr_flow[1] = _ekf.aid_src_terrain_optical_flow().test_ratio[1];
-# endif // CONFIG_EKF2_TERRAIN
 #endif // CONFIG_EKF2_OPTICAL_FLOW

 	// heading
@@ -1447,7 +1453,7 @@ void EKF2::PublishInnovationTestRatios(const hrt_abstime &timestamp)

 #if defined(CONFIG_EKF2_TERRAIN) && defined(CONFIG_EKF2_RANGE_FINDER)
 	// hagl
-	test_ratios.hagl = _ekf.aid_src_terrain_range_finder().test_ratio;
+	test_ratios.hagl = _ekf.aid_src_rng_hgt().test_ratio;
 #endif // CONFIG_EKF2_TERRAIN && CONFIG_EKF2_RANGE_FINDER

 #if defined(CONFIG_EKF2_RANGE_FINDER)
@@ -1503,10 +1509,6 @@ void EKF2::PublishInnovationVariances(const hrt_abstime &timestamp)
 	// Optical flow
 	variances.flow[0] = _ekf.aid_src_optical_flow().innovation_variance[0];
 	variances.flow[1] = _ekf.aid_src_optical_flow().innovation_variance[1];
-# if defined(CONFIG_EKF2_TERRAIN)
-	variances.terr_flow[0] = _ekf.aid_src_terrain_optical_flow().innovation_variance[0];
-	variances.terr_flow[1] = _ekf.aid_src_terrain_optical_flow().innovation_variance[1];
-# endif // CONFIG_EKF2_TERRAIN
 #endif // CONFIG_EKF2_OPTICAL_FLOW

 	// heading
@@ -1544,7 +1546,7 @@ void EKF2::PublishInnovationVariances(const hrt_abstime &timestamp)

 #if defined(CONFIG_EKF2_TERRAIN) && defined(CONFIG_EKF2_RANGE_FINDER)
 	// hagl
-	variances.hagl = _ekf.aid_src_terrain_range_finder().innovation_variance;
+	variances.hagl = _ekf.aid_src_rng_hgt().innovation_variance;
 #endif // CONFIG_EKF2_TERRAIN && CONFIG_EKF2_RANGE_FINDER

 #if defined(CONFIG_EKF2_RANGE_FINDER)
@@ -1620,7 +1622,7 @@ void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)

 #if defined(CONFIG_EKF2_TERRAIN)
 	// Distance to bottom surface (ground) in meters, must be positive
-	lpos.dist_bottom = math::max(_ekf.getTerrainVertPos() - lpos.z, 0.f);
+	lpos.dist_bottom = math::max(_ekf.getHagl(), 0.f);
 	lpos.dist_bottom_valid = _ekf.isTerrainEstimateValid();
 	lpos.dist_bottom_sensor_bitfield = _ekf.getTerrainEstimateSensorBitfield();
 #endif // CONFIG_EKF2_TERRAIN
@@ -1802,15 +1804,13 @@ void EKF2::PublishStatus(const hrt_abstime &timestamp)
 	status.control_mode_flags = _ekf.control_status().value;
 	status.filter_fault_flags = _ekf.fault_status().value;

-	uint16_t innov_check_flags_temp = 0;
-	_ekf.get_innovation_test_status(innov_check_flags_temp, status.mag_test_ratio,
-					status.vel_test_ratio, status.pos_test_ratio,
-					status.hgt_test_ratio, status.tas_test_ratio,
-					status.hagl_test_ratio, status.beta_test_ratio);
-
-	// Bit mismatch between ecl and Firmware, combine the 2 first bits to preserve msg definition
-	// TODO: legacy use only, those flags are also in estimator_status_flags
-	status.innovation_check_flags = (innov_check_flags_temp >> 1) | (innov_check_flags_temp & 0x1);
+	status.mag_test_ratio = _ekf.getHeadingInnovationTestRatio();
+	status.vel_test_ratio = _ekf.getVelocityInnovationTestRatio();
+	status.pos_test_ratio = _ekf.getHorizontalPositionInnovationTestRatio();
+	status.hgt_test_ratio = _ekf.getVerticalPositionInnovationTestRatio();
+	status.tas_test_ratio = _ekf.getAirspeedInnovationTestRatio();
+	status.hagl_test_ratio = _ekf.getHeightAboveGroundInnovationTestRatio();
+	status.beta_test_ratio = _ekf.getSyntheticSideslipInnovationTestRatio();

 	_ekf.get_ekf_lpos_accuracy(&status.pos_horiz_accuracy, &status.pos_vert_accuracy);
 	_ekf.get_ekf_soln_status(&status.solution_status_flags);
@@ -2364,6 +2364,8 @@ bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps)
 			new_optical_flow = true;
 		}

+#if defined(CONFIG_EKF2_RANGE_FINDER)
+
 		// use optical_flow distance as range sample if distance_sensor unavailable
 		if (PX4_ISFINITE(optical_flow.distance_m) && (ekf2_timestamps.timestamp > _last_range_sensor_update + 1_s)) {

@@ -2380,6 +2382,8 @@ bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps)
 			_ekf.set_rangefinder_limits(optical_flow.min_ground_distance, optical_flow.max_ground_distance);
 		}

+#endif // CONFIG_EKF2_RANGE_FINDER
+
 		ekf2_timestamps.optical_flow_timestamp_rel = (int16_t)((int64_t)optical_flow.timestamp / 100 -
 				(int64_t)ekf2_timestamps.timestamp / 100);
 	}
@@ -162,6 +162,7 @@ private:

 	void Run() override;

+	void AdvertiseTopics();
 	void VerifyParams();

 	void PublishAidSourceStatus(const hrt_abstime &timestamp);
@@ -337,19 +338,6 @@ private:
 	hrt_abstime _status_aux_vel_pub_last{0};
 #endif // CONFIG_EKF2_AUXVEL

-#if defined(CONFIG_EKF2_TERRAIN)
-
-# if defined(CONFIG_EKF2_RANGE_FINDER)
-	uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_terrain_range_finder_pub {ORB_ID(estimator_aid_src_terrain_range_finder)};
-	hrt_abstime _status_terrain_range_finder_pub_last{0};
-# endif // CONFIG_EKF2_RANGE_FINDER
-
-# if defined(CONFIG_EKF2_OPTICAL_FLOW)
-	uORB::PublicationMulti<estimator_aid_source2d_s> _estimator_aid_src_terrain_optical_flow_pub {ORB_ID(estimator_aid_src_terrain_optical_flow)};
-	hrt_abstime _status_terrain_optical_flow_pub_last{0};
-# endif // CONFIG_EKF2_OPTICAL_FLOW
-#endif // CONFIG_EKF2_TERRAIN
-
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
 	uORB::Subscription _vehicle_optical_flow_sub {ORB_ID(vehicle_optical_flow)};
 	uORB::PublicationMulti<vehicle_optical_flow_vel_s> _estimator_optical_flow_vel_pub{ORB_ID(estimator_optical_flow_vel)};
@@ -407,9 +395,7 @@ private:

 #if defined(CONFIG_EKF2_RANGE_FINDER)
 	hrt_abstime _status_rng_hgt_pub_last {0};
-	float _last_rng_hgt_bias_published{};

-	uORB::PublicationMulti<estimator_bias_s> _estimator_rng_hgt_bias_pub {ORB_ID(estimator_rng_hgt_bias)};
 	uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_rng_hgt_pub{ORB_ID(estimator_aid_src_rng_hgt)};

 	uORB::SubscriptionMultiArray<distance_sensor_s> _distance_sensor_subs{ORB_ID::distance_sensor};
@@ -497,6 +483,7 @@ private:
 	parameters *_params;	///< pointer to ekf parameter struct (located in _ekf class instance)

 	DEFINE_PARAMETERS(
+		(ParamBool<px4::params::EKF2_LOG_VERBOSE>) _param_ekf2_log_verbose,
 		(ParamExtInt<px4::params::EKF2_PREDICT_US>) _param_ekf2_predict_us,
 		(ParamExtFloat<px4::params::EKF2_DELAY_MAX>) _param_ekf2_delay_max,
 		(ParamExtInt<px4::params::EKF2_IMU_CTRL>) _param_ekf2_imu_ctrl,
@@ -619,7 +606,6 @@ private:
 		(ParamExtFloat<px4::params::EKF2_MIN_RNG>) _param_ekf2_min_rng,
 #endif // CONFIG_EKF2_TERRAIN || CONFIG_EKF2_OPTICAL_FLOW || CONFIG_EKF2_RANGE_FINDER
 #if defined(CONFIG_EKF2_TERRAIN)
-		(ParamExtInt<px4::params::EKF2_TERR_MASK>) _param_ekf2_terr_mask,
 		(ParamExtFloat<px4::params::EKF2_TERR_NOISE>) _param_ekf2_terr_noise,
 		(ParamExtFloat<px4::params::EKF2_TERR_GRAD>) _param_ekf2_terr_grad,
 #endif // CONFIG_EKF2_TERRAIN
@@ -110,7 +110,6 @@ depends on MODULES_EKF2
        bool "optical flow fusion support"
        default y
 	select EKF2_TERRAIN
-	depends on EKF2_RANGE_FINDER
 	---help---
 		EKF2 optical flow fusion support.

@@ -524,7 +524,7 @@ parameters:
        1: Vertical position
        2: 3D velocity
        3: Yaw
-      default: 15
+      default: 0
      min: 0
      max: 15
    EKF2_GPS_CTRL:
@@ -549,8 +549,7 @@ parameters:
          unexpected errors. For these reasons, if accurate control of height relative
          to ground is required, it is recommended to use the MPC_ALT_MODE parameter
          instead, unless baro errors are severe enough to cause problems with landing
-          and takeoff. To en-/disable range finder for terrain height estimation,
-          use EKF2_TERR_MASK instead. If this parameter is enabled then the estimator
+          and takeoff. If this parameter is enabled then the estimator
          will make use of the range finder measurements to estimate its height in
          addition to other height sources (if activated). Range sensor aiding can
          be enabled (i.e.: always use) or set in "conditional" mode. Conditional
@@ -565,19 +564,6 @@ parameters:
        1: Enabled (conditional mode)
        2: Enabled
      default: 1
-    EKF2_TERR_MASK:
-      description:
-        short: Integer bitmask controlling fusion sources of the terrain estimator
-        long: 'Set bits in the following positions to enable: 0 : Set to true to use
-          range finder data if available 1 : Set to true to use optical flow data
-          if available'
-      type: bitmask
-      bit:
-        0: use range finder
-        1: use optical flow
-      default: 3
-      min: 0
-      max: 3
    EKF2_NOAID_TOUT:
      description:
        short: Maximum inertial dead-reckoning time
@@ -1317,7 +1303,7 @@ parameters:
      bit:
        0: Horizontal position
        1: Vertical position
-      default: 1
+      default: 0
      min: 0
      max: 3
    EKF2_AGP_DELAY:
@@ -1349,3 +1335,8 @@ parameters:
      min: 1.0
      unit: SD
      decimal: 1
+    EKF2_LOG_VERBOSE:
+      description:
+        short: Verbose logging
+      type: boolean
+      default: 1
@@ -41,6 +41,7 @@ px4_add_unit_gtest(SRC test_EKF_airspeed.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
 px4_add_unit_gtest(SRC test_EKF_basics.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
 px4_add_unit_gtest(SRC test_EKF_externalVision.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper)
 px4_add_unit_gtest(SRC test_EKF_flow.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper)
+px4_add_unit_gtest(SRC test_EKF_flow_generated.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper)
 px4_add_unit_gtest(SRC test_EKF_gyroscope.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
 px4_add_unit_gtest(SRC test_EKF_fusionLogic.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper)
 px4_add_unit_gtest(SRC test_EKF_gps.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper)
@@ -53,7 +54,7 @@ px4_add_unit_gtest(SRC test_EKF_mag.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
 px4_add_unit_gtest(SRC test_EKF_mag_declination_generated.cpp LINKLIBS ecl_EKF ecl_test_helper)
 px4_add_unit_gtest(SRC test_EKF_measurementSampling.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
 px4_add_unit_gtest(SRC test_EKF_ringbuffer.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
-px4_add_unit_gtest(SRC test_EKF_terrain_estimator.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper)
+px4_add_unit_gtest(SRC test_EKF_terrain.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper)
 px4_add_unit_gtest(SRC test_EKF_utils.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
 px4_add_unit_gtest(SRC test_EKF_withReplayData.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
 px4_add_unit_gtest(SRC test_EKF_yaw_estimator.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper)
--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
Marco Hauswirth	503dc210ee	adjustments for wind fusion	2024-07-11 09:44:19 +02:00
RomanBapst	16cf77da38	only allow VEHICLE_CMD_EXTERNAL_WIND_ESTIMATE during wind dead-reckoning and increase horizontal velocity variance to allow velocity states to move towards solution that is aligned with the newly set wind Signed-off-by: RomanBapst <bapstroman@gmail.com>	2024-07-03 18:36:44 +02:00
RomanBapst	ec5f09d5cb	ekf2: implement wind reset Signed-off-by: RomanBapst <bapstroman@gmail.com>	2024-07-03 18:36:44 +02:00
RomanBapst	3b0f522951	correctly compute variances Signed-off-by: RomanBapst <bapstroman@gmail.com>	2024-07-03 18:36:44 +02:00
RomanBapst	43285b020f	added VEHICLE_CMD_EXTERNAL_WIND_ESTIMATE Signed-off-by: RomanBapst <bapstroman@gmail.com>	2024-07-03 18:36:44 +02:00
RomanBapst	fa64b9887f	updated mavlink	2024-07-03 18:36:44 +02:00
RomanBapst	9877aed1ef	moved wind related functions into separate file Signed-off-by: RomanBapst <bapstroman@gmail.com>	2024-07-03 18:36:44 +02:00
RomanBapst	b6c86d841a	added support for resetting wind states to external observation Signed-off-by: RomanBapst <bapstroman@gmail.com>	2024-07-03 18:36:44 +02:00
Matthias Grob	c8c46788ed	Autostart: load airframes with priority ROMFS -> SD card	2024-07-03 18:32:16 +02:00
Thomas Frans	c0663ee85c	gnss(septentrio): fix line lenghth of module documentation	2024-07-03 11:21:34 -04:00
Thomas Frans	e27b252433	gnss(septentrio): fix incorrect heading offset configuration Heading offset was configured as radians but should be configured as degrees on Septentrio receivers. The parameter was already in degrees but the configuration logic was changing it into radians. Also allow the entire allowed range of heading offset values for Septentrio receivers.	2024-07-03 11:21:34 -04:00
Thomas Frans	49dc896d20	gnss(septentrio): fix broken heading Heading wasn't working because of an incorrect check during parsing.	2024-07-03 11:21:34 -04:00
Thomas Frans	bfbbf2ff6f	gnss(septentrio): improve `SEP_DUMP_COMM` parameter documentation The documentation for `SEP_DUMP_COMM` was quite unclear and users had to use the user guide to find out what exactly it did. The new documentation tries to make the purpose clearer.	2024-07-03 11:21:34 -04:00
Thomas Frans	7bb239637e	gnss(septentrio): fix error on driver start with same device paths This fixes an incorrect check of the device paths during instantiation of the Septentrio driver that caused the driver to start and not print an error message.	2024-07-03 11:21:34 -04:00
Thomas Frans	522a25a410	gnss(septentrio): first batch of bugfixes after internal testing Internal testing revealed usability issues. Those and some other problems are fixed.	2024-07-03 11:21:34 -04:00
Silvan Fuhrer	33701aa3d5	BatteryStatus: remove voltage_filtered_a Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2024-07-03 16:41:49 +02:00
Silvan Fuhrer	c2ae6a7e24	BatteryStatus: remove current_filtered_a Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2024-07-03 16:41:49 +02:00
zhangteng0526	e03e0261a1	Fix buffer overflow in mavlink_receive.cpp	2024-07-03 08:11:32 +02:00
chfriedrich98	f65653a391	battery: add internal resistance estimation	2024-07-02 19:05:13 +02:00
chfriedrich98	71029689e7	battery: add replay file for internal resistance estimation	2024-07-02 19:05:13 +02:00
Silvan Fuhrer	6d549811bc	fmu v3: disable GYRO_FFT to save flash Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2024-07-02 11:44:54 -04:00
Marco Hauswirth	3880073716	ekf2: fix timeout after gps failure (#23346 )	2024-07-02 10:38:49 -04:00
Daniel Agar	0742d356f5	ekf2: more conservative clipping checks for bad_acc_clipping fault status (#23337 ) - track accel clipping count per axis - only set bad_acc_clipping fault_status if at least one axis is clipping continuously or if all have been clipping at warning level - Note: this doesn't impact the clipping projections that boost the accel process noise, pause bias estimation, and skip gravity fusion on a per sample basis	2024-06-28 16:45:08 -04:00
bluedisk	408d8abe95	Tools/setup: fix the wrong - deprecated - expression in the requirements.txt - Fixes matplotlib version expression from ">=3.0.*" ro ">=3.0" which is the right syntax Fixes issue #23329 Co-authored-by: lee wonwoo <leewonwoo@leeui-MacBookPro.local>	2024-06-28 10:20:26 -04:00
Alex Klimaj	053b4a4423	drivers/uavcan: GNSS set system time based on fix_type instead of valid_pos_cov	2024-06-27 21:35:45 -04:00
Peter van der Perk	58f7c3e9c9	NuttX with imxrt1170 soc vdd backport (#23333 )	2024-06-27 16:21:45 -04:00
PX4 BuildBot	8b26e5e252	Update submodule libevents to latest Thu Jun 27 12:39:19 UTC 2024 - libevents in PX4/Firmware (4e3561cad8d24fefe66d266e969652d7ab20162b): https://github.com/mavlink/libevents/commit/8d5c44661bf79106361eb0b5170025b86e85a525 - libevents current upstream: https://github.com/mavlink/libevents/commit/9474657606d13301d426e044450c4f84de2221be - Changes: https://github.com/mavlink/libevents/compare/8d5c44661bf79106361eb0b5170025b86e85a525...9474657606d13301d426e044450c4f84de2221be 9474657 2024-06-13 Beat Küng - cmake: add namespaced target & installation include dir 9f2e68d 2024-06-12 Beat Küng - CMakeLists: set CMAKE_CXX_STANDARD if not set 3204e8f 2024-06-12 Beat Küng - parser.h: use std::vector<EventArgumentDefinition>::size_type eab8144 2024-04-29 Beat Küng - fix parser: avoid signed to unsigned conversion 159f83e 2024-04-29 Beat Küng - cpp: only enable Wall and others for GCC	2024-06-27 16:21:20 -04:00
Matthias Grob	e4446adba1	Add check for high RAM usage We had a case where someone took off with an experimental system with 100% RAM usage on the embedded system without noticing. This lead to problems during flight. Since we already have a CPU load check it seems natural to also check the reported RAM usage.	2024-06-27 11:20:22 +02:00
Daniel Agar	30b854da35	ekf2: verbose logging control (new EKF2_LOG_VERBOSE) - new parameter EKF2_LOG_VERBOSE to enable (currently enabled by default) - force advertise topics immediately (based on EKF2_LOG_VERBOSE and per aid source configuration) - logger optionally log all estimator topics at minimal rate	2024-06-27 01:10:57 -04:00
Patrik Dominik Pordi	8070c70f2c	uxrce_dds_client: dds_topics.yaml add vehicle_land_detected - px4_msgs::msg::VehicleLandDetected has been added to dds_topics.yaml	2024-06-27 01:10:04 -04:00
Daniel Agar	78fd9a15f8	flight_mode_manager: delete unused avoidance waypoint	2024-06-27 01:08:16 -04:00
Daniel Agar	338bcc6ca3	ekf2: disable EKF2_EV_CTRL and EKF2_AGP_CTRL by default	2024-06-26 17:10:28 -04:00
alexklimaj	9cc4e2ac01	boards ark pi6x add vl53l0x	2024-06-26 17:09:37 -04:00
Silvan Fuhrer	1ae96d6509	EKF2: fix builds without CONFIG_EKF2_RANGE_FINDER Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2024-06-26 11:05:38 +02:00
bresch	a50ef2eb5e	ekf2-terrain: make terrain validity based on uncertainty When using optical flow for terrain aiding, we cannot assume that the terrain estimate is valid if flow is fused as is can only be observed during motion. When no direct observation is available, the terrain is assumed to be valid if its 1sigma uncertainty is < 10% of the hagl.	2024-06-26 11:05:38 +02:00
bresch	a665764b0e	ekf2: remove unused EKF2_TERR_MASK	2024-06-26 11:05:38 +02:00
bresch	7903ddf5df	ekf2-terrain: terrain is not a separate estimator	2024-06-26 11:05:38 +02:00
bresch	9001c23926	ekf2: clean up hagl vs terrain naming Terrain is the state: terrain vertical position Hagl (height above ground level) is the vertical distance between the vertical position and the terrain vertical position	2024-06-26 11:05:38 +02:00
bresch	68980b59e2	ekf2: add terrain state	2024-06-26 11:05:38 +02:00
KonradRudin	09f066a73a	mission: skip a vtol takoff mission item if already in air (#23319 ) * mission: skip a vtol takoff mission item if already in air and a fixed wing * MissionBase: also skip FW takeoff when already in-air Signed-off-by: Silvan Fuhrer <silvan@auterion.com> * mission: use setNextMissionItem to skip vtol takeoff when already in air * mission: Only skip the VTOL takeoff in air for mission and rtl mission If flying RTL mission reverse it must still include the takeoff point. --------- Signed-off-by: Silvan Fuhrer <silvan@auterion.com> Co-authored-by: Silvan Fuhrer <silvan@auterion.com>	2024-06-25 16:33:45 +02:00
Nate	6fd0e98a69	Correct units of CRSF GPS altitude Bug fix to correct returning mm units of altitude to m.	2024-06-24 12:27:21 +02:00
David Sidrane	e8e8a60ca8	NuttX with backport of stm32h7 No phy	2024-06-24 06:12:12 -04:00
Matthias Grob	8cc7c99b59	mavlink: report generator error (#23313 ) Without this flag the command silently succeeds even though the logs contains an error. It's much more developer friendly to fail early in case of an error. The log path is then also shown in the console output.	2024-06-24 10:00:03 +02:00
Daniel Agar	30ce560e3a	ekf2: mag control reset filtered test ratio on start (if aligning yaw)	2024-06-20 13:41:54 -04:00
Daniel Agar	dcb1103299	ekf2: move estimator_status test ratios to filtered values	2024-06-20 13:41:54 -04:00