TEST: upgrade cache GitHub action to v4

* reset position by fusion

* handle local_pos_valid for fixed wing in gnss denied

* [WIP] ekf2: setEkfGlobalOrigin respect current height reference and vertical position aiding

* global origin, also reset vertical pos without gps as ref

* fix wo gnss, that bitcraze ci passes

* revert some changes as requested

* remove duplicate reset messages

* undo unrelated whitespace changes, I'll fix it everywhere in a followup

* [SQUASH] ekf2: add vehicle_command_ack

* resetGlobalPosToExternalObservation consolidate logic

* remove gnss check from local_pos validation check

* reset when  0<accuracy<1, otherwise fuse

* replace gps param with flag

* ekf2: dead reckon time exceeded, respect ZUPT preflight if air data or optical flow expected

* subtract timeout from last inertial dead-reck, change fake pos conditions, save flash

---------

Co-authored-by: Daniel Agar <daniel@agar.ca>

.github/workflows/mavros_mission_tests.yml

@@ -4,6 +4,7 @@ on:
   push:
     branches:
     - 'main'
+    - '*'
   pull_request:
     branches:
     - '*'
@@ -25,7 +26,7 @@ jobs:
           #- {vehicle: "tiltrotor",     mission: "VTOL_mission_1",  build_type: "RelWithDebInfo"}
 
     container:
-      image: px4io/px4-dev-ros-melodic:2021-09-08
+      image: px4io/px4-dev-ros-melodic:2024-05-18
       options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined
     steps:
     - uses: actions/checkout@v1
@@ -39,7 +40,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@v2
+      uses: actions/cache@v4
       with:
         path: ~/.ccache
         key: sitl_tests-${{matrix.config.build_type}}-ccache-${{steps.ccache_cache_timestamp.outputs.timestamp}}

ROMFS/px4fmu_common/init.d-posix/rcS

@@ -157,6 +157,7 @@ param set-default CBRK_SUPPLY_CHK 894281
 
 # disable check, no CPU load reported on posix yet
 param set-default COM_CPU_MAX -1
+param set-default COM_RAM_MAX -1
 
 # Don't require RC calibration and configuration
 param set-default COM_RC_IN_MODE 1

ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu

@@ -7,6 +7,8 @@
 #
 # @maintainer
 # @board px4_fmu-v2 exclude
+# @board px4_fmu-v5x exclude
+# @board px4_fmu-v6x exclude
 #
 
 . ${R}etc/init.d/rc.mc_defaults
@@ -36,7 +38,6 @@ param set-default COM_RC_LOSS_T 3
 
 
 # ekf2
-param set-default EKF2_TERR_MASK 1
 param set-default EKF2_BARO_NOISE 2
 
 param set-default EKF2_BCOEF_X 31.5

ROMFS/px4fmu_common/init.d/airframes/4601_droneblocks_dexi_5

@@ -25,7 +25,6 @@
 param set-default BAT1_CAPACITY 4000
 param set-default BAT1_N_CELLS 6
 param set-default BAT1_V_EMPTY 3.3
-param set-default BAT1_V_LOAD_DROP 0.5
 param set-default BAT_AVRG_CURRENT 13
 
 # Square quadrotor X PX4 numbering

ROMFS/px4fmu_common/init.d/airframes/4901_crazyflie21

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

ROMFS/px4fmu_common/init.d/rcS

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

Tools/ecl_ekf/plotting/pdf_report.py

@@ -223,7 +223,7 @@ def create_pdf_report(ulog: ULog, multi_instance: int, output_plot_filename: str
         data_plot.save()
         data_plot.close()
 
-        # plot innovation_check_flags summary
+        # plot innovation flags summary
         data_plot = CheckFlagsPlot(
             status_flags_time, estimator_status_flags, [['reject_hor_vel', 'reject_hor_pos'], ['reject_ver_vel', 'reject_ver_pos',
                                                                                'reject_hagl'],

Tools/px4airframes/rcout.py

@@ -79,12 +79,6 @@ class RCOutput():
         result += "then\n"
         result += "\techo \"Loading airframe: /etc/init.d/airframes/${AIRFRAME}\"\n"
         result += "\t. /etc/init.d/airframes/${AIRFRAME}\n"
-        if not post_start:
-            result += "else\n"
-            result += "\techo \"ERROR [init] No file matches SYS_AUTOSTART value found in : /etc/init.d/airframes\"\n"
-            # Reset the configuration
-            result += "\tparam set SYS_AUTOSTART 0\n"
-            result += "\ttone_alarm ${TUNE_ERR}\n"
         result += "fi\n"
         result += "unset AIRFRAME"
         self.output = result

Tools/setup/requirements.txt

@@ -7,7 +7,7 @@ jinja2>=2.8
 jsonschema
 kconfiglib
 lxml
-matplotlib>=3.0.*
+matplotlib>=3.0
 numpy>=1.13
 nunavut>=1.1.0
 packaging

boards/ark/pi6x/default.px4board

@@ -10,6 +10,7 @@ CONFIG_DRIVERS_BAROMETER_BMP388=y
 CONFIG_DRIVERS_CDCACM_AUTOSTART=y
 CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
 CONFIG_DRIVERS_DISTANCE_SENSOR_BROADCOM_AFBRS50=y
+CONFIG_DRIVERS_DISTANCE_SENSOR_VL53L0X=y
 CONFIG_DRIVERS_DSHOT=y
 CONFIG_DRIVERS_GPS=y
 CONFIG_DRIVERS_HEATER=y

boards/modalai/voxl2-slpi/src/drivers/dsp_hitl/dsp_hitl.cpp

@@ -1148,7 +1148,6 @@ handle_message_hil_sensor_dsp(mavlink_message_t *msg)
 
 		hil_battery_status.timestamp = gyro_accel_time;
 		hil_battery_status.voltage_v = 16.0f;
-		hil_battery_status.voltage_filtered_v = 16.0f;
 		hil_battery_status.current_a = 10.0f;
 		hil_battery_status.discharged_mah = -1.0f;
 		hil_battery_status.connected = true;

boards/px4/fmu-v3/default.px4board

@@ -55,7 +55,7 @@ CONFIG_MODULES_FW_POS_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
-CONFIG_MODULES_GYRO_FFT=y
+CONFIG_MODULES_GYRO_FFT=n
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y

boards/px4/fmu-v6xrt/nuttx-config/scripts/itcm_functions_includes.ld

@@ -154,7 +154,6 @@
 *(.text._ZN3Ekf20updateIMUBiasInhibitERKN9estimator9imuSampleE)
 *(.text._ZN9Commander13dataLinkCheckEv)
 *(.text._ZN17FlightModeManager10switchTaskE15FlightTaskIndex)
-*(.text._ZNK3Ekf26get_innovation_test_statusERtRfS1_S1_S1_S1_S1_S1_)
 *(.text._ZN12PX4Gyroscope9set_scaleEf)
 *(.text._ZN12FailsafeBase6updateERKyRKNS_5StateEbbRK16failsafe_flags_s)
 *(.text._ZN18MavlinkStreamDebug4sendEv)

boards/px4/fmu-v6xrt/src/mtd.cpp

@@ -53,18 +53,12 @@ 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 = 2,
+	.npart = 1,
 	.partd = {
 		{
 			.type = MTD_PARAMETERS,
 			.path = "/fs/mtd_params",
-			.nblocks = 32
-		},
-		{
-			.type = MTD_WAYPOINTS,
-			.path = "/fs/mtd_waypoints",
-			.nblocks = 32
-
+			.nblocks = 256
 		}
 	},
 };

msg/BatteryStatus.msg

@@ -1,9 +1,7 @@
 uint64 timestamp			# time since system start (microseconds)
 bool connected				# Whether or not a battery is connected, based on a voltage threshold
 float32 voltage_v			# Battery voltage in volts, 0 if unknown
-float32 voltage_filtered_v	# Battery voltage in volts, filtered, 0 if unknown
 float32 current_a			# Battery current in amperes, -1 if unknown
-float32 current_filtered_a	# Battery current in amperes, filtered, 0 if unknown
 float32 current_average_a	# Battery current average in amperes (for FW average in level flight), -1 if unknown
 float32 discharged_mah		# Discharged amount in mAh, -1 if unknown
 float32 remaining			# From 1 to 0, -1 if unknown
@@ -68,11 +66,15 @@ 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

msg/EstimatorAidSource1d.msg

@@ -25,4 +25,3 @@ bool fused                   # true if the sample was successfully fused
 # TOPICS estimator_aid_src_airspeed estimator_aid_src_sideslip
 # TOPICS estimator_aid_src_fake_hgt
 # TOPICS estimator_aid_src_gnss_yaw estimator_aid_src_ev_yaw
-# TOPICS estimator_aid_src_terrain_range_finder

msg/EstimatorAidSource2d.msg

@@ -22,5 +22,5 @@ bool innovation_rejected        # true if the observation has been rejected
 bool fused                      # true if the sample was successfully fused
 
 # TOPICS estimator_aid_src_ev_pos estimator_aid_src_fake_pos estimator_aid_src_gnss_pos estimator_aid_src_aux_global_position
-# TOPICS estimator_aid_src_aux_vel estimator_aid_src_optical_flow estimator_aid_src_terrain_optical_flow
+# TOPICS estimator_aid_src_aux_vel estimator_aid_src_optical_flow
 # TOPICS estimator_aid_src_drag

msg/EstimatorBias.msg

@@ -9,4 +9,4 @@ float32 innov			# innovation of the last measurement fusion (m)
 float32 innov_var		# innovation variance of the last measurement fusion (m^2)
 float32 innov_test_ratio	# normalized innovation squared test ratio
 
-# TOPICS estimator_baro_bias estimator_gnss_hgt_bias estimator_rng_hgt_bias
+# TOPICS estimator_baro_bias estimator_gnss_hgt_bias

msg/EstimatorInnovations.msg

@@ -22,7 +22,6 @@ float32[2] aux_hvel	# horizontal auxiliary velocity innovation from landing targ
 
 # Optical flow
 float32[2] flow		# flow innvoation (rad/sec) and innovation variance ((rad/sec)**2)
-float32[2] terr_flow	# flow innvoation (rad/sec) and innovation variance computed by the terrain estimator ((rad/sec)**2)
 
 # Various
 float32 heading		# heading innovation (rad) and innovation variance (rad**2)

msg/EstimatorStates.msg

@@ -1,7 +1,7 @@
 uint64 timestamp		# time since system start (microseconds)
 uint64 timestamp_sample         # the timestamp of the raw data (microseconds)
 
-float32[24] states		# Internal filter states
+float32[25] states		# Internal filter states
 uint8 n_states		# Number of states effectively used
 
-float32[23] covariances	# Diagonal Elements of Covariance Matrix
+float32[24] covariances	# Diagonal Elements of Covariance Matrix

msg/EstimatorStatus.msg

@@ -69,27 +69,14 @@ uint32 filter_fault_flags	# Bitmask to indicate EKF internal faults
 
 float32 pos_horiz_accuracy # 1-Sigma estimated horizontal position accuracy relative to the estimators origin (m)
 float32 pos_vert_accuracy # 1-Sigma estimated vertical position accuracy relative to the estimators origin (m)
-uint16 innovation_check_flags # Bitmask to indicate pass/fail status of innovation consistency checks
-# 0 - true if velocity observations have been rejected
-# 1 - true if horizontal position observations have been rejected
-# 2 - true if true if vertical position observations have been rejected
-# 3 - true if the X magnetometer observation has been rejected
-# 4 - true if the Y magnetometer observation has been rejected
-# 5 - true if the Z magnetometer observation has been rejected
-# 6 - true if the yaw observation has been rejected
-# 7 - true if the airspeed observation has been rejected
-# 8 - true if the synthetic sideslip observation has been rejected
-# 9 - true if the height above ground observation has been rejected
-# 10 - true if the X optical flow observation has been rejected
-# 11 - true if the Y optical flow observation has been rejected
 
-float32 mag_test_ratio # ratio of the largest magnetometer innovation component to the innovation test limit
-float32 vel_test_ratio # ratio of the largest velocity innovation component to the innovation test limit
-float32 pos_test_ratio # ratio of the largest horizontal position innovation component to the innovation test limit
-float32 hgt_test_ratio # ratio of the vertical position innovation to the innovation test limit
-float32 tas_test_ratio # ratio of the true airspeed innovation to the innovation test limit
-float32 hagl_test_ratio # ratio of the height above ground innovation to the innovation test limit
-float32 beta_test_ratio # ratio of the synthetic sideslip innovation to the innovation test limit
+float32 mag_test_ratio # low-pass filtered ratio of the largest magnetometer innovation component to the innovation test limit
+float32 vel_test_ratio # low-pass filtered ratio of the largest velocity innovation component to the innovation test limit
+float32 pos_test_ratio # low-pass filtered ratio of the largest horizontal position innovation component to the innovation test limit
+float32 hgt_test_ratio # low-pass filtered ratio of the vertical position innovation to the innovation test limit
+float32 tas_test_ratio # low-pass filtered ratio of the true airspeed innovation to the innovation test limit
+float32 hagl_test_ratio # low-pass filtered ratio of the height above ground innovation to the innovation test limit
+float32 beta_test_ratio # low-pass filtered ratio of the synthetic sideslip innovation to the innovation test limit
 
 uint16 solution_status_flags # Bitmask indicating which filter kinematic state outputs are valid for flight control use.
 # 0 - True if the attitude estimate is good

platforms/common/include/px4_platform_common/module.h

@@ -239,10 +239,21 @@ public:
 					unlock_module();
 					px4_usleep(10000); // 10 ms
 					lock_module();
-					i++;
 
-					if (i % 500 == 0) {
-						PX4_INFO("Waiting for task to stop...");
+					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;
 					}
 				} while (_task_id != -1);
 

platforms/nuttx/src/px4/common/tasks.cpp

@@ -45,7 +45,6 @@
 #include <nuttx/kthread.h>
 
 #include <sys/wait.h>
-#include <syslog.h>
 #include <stdbool.h>
 #include <stdio.h>
 #include <stdlib.h>
@@ -89,8 +88,7 @@ int px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_
 
 int px4_task_delete(int pid)
 {
-	syslog(LOG_ERR, "Ignoring force task delete on NuttX\n");
-	return ERROR;
+	return task_delete(pid);
 }
 
 const char *px4_get_taskname(void)

src/drivers/batt_smbus/batt_smbus.cpp

@@ -116,13 +116,11 @@ void BATT_SMBUS::RunImpl()
 
 	// Convert millivolts to volts.
 	new_report.voltage_v = ((float)result) / 1000.0f;
-	new_report.voltage_filtered_v = new_report.voltage_v;
 
 	// Read current.
 	ret |= _interface->read_word(BATT_SMBUS_CURRENT, result);
 
 	new_report.current_a = (-1.0f * ((float)(*(int16_t *)&result)) / 1000.0f) * _c_mult;
-	new_report.current_filtered_a = new_report.current_a;
 
 	// Read average current.
 	ret |= _interface->read_word(BATT_SMBUS_AVERAGE_CURRENT, result);

src/drivers/cyphal/Actuators/EscClient.hpp

@@ -37,12 +37,11 @@
  * Client-side implementation of UDRAL specification ESC service
  *
  * Publishes the following Cyphal messages:
- *   reg.drone.service.actuator.common.sp.Value8.0.1
- *   reg.drone.service.common.Readiness.0.1
+ *   reg.udral.service.actuator.common.sp.Value31.0.1
+ *   reg.udral.service.common.Readiness.0.1
  *
  * Subscribes to the following Cyphal messages:
- *   reg.drone.service.actuator.common.Feedback.0.1
- *   reg.drone.service.actuator.common.Status.0.1
+ *   zubax.telega.CompactFeedback.0.1
  *
  * @author Pavel Kirienko <pavel.kirienko@gmail.com>
  * @author Jacob Crabill <jacob@flyvoly.com>
@@ -51,11 +50,13 @@
 #pragma once
 
 #include <lib/perf/perf_counter.h>
-#include <uORB/PublicationMulti.hpp>
+#include <uORB/Publication.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
@@ -63,16 +64,15 @@ using std::isfinite;
 #include <reg/udral/service/actuator/common/sp/Vector31_0_1.h>
 #include <reg/udral/service/common/Readiness_0_1.h>
 
-/// TODO: Allow derived class of Subscription at same time, to handle ESC Feedback/Status
-class UavcanEscController : public UavcanPublisher
+
+class ReadinessPublisher : public UavcanPublisher
 {
 public:
-	static constexpr int MAX_ACTUATORS = MixingOutput::MAX_ACTUATORS;
 
-	UavcanEscController(CanardHandle &handle, UavcanParamManager &pmgr) :
-		UavcanPublisher(handle, pmgr, "udral.", "esc") { };
+	ReadinessPublisher(CanardHandle &handle, UavcanParamManager &pmgr) :
+		UavcanPublisher(handle, pmgr, "udral.", "readiness") { };
 
-	~UavcanEscController() {};
+	~ReadinessPublisher() {};
 
 	void update() override
 	{
@@ -95,58 +95,18 @@ public:
 		if (hrt_absolute_time() > _previous_pub_time + READINESS_PUBLISH_PERIOD) {
 			publish_readiness();
 		}
-	};
+	}
 
-	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_;
+	static constexpr hrt_abstime READINESS_PUBLISH_PERIOD = 100000;
+	hrt_abstime _previous_pub_time = 0;
 
-			for (uint8_t i = 0; i < MAX_ACTUATORS; i++) {
-				if (i < num_outputs) {
-					msg_sp.value[i] = static_cast<float>(outputs[i]);
+	uORB::Subscription _armed_sub{ORB_ID(actuator_armed)};
+	actuator_armed_s _armed {};
 
-				} else {
-					// "unset" values published as NaN
-					msg_sp.value[i] = NAN;
-				}
-			}
+	uORB::Subscription _actuator_test_sub{ORB_ID(actuator_test)};
+	uint64_t _actuator_test_timestamp{0};
 
-			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);
+	CanardTransferID _arming_transfer_id;
 
 	void publish_readiness()
 	{
@@ -155,8 +115,7 @@ private:
 
 		size_t payload_size = reg_udral_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_;
 
-		// Only publish if we have a valid publication ID set
-		if (_port_id == 0) {
+		if (_port_id == 0 || _port_id == CANARD_PORT_ID_UNSET) {
 			return;
 		}
 
@@ -174,12 +133,12 @@ private:
 
 		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) + 1);
+		CanardPortID arming_pid = static_cast<CanardPortID>(static_cast<uint32_t>(_port_id));
 		const CanardTransferMetadata transfer_metadata = {
 			.priority       = CanardPriorityNominal,
 			.transfer_kind  = CanardTransferKindMessage,
-			.port_id        = arming_pid,                // This is the subject-ID.
-			.remote_node_id = CANARD_NODE_ID_UNSET,      // Messages cannot be unicast, so use UNSET.
+			.port_id        = arming_pid,
+			.remote_node_id = CANARD_NODE_ID_UNSET,
 			.transfer_id    = _arming_transfer_id,
 		};
 
@@ -187,25 +146,143 @@ private:
 				&payload_size);
 
 		if (result == 0) {
-			// 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.
+			++_arming_transfer_id;
 			result = _canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
 						       &transfer_metadata,
 						       payload_size,
 						       &arming_payload_buffer);
 		}
 	};
-
-	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;
+};
+
+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)};
 };

src/drivers/cyphal/Cyphal.cpp

