module.h fix trivial whitespace astyle failure

system: never call task_delete on nuttx, never force stop tasks
2026-05-27 23:30:06 +08:00 · 2024-06-20 15:50:25 -04:00 · 2024-06-20 13:31:51 +02:00
138 changed files with 3243 additions and 4248 deletions
@@ -4,7 +4,6 @@ on:
  push:
    branches:
    - 'main'
-    - '*'
  pull_request:
    branches:
    - '*'
@@ -26,7 +25,7 @@ jobs:
          #- {vehicle: "tiltrotor",     mission: "VTOL_mission_1",  build_type: "RelWithDebInfo"}

    container:
-      image: px4io/px4-dev-ros-melodic:2024-05-18
+      image: px4io/px4-dev-ros-melodic:2021-09-08
      options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined
    steps:
    - uses: actions/checkout@v1
@@ -40,7 +39,7 @@ jobs:
        string(TIMESTAMP current_date "%Y-%m-%d-%H;%M;%S" UTC)
        message("::set-output name=timestamp::${current_date}")
    - name: ccache cache files
-      uses: actions/cache@v4
+      uses: actions/cache@v2
      with:
        path: ~/.ccache
        key: sitl_tests-${{matrix.config.build_type}}-ccache-${{steps.ccache_cache_timestamp.outputs.timestamp}}
@@ -157,7 +157,6 @@ 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
@@ -7,8 +7,6 @@
 #
 # @maintainer
 # @board px4_fmu-v2 exclude
-# @board px4_fmu-v5x exclude
-# @board px4_fmu-v6x exclude
 #

 . ${R}etc/init.d/rc.mc_defaults
@@ -38,6 +36,7 @@ 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,6 +25,7 @@
 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
@@ -13,7 +13,6 @@
 # @board px4_fmu-v4pro exclude
 # @board px4_fmu-v5 exclude
 # @board px4_fmu-v5x exclude
-# @board px4_fmu-v6x exclude
 # @board diatone_mamba-f405-mk2 exclude
 #
 . ${R}etc/init.d/rc.mc_defaults
@@ -217,31 +217,25 @@ else
 	fi
 	unset BOARD_RC_DEFAULTS

-	# Load airframe configuration based on SYS_AUTOSTART parameter
+	#
+	# Set parameters and env variables for selected SYS_AUTOSTART.
+	#
+	set AUTOSTART_PATH etc/init.d/rc.autostart
 	if ! param compare SYS_AUTOSTART 0
 	then
-		# 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 ]
+		if param greater SYS_AUTOSTART 1000000
 		then
-			# Look for airframe on SD card
+			# Use external startup file
 			if [ $SDCARD_AVAILABLE = yes ]
 			then
-				. ${R}etc/init.d/rc.autostart_ext
+				set AUTOSTART_PATH etc/init.d/rc.autostart_ext
 			else
-				echo "ERROR [init] SD card not mounted - can't load external airframe"
+				echo "ERROR [init] SD card not mounted - trying to load airframe from ROMFS"
 			fi
 		fi
-
-		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
+		. ${R}$AUTOSTART_PATH
 	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 flags summary
+        # plot innovation_check_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,6 +79,12 @@ 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,7 +10,6 @@ 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,6 +1148,7 @@ 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=n
+CONFIG_MODULES_GYRO_FFT=y
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
@@ -154,6 +154,7 @@
 *(.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)
@@ -53,12 +53,18 @@ static const px4_mft_device_t i2c6 = {             // 24LC64T on BASE  8K 32 X 2

 static const px4_mtd_entry_t fmum_fram = {
 	.device = &qspi_flash,
-	.npart = 1,
+	.npart = 2,
 	.partd = {
 		{
 			.type = MTD_PARAMETERS,
 			.path = "/fs/mtd_params",
-			.nblocks = 256
+			.nblocks = 32
+		},
+		{
+			.type = MTD_WAYPOINTS,
+			.path = "/fs/mtd_waypoints",
+			.nblocks = 32
+
 		}
 	},
 };
@@ -1,7 +1,9 @@
 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
@@ -66,15 +68,11 @@ uint8 BATTERY_MODE_COUNT = 3 # Counter - keep it as last element (once we're ful

 uint8 MAX_INSTANCES = 4

+float32 average_power               # The average power of the current discharge
+float32 available_energy            # The predicted charge or energy remaining in the battery
 float32 full_charge_capacity_wh     # The compensated battery capacity
 float32 remaining_capacity_wh       # The compensated battery capacity remaining
+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,3 +25,4 @@ 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
+# TOPICS estimator_aid_src_aux_vel estimator_aid_src_optical_flow estimator_aid_src_terrain_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
+# TOPICS estimator_baro_bias estimator_gnss_hgt_bias estimator_rng_hgt_bias
@@ -22,6 +22,7 @@ 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[25] states		# Internal filter states
+float32[24] states		# Internal filter states
 uint8 n_states		# Number of states effectively used

-float32[24] covariances	# Diagonal Elements of Covariance Matrix
+float32[23] covariances	# Diagonal Elements of Covariance Matrix
@@ -69,14 +69,27 @@ 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 # 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
+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

 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
@@ -239,21 +239,10 @@ public:
 					unlock_module();
 					px4_usleep(10000); // 10 ms
 					lock_module();
+					i++;

-					if (++i > 500 && _task_id != -1) { // wait at most 5 sec
-						PX4_ERR("timeout, forcing stop");
-
-						if (_task_id != task_id_is_work_queue) {
-							px4_task_delete(_task_id);
-						}
-
-						_task_id = -1;
-
-						delete _object.load();
-						_object.store(nullptr);
-
-						ret = -1;
-						break;
+					if (i % 500 == 0) {
+						PX4_INFO("Waiting for task to stop...");
 					}
 				} while (_task_id != -1);

@@ -45,6 +45,7 @@
 #include <nuttx/kthread.h>

 #include <sys/wait.h>
+#include <syslog.h>
 #include <stdbool.h>
 #include <stdio.h>
 #include <stdlib.h>
@@ -88,7 +89,8 @@ int px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_

 int px4_task_delete(int pid)
 {
-	return task_delete(pid);
+	syslog(LOG_ERR, "Ignoring force task delete on NuttX\n");
+	return ERROR;
 }

 const char *px4_get_taskname(void)
@@ -116,11 +116,13 @@ 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);
@@ -37,11 +37,12 @@
 * Client-side implementation of UDRAL specification ESC service
 *
 * Publishes the following Cyphal messages:
- *   reg.udral.service.actuator.common.sp.Value31.0.1
- *   reg.udral.service.common.Readiness.0.1
+ *   reg.drone.service.actuator.common.sp.Value8.0.1
+ *   reg.drone.service.common.Readiness.0.1
 *
 * Subscribes to the following Cyphal messages:
- *   zubax.telega.CompactFeedback.0.1
+ *   reg.drone.service.actuator.common.Feedback.0.1
+ *   reg.drone.service.actuator.common.Status.0.1
 *
 * @author Pavel Kirienko <pavel.kirienko@gmail.com>
 * @author Jacob Crabill <jacob@flyvoly.com>
@@ -50,13 +51,11 @@
 #pragma once

 #include <lib/perf/perf_counter.h>
-#include <uORB/Publication.hpp>
+#include <uORB/PublicationMulti.hpp>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/actuator_test.h>
 #include <uORB/topics/esc_status.h>
 #include <drivers/drv_hrt.h>
-#include "../Subscribers/DynamicPortSubscriber.hpp"
-#include "../Publishers/Publisher.hpp"
 #include <lib/mixer_module/mixer_module.hpp>

 // UDRAL Specification Messages
@@ -64,15 +63,16 @@ using std::isfinite;
 #include <reg/udral/service/actuator/common/sp/Vector31_0_1.h>
 #include <reg/udral/service/common/Readiness_0_1.h>

-
-class ReadinessPublisher : public UavcanPublisher
+/// TODO: Allow derived class of Subscription at same time, to handle ESC Feedback/Status
+class UavcanEscController : public UavcanPublisher
 {
 public:
+	static constexpr int MAX_ACTUATORS = MixingOutput::MAX_ACTUATORS;

-	ReadinessPublisher(CanardHandle &handle, UavcanParamManager &pmgr) :
-		UavcanPublisher(handle, pmgr, "udral.", "readiness") { };
+	UavcanEscController(CanardHandle &handle, UavcanParamManager &pmgr) :
+		UavcanPublisher(handle, pmgr, "udral.", "esc") { };

-	~ReadinessPublisher() {};
+	~UavcanEscController() {};

 	void update() override
 	{
@@ -95,18 +95,58 @@ public:
 		if (hrt_absolute_time() > _previous_pub_time + READINESS_PUBLISH_PERIOD) {
 			publish_readiness();
 		}
-	}
+	};

-	static constexpr hrt_abstime READINESS_PUBLISH_PERIOD = 100000;
-	hrt_abstime _previous_pub_time = 0;
+	void update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs)
+	{
+		if (_port_id > 0) {
+			reg_udral_service_actuator_common_sp_Vector31_0_1 msg_sp {0};
+			size_t payload_size = reg_udral_service_actuator_common_sp_Vector31_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_;

-	uORB::Subscription _armed_sub{ORB_ID(actuator_armed)};
-	actuator_armed_s _armed {};
+			for (uint8_t i = 0; i < MAX_ACTUATORS; i++) {
+				if (i < num_outputs) {
+					msg_sp.value[i] = static_cast<float>(outputs[i]);

-	uORB::Subscription _actuator_test_sub{ORB_ID(actuator_test)};
-	uint64_t _actuator_test_timestamp{0};
+				} else {
+					// "unset" values published as NaN
+					msg_sp.value[i] = NAN;
+				}
+			}

-	CanardTransferID _arming_transfer_id;
+			uint8_t esc_sp_payload_buffer[reg_udral_service_actuator_common_sp_Vector31_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_];
+
+			const CanardTransferMetadata transfer_metadata = {
+				.priority       = CanardPriorityNominal,
+				.transfer_kind  = CanardTransferKindMessage,
+				.port_id        = _port_id,                // This is the subject-ID.
+				.remote_node_id = CANARD_NODE_ID_UNSET,       // Messages cannot be unicast, so use UNSET.
+				.transfer_id    = _transfer_id,
+			};
+
+			int result = reg_udral_service_actuator_common_sp_Vector31_0_1_serialize_(&msg_sp, esc_sp_payload_buffer,
+					&payload_size);
+
+			if (result == 0) {
+				// set the data ready in the buffer and chop if needed
+				++_transfer_id;  // The transfer-ID shall be incremented after every transmission on this subject.
+				result = _canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
+							       &transfer_metadata,
+							       payload_size,
+							       &esc_sp_payload_buffer);
+			}
+		}
+	};
+
+	/**
+	 * Sets the number of rotors
+	 */
+	void set_rotor_count(uint8_t count) { _rotor_count = count; }
+
+private:
+	/**
+	 * ESC status message reception will be reported via this callback.
+	 */
+	void esc_status_sub_cb(const CanardRxTransfer &msg);

 	void publish_readiness()
 	{
@@ -115,7 +155,8 @@ public:

 		size_t payload_size = reg_udral_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_;

-		if (_port_id == 0 || _port_id == CANARD_PORT_ID_UNSET) {
+		// Only publish if we have a valid publication ID set
+		if (_port_id == 0) {
 			return;
 		}

@@ -133,12 +174,12 @@ public:

 		uint8_t arming_payload_buffer[reg_udral_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_];

-		CanardPortID arming_pid = static_cast<CanardPortID>(static_cast<uint32_t>(_port_id));
+		CanardPortID arming_pid = static_cast<CanardPortID>(static_cast<uint32_t>(_port_id) + 1);
 		const CanardTransferMetadata transfer_metadata = {
 			.priority       = CanardPriorityNominal,
 			.transfer_kind  = CanardTransferKindMessage,
-			.port_id        = arming_pid,
-			.remote_node_id = CANARD_NODE_ID_UNSET,
+			.port_id        = arming_pid,                // This is the subject-ID.
+			.remote_node_id = CANARD_NODE_ID_UNSET,      // Messages cannot be unicast, so use UNSET.
 			.transfer_id    = _arming_transfer_id,
 		};

@@ -146,143 +187,25 @@ public:
 				&payload_size);

 		if (result == 0) {
-			++_arming_transfer_id;
+			// set the data ready in the buffer and chop if needed
+			++_arming_transfer_id;  // The transfer-ID shall be incremented after every transmission on this subject.
 			result = _canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
 						       &transfer_metadata,
 						       payload_size,
 						       &arming_payload_buffer);
 		}
 	};