@@ -62,6 +62,8 @@ 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),
@@ -125,7 +127,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, 64, CANARD_MTU_CAN_CLASSIC);
+		_instance = new CyphalNode(node_id, 512, CANARD_MTU_CAN_CLASSIC);
 	}
 
 	if (_instance == nullptr) {
@@ -188,6 +190,8 @@ void CyphalNode::Run()
 		// send uavcan::node::Heartbeat_1_0 @ 1 Hz
 		sendHeartbeat();
 
+		sendPortList();
+
 		// Check all publishers
 		_pub_manager.update();
 
@@ -359,10 +363,10 @@ void CyphalNode::sendHeartbeat()
 	if (hrt_elapsed_time(&_uavcan_node_heartbeat_last) >= 1_s) {
 
 		uavcan_node_Heartbeat_1_0 heartbeat{};
-		heartbeat.uptime = _uavcan_node_heartbeat_transfer_id; // TODO: use real uptime
+		const hrt_abstime now = hrt_absolute_time();
+		heartbeat.uptime = now / 1000000;
 		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 = {
@@ -392,6 +396,45 @@ 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)
 {

src/drivers/cyphal/Cyphal.hpp

@@ -137,6 +137,9 @@ 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

src/drivers/cyphal/ParamManager.cpp

@@ -56,6 +56,15 @@ 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;
 }
 
@@ -73,19 +82,36 @@ 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)
 {
-	if (id >= sizeof(_uavcan_params) / sizeof(UavcanParamBinder)) {
+	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 {
 		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;
 }
 

src/drivers/cyphal/ParamManager.hpp

@@ -103,6 +103,10 @@ typedef struct {
 	bool is_persistent {true};
 } UavcanParamBinder;
 
+typedef struct {
+	const char *name;
+	const char *value;
+} CyphalTypeRegister;
 
 class UavcanParamManager
 {
@@ -116,8 +120,9 @@ public:
 private:
 
 
-	const UavcanParamBinder _uavcan_params[13] {
+	const UavcanParamBinder _uavcan_params[22] {
 		{"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},
@@ -130,7 +135,28 @@ 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"},
+	};
 };

src/drivers/cyphal/PublicationManager.cpp

@@ -125,6 +125,15 @@ 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()
 {

src/drivers/cyphal/PublicationManager.hpp

@@ -67,7 +67,7 @@
 /* Preprocessor calculation of publisher count */
 
 #define UAVCAN_PUB_COUNT CONFIG_CYPHAL_GNSS_PUBLISHER + \
-	CONFIG_CYPHAL_ESC_CONTROLLER + \
+	2 * CONFIG_CYPHAL_ESC_CONTROLLER + \
 	CONFIG_CYPHAL_READINESS_PUBLISHER + \
 	CONFIG_CYPHAL_UORB_ACTUATOR_OUTPUTS_PUBLISHER + \
 	CONFIG_CYPHAL_UORB_SENSOR_GPS_PUBLISHER
@@ -79,6 +79,7 @@
 
 #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"
@@ -103,6 +104,7 @@ public:
 
 	UavcanPublisher *getPublisher(const char *subject_name);
 
+	void fillSubjectIdList(uavcan_node_port_SubjectIDList_0_1 &publishers_list);
 private:
 	void updateDynamicPublications();
 
@@ -131,6 +133,14 @@ private:
 			"udral.esc",
 			0
 		},
+		{
+			[](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanPublisher *
+			{
+				return new ReadinessPublisher(handle, pmgr);
+			},
+			"udral.readiness",
+			0
+		},
 #endif
 #if CONFIG_CYPHAL_READINESS_PUBLISHER
 		{

src/drivers/cyphal/Subscribers/legacy/LegacyBatteryInfo.hpp

@@ -76,8 +76,6 @@ public:
 
 		battery_status_s bat_status {0};
 		bat_status.timestamp = hrt_absolute_time();
-		bat_status.voltage_filtered_v = bat_info.voltage;
-		bat_status.current_filtered_a = bat_info.current;
 		bat_status.current_average_a = bat_info.average_power_10sec;
 		bat_status.remaining = bat_info.state_of_charge_pct / 100.0f;
 		bat_status.scale = -1;

src/drivers/cyphal/SubscriptionManager.cpp

@@ -158,3 +158,24 @@ 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();
+	}
+}

src/drivers/cyphal/SubscriptionManager.hpp

@@ -45,6 +45,10 @@
 #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
@@ -65,12 +69,15 @@
 
 #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"
 
@@ -100,6 +107,7 @@ public:
 	void subscribe();
 	void printInfo();
 	void updateParams();
+	void fillSubjectIdList(uavcan_node_port_SubjectIDList_0_1 &subscribers_list);
 
 private:
 	void updateDynamicSubscriptions();
@@ -130,6 +138,72 @@ 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 *

src/drivers/cyphal/parameters.c

@@ -162,6 +162,87 @@ 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.
  *

src/drivers/gnss/septentrio/module.h

@@ -56,7 +56,7 @@
 #endif
 
 #ifdef SEP_LOG_ERROR
-#define SEP_ERR(...)            {PX4_WARN(__VA_ARGS__);}
+#define SEP_ERR(...)            {PX4_ERR(__VA_ARGS__);}
 #else
 #define SEP_ERR(...)            {}
 #endif

src/drivers/gnss/septentrio/module.yaml

@@ -71,7 +71,7 @@ parameters:
         type: float
         decimal: 3
         default: 0
-        min: 0
+        min: -360
         max: 360
         unit: deg
         reboot_required: true
@@ -104,7 +104,8 @@ parameters:
         description:
           short: Log GPS communication data
           long: |
-            Dump raw communication data from and to the connected receiver to the log file.
+            Log raw communication between the driver and connected receivers.
+            For example, "To receiver" will log all commands and corrections sent by the driver to the receiver.
         type: enum
         default: 0
         min: 0

src/drivers/gnss/septentrio/sbf/messages.h

@@ -60,8 +60,8 @@ constexpr uint32_t k_dnu_u4_value {4294967295};
 constexpr uint32_t k_dnu_u4_bitfield {0};
 constexpr uint16_t k_dnu_u2_value {65535};
 constexpr uint16_t k_dnu_u2_bitfield {0};
-constexpr float k_dnu_f4_value {-2 * 10000000000};
-constexpr double k_dnu_f8_value {-2 * 10000000000};
+constexpr float k_dnu_f4_value {-2.0f * 10000000000.0f};
+constexpr double k_dnu_f8_value {-2.0 * 10000000000.0};
 
 /// Maximum size of all expected messages.
 /// This needs to be bigger than the maximum size of all declared SBF blocks so that `memcpy()` can safely copy from the decoding buffer using this value into messages.

src/drivers/gnss/septentrio/septentrio.cpp

@@ -55,6 +55,7 @@
 #include <px4_platform_common/defines.h>
 #include <px4_platform_common/log.h>
 #include <px4_platform_common/time.h>
+#include <lib/systemlib/mavlink_log.h>
 #include <uORB/topics/gps_inject_data.h>
 #include <uORB/topics/sensor_gps.h>
 
@@ -86,6 +87,11 @@ constexpr int k_max_receiver_read_timeout = 50;
 */
 constexpr size_t k_min_receiver_read_bytes = 32;
 
+/**
+ * The baud rate of Septentrio receivers with factory default configuration.
+*/
+constexpr uint32_t k_septentrio_receiver_default_baud_rate = 115200;
+
 constexpr uint8_t k_max_command_size            = 120;
 constexpr uint16_t k_timeout_5hz                = 500;
 constexpr uint32_t k_read_buffer_size           = 150;
@@ -134,18 +140,19 @@ constexpr const char *k_frequency_25_0hz = "msec40";
 constexpr const char *k_frequency_50_0hz = "msec20";
 
 px4::atomic<SeptentrioDriver *> SeptentrioDriver::_secondary_instance {nullptr};
+uint32_t SeptentrioDriver::k_supported_baud_rates[] {0, 38400, 57600, 115200, 230400, 460800, 500000, 576000, 921600, 1000000, 1500000};
+uint32_t SeptentrioDriver::k_default_baud_rate {230400};
+orb_advert_t SeptentrioDriver::k_mavlink_log_pub {nullptr};
 
 SeptentrioDriver::SeptentrioDriver(const char *device_path, Instance instance, uint32_t baud_rate) :
 	Device(MODULE_NAME),
 	_instance(instance),
-	_baud_rate(baud_rate)
+	_chosen_baud_rate(baud_rate)
 {
 	strncpy(_port, device_path, sizeof(_port) - 1);
 	// Enforce null termination.
 	_port[sizeof(_port) - 1] = '\0';
 
-	reset_gps_state_message();
-
 	int32_t enable_sat_info {0};
 	get_parameter("SEP_SAT_INFO", &enable_sat_info);
 
@@ -171,6 +178,10 @@ SeptentrioDriver::SeptentrioDriver(const char *device_path, Instance instance, u
 	get_parameter("SEP_STREAM_LOG", &receiver_stream_log);
 	_receiver_stream_log = receiver_stream_log;
 
+	if (_receiver_stream_log == _receiver_stream_main) {
+		mavlink_log_warning(&k_mavlink_log_pub, "Septentrio: Logging stream should be different from main stream");
+	}
+
 	int32_t automatic_configuration {true};
 	get_parameter("SEP_AUTO_CONFIG", &automatic_configuration);
 	_automatic_configuration = static_cast<bool>(automatic_configuration);
@@ -198,6 +209,8 @@ SeptentrioDriver::SeptentrioDriver(const char *device_path, Instance instance, u
 	}
 
 	set_device_type(DRV_GPS_DEVTYPE_SBF);
+
+	reset_gps_state_message();
 }
 
 SeptentrioDriver::~SeptentrioDriver()
@@ -240,15 +253,13 @@ int SeptentrioDriver::print_status()
 		break;
 	}
 
-	PX4_INFO("health: %s, port: %s, baud rate: %lu", is_healthy() ? "OK" : "NOT OK", _port, _baud_rate);
+	PX4_INFO("health: %s, port: %s, baud rate: %lu", is_healthy() ? "OK" : "NOT OK", _port, _uart.getBaudrate());
 	PX4_INFO("controller -> receiver data rate: %lu B/s", output_data_rate());
 	PX4_INFO("receiver -> controller data rate: %lu B/s", input_data_rate());
 	PX4_INFO("sat info: %s", (_message_satellite_info != nullptr) ? "enabled" : "disabled");
 
-	if (_message_gps_state.timestamp != 0) {
-
+	if (first_gps_uorb_message_created() && _state == State::ReceivingData) {
 		PX4_INFO("rate RTCM injection: %6.2f Hz", static_cast<double>(rtcm_injection_frequency()));
-
 		print_message(ORB_ID(sensor_gps), _message_gps_state);
 	}
 
@@ -267,7 +278,7 @@ void SeptentrioDriver::run()
 				_uart.setPort(_port);
 
 				if (_uart.open()) {
-					_state = State::ConfiguringDevice;
+					_state = State::DetectingBaudRate;
 
 				} else {
 					// Failed to open port, so wait a bit before trying again.
@@ -277,14 +288,42 @@ void SeptentrioDriver::run()
 				break;
 			}
 
+		case State::DetectingBaudRate: {
+				static uint32_t expected_baud_rates[] = {k_septentrio_receiver_default_baud_rate, 115200, 230400, 57600, 460800, 500000, 576000, 38400, 921600, 1000000, 1500000};
+				expected_baud_rates[0] = _chosen_baud_rate != 0 ? _chosen_baud_rate : k_septentrio_receiver_default_baud_rate;
+
+				if (detect_receiver_baud_rate(expected_baud_rates[_current_baud_rate_index], true)) {
+					if (set_baudrate(expected_baud_rates[_current_baud_rate_index]) == PX4_OK) {
+						_state = State::ConfiguringDevice;
+
+					} else {
+						SEP_ERR("Setting local baud rate failed");
+					}
+
+				} else {
+					_current_baud_rate_index++;
+
+					if (_current_baud_rate_index == sizeof(expected_baud_rates) / sizeof(expected_baud_rates[0])) {
+						_current_baud_rate_index = 0;
+					}
+				}
+
+				break;
+			}
+
 		case State::ConfiguringDevice: {
-				if (configure() == PX4_OK) {
-					SEP_INFO("Automatic configuration successful");
+				ConfigureResult result = configure();
+
+				if (!(static_cast<int32_t>(result) & static_cast<int32_t>(ConfigureResult::FailedCompletely))) {
+					if (static_cast<int32_t>(result) & static_cast<int32_t>(ConfigureResult::NoLogging)) {
+						mavlink_log_warning(&k_mavlink_log_pub, "Septentrio: Failed to configure receiver internal logging");
+					}
+
+					SEP_INFO("Automatic configuration finished");
 					_state = State::ReceivingData;
 
 				} else {
-					// Failed to configure device, so wait a bit before trying again.
-					px4_usleep(200000);
+					_state = State::DetectingBaudRate;
 				}
 
 				break;
@@ -296,7 +335,7 @@ void SeptentrioDriver::run()
 				receive_result = receive(k_timeout_5hz);
 
 				if (receive_result == -1) {
-					_state = State::ConfiguringDevice;
+					_state = State::DetectingBaudRate;
 				}
 
 				if (_message_satellite_info && (receive_result & 2)) {
@@ -385,19 +424,51 @@ SeptentrioDriver *SeptentrioDriver::instantiate(int argc, char *argv[])
 
 SeptentrioDriver *SeptentrioDriver::instantiate(int argc, char *argv[], Instance instance)
 {
-	ModuleArguments arguments{};
-	SeptentrioDriver *gps{nullptr};
+	ModuleArguments arguments {};
+	SeptentrioDriver *gps {nullptr};
 
 	if (parse_cli_arguments(argc, argv, arguments) == PX4_ERROR) {
 		return nullptr;
 	}
 
+	if (arguments.device_path_main && arguments.device_path_secondary
+	    && strcmp(arguments.device_path_main, arguments.device_path_secondary) == 0) {
+		mavlink_log_critical(&k_mavlink_log_pub, "Septentrio: Device paths must be different");
+		return nullptr;
+	}
+
+	bool valid_chosen_baud_rate {false};
+
+	for (uint8_t i = 0; i < sizeof(k_supported_baud_rates) / sizeof(k_supported_baud_rates[0]); i++) {
+		switch (instance) {
+		case Instance::Main:
+			if (arguments.baud_rate_main == static_cast<int>(k_supported_baud_rates[i])) {
+				valid_chosen_baud_rate = true;
+			}
+
+			break;
+
+		case Instance::Secondary:
+			if (arguments.baud_rate_secondary == static_cast<int>(k_supported_baud_rates[i])) {
+				valid_chosen_baud_rate = true;
+			}
+
+			break;
+		}
+	}
+
+	if (!valid_chosen_baud_rate) {
+		mavlink_log_critical(&k_mavlink_log_pub, "Septentrio: Baud rate %d is unsupported, falling back to default %lu",
+				     instance == Instance::Main ? arguments.baud_rate_main : arguments.baud_rate_secondary, k_default_baud_rate);
+	}
+
 	if (instance == Instance::Main) {
 		if (Serial::validatePort(arguments.device_path_main)) {
-			gps = new SeptentrioDriver(arguments.device_path_main, instance, arguments.baud_rate_main);
+			gps = new SeptentrioDriver(arguments.device_path_main, instance,
+						   valid_chosen_baud_rate ? arguments.baud_rate_main : k_default_baud_rate);
 
 		} else {
-			PX4_ERR("invalid device (-d) %s", arguments.device_path_main ? arguments.device_path_main : "");
+			PX4_ERR("Invalid device (-d) %s", arguments.device_path_main ? arguments.device_path_main : "");
 		}
 
 		if (gps && arguments.device_path_secondary) {
@@ -410,7 +481,8 @@ SeptentrioDriver *SeptentrioDriver::instantiate(int argc, char *argv[], Instance
 
 	} else {
 		if (Serial::validatePort(arguments.device_path_secondary)) {
-			gps = new SeptentrioDriver(arguments.device_path_secondary, instance, arguments.baud_rate_secondary);
+			gps = new SeptentrioDriver(arguments.device_path_secondary, instance,
+						   valid_chosen_baud_rate ? arguments.baud_rate_secondary : k_default_baud_rate);
 
 		} else {
 			PX4_ERR("Invalid secondary device (-e) %s", arguments.device_path_secondary ? arguments.device_path_secondary : "");
@@ -425,6 +497,7 @@ SeptentrioDriver *SeptentrioDriver::instantiate(int argc, char *argv[], Instance
 int SeptentrioDriver::custom_command(int argc, char *argv[])
 {
 	bool handled = false;
+	const char *failure_reason {"unknown command"};
 	SeptentrioDriver *driver_instance;
 
 	if (!is_running()) {
@@ -448,7 +521,7 @@ int SeptentrioDriver::custom_command(int argc, char *argv[])
 				type = ReceiverResetType::Cold;
 
 			} else {
-				print_usage("incorrect reset type");
+				failure_reason = "unknown reset type";
 			}
 
 			if (type != ReceiverResetType::None) {
@@ -457,11 +530,11 @@ int SeptentrioDriver::custom_command(int argc, char *argv[])
 			}
 
 		} else {
-			print_usage("incorrect usage of reset command");
+			failure_reason = "incorrect usage of reset command";
 		}
 	}
 
-	return (handled) ? 0 : print_usage("unknown command");
+	return handled ? 0 : print_usage(failure_reason);
 }
 
 int SeptentrioDriver::print_usage(const char *reason)
@@ -473,25 +546,30 @@ int SeptentrioDriver::print_usage(const char *reason)
 	PRINT_MODULE_DESCRIPTION(
 		R"DESCR_STR(
 ### Description
-GPS driver module that handles the communication with Septentrio devices and publishes the position via uORB.
-
-The module supports a secondary GPS device, specified via `-e` parameter. The position will be published on
-the second uORB topic instance. It can be used for logging and heading computation.
+Driver for Septentrio GNSS receivers.
+It can automatically configure them and make their output available for the rest of the system.
+A secondary receiver is supported for redundancy, logging and dual-receiver heading.
+Septentrio receiver baud rates from 57600 to 1500000 are supported.
+If others are used, the driver will use 230400 and give a warning.
 
 ### Examples
 
-Starting 2 GPS devices (main one on /dev/ttyS3, secondary on /dev/ttyS4)
+Use one receiver on port `/dev/ttyS0` and automatically configure it to use baud rate 230400:
+$ septentrio start -d /dev/ttyS0 -b 230400
+
+Use two receivers, the primary on port `/dev/ttyS3` and the secondary on `/dev/ttyS4`,
+detect baud rate automatically and preserve them:
 $ septentrio start -d /dev/ttyS3 -e /dev/ttyS4
 
-Initiate warm restart of GPS device
+Perform warm reset of the receivers:
 $ gps reset warm
 )DESCR_STR");
 	PRINT_MODULE_USAGE_NAME("septentrio", "driver");
 	PRINT_MODULE_USAGE_COMMAND("start");
-	PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, "<file:dev>", "Primary Septentrio receiver", false);
-	PRINT_MODULE_USAGE_PARAM_INT('b', 0, 57600, 1500000, "Primary baud rate", true);
-	PRINT_MODULE_USAGE_PARAM_STRING('e', nullptr, "<file:dev>", "Secondary Septentrio receiver", true);
-	PRINT_MODULE_USAGE_PARAM_INT('g', 0, 57600, 1500000, "Secondary baud rate", true);
+	PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, "<file:dev>", "Primary receiver port", false);
+	PRINT_MODULE_USAGE_PARAM_INT('b', 0, 57600, 1500000, "Primary receiver baud rate", true);
+	PRINT_MODULE_USAGE_PARAM_STRING('e', nullptr, "<file:dev>", "Secondary receiver port", true);
+	PRINT_MODULE_USAGE_PARAM_INT('g', 0, 57600, 1500000, "Secondary receiver baud rate", true);
 
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 	PRINT_MODULE_USAGE_COMMAND_DESCR("reset", "Reset connected receiver");
@@ -508,15 +586,15 @@ int SeptentrioDriver::reset(ReceiverResetType type)
 
 	switch (type) {
 	case ReceiverResetType::Hot:
-		res = send_message_and_wait_for_ack(k_command_reset_hot, k_receiver_ack_timeout);
+		res = send_message_and_wait_for_ack(k_command_reset_hot, k_receiver_ack_timeout_fast);
 		break;
 
 	case ReceiverResetType::Warm:
-		res = send_message_and_wait_for_ack(k_command_reset_warm, k_receiver_ack_timeout);
+		res = send_message_and_wait_for_ack(k_command_reset_warm, k_receiver_ack_timeout_fast);
 		break;
 
 	case ReceiverResetType::Cold:
-		res = send_message_and_wait_for_ack(k_command_reset_cold, k_receiver_ack_timeout);
+		res = send_message_and_wait_for_ack(k_command_reset_cold, k_receiver_ack_timeout_fast);
 		break;
 
 	default:
@@ -553,13 +631,13 @@ int SeptentrioDriver::parse_cli_arguments(int argc, char *argv[], ModuleArgument
 		switch (ch) {
 		case 'b':
 			if (px4_get_parameter_value(myoptarg, arguments.baud_rate_main) != 0) {
-				PX4_ERR("baud rate parsing failed");
+				PX4_ERR("Baud rate parsing failed");
 				return PX4_ERROR;
 			}
 			break;
 		case 'g':
 			if (px4_get_parameter_value(myoptarg, arguments.baud_rate_secondary) != 0) {
-				PX4_ERR("baud rate parsing failed");
+				PX4_ERR("Baud rate parsing failed");
 				return PX4_ERROR;
 			}
 			break;
@@ -637,42 +715,31 @@ void SeptentrioDriver::schedule_reset(ReceiverResetType reset_type)
 	}
 }
 
-uint32_t SeptentrioDriver::detect_receiver_baud_rate(bool forced_input) {
-	// So we can restore the port to its original state.
-	const uint32_t original_baud_rate = _uart.getBaudrate();
-
-	// Baud rates we expect the receiver to be running at.
-	uint32_t expected_baud_rates[] = {115200, 115200, 230400, 57600, 460800, 500000, 576000, 38400, 921600, 1000000, 1500000};
-	if (_baud_rate != 0) {
-		expected_baud_rates[0] = _baud_rate;
+bool SeptentrioDriver::detect_receiver_baud_rate(const uint32_t &baud_rate, bool forced_input) {
+	if (set_baudrate(baud_rate) != PX4_OK) {
+		return false;
 	}
 
-	for (uint i = 0; i < sizeof(expected_baud_rates)/sizeof(expected_baud_rates[0]); i++) {
-		if (set_baudrate(expected_baud_rates[i]) != PX4_OK) {
-			set_baudrate(original_baud_rate);
-			return 0;
-		}
-
-		if (forced_input) {
-			force_input();
-		}
-
-		if (send_message_and_wait_for_ack(k_command_ping, k_receiver_ack_timeout)) {
-			SEP_INFO("Detected baud rate: %lu", expected_baud_rates[i]);
-			set_baudrate(original_baud_rate);
-			return expected_baud_rates[i];
-		}
+	if (forced_input) {
+		force_input();
 	}
 
-	set_baudrate(original_baud_rate);
-	return 0;
+	// Make sure that any weird data is "flushed" in the receiver.
+	(void)send_message("\n");
+
+	if (send_message_and_wait_for_ack(k_command_ping, k_receiver_ack_timeout_fast)) {
+		SEP_INFO("Detected baud rate: %lu", baud_rate);
+		return true;
+	}
+
+	return false;
 }
 
 int SeptentrioDriver::detect_serial_port(char* const port_name) {
 	// Read buffer to get the COM port
 	char buf[k_read_buffer_size];
 	size_t buffer_offset = 0;   // The offset into the string where the next data should be read to.
-	hrt_abstime timeout_time = hrt_absolute_time() + 5 * 1000 * k_receiver_ack_timeout;
+	hrt_abstime timeout_time = hrt_absolute_time() + 5 * 1000 * k_receiver_ack_timeout_fast;
 	bool response_detected = false;
 
 	// Receiver prints prompt after a message.
@@ -682,7 +749,7 @@ int SeptentrioDriver::detect_serial_port(char* const port_name) {
 
 	do {
 		// Read at most the amount of available bytes in the buffer after the current offset, -1 because we need '\0' at the end for a valid string.
-		int read_result = read(reinterpret_cast<uint8_t *>(buf) + buffer_offset, sizeof(buf) - buffer_offset - 1, k_receiver_ack_timeout);
+		int read_result = read(reinterpret_cast<uint8_t *>(buf) + buffer_offset, sizeof(buf) - buffer_offset - 1, k_receiver_ack_timeout_fast);
 
 		if (read_result < 0) {
 			SEP_WARN("SBF read error");
@@ -734,132 +801,81 @@ int SeptentrioDriver::detect_serial_port(char* const port_name) {
 	}
 }
 
-int SeptentrioDriver::configure()
+SeptentrioDriver::ConfigureResult SeptentrioDriver::configure()
 {
 	char msg[k_max_command_size] {};
-
-	// Passively detect receiver baud rate.
-	uint32_t detected_receiver_baud_rate = detect_receiver_baud_rate(true);
-
-	if (detected_receiver_baud_rate == 0) {
-		SEP_INFO("CONFIG: failed baud detection");
-		return PX4_ERROR;
-	}
-
-	// Set same baud rate on our end.
-	if (set_baudrate(detected_receiver_baud_rate) != PX4_OK) {
-		SEP_INFO("CONFIG: failed local baud rate setting");
-		return PX4_ERROR;
-	}
-
 	char com_port[5] {};
+	ConfigureResult result {ConfigureResult::OK};
 
 	// Passively detect receiver port.
 	if (detect_serial_port(com_port) != PX4_OK) {
-		SEP_INFO("CONFIG: failed port detection");
-		return PX4_ERROR;
+		SEP_WARN("CONFIG: failed port detection");
+		return ConfigureResult::FailedCompletely;
 	}
 
 	// We should definitely match baud rates and detect used port, but don't do other configuration if not requested.
 	// This will force input on the receiver. That shouldn't be a problem as it's on our own connection.
 	if (!_automatic_configuration) {
-		return PX4_OK;
+		return ConfigureResult::OK;
 	}
 
 	// If user requested specific baud rate, set it now. Otherwise keep detected baud rate.
-	if (strstr(com_port, "COM") != nullptr && _baud_rate != 0) {
-		snprintf(msg, sizeof(msg), k_command_set_baud_rate, com_port, _baud_rate);
+	if (strstr(com_port, "COM") != nullptr && _chosen_baud_rate != 0) {
+		snprintf(msg, sizeof(msg), k_command_set_baud_rate, com_port, _chosen_baud_rate);
 
 		if (!send_message(msg)) {
-			SEP_INFO("CONFIG: baud rate command write error");
-			return PX4_ERROR;
+			SEP_WARN("CONFIG: baud rate command write error");
+			return ConfigureResult::FailedCompletely;
 		}
 
 		// When sending a command and setting the baud rate right after, the controller could send the command at the new baud rate.
 		// From testing this could take some time.
-		px4_usleep(1000000);
+		px4_usleep(2000000);
 
-		if (set_baudrate(_baud_rate) != PX4_OK) {
-			SEP_INFO("CONFIG: failed local baud rate setting");
-			return PX4_ERROR;
+		if (set_baudrate(_chosen_baud_rate) != PX4_OK) {
+			SEP_WARN("CONFIG: failed local baud rate setting");
+			return ConfigureResult::FailedCompletely;
 		}
 
-		if (!send_message_and_wait_for_ack(k_command_ping, k_receiver_ack_timeout)) {
-			SEP_INFO("CONFIG: failed remote baud rate setting");
-			return PX4_ERROR;
+		if (!send_message_and_wait_for_ack(k_command_ping, k_receiver_ack_timeout_fast)) {
+			SEP_WARN("CONFIG: failed remote baud rate setting");
+			return ConfigureResult::FailedCompletely;
 		}
-	} else {
-		_baud_rate = detected_receiver_baud_rate;
 	}
 
 	// Delete all sbf outputs on current COM port to remove clutter data
 	snprintf(msg, sizeof(msg), k_command_clear_sbf, _receiver_stream_main, com_port);
 
-	if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout)) {
-		SEP_INFO("CONFIG: failed delete output");
-		return PX4_ERROR; // connection and/or baudrate detection failed
+	if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_fast)) {
+		SEP_WARN("CONFIG: failed delete output");
+		return ConfigureResult::FailedCompletely; // connection and/or baudrate detection failed
 	}
 
-	// Define/inquire the type of data that the receiver should accept/send on a given connection descriptor
-	snprintf(msg, sizeof(msg), k_command_set_data_io, com_port, "SBF");
-
-	if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout)) {
-		return PX4_ERROR;
-	}
-
-	// Specify the offsets that the receiver applies to the computed attitude angles.
-	snprintf(msg, sizeof(msg), k_command_set_attitude_offset, (double)(_heading_offset * 180 / M_PI_F), (double)_pitch_offset);
-
-	if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout)) {
-		return PX4_ERROR;
-	}
-
-	snprintf(msg, sizeof(msg), k_command_set_dynamics, "high");
-	if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout)) {
-		return PX4_ERROR;
-	}
-
-	const char *sbf_frequency {k_frequency_10_0hz};
-	switch (_sbf_output_frequency) {
-	case SBFOutputFrequency::Hz5_0:
-		sbf_frequency = k_frequency_5_0hz;
-		break;
-	case SBFOutputFrequency::Hz10_0:
-		sbf_frequency = k_frequency_10_0hz;
-		break;
-	case SBFOutputFrequency::Hz20_0:
-		sbf_frequency = k_frequency_20_0hz;
-		break;
-	case SBFOutputFrequency::Hz25_0:
-		sbf_frequency = k_frequency_25_0hz;
-		break;
-	}
-
-	// Output a set of SBF blocks on a given connection at a regular interval.
-	snprintf(msg, sizeof(msg), k_command_sbf_output_pvt, _receiver_stream_main, com_port, sbf_frequency);
-	if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout)) {
-		SEP_INFO("Failed to configure SBF");
-		return PX4_ERROR;
-	}
-
-	if (_receiver_setup == ReceiverSetup::MovingBase) {
-		if (_instance == Instance::Secondary) {
-			snprintf(msg, sizeof(msg), k_command_set_data_io, com_port, "+RTCMv3");
-			if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout)) {
-				SEP_INFO("Failed to configure RTCM output");
-			}
-		} else {
-			snprintf(msg, sizeof(msg), k_command_set_gnss_attitude, k_gnss_attitude_source_moving_base);
-			if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout)) {
-				SEP_INFO("Failed to configure attitude source");
-			}
+	// Set up the satellites used in PVT computation.
+	if (_receiver_constellation_usage != static_cast<int32_t>(SatelliteUsage::Default)) {
+		char requested_satellites[40] {};
+		if (_receiver_constellation_usage & static_cast<int32_t>(SatelliteUsage::GPS)) {
+			strcat(requested_satellites, "GPS+");
 		}
-	} else {
-		snprintf(msg, sizeof(msg), k_command_set_gnss_attitude, k_gnss_attitude_source_multi_antenna);
-		// This fails on single-antenna receivers, which is fine. Therefore don't check for acknowledgement.
-		if (!send_message(msg)) {
-			SEP_INFO("Failed to configure attitude source");
-			return PX4_ERROR;
+		if (_receiver_constellation_usage & static_cast<int32_t>(SatelliteUsage::GLONASS)) {
+			strcat(requested_satellites, "GLONASS+");
+		}
+		if (_receiver_constellation_usage & static_cast<int32_t>(SatelliteUsage::Galileo)) {
+			strcat(requested_satellites, "GALILEO+");
+		}
+		if (_receiver_constellation_usage & static_cast<int32_t>(SatelliteUsage::SBAS)) {
+			strcat(requested_satellites, "SBAS+");
+		}
+		if (_receiver_constellation_usage & static_cast<int32_t>(SatelliteUsage::BeiDou)) {
+			strcat(requested_satellites, "BEIDOU+");
+		}
+		// Make sure to remove the trailing '+' if any.
+		requested_satellites[math::max(static_cast<int>(strlen(requested_satellites)) - 1, 0)] = '\0';
+		snprintf(msg, sizeof(msg), k_command_set_satellite_usage, requested_satellites);
+		// Use a longer timeout as the `setSatelliteUsage` command acknowledges a bit slower on mosaic-H-based receivers.
+		if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_slow)) {
+			SEP_WARN("CONFIG: Failed to configure constellation usage");
+			return ConfigureResult::FailedCompletely;
 		}
 	}
 
@@ -919,42 +935,77 @@ int SeptentrioDriver::configure()
 		}
 
 		snprintf(msg, sizeof(msg), k_command_set_sbf_output, _receiver_stream_log, "DSK1", _receiver_logging_overwrite ? "" : "+", level, frequency);
-		if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout)) {
-			SEP_ERR("Failed to configure logging");
-			return PX4_ERROR;
+		if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_fast)) {
+			result = static_cast<ConfigureResult>(static_cast<int32_t>(result) | static_cast<int32_t>(ConfigureResult::NoLogging));
 		}
 	} else if (_receiver_stream_log == _receiver_stream_main) {
-		SEP_WARN("Skipping logging setup: logging stream can't equal main stream");
+		result = static_cast<ConfigureResult>(static_cast<int32_t>(result) | static_cast<int32_t>(ConfigureResult::NoLogging));
 	}
 
-	// Set up the satellites used in PVT computation.
-	if (_receiver_constellation_usage != static_cast<int32_t>(SatelliteUsage::Default)) {
-		char requested_satellites[40] {};
-		if (_receiver_constellation_usage & static_cast<int32_t>(SatelliteUsage::GPS)) {
-			strcat(requested_satellites, "GPS+");
+	// Define/inquire the type of data that the receiver should accept/send on a given connection descriptor
+	snprintf(msg, sizeof(msg), k_command_set_data_io, com_port, "SBF");
+
+	if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_fast)) {
+		return ConfigureResult::FailedCompletely;
+	}
+
+	// Specify the offsets that the receiver applies to the computed attitude angles.
+	snprintf(msg, sizeof(msg), k_command_set_attitude_offset, static_cast<double>(_heading_offset), static_cast<double>(_pitch_offset));
+
+	if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_fast)) {
+		return ConfigureResult::FailedCompletely;
+	}
+
+	snprintf(msg, sizeof(msg), k_command_set_dynamics, "high");
+	if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_fast)) {
+		return ConfigureResult::FailedCompletely;
+	}
+
+	const char *sbf_frequency {k_frequency_10_0hz};
+	switch (_sbf_output_frequency) {
+	case SBFOutputFrequency::Hz5_0:
+		sbf_frequency = k_frequency_5_0hz;
+		break;
+	case SBFOutputFrequency::Hz10_0:
+		sbf_frequency = k_frequency_10_0hz;
+		break;
+	case SBFOutputFrequency::Hz20_0:
+		sbf_frequency = k_frequency_20_0hz;
+		break;
+	case SBFOutputFrequency::Hz25_0:
+		sbf_frequency = k_frequency_25_0hz;
+		break;
+	}
+
+	// Output a set of SBF blocks on a given connection at a regular interval.
+	snprintf(msg, sizeof(msg), k_command_sbf_output_pvt, _receiver_stream_main, com_port, sbf_frequency);
+	if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_fast)) {
+		SEP_WARN("CONFIG: Failed to configure SBF");
+		return ConfigureResult::FailedCompletely;
+	}
+
+	if (_receiver_setup == ReceiverSetup::MovingBase) {
+		if (_instance == Instance::Secondary) {
+			snprintf(msg, sizeof(msg), k_command_set_data_io, com_port, "+RTCMv3");
+			if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_fast)) {
+				SEP_WARN("CONFIG: Failed to configure RTCM output");
+			}
+		} else {
+			snprintf(msg, sizeof(msg), k_command_set_gnss_attitude, k_gnss_attitude_source_moving_base);
+			if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_fast)) {
+				SEP_WARN("CONFIG: Failed to configure attitude source");
+			}
 		}
-		if (_receiver_constellation_usage & static_cast<int32_t>(SatelliteUsage::GLONASS)) {
-			strcat(requested_satellites, "GLONASS+");
-		}
-		if (_receiver_constellation_usage & static_cast<int32_t>(SatelliteUsage::Galileo)) {
-			strcat(requested_satellites, "GALILEO+");
-		}
-		if (_receiver_constellation_usage & static_cast<int32_t>(SatelliteUsage::SBAS)) {
-			strcat(requested_satellites, "SBAS+");
-		}
-		if (_receiver_constellation_usage & static_cast<int32_t>(SatelliteUsage::BeiDou)) {
-			strcat(requested_satellites, "BEIDOU+");
-		}
-		// Make sure to remove the trailing '+' if any.
-		requested_satellites[math::max(static_cast<int>(strlen(requested_satellites)) - 1, 0)] = '\0';
-		snprintf(msg, sizeof(msg), k_command_set_satellite_usage, requested_satellites);
-		if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout)) {
-			SEP_ERR("Failed to configure constellation usage");
-			return PX4_ERROR;
+	} else {
+		snprintf(msg, sizeof(msg), k_command_set_gnss_attitude, k_gnss_attitude_source_multi_antenna);
+		// This fails on single-antenna receivers, which is fine. Therefore don't check for acknowledgement.
+		if (!send_message(msg)) {
+			SEP_WARN("CONFIG: Failed to configure attitude source");
+			return ConfigureResult::FailedCompletely;
 		}
 	}
 
-	return PX4_OK;
+	return result;
 }
 
 int SeptentrioDriver::parse_char(const uint8_t byte)
@@ -1035,36 +1086,37 @@ int SeptentrioDriver::process_message()
 			PVTGeodetic pvt_geodetic;
 
 			if (_sbf_decoder.parse(&header) == PX4_OK && _sbf_decoder.parse(&pvt_geodetic) == PX4_OK) {
-				if (pvt_geodetic.mode_type < ModeType::StandAlonePVT) {
+				switch (pvt_geodetic.mode_type) {
+				case ModeType::NoPVT:
 					_message_gps_state.fix_type = sensor_gps_s::FIX_TYPE_NONE;
-
-				} else if (pvt_geodetic.mode_type == ModeType::PVTWithSBAS) {
+					break;
+				case ModeType::PVTWithSBAS:
 					_message_gps_state.fix_type = sensor_gps_s::FIX_TYPE_RTCM_CODE_DIFFERENTIAL;
-
-				} else if (pvt_geodetic.mode_type == ModeType::RTKFloat || pvt_geodetic.mode_type == ModeType::MovingBaseRTKFloat) {
+					break;
+				case ModeType::RTKFloat:
+				case ModeType::MovingBaseRTKFloat:
 					_message_gps_state.fix_type = sensor_gps_s::FIX_TYPE_RTK_FLOAT;
-
-				} else if (pvt_geodetic.mode_type == ModeType::RTKFixed || pvt_geodetic.mode_type == ModeType::MovingBaseRTKFixed) {
+					break;
+				case ModeType::RTKFixed:
+				case ModeType::MovingBaseRTKFixed:
 					_message_gps_state.fix_type = sensor_gps_s::FIX_TYPE_RTK_FIXED;
-
-				} else {
+					break;
+				default:
 					_message_gps_state.fix_type = sensor_gps_s::FIX_TYPE_3D;
+					break;
 				}
 
-				// Check fix and error code
-				_message_gps_state.vel_ned_valid = _message_gps_state.fix_type > sensor_gps_s::FIX_TYPE_NONE && pvt_geodetic.error == Error::None;
-
 				// Check boundaries and invalidate GPS velocities
 				if (pvt_geodetic.vn <= k_dnu_f4_value || pvt_geodetic.ve <= k_dnu_f4_value || pvt_geodetic.vu <= k_dnu_f4_value) {
 					_message_gps_state.vel_ned_valid = false;
 				}
 
-				// Check boundaries and invalidate position
-				// We're not just checking for the do-not-use value but for any value beyond the specified max values
-				if (pvt_geodetic.latitude <= k_dnu_f8_value ||
-				    pvt_geodetic.longitude <= k_dnu_f8_value ||
-				    pvt_geodetic.height <= k_dnu_f8_value ||
-				    pvt_geodetic.undulation <= k_dnu_f4_value) {
+				if (pvt_geodetic.latitude > k_dnu_f8_value && pvt_geodetic.longitude > k_dnu_f8_value && pvt_geodetic.height > k_dnu_f8_value && pvt_geodetic.undulation > k_dnu_f4_value) {
+					_message_gps_state.latitude_deg = pvt_geodetic.latitude * M_RAD_TO_DEG;
+					_message_gps_state.longitude_deg = pvt_geodetic.longitude * M_RAD_TO_DEG;
+					_message_gps_state.altitude_msl_m = pvt_geodetic.height - static_cast<double>(pvt_geodetic.undulation);
+					_message_gps_state.altitude_ellipsoid_m = pvt_geodetic.height;
+				} else {
 					_message_gps_state.fix_type = sensor_gps_s::FIX_TYPE_NONE;
 				}
 
@@ -1082,23 +1134,22 @@ int SeptentrioDriver::process_message()
 					_message_gps_state.satellites_used = 0;
 				}
 
-				_message_gps_state.latitude_deg = pvt_geodetic.latitude * M_RAD_TO_DEG;
-				_message_gps_state.longitude_deg = pvt_geodetic.longitude * M_RAD_TO_DEG;
-				_message_gps_state.altitude_msl_m = pvt_geodetic.height - static_cast<double>(pvt_geodetic.undulation);
-				_message_gps_state.altitude_ellipsoid_m = pvt_geodetic.height;
-
 				/* H and V accuracy are reported in 2DRMS, but based off the u-blox reporting we expect RMS.
 				 * Divide by 100 from cm to m and in addition divide by 2 to get RMS. */
 				_message_gps_state.eph = static_cast<float>(pvt_geodetic.h_accuracy) / 200.0f;
 				_message_gps_state.epv = static_cast<float>(pvt_geodetic.v_accuracy) / 200.0f;
 
+				// Check fix and error code
+				_message_gps_state.vel_ned_valid = _message_gps_state.fix_type > sensor_gps_s::FIX_TYPE_NONE && pvt_geodetic.error == Error::None;
 				_message_gps_state.vel_n_m_s = pvt_geodetic.vn;
 				_message_gps_state.vel_e_m_s = pvt_geodetic.ve;
 				_message_gps_state.vel_d_m_s = -1.0f * pvt_geodetic.vu;
 				_message_gps_state.vel_m_s = sqrtf(_message_gps_state.vel_n_m_s * _message_gps_state.vel_n_m_s +
 							_message_gps_state.vel_e_m_s * _message_gps_state.vel_e_m_s);
 
-				_message_gps_state.cog_rad = pvt_geodetic.cog * M_DEG_TO_RAD_F;
+				if (pvt_geodetic.cog > k_dnu_f4_value) {
+					_message_gps_state.cog_rad = pvt_geodetic.cog * M_DEG_TO_RAD_F;
+				}
 				_message_gps_state.c_variance_rad = M_DEG_TO_RAD_F;
 
 				_message_gps_state.time_utc_usec = 0;
@@ -1178,14 +1229,8 @@ int SeptentrioDriver::process_message()
 			VelCovGeodetic vel_cov_geodetic;
 
 			if (_sbf_decoder.parse(&vel_cov_geodetic) == PX4_OK) {
-				_message_gps_state.s_variance_m_s = vel_cov_geodetic.cov_ve_ve;
-
-				if (_message_gps_state.s_variance_m_s < vel_cov_geodetic.cov_vn_vn) {
-					_message_gps_state.s_variance_m_s = vel_cov_geodetic.cov_vn_vn;
-				}
-
-				if (_message_gps_state.s_variance_m_s < vel_cov_geodetic.cov_vu_vu) {
-					_message_gps_state.s_variance_m_s = vel_cov_geodetic.cov_vu_vu;
+				if (vel_cov_geodetic.cov_ve_ve > k_dnu_f4_value && vel_cov_geodetic.cov_vn_vn > k_dnu_f4_value && vel_cov_geodetic.cov_vu_vu > k_dnu_f4_value) {
+					_message_gps_state.s_variance_m_s = math::max(math::max(vel_cov_geodetic.cov_ve_ve, vel_cov_geodetic.cov_vn_vn), vel_cov_geodetic.cov_vu_vu);
 				}
 			}
 
@@ -1203,10 +1248,11 @@ int SeptentrioDriver::process_message()
 
 			AttEuler att_euler;
 
-			if (_sbf_decoder.parse(&att_euler) &&
+			if (_sbf_decoder.parse(&att_euler) == PX4_OK &&
 			    !att_euler.error_not_requested &&
 			    att_euler.error_aux1 == Error::None &&
-			    att_euler.error_aux2 == Error::None) {
+			    att_euler.error_aux2 == Error::None &&
+			    att_euler.heading > k_dnu_f4_value) {
 				float heading = att_euler.heading * M_PI_F / 180.0f; // Range of degrees to range of radians in [0, 2PI].
 
 				// Ensure range is in [-PI, PI].
@@ -1230,7 +1276,8 @@ int SeptentrioDriver::process_message()
 			if (_sbf_decoder.parse(&att_cov_euler) == PX4_OK &&
 			    !att_cov_euler.error_not_requested &&
 			    att_cov_euler.error_aux1 == Error::None &&
-			    att_cov_euler.error_aux2 == Error::None) {
+			    att_cov_euler.error_aux2 == Error::None &&
+			    att_cov_euler.cov_headhead > k_dnu_f4_value) {
 				_message_gps_state.heading_accuracy = att_cov_euler.cov_headhead * M_PI_F / 180.0f; // Convert range of degrees to range of radians in [0, 2PI]
 			}
 
@@ -1289,13 +1336,16 @@ bool SeptentrioDriver::send_message_and_wait_for_ack(const char *msg, const int
 				return true;
 			} else if (expected_response[response_check_character] == buf[i]) {
 				++response_check_character;
+			} else if (buf[i] == '$') {
+				// Special case makes sure we don't miss start of new response if that happened to be the character we weren't expecting next (e.g., `$R: ge$R: gecm`)
+				response_check_character = 1;
 			} else {
 				response_check_character = 0;
 			}
 		}
 	} while (timeout_time > hrt_absolute_time());
 
-	PX4_DEBUG("Response: timeout");
+	SEP_WARN("Response: timeout");
 	return false;
 }
 
@@ -1492,10 +1542,6 @@ void SeptentrioDriver::publish()
 
 	_sensor_gps_pub.publish(_message_gps_state);
 
-	// Heading/yaw data can be updated at a lower rate than the other navigation data.
-	// The uORB message definition requires this data to be set to a NAN if no new valid data is available.
-	_message_gps_state.heading = NAN;
-
 	if (_message_gps_state.spoofing_state != _spoofing_state) {
 
 		if (_message_gps_state.spoofing_state > sensor_gps_s::SPOOFING_STATE_NONE) {
@@ -1523,6 +1569,11 @@ void SeptentrioDriver::publish_satellite_info()
 	}
 }
 
+bool SeptentrioDriver::first_gps_uorb_message_created() const
+{
+	return _message_gps_state.timestamp != 0;
+}
+
 void SeptentrioDriver::publish_rtcm_corrections(uint8_t *data, size_t len)
 {
 	gps_inject_data_s gps_inject_data{};
@@ -1668,13 +1719,11 @@ void SeptentrioDriver::set_clock(timespec rtc_gps_time)
 
 bool SeptentrioDriver::is_healthy() const
 {
-	if (_state == State::ReceivingData) {
-		if (!receiver_configuration_healthy()) {
-			return false;
-		}
+	if (_state == State::ReceivingData && receiver_configuration_healthy()) {
+		return true;
 	}
 
-	return true;
+	return false;
 }
 
 void SeptentrioDriver::reset_gps_state_message()

src/drivers/gnss/septentrio/septentrio.h

@@ -47,6 +47,7 @@
 #include <px4_platform_common/getopt.h>
 #include <px4_platform_common/module.h>
 #include <px4_platform_common/Serial.hpp>
+#include <uORB/uORB.h>
 #include <uORB/Publication.hpp>
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/SubscriptionMultiArray.hpp>
@@ -271,9 +272,20 @@ public:
 	 * @return `PX4_OK` on success, `PX4_ERROR` otherwise
 	 */
 	int force_input();
+
+	/**
+	 * Standard baud rates the driver can be started with. `0` means the driver matches baud rates but does not change them.
+	 */
+	static uint32_t k_supported_baud_rates[];
+
+	/**
+	 * Default baud rate, used when the user requested an invalid baud rate.
+	 */
+	static uint32_t k_default_baud_rate;
 private:
 	enum class State {
 		OpeningSerialPort,
+		DetectingBaudRate,
 		ConfiguringDevice,
 		ReceivingData,
 	};
@@ -295,9 +307,24 @@ private:
 	};
 
 	/**
-	 * Maximum timeout to wait for command acknowledgement by the receiver.
+	 * The result of trying to configure the receiver.
+	 */
+	enum class ConfigureResult : int32_t {
+		OK               = 0,
+		FailedCompletely = 1 << 0,
+		NoLogging        = 1 << 1,
+	};
+
+	/**
+	 * Maximum timeout to wait for fast command acknowledgement by the receiver.
 	*/
-	static constexpr uint16_t k_receiver_ack_timeout = 200;
+	static constexpr uint16_t k_receiver_ack_timeout_fast = 200;
+
+	/**
+	 * Maximum timeout to wait for slow command acknowledgement by the receiver.
+	 * Might be the case for commands that send more output back as reply.
+	 */
+	static constexpr uint16_t k_receiver_ack_timeout_slow = 400;
 
 	/**
 	 * Duration of one update monitoring interval in us.
@@ -306,6 +333,11 @@ private:
 	*/
 	static constexpr hrt_abstime k_update_monitoring_interval_duration = 5_s;
 
+	/**
+	 * uORB type to send messages to ground control stations.
+	 */
+	static orb_advert_t k_mavlink_log_pub;
+
 	/**
 	 * The default stream for output of PVT data.
 	*/
@@ -347,13 +379,15 @@ private:
 	void schedule_reset(ReceiverResetType type);
 
 	/**
-	 * @brief Detect the current baud rate used by the receiver on the connected port.
+	 * @brief Detect whether the receiver is running at the given baud rate.
+	 * Does not preserve local baud rate!
 	 *
-	 * @param force_input Choose whether the receiver forces input on the port
+	 * @param baud_rate The baud rate to check.
+	 * @param force_input Choose whether the receiver forces input on the port.
 	 *
-	 * @return The detected baud rate on success, or `0` on error
+	 * @return `true` if running at the baud rate, or `false` on error.
 	 */
-	uint32_t detect_receiver_baud_rate(bool forced_input);
+	bool detect_receiver_baud_rate(const uint32_t &baud_rate, bool forced_input);
 
 	/**
 	 * @brief Try to detect the serial port used on the receiver side.
@@ -369,9 +403,9 @@ private:
 	 *
 	 * If the user has disabled automatic configuration, only execute the steps that do not touch the receiver (e.g., baud rate detection, port detection...).
 	 *
-	 * @return `PX4_OK` on success, `PX4_ERROR` otherwise.
+	 * @return `ConfigureResult::OK` if configured, or error.
 	 */
-	int configure();
+	ConfigureResult configure();
 
 	/**
 	 * @brief Parse the next byte of a received message from the receiver.
@@ -505,6 +539,13 @@ private:
 	 */
 	void publish_satellite_info();
 
+	/**
+	 * @brief Check whether the driver has created its first complete `SensorGPS` uORB message.
+	 *
+	 * @return `true` if the driver has created its first complete `SensorGPS` uORB message, `false` if still waiting.
+	 */
+	bool first_gps_uorb_message_created() const;
+
 	/**
 	 * @brief Publish RTCM corrections.
 	 *
@@ -579,6 +620,9 @@ private:
 	/**
 	 * @brief Check whether the current receiver configuration is likely healthy.
 	 *
+	 * This is checked by passively monitoring output from the receiver and validating whether it is what is
+	 * expected.
+	 *
 	 * @return `true` if the receiver is operating correctly, `false` if it needs to be reconfigured.
 	*/
 	bool receiver_configuration_healthy() const;
@@ -611,6 +655,9 @@ private:
 	/**
 	 * @brief Check whether the driver is operating correctly.
 	 *
+	 * The driver is operating correctly when it has fully configured the receiver and is actively receiving all the
+	 * expected data.
+	 *
 	 * @return `true` if the driver is working as expected, `false` otherwise.
 	*/
 	bool is_healthy() const;
@@ -666,7 +713,7 @@ private:
 	uint8_t                                _spoofing_state {0};                                          ///< Receiver spoofing state
 	uint8_t                                _jamming_state {0};                                           ///< Receiver jamming state
 	const Instance                         _instance {Instance::Main};                                   ///< The receiver that this instance of the driver controls
-	uint32_t                               _baud_rate {0};
+	uint32_t                               _chosen_baud_rate {0};                                        ///< The baud rate requested by the user
 	static px4::atomic<SeptentrioDriver *> _secondary_instance;
 	hrt_abstime                            _sleep_end {0};                                               ///< End time for sleeping
 	State                                  _resume_state {State::OpeningSerialPort};                     ///< Resume state after sleep
@@ -683,6 +730,7 @@ private:
 	bool                                   _automatic_configuration {true};                              ///< Automatic configuration of the receiver given by the `SEP_AUTO_CONFIG` parameter
 	ReceiverSetup                          _receiver_setup {ReceiverSetup::Default};                     ///< Purpose of the receivers, given by the `SEP_HARDW_SETUP` parameter
 	int32_t                                _receiver_constellation_usage {0};                            ///< Constellation usage in PVT computation given by the `SEP_CONST_USAGE` parameter
+	uint8_t                                _current_baud_rate_index {0};                                 ///< Index of the current baud rate to check
 
 	// Decoding and parsing
 	DecodingStatus                                 _active_decoder {DecodingStatus::Searching}; ///< Currently active decoder that new data has to be fed into

src/drivers/osd/atxxxx/atxxxx.cpp

@@ -241,7 +241,7 @@ OSDatxxxx::add_battery_info(uint8_t pos_x, uint8_t pos_y)
 	int ret = PX4_OK;
 
 	// TODO: show battery symbol based on battery fill level
-	snprintf(buf, sizeof(buf), "%c%5.2f", OSD_SYMBOL_BATT_3, (double)_battery_voltage_filtered_v);
+	snprintf(buf, sizeof(buf), "%c%5.2f", OSD_SYMBOL_BATT_3, (double)_battery_voltage_v);
 	buf[sizeof(buf) - 1] = '\0';
 
 	for (int i = 0; buf[i] != '\0'; i++) {
@@ -330,7 +330,7 @@ OSDatxxxx::update_topics()
 		_battery_sub.copy(&battery);
 
 		if (battery.connected) {
-			_battery_voltage_filtered_v = battery.voltage_filtered_v;
+			_battery_voltage_v = battery.voltage_v;
 			_battery_discharge_mah = battery.discharged_mah;
 			_battery_valid = true;
 

src/drivers/osd/atxxxx/atxxxx.h

@@ -111,7 +111,7 @@ private:
 	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
 
 	// battery
-	float _battery_voltage_filtered_v{0.f};
+	float _battery_voltage_v{0.f};
 	float _battery_discharge_mah{0.f};
 	bool _battery_valid{false};
 

src/drivers/rc/crsf_rc/CrsfRc.cpp

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

src/drivers/rc_input/crsf_telemetry.cpp

@@ -81,8 +81,8 @@ bool CRSFTelemetry::send_battery()
 		return false;
 	}
 
-	uint16_t voltage = battery_status.voltage_filtered_v * 10;
-	uint16_t current = battery_status.current_filtered_a * 10;
+	uint16_t voltage = battery_status.voltage_v * 10;
+	uint16_t current = battery_status.current_a * 10;
 	int fuel = battery_status.discharged_mah;
 	uint8_t remaining = battery_status.remaining * 100;
 	return crsf_send_telemetry_battery(_uart_fd, voltage, current, fuel, remaining);

src/drivers/rc_input/ghst_telemetry.cpp

@@ -90,8 +90,8 @@ bool GHSTTelemetry::send_battery_status()
 	battery_status_s battery_status;
 
 	if (_battery_status_sub.update(&battery_status)) {
-		voltage_in_10mV = battery_status.voltage_filtered_v * FACTOR_VOLTS_TO_10MV;
-		current_in_10mA = battery_status.current_filtered_a * FACTOR_AMPS_TO_10MA;
+		voltage_in_10mV = battery_status.voltage_v * FACTOR_VOLTS_TO_10MV;
+		current_in_10mA = battery_status.current_a * FACTOR_AMPS_TO_10MA;
 		fuel_in_10mAh = battery_status.discharged_mah * FACTOR_MAH_TO_10MAH;
 		success = ghst_send_telemetry_battery_status(_uart_fd,
 				static_cast<uint16_t>(voltage_in_10mV),

src/drivers/smart_battery/batmon/batmon.cpp

@@ -146,13 +146,11 @@ void Batmon::RunImpl()
 
 	// Convert millivolts to volts.
 	new_report.voltage_v = ((float)result) / 1000.0f;
-	new_report.voltage_filtered_v = new_report.voltage_v;
 
 	// Read current.
 	ret |= _interface->read_word(BATT_SMBUS_CURRENT, result);
 
 	new_report.current_a = (-1.0f * ((float)(*(int16_t *)&result)) / 1000.0f);
-	new_report.current_filtered_a = new_report.current_a;
 
 	// Read average current.
 	ret |= _interface->read_word(BATT_SMBUS_AVERAGE_CURRENT, result);

src/drivers/tattu_can/TattuCan.cpp

@@ -115,9 +115,7 @@ void TattuCan::Run()
 		battery_status.state_of_health = static_cast<uint16_t>(tattu_message.health_status);
 
 		battery_status.voltage_v = static_cast<float>(tattu_message.voltage) / 1000.0f;
-		battery_status.voltage_filtered_v = static_cast<float>(tattu_message.voltage) / 1000.0f;
 		battery_status.current_a = static_cast<float>(tattu_message.current) / 1000.0f;
-		battery_status.current_filtered_a = static_cast<float>(tattu_message.current) / 1000.0f;
 		battery_status.remaining = static_cast<float>(tattu_message.remaining_percent) / 100.0f;
 		battery_status.temperature = static_cast<float>(tattu_message.temperature);
 		battery_status.capacity = tattu_message.standard_capacity;

src/drivers/uavcan/sensors/battery.cpp

@@ -104,9 +104,7 @@ UavcanBatteryBridge::battery_sub_cb(const uavcan::ReceivedDataStructure<uavcan::
 
 	_battery_status[instance].timestamp = hrt_absolute_time();
 	_battery_status[instance].voltage_v = msg.voltage;
-	_battery_status[instance].voltage_filtered_v = msg.voltage;
 	_battery_status[instance].current_a = msg.current;
-	_battery_status[instance].current_filtered_a = msg.current;
 	_battery_status[instance].current_average_a = msg.current;
 
 	if (_batt_update_mod[instance] == BatteryDataType::Raw) {

src/drivers/uavcan/sensors/gnss.cpp

@@ -436,7 +436,7 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure<FixType>
 	}
 
 	// If we haven't already done so, set the system clock using GPS data
-	if (valid_pos_cov && !_system_clock_set) {
+	if ((fix_type >= sensor_gps_s::FIX_TYPE_2D) && !_system_clock_set) {
 		timespec ts{};
 
 		// get the whole microseconds

src/lib/battery/battery.cpp

@@ -46,6 +46,7 @@
 #include <px4_platform_common/defines.h>
 
 using namespace time_literals;
+using namespace matrix;
 
 Battery::Battery(int index, ModuleParams *parent, const int sample_interval_us, const uint8_t source) :
 	ModuleParams(parent),
@@ -53,10 +54,9 @@ Battery::Battery(int index, ModuleParams *parent, const int sample_interval_us,
 	_source(source)
 {
 	const float expected_filter_dt = static_cast<float>(sample_interval_us) / 1_s;
-	_voltage_filter_v.setParameters(expected_filter_dt, 1.f);
-	_current_filter_a.setParameters(expected_filter_dt, .5f);
 	_current_average_filter_a.setParameters(expected_filter_dt, 50.f);
-	_throttle_filter.setParameters(expected_filter_dt, 1.f);
+	_ocv_filter_v.setParameters(expected_filter_dt, 1.f);
+	_cell_voltage_filter_v.setParameters(expected_filter_dt, 1.f);
 
 	if (index > 9 || index < 1) {
 		PX4_ERR("Battery index must be between 1 and 9 (inclusive). Received %d. Defaulting to 1.", index);
@@ -81,9 +81,6 @@ Battery::Battery(int index, ModuleParams *parent, const int sample_interval_us,
 	snprintf(param_name, sizeof(param_name), "BAT%d_CAPACITY", _index);
 	_param_handles.capacity = param_find(param_name);
 
-	snprintf(param_name, sizeof(param_name), "BAT%d_V_LOAD_DROP", _index);
-	_param_handles.v_load_drop = param_find(param_name);
-
 	snprintf(param_name, sizeof(param_name), "BAT%d_R_INTERNAL", _index);
 	_param_handles.r_internal = param_find(param_name);
 
@@ -97,29 +94,36 @@ Battery::Battery(int index, ModuleParams *parent, const int sample_interval_us,
 	_param_handles.bat_avrg_current = param_find("BAT_AVRG_CURRENT");
 
 	updateParams();
+
+	// Internal resistance estimation initializations
+	_RLS_est(0) = OCV_DEFAULT * _params.n_cells;
+	_RLS_est(1) = R_DEFAULT * _params.n_cells;
+	_estimation_covariance(0, 0) = OCV_COVARIANCE * _params.n_cells;
+	_estimation_covariance(0, 1) = 0.f;
+	_estimation_covariance(1, 0) = 0.f;
+	_estimation_covariance(1, 1) = R_COVARIANCE * _params.n_cells;
+	_estimation_covariance_norm = sqrtf(powf(_estimation_covariance(0, 0), 2.f) + 2.f * powf(_estimation_covariance(1, 0),
+					    2.f) + powf(_estimation_covariance(1, 1), 2.f));
 }
 
 void Battery::updateVoltage(const float voltage_v)
 {
 	_voltage_v = voltage_v;
-	_voltage_filter_v.update(voltage_v);
 }
 
 void Battery::updateCurrent(const float current_a)
 {
 	_current_a = current_a;
-	_current_filter_a.update(current_a);
 }
 
 void Battery::updateBatteryStatus(const hrt_abstime &timestamp)
 {
 	if (!_battery_initialized) {
-		_voltage_filter_v.reset(_voltage_v);
-		_current_filter_a.reset(_current_a);
+		resetInternalResistanceEstimation(_voltage_v, _current_a);
 	}
 
 	// Require minimum voltage otherwise override connected status
-	if (_voltage_filter_v.getState() < LITHIUM_BATTERY_RECOGNITION_VOLTAGE) {
+	if (_voltage_v < LITHIUM_BATTERY_RECOGNITION_VOLTAGE) {
 		_connected = false;
 	}
 
@@ -132,7 +136,7 @@ void Battery::updateBatteryStatus(const hrt_abstime &timestamp)
 
 	sumDischarged(timestamp, _current_a);
 	_state_of_charge_volt_based =
-		calculateStateOfChargeVoltageBased(_voltage_filter_v.getState(), _current_filter_a.getState());
+		calculateStateOfChargeVoltageBased(_voltage_v, _current_a);
 
 	if (!_external_state_of_charge) {
 		estimateStateOfCharge();
@@ -149,9 +153,7 @@ battery_status_s Battery::getBatteryStatus()
 {
 	battery_status_s battery_status{};
 	battery_status.voltage_v = _voltage_v;
-	battery_status.voltage_filtered_v = _voltage_filter_v.getState();
 	battery_status.current_a = _current_a;
-	battery_status.current_filtered_a = _current_filter_a.getState();
 	battery_status.current_average_a = _current_average_filter_a.getState();
 	battery_status.discharged_mah = _discharged_mah;
 	battery_status.remaining = _state_of_charge;
@@ -167,6 +169,14 @@ battery_status_s Battery::getBatteryStatus()
 	battery_status.warning = _warning;
 	battery_status.timestamp = hrt_absolute_time();
 	battery_status.faults = determineFaults();
+	battery_status.internal_resistance_estimate = _internal_resistance_estimate;
+	battery_status.ocv_estimate = _voltage_v + _internal_resistance_estimate * _params.n_cells * _current_a;
+	battery_status.ocv_estimate_filtered = _ocv_filter_v.getState();
+	battery_status.volt_based_soc_estimate = math::interpolate(_ocv_filter_v.getState() / _params.n_cells,
+			_params.v_empty, _params.v_charged, 0.f, 1.f);
+	battery_status.voltage_prediction = _voltage_prediction;
+	battery_status.prediction_error = _prediction_error;
+	battery_status.estimation_covariance_norm = _estimation_covariance_norm;
 	return battery_status;
 }
 
@@ -213,27 +223,69 @@ float Battery::calculateStateOfChargeVoltageBased(const float voltage_v, const f
 	// remaining battery capacity based on voltage
 	float cell_voltage = voltage_v / _params.n_cells;
 
-	// correct battery voltage locally for load drop to avoid estimation fluctuations
-	if (_params.r_internal >= 0.f && current_a > FLT_EPSILON) {
-		cell_voltage += _params.r_internal * current_a;
+	// correct battery voltage locally for load drop according to internal resistance and current
+	if (current_a > FLT_EPSILON) {
+		updateInternalResistanceEstimation(voltage_v, current_a);
 
-	} else {
-		vehicle_thrust_setpoint_s vehicle_thrust_setpoint{};
-		_vehicle_thrust_setpoint_0_sub.copy(&vehicle_thrust_setpoint);
-		const matrix::Vector3f thrust_setpoint = matrix::Vector3f(vehicle_thrust_setpoint.xyz);
-		const float throttle = thrust_setpoint.length();
+		if (_params.r_internal >= 0.f) { // Use user specified internal resistance value
+			cell_voltage += _params.r_internal * current_a;
 
-		_throttle_filter.update(throttle);
-
-		if (!_battery_initialized) {
-			_throttle_filter.reset(throttle);
+		} else { // Use estimated internal resistance value
+			cell_voltage += _internal_resistance_estimate * current_a;
 		}
 
-		// assume linear relation between throttle and voltage drop
-		cell_voltage += throttle * _params.v_load_drop;
 	}
 
-	return math::interpolate(cell_voltage, _params.v_empty, _params.v_charged, 0.f, 1.f);
+	_cell_voltage_filter_v.update(cell_voltage);
+	return math::interpolate(_cell_voltage_filter_v.getState(), _params.v_empty, _params.v_charged, 0.f, 1.f);
+}
+
+void Battery::updateInternalResistanceEstimation(const float voltage_v, const float current_a)
+{
+	Vector2f x{1, -current_a};
+	_voltage_prediction = (x.transpose() * _RLS_est)(0, 0);
+	_prediction_error = voltage_v - _voltage_prediction;
+	const Vector2f gamma = _estimation_covariance * x / (LAMBDA + (x.transpose() * _estimation_covariance * x)(0, 0));
+	const Vector2f RSL_est_temp = _RLS_est + gamma * _prediction_error;
+	const Matrix2f estimation_covariance_temp = (_estimation_covariance
+			- Matrix<float, 2, 1>(gamma) * (x.transpose() * _estimation_covariance)) / LAMBDA;
+	const float estimation_covariance_temp_norm =
+		sqrtf(powf(estimation_covariance_temp(0, 0), 2.f)
+		      + 2.f * powf(estimation_covariance_temp(1, 0), 2.f)
+		      + powf(estimation_covariance_temp(1, 1), 2.f));
+
+	if (estimation_covariance_temp_norm < _estimation_covariance_norm) { // Only update if estimation improves
+		_RLS_est = RSL_est_temp;
+		_estimation_covariance = estimation_covariance_temp;
+		_estimation_covariance_norm = estimation_covariance_temp_norm;
+		_internal_resistance_estimate =
+			math::max(_RLS_est(1) / _params.n_cells, 0.f); // Only use positive values
+
+	} else { // Update OCV estimate with IR estimate
+		_RLS_est(0) = voltage_v + _RLS_est(1) * current_a;
+	}
+
+	_ocv_filter_v.update(voltage_v + _internal_resistance_estimate * _params.n_cells * current_a);
+}
+
+void Battery::resetInternalResistanceEstimation(const float voltage_v, const float current_a)
+{
+	_RLS_est(0) = voltage_v;
+	_RLS_est(1) = R_DEFAULT * _params.n_cells;
+	_estimation_covariance.setZero();
+	_estimation_covariance(0, 0) = OCV_COVARIANCE * _params.n_cells;
+	_estimation_covariance(1, 1) = R_COVARIANCE * _params.n_cells;
+	_estimation_covariance_norm = sqrtf(powf(_estimation_covariance(0, 0), 2.f) + 2.f * powf(_estimation_covariance(1, 0),
+					    2.f) + powf(_estimation_covariance(1, 1), 2.f));
+	_internal_resistance_estimate = R_DEFAULT;
+	_ocv_filter_v.reset(voltage_v + _internal_resistance_estimate * _params.n_cells * current_a);
+
+	if (_params.r_internal >= 0.f) { // Use user specified internal resistance value
+		_cell_voltage_filter_v.reset(voltage_v / _params.n_cells + _params.r_internal * current_a);
+
+	} else { // Use estimated internal resistance value
+		_cell_voltage_filter_v.reset(voltage_v / _params.n_cells + _internal_resistance_estimate * current_a);
+	}
 }
 
 void Battery::estimateStateOfCharge()
@@ -354,7 +406,6 @@ void Battery::updateParams()
 	param_get(_param_handles.v_charged, &_params.v_charged);
 	param_get(_param_handles.n_cells, &_params.n_cells);
 	param_get(_param_handles.capacity, &_params.capacity);
-	param_get(_param_handles.v_load_drop, &_params.v_load_drop);
 	param_get(_param_handles.r_internal, &_params.r_internal);
 	param_get(_param_handles.source, &_params.source);
 	param_get(_param_handles.low_thr, &_params.low_thr);

src/lib/battery/battery.h

@@ -58,7 +58,6 @@
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/flight_phase_estimation.h>
 #include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/vehicle_thrust_setpoint.h>
 
 /**
  * BatteryBase is a base class for any type of battery.
@@ -118,7 +117,6 @@ protected:
 		param_t v_charged;
 		param_t n_cells;
 		param_t capacity;
-		param_t v_load_drop;
 		param_t r_internal;
 		param_t low_thr;
 		param_t crit_thr;
@@ -132,7 +130,6 @@ protected:
 		float v_charged;
 		int32_t  n_cells;
 		float capacity;
-		float v_load_drop;
 		float r_internal;
 		float low_thr;
 		float crit_thr;
@@ -155,7 +152,6 @@ private:
 	void computeScale();
 	float computeRemainingTime(float current_a);
 
-	uORB::Subscription _vehicle_thrust_setpoint_0_sub{ORB_ID(vehicle_thrust_setpoint)};
 	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
 	uORB::SubscriptionData<flight_phase_estimation_s> _flight_phase_estimation_sub{ORB_ID(flight_phase_estimation)};
 	uORB::PublicationMulti<battery_status_s> _battery_status_pub{ORB_ID(battery_status)};
@@ -167,12 +163,11 @@ private:
 	uint8_t _priority{0};
 	bool _battery_initialized{false};
 	float _voltage_v{0.f};
-	AlphaFilter<float> _voltage_filter_v;
+	AlphaFilter<float> _ocv_filter_v;
+	AlphaFilter<float> _cell_voltage_filter_v;
 	float _current_a{-1};
-	AlphaFilter<float> _current_filter_a;
 	AlphaFilter<float>
 	_current_average_filter_a; ///< averaging filter for current. For FW, it is the current in level flight.
-	AlphaFilter<float> _throttle_filter;
 	float _discharged_mah{0.f};
 	float _discharged_mah_loop{0.f};
 	float _state_of_charge_volt_based{-1.f}; // [0,1]
@@ -183,4 +178,19 @@ private:
 	bool _armed{false};
 	bool _vehicle_status_is_fw{false};
 	hrt_abstime _last_unconnected_timestamp{0};
+
+	// Internal Resistance estimation
+	void updateInternalResistanceEstimation(const float voltage_v, const float current_a);
+	void resetInternalResistanceEstimation(const float voltage_v, const float current_a);
+	matrix::Vector2f _RLS_est; // [Open circuit voltage estimate [V], Total internal resistance estimate [Ohm]]^T
+	matrix::Matrix2f _estimation_covariance;
+	float _estimation_covariance_norm{0.f};
+	float _internal_resistance_estimate{0.005f}; // [Ohm] Per cell estimate of the internal resistance
+	float _voltage_prediction{0.f}; // [V] Predicted voltage of the estimator
+	float _prediction_error{0.f}; // [V] Error between the predicted and measured voltage
+	static constexpr float LAMBDA = 0.95f; 	// [0, 1] Forgetting factor (Tuning parameter for the RLS algorithm)
+	static constexpr float R_DEFAULT = 0.005f; // [Ohm] Initial per cell estimate of the internal resistance
+	static constexpr float OCV_DEFAULT = 4.2f; // [V] Initial per cell estimate of the open circuit voltage
+	static constexpr float R_COVARIANCE = 0.1f; // Initial per cell covariance of the internal resistance
+	static constexpr float OCV_COVARIANCE = 1.5f; // Initial per cell covariance of the open circuit voltage
 };

src/lib/battery/int_res_est_replay.py

@@ -0,0 +1,208 @@
+# 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)

src/lib/battery/module.yaml

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

src/lib/drivers/smbus_sbs/SBS.hpp

@@ -261,13 +261,11 @@ int SMBUS_SBS_BaseClass<T>::populate_smbus_data(battery_status_s &data)
 
 	// Convert millivolts to volts.
 	data.voltage_v = ((float)result) * 0.001f;
-	data.voltage_filtered_v = data.voltage_v;
 
 	// Read current.
 	ret |= _interface->read_word(BATT_SMBUS_CURRENT, result);
 
 	data.current_a = (-1.0f * ((float)(*(int16_t *)&result)) * 0.001f);
-	data.current_filtered_a = data.current_a;
 
 	// Read remaining capacity.
 	ret |= _interface->read_word(BATT_SMBUS_RELATIVE_SOC, result);

src/lib/matrix/matrix/SquareMatrix.hpp

@@ -618,6 +618,7 @@ SquareMatrix <Type, M> choleskyInv(const SquareMatrix<Type, M> &A)
 	return L_inv.T() * L_inv;
 }
 
+using Matrix2f = SquareMatrix<float, 2>;
 using Matrix3f = SquareMatrix<float, 3>;
 using Matrix3d = SquareMatrix<double, 3>;
 

src/modules/commander/HealthAndArmingChecks/checks/cpuResourceCheck.cpp

@@ -43,7 +43,10 @@ CpuResourceChecks::CpuResourceChecks()
 
 void CpuResourceChecks::checkAndReport(const Context &context, Report &reporter)
 {
-	if (_param_com_cpu_max.get() < FLT_EPSILON) {
+	const bool cpu_load_check_enabled = _param_com_cpu_max.get() > FLT_EPSILON;
+	const bool ram_usage_check_enabled = _param_com_ram_max.get() > FLT_EPSILON;
+
+	if (!cpu_load_check_enabled && !ram_usage_check_enabled) {
 		return;
 	}
 
@@ -54,15 +57,15 @@ void CpuResourceChecks::checkAndReport(const Context &context, Report &reporter)
 		/* EVENT
 		 * @description
 		 * <profile name="dev">
-		 * If the system does not provide any CPU load information, use the parameter <param>COM_CPU_MAX</param>
-		 * to disable the check.
+		 * If the system does not provide any CPU and RAM load information, use the parameters <param>COM_CPU_MAX</param>
+		 * and <param>COM_RAM_MAX</param> to disable the checks.
 		 * </profile>
 		 */
 		reporter.healthFailure(NavModes::All, health_component_t::system, events::ID("check_missing_cpuload"),
-				       events::Log::Error, "No CPU load information");
+				       events::Log::Error, "No CPU and RAM load information");
 
 		if (reporter.mavlink_log_pub()) {
-			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: No CPU load information");
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: No CPU and RAM load information");
 		}
 
 	} else {
@@ -71,7 +74,7 @@ void CpuResourceChecks::checkAndReport(const Context &context, Report &reporter)
 		_high_cpu_load_hysteresis.set_state_and_update(high_cpu_load, hrt_absolute_time());
 
 		// fail check if CPU load is above the threshold for 2 seconds
-		if (_high_cpu_load_hysteresis.get_state()) {
+		if (cpu_load_check_enabled && _high_cpu_load_hysteresis.get_state()) {
 			/* EVENT
 			 * @description
 			 * The CPU load can be reduced for example by disabling unused modules (e.g. mavlink instances) or reducing the gyro update
@@ -88,5 +91,26 @@ void CpuResourceChecks::checkAndReport(const Context &context, Report &reporter)
 				mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: CPU load too high: %3.1f%%", (double)cpuload_percent);
 			}
 		}
+
+		const float ram_usage_percent = cpuload.ram_usage * 100.f;
+		const bool high_ram_usage = ram_usage_percent > _param_com_ram_max.get();
+
+		if (ram_usage_check_enabled && high_ram_usage) {
+			/* EVENT
+			 * @description
+			 * The RAM usage can be reduced for example by disabling unused modules (e.g. mavlink instances).
+			 *
+			 * <profile name="dev">
+			 * The threshold can be adjusted via <param>COM_RAM_MAX</param> parameter.
+			 * </profile>
+			 */
+			reporter.healthFailure<float>(NavModes::All, health_component_t::system, events::ID("check_ram_usage_too_high"),
+						      events::Log::Error, "RAM usage too high: {1:.1}%", ram_usage_percent);
+
+			if (reporter.mavlink_log_pub()) {
+				mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: RAM usage too high: %3.1f%%",
+						     (double)ram_usage_percent);
+			}
+		}
 	}
 }

src/modules/commander/HealthAndArmingChecks/checks/cpuResourceCheck.hpp

@@ -54,6 +54,7 @@ private:
 	systemlib::Hysteresis _high_cpu_load_hysteresis{false};
 
 	DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
-					(ParamFloat<px4::params::COM_CPU_MAX>) _param_com_cpu_max
+					(ParamFloat<px4::params::COM_CPU_MAX>) _param_com_cpu_max,
+					(ParamFloat<px4::params::COM_RAM_MAX>) _param_com_ram_max
 				       )
 };

src/modules/commander/commander_params.c

@@ -802,6 +802,21 @@ PARAM_DEFINE_FLOAT(COM_KILL_DISARM, 5.0f);
  */
 PARAM_DEFINE_FLOAT(COM_CPU_MAX, 95.0f);
 
+/**
+ * Maximum allowed RAM usage to pass checks
+ *
+ * The check fails if the RAM usage is above this threshold.
+ *
+ * A negative value disables the check.
+ *
+ * @group Commander
+ * @unit %
+ * @min -1
+ * @max 100
+ * @increment 1
+ */
+PARAM_DEFINE_FLOAT(COM_RAM_MAX, 95.0f);
+
 /**
  * Required number of redundant power modules
  *

src/modules/ekf2/CMakeLists.txt

@@ -203,6 +203,7 @@ if(CONFIG_EKF2_RANGE_FINDER)
 	list(APPEND EKF_SRCS
 		EKF/aid_sources/range_finder/range_finder_consistency_check.cpp
 		EKF/aid_sources/range_finder/range_height_control.cpp
+		EKF/aid_sources/range_finder/range_height_fusion.cpp
 		EKF/aid_sources/range_finder/sensor_range_finder.cpp
 	)
 endif()
@@ -212,7 +213,7 @@ if(CONFIG_EKF2_SIDESLIP)
 endif()
 
 if(CONFIG_EKF2_TERRAIN)
-	list(APPEND EKF_SRCS EKF/terrain_estimator/terrain_estimator.cpp)
+	list(APPEND EKF_SRCS EKF/terrain_control.cpp)
 endif()
 
 add_subdirectory(EKF)

src/modules/ekf2/EKF/CMakeLists.txt

@@ -125,6 +125,7 @@ if(CONFIG_EKF2_RANGE_FINDER)
 	list(APPEND EKF_SRCS
 		aid_sources/range_finder/range_finder_consistency_check.cpp
 		aid_sources/range_finder/range_height_control.cpp
+		aid_sources/range_finder/range_height_fusion.cpp
 		aid_sources/range_finder/sensor_range_finder.cpp
 	)
 endif()
@@ -134,7 +135,7 @@ if(CONFIG_EKF2_SIDESLIP)
 endif()
 
 if(CONFIG_EKF2_TERRAIN)
-	list(APPEND EKF_SRCS terrain_estimator/terrain_estimator.cpp)
+	list(APPEND EKF_SRCS terrain_control.cpp)
 endif()
 
 include_directories(${CMAKE_CURRENT_SOURCE_DIR})

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

@@ -42,13 +42,13 @@ ZeroVelocityUpdate::ZeroVelocityUpdate()
 
 void ZeroVelocityUpdate::reset()
 {
-	_time_last_zero_velocity_fuse = 0;
+	_time_last_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_zero_velocity_fuse + 200'000 < imu_delayed.time_us);
+	const bool zero_velocity_update_data_ready = (_time_last_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_zero_velocity_fuse = imu_delayed.time_us;
+			_time_last_fuse = imu_delayed.time_us;
 
 			return true;
 		}

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

@@ -45,9 +45,11 @@ 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_zero_velocity_fuse{0}; ///< last time of zero velocity update (uSec)
+	uint64_t _time_last_fuse{0}; ///< last time of zero velocity update (uSec)
 
 };
 

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

@@ -151,6 +151,8 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed
 #if defined(MODULE_NAME)
 		aid_src.timestamp = hrt_absolute_time();
 		_estimator_aid_src_aux_global_position_pub.publish(aid_src);
+
+		_test_ratio_filtered = math::max(fabsf(aid_src.test_ratio_filtered[0]), fabsf(aid_src.test_ratio_filtered[1]));
 #endif // MODULE_NAME
 
 	} else if ((_state != State::stopped) && isTimedOut(_time_last_buffer_push, imu_delayed.time_us, (uint64_t)5e6)) {

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

@@ -74,6 +74,8 @@ public:
 		updateParams();
 	}
 
+	float test_ratio_filtered() const { return _test_ratio_filtered; }
+
 private:
 	bool isTimedOut(uint64_t last_sensor_timestamp, uint64_t time_delayed_us, uint64_t timeout_period) const
 	{
@@ -106,6 +108,8 @@ private:
 
 	State _state{State::stopped};
 
+	float _test_ratio_filtered{INFINITY};
+
 #if defined(MODULE_NAME)
 	struct reset_counters_s {
 		uint8_t lat_lon{};

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

@@ -37,6 +37,9 @@
  */
 
 #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)
@@ -57,14 +60,16 @@ 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 vel{NAN, NAN, NAN};
-	Matrix3f vel_cov{};
+	Vector3f measurement{};
+	Vector3f measurement_var{};
+
+	float minimum_variance = math::max(sq(0.01f), sq(_params.ev_vel_noise));
 
 	switch (ev_sample.vel_frame) {
 	case VelocityFrame::LOCAL_FRAME_NED:
 		if (_control_status.flags.yaw_align) {
-			vel = ev_sample.vel - vel_offset_earth;
-			vel_cov = matrix::diag(ev_sample.velocity_var);
+			measurement = ev_sample.vel - vel_offset_earth;
+			measurement_var = ev_sample.velocity_var;
 
 		} else {
 			continuing_conditions_passing = false;
@@ -75,31 +80,28 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common
 	case VelocityFrame::LOCAL_FRAME_FRD:
 		if (_control_status.flags.ev_yaw) {
 			// using EV frame
-			vel = ev_sample.vel - vel_offset_earth;
-			vel_cov = matrix::diag(ev_sample.velocity_var);
+			measurement = ev_sample.vel - vel_offset_earth;
+			measurement_var = ev_sample.velocity_var;
 
 		} else {
 			// rotate EV to the EKF reference frame
 			const Dcmf R_ev_to_ekf = Dcmf(_ev_q_error_filt.getState());
 
-			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);
-			}
+			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());
 		}
 
 		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;
+	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;
+		}
 
 	default:
 		continuing_conditions_passing = false;
@@ -111,48 +113,56 @@ 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++) {
-			vel_cov(i, i) = math::max(vel_cov(i, i), sq(_params.gps_vel_noise));
+			measurement_var(i) = math::max(measurement_var(i), sq(_params.gps_vel_noise));
 		}
 	}
 
 #endif // CONFIG_EKF2_GNSS
 
-	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))
+	measurement_var = Vector3f{
+		math::max(measurement_var(0), minimum_variance),
+		math::max(measurement_var(1), minimum_variance),
+		math::max(measurement_var(2), minimum_variance)
 	};
+	continuing_conditions_passing &= measurement.isAllFinite() && measurement_var.isAllFinite();
 
-	const bool measurement_valid = 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);
 
-	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;
+	} 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
 	}
 
+
 	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;
-					resetVelocityTo(measurement, measurement_var);
+					resetVelocityToEV(measurement, measurement_var, ev_sample.vel_frame);
 					resetAidSourceStatusZeroInnovation(aid_src);
 
 				} else {
@@ -163,7 +173,7 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common
 				}
 
 			} else if (quality_sufficient) {
-				fuseVelocity(aid_src);
+				fuseEvVelocity(aid_src, ev_sample);
 
 			} else {
 				aid_src.innovation_rejected = true;
@@ -177,24 +187,27 @@ 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);
-					resetVelocityTo(measurement, measurement_var);
+					resetVelocityToEV(measurement, measurement_var, ev_sample.vel_frame);
 					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 {
-					// 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);
+					// 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);
+					}
+
 					stopEvVelFusion();
 				}
 			}