-};
-
-class UavcanEscController : public UavcanPublisher
-{
-public:
-	static constexpr int MAX_ACTUATORS = MixingOutput::MAX_ACTUATORS;
-
-	UavcanEscController(CanardHandle &handle, UavcanParamManager &pmgr) :
-		UavcanPublisher(handle, pmgr, "udral.", "esc") { }
-
-	~UavcanEscController() {}
-
-	void update() override
-	{
-	}
-
-	void update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs)
-	{
-		if (_port_id == 0 || _port_id == CANARD_PORT_ID_UNSET) {
-			return;
-		}
-
-		uint8_t max_num_outputs = MAX_ACTUATORS > num_outputs ? num_outputs : MAX_ACTUATORS;
-
-		for (int8_t i = max_num_outputs - 1; i >= _max_number_of_nonzero_outputs; i--) {
-			if (outputs[i] != 0) {
-				_max_number_of_nonzero_outputs = i + 1;
-				break;
-			}
-		}
-
-		uint16_t payload_buffer[reg_udral_service_actuator_common_sp_Vector31_0_1_value_ARRAY_CAPACITY_];
-
-		for (uint8_t i = 0; i < _max_number_of_nonzero_outputs; i++) {
-			payload_buffer[i] = nunavutFloat16Pack(outputs[i] / 8192.0);
-		}
-
-		const CanardTransferMetadata transfer_metadata = {
-			.priority       = CanardPriorityNominal,
-			.transfer_kind  = CanardTransferKindMessage,
-			.port_id        = _port_id,
-			.remote_node_id = CANARD_NODE_ID_UNSET,
-			.transfer_id    = _transfer_id,
-		};
-
-		++_transfer_id;
-		_canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
-				      &transfer_metadata,
-				      _max_number_of_nonzero_outputs * 2,
-				      &payload_buffer);
-	}
-
-private:
-	uint8_t _max_number_of_nonzero_outputs{1};
-};
-
-class UavcanEscFeedbackSubscriber : public UavcanDynamicPortSubscriber
-{
-public:
-	UavcanEscFeedbackSubscriber(CanardHandle &handle, UavcanParamManager &pmgr, uint8_t instance = 0) :
-		UavcanDynamicPortSubscriber(handle, pmgr, "zubax.", "feedback", instance) {}
-
-	void subscribe() override
-	{
-		_canard_handle.RxSubscribe(CanardTransferKindMessage,
-					   _subj_sub._canard_sub.port_id,
-					   zubax_telega_CompactFeedback_0_1_SERIALIZATION_BUFFER_SIZE_BYTES,
-					   CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
-					   &_subj_sub._canard_sub);
-		_esc_status.esc_armed_flags |= 1 << _instance;
-		_esc_status.esc_count++;
-	};
-
-	void unsubscribe() override
-	{
-		_canard_handle.RxUnsubscribe(CanardTransferKindMessage, _subj_sub._canard_sub.port_id);
-		_esc_status.esc_armed_flags &= ~(1 << _instance);
-		_esc_status.esc_count--;
-	};
-
-	void callback(const CanardRxTransfer &receive) override
-	{
-		if (_instance >= esc_status_s::CONNECTED_ESC_MAX) {
-			return;
-		}
-
-		auto &ref = _esc_status.esc[_instance];
-		const ZubaxCompactFeedback *feedback = ((const ZubaxCompactFeedback *)(receive.payload));
-
-		ref.timestamp       = hrt_absolute_time();
-		ref.esc_address     = receive.metadata.remote_node_id;
-		ref.esc_voltage     = 0.2 * feedback->dc_voltage;
-		ref.esc_current     = 0.2 * feedback->dc_current;
-		ref.esc_temperature = NAN;
-		ref.esc_rpm         = feedback->velocity * RAD_PER_SEC_TO_RPM;
-		ref.esc_errorcount  = 0;
-
-		_esc_status.counter++;
-		_esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_CAN;
-		_esc_status.esc_armed_flags = (1 << _esc_status.esc_count) - 1;
-		_esc_status.timestamp = hrt_absolute_time();
-		_esc_status_pub.publish(_esc_status);
-
-		_esc_status.esc_online_flags = 0;
-		const hrt_abstime now = hrt_absolute_time();
-
-		for (int index = 0; index < esc_status_s::CONNECTED_ESC_MAX; index++) {
-			if (_esc_status.esc[index].timestamp > 0 && now - _esc_status.esc[index].timestamp < 1200_ms) {
-				_esc_status.esc_online_flags |= (1 << index);
-			}
-		}
-	};
-
-private:
-	static constexpr float RAD_PER_SEC_TO_RPM = 9.5492968;
-	static constexpr size_t zubax_telega_CompactFeedback_0_1_SERIALIZATION_BUFFER_SIZE_BYTES = 7;
-
-	// https://telega.zubax.com/interfaces/cyphal.html#compact
-#pragma pack(push, 1)
-	struct ZubaxCompactFeedback {
-		uint32_t dc_voltage 		: 11;
-		int32_t dc_current 		: 12;
-		int32_t phase_current_amplitude	: 12;
-		int32_t velocity 		: 13;
-		int8_t demand_factor_pct 	: 8;
-	};
-#pragma pack(pop)
-	static_assert(sizeof(ZubaxCompactFeedback) == zubax_telega_CompactFeedback_0_1_SERIALIZATION_BUFFER_SIZE_BYTES);
-
-	static esc_status_s _esc_status;
-
-	uORB::Publication<esc_status_s> _esc_status_pub{ORB_ID(esc_status)};
+
+	uint8_t _rotor_count {0};
+
+	static constexpr hrt_abstime READINESS_PUBLISH_PERIOD = 100000;
+	hrt_abstime _previous_pub_time = 0;
+
+	uORB::Subscription _armed_sub{ORB_ID(actuator_armed)};
+	actuator_armed_s _armed {};
+
+	uORB::Subscription _actuator_test_sub{ORB_ID(actuator_test)};
+	uint64_t _actuator_test_timestamp{0};
+
+	CanardTransferID _arming_transfer_id;
 };
@@ -62,8 +62,6 @@ using namespace time_literals;

 CyphalNode *CyphalNode::_instance;

-esc_status_s UavcanEscFeedbackSubscriber::_esc_status;
-
 CyphalNode::CyphalNode(uint32_t node_id, size_t capacity, size_t mtu_bytes) :
 	ModuleParams(nullptr),
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::uavcan),
@@ -127,7 +125,7 @@ int CyphalNode::start(uint32_t node_id, uint32_t bitrate)
 		_instance = new CyphalNode(node_id, 8, CANARD_MTU_CAN_FD);

 	} else {
-		_instance = new CyphalNode(node_id, 512, CANARD_MTU_CAN_CLASSIC);
+		_instance = new CyphalNode(node_id, 64, CANARD_MTU_CAN_CLASSIC);
 	}

 	if (_instance == nullptr) {
@@ -190,8 +188,6 @@ void CyphalNode::Run()
 		// send uavcan::node::Heartbeat_1_0 @ 1 Hz
 		sendHeartbeat();

-		sendPortList();
-
 		// Check all publishers
 		_pub_manager.update();

@@ -363,10 +359,10 @@ void CyphalNode::sendHeartbeat()
 	if (hrt_elapsed_time(&_uavcan_node_heartbeat_last) >= 1_s) {

 		uavcan_node_Heartbeat_1_0 heartbeat{};
-		const hrt_abstime now = hrt_absolute_time();
-		heartbeat.uptime = now / 1000000;
+		heartbeat.uptime = _uavcan_node_heartbeat_transfer_id; // TODO: use real uptime
 		heartbeat.health.value = uavcan_node_Health_1_0_NOMINAL;
 		heartbeat.mode.value = uavcan_node_Mode_1_0_OPERATIONAL;
+		const hrt_abstime now = hrt_absolute_time();
 		size_t payload_size = uavcan_node_Heartbeat_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_;

 		const CanardTransferMetadata transfer_metadata = {
@@ -396,45 +392,6 @@ void CyphalNode::sendHeartbeat()
 	}
 }

-void CyphalNode::sendPortList()
-{
-	static hrt_abstime _uavcan_node_port_List_last{0};
-
-	if (hrt_elapsed_time(&_uavcan_node_port_List_last) < 3_s) {
-		return;
-	}
-
-	static uavcan_node_port_List_0_1 msg{};
-	static uint8_t uavcan_node_port_List_0_1_buffer[uavcan_node_port_List_0_1_EXTENT_BYTES_];
-	static CanardTransferID _uavcan_node_port_List_transfer_id{0};
-	size_t payload_size = uavcan_node_port_List_0_1_EXTENT_BYTES_;
-	const hrt_abstime now = hrt_absolute_time();
-
-	const CanardTransferMetadata transfer_metadata = {
-		.priority       = CanardPriorityNominal,
-		.transfer_kind  = CanardTransferKindMessage,
-		.port_id        = uavcan_node_port_List_0_1_FIXED_PORT_ID_,
-		.remote_node_id = CANARD_NODE_ID_UNSET,
-		.transfer_id    = _uavcan_node_port_List_transfer_id++
-	};
-
-	// memset(uavcan_node_port_List_0_1_buffer, 0x00, uavcan_node_port_List_0_1_EXTENT_BYTES_);
-	uavcan_node_port_List_0_1_initialize_(&msg);
-
-	_pub_manager.fillSubjectIdList(msg.publishers);
-	_sub_manager.fillSubjectIdList(msg.subscribers);
-
-	uavcan_node_port_List_0_1_serialize_(&msg, uavcan_node_port_List_0_1_buffer, &payload_size);
-
-	_canard_handle.TxPush(now + PUBLISHER_DEFAULT_TIMEOUT_USEC,
-			      &transfer_metadata,
-			      payload_size,
-			      &uavcan_node_port_List_0_1_buffer
-			     );
-
-	_uavcan_node_port_List_last = now;
-}
-
 bool UavcanMixingInterface::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
 		unsigned num_control_groups_updated)
 {
@@ -137,9 +137,6 @@ private:
 	// Sends a heartbeat at 1s intervals
 	void sendHeartbeat();

-	// Sends a port.List at 3s intervals
-	void sendPortList();
-
 	px4::atomic_bool _task_should_exit{false};	///< flag to indicate to tear down the CAN driver

 	bool _initialized{false};		///< number of actuators currently available
@@ -56,15 +56,6 @@ bool UavcanParamManager::GetParamByName(const char *param_name, uavcan_register_
 		}
 	}

-	for (auto &param : _type_registers) {
-		if (strcmp(param_name, param.name) == 0) {
-			uavcan_register_Value_1_0_select_string_(&value);
-			value._string.value.count = strlen(param.value);
-			memcpy(&value._string, param.value, value._string.value.count);
-			return true;
-		}
-	}
-
 	return false;
 }

@@ -82,36 +73,19 @@ bool UavcanParamManager::GetParamByName(const uavcan_register_Name_1_0 &name, ua
 		}
 	}