@@ -211,15 +224,13 @@ 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;
-
-				resetVelocityTo(measurement, measurement_var);
+				resetVelocityToEV(measurement, measurement_var, ev_sample.vel_frame);
 				resetAidSourceStatusZeroInnovation(aid_src);
 
 				_control_status.flags.ev_vel = true;
 
-			} else if (fuseVelocity(aid_src)) {
+			} else if (fuseEvVelocity(aid_src, ev_sample)) {
 				ECL_INFO("starting %s fusion", AID_SRC_NAME);
 				_control_status.flags.ev_vel = true;
 			}
@@ -232,6 +243,63 @@ 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) {
@@ -239,3 +307,23 @@ 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();
+}

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

@@ -75,15 +75,13 @@ void Ekf::controlFakePosFusion()
 					 Vector2f(getStateVariance<State::pos>()) + obs_var, // innovation variance
 					 innov_gate);                                        // innovation gate
 
-		const bool continuing_conditions_passing = !isHorizontalAidingActive()
+		const bool enable_conditions_passing = !isHorizontalAidingActive()
 				&& ((getTiltVariance() > sq(math::radians(3.f))) || _control_status.flags.vehicle_at_rest)
-				&& (!(_params.imu_ctrl & static_cast<int32_t>(ImuCtrl::GravityVector)) || _control_status.flags.vehicle_at_rest);
-
-		const bool starting_conditions_passing = continuing_conditions_passing
+				&& (!(_params.imu_ctrl & static_cast<int32_t>(ImuCtrl::GravityVector)) || _control_status.flags.vehicle_at_rest)
 				&& _horizontal_deadreckon_time_exceeded;
 
 		if (_control_status.flags.fake_pos) {
-			if (continuing_conditions_passing) {
+			if (enable_conditions_passing) {
 
 				// always protect against extreme values that could result in a NaN
 				if ((aid_src.test_ratio[0] < sq(100.0f / innov_gate))
@@ -104,7 +102,7 @@ void Ekf::controlFakePosFusion()
 			}
 
 		} else {
-			if (starting_conditions_passing) {
+			if (enable_conditions_passing) {
 				ECL_INFO("start fake position fusion");
 				_control_status.flags.fake_pos = true;
 				resetFakePosFusion();

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

@@ -63,7 +63,8 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
 	if (_gps_data_ready) {
 		const gnssSample &gnss_sample = _gps_sample_delayed;
 
-		if (runGnssChecks(gnss_sample) && isTimedOut(_last_gps_fail_us, (uint64_t)_min_gps_health_time_us / 2)) {
+		if (runGnssChecks(gnss_sample)
+		    && isTimedOut(_last_gps_fail_us, max((uint64_t)1e6, (uint64_t)_min_gps_health_time_us / 10))) {
 			if (isTimedOut(_last_gps_fail_us, (uint64_t)_min_gps_health_time_us)) {
 				// First time checks are passing, latching.
 				_gps_checks_passed = true;

src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp

@@ -245,6 +245,7 @@ void Ekf::controlMagFusion()
 
 					if (reset_heading) {
 						_control_status.flags.yaw_align = true;
+						resetAidSourceStatusZeroInnovation(aid_src);
 					}
 
 					_control_status.flags.mag = true;

src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp

@@ -53,15 +53,17 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
 		const bool is_quality_good = (_flow_sample_delayed.quality >= min_quality);
 		const bool is_magnitude_good = _flow_sample_delayed.flow_rate.isAllFinite()
 					       && !_flow_sample_delayed.flow_rate.longerThan(_flow_max_rate);
-		const bool is_tilt_good = (_R_to_earth(2, 2) > _params.range_cos_max_tilt);
 
-		// don't enforce this condition if terrain estimate is not valid as we have logic below to coast through bad range finder data
-		const bool is_within_max_sensor_dist = isTerrainEstimateValid() ? (_terrain_vpos - _state.pos(2) <= _flow_max_distance) : true;
+		bool is_tilt_good = true;
+
+#if defined(CONFIG_EKF2_RANGE_FINDER)
+		is_tilt_good = (_R_to_earth(2, 2) > _params.range_cos_max_tilt);
+
+#endif // CONFIG_EKF2_RANGE_FINDER
 
 		if (is_quality_good
 		    && is_magnitude_good
-		    && is_tilt_good
-		    && is_within_max_sensor_dist) {
+		    && is_tilt_good) {
 			// compensate for body motion to give a LOS rate
 			calcOptFlowBodyRateComp(imu_delayed);
 			_flow_rate_compensated = _flow_sample_delayed.flow_rate - _flow_sample_delayed.gyro_rate.xy();
@@ -79,22 +81,23 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
 		// Check if we are in-air and require optical flow to control position drift
 		bool is_flow_required = _control_status.flags.in_air
 					&& (_control_status.flags.inertial_dead_reckoning // is doing inertial dead-reckoning so must constrain drift urgently
-					|| isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow));
+					    || isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow));
 
-		// Fuse optical flow LOS rate observations into the main filter only if height above ground has been updated recently
-		// use a relaxed time criteria to enable it to coast through bad range finder data
-		const bool terrain_available = isTerrainEstimateValid() || isRecent(_aid_src_terrain_range_finder.time_last_fuse, (uint64_t)10e6);
+		const bool is_within_max_sensor_dist = getHagl() <= _flow_max_distance;
 
 		const bool continuing_conditions_passing = (_params.flow_ctrl == 1)
-							   && _control_status.flags.tilt_align
-							   && (terrain_available || is_flow_required);
+				&& _control_status.flags.tilt_align
+				&& is_within_max_sensor_dist;
 
 		const bool starting_conditions_passing = continuing_conditions_passing
-							 && isTimedOut(_aid_src_optical_flow.time_last_fuse, (uint64_t)2e6); // Prevent rapid switching
+				&& isTimedOut(_aid_src_optical_flow.time_last_fuse, (uint64_t)2e6); // Prevent rapid switching
+
+		// If the height is relative to the ground, terrain height cannot be observed.
+		_hagl_sensor_status.flags.flow = _control_status.flags.opt_flow && !(_height_sensor_ref == HeightSensor::RANGE);
 
 		if (_control_status.flags.opt_flow) {
 			if (continuing_conditions_passing) {
-				fuseOptFlow();
+				fuseOptFlow(_hagl_sensor_status.flags.flow);
 
 				// handle the case when we have optical flow, are reliant on it, but have not been using it for an extended period
 				if (isTimedOut(_aid_src_optical_flow.time_last_fuse, _params.no_aid_timeout_max)) {
@@ -125,19 +128,35 @@ void Ekf::startFlowFusion()
 {
 	ECL_INFO("starting optical flow fusion");
 
-	if (!_aid_src_optical_flow.innovation_rejected && isHorizontalAidingActive()) {
-		// Consistent with the current velocity state, simply fuse the data without reset
-		fuseOptFlow();
-		_control_status.flags.opt_flow = true;
+	if (_height_sensor_ref != HeightSensor::RANGE) {
+		// If the height is relative to the ground, terrain height cannot be observed.
+		_hagl_sensor_status.flags.flow = true;
+	}
 
-	} else if (!isHorizontalAidingActive()) {
-		resetFlowFusion();
-		_control_status.flags.opt_flow = true;
+	if (isHorizontalAidingActive()) {
+		if (!_aid_src_optical_flow.innovation_rejected) {
+			ECL_INFO("starting optical flow no reset");
+			fuseOptFlow(_hagl_sensor_status.flags.flow);
+
+		} else if (_hagl_sensor_status.flags.flow && !_hagl_sensor_status.flags.range_finder) {
+			resetTerrainToFlow();
+
+		} else {
+			ECL_INFO("optical flow fusion failed to start");
+			_control_status.flags.opt_flow = false;
+			_hagl_sensor_status.flags.flow = false;
+		}
 
 	} else {
-		ECL_INFO("optical flow fusion failed to start");
-		_control_status.flags.opt_flow = false;
+		if (isTerrainEstimateValid() || (_height_sensor_ref == HeightSensor::RANGE)) {
+			resetFlowFusion();
+
+		} else if (_hagl_sensor_status.flags.flow) {
+			resetTerrainToFlow();
+		}
 	}
+
+	_control_status.flags.opt_flow = true; // needs to be here because of isHorizontalAidingActive
 }
 
 void Ekf::resetFlowFusion()
@@ -159,18 +178,34 @@ void Ekf::resetFlowFusion()
 	_aid_src_optical_flow.time_last_fuse = _time_delayed_us;
 }
 
+void Ekf::resetTerrainToFlow()
+{
+	ECL_INFO("reset hagl to flow");
+	// TODO: use the flow data
+	_state.terrain = fmaxf(0.0f, _state.pos(2));
+	P.uncorrelateCovarianceSetVariance<State::terrain.dof>(State::terrain.idx, 100.f);
+	_terrain_vpos_reset_counter++;
+
+	_innov_check_fail_status.flags.reject_optflow_X = false;
+	_innov_check_fail_status.flags.reject_optflow_Y = false;
+
+	_aid_src_optical_flow.time_last_fuse = _time_delayed_us;
+}
+
 void Ekf::stopFlowFusion()
 {
 	if (_control_status.flags.opt_flow) {
 		ECL_INFO("stopping optical flow fusion");
 		_control_status.flags.opt_flow = false;
+		_hagl_sensor_status.flags.flow = false;
 	}
 }
 
 void Ekf::calcOptFlowBodyRateComp(const imuSample &imu_delayed)
 {
 	if (imu_delayed.delta_ang_dt > FLT_EPSILON) {
-		_ref_body_rate = -imu_delayed.delta_ang / imu_delayed.delta_ang_dt - getGyroBias(); // flow gyro has opposite sign convention
+		_ref_body_rate = -imu_delayed.delta_ang / imu_delayed.delta_ang_dt -
+				 getGyroBias(); // flow gyro has opposite sign convention
 
 	} else {
 		_ref_body_rate.zero();
@@ -185,5 +220,6 @@ void Ekf::calcOptFlowBodyRateComp(const imuSample &imu_delayed)
 	}
 
 	// calculate the bias estimate using  a combined LPF and spike filter
-	_flow_gyro_bias = _flow_gyro_bias * 0.99f + matrix::constrain(_flow_sample_delayed.gyro_rate - _ref_body_rate, -0.1f, 0.1f) * 0.01f;
+	_flow_gyro_bias = _flow_gyro_bias * 0.99f + matrix::constrain(_flow_sample_delayed.gyro_rate - _ref_body_rate, -0.1f,
+			  0.1f) * 0.01f;
 }

src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp

@@ -33,12 +33,6 @@
 
 /**
  * @file optflow_fusion.cpp
- * Function for fusing gps and baro measurements/
- * equations generated using EKF/python/ekf_derivation/main.py
- *
- * @author Paul Riseborough <p_riseborough@live.com.au>
- * @author Siddharth Bharat Purohit <siddharthbharatpurohit@gmail.com>
- *
  */
 
 #include "ekf.h"
@@ -73,7 +67,7 @@ void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src)
 
 	Vector2f innov_var;
 	VectorState H;
-	sym::ComputeFlowXyInnovVarAndHx(_state.vector(), P, range, R_LOS, FLT_EPSILON, &innov_var, &H);
+	sym::ComputeFlowXyInnovVarAndHx(_state.vector(), P, R_LOS, FLT_EPSILON, &innov_var, &H);
 
 	// run the innovation consistency check and record result
 	updateAidSourceStatus(aid_src,
@@ -85,7 +79,7 @@ void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src)
 		math::max(_params.flow_innov_gate, 1.f));      // innovation gate
 }
 
-void Ekf::fuseOptFlow()
+void Ekf::fuseOptFlow(const bool update_terrain)
 {
 	const float R_LOS = _aid_src_optical_flow.observation_variance[0];
 
@@ -97,7 +91,7 @@ void Ekf::fuseOptFlow()
 
 	Vector2f innov_var;
 	VectorState H;
-	sym::ComputeFlowXyInnovVarAndHx(state_vector, P, range, R_LOS, FLT_EPSILON, &innov_var, &H);
+	sym::ComputeFlowXyInnovVarAndHx(state_vector, P, R_LOS, FLT_EPSILON, &innov_var, &H);
 	innov_var.copyTo(_aid_src_optical_flow.innovation_variance);
 
 	if ((innov_var(0) < R_LOS) || (innov_var(1) < R_LOS)) {
@@ -124,7 +118,7 @@ void Ekf::fuseOptFlow()
 
 		} else if (index == 1) {
 			// recalculate innovation variance because state covariances have changed due to previous fusion (linearise using the same initial state for all axes)
-			sym::ComputeFlowYInnovVarAndH(state_vector, P, range, R_LOS, FLT_EPSILON, &_aid_src_optical_flow.innovation_variance[1], &H);
+			sym::ComputeFlowYInnovVarAndH(state_vector, P, R_LOS, FLT_EPSILON, &_aid_src_optical_flow.innovation_variance[1], &H);
 
 			// recalculate the innovation using the updated state
 			const Vector2f vel_body = predictFlowVelBody();
@@ -141,6 +135,10 @@ void Ekf::fuseOptFlow()
 
 		VectorState Kfusion = P * H / _aid_src_optical_flow.innovation_variance[index];
 
+		if (!update_terrain) {
+			Kfusion(State::terrain.idx) = 0.f;
+		}
+
 		if (measurementUpdate(Kfusion, H, _aid_src_optical_flow.observation_variance[index], _aid_src_optical_flow.innovation[index])) {
 			fused[index] = true;
 		}
@@ -165,10 +163,17 @@ float Ekf::predictFlowRange()
 
 	// calculate the height above the ground of the optical flow camera. Since earth frame is NED
 	// a positive offset in earth frame leads to a smaller height above the ground.
-	const float height_above_gnd_est = math::max(_terrain_vpos - _state.pos(2) - pos_offset_earth(2), fmaxf(_params.rng_gnd_clearance, 0.01f));
+	const float height_above_gnd_est = getHagl() - pos_offset_earth(2);
 
 	// calculate range from focal point to centre of image
-	return height_above_gnd_est / _R_to_earth(2, 2); // absolute distance to the frame region in view
+	float flow_range = height_above_gnd_est / _R_to_earth(2, 2); // absolute distance to the frame region in view
+
+	// avoid the flow prediction singularity at range = 0
+	if (fabsf(flow_range) < FLT_EPSILON) {
+		flow_range = signNoZero(flow_range) * FLT_EPSILON;
+	}
+
+	return flow_range;
 }
 
 Vector2f Ekf::predictFlowVelBody()

src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp

@@ -37,8 +37,9 @@
  */
 
 #include "ekf.h"
+#include "ekf_derivation/generated/compute_hagl_innov_var.h"
 
-void Ekf::controlRangeHeightFusion()
+void Ekf::controlRangeHaglFusion()
 {
 	static constexpr const char *HGT_SRC_NAME = "RNG";
 
@@ -93,58 +94,91 @@ void Ekf::controlRangeHeightFusion()
 	}
 
 	auto &aid_src = _aid_src_rng_hgt;
-	HeightBiasEstimator &bias_est = _rng_hgt_b_est;
-
-	bias_est.predict(_dt_ekf_avg);
 
 	if (rng_data_ready && _range_sensor.getSampleAddress()) {
 
-		const float measurement = math::max(_range_sensor.getDistBottom(), _params.rng_gnd_clearance);
-		const float measurement_var = sq(_params.range_noise) + sq(_params.range_noise_scaler * _range_sensor.getDistBottom());
+		updateRangeHagl(aid_src);
+		const bool measurement_valid = PX4_ISFINITE(aid_src.observation) && PX4_ISFINITE(aid_src.observation_variance);
 
-		const bool measurement_valid = PX4_ISFINITE(measurement) && PX4_ISFINITE(measurement_var);
-
-		// vertical position innovation - baro measurement has opposite sign to earth z axis
-		updateVerticalPositionAidStatus(aid_src,
-						_range_sensor.getSampleAddress()->time_us, // sample timestamp
-						-(measurement - bias_est.getBias()),       // observation
-						measurement_var + bias_est.getBiasVar(),   // observation variance
-						math::max(_params.range_innov_gate, 1.f)); // innovation gate
-
-		// update the bias estimator before updating the main filter but after
-		// using its current state to compute the vertical position innovation
-		if (measurement_valid && _range_sensor.isDataHealthy()) {
-			bias_est.setMaxStateNoise(sqrtf(measurement_var));
-			bias_est.setProcessNoiseSpectralDensity(_params.rng_hgt_bias_nsd);
-			bias_est.fuseBias(measurement - (-_state.pos(2)), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2));
-		}
-
-		// determine if we should use height aiding
-		const bool do_conditional_range_aid = (_params.rng_ctrl == static_cast<int32_t>(RngCtrl::CONDITIONAL))
-						      && isConditionalRangeAidSuitable();
-
-		const bool continuing_conditions_passing = ((_params.rng_ctrl == static_cast<int32_t>(RngCtrl::ENABLED)) || do_conditional_range_aid)
-				&& measurement_valid
-				&& _range_sensor.isDataHealthy();
+		const bool continuing_conditions_passing = ((_params.rng_ctrl == static_cast<int32_t>(RngCtrl::ENABLED))
+							    || (_params.rng_ctrl == static_cast<int32_t>(RngCtrl::CONDITIONAL)))
+							   && _control_status.flags.tilt_align
+							   && measurement_valid
+							   && _range_sensor.isDataHealthy()
+						           && _rng_consistency_check.isKinematicallyConsistent();
 
 		const bool starting_conditions_passing = continuing_conditions_passing
 				&& isNewestSampleRecent(_time_last_range_buffer_push, 2 * estimator::sensor::RNG_MAX_INTERVAL)
 				&& _range_sensor.isRegularlySendingData();
 
+
+		const bool do_conditional_range_aid = (_hagl_sensor_status.flags.range_finder || _control_status.flags.rng_hgt)
+						      && (_params.rng_ctrl == static_cast<int32_t>(RngCtrl::CONDITIONAL))
+						      && isConditionalRangeAidSuitable();
+
+		const bool do_range_aid = (_hagl_sensor_status.flags.range_finder || _control_status.flags.rng_hgt)
+					  && (_params.rng_ctrl == static_cast<int32_t>(RngCtrl::ENABLED));
+
 		if (_control_status.flags.rng_hgt) {
+			if (!(do_conditional_range_aid || do_range_aid)) {
+				ECL_INFO("stopping %s fusion", HGT_SRC_NAME);
+				stopRngHgtFusion();
+			}
+
+		} else {
+			if (_params.height_sensor_ref == static_cast<int32_t>(HeightSensor::RANGE)) {
+				if (do_conditional_range_aid) {
+					// Range finder is used while hovering to stabilize the height estimate. Don't reset but use it as height reference.
+					ECL_INFO("starting conditional %s height fusion", HGT_SRC_NAME);
+					_height_sensor_ref = HeightSensor::RANGE;
+
+					_control_status.flags.rng_hgt = true;
+					stopRngTerrFusion();
+
+					if (!_hagl_sensor_status.flags.flow && aid_src.innovation_rejected) {
+						resetTerrainToRng(aid_src);
+					}
+
+				} else if (do_range_aid) {
+					// Range finder is the primary height source, the ground is now the datum used
+					// to compute the local vertical position
+					ECL_INFO("starting %s height fusion, resetting height", HGT_SRC_NAME);
+					_height_sensor_ref = HeightSensor::RANGE;
+
+					_information_events.flags.reset_hgt_to_rng = true;
+					resetVerticalPositionTo(-aid_src.observation, aid_src.observation_variance);
+					_state.terrain = 0.f;
+					_control_status.flags.rng_hgt = true;
+					stopRngTerrFusion();
+
+					aid_src.time_last_fuse = _time_delayed_us;
+				}
+
+			} else {
+				if (do_conditional_range_aid || do_range_aid) {
+					ECL_INFO("starting %s height fusion", HGT_SRC_NAME);
+					_control_status.flags.rng_hgt = true;
+
+					if (!_hagl_sensor_status.flags.flow && aid_src.innovation_rejected) {
+						resetTerrainToRng(aid_src);
+					}
+				}
+			}
+		}
+
+		if (_control_status.flags.rng_hgt || _hagl_sensor_status.flags.range_finder) {
 			if (continuing_conditions_passing) {
 
-				fuseVerticalPosition(aid_src);
+				fuseHaglRng(aid_src, _control_status.flags.rng_hgt, _hagl_sensor_status.flags.range_finder);
 
 				const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.hgt_fusion_timeout_max);
 
-				if (isHeightResetRequired()) {
+				if (isHeightResetRequired() && _control_status.flags.rng_hgt) {
 					// All height sources are failing
 					ECL_WARN("%s height fusion reset required, all height sources failing", HGT_SRC_NAME);
 
 					_information_events.flags.reset_hgt_to_rng = true;
-					resetVerticalPositionTo(-(measurement - bias_est.getBias()));
-					bias_est.setBias(_state.pos(2) + measurement);
+					resetVerticalPositionTo(-(aid_src.observation - _state.terrain));
 
 					// reset vertical velocity
 					resetVerticalVelocityToZero();
@@ -153,99 +187,120 @@ void Ekf::controlRangeHeightFusion()
 
 				} else if (is_fusion_failing) {
 					// Some other height source is still working
-					ECL_WARN("stopping %s height fusion, fusion failing", HGT_SRC_NAME);
-					stopRngHgtFusion();
-					_control_status.flags.rng_fault = true;
-					_range_sensor.setFaulty();
+					if (_hagl_sensor_status.flags.flow) {
+						ECL_WARN("stopping %s fusion, fusion failing", HGT_SRC_NAME);
+						stopRngHgtFusion();
+						stopRngTerrFusion();
+
+					} else {
+						resetTerrainToRng(aid_src);
+					}
 				}
 
 			} else {
-				ECL_WARN("stopping %s height fusion, continuing conditions failing", HGT_SRC_NAME);
+				ECL_WARN("stopping %s fusion, continuing conditions failing", HGT_SRC_NAME);
 				stopRngHgtFusion();
+				stopRngTerrFusion();
 			}
 
 		} else {
 			if (starting_conditions_passing) {
-				if ((_params.height_sensor_ref == static_cast<int32_t>(HeightSensor::RANGE))
-				    && (_params.rng_ctrl == static_cast<int32_t>(RngCtrl::CONDITIONAL))
-				) {
-					// Range finder is used while hovering to stabilize the height estimate. Don't reset but use it as height reference.
-					ECL_INFO("starting conditional %s height fusion", HGT_SRC_NAME);
-					_height_sensor_ref = HeightSensor::RANGE;
-					bias_est.setBias(_state.pos(2) + measurement);
-
-				} else if ((_params.height_sensor_ref == static_cast<int32_t>(HeightSensor::RANGE))
-					   && (_params.rng_ctrl != static_cast<int32_t>(RngCtrl::CONDITIONAL))
-				) {
-					// Range finder is the primary height source, the ground is now the datum used
-					// to compute the local vertical position
-					ECL_INFO("starting %s height fusion, resetting height", HGT_SRC_NAME);
-					_height_sensor_ref = HeightSensor::RANGE;
-
-					_information_events.flags.reset_hgt_to_rng = true;
-					resetVerticalPositionTo(-measurement, measurement_var);
-					bias_est.reset();
+				if (_hagl_sensor_status.flags.flow) {
+					if (!aid_src.innovation_rejected) {
+						_hagl_sensor_status.flags.range_finder = true;
+						fuseHaglRng(aid_src, _control_status.flags.rng_hgt, _hagl_sensor_status.flags.range_finder);
+					}
 
 				} else {
-					ECL_INFO("starting %s height fusion", HGT_SRC_NAME);
-					bias_est.setBias(_state.pos(2) + measurement);
-				}
+					if (aid_src.innovation_rejected) {
+						resetTerrainToRng(aid_src);
+					}
 
-				aid_src.time_last_fuse = _time_delayed_us;
-				bias_est.setFusionActive();
-				_control_status.flags.rng_hgt = true;
+					_hagl_sensor_status.flags.range_finder = true;
+				}
 			}
 		}
 
-	} else if (_control_status.flags.rng_hgt
+	} else if ((_control_status.flags.rng_hgt || _hagl_sensor_status.flags.range_finder)
 		   && !isNewestSampleRecent(_time_last_range_buffer_push, 2 * estimator::sensor::RNG_MAX_INTERVAL)) {
 		// No data anymore. Stop until it comes back.
-		ECL_WARN("stopping %s height fusion, no data", HGT_SRC_NAME);
+		ECL_WARN("stopping %s fusion, no data", HGT_SRC_NAME);
 		stopRngHgtFusion();
+		stopRngTerrFusion();
 	}
 }
 
+void Ekf::updateRangeHagl(estimator_aid_source1d_s &aid_src)
+{
+	aid_src.observation = math::max(_range_sensor.getDistBottom(), _params.rng_gnd_clearance);
+	aid_src.innovation = getHagl() - aid_src.observation;
+
+	const float observation_variance = getRngVar();
+	float innovation_variance;
+	sym::ComputeHaglInnovVar(P, observation_variance, &innovation_variance);
+
+	const float innov_gate = math::max(_params.range_innov_gate, 1.f);
+	updateAidSourceStatus(aid_src,
+			      _range_sensor.getSampleAddress()->time_us,                           // sample timestamp
+			      math::max(_range_sensor.getDistBottom(), _params.rng_gnd_clearance), // observation
+			      observation_variance,                                                // observation variance
+			      getHagl() - aid_src.observation,                                     // innovation
+			      innovation_variance,                                                 // innovation variance
+			      math::max(_params.range_innov_gate, 1.f));                            // innovation gate
+
+	// z special case if there is bad vertical acceleration data, then don't reject measurement,
+	// but limit innovation to prevent spikes that could destabilise the filter
+	if (_fault_status.flags.bad_acc_vertical && aid_src.innovation_rejected) {
+		const float innov_limit = innov_gate * sqrtf(aid_src.innovation_variance);
+		aid_src.innovation = math::constrain(aid_src.innovation, -innov_limit, innov_limit);
+		aid_src.innovation_rejected = false;
+	}
+}
+
+float Ekf::getRngVar() const
+{
+	return fmaxf(
+		     P(State::pos.idx + 2, State::pos.idx + 2)
+		     + sq(_params.range_noise)
+		     + sq(_params.range_noise_scaler * _range_sensor.getRange()),
+	       0.f);
+}
+
+void Ekf::resetTerrainToRng(estimator_aid_source1d_s &aid_src)
+{
+	_state.terrain = _state.pos(2) + aid_src.observation;
+	P.uncorrelateCovarianceSetVariance<State::terrain.dof>(State::terrain.idx, aid_src.observation_variance);
+	_terrain_vpos_reset_counter++;
+
+	aid_src.time_last_fuse = _time_delayed_us;
+}
+
 bool Ekf::isConditionalRangeAidSuitable()
 {
-#if defined(CONFIG_EKF2_TERRAIN)
+	// check if we can use range finder measurements to estimate height, use hysteresis to avoid rapid switching
+	// Note that the 0.7 coefficients and the innovation check are arbitrary values but work well in practice
+	float range_hagl_max = _params.max_hagl_for_range_aid;
+	float max_vel_xy = _params.max_vel_for_range_aid;
 
-	if (_control_status.flags.in_air
-	    && _range_sensor.isHealthy()
-	    && isTerrainEstimateValid()) {
-		// check if we can use range finder measurements to estimate height, use hysteresis to avoid rapid switching
-		// Note that the 0.7 coefficients and the innovation check are arbitrary values but work well in practice
-		float range_hagl_max = _params.max_hagl_for_range_aid;
-		float max_vel_xy = _params.max_vel_for_range_aid;
+	const float hagl_test_ratio = _aid_src_rng_hgt.test_ratio;
 
-		const float hagl_innov = _aid_src_terrain_range_finder.innovation;
-		const float hagl_innov_var = _aid_src_terrain_range_finder.innovation_variance;
+	bool is_hagl_stable = (hagl_test_ratio < 1.f);
 
-		const float hagl_test_ratio = (hagl_innov * hagl_innov / (sq(_params.range_aid_innov_gate) * hagl_innov_var));
-
-		bool is_hagl_stable = (hagl_test_ratio < 1.f);
-
-		if (!_control_status.flags.rng_hgt) {
-			range_hagl_max = 0.7f * _params.max_hagl_for_range_aid;
-			max_vel_xy = 0.7f * _params.max_vel_for_range_aid;
-			is_hagl_stable = (hagl_test_ratio < 0.01f);
-		}
-
-		const float range_hagl = _terrain_vpos - _state.pos(2);
-
-		const bool is_in_range = (range_hagl < range_hagl_max);
-
-		bool is_below_max_speed = true;
-
-		if (isHorizontalAidingActive()) {
-			is_below_max_speed = !_state.vel.xy().longerThan(max_vel_xy);
-		}
-
-		return is_in_range && is_hagl_stable && is_below_max_speed;
+	if (!_control_status.flags.rng_hgt) {
+		range_hagl_max = 0.7f * _params.max_hagl_for_range_aid;
+		max_vel_xy = 0.7f * _params.max_vel_for_range_aid;
+		is_hagl_stable = (hagl_test_ratio < 0.01f);
 	}
 
-#endif // CONFIG_EKF2_TERRAIN
+	const bool is_in_range = (getHagl() < range_hagl_max);
 
-	return false;
+	bool is_below_max_speed = true;
+
+	if (isHorizontalAidingActive()) {
+		is_below_max_speed = !_state.vel.xy().longerThan(max_vel_xy);
+	}
+
+	return is_in_range && is_hagl_stable && is_below_max_speed;
 }
 
 void Ekf::stopRngHgtFusion()
@@ -256,8 +311,11 @@ void Ekf::stopRngHgtFusion()
 			_height_sensor_ref = HeightSensor::UNKNOWN;
 		}
 
-		_rng_hgt_b_est.setFusionInactive();
-
 		_control_status.flags.rng_hgt = false;
 	}
 }
+
+void Ekf::stopRngTerrFusion()
+{
+	_hagl_sensor_status.flags.range_finder = false;
+}

src/modules/ekf2/EKF/aid_sources/range_finder/range_height_fusion.cpp

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

src/modules/ekf2/EKF/common.h

@@ -107,7 +107,7 @@ enum MagFuseType : uint8_t {
 #endif // CONFIG_EKF2_MAGNETOMETER
 
 #if defined(CONFIG_EKF2_TERRAIN)
-enum TerrainFusionMask : uint8_t {
+enum class TerrainFusionMask : uint8_t {
 	TerrainFuseRangeFinder = (1 << 0),
 	TerrainFuseOpticalFlow = (1 << 1)
 };
@@ -382,9 +382,6 @@ struct parameters {
 #endif // CONFIG_EKF2_SIDESLIP
 
 #if defined(CONFIG_EKF2_TERRAIN)
-	int32_t terrain_fusion_mode{TerrainFusionMask::TerrainFuseRangeFinder |
-				    TerrainFusionMask::TerrainFuseOpticalFlow}; ///< aiding source(s) selection bitmask for the terrain estimator
-
 	float terrain_p_noise{5.0f};            ///< process noise for terrain offset (m/sec)
 	float terrain_gradient{0.5f};           ///< gradient of terrain used to estimate process noise due to changing position (m/m)
 	const float terrain_timeout{10.f};      ///< maximum time for invalid bottom distance measurements before resetting terrain estimate (s)
@@ -401,10 +398,8 @@ struct parameters {
 	float range_delay_ms{5.0f};             ///< range finder measurement delay relative to the IMU (mSec)
 	float range_noise{0.1f};                ///< observation noise for range finder measurements (m)
 	float range_innov_gate{5.0f};           ///< range finder fusion innovation consistency gate size (STD)
-	float rng_hgt_bias_nsd{0.13f};          ///< process noise for range height bias estimation (m/s/sqrt(Hz))
 	float rng_sens_pitch{0.0f};             ///< Pitch offset of the range sensor (rad). Sensor points out along Z axis when offset is zero. Positive rotation is RH about Y axis.
 	float range_noise_scaler{0.0f};         ///< scaling from range measurement to noise (m/m)
-	const float vehicle_variance_scaler{0.0f};      ///< gain applied to vehicle height variance used in calculation of height above ground observation variance
 	float max_hagl_for_range_aid{5.0f};     ///< maximum height above ground for which we allow to use the range finder as height source (if rng_control == 1)
 	float max_vel_for_range_aid{1.0f};      ///< maximum ground velocity for which we allow to use the range finder as height source (if rng_control == 1)
 	float range_aid_innov_gate{1.0f};       ///< gate size used for innovation consistency checks for range aid fusion
@@ -521,7 +516,7 @@ union innovation_fault_status_u {
 		bool reject_yaw       : 1; ///< 7 - true if the yaw observation has been rejected
 		bool reject_airspeed  : 1; ///< 8 - true if the airspeed observation has been rejected
 		bool reject_sideslip  : 1; ///< 9 - true if the synthetic sideslip observation has been rejected
-		bool reject_hagl      : 1; ///< 10 - true if the height above ground observation has been rejected
+		bool reject_hagl      : 1; ///< 10 - unused
 		bool reject_optflow_X : 1; ///< 11 - true if the X optical flow observation has been rejected
 		bool reject_optflow_Y : 1; ///< 12 - true if the Y optical flow observation has been rejected
 	} flags;

src/modules/ekf2/EKF/control.cpp

@@ -144,6 +144,10 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
 	// Additional horizontal velocity data from an auxiliary sensor can be fused
 	controlAuxVelFusion();
 #endif // CONFIG_EKF2_AUXVEL
+	//
+#if defined(CONFIG_EKF2_TERRAIN)
+	controlTerrainFakeFusion();
+#endif // CONFIG_EKF2_TERRAIN
 
 	controlZeroInnovationHeadingUpdate();
 

src/modules/ekf2/EKF/covariance.cpp

@@ -99,6 +99,11 @@ void Ekf::initialiseCovariance()
 #if defined(CONFIG_EKF2_WIND)
 	resetWindCov();
 #endif // CONFIG_EKF2_WIND
+
+#if defined(CONFIG_EKF2_TERRAIN)
+	// use the ground clearance value as our uncertainty
+	P.uncorrelateCovarianceSetVariance<State::terrain.dof>(State::terrain.idx, sq(_params.rng_gnd_clearance));
+#endif // CONFIG_EKF2_TERRAIN
 }
 
 void Ekf::predictCovariance(const imuSample &imu_delayed)
@@ -202,6 +207,17 @@ 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++) {
@@ -239,6 +255,10 @@ void Ekf::constrainStateVariances()
 		constrainStateVarLimitRatio(State::wind_vel, 1e-6f, 1e6f);
 	}
 #endif // CONFIG_EKF2_WIND
+
+#if defined(CONFIG_EKF2_TERRAIN)
+	constrainStateVarLimitRatio(State::terrain, 0.f, 1e4f);
+#endif // CONFIG_EKF2_TERRAIN
 }
 
 void Ekf::constrainStateVar(const IdxDof &state, float min, float max)

src/modules/ekf2/EKF/ekf.cpp

@@ -71,6 +71,11 @@ void Ekf::reset()
 #if defined(CONFIG_EKF2_WIND)
 	_state.wind_vel.setZero();
 #endif // CONFIG_EKF2_WIND
+	//
+#if defined(CONFIG_EKF2_TERRAIN)
+	// assume a ground clearance
+	_state.terrain = _state.pos(2) + _params.rng_gnd_clearance;
+#endif // CONFIG_EKF2_TERRAIN
 
 #if defined(CONFIG_EKF2_RANGE_FINDER)
 	_range_sensor.setPitchOffset(_params.rng_sens_pitch);
@@ -122,7 +127,10 @@ void Ekf::reset()
 
 	_time_bad_vert_accel = 0;
 	_time_good_vert_accel = 0;
-	_clip_counter = 0;
+
+	for (auto &clip_count : _clip_counter) {
+		clip_count = 0;
+	}
 
 	_zero_velocity_update.reset();
 }
@@ -160,11 +168,6 @@ bool Ekf::update()
 		// control fusion of observation data
 		controlFusionModes(imu_sample_delayed);
 
-#if defined(CONFIG_EKF2_TERRAIN)
-		// run a separate filter for terrain estimation
-		runTerrainEstimator(imu_sample_delayed);
-#endif // CONFIG_EKF2_TERRAIN
-
 		_output_predictor.correctOutputStates(imu_sample_delayed.time_us, _state.quat_nominal, _state.vel, _state.pos, _state.gyro_bias, _state.accel_bias);
 
 		return true;
@@ -200,11 +203,6 @@ bool Ekf::initialiseFilter()
 	// initialise the state covariance matrix now we have starting values for all the states
 	initialiseCovariance();
 
-#if defined(CONFIG_EKF2_TERRAIN)
-	// Initialise the terrain estimator
-	initHagl();
-#endif // CONFIG_EKF2_TERRAIN
-
 	// reset the output predictor state history to match the EKF initial values
 	_output_predictor.alignOutputFilter(_state.quat_nominal, _state.vel, _state.pos);
 
@@ -283,23 +281,66 @@ 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;
 }
 
-void Ekf::resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float accuracy, uint64_t timestamp_observation)
+bool Ekf::resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float accuracy, uint64_t timestamp_observation)
 {
 	if (!_pos_ref.isInitialized()) {
-		return;
+		ECL_WARN("unable to reset global position, position reference not initialized");
+		return false;
 	}
 
-	// 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;
+	Vector2f pos_corrected = _pos_ref.project(lat_deg, lon_deg);
 
-	Vector2f pos_corrected = _pos_ref.project(lat_deg, lon_deg) + _state.vel.xy() * dt;
+	// 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)) {
 
-	resetHorizontalPositionTo(pos_corrected, sq(math::max(accuracy, 0.01f)));
+		timestamp_observation = math::min(_time_latest_us, timestamp_observation);
 
-	ECL_INFO("reset position to external observation");
-	_information_events.flags.reset_pos_to_ext_obs = true;
+		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;
 }
 
 void Ekf::updateParameters()
@@ -377,6 +418,14 @@ void Ekf::print_status()
 	      );
 #endif // CONFIG_EKF2_WIND
 
+#if defined(CONFIG_EKF2_TERRAIN)
+	printf("Terrain position (%d): %.3f var: %.1e\n",
+	       State::terrain.idx,
+	       (double)_state.terrain,
+	       (double)getStateVariance<State::terrain>()(0)
+	      );
+#endif // CONFIG_EKF2_TERRAIN
+
 	printf("\nP:\n");
 	P.print();
 

src/modules/ekf2/EKF/ekf.h

@@ -73,7 +73,6 @@ class Ekf final : public EstimatorInterface
 public:
 	typedef matrix::Vector<float, State::size> VectorState;
 	typedef matrix::SquareMatrix<float, State::size> SquareMatrixState;
-	typedef matrix::SquareMatrix<float, 2> Matrix2f;
 
 	Ekf()
 	{
@@ -104,27 +103,19 @@ public:
 	uint8_t getTerrainEstimateSensorBitfield() const { return _hagl_sensor_status.value; }
 
 	// get the estimated terrain vertical position relative to the NED origin
-	float getTerrainVertPos() const { return _terrain_vpos; };
+	float getTerrainVertPos() const { return _state.terrain; };
+	float getHagl() const { return _state.terrain - _state.pos(2); }
 
 	// get the number of times the vertical terrain position has been reset
 	uint8_t getTerrainVertPosResetCounter() const { return _terrain_vpos_reset_counter; };
 
 	// get the terrain variance
-	float get_terrain_var() const { return _terrain_var; }
-
-# if defined(CONFIG_EKF2_RANGE_FINDER)
-	const auto &aid_src_terrain_range_finder() const { return _aid_src_terrain_range_finder; }
-# endif // CONFIG_EKF2_RANGE_FINDER
-
-# if defined(CONFIG_EKF2_OPTICAL_FLOW)
-	const auto &aid_src_terrain_optical_flow() const { return _aid_src_terrain_optical_flow; }
-# endif // CONFIG_EKF2_OPTICAL_FLOW
+	float getTerrainVariance() const { return P(State::terrain.idx, State::terrain.idx); }
 
 #endif // CONFIG_EKF2_TERRAIN
 
 #if defined(CONFIG_EKF2_RANGE_FINDER)
 	// range height
-	const BiasEstimator::status &getRngHgtBiasEstimatorStatus() const { return _rng_hgt_b_est.getStatus(); }
 	const auto &aid_src_rng_hgt() const { return _aid_src_rng_hgt; }
 
 	float getHaglRateInnov() const { return _rng_consistency_check.getInnov(); }
@@ -383,13 +374,17 @@ public:
 		*counter = _state_reset_status.reset_count.quat;
 	}
 