-	for (auto &param : _type_registers) {
-		if (strncmp((char *)name.name.elements, param.name, name.name.count) == 0) {
-			uavcan_register_Value_1_0_select_string_(&value);
-			value._string.value.count = strlen(param.value);
-			memcpy(&value._string, param.value, value._string.value.count);
-			return true;
-		}
-	}
-
 	return false;
 }

 bool UavcanParamManager::GetParamName(uint32_t id, uavcan_register_Name_1_0 &name)
 {
-	size_t number_of_integer_registers = sizeof(_uavcan_params) / sizeof(UavcanParamBinder);
-	size_t number_of_type_registers = sizeof(_type_registers) / sizeof(CyphalTypeRegister);
-
-	if (id < sizeof(_uavcan_params) / sizeof(UavcanParamBinder)) {
-		strncpy((char *)name.name.elements, _uavcan_params[id].uavcan_name, uavcan_register_Name_1_0_name_ARRAY_CAPACITY_);
-		name.name.count = strlen(_uavcan_params[id].uavcan_name);
-
-	} else if (id < number_of_integer_registers + number_of_type_registers) {
-		id -= number_of_integer_registers;
-		strncpy((char *)name.name.elements, _type_registers[id].name, strlen(_type_registers[id].name));
-		name.name.count = strlen(_type_registers[id].name);
-
-	} else {
+	if (id >= sizeof(_uavcan_params) / sizeof(UavcanParamBinder)) {
 		return false;
 	}

+	strncpy((char *)name.name.elements, _uavcan_params[id].uavcan_name, uavcan_register_Name_1_0_name_ARRAY_CAPACITY_);
+
+	name.name.count = strlen(_uavcan_params[id].uavcan_name);
+
 	return true;
 }

@@ -103,10 +103,6 @@ typedef struct {
 	bool is_persistent {true};
 } UavcanParamBinder;

-typedef struct {
-	const char *name;
-	const char *value;
-} CyphalTypeRegister;

 class UavcanParamManager
 {
@@ -120,9 +116,8 @@ public:
 private:


-	const UavcanParamBinder _uavcan_params[22] {
+	const UavcanParamBinder _uavcan_params[13] {
 		{"uavcan.pub.udral.esc.0.id",                	"UCAN1_ESC_PUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
-		{"uavcan.pub.udral.readiness.0.id",            	"UCAN1_READ_PUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
 		{"uavcan.pub.udral.servo.0.id",              	"UCAN1_SERVO_PUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
 		{"uavcan.pub.udral.gps.0.id",                	"UCAN1_GPS_PUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
 		{"uavcan.pub.udral.actuator_outputs.0.id",   	"UCAN1_ACTR_PUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
@@ -135,28 +130,7 @@ private:
 		{"uavcan.sub.udral.legacy_bms.0.id",         	"UCAN1_LG_BMS_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
 		{"uavcan.sub.uorb.sensor_gps.0.id",    		"UCAN1_UORB_GPS",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
 		{"uavcan.pub.uorb.sensor_gps.0.id",    		"UCAN1_UORB_GPS_P",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
-		{"uavcan.sub.zubax.feedback.0.id",            	"UCAN1_FB0_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
-		{"uavcan.sub.zubax.feedback.1.id",            	"UCAN1_FB1_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
-		{"uavcan.sub.zubax.feedback.2.id",            	"UCAN1_FB2_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
-		{"uavcan.sub.zubax.feedback.3.id",            	"UCAN1_FB3_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
-		{"uavcan.sub.zubax.feedback.4.id",            	"UCAN1_FB4_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
-		{"uavcan.sub.zubax.feedback.5.id",            	"UCAN1_FB5_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
-		{"uavcan.sub.zubax.feedback.6.id",            	"UCAN1_FB6_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
-		{"uavcan.sub.zubax.feedback.7.id",            	"UCAN1_FB7_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
 		//{"uavcan.sub.bms.0.id",   "UCAN1_BMS0_SUB"}, //FIXME instancing
 		//{"uavcan.sub.bms.1.id",   "UCAN1_BMS1_SUB"},
 	};
-
-	CyphalTypeRegister _type_registers[10] {
-		{"uavcan.pub.udral.esc.0.type", 		"reg.udral.service.actuator.common.sp.Vector31"},
-		{"uavcan.pub.udral.readiness.0.type", 		"reg.udral.service.common.Readiness.0.1"},
-		{"uavcan.sub.zubax.feedback.0.type", 		"zubax.telega.CompactFeedback.0.1"},
-		{"uavcan.sub.zubax.feedback.1.type", 		"zubax.telega.CompactFeedback.0.1"},
-		{"uavcan.sub.zubax.feedback.2.type", 		"zubax.telega.CompactFeedback.0.1"},
-		{"uavcan.sub.zubax.feedback.3.type", 		"zubax.telega.CompactFeedback.0.1"},
-		{"uavcan.sub.zubax.feedback.4.type", 		"zubax.telega.CompactFeedback.0.1"},
-		{"uavcan.sub.zubax.feedback.5.type", 		"zubax.telega.CompactFeedback.0.1"},
-		{"uavcan.sub.zubax.feedback.6.type", 		"zubax.telega.CompactFeedback.0.1"},
-		{"uavcan.sub.zubax.feedback.7.type", 		"zubax.telega.CompactFeedback.0.1"},
-	};
 };
@@ -125,15 +125,6 @@ UavcanPublisher *PublicationManager::getPublisher(const char *subject_name)
 	return NULL;
 }

-void PublicationManager::fillSubjectIdList(uavcan_node_port_SubjectIDList_0_1 &publishers_list)
-{
-	uavcan_node_port_SubjectIDList_0_1_select_sparse_list_(&publishers_list);
-
-	for (auto &dynpub : _dynpublishers) {
-		publishers_list.sparse_list.elements[publishers_list.sparse_list.count].value = dynpub->id();
-		publishers_list.sparse_list.count++;
-	}
-}

 void PublicationManager::update()
 {
@@ -67,7 +67,7 @@
 /* Preprocessor calculation of publisher count */

 #define UAVCAN_PUB_COUNT CONFIG_CYPHAL_GNSS_PUBLISHER + \
-	2 * CONFIG_CYPHAL_ESC_CONTROLLER + \
+	CONFIG_CYPHAL_ESC_CONTROLLER + \
 	CONFIG_CYPHAL_READINESS_PUBLISHER + \
 	CONFIG_CYPHAL_UORB_ACTUATOR_OUTPUTS_PUBLISHER + \
 	CONFIG_CYPHAL_UORB_SENSOR_GPS_PUBLISHER
@@ -79,7 +79,6 @@

 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/sensor_gps.h>
-#include <uavcan/node/port/List_0_1.h>

 #include "Actuators/EscClient.hpp"
 #include "Publishers/udral/Readiness.hpp"
@@ -104,7 +103,6 @@ public:

 	UavcanPublisher *getPublisher(const char *subject_name);

-	void fillSubjectIdList(uavcan_node_port_SubjectIDList_0_1 &publishers_list);
 private:
 	void updateDynamicPublications();

@@ -133,14 +131,6 @@ private:
 			"udral.esc",
 			0
 		},
-		{
-			[](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanPublisher *
-			{
-				return new ReadinessPublisher(handle, pmgr);
-			},
-			"udral.readiness",
-			0
-		},
 #endif
 #if CONFIG_CYPHAL_READINESS_PUBLISHER
 		{
@@ -76,6 +76,8 @@ 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;
@@ -158,24 +158,3 @@ void SubscriptionManager::updateParams()
 	// Check for any newly-enabled subscriptions
 	updateDynamicSubscriptions();
 }
-
-void SubscriptionManager::fillSubjectIdList(uavcan_node_port_SubjectIDList_0_1 &subscribers_list)
-{
-	uavcan_node_port_SubjectIDList_0_1_select_sparse_list_(&subscribers_list);
-
-	UavcanDynamicPortSubscriber *dynsub = _dynsubscribers;
-
-	auto &sparse_list = subscribers_list.sparse_list;
-
-	while (dynsub != nullptr) {
-		int32_t instance_idx = 0;
-
-		while (dynsub->isValidPortId(dynsub->id(instance_idx))) {
-			sparse_list.elements[sparse_list.count].value = dynsub->id(instance_idx);
-			sparse_list.count++;
-			instance_idx++;
-		}
-
-		dynsub = dynsub->next();
-	}
-}
@@ -45,10 +45,6 @@
 #define CONFIG_CYPHAL_ESC_SUBSCRIBER 0
 #endif

-#ifndef CONFIG_CYPHAL_ESC_CONTROLLER
-#define CONFIG_CYPHAL_ESC_CONTROLLER 0
-#endif
-
 #ifndef CONFIG_CYPHAL_GNSS_SUBSCRIBER_0
 #define CONFIG_CYPHAL_GNSS_SUBSCRIBER_0 0
 #endif
@@ -69,15 +65,12 @@

 #define UAVCAN_SUB_COUNT CONFIG_CYPHAL_ESC_SUBSCRIBER + \
 	CONFIG_CYPHAL_GNSS_SUBSCRIBER_0 + \
-	8 * CONFIG_CYPHAL_ESC_CONTROLLER + \
 	CONFIG_CYPHAL_GNSS_SUBSCRIBER_1 + \
 	CONFIG_CYPHAL_BMS_SUBSCRIBER + \
 	CONFIG_CYPHAL_UORB_SENSOR_GPS_SUBSCRIBER

 #include <px4_platform_common/defines.h>
 #include <drivers/drv_hrt.h>
-#include <uavcan/node/port/List_0_1.h>
-#include "Actuators/EscClient.hpp"
 #include "Subscribers/DynamicPortSubscriber.hpp"
 #include "CanardInterface.hpp"

@@ -107,7 +100,6 @@ public:
 	void subscribe();
 	void printInfo();
 	void updateParams();
-	void fillSubjectIdList(uavcan_node_port_SubjectIDList_0_1 &subscribers_list);

 private:
 	void updateDynamicSubscriptions();
@@ -138,72 +130,6 @@ private:
 			0
 		},
 #endif
-#if CONFIG_CYPHAL_ESC_CONTROLLER
-		{
-			[](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
-			{
-				return new UavcanEscFeedbackSubscriber(handle, pmgr, 0);
-			},
-			"zubax.feedback",
-			0
-		},
-		{
-			[](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
-			{
-				return new UavcanEscFeedbackSubscriber(handle, pmgr, 1);
-			},
-			"zubax.feedback",
-			1
-		},
-		{
-			[](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
-			{
-				return new UavcanEscFeedbackSubscriber(handle, pmgr, 2);
-			},
-			"zubax.feedback",
-			2
-		},
-		{
-			[](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
-			{
-				return new UavcanEscFeedbackSubscriber(handle, pmgr, 3);
-			},
-			"zubax.feedback",
-			3
-		},
-		{
-			[](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
-			{
-				return new UavcanEscFeedbackSubscriber(handle, pmgr, 4);
-			},
-			"zubax.feedback",
-			4
-		},
-		{
-			[](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
-			{
-				return new UavcanEscFeedbackSubscriber(handle, pmgr, 5);
-			},
-			"zubax.feedback",
-			5
-		},
-		{
-			[](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
-			{
-				return new UavcanEscFeedbackSubscriber(handle, pmgr, 6);
-			},
-			"zubax.feedback",
-			6
-		},
-		{
-			[](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
-			{
-				return new UavcanEscFeedbackSubscriber(handle, pmgr, 7);
-			},
-			"zubax.feedback",
-			7
-		},
-#endif
 #if CONFIG_CYPHAL_GNSS_SUBSCRIBER_0
 		{
 			[](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
@@ -162,87 +162,6 @@ PARAM_DEFINE_INT32(UCAN1_UORB_GPS_P, -1);
 */
 PARAM_DEFINE_INT32(UCAN1_ESC_PUB, -1);

-/**
- * Cyphal ESC readiness port ID.
- *
- * @min -1
- * @max 6143
- * @group Cyphal
- */
-PARAM_DEFINE_INT32(UCAN1_READ_PUB, -1);
-
-/**
- * Cyphal ESC 0 zubax feedback port ID.
- *
- * @min -1
- * @max 6143
- * @group Cyphal
- */
-PARAM_DEFINE_INT32(UCAN1_FB0_SUB, -1);
-
-/**
- * Cyphal ESC 1 zubax feedback port ID.
- *
- * @min -1
- * @max 6143
- * @group Cyphal
- */
-PARAM_DEFINE_INT32(UCAN1_FB1_SUB, -1);
-
-/**
- * Cyphal ESC 2 zubax feedback port ID.
- *
- * @min -1
- * @max 6143
- * @group Cyphal
- */
-PARAM_DEFINE_INT32(UCAN1_FB2_SUB, -1);
-
-/**
- * Cyphal ESC 3 zubax feedback port ID.
- *
- * @min -1
- * @max 6143
- * @group Cyphal
- */
-PARAM_DEFINE_INT32(UCAN1_FB3_SUB, -1);
-
-/**
- * Cyphal ESC 4 zubax feedback port ID.
- *
- * @min -1
- * @max 6143
- * @group Cyphal
- */
-PARAM_DEFINE_INT32(UCAN1_FB4_SUB, -1);
-
-/**
- * Cyphal ESC 5 zubax feedback port ID.
- *
- * @min -1
- * @max 6143
- * @group Cyphal
- */
-PARAM_DEFINE_INT32(UCAN1_FB5_SUB, -1);
-
-/**
- * Cyphal ESC 6 zubax feedback port ID.
- *
- * @min -1
- * @max 6143
- * @group Cyphal
- */
-PARAM_DEFINE_INT32(UCAN1_FB6_SUB, -1);
-
-/**
- * Cyphal ESC 7 zubax feedback port ID.
- *
- * @min -1
- * @max 6143
- * @group Cyphal
- */
-PARAM_DEFINE_INT32(UCAN1_FB7_SUB, -1);
-
 /**
 * Cyphal GPS publication port ID.
 *
@@ -56,7 +56,7 @@
 #endif

 #ifdef SEP_LOG_ERROR
-#define SEP_ERR(...)            {PX4_ERR(__VA_ARGS__);}
+#define SEP_ERR(...)            {PX4_WARN(__VA_ARGS__);}
 #else
 #define SEP_ERR(...)            {}
 #endif
@@ -71,7 +71,7 @@ parameters:
        type: float
        decimal: 3
        default: 0
-        min: -360
+        min: 0
        max: 360
        unit: deg
        reboot_required: true
@@ -104,8 +104,7 @@ parameters:
        description:
          short: Log GPS communication data
          long: |
-            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.
+            Dump raw communication data from and to the connected receiver to the log file.
        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.0f * 10000000000.0f};
-constexpr double k_dnu_f8_value {-2.0 * 10000000000.0};
+constexpr float k_dnu_f4_value {-2 * 10000000000};
+constexpr double k_dnu_f8_value {-2 * 10000000000};

 /// 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,7 +55,6 @@
 #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>

@@ -87,11 +86,6 @@ 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;
@@ -140,19 +134,18 @@ 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),
-	_chosen_baud_rate(baud_rate)
+	_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);

@@ -178,10 +171,6 @@ 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);
@@ -209,8 +198,6 @@ SeptentrioDriver::SeptentrioDriver(const char *device_path, Instance instance, u
 	}

 	set_device_type(DRV_GPS_DEVTYPE_SBF);
-
-	reset_gps_state_message();
 }

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

-	PX4_INFO("health: %s, port: %s, baud rate: %lu", is_healthy() ? "OK" : "NOT OK", _port, _uart.getBaudrate());
+	PX4_INFO("health: %s, port: %s, baud rate: %lu", is_healthy() ? "OK" : "NOT OK", _port, _baud_rate);
 	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 (first_gps_uorb_message_created() && _state == State::ReceivingData) {
+	if (_message_gps_state.timestamp != 0) {
+
 		PX4_INFO("rate RTCM injection: %6.2f Hz", static_cast<double>(rtcm_injection_frequency()));
+
 		print_message(ORB_ID(sensor_gps), _message_gps_state);
 	}

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

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

 				} else {
 					// Failed to open port, so wait a bit before trying again.
@@ -288,42 +277,14 @@ 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: {
-				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");
+				if (configure() == PX4_OK) {
+					SEP_INFO("Automatic configuration successful");
 					_state = State::ReceivingData;

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

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

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

 				if (_message_satellite_info && (receive_result & 2)) {
@@ -424,51 +385,19 @@ 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,
-						   valid_chosen_baud_rate ? arguments.baud_rate_main : k_default_baud_rate);
+			gps = new SeptentrioDriver(arguments.device_path_main, instance, arguments.baud_rate_main);

 		} 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) {
@@ -481,8 +410,7 @@ SeptentrioDriver *SeptentrioDriver::instantiate(int argc, char *argv[], Instance

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

 		} else {
 			PX4_ERR("Invalid secondary device (-e) %s", arguments.device_path_secondary ? arguments.device_path_secondary : "");
@@ -497,7 +425,6 @@ 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()) {
@@ -521,7 +448,7 @@ int SeptentrioDriver::custom_command(int argc, char *argv[])
 				type = ReceiverResetType::Cold;

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

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

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

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

 int SeptentrioDriver::print_usage(const char *reason)
@@ -546,30 +473,25 @@ int SeptentrioDriver::print_usage(const char *reason)
 	PRINT_MODULE_DESCRIPTION(
 		R"DESCR_STR(
 ### Description
-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.
+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.

 ### Examples

-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:
+Starting 2 GPS devices (main one on /dev/ttyS3, secondary on /dev/ttyS4)
 $ septentrio start -d /dev/ttyS3 -e /dev/ttyS4

-Perform warm reset of the receivers:
+Initiate warm restart of GPS device
 $ 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 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_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_DEFAULT_COMMANDS();
 	PRINT_MODULE_USAGE_COMMAND_DESCR("reset", "Reset connected receiver");
@@ -586,15 +508,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_fast);
+		res = send_message_and_wait_for_ack(k_command_reset_hot, k_receiver_ack_timeout);
 		break;

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

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

 	default:
@@ -631,13 +553,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;
@@ -715,31 +637,42 @@ void SeptentrioDriver::schedule_reset(ReceiverResetType reset_type)
 	}
 }

-bool SeptentrioDriver::detect_receiver_baud_rate(const uint32_t &baud_rate, bool forced_input) {
-	if (set_baudrate(baud_rate) != PX4_OK) {
-		return false;
+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;
 	}

-	if (forced_input) {
-		force_input();
+	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];
+		}
 	}

-	// 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;
+	set_baudrate(original_baud_rate);
+	return 0;
 }

 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_fast;
+	hrt_abstime timeout_time = hrt_absolute_time() + 5 * 1000 * k_receiver_ack_timeout;
 	bool response_detected = false;

 	// Receiver prints prompt after a message.
@@ -749,7 +682,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_fast);
+		int read_result = read(reinterpret_cast<uint8_t *>(buf) + buffer_offset, sizeof(buf) - buffer_offset - 1, k_receiver_ack_timeout);

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

-SeptentrioDriver::ConfigureResult SeptentrioDriver::configure()
+int 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_WARN("CONFIG: failed port detection");
-		return ConfigureResult::FailedCompletely;
+		SEP_INFO("CONFIG: failed port detection");
+		return PX4_ERROR;
 	}

 	// 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 ConfigureResult::OK;
+		return PX4_OK;
 	}

 	// If user requested specific baud rate, set it now. Otherwise keep detected 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 (strstr(com_port, "COM") != nullptr && _baud_rate != 0) {
+		snprintf(msg, sizeof(msg), k_command_set_baud_rate, com_port, _baud_rate);

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

 		// 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(2000000);
+		px4_usleep(1000000);

-		if (set_baudrate(_chosen_baud_rate) != PX4_OK) {
-			SEP_WARN("CONFIG: failed local baud rate setting");
-			return ConfigureResult::FailedCompletely;
+		if (set_baudrate(_baud_rate) != PX4_OK) {
+			SEP_INFO("CONFIG: failed local 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;
+		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;
 		}
+	} 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_fast)) {
-		SEP_WARN("CONFIG: failed delete output");
-		return ConfigureResult::FailedCompletely; // connection and/or baudrate detection failed
+	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
 	}

-	// 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)) {
+		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");
+			}
 		}
-		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;
+	} 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;
 		}
 	}

@@ -935,77 +919,42 @@ SeptentrioDriver::ConfigureResult 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_fast)) {
-			result = static_cast<ConfigureResult>(static_cast<int32_t>(result) | static_cast<int32_t>(ConfigureResult::NoLogging));
+		if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout)) {
+			SEP_ERR("Failed to configure logging");
+			return PX4_ERROR;
 		}
 	} else if (_receiver_stream_log == _receiver_stream_main) {
-		result = static_cast<ConfigureResult>(static_cast<int32_t>(result) | static_cast<int32_t>(ConfigureResult::NoLogging));
+		SEP_WARN("Skipping logging setup: logging stream can't equal main stream");
 	}

-	// 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");
-			}
+	// 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_WARN("CONFIG: Failed to configure attitude source");
-			return ConfigureResult::FailedCompletely;
+		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;
 		}
 	}

-	return result;
+	return PX4_OK;
 }

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

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

-				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 {
+				// 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) {
 					_message_gps_state.fix_type = sensor_gps_s::FIX_TYPE_NONE;
 				}

@@ -1134,22 +1082,23 @@ 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);

-				if (pvt_geodetic.cog > k_dnu_f4_value) {
-					_message_gps_state.cog_rad = pvt_geodetic.cog * M_DEG_TO_RAD_F;
-				}
+				_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;
@@ -1229,8 +1178,14 @@ int SeptentrioDriver::process_message()
 			VelCovGeodetic vel_cov_geodetic;

 			if (_sbf_decoder.parse(&vel_cov_geodetic) == PX4_OK) {
-				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);
+				_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;
 				}
 			}

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

 			AttEuler att_euler;

-			if (_sbf_decoder.parse(&att_euler) == PX4_OK &&
+			if (_sbf_decoder.parse(&att_euler) &&
 			    !att_euler.error_not_requested &&
 			    att_euler.error_aux1 == Error::None &&
-			    att_euler.error_aux2 == Error::None &&
-			    att_euler.heading > k_dnu_f4_value) {
+			    att_euler.error_aux2 == Error::None) {
 				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].
@@ -1276,8 +1230,7 @@ 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.cov_headhead > k_dnu_f4_value) {
+			    att_cov_euler.error_aux2 == Error::None) {
 				_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]
 			}

@@ -1336,16 +1289,13 @@ 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());

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

@@ -1542,6 +1492,10 @@ 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) {
@@ -1569,11 +1523,6 @@ 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{};
@@ -1719,11 +1668,13 @@ void SeptentrioDriver::set_clock(timespec rtc_gps_time)

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

-	return false;
+	return true;
 }

 void SeptentrioDriver::reset_gps_state_message()
@@ -47,7 +47,6 @@
 #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>
@@ -272,20 +271,9 @@ 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,
 	};
@@ -307,24 +295,9 @@ private:
 	};

 	/**
-	 * 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.
+	 * Maximum timeout to wait for command acknowledgement by the receiver.
 	*/
-	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;
+	static constexpr uint16_t k_receiver_ack_timeout = 200;

 	/**
 	 * Duration of one update monitoring interval in us.
@@ -333,11 +306,6 @@ 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.
 	*/
@@ -379,15 +347,13 @@ private:
 	void schedule_reset(ReceiverResetType type);

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

 	/**
 	 * @brief Try to detect the serial port used on the receiver side.
@@ -403,9 +369,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 `ConfigureResult::OK` if configured, or error.
+	 * @return `PX4_OK` on success, `PX4_ERROR` otherwise.
 	 */
-	ConfigureResult configure();
+	int configure();

 	/**
 	 * @brief Parse the next byte of a received message from the receiver.
@@ -539,13 +505,6 @@ 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.
 	 *
@@ -620,9 +579,6 @@ 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;
@@ -655,9 +611,6 @@ 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;
@@ -713,7 +666,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                               _chosen_baud_rate {0};                                        ///< The baud rate requested by the user
+	uint32_t                               _baud_rate {0};
 	static px4::atomic<SeptentrioDriver *> _secondary_instance;
 	hrt_abstime                            _sleep_end {0};                                               ///< End time for sleeping
 	State                                  _resume_state {State::OpeningSerialPort};                     ///< Resume state after sleep
@@ -730,7 +683,6 @@ 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_v);
+	snprintf(buf, sizeof(buf), "%c%5.2f", OSD_SYMBOL_BATT_3, (double)_battery_voltage_filtered_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_v = battery.voltage_v;
+			_battery_voltage_filtered_v = battery.voltage_filtered_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_v{0.f};
+	float _battery_voltage_filtered_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_v * 10;
-					uint16_t current = battery_status.current_a * 10;
+					uint16_t voltage = battery_status.voltage_filtered_v * 10;
+					uint16_t current = battery_status.current_filtered_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) + 1000;
+					uint16_t altitude = static_cast<int16_t>(sensor_gps.altitude_msl_m * 1e3) + 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_v * 10;
-	uint16_t current = battery_status.current_a * 10;
+	uint16_t voltage = battery_status.voltage_filtered_v * 10;
+	uint16_t current = battery_status.current_filtered_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_v * FACTOR_VOLTS_TO_10MV;
-		current_in_10mA = battery_status.current_a * FACTOR_AMPS_TO_10MA;
+		voltage_in_10mV = battery_status.voltage_filtered_v * FACTOR_VOLTS_TO_10MV;
+		current_in_10mA = battery_status.current_filtered_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,11 +146,13 @@ 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,7 +115,9 @@ 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,7 +104,9 @@ 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 ((fix_type >= sensor_gps_s::FIX_TYPE_2D) && !_system_clock_set) {
+	if (valid_pos_cov && !_system_clock_set) {
 		timespec ts{};

 		// get the whole microseconds
@@ -46,7 +46,6 @@
 #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),
@@ -54,9 +53,10 @@ 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);
-	_ocv_filter_v.setParameters(expected_filter_dt, 1.f);
-	_cell_voltage_filter_v.setParameters(expected_filter_dt, 1.f);
+	_throttle_filter.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,6 +81,9 @@ 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);

@@ -94,36 +97,29 @@ 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) {
-		resetInternalResistanceEstimation(_voltage_v, _current_a);
+		_voltage_filter_v.reset(_voltage_v);
+		_current_filter_a.reset(_current_a);
 	}

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

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

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

 	if (!_external_state_of_charge) {
 		estimateStateOfCharge();
@@ -153,7 +149,9 @@ 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;
@@ -169,14 +167,6 @@ 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;
 }

@@ -223,69 +213,27 @@ 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 according to internal resistance and current
-	if (current_a > FLT_EPSILON) {
-		updateInternalResistanceEstimation(voltage_v, current_a);
+	// 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;

-		if (_params.r_internal >= 0.f) { // Use user specified internal resistance value
-			cell_voltage += _params.r_internal * 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();

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

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

-	_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);
-	}
+	return math::interpolate(cell_voltage, _params.v_empty, _params.v_charged, 0.f, 1.f);
 }

 void Battery::estimateStateOfCharge()
@@ -406,6 +354,7 @@ 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,6 +58,7 @@
 #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.
@@ -117,6 +118,7 @@ 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;
@@ -130,6 +132,7 @@ protected:
 		float v_charged;
 		int32_t  n_cells;
 		float capacity;
+		float v_load_drop;
 		float r_internal;
 		float low_thr;
 		float crit_thr;
@@ -152,6 +155,7 @@ 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)};
@@ -163,11 +167,12 @@ private:
 	uint8_t _priority{0};
 	bool _battery_initialized{false};
 	float _voltage_v{0.f};
-	AlphaFilter<float> _ocv_filter_v;
-	AlphaFilter<float> _cell_voltage_filter_v;
+	AlphaFilter<float> _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]
@@ -178,19 +183,4 @@ 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
 };
@@ -1,208 +0,0 @@
-# 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 getParam(log, param_name):
-    if param_name in log.initial_parameters:
-        return log.initial_parameters[param_name]
-    else:
-        print(f"Parameter {param_name} not found in log.")
-    return None
-
-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)
-
-    log_n_cells = getParam(log, 'BAT1_N_CELLS')
-    log_full_cell = getParam(log, 'BAT1_V_CHARGED')
-    log_empty_cell = getParam(log, 'BAT1_V_EMPTY')
-
-    # Debug information
-    print(f"Extracted from log - BAT1_N_CELLS: {log_n_cells}, BAT1_V_CHARGED: {log_full_cell}, BAT1_V_EMPTY: {log_empty_cell}")
-
-    # Use log parameters unless overridden
-    if n_cells is None:
-        n_cells = log_n_cells
-    else:
-        print(f"Using override for n_cells: {n_cells}")
-    if full_cell is None:
-        full_cell = log_full_cell
-    else:
-        print(f"Using override for full_cell: {full_cell}")
-    if empty_cell is None:
-        empty_cell = log_empty_cell
-    else:
-        print(f"Using override for empty_cell: {empty_cell}")
-
-    # Debug information for final parameter values
-    print(f"Using parameters - n_cells: {n_cells}, full_cell: {full_cell}, empty_cell: {empty_cell}")
-
-    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 = False, help = 'Number of cells in battery')
-    parser.add_argument('-u', type = float, required = False, default = None, help = 'Full cell voltage')
-    parser.add_argument('-e', type = float, required = False, default = None,  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,11 +39,33 @@ 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 instead of the online estimated internal resistance.
+                    If non-negative, then this will be used in place of
+                    BAT${i}_V_LOAD_DROP for all calculations.

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

        BAT${i}_N_CELLS:
            description:
@@ -261,11 +261,13 @@ 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,7 +618,6 @@ 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,10 +43,7 @@ CpuResourceChecks::CpuResourceChecks()

 void CpuResourceChecks::checkAndReport(const Context &context, Report &reporter)
 {
-	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) {
+	if (_param_com_cpu_max.get() < FLT_EPSILON) {
 		return;
 	}

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

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

 	} else {
@@ -74,7 +71,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 (cpu_load_check_enabled && _high_cpu_load_hysteresis.get_state()) {
+		if (_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
@@ -91,26 +88,5 @@ 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,7 +54,6 @@ 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_RAM_MAX>) _param_com_ram_max
+					(ParamFloat<px4::params::COM_CPU_MAX>) _param_com_cpu_max
 				       )
 };
@@ -802,21 +802,6 @@ 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,7 +203,6 @@ 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()
@@ -213,7 +212,7 @@ if(CONFIG_EKF2_SIDESLIP)
 endif()

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

 add_subdirectory(EKF)
@@ -125,7 +125,6 @@ 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()
@@ -135,7 +134,7 @@ if(CONFIG_EKF2_SIDESLIP)
 endif()

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

 include_directories(${CMAKE_CURRENT_SOURCE_DIR})
@@ -42,13 +42,13 @@ ZeroVelocityUpdate::ZeroVelocityUpdate()

 void ZeroVelocityUpdate::reset()
 {
-	_time_last_fuse = 0;
+	_time_last_zero_velocity_fuse = 0;
 }

 bool ZeroVelocityUpdate::update(Ekf &ekf, const estimator::imuSample &imu_delayed)
 {
 	// Fuse zero velocity at a limited rate (every 200 milliseconds)
-	const bool zero_velocity_update_data_ready = (_time_last_fuse + 200'000 < imu_delayed.time_us);
+	const bool zero_velocity_update_data_ready = (_time_last_zero_velocity_fuse + 200'000 < imu_delayed.time_us);

 	if (zero_velocity_update_data_ready) {
 		const bool continuing_conditions_passing = ekf.control_status_flags().vehicle_at_rest
@@ -69,7 +69,7 @@ bool ZeroVelocityUpdate::update(Ekf &ekf, const estimator::imuSample &imu_delaye
 				ekf.fuseDirectStateMeasurement(innovation, innov_var(i), obs_var, State::vel.idx + i);
 			}

-			_time_last_fuse = imu_delayed.time_us;
+			_time_last_zero_velocity_fuse = imu_delayed.time_us;

 			return true;
 		}
@@ -45,11 +45,9 @@ public:
 	void reset() override;
 	bool update(Ekf &ekf, const estimator::imuSample &imu_delayed) override;

-	const auto &time_last_fuse() const { return _time_last_fuse; }
-
 private:

-	uint64_t _time_last_fuse{0}; ///< last time of zero velocity update (uSec)
+	uint64_t _time_last_zero_velocity_fuse{0}; ///< last time of zero velocity update (uSec)

 };

@@ -151,8 +151,6 @@ 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,8 +74,6 @@ 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
 	{
@@ -108,8 +106,6 @@ private:

 	State _state{State::stopped};

-	float _test_ratio_filtered{INFINITY};
-
 #if defined(MODULE_NAME)
 	struct reset_counters_s {
 		uint8_t lat_lon{};
@@ -37,9 +37,6 @@
 */

 #include "ekf.h"
-#include <ekf_derivation/generated/compute_ev_body_vel_hx.h>
-#include <ekf_derivation/generated/compute_ev_body_vel_hy.h>
-#include <ekf_derivation/generated/compute_ev_body_vel_hz.h>

 void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing,
 			     const bool ev_reset, const bool quality_sufficient, estimator_aid_source3d_s &aid_src)
@@ -60,16 +57,14 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common
 	const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;

 	// rotate measurement into correct earth frame if required
-	Vector3f measurement{};
-	Vector3f measurement_var{};
-
-	float minimum_variance = math::max(sq(0.01f), sq(_params.ev_vel_noise));
+	Vector3f vel{NAN, NAN, NAN};
+	Matrix3f vel_cov{};

 	switch (ev_sample.vel_frame) {
 	case VelocityFrame::LOCAL_FRAME_NED:
 		if (_control_status.flags.yaw_align) {
-			measurement = ev_sample.vel - vel_offset_earth;
-			measurement_var = ev_sample.velocity_var;
+			vel = ev_sample.vel - vel_offset_earth;
+			vel_cov = matrix::diag(ev_sample.velocity_var);

 		} else {
 			continuing_conditions_passing = false;
@@ -80,28 +75,31 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common
 	case VelocityFrame::LOCAL_FRAME_FRD:
 		if (_control_status.flags.ev_yaw) {
 			// using EV frame
-			measurement = ev_sample.vel - vel_offset_earth;
-			measurement_var = ev_sample.velocity_var;
+			vel = ev_sample.vel - vel_offset_earth;
+			vel_cov = matrix::diag(ev_sample.velocity_var);

 		} else {
 			// rotate EV to the EKF reference frame
 			const Dcmf R_ev_to_ekf = Dcmf(_ev_q_error_filt.getState());

-			measurement = R_ev_to_ekf * ev_sample.vel - vel_offset_earth;
-			measurement_var = matrix::SquareMatrix3f(R_ev_to_ekf * matrix::diag(ev_sample.velocity_var) *
-					  R_ev_to_ekf.transpose()).diag();
-			minimum_variance = math::max(minimum_variance, ev_sample.orientation_var.max());
+			vel = R_ev_to_ekf * ev_sample.vel - vel_offset_earth;
+			vel_cov = R_ev_to_ekf * matrix::diag(ev_sample.velocity_var) * R_ev_to_ekf.transpose();
+
+			// increase minimum variance to include EV orientation variance
+			// TODO: do this properly
+			const float orientation_var_max = ev_sample.orientation_var.max();
+
+			for (int i = 0; i < 2; i++) {
+				vel_cov(i, i) = math::max(vel_cov(i, i), orientation_var_max);
+			}
 		}

 		break;

-	case VelocityFrame::BODY_FRAME_FRD: {
-
-			// currently it is assumed that the orientation of the EV frame and the body frame are the same
-			measurement = ev_sample.vel - vel_offset_body;
-			measurement_var = ev_sample.velocity_var;
-			break;
-		}
+	case VelocityFrame::BODY_FRAME_FRD:
+		vel = _R_to_earth * (ev_sample.vel - vel_offset_body);
+		vel_cov = _R_to_earth * matrix::diag(ev_sample.velocity_var) * _R_to_earth.transpose();
+		break;

 	default:
 		continuing_conditions_passing = false;
@@ -113,56 +111,48 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common
 	// increase minimum variance if GPS active (position reference)
 	if (_control_status.flags.gps) {
 		for (int i = 0; i < 2; i++) {
-			measurement_var(i) = math::max(measurement_var(i), sq(_params.gps_vel_noise));
+			vel_cov(i, i) = math::max(vel_cov(i, i), sq(_params.gps_vel_noise));
 		}
 	}

 #endif // CONFIG_EKF2_GNSS

-	measurement_var = Vector3f{
-		math::max(measurement_var(0), minimum_variance),
-		math::max(measurement_var(1), minimum_variance),
-		math::max(measurement_var(2), minimum_variance)
+	const Vector3f measurement{vel};
+
+	const Vector3f measurement_var{
+		math::max(vel_cov(0, 0), sq(_params.ev_vel_noise), sq(0.01f)),
+		math::max(vel_cov(1, 1), sq(_params.ev_vel_noise), sq(0.01f)),
+		math::max(vel_cov(2, 2), sq(_params.ev_vel_noise), sq(0.01f))
 	};
-	continuing_conditions_passing &= measurement.isAllFinite() && measurement_var.isAllFinite();

-	if (ev_sample.vel_frame == VelocityFrame::BODY_FRAME_FRD) {
-		const Vector3f measurement_var_ekf_frame = rotateVarianceToEkf(measurement_var);
-		const Vector3f measurement_ekf_frame = _R_to_earth * measurement;
-		const uint64_t t = aid_src.timestamp_sample;
-		updateAidSourceStatus(aid_src,
-				      ev_sample.time_us,					// sample timestamp
-				      measurement_ekf_frame,					// observation
-				      measurement_var_ekf_frame,				// observation variance
-				      _state.vel - measurement_ekf_frame,			// innovation
-				      getVelocityVariance() + measurement_var_ekf_frame,	// innovation variance
-				      math::max(_params.ev_vel_innov_gate, 1.f));		// innovation gate
-		aid_src.timestamp_sample = t;
-		measurement.copyTo(aid_src.observation);
-		measurement_var.copyTo(aid_src.observation_variance);
+	const bool measurement_valid = measurement.isAllFinite() && measurement_var.isAllFinite();

-	} else {
-		updateAidSourceStatus(aid_src,
-				      ev_sample.time_us,				// sample timestamp
-				      measurement,					// observation
-				      measurement_var,					// observation variance
-				      _state.vel - measurement,				// innovation
-				      getVelocityVariance() + measurement_var,		// innovation variance
-				      math::max(_params.ev_vel_innov_gate, 1.f));	// innovation gate
+	updateAidSourceStatus(aid_src,
+				 ev_sample.time_us,                          // sample timestamp
+				 measurement,                                // observation
+				 measurement_var,                            // observation variance
+				 _state.vel - measurement,                   // innovation
+				 getVelocityVariance() + measurement_var,    // innovation variance
+				 math::max(_params.ev_vel_innov_gate, 1.f)); // innovation gate
+
+	if (!measurement_valid) {
+		continuing_conditions_passing = false;
 	}

-
 	const bool starting_conditions_passing = common_starting_conditions_passing
 			&& continuing_conditions_passing
 			&& ((Vector3f(aid_src.test_ratio).max() < 0.1f) || !isHorizontalAidingActive());

 	if (_control_status.flags.ev_vel) {
+
 		if (continuing_conditions_passing) {
+
 			if ((ev_reset && isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.ev_vel)) || yaw_alignment_changed) {
+
 				if (quality_sufficient) {
 					ECL_INFO("reset to %s", AID_SRC_NAME);
 					_information_events.flags.reset_vel_to_vision = true;
-					resetVelocityToEV(measurement, measurement_var, ev_sample.vel_frame);
+					resetVelocityTo(measurement, measurement_var);
 					resetAidSourceStatusZeroInnovation(aid_src);

 				} else {
@@ -173,7 +163,7 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common
 				}

 			} else if (quality_sufficient) {
-				fuseEvVelocity(aid_src, ev_sample);
+				fuseVelocity(aid_src);

 			} else {
 				aid_src.innovation_rejected = true;
@@ -187,27 +177,24 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common
 					// Data seems good, attempt a reset
 					_information_events.flags.reset_vel_to_vision = true;
 					ECL_WARN("%s fusion failing, resetting", AID_SRC_NAME);
-					resetVelocityToEV(measurement, measurement_var, ev_sample.vel_frame);
+					resetVelocityTo(measurement, measurement_var);
 					resetAidSourceStatusZeroInnovation(aid_src);

 					if (_control_status.flags.in_air) {
 						_nb_ev_vel_reset_available--;
 					}

+				} else if (starting_conditions_passing) {
+					// Data seems good, but previous reset did not fix the issue
+					// something else must be wrong, declare the sensor faulty and stop the fusion
+					//_control_status.flags.ev_vel_fault = true;
+					ECL_WARN("stopping %s fusion, starting conditions failing", AID_SRC_NAME);
+					stopEvVelFusion();
+
 				} else {
-					// differ warning message based on whether the starting conditions are passing
-					if (starting_conditions_passing) {
-						// Data seems good, but previous reset did not fix the issue
-						// something else must be wrong, declare the sensor faulty and stop the fusion
-						//_control_status.flags.ev_vel_fault = true;
-						ECL_WARN("stopping %s fusion, starting conditions failing", AID_SRC_NAME);
-
-					} else {
-						// A reset did not fix the issue but all the starting checks are not passing
-						// This could be a temporary issue, stop the fusion without declaring the sensor faulty
-						ECL_WARN("stopping %s, fusion failing", AID_SRC_NAME);
-					}
-
+					// A reset did not fix the issue but all the starting checks are not passing
+					// This could be a temporary issue, stop the fusion without declaring the sensor faulty
+					ECL_WARN("stopping %s, fusion failing", AID_SRC_NAME);
 					stopEvVelFusion();
 				}
 			}
@@ -224,13 +211,15 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common
 			if (!isHorizontalAidingActive() || yaw_alignment_changed) {
 				ECL_INFO("starting %s fusion, resetting velocity to (%.3f, %.3f, %.3f)", AID_SRC_NAME,
 					 (double)measurement(0), (double)measurement(1), (double)measurement(2));
+
 				_information_events.flags.reset_vel_to_vision = true;
-				resetVelocityToEV(measurement, measurement_var, ev_sample.vel_frame);
+
+				resetVelocityTo(measurement, measurement_var);
 				resetAidSourceStatusZeroInnovation(aid_src);

 				_control_status.flags.ev_vel = true;

-			} else if (fuseEvVelocity(aid_src, ev_sample)) {
+			} else if (fuseVelocity(aid_src)) {
 				ECL_INFO("starting %s fusion", AID_SRC_NAME);
 				_control_status.flags.ev_vel = true;
 			}
@@ -243,63 +232,6 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common
 	}
 }

-bool Ekf::fuseEvVelocity(estimator_aid_source3d_s &aid_src, const extVisionSample &ev_sample)
-{
-	if (ev_sample.vel_frame == VelocityFrame::BODY_FRAME_FRD) {
-
-		VectorState H;
-		estimator_aid_source1d_s current_aid_src;
-		const auto state_vector = _state.vector();
-
-		for (uint8_t index = 0; index <= 2; index++) {
-			current_aid_src.timestamp_sample = aid_src.timestamp_sample;
-
-			if (index == 0) {
-				sym::ComputeEvBodyVelHx(state_vector, &H);
-
-			} else if (index == 1) {
-				sym::ComputeEvBodyVelHy(state_vector, &H);
-
-			} else {
-				sym::ComputeEvBodyVelHz(state_vector, &H);
-			}
-
-			const float innov_var = (H.T() * P * H)(0, 0) + aid_src.observation_variance[index];
-			const float innov = (_R_to_earth.transpose() * _state.vel - Vector3f(aid_src.observation))(index, 0);
-
-			updateAidSourceStatus(current_aid_src,
-					      ev_sample.time_us,				// sample timestamp
-					      aid_src.observation[index],			// observation
-					      aid_src.observation_variance[index],		// observation variance
-					      innov,						// innovation
-					      innov_var,    					// innovation variance
-					      math::max(_params.ev_vel_innov_gate, 1.f));	// innovation gate
-
-			if (!current_aid_src.innovation_rejected) {
-				fuseBodyVelocity(current_aid_src, current_aid_src.innovation_variance, H);
-
-			}
-
-			aid_src.innovation[index] = current_aid_src.innovation;
-			aid_src.innovation_variance[index] = current_aid_src.innovation_variance;
-			aid_src.test_ratio[index] = current_aid_src.test_ratio;
-			aid_src.fused = current_aid_src.fused;
-			aid_src.innovation_rejected |= current_aid_src.innovation_rejected;
-
-			if (aid_src.fused) {
-				aid_src.time_last_fuse = _time_delayed_us;
-			}
-
-		}
-
-		aid_src.timestamp_sample = current_aid_src.timestamp_sample;
-		return !aid_src.innovation_rejected;
-
-	} else {
-		return fuseVelocity(aid_src);
-	}
-}
-
 void Ekf::stopEvVelFusion()
 {
 	if (_control_status.flags.ev_vel) {
@@ -307,23 +239,3 @@ void Ekf::stopEvVelFusion()
 		_control_status.flags.ev_vel = false;
 	}
 }
-
-void Ekf::resetVelocityToEV(const Vector3f &measurement, const Vector3f &measurement_var,
-			    const VelocityFrame &vel_frame)
-{
-	if (vel_frame == VelocityFrame::BODY_FRAME_FRD) {
-		const Vector3f measurement_var_ekf_frame = rotateVarianceToEkf(measurement_var);
-		resetVelocityTo(_R_to_earth * measurement, measurement_var_ekf_frame);
-
-	} else {
-		resetVelocityTo(measurement, measurement_var);
-	}
-
-}
-
-Vector3f Ekf::rotateVarianceToEkf(const Vector3f &measurement_var)
-{
-	// rotate the covariance matrix into the EKF frame
-	const matrix::SquareMatrix<float, 3> R_cov = _R_to_earth * matrix::diag(measurement_var) * _R_to_earth.transpose();
-	return R_cov.diag();
-}
@@ -75,13 +75,15 @@ void Ekf::controlFakePosFusion()
 					 Vector2f(getStateVariance<State::pos>()) + obs_var, // innovation variance
 					 innov_gate);                                        // innovation gate

-		const bool enable_conditions_passing = !isHorizontalAidingActive()
+		const bool continuing_conditions_passing = !isHorizontalAidingActive()
 				&& ((getTiltVariance() > sq(math::radians(3.f))) || _control_status.flags.vehicle_at_rest)
-				&& (!(_params.imu_ctrl & static_cast<int32_t>(ImuCtrl::GravityVector)) || _control_status.flags.vehicle_at_rest)
+				&& (!(_params.imu_ctrl & static_cast<int32_t>(ImuCtrl::GravityVector)) || _control_status.flags.vehicle_at_rest);
+
+		const bool starting_conditions_passing = continuing_conditions_passing
 				&& _horizontal_deadreckon_time_exceeded;

 		if (_control_status.flags.fake_pos) {
-			if (enable_conditions_passing) {
+			if (continuing_conditions_passing) {

 				// always protect against extreme values that could result in a NaN
 				if ((aid_src.test_ratio[0] < sq(100.0f / innov_gate))
@@ -102,7 +104,7 @@ void Ekf::controlFakePosFusion()
 			}

 		} else {
-			if (enable_conditions_passing) {
+			if (starting_conditions_passing) {
 				ECL_INFO("start fake position fusion");
 				_control_status.flags.fake_pos = true;
 				resetFakePosFusion();
@@ -63,8 +63,7 @@ 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, max((uint64_t)1e6, (uint64_t)_min_gps_health_time_us / 10))) {
+		if (runGnssChecks(gnss_sample) && isTimedOut(_last_gps_fail_us, (uint64_t)_min_gps_health_time_us / 2)) {
 			if (isTimedOut(_last_gps_fail_us, (uint64_t)_min_gps_health_time_us)) {
 				// First time checks are passing, latching.
 				_gps_checks_passed = true;
@@ -245,7 +245,6 @@ void Ekf::controlMagFusion()

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

 					_control_status.flags.mag = true;
@@ -53,17 +53,15 @@ 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);

-		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
+		// 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;

 		if (is_quality_good
 		    && is_magnitude_good
-		    && is_tilt_good) {
+		    && is_tilt_good
+		    && is_within_max_sensor_dist) {
 			// 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();
@@ -81,23 +79,22 @@ 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));

-		const bool is_within_max_sensor_dist = getHagl() <= _flow_max_distance;
+		// 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 continuing_conditions_passing = (_params.flow_ctrl == 1)
-				&& _control_status.flags.tilt_align
-				&& is_within_max_sensor_dist;
+							   && _control_status.flags.tilt_align
+							   && (terrain_available || is_flow_required);

 		const bool starting_conditions_passing = continuing_conditions_passing
-				&& 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);
+							 && isTimedOut(_aid_src_optical_flow.time_last_fuse, (uint64_t)2e6); // Prevent rapid switching

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

 				// 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)) {
@@ -128,35 +125,19 @@ void Ekf::startFlowFusion()
 {
 	ECL_INFO("starting optical flow fusion");

-	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;
-	}
+	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 (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 if (!isHorizontalAidingActive()) {
+		resetFlowFusion();
+		_control_status.flags.opt_flow = true;

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

 void Ekf::resetFlowFusion()
@@ -178,34 +159,18 @@ 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();
@@ -220,6 +185,5 @@ 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,6 +33,12 @@

 /**
 * @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"
@@ -67,7 +73,7 @@ void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src)

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

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

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

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

 	Vector2f innov_var;
 	VectorState H;
-	sym::ComputeFlowXyInnovVarAndHx(state_vector, P, R_LOS, FLT_EPSILON, &innov_var, &H);
+	sym::ComputeFlowXyInnovVarAndHx(state_vector, P, range, 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)) {
@@ -118,7 +124,7 @@ void Ekf::fuseOptFlow(const bool update_terrain)

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

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

 		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;
 		}
@@ -163,17 +165,10 @@ 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 = getHagl() - pos_offset_earth(2);
+	const float height_above_gnd_est = math::max(_terrain_vpos - _state.pos(2) - pos_offset_earth(2), fmaxf(_params.rng_gnd_clearance, 0.01f));

 	// calculate range from focal point to centre of image
-	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;
+	return height_above_gnd_est / _R_to_earth(2, 2); // absolute distance to the frame region in view
 }

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

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

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

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

 	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()) {

-		updateRangeHagl(aid_src);
-		const bool measurement_valid = PX4_ISFINITE(aid_src.observation) && PX4_ISFINITE(aid_src.observation_variance);
+		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());

-		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 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 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) {

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

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

-				if (isHeightResetRequired() && _control_status.flags.rng_hgt) {
+				if (isHeightResetRequired()) {
 					// 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(-(aid_src.observation - _state.terrain));
+					resetVerticalPositionTo(-(measurement - bias_est.getBias()));
+					bias_est.setBias(_state.pos(2) + measurement);

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

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

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

 		} else {
 			if (starting_conditions_passing) {
-				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);
-					}
+				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();

 				} else {
-					if (aid_src.innovation_rejected) {
-						resetTerrainToRng(aid_src);
-					}
-
-					_hagl_sensor_status.flags.range_finder = true;
+					ECL_INFO("starting %s height fusion", HGT_SRC_NAME);
+					bias_est.setBias(_state.pos(2) + measurement);
 				}
+
+				aid_src.time_last_fuse = _time_delayed_us;
+				bias_est.setFusionActive();
+				_control_status.flags.rng_hgt = true;
 			}
 		}

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

-	const float hagl_test_ratio = _aid_src_rng_hgt.test_ratio;
+	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;

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

-	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 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;
 	}

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

-	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;
+	return false;
 }

 void Ekf::stopRngHgtFusion()
@@ -311,11 +256,8 @@ 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;
-}
@@ -1,70 +0,0 @@
-/****************************************************************************
- *
- *   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;
-}
@@ -107,7 +107,7 @@ enum MagFuseType : uint8_t {
 #endif // CONFIG_EKF2_MAGNETOMETER

 #if defined(CONFIG_EKF2_TERRAIN)
-enum class TerrainFusionMask : uint8_t {
+enum TerrainFusionMask : uint8_t {
 	TerrainFuseRangeFinder = (1 << 0),
 	TerrainFuseOpticalFlow = (1 << 1)
 };
@@ -382,6 +382,9 @@ 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)
@@ -398,8 +401,10 @@ 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
@@ -516,7 +521,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 - unused
+		bool reject_hagl      : 1; ///< 10 - true if the height above ground observation has been rejected
 		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;
@@ -144,10 +144,6 @@ 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,11 +99,6 @@ 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)
@@ -207,17 +202,6 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
 	}
 #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++) {
@@ -255,10 +239,6 @@ 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)
@@ -71,11 +71,6 @@ 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);
@@ -127,10 +122,7 @@ void Ekf::reset()

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

 	_zero_velocity_update.reset();
 }
@@ -168,6 +160,11 @@ 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;
@@ -203,6 +200,11 @@ 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);

@@ -281,66 +283,23 @@ void Ekf::predictState(const imuSample &imu_delayed)
 	_height_rate_lpf = _height_rate_lpf * (1.0f - alpha_height_rate_lpf) + _state.vel(2) * alpha_height_rate_lpf;
 }

-bool Ekf::resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float accuracy, uint64_t timestamp_observation)
+void Ekf::resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float accuracy, uint64_t timestamp_observation)
 {
 	if (!_pos_ref.isInitialized()) {
-		ECL_WARN("unable to reset global position, position reference not initialized");
-		return false;
+		return;
 	}

-	Vector2f pos_corrected = _pos_ref.project(lat_deg, lon_deg);
+	// apply a first order correction using velocity at the delated time horizon and the delta time
+	timestamp_observation = math::min(_time_latest_us, timestamp_observation);
+	const float dt = _time_delayed_us > timestamp_observation ? static_cast<float>(_time_delayed_us - timestamp_observation)
+			 * 1e-6f : -static_cast<float>(timestamp_observation - _time_delayed_us) * 1e-6f;

-	// apply a first order correction using velocity at the delayed time horizon and the delta time
-	if ((timestamp_observation > 0) && (isHorizontalAidingActive() || !_horizontal_deadreckon_time_exceeded)) {
+	Vector2f pos_corrected = _pos_ref.project(lat_deg, lon_deg) + _state.vel.xy() * dt;

-		timestamp_observation = math::min(_time_latest_us, timestamp_observation);
+	resetHorizontalPositionTo(pos_corrected, sq(math::max(accuracy, 0.01f)));

-		float diff_us = 0.f;
-
-		if (_time_delayed_us >= timestamp_observation) {
-			diff_us = static_cast<float>(_time_delayed_us - timestamp_observation);
-
-		} else {
-			diff_us = -static_cast<float>(timestamp_observation - _time_delayed_us);
-		}
-
-		const float dt_s = diff_us * 1e-6f;
-		pos_corrected += _state.vel.xy() * dt_s;
-	}
-
-	const float obs_var = math::max(sq(accuracy), sq(0.01f));
-
-	const Vector2f innov = Vector2f(_state.pos.xy()) - pos_corrected;
-	const Vector2f innov_var = Vector2f(getStateVariance<State::pos>()) + obs_var;
-
-	const float sq_gate = sq(5.f); // magic hardcoded gate
-	const Vector2f test_ratio{sq(innov(0)) / (sq_gate * innov_var(0)),
-					sq(innov(1)) / (sq_gate * innov_var(1))};
-
-	const bool innov_rejected = (test_ratio.max() > 1.f);
-
-	if (!_control_status.flags.in_air || (accuracy > 0.f && accuracy < 1.f) || innov_rejected) {
-		// when on ground or accuracy chosen to be very low, we hard reset position
-		// this allows the user to still send hard resets at any time
-		ECL_INFO("reset position to external observation");
-		_information_events.flags.reset_pos_to_ext_obs = true;
-
-		resetHorizontalPositionTo(pos_corrected, obs_var);
-		_last_known_pos.xy() = _state.pos.xy();
-		return true;
-
-	} else {
-		if (fuseDirectStateMeasurement(innov(0), innov_var(0), obs_var, State::pos.idx + 0)
-		 && fuseDirectStateMeasurement(innov(1), innov_var(1), obs_var, State::pos.idx + 1)
-		) {
-			ECL_INFO("fused external observation as position measurement");
-			_time_last_hor_pos_fuse = _time_delayed_us;
-			_last_known_pos.xy() = _state.pos.xy();
-			return true;
-		}
-	}
-
-	return false;
+	ECL_INFO("reset position to external observation");
+	_information_events.flags.reset_pos_to_ext_obs = true;
 }

 void Ekf::updateParameters()
@@ -418,14 +377,6 @@ 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,6 +73,7 @@ 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()
 	{
@@ -103,19 +104,27 @@ 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 _state.terrain; };
-	float getHagl() const { return _state.terrain - _state.pos(2); }
+	float getTerrainVertPos() const { return _terrain_vpos; };

 	// 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 getTerrainVariance() const { return P(State::terrain.idx, State::terrain.idx); }
+	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

 #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(); }
@@ -374,17 +383,13 @@ public:
 		*counter = _state_reset_status.reset_count.quat;
 	}

-	float getHeadingInnovationTestRatio() const;
-
-	float getVelocityInnovationTestRatio() const;
-
-	float getHorizontalPositionInnovationTestRatio() const;
-	float getVerticalPositionInnovationTestRatio() const;
-
-	float getAirspeedInnovationTestRatio() const;
-	float getSyntheticSideslipInnovationTestRatio() const;
-
-	float getHeightAboveGroundInnovationTestRatio() const;
+	// 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;

 	// return a bitmask integer that describes which state estimates are valid
 	void get_ekf_soln_status(uint16_t *status) const;
@@ -501,7 +506,7 @@ public:
 		return true;
 	}

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

 	void updateParameters();

@@ -583,14 +588,27 @@ 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)
@@ -705,7 +723,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[3];		///< counter per axis that increments when clipping ad decrements when not
+	uint16_t _clip_counter{0};		///< counter 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);
@@ -806,30 +824,41 @@ private:
 	bool fuseVelocity(estimator_aid_source3d_s &vel_aid_src);

 #if defined(CONFIG_EKF2_TERRAIN)
-	void initTerrain();
-	float getTerrainVPos() const { return isTerrainEstimateValid() ? _state.terrain : _last_on_ground_posD; }
-	void controlTerrainFakeFusion();
+	// 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);

 # if defined(CONFIG_EKF2_RANGE_FINDER)
 	// update the terrain vertical position estimate using a height above ground measurement from the range finder
-	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);
+	void controlHaglRngFusion();
+	void updateHaglRng(estimator_aid_source1d_s &aid_src) const;
+	void fuseHaglRng(estimator_aid_source1d_s &aid_src);
+	void resetHaglRng();
+	void stopHaglRngFusion();
 	float getRngVar() const;
 # endif // CONFIG_EKF2_RANGE_FINDER

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

 #endif // CONFIG_EKF2_TERRAIN

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

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

 	// fuse optical flow line of sight rate measurements
 	void updateOptFlow(estimator_aid_source2d_s &aid_src);
-	void fuseOptFlow(bool update_terrain);
+	void fuseOptFlow();
 	float predictFlowRange();
 	Vector2f predictFlowVelBody();
 #endif // CONFIG_EKF2_OPTICAL_FLOW
@@ -921,8 +950,6 @@ private:
 	void controlEvPosFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source2d_s &aid_src);
 	void controlEvVelFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source3d_s &aid_src);
 	void controlEvYawFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s &aid_src);
-	void resetVelocityToEV(const Vector3f &measurement, const Vector3f &measurement_var, const VelocityFrame &vel_frame);
-	Vector3f rotateVarianceToEkf(const Vector3f &measurement_var);

 	void startEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, estimator_aid_source2d_s &aid_src);
 	void updateEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, bool quality_sufficient, bool reset, estimator_aid_source2d_s &aid_src);
@@ -930,12 +957,6 @@ private:
 	void stopEvHgtFusion();
 	void stopEvVelFusion();
 	void stopEvYawFusion();
-	bool fuseEvVelocity(estimator_aid_source3d_s &aid_src, const extVisionSample &ev_sample);
-	void fuseBodyVelocity(estimator_aid_source1d_s &aid_src, float &innov_var, VectorState &H)
-	{
-		VectorState Kfusion = P * H / innov_var;
-		aid_src.fused = measurementUpdate(Kfusion, H, aid_src.observation_variance, aid_src.innovation);
-	}
 #endif // CONFIG_EKF2_EXTERNAL_VISION

 #if defined(CONFIG_EKF2_GNSS)
@@ -89,7 +89,7 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons
 			current_pos_available = true;
 		}

-		const float gps_alt_ref_prev = _gps_alt_ref;
+		const float gps_alt_ref_prev = getEkfGlobalOriginAltitude();

 		// reinitialize map projection to latitude, longitude, altitude, and reset position
 		_pos_ref.initReference(latitude, longitude, _time_delayed_us);
@@ -114,24 +114,30 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons

 		_NED_origin_initialised = true;

+		// minimum change in position or height that triggers a reset
+		static constexpr float MIN_RESET_DIST_M = 0.01f;
+
 		if (current_pos_available) {
-			// reset horizontal position if we already have a global origin
+			// reset horizontal position
 			Vector2f position = _pos_ref.project(current_lat, current_lon);
-			resetHorizontalPositionTo(position);
+
+			if (Vector2f(position - Vector2f(_state.pos)).longerThan(MIN_RESET_DIST_M)) {
+				resetHorizontalPositionTo(position);
+			}
 		}

-		if (PX4_ISFINITE(gps_alt_ref_prev) && isVerticalPositionAidingActive()) {
+		// reset vertical position (if there's any change)
+		if (fabsf(altitude - gps_alt_ref_prev) > MIN_RESET_DIST_M) {
 			// determine current z
-			const float z_prev = _state.pos(2);
-			const float current_alt = -z_prev + gps_alt_ref_prev;
+			float current_alt = -_state.pos(2) + gps_alt_ref_prev;
+
 #if defined(CONFIG_EKF2_GNSS)
 			const float gps_hgt_bias = _gps_hgt_b_est.getBias();
 #endif // CONFIG_EKF2_GNSS
 			resetVerticalPositionTo(_gps_alt_ref - current_alt);
-			ECL_DEBUG("EKF global origin updated, resetting vertical position %.1fm -> %.1fm", (double)z_prev,
-				  (double)_state.pos(2));
+
 #if defined(CONFIG_EKF2_GNSS)
-			// adjust existing GPS height bias
+			// preserve GPS height bias
 			_gps_hgt_b_est.setBias(gps_hgt_bias);
 #endif // CONFIG_EKF2_GNSS
 		}
@@ -205,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(getHagl(), gndclearance) * Vector2f(_aid_src_optical_flow.innovation).norm();
+			vel_err_conservative = math::max((_terrain_vpos - _state.pos(2)), gndclearance) * Vector2f(_aid_src_optical_flow.innovation).norm();
 		}
 #endif // CONFIG_EKF2_OPTICAL_FLOW

@@ -265,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(getHagl(), flow_hagl_min, flow_hagl_max);
+		const float flow_constrained_height = math::constrain(_terrain_vpos - _state.pos(2), 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;
@@ -295,187 +301,131 @@ void Ekf::resetAccelBias()
 	resetAccelBiasCov();
 }

-float Ekf::getHeadingInnovationTestRatio() const
+void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, float &pos, float &hgt, float &tas,
+				     float &hagl, float &beta) const
 {
-	// return the largest heading innovation test ratio
-	float test_ratio = 0.f;
+	// 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;

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

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

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

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

 #if defined(CONFIG_EKF2_GNSS)
 	if (_control_status.flags.gps) {
-		for (int i = 0; i < 3; i++) {
-			test_ratio = math::max(test_ratio, fabsf(_aid_src_gnss_vel.test_ratio_filtered[i]));
-		}
+		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);
 	}
 #endif // CONFIG_EKF2_GNSS

 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	if (_control_status.flags.ev_vel) {
-		for (int i = 0; i < 3; i++) {
-			test_ratio = math::max(test_ratio, fabsf(_aid_src_ev_vel.test_ratio_filtered[i]));
-		}
+		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);
 	}
 #endif // CONFIG_EKF2_EXTERNAL_VISION

 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
 	if (isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow)) {
-		for (auto &test_ratio_filtered : _aid_src_optical_flow.test_ratio_filtered) {
-			test_ratio = math::max(test_ratio, fabsf(test_ratio_filtered));
-		}
+		float of_vel = sqrtf(Vector2f(_aid_src_optical_flow.test_ratio).max());
+		vel = math::max(of_vel, FLT_MIN);
 	}
 #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(fabsf(_aid_src_baro_hgt.test_ratio_filtered));
+		hgt_sum += sqrtf(_aid_src_baro_hgt.test_ratio);
 		n_hgt_sources++;
 	}
 #endif // CONFIG_EKF2_BAROMETER

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

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

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

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

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

-	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;
-
+	hagl = NAN;
 #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
-		test_ratio = math::max(test_ratio, fabsf(_aid_src_rng_hgt.test_ratio_filtered));
+		hagl = sqrtf(_aid_src_terrain_range_finder.test_ratio);
 	}
-# endif // CONFIG_EKF2_RANGE_FINDER
+#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_TERRAIN

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

 void Ekf::get_ekf_soln_status(uint16_t *status) const
@@ -554,10 +504,6 @@ 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()
@@ -568,91 +514,44 @@ void Ekf::updateDeadReckoningStatus()

 void Ekf::updateHorizontalDeadReckoningstatus()
 {
-	bool inertial_dead_reckoning = true;
-	bool aiding_expected_in_air = false;
-
-	// velocity aiding active
-	if ((_control_status.flags.gps || _control_status.flags.ev_vel)
-	&& isRecent(_time_last_hor_vel_fuse, _params.no_aid_timeout_max)
-	) {
-		inertial_dead_reckoning = false;
-	}
-
-	// position aiding active
-	if ((_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.aux_gpos)
-	&& isRecent(_time_last_hor_pos_fuse, _params.no_aid_timeout_max)
-	) {
-		inertial_dead_reckoning = false;
-	}
+	const bool velPosAiding = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.ev_vel || _control_status.flags.aux_gpos)
+				  && (isRecent(_time_last_hor_pos_fuse, _params.no_aid_timeout_max)
+				      || isRecent(_time_last_hor_vel_fuse, _params.no_aid_timeout_max));

+	bool optFlowAiding = false;
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
-	// optical flow active
-	if (_control_status.flags.opt_flow
-	&& isRecent(_aid_src_optical_flow.time_last_fuse, _params.no_aid_timeout_max)
-	) {
-		inertial_dead_reckoning = false;
-
-	} else {
-		if (!_control_status.flags.in_air && (_params.flow_ctrl == 1)
-		&& isRecent(_aid_src_optical_flow.timestamp_sample, _params.no_aid_timeout_max)
-		) {
-			// currently landed, but optical flow aiding should be possible once in air
-			aiding_expected_in_air = true;
-		}
-	}
+	optFlowAiding = _control_status.flags.opt_flow && isRecent(_aid_src_optical_flow.time_last_fuse, _params.no_aid_timeout_max);
 #endif // CONFIG_EKF2_OPTICAL_FLOW

+	bool airDataAiding = false;
+
 #if defined(CONFIG_EKF2_AIRSPEED)
-	// air data aiding active
-	if ((_control_status.flags.fuse_aspd && isRecent(_aid_src_airspeed.time_last_fuse, _params.no_aid_timeout_max))
-	&& (_control_status.flags.fuse_beta && isRecent(_aid_src_sideslip.time_last_fuse, _params.no_aid_timeout_max))
-	) {
-		// wind_dead_reckoning: no other aiding but air data
-		_control_status.flags.wind_dead_reckoning = inertial_dead_reckoning;
+	airDataAiding = _control_status.flags.wind &&
+				   isRecent(_aid_src_airspeed.time_last_fuse, _params.no_aid_timeout_max) &&
+				   isRecent(_aid_src_sideslip.time_last_fuse, _params.no_aid_timeout_max);

-		// air data aiding is active, we're not inertial dead reckoning
-		inertial_dead_reckoning = false;
-
-	} else {
-		_control_status.flags.wind_dead_reckoning = false;
-
-		if (!_control_status.flags.in_air && _control_status.flags.fixed_wing
-		&& (_params.beta_fusion_enabled == 1)
-		&& (_params.arsp_thr > 0.f) && isRecent(_aid_src_airspeed.timestamp_sample, _params.no_aid_timeout_max)
-		) {
-			// currently landed, but air data aiding should be possible once in air
-			aiding_expected_in_air = true;
-		}
-	}
+	_control_status.flags.wind_dead_reckoning = !velPosAiding && !optFlowAiding && airDataAiding;
+#else
+	_control_status.flags.wind_dead_reckoning = false;
 #endif // CONFIG_EKF2_AIRSPEED

-	// zero velocity update
-	if (isRecent(_zero_velocity_update.time_last_fuse(), _params.no_aid_timeout_max)) {
-		// only respect as a valid aiding source now if we expect to have another valid source once in air
-		if (aiding_expected_in_air) {
-			inertial_dead_reckoning = false;
-		}
-	}
+	_control_status.flags.inertial_dead_reckoning = !velPosAiding && !optFlowAiding && !airDataAiding;

-	if (inertial_dead_reckoning) {
-		if (isTimedOut(_time_last_horizontal_aiding, (uint64_t)_params.valid_timeout_max)) {
-			// deadreckon time exceeded
-			if (!_horizontal_deadreckon_time_exceeded) {
-				ECL_WARN("horizontal dead reckon time exceeded");
-				_horizontal_deadreckon_time_exceeded = true;
-			}
-		}
-
-	} else {
+	if (!_control_status.flags.inertial_dead_reckoning) {
 		if (_time_delayed_us > _params.no_aid_timeout_max) {
 			_time_last_horizontal_aiding = _time_delayed_us - _params.no_aid_timeout_max;
 		}
-
-		_horizontal_deadreckon_time_exceeded = false;
-
 	}

-	_control_status.flags.inertial_dead_reckoning = inertial_dead_reckoning;
+	// report if we have been deadreckoning for too long, initial state is deadreckoning until aiding is present
+	bool deadreckon_time_exceeded = isTimedOut(_time_last_horizontal_aiding, (uint64_t)_params.valid_timeout_max);
+
+	if (!_horizontal_deadreckon_time_exceeded && deadreckon_time_exceeded) {
+		// deadreckon time now exceeded
+		ECL_WARN("dead reckon time exceeded");
+	}
+
+	_horizontal_deadreckon_time_exceeded = deadreckon_time_exceeded;
 }

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

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

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

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

 	Likelihood inertial_nav_falling_likelihood = estimateInertialNavFallingLikelihood();

-	const uint16_t kClipCountLimit = 1.f / _dt_ekf_avg;
+	// 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];

-	bool acc_clip_warning[3] {};
-	bool acc_clip_critical[3] {};
+	if (is_clipping && _clip_counter < clip_count_limit) {
+		_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;
+	} else if (_clip_counter > 0) {
+		_clip_counter--;
 	}

-	// 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]);
+	_fault_status.flags.bad_acc_clipping = _clip_counter > clip_count_limit / 2;

-	_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;
+	const bool is_clipping_frequently = _clip_counter > 0;

 	// 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))
@@ -311,8 +301,7 @@ Likelihood Ekf::estimateInertialNavFallingLikelihood() const

 #if defined(CONFIG_EKF2_RANGE_FINDER)
 	if (_control_status.flags.rng_hgt) {
-		// 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};
+		checks[3] = {ReferenceType::GROUND, _aid_src_rng_hgt.innovation, _aid_src_rng_hgt.innovation_variance};
 	}
 #endif // CONFIG_EKF2_RANGE_FINDER

@@ -161,9 +161,12 @@ 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)
-	_state.terrain += delta_z;
+	terrainHandleVerticalPositionReset(delta_z);
 #endif

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

 if args.disable_mag:
@@ -133,8 +132,7 @@ 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"),
-        terrain = sf.V1.symbolic("terrain")
+        wind_vel = sf.V2.symbolic("wind_vel")
    )

    if args.disable_mag:
@@ -362,40 +360,6 @@ def compute_sideslip_h_and_k(

    return (H.T, K)

-def predict_vel_body(
-        state: VState
-) -> (sf.V3):
-    vel = state["vel"]
-    R_to_body = state["quat_nominal"].inverse()
-    return R_to_body * vel
-
-def compute_ev_body_vel_hx(
-        state: VState,
-) -> (VTangent):
-
-    state = vstate_to_state(state)
-    meas_pred = predict_vel_body(state)
-    Hx = jacobian_chain_rule(meas_pred[0], state)
-    return (Hx.T)
-
-def compute_ev_body_vel_hy(
-        state: VState,
-) -> (VTangent):
-
-    state = vstate_to_state(state)
-    meas_pred = predict_vel_body(state)[1]
-    Hy = jacobian_chain_rule(meas_pred, state)
-    return (Hy.T)
-
-def compute_ev_body_vel_hz(
-        state: VState,
-) -> (VTangent):
-
-    state = vstate_to_state(state)
-    meas_pred = predict_vel_body(state)[2]
-    Hz = jacobian_chain_rule(meas_pred, state)
-    return (Hz.T)
-
 def predict_mag_body(state) -> sf.V3:
    mag_field_earth = state["mag_I"]
    mag_bias_body = state["mag_B"]
@@ -492,10 +456,7 @@ def compute_mag_declination_pred_innov_var_and_h(

    return (meas_pred, innov_var, H.T)

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

    # Calculate earth relative velocity in a non-rotating sensor frame
@@ -503,23 +464,23 @@ def predict_opt_flow(state, 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] / hagl * R_to_earth[2, 2]
-    flow_pred[1] = -rel_vel_sensor[0] / hagl * R_to_earth[2, 2]
+    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)

    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, epsilon)
+    meas_pred = predict_opt_flow(state, distance, epsilon);

    innov_var = sf.V2()
    Hx = jacobian_chain_rule(meas_pred[0], state)
@@ -532,40 +493,18 @@ 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, epsilon)
+    meas_pred = predict_opt_flow(state, distance, 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,
@@ -725,14 +664,9 @@ 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"])
 generate_px4_function(compute_gravity_z_innov_var_and_h, output_names=["innov_var", "Hz"])
-generate_px4_function(compute_ev_body_vel_hx, output_names=["H"])
-generate_px4_function(compute_ev_body_vel_hy, output_names=["H"])
-generate_px4_function(compute_ev_body_vel_hz, output_names=["H"])

 generate_px4_state(State, tangent_idx)
@@ -16,21 +16,21 @@ namespace sym {
 * Symbolic function: compute_airspeed_h_and_k
 *
 * Args:
- *     state: Matrix25_1
- *     P: Matrix24_24
+ *     state: Matrix24_1
+ *     P: Matrix23_23
 *     innov_var: Scalar
 *     epsilon: Scalar
 *
 * Outputs:
- *     H: Matrix24_1
- *     K: Matrix24_1
+ *     H: Matrix23_1
+ *     K: Matrix23_1
 */
 template <typename Scalar>
-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
+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

  // Input arrays

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

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

    _h.setZero();

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

  if (K != nullptr) {
-    matrix::Matrix<Scalar, 24, 1>& _k = (*K);
+    matrix::Matrix<Scalar, 23, 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,8 +107,6 @@ void ComputeAirspeedHAndK(const matrix::Matrix<Scalar, 25, 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: Matrix25_1
- *     P: Matrix24_24
+ *     state: Matrix24_1
+ *     P: Matrix23_23
 *     airspeed: Scalar
 *     R: Scalar
 *     epsilon: Scalar
@@ -27,8 +27,8 @@ namespace sym {
 *     innov_var: Scalar
 */
 template <typename Scalar>
-void ComputeAirspeedInnovAndInnovVar(const matrix::Matrix<Scalar, 25, 1>& state,
-                                     const matrix::Matrix<Scalar, 24, 24>& P, const Scalar airspeed,
+void ComputeAirspeedInnovAndInnovVar(const matrix::Matrix<Scalar, 24, 1>& state,
+                                     const matrix::Matrix<Scalar, 23, 23>& 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: Matrix25_1
- *     P: Matrix24_24
+ *     state: Matrix24_1
+ *     P: Matrix23_23
 *     rho: Scalar
 *     cd: Scalar
 *     cm: Scalar
@@ -26,14 +26,14 @@ namespace sym {
 *
 * Outputs:
 *     innov_var: Scalar
- *     Hx: Matrix24_1
+ *     Hx: Matrix23_1
 */
 template <typename Scalar>
-void ComputeDragXInnovVarAndH(const matrix::Matrix<Scalar, 25, 1>& state,
-                              const matrix::Matrix<Scalar, 24, 24>& P, const Scalar rho,
+void ComputeDragXInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
+                              const matrix::Matrix<Scalar, 23, 23>& P, const Scalar rho,
                              const Scalar cd, const Scalar cm, const Scalar R,
                              const Scalar epsilon, Scalar* const innov_var = nullptr,
-                              matrix::Matrix<Scalar, 24, 1>* const Hx = nullptr) {
+                              matrix::Matrix<Scalar, 23, 1>* const Hx = nullptr) {
  // Total ops: 357

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

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

    _hx.setZero();

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

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

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

    _hy.setZero();

@@ -1,63 +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: compute_ev_body_vel_hx
- *
- * Args:
- *     state: Matrix25_1
- *
- * Outputs:
- *     H: Matrix24_1
- */
-template <typename Scalar>
-void ComputeEvBodyVelHx(const matrix::Matrix<Scalar, 25, 1>& state,
-                        matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
-  // Total ops: 60
-
-  // Input arrays
-
-  // Intermediate terms (13)
-  const Scalar _tmp0 = 2 * state(5, 0);
-  const Scalar _tmp1 = 2 * state(6, 0);
-  const Scalar _tmp2 = _tmp0 * state(3, 0) - _tmp1 * state(2, 0);
-  const Scalar _tmp3 = (Scalar(1) / Scalar(2)) * state(1, 0);
-  const Scalar _tmp4 =
-      (Scalar(1) / Scalar(2)) * _tmp0 * state(2, 0) + (Scalar(1) / Scalar(2)) * _tmp1 * state(3, 0);
-  const Scalar _tmp5 = 4 * state(4, 0);
-  const Scalar _tmp6 = 2 * state(1, 0);
-  const Scalar _tmp7 = _tmp0 * state(0, 0) - _tmp5 * state(3, 0) + _tmp6 * state(6, 0);
-  const Scalar _tmp8 = (Scalar(1) / Scalar(2)) * _tmp7;
-  const Scalar _tmp9 = 2 * state(0, 0);
-  const Scalar _tmp10 = _tmp0 * state(1, 0) - _tmp5 * state(2, 0) - _tmp9 * state(6, 0);
-  const Scalar _tmp11 = (Scalar(1) / Scalar(2)) * _tmp10;
-  const Scalar _tmp12 = (Scalar(1) / Scalar(2)) * _tmp2;
-
-  // Output terms (1)
-  if (H != nullptr) {
-    matrix::Matrix<Scalar, 24, 1>& _h = (*H);
-
-    _h.setZero();
-
-    _h(0, 0) = -_tmp11 * state(3, 0) - _tmp2 * _tmp3 + _tmp4 * state(0, 0) + _tmp8 * state(2, 0);
-    _h(1, 0) = _tmp11 * state(0, 0) - _tmp12 * state(2, 0) - _tmp3 * _tmp7 + _tmp4 * state(3, 0);
-    _h(2, 0) = _tmp10 * _tmp3 - _tmp12 * state(3, 0) - _tmp4 * state(2, 0) + _tmp8 * state(0, 0);
-    _h(3, 0) = -2 * std::pow(state(2, 0), Scalar(2)) - 2 * std::pow(state(3, 0), Scalar(2)) + 1;
-    _h(4, 0) = _tmp6 * state(2, 0) + _tmp9 * state(3, 0);
-    _h(5, 0) = _tmp6 * state(3, 0) - _tmp9 * state(2, 0);
-  }
-}  // NOLINT(readability/fn_size)
-
-// NOLINTNEXTLINE(readability/fn_size)
-}  // namespace sym
@@ -1,67 +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: compute_ev_body_vel_hy
- *
- * Args:
- *     state: Matrix25_1
- *
- * Outputs:
- *     H: Matrix24_1
- */
-template <typename Scalar>
-void ComputeEvBodyVelHy(const matrix::Matrix<Scalar, 25, 1>& state,
-                        matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
-  // Total ops: 64
-
-  // Input arrays
-
-  // Intermediate terms (9)
-  const Scalar _tmp0 = 2 * state(4, 0);
-  const Scalar _tmp1 = 2 * state(1, 0);
-  const Scalar _tmp2 =
-      -Scalar(1) / Scalar(2) * _tmp0 * state(3, 0) + (Scalar(1) / Scalar(2)) * _tmp1 * state(6, 0);
-  const Scalar _tmp3 = 2 * state(3, 0);
-  const Scalar _tmp4 =
-      (Scalar(1) / Scalar(2)) * _tmp0 * state(1, 0) + (Scalar(1) / Scalar(2)) * _tmp3 * state(6, 0);
-  const Scalar _tmp5 = 4 * state(5, 0);
-  const Scalar _tmp6 = 2 * state(6, 0);
-  const Scalar _tmp7 = -Scalar(1) / Scalar(2) * _tmp0 * state(0, 0) -
-                       Scalar(1) / Scalar(2) * _tmp5 * state(3, 0) +
-                       (Scalar(1) / Scalar(2)) * _tmp6 * state(2, 0);
-  const Scalar _tmp8 = (Scalar(1) / Scalar(2)) * _tmp0 * state(2, 0) -
-                       Scalar(1) / Scalar(2) * _tmp5 * state(1, 0) +
-                       (Scalar(1) / Scalar(2)) * _tmp6 * state(0, 0);
-
-  // Output terms (1)
-  if (H != nullptr) {
-    matrix::Matrix<Scalar, 24, 1>& _h = (*H);
-
-    _h.setZero();
-
-    _h(0, 0) =
-        -_tmp2 * state(1, 0) - _tmp4 * state(3, 0) + _tmp7 * state(2, 0) + _tmp8 * state(0, 0);
-    _h(1, 0) =
-        -_tmp2 * state(2, 0) + _tmp4 * state(0, 0) - _tmp7 * state(1, 0) + _tmp8 * state(3, 0);
-    _h(2, 0) =
-        -_tmp2 * state(3, 0) + _tmp4 * state(1, 0) + _tmp7 * state(0, 0) - _tmp8 * state(2, 0);
-    _h(3, 0) = _tmp1 * state(2, 0) - _tmp3 * state(0, 0);
-    _h(4, 0) = -2 * std::pow(state(1, 0), Scalar(2)) - 2 * std::pow(state(3, 0), Scalar(2)) + 1;
-    _h(5, 0) = _tmp1 * state(0, 0) + _tmp3 * state(2, 0);
-  }
-}  // NOLINT(readability/fn_size)
-
-// NOLINTNEXTLINE(readability/fn_size)
-}  // namespace sym
@@ -1,63 +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: compute_ev_body_vel_hz
- *
- * Args:
- *     state: Matrix25_1
- *
- * Outputs:
- *     H: Matrix24_1
- */
-template <typename Scalar>
-void ComputeEvBodyVelHz(const matrix::Matrix<Scalar, 25, 1>& state,
-                        matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
-  // Total ops: 60
-
-  // Input arrays
-
-  // Intermediate terms (13)
-  const Scalar _tmp0 = 2 * state(4, 0);
-  const Scalar _tmp1 = 2 * state(5, 0);
-  const Scalar _tmp2 =
-      (Scalar(1) / Scalar(2)) * _tmp0 * state(1, 0) + (Scalar(1) / Scalar(2)) * _tmp1 * state(2, 0);
-  const Scalar _tmp3 = _tmp0 * state(2, 0) - _tmp1 * state(1, 0);
-  const Scalar _tmp4 = (Scalar(1) / Scalar(2)) * _tmp3;
-  const Scalar _tmp5 = 4 * state(6, 0);
-  const Scalar _tmp6 = _tmp0 * state(3, 0) - _tmp1 * state(0, 0) - _tmp5 * state(1, 0);
-  const Scalar _tmp7 = (Scalar(1) / Scalar(2)) * _tmp6;
-  const Scalar _tmp8 = _tmp0 * state(0, 0) + _tmp1 * state(3, 0) - _tmp5 * state(2, 0);
-  const Scalar _tmp9 = (Scalar(1) / Scalar(2)) * state(3, 0);
-  const Scalar _tmp10 = (Scalar(1) / Scalar(2)) * _tmp8;
-  const Scalar _tmp11 = 2 * state(2, 0);
-  const Scalar _tmp12 = 2 * state(1, 0);
-
-  // Output terms (1)
-  if (H != nullptr) {
-    matrix::Matrix<Scalar, 24, 1>& _h = (*H);
-
-    _h.setZero();
-
-    _h(0, 0) = _tmp2 * state(2, 0) - _tmp4 * state(1, 0) + _tmp7 * state(0, 0) - _tmp8 * _tmp9;
-    _h(1, 0) = _tmp10 * state(0, 0) - _tmp2 * state(1, 0) - _tmp4 * state(2, 0) + _tmp6 * _tmp9;
-    _h(2, 0) = _tmp10 * state(1, 0) + _tmp2 * state(0, 0) - _tmp3 * _tmp9 - _tmp7 * state(2, 0);
-    _h(3, 0) = _tmp11 * state(0, 0) + _tmp12 * state(3, 0);
-    _h(4, 0) = _tmp11 * state(3, 0) - _tmp12 * state(0, 0);
-    _h(5, 0) = -2 * std::pow(state(1, 0), Scalar(2)) - 2 * std::pow(state(2, 0), Scalar(2)) + 1;
-  }
-}  // NOLINT(readability/fn_size)
-
-// NOLINTNEXTLINE(readability/fn_size)
-}  // namespace sym
@@ -16,153 +16,117 @@ namespace sym {
 * Symbolic function: compute_flow_xy_innov_var_and_hx
 *
 * Args:
- *     state: Matrix25_1
- *     P: Matrix24_24
+ *     state: Matrix24_1
+ *     P: Matrix23_23
+ *     distance: Scalar
 *     R: Scalar
 *     epsilon: Scalar
 *
 * Outputs:
 *     innov_var: Matrix21
- *     H: Matrix24_1
+ *     H: Matrix23_1
 */
 template <typename Scalar>
-void ComputeFlowXyInnovVarAndHx(const matrix::Matrix<Scalar, 25, 1>& state,
-                                const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
-                                const Scalar epsilon,
+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,
                                matrix::Matrix<Scalar, 2, 1>* const innov_var = nullptr,
-                                matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
-  // Total ops: 431
+                                matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
+  // Total ops: 275

  // Input arrays

-  // 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;
+  // 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));

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

-    _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);
+    _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);
  }

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

    _h.setZero();

-    _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;
+    _h(0, 0) = _tmp23;
+    _h(1, 0) = _tmp18;
+    _h(2, 0) = _tmp24;
+    _h(3, 0) = _tmp27;
+    _h(4, 0) = _tmp20;
+    _h(5, 0) = _tmp28;
  }
 }  // NOLINT(readability/fn_size)

@@ -16,100 +16,85 @@ namespace sym {
 * Symbolic function: compute_flow_y_innov_var_and_h
 *
 * Args:
- *     state: Matrix25_1
- *     P: Matrix24_24
+ *     state: Matrix24_1
+ *     P: Matrix23_23
+ *     distance: Scalar
 *     R: Scalar
 *     epsilon: Scalar
 *
 * Outputs:
 *     innov_var: Scalar
- *     H: Matrix24_1
+ *     H: Matrix23_1
 */
 template <typename Scalar>
-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
+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

  // Input arrays

-  // 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);
+  // 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);
  const Scalar _tmp18 =
-      (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));
+      -_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));

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

-    _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);
+    _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);
  }

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

    _h.setZero();

-    _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;
+    _h(0, 0) = _tmp15;
+    _h(1, 0) = _tmp18;
+    _h(2, 0) = _tmp17;
+    _h(3, 0) = -_tmp1;
+    _h(4, 0) = -_tmp19;
+    _h(5, 0) = -_tmp20;
  }
 }  // NOLINT(readability/fn_size)

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

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

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

    _h.setZero();

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

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

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

    _hx.setZero();

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

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

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

    _hy.setZero();

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

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

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

    _hz.setZero();

@@ -1,43 +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: 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
@@ -1,43 +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: 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: Matrix25_1
- *     P: Matrix24_24
+ *     state: Matrix24_1
+ *     P: Matrix23_23
 *     R: Scalar
 *     epsilon: Scalar
 *
 * Outputs:
 *     pred: Scalar
 *     innov_var: Scalar
- *     H: Matrix24_1
+ *     H: Matrix23_1
 */
 template <typename Scalar>
-void ComputeMagDeclinationPredInnovVarAndH(const matrix::Matrix<Scalar, 25, 1>& state,
-                                           const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
+void ComputeMagDeclinationPredInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
+                                           const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
                                           const Scalar epsilon, Scalar* const pred = nullptr,
                                           Scalar* const innov_var = nullptr,
-                                           matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
+                                           matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
  // Total ops: 22

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

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

    _h.setZero();

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

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

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

    _hx.setZero();

--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
Daniel Agar	153b683a0e	module.h fix trivial whitespace astyle failure	2024-06-20 15:50:25 -04:00
Thomas Debrunner	eba9276e6f	system: never call task_delete on nuttx, never force stop tasks	2024-06-20 13:31:51 +02:00