-	// get EKF innovation consistency check status information comprising of:
-	// status - a bitmask integer containing the pass/fail status for each EKF measurement innovation consistency check
-	// Innovation Test Ratios - these are the ratio of the innovation to the acceptance threshold.
-	// A value > 1 indicates that the sensor measurement has exceeded the maximum acceptable level and has been rejected by the EKF
-	// Where a measurement type is a vector quantity, eg magnetometer, GPS position, etc, the maximum value is returned.
-	void get_innovation_test_status(uint16_t &status, float &mag, float &vel, float &pos, float &hgt, float &tas,
-					float &hagl, float &beta) const;
+	float getHeadingInnovationTestRatio() const;
+
+	float getVelocityInnovationTestRatio() const;
+
+	float getHorizontalPositionInnovationTestRatio() const;
+	float getVerticalPositionInnovationTestRatio() const;
+
+	float getAirspeedInnovationTestRatio() const;
+	float getSyntheticSideslipInnovationTestRatio() const;
+
+	float getHeightAboveGroundInnovationTestRatio() const;
 
 	// return a bitmask integer that describes which state estimates are valid
 	void get_ekf_soln_status(uint16_t *status) const;
@@ -506,7 +501,7 @@ public:
 		return true;
 	}
 
-	void resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float accuracy, uint64_t timestamp_observation);
+	bool resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float accuracy, uint64_t timestamp_observation);
 
 	void updateParameters();
 
@@ -588,27 +583,14 @@ private:
 
 #if defined(CONFIG_EKF2_TERRAIN)
 	// Terrain height state estimation
-	float _terrain_vpos{0.0f};		///< estimated vertical position of the terrain underneath the vehicle in local NED frame (m)
-	float _terrain_var{1e4f};		///< variance of terrain position estimate (m**2)
 	uint8_t _terrain_vpos_reset_counter{0};	///< number of times _terrain_vpos has been reset
 
 	terrain_fusion_status_u _hagl_sensor_status{}; ///< Struct indicating type of sensor used to estimate height above ground
 	float _last_on_ground_posD{0.0f};	///< last vertical position when the in_air status was false (m)
-
-# if defined(CONFIG_EKF2_RANGE_FINDER)
-	estimator_aid_source1d_s _aid_src_terrain_range_finder{};
-	uint64_t _time_last_healthy_rng_data{0};
-# endif // CONFIG_EKF2_RANGE_FINDER
-
-# if defined(CONFIG_EKF2_OPTICAL_FLOW)
-	estimator_aid_source2d_s _aid_src_terrain_optical_flow{};
-# endif // CONFIG_EKF2_OPTICAL_FLOW
-
 #endif // CONFIG_EKF2_TERRAIN
 
 #if defined(CONFIG_EKF2_RANGE_FINDER)
 	estimator_aid_source1d_s _aid_src_rng_hgt{};
-	HeightBiasEstimator _rng_hgt_b_est{HeightSensor::RANGE, _height_sensor_ref};
 #endif // CONFIG_EKF2_RANGE_FINDER
 
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
@@ -723,7 +705,7 @@ private:
 	// imu fault status
 	uint64_t _time_bad_vert_accel{0};	///< last time a bad vertical accel was detected (uSec)
 	uint64_t _time_good_vert_accel{0};	///< last time a good vertical accel was detected (uSec)
-	uint16_t _clip_counter{0};		///< counter that increments when clipping ad decrements when not
+	uint16_t _clip_counter[3];		///< counter per axis that increments when clipping ad decrements when not
 
 	// initialise filter states of both the delayed ekf and the real time complementary filter
 	bool initialiseFilter(void);
@@ -824,41 +806,30 @@ private:
 	bool fuseVelocity(estimator_aid_source3d_s &vel_aid_src);
 
 #if defined(CONFIG_EKF2_TERRAIN)
-	// terrain vertical position estimator
-	void initHagl();
-	void runTerrainEstimator(const imuSample &imu_delayed);
-	void predictHagl(const imuSample &imu_delayed);
-
-	float getTerrainVPos() const { return isTerrainEstimateValid() ? _terrain_vpos : _last_on_ground_posD; }
-
-	void controlHaglFakeFusion();
-	void terrainHandleVerticalPositionReset(float delta_z);
+	void initTerrain();
+	float getTerrainVPos() const { return isTerrainEstimateValid() ? _state.terrain : _last_on_ground_posD; }
+	void controlTerrainFakeFusion();
 
 # if defined(CONFIG_EKF2_RANGE_FINDER)
 	// update the terrain vertical position estimate using a height above ground measurement from the range finder
-	void controlHaglRngFusion();
-	void updateHaglRng(estimator_aid_source1d_s &aid_src) const;
-	void fuseHaglRng(estimator_aid_source1d_s &aid_src);
-	void resetHaglRng();
-	void stopHaglRngFusion();
+	bool fuseHaglRng(estimator_aid_source1d_s &aid_src, bool update_height, bool update_terrain);
+	void updateRangeHagl(estimator_aid_source1d_s &aid_src);
+	void resetTerrainToRng(estimator_aid_source1d_s &aid_src);
 	float getRngVar() const;
 # endif // CONFIG_EKF2_RANGE_FINDER
 
 # if defined(CONFIG_EKF2_OPTICAL_FLOW)
-	// update the terrain vertical position estimate using an optical flow measurement
-	void controlHaglFlowFusion();
-	void resetHaglFlow();
-	void stopHaglFlowFusion();
-	void fuseFlowForTerrain(estimator_aid_source2d_s &flow);
+	void resetTerrainToFlow();
 # endif // CONFIG_EKF2_OPTICAL_FLOW
 
 #endif // CONFIG_EKF2_TERRAIN
 
 #if defined(CONFIG_EKF2_RANGE_FINDER)
 	// range height
-	void controlRangeHeightFusion();
+	void controlRangeHaglFusion();
 	bool isConditionalRangeAidSuitable();
 	void stopRngHgtFusion();
+	void stopRngTerrFusion();
 #endif // CONFIG_EKF2_RANGE_FINDER
 
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
@@ -879,7 +850,7 @@ private:
 
 	// fuse optical flow line of sight rate measurements
 	void updateOptFlow(estimator_aid_source2d_s &aid_src);
-	void fuseOptFlow();
+	void fuseOptFlow(bool update_terrain);
 	float predictFlowRange();
 	Vector2f predictFlowVelBody();
 #endif // CONFIG_EKF2_OPTICAL_FLOW
@@ -950,6 +921,8 @@ 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);
@@ -957,6 +930,12 @@ 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)

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -89,7 +89,7 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons
 			current_pos_available = true;
 		}
 
-		const float gps_alt_ref_prev = getEkfGlobalOriginAltitude();
+		const float gps_alt_ref_prev = _gps_alt_ref;
 
 		// reinitialize map projection to latitude, longitude, altitude, and reset position
 		_pos_ref.initReference(latitude, longitude, _time_delayed_us);
@@ -114,30 +114,24 @@ 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
+			// reset horizontal position if we already have a global origin
 			Vector2f position = _pos_ref.project(current_lat, current_lon);
-
-			if (Vector2f(position - Vector2f(_state.pos)).longerThan(MIN_RESET_DIST_M)) {
-				resetHorizontalPositionTo(position);
-			}
+			resetHorizontalPositionTo(position);
 		}
 
-		// reset vertical position (if there's any change)
-		if (fabsf(altitude - gps_alt_ref_prev) > MIN_RESET_DIST_M) {
+		if (PX4_ISFINITE(gps_alt_ref_prev) && isVerticalPositionAidingActive()) {
 			// determine current z
-			float current_alt = -_state.pos(2) + gps_alt_ref_prev;
-
+			const float z_prev = _state.pos(2);
+			const float current_alt = -z_prev + 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)
-			// preserve GPS height bias
+			// adjust existing GPS height bias
 			_gps_hgt_b_est.setBias(gps_hgt_bias);
 #endif // CONFIG_EKF2_GNSS
 		}
@@ -211,7 +205,7 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
 		if (_control_status.flags.opt_flow) {
 			float gndclearance = math::max(_params.rng_gnd_clearance, 0.1f);
-			vel_err_conservative = math::max((_terrain_vpos - _state.pos(2)), gndclearance) * Vector2f(_aid_src_optical_flow.innovation).norm();
+			vel_err_conservative = math::max(getHagl(), gndclearance) * Vector2f(_aid_src_optical_flow.innovation).norm();
 		}
 #endif // CONFIG_EKF2_OPTICAL_FLOW
 
@@ -271,7 +265,7 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl
 		const float flow_hagl_min = fmaxf(rangefinder_hagl_min, _flow_min_distance);
 		const float flow_hagl_max = fminf(rangefinder_hagl_max, _flow_max_distance);
 
-		const float flow_constrained_height = math::constrain(_terrain_vpos - _state.pos(2), flow_hagl_min, flow_hagl_max);
+		const float flow_constrained_height = math::constrain(getHagl(), flow_hagl_min, flow_hagl_max);
 
 		// Allow ground relative velocity to use 50% of available flow sensor range to allow for angular motion
 		const float flow_vxy_max = 0.5f * _flow_max_rate * flow_constrained_height;
@@ -301,131 +295,187 @@ void Ekf::resetAccelBias()
 	resetAccelBiasCov();
 }
 
-void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, float &pos, float &hgt, float &tas,
-				     float &hagl, float &beta) const
+float Ekf::getHeadingInnovationTestRatio() const
 {
-	// return the integer bitmask containing the consistency check pass/fail status
-	status = _innov_check_fail_status.value;
-
-	// return the largest magnetometer innovation test ratio
-	mag = 0.f;
+	// return the largest heading innovation test ratio
+	float test_ratio = 0.f;
 
 #if defined(CONFIG_EKF2_MAGNETOMETER)
 	if (_control_status.flags.mag_hdg ||_control_status.flags.mag_3D) {
-		mag = math::max(mag, sqrtf(Vector3f(_aid_src_mag.test_ratio).max()));
+		for (auto &test_ratio_filtered : _aid_src_mag.test_ratio_filtered) {
+			test_ratio = math::max(test_ratio, fabsf(test_ratio_filtered));
+		}
 	}
 #endif // CONFIG_EKF2_MAGNETOMETER
 
 #if defined(CONFIG_EKF2_GNSS_YAW)
 	if (_control_status.flags.gps_yaw) {
-		mag = math::max(mag, sqrtf(_aid_src_gnss_yaw.test_ratio));
+		test_ratio = math::max(test_ratio, fabsf(_aid_src_gnss_yaw.test_ratio_filtered));
 	}
 #endif // CONFIG_EKF2_GNSS_YAW
 
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	if (_control_status.flags.ev_yaw) {
-		mag = math::max(mag, sqrtf(_aid_src_ev_yaw.test_ratio));
+		test_ratio = math::max(test_ratio, fabsf(_aid_src_ev_yaw.test_ratio_filtered));
 	}
 #endif // CONFIG_EKF2_EXTERNAL_VISION
 
-	// return the largest velocity and position innovation test ratio
-	vel = NAN;
-	pos = NAN;
+	return sqrtf(test_ratio);
+}
+
+float Ekf::getVelocityInnovationTestRatio() const
+{
+	// return the largest velocity innovation test ratio
+	float test_ratio = -1.f;
 
 #if defined(CONFIG_EKF2_GNSS)
 	if (_control_status.flags.gps) {
-		float gps_vel = sqrtf(Vector3f(_aid_src_gnss_vel.test_ratio).max());
-		vel = math::max(gps_vel, FLT_MIN);
-
-		float gps_pos = sqrtf(Vector2f(_aid_src_gnss_pos.test_ratio).max());
-		pos = math::max(gps_pos, FLT_MIN);
+		for (int i = 0; i < 3; i++) {
+			test_ratio = math::max(test_ratio, fabsf(_aid_src_gnss_vel.test_ratio_filtered[i]));
+		}
 	}
 #endif // CONFIG_EKF2_GNSS
 
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	if (_control_status.flags.ev_vel) {
-		float ev_vel = sqrtf(Vector3f(_aid_src_ev_vel.test_ratio).max());
-		vel = math::max(vel, ev_vel, FLT_MIN);
-	}
-
-	if (_control_status.flags.ev_pos) {
-		float ev_pos = sqrtf(Vector2f(_aid_src_ev_pos.test_ratio).max());
-		pos = math::max(pos, ev_pos, FLT_MIN);
+		for (int i = 0; i < 3; i++) {
+			test_ratio = math::max(test_ratio, fabsf(_aid_src_ev_vel.test_ratio_filtered[i]));
+		}
 	}
 #endif // CONFIG_EKF2_EXTERNAL_VISION
 
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
 	if (isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow)) {
-		float of_vel = sqrtf(Vector2f(_aid_src_optical_flow.test_ratio).max());
-		vel = math::max(of_vel, FLT_MIN);
+		for (auto &test_ratio_filtered : _aid_src_optical_flow.test_ratio_filtered) {
+			test_ratio = math::max(test_ratio, fabsf(test_ratio_filtered));
+		}
 	}
 #endif // CONFIG_EKF2_OPTICAL_FLOW
 
+	if (PX4_ISFINITE(test_ratio) && (test_ratio >= 0.f)) {
+		return sqrtf(test_ratio);
+	}
+
+	return NAN;
+}
+
+float Ekf::getHorizontalPositionInnovationTestRatio() const
+{
+	// return the largest position innovation test ratio
+	float test_ratio = -1.f;
+
+#if defined(CONFIG_EKF2_GNSS)
+	if (_control_status.flags.gps) {
+		for (auto &test_ratio_filtered : _aid_src_gnss_pos.test_ratio_filtered) {
+			test_ratio = math::max(test_ratio, fabsf(test_ratio_filtered));
+		}
+	}
+#endif // CONFIG_EKF2_GNSS
+
+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
+	if (_control_status.flags.ev_pos) {
+		for (auto &test_ratio_filtered : _aid_src_ev_pos.test_ratio_filtered) {
+			test_ratio = math::max(test_ratio, fabsf(test_ratio_filtered));
+		}
+	}
+#endif // CONFIG_EKF2_EXTERNAL_VISION
+
+#if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME)
+	if (_control_status.flags.aux_gpos) {
+		test_ratio = math::max(test_ratio, fabsf(_aux_global_position.test_ratio_filtered()));
+	}
+#endif // CONFIG_EKF2_AUX_GLOBAL_POSITION
+
+	if (PX4_ISFINITE(test_ratio) && (test_ratio >= 0.f)) {
+		return sqrtf(test_ratio);
+	}
+
+	return NAN;
+}
+
+float Ekf::getVerticalPositionInnovationTestRatio() const
+{
 	// return the combined vertical position innovation test ratio
 	float hgt_sum = 0.f;
 	int n_hgt_sources = 0;
 
 #if defined(CONFIG_EKF2_BAROMETER)
 	if (_control_status.flags.baro_hgt) {
-		hgt_sum += sqrtf(_aid_src_baro_hgt.test_ratio);
+		hgt_sum += sqrtf(fabsf(_aid_src_baro_hgt.test_ratio_filtered));
 		n_hgt_sources++;
 	}
 #endif // CONFIG_EKF2_BAROMETER
 
 #if defined(CONFIG_EKF2_GNSS)
 	if (_control_status.flags.gps_hgt) {
-		hgt_sum += sqrtf(_aid_src_gnss_hgt.test_ratio);
+		hgt_sum += sqrtf(fabsf(_aid_src_gnss_hgt.test_ratio_filtered));
 		n_hgt_sources++;
 	}
 #endif // CONFIG_EKF2_GNSS
 
 #if defined(CONFIG_EKF2_RANGE_FINDER)
 	if (_control_status.flags.rng_hgt) {
-		hgt_sum += sqrtf(_aid_src_rng_hgt.test_ratio);
+		hgt_sum += sqrtf(fabsf(_aid_src_rng_hgt.test_ratio_filtered));
 		n_hgt_sources++;
 	}
 #endif // CONFIG_EKF2_RANGE_FINDER
 
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	if (_control_status.flags.ev_hgt) {
-		hgt_sum += sqrtf(_aid_src_ev_hgt.test_ratio);
+		hgt_sum += sqrtf(fabsf(_aid_src_ev_hgt.test_ratio_filtered));
 		n_hgt_sources++;
 	}
 #endif // CONFIG_EKF2_EXTERNAL_VISION
 
 	if (n_hgt_sources > 0) {
-		hgt = math::max(hgt_sum / static_cast<float>(n_hgt_sources), FLT_MIN);
-
-	} else {
-		hgt = NAN;
+		return math::max(hgt_sum / static_cast<float>(n_hgt_sources), FLT_MIN);
 	}
 
+	return NAN;
+}
+
+float Ekf::getAirspeedInnovationTestRatio() const
+{
 #if defined(CONFIG_EKF2_AIRSPEED)
-	// return the airspeed fusion innovation test ratio
-	tas = sqrtf(_aid_src_airspeed.test_ratio);
+	if (_control_status.flags.fuse_aspd) {
+		// return the airspeed fusion innovation test ratio
+		return sqrtf(fabsf(_aid_src_airspeed.test_ratio_filtered));
+	}
 #endif // CONFIG_EKF2_AIRSPEED
 
-	hagl = NAN;
+	return NAN;
+}
+
+float Ekf::getSyntheticSideslipInnovationTestRatio() const
+{
+#if defined(CONFIG_EKF2_SIDESLIP)
+	if (_control_status.flags.fuse_beta) {
+		// return the synthetic sideslip innovation test ratio
+		return sqrtf(fabsf(_aid_src_sideslip.test_ratio_filtered));
+	}
+#endif // CONFIG_EKF2_SIDESLIP
+
+	return NAN;
+}
+
+float Ekf::getHeightAboveGroundInnovationTestRatio() const
+{
+	float test_ratio = -1.f;
+
 #if defined(CONFIG_EKF2_TERRAIN)
 # if defined(CONFIG_EKF2_RANGE_FINDER)
 	if (_hagl_sensor_status.flags.range_finder) {
 		// return the terrain height innovation test ratio
-		hagl = sqrtf(_aid_src_terrain_range_finder.test_ratio);
+		test_ratio = math::max(test_ratio, fabsf(_aid_src_rng_hgt.test_ratio_filtered));
 	}
-#endif // CONFIG_EKF2_RANGE_FINDER
-
-# if defined(CONFIG_EKF2_OPTICAL_FLOW)
-	if (_hagl_sensor_status.flags.flow) {
-		// return the terrain height innovation test ratio
-		hagl = sqrtf(math::max(_aid_src_terrain_optical_flow.test_ratio[0], _aid_src_terrain_optical_flow.test_ratio[1]));
-	}
-# endif // CONFIG_EKF2_OPTICAL_FLOW
+# endif // CONFIG_EKF2_RANGE_FINDER
 #endif // CONFIG_EKF2_TERRAIN
 
-#if defined(CONFIG_EKF2_SIDESLIP)
-	// return the synthetic sideslip innovation test ratio
-	beta = sqrtf(_aid_src_sideslip.test_ratio);
-#endif // CONFIG_EKF2_SIDESLIP
+	if (PX4_ISFINITE(test_ratio) && (test_ratio >= 0.f)) {
+		return sqrtf(test_ratio);
+	}
+
+	return NAN;
 }
 
 void Ekf::get_ekf_soln_status(uint16_t *status) const
@@ -504,6 +554,10 @@ void Ekf::fuse(const VectorState &K, float innovation)
 		_state.wind_vel = matrix::constrain(_state.wind_vel - K.slice<State::wind_vel.dof, 1>(State::wind_vel.idx, 0) * innovation, -1.e2f, 1.e2f);
 	}
 #endif // CONFIG_EKF2_WIND
+
+#if defined(CONFIG_EKF2_TERRAIN)
+	_state.terrain = math::constrain(_state.terrain - K(State::terrain.idx) * innovation, -1e4f, 1e4f);
+#endif // CONFIG_EKF2_TERRAIN
 }
 
 void Ekf::updateDeadReckoningStatus()
@@ -514,44 +568,91 @@ void Ekf::updateDeadReckoningStatus()
 
 void Ekf::updateHorizontalDeadReckoningstatus()
 {
-	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 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;
+	}
 
-	bool optFlowAiding = false;
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
-	optFlowAiding = _control_status.flags.opt_flow && isRecent(_aid_src_optical_flow.time_last_fuse, _params.no_aid_timeout_max);
+	// 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;
+		}
+	}
 #endif // CONFIG_EKF2_OPTICAL_FLOW
 
-	bool airDataAiding = false;
-
 #if defined(CONFIG_EKF2_AIRSPEED)
-	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 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;
 
-	_control_status.flags.wind_dead_reckoning = !velPosAiding && !optFlowAiding && airDataAiding;
-#else
-	_control_status.flags.wind_dead_reckoning = false;
+		// 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;
+		}
+	}
 #endif // CONFIG_EKF2_AIRSPEED
 
-	_control_status.flags.inertial_dead_reckoning = !velPosAiding && !optFlowAiding && !airDataAiding;
-
-	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;
+	// 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;
 		}
 	}
 
-	// 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 (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 (_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;
 
-	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;
+	_control_status.flags.inertial_dead_reckoning = inertial_dead_reckoning;
 }
 
 void Ekf::updateVerticalDeadReckoningStatus()
@@ -605,7 +706,7 @@ void Ekf::updateGroundEffect()
 #if defined(CONFIG_EKF2_TERRAIN)
 		if (isTerrainEstimateValid()) {
 			// automatically set ground effect if terrain is valid
-			float height = _terrain_vpos - _state.pos(2);
+			float height = getHagl();
 			_control_status.flags.gnd_effect = (height < _params.gnd_effect_max_hgt);
 
 		} else

src/modules/ekf2/EKF/height_control.cpp

@@ -53,7 +53,7 @@ void Ekf::controlHeightFusion(const imuSample &imu_delayed)
 #endif // CONFIG_EKF2_GNSS
 
 #if defined(CONFIG_EKF2_RANGE_FINDER)
-	controlRangeHeightFusion();
+	controlRangeHaglFusion();
 #endif // CONFIG_EKF2_RANGE_FINDER
 
 	checkHeightSensorRefFallback();
@@ -230,22 +230,32 @@ void Ekf::checkVerticalAccelerationHealth(const imuSample &imu_delayed)
 
 	Likelihood inertial_nav_falling_likelihood = estimateInertialNavFallingLikelihood();
 
-	// Check for more than 50% clipping affected IMU samples within the past 1 second
-	const uint16_t clip_count_limit = 1.f / _dt_ekf_avg;
-	const bool is_clipping = imu_delayed.delta_vel_clipping[0] ||
-				 imu_delayed.delta_vel_clipping[1] ||
-				 imu_delayed.delta_vel_clipping[2];
+	const uint16_t kClipCountLimit = 1.f / _dt_ekf_avg;
 
-	if (is_clipping && _clip_counter < clip_count_limit) {
-		_clip_counter++;
+	bool acc_clip_warning[3] {};
+	bool acc_clip_critical[3] {};
 
-	} else if (_clip_counter > 0) {
-		_clip_counter--;
+	for (int axis = 0; axis < 3; axis++) {
+		if (imu_delayed.delta_vel_clipping[axis] && (_clip_counter[axis] < kClipCountLimit)) {
+			_clip_counter[axis]++;
+
+		} else if (_clip_counter[axis] > 0) {
+			_clip_counter[axis]--;
+		}
+
+		// warning if more than 50% clipping affected IMU samples within the past 1 second
+		acc_clip_warning[axis] = _clip_counter[axis] >= kClipCountLimit / 2;
+		acc_clip_critical[axis] = _clip_counter[axis] >= kClipCountLimit;
 	}
 
-	_fault_status.flags.bad_acc_clipping = _clip_counter > clip_count_limit / 2;
+	// bad_acc_clipping if ALL axes are reporting warning or if ANY axis is critical
+	const bool all_axis_warning = (acc_clip_warning[0] && acc_clip_warning[1] && acc_clip_warning[2]);
+	const bool any_axis_critical = (acc_clip_critical[0] || acc_clip_critical[1] || acc_clip_critical[2]);
 
-	const bool is_clipping_frequently = _clip_counter > 0;
+	_fault_status.flags.bad_acc_clipping = all_axis_warning || any_axis_critical;
+
+	// if Z axis is warning or any other axis critical
+	const bool is_clipping_frequently = acc_clip_warning[2] || _fault_status.flags.bad_acc_clipping;
 
 	// Do not require evidence of clipping if the likelihood of having the INS falling is high
 	const bool bad_vert_accel = (is_clipping_frequently && (inertial_nav_falling_likelihood == Likelihood::MEDIUM))
@@ -301,7 +311,8 @@ Likelihood Ekf::estimateInertialNavFallingLikelihood() const
 
 #if defined(CONFIG_EKF2_RANGE_FINDER)
 	if (_control_status.flags.rng_hgt) {
-		checks[3] = {ReferenceType::GROUND, _aid_src_rng_hgt.innovation, _aid_src_rng_hgt.innovation_variance};
+		// Range is a distance to ground measurement, not a direct height observation and has an opposite sign
+		checks[3] = {ReferenceType::GROUND, -_aid_src_rng_hgt.innovation, _aid_src_rng_hgt.innovation_variance};
 	}
 #endif // CONFIG_EKF2_RANGE_FINDER
 

src/modules/ekf2/EKF/position_fusion.cpp

@@ -161,12 +161,9 @@ void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_v
 #if defined(CONFIG_EKF2_GNSS)
 	_gps_hgt_b_est.setBias(_gps_hgt_b_est.getBias() + delta_z);
 #endif // CONFIG_EKF2_GNSS
-#if defined(CONFIG_EKF2_RANGE_FINDER)
-	_rng_hgt_b_est.setBias(_rng_hgt_b_est.getBias() + delta_z);
-#endif // CONFIG_EKF2_RANGE_FINDER
 
 #if defined(CONFIG_EKF2_TERRAIN)
-	terrainHandleVerticalPositionReset(delta_z);
+	_state.terrain += delta_z;
 #endif
 
 	// Reset the timout timer

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

@@ -68,7 +68,8 @@ State = Values(
     accel_bias = sf.V3(),
     mag_I = sf.V3(),
     mag_B = sf.V3(),
-    wind_vel = sf.V2()
+    wind_vel = sf.V2(),
+    terrain = sf.V1()
 )
 
 if args.disable_mag:
@@ -132,7 +133,8 @@ def predict_covariance(
         accel_bias = sf.V3.symbolic("delta_a_b"),
         mag_I = sf.V3.symbolic("mag_I"),
         mag_B = sf.V3.symbolic("mag_B"),
-        wind_vel = sf.V2.symbolic("wind_vel")
+        wind_vel = sf.V2.symbolic("wind_vel"),
+        terrain = sf.V1.symbolic("terrain")
     )
 
     if args.disable_mag:
@@ -360,6 +362,40 @@ 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"]
@@ -456,7 +492,10 @@ def compute_mag_declination_pred_innov_var_and_h(
 
     return (meas_pred, innov_var, H.T)
 
-def predict_opt_flow(state, distance, epsilon):
+def predict_hagl(state):
+    return state["terrain"][0] - state["pos"][2]
+
+def predict_opt_flow(state, epsilon):
     R_to_body = state["quat_nominal"].inverse()
 
     # Calculate earth relative velocity in a non-rotating sensor frame
@@ -464,23 +503,23 @@ def predict_opt_flow(state, distance, epsilon):
 
     # Divide by range to get predicted angular LOS rates relative to X and Y
     # axes. Note these are rates in a non-rotating sensor frame
+    hagl = predict_hagl(state)
+    hagl = add_epsilon_sign(hagl, hagl, epsilon)
+    R_to_earth = state["quat_nominal"].to_rotation_matrix()
     flow_pred = sf.V2()
-    flow_pred[0] =  rel_vel_sensor[1] / distance
-    flow_pred[1] = -rel_vel_sensor[0] / distance
-    flow_pred = add_epsilon_sign(flow_pred, distance, epsilon)
+    flow_pred[0] =  rel_vel_sensor[1] / hagl * R_to_earth[2, 2]
+    flow_pred[1] = -rel_vel_sensor[0] / hagl * R_to_earth[2, 2]
 
     return flow_pred
 
-
 def compute_flow_xy_innov_var_and_hx(
         state: VState,
         P: MTangent,
-        distance: sf.Scalar,
         R: sf.Scalar,
         epsilon: sf.Scalar
 ) -> (sf.V2, VTangent):
     state = vstate_to_state(state)
-    meas_pred = predict_opt_flow(state, distance, epsilon);
+    meas_pred = predict_opt_flow(state, epsilon)
 
     innov_var = sf.V2()
     Hx = jacobian_chain_rule(meas_pred[0], state)
@@ -493,18 +532,40 @@ def compute_flow_xy_innov_var_and_hx(
 def compute_flow_y_innov_var_and_h(
         state: VState,
         P: MTangent,
-        distance: sf.Scalar,
         R: sf.Scalar,
         epsilon: sf.Scalar
 ) -> (sf.Scalar, VTangent):
     state = vstate_to_state(state)
-    meas_pred = predict_opt_flow(state, distance, epsilon);
+    meas_pred = predict_opt_flow(state, epsilon)
 
     Hy = jacobian_chain_rule(meas_pred[1], state)
     innov_var = (Hy * P * Hy.T + R)[0,0]
 
     return (innov_var, Hy.T)
 
+def compute_hagl_innov_var(
+        P: MTangent,
+        R: sf.Scalar,
+) -> (sf.Scalar):
+    state = VState.symbolic("state")
+    state = vstate_to_state(state)
+    meas_pred = predict_hagl(state)
+
+    H = jacobian_chain_rule(meas_pred, state)
+    innov_var = (H * P * H.T + R)[0,0]
+
+    return (innov_var)
+
+def compute_hagl_h(
+) -> (VTangent):
+    state = VState.symbolic("state")
+    state = vstate_to_state(state)
+    meas_pred = predict_hagl(state)
+
+    H = jacobian_chain_rule(meas_pred, state)
+
+    return (H.T)
+
 def compute_gnss_yaw_pred_innov_var_and_h(
         state: VState,
         P: MTangent,
@@ -664,9 +725,14 @@ 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)

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

@@ -16,21 +16,21 @@ namespace sym {
  * Symbolic function: compute_airspeed_h_and_k
  *
  * Args:
- *     state: Matrix24_1
- *     P: Matrix23_23
+ *     state: Matrix25_1
+ *     P: Matrix24_24
  *     innov_var: Scalar
  *     epsilon: Scalar
  *
  * Outputs:
- *     H: Matrix23_1
- *     K: Matrix23_1
+ *     H: Matrix24_1
+ *     K: Matrix24_1
  */
 template <typename Scalar>
-void ComputeAirspeedHAndK(const matrix::Matrix<Scalar, 24, 1>& state,
-                          const matrix::Matrix<Scalar, 23, 23>& P, const Scalar innov_var,
-                          const Scalar epsilon, matrix::Matrix<Scalar, 23, 1>* const H = nullptr,
-                          matrix::Matrix<Scalar, 23, 1>* const K = nullptr) {
-  // Total ops: 246
+void ComputeAirspeedHAndK(const matrix::Matrix<Scalar, 25, 1>& state,
+                          const matrix::Matrix<Scalar, 24, 24>& P, const Scalar innov_var,
+                          const Scalar epsilon, matrix::Matrix<Scalar, 24, 1>* const H = nullptr,
+                          matrix::Matrix<Scalar, 24, 1>* const K = nullptr) {
+  // Total ops: 256
 
   // Input arrays
 
@@ -47,7 +47,7 @@ void ComputeAirspeedHAndK(const matrix::Matrix<Scalar, 24, 1>& state,
 
   // Output terms (2)
   if (H != nullptr) {
-    matrix::Matrix<Scalar, 23, 1>& _h = (*H);
+    matrix::Matrix<Scalar, 24, 1>& _h = (*H);
 
     _h.setZero();
 
@@ -59,7 +59,7 @@ void ComputeAirspeedHAndK(const matrix::Matrix<Scalar, 24, 1>& state,
   }
 
   if (K != nullptr) {
-    matrix::Matrix<Scalar, 23, 1>& _k = (*K);
+    matrix::Matrix<Scalar, 24, 1>& _k = (*K);
 
     _k(0, 0) = _tmp6 * (-P(0, 21) * _tmp3 - P(0, 22) * _tmp4 + P(0, 3) * _tmp3 + P(0, 4) * _tmp4 +
                         P(0, 5) * _tmp5);
@@ -107,6 +107,8 @@ void ComputeAirspeedHAndK(const matrix::Matrix<Scalar, 24, 1>& state,
                          P(21, 4) * _tmp4 + P(21, 5) * _tmp5);
     _k(22, 0) = _tmp6 * (-P(22, 21) * _tmp3 - P(22, 22) * _tmp4 + P(22, 3) * _tmp3 +
                          P(22, 4) * _tmp4 + P(22, 5) * _tmp5);
+    _k(23, 0) = _tmp6 * (-P(23, 21) * _tmp3 - P(23, 22) * _tmp4 + P(23, 3) * _tmp3 +
+                         P(23, 4) * _tmp4 + P(23, 5) * _tmp5);
   }
 }  // NOLINT(readability/fn_size)
 

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

@@ -16,8 +16,8 @@ namespace sym {
  * Symbolic function: compute_airspeed_innov_and_innov_var
  *
  * Args:
- *     state: Matrix24_1
- *     P: Matrix23_23
+ *     state: Matrix25_1
+ *     P: Matrix24_24
  *     airspeed: Scalar
  *     R: Scalar
  *     epsilon: Scalar
@@ -27,8 +27,8 @@ namespace sym {
  *     innov_var: Scalar
  */
 template <typename Scalar>
-void ComputeAirspeedInnovAndInnovVar(const matrix::Matrix<Scalar, 24, 1>& state,
-                                     const matrix::Matrix<Scalar, 23, 23>& P, const Scalar airspeed,
+void ComputeAirspeedInnovAndInnovVar(const matrix::Matrix<Scalar, 25, 1>& state,
+                                     const matrix::Matrix<Scalar, 24, 24>& P, const Scalar airspeed,
                                      const Scalar R, const Scalar epsilon,
                                      Scalar* const innov = nullptr,
                                      Scalar* const innov_var = nullptr) {

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

@@ -16,8 +16,8 @@ namespace sym {
  * Symbolic function: compute_drag_x_innov_var_and_h
  *
  * Args:
- *     state: Matrix24_1
- *     P: Matrix23_23
+ *     state: Matrix25_1
+ *     P: Matrix24_24
  *     rho: Scalar
  *     cd: Scalar
  *     cm: Scalar
@@ -26,14 +26,14 @@ namespace sym {
  *
  * Outputs:
  *     innov_var: Scalar
- *     Hx: Matrix23_1
+ *     Hx: Matrix24_1
  */
 template <typename Scalar>
-void ComputeDragXInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
-                              const matrix::Matrix<Scalar, 23, 23>& P, const Scalar rho,
+void ComputeDragXInnovVarAndH(const matrix::Matrix<Scalar, 25, 1>& state,
+                              const matrix::Matrix<Scalar, 24, 24>& P, const Scalar rho,
                               const Scalar cd, const Scalar cm, const Scalar R,
                               const Scalar epsilon, Scalar* const innov_var = nullptr,
-                              matrix::Matrix<Scalar, 23, 1>* const Hx = nullptr) {
+                              matrix::Matrix<Scalar, 24, 1>* const Hx = nullptr) {
   // Total ops: 357
 
   // Input arrays
@@ -162,7 +162,7 @@ void ComputeDragXInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
   }
 
   if (Hx != nullptr) {
-    matrix::Matrix<Scalar, 23, 1>& _hx = (*Hx);
+    matrix::Matrix<Scalar, 24, 1>& _hx = (*Hx);
 
     _hx.setZero();
 

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

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

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

@@ -0,0 +1,63 @@
+// -----------------------------------------------------------------------------
+// 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

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

@@ -0,0 +1,67 @@
+// -----------------------------------------------------------------------------
+// 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

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

@@ -0,0 +1,63 @@
+// -----------------------------------------------------------------------------
+// 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

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

@@ -16,117 +16,153 @@ namespace sym {
  * Symbolic function: compute_flow_xy_innov_var_and_hx
  *
  * Args:
- *     state: Matrix24_1
- *     P: Matrix23_23
- *     distance: Scalar
+ *     state: Matrix25_1
+ *     P: Matrix24_24
  *     R: Scalar
  *     epsilon: Scalar
  *
  * Outputs:
  *     innov_var: Matrix21
- *     H: Matrix23_1
+ *     H: Matrix24_1
  */
 template <typename Scalar>
-void ComputeFlowXyInnovVarAndHx(const matrix::Matrix<Scalar, 24, 1>& state,
-                                const matrix::Matrix<Scalar, 23, 23>& P, const Scalar distance,
-                                const Scalar R, const Scalar epsilon,
+void ComputeFlowXyInnovVarAndHx(const matrix::Matrix<Scalar, 25, 1>& state,
+                                const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
+                                const Scalar epsilon,
                                 matrix::Matrix<Scalar, 2, 1>* const innov_var = nullptr,
-                                matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
-  // Total ops: 275
+                                matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
+  // Total ops: 431
 
   // Input arrays
 
-  // Intermediate terms (42)
-  const Scalar _tmp0 = 2 * state(4, 0);
-  const Scalar _tmp1 = 2 * state(1, 0);
-  const Scalar _tmp2 = _tmp1 * state(6, 0);
-  const Scalar _tmp3 = -_tmp0 * state(3, 0) + _tmp2;
-  const Scalar _tmp4 =
-      Scalar(1.0) /
-      (distance + epsilon * (2 * math::min<Scalar>(0, (((distance) > 0) - ((distance) < 0))) + 1));
-  const Scalar _tmp5 = (Scalar(1) / Scalar(2)) * _tmp4;
-  const Scalar _tmp6 = _tmp5 * state(2, 0);
-  const Scalar _tmp7 = 2 * state(3, 0) * state(6, 0);
-  const Scalar _tmp8 = _tmp5 * (_tmp0 * state(1, 0) + _tmp7);
-  const Scalar _tmp9 = 2 * state(0, 0);
-  const Scalar _tmp10 = state(3, 0) * state(5, 0);
-  const Scalar _tmp11 = 2 * state(2, 0);
-  const Scalar _tmp12 = _tmp11 * state(6, 0);
-  const Scalar _tmp13 = -4 * _tmp10 + _tmp12 - _tmp9 * state(4, 0);
-  const Scalar _tmp14 = _tmp5 * state(1, 0);
-  const Scalar _tmp15 = _tmp9 * state(6, 0);
-  const Scalar _tmp16 = _tmp0 * state(2, 0) + _tmp15 - 4 * state(1, 0) * state(5, 0);
-  const Scalar _tmp17 = _tmp5 * state(3, 0);
-  const Scalar _tmp18 = -_tmp13 * _tmp14 + _tmp16 * _tmp17 - _tmp3 * _tmp6 + _tmp8 * state(0, 0);
-  const Scalar _tmp19 = 1 - 2 * std::pow(state(3, 0), Scalar(2));
-  const Scalar _tmp20 = _tmp4 * (_tmp19 - 2 * std::pow(state(1, 0), Scalar(2)));
-  const Scalar _tmp21 = _tmp3 * _tmp5;
-  const Scalar _tmp22 = _tmp5 * state(0, 0);
-  const Scalar _tmp23 =
-      _tmp13 * _tmp6 + _tmp16 * _tmp22 - _tmp21 * state(1, 0) - _tmp8 * state(3, 0);
-  const Scalar _tmp24 =
-      _tmp13 * _tmp22 - _tmp16 * _tmp6 - _tmp21 * state(3, 0) + _tmp8 * state(1, 0);
-  const Scalar _tmp25 = _tmp9 * state(3, 0);
-  const Scalar _tmp26 = _tmp1 * state(2, 0);
-  const Scalar _tmp27 = _tmp4 * (-_tmp25 + _tmp26);
-  const Scalar _tmp28 = _tmp4 * (_tmp11 * state(3, 0) + _tmp9 * state(1, 0));
-  const Scalar _tmp29 = _tmp4 * (_tmp19 - 2 * std::pow(state(2, 0), Scalar(2)));
-  const Scalar _tmp30 = 4 * state(4, 0);
-  const Scalar _tmp31 = _tmp2 - _tmp30 * state(3, 0) + _tmp9 * state(5, 0);
-  const Scalar _tmp32 = 2 * state(5, 0);
-  const Scalar _tmp33 = -_tmp15 - _tmp30 * state(2, 0) + _tmp32 * state(1, 0);
-  const Scalar _tmp34 = _tmp32 * state(2, 0) + _tmp7;
-  const Scalar _tmp35 = 2 * _tmp10 - _tmp12;
-  const Scalar _tmp36 = _tmp35 * _tmp5;
-  const Scalar _tmp37 = _tmp17 * _tmp33 - _tmp22 * _tmp34 - _tmp31 * _tmp6 + _tmp36 * state(1, 0);
-  const Scalar _tmp38 = -_tmp14 * _tmp33 - _tmp22 * _tmp31 + _tmp34 * _tmp6 + _tmp36 * state(3, 0);
-  const Scalar _tmp39 = _tmp14 * _tmp31 - _tmp17 * _tmp34 - _tmp22 * _tmp33 + _tmp35 * _tmp6;
-  const Scalar _tmp40 = _tmp4 * (_tmp25 + _tmp26);
-  const Scalar _tmp41 = _tmp4 * (_tmp1 * state(3, 0) - _tmp9 * state(2, 0));
+  // Intermediate terms (66)
+  const Scalar _tmp0 = state(2, 0) * state(4, 0);
+  const Scalar _tmp1 = state(1, 0) * state(5, 0);
+  const Scalar _tmp2 = 2 * state(6, 0);
+  const Scalar _tmp3 = _tmp2 * state(0, 0);
+  const Scalar _tmp4 = -2 * std::pow(state(2, 0), Scalar(2));
+  const Scalar _tmp5 = 1 - 2 * std::pow(state(1, 0), Scalar(2));
+  const Scalar _tmp6 = _tmp4 + _tmp5;
+  const Scalar _tmp7 = state(24, 0) - state(9, 0);
+  const Scalar _tmp8 =
+      _tmp7 + epsilon * (2 * math::min<Scalar>(0, (((_tmp7) > 0) - ((_tmp7) < 0))) + 1);
+  const Scalar _tmp9 = Scalar(1.0) / (_tmp8);
+  const Scalar _tmp10 = _tmp6 * _tmp9;
+  const Scalar _tmp11 = 2 * state(0, 0) * state(3, 0);
+  const Scalar _tmp12 = 2 * state(1, 0);
+  const Scalar _tmp13 = _tmp12 * state(2, 0);
+  const Scalar _tmp14 = -_tmp11 + _tmp13;
+  const Scalar _tmp15 = 2 * state(2, 0);
+  const Scalar _tmp16 = _tmp12 * state(0, 0) + _tmp15 * state(3, 0);
+  const Scalar _tmp17 = -2 * std::pow(state(3, 0), Scalar(2));
+  const Scalar _tmp18 = _tmp17 + _tmp5;
+  const Scalar _tmp19 = _tmp14 * state(4, 0) + _tmp16 * state(6, 0) + _tmp18 * state(5, 0);
+  const Scalar _tmp20 = 4 * _tmp9;
+  const Scalar _tmp21 = _tmp19 * _tmp20;
+  const Scalar _tmp22 = _tmp10 * (2 * _tmp0 - 4 * _tmp1 + _tmp3) - _tmp21 * state(1, 0);
+  const Scalar _tmp23 = (Scalar(1) / Scalar(2)) * _tmp22;
+  const Scalar _tmp24 = 2 * state(4, 0);
+  const Scalar _tmp25 = 4 * state(3, 0);
+  const Scalar _tmp26 = _tmp2 * state(2, 0);
+  const Scalar _tmp27 = -_tmp24 * state(0, 0) - _tmp25 * state(5, 0) + _tmp26;
+  const Scalar _tmp28 = (Scalar(1) / Scalar(2)) * _tmp10;
+  const Scalar _tmp29 = _tmp28 * state(0, 0);
+  const Scalar _tmp30 = (Scalar(1) / Scalar(2)) * state(3, 0);
+  const Scalar _tmp31 = _tmp2 * state(1, 0);
+  const Scalar _tmp32 = _tmp10 * (-_tmp24 * state(3, 0) + _tmp31);
+  const Scalar _tmp33 = _tmp2 * state(3, 0);
+  const Scalar _tmp34 = _tmp10 * (_tmp24 * state(1, 0) + _tmp33) - _tmp21 * state(2, 0);
+  const Scalar _tmp35 = (Scalar(1) / Scalar(2)) * _tmp34;
+  const Scalar _tmp36 =
+      -_tmp23 * state(2, 0) + _tmp27 * _tmp29 - _tmp30 * _tmp32 + _tmp35 * state(1, 0);
+  const Scalar _tmp37 = _tmp10 * _tmp14;
+  const Scalar _tmp38 = _tmp28 * state(2, 0);
+  const Scalar _tmp39 = (Scalar(1) / Scalar(2)) * _tmp32;
+  const Scalar _tmp40 =
+      _tmp23 * state(0, 0) + _tmp27 * _tmp38 - _tmp30 * _tmp34 - _tmp39 * state(1, 0);
+  const Scalar _tmp41 = _tmp28 * state(1, 0);
+  const Scalar _tmp42 =
+      _tmp22 * _tmp30 - _tmp27 * _tmp41 + _tmp35 * state(0, 0) - _tmp39 * state(2, 0);
+  const Scalar _tmp43 = _tmp10 * _tmp18;
+  const Scalar _tmp44 = _tmp6 / std::pow(_tmp8, Scalar(2));
+  const Scalar _tmp45 = _tmp19 * _tmp44;
+  const Scalar _tmp46 = _tmp10 * _tmp16;
+  const Scalar _tmp47 = _tmp12 * state(3, 0) - _tmp15 * state(0, 0);
+  const Scalar _tmp48 = _tmp10 * _tmp47;
+  const Scalar _tmp49 = _tmp11 + _tmp13;
+  const Scalar _tmp50 = _tmp10 * _tmp49;
+  const Scalar _tmp51 = _tmp17 + _tmp4 + 1;
+  const Scalar _tmp52 = _tmp10 * _tmp51;
+  const Scalar _tmp53 = _tmp47 * state(6, 0) + _tmp49 * state(5, 0) + _tmp51 * state(4, 0);
+  const Scalar _tmp54 = _tmp20 * _tmp53;
+  const Scalar _tmp55 = 2 * state(5, 0);
+  const Scalar _tmp56 = -_tmp10 * (_tmp33 + _tmp55 * state(2, 0)) + _tmp54 * state(1, 0);
+  const Scalar _tmp57 = -_tmp10 * (-4 * _tmp0 + 2 * _tmp1 - _tmp3) + _tmp54 * state(2, 0);
+  const Scalar _tmp58 = (Scalar(1) / Scalar(2)) * _tmp57;
+  const Scalar _tmp59 = -_tmp25 * state(4, 0) + _tmp31 + _tmp55 * state(0, 0);
+  const Scalar _tmp60 = -_tmp26 + _tmp55 * state(3, 0);
+  const Scalar _tmp61 = _tmp30 * _tmp56 + _tmp38 * _tmp60 + _tmp41 * _tmp59 + _tmp58 * state(0, 0);
+  const Scalar _tmp62 = (Scalar(1) / Scalar(2)) * _tmp56;
+  const Scalar _tmp63 = -_tmp30 * _tmp57 - _tmp38 * _tmp59 + _tmp41 * _tmp60 + _tmp62 * state(0, 0);
+  const Scalar _tmp64 =
+      _tmp10 * _tmp30 * _tmp60 - _tmp29 * _tmp59 + _tmp58 * state(1, 0) - _tmp62 * state(2, 0);
+  const Scalar _tmp65 = _tmp44 * _tmp53;
 
   // Output terms (2)
   if (innov_var != nullptr) {
     matrix::Matrix<Scalar, 2, 1>& _innov_var = (*innov_var);
 
-    _innov_var(0, 0) = R +
-                       _tmp18 * (P(0, 1) * _tmp23 + P(1, 1) * _tmp18 + P(2, 1) * _tmp24 +
-                                 P(3, 1) * _tmp27 + P(4, 1) * _tmp20 + P(5, 1) * _tmp28) +
-                       _tmp20 * (P(0, 4) * _tmp23 + P(1, 4) * _tmp18 + P(2, 4) * _tmp24 +
-                                 P(3, 4) * _tmp27 + P(4, 4) * _tmp20 + P(5, 4) * _tmp28) +
-                       _tmp23 * (P(0, 0) * _tmp23 + P(1, 0) * _tmp18 + P(2, 0) * _tmp24 +
-                                 P(3, 0) * _tmp27 + P(4, 0) * _tmp20 + P(5, 0) * _tmp28) +
-                       _tmp24 * (P(0, 2) * _tmp23 + P(1, 2) * _tmp18 + P(2, 2) * _tmp24 +
-                                 P(3, 2) * _tmp27 + P(4, 2) * _tmp20 + P(5, 2) * _tmp28) +
-                       _tmp27 * (P(0, 3) * _tmp23 + P(1, 3) * _tmp18 + P(2, 3) * _tmp24 +
-                                 P(3, 3) * _tmp27 + P(4, 3) * _tmp20 + P(5, 3) * _tmp28) +
-                       _tmp28 * (P(0, 5) * _tmp23 + P(1, 5) * _tmp18 + P(2, 5) * _tmp24 +
-                                 P(3, 5) * _tmp27 + P(4, 5) * _tmp20 + P(5, 5) * _tmp28);
-    _innov_var(1, 0) = R -
-                       _tmp29 * (P(0, 3) * _tmp37 + P(1, 3) * _tmp39 + P(2, 3) * _tmp38 -
-                                 P(3, 3) * _tmp29 - P(4, 3) * _tmp40 - P(5, 3) * _tmp41) +
-                       _tmp37 * (P(0, 0) * _tmp37 + P(1, 0) * _tmp39 + P(2, 0) * _tmp38 -
-                                 P(3, 0) * _tmp29 - P(4, 0) * _tmp40 - P(5, 0) * _tmp41) +
-                       _tmp38 * (P(0, 2) * _tmp37 + P(1, 2) * _tmp39 + P(2, 2) * _tmp38 -
-                                 P(3, 2) * _tmp29 - P(4, 2) * _tmp40 - P(5, 2) * _tmp41) +
-                       _tmp39 * (P(0, 1) * _tmp37 + P(1, 1) * _tmp39 + P(2, 1) * _tmp38 -
-                                 P(3, 1) * _tmp29 - P(4, 1) * _tmp40 - P(5, 1) * _tmp41) -
-                       _tmp40 * (P(0, 4) * _tmp37 + P(1, 4) * _tmp39 + P(2, 4) * _tmp38 -
-                                 P(3, 4) * _tmp29 - P(4, 4) * _tmp40 - P(5, 4) * _tmp41) -
-                       _tmp41 * (P(0, 5) * _tmp37 + P(1, 5) * _tmp39 + P(2, 5) * _tmp38 -
-                                 P(3, 5) * _tmp29 - P(4, 5) * _tmp40 - P(5, 5) * _tmp41);
+    _innov_var(0, 0) =
+        R +
+        _tmp36 * (P(0, 2) * _tmp40 + P(1, 2) * _tmp42 + P(2, 2) * _tmp36 - P(23, 2) * _tmp45 +
+                  P(3, 2) * _tmp37 + P(4, 2) * _tmp43 + P(5, 2) * _tmp46 + P(8, 2) * _tmp45) +
+        _tmp37 * (P(0, 3) * _tmp40 + P(1, 3) * _tmp42 + P(2, 3) * _tmp36 - P(23, 3) * _tmp45 +
+                  P(3, 3) * _tmp37 + P(4, 3) * _tmp43 + P(5, 3) * _tmp46 + P(8, 3) * _tmp45) +
+        _tmp40 * (P(0, 0) * _tmp40 + P(1, 0) * _tmp42 + P(2, 0) * _tmp36 - P(23, 0) * _tmp45 +
+                  P(3, 0) * _tmp37 + P(4, 0) * _tmp43 + P(5, 0) * _tmp46 + P(8, 0) * _tmp45) +
+        _tmp42 * (P(0, 1) * _tmp40 + P(1, 1) * _tmp42 + P(2, 1) * _tmp36 - P(23, 1) * _tmp45 +
+                  P(3, 1) * _tmp37 + P(4, 1) * _tmp43 + P(5, 1) * _tmp46 + P(8, 1) * _tmp45) +
+        _tmp43 * (P(0, 4) * _tmp40 + P(1, 4) * _tmp42 + P(2, 4) * _tmp36 - P(23, 4) * _tmp45 +
+                  P(3, 4) * _tmp37 + P(4, 4) * _tmp43 + P(5, 4) * _tmp46 + P(8, 4) * _tmp45) -
+        _tmp45 * (P(0, 23) * _tmp40 + P(1, 23) * _tmp42 + P(2, 23) * _tmp36 - P(23, 23) * _tmp45 +
+                  P(3, 23) * _tmp37 + P(4, 23) * _tmp43 + P(5, 23) * _tmp46 + P(8, 23) * _tmp45) +
+        _tmp45 * (P(0, 8) * _tmp40 + P(1, 8) * _tmp42 + P(2, 8) * _tmp36 - P(23, 8) * _tmp45 +
+                  P(3, 8) * _tmp37 + P(4, 8) * _tmp43 + P(5, 8) * _tmp46 + P(8, 8) * _tmp45) +
+        _tmp46 * (P(0, 5) * _tmp40 + P(1, 5) * _tmp42 + P(2, 5) * _tmp36 - P(23, 5) * _tmp45 +
+                  P(3, 5) * _tmp37 + P(4, 5) * _tmp43 + P(5, 5) * _tmp46 + P(8, 5) * _tmp45);
+    _innov_var(1, 0) =
+        R -
+        _tmp48 * (P(0, 5) * _tmp63 + P(1, 5) * _tmp61 + P(2, 5) * _tmp64 + P(23, 5) * _tmp65 -
+                  P(3, 5) * _tmp52 - P(4, 5) * _tmp50 - P(5, 5) * _tmp48 - P(8, 5) * _tmp65) -
+        _tmp50 * (P(0, 4) * _tmp63 + P(1, 4) * _tmp61 + P(2, 4) * _tmp64 + P(23, 4) * _tmp65 -
+                  P(3, 4) * _tmp52 - P(4, 4) * _tmp50 - P(5, 4) * _tmp48 - P(8, 4) * _tmp65) -
+        _tmp52 * (P(0, 3) * _tmp63 + P(1, 3) * _tmp61 + P(2, 3) * _tmp64 + P(23, 3) * _tmp65 -
+                  P(3, 3) * _tmp52 - P(4, 3) * _tmp50 - P(5, 3) * _tmp48 - P(8, 3) * _tmp65) +
+        _tmp61 * (P(0, 1) * _tmp63 + P(1, 1) * _tmp61 + P(2, 1) * _tmp64 + P(23, 1) * _tmp65 -
+                  P(3, 1) * _tmp52 - P(4, 1) * _tmp50 - P(5, 1) * _tmp48 - P(8, 1) * _tmp65) +
+        _tmp63 * (P(0, 0) * _tmp63 + P(1, 0) * _tmp61 + P(2, 0) * _tmp64 + P(23, 0) * _tmp65 -
+                  P(3, 0) * _tmp52 - P(4, 0) * _tmp50 - P(5, 0) * _tmp48 - P(8, 0) * _tmp65) +
+        _tmp64 * (P(0, 2) * _tmp63 + P(1, 2) * _tmp61 + P(2, 2) * _tmp64 + P(23, 2) * _tmp65 -
+                  P(3, 2) * _tmp52 - P(4, 2) * _tmp50 - P(5, 2) * _tmp48 - P(8, 2) * _tmp65) +
+        _tmp65 * (P(0, 23) * _tmp63 + P(1, 23) * _tmp61 + P(2, 23) * _tmp64 + P(23, 23) * _tmp65 -
+                  P(3, 23) * _tmp52 - P(4, 23) * _tmp50 - P(5, 23) * _tmp48 - P(8, 23) * _tmp65) -
+        _tmp65 * (P(0, 8) * _tmp63 + P(1, 8) * _tmp61 + P(2, 8) * _tmp64 + P(23, 8) * _tmp65 -
+                  P(3, 8) * _tmp52 - P(4, 8) * _tmp50 - P(5, 8) * _tmp48 - P(8, 8) * _tmp65);
   }
 
   if (H != nullptr) {
-    matrix::Matrix<Scalar, 23, 1>& _h = (*H);
+    matrix::Matrix<Scalar, 24, 1>& _h = (*H);
 
     _h.setZero();
 
-    _h(0, 0) = _tmp23;
-    _h(1, 0) = _tmp18;
-    _h(2, 0) = _tmp24;
-    _h(3, 0) = _tmp27;
-    _h(4, 0) = _tmp20;
-    _h(5, 0) = _tmp28;
+    _h(0, 0) = _tmp40;
+    _h(1, 0) = _tmp42;
+    _h(2, 0) = _tmp36;
+    _h(3, 0) = _tmp37;
+    _h(4, 0) = _tmp43;
+    _h(5, 0) = _tmp46;
+    _h(8, 0) = _tmp45;
+    _h(23, 0) = -_tmp45;
   }
 }  // NOLINT(readability/fn_size)
 

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

@@ -16,85 +16,100 @@ namespace sym {
  * Symbolic function: compute_flow_y_innov_var_and_h
  *
  * Args:
- *     state: Matrix24_1
- *     P: Matrix23_23
- *     distance: Scalar
+ *     state: Matrix25_1
+ *     P: Matrix24_24
  *     R: Scalar
  *     epsilon: Scalar
  *
  * Outputs:
  *     innov_var: Scalar
- *     H: Matrix23_1
+ *     H: Matrix24_1
  */
 template <typename Scalar>
-void ComputeFlowYInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
-                              const matrix::Matrix<Scalar, 23, 23>& P, const Scalar distance,
-                              const Scalar R, const Scalar epsilon,
-                              Scalar* const innov_var = nullptr,
-                              matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
-  // Total ops: 151
+void ComputeFlowYInnovVarAndH(const matrix::Matrix<Scalar, 25, 1>& state,
+                              const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
+                              const Scalar epsilon, Scalar* const innov_var = nullptr,
+                              matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
+  // Total ops: 232
 
   // Input arrays
 
-  // Intermediate terms (21)
-  const Scalar _tmp0 =
-      Scalar(1.0) /
-      (distance + epsilon * (2 * math::min<Scalar>(0, (((distance) > 0) - ((distance) < 0))) + 1));
-  const Scalar _tmp1 =
-      _tmp0 * (-2 * std::pow(state(2, 0), Scalar(2)) - 2 * std::pow(state(3, 0), Scalar(2)) + 1);
-  const Scalar _tmp2 = 4 * state(4, 0);
-  const Scalar _tmp3 = 2 * state(0, 0);
-  const Scalar _tmp4 = 2 * state(6, 0);
-  const Scalar _tmp5 = -_tmp2 * state(3, 0) + _tmp3 * state(5, 0) + _tmp4 * state(1, 0);
-  const Scalar _tmp6 = (Scalar(1) / Scalar(2)) * _tmp0;
-  const Scalar _tmp7 = _tmp5 * _tmp6;
-  const Scalar _tmp8 = 2 * state(1, 0);
-  const Scalar _tmp9 = -_tmp2 * state(2, 0) - _tmp4 * state(0, 0) + _tmp8 * state(5, 0);
-  const Scalar _tmp10 = _tmp6 * _tmp9;
-  const Scalar _tmp11 = 2 * state(5, 0);
-  const Scalar _tmp12 = _tmp6 * (_tmp11 * state(2, 0) + _tmp4 * state(3, 0));
-  const Scalar _tmp13 = _tmp11 * state(3, 0) - _tmp4 * state(2, 0);
-  const Scalar _tmp14 = _tmp6 * state(1, 0);
-  const Scalar _tmp15 =
-      _tmp10 * state(3, 0) - _tmp12 * state(0, 0) + _tmp13 * _tmp14 - _tmp7 * state(2, 0);
-  const Scalar _tmp16 = _tmp13 * _tmp6;
-  const Scalar _tmp17 =
-      _tmp12 * state(2, 0) - _tmp14 * _tmp9 + _tmp16 * state(3, 0) - _tmp7 * state(0, 0);
+  // Intermediate terms (28)
+  const Scalar _tmp0 = 2 * state(0, 0);
+  const Scalar _tmp1 = 2 * state(1, 0);
+  const Scalar _tmp2 = -_tmp0 * state(2, 0) + _tmp1 * state(3, 0);
+  const Scalar _tmp3 = 1 - 2 * std::pow(state(2, 0), Scalar(2));
+  const Scalar _tmp4 = _tmp3 - 2 * std::pow(state(1, 0), Scalar(2));
+  const Scalar _tmp5 = state(24, 0) - state(9, 0);
+  const Scalar _tmp6 =
+      _tmp5 + epsilon * (2 * math::min<Scalar>(0, (((_tmp5) > 0) - ((_tmp5) < 0))) + 1);
+  const Scalar _tmp7 = Scalar(1.0) / (_tmp6);
+  const Scalar _tmp8 = _tmp4 * _tmp7;
+  const Scalar _tmp9 = _tmp2 * _tmp8;
+  const Scalar _tmp10 = _tmp0 * state(3, 0) + _tmp1 * state(2, 0);
+  const Scalar _tmp11 = _tmp10 * _tmp8;
+  const Scalar _tmp12 = _tmp3 - 2 * std::pow(state(3, 0), Scalar(2));
+  const Scalar _tmp13 = _tmp12 * _tmp8;
+  const Scalar _tmp14 = _tmp10 * state(5, 0) + _tmp12 * state(4, 0) + _tmp2 * state(6, 0);
+  const Scalar _tmp15 = 4 * _tmp14 * _tmp7;
+  const Scalar _tmp16 = 2 * state(5, 0);
+  const Scalar _tmp17 = 2 * state(6, 0);
   const Scalar _tmp18 =
-      -_tmp10 * state(0, 0) - _tmp12 * state(3, 0) + _tmp14 * _tmp5 + _tmp16 * state(2, 0);
-  const Scalar _tmp19 = _tmp0 * (_tmp3 * state(3, 0) + _tmp8 * state(2, 0));
-  const Scalar _tmp20 = _tmp0 * (-_tmp3 * state(2, 0) + _tmp8 * state(3, 0));
+      (Scalar(1) / Scalar(2)) * _tmp15 * state(1, 0) -
+      Scalar(1) / Scalar(2) * _tmp8 * (_tmp16 * state(2, 0) + _tmp17 * state(3, 0));
+  const Scalar _tmp19 = 4 * state(4, 0);
+  const Scalar _tmp20 = (Scalar(1) / Scalar(2)) * _tmp15 * state(2, 0) -
+                        Scalar(1) / Scalar(2) * _tmp8 *
+                            (_tmp16 * state(1, 0) - _tmp17 * state(0, 0) - _tmp19 * state(2, 0));
+  const Scalar _tmp21 = (Scalar(1) / Scalar(2)) * _tmp8;
+  const Scalar _tmp22 =
+      _tmp21 * (_tmp16 * state(0, 0) + _tmp17 * state(1, 0) - _tmp19 * state(3, 0));
+  const Scalar _tmp23 = _tmp21 * (_tmp16 * state(3, 0) - _tmp17 * state(2, 0));
+  const Scalar _tmp24 =
+      _tmp18 * state(3, 0) + _tmp20 * state(0, 0) + _tmp22 * state(1, 0) + _tmp23 * state(2, 0);
+  const Scalar _tmp25 =
+      _tmp18 * state(0, 0) - _tmp20 * state(3, 0) - _tmp22 * state(2, 0) + _tmp23 * state(1, 0);
+  const Scalar _tmp26 =
+      -_tmp18 * state(2, 0) + _tmp20 * state(1, 0) - _tmp22 * state(0, 0) + _tmp23 * state(3, 0);
+  const Scalar _tmp27 = _tmp14 * _tmp4 / std::pow(_tmp6, Scalar(2));
 
   // Output terms (2)
   if (innov_var != nullptr) {
     Scalar& _innov_var = (*innov_var);
 
-    _innov_var = R -
-                 _tmp1 * (P(0, 3) * _tmp15 + P(1, 3) * _tmp18 + P(2, 3) * _tmp17 - P(3, 3) * _tmp1 -
-                          P(4, 3) * _tmp19 - P(5, 3) * _tmp20) +
-                 _tmp15 * (P(0, 0) * _tmp15 + P(1, 0) * _tmp18 + P(2, 0) * _tmp17 -
-                           P(3, 0) * _tmp1 - P(4, 0) * _tmp19 - P(5, 0) * _tmp20) +
-                 _tmp17 * (P(0, 2) * _tmp15 + P(1, 2) * _tmp18 + P(2, 2) * _tmp17 -
-                           P(3, 2) * _tmp1 - P(4, 2) * _tmp19 - P(5, 2) * _tmp20) +
-                 _tmp18 * (P(0, 1) * _tmp15 + P(1, 1) * _tmp18 + P(2, 1) * _tmp17 -
-                           P(3, 1) * _tmp1 - P(4, 1) * _tmp19 - P(5, 1) * _tmp20) -
-                 _tmp19 * (P(0, 4) * _tmp15 + P(1, 4) * _tmp18 + P(2, 4) * _tmp17 -
-                           P(3, 4) * _tmp1 - P(4, 4) * _tmp19 - P(5, 4) * _tmp20) -
-                 _tmp20 * (P(0, 5) * _tmp15 + P(1, 5) * _tmp18 + P(2, 5) * _tmp17 -
-                           P(3, 5) * _tmp1 - P(4, 5) * _tmp19 - P(5, 5) * _tmp20);
+    _innov_var =
+        R -
+        _tmp11 * (P(0, 4) * _tmp25 + P(1, 4) * _tmp24 + P(2, 4) * _tmp26 + P(23, 4) * _tmp27 -
+                  P(3, 4) * _tmp13 - P(4, 4) * _tmp11 - P(5, 4) * _tmp9 - P(8, 4) * _tmp27) -
+        _tmp13 * (P(0, 3) * _tmp25 + P(1, 3) * _tmp24 + P(2, 3) * _tmp26 + P(23, 3) * _tmp27 -
+                  P(3, 3) * _tmp13 - P(4, 3) * _tmp11 - P(5, 3) * _tmp9 - P(8, 3) * _tmp27) +
+        _tmp24 * (P(0, 1) * _tmp25 + P(1, 1) * _tmp24 + P(2, 1) * _tmp26 + P(23, 1) * _tmp27 -
+                  P(3, 1) * _tmp13 - P(4, 1) * _tmp11 - P(5, 1) * _tmp9 - P(8, 1) * _tmp27) +
+        _tmp25 * (P(0, 0) * _tmp25 + P(1, 0) * _tmp24 + P(2, 0) * _tmp26 + P(23, 0) * _tmp27 -
+                  P(3, 0) * _tmp13 - P(4, 0) * _tmp11 - P(5, 0) * _tmp9 - P(8, 0) * _tmp27) +
+        _tmp26 * (P(0, 2) * _tmp25 + P(1, 2) * _tmp24 + P(2, 2) * _tmp26 + P(23, 2) * _tmp27 -
+                  P(3, 2) * _tmp13 - P(4, 2) * _tmp11 - P(5, 2) * _tmp9 - P(8, 2) * _tmp27) +
+        _tmp27 * (P(0, 23) * _tmp25 + P(1, 23) * _tmp24 + P(2, 23) * _tmp26 + P(23, 23) * _tmp27 -
+                  P(3, 23) * _tmp13 - P(4, 23) * _tmp11 - P(5, 23) * _tmp9 - P(8, 23) * _tmp27) -
+        _tmp27 * (P(0, 8) * _tmp25 + P(1, 8) * _tmp24 + P(2, 8) * _tmp26 + P(23, 8) * _tmp27 -
+                  P(3, 8) * _tmp13 - P(4, 8) * _tmp11 - P(5, 8) * _tmp9 - P(8, 8) * _tmp27) -
+        _tmp9 * (P(0, 5) * _tmp25 + P(1, 5) * _tmp24 + P(2, 5) * _tmp26 + P(23, 5) * _tmp27 -
+                 P(3, 5) * _tmp13 - P(4, 5) * _tmp11 - P(5, 5) * _tmp9 - P(8, 5) * _tmp27);
   }
 
   if (H != nullptr) {
-    matrix::Matrix<Scalar, 23, 1>& _h = (*H);
+    matrix::Matrix<Scalar, 24, 1>& _h = (*H);
 
     _h.setZero();
 
-    _h(0, 0) = _tmp15;
-    _h(1, 0) = _tmp18;
-    _h(2, 0) = _tmp17;
-    _h(3, 0) = -_tmp1;
-    _h(4, 0) = -_tmp19;
-    _h(5, 0) = -_tmp20;
+    _h(0, 0) = _tmp25;
+    _h(1, 0) = _tmp24;
+    _h(2, 0) = _tmp26;
+    _h(3, 0) = -_tmp13;
+    _h(4, 0) = -_tmp11;
+    _h(5, 0) = -_tmp9;
+    _h(8, 0) = -_tmp27;
+    _h(23, 0) = _tmp27;
   }
 }  // NOLINT(readability/fn_size)
 

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

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

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

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

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

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

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

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

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

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

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

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

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

@@ -16,22 +16,22 @@ namespace sym {
  * Symbolic function: compute_mag_declination_pred_innov_var_and_h
  *
  * Args:
- *     state: Matrix24_1
- *     P: Matrix23_23
+ *     state: Matrix25_1
+ *     P: Matrix24_24
  *     R: Scalar
  *     epsilon: Scalar
  *
  * Outputs:
  *     pred: Scalar
  *     innov_var: Scalar
- *     H: Matrix23_1
+ *     H: Matrix24_1
  */
 template <typename Scalar>
-void ComputeMagDeclinationPredInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
-                                           const matrix::Matrix<Scalar, 23, 23>& P, const Scalar R,
+void ComputeMagDeclinationPredInnovVarAndH(const matrix::Matrix<Scalar, 25, 1>& state,
+                                           const matrix::Matrix<Scalar, 24, 24>& P, const Scalar R,
                                            const Scalar epsilon, Scalar* const pred = nullptr,
                                            Scalar* const innov_var = nullptr,
-                                           matrix::Matrix<Scalar, 23, 1>* const H = nullptr) {
+                                           matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
   // Total ops: 22
 
   // Input arrays
@@ -59,7 +59,7 @@ void ComputeMagDeclinationPredInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>&
   }
 
   if (H != nullptr) {
-    matrix::Matrix<Scalar, 23, 1>& _h = (*H);
+    matrix::Matrix<Scalar, 24, 1>& _h = (*H);
 
     _h.setZero();
 

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

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