diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1011_gazebo-classic_iris_irlock b/ROMFS/px4fmu_common/init.d-posix/airframes/1011_gazebo-classic_iris_irlock
index 2f064e2c5ff0..29776d8f9d33 100644
--- a/ROMFS/px4fmu_common/init.d-posix/airframes/1011_gazebo-classic_iris_irlock
+++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1011_gazebo-classic_iris_irlock
@@ -35,4 +35,7 @@ param set-default PLD_HACC_RAD 0.1
param set-default RTL_PLD_MD 2
# Start up Landing Target Estimator module
-landing_target_estimator start
+if param compare VTE_EN 0
+then
+ landing_target_estimator start
+fi
diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps
index 24a3f81ed705..e826cacf791b 100644
--- a/ROMFS/px4fmu_common/init.d/rc.mc_apps
+++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps
@@ -25,6 +25,15 @@ then
mc_autotune_attitude_control start
fi
+#
+# Start Vision Target Estimator.
+#
+
+if param greater -s VTE_EN 0
+then
+ vision_target_estimator start
+fi
+
#
# Start Multicopter Position Controller.
#
diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_apps b/ROMFS/px4fmu_common/init.d/rc.vtol_apps
index 16fb6ee5d660..2efa29709d9d 100644
--- a/ROMFS/px4fmu_common/init.d/rc.vtol_apps
+++ b/ROMFS/px4fmu_common/init.d/rc.vtol_apps
@@ -25,6 +25,15 @@ then
mc_autotune_attitude_control start
fi
+#
+# Start Vision Target Estimator.
+#
+
+if param greater -s VTE_EN 0
+then
+ vision_target_estimator start
+fi
+
fw_rate_control start vtol
fw_att_control start vtol
fw_mode_manager start
diff --git a/boards/px4/fmu-v6c/visionTargetEst.px4board b/boards/px4/fmu-v6c/visionTargetEst.px4board
new file mode 100644
index 000000000000..1031765cb533
--- /dev/null
+++ b/boards/px4/fmu-v6c/visionTargetEst.px4board
@@ -0,0 +1,95 @@
+CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
+CONFIG_BOARD_ARCHITECTURE="cortex-m7"
+CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS0"
+CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS6"
+CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS5"
+CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS3"
+CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS1"
+CONFIG_DRIVERS_ADC_BOARD_ADC=y
+CONFIG_DRIVERS_BAROMETER_MS5611=y
+CONFIG_DRIVERS_BATT_SMBUS=y
+CONFIG_DRIVERS_CAMERA_CAPTURE=y
+CONFIG_DRIVERS_CAMERA_TRIGGER=y
+CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
+CONFIG_COMMON_DISTANCE_SENSOR=y
+CONFIG_DRIVERS_DSHOT=y
+CONFIG_DRIVERS_GPS=y
+CONFIG_DRIVERS_HEATER=y
+CONFIG_COMMON_INS=n
+CONFIG_COMMON_LIGHT=y
+CONFIG_COMMON_MAGNETOMETER=y
+CONFIG_COMMON_OPTICAL_FLOW=y
+CONFIG_DRIVERS_POWER_MONITOR_INA226=y
+CONFIG_DRIVERS_POWER_MONITOR_INA228=y
+CONFIG_DRIVERS_POWER_MONITOR_INA238=y
+CONFIG_DRIVERS_PWM_OUT=y
+CONFIG_DRIVERS_PX4IO=y
+CONFIG_COMMON_TELEMETRY=y
+CONFIG_DRIVERS_TONE_ALARM=y
+CONFIG_DRIVERS_UAVCAN=y
+CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
+CONFIG_MODULES_AIRSPEED_SELECTOR=y
+CONFIG_MODULES_BATTERY_STATUS=y
+CONFIG_MODULES_COMMANDER=y
+CONFIG_MODULES_CONTROL_ALLOCATOR=y
+CONFIG_MODULES_DATAMAN=y
+CONFIG_MODULES_EKF2=y
+CONFIG_MODULES_ESC_BATTERY=y
+CONFIG_MODULES_EVENTS=y
+CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
+CONFIG_MODULES_FW_ATT_CONTROL=y
+CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
+CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_RATE_CONTROL=y
+CONFIG_MODULES_GYRO_CALIBRATION=y
+CONFIG_MODULES_GYRO_FFT=y
+CONFIG_MODULES_LAND_DETECTOR=y
+CONFIG_MODULES_LOAD_MON=y
+CONFIG_MODULES_LOGGER=y
+CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
+CONFIG_MODULES_MANUAL_CONTROL=y
+CONFIG_MODULES_MAVLINK=y
+CONFIG_MODULES_MC_ATT_CONTROL=y
+CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y
+CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
+CONFIG_MODULES_MC_POS_CONTROL=y
+CONFIG_MODULES_MC_RATE_CONTROL=y
+CONFIG_MODULES_NAVIGATOR=y
+CONFIG_MODULES_RC_UPDATE=y
+CONFIG_MODULES_SENSORS=y
+CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
+CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
+CONFIG_MODULES_VTOL_ATT_CONTROL=y
+CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
+CONFIG_SYSTEMCMDS_BSONDUMP=y
+CONFIG_SYSTEMCMDS_DMESG=y
+CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
+CONFIG_SYSTEMCMDS_I2CDETECT=y
+CONFIG_SYSTEMCMDS_LED_CONTROL=y
+CONFIG_SYSTEMCMDS_MFT=y
+CONFIG_SYSTEMCMDS_MTD=y
+CONFIG_SYSTEMCMDS_NSHTERM=y
+CONFIG_SYSTEMCMDS_PARAM=y
+CONFIG_SYSTEMCMDS_PERF=y
+CONFIG_SYSTEMCMDS_REBOOT=y
+CONFIG_SYSTEMCMDS_SD_BENCH=y
+CONFIG_SYSTEMCMDS_SYSTEM_TIME=y
+CONFIG_SYSTEMCMDS_TOP=y
+CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
+CONFIG_SYSTEMCMDS_TUNE_CONTROL=y
+CONFIG_SYSTEMCMDS_UORB=y
+CONFIG_SYSTEMCMDS_VER=y
+CONFIG_SYSTEMCMDS_WORK_QUEUE=y
+
+CONFIG_MAVLINK_DIALECT="development"
+CONFIG_MODULES_VISION_TARGET_ESTIMATOR=y
+CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=n
+CONFIG_MODULES_ROVER_POS_CONTROL=n
+CONFIG_MODULES_CAMERA_FEEDBACK=n
+CONFIG_MODULES_GIMBAL=n
+CONFIG_MODULES_UXRCE_DDS_CLIENT=y
+CONFIG_UXRCE_DDS_CLIENT_SERIAL=n
+CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=n
+CONFIG_DRIVERS_GNSS_SEPTENTRIO=n
+CONFIG_DRIVERS_IMU_BOSCH_BMI055=n
+CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board
index e2a3996ca364..3e2edf7ea26c 100644
--- a/boards/px4/sitl/default.px4board
+++ b/boards/px4/sitl/default.px4board
@@ -28,7 +28,7 @@ CONFIG_MODULES_GIMBAL=y
CONFIG_MODULES_GYRO_CALIBRATION=y
CONFIG_MODULES_GYRO_FFT=y
CONFIG_MODULES_LAND_DETECTOR=y
-CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
+CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=n
CONFIG_MODULES_LOAD_MON=y
CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y
CONFIG_MODULES_LOGGER=y
@@ -59,6 +59,7 @@ CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
CONFIG_MODULES_UUV_ATT_CONTROL=y
CONFIG_MODULES_UUV_POS_CONTROL=y
CONFIG_MODULES_UXRCE_DDS_CLIENT=y
+CONFIG_MODULES_VISION_TARGET_ESTIMATOR=y
CONFIG_MODULES_VTOL_ATT_CONTROL=y
CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
CONFIG_SYSTEMCMDS_BSONDUMP=y
@@ -84,3 +85,4 @@ CONFIG_EXAMPLES_PX4_MAVLINK_DEBUG=y
CONFIG_EXAMPLES_PX4_SIMPLE_APP=y
CONFIG_EXAMPLES_WORK_ITEM=y
CONFIG_MODULES_SPACECRAFT=n
+CONFIG_MAVLINK_DIALECT="development"
diff --git a/docs/assets/vision_target_estimator/VtestAllObservations.png b/docs/assets/vision_target_estimator/VtestAllObservations.png
new file mode 100644
index 000000000000..6724c710d4bd
Binary files /dev/null and b/docs/assets/vision_target_estimator/VtestAllObservations.png differ
diff --git a/docs/assets/vision_target_estimator/VtestInnovations.png b/docs/assets/vision_target_estimator/VtestInnovations.png
new file mode 100644
index 000000000000..1cac7ea2e8be
Binary files /dev/null and b/docs/assets/vision_target_estimator/VtestInnovations.png differ
diff --git a/docs/assets/vision_target_estimator/VtestMeasNotFused.png b/docs/assets/vision_target_estimator/VtestMeasNotFused.png
new file mode 100644
index 000000000000..50c5d5b76631
Binary files /dev/null and b/docs/assets/vision_target_estimator/VtestMeasNotFused.png differ
diff --git a/docs/assets/vision_target_estimator/VtestOrientation.png b/docs/assets/vision_target_estimator/VtestOrientation.png
new file mode 100644
index 000000000000..0973e02934d9
Binary files /dev/null and b/docs/assets/vision_target_estimator/VtestOrientation.png differ
diff --git a/docs/assets/vision_target_estimator/VtestPrecland.png b/docs/assets/vision_target_estimator/VtestPrecland.png
new file mode 100644
index 000000000000..66587acdf0ab
Binary files /dev/null and b/docs/assets/vision_target_estimator/VtestPrecland.png differ
diff --git a/docs/en/SUMMARY.md b/docs/en/SUMMARY.md
index b90c2635a238..ead812fbbe91 100644
--- a/docs/en/SUMMARY.md
+++ b/docs/en/SUMMARY.md
@@ -20,6 +20,8 @@
- [Offboard Mode (MC)](flight_modes_mc/offboard.md)
- [Collision Prevention](computer_vision/collision_prevention.md)
- [Precision Landing](advanced_features/precland.md)
+ - [Vision Target Estimator](advanced_features/vision_target_estimator.md)
+ - [Vision Target Estimator Deep Dive](advanced_features/vision_target_estimator_advanced.md)
- [Terrain Following/Holding](flying/terrain_following_holding.md)
- [Throw Launch](flight_modes_mc/throw_launch.md)
- [Assembly](assembly/assembly_mc.md)
diff --git a/docs/en/advanced_config/parameter_reference.md b/docs/en/advanced_config/parameter_reference.md
index a6ed64793742..d1c6f1b407a5 100644
--- a/docs/en/advanced_config/parameter_reference.md
+++ b/docs/en/advanced_config/parameter_reference.md
@@ -1851,7 +1851,6 @@ PCA9685 Output Channel 6 Output Function.
Select what should be output on PCA9685 Output Channel 6.
The default failsafe value is set according to the selected function:
-
- 'Min' for ConstantMin
- 'Max' for ConstantMax
- 'Max' for Parachute
@@ -1925,7 +1924,6 @@ PCA9685 Output Channel 7 Output Function.
Select what should be output on PCA9685 Output Channel 7.
The default failsafe value is set according to the selected function:
-
- 'Min' for ConstantMin
- 'Max' for ConstantMax
- 'Max' for Parachute
@@ -1998,6 +1996,76 @@ PCA9685 Output Channel 8 Output Function.
Select what should be output on PCA9685 Output Channel 8.
+The default failsafe value is set according to the selected function:
+- 'Min' for ConstantMin
+- 'Max' for ConstantMax
+- 'Max' for Parachute
+- ('Max'+'Min')/2 for Servos
+- 'Disarmed' for the rest
+
+**Values:**
+
+- `0`: Disabled
+- `1`: Constant Min
+- `2`: Constant Max
+- `101`: Motor 1
+- `102`: Motor 2
+- `103`: Motor 3
+- `104`: Motor 4
+- `105`: Motor 5
+- `106`: Motor 6
+- `107`: Motor 7
+- `108`: Motor 8
+- `109`: Motor 9
+- `110`: Motor 10
+- `111`: Motor 11
+- `112`: Motor 12
+- `201`: Servo 1
+- `202`: Servo 2
+- `203`: Servo 3
+- `204`: Servo 4
+- `205`: Servo 5
+- `206`: Servo 6
+- `207`: Servo 7
+- `208`: Servo 8
+- `301`: Peripheral via Actuator Set 1
+- `302`: Peripheral via Actuator Set 2
+- `303`: Peripheral via Actuator Set 3
+- `304`: Peripheral via Actuator Set 4
+- `305`: Peripheral via Actuator Set 5
+- `306`: Peripheral via Actuator Set 6
+- `400`: Landing Gear
+- `401`: Parachute
+- `402`: RC Roll
+- `403`: RC Pitch
+- `404`: RC Throttle
+- `405`: RC Yaw
+- `406`: RC Flaps
+- `407`: RC AUX 1
+- `408`: RC AUX 2
+- `409`: RC AUX 3
+- `410`: RC AUX 4
+- `411`: RC AUX 5
+- `412`: RC AUX 6
+- `420`: Gimbal Roll
+- `421`: Gimbal Pitch
+- `422`: Gimbal Yaw
+- `430`: Gripper
+- `440`: Landing Gear Wheel
+- `450`: IC Engine Ignition
+- `451`: IC Engine Throttle
+- `452`: IC Engine Choke
+- `453`: IC Engine Starter
+
+| Reboot | minValue | maxValue | increment | default | unit |
+| ------ | -------- | -------- | --------- | ------- | ---- |
+| | | | | 0 |
+
+### PCA9685_FUNC9 (`INT32`) {#PCA9685_FUNC9}
+
+PCA9685 Output Channel 9 Output Function.
+
+Select what should be output on PCA9685 Output Channel 9.
The default failsafe value is set according to the selected function:
- 'Min' for ConstantMin
@@ -2060,6 +2128,76 @@ The default failsafe value is set according to the selected function:
- `452`: IC Engine Choke
- `453`: IC Engine Starter
+| Reboot | minValue | maxValue | increment | default | unit |
+| ------ | -------- | -------- | --------- | ------- | ---- |
+| | | | | 0 |
+
+### PCA9685_FUNC9 (`INT32`) {#PCA9685_FUNC9}
+
+PCA9685 Output Channel 9 Output Function.
+
+Select what should be output on PCA9685 Output Channel 9.
+The default failsafe value is set according to the selected function:
+- 'Min' for ConstantMin
+- 'Max' for ConstantMax
+- 'Max' for Parachute
+- ('Max'+'Min')/2 for Servos
+- 'Disarmed' for the rest
+
+**Values:**
+
+- `0`: Disabled
+- `1`: Constant Min
+- `2`: Constant Max
+- `101`: Motor 1
+- `102`: Motor 2
+- `103`: Motor 3
+- `104`: Motor 4
+- `105`: Motor 5
+- `106`: Motor 6
+- `107`: Motor 7
+- `108`: Motor 8
+- `109`: Motor 9
+- `110`: Motor 10
+- `111`: Motor 11
+- `112`: Motor 12
+- `201`: Servo 1
+- `202`: Servo 2
+- `203`: Servo 3
+- `204`: Servo 4
+- `205`: Servo 5
+- `206`: Servo 6
+- `207`: Servo 7
+- `208`: Servo 8
+- `301`: Peripheral via Actuator Set 1
+- `302`: Peripheral via Actuator Set 2
+- `303`: Peripheral via Actuator Set 3
+- `304`: Peripheral via Actuator Set 4
+- `305`: Peripheral via Actuator Set 5
+- `306`: Peripheral via Actuator Set 6
+- `400`: Landing Gear
+- `401`: Parachute
+- `402`: RC Roll
+- `403`: RC Pitch
+- `404`: RC Throttle
+- `405`: RC Yaw
+- `406`: RC Flaps
+- `407`: RC AUX 1
+- `408`: RC AUX 2
+- `409`: RC AUX 3
+- `410`: RC AUX 4
+- `411`: RC AUX 5
+- `412`: RC AUX 6
+- `420`: Gimbal Roll
+- `421`: Gimbal Pitch
+- `422`: Gimbal Yaw
+- `430`: Gripper
+- `440`: Landing Gear Wheel
+- `450`: IC Engine Ignition
+- `451`: IC Engine Throttle
+- `452`: IC Engine Choke
+- `453`: IC Engine Starter
+
| Reboot | minValue | maxValue | increment | default | unit |
| ------ | -------- | -------- | --------- | ------- | ---- |
| | | | | 0 | |
@@ -3750,7 +3888,6 @@ PWM Aux 8 Output Function.
Select what should be output on PWM Aux 8.
The default failsafe value is set according to the selected function:
-
- 'Min' for ConstantMin
- 'Max' for ConstantMax
- 'Max' for Parachute
@@ -3828,7 +3965,6 @@ PWM Capture 1 Output Function.
Select what should be output on PWM Capture 1.
The default failsafe value is set according to the selected function:
-
- 'Min' for ConstantMin
- 'Max' for ConstantMax
- 'Max' for Parachute
@@ -8959,7 +9095,6 @@ When set to -1 (default), the value depends on the function (see SIM_GZ_WH_FUNC4
SIM_GZ Wheels 1 Output Function.
Select what should be output on SIM_GZ Wheels 1.
-
The default failsafe value is set according to the selected function:
- 'Min' for ConstantMin
@@ -9033,7 +9168,6 @@ The default failsafe value is set according to the selected function:
SIM_GZ Wheels 2 Output Function.
Select what should be output on SIM_GZ Wheels 2.
-
The default failsafe value is set according to the selected function:
- 'Min' for ConstantMin
@@ -9109,7 +9243,6 @@ SIM_GZ Wheels 3 Output Function.
Select what should be output on SIM_GZ Wheels 3.
The default failsafe value is set according to the selected function:
-
- 'Min' for ConstantMin
- 'Max' for ConstantMax
- 'Max' for Parachute
@@ -9889,7 +10022,6 @@ The default failsafe value is set according to the selected function:
TAP ESC Output ESC 8 Output Function.
Select what should be output on TAP ESC Output ESC 8.
-
The default failsafe value is set according to the selected function:
- 'Min' for ConstantMin
@@ -9952,6 +10084,71 @@ The default failsafe value is set according to the selected function:
- `452`: IC Engine Choke
- `453`: IC Engine Starter
+| Reboot | minValue | maxValue | increment | default | unit |
+| ------ | -------- | -------- | --------- | ------- | ---- |
+| | | | | 0 |
+
+### UAVCAN_EC_FUNC2 (`INT32`) {#UAVCAN_EC_FUNC2}
+
+UAVCAN ESC 2 Output Function.
+
+Select what should be output on UAVCAN ESC 2.
+The default failsafe value is set according to the selected function:
+
+**Values:**
+
+- `0`: Disabled
+- `1`: Constant Min
+- `2`: Constant Max
+- `101`: Motor 1
+- `102`: Motor 2
+- `103`: Motor 3
+- `104`: Motor 4
+- `105`: Motor 5
+- `106`: Motor 6
+- `107`: Motor 7
+- `108`: Motor 8
+- `109`: Motor 9
+- `110`: Motor 10
+- `111`: Motor 11
+- `112`: Motor 12
+- `201`: Servo 1
+- `202`: Servo 2
+- `203`: Servo 3
+- `204`: Servo 4
+- `205`: Servo 5
+- `206`: Servo 6
+- `207`: Servo 7
+- `208`: Servo 8
+- `301`: Peripheral via Actuator Set 1
+- `302`: Peripheral via Actuator Set 2
+- `303`: Peripheral via Actuator Set 3
+- `304`: Peripheral via Actuator Set 4
+- `305`: Peripheral via Actuator Set 5
+- `306`: Peripheral via Actuator Set 6
+- `400`: Landing Gear
+- `401`: Parachute
+- `402`: RC Roll
+- `403`: RC Pitch
+- `404`: RC Throttle
+- `405`: RC Yaw
+- `406`: RC Flaps
+- `407`: RC AUX 1
+- `408`: RC AUX 2
+- `409`: RC AUX 3
+- `410`: RC AUX 4
+- `411`: RC AUX 5
+- `412`: RC AUX 6
+- `420`: Gimbal Roll
+- `421`: Gimbal Pitch
+- `422`: Gimbal Yaw
+- `430`: Gripper
+- `440`: Landing Gear Wheel
+- `450`: IC Engine Ignition
+- `451`: IC Engine Throttle
+- `452`: IC Engine Choke
+- `453`: IC Engine Starter
+
| Reboot | minValue | maxValue | increment | default | unit |
| ------ | -------- | -------- | --------- | ------- | ---- |
| | | | | 0 | |
@@ -10675,7 +10872,6 @@ UAVCAN ESC 5 Output Function.
Select what should be output on UAVCAN ESC 5.
The default failsafe value is set according to the selected function:
-
- 'Min' for ConstantMin
- 'Max' for ConstantMax
- 'Max' for Parachute
@@ -10749,7 +10945,6 @@ UAVCAN ESC 6 Output Function.
Select what should be output on UAVCAN ESC 6.
The default failsafe value is set according to the selected function:
-
- 'Min' for ConstantMin
- 'Max' for ConstantMax
- 'Max' for Parachute
@@ -10823,7 +11018,6 @@ UAVCAN ESC 7 Output Function.
Select what should be output on UAVCAN ESC 7.
The default failsafe value is set according to the selected function:
-
- 'Min' for ConstantMin
- 'Max' for ConstantMax
- 'Max' for Parachute
@@ -10971,7 +11165,6 @@ UAVCAN ESC 9 Output Function.
Select what should be output on UAVCAN ESC 9.
The default failsafe value is set according to the selected function:
-
- 'Min' for ConstantMin
- 'Max' for ConstantMax
- 'Max' for Parachute
@@ -12103,7 +12296,6 @@ UAVCAN Servo 8 Output Function.
Select what should be output on UAVCAN Servo 8.
The default failsafe value is set according to the selected function:
-
- 'Min' for ConstantMin
- 'Max' for ConstantMax
- 'Max' for Parachute
@@ -12987,7 +13179,6 @@ UAVCANv1 ESC 14 Output Function.
Select what should be output on UAVCANv1 ESC 14.
The default failsafe value is set according to the selected function:
-
- 'Min' for ConstantMin
- 'Max' for ConstantMax
- 'Max' for Parachute
@@ -13505,7 +13696,6 @@ UAVCANv1 ESC 6 Output Function.
Select what should be output on UAVCANv1 ESC 6.
The default failsafe value is set according to the selected function:
-
- 'Min' for ConstantMin
- 'Max' for ConstantMax
- 'Max' for Parachute
@@ -13579,7 +13769,6 @@ UAVCANv1 ESC 7 Output Function.
Select what should be output on UAVCANv1 ESC 7.
The default failsafe value is set according to the selected function:
-
- 'Min' for ConstantMin
- 'Max' for ConstantMax
- 'Max' for Parachute
@@ -13653,7 +13842,6 @@ UAVCANv1 ESC 8 Output Function.
Select what should be output on UAVCANv1 ESC 8.
The default failsafe value is set according to the selected function:
-
- 'Min' for ConstantMin
- 'Max' for ConstantMax
- 'Max' for Parachute
@@ -13727,7 +13915,6 @@ UAVCANv1 ESC 9 Output Function.
Select what should be output on UAVCANv1 ESC 9.
The default failsafe value is set according to the selected function:
-
- 'Min' for ConstantMin
- 'Max' for ConstantMax
- 'Max' for Parachute
@@ -14587,7 +14774,6 @@ VOXL2 IO Output PWM Channel 6 Output Function.
Select what should be output on VOXL2 IO Output PWM Channel 6.
The default failsafe value is set according to the selected function:
-
- 'Min' for ConstantMin
- 'Max' for ConstantMax
- 'Max' for Parachute
@@ -21270,6 +21456,8 @@ Multi-EKF IMUs.
Maximum number of IMUs to use for Multi-EKF. Set 0 to disable. Requires SENS_IMU_MODE 0.
+Maximum number of IMUs to use for Multi-EKF. Set 0 to disable. Requires SENS_IMU_MODE 0.
+
| Reboot | minValue | maxValue | increment | default | unit |
| ------- | -------- | -------- | --------- | ------- | ---- |
| ✓ | 0 | 4 | | 0 | |
@@ -22235,7 +22423,6 @@ Landing flare time.
Multiplied by the descent rate to calculate a dynamic altitude at which
to trigger the flare.
-
NOTE: max(FW_LND_FLALT, FW_LND_FL_TIME \* descent rate) is taken as the flare altitude
| Reboot | minValue | maxValue | increment | default | unit |
@@ -22758,7 +22945,6 @@ Height above ground threshold below which tighter altitude
tracking gets enabled (see FW_LND_THRTC_SC). Below this height, TECS smoothly
(1 sec / sec) transitions the altitude tracking time constant from FW_T_ALT_TC
to FW_LND_THRTC_SC\*FW_T_ALT_TC.
-
-1 to disable.
| Reboot | minValue | maxValue | increment | default | unit |
@@ -24576,7 +24762,6 @@ is the output signal sent to the motor controller.
Moment coefficient of rotor 0.
The moment coefficient if defined as Torque = KM \* Thrust.
-
Use a positive value for a rotor with CCW rotation.
Use a negative value for a rotor with CW rotation.
@@ -24673,7 +24858,6 @@ is the output signal sent to the motor controller.
Moment coefficient of rotor 10.
The moment coefficient if defined as Torque = KM \* Thrust.
-
Use a positive value for a rotor with CCW rotation.
Use a negative value for a rotor with CW rotation.
@@ -24770,7 +24954,6 @@ is the output signal sent to the motor controller.
Moment coefficient of rotor 11.
The moment coefficient if defined as Torque = KM \* Thrust.
-
Use a positive value for a rotor with CCW rotation.
Use a negative value for a rotor with CW rotation.
@@ -24867,7 +25050,6 @@ is the output signal sent to the motor controller.
Moment coefficient of rotor 1.
The moment coefficient if defined as Torque = KM \* Thrust.
-
Use a positive value for a rotor with CCW rotation.
Use a negative value for a rotor with CW rotation.
@@ -24964,7 +25146,6 @@ is the output signal sent to the motor controller.
Moment coefficient of rotor 2.
The moment coefficient if defined as Torque = KM \* Thrust.
-
Use a positive value for a rotor with CCW rotation.
Use a negative value for a rotor with CW rotation.
@@ -25061,7 +25242,6 @@ is the output signal sent to the motor controller.
Moment coefficient of rotor 3.
The moment coefficient if defined as Torque = KM \* Thrust.
-
Use a positive value for a rotor with CCW rotation.
Use a negative value for a rotor with CW rotation.
@@ -25158,7 +25338,6 @@ is the output signal sent to the motor controller.
Moment coefficient of rotor 4.
The moment coefficient if defined as Torque = KM \* Thrust.
-
Use a positive value for a rotor with CCW rotation.
Use a negative value for a rotor with CW rotation.
@@ -25255,7 +25434,6 @@ is the output signal sent to the motor controller.
Moment coefficient of rotor 5.
The moment coefficient if defined as Torque = KM \* Thrust.
-
Use a positive value for a rotor with CCW rotation.
Use a negative value for a rotor with CW rotation.
@@ -25352,7 +25530,6 @@ is the output signal sent to the motor controller.
Moment coefficient of rotor 6.
The moment coefficient if defined as Torque = KM \* Thrust.
-
Use a positive value for a rotor with CCW rotation.
Use a negative value for a rotor with CW rotation.
@@ -25449,7 +25626,6 @@ is the output signal sent to the motor controller.
Moment coefficient of rotor 7.
The moment coefficient if defined as Torque = KM \* Thrust.
-
Use a positive value for a rotor with CCW rotation.
Use a negative value for a rotor with CW rotation.
@@ -25546,7 +25722,6 @@ is the output signal sent to the motor controller.
Moment coefficient of rotor 8.
The moment coefficient if defined as Torque = KM \* Thrust.
-
Use a positive value for a rotor with CCW rotation.
Use a negative value for a rotor with CW rotation.
@@ -25643,7 +25818,6 @@ is the output signal sent to the motor controller.
Moment coefficient of rotor 9.
The moment coefficient if defined as Torque = KM \* Thrust.
-
Use a positive value for a rotor with CCW rotation.
Use a negative value for a rotor with CW rotation.
@@ -27451,7 +27625,6 @@ Higher values result in less aggressive following of the measurement and a smoot
Landing target mode.
Configure the mode of the landing target. Depending on the mode, the landing target observations are used differently to aid position estimation.
-
Mode Moving: The landing target may be moving around while in the field of view of the vehicle. Landing target measurements are not used to aid positioning.
Mode Stationary: The landing target is stationary. Measured velocity w.r.t. the landing target is used to aid velocity estimation.
@@ -29757,7 +29930,6 @@ The descend speed is amended:
stick full up - 0
stick centered - MPC_LAND_SPEED
stick full down - 2 \* MPC_LAND_SPEED
-
Manual override during auto modes has to be disabled to use this feature (see COM_RC_OVERRIDE).
**Values:**
@@ -30126,7 +30298,7 @@ Defined as corrective acceleration in m/s^2 per m/s velocity error
| ------ | -------- | -------- | --------- | ------- | ---- |
| | 1.2 | 5 | 0.1 | 1.8 | |
-### MPC_Z_P (`FLOAT`) {#MPC_Z_P}
+Defined as corrective velocity in m/s per m position error
Proportional gain for vertical position error.
@@ -30666,7 +30838,6 @@ Yaw rate proportional gain, i.e. control output for angular speed error 1 rad/s.
| ------ | -------- | -------- | --------- | ------- | ---- |
| | 0.0 | 0.6 | 0.01 | 0.2 | |
-### MC_YAW_TQ_CUTOFF (`FLOAT`) {#MC_YAW_TQ_CUTOFF}
Low pass filter cutoff frequency for yaw torque setpoint.
@@ -39696,7 +39867,6 @@ Note: certain drivers such as the GPS can determine the Baudrate automatically.
Baudrate for the TELEM 2 Serial Port.
Configure the Baudrate for the TELEM 2 Serial Port.
-
Note: certain drivers such as the GPS can determine the Baudrate automatically.
**Values:**
diff --git a/docs/en/advanced_features/index.md b/docs/en/advanced_features/index.md
index 39820576978b..1d81d968e259 100644
--- a/docs/en/advanced_features/index.md
+++ b/docs/en/advanced_features/index.md
@@ -10,3 +10,5 @@ This section contains topics related to some of the more advanced features of th
- [Iridium/RockBlock Satellite Communication System](../advanced_features/satcom_roadblock.md)
- [Precision Landing](../advanced_features/precland.md)
- [RTK GNSS (GPS)](../gps_compass/rtk_gps.md)
+- [Vision Target Estimator](../advanced_features/vision_target_estimator.md)
+- [Vision Target Estimator Deep Dive](../advanced_features/vision_target_estimator_advanced.md)
diff --git a/docs/en/advanced_features/precland.md b/docs/en/advanced_features/precland.md
index 2da2d5619bcb..d933bcde18c3 100644
--- a/docs/en/advanced_features/precland.md
+++ b/docs/en/advanced_features/precland.md
@@ -52,6 +52,13 @@ Landing Phases Flow Diagram
A flow diagram showing the phases can be found in [landing phases flow Diagram](#landing-phases-flow-diagram) below.
+### Yaw Alignment
+
+Precision landing can optionally align the multicopter's yaw to match the detected target orientation while the vehicle is approaching or descending above the pad.
+Enable this behaviour with [PLD_YAW_EN](../advanced_config/parameter_reference.md#PLD_YAW_EN).
+When enabled, PX4 uses the `vision_target_est_orientation` topic (published by the [Vision Target Estimator](../advanced_features/vision_target_estimator.md)) to command the yaw setpoint as long as the orientation data remains valid.
+If the orientation feed times out (see [PLD_BTOUT](../advanced_config/parameter_reference.md#PLD_BTOUT)) or the parameter is disabled, yaw control falls back to the default mission behaviour.
+
## Initiating a Precision Landing
Precision landing can be used in missions, during the landing phase in _Return mode_, or by entering the _Precision Land_ mode.
@@ -157,6 +164,7 @@ Some of the most useful ones are listed below.
| [PLD_BTOUT](../advanced_config/parameter_reference.md#PLD_BTOUT) | Landing Target Timeout, after which the target is assumed lost. Default is 5 seconds. |
| [PLD_FAPPR_ALT](../advanced_config/parameter_reference.md#PLD_FAPPR_ALT) | Final approach altitude. Default is 0.1 metres. |
| [PLD_MAX_SRCH](../advanced_config/parameter_reference.md#PLD_MAX_SRCH) | Maximum number of search attempts in an required landing. |
+| [PLD_YAW_EN](../advanced_config/parameter_reference.md#PLD_YAW_EN) | Enable yaw alignment during precision landing when target orientation is available. |
| [RTL_PLD_MD](../advanced_config/parameter_reference.md#RTL_PLD_MD) | RTL precision land mode. `0`: disabled, `1`: [Opportunistic](#opportunistic-mode), `2`: [Required](#required-mode). |
### IR Beacon Scaling
diff --git a/docs/en/advanced_features/vision_target_estimator.md b/docs/en/advanced_features/vision_target_estimator.md
new file mode 100644
index 000000000000..4daa563016f8
--- /dev/null
+++ b/docs/en/advanced_features/vision_target_estimator.md
@@ -0,0 +1,312 @@
+# Vision Target Estimator (VTEST)
+
+The Vision Target Estimator (VTEST) fuses vision-based relative target measurements with absolute positioning sources (vehicle GNSS, a mission landing waypoint, and/or target-mounted GNSS). It runs alongside the primary vehicle estimator and publishes:
+
+- `landing_target_pose` - relative and absolute target pose in NED for precision landing.
+- `vision_target_est_position` - the full position-state vector (with covariances) for logging and tuning.
+- `vision_target_est_orientation` - target yaw and yaw-rate estimates with associated uncertainty.
+- `vision_target_est_input` - downsampled NED acceleration and attitude quaternion used by the position prediction step.
+- `vte_bias_init_status` - raw and low-pass filtered GNSS/vision bias, plus the consecutive raw-bias delta norm, during the initial bias averaging phase.
+- `vte_aid_*` - innovation logs per sensor, used for observability checks and noise tuning.
+
+When [`VTE_EKF_AID`](../advanced_config/parameter_reference.md#VTE_EKF_AID)=1 and the target is static, the relative velocity estimate is forwarded to the main EKF2 estimator (see [EKF2 aiding](#ekf2-aiding) for the required conditions).
+
+> [!WARNING]
+> VTEST is a beta feature, disabled in default board configurations, and should only be enabled on custom builds after careful bench and flight testing.
+
+For tuning workflows, log analysis guidance, and instructions on extending the estimator, see the [Vision Target Estimator deep dive](../advanced_features/vision_target_estimator_advanced.md).
+
+## Table of Contents
+- [Vision Target Estimator (VTEST)](#vision-target-estimator-vtest)
+ - [Table of Contents](#table-of-contents)
+ - [Architecture Overview](#architecture-overview)
+ - [Building the Module](#building-the-module)
+ - [Dynamic Models](#dynamic-models)
+ - [Position state](#position-state)
+ - [Orientation state](#orientation-state)
+ - [Outlier detection](#outlier-detection)
+ - [Time alignment](#time-alignment)
+ - [Estimator Lifecycle](#estimator-lifecycle)
+ - [Measurement Sources](#measurement-sources)
+ - [Measurements validity](#measurements-validity)
+ - [Configuration](#configuration)
+ - [Module enable and scheduling](#module-enable-and-scheduling)
+ - [Task selection](#task-selection)
+ - [Sensor fusion selection](#sensor-fusion-selection)
+ - [Noise and gating](#noise-and-gating)
+ - [Sensor-specific settings](#sensor-specific-settings)
+ - [EKF2 aiding](#ekf2-aiding)
+ - [MAVLink Messages](#mavlink-messages)
+ - [TARGET\_RELATIVE (ID 511)](#target_relative-id-511)
+ - [TARGET\_ABSOLUTE (ID 510)](#target_absolute-id-510)
+ - [Moving-target projection parameters (experimental)](#moving-target-projection-parameters-experimental)
+ - [Gazebo Classic Simulation](#gazebo-classic-simulation)
+ - [Generated SymForce Functions](#generated-symforce-functions)
+ - [Monitoring](#monitoring)
+ - [Operational Notes](#operational-notes)
+
+## Architecture Overview
+
+VTEST is implemented as two tightly coupled, but independent, estimators managed by the `VisionTargetEst` work item:
+
+- **Position filter (`VTEPosition`)** - three decoupled 1D Kalman filters (one per NED axis) that estimate relative position, vehicle velocity, and GNSS bias. This bias is the offset between the active absolute target reference (target-mounted GNSS or the mission landing waypoint) and the vision-based target position, so the absolute reference can still point to the target if vision is lost. When the firmware is built with `CONFIG_VTEST_MOVING`, target velocity and acceleration states are also estimated (experimental).
+- **Orientation filter (`VTEOrientation`)** - a planar yaw filter that estimates the target's heading and yaw rate.
+
+Both filters run at a fixed 20 ms interval (`kPosUpdatePeriodUs = kYawUpdatePeriodUs`), so the target rate is 50 Hz. The position loop downsamples vehicle acceleration, rotates it into NED, forwards attitude/local position/local velocity/range data to `VTEPosition`, and publishes `vision_target_est_input`. The yaw loop forwards range data to `VTEOrientation`. The work item also enforces timeouts, parameter reloads, and task activation.
+
+> [!WARNING]
+> The moving-target mode `CONFIG_VTEST_MOVING=y` is experimental and has not yet been flight tested.
+
+## Building the Module
+
+The estimator is not part of the default PX4 board configurations. Build a board variant that enables the module, or add `CONFIG_MODULES_VISION_TARGET_ESTIMATOR=y` to a custom `.px4board` file.
+
+Set `CONFIG_MAVLINK_DIALECT="development"` so the [MAVLink messages](#mavlink-messages) for relative and absolute target measurements are available, and disable the landing target estimator by adding `CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=n` (both modules publish `landing_target_pose` and will conflict if enabled together).
+
+Common build targets that already include the module are:
+- `make px4_fmu-v6c_visionTargetEst`
+- `make px4_sitl_default`
+
+Other boards provide the same `_visionTargetEst` suffix. Enable `CONFIG_VTEST_MOVING=y` in the board configuration if you need the experimental moving-target states. Orientation estimation can be enabled at runtime with [`VTE_YAW_EN`](../advanced_config/parameter_reference.md#VTE_YAW_EN), while [`VTE_POS_EN`](../advanced_config/parameter_reference.md#VTE_POS_EN) controls the position filter.
+
+## Dynamic Models
+
+### Position state
+
+For a static target the per-axis state is $x = [ r, v^{uav}, b ]^T$, where $r$ is the relative NED displacement (target minus vehicle), $v^{uav}$ is the vehicle velocity, and $b$ is the constant offset between GNSS-based target position and the relative target position from vision. Once this offset is known, GNSS can keep the estimate centred on the target even if vision drops out. The discrete prediction model, assuming constant NED acceleration input $a^{uav}$ over the integration interval $dt$, is
+
+$$
+\begin{aligned}
+r_{k+1} &= r_k - dt\thinspace v^{uav}_k - \tfrac{1}{2}\thinspace dt^2\thinspace a^{uav} \\
+v^{uav}_{k+1} &= v^{uav}_k + dt\thinspace a^{uav} \\
+b_{k+1} &= b_k
+\end{aligned}
+$$
+
+When `CONFIG_VTEST_MOVING` is enabled, two additional states capture the target dynamics: $x = [ r, v^{uav}, b, a^{t}, v^{t} ]^T$, where $a^{t}$ and $v^{t}$ are the target acceleration and velocity along the axis. The prediction model becomes
+
+$$
+\begin{aligned}
+r_{k+1} &= r_k - dt\thinspace (v^{uav}_k - v^{t}_k) - \tfrac{1}{2}\thinspace dt^2\thinspace (a^{uav} - a^{t}_k) \\
+v^{uav}_{k+1} &= v^{uav}_k + dt\thinspace a^{uav} \\
+v^{t}_{k+1} &= v^{t}_k + dt\thinspace a^{t}_k \\
+a^{t}_{k+1} &= a^{t}_k \\
+b_{k+1} &= b_k
+\end{aligned}
+$$
+
+Process noise is assumed diagonal. The parameters [`VTE_ACC_D_UNC`](../advanced_config/parameter_reference.md#VTE_ACC_D_UNC), [`VTE_ACC_T_UNC`](../advanced_config/parameter_reference.md#VTE_ACC_T_UNC), and [`VTE_BIAS_UNC`](../advanced_config/parameter_reference.md#VTE_BIAS_UNC) set the input variance on the vehicle acceleration, target acceleration (moving mode), and GNSS bias terms. Initial variances are seeded from [`VTE_POS_UNC_IN`](../advanced_config/parameter_reference.md#VTE_POS_UNC_IN), [`VTE_VEL_UNC_IN`](../advanced_config/parameter_reference.md#VTE_VEL_UNC_IN), [`VTE_BIA_UNC_IN`](../advanced_config/parameter_reference.md#VTE_BIA_UNC_IN), and [`VTE_ACC_UNC_IN`](../advanced_config/parameter_reference.md#VTE_ACC_UNC_IN) (moving mode).
+
+### Orientation state
+
+The yaw filter tracks $x = [ \psi, \dot{\psi} ]^T$ and propagates it with a constant-rate model:
+
+$$
+\begin{aligned}
+\psi_{k+1} &= \text{wrap}\negthinspace\left(\psi_k + dt\thinspace\dot{\psi}_k\right) \\
+\dot{\psi}_{k+1} &= \dot{\psi}_k
+\end{aligned}
+$$
+
+Yaw angles are wrapped to $[-\pi, \pi]$.
+
+### Outlier detection
+
+Measurement residuals are gated with the Normalized Innovation Squared (NIS) test. [`VTE_POS_NIS_THRE`](../advanced_config/parameter_reference.md#VTE_POS_NIS_THRE) applies to every position-axis update, [`VTE_YAW_NIS_THRE`](../advanced_config/parameter_reference.md#VTE_YAW_NIS_THRE) applies to yaw updates, and the defaults (3.84) correspond to a $95\%$ chi-squared confidence interval. Innovations and gate ratios are published on the `vte_aid_*` topics for log review.
+
+### Time alignment
+
+Vision and GNSS observations can arrive delayed due to transport and processing latency. The position and orientation filters therefore support an **Out-of-Sequence Measurements (OOSM)** approximation which uses a **history-consistent projected correction** strategy. More details can be found in [Vision Target Estimator deep dive](../advanced_features/vision_target_estimator_advanced.md)
+
+## Estimator Lifecycle
+
+1. **Task activation**: `VisionTargetEst` enables the position and/or orientation filters when the UAV is performing a task selected in [`VTE_TASK_MASK`](../advanced_config/parameter_reference.md#VTE_TASK_MASK). Bit 0 activates precision landing, while bit 1 keeps the estimator running continuously for debugging; future task bits can enable additional mission profiles.
+2. **Initialization**: The position filter starts only after it has at least one position-like observation and a recent vehicle velocity estimate. This velocity is needed to initialize the `v^{uav}` state. The orientation filter starts on the first valid vision yaw sample and initializes yaw rate to zero. For the position state, VTEST uses the first source it can trust:
+
+ - If the GNSS bias is not observable yet (for example, only vision is available), it starts from the best direct position source: vision first, otherwise target GNSS or mission position.
+ - If GNSS and vision are both available and the bias can be trusted immediately, it initializes with `r = pos_rel_vision` and `b = pos_rel_gnss(t_vision) - pos_rel_vision`. If needed, the GNSS-relative sample is propagated to the vision timestamp using UAV velocity before the subtraction.
+ - If GNSS is already the active position reference when vision first becomes available, it initializes from the GNSS-relative source and keeps `b = 0` until the low-pass filter finishes. Once that filtered bias is activated, the filter resets to `r = pos_rel_gnss(t_vision) - b_filtered`, `b = b_filtered`.
+3. **Bias estimation**: The bias becomes observable only when a GNSS-relative position source and vision are both available. VTEST forms raw samples as `b_raw = pos_rel_gnss(t_vision) - pos_rel_vision`; if the GNSS sample is older than the vision sample but still valid, it is propagated forward with the UAV velocity estimate before the subtraction. For the design details see [Bias initialization design](../advanced_features/vision_target_estimator_advanced.md#bias-initialization-design).
+
+ - The behavior is intentionally asymmetric. If GNSS is the active position reference first, VTEST delays vision until the bias is ready so it does not mix a GNSS-referenced state with an uncorrected vision measurement. If vision is the active position reference first, VTEST keeps trusting vision and activates the bias immediately on the first joint GNSS+vision sample, which is safer because the relative position state stays vision-based.
+ - The low-pass filter (LPF) phase is skipped when [`VTE_BIA_AVG_TOUT`](../advanced_config/parameter_reference.md#VTE_BIA_AVG_TOUT)=0 and also whenever the estimator is already vision-referenced before bias activation. That includes the case where vision was available first and GNSS arrives later.
+ - When averaging is enabled, the low-pass filter is updated at the vision rate. This only happens in the GNSS-first case. GNSS remains the active position source during this phase, and vision samples update the low-pass filter but are not fused into the position state yet.
+ - The low-pass filter exits when either 5 consecutive raw-bias delta norms stay below [`VTE_BIA_AVG_THR`](../advanced_config/parameter_reference.md#VTE_BIA_AVG_THR) and at least `2 * tau` has elapsed (`tau = 0.3 s`), or [`VTE_BIA_AVG_TOUT`](../advanced_config/parameter_reference.md#VTE_BIA_AVG_TOUT) expires.
+ - When the averaged bias is activated, the filter uses `b = b_filtered` and resets the relative position to `r = pos_rel_gnss(t_vision) - b_filtered`.
+ - If GNSS goes stale while averaging, VTEST aborts the averaging phase, keeps the current filtered bias, switches the position state to the current vision measurement, and starts fusing vision immediately.
+ - Once the bias has been activated for the current estimator run, a temporary vision dropout does not restart bias initialization when vision returns.
+4. **Prediction**: Predictions run whenever the fixed 20 ms scheduler interval elapses (50 Hz).
+5. **Update**: When new sensor observations are available, measurement handlers check the validity of the measurement (timestamp, value, and fusion requested in [`VTE_AID_MASK`](../advanced_config/parameter_reference.md#VTE_AID_MASK)), process it, and populate observation buffers. The observations that pass the NIS gates defined by [`VTE_POS_NIS_THRE`](../advanced_config/parameter_reference.md#VTE_POS_NIS_THRE) and [`VTE_YAW_NIS_THRE`](../advanced_config/parameter_reference.md#VTE_YAW_NIS_THRE) are then fused.
+6. **Timeout handling** - if no position measurement is fused for [`VTE_BTOUT`](../advanced_config/parameter_reference.md#VTE_BTOUT), the filters are stopped; the published pose/yaw is flagged invalid once [`VTE_TGT_TOUT`](../advanced_config/parameter_reference.md#VTE_TGT_TOUT) elapses without fresh data.
+
+## Measurement Sources
+
+All measurements are fused sequentially. For each observation `z` a one-row Jacobian is formed and applied to a single axis (position filter) or to the yaw state (orientation filter). Enabled sensors are defined by the [`VTE_AID_MASK`](#sensor-fusion-selection) bitmask.
+
+| Source | uORB topic | H structure | Notes |
+| --- | --- | --- | --- |
+| Target GNSS position | `target_gnss` | $z = r + b$ once the bias is observable, otherwise $z = r$ | The vehicle GNSS sample is interpolated to the target timestamp using the vehicle velocity so the two receivers share a common epoch. Requires [`VTE_AID_MASK`](../advanced_config/parameter_reference.md#VTE_AID_MASK) bit 0. Before bias activation, this source is held back if the estimator is already vision-referenced. |
+| Mission landing waypoint | `position_setpoint_triplet` | $z = r$ | Provides a fallback absolute reference when target GNSS is unavailable. Enable [`VTE_AID_MASK`](../advanced_config/parameter_reference.md#VTE_AID_MASK) bit 3 and avoid combining it with target GNSS because only one GNSS bias can be estimated. Before bias activation, this source is held back if the estimator is already vision-referenced. |
+| Vision pose | `fiducial_marker_pos_report` | $z = r$ after rotating the measurement (`rel_pos`) into NED using `q` | Uses the message variances, lower-bounded by [`VTE_EVP_NOISE`](../advanced_config/parameter_reference.md#VTE_EVP_NOISE). Recent vision fusions are required for EKF aiding. During the initial GNSS/vision bias averaging phase, valid vision samples update the bias low-pass filter but are not fused into the position state yet. This averaging phase only exists when GNSS became the active reference first. |
+| Vehicle GNSS velocity | `sensor_gps` | $z = v^{uav}$ | Removes rotation-induced velocity using `sensor_gps.antenna_offset_{x,y,z}`, which are populated from the vehicle GPS antenna offset parameters (`SENS_GPS0_OFF*`). Enable [`VTE_AID_MASK`](../advanced_config/parameter_reference.md#VTE_AID_MASK) bit 1. |
+| Target GNSS velocity (moving mode) | `target_gnss` | $z = v^{t}$ | Available only when `CONFIG_VTEST_MOVING=y` and [`VTE_AID_MASK`](../advanced_config/parameter_reference.md#VTE_AID_MASK) bit 4 is set. |
+| Vision yaw | `fiducial_marker_yaw_report` | $z = \psi$ | Only source used by the orientation filter. Requires [`VTE_YAW_EN`](../advanced_config/parameter_reference.md#VTE_YAW_EN) and [`VTE_AID_MASK`](../advanced_config/parameter_reference.md#VTE_AID_MASK) bit 2. Variance is taken from the message and lower-bounded by [`VTE_EVA_NOISE`](../advanced_config/parameter_reference.md#VTE_EVA_NOISE). |
+
+All innovation data are published on dedicated topics (`vte_aid_gps_pos_target`, `vte_aid_fiducial_marker`, `vte_aid_ev_yaw`, etc.), making it easy to inspect residuals and test ratios in logs. Rejected measurements still emit innovation messages with `fused=false` so tuning sessions can identify time skew or noise mismatches.
+
+> [!NOTE]
+> UWB and IRLock are candidates for future development once representative test data is available.
+
+### Measurements validity
+
+Two time horizons guard incoming data: [`VTE_M_REC_TOUT`](../advanced_config/parameter_reference.md#VTE_M_REC_TOUT) defines how old a sample can be and still be considered for fusion, while [`VTE_M_UPD_TOUT`](../advanced_config/parameter_reference.md#VTE_M_UPD_TOUT) bounds how long cached measurements remain valid in the estimator state. The published outputs toggle `*_valid` false after [`VTE_TGT_TOUT`](../advanced_config/parameter_reference.md#VTE_TGT_TOUT) (and the estimators fully stop once [`VTE_BTOUT`](../advanced_config/parameter_reference.md#VTE_BTOUT) is exceeded). Measurements timestamped in the future relative to the latest prediction are always rejected and reported on the innovation topics.
+
+## Configuration
+
+### Module enable and scheduling
+
+- [`VTE_EN`](../advanced_config/parameter_reference.md#VTE_EN) - global module enable (reboot required).
+- [`VTE_POS_EN`](../advanced_config/parameter_reference.md#VTE_POS_EN) / [`VTE_YAW_EN`](../advanced_config/parameter_reference.md#VTE_YAW_EN) - enable the position and orientation filters respectively (reboot required).
+- [`VTE_BTOUT`](../advanced_config/parameter_reference.md#VTE_BTOUT), [`VTE_TGT_TOUT`](../advanced_config/parameter_reference.md#VTE_TGT_TOUT) - timeouts for estimator shutdown and published validity flags.
+- [`VTE_M_REC_TOUT`](../advanced_config/parameter_reference.md#VTE_M_REC_TOUT), [`VTE_M_UPD_TOUT`](../advanced_config/parameter_reference.md#VTE_M_UPD_TOUT) - maximum ages for measurements to be fused or retained.
+
+### Task selection
+
+[`VTE_TASK_MASK`](../advanced_config/parameter_reference.md#VTE_TASK_MASK) selects runtime tasks during which the estimators perform computations and estimate the state of the target.
+
+| Bit | Value | Meaning |
+| --- | --- | --- |
+| 0 | `1` | precision landing |
+| 1 | `2` | DEBUG, always active |
+
+> [!IMPORTANT]
+> Precision landing yaw control is disabled by default. Enable [PLD_YAW_EN](../advanced_config/parameter_reference.md#PLD_YAW_EN) when you want the mission controller to align the vehicle with the target heading, and configure the landing waypoint for precision landing (see [Mission precision landing](../advanced_features/precland.md#mission)). Without both settings the aircraft will only track the position estimate from the Vision Target Estimator.
+
+### Sensor fusion selection
+
+[`VTE_AID_MASK`](../advanced_config/parameter_reference.md#VTE_AID_MASK) defines which measurements can be fused:
+
+| Bit | Value | Meaning |
+| --- | --- | --- |
+| 0 | `1` | Target GNSS position |
+| 1 | `2` | Vehicle GNSS velocity |
+| 2 | `4` | Vision-relative position |
+| 3 | `8` | Mission landing waypoint |
+| 4 | `16` | Target GNSS velocity (moving mode only) |
+
+Bit 2 also enables processing of `fiducial_marker_yaw_report` in the orientation filter.
+
+### Noise and gating
+
+- Adjust [`VTE_POS_UNC_IN`](../advanced_config/parameter_reference.md#VTE_POS_UNC_IN), [`VTE_VEL_UNC_IN`](../advanced_config/parameter_reference.md#VTE_VEL_UNC_IN), [`VTE_BIA_UNC_IN`](../advanced_config/parameter_reference.md#VTE_BIA_UNC_IN), [`VTE_ACC_UNC_IN`](../advanced_config/parameter_reference.md#VTE_ACC_UNC_IN), and [`VTE_YAW_UNC_IN`](../advanced_config/parameter_reference.md#VTE_YAW_UNC_IN) to reflect initial uncertainty. These parameters seed the estimator state at initialization. Updating them while the estimator is already running has no immediate effect; the new values are used the next time the estimator initializes or resets. Larger numbers slow initial convergence but reduce the chance of aggressive transients.
+- [`VTE_ACC_D_UNC`](../advanced_config/parameter_reference.md#VTE_ACC_D_UNC), [`VTE_ACC_T_UNC`](../advanced_config/parameter_reference.md#VTE_ACC_T_UNC), [`VTE_BIAS_UNC`](../advanced_config/parameter_reference.md#VTE_BIAS_UNC), [`VTE_YAW_ACC_UNC`](../advanced_config/parameter_reference.md#VTE_YAW_ACC_UNC) - process noise for vehicle acceleration, target acceleration, GNSS bias, and yaw acceleration. Increase [`VTE_ACC_D_UNC`](../advanced_config/parameter_reference.md#VTE_ACC_D_UNC) if the estimator lags behind real motion, raise [`VTE_BIAS_UNC`](../advanced_config/parameter_reference.md#VTE_BIAS_UNC) if GNSS bias corrections should respond more quickly, and use [`VTE_YAW_ACC_UNC`](../advanced_config/parameter_reference.md#VTE_YAW_ACC_UNC) to match expected target heading dynamics. In moving-target builds, use [`VTE_ACC_T_UNC`](../advanced_config/parameter_reference.md#VTE_ACC_T_UNC) to match expected target manoeuvrability.
+- [`VTE_BIA_AVG_THR`](../advanced_config/parameter_reference.md#VTE_BIA_AVG_THR), [`VTE_BIA_AVG_TOUT`](../advanced_config/parameter_reference.md#VTE_BIA_AVG_TOUT) - control the first GNSS/vision bias averaging step in the GNSS-first case. Stability requires 5 consecutive raw-bias delta norms below the threshold and at least `2 * tau` of low-pass filter runtime (`tau = 0.3 s`); the timeout is the fallback exit. Set [`VTE_BIA_AVG_TOUT`](../advanced_config/parameter_reference.md#VTE_BIA_AVG_TOUT)=0 to skip averaging and activate the bias immediately.
+- [`VTE_POS_NIS_THRE`](../advanced_config/parameter_reference.md#VTE_POS_NIS_THRE), [`VTE_YAW_NIS_THRE`](../advanced_config/parameter_reference.md#VTE_YAW_NIS_THRE) - chi-squared thresholds (e.g. 3.84 corresponds to $95\%$ confidence). Runtime values are lower-bounded by `kMinNisThreshold = 0.1`.
+- Measurement variance floors: [`VTE_GPS_P_NOISE`](../advanced_config/parameter_reference.md#VTE_GPS_P_NOISE), [`VTE_GPS_V_NOISE`](../advanced_config/parameter_reference.md#VTE_GPS_V_NOISE), [`VTE_EVP_NOISE`](../advanced_config/parameter_reference.md#VTE_EVP_NOISE), [`VTE_EVA_NOISE`](../advanced_config/parameter_reference.md#VTE_EVA_NOISE). Runtime values are floored at `kMinObservationNoise = 1e-2` (see `src/modules/vision_target_estimator/common.h`) to keep the Kalman gains bounded.
+
+### Sensor-specific settings
+
+- **GNSS antenna offsets** - `VisionTargetEst` reads `sensor_gps.antenna_offset_{x,y,z}` to remove position- and rotation-induced velocity offsets before forming GNSS measurements. Configure the vehicle GPS antenna location with [`SENS_GPS0_OFFX`](../advanced_config/parameter_reference.md#SENS_GPS0_OFFX), [`SENS_GPS0_OFFY`](../advanced_config/parameter_reference.md#SENS_GPS0_OFFY), and [`SENS_GPS0_OFFZ`](../advanced_config/parameter_reference.md#SENS_GPS0_OFFZ);
+
+### EKF2 aiding
+
+When [`VTE_EKF_AID`](../advanced_config/parameter_reference.md#VTE_EKF_AID)=1, `VisionTargetEst` sets `landing_target_pose.rel_vel_valid` so EKF2 can fuse the relative target velocity. This requires all of the following:
+
+- `landing_target_pose.is_static = true`, which is only the case when `CONFIG_VTEST_MOVING` is disabled.
+- `landing_target_pose.rel_pos_valid = true` and a recent relative measurement (within [`VTE_M_REC_TOUT`](../advanced_config/parameter_reference.md#VTE_M_REC_TOUT)).
+- A vision-relative position measurement has been fused.
+
+If any condition fails, `rel_vel_valid` is cleared and EKF2 ignores the input.
+
+### MAVLink Messages
+
+To run the vision target estimator, `CONFIG_MAVLINK_DIALECT="development"` is required to expose `TARGET_RELATIVE` for vision-based observations and `TARGET_ABSOLUTE` for target-mounted GNSS data, two PX4-specific messages that feed the estimator. Note that there are open discussions to include them into the common MAVLink dialect.
+
+#### TARGET_RELATIVE (ID 511)
+
+`TARGET_RELATIVE` extends the `LANDING_TARGET` message with a full 3D report that includes orientation and measurement uncertainty:
+
+- A coordinate frame selector (`TARGET_OBS_FRAME`) and a sensor quaternion (`q_sensor`) that, when provided, rotates the measurement into vehicle-carried NED.
+- Target pose (`x`, `y`, `z`, `q_target`) and variances (`pos_std`, `yaw_std`), collected from onboard vision pipelines.
+
+`mavlink_receiver` validates the frame/type and handles the message differently depending on the Vision Target Estimator status:
+
+- When `VTE_EN=0`, the measurement is rotated (using `q_sensor` or the vehicle attitude for body-frame reports) and published straight to `landing_target_pose` so precision-landing can operate without the estimator.
+- When `VTE_EN=1`, the message is split into `fiducial_marker_pos_report` and `fiducial_marker_yaw_report`. `VisionTargetEst` consumes these uORB topics to drive the position and orientation filters (using `fiducial_marker_pos_report.q` to rotate `fiducial_marker_pos_report.rel_pos` into NED at `timestamp_sample`).
+
+#### TARGET_ABSOLUTE (ID 510)
+
+`TARGET_ABSOLUTE` reports the target's absolute state when it carries its own GNSS (and optionally IMU). A capability bitmap advertises which fields are valid. PX4 maps the available content into the `target_gnss` uORB topic:
+
+- Bit 0 (position) triggers publication of latitude/longitude/altitude along with the horizontal and vertical accuracy estimates (`position_std`).
+- Bit 1 (velocity) forwards the NED velocity vector (`vel`) and its standard deviations (`vel_std`).
+- Additional fields (acceleration, quaternion `q_target`, rates, uncertainties) are not supported and reserved for future fusion logic once flight testing is available.
+
+### Moving-target projection parameters (experimental)
+
+When `CONFIG_VTEST_MOVING` is active, [`VTE_MOVING_T_MIN`](../advanced_config/parameter_reference.md#VTE_MOVING_T_MIN) and [`VTE_MOVING_T_MAX`](../advanced_config/parameter_reference.md#VTE_MOVING_T_MAX) determine how far ahead the target position is projected when publishing the absolute landing setpoint. The estimator computes an intersection time $\Delta t$ by constraining $|z_{rel}| / |v_{descent}|$ between the two parameters, then shifts the absolute landing target by $\Delta t$ along the estimated target velocity (with a correction from the target acceleration state). The goal is to command the vehicle towards where the moving target will be at touchdown, not where it was at the last measurement.
+
+## Gazebo Classic Simulation
+
+Run the SITL world `gazebo-classic_iris_irlock` to simulate precision landing using the VTEST fusing vision (ArUco-based) and target GNSS aiding. The world name is retained for historical reasons. The models were introduced in [PX4/PX4-SITL_gazebo-classic#950](https://github.com/PX4/PX4-SITL_gazebo-classic/pull/950).
+
+> [!TIP]
+> The ArUco vision observation path implemented in `Tools/simulation/gazebo-classic/sitl_gazebo-classic/src/gazebo_aruco_plugin.cpp` provides a concrete example of how to obtain a vision-based observation of a target and how to publish the `TARGET_RELATIVE` message.
+
+1. Launch the simulator with:
+
+ ```sh
+ make px4_sitl gazebo-classic_iris_irlock
+ ```
+
+2. If CMake cannot locate the expected OpenCV version, query the version present on your system (`opencv_version --short` or `pkg-config --modversion opencv4`) and update `Tools/simulation/gazebo-classic/sitl_gazebo-classic/CMakeLists.txt` accordingly, for example:
+
+ ```cmake
+ find_package(OpenCV 4.2.0 REQUIRED EXACT)
+ ```
+
+ Re-run the build after adjusting the `find_package` line.
+
+> [!TIP]Tips
+> - **Pad visibility**: In `Tools/simulation/gazebo-classic/sitl_gazebo-classic/models/land_pad/land_pad.sdf`, increase the visual box size to `1.5 1.5 0.01` so the pad stays in view longer while the vehicle descends.
+> - **Mission waypoint bias**: Enable vision and mission position aiding in [`VTE_AID_MASK`](../advanced_config/parameter_reference.md#VTE_AID_MASK) (set bits 2 and 3, disable bit 0). Place the landing waypoint 3 to 4 m away from the pad in QGroundControl to watch the UAV correct towards the pad once it is detected. In the logs, observe how the GNSS bias compensates for the distance between the land waypoint and the actual pad.
+> - **Measurement noise experiments**: The ArUco plugin publishes nominal standard deviations through `set_std_x` and `set_std_y` in `Tools/simulation/gazebo-classic/sitl_gazebo-classic/src/gazebo_aruco_plugin.cpp`. Modify these assignments, and optionally the camera noise block in `.../models/aruco_cam/aruco_cam.sdf`, to see how innovation gates react to noisier vision.
+> - **Moving target trials**: To emulate a moving pad, edit `` in `.../models/land_pad/land_pad.sdf` (for example `0.5 0 0`) and enable `CONFIG_VTEST_MOVING` so the estimator tracks the target velocity.
+
+## Generated SymForce Functions
+
+The symbolic models live in:
+
+- `src/modules/vision_target_estimator/Position/vtest_derivation/derivation.py` for the position filter
+- `src/modules/vision_target_estimator/Orientation/vtest_derivation/derivation.py` for the yaw filter
+
+During the build:
+
+- By default, CMake copies the committed pre-generated source-tree files from `Position/vtest_derivation/generated` (or `generated_moving` when `CONFIG_VTEST_MOVING=y`) into `build//src/modules/vision_target_estimator/vtest_derivation/generated/`, and from `Orientation/vtest_derivation/generated` into `build//src/modules/vision_target_estimator/vte_orientation_derivation/generated/`.
+- Setting `VTEST_SYMFORCE_GEN=ON` and having SymForce available regenerates both the position and orientation files at configure time.
+- Developers can refresh the committed reference outputs with `-DVTEST_UPDATE_COMMITTED_DERIVATION=ON`.
+
+The position filter uses generated `state.h`, `predictState.h`, `predictCov.h`, `computeInnovCov.h`, `getTransitionMatrix.h`, and `applyCorrection.h`. The orientation filter uses generated `predictState.h`, `predictCov.h`, and `getTransitionMatrix.h`; innovation computation and correction remain hand-written in `Orientation/KF_orientation.cpp` so yaw innovations and corrected states can be wrapped to $[-\pi, \pi]$.
+
+These generated files are included from the build directory and must not be edited manually. See the [Vision Target Estimator deep dive](../advanced_features/vision_target_estimator_advanced.md#regenerating-the-symbolic-model) for regeneration prerequisites and troubleshooting tips.
+
+## Monitoring
+
+For a detailed log analysis guidance, see the [Vision Target Estimator deep dive](../advanced_features/vision_target_estimator_advanced.md).
+
+- `landing_target_pose.rel_pos_valid` and `.abs_pos_valid` indicate whether recent measurements support relative and absolute positioning.
+- `vision_target_est_position` exposes every state component (relative position, vehicle velocity, GNSS bias, and optional target motion) together with diagonal covariance entries.
+- `vision_target_est_orientation` provides yaw, yaw-rate, and their variances.
+- `vision_target_est_input` records the downsampled NED acceleration and attitude quaternion actually fed to the position prediction step.
+- `vte_bias_init_status` shows raw and filtered GNSS/vision bias while the initial averaging phase is active.
+- Innovations published on `vte_aid_*` topics include the raw measurement, innovation, innovation variance, and chi-squared test ratio for post-flight analysis.
+
+## Operational Notes
+
+- Accurate timestamp alignment between measurement sources is critical. Large skews will cause innovations to fail the NIS gate and be rejected.
+- Absolute target pose is only published when `vehicle_local_position` reports a valid local frame.
+- When you expect yaw alignment during landing, enable [`PLD_YAW_EN`](../advanced_config/parameter_reference.md#PLD_YAW_EN) and configure the mission land item for precision landing as described in [Precision landing](../advanced_features/precland.md#mission). In practice this means setting the QGroundControl land waypoint `Precision landing` drop-down (or `MAV_CMD_NAV_LAND` `param2`) to Opportunistic or Required so the controller requests the estimator output. Otherwise only positional cues are used.
+- For extended parameter descriptions, log analysis checklists, and developer workflows, refer to the [Vision Target Estimator deep dive](../advanced_features/vision_target_estimator_advanced.md).
+
+> [!WARNING]
+> The moving-target mode is experimental and has not been flight tested; use it only for controlled experiments.
diff --git a/docs/en/advanced_features/vision_target_estimator_advanced.md b/docs/en/advanced_features/vision_target_estimator_advanced.md
new file mode 100644
index 000000000000..c2a442cec7e6
--- /dev/null
+++ b/docs/en/advanced_features/vision_target_estimator_advanced.md
@@ -0,0 +1,340 @@
+# Vision Target Estimator Deep Dive
+
+This guide expands on the [Vision Target Estimator module overview](../advanced_features/vision_target_estimator.md) and targets advanced users who want to tune, extend, and debug the estimator in detail. It documents the system architecture of the Vision Target Estimator, and outlines workflows for log analysis and sensor integration.
+
+**Table of Contents**
+- [Vision Target Estimator Deep Dive](#vision-target-estimator-deep-dive)
+ - [System architecture](#system-architecture)
+ - [Bias initialization design](#bias-initialization-design)
+ - [Time alignment](#time-alignment)
+ - [OOSM implementation](#oosm-implementation)
+ - [OOSM approximation assumptions](#oosm-approximation-assumptions)
+ - [Log analysis and expected plots](#log-analysis-and-expected-plots)
+ - [Estimator outputs](#estimator-outputs)
+ - [Main input feeds](#main-input-feeds)
+ - [What to look for in logs](#what-to-look-for-in-logs)
+ - [Troubleshooting checklist](#troubleshooting-checklist)
+ - [Plot examples](#plot-examples)
+ - [Development and debugging tips](#development-and-debugging-tips)
+ - [Adding new measurement sources](#adding-new-measurement-sources)
+ - [Regenerating the symbolic model](#regenerating-the-symbolic-model)
+ - [Unit test suites](#unit-test-suites)
+
+## System architecture
+
+The implementation is split across a scheduler and two independent estimators:
+
+- `VisionTargetEst` (`src/modules/vision_target_estimator/VisionTargetEst.cpp`) owns the work-queue task. Its main handles vehicle inputs (`vehicle_attitude`, `vehicle_acceleration`, `vehicle_local_position`, range, and angular rates), downsamples acceleration, publishes `vision_target_est_input`, and sends the samples to the position and orientation filters every 20 ms (50 Hz).
+- `VTEPosition` (`Position/VTEPosition.cpp`) implements the per-axis filters, observation buffers, timeout logic, and the publication of `landing_target_pose`, `vision_target_est_position`, and every `vte_aid_*` innovation topic. Helper unions (`SensorFusionMaskU`, `ObsValidMaskU`) mirror the bit layout of [`VTE_AID_MASK`](../advanced_features/vision_target_estimator.md#sensor-fusion-selection), making it straightforward to add new observation types. It also handles the absolute-reference/vision bias logic: this bias stores the offset between the active absolute reference (target GNSS or mission landing waypoint) and vision so the corrected absolute reference can still point to the target if vision drops out. Before that bias is trusted, the state is initialized from the best direct position source. The behavior is intentionally asymmetric. When GNSS is the active position reference first, VTE delays vision and may average the first bias samples so it does not mix a GNSS-referenced state with an uncorrected vision measurement. When vision is the active position reference first, VTE keeps trusting vision and activates the bias immediately on the first joint GNSS+vision sample. That keeps `r` aligned with the already trusted vision measurement and avoids delaying recovery for no gain. Once the filtered or immediate bias is activated, the state is reset with the chosen `r` and `b`. If GNSS goes stale during GNSS-first averaging, averaging stops, the current vision position is used, and the current filtered bias is activated. If the bias is already set and vision later drops out and returns, the estimator does not re-average it.
+ - `Position/KF_position.cpp` encapsulates the 1D Kalman filter math described in the [position-state model](../advanced_features/vision_target_estimator.md#position-state). The prediction, covariance, innovation-covariance, and correction routines (`sym::Predictstate`, `sym::Predictcov`, `sym::Computeinnovcov`, `sym::Applycorrection`) are generated by SymForce and copied into the build-tree `vtest_derivation/generated/` directory during the build.
+ - `Position/vtest_derivation/derivation.py` defines the symbolic system that SymForce expands into C++; rerun the generator via CMake whenever the model changes as detailed in [Regenerating the symbolic model](#regenerating-the-symbolic-model).
+- `VTEOrientation` (`Orientation/VTEOrientation.cpp`) handles yaw fusion, using a simplified state vector but the same measurement staging helpers as the position filter.
+ - `Orientation/KF_orientation.cpp` implements the constant-turn-rate Kalman filter described in the [orientation-state model](../advanced_features/vision_target_estimator.md#orientation-state). State prediction, covariance propagation, and the transition matrix are generated by SymForce, while innovation computation and correction remain hand-written so yaw angles can be wrapped to $[-\pi, \pi]$.
+- Shared utilities live in `common.h` and `VTEOosm.h` (templated OOSM manager).
+
+## Bias initialization design
+
+The bias state exists because the absolute target reference and the relative vision reference do not necessarily agree:
+
+- If [`VTE_AID_MASK`](../advanced_features/vision_target_estimator.md#sensor-fusion-selection) uses the mission landing point, that waypoint can be offset from the real pad.
+- If [`VTE_AID_MASK`](../advanced_features/vision_target_estimator.md#sensor-fusion-selection) uses target GNSS, the target receiver can still be off by metres when it is not RTK-grade.
+- Vision measures the target relative to the vehicle, so it can be locally accurate even when the absolute GNSS reference is biased.
+
+VTE therefore models GNSS-like position observations as `z = r + b` once the bias becomes observable. This lets corrected GNSS continue to point to the target when vision drops out.
+
+Naive bias initialization is not robust:
+
+- **Zero bias** assumes GNSS and vision already agree. If the real offset is large, the first vision updates can look like outliers and get rejected.
+- **Instantaneous bias from the first sample** trusts a single early vision frame. If that frame is noisy and vision is lost soon after, the estimator can continue descending with the wrong corrected GNSS offset.
+
+The implementation therefore uses two different bias-initialization paths depending on which position source is trusted first:
+
+1. **Startup prerequisites**: `VTEPosition::initializeEstimator()` requires a recent local-velocity or UAV-GNSS-velocity estimate before the filter can start. This is needed to initialize the `v^{uav}` state and also to support the GNSS-to-vision time alignment used by the bias logic.
+2. **Raw bias sample**: whenever a valid GNSS-relative sample exists, the code computes the initial bias from `selectBiasGnssSample(sample_time)`, where `sample_time` is the vision timestamp. If the GNSS-relative sample is older than vision but still valid, the helper propagates it forward with the UAV velocity estimate. In other words, the bias logic uses `pos_rel_gnss(t_vision)`, not the stale stored `pos_rel_gnss`.
+3. **Why the behavior is asymmetric**: if GNSS is already driving the position state, feeding in raw vision before the bias is ready would mix two frames that can differ by metres. That is why VTE delays vision in the GNSS-first case. If vision is already driving the position state, the safer choice is the opposite: keep the trusted vision-based `r` and solve for the bias immediately when GNSS and vision overlap for the first time.
+4. **Immediate activation case**: when the estimator is already vision-referenced, VTE computes `b = pos_rel_gnss(t_vision) - pos_rel_vision` and keeps `r = pos_rel_vision`. The same immediate-activation path is also used when [`VTE_BIA_AVG_TOUT`](../advanced_config/parameter_reference.md#VTE_BIA_AVG_TOUT)=0, even if GNSS was active first.
+5. **Averaging case**: when GNSS is active first and averaging is enabled, vision updates the LPF at the vision rate while GNSS remains the active position source. The raw sample is still `pos_rel_gnss(t_vision) - pos_rel_vision`, so the rate mismatch between GNSS and vision does not force the estimator to discard intermediate vision frames.
+6. **Exit condition**: the LPF is accepted only after 5 consecutive raw-bias delta norms stay below [`VTE_BIA_AVG_THR`](../advanced_config/parameter_reference.md#VTE_BIA_AVG_THR) and at least `2 * tau` has elapsed (`tau = 0.3 s`), or when [`VTE_BIA_AVG_TOUT`](../advanced_config/parameter_reference.md#VTE_BIA_AVG_TOUT) expires.
+7. **Activation after averaging**: once the LPF is accepted, the state reset is `activateBiasEstimate(gnss_sample - filtered_bias, filtered_bias)`, which means `r = pos_rel_gnss(t_vision) - b_filtered`, `b = b_filtered`.
+8. **Stale-GNSS fallback**: if GNSS goes stale during averaging, `selectBiasGnssSample()` fails and the code intentionally switches to the current vision position while keeping the current filtered bias. This is the only bias-initialization branch that does not use `pos_rel_gnss(t_vision)`, because no valid GNSS sample exists anymore.
+9. **No re-initialization after recovery**: once the bias is set for the current estimator run, a temporary vision dropout does not restart the averaging phase. When vision returns, the existing bias stays active.
+
+## Time alignment
+
+Vision and GNSS observations can arrive delayed due to transport and processing latency. The position and orientation filters therefore support an **Out-of-Sequence Measurements (OOSM)** approximation which uses a **history-consistent projected correction** strategy.
+
+### OOSM implementation
+
+Each filter maintains a fixed-size ring buffer of recent state snapshots spanning roughly the last half second of operation (25 samples ≈ 0.5 s at 50 Hz). For the position filter the snapshot stores $(t, x, P, a^{uav})$; for the orientation filter it stores $(t, x, P)$. When a delayed scalar measurement arrives with timestamp $t_{meas}$:
+
+1. **Retrieve**: fetch the closest $(x_{old}, P_{old}, a^{uav}_{old})$ before $t_{meas}$.
+
+$$
+t_{old} ≤ t_{meas}
+$$
+
+1. **Predict**: use the KF model to predict the state $(x_{meas}, P_{meas})$ using $\Delta t = t_{meas} - t_{old}$ (for the orientation filter the control-input term is omitted)
+
+
+$$
+ x_{meas} = \Phi(\Delta t) x_{old} + G a^{uav}_{old} \\
+ P_{meas} = \Phi(\Delta t)P_{old}\Phi^T(\Delta t) + R
+$$
+
+
+3. **Innovate**: compute the innovation $y$ and innovation variance $S$:
+
+$$
+y = z - Hx_{meas} \\
+S = H P_{meas} H^T + R
+$$
+
+4. **Correct**: compute the optimal correction vector:
+
+$$
+ \delta x_{meas} = Ky \\ \thickspace K = P_{meas} H^T S^{-1}
+$$
+5. **Project**: project this correction forward with the state transition matrix $\Phi(\Delta t)$:
+
+ $$
+ \delta x(t) = \Phi(t - t_{meas})\thinspace \delta x_{meas} = \Phi(t - t_{meas})\thinspace Ky \\
+
+ $$
+
+6. **Apply**: apply the projected correction to the current live state *and* to every stored history sample $i$ that occurred after $t_{meas}$ i.e. $t_{i} > t_{meas}$.
+
+ $$
+ K_{i} = \Phi(t_{i} - t_{meas})K \\
+ x_{i} = x_{i} + K_{i}y \\
+ P_{i} = P_{i} - K_{i}SK_{i}^T
+ $$
+
+Updating the history buffer keeps it self-consistent for subsequent delayed measurements and provides the practical benefits of a lag-smoother while remaining deterministic (fixed buffer, bounded runtime) and avoiding matrix inversion or backward retrodiction.
+
+Note that the state prediction is $x_{k+1} = \Phi x_k + G u_k$, however, when projecting the correction forward we don't explicitely use $Gu_{k}$ because we're projecting the correction and thus the $Gu_{k}$ term cancels out:
+
+$$
+(x'_{k+1} - x_{k+1}) = (\Phi x'_k + G u_k) - (\Phi x_k + G u_k) \\
+\Delta x_{k+1} = \Phi (x'_k - x_k) = \Phi \Delta x_k
+$$
+
+### OOSM approximation assumptions
+
+In the Algorithm I of *Zhang et al. Optimal Update with Out-of-Sequence Measurements*, the optimal gain for an Out-of-Sequence Measurement (OOSM) depends on $U_{k,d}$, which represents the cross-covariance between the current state $x_k$ and the past state error $\tilde{x}_{d|k}$
+
+$$
+
+x̂_{k|k,d} = x̂_{k|k} + K_d (z_d − H_d x̂_{d|k} ) \\
+K_d = U_{k,d} H_d' S_d^{−1}
+
+$$
+
+
+With the optimal recursion Eq. 5:
+
+$$
+U_{n+1,d} = (I − K_{n+1} H_{n+1} )F_{n+1,n} U_{n,d}
+$$
+
+
+$F_{n+1,n}$ propagates the correlation forward in time based on system dynamics.$(I - K_{n+1}H_{n+1})$ reduces the correlation based on the information gained from intermediate measurements processed between $t_d$ and $t_k$.
+
+**First Approximation:**
+To avoid the computational complexity and storage required to track every intermediate gain $K_n$ and measurement matrix $H_n$, we approximate the cross-covariance term. We assume that the reduction in correlation due to intermediate updates is negligible for the purpose of the OOSM projection. Mathematically, this assumes the update factor is close to the identity matrix:
+
+$$
+(I - K_{n}H_{n}) \approx I
+$$
+
+Under this assumption, the recursion in Eq. (5) simplifies to a pure dynamic propagation:
+
+$$U_{n+1,d} \approx F_{n+1,n} U_{n,d}$$
+
+By iterating this simplified recursion from the measurement time $d$ to the current time $k$, and recognizing that the initial cross-covariance $U_{d,d}$ corresponds to the prediction covariance $P_{d|d-1}$ (or $P_{d|k-l}$ in the paper's notation Eq. 6), we obtain:
+
+$$U_{k,d} \approx F_{k,d}​U_{d,d} \approx \Phi_{k,d} P_{d|d-1}$$
+
+Substituting this approximation back into the gain formula:
+$$K_d \approx (\Phi_{k,d} P_{d|d-1}) H_d^T S_d^{-1}$$
+
+Since the standard Kalman gain at time $d$ is $K = P_{d|d-1} H_d^T S_d^{-1}$, the OOSM gain simplifies to:
+
+$$K_d \approx \Phi_{k,d} K$$
+
+Effectively, this justifies the "Project" step in the implementation: we calculate the correction vector using the snapshot at $t_{meas}$ and propagate it forward using only the state transition matrix $\Phi(\Delta t)$ ($F_{k,d}$), ignoring the complex coupling effects of intermediate measurements described in the optimal recursion. This reduces storage complexity from $O(N)$ matrices to a simple circular buffer of snapshots while ensuring the correction remains consistent with the system's motion model.
+
+
+**Second Approximation:** No SmoothingContext:
+The paper’s globally optimal update uses the "retrodicted" (or smoothed) state estimate and covariance.
+- Optimal Innovation: The innovation in Eq. (3) is defined as
+ $$\tilde{z}_{d|k} = z_d - H_d \hat{x}_{d|k}$$
+ This $\hat{x}_{d|k}$ is the estimate of the state at time $d$ conditioned on all measurements up to time $k$.
+- Optimal Covariance: The innovation covariance $S_d$ in Eq. (4) uses $P_{d|k}$, which is the error covariance at time $d$ given all data up to $k$.
+
+The Approximation: The current implementation calculates the innovation using a "snapshot" retrieved from history ($x_{old}$) and predicted to the measurement time ($x_{meas}$). Since this snapshot was saved before the subsequent measurements ($z_{d+1} \dots z_k$) were processed, it only contains information available up to time $d$.
+
+- State Approximation: The predicted state $\hat{x}_{d|d^-}$ (or filtered state $\hat{x}_{d|d}$) is substituted for the smoothed state $\hat{x}_{d|k}$.
+ $$\hat{x}_{d|k} \approx \hat{x}_{d|d^-}$$
+- Covariance Approximation: Similarly, $S$ is calculated using $P_{meas}$ (which is $P_{d|d^-}$), substituting the prediction covariance for the smoothed covariance.
+$$P_{d|k} \approx P_{d|d^-}$$
+
+Justification and Impact: Calculating the exact $\hat{x}_{d|k}$ and $P_{d|k}$ would require the "retrodiction" or "backward smoothing" machinery described in the paper (Section 4.1), which necessitates storing all intermediate measurements and transition matrices. By using the stored snapshot, the implementation avoids this storage overhead. This approximation assumes that the measurements received between $t_{meas}$ and $t_{now}$ ($z_{d+1} \dots z_k$) would not have significantly altered the estimate of the state at $t_{meas}$ had they been available.
+
+## Log analysis and expected plots
+
+This section provides an overview of the topics and fields that matter during log review: the published [estimator outputs](#estimator-outputs), the upstream [input feeds](#main-input-feeds). Then log analysis guidance is provided in [What to look for in logs](#what-to-look-for-in-logs) and the [troubleshooting checklist](#troubleshooting-checklist). The [plot examples](#plot-examples) at the end illustrate the expected convergence behaviour.
+
+### Estimator outputs
+
+- Full state estimations with variance and validity flags for position and orientation filters:
+ - `vision_target_est_position`
+ - `vision_target_est_orientation`
+- `vte_bias_init_status`: raw GNSS/vision bias sample, low-pass filtered bias, and the norm of the consecutive raw-bias delta while the initial bias averaging phase is running.
+- `landing_target_pose`: controller-facing pose plus `rel_pos_valid`, `rel_vel_valid`, and `fused` booleans.
+- `vision_target_est_input`: downsampled acceleration in NED and the quaternion fed to each prediction step. Can be used to correlate estimator behaviour with vehicle attitude changes.
+- `vte_aid_*`: one topic per fused source (`vte_aid_gps_pos_target`, `vte_aid_gps_pos_mission`, `vte_aid_gps_vel_uav`, `vte_aid_gps_vel_target`, `vte_aid_fiducial_marker`, `vte_aid_ev_yaw`). Each publishes the innovation, variance, observation, observation variance, chi-squared `test_ratio`, timestamps, and the `fused` flag (true when the update passed the gate).
+
+### Main input feeds
+
+- Vision: `fiducial_marker_pos_report` / `fiducial_marker_yaw_report`
+- GNSS on the target: `target_gnss`
+- Vehicle GNSS: `sensor_gps` (used to convert absolute GNSS measurements to relative vehicle-carried NED measurements)
+- Mission position: `position_setpoint_triplet`
+- `vehicle_local_position` and `vehicle_attitude` (used for frame transforms and timeout checks)
+
+### What to look for in logs
+
+1. **Estimator output vs. observations**: In all axis directions, overlay the estimator outputs position `vision_target_est_position.rel_pos[0]`, `vision_target_est_orientation.yaw` with measurement observations `vte_aid_*.observation[0]` (e.g. `vte_aid_fiducial_marker.observation[0]` or the relevant GNSS observation). The traces should converge after a short transient. Large steady offsets point to calibration errors or incorrect measurement-frame-to-NED transforms.
+2. **Initial bias averaging**: When GNSS is active before vision, inspect `vte_bias_init_status.raw_bias`, `vte_bias_init_status.filtered_bias`, and `vte_bias_init_status.delta_norm`. Here `delta_norm` is the norm between consecutive raw bias samples, so it directly reflects the stability criterion used by the low-pass filter exit. The filtered bias should settle before `vision_target_est_position.bias` is initialized. Large swings in the raw bias or a persistently large `delta_norm` point to unstable early vision data or GNSS/vision time-alignment problems. If vision is active first, you should normally see immediate bias activation instead of an averaging phase.
+3. **Innovation behaviour**: `vte_aid_*.innovation` (e.g. `vte_aid_fiducial_marker.innovation`) should be centred at zero and resemble white noise. Recall that the innovation is defined as the difference between the state prediction and the measurement observation of the state. It follows that the drifting innovations can come from
+ - State prediction errors:
+ - check the estimators state outputs which correspond to the prediction of the state when no measurements are fused
+ - check `vision_target_est_input` (accelerations, attitude)
+ - check the frequency of the prediction `dt`
+ - State update errors:
+ - errors in the measurement implementation, compare the raw sensor input (e.g. `fiducial_marker_pos_report`) with the processed observation (e.g. `vte_aid_fiducial_marker`)
+ - time delays between the attitude of the drone and the observation (causing biases in the frame transform from sensor to NED)
+ - Incorrect noise assumptions
+4. **Measurement acceptance**: Inspect the `fused` boolean alongside `test_ratio`. If the NIS gate rejected the measurements frequently, compare the observation variance being logged with the expected sensor accuracy and update the noise parameters. Remember that gaps longer than [`VTE_TGT_TOUT`](../advanced_config/parameter_reference.md#VTE_TGT_TOUT) clear the validity flags, and exceeding [`VTE_BTOUT`](../advanced_config/parameter_reference.md#VTE_BTOUT) stops the estimator entirely until new data arrives.
+5. **Time alignment**: Compare `timestamp_sample` (measurement time of validity) with `time_last_fuse` (contains the prediction time) on the same `vte_aid_*` topic. The difference should stay within a few milliseconds of the estimator loop period. Persistent offsets are a sign of delayed sensor delivery or missing time synchronisation.
+6. **Coordinate transforms**: Plot the raw measurement (e.g. `fiducial_marker_pos_report.rel_pos[0]`) together with its processed observation `vte_aid_fiducial_marker.observation`. Ensure that there is no mistake in the rotation (`fiducial_marker_pos_report.q`), covariance handling, or offsets applied in the handler.
+7. **Prediction inputs**: Use `vision_target_est_input` to track the downsampled acceleration and quaternion. Compare them to `vehicle_attitude.q` and `vehicle_acceleration` to ensure the estimator sees the expected attitude, especially when diagnosing timestamp mismatches.
+
+### Troubleshooting checklist
+
+Start by confirming that the estimator is running (`vision_target_estimator status`) and that the relevant `vte_aid_*` topic is present in the log. If a topic is missing, verify that the upstream sensor message is publishing and that the fusion mask bit is enabled.
+
+>[!TIP]
+> **Task activation**: Enable the debug bit in [`VTE_TASK_MASK`](../advanced_config/parameter_reference.md#VTE_TASK_MASK). When the debug bit is set the estimators always runs, making it ideal for bench testing. Enable **debug prints** (`PX4_DEBUG`).
+
+| Symptom | Likely cause | Remedy |
+| --- | --- | --- |
+| Vehicle misses the pad or ignores target yaw | Mission land waypoint not set to precision mode or yaw alignment disabled | In QGroundControl set the land waypoint `Precision landing` field (or `MAV_CMD_NAV_LAND` `param2`) to Opportunistic/Required as described in [Precision landing missions](../advanced_features/precland.md#mission), and enable [`PLD_YAW_EN`](../advanced_config/parameter_reference.md#PLD_YAW_EN). In logs verify that `trajectory_setpoint.x/y` converge to `landing_target_pose.x_abs/y_abs` and that `trajectory_setpoint.yaw` follows `vision_target_est_orientation.yaw`. |
+| Frequent innovation rejections | Incorrect noise floors or timestamp skew | Verify sensor variances, verify the NIS threholds ([`VTE_POS_NIS_THRE`](../advanced_config/parameter_reference.md#VTE_POS_NIS_THRE) and [`VTE_YAW_NIS_THRE`](../advanced_config/parameter_reference.md#VTE_YAW_NIS_THRE)), compare timestamp_sample (measurement time of validity) with time_last_fuse (contains the prediction time) on the same vte_aid_* topic.. |
+| Bias does not converge | No secondary position source, or the GNSS/vision agreement check is too strict for the available vision quality | Ensure vision fusion is enabled so the filter can observe GNSS bias. If GNSS is active before vision and the bias stays near zero, inspect the first GNSS/vision agreement window and tune [`VTE_BIA_AVG_THR`](../advanced_config/parameter_reference.md#VTE_BIA_AVG_THR) / [`VTE_BIA_AVG_TOUT`](../advanced_config/parameter_reference.md#VTE_BIA_AVG_TOUT). If vision is active first, expect the bias to appear on the first overlapping GNSS+vision sample rather than after an averaging phase. |
+| Orientation estimate drifts | Missing yaw measurements or low NIS gate | Enable [`VTE_YAW_EN`](../advanced_config/parameter_reference.md#VTE_YAW_EN) and ensure vision yaw data is present. Increase [`VTE_YAW_NIS_THRE`](../advanced_config/parameter_reference.md#VTE_YAW_NIS_THRE) if legitimate data is being rejected. |
+| `rel_pos_valid` toggles during descent | Position measurements arriving too slowly | Increase [`VTE_TGT_TOUT`](../advanced_config/parameter_reference.md#VTE_TGT_TOUT) or improve the measurement rate so updates remain inside [`VTE_M_REC_TOUT`](../advanced_config/parameter_reference.md#VTE_M_REC_TOUT). |
+| No `vte_aid_*` topics in the log | Sensor not publishing or fusion mask disabled | Use `listener` on the raw sensor topic, confirm [`VTE_AID_MASK`](../advanced_config/parameter_reference.md#VTE_AID_MASK) includes the relevant bit, and rerun the test with the debug task active. |
+| Estimator never starts | Task mask disabled or mission not requesting precision landing | Set [`VTE_TASK_MASK`](../advanced_config/parameter_reference.md#VTE_TASK_MASK)=1 (or 3 for continuous debugging) and verify that new measurements arrive with valid timestamps. |
+
+### Plot examples
+
+The next four dashboards provide hints on how to analyse logs of the Vision Target Estimator.
+
+>[!TIP]
+> Plot Juggler (or the PX4 DevTools log viewer) is the easiest way to inspect the estimator as it allows you to group related signals into subplots that share the time axis.
+
+**Estimator output and observation consistency**: Quick health check that the fused sensors agree with the estimated state before diving into per-axis innovations.
+- **Top row (observations and state output)**: For every axis, plot all the available observations `vte_aid_*.observation[0]` (in this example: vision/IRLock and target GNSS) alongside `vision_target_est_position`. Expect a smooth state trace following the trend of the most precise observations.
+- **Second row (GNSS bias estimate)**: Plot `vision_target_est_position.bias[0]`, `vision_target_est_position.bias[1]`, `vision_target_est_position.bias[2]`. The bias should settle once both a GNSS observation and a relative observation are available. In the GNSS-first case it can remain near zero during the initial averaging phase while valid vision samples are still updating the low-pass filter. In the vision-first case it should appear immediately on the first overlapping GNSS+vision sample. If GNSS is slightly older than vision, VTEST propagates the current GNSS-relative measurement with the UAV velocity estimate to match the vision timestamp. A non-zero bias is expected after activation; what matters is that it remains stable so the corrected GNSS still points to the pad if the relative measurement temporarily drops out.
+- **Third row (sensor variances)**: Compare `vte_aid_*.observation_variance[0]` across sensors. Large gaps mean one source is trusted far more than the others. Tune [`VTE_EVP_NOISE`](../advanced_config/parameter_reference.md#VTE_EVP_NOISE) or [`VTE_GPS_P_NOISE`](../advanced_config/parameter_reference.md#VTE_GPS_P_NOISE) until variances reflect the real-world accuracy.
+- **Bottom row (approach context)**: `vehicle_local_position.dist_bottom` indicates the descent phase and helps correlate changes in variance or bias with altitude.
+
+
+
+**Innovation Consistency**: Use this view to confirm that every fused sensor produces zero-mean, white-noise innovations; large drifts reveal modelling errors, mis-scaled noise, or frame-misaligned observations.
+- **Top row (x innovations)**: For every measurement available, plot all the innovations (e.g. `vte_aid_fiducial_marker.innovation[0]` and `vte_aid_gps_pos_target.innovation[0]`). The innovations should fluctuate around zero.
+- **Second row (y innovations)**: The same signals for the y direction. White-noise like innovations indicate the measurements and the assumed noise matches reality.
+- **Third row (z innovations)**: Index 2 is only populated by sources that provide altitude information. A persistent offset, such as the ~2 m bias from `vte_aid_fiducial_marker` in the example, shows that the camera observations are not precise at higher altitudes. The measurement variance should reflect this behaviour.
+- **Bottom row (fusion decisions)**: The boolean `*.fused` arrays confirm that the Kalman update is skipped whenever the NIS exceeds the gate. If a good sensor is frequently rejected, revisit the variance floors listed earlier.
+
+
+
+**Rejecting a Corrupted Measurement**: Demonstrates how the estimator isolates a faulty measurement dimension. The healthy axes continue to fuse while the outlier repeatedly fails the gate, making it easy to spot sensor degradations.
+
+- **Top row (y observation)**: `vte_aid_fiducial_marker.observation[1]` tracks `vision_target_est_position.rel_pos[1]`, showing how a healthy measurement pulls the estimate.
+- **Second row (z observation)**: `vte_aid_fiducial_marker.observation[2]` deviates strongly from `vision_target_est_position.rel_pos[2]`; the estimator sensibly refuses to follow it.
+- **Third row (fusion flag)**: `vte_aid_fiducial_marker.fused[2]` drops to zero whenever the vertical observation disagrees with the filter prediction, confirming that the gate is working.
+- **Bottom row (test ratios)**: `vte_aid_fiducial_marker.test_ratio[1]` stays near zero, while index 2 spikes above one, breaching [`VTE_POS_NIS_THRE`](../advanced_config/parameter_reference.md#VTE_POS_NIS_THRE). Investigate the vision covariance estimate or adjust [`VTE_EVP_NOISE`](../advanced_config/parameter_reference.md#VTE_EVP_NOISE) if this pattern occurs frequently.
+
+
+
+**Precision Landing Alignment**: Compare the precision landing target with the vehicle position to ensure that the vehicle navigates towards the target. It can help catch misconfigurations e.g. [`PLD_YAW_EN`](../advanced_config/parameter_reference.md#PLD_YAW_EN) disabled or it can if the controller struggles to follow, review the [multicopter position tuning guide](../config_mc/pid_tuning_guide_multicopter.md). Check that the absolute position of the target remains stable.
+
+- **Top row (north alignment)**: Compare `landing_target_pose.x_abs` with `vehicle_local_position.x`. The curves should overlay once precision landing is triggered.
+- **Second row (east alignment)**: `landing_target_pose.y_abs` versus `vehicle_local_position.y` reveals lateral errors.
+- **Third row (yaw alignment)**: `vision_target_est_orientation.yaw` should remain steady while `trajectory_setpoint.yaw` tracks it when [`PLD_YAW_EN`](../advanced_config/parameter_reference.md#PLD_YAW_EN) is enabled.
+- **Bottom row (descent context)**: `vehicle_local_position.dist_bottom` indicates when the final approach begins, providing context for any transients above.
+
+
+
+**Orientation Filter**: Validate that yaw aiding stays smooth yet responsive; this plot highlights whether orientation noise is properly tuned and whether the gating logic keeps unreliable observations out of the state.
+
+- **Top row (yaw observation vs. state)**: `vte_aid_ev_yaw.observation` compared with `vision_target_est_orientation.yaw`. The smoothed state should stay close without mirroring the high-frequency noise; if it does, increase [`VTE_EVA_NOISE`](../advanced_config/parameter_reference.md#VTE_EVA_NOISE).
+- **Second row (innovation)**: `vte_aid_ev_yaw.innovation` should resemble white noise. Long biases indicate an incorrect camera-to-vehicle rotation.
+- **Third row (test ratio)**: `vte_aid_ev_yaw.test_ratio` highlights altitude-driven noise. Brief spikes near touchdown are acceptable, but sustained high ratios mean yaw gating might be too tight.
+- **Bottom row (fusion flag)**: `vte_aid_ev_yaw.fused` shows when yaw measurements actually update the state. Loss of fusion at high altitude is common; ensure it returns before `dist_bottom` goes below the final approach threshold.
+
+
+
+## Development and debugging tips
+
+- To print `PX4_DEBUG` statements from the module, launch SITL with `PX4_LOG_LEVEL=debug` (for example, `PX4_LOG_LEVEL=debug make px4_sitl_visionTargetEst`). On hardware builds, compile with the debug configuration or enable the console log level before running tests so the additional diagnostics appear on the shell.
+- Keep the estimator alive on the bench by setting [`VTE_TASK_MASK`](../advanced_config/parameter_reference.md#VTE_TASK_MASK)=3; the debug bit enables the continuous update of the position and orientation estimators (if enabled via [`VTE_YAW_EN`](../advanced_config/parameter_reference.md#VTE_YAW_EN) and [`VTE_POS_EN`](../advanced_config/parameter_reference.md#VTE_POS_EN)).
+- Use shell helpers while iterating: `listener landing_target_pose`, `listener vte_bias_init_status`, `listener vte_aid_fiducial_marker` (or the relevant `vte_aid_*`) to inspect bias averaging and innovations, `listener vision_target_est_input 5` for prediction inputs, and `vision_target_estimator status` to ensure both filters are running.
+
+### Adding new measurement sources
+
+To integrate a new sensor:
+
+1. **Define a uORB message** that carries the measurement in either the vehicle body frame or NED, complete with variance estimates and timestamps.
+2. **Extend the fusion mask**: add a bit to `SensorFusionMaskU` (`src/modules/vision_target_estimator/common.h`) and update the parameter definition for [`VTE_AID_MASK`](../advanced_config/parameter_reference.md#VTE_AID_MASK) (see also [sensor fusion selection](../advanced_features/vision_target_estimator.md#sensor-fusion-selection)) in `vision_target_estimator_params.yaml`.
+3. **Augment observation enums**: append the new entry to the relevant `ObsType` enum (`VTEPosition.h` or `VTEOrientation.h`), update `ObsValidMaskU`, and update helper functions such as `hasNewNonGpsPositionSensorData()` and `selectInitialPosition()` if the measurement can provide a a relative position.
+4. **Subscribe and validate**: add a `uORB::Subscription` to the filter, check for finite values, and reject samples that are too old `isMeasUpdated` or timestamped in the future before marking the observation valid.
+5. **Implement the handler** in `processObservations()`. Convert the measurement into NED coordinates, populate `TargetObs::meas_xyz`, `meas_unc_xyz`, and the observation Jacobian (`meas_h_xyz` or `meas_h_theta`), and set the fusion-mask flag only after the data passes validation.
+6. **Provide tunable noise**: declare a parameter (e.g. `VTE__NOISE`) and clamp it with `kMinObservationNoise` so the estimator never believes a measurement is perfect.
+7. **Log the innovations**: add a publication member and ORB topic (see `vte_aid_fiducial_marker` for reference) so that logs include the innovation, variance, and fused flag for the new sensor.
+8. **Exercise SITL**: update the Gazebo (or other) simulation so that replay tests produce the new measurement. This keeps CI coverage intact and provides a reference data set for tuning.
+9. **Document the workflow**: update this deep dive and any setup how-tos so users know how to enable the new bit, calibrate the sensor, and interpret its logs.
+
+>[!IMPORTANT]
+> **Timeout policy**: Every measurement must be time-aligned and checked so that stale data never reaches the update step. Reject samples older than [`VTE_M_REC_TOUT`](../advanced_config/parameter_reference.md#VTE_M_REC_TOUT) (`isMeasRecent(hrt_abstime ts)`) and double-check `timestamp_sample` vs. `time_last_fuse` in logs to confirm the new sensor fits inside the estimator deadlines. If observations are stored in cache, invalidate it inside `checkMeasurementInputs` when older than [`VTE_M_UPD_TOUT`](../advanced_config/parameter_reference.md#VTE_M_UPD_TOUT) (`isMeasUpdated(hrt_abstime ts)`).
+
+### Regenerating the symbolic model
+
+The generated headers (`predictState.h`, `predictCov.h`, `computeInnovCov.h`, `getTransitionMatrix.h`, `state.h`) are copied into the build directory and should never be edited by hand. To regenerate them:
+
+1. Configure CMake with `-DVTEST_SYMFORCE_GEN=ON` (automatically enabled when `CONFIG_VTEST_MOVING=y`) and ensure SymForce is available in the Python environment.
+2. Reconfigure (`cmake --fresh ...`) so the custom command in `src/modules/vision_target_estimator/CMakeLists.txt` runs. The generated outputs land under `build//src/modules/vision_target_estimator/vtest_derivation/generated/` for position and `build//src/modules/vision_target_estimator/vte_orientation_derivation/generated/` for orientation.
+3. If you need to refresh the committed defaults, set `-DVTEST_UPDATE_COMMITTED_DERIVATION=ON` and commit the regenerated files in `Position/vtest_derivation/generated*/` once vetted.
+
+If the build fails during regeneration, inspect the CMake output for the SymForce invocation and rerun it manually inside `Position/vtest_derivation/` to catch Python errors. After regenerating, rebuild the module to ensure the Jacobians and code stay in sync.
+
+## Unit test suites
+
+- `TEST_VTE_KF_position`: Kalman filter math for the position state (prediction, NIS gating, bias-aware H, OOSM gold standard). Static model (state size 3) by default, moving-target tests (state size 5) build only when `CONFIG_VTEST_MOVING` is enabled.
+- `TEST_VTE_KF_orientation`: Kalman filter math for yaw/yaw-rate (wrap logic, process noise, dt edge cases, covariance symmetry).
+- `TEST_VTE_VTEPosition`: Module logic for vision/GNSS fusion, offsets, interpolation, ordering, and uORB innovation topics (static + moving gated by `CONFIG_VTEST_MOVING`).
+- `TEST_VTE_VTEOrientation`: Module logic for yaw fusion, noise models, resets, and OOSM handling.
+- `TEST_VTE_VTEOosm`: Generic OOSM manager behavior.
+
+Run locally:
+
+```sh
+make tests TESTFILTER=TEST_VTE*
+```
+
+Timing constraints to keep in mind when exercising OOSM paths:
+
+- OOSM activates when measurement lag exceeds 20 ms.
+- History accepts measurements up to 500 ms old before rejecting them.
diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt
index 3ecb1ee59769..429874edbf72 100644
--- a/msg/CMakeLists.txt
+++ b/msg/CMakeLists.txt
@@ -286,6 +286,20 @@ set(msg_files
)
list(SORT msg_files)
+if(CONFIG_MODULES_VISION_TARGET_ESTIMATOR)
+ list(APPEND msg_files
+ TargetGnss.msg
+ VisionTargetEstInput.msg
+ VisionTargetEstPosition.msg
+ VisionTargetEstOrientation.msg
+ VteAidSource1d.msg
+ VteAidSource3d.msg
+ VteBiasInitStatus.msg
+ FiducialMarkerPosReport.msg
+ FiducialMarkerYawReport.msg
+ PrecLandStatus.msg)
+endif()
+
px4_list_make_absolute(msg_files ${CMAKE_CURRENT_SOURCE_DIR} ${msg_files})
if(NOT EXTERNAL_MODULES_LOCATION STREQUAL "")
diff --git a/msg/FiducialMarkerPosReport.msg b/msg/FiducialMarkerPosReport.msg
new file mode 100644
index 000000000000..931d72f1dd2a
--- /dev/null
+++ b/msg/FiducialMarkerPosReport.msg
@@ -0,0 +1,9 @@
+# Relative position of precision land target
+
+uint64 timestamp # [us] time since system start
+uint64 timestamp_sample # [us] timestamp of the raw observation
+
+float32[3] rel_pos # [m] Target position relative to vehicle, expressed in the frame defined by q
+float32[3] cov_rel_pos # [m^2] Target position variance, expressed in the frame defined by q
+
+float32[4] q # [-] Quaternion rotation from the rel_pos frame to the NED earth frame
diff --git a/msg/FiducialMarkerYawReport.msg b/msg/FiducialMarkerYawReport.msg
new file mode 100644
index 000000000000..9024e7beedf8
--- /dev/null
+++ b/msg/FiducialMarkerYawReport.msg
@@ -0,0 +1,7 @@
+# Relative orientation of landing target in NED (North, East, Down)
+
+uint64 timestamp # [us] time since system start
+uint64 timestamp_sample # [us] timestamp of the raw observation
+
+float32 yaw_ned # [rad] [@frame NED] orientation of the target relative to the NED frame [-Pi ; Pi]
+float32 yaw_var_ned # [rad^2] orientation uncertainty
diff --git a/msg/LandingTargetPose.msg b/msg/LandingTargetPose.msg
index 875920f183b8..1c4495bb9418 100644
--- a/msg/LandingTargetPose.msg
+++ b/msg/LandingTargetPose.msg
@@ -1,26 +1,30 @@
# Relative position of precision land target in navigation (body fixed, north aligned, NED) and inertial (world fixed, north aligned, NED) frames
-uint64 timestamp # time since system start (microseconds)
+uint64 timestamp # [us] time since system start
-bool is_static # Flag indicating whether the landing target is static or moving with respect to the ground
+bool is_static # [-] Flag indicating whether the landing target is static or moving with respect to the ground
-bool rel_pos_valid # Flag showing whether relative position is valid
-bool rel_vel_valid # Flag showing whether relative velocity is valid
+bool rel_pos_valid # [-] Flag showing whether relative position is valid
+bool rel_vel_valid # [-] Flag showing whether relative velocity is valid
-float32 x_rel # X/north position of target, relative to vehicle (navigation frame) [meters]
-float32 y_rel # Y/east position of target, relative to vehicle (navigation frame) [meters]
-float32 z_rel # Z/down position of target, relative to vehicle (navigation frame) [meters]
+float32 x_rel # [m] X/north position of target, relative to vehicle (navigation frame)
+float32 y_rel # [m] Y/east position of target, relative to vehicle (navigation frame)
+float32 z_rel # [m] Z/down position of target, relative to vehicle (navigation frame)
-float32 vx_rel # X/north velocity of target, relative to vehicle (navigation frame) [meters/second]
-float32 vy_rel # Y/east velocity of target, relative to vehicle (navigation frame) [meters/second]
+float32 vx_rel # [m/s] X/north velocity of target, relative to vehicle (navigation frame)
+float32 vy_rel # [m/s] Y/east velocity of target, relative to vehicle (navigation frame)
+float32 vz_rel # [m/s] Z/down velocity of target, relative to vehicle (navigation frame)
-float32 cov_x_rel # X/north position variance [meters^2]
-float32 cov_y_rel # Y/east position variance [meters^2]
+float32 cov_x_rel # [m^2] X/north position variance
+float32 cov_y_rel # [m^2] Y/east position variance
+float32 cov_z_rel # [m^2] Z/down position variance
-float32 cov_vx_rel # X/north velocity variance [(meters/second)^2]
-float32 cov_vy_rel # Y/east velocity variance [(meters/second)^2]
+float32 cov_vx_rel # [(m/s)^2] X/north velocity variance
+float32 cov_vy_rel # [(m/s)^2] Y/east velocity variance
+float32 cov_vz_rel # [(m/s)^2] Z/down velocity variance
-bool abs_pos_valid # Flag showing whether absolute position is valid
-float32 x_abs # X/north position of target, relative to origin (navigation frame) [meters]
-float32 y_abs # Y/east position of target, relative to origin (navigation frame) [meters]
-float32 z_abs # Z/down position of target, relative to origin (navigation frame) [meters]
+bool abs_pos_valid # [-] Flag showing whether absolute position is valid
+
+float32 x_abs # [m] X/north position of target, relative to origin (navigation frame)
+float32 y_abs # [m] Y/east position of target, relative to origin (navigation frame)
+float32 z_abs # [m] Z/down position of target, relative to origin (navigation frame)
diff --git a/msg/PrecLandStatus.msg b/msg/PrecLandStatus.msg
new file mode 100644
index 000000000000..fc9fb988a19c
--- /dev/null
+++ b/msg/PrecLandStatus.msg
@@ -0,0 +1,16 @@
+# PREC_LAND_STATUS message data
+uint8 PREC_LAND_STATE_STOPPED = 0
+uint8 PREC_LAND_STATE_ONGOING = 1
+
+uint8 PREC_LAND_NAV_STATE_START = 0
+uint8 PREC_LAND_NAV_STATE_HORIZONTAL = 1
+uint8 PREC_LAND_NAV_STATE_DESCEND = 2
+uint8 PREC_LAND_NAV_STATE_FINAL = 3
+uint8 PREC_LAND_NAV_STATE_SEARCH = 4
+uint8 PREC_LAND_NAV_STATE_FALLBACK = 5
+uint8 PREC_LAND_NAV_STATE_DONE = 6
+
+uint64 timestamp # [us] time since system start
+
+uint8 state # [-] PX4 state of delivery (PREC_LAND_STATE_*)
+uint8 nav_state # [-] Precision delivery state (PREC_LAND_NAV_STATE_*)
diff --git a/msg/TargetGnss.msg b/msg/TargetGnss.msg
new file mode 100644
index 000000000000..b62b2269416c
--- /dev/null
+++ b/msg/TargetGnss.msg
@@ -0,0 +1,21 @@
+# landing target GNSS position in WGS84 coordinates.
+
+uint64 timestamp # [us] time since system start
+uint64 timestamp_sample # [us] timestamp of the raw observation
+
+float64 latitude_deg # [deg] Latitude, allows centimeter level RTK precision
+float64 longitude_deg # [deg] Longitude, allows centimeter level RTK precision
+float32 altitude_msl_m # [m] Altitude above MSL
+
+float32 eph # [m] GNSS horizontal position accuracy
+float32 epv # [m] GNSS vertical position accuracy
+
+bool abs_pos_updated # [-] True if WGS84 position is updated
+
+float32 vel_n_m_s # [m/s] GNSS North velocity
+float32 vel_e_m_s # [m/s] GNSS East velocity
+float32 vel_d_m_s # [m/s] GNSS Down velocity
+
+float32 s_acc_m_s # [m/s] GNSS speed accuracy estimate
+
+bool vel_ned_updated # [-] True if NED velocity is updated
diff --git a/msg/VisionTargetEstInput.msg b/msg/VisionTargetEstInput.msg
new file mode 100644
index 000000000000..b62b1de6fa03
--- /dev/null
+++ b/msg/VisionTargetEstInput.msg
@@ -0,0 +1,8 @@
+# Vision Target Estimator input
+
+
+uint64 timestamp # [us] time since system start
+uint64 timestamp_sample # [us] timestamp of the raw input data
+
+float32[3] acc_xyz # [m/s^2] [@frame NED] downsampled UAV bias-corrected acceleration (including gravity)
+float32[4] q_att # [-] downsampled UAV attitude quaternion (FRD body -> NED earth)
diff --git a/msg/VisionTargetEstOrientation.msg b/msg/VisionTargetEstOrientation.msg
new file mode 100644
index 000000000000..29e723c9fda7
--- /dev/null
+++ b/msg/VisionTargetEstOrientation.msg
@@ -0,0 +1,11 @@
+# Vision Target Estimator orientation state
+
+uint64 timestamp # [us] time since system start
+
+bool orientation_valid # [-] relative orientation estimate valid
+
+float32 yaw # [rad] [@frame NED] target yaw angle
+float32 cov_yaw # [rad^2] variance of yaw
+
+float32 yaw_rate # [rad/s] [@frame NED] target yaw rate
+float32 cov_yaw_rate # [(rad/s)^2] variance of yaw_rate
diff --git a/msg/VisionTargetEstPosition.msg b/msg/VisionTargetEstPosition.msg
new file mode 100644
index 000000000000..8882208066a4
--- /dev/null
+++ b/msg/VisionTargetEstPosition.msg
@@ -0,0 +1,18 @@
+# Vision Target Estimator position state
+
+uint64 timestamp # [us] time since system start
+
+bool rel_pos_valid # [-] relative position estimate valid
+bool rel_vel_valid # [-] relative velocity estimate valid
+
+float32[3] rel_pos # [m] [@frame NED] target position relative to vehicle
+float32[3] vel_uav # [m/s] [@frame NED] vehicle velocity
+float32[3] vel_target # [m/s] [@frame NED] target velocity
+float32[3] bias # [m] [@frame NED] GNSS bias between vehicle and target receivers
+float32[3] acc_target # [m/s^2] [@frame NED] target acceleration
+
+float32[3] cov_rel_pos # [m^2] [@frame NED] variance of rel_pos
+float32[3] cov_vel_uav # [(m/s)^2] [@frame NED] variance of vel_uav
+float32[3] cov_bias # [m^2] [@frame NED] variance of bias
+float32[3] cov_vel_target # [(m/s)^2] [@frame NED] variance of vel_target
+float32[3] cov_acc_target # [(m/s^2)^2] [@frame NED] variance of acc_target
diff --git a/msg/VteAidSource1d.msg b/msg/VteAidSource1d.msg
new file mode 100644
index 000000000000..4fef8ac99a60
--- /dev/null
+++ b/msg/VteAidSource1d.msg
@@ -0,0 +1,32 @@
+# Vision Target Estimator 1D fusion aid source
+
+uint64 timestamp # [us] time since system start
+uint64 timestamp_sample # [us] timestamp of the raw observation
+uint64 time_last_predict # [us] timestamp of last filter prediction
+
+# Observation & Innovation
+float32 observation # [-] observation attempted to be fused
+float32 observation_variance # [-] variance of observation attempted to be fused
+
+float32 innovation # [-] Kalman Filter innovation (y = z - Hx)
+float32 innovation_variance # [-] Kalman Filter variance of the innovation
+
+float32 test_ratio # [-] normalized innovation squared (NIS)
+
+# Detailed status code for debugging "Why did it fail?"
+# 0: STATUS_IDLE
+# 1: STATUS_FUSED_CURRENT # Fused immediately (low latency)
+# 2: STATUS_FUSED_OOSM # Fused via history buffer
+# 3: STATUS_REJECT_NIS # Rejected by Normalized Innovation Squared check
+# 4: STATUS_REJECT_COV # Rejected due to invalid/infinite covariance or numerical error
+# 5: STATUS_REJECT_TOO_OLD # Rejected: older than buffer limit (kOosmMaxTimeUs) or oldest sample
+# 6: STATUS_REJECT_TOO_NEW # Rejected: timestamp in the future (beyond tolerance)
+# 7: STATUS_REJECT_STALE # Rejected: history was reset due to staleness/discontinuity
+# 8: STATUS_REJECT_EMPTY # Rejected: history buffer not yet populated
+uint8 fusion_status # [-] fusion status code
+
+# OOSM Diagnostics
+float32 time_since_meas_ms # [ms] (now - timestamp_sample)
+uint8 history_steps # [-] number of steps replayed in OOSM (0 if current or failed)
+
+# TOPICS vte_aid_ev_yaw
diff --git a/msg/VteAidSource3d.msg b/msg/VteAidSource3d.msg
new file mode 100644
index 000000000000..f587024cb4b1
--- /dev/null
+++ b/msg/VteAidSource3d.msg
@@ -0,0 +1,32 @@
+# Vision Target Estimator 3D fusion aid source
+
+uint64 timestamp # [us] time since system start
+uint64 timestamp_sample # [us] timestamp of the raw observation
+uint64 time_last_predict # [us] timestamp of last filter prediction
+
+# Observation & Innovation
+float32[3] observation # [-] [@frame NED] sensor observation attempted to be fused
+float32[3] observation_variance # [-] [@frame NED] variance of the observation attempted to be fused
+
+float32[3] innovation # [-] [@frame NED] Kalman Filter innovation (y = z - Hx)
+float32[3] innovation_variance # [-] [@frame NED] Kalman Filter variance of the innovation
+
+float32[3] test_ratio # [-] normalized innovation squared (NIS)
+
+# 0: STATUS_IDLE
+# 1: STATUS_FUSED_CURRENT # Fused immediately (low latency)
+# 2: STATUS_FUSED_OOSM # Fused via history buffer
+# 3: STATUS_REJECT_NIS # Rejected by Normalized Innovation Squared check
+# 4: STATUS_REJECT_COV # Rejected due to invalid/infinite covariance or numerical error
+# 5: STATUS_REJECT_TOO_OLD # Rejected: older than buffer limit (kOosmMaxTimeUs) or oldest sample
+# 6: STATUS_REJECT_TOO_NEW # Rejected: timestamp in the future (beyond tolerance)
+# 7: STATUS_REJECT_STALE # Rejected: history was reset due to staleness/discontinuity
+# 8: STATUS_REJECT_EMPTY # Rejected: history buffer not yet populated
+uint8[3] fusion_status # [-] Enum detailing the status code of the sensor fusion
+
+# OOSM Diagnostics (Shared across axes)
+float32 time_since_meas_ms # [ms] (now - timestamp_sample)
+uint8 history_steps # [-] number of steps replayed in OOSM (0 if current or failed)
+
+# TOPICS vte_aid_gps_pos_target vte_aid_gps_pos_mission vte_aid_gps_vel_target vte_aid_gps_vel_uav
+# TOPICS vte_aid_fiducial_marker
diff --git a/msg/VteBiasInitStatus.msg b/msg/VteBiasInitStatus.msg
new file mode 100644
index 000000000000..236e6a0eda44
--- /dev/null
+++ b/msg/VteBiasInitStatus.msg
@@ -0,0 +1,7 @@
+# Vision Target Estimator initial GNSS bias averaging status
+
+uint64 timestamp # [us] time since system start
+
+float32[3] raw_bias # [m] [@frame NED] current GNSS-vision bias sample
+float32[3] filtered_bias # [m] [@frame NED] low-pass filtered bias sample
+float32 delta_norm # [m] norm(raw_bias_k - raw_bias_k-1)
diff --git a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp
index 4ce4db1e7434..dbee70a8ed9e 100644
--- a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp
+++ b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp
@@ -70,6 +70,8 @@ static constexpr wq_config_t I2C4{"wq:I2C4", CONFIG_WQ_I2C_STACKSIZE, (int8_t)CO
// PX4 att/pos controllers, highest priority after sensors.
static constexpr wq_config_t nav_and_controllers{"wq:nav_and_controllers", CONFIG_WQ_NAV_AND_CONTROLLERS_STACKSIZE, (int8_t)CONFIG_WQ_NAV_AND_CONTROLLERS_PRIORITY};
+static constexpr wq_config_t vte{"wq:vte", CONFIG_WQ_VTE_STACKSIZE, (int8_t)CONFIG_WQ_VTE_PRIORITY};
+
static constexpr wq_config_t INS0{"wq:INS0", CONFIG_WQ_INS_STACKSIZE, (int8_t)CONFIG_WQ_INS0_PRIORITY};
static constexpr wq_config_t INS1{"wq:INS1", CONFIG_WQ_INS_STACKSIZE, (int8_t)CONFIG_WQ_INS1_PRIORITY};
static constexpr wq_config_t INS2{"wq:INS2", CONFIG_WQ_INS_STACKSIZE, (int8_t)CONFIG_WQ_INS2_PRIORITY};
diff --git a/platforms/common/px4_work_queue/Kconfig b/platforms/common/px4_work_queue/Kconfig
index bb320e9519c7..0f2a2ac004ce 100644
--- a/platforms/common/px4_work_queue/Kconfig
+++ b/platforms/common/px4_work_queue/Kconfig
@@ -138,6 +138,20 @@ config WQ_NAV_AND_CONTROLLERS_PRIORITY
help
Sets the relative priority for the nav_and_controllers work queue.
+config WQ_VTE_STACKSIZE
+ int "Stack size for vte"
+ default 6000
+ range 1000 10000
+ help
+ Sets the stack size for the vte work queue.
+
+config WQ_VTE_PRIORITY
+ int "Relative priority for vte"
+ default -14
+ range -255 0
+ help
+ Sets the relative priority for the vte work queue.
+
menu "INS Work Queues"
config WQ_INS_STACKSIZE
diff --git a/src/lib/ringbuffer/CMakeLists.txt b/src/lib/ringbuffer/CMakeLists.txt
index 538c1ae90717..af30832d62cf 100644
--- a/src/lib/ringbuffer/CMakeLists.txt
+++ b/src/lib/ringbuffer/CMakeLists.txt
@@ -38,4 +38,5 @@ px4_add_library(ringbuffer
target_include_directories(ringbuffer PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
px4_add_unit_gtest(SRC RingbufferTest.cpp LINKLIBS ringbuffer)
-px4_add_unit_gtest(SRC TimestampedRingBufferTest.cpp)
+px4_add_unit_gtest(SRC TimestampedRingBufferDynamicTest.cpp)
+px4_add_unit_gtest(SRC TimestampedRingBufferStaticTest.cpp)
diff --git a/src/lib/ringbuffer/TimestampedRingBuffer.hpp b/src/lib/ringbuffer/TimestampedRingBuffer.hpp
index a96f1c35a023..4548ce7b0d10 100644
--- a/src/lib/ringbuffer/TimestampedRingBuffer.hpp
+++ b/src/lib/ringbuffer/TimestampedRingBuffer.hpp
@@ -32,21 +32,134 @@
****************************************************************************/
/**
- * @file TimestampedRingBuffer.h
- * @author Roman Bapst
- * @brief Template ring buffer for timestamped samples (requires data_type::time_us).
- * Note: This is not the same as `Ringbuffer` (byte FIFO) in `src/lib/ringbuffer/Ringbuffer.hpp`.
- */
+* @file TimestampedRingBuffer.hpp
+* @author Roman Bapst
+* @brief Template ring buffer for timestamped samples (requires data_type::time_us).
+*
+* Note: This is not the same as `Ringbuffer` (byte FIFO) in `src/lib/ringbuffer/Ringbuffer.hpp`.
+*/
#pragma once
+#include
#include
-#include
-#include
-template
+template
+class TimestampedRingBuffer;
+
+/**
+ * Static-size ring buffer specialization (SIZE > 0).
+ */
+template
class TimestampedRingBuffer
{
+public:
+ static_assert(SIZE > 0, "SIZE must be > 0");
+ static_assert(SIZE <= UINT8_MAX, "SIZE must fit in uint8_t");
+ static constexpr uint8_t kSize = static_cast(SIZE);
+
+ TimestampedRingBuffer() = default;
+ ~TimestampedRingBuffer() = default;
+
+ // no copy, assignment, move, move assignment
+ TimestampedRingBuffer(const TimestampedRingBuffer &) = delete;
+ TimestampedRingBuffer &operator=(const TimestampedRingBuffer &) = delete;
+ TimestampedRingBuffer(TimestampedRingBuffer &&) = delete;
+ TimestampedRingBuffer &operator=(TimestampedRingBuffer &&) = delete;
+
+ bool allocate(size_t size) { return size == SIZE; }
+
+ bool valid() const { return SIZE > 0; }
+
+ void push(const data_type &sample)
+ {
+ _buffer[_head] = sample;
+ _head = next(_head);
+
+ if (_entries < kSize) {
+ _entries++;
+ }
+ }
+
+ uint8_t get_length() const { return kSize; }
+
+ data_type &operator[](const uint8_t index) { return _buffer[index]; }
+
+ const data_type &get_newest() const { return _buffer[get_newest_index()]; }
+ const data_type &get_oldest() const { return _buffer[get_oldest_index()]; }
+
+ uint8_t get_newest_index() const { return (_entries > 0) ? prev(_head) : 0; }
+ uint8_t get_oldest_index() const
+ {
+ if (_entries == 0) {
+ return 0;
+ }
+
+ const uint16_t idx = static_cast(_head) + static_cast(kSize) - static_cast(_entries);
+ return (idx >= kSize) ? static_cast(idx - kSize) : static_cast(idx);
+ }
+
+ uint8_t next(uint8_t i) const { return (i + 1 >= kSize) ? 0 : i + 1; }
+ uint8_t prev(uint8_t i) const { return (i == 0) ? kSize - 1 : i - 1; }
+
+ bool pop_first_older_than(const uint64_t ×tamp, data_type *sample)
+ {
+ // start looking from newest observation data
+ const uint8_t newest_idx = get_newest_index();
+ const uint8_t oldest_idx = get_oldest_index();
+
+ for (uint8_t i = 0; i < _entries; i++) {
+ int index = (newest_idx - i);
+ index = index < 0 ? kSize + index : index;
+
+ if (timestamp >= _buffer[index].time_us && timestamp < _buffer[index].time_us + (uint64_t)1e5) {
+ *sample = _buffer[index];
+
+ // Now we can set the tail to the item which
+ // comes after the one we removed since we don't
+ // want to have any older data in the buffer
+ const uint8_t index_u = static_cast(index);
+ const uint8_t distance = (index_u >= oldest_idx) ? (index_u - oldest_idx) : (index_u + kSize - oldest_idx);
+ const uint8_t discard_count = distance + 1;
+ _entries = (_entries > discard_count) ? (_entries - discard_count) : 0;
+ _buffer[index].time_us = 0;
+
+ return true;
+ }
+ }
+
+ return false;
+ }
+
+ int get_used_size() const { return sizeof(*this) + sizeof(data_type) * entries(); }
+ int get_total_size() const { return sizeof(*this) + sizeof(data_type) * kSize; }
+
+ int entries() const { return _entries; }
+ bool empty() const { return _entries == 0; }
+
+ void reset()
+ {
+ for (uint8_t i = 0; i < kSize; i++) {
+ _buffer[i].time_us = 0;
+ }
+
+ _head = 0;
+ _entries = 0;
+ }
+
+private:
+ data_type _buffer[SIZE] {};
+
+ uint8_t _head{0};
+ uint8_t _entries{0};
+};
+
+/**
+ * Dynamic-size ring buffer specialization (SIZE == 0).
+ */
+template
+class TimestampedRingBuffer
+{
public:
explicit TimestampedRingBuffer(size_t size) { allocate(size); }
TimestampedRingBuffer() = delete;
@@ -58,14 +171,14 @@ class TimestampedRingBuffer
TimestampedRingBuffer(TimestampedRingBuffer &&) = delete;
TimestampedRingBuffer &operator=(TimestampedRingBuffer &&) = delete;
- bool allocate(uint8_t size)
+ bool allocate(size_t size)
{
if (valid() && (size == _size)) {
// no change
return true;
}
- if (size == 0) {
+ if (size == 0 || size > UINT8_MAX) {
return false;
}
@@ -77,7 +190,7 @@ class TimestampedRingBuffer
return false;
}
- _size = size;
+ _size = static_cast(size);
reset();
@@ -91,7 +204,7 @@ class TimestampedRingBuffer
uint8_t head_new = _head;
if (!_first_write) {
- head_new = (_head + 1) % _size;
+ head_new = next(_head);
}
_buffer[head_new] = sample;
@@ -99,7 +212,7 @@ class TimestampedRingBuffer
// move tail if we overwrite it
if (_head == _tail && !_first_write) {
- _tail = (_tail + 1) % _size;
+ _tail = next(_tail);
} else {
_first_write = false;
@@ -113,8 +226,12 @@ class TimestampedRingBuffer
const data_type &get_newest() const { return _buffer[_head]; }
const data_type &get_oldest() const { return _buffer[_tail]; }
+ uint8_t get_newest_index() const { return _head; }
uint8_t get_oldest_index() const { return _tail; }
+ uint8_t next(uint8_t i) const { return (_size > 0) ? ((i + 1 >= _size) ? 0 : i + 1) : 0; }
+ uint8_t prev(uint8_t i) const { return (_size > 0) ? ((i == 0) ? (_size - 1) : (i - 1)) : 0; }
+
bool pop_first_older_than(const uint64_t ×tamp, data_type *sample)
{
// start looking from newest observation data
@@ -133,7 +250,7 @@ class TimestampedRingBuffer
_first_write = true;
} else {
- _tail = (index + 1) % _size;
+ _tail = next(static_cast(index));
}
_buffer[index].time_us = 0;
@@ -167,11 +284,13 @@ class TimestampedRingBuffer
return count;
}
+ bool empty() const { return entries() == 0; }
+
void reset()
{
if (_buffer) {
for (uint8_t i = 0; i < _size; i++) {
- _buffer[i] = {};
+ _buffer[i].time_us = 0;
}
_head = 0;
diff --git a/src/lib/ringbuffer/TimestampedRingBufferTest.cpp b/src/lib/ringbuffer/TimestampedRingBufferDynamicTest.cpp
similarity index 78%
rename from src/lib/ringbuffer/TimestampedRingBufferTest.cpp
rename to src/lib/ringbuffer/TimestampedRingBufferDynamicTest.cpp
index 83f5fa7940e6..15a64a5be9ee 100644
--- a/src/lib/ringbuffer/TimestampedRingBufferTest.cpp
+++ b/src/lib/ringbuffer/TimestampedRingBufferDynamicTest.cpp
@@ -41,7 +41,7 @@ struct sample {
};
-class TimestampedRingBufferTest : public ::testing::Test
+class TimestampedRingBufferDynamicTest : public ::testing::Test
{
public:
@@ -67,15 +67,24 @@ class TimestampedRingBufferTest : public ::testing::Test
}
};
-TEST_F(TimestampedRingBufferTest, goodInitialisation)
+TEST_F(TimestampedRingBufferDynamicTest, goodInitialisation)
{
// WHEN: buffer was allocated
// THEN: allocation should have succeed
- ASSERT_EQ(true, _buffer->allocate(3));
+ EXPECT_TRUE(_buffer->allocate(3));
+ EXPECT_TRUE(_buffer->valid());
+ EXPECT_EQ(_buffer->get_length(), 3);
+}
+TEST_F(TimestampedRingBufferDynamicTest, RejectsInvalidSize)
+{
+ EXPECT_FALSE(_buffer->allocate(0));
+ EXPECT_FALSE(_buffer->allocate(UINT8_MAX + 1u));
+ EXPECT_TRUE(_buffer->valid());
+ EXPECT_EQ(_buffer->get_length(), 3);
}
-TEST_F(TimestampedRingBufferTest, badInitialisation)
+TEST_F(TimestampedRingBufferDynamicTest, badInitialisation)
{
// WHEN: buffer allocation input is bad
// THEN: allocation should fail
@@ -85,7 +94,7 @@ TEST_F(TimestampedRingBufferTest, badInitialisation)
// ASSERT_EQ(false, _buffer->allocate(0));
}
-TEST_F(TimestampedRingBufferTest, orderOfSamples)
+TEST_F(TimestampedRingBufferDynamicTest, orderOfSamples)
{
ASSERT_EQ(true, _buffer->allocate(3));
// GIVEN: allocated buffer
@@ -103,7 +112,7 @@ TEST_F(TimestampedRingBufferTest, orderOfSamples)
EXPECT_EQ(_y.time_us, _buffer->get_newest().time_us);
}
-TEST_F(TimestampedRingBufferTest, popSample)
+TEST_F(TimestampedRingBufferDynamicTest, popSample)
{
ASSERT_EQ(true, _buffer->allocate(3));
_buffer->push(_x);
@@ -115,20 +124,20 @@ TEST_F(TimestampedRingBufferTest, popSample)
sample pop = {};
// WHEN: we want to retrieve a sample that is older than any in the buffer
// THEN: we should not get any sample
- EXPECT_EQ(false, _buffer->pop_first_older_than(0, &pop));
+ EXPECT_FALSE(_buffer->pop_first_older_than(0, &pop));
// WHEN: when calling "pop_first_older_than"
// THEN: we should get the first sample from the head that is older
- EXPECT_EQ(true, _buffer->pop_first_older_than(_x.time_us + 1, &pop));
+ EXPECT_TRUE(_buffer->pop_first_older_than(_x.time_us + 1, &pop));
EXPECT_EQ(_x.time_us, pop.time_us);
- EXPECT_EQ(true, _buffer->pop_first_older_than(_y.time_us + 10, &pop));
+ EXPECT_TRUE(_buffer->pop_first_older_than(_y.time_us + 10, &pop));
EXPECT_EQ(_y.time_us, pop.time_us);
- EXPECT_EQ(true, _buffer->pop_first_older_than(_z.time_us + 100, &pop));
+ EXPECT_TRUE(_buffer->pop_first_older_than(_z.time_us + 100, &pop));
EXPECT_EQ(_z.time_us, pop.time_us);
// TODO: When changing the order of popping sample it does not behave as expected, fix this
}
-TEST_F(TimestampedRingBufferTest, askingForTooNewSample)
+TEST_F(TimestampedRingBufferDynamicTest, askingForTooNewSample)
{
ASSERT_EQ(true, _buffer->allocate(3));
_buffer->push(_x);
@@ -138,11 +147,11 @@ TEST_F(TimestampedRingBufferTest, askingForTooNewSample)
sample pop = {};
// WHEN: all buffered samples are older by 0.1s than your query timestamp
// THEN: should get no sample returned
- EXPECT_EQ(true, _buffer->pop_first_older_than(_z.time_us + 99000, &pop));
- EXPECT_EQ(false, _buffer->pop_first_older_than(_y.time_us + 100000, &pop));
+ EXPECT_TRUE(_buffer->pop_first_older_than(_z.time_us + 99000, &pop));
+ EXPECT_FALSE(_buffer->pop_first_older_than(_y.time_us + 100000, &pop));
}
-TEST_F(TimestampedRingBufferTest, reallocateBuffer)
+TEST_F(TimestampedRingBufferDynamicTest, reallocateBuffer)
{
ASSERT_EQ(true, _buffer->allocate(5));
_buffer->push(_x);
@@ -153,8 +162,7 @@ TEST_F(TimestampedRingBufferTest, reallocateBuffer)
// GIVEN: allocated and filled buffer
// WHEN: do another allocate call
- _buffer->allocate(3);
+ EXPECT_TRUE(_buffer->allocate(3));
// THEN: its length should update
EXPECT_EQ(3, _buffer->get_length());
-
}
diff --git a/src/lib/ringbuffer/TimestampedRingBufferStaticTest.cpp b/src/lib/ringbuffer/TimestampedRingBufferStaticTest.cpp
new file mode 100644
index 000000000000..1fa223eab1a5
--- /dev/null
+++ b/src/lib/ringbuffer/TimestampedRingBufferStaticTest.cpp
@@ -0,0 +1,152 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2019-2023 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include
+#include
+#include "TimestampedRingBuffer.hpp"
+
+struct Sample {
+ uint64_t time_us;
+ float data[3];
+};
+
+
+class TimestampedRingBufferStaticTest : public ::testing::Test
+{
+public:
+ Sample _x, _y, _z, _w;
+ TimestampedRingBuffer *_buffer{nullptr};
+
+ void SetUp() override
+ {
+ _buffer = new TimestampedRingBuffer();
+ _x.time_us = 1000000;
+ _x.data[0] = _x.data[1] = _x.data[2] = 1.5f;
+
+ _y.time_us = 2000000;
+ _y.data[0] = _y.data[1] = _y.data[2] = 3.0f;
+
+ _z.time_us = 3000000;
+ _z.data[0] = _z.data[1] = _z.data[2] = 4.0f;
+
+ _w.time_us = 4000000;
+ _w.data[0] = _w.data[1] = _w.data[2] = 5.0f;
+ }
+
+ void TearDown() override
+ {
+ delete _buffer;
+ }
+};
+
+TEST_F(TimestampedRingBufferStaticTest, goodInitialisation)
+{
+ // WHEN: buffer was allocated
+ // THEN: allocation should have succeed
+ EXPECT_TRUE(_buffer->valid());
+ EXPECT_FALSE(_buffer->allocate(2));
+ EXPECT_EQ(_buffer->get_length(), 3);
+}
+
+TEST_F(TimestampedRingBufferStaticTest, NextPrevHelpers)
+{
+ EXPECT_EQ(_buffer->next(0), 1);
+ EXPECT_EQ(_buffer->next(1), 2);
+ EXPECT_EQ(_buffer->next(2), 0);
+
+ EXPECT_EQ(_buffer->prev(0), 2);
+ EXPECT_EQ(_buffer->prev(1), 0);
+ EXPECT_EQ(_buffer->prev(2), 1);
+}
+
+TEST_F(TimestampedRingBufferStaticTest, PushWrapAndIndices)
+{
+ _buffer->reset();
+ ASSERT_TRUE(_buffer->empty());
+
+ _buffer->push(_x);
+ _buffer->push(_y);
+ EXPECT_EQ(_buffer->entries(), 2);
+ _buffer->push(_z);
+
+ EXPECT_EQ(_buffer->entries(), 3);
+ EXPECT_EQ(_buffer->get_oldest().time_us, _x.time_us);
+ EXPECT_EQ(_buffer->get_newest().time_us, _z.time_us);
+
+ // Overwrite once (ring wrap-around).
+ _buffer->push(_w);
+
+ EXPECT_EQ(_buffer->entries(), 3);
+ EXPECT_EQ(_buffer->get_oldest().time_us, _y.time_us);
+ EXPECT_EQ(_buffer->get_newest().time_us, _w.time_us);
+}
+
+TEST_F(TimestampedRingBufferStaticTest, PopFirstOlderThanDiscardsOlderHistory)
+{
+ _buffer->reset();
+ _buffer->push(_x);
+ _buffer->push(_y);
+ _buffer->push(_z);
+
+ Sample pop = {};
+
+ // Query timestamp older than any in the buffer: no match.
+ EXPECT_FALSE(_buffer->pop_first_older_than(0, &pop));
+
+ // Pop y (also discards x), leaving z as the only remaining Sample.
+ EXPECT_TRUE(_buffer->pop_first_older_than(_y.time_us + 10, &pop));
+ EXPECT_EQ(pop.time_us, _y.time_us);
+
+ EXPECT_EQ(_buffer->entries(), 1);
+ EXPECT_EQ(_buffer->get_oldest().time_us, _z.time_us);
+ EXPECT_EQ(_buffer->get_newest().time_us, _z.time_us);
+
+ // Pop z (buffer becomes empty).
+ EXPECT_TRUE(_buffer->pop_first_older_than(_z.time_us + 1, &pop));
+ EXPECT_EQ(pop.time_us, _z.time_us);
+ EXPECT_TRUE(_buffer->empty());
+}
+
+TEST_F(TimestampedRingBufferStaticTest, ResetClearsTimestamps)
+{
+ _buffer->push(_x);
+ _buffer->push(_y);
+ EXPECT_FALSE(_buffer->empty());
+
+ _buffer->reset();
+ EXPECT_TRUE(_buffer->empty());
+
+ for (uint8_t i = 0; i < _buffer->get_length(); i++) {
+ EXPECT_EQ((*_buffer)[i].time_us, 0u);
+ }
+}
diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
index 7fdf29ec0469..61efb14fec56 100644
--- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
+++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
@@ -93,6 +93,9 @@ bool FlightTaskAuto::updateInitialize()
_sub_home_position.update();
_sub_vehicle_status.update();
_position_setpoint_triplet_sub.update();
+#if defined(CONFIG_MODULES_VISION_TARGET_ESTIMATOR) && CONFIG_MODULES_VISION_TARGET_ESTIMATOR
+ _prec_land_status_sub.update();
+#endif // CONFIG_MODULES_VISION_TARGET_ESTIMATOR
// require valid reference and valid target
ret = ret && _evaluateGlobalReference() && _evaluatePositionSetpointTriplet();
@@ -238,7 +241,12 @@ void FlightTaskAuto::_prepareLandSetpoints()
if (_type_previous != WaypointType::land) {
// initialize yaw and xy-position
- _land_heading = _yaw_setpoint;
+ _land_heading = PX4_ISFINITE(_yaw_setpoint) ? _yaw_setpoint : _yaw_setpoint_previous;
+
+ if (!PX4_ISFINITE(_land_heading)) {
+ _land_heading = _yaw;
+ }
+
_stick_acceleration_xy.resetPosition(Vector2f(_triplet_current));
_initial_land_position = Vector3f(_triplet_current(0), _triplet_current(1), NAN);
}
@@ -246,6 +254,18 @@ void FlightTaskAuto::_prepareLandSetpoints()
// Update xy-position in case of landing position changes (etc. precision landing)
_land_position = Vector3f(_triplet_current(0), _triplet_current(1), NAN);
+#if defined(CONFIG_MODULES_VISION_TARGET_ESTIMATOR) && CONFIG_MODULES_VISION_TARGET_ESTIMATOR
+
+ // Use the raw navigator/precland yaw command here. _yaw_setpoint can stay finite from
+ // generic land yaw logic even after precland clears current.yaw.
+ if (_param_pld_yaw_en.get()
+ && _prec_land_status_sub.get().state == prec_land_status_s::PREC_LAND_STATE_ONGOING) {
+ const float prec_land_yaw = _position_setpoint_triplet_sub.get().current.yaw;
+ _land_heading = PX4_ISFINITE(prec_land_yaw) ? prec_land_yaw : _yaw;
+ }
+
+#endif // CONFIG_MODULES_VISION_TARGET_ESTIMATOR
+
// User input assisted landing
if (_param_mpc_land_rc_help.get() && _sticks.checkAndUpdateStickInputs()) {
// Stick full up -1 -> stop, stick full down 1 -> double the speed
diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp
index 6541c862d0ec..eb21ca3a051c 100644
--- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp
+++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp
@@ -44,6 +44,9 @@
#include
#include
#include
+#if defined(CONFIG_MODULES_VISION_TARGET_ESTIMATOR) && CONFIG_MODULES_VISION_TARGET_ESTIMATOR
+#include
+#endif // CONFIG_MODULES_VISION_TARGET_ESTIMATOR
#include
#include
#include
@@ -120,7 +123,10 @@ class FlightTaskAuto : public FlightTask
WaypointType _type{WaypointType::idle}; /**< Type of current target triplet. */
uORB::SubscriptionData _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)};
- uORB::SubscriptionData _sub_home_position{ORB_ID(home_position)};
+#if defined(CONFIG_MODULES_VISION_TARGET_ESTIMATOR) && CONFIG_MODULES_VISION_TARGET_ESTIMATOR
+ uORB::SubscriptionData _prec_land_status_sub {ORB_ID(prec_land_status)};
+#endif // CONFIG_MODULES_VISION_TARGET_ESTIMATOR
+ uORB::SubscriptionData _sub_home_position {ORB_ID(home_position)};
uORB::SubscriptionData _sub_vehicle_status{ORB_ID(vehicle_status)};
float _target_acceptance_radius{0.0f}; /**< Acceptances radius of the target */
@@ -141,6 +147,9 @@ class FlightTaskAuto : public FlightTask
bool _want_takeoff{false};
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
+#if defined(CONFIG_MODULES_VISION_TARGET_ESTIMATOR) && CONFIG_MODULES_VISION_TARGET_ESTIMATOR
+ (ParamInt) _param_pld_yaw_en,
+#endif // CONFIG_MODULES_VISION_TARGET_ESTIMATOR
(ParamFloat) _param_mpc_xy_cruise,
(ParamFloat)
_param_nav_mc_alt_rad, //vertical acceptance radius at which waypoints are updated
diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
index 8baf8b292c98..e6c59c2ae5ef 100644
--- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
+++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
@@ -300,12 +300,6 @@ class BlockLocalPositionEstimator : public ModuleBase, public ModuleParams,
MapProjection _global_local_proj_ref{};
float _global_local_alt0{NAN};
- // target mode paramters from landing_target_estimator module
- enum TargetMode {
- Target_Moving = 0,
- Target_Stationary = 1
- };
-
// flow gyro filter
BlockHighPass _flow_gyro_x_high_pass;
BlockHighPass _flow_gyro_y_high_pass;
@@ -458,7 +452,6 @@ class BlockLocalPositionEstimator : public ModuleBase, public ModuleParams,
(ParamFloat) _param_lpe_t_max_grade,
(ParamFloat) _param_lpe_lt_cov,
- (ParamInt) _param_ltest_mode,
// init origin
(ParamInt) _param_lpe_fake_origin,
diff --git a/src/modules/local_position_estimator/sensors/landing_target.cpp b/src/modules/local_position_estimator/sensors/landing_target.cpp
index 6472fc198fab..315286288935 100644
--- a/src/modules/local_position_estimator/sensors/landing_target.cpp
+++ b/src/modules/local_position_estimator/sensors/landing_target.cpp
@@ -8,7 +8,7 @@ static const uint64_t TARGET_TIMEOUT = 2000000; // [us]
void BlockLocalPositionEstimator::landingTargetInit()
{
- if (_param_ltest_mode.get() == Target_Moving) {
+ if (!_sub_landing_target_pose.get().is_static) {
// target is in moving mode, do not initialize
return;
}
@@ -24,7 +24,7 @@ void BlockLocalPositionEstimator::landingTargetInit()
int BlockLocalPositionEstimator::landingTargetMeasure(Vector &y)
{
- if (_param_ltest_mode.get() == Target_Stationary) {
+ if (_sub_landing_target_pose.get().is_static) {
if (_sub_landing_target_pose.get().rel_vel_valid) {
y(0) = _sub_landing_target_pose.get().vx_rel;
y(1) = _sub_landing_target_pose.get().vy_rel;
@@ -43,7 +43,7 @@ int BlockLocalPositionEstimator::landingTargetMeasure(Vector
void BlockLocalPositionEstimator::landingTargetCorrect()
{
- if (_param_ltest_mode.get() == Target_Moving) {
+ if (!_sub_landing_target_pose.get().is_static) {
// nothing to do in this mode
return;
}
diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp
index e3044c1ee71b..68739a1856d1 100644
--- a/src/modules/logger/logged_topics.cpp
+++ b/src/modules/logger/logged_topics.cpp
@@ -195,6 +195,23 @@ void LoggedTopics::add_default_topics()
add_optional_topic_multi("estimator_status_flags", 10);
add_optional_topic_multi("yaw_estimator_status", 1000);
+ // Vision target estimator topics
+#if defined(CONFIG_MODULES_VISION_TARGET_ESTIMATOR) && CONFIG_MODULES_VISION_TARGET_ESTIMATOR
+ add_topic("vision_target_est_input", 50);
+ add_topic("vision_target_est_position", 100);
+ add_topic("vision_target_est_orientation", 100);
+ add_optional_topic("vte_bias_init_status", 10); // High rate because rarely published and only for a short period of time
+ add_optional_topic("vte_aid_gps_pos_target", 100);
+ add_optional_topic("vte_aid_gps_pos_mission", 100);
+ add_optional_topic("vte_aid_gps_vel_uav", 100);
+ add_optional_topic("vte_aid_gps_vel_target", 100);
+ add_optional_topic("vte_aid_fiducial_marker", 100);
+ add_optional_topic("vte_aid_ev_yaw", 100);
+ add_optional_topic("fiducial_marker_pos_report", 100);
+ add_optional_topic("fiducial_marker_yaw_report", 100);
+ add_optional_topic("target_gnss", 100);
+#endif // CONFIG_MODULES_VISION_TARGET_ESTIMATOR
+
// log all raw sensors at minimal rate (at least 1 Hz)
add_topic_multi("battery_status", 200, 3);
add_topic_multi("differential_pressure", 1000, 2);
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index ce59b940ad45..4ec4df9dd32c 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -116,6 +116,9 @@ MavlinkReceiver::MavlinkReceiver(Mavlink &parent) :
_parameters_manager(parent),
_mavlink_timesync(parent)
{
+#if defined(CONFIG_MODULES_VISION_TARGET_ESTIMATOR) && CONFIG_MODULES_VISION_TARGET_ESTIMATOR
+ _param_vte_en = param_find("VTE_EN");
+#endif // CONFIG_MODULES_VISION_TARGET_ESTIMATOR
}
void
@@ -315,6 +318,23 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
break;
#endif // !CONSTRAINED_FLASH
+#if defined(CONFIG_MODULES_VISION_TARGET_ESTIMATOR) && CONFIG_MODULES_VISION_TARGET_ESTIMATOR
+#if defined(MAVLINK_MSG_ID_TARGET_RELATIVE)
+
+ case MAVLINK_MSG_ID_TARGET_RELATIVE:
+ handle_message_target_relative(msg);
+ break;
+#endif // MAVLINK_MSG_ID_TARGET_RELATIVE
+
+#if defined(MAVLINK_MSG_ID_TARGET_ABSOLUTE)
+
+ case MAVLINK_MSG_ID_TARGET_ABSOLUTE:
+ handle_message_target_absolute(msg);
+ break;
+#endif // MAVLINK_MSG_ID_TARGET_ABSOLUTE
+
+#endif // CONFIG_MODULES_VISION_TARGET_ESTIMATOR
+
case MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE:
handle_message_gimbal_manager_set_attitude(msg);
break;
@@ -2963,6 +2983,241 @@ MavlinkReceiver::handle_message_debug_float_array(mavlink_message_t *msg)
}
#endif // !CONSTRAINED_FLASH
+#if defined(CONFIG_MODULES_VISION_TARGET_ESTIMATOR) && CONFIG_MODULES_VISION_TARGET_ESTIMATOR
+
+#if defined(MAVLINK_MSG_ID_TARGET_ABSOLUTE)
+void
+MavlinkReceiver::handle_message_target_absolute(mavlink_message_t *msg)
+{
+ mavlink_target_absolute_t target_absolute;
+ mavlink_msg_target_absolute_decode(msg, &target_absolute);
+
+ target_gnss_s target_GNSS_report{};
+
+ target_GNSS_report.timestamp = hrt_absolute_time();
+ target_GNSS_report.timestamp_sample = _mavlink_timesync.sync_stamp(target_absolute.timestamp);
+
+ bool updated = false;
+
+ // Position: bit 0
+ if (!(target_absolute.sensor_capabilities & static_cast(TargetAbsoluteSensorCapability::kPosition))) {
+ target_GNSS_report.abs_pos_updated = false;
+
+ } else {
+ target_GNSS_report.abs_pos_updated = true;
+ target_GNSS_report.latitude_deg = target_absolute.lat * 1e-7;
+ target_GNSS_report.longitude_deg = target_absolute.lon * 1e-7;
+ target_GNSS_report.altitude_msl_m = target_absolute.alt;
+ target_GNSS_report.eph = target_absolute.position_std[0];
+ target_GNSS_report.epv = target_absolute.position_std[1];
+ updated = true;
+ }
+
+ // Velocity: bit 1
+ if (!(target_absolute.sensor_capabilities & static_cast(TargetAbsoluteSensorCapability::kVelocity))) {
+ target_GNSS_report.vel_ned_updated = false;
+
+ } else {
+ const matrix::Quatf q_target(target_absolute.q_target);
+ const bool target_attitude_available = (fabsf(q_target(0)) + fabsf(q_target(1)) + fabsf(q_target(2)) +
+ fabsf(q_target(3))) > FLT_EPSILON;
+
+ if (target_attitude_available) {
+ const matrix::Vector3f vel_target_body(target_absolute.vel[0], target_absolute.vel[1], target_absolute.vel[2]);
+ const matrix::Vector3f vel_target_ned = q_target.rotateVector(vel_target_body);
+
+ target_GNSS_report.vel_ned_updated = true;
+ target_GNSS_report.vel_n_m_s = vel_target_ned(0);
+ target_GNSS_report.vel_e_m_s = vel_target_ned(1);
+ target_GNSS_report.vel_d_m_s = vel_target_ned(2);
+ target_GNSS_report.s_acc_m_s = fmaxf(fabsf(target_absolute.vel_std[0]),
+ fmaxf(fabsf(target_absolute.vel_std[1]),
+ fabsf(target_absolute.vel_std[2])));
+ updated = true;
+
+ } else {
+ target_GNSS_report.vel_ned_updated = false;
+ }
+ }
+
+ if (updated) { _target_gnss_pub.publish(target_GNSS_report); }
+}
+#endif // MAVLINK_MSG_ID_TARGET_ABSOLUTE
+
+#if defined(MAVLINK_MSG_ID_TARGET_RELATIVE)
+void
+MavlinkReceiver::handle_message_target_relative(mavlink_message_t *msg)
+{
+ mavlink_target_relative_t target_relative;
+ mavlink_msg_target_relative_decode(msg, &target_relative);
+
+ vehicle_attitude_s vehicle_attitude;
+ vehicle_local_position_s vehicle_local_position;
+
+ // Unsupported sensor frame
+ if (target_relative.frame != TARGET_OBS_FRAME_LOCAL_OFFSET_NED && target_relative.frame != TARGET_OBS_FRAME_LOCAL_NED &&
+ target_relative.frame != TARGET_OBS_FRAME_BODY_FRD && target_relative.frame != TARGET_OBS_FRAME_OTHER) {
+ mavlink_log_critical(&_mavlink_log_pub, "target relative: coordinate frame %" PRIu8 " unsupported.\t",
+ target_relative.frame);
+ events::send(events::ID("mavlink_rcv_target_rel_unsup_coord_frame"), events::Log::Error,
+ "Target relative: unsupported coordinate frame {1}", target_relative.frame);
+
+ return;
+ }
+
+ // Unsupported measurement type
+ if (target_relative.type != LANDING_TARGET_TYPE_VISION_FIDUCIAL) {
+ mavlink_log_critical(&_mavlink_log_pub, "target relative: type %" PRIu8 " unsupported.\t",
+ target_relative.type);
+ events::send(events::ID("mavlink_rcv_target_rel_unsup_type"), events::Log::Error,
+ "Target relative: unsupported type {1}", target_relative.type);
+ return;
+ }
+
+ matrix::Quatf q_sensor(target_relative.q_sensor);
+ bool attitude_available = (fabsf(q_sensor(0)) + fabsf(q_sensor(1)) + fabsf(q_sensor(2)) +
+ fabsf(q_sensor(3))) > FLT_EPSILON;
+
+ // Check if attitude can be infered based on the frame
+ if (!attitude_available) {
+ if (target_relative.frame == TARGET_OBS_FRAME_LOCAL_OFFSET_NED || target_relative.frame == TARGET_OBS_FRAME_LOCAL_NED) {
+ // Observation already in NED, set quaternion to identity (1,0,0,0)
+ q_sensor.setIdentity();
+ attitude_available = true;
+
+ } else if (target_relative.frame == TARGET_OBS_FRAME_BODY_FRD && _vehicle_attitude_sub.copy(&vehicle_attitude)) {
+ // Set the quaternion to the current attitude
+ const matrix::Quaternionf quat_att(&vehicle_attitude.q[0]);
+ q_sensor = quat_att;
+ attitude_available = true;
+
+ } else if (target_relative.frame == TARGET_OBS_FRAME_OTHER) {
+ // Target sensor frame: OTHER and no quaternion is provided
+ mavlink_log_critical(&_mavlink_log_pub,
+ "target relative: coordinate frame %" PRIu8 " unsupported when the q_sensor is not filled.\t",
+ target_relative.frame);
+ events::send(events::ID("mavlink_rcv_target_rel_q_sensor_not_filled"), events::Log::Error,
+ "Target relative: unsupported coordinate frame {1} when the q_sensor is not filled", target_relative.frame);
+ }
+ }
+
+ // Target cannot be sent withtout valid attitude
+ if (!attitude_available) {
+ return;
+ }
+
+ // Forward target to the vision target estimator (VTE) or precland based VTE_EN
+ int32_t vte_enabled = 0;
+
+ if (_param_vte_en == PARAM_INVALID) {
+ if (hrt_elapsed_time(&_vte_en_invalid_warn_last) > 20_s) {
+ PX4_WARN(" Could not find VTE_EN, target_relative msg ignored.");
+ _vte_en_invalid_warn_last = hrt_absolute_time();
+ }
+
+ return;
+
+ } else {
+ param_get(_param_vte_en, &vte_enabled);
+ }
+
+ if (!vte_enabled) {
+
+ // Send the target directly to the precision landing algorithm in local NED frame
+ landing_target_pose_s landing_target_pose{};
+ landing_target_pose.timestamp = _mavlink_timesync.sync_stamp(target_relative.timestamp);
+
+ // Measurement already in local NED
+ if (target_relative.frame == TARGET_OBS_FRAME_LOCAL_NED) {
+
+ landing_target_pose.x_abs = target_relative.x;
+ landing_target_pose.y_abs = target_relative.y;
+ landing_target_pose.z_abs = target_relative.z;
+ landing_target_pose.abs_pos_valid = true;
+
+ _landing_target_pose_pub.publish(landing_target_pose);
+
+ } else if (_vehicle_local_position_sub.copy(&vehicle_local_position)
+ && vehicle_local_position.xy_valid && vehicle_local_position.z_valid) {
+
+ // If the local position is available, convert from sensor frame to local NED
+ matrix::Vector3f target_relative_frame(target_relative.x, target_relative.y, target_relative.z);
+
+ if (target_relative.frame == TARGET_OBS_FRAME_BODY_FRD || target_relative.frame == TARGET_OBS_FRAME_OTHER) {
+ // Rotate measurement into vehicle-carried NED
+ target_relative_frame = q_sensor.rotateVector(target_relative_frame);
+ }
+
+ // Convert from vehicle-carried NED to local NED
+ landing_target_pose.x_abs = target_relative_frame(0) + vehicle_local_position.x;
+ landing_target_pose.y_abs = target_relative_frame(1) + vehicle_local_position.y;
+ landing_target_pose.z_abs = target_relative_frame(2) + vehicle_local_position.z;
+ landing_target_pose.abs_pos_valid = true;
+
+ _landing_target_pose_pub.publish(landing_target_pose);
+ }
+
+ } else {
+
+ // Only supported type: LANDING_TARGET_TYPE_VISION_FIDUCIAL
+
+ // Position report
+ fiducial_marker_pos_report_s fiducial_marker_pos_report{};
+
+ const hrt_abstime now = hrt_absolute_time();
+ const hrt_abstime timestamp_sample = _mavlink_timesync.sync_stamp(target_relative.timestamp);
+
+ fiducial_marker_pos_report.timestamp = now;
+ fiducial_marker_pos_report.timestamp_sample = timestamp_sample;
+ matrix::Vector3f target_relative_frame(target_relative.x, target_relative.y, target_relative.z);
+
+ // TARGET_OBS_FRAME_LOCAL_NED is absolute in local frame: convert to vehicle-relative.
+ if (target_relative.frame == TARGET_OBS_FRAME_LOCAL_NED) {
+ if (!_vehicle_local_position_sub.copy(&vehicle_local_position)
+ || !vehicle_local_position.xy_valid || !vehicle_local_position.z_valid) {
+ return;
+ }
+
+ target_relative_frame(0) -= vehicle_local_position.x;
+ target_relative_frame(1) -= vehicle_local_position.y;
+ target_relative_frame(2) -= vehicle_local_position.z;
+ }
+
+ fiducial_marker_pos_report.rel_pos[0] = target_relative_frame(0);
+ fiducial_marker_pos_report.rel_pos[1] = target_relative_frame(1);
+ fiducial_marker_pos_report.rel_pos[2] = target_relative_frame(2);
+
+ fiducial_marker_pos_report.cov_rel_pos[0] = target_relative.pos_std[0] * target_relative.pos_std[0];
+ fiducial_marker_pos_report.cov_rel_pos[1] = target_relative.pos_std[1] * target_relative.pos_std[1];
+ fiducial_marker_pos_report.cov_rel_pos[2] = target_relative.pos_std[2] * target_relative.pos_std[2];
+
+ q_sensor.copyTo(fiducial_marker_pos_report.q);
+ _fiducial_marker_pos_report_pub.publish(fiducial_marker_pos_report);
+
+ const matrix::Quatf q_target(target_relative.q_target);
+ const bool target_attitude_available = (fabsf(q_target(0)) + fabsf(q_target(1)) + fabsf(q_target(2)) +
+ fabsf(q_target(3))) > FLT_EPSILON;
+
+ if (target_attitude_available) {
+ // Yaw report
+ fiducial_marker_yaw_report_s fiducial_marker_yaw_report{};
+ fiducial_marker_yaw_report.timestamp = now;
+ fiducial_marker_yaw_report.timestamp_sample = timestamp_sample;
+
+ // Transform quaternion from the target's frame to the TARGET_OBS_FRAME to the yaw relative to NED
+ const matrix::Quatf q_in_ned = q_sensor * q_target;
+ const float target_yaw_ned = matrix::Eulerf(q_in_ned).psi();
+
+ fiducial_marker_yaw_report.yaw_ned = target_yaw_ned;
+ fiducial_marker_yaw_report.yaw_var_ned = target_relative.yaw_std * target_relative.yaw_std;
+ _fiducial_marker_yaw_report_pub.publish(fiducial_marker_yaw_report);
+ }
+ }
+
+}
+#endif // MAVLINK_MSG_ID_TARGET_RELATIVE
+#endif // CONFIG_MODULES_VISION_TARGET_ESTIMATOR
+
void
MavlinkReceiver::handle_message_onboard_computer_status(mavlink_message_t *msg)
{
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index e2adf224dbc1..2362d8369498 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -125,8 +125,24 @@
# include
#endif // !CONSTRAINED_FLASH
+# if defined(CONFIG_MODULES_VISION_TARGET_ESTIMATOR) && CONFIG_MODULES_VISION_TARGET_ESTIMATOR
+# include
+# include
+# include
+# endif // CONFIG_MODULES_VISION_TARGET_ESTIMATOR
+
using namespace time_literals;
+#if defined(CONFIG_MODULES_VISION_TARGET_ESTIMATOR) && CONFIG_MODULES_VISION_TARGET_ESTIMATOR
+enum class TargetAbsoluteSensorCapability : uint8_t {
+ kPosition = (1 << 0),
+ kVelocity = (1 << 1),
+ kAcceleration = (1 << 2),
+ kAttitude = (1 << 3),
+ kRates = (1 << 4),
+};
+#endif // CONFIG_MODULES_VISION_TARGET_ESTIMATOR
+
class Mavlink;
class MavlinkReceiver : public ModuleParams
@@ -228,6 +244,12 @@ class MavlinkReceiver : public ModuleParams
void handle_message_debug_vect(mavlink_message_t *msg);
void handle_message_named_value_float(mavlink_message_t *msg);
#endif // !CONSTRAINED_FLASH
+
+# if defined(CONFIG_MODULES_VISION_TARGET_ESTIMATOR) && CONFIG_MODULES_VISION_TARGET_ESTIMATOR
+ void handle_message_target_relative(mavlink_message_t *msg);
+ void handle_message_target_absolute(mavlink_message_t *msg);
+# endif // CONFIG_MODULES_VISION_TARGET_ESTIMATOR
+
void handle_message_request_event(mavlink_message_t *msg);
void CheckHeartbeats(const hrt_abstime &t, bool force = false);
@@ -357,6 +379,14 @@ class MavlinkReceiver : public ModuleParams
uORB::Publication _debug_vect_pub{ORB_ID(debug_vect)};
#endif // !CONSTRAINED_FLASH
+# if defined(CONFIG_MODULES_VISION_TARGET_ESTIMATOR) && CONFIG_MODULES_VISION_TARGET_ESTIMATOR
+ uORB::Publication _fiducial_marker_pos_report_pub {ORB_ID(fiducial_marker_pos_report)};
+ uORB::Publication _fiducial_marker_yaw_report_pub{ORB_ID(fiducial_marker_yaw_report)};
+ uORB::Publication _target_gnss_pub{ORB_ID(target_gnss)};
+ param_t _param_vte_en{PARAM_INVALID};
+ hrt_abstime _vte_en_invalid_warn_last{0};
+# endif // CONFIG_MODULES_VISION_TARGET_ESTIMATOR
+
// ORB publications (multi)
uORB::PublicationMulti _distance_sensor_pub{ORB_ID(distance_sensor)};
uORB::PublicationMulti _aux_global_position_pub{ORB_ID(aux_global_position)};
diff --git a/src/modules/navigator/precland.cpp b/src/modules/navigator/precland.cpp
index a111bf75c932..45133dc21b6b 100644
--- a/src/modules/navigator/precland.cpp
+++ b/src/modules/navigator/precland.cpp
@@ -78,6 +78,10 @@ PrecLand::on_activation()
_search_cnt = 0;
_last_slewrate_time = 0;
+#if defined(CONFIG_MODULES_VISION_TARGET_ESTIMATOR) && CONFIG_MODULES_VISION_TARGET_ESTIMATOR
+ reset_target_yaw_state();
+#endif // CONFIG_MODULES_VISION_TARGET_ESTIMATOR
+
vehicle_local_position_s *vehicle_local_position = _navigator->get_local_position();
if (!_map_ref.isInitialized()) {
@@ -122,8 +126,35 @@ PrecLand::on_active()
_target_pose_valid = false;
}
+#if defined(CONFIG_MODULES_VISION_TARGET_ESTIMATOR) && CONFIG_MODULES_VISION_TARGET_ESTIMATOR
+
+ // get orientation measurement
+ if (_param_pld_yaw_en.get()) {
+ vision_target_est_orientation_s target_orientation;
+
+ if (_target_orientation_sub.update(&target_orientation)) {
+ if (target_orientation.orientation_valid) {
+ _target_yaw_valid = true;
+ _last_target_yaw_update = target_orientation.timestamp;
+ _target_yaw = target_orientation.yaw;
+
+ } else {
+ reset_target_yaw_state();
+ }
+ }
+
+ if (_target_yaw_valid && ((hrt_elapsed_time(&_last_target_yaw_update) / 1e6f) > _param_pld_btout.get())) {
+ reset_target_yaw_state();
+ }
+
+ } else {
+ reset_target_yaw_state();
+ }
+
+#endif // CONFIG_MODULES_VISION_TARGET_ESTIMATOR
+
// stop if we are landed
- if (_navigator->get_land_detected()->landed) {
+ if (_navigator->get_land_detected()->landed && _state != PrecLandState::Done) {
switch_to_state_done();
}
@@ -161,11 +192,21 @@ PrecLand::on_active()
break;
}
+#if defined(CONFIG_MODULES_VISION_TARGET_ESTIMATOR) && CONFIG_MODULES_VISION_TARGET_ESTIMATOR
+ publish_prec_land_status(_state != PrecLandState::Done);
+#endif // CONFIG_MODULES_VISION_TARGET_ESTIMATOR
}
void
PrecLand::on_inactivation()
{
+ clear_current_yaw_setpoint();
+ _navigator->set_position_setpoint_triplet_updated();
+
+#if defined(CONFIG_MODULES_VISION_TARGET_ESTIMATOR) && CONFIG_MODULES_VISION_TARGET_ESTIMATOR
+ reset_target_yaw_state();
+ publish_prec_land_status(false);
+#endif // CONFIG_MODULES_VISION_TARGET_ESTIMATOR
_is_activated = false;
}
@@ -268,6 +309,10 @@ PrecLand::run_state_horizontal_approach()
pos_sp_triplet->current.alt = _approach_alt;
pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
+#if defined(CONFIG_MODULES_VISION_TARGET_ESTIMATOR) && CONFIG_MODULES_VISION_TARGET_ESTIMATOR
+ update_current_yaw_setpoint();
+#endif // CONFIG_MODULES_VISION_TARGET_ESTIMATOR
+
_navigator->set_position_setpoint_triplet_updated();
}
@@ -298,6 +343,9 @@ PrecLand::run_state_descend_above_target()
_map_ref.reproject(_target_pose.x_abs, _target_pose.y_abs, pos_sp_triplet->current.lat, pos_sp_triplet->current.lon);
pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_LAND;
+#if defined(CONFIG_MODULES_VISION_TARGET_ESTIMATOR) && CONFIG_MODULES_VISION_TARGET_ESTIMATOR
+ update_current_yaw_setpoint();
+#endif // CONFIG_MODULES_VISION_TARGET_ESTIMATOR
_navigator->set_position_setpoint_triplet_updated();
}
@@ -352,6 +400,7 @@ PrecLand::switch_to_state_start()
if (check_state_conditions(PrecLandState::Start)) {
position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
+ clear_current_yaw_setpoint();
_navigator->set_position_setpoint_triplet_updated();
_search_cnt++;
@@ -400,6 +449,8 @@ PrecLand::switch_to_state_final_approach()
{
if (check_state_conditions(PrecLandState::FinalApproach)) {
print_state_switch_message("final approach");
+ clear_current_yaw_setpoint();
+ _navigator->set_position_setpoint_triplet_updated();
_state = PrecLandState::FinalApproach;
_state_start_time = hrt_absolute_time();
return true;
@@ -415,6 +466,7 @@ PrecLand::switch_to_state_search()
position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
pos_sp_triplet->current.alt = _navigator->get_home_position()->alt + _param_pld_srch_alt.get();
pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
+ clear_current_yaw_setpoint();
_navigator->set_position_setpoint_triplet_updated();
_target_acquired_time = 0;
@@ -433,6 +485,7 @@ PrecLand::switch_to_state_fallback()
pos_sp_triplet->current.lon = _navigator->get_global_position()->lon;
pos_sp_triplet->current.alt = _navigator->get_global_position()->alt;
pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_LAND;
+ clear_current_yaw_setpoint();
_navigator->set_position_setpoint_triplet_updated();
_state = PrecLandState::Fallback;
@@ -443,6 +496,8 @@ PrecLand::switch_to_state_fallback()
bool
PrecLand::switch_to_state_done()
{
+ clear_current_yaw_setpoint();
+ _navigator->set_position_setpoint_triplet_updated();
_state = PrecLandState::Done;
_state_start_time = hrt_absolute_time();
return true;
@@ -575,3 +630,41 @@ void PrecLand::slewrate(float &sp_x, float &sp_y)
sp_x = sp_curr(0);
sp_y = sp_curr(1);
}
+
+void PrecLand::clear_current_yaw_setpoint()
+{
+ position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+ pos_sp_triplet->current.yaw = NAN;
+}
+
+#if defined(CONFIG_MODULES_VISION_TARGET_ESTIMATOR) && CONFIG_MODULES_VISION_TARGET_ESTIMATOR
+void PrecLand::update_current_yaw_setpoint()
+{
+ position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+ pos_sp_triplet->current.yaw = (_param_pld_yaw_en.get() && _target_yaw_valid) ? _target_yaw : NAN;
+}
+
+void PrecLand::reset_target_yaw_state()
+{
+ _target_yaw = 0.f;
+ _target_yaw_valid = false;
+ _last_target_yaw_update = 0;
+}
+
+void PrecLand::publish_prec_land_status(const bool prec_land_ongoing)
+{
+ prec_land_status_s prec_land_status{};
+ prec_land_status.timestamp = hrt_absolute_time();
+
+ if (prec_land_ongoing) {
+ prec_land_status.state = prec_land_status_s::PREC_LAND_STATE_ONGOING;
+
+ } else {
+ prec_land_status.state = prec_land_status_s::PREC_LAND_STATE_STOPPED;
+ }
+
+ prec_land_status.nav_state = (int)_state;
+ _prec_land_status_pub.publish(prec_land_status);
+}
+
+#endif // CONFIG_MODULES_VISION_TARGET_ESTIMATOR
diff --git a/src/modules/navigator/precland.h b/src/modules/navigator/precland.h
index df07ff7bd5f2..6200ca3a0b33 100644
--- a/src/modules/navigator/precland.h
+++ b/src/modules/navigator/precland.h
@@ -46,6 +46,11 @@
#include
#include
+#if defined(CONFIG_MODULES_VISION_TARGET_ESTIMATOR) && CONFIG_MODULES_VISION_TARGET_ESTIMATOR
+#include
+#include
+#endif // CONFIG_MODULES_VISION_TARGET_ESTIMATOR
+
#include "navigator_mode.h"
#include "mission_block.h"
@@ -106,10 +111,26 @@ class PrecLand : public MissionBlock, public ModuleParams
// check if a given state could be changed into. Return true if possible to transition to state, false otherwise
bool check_state_conditions(PrecLandState state);
void slewrate(float &sp_x, float &sp_y);
+ void clear_current_yaw_setpoint();
+
+#if defined(CONFIG_MODULES_VISION_TARGET_ESTIMATOR) && CONFIG_MODULES_VISION_TARGET_ESTIMATOR
+ void update_current_yaw_setpoint();
+ void reset_target_yaw_state();
+#endif // CONFIG_MODULES_VISION_TARGET_ESTIMATOR
landing_target_pose_s _target_pose{}; /**< precision landing target position */
uORB::Subscription _target_pose_sub{ORB_ID(landing_target_pose)};
+
+#if defined(CONFIG_MODULES_VISION_TARGET_ESTIMATOR) && CONFIG_MODULES_VISION_TARGET_ESTIMATOR
+ uORB::Subscription _target_orientation_sub {ORB_ID(vision_target_est_orientation)};
+ float _target_yaw{0.f};
+ bool _target_yaw_valid{false};
+ hrt_abstime _last_target_yaw_update{0};
+ uORB::Publication _prec_land_status_pub {ORB_ID(prec_land_status)};
+ void publish_prec_land_status(const bool prec_land_ongoing);
+#endif // CONFIG_MODULES_VISION_TARGET_ESTIMATOR
+
bool _target_pose_valid{false}; /**< whether we have received a landing target position message */
bool _target_pose_updated{false}; /**< wether the landing target position message is updated */
@@ -133,6 +154,9 @@ class PrecLand : public MissionBlock, public ModuleParams
bool _is_activated {false}; /**< indicates if precland is activated */
DEFINE_PARAMETERS(
+#if defined(CONFIG_MODULES_VISION_TARGET_ESTIMATOR) && CONFIG_MODULES_VISION_TARGET_ESTIMATOR
+ (ParamInt) _param_pld_yaw_en,
+#endif // CONFIG_MODULES_VISION_TARGET_ESTIMATOR
(ParamFloat) _param_pld_btout,
(ParamFloat) _param_pld_hacc_rad,
(ParamFloat) _param_pld_fappr_alt,
diff --git a/src/modules/navigator/precland_params.yaml b/src/modules/navigator/precland_params.yaml
index 979123c4dc37..562af60bf26e 100644
--- a/src/modules/navigator/precland_params.yaml
+++ b/src/modules/navigator/precland_params.yaml
@@ -70,3 +70,11 @@ parameters:
default: 3
min: 0
max: 100
+ PLD_YAW_EN:
+ description:
+ short: Set to true to control yaw while landing.
+ long: Control the orientation when landing. The orientation comes from the topic vision_target_est_orientation.
+ type: boolean
+ default: 0
+ min: 0
+ max: 1
\ No newline at end of file
diff --git a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp
index c579a9ba4b3c..60663df8813e 100644
--- a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp
+++ b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp
@@ -97,6 +97,12 @@ SimulatorMavlink::SimulatorMavlink() :
}
_esc_status_pub.advertise();
+#if defined(CONFIG_MODULES_VISION_TARGET_ESTIMATOR) && CONFIG_MODULES_VISION_TARGET_ESTIMATOR
+ _fiducial_marker_pos_report_pub.advertise();
+ _fiducial_marker_yaw_report_pub.advertise();
+ _target_gnss_pub.advertise();
+ _param_vte_en = param_find("VTE_EN");
+#endif // CONFIG_MODULES_VISION_TARGET_ESTIMATOR
}
void SimulatorMavlink::parameters_update(bool force)
@@ -382,6 +388,22 @@ void SimulatorMavlink::handle_message(const mavlink_message_t *msg)
handle_message_landing_target(msg);
break;
+#if defined(CONFIG_MODULES_VISION_TARGET_ESTIMATOR) && CONFIG_MODULES_VISION_TARGET_ESTIMATOR
+#if defined(MAVLINK_MSG_ID_TARGET_RELATIVE)
+
+ case MAVLINK_MSG_ID_TARGET_RELATIVE:
+ handle_message_target_relative(msg);
+ break;
+#endif // MAVLINK_MSG_ID_TARGET_RELATIVE
+
+#if defined(MAVLINK_MSG_ID_TARGET_ABSOLUTE)
+
+ case MAVLINK_MSG_ID_TARGET_ABSOLUTE:
+ handle_message_target_absolute(msg);
+ break;
+#endif // MAVLINK_MSG_ID_TARGET_ABSOLUTE
+#endif // CONFIG_MODULES_VISION_TARGET_ESTIMATOR
+
case MAVLINK_MSG_ID_HIL_STATE_QUATERNION:
handle_message_hil_state_quaternion(msg);
break;
@@ -659,6 +681,217 @@ void SimulatorMavlink::handle_message_landing_target(const mavlink_message_t *ms
}
}
+
+#if defined(CONFIG_MODULES_VISION_TARGET_ESTIMATOR) && CONFIG_MODULES_VISION_TARGET_ESTIMATOR
+
+#if defined(MAVLINK_MSG_ID_TARGET_ABSOLUTE)
+void
+SimulatorMavlink::handle_message_target_absolute(const mavlink_message_t *msg)
+{
+ mavlink_target_absolute_t target_absolute;
+ mavlink_msg_target_absolute_decode(msg, &target_absolute);
+
+ target_gnss_s target_GNSS_report{};
+ target_GNSS_report.timestamp = hrt_absolute_time();
+ target_GNSS_report.timestamp_sample = target_GNSS_report.timestamp;
+
+ bool updated = false;
+
+ // Position: bit 0
+ if (!(target_absolute.sensor_capabilities & static_cast(TargetAbsoluteSensorCapability::kPosition))) {
+ target_GNSS_report.abs_pos_updated = false;
+
+ } else {
+ target_GNSS_report.abs_pos_updated = true;
+ target_GNSS_report.latitude_deg = target_absolute.lat * 1e-7;
+ target_GNSS_report.longitude_deg = target_absolute.lon * 1e-7;
+ target_GNSS_report.altitude_msl_m = target_absolute.alt;
+ target_GNSS_report.eph = target_absolute.position_std[0];
+ target_GNSS_report.epv = target_absolute.position_std[1];
+ updated = true;
+ }
+
+ // Velocity: bit 1
+ if (!(target_absolute.sensor_capabilities & static_cast(TargetAbsoluteSensorCapability::kVelocity))) {
+ target_GNSS_report.vel_ned_updated = false;
+
+ } else {
+ const matrix::Quatf q_target(target_absolute.q_target);
+ const bool target_attitude_available = (fabsf(q_target(0)) + fabsf(q_target(1)) + fabsf(q_target(2)) +
+ fabsf(q_target(3))) > FLT_EPSILON;
+
+ if (target_attitude_available) {
+ const matrix::Vector3f vel_target_body(target_absolute.vel[0], target_absolute.vel[1], target_absolute.vel[2]);
+ const matrix::Vector3f vel_target_ned = q_target.rotateVector(vel_target_body);
+
+ target_GNSS_report.vel_ned_updated = true;
+ target_GNSS_report.vel_n_m_s = vel_target_ned(0);
+ target_GNSS_report.vel_e_m_s = vel_target_ned(1);
+ target_GNSS_report.vel_d_m_s = vel_target_ned(2);
+ target_GNSS_report.s_acc_m_s = fmaxf(fabsf(target_absolute.vel_std[0]),
+ fmaxf(fabsf(target_absolute.vel_std[1]),
+ fabsf(target_absolute.vel_std[2])));
+ updated = true;
+
+ } else {
+ target_GNSS_report.vel_ned_updated = false;
+ }
+ }
+
+ if (updated) { _target_gnss_pub.publish(target_GNSS_report); }
+}
+#endif // MAVLINK_MSG_ID_TARGET_ABSOLUTE
+
+
+#if defined(MAVLINK_MSG_ID_TARGET_RELATIVE)
+void SimulatorMavlink::handle_message_target_relative(const mavlink_message_t *msg)
+{
+ mavlink_target_relative_t target_relative;
+ mavlink_msg_target_relative_decode(msg, &target_relative);
+
+ vehicle_attitude_s vehicle_attitude;
+ vehicle_local_position_s vehicle_local_position;
+
+ // Unsupported sensor frame
+ if (target_relative.frame != TARGET_OBS_FRAME_LOCAL_OFFSET_NED && target_relative.frame != TARGET_OBS_FRAME_LOCAL_NED &&
+ target_relative.frame != TARGET_OBS_FRAME_BODY_FRD && target_relative.frame != TARGET_OBS_FRAME_OTHER) {
+ PX4_WARN("target relative: coordinate frame %d unsupported", (int)target_relative.frame);
+ return;
+ }
+
+ // Unsupported measurement type
+ if (target_relative.type != LANDING_TARGET_TYPE_VISION_FIDUCIAL) {
+ PX4_WARN("target relative: coordinate frame %d type", (int)target_relative.type);
+ return;
+ }
+
+ matrix::Quatf q_sensor(target_relative.q_sensor);
+ bool attitude_available = (fabsf(q_sensor(0)) + fabsf(q_sensor(1)) + fabsf(q_sensor(2)) +
+ fabsf(q_sensor(3))) > FLT_EPSILON;
+
+ // Check if attitude can be infered based on the frame
+ if (!attitude_available) {
+ if (target_relative.frame == TARGET_OBS_FRAME_LOCAL_OFFSET_NED || target_relative.frame == TARGET_OBS_FRAME_LOCAL_NED) {
+ // Observation already in NED, set quaternion to identity (1,0,0,0)
+ q_sensor.setIdentity();
+ attitude_available = true;
+
+ } else if (target_relative.frame == TARGET_OBS_FRAME_BODY_FRD && _vehicle_attitude_sub.copy(&vehicle_attitude)) {
+ // Set the quaternion to the current attitude
+ const matrix::Quaternionf quat_att(&vehicle_attitude.q[0]);
+ q_sensor = quat_att;
+ attitude_available = true;
+
+ } else if (target_relative.frame == TARGET_OBS_FRAME_OTHER) {
+ // Target sensor frame: OTHER and no quaternion is provided
+ PX4_WARN("target relative: coordinate frame %d unsupported when the q_sensor is not filled",
+ (int)target_relative.frame);
+ }
+ }
+
+ // Target cannot be sent withtout valid attitude
+ if (!attitude_available) {
+ return;
+ }
+
+ // Forward target to the vision target estimator (VTE) or precland based VTE_EN
+ int32_t vte_enabled = 0;
+
+ if (_param_vte_en == PARAM_INVALID) {
+ if (hrt_elapsed_time(&_vte_en_invalid_warn_last) > 20_s) {
+ PX4_WARN(" Could not find VTE_EN, target_relative msg ignored.");
+ _vte_en_invalid_warn_last = hrt_absolute_time();
+ }
+
+ return;
+
+ } else {
+ param_get(_param_vte_en, &vte_enabled);
+ }
+
+ if (!vte_enabled) {
+
+ // Send the target directly to the precision landing algorithm in local NED frame
+ landing_target_pose_s landing_target_pose{};
+ landing_target_pose.timestamp = target_relative.timestamp;
+
+ // Measurement already in local NED
+ if (target_relative.frame == TARGET_OBS_FRAME_LOCAL_NED) {
+
+ landing_target_pose.x_abs = target_relative.x;
+ landing_target_pose.y_abs = target_relative.y;
+ landing_target_pose.z_abs = target_relative.z;
+ landing_target_pose.abs_pos_valid = true;
+
+ _landing_target_pose_pub.publish(landing_target_pose);
+
+ } else if (_vehicle_local_position_sub.copy(&vehicle_local_position)
+ && vehicle_local_position.xy_valid && vehicle_local_position.z_valid) {
+
+ // If the local position is available, convert from sensor frame to local NED
+ matrix::Vector3f target_relative_frame(target_relative.x, target_relative.y, target_relative.z);
+
+ if (target_relative.frame == TARGET_OBS_FRAME_BODY_FRD || target_relative.frame == TARGET_OBS_FRAME_OTHER) {
+ // Rotate measurement into vehicle-carried NED
+ target_relative_frame = q_sensor.rotateVector(target_relative_frame);
+ }
+
+ // Convert from vehicle-carried NED to local NED
+ landing_target_pose.x_abs = target_relative_frame(0) + vehicle_local_position.x;
+ landing_target_pose.y_abs = target_relative_frame(1) + vehicle_local_position.y;
+ landing_target_pose.z_abs = target_relative_frame(2) + vehicle_local_position.z;
+ landing_target_pose.abs_pos_valid = true;
+
+ _landing_target_pose_pub.publish(landing_target_pose);
+ }
+
+ } else {
+
+ // Only supported type: LANDING_TARGET_TYPE_VISION_FIDUCIAL
+
+ // Position report
+ fiducial_marker_pos_report_s fiducial_marker_pos_report{};
+
+ const hrt_abstime now = hrt_absolute_time();
+ const hrt_abstime timestamp_sample = target_relative.timestamp;
+
+ fiducial_marker_pos_report.timestamp = now;
+ fiducial_marker_pos_report.timestamp_sample = timestamp_sample;
+ fiducial_marker_pos_report.rel_pos[0] = target_relative.x;
+ fiducial_marker_pos_report.rel_pos[1] = target_relative.y;
+ fiducial_marker_pos_report.rel_pos[2] = target_relative.z;
+
+ fiducial_marker_pos_report.cov_rel_pos[0] = target_relative.pos_std[0] * target_relative.pos_std[0];
+ fiducial_marker_pos_report.cov_rel_pos[1] = target_relative.pos_std[1] * target_relative.pos_std[1];
+ fiducial_marker_pos_report.cov_rel_pos[2] = target_relative.pos_std[2] * target_relative.pos_std[2];
+
+ q_sensor.copyTo(fiducial_marker_pos_report.q);
+ _fiducial_marker_pos_report_pub.publish(fiducial_marker_pos_report);
+
+ const matrix::Quatf q_target(target_relative.q_target);
+ const bool target_attitude_available = (fabsf(q_target(0)) + fabsf(q_target(1)) + fabsf(q_target(2)) +
+ fabsf(q_target(3))) > FLT_EPSILON;
+
+ if (target_attitude_available) {
+ // Yaw report
+ fiducial_marker_yaw_report_s fiducial_marker_yaw_report{};
+ fiducial_marker_yaw_report.timestamp = now;
+ fiducial_marker_yaw_report.timestamp_sample = timestamp_sample;
+
+ // Transform quaternion from the target's frame to the TARGET_OBS_FRAME to the yaw relative to NED
+ const matrix::Quatf q_in_ned = q_sensor * q_target;
+ const float target_yaw_ned = matrix::Eulerf(q_in_ned).psi();
+
+ fiducial_marker_yaw_report.yaw_ned = target_yaw_ned;
+ fiducial_marker_yaw_report.yaw_var_ned = target_relative.yaw_std * target_relative.yaw_std;
+ _fiducial_marker_yaw_report_pub.publish(fiducial_marker_yaw_report);
+ }
+ }
+
+}
+#endif // MAVLINK_MSG_ID_TARGET_RELATIVE
+#endif // CONFIG_MODULES_VISION_TARGET_ESTIMATOR
+
void SimulatorMavlink::handle_message_odometry(const mavlink_message_t *msg)
{
mavlink_odometry_t odom_in;
diff --git a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp
index 5c1e9481e8ae..605b68ffbb20 100644
--- a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp
+++ b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp
@@ -63,6 +63,12 @@
#include
#include
#include
+#include
+#if defined(CONFIG_MODULES_VISION_TARGET_ESTIMATOR) && CONFIG_MODULES_VISION_TARGET_ESTIMATOR
+#include
+#include
+#include
+#endif // CONFIG_MODULES_VISION_TARGET_ESTIMATOR
#include
#include
#include
@@ -113,6 +119,16 @@ static inline SensorSource operator &(A lhs, B rhs)
);
}
+#if defined(CONFIG_MODULES_VISION_TARGET_ESTIMATOR) && CONFIG_MODULES_VISION_TARGET_ESTIMATOR
+enum class TargetAbsoluteSensorCapability : uint8_t {
+ kPosition = (1 << 0),
+ kVelocity = (1 << 1),
+ kAcceleration = (1 << 2),
+ kAttitude = (1 << 3),
+ kRates = (1 << 4),
+};
+#endif // CONFIG_MODULES_VISION_TARGET_ESTIMATOR
+
class SimulatorMavlink : public ModuleParams
{
public:
@@ -226,6 +242,8 @@ class SimulatorMavlink : public ModuleParams
void handle_message_hil_sensor(const mavlink_message_t *msg);
void handle_message_hil_state_quaternion(const mavlink_message_t *msg);
void handle_message_landing_target(const mavlink_message_t *msg);
+ void handle_message_target_relative(const mavlink_message_t *msg);
+ void handle_message_target_absolute(const mavlink_message_t *msg);
void handle_message_odometry(const mavlink_message_t *msg);
void handle_message_optical_flow(const mavlink_message_t *msg);
void handle_message_rc_channels(const mavlink_message_t *msg);
@@ -251,6 +269,14 @@ class SimulatorMavlink : public ModuleParams
uORB::Publication _gpos_ground_truth_pub{ORB_ID(vehicle_global_position_groundtruth)};
uORB::Publication _lpos_ground_truth_pub{ORB_ID(vehicle_local_position_groundtruth)};
uORB::Publication _input_rc_pub{ORB_ID(input_rc)};
+ uORB::Publication _landing_target_pose_pub{ORB_ID(landing_target_pose)};
+#if defined(CONFIG_MODULES_VISION_TARGET_ESTIMATOR) && CONFIG_MODULES_VISION_TARGET_ESTIMATOR
+ uORB::Publication _fiducial_marker_pos_report_pub {ORB_ID(fiducial_marker_pos_report)};
+ uORB::Publication _fiducial_marker_yaw_report_pub{ORB_ID(fiducial_marker_yaw_report)};
+ uORB::Publication _target_gnss_pub{ORB_ID(target_gnss)};
+ param_t _param_vte_en{PARAM_INVALID};
+ hrt_abstime _vte_en_invalid_warn_last{0};
+#endif // CONFIG_MODULES_VISION_TARGET_ESTIMATOR
//rpm
uORB::Publication _rpm_pub{ORB_ID(rpm)};
@@ -268,6 +294,8 @@ class SimulatorMavlink : public ModuleParams
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
uORB::Subscription _battery_status_sub{ORB_ID(battery_status)};
+ uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
+ uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
// hil map_ref data
MapProjection _global_local_proj_ref{};
diff --git a/src/modules/vision_target_estimator/CMakeLists.txt b/src/modules/vision_target_estimator/CMakeLists.txt
new file mode 100644
index 000000000000..3c1f946cde67
--- /dev/null
+++ b/src/modules/vision_target_estimator/CMakeLists.txt
@@ -0,0 +1,290 @@
+############################################################################
+#
+# Copyright (c) 2025 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.
+#
+############################################################################
+
+#
+# Vision Target Estimator (VTEST) Symforce code generation
+#
+option(VTEST_SYMFORCE_GEN "vtest generate symforce output" OFF)
+option(VTEST_UPDATE_COMMITTED_DERIVATION "Rewrite committed default generated files" OFF)
+
+# Check if symforce is available
+execute_process(
+ COMMAND ${PYTHON_EXECUTABLE} -m symforce.symbolic
+ RESULT_VARIABLE PYTHON_SYMFORCE_EXIT_CODE
+ OUTPUT_QUIET
+ ERROR_QUIET
+)
+
+# Enable symforce generation if a non-default configuration is used
+if(CONFIG_VTEST_MOVING)
+ set(VTEST_SYMFORCE_GEN ON)
+endif()
+
+# Set up paths and default variables
+set(VTEST_DERIVATION_SRC_DIR ${CMAKE_CURRENT_SOURCE_DIR}/Position/vtest_derivation)
+set(VTEST_PREGENERATED_STATIC_DIR ${VTEST_DERIVATION_SRC_DIR}/generated)
+set(VTEST_PREGENERATED_MOVING_DIR ${VTEST_DERIVATION_SRC_DIR}/generated_moving)
+
+set(VTEST_PREGENERATED_STATIC_FILES
+ ${VTEST_PREGENERATED_STATIC_DIR}/state.h
+ ${VTEST_PREGENERATED_STATIC_DIR}/predictCov.h
+ ${VTEST_PREGENERATED_STATIC_DIR}/computeInnovCov.h
+ ${VTEST_PREGENERATED_STATIC_DIR}/predictState.h
+ ${VTEST_PREGENERATED_STATIC_DIR}/getTransitionMatrix.h
+ ${VTEST_PREGENERATED_STATIC_DIR}/applyCorrection.h
+)
+
+set(VTEST_PREGENERATED_MOVING_FILES
+ ${VTEST_PREGENERATED_MOVING_DIR}/state.h
+ ${VTEST_PREGENERATED_MOVING_DIR}/predictCov.h
+ ${VTEST_PREGENERATED_MOVING_DIR}/computeInnovCov.h
+ ${VTEST_PREGENERATED_MOVING_DIR}/predictState.h
+ ${VTEST_PREGENERATED_MOVING_DIR}/getTransitionMatrix.h
+ ${VTEST_PREGENERATED_MOVING_DIR}/applyCorrection.h
+)
+
+set(VTEST_DERIVATION_DST_DIR ${CMAKE_CURRENT_BINARY_DIR}/vtest_derivation)
+file(MAKE_DIRECTORY ${VTEST_DERIVATION_DST_DIR})
+
+set(VTEST_GENERATED_FILE_LIST_DST
+ ${VTEST_DERIVATION_DST_DIR}/generated/state.h
+ ${VTEST_DERIVATION_DST_DIR}/generated/predictCov.h
+ ${VTEST_DERIVATION_DST_DIR}/generated/computeInnovCov.h
+ ${VTEST_DERIVATION_DST_DIR}/generated/predictState.h
+ ${VTEST_DERIVATION_DST_DIR}/generated/getTransitionMatrix.h
+ ${VTEST_DERIVATION_DST_DIR}/generated/applyCorrection.h
+)
+
+# Use one file as the dependency anchor. The custom_command OUTPUT will ensure all are generated.
+set(VTEST_GENERATED_FILES ${VTEST_DERIVATION_DST_DIR}/generated/state.h)
+set(VTEST_GENERATED_DERIVATION_INCLUDE_PATH ${CMAKE_CURRENT_BINARY_DIR})
+
+set(VTEST_ORIENTATION_DERIVATION_SRC_DIR ${CMAKE_CURRENT_SOURCE_DIR}/Orientation/vtest_derivation)
+set(VTEST_ORIENTATION_PREGENERATED_DIR ${VTEST_ORIENTATION_DERIVATION_SRC_DIR}/generated)
+set(VTEST_ORIENTATION_PREGENERATED_FILES
+ ${VTEST_ORIENTATION_PREGENERATED_DIR}/predictState.h
+ ${VTEST_ORIENTATION_PREGENERATED_DIR}/getTransitionMatrix.h
+ ${VTEST_ORIENTATION_PREGENERATED_DIR}/predictCov.h
+)
+set(VTEST_ORIENTATION_DERIVATION_DST_DIR ${CMAKE_CURRENT_BINARY_DIR}/vte_orientation_derivation)
+file(MAKE_DIRECTORY ${VTEST_ORIENTATION_DERIVATION_DST_DIR})
+set(VTEST_ORIENTATION_GENERATED_FILE_LIST_DST
+ ${VTEST_ORIENTATION_DERIVATION_DST_DIR}/generated/predictState.h
+ ${VTEST_ORIENTATION_DERIVATION_DST_DIR}/generated/getTransitionMatrix.h
+ ${VTEST_ORIENTATION_DERIVATION_DST_DIR}/generated/predictCov.h
+)
+set(VTEST_ORIENTATION_GENERATED_FILES ${VTEST_ORIENTATION_DERIVATION_DST_DIR}/generated/predictCov.h)
+
+set(VTEST_GENERATED_TARGET "")
+
+if(VTEST_SYMFORCE_GEN AND (${PYTHON_SYMFORCE_EXIT_CODE} EQUAL 0))
+
+ set(VTEST_SYMFORCE_TARGET_DEPS)
+
+ # Regenerate committed defaults when requested.
+ if(VTEST_UPDATE_COMMITTED_DERIVATION)
+ add_custom_command(
+ OUTPUT
+ ${VTEST_PREGENERATED_STATIC_FILES}
+ COMMAND
+ ${PYTHON_EXECUTABLE} ${VTEST_DERIVATION_SRC_DIR}/derivation.py
+ DEPENDS
+ ${VTEST_DERIVATION_SRC_DIR}/derivation.py
+ ${VTEST_DERIVATION_SRC_DIR}/utils/derivation_utils.py
+
+ WORKING_DIRECTORY ${VTEST_DERIVATION_SRC_DIR}
+ COMMENT "VTEST Symforce code generation (default, committed)"
+ USES_TERMINAL
+ )
+ list(APPEND VTEST_SYMFORCE_TARGET_DEPS ${VTEST_PREGENERATED_STATIC_FILES})
+
+ if(CONFIG_VTEST_MOVING)
+ set(VTEST_DERIVATION_MOVING_TMP_DIR ${CMAKE_CURRENT_BINARY_DIR}/vtest_derivation_committed_moving)
+ file(MAKE_DIRECTORY ${VTEST_DERIVATION_MOVING_TMP_DIR})
+
+ add_custom_command(
+ OUTPUT
+ ${VTEST_PREGENERATED_MOVING_FILES}
+ COMMAND
+ ${CMAKE_COMMAND} -E remove_directory ${VTEST_DERIVATION_MOVING_TMP_DIR}/generated
+ COMMAND
+ ${PYTHON_EXECUTABLE} ${VTEST_DERIVATION_SRC_DIR}/derivation.py --moving
+ COMMAND
+ ${CMAKE_COMMAND} -E remove_directory ${VTEST_PREGENERATED_MOVING_DIR}
+ COMMAND
+ ${CMAKE_COMMAND} -E make_directory ${VTEST_PREGENERATED_MOVING_DIR}
+ COMMAND
+ ${CMAKE_COMMAND} -E copy_directory ${VTEST_DERIVATION_MOVING_TMP_DIR}/generated ${VTEST_PREGENERATED_MOVING_DIR}
+ DEPENDS
+ ${VTEST_DERIVATION_SRC_DIR}/derivation.py
+ ${VTEST_DERIVATION_SRC_DIR}/utils/derivation_utils.py
+ WORKING_DIRECTORY ${VTEST_DERIVATION_MOVING_TMP_DIR}
+ COMMENT "VTEST Symforce code generation (moving, committed)"
+ USES_TERMINAL
+ )
+ list(APPEND VTEST_SYMFORCE_TARGET_DEPS ${VTEST_PREGENERATED_MOVING_FILES})
+ endif()
+
+ add_custom_command(
+ OUTPUT
+ ${VTEST_ORIENTATION_PREGENERATED_FILES}
+ COMMAND
+ ${PYTHON_EXECUTABLE} ${VTEST_ORIENTATION_DERIVATION_SRC_DIR}/derivation.py
+ DEPENDS
+ ${VTEST_ORIENTATION_DERIVATION_SRC_DIR}/derivation.py
+ WORKING_DIRECTORY ${VTEST_ORIENTATION_DERIVATION_SRC_DIR}
+ COMMENT "VTEST orientation Symforce code generation (committed)"
+ USES_TERMINAL
+ )
+ list(APPEND VTEST_SYMFORCE_TARGET_DEPS ${VTEST_ORIENTATION_PREGENERATED_FILES})
+ endif()
+
+ set(SYMFORCE_ARGS)
+ if(CONFIG_VTEST_MOVING)
+ message(STATUS "vtest: symforce --moving")
+ list(APPEND SYMFORCE_ARGS --moving)
+ endif()
+
+ add_custom_command(
+ OUTPUT
+ ${VTEST_GENERATED_FILE_LIST_DST}
+ COMMAND
+ ${PYTHON_EXECUTABLE} ${VTEST_DERIVATION_SRC_DIR}/derivation.py ${SYMFORCE_ARGS}
+ DEPENDS
+ ${VTEST_DERIVATION_SRC_DIR}/derivation.py
+ ${VTEST_DERIVATION_SRC_DIR}/utils/derivation_utils.py
+
+ WORKING_DIRECTORY ${VTEST_DERIVATION_DST_DIR}
+ COMMENT "VTEST Symforce code generation"
+ USES_TERMINAL
+ )
+ list(APPEND VTEST_SYMFORCE_TARGET_DEPS ${VTEST_GENERATED_FILE_LIST_DST})
+
+ add_custom_command(
+ OUTPUT
+ ${VTEST_ORIENTATION_GENERATED_FILE_LIST_DST}
+ COMMAND
+ ${PYTHON_EXECUTABLE} ${VTEST_ORIENTATION_DERIVATION_SRC_DIR}/derivation.py
+ DEPENDS
+ ${VTEST_ORIENTATION_DERIVATION_SRC_DIR}/derivation.py
+ WORKING_DIRECTORY ${VTEST_ORIENTATION_DERIVATION_DST_DIR}
+ COMMENT "VTEST orientation Symforce code generation"
+ USES_TERMINAL
+ )
+ list(APPEND VTEST_SYMFORCE_TARGET_DEPS ${VTEST_ORIENTATION_GENERATED_FILE_LIST_DST})
+
+ add_custom_target(vtest_symforce_generate
+ DEPENDS ${VTEST_SYMFORCE_TARGET_DEPS}
+ )
+ set(VTEST_GENERATED_TARGET vtest_symforce_generate)
+
+else()
+ if(CONFIG_VTEST_MOVING)
+ set(VTEST_PREGENERATED_SOURCE_DIR ${VTEST_PREGENERATED_MOVING_DIR})
+ set(VTEST_PREGENERATED_SOURCE_FILES ${VTEST_PREGENERATED_MOVING_FILES})
+ set(VTEST_PREGENERATED_CONFIG moving)
+ else()
+ set(VTEST_PREGENERATED_SOURCE_DIR ${VTEST_PREGENERATED_STATIC_DIR})
+ set(VTEST_PREGENERATED_SOURCE_FILES ${VTEST_PREGENERATED_STATIC_FILES})
+ set(VTEST_PREGENERATED_CONFIG static)
+ endif()
+
+ add_custom_command(
+ OUTPUT
+ ${VTEST_GENERATED_FILE_LIST_DST}
+ COMMAND
+ ${CMAKE_COMMAND} -E remove_directory ${VTEST_DERIVATION_DST_DIR}/generated
+ COMMAND
+ ${CMAKE_COMMAND} -E make_directory ${VTEST_DERIVATION_DST_DIR}/generated
+ COMMAND
+ ${CMAKE_COMMAND} -E copy_directory ${VTEST_PREGENERATED_SOURCE_DIR} ${VTEST_DERIVATION_DST_DIR}/generated
+ DEPENDS
+ ${VTEST_PREGENERATED_SOURCE_FILES}
+ COMMENT "VTEST using pre-generated symforce outputs (${VTEST_PREGENERATED_CONFIG})"
+ )
+
+ add_custom_command(
+ OUTPUT
+ ${VTEST_ORIENTATION_GENERATED_FILE_LIST_DST}
+ COMMAND
+ ${CMAKE_COMMAND} -E remove_directory ${VTEST_ORIENTATION_DERIVATION_DST_DIR}/generated
+ COMMAND
+ ${CMAKE_COMMAND} -E make_directory ${VTEST_ORIENTATION_DERIVATION_DST_DIR}/generated
+ COMMAND
+ ${CMAKE_COMMAND} -E copy_directory ${VTEST_ORIENTATION_PREGENERATED_DIR}
+ ${VTEST_ORIENTATION_DERIVATION_DST_DIR}/generated
+ DEPENDS
+ ${VTEST_ORIENTATION_PREGENERATED_FILES}
+ COMMENT "VTEST orientation using pre-generated symforce outputs"
+ )
+
+ add_custom_target(vtest_derivation_prebuilt
+ DEPENDS ${VTEST_GENERATED_FILE_LIST_DST} ${VTEST_ORIENTATION_GENERATED_FILE_LIST_DST}
+ )
+ set(VTEST_GENERATED_TARGET vtest_derivation_prebuilt)
+endif()
+
+if(VTEST_GENERATED_TARGET)
+ add_dependencies(prebuild_targets ${VTEST_GENERATED_TARGET})
+endif()
+
+if(CONFIG_MODULES_VISION_TARGET_ESTIMATOR)
+ px4_add_module(
+ MODULE modules__vision_target_estimator
+ MAIN vision_target_estimator
+ STACK_MAX
+ 5000
+ SRCS
+ VisionTargetEst.cpp
+ Position/VTEPosition.cpp
+ Position/KF_position.cpp
+ Orientation/VTEOrientation.cpp
+ Orientation/KF_orientation.cpp
+
+ ${VTEST_GENERATED_FILES}
+ ${VTEST_ORIENTATION_GENERATED_FILES}
+ MODULE_CONFIG
+ vision_target_estimator_params.yaml
+ DEPENDS
+ mathlib
+ px4_work_queue
+ )
+
+ target_include_directories(modules__vision_target_estimator BEFORE PRIVATE
+ ${VTEST_GENERATED_DERIVATION_INCLUDE_PATH}
+ )
+endif()
+
+if(BUILD_TESTING)
+ add_subdirectory(test)
+endif()
diff --git a/src/modules/vision_target_estimator/Kconfig b/src/modules/vision_target_estimator/Kconfig
new file mode 100644
index 000000000000..510e2b5c83d8
--- /dev/null
+++ b/src/modules/vision_target_estimator/Kconfig
@@ -0,0 +1,24 @@
+menuconfig MODULES_VISION_TARGET_ESTIMATOR
+ bool "vision_target_estimator"
+ default n
+ depends on !BOARD_CONSTRAINED_MEMORY
+ depends on !BOARD_CONSTRAINED_FLASH
+ ---help---
+ Enable support for vision_target_estimator
+
+menuconfig USER_VISION_TARGET_ESTIMATOR
+ bool "vision_target_estimator running as userspace module"
+ default y
+ depends on BOARD_PROTECTED && MODULES_VISION_TARGET_ESTIMATOR
+ ---help---
+ Put vision_target_estimator in userspace memory
+
+menuconfig VTEST_MOVING
+ depends on MODULES_VISION_TARGET_ESTIMATOR
+ bool "vtest with moving target"
+ default n
+ depends on !BOARD_CONSTRAINED_MEMORY
+ depends on !BOARD_CONSTRAINED_FLASH
+ ---help---
+ Enable support for estimation of moving targets.
+ WARNING: Experimental beta version
diff --git a/src/modules/vision_target_estimator/Orientation/KF_orientation.cpp b/src/modules/vision_target_estimator/Orientation/KF_orientation.cpp
new file mode 100644
index 000000000000..54739ecff491
--- /dev/null
+++ b/src/modules/vision_target_estimator/Orientation/KF_orientation.cpp
@@ -0,0 +1,145 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2025 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file KF_orientation.cpp
+ * @brief Filter to estimate the orientation of static and moving targets. State: [yaw, yaw_rate]
+ *
+ * @author Jonas Perolini
+ *
+ */
+
+#include "KF_orientation.h"
+
+#include
+#include
+
+#include
+#include
+#include
+#include
+
+namespace vision_target_estimator
+{
+
+using VectorState = KF_orientation::VectorState;
+using SquareMatrixState = KF_orientation::SquareMatrixState;
+
+void KF_orientation::predict(float dt)
+{
+ if (!PX4_ISFINITE(dt) || (dt < 0.f)) {
+ return;
+ }
+
+ VectorState state_next;
+ SquareMatrixState cov_next;
+
+ predictState(dt, EmptyInput{}, _state, _state_covariance, state_next, cov_next);
+
+ _state = state_next;
+ _state_covariance = cov_next;
+}
+
+SquareMatrixState KF_orientation::getTransitionMatrix(float dt) const
+{
+ return sym::Gettransitionmatrix(dt);
+}
+
+void KF_orientation::predictState(float dt, const EmptyInput &input,
+ const VectorState &x_in,
+ const SquareMatrixState &P_in,
+ VectorState &x_out,
+ SquareMatrixState &P_out)
+{
+ (void)input;
+
+ x_out = sym::Predictstate(dt, x_in);
+ x_out(KF_orientation::kYaw) = matrix::wrap_pi(x_out(KF_orientation::kYaw));
+
+ // Keep invalid values from injecting covariance and use generated covariance propagation.
+ const float yaw_acc_var = (PX4_ISFINITE(_yaw_acc_var) && (_yaw_acc_var > 0.f)) ? _yaw_acc_var : 0.f;
+ P_out = sym::Predictcov(dt, yaw_acc_var, P_in);
+}
+
+void KF_orientation::computeInnovation(const VectorState &state,
+ const SquareMatrixState &cov,
+ const ScalarMeas &meas,
+ float &innov, float &innov_var) const
+{
+
+ innov = meas.val - (meas.H.transpose() * state)(0, 0);
+ innov_var = (meas.H.transpose() * cov * meas.H)(0, 0) + meas.unc;
+
+ // Wrap innovations for yaw measurements
+ const bool is_yaw_meas = (fabsf(meas.H(KF_orientation::kYaw)) > 0.f) && (fabsf(meas.H(KF_orientation::kYawRate)) < FLT_EPSILON);
+
+ if (is_yaw_meas) {
+ innov = matrix::wrap_pi(innov);
+ }
+}
+
+void KF_orientation::pushHistory(const uint64_t time_us)
+{
+ _oosm.push(time_us, _state, _state_covariance, EmptyInput{});
+}
+
+void KF_orientation::resetHistory()
+{
+ _oosm.reset();
+}
+
+FusionResult KF_orientation::fuseScalarAtTime(const ScalarMeas &meas, uint64_t now_us, float nis_threshold)
+{
+ return _oosm.fuse(*this, meas, now_us, nis_threshold, _state, _state_covariance, EmptyInput{});
+}
+
+void KF_orientation::applyCorrection(VectorState &state,
+ SquareMatrixState &cov,
+ const VectorState &K,
+ float innov, float S)
+{
+ state = state + K * innov;
+ state(KF_orientation::kYaw) = matrix::wrap_pi(state(KF_orientation::kYaw));
+
+ for (size_t row = 0; row < KF_orientation::kSize; row++) {
+ for (size_t col = 0; col < KF_orientation::kSize; col++) {
+ cov(row, col) -= K(row) * K(col) * S;
+ }
+ }
+
+ for (size_t i = 0; i < KF_orientation::kSize; i++) {
+ cov(i, i) = fmaxf(cov(i, i), kMinVar);
+ }
+}
+
+} // namespace vision_target_estimator
diff --git a/src/modules/vision_target_estimator/Orientation/KF_orientation.h b/src/modules/vision_target_estimator/Orientation/KF_orientation.h
new file mode 100644
index 000000000000..80dec47fb0f5
--- /dev/null
+++ b/src/modules/vision_target_estimator/Orientation/KF_orientation.h
@@ -0,0 +1,133 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2025 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file KF_orientation.h
+ * @brief Filter to estimate the orientation of static and moving targets. State: [yaw, yaw_rate]
+ *
+ * @author Jonas Perolini
+ *
+ */
+
+#include
+
+#include
+
+#pragma once
+
+#include "../VTEOosm.h"
+#include "../common.h"
+
+namespace vision_target_estimator
+{
+
+class KF_orientation
+{
+public:
+ enum State : uint8_t {
+ kYaw = 0,
+ kYawRate,
+ kSize
+ };
+ using VectorState = matrix::Vector;
+ using SquareMatrixState = matrix::SquareMatrix;
+
+ friend class OOSMManager;
+
+ KF_orientation() = default;
+ ~KF_orientation() = default;
+ static constexpr float kMinVar = 1e-9f;
+
+ struct ScalarMeas {
+ uint64_t time_us;
+ float val;
+ float unc;
+ VectorState H;
+ };
+
+ void predict(float dt);
+
+ // Primary Fusion Interface (history-consistent OOSM)
+ // Projected correction OOSM approximation:
+ // - Most accurate with bounded delays and when fusions are processed in timestamp order (oldest -> newest).
+ // - Does not replay intermediate measurement updates (unlike EKF2's delayed fusion horizon).
+ // Returns FusionResult describing the result for logging.
+ FusionResult fuseScalarAtTime(const ScalarMeas &meas, uint64_t now_us, float nis_threshold);
+
+ // History Management
+ void pushHistory(const uint64_t time_us);
+ void resetHistory();
+
+ // State
+ void setState(const VectorState &state) { _state = state; }
+ void setStateCovarianceDiag(const VectorState &var)
+ {
+ const SquareMatrixState var_mat = diag(var);
+ _state_covariance = var_mat;
+ };
+
+ const VectorState &getState() const { return _state; }
+ const SquareMatrixState &getStateCovariance() const { return _state_covariance; }
+ VectorState getStateCovarianceDiag() const { return _state_covariance.diag(); }
+
+ void setYawAccVar(float var) { _yaw_acc_var = var; }
+
+private:
+ SquareMatrixState getTransitionMatrix(float dt) const;
+
+ void predictState(float dt, const EmptyInput &input,
+ const VectorState &x_in,
+ const SquareMatrixState &P_in,
+ VectorState &x_out,
+ SquareMatrixState &P_out);
+
+ void computeInnovation(const VectorState &state,
+ const SquareMatrixState &cov, const ScalarMeas &meas,
+ float &innov, float &innov_var) const;
+
+ void applyCorrection(VectorState &state,
+ SquareMatrixState &cov,
+ const VectorState &K,
+ float innov, float S);
+
+ VectorState _state{};
+ SquareMatrixState _state_covariance{};
+
+ float _yaw_acc_var{0.f};
+
+ // OOSM history buffer:
+ // 0.5 s window @ 50 Hz predict rate = 25 samples.
+ // Note that the 0.5 s window is enforced with kOosmMaxTimeUs = 500_ms.
+ OOSMManager _oosm;
+};
+} // namespace vision_target_estimator
diff --git a/src/modules/vision_target_estimator/Orientation/VTEOrientation.cpp b/src/modules/vision_target_estimator/Orientation/VTEOrientation.cpp
new file mode 100644
index 000000000000..c5df6bd51745
--- /dev/null
+++ b/src/modules/vision_target_estimator/Orientation/VTEOrientation.cpp
@@ -0,0 +1,314 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2025 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file VTEOrientation.cpp
+ * @brief Estimate the orientation of a target by processing and fusing sensor
+ * data in a Kalman Filter.
+ *
+ * @author Jonas Perolini
+ *
+ */
+
+#include
+
+#include "VTEOrientation.h"
+
+
+namespace vision_target_estimator
+{
+
+using namespace matrix;
+using OrientationStateVec = KF_orientation::VectorState;
+
+VTEOrientation::VTEOrientation() : ModuleParams(nullptr)
+{
+ _vte_aid_ev_yaw_pub.advertise();
+ _target_orientation_pub.advertise();
+
+ updateParams();
+}
+
+VTEOrientation::~VTEOrientation() = default;
+
+bool VTEOrientation::init()
+{
+ _target_est_yaw.resetHistory();
+ return true;
+}
+
+void VTEOrientation::resetFilter()
+{
+ _estimator_initialized = false;
+ _target_est_yaw.resetHistory();
+}
+
+void VTEOrientation::update()
+{
+ if (_parameter_update_sub.updated()) {
+ updateParams();
+ }
+
+ checkMeasurementInputs();
+
+ // Predict the target yaw state using the configured kinematic model.
+ if (_estimator_initialized) {
+ const hrt_abstime now_us = hrt_absolute_time();
+
+ if (now_us < _last_predict) {
+ resetFilter();
+ PX4_WARN("VTE orientation invalid _last_predict time");
+ return;
+ }
+
+ const uint64_t dt_us = now_us - _last_predict;
+
+ // Guard against long gaps; this estimator is expected to run near 50 Hz.
+ static constexpr uint64_t kMaxValidDeltaTimeUs = 100_ms;
+
+ if (dt_us > kMaxValidDeltaTimeUs) {
+ resetFilter();
+ PX4_WARN("VTE orientation stale, resetting filter");
+ return;
+ }
+
+ predictionStep(dt_us * kMicrosecondsToSeconds);
+ _last_predict = now_us;
+ }
+
+ // Update with the latest observations and publish any resulting innovations.
+ if (performUpdateStep()) {
+ _last_update = _last_predict;
+ }
+
+ if (_estimator_initialized) {
+ // Store the post-fusion posterior snapshot at the current timestamp for OOSM fusion.
+ _target_est_yaw.pushHistory(_last_predict);
+ publishTarget();
+ }
+}
+
+bool VTEOrientation::initEstimator(const ObsValidMaskU &fusion_mask,
+ const TargetObs observations[kObsTypeCount])
+{
+ // Until a sensor measures the yaw rate, the estimator starts with zero yaw-rate.
+ OrientationStateVec state_init{};
+
+ // Yaw init
+ if (fusion_mask.flags.fuse_vision) {
+ state_init(KF_orientation::kYaw) = observations[obsIndex(ObsType::kFiducialMarker)].meas;
+ }
+
+ OrientationStateVec state_var_init;
+ state_var_init.setAll(fmaxf(_param_vte_yaw_unc_in.get(), kMinVariance));
+
+ _target_est_yaw.setState(state_init);
+ _target_est_yaw.setStateCovarianceDiag(state_var_init);
+ _target_est_yaw.resetHistory();
+
+ PX4_DEBUG("Orientation filter init yaw: %.2f [rad] yaw_rate: %.2f [rad/s]",
+ (double)state_init(KF_orientation::kYaw),
+ (double)state_init(KF_orientation::kYawRate));
+
+ PX4_DEBUG("VTE Orientation Estimator properly initialized.");
+ _estimator_initialized = true;
+ _last_update = hrt_absolute_time();
+ _last_predict = _last_update;
+
+ return true;
+}
+
+void VTEOrientation::predictionStep(float dt)
+{
+ _target_est_yaw.setYawAccVar(fmaxf(_param_vte_yaw_acc_unc.get(), kMinVariance));
+ _target_est_yaw.predict(dt);
+}
+
+bool VTEOrientation::performUpdateStep()
+{
+ ObsValidMaskU fusion_mask{};
+ resetObservations();
+ // Gather the latest observations from each enabled data source.
+ processObservations(fusion_mask, _obs_buffer);
+
+ if (fusion_mask.value == 0) {
+ return false;
+ }
+
+ if (!_estimator_initialized && !initEstimator(fusion_mask, _obs_buffer)) {
+ return false;
+ }
+
+ return fuseActiveMeasurements(fusion_mask, _obs_buffer);
+}
+
+void VTEOrientation::resetObservations()
+{
+ for (auto &obs : _obs_buffer) {
+ obs = {};
+ }
+}
+
+void VTEOrientation::processObservations(ObsValidMaskU &fusion_mask,
+ TargetObs observations[kObsTypeCount])
+{
+ fusion_mask.flags.fuse_vision =
+ _vte_aid_mask.flags.use_vision_pos &&
+ processObsVision(observations[obsIndex(ObsType::kFiducialMarker)]);
+}
+
+bool VTEOrientation::isVisionDataValid(const fiducial_marker_yaw_report_s &fiducial_marker_yaw) const
+{
+ if (!isMeasRecent(fiducial_marker_yaw.timestamp_sample)) {
+ PX4_DEBUG("Vision yaw is outdated!"); // TODO: throttle, similar to _vision_pos_warn_last
+ return false;
+ }
+
+ if (!PX4_ISFINITE(fiducial_marker_yaw.yaw_ned)) {
+ PX4_DEBUG("Vision yaw is corrupt!"); // TODO: throttle, similar to _vision_pos_warn_last
+ return false;
+ }
+
+ if (!PX4_ISFINITE(fiducial_marker_yaw.yaw_var_ned)) {
+ PX4_DEBUG("Vision yaw var is corrupt!"); // TODO: throttle, similar to _vision_pos_warn_last
+ return false;
+ }
+
+ return true;
+}
+
+bool VTEOrientation::processObsVision(TargetObs &obs)
+{
+ fiducial_marker_yaw_report_s fiducial_marker_yaw;
+
+ if (!_fiducial_marker_yaw_report_sub.update(&fiducial_marker_yaw) ||
+ !isVisionDataValid(fiducial_marker_yaw)) {
+ return false;
+ }
+
+ const float min_ev_angle_var = sq(fmaxf(_param_vte_ev_angle_noise.get(), kMinObservationNoise));
+ const float yaw_unc = fmaxf(fiducial_marker_yaw.yaw_var_ned, min_ev_angle_var);
+
+ obs.timestamp = fiducial_marker_yaw.timestamp_sample;
+ obs.updated = true;
+ obs.meas = wrap_pi(fiducial_marker_yaw.yaw_ned);
+ obs.meas_unc = yaw_unc;
+ obs.meas_h_theta(KF_orientation::kYaw) = 1.f;
+ obs.type = ObsType::kFiducialMarker;
+
+ return true;
+}
+
+bool VTEOrientation::fuseActiveMeasurements(ObsValidMaskU &fusion_mask,
+ const TargetObs observations[kObsTypeCount])
+{
+ bool meas_fused = false;
+
+ // Fuse vision position
+ if (fusion_mask.flags.fuse_vision) {
+ meas_fused |= fuseMeas(observations[obsIndex(ObsType::kFiducialMarker)]);
+ }
+
+ return meas_fused;
+}
+
+bool VTEOrientation::fuseMeas(const TargetObs &target_obs)
+{
+ vte_aid_source1d_s target_innov = {};
+ const float nis_threshold = fmaxf(_param_vte_yaw_nis_thre.get(), kMinNisThreshold);
+
+ target_innov.timestamp_sample = target_obs.timestamp;
+ target_innov.timestamp = hrt_absolute_time();
+ target_innov.time_last_predict = _last_predict;
+ target_innov.time_since_meas_ms = static_cast(signedTimeDiffUs(
+ _last_predict, target_obs.timestamp)) * kMicrosecondsToMilliseconds;
+
+ if (!target_obs.updated) {
+ target_innov.fusion_status = static_cast(FusionStatus::kIdle);
+ _vte_aid_ev_yaw_pub.publish(target_innov);
+ return false;
+ }
+
+ const KF_orientation::ScalarMeas meas_input{
+ target_obs.timestamp, target_obs.meas, target_obs.meas_unc,
+ target_obs.meas_h_theta};
+
+ target_innov.observation = meas_input.val;
+ target_innov.observation_variance = meas_input.unc;
+
+ const FusionResult result = _target_est_yaw.fuseScalarAtTime(meas_input, _last_predict, nis_threshold);
+
+ target_innov.innovation = result.innov;
+ target_innov.innovation_variance = result.innov_var;
+ target_innov.test_ratio = result.test_ratio;
+ target_innov.fusion_status = static_cast(result.status);
+ target_innov.history_steps = result.history_steps;
+
+ _vte_aid_ev_yaw_pub.publish(target_innov);
+
+ return (result.status == FusionStatus::kFusedCurrent || result.status == FusionStatus::kFusedOosm);
+}
+
+void VTEOrientation::publishTarget()
+{
+ vision_target_est_orientation_s &vision_target_orientation = _orientation_msg;
+ vision_target_orientation = {};
+
+ const OrientationStateVec &state = _target_est_yaw.getState();
+ const OrientationStateVec state_var = _target_est_yaw.getStateCovarianceDiag();
+
+ vision_target_orientation.timestamp = _last_predict;
+ vision_target_orientation.orientation_valid = !hasTimedOut(_last_update, _target_valid_timeout_us);
+
+ vision_target_orientation.yaw = state(KF_orientation::kYaw);
+ vision_target_orientation.cov_yaw = state_var(KF_orientation::kYaw);
+
+ vision_target_orientation.yaw_rate = state(KF_orientation::kYawRate);
+ vision_target_orientation.cov_yaw_rate = state_var(KF_orientation::kYawRate);
+
+ _target_orientation_pub.publish(vision_target_orientation);
+}
+
+void VTEOrientation::checkMeasurementInputs()
+{
+}
+
+void VTEOrientation::updateParams()
+{
+ parameter_update_s pupdate;
+ _parameter_update_sub.copy(&pupdate);
+
+ ModuleParams::updateParams();
+}
+
+} // namespace vision_target_estimator
diff --git a/src/modules/vision_target_estimator/Orientation/VTEOrientation.h b/src/modules/vision_target_estimator/Orientation/VTEOrientation.h
new file mode 100644
index 000000000000..d1ed18aab3a9
--- /dev/null
+++ b/src/modules/vision_target_estimator/Orientation/VTEOrientation.h
@@ -0,0 +1,205 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2025 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file VTEOrientation.h
+ * @brief Estimate the orientation of a target by processing and fusing sensor
+ * data in a Kalman Filter.
+ *
+ * @author Jonas Perolini
+ *
+ */
+
+#pragma once
+
+#include
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include
+
+#include "../common.h"
+#include "KF_orientation.h"
+
+namespace vision_target_estimator
+{
+
+using namespace time_literals;
+
+class VTEOrientation : public ModuleParams
+{
+public:
+ VTEOrientation();
+ virtual ~VTEOrientation();
+
+ /*
+ * Get new measurements and update the state estimate
+ */
+ void update();
+
+ bool init();
+
+ void resetFilter();
+
+ void setVteTimeout(uint32_t tout) { _vte_timeout_us = tout; }
+ void setTargetValidTimeout(uint32_t tout)
+ {
+ _target_valid_timeout_us = tout;
+ }
+ void setMeasRecentTimeout(uint32_t tout)
+ {
+ _meas_recent_timeout_us = tout;
+ }
+ void setMeasUpdatedTimeout(uint32_t tout)
+ {
+ _meas_updated_timeout_us = tout;
+ }
+ void setVteAidMask(uint16_t mask_value)
+ {
+ _vte_aid_mask.value = mask_value;
+ }
+
+ bool timedOut() const
+ {
+ return _estimator_initialized && hasTimedOut(_last_update, _vte_timeout_us);
+ }
+ bool fusionEnabled() const
+ {
+ return _vte_aid_mask.flags.use_vision_pos;
+ } // only vision yaw measurements are handled
+
+protected:
+ /*
+ * update parameters.
+ */
+ void updateParams() override;
+
+ uORB::Publication _target_orientation_pub{ORB_ID(vision_target_est_orientation)};
+
+ // publish innovations target_estimator_gps_pos
+ uORB::Publication _vte_aid_ev_yaw_pub{ORB_ID(vte_aid_ev_yaw)};
+
+ uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
+
+private:
+ enum class ObsType : uint8_t { kFiducialMarker, kTypeCount };
+
+ static constexpr size_t kObsTypeCount = static_cast(ObsType::kTypeCount);
+ static constexpr size_t obsIndex(ObsType type)
+ {
+ return static_cast(type);
+ }
+
+ union ObsValidMaskU {
+ struct {
+ uint8_t fuse_vision : 1; ///< bit0: external vision data ready to be fused
+ uint8_t reserved : 7; ///< bit1..7: reserved for future use
+ } flags;
+
+ uint8_t value{0};
+ };
+
+ static_assert(sizeof(ObsValidMaskU) == 1, "Unexpected masking size");
+
+ struct TargetObs {
+ ObsType type;
+ hrt_abstime timestamp = 0;
+ bool updated = false;
+ float meas{};
+ float meas_unc{};
+ KF_orientation::VectorState meas_h_theta{};
+ };
+
+ bool initEstimator(const ObsValidMaskU &fusion_mask, const TargetObs observations[kObsTypeCount]);
+ bool performUpdateStep();
+ void predictionStep(float dt);
+
+ inline bool isMeasRecent(hrt_abstime ts) const
+ {
+ return !hasTimedOut(ts, _meas_recent_timeout_us);
+ }
+
+ inline bool isMeasUpdated(hrt_abstime ts) const
+ {
+ return !hasTimedOut(ts, _meas_updated_timeout_us);
+ }
+
+ void processObservations(ObsValidMaskU &fusion_mask,
+ TargetObs observations[kObsTypeCount]);
+ bool fuseActiveMeasurements(ObsValidMaskU &fusion_mask, const TargetObs observations[kObsTypeCount]);
+
+ /* Vision data */
+ bool isVisionDataValid(const fiducial_marker_yaw_report_s &fiducial_marker_yaw) const;
+ bool processObsVision(TargetObs &obs);
+
+ bool fuseMeas(const TargetObs &target_pos_obs);
+ void publishTarget();
+ void resetObservations();
+
+ uORB::Subscription _fiducial_marker_yaw_report_sub{ORB_ID(fiducial_marker_yaw_report)};
+
+ bool _estimator_initialized{false};
+
+ KF_orientation _target_est_yaw{};
+ TargetObs _obs_buffer[kObsTypeCount] {};
+ vision_target_est_orientation_s _orientation_msg{};
+
+ hrt_abstime _last_predict{0}; // timestamp of last filter prediction
+ hrt_abstime _last_update{0}; // timestamp of last filter update (used to check timeout)
+
+ void checkMeasurementInputs();
+
+ /* parameters from vision_target_estimator_params.yaml */
+ uint32_t _vte_timeout_us{3_s};
+ uint32_t _target_valid_timeout_us{2_s};
+ uint32_t _meas_recent_timeout_us{1_s};
+ uint32_t _meas_updated_timeout_us{100_ms};
+ SensorFusionMaskU _vte_aid_mask{};
+
+ DEFINE_PARAMETERS(
+ (ParamFloat) _param_vte_yaw_unc_in,
+ (ParamFloat) _param_vte_yaw_acc_unc,
+ (ParamInt) _param_vte_yaw_en,
+ (ParamFloat) _param_vte_ev_angle_noise,
+ (ParamFloat) _param_vte_yaw_nis_thre
+ )
+};
+} // namespace vision_target_estimator
diff --git a/src/modules/vision_target_estimator/Orientation/vtest_derivation/derivation.py b/src/modules/vision_target_estimator/Orientation/vtest_derivation/derivation.py
new file mode 100644
index 000000000000..897847093c3c
--- /dev/null
+++ b/src/modules/vision_target_estimator/Orientation/vtest_derivation/derivation.py
@@ -0,0 +1,174 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+ Copyright (c) 2022-2026 PX4 Development Team
+ Redistribution and use in source and binary forms, with or without
+ modification, are permitted provided that the following conditions
+ are met:
+
+ 1. Redistributions of source code must retain the above copyright
+ notice, this list of conditions and the following disclaimer.
+ 2. Redistributions in binary form must reproduce the above copyright
+ notice, this list of conditions and the following disclaimer in
+ the documentation and/or other materials provided with the
+ distribution.
+ 3. Neither the name PX4 nor the names of its contributors may be
+ used to endorse or promote products derived from this software
+ without specific prior written permission.
+
+ THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ POSSIBILITY OF SUCH DAMAGE.
+
+File: derivation.py
+Description:
+ Orientation state and covariance prediction derivation.
+"""
+
+import re
+
+import symforce
+symforce.set_epsilon_to_symbol()
+
+import symforce.symbolic as sf
+from symforce.values import Values
+import sympy as sp
+
+
+def generate_px4_function(function_name, output_names):
+ from symforce.codegen import Codegen, CppConfig
+ import os
+
+ return_key = output_names[0] if output_names is not None and len(output_names) == 1 else None
+
+ codegen = Codegen.function(
+ function_name,
+ output_names=output_names,
+ return_key=return_key,
+ config=CppConfig(zero_initialization_sparsity_threshold=1),
+ )
+
+ metadata = codegen.generate_function(
+ output_dir="generated",
+ skip_directory_nesting=True,
+ )
+
+ for generated_file in metadata.generated_files:
+ print(f" |- {os.path.relpath(generated_file, metadata.output_dir)}")
+
+ generated_path = os.path.abspath(metadata.generated_files[0])
+
+ with open(generated_path) as generated_file:
+ content = generated_file.read()
+
+ content = content.replace("std::max", "math::max")
+ content = content.replace("std::min", "math::min")
+ content = content.replace("Eigen", "matrix")
+ content = content.replace("matrix/Dense", "matrix/math.hpp")
+
+ # Avoid reserved naming pattern: underscore + uppercase identifier.
+ content = re.sub(r'_([A-Z])', lambda match: '_' + match.group(1).lower(), content)
+
+ with open(generated_path, "w") as generated_file:
+ generated_file.write(content)
+
+
+class MState(sf.Matrix):
+ SHAPE = (2, 2)
+
+
+class VState(sf.Matrix):
+ SHAPE = (2, 1)
+
+
+State = Values(
+ yaw=sf.V1(),
+ yaw_rate=sf.V1(),
+)
+
+
+def symbolic_state(state_structure):
+ symbolic = Values()
+ for key in state_structure.keys_recursive():
+ var_type = type(state_structure[key])
+ if hasattr(var_type, "symbolic"):
+ symbolic[key] = var_type.symbolic(key)
+ return symbolic
+
+
+def matrix_to_state(state_vector: VState) -> Values:
+ state_values = Values()
+ for idx, key in enumerate(State.keys_recursive()):
+ state_values[key] = state_vector[idx]
+ return state_values
+
+
+def state_to_matrix(state_values: Values) -> VState:
+ return VState(state_values.to_storage())
+
+
+def predict_state_with_input(dt: sf.Scalar, state_values: Values, yaw_acc: sf.Scalar) -> Values:
+ state_pred = state_values.copy()
+ state_pred["yaw"] = state_values["yaw"] + dt * state_values["yaw_rate"] + dt**2 * yaw_acc / 2
+ state_pred["yaw_rate"] = state_values["yaw_rate"] + dt * yaw_acc
+ return state_pred
+
+
+def get_Phi_and_G(dt: sf.Scalar):
+ state = symbolic_state(State)
+ yaw_acc = sf.Symbol("yaw_acc")
+ state_pred_matrix = state_to_matrix(predict_state_with_input(dt, state, yaw_acc))
+
+ # Compute Phi = ∂f/∂x
+ Phi = state_pred_matrix.jacobian([state])
+ # Compute G = ∂f/∂w
+ G = state_pred_matrix.jacobian(yaw_acc)
+ return Phi, G
+
+
+def predictState(dt: sf.Scalar, state: VState) -> VState:
+ state_values = matrix_to_state(state)
+ return state_to_matrix(predict_state_with_input(dt, state_values, sf.S(0)))
+
+
+def getTransitionMatrix(dt: sf.Scalar) -> MState:
+ Phi, _ = get_Phi_and_G(dt)
+ return Phi
+
+
+def derive_discrete_process_noise(dt: sf.Scalar, yaw_acc_var: sf.Scalar) -> MState:
+ tau = sf.Symbol("tau")
+ _, G_tau = get_Phi_and_G(tau)
+
+ # Continuous-time white yaw acceleration enters through d/dtau of the discrete G(tau).
+ G_continuous = G_tau.jacobian(tau)
+
+ Q = sf.Matrix.zeros(State.storage_dim(), State.storage_dim())
+
+ for row in range(State.storage_dim()):
+ for col in range(State.storage_dim()):
+ integrand = G_continuous[row, 0] * G_continuous[col, 0] * yaw_acc_var
+ Q[row, col] = sp.integrate(integrand, (tau, 0, dt))
+
+ return Q
+
+
+def predictCov(dt: sf.Scalar, yaw_acc_var: sf.Scalar, covariance: MState) -> MState:
+ Phi, _ = get_Phi_and_G(dt)
+ Q = derive_discrete_process_noise(dt, yaw_acc_var)
+ return Phi * covariance * Phi.T + Q
+
+
+print("Derive VTEST orientation equations...")
+generate_px4_function(predictState, output_names=["predict_state"])
+generate_px4_function(getTransitionMatrix, output_names=["transition_matrix"])
+generate_px4_function(predictCov, output_names=["cov_updated"])
diff --git a/src/modules/vision_target_estimator/Orientation/vtest_derivation/generated/getTransitionMatrix.h b/src/modules/vision_target_estimator/Orientation/vtest_derivation/generated/getTransitionMatrix.h
new file mode 100644
index 000000000000..a9c52066de5f
--- /dev/null
+++ b/src/modules/vision_target_estimator/Orientation/vtest_derivation/generated/getTransitionMatrix.h
@@ -0,0 +1,47 @@
+// -----------------------------------------------------------------------------
+// This file was autogenerated by symforce from template:
+// function/FUNCTION.h.jinja
+// Do NOT modify by hand.
+// -----------------------------------------------------------------------------
+
+#pragma once
+
+#include
+
+namespace sym
+{
+
+/**
+ * This function was autogenerated from a symbolic function. Do not modify by hand.
+ *
+ * Symbolic function: getTransitionMatrix
+ *
+ * Args:
+ * dt: Scalar
+ *
+ * Outputs:
+ * transition_matrix: Matrix22
+ */
+template
+matrix::Matrix Gettransitionmatrix(const Scalar dt)
+{
+ // Total ops: 0
+
+ // Input arrays
+
+ // Intermediate terms (0)
+
+ // Output terms (1)
+ matrix::Matrix _transition_matrix;
+
+ _transition_matrix.setZero();
+
+ _transition_matrix(0, 0) = 1;
+ _transition_matrix(0, 1) = dt;
+ _transition_matrix(1, 1) = 1;
+
+ return _transition_matrix;
+} // NOLINT(readability/fn_size)
+
+// NOLINTNEXTLINE(readability/fn_size)
+} // namespace sym
diff --git a/src/modules/vision_target_estimator/Orientation/vtest_derivation/generated/predictCov.h b/src/modules/vision_target_estimator/Orientation/vtest_derivation/generated/predictCov.h
new file mode 100644
index 000000000000..d0dc011759fd
--- /dev/null
+++ b/src/modules/vision_target_estimator/Orientation/vtest_derivation/generated/predictCov.h
@@ -0,0 +1,56 @@
+// -----------------------------------------------------------------------------
+// This file was autogenerated by symforce from template:
+// function/FUNCTION.h.jinja
+// Do NOT modify by hand.
+// -----------------------------------------------------------------------------
+
+#pragma once
+
+#include
+
+namespace sym
+{
+
+/**
+ * This function was autogenerated from a symbolic function. Do not modify by hand.
+ *
+ * Symbolic function: predictCov
+ *
+ * Args:
+ * dt: Scalar
+ * yaw_acc_var: Scalar
+ * covariance: Matrix22
+ *
+ * Outputs:
+ * cov_updated: Matrix22
+ */
+template
+matrix::Matrix Predictcov(const Scalar dt, const Scalar yaw_acc_var,
+ const matrix::Matrix &covariance)
+{
+ // Total ops: 18
+
+ // Input arrays
+
+ // Intermediate terms (3)
+ const Scalar _tmp0 = covariance(1, 1) * dt;
+ const Scalar _tmp1 = _tmp0 + covariance(0, 1);
+ const Scalar _tmp2 = (Scalar(1) / Scalar(2)) * std::pow(dt, Scalar(2)) * yaw_acc_var;
+
+ // Output terms (1)
+ matrix::Matrix _cov_updated;
+
+ _cov_updated(0, 0) =
+ _tmp1 * dt + covariance(0, 0) + covariance(1, 0) * dt + (Scalar(1) / Scalar(3)) * [&]() {
+ const Scalar base = dt;
+ return base * base * base;
+ }() * yaw_acc_var;
+ _cov_updated(1, 0) = _tmp0 + _tmp2 + covariance(1, 0);
+ _cov_updated(0, 1) = _tmp1 + _tmp2;
+ _cov_updated(1, 1) = covariance(1, 1) + dt * yaw_acc_var;
+
+ return _cov_updated;
+} // NOLINT(readability/fn_size)
+
+// NOLINTNEXTLINE(readability/fn_size)
+} // namespace sym
diff --git a/src/modules/vision_target_estimator/Orientation/vtest_derivation/generated/predictState.h b/src/modules/vision_target_estimator/Orientation/vtest_derivation/generated/predictState.h
new file mode 100644
index 000000000000..6be511e4157d
--- /dev/null
+++ b/src/modules/vision_target_estimator/Orientation/vtest_derivation/generated/predictState.h
@@ -0,0 +1,46 @@
+// -----------------------------------------------------------------------------
+// This file was autogenerated by symforce from template:
+// function/FUNCTION.h.jinja
+// Do NOT modify by hand.
+// -----------------------------------------------------------------------------
+
+#pragma once
+
+#include
+
+namespace sym
+{
+
+/**
+ * This function was autogenerated from a symbolic function. Do not modify by hand.
+ *
+ * Symbolic function: predictState
+ *
+ * Args:
+ * dt: Scalar
+ * state: Matrix21
+ *
+ * Outputs:
+ * predict_state: Matrix21
+ */
+template
+matrix::Matrix Predictstate(const Scalar dt,
+ const matrix::Matrix &state)
+{
+ // Total ops: 2
+
+ // Input arrays
+
+ // Intermediate terms (0)
+
+ // Output terms (1)
+ matrix::Matrix _predict_state;
+
+ _predict_state(0, 0) = dt * state(1, 0) + state(0, 0);
+ _predict_state(1, 0) = state(1, 0);
+
+ return _predict_state;
+} // NOLINT(readability/fn_size)
+
+// NOLINTNEXTLINE(readability/fn_size)
+} // namespace sym
diff --git a/src/modules/vision_target_estimator/Position/KF_position.cpp b/src/modules/vision_target_estimator/Position/KF_position.cpp
new file mode 100644
index 000000000000..957d0685bcea
--- /dev/null
+++ b/src/modules/vision_target_estimator/Position/KF_position.cpp
@@ -0,0 +1,135 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2025 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file KF_position.cpp
+ * @brief Filter to estimate the pose of moving targets. State: [pos_rel, vel_uav, bias, acc_target, vel_target]
+ *
+ * @author Jonas Perolini
+ *
+ */
+
+#include "KF_position.h"
+
+#include
+
+#include
+#include
+#include
+#include
+#include
+#include
+
+namespace vision_target_estimator
+{
+
+using VectorState = KF_position::VectorState;
+using SquareMatrixState = KF_position::SquareMatrixState;
+
+void KF_position::predict(float dt, float acc_uav)
+{
+
+ if (!PX4_ISFINITE(dt) || (dt < 0.f) || !PX4_ISFINITE(acc_uav)) {
+ return;
+ }
+
+ _last_acc = acc_uav;
+
+ VectorState state_next;
+ SquareMatrixState cov_next;
+
+ predictState(dt, acc_uav, _state, _state_covariance, state_next, cov_next);
+
+ _state = state_next;
+ _state_covariance = cov_next;
+}
+
+SquareMatrixState KF_position::getTransitionMatrix(float dt) const
+{
+ return sym::Gettransitionmatrix(dt);
+}
+
+void KF_position::predictState(float dt, float acc,
+ const VectorState &x_in,
+ const SquareMatrixState &P_in,
+ VectorState &x_out,
+ SquareMatrixState &P_out)
+{
+ x_out = sym::Predictstate(dt, x_in, acc);
+ P_out = sym::Predictcov(dt, _input_var, _bias_var, _acc_var, P_in);
+}
+
+void KF_position::computeInnovation(const VectorState &state,
+ const SquareMatrixState &cov,
+ const ScalarMeas &meas,
+ float &innov, float &innov_var) const
+{
+ innov = meas.val - (meas.H.transpose() * state)(0, 0);
+ innov_var = sym::Computeinnovcov(meas.unc, cov, meas.H.transpose());
+}
+
+void KF_position::pushHistory(const uint64_t time_us)
+{
+ _oosm.push(time_us, _state, _state_covariance, _last_acc);
+}
+
+void KF_position::resetHistory()
+{
+ _oosm.reset();
+}
+
+FusionResult KF_position::fuseScalarAtTime(const ScalarMeas &meas, uint64_t now_us, float nis_threshold)
+{
+ return _oosm.fuse(*this, meas, now_us, nis_threshold, _state, _state_covariance, _last_acc);
+}
+
+void KF_position::applyCorrection(VectorState &state,
+ SquareMatrixState &cov,
+ const VectorState &K,
+ float innov, float S)
+{
+ VectorState state_new;
+ SquareMatrixState cov_new;
+
+ sym::Applycorrection(state, cov, K, innov, S, &state_new, &cov_new);
+
+ state = state_new;
+ cov = cov_new;
+
+ // Clamp diagonal assuming a small epsilon for stability:
+ for (size_t i = 0; i < kStateSize; i++) {
+ cov(i, i) = fmaxf(cov(i, i), kMinVar);
+ }
+}
+
+} // namespace vision_target_estimator
diff --git a/src/modules/vision_target_estimator/Position/KF_position.h b/src/modules/vision_target_estimator/Position/KF_position.h
new file mode 100644
index 000000000000..951eaeaac74c
--- /dev/null
+++ b/src/modules/vision_target_estimator/Position/KF_position.h
@@ -0,0 +1,137 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2025 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file KF_position.h
+ * @brief Per-axis Kalman Filter for VTEST position states.
+ *
+ * @author Jonas Perolini
+ *
+ */
+
+#pragma once
+
+#include
+#include
+
+#include
+#include
+
+#include "../VTEOosm.h"
+#include "../common.h"
+
+namespace vision_target_estimator
+{
+
+class KF_position
+{
+public:
+ static constexpr size_t kStateSize = vtest::State::size;
+ using VectorState = matrix::Vector;
+ using SquareMatrixState = matrix::SquareMatrix;
+
+ friend class OOSMManager;
+
+ KF_position() = default;
+ ~KF_position() = default;
+ static constexpr float kMinVar = 1e-9f;
+
+ struct ScalarMeas {
+ uint64_t time_us;
+ float val;
+ float unc;
+ VectorState H;
+ };
+
+ void predict(float dt, float acc_uav);
+
+ // Primary Fusion Interface (history-consistent OOSM)
+ // Projected correction OOSM approximation:
+ // - Most accurate with bounded delays and when fusions are processed in timestamp order (oldest -> newest).
+ // - Does not replay intermediate measurement updates (unlike EKF2's delayed fusion horizon).
+ // Returns FusionResult describing the result for logging.
+ FusionResult fuseScalarAtTime(const ScalarMeas &meas, uint64_t now_us, float nis_threshold);
+
+ // History Management
+ void pushHistory(const uint64_t time_us);
+ void resetHistory();
+
+ // Setters / Getters
+ void setState(const VectorState &state) { _state = state; }
+ void setStateCovarianceDiag(const VectorState &var)
+ {
+ const SquareMatrixState var_mat = diag(var);
+ _state_covariance = var_mat;
+ };
+
+ const VectorState &getState() const { return _state; }
+ const SquareMatrixState &getStateCovariance() const { return _state_covariance; }
+ VectorState getStateCovarianceDiag() const { return _state_covariance.diag(); }
+
+ void setInputVar(float var) { _input_var = var; }
+ void setBiasVar(float var) { _bias_var = var; }
+ void setTargetAccVar(float var) { _acc_var = var; }
+
+private:
+ SquareMatrixState getTransitionMatrix(float dt) const;
+
+ void predictState(float dt, float acc,
+ const VectorState &x_in,
+ const SquareMatrixState &P_in,
+ VectorState &x_out,
+ SquareMatrixState &P_out);
+
+ void computeInnovation(const VectorState &state,
+ const SquareMatrixState &cov, const ScalarMeas &meas,
+ float &innov, float &innov_var) const;
+
+ void applyCorrection(VectorState &state,
+ SquareMatrixState &cov,
+ const VectorState &K,
+ float innov, float S);
+
+ VectorState _state{};
+ SquareMatrixState _state_covariance{};
+
+ float _bias_var{0.f}; // target/UAV GNSS bias variance
+ float _acc_var{0.f}; // target acceleration variance
+ float _input_var{0.f}; // UAV acceleration variance
+
+ float _last_acc{0.f}; // last UAV acceleration input (also used as OOSM fallback input)
+
+ // OOSM history buffer:
+ // 0.5 s window @ 50 Hz predict rate = 25 samples.
+ // Note that the 0.5 s window is enforced with kOosmMaxTimeUs = 500_ms.
+ OOSMManager _oosm;
+};
+} // namespace vision_target_estimator
diff --git a/src/modules/vision_target_estimator/Position/VTEPosition.cpp b/src/modules/vision_target_estimator/Position/VTEPosition.cpp
new file mode 100644
index 000000000000..4f9c83727492
--- /dev/null
+++ b/src/modules/vision_target_estimator/Position/VTEPosition.cpp
@@ -0,0 +1,1676 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2025 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file VTEPosition.cpp
+ * @brief Estimate the state of a target by processing and fusing sensor data in a Kalman Filter.
+ *
+ * @author Jonas Perolini
+ *
+ */
+
+#include
+
+#include
+#include
+
+#include
+
+#include "VTEPosition.h"
+
+namespace vision_target_estimator
+{
+
+using namespace matrix;
+
+VTEPosition::VTEPosition() :
+ ModuleParams(nullptr)
+{
+ _target_pose_pub.advertise();
+ _target_estimator_state_pub.advertise();
+ _vte_aid_gps_pos_target_pub.advertise();
+ _vte_aid_gps_pos_mission_pub.advertise();
+ _vte_aid_gps_vel_uav_pub.advertise();
+ _vte_aid_gps_vel_target_pub.advertise();
+ _vte_aid_fiducial_marker_pub.advertise();
+ _vte_bias_init_status_pub.advertise();
+
+ updateParams();
+}
+
+VTEPosition::~VTEPosition()
+{
+ perf_free(_vte_predict_perf);
+ perf_free(_vte_update_perf);
+ perf_free(_vte_fusion_perf);
+}
+
+bool VTEPosition::shouldEmitWarning(hrt_abstime &last_warn)
+{
+ const hrt_abstime now = hrt_absolute_time();
+
+ if ((last_warn == 0) || (now < last_warn) || ((now - last_warn) > kWarnThrottleIntervalUs)) {
+ last_warn = now;
+ return true;
+ }
+
+ return false;
+}
+
+bool VTEPosition::init()
+{
+ // Check valid vtest_derivation/generated/state.h
+ if (vtest::Axis::size != kExpectedAxisCount) {
+ PX4_ERR("VTE: Invalid axis size: %d, expected %d. Generate vtest_derivation/derivation.py",
+ static_cast(vtest::Axis::size), kExpectedAxisCount);
+ return false;
+ }
+
+#if defined(CONFIG_VTEST_MOVING)
+ PX4_INFO("VTE for moving target init");
+ PX4_WARN("VTE for moving targets has not been thoroughly tested");
+
+ if (vtest::State::size != kExpectedMovingPositionStateCount) {
+ PX4_ERR("VTE: Invalid state size: %d, expected %d. Generate vtest_derivation/derivation.py",
+ static_cast(vtest::State::size), kExpectedMovingPositionStateCount);
+ return false;
+ }
+
+ _param_mpc_z_v_auto_dn = param_find("MPC_Z_V_AUTO_DN");
+#else
+ PX4_INFO("VTE for static target init");
+
+ if (vtest::State::size != kExpectedStaticPositionStateCount) {
+ PX4_ERR("VTE: Invalid state size: %d, expected %d. Generate vtest_derivation/derivation.py",
+ static_cast(vtest::State::size), kExpectedStaticPositionStateCount);
+ return false;
+ }
+
+#endif // CONFIG_VTEST_MOVING
+
+ for (int axis = 0; axis < vtest::Axis::size; ++axis) {
+ _target_est_pos[axis].resetHistory();
+ }
+
+ return true;
+}
+
+void VTEPosition::resetFilter()
+{
+ _estimator_initialized = false;
+ _last_relative_meas_fused_time = 0;
+ _bias.set = false;
+ _pre_bias_reference = PreBiasReference::kUnknown;
+ resetBiasAveraging();
+ _mission_land_position.valid = false;
+
+ for (int axis = 0; axis < vtest::Axis::size; ++axis) {
+ _target_est_pos[axis].resetHistory();
+ }
+}
+
+void VTEPosition::update(const Vector3f &acc_ned)
+{
+ if (_parameter_update_sub.updated()) {
+ updateParams();
+ }
+
+ checkMeasurementInputs();
+
+ // Predict the target state using a constant relative-acceleration model.
+ if (_estimator_initialized) {
+ const hrt_abstime now_us = hrt_absolute_time();
+
+ if (now_us < _last_predict) {
+ resetFilter();
+ PX4_WARN("VTE position invalid _last_predict time");
+ return;
+ }
+
+ const uint64_t dt_us = now_us - _last_predict;
+
+ // Guard against large dt values for which the constant acceleration assumption does not hold.
+ static constexpr uint64_t kMaxPredictionDeltaTimeUs = 100_ms; // Filter runs at 50 Hz, expect dt = 20 ms.
+
+ if (dt_us > kMaxPredictionDeltaTimeUs) {
+ resetFilter();
+ PX4_WARN("VTE position stale, resetting filter");
+ return;
+ }
+
+ const float dt = dt_us * kMicrosecondsToSeconds;
+ perf_begin(_vte_predict_perf);
+ predictionStep(acc_ned, dt);
+ perf_end(_vte_predict_perf);
+
+ _last_predict = now_us;
+ }
+
+ // Update with the newest observations, fuse if the filter is initialised,
+ // and publish any resulting innovations.
+ if (performUpdateStep(acc_ned)) {
+ _last_update = _last_predict;
+ }
+
+ if (_estimator_initialized) {
+ // Store the post-fusion posterior snapshot at the current timestamp for OOSM fusion.
+ for (int axis = 0; axis < vtest::Axis::size; ++axis) {
+ _target_est_pos[axis].pushHistory(_last_predict);
+ }
+
+ publishTarget();
+ }
+}
+
+bool VTEPosition::initEstimator(const AxisEstimatorStates &state_init)
+{
+ // Get initial variance from params
+ const float state_pos_var = _param_vte_pos_unc_in.get();
+ const float state_vel_var = _param_vte_vel_unc_in.get();
+ const float state_bias_var = _param_vte_bias_unc_in.get();
+
+ EstimatorState state_var_init{};
+ state_var_init(vtest::State::pos_rel) = state_pos_var;
+ state_var_init(vtest::State::vel_uav) = state_vel_var;
+ state_var_init(vtest::State::bias) = state_bias_var;
+
+#if defined(CONFIG_VTEST_MOVING)
+ const float state_acc_var = _param_vte_acc_unc_in.get();
+ const float state_target_vel_var = _param_vte_vel_unc_in.get();
+ state_var_init(vtest::State::acc_target) = state_acc_var;
+ state_var_init(vtest::State::vel_target) = state_target_vel_var;
+
+#endif // CONFIG_VTEST_MOVING
+
+ for (int i = 0; i < vtest::Axis::size; i++) {
+ _target_est_pos[i].setState(state_init[i]);
+ _target_est_pos[i].setStateCovarianceDiag(state_var_init);
+ }
+
+ // Debug INFO
+ PX4_DEBUG("Rel pos init %.2f %.2f %.2f", (double)state_init[vtest::Axis::x](vtest::State::pos_rel),
+ (double)state_init[vtest::Axis::y](vtest::State::pos_rel),
+ (double)state_init[vtest::Axis::z](vtest::State::pos_rel));
+ PX4_DEBUG("Vel uav init %.2f %.2f %.2f", (double)state_init[vtest::Axis::x](vtest::State::vel_uav),
+ (double)state_init[vtest::Axis::y](vtest::State::vel_uav),
+ (double)state_init[vtest::Axis::z](vtest::State::vel_uav));
+ PX4_DEBUG("GNSS bias init %.2f %.2f %.2f", (double)state_init[vtest::Axis::x](vtest::State::bias),
+ (double)state_init[vtest::Axis::y](vtest::State::bias),
+ (double)state_init[vtest::Axis::z](vtest::State::bias));
+
+#if defined(CONFIG_VTEST_MOVING)
+ PX4_DEBUG("Target acc init %.2f %.2f %.2f", (double)state_init[vtest::Axis::x](vtest::State::acc_target),
+ (double)state_init[vtest::Axis::y](vtest::State::acc_target),
+ (double)state_init[vtest::Axis::z](vtest::State::acc_target));
+ PX4_DEBUG("Target vel init %.2f %.2f %.2f", (double)state_init[vtest::Axis::x](vtest::State::vel_target),
+ (double)state_init[vtest::Axis::y](vtest::State::vel_target),
+ (double)state_init[vtest::Axis::z](vtest::State::vel_target));
+#endif // CONFIG_VTEST_MOVING
+
+ return true;
+}
+
+void VTEPosition::predictionStep(const Vector3f &vehicle_acc_ned, const float dt)
+{
+ const float bias_var = fmaxf(_param_vte_bias_unc.get(), kMinVariance);
+ const float uav_acc_var = fmaxf(_param_vte_acc_d_unc.get(), kMinVariance);
+
+ //Decoupled dynamics, we neglect the off diag elements.
+ for (int i = 0; i < vtest::Axis::size; i++) {
+#if defined(CONFIG_VTEST_MOVING)
+ const float target_acc_var = fmaxf(_param_vte_acc_t_unc.get(), kMinVariance);
+ _target_est_pos[i].setTargetAccVar(target_acc_var);
+#endif // CONFIG_VTEST_MOVING
+
+ // We assume an isotropic uncertainty (cov = sigma^2 * I)
+ // The rotated input covariance from body to NED R*(sigma^2 * I)*R^T = sigma^2*I because R*R^T = I.
+ _target_est_pos[i].setBiasVar(bias_var);
+ _target_est_pos[i].setInputVar(uav_acc_var);
+
+ _target_est_pos[i].predict(dt, vehicle_acc_ned(i));
+ }
+}
+
+bool VTEPosition::performUpdateStep(const Vector3f &vehicle_acc_ned)
+{
+ // Gather observations and tag the fusion mask bits for each new valid source.
+ resetObservations();
+ ObsValidMaskU fusion_mask{};
+ processObservations(fusion_mask, _obs_buffer);
+
+ if (fusion_mask.value == 0) {
+ return false;
+ }
+
+ if (!_estimator_initialized) {
+ if (!hasNewPositionSensorData(fusion_mask)) {
+ return false;
+ }
+
+ if (!initializeEstimator(fusion_mask, _obs_buffer)) {
+ return false;
+ }
+ }
+
+ if (!_bias.set) {
+ updateBiasIfObservable(fusion_mask, _obs_buffer);
+ }
+
+ return fuseActiveMeasurements(vehicle_acc_ned, fusion_mask, _obs_buffer);
+}
+
+void VTEPosition::resetObservations()
+{
+ for (auto &obs : _obs_buffer) {
+ obs = {};
+ }
+}
+
+void VTEPosition::processObservations(ObsValidMaskU &fusion_mask,
+ TargetObs observations[kObsTypeCount])
+{
+ if (_vte_aid_mask.flags.use_vision_pos) {
+ fusion_mask.flags.fuse_vision = processObsVision(observations[obsIndex(ObsType::kFiducialMarker)]);
+ }
+
+ if (updateUavGpsData()) {
+ if (_vte_aid_mask.flags.use_mission_pos && _mission_land_position.valid) {
+ fusion_mask.flags.fuse_mission_pos = processObsGNSSPosMission(observations[obsIndex(ObsType::kMissionGpsPos)]);
+ }
+
+ if (_vte_aid_mask.flags.use_uav_gps_vel && _uav_gps_vel.valid) {
+ fusion_mask.flags.fuse_uav_gps_vel = processObsGNSSVelUav(observations[obsIndex(ObsType::kUavGpsVel)]);
+ }
+ }
+
+ target_gnss_s target_gnss{};
+
+ if (_target_gnss_sub.update(&target_gnss)) {
+ const bool target_gps_position_valid = isTargetGpsPositionValid(target_gnss);
+
+ if (_vte_aid_mask.flags.use_target_gps_pos && _uav_gps_position.valid
+ && target_gps_position_valid && target_gnss.abs_pos_updated) {
+ fusion_mask.flags.fuse_target_gps_pos = processObsGNSSPosTarget(target_gnss,
+ observations[obsIndex(ObsType::kTargetGpsPos)]);
+ }
+
+#if defined(CONFIG_VTEST_MOVING)
+
+ if (isTargetGpsVelocityValid(target_gnss)) {
+
+ updateTargetGpsVelocity(target_gnss);
+
+ if (_vte_aid_mask.flags.use_target_gps_vel && _target_gps_vel.valid) {
+ fusion_mask.flags.fuse_target_gps_vel = processObsGNSSVelTarget(target_gnss,
+ observations[obsIndex(ObsType::kTargetGpsVel)]);
+ }
+ }
+
+#endif // CONFIG_VTEST_MOVING
+
+ }
+}
+
+bool VTEPosition::isVisionDataValid(const fiducial_marker_pos_report_s &fiducial_marker_pose)
+{
+ if (!isMeasRecent(fiducial_marker_pose.timestamp_sample)) {
+ if (shouldEmitWarning(_vision_pos_warn_last)) {
+ PX4_WARN("VTE: Vision data too old!");
+ }
+
+ return false;
+ }
+
+ const bool finite_measurement = PX4_ISFINITE(fiducial_marker_pose.rel_pos[0])
+ && PX4_ISFINITE(fiducial_marker_pose.rel_pos[1])
+ && PX4_ISFINITE(fiducial_marker_pose.rel_pos[2]);
+
+ if (!finite_measurement) {
+ if (shouldEmitWarning(_vision_pos_warn_last)) {
+ PX4_WARN("VTE: Vision meas is corrupt!");
+ }
+
+ return false;
+ }
+
+ const bool finite_cov = PX4_ISFINITE(fiducial_marker_pose.cov_rel_pos[0])
+ && PX4_ISFINITE(fiducial_marker_pose.cov_rel_pos[1])
+ && PX4_ISFINITE(fiducial_marker_pose.cov_rel_pos[2]);
+
+ if (!finite_cov) {
+ if (shouldEmitWarning(_vision_pos_warn_last)) {
+ PX4_WARN("VTE: Vision cov is corrupt!");
+ }
+
+ return false;
+ }
+
+ // Rotate vision observation into vc-NED using the reported attitude quaternion
+ const bool finite_q = PX4_ISFINITE(fiducial_marker_pose.q[0])
+ && PX4_ISFINITE(fiducial_marker_pose.q[1])
+ && PX4_ISFINITE(fiducial_marker_pose.q[2])
+ && PX4_ISFINITE(fiducial_marker_pose.q[3]);
+
+ if (!finite_q) {
+ if (shouldEmitWarning(_vision_pos_warn_last)) {
+ PX4_WARN("VTE: Vision attitude quaternion is corrupt!");
+ }
+
+ return false;
+ }
+
+ return true;
+}
+
+bool VTEPosition::isUavGpsPositionValid()
+{
+ if (!isMeasRecent(_uav_gps_position.timestamp)) {
+ return false;
+ }
+
+ if (!isLatLonAltValid(_uav_gps_position.lat_deg, _uav_gps_position.lon_deg,
+ _uav_gps_position.alt_m, "UAV GPS ", &_uav_gps_pos_warn_last)) {
+ return false;
+ }
+
+ if (_gps_pos_is_offset && !_gps_pos_offset_ned.valid) {
+ PX4_DEBUG("Uav Gps position invalid offset!"); // Debug because this message can spam
+ return false;
+ }
+
+ return true;
+}
+
+bool VTEPosition::isUavGpsVelocityValid()
+{
+ if (!PX4_ISFINITE(_uav_gps_vel.xyz(vtest::Axis::x)) ||
+ !PX4_ISFINITE(_uav_gps_vel.xyz(vtest::Axis::y)) ||
+ !PX4_ISFINITE(_uav_gps_vel.xyz(vtest::Axis::z))) {
+ if (shouldEmitWarning(_uav_gps_vel_warn_last)) {
+ PX4_WARN("UAV GPS velocity not finite! vx: %.1f, vy: %.1f, vz: %.1f",
+ (double)_uav_gps_vel.xyz(vtest::Axis::x), (double)_uav_gps_vel.xyz(vtest::Axis::y),
+ (double)_uav_gps_vel.xyz(vtest::Axis::z));
+ }
+
+ return false;
+ }
+
+ if (!isMeasRecent(_uav_gps_vel.timestamp)) {
+ return false;
+ }
+
+ if (_gps_pos_is_offset && !_velocity_offset_ned.valid) {
+ PX4_DEBUG("Uav Gps velocity invalid offset!");
+ return false;
+ }
+
+ return true;
+}
+
+bool VTEPosition::updateUavGpsData()
+{
+ sensor_gps_s vehicle_gps_position;
+ const bool vehicle_gps_position_updated = _vehicle_gps_position_sub.update(&vehicle_gps_position);
+
+ if (vehicle_gps_position_updated) {
+ // Position
+ _uav_gps_position.lat_deg = vehicle_gps_position.latitude_deg;
+ _uav_gps_position.lon_deg = vehicle_gps_position.longitude_deg;
+ _uav_gps_position.alt_m = (float)vehicle_gps_position.altitude_msl_m;
+ _uav_gps_position.timestamp = vehicle_gps_position.timestamp_sample;
+ _uav_gps_position.eph = vehicle_gps_position.eph;
+ _uav_gps_position.epv = vehicle_gps_position.epv;
+ _uav_gps_position.valid = isUavGpsPositionValid();
+
+ // Velocity
+ _uav_gps_vel.timestamp = vehicle_gps_position.timestamp_sample;
+ _uav_gps_vel.xyz(vtest::Axis::x) = vehicle_gps_position.vel_n_m_s;
+ _uav_gps_vel.xyz(vtest::Axis::y) = vehicle_gps_position.vel_e_m_s;
+ _uav_gps_vel.xyz(vtest::Axis::z) = vehicle_gps_position.vel_d_m_s;
+ _uav_gps_vel.uncertainty = vehicle_gps_position.s_variance_m_s;
+ _uav_gps_vel.valid = vehicle_gps_position.vel_ned_valid && isUavGpsVelocityValid();
+
+ } else {
+ // Check if stored data is still valid
+ _uav_gps_position.valid = _uav_gps_position.valid && isMeasRecent(_uav_gps_position.timestamp);
+ _uav_gps_vel.valid = _uav_gps_vel.valid && isMeasRecent(_uav_gps_vel.timestamp);
+ }
+
+ return vehicle_gps_position_updated;
+}
+
+#if defined(CONFIG_VTEST_MOVING)
+void VTEPosition::updateTargetGpsVelocity(const target_gnss_s &target_gnss)
+{
+ _target_gps_vel.timestamp = target_gnss.timestamp_sample;
+ _target_gps_vel.valid = isMeasRecent(target_gnss.timestamp_sample);
+
+ _target_gps_vel.xyz(vtest::Axis::x) = target_gnss.vel_n_m_s;
+ _target_gps_vel.xyz(vtest::Axis::y) = target_gnss.vel_e_m_s;
+ _target_gps_vel.xyz(vtest::Axis::z) = target_gnss.vel_d_m_s;
+}
+#endif // CONFIG_VTEST_MOVING
+
+bool VTEPosition::isTargetGpsPositionValid(const target_gnss_s &target_gnss)
+{
+ if (!isMeasRecent(target_gnss.timestamp_sample)) {
+ return false;
+ }
+
+ if (!isLatLonAltValid(target_gnss.latitude_deg, target_gnss.longitude_deg,
+ target_gnss.altitude_msl_m, "Target GPS ", &_target_gps_pos_warn_last)) {
+ return false;
+ }
+
+ return true;
+}
+
+bool VTEPosition::isTargetGpsVelocityValid(const target_gnss_s &target_gnss)
+{
+ if (!isMeasRecent(target_gnss.timestamp_sample)) {
+ return false;
+ }
+
+ if (!target_gnss.vel_ned_updated ||
+ !PX4_ISFINITE(target_gnss.vel_n_m_s) ||
+ !PX4_ISFINITE(target_gnss.vel_e_m_s) ||
+ !PX4_ISFINITE(target_gnss.vel_d_m_s)) {
+ if (shouldEmitWarning(_target_gps_vel_warn_last)) {
+ PX4_WARN("Target GPS velocity is corrupt!");
+ }
+
+ return false;
+ }
+
+ return true;
+}
+
+bool VTEPosition::initializeEstimator(const ObsValidMaskU &fusion_mask,
+ const TargetObs observations[kObsTypeCount])
+{
+ if (!hasNewPositionSensorData(fusion_mask)) {
+ return false;
+ }
+
+ // Check for initial velocity estimate
+ const bool has_initial_velocity_estimate = (_local_velocity.valid && isMeasRecent(_local_velocity.timestamp)) ||
+ (_uav_gps_vel.valid && isMeasRecent(_uav_gps_vel.timestamp));
+
+ if (!has_initial_velocity_estimate) {
+ if (shouldEmitWarning(_init_vel_warn_last)) {
+ PX4_WARN("No UAV velocity estimate. Estimator cannot be started.");
+ }
+
+ return false;
+ }
+
+ // Initialize state variables
+ Vector3f initial_position{};
+ Vector3f initial_uav_velocity{};
+ Vector3f initial_bias{};
+#if defined(CONFIG_VTEST_MOVING)
+ Vector3f initial_target_accel {}; // Assume null target absolute acceleration
+ Vector3f initial_target_velocity{};
+#endif // CONFIG_VTEST_MOVING
+
+ // If both GNSS and vision are available at startup, only keep the GNSS-first
+ // initialization path when bias averaging is explicitly enabled. Otherwise,
+ // trust the current vision position and activate the bias immediately.
+ const bool can_average_initial_bias = shouldSetBias(fusion_mask) && isBiasAveragingEnabled();
+ const PreBiasReference initial_reference = selectInitialPosition(fusion_mask, observations, initial_position,
+ !can_average_initial_bias);
+ const bool average_initial_bias = shouldAverageInitialBias(fusion_mask, initial_reference);
+
+ // If the current GNSS/vision pair is already trusted, initialize both position and bias from that pair.
+ if (shouldSetBias(fusion_mask) && !average_initial_bias) {
+ PX4_DEBUG("VTE Position setting GNSS bias.");
+ const hrt_abstime sample_time = observations[obsIndex(ObsType::kFiducialMarker)].timestamp;
+ Vector3f gnss_sample{};
+
+ if (selectBiasGnssSample(sample_time, gnss_sample)) {
+ initial_bias = gnss_sample - initial_position;
+ }
+
+ _bias.set = true;
+ }
+
+#if defined(CONFIG_VTEST_MOVING)
+
+ if (_target_gps_vel.valid && (isMeasRecent(_target_gps_vel.timestamp))) {
+ initial_target_velocity = _target_gps_vel.xyz;
+ }
+
+#endif // CONFIG_VTEST_MOVING
+
+ // Define initial UAV velocity
+ if (_uav_gps_vel.valid && isMeasRecent(_uav_gps_vel.timestamp)) {
+ initial_uav_velocity = _uav_gps_vel.xyz;
+
+ } else if (_local_velocity.valid && isMeasRecent(_local_velocity.timestamp)) {
+ initial_uav_velocity = _local_velocity.xyz;
+ }
+
+ // Initialize estimator state
+ AxisEstimatorStates state_init{};
+
+ for (int axis = 0; axis < vtest::Axis::size; ++axis) {
+ state_init[axis](vtest::State::pos_rel) = initial_position(axis);
+ state_init[axis](vtest::State::vel_uav) = initial_uav_velocity(axis);
+ state_init[axis](vtest::State::bias) = initial_bias(axis);
+
+#if defined(CONFIG_VTEST_MOVING)
+ state_init[axis](vtest::State::acc_target) = initial_target_accel(axis);
+ state_init[axis](vtest::State::vel_target) = initial_target_velocity(axis);
+#endif // CONFIG_VTEST_MOVING
+ }
+
+ if (!initEstimator(state_init)) {
+ resetFilter();
+ return false;
+ }
+
+ PX4_DEBUG("VTE Position Estimator properly initialized.");
+ _estimator_initialized = true;
+ _pre_bias_reference = _bias.set ? PreBiasReference::kUnknown : initial_reference;
+ _last_update = hrt_absolute_time();
+ _last_predict = _last_update;
+
+ for (int axis = 0; axis < vtest::Axis::size; ++axis) {
+ _target_est_pos[axis].resetHistory();
+ }
+
+ return true;
+}
+
+VTEPosition::PreBiasReference VTEPosition::selectInitialPosition(const ObsValidMaskU &fusion_mask,
+ const TargetObs observations[kObsTypeCount], matrix::Vector3f &initial_position,
+ const bool prefer_vision)
+{
+ // Select the best direct position source before the GNSS bias is trusted.
+ if (prefer_vision && fusion_mask.flags.fuse_vision) {
+ initial_position = observations[obsIndex(ObsType::kFiducialMarker)].meas_xyz;
+ return PreBiasReference::kVision;
+ }
+
+ if (fusion_mask.flags.fuse_target_gps_pos) {
+ initial_position = observations[obsIndex(ObsType::kTargetGpsPos)].meas_xyz;
+ return PreBiasReference::kGnss;
+ }
+
+ if (fusion_mask.flags.fuse_mission_pos) {
+ initial_position = observations[obsIndex(ObsType::kMissionGpsPos)].meas_xyz;
+ return PreBiasReference::kGnss;
+ }
+
+ if (!prefer_vision && fusion_mask.flags.fuse_vision) {
+ initial_position = observations[obsIndex(ObsType::kFiducialMarker)].meas_xyz;
+ return PreBiasReference::kVision;
+ }
+
+ return PreBiasReference::kUnknown;
+}
+
+bool VTEPosition::getOffsetUavVel(const hrt_abstime sample_time, Vector3f &uav_velocity_ned) const
+{
+ if (!_uav_gps_vel.valid || !isTimeDifferenceWithin(_uav_gps_vel.timestamp, sample_time, _meas_updated_timeout_us)) {
+ return false;
+ }
+
+ uav_velocity_ned = _uav_gps_vel.xyz;
+
+ if (_gps_pos_is_offset) {
+ if (!_velocity_offset_ned.valid
+ || !isTimeDifferenceWithin(_velocity_offset_ned.timestamp, _uav_gps_vel.timestamp, _meas_updated_timeout_us)) {
+ return false;
+ }
+
+ uav_velocity_ned -= _velocity_offset_ned.xyz;
+ }
+
+ return true;
+}
+
+bool VTEPosition::selectBiasGnssSample(const hrt_abstime sample_time, Vector3f &gnss_sample)
+{
+ if (!_pos_rel_gnss.valid || !isMeasRecent(_pos_rel_gnss.timestamp)) {
+ return false;
+ }
+
+ gnss_sample = _pos_rel_gnss.xyz;
+
+ const int64_t dt_us = signedTimeDiffUs(sample_time, _pos_rel_gnss.timestamp);
+
+ if (dt_us != 0) {
+ const float dt = static_cast(dt_us) * kMicrosecondsToSeconds;
+ Vector3f uav_velocity_ned{};
+ bool has_velocity_for_propagation = false;
+
+ if (_local_velocity.valid
+ && isTimeDifferenceWithin(_local_velocity.timestamp, sample_time, _meas_updated_timeout_us)) {
+ uav_velocity_ned = _local_velocity.xyz;
+ has_velocity_for_propagation = true;
+
+ } else if (getOffsetUavVel(sample_time, uav_velocity_ned)) {
+ has_velocity_for_propagation = true;
+ }
+
+ if (has_velocity_for_propagation) {
+ gnss_sample -= uav_velocity_ned * dt;
+ }
+ }
+
+ return true;
+}
+
+void VTEPosition::updateBiasIfObservable(const ObsValidMaskU &fusion_mask,
+ const TargetObs observations[kObsTypeCount])
+{
+ if (!fusion_mask.flags.fuse_vision) {
+ return;
+ }
+
+ const TargetObs &vision_obs = observations[obsIndex(ObsType::kFiducialMarker)];
+ const hrt_abstime sample_time = vision_obs.timestamp;
+
+ if (sample_time == 0) {
+ return;
+ }
+
+ Vector3f initial_position{};
+ selectInitialPosition(fusion_mask, observations, initial_position);
+
+ Vector3f gnss_sample{};
+
+ if (!selectBiasGnssSample(sample_time, gnss_sample)) {
+ if (_bias.averaging_active) {
+ PX4_INFO("GNSS unavailable, ending bias averaging and switching to vision.");
+ activateBiasEstimate(initial_position, _bias.initial_lpf.getState());
+ }
+
+ return;
+ }
+
+ const Vector3f initial_bias = gnss_sample - initial_position;
+
+ if (!_bias.averaging_active && shouldAverageInitialBias(fusion_mask)) {
+ PX4_INFO("GNSS/vision bias observable, averaging initial GNSS bias.");
+ startBiasAveraging(initial_bias, sample_time);
+ return;
+ }
+
+ if (_bias.averaging_active) {
+ if (!updateBiasAveraging(initial_bias, sample_time)) {
+ return;
+ }
+
+ PX4_INFO("GNSS bias averaging complete, setting position and bias.");
+ const Vector3f filtered_bias = _bias.initial_lpf.getState();
+ activateBiasEstimate(gnss_sample - filtered_bias, filtered_bias);
+ return;
+ }
+
+ PX4_INFO("GNSS/vision bias observable, setting position and bias.");
+ activateBiasEstimate(initial_position, initial_bias);
+}
+
+void VTEPosition::startBiasAveraging(const Vector3f &bias_sample, const hrt_abstime sample_time)
+{
+ _bias.initial_lpf.reset(bias_sample);
+ _bias.averaging_active = true;
+ _bias.stable_delta_count = 0;
+ _bias.averaging_start_time = sample_time;
+ _bias.last_sample_time = sample_time;
+
+ publishBiasInitStatus(bias_sample, bias_sample, 0.f, sample_time);
+}
+
+bool VTEPosition::updateBiasAveraging(const Vector3f &bias_sample, const hrt_abstime sample_time)
+{
+ static constexpr float kInitialBiasLpfTimeConstantS{0.3f};
+ static constexpr float kInitialBiasLpfMinTimeFactor{2.f};
+ static constexpr uint8_t kRequiredStableBiasDeltas{5};
+
+ if (!_bias.averaging_active) {
+ return false;
+ }
+
+ const Vector3f filtered_bias_before_update = _bias.initial_lpf.getState();
+ const float bias_delta = (bias_sample - filtered_bias_before_update).norm();
+
+ if (sample_time > _bias.last_sample_time) {
+ const float dt = static_cast(sample_time - _bias.last_sample_time) * kMicrosecondsToSeconds;
+ _bias.initial_lpf.setParameters(dt, kInitialBiasLpfTimeConstantS);
+ _bias.initial_lpf.update(bias_sample);
+
+ } else {
+ _bias.initial_lpf.update(bias_sample);
+ }
+
+ _bias.last_sample_time = sample_time;
+
+ const Vector3f filtered_bias_logged = _bias.initial_lpf.getState();
+ publishBiasInitStatus(bias_sample, filtered_bias_logged, bias_delta, sample_time);
+
+ if (bias_delta <= getBiasAveragingThreshold()) {
+ if (_bias.stable_delta_count < UINT8_MAX) {
+ ++_bias.stable_delta_count;
+ }
+
+ } else {
+ _bias.stable_delta_count = 0;
+ }
+
+ const hrt_abstime min_averaging_time_us = static_cast(
+ kInitialBiasLpfMinTimeFactor * kInitialBiasLpfTimeConstantS * 1e6f);
+ const bool min_time_elapsed = (sample_time >= _bias.averaging_start_time)
+ && ((sample_time - _bias.averaging_start_time) >= min_averaging_time_us);
+ const bool stable = (_bias.stable_delta_count >= kRequiredStableBiasDeltas) && min_time_elapsed;
+ const bool timed_out = (sample_time >= _bias.averaging_start_time)
+ && ((sample_time - _bias.averaging_start_time) >= getBiasAveragingTimeoutUs());
+
+ return stable || timed_out;
+}
+
+void VTEPosition::resetBiasAveraging()
+{
+ _bias.averaging_active = false;
+ _bias.stable_delta_count = 0;
+ _bias.averaging_start_time = 0;
+ _bias.last_sample_time = 0;
+ _bias.initial_lpf.reset(Vector3f{});
+}
+
+void VTEPosition::activateBiasEstimate(const Vector3f &initial_position, const Vector3f &initial_bias)
+{
+ const float pos_var = _param_vte_pos_unc_in.get();
+ const float bias_var = _param_vte_bias_unc_in.get();
+
+ for (int axis = 0; axis < vtest::Axis::size; ++axis) {
+ KF_position &filter = _target_est_pos[axis];
+ EstimatorState state = filter.getState();
+ state(vtest::State::bias) = initial_bias(axis);
+ state(vtest::State::pos_rel) = initial_position(axis);
+ filter.setState(state);
+
+ EstimatorState state_var = filter.getStateCovarianceDiag();
+ state_var(vtest::State::bias) = bias_var;
+ state_var(vtest::State::pos_rel) = pos_var;
+ filter.setStateCovarianceDiag(state_var);
+
+ // Bias initialization is a state reset: restart OOSM history to avoid mixing pre/post-reset states.
+ filter.resetHistory();
+ }
+
+ PX4_DEBUG("Rel pos init %.2f %.2f %.2f", (double)initial_position(vtest::Axis::x),
+ (double)initial_position(vtest::Axis::y), (double)initial_position(vtest::Axis::z));
+
+ PX4_DEBUG("GNSS bias init %.2f %.2f %.2f", (double)initial_bias(vtest::Axis::x),
+ (double)initial_bias(vtest::Axis::y), (double)initial_bias(vtest::Axis::z));
+
+ _bias.set = true;
+ _pre_bias_reference = PreBiasReference::kUnknown;
+ resetBiasAveraging();
+}
+
+void VTEPosition::publishBiasInitStatus(const Vector3f &raw_bias, const Vector3f &filtered_bias,
+ const float raw_bias_delta_norm, const hrt_abstime sample_time)
+{
+ vte_bias_init_status_s status{};
+ status.timestamp = sample_time;
+ status.raw_bias[0] = raw_bias(vtest::Axis::x);
+ status.raw_bias[1] = raw_bias(vtest::Axis::y);
+ status.raw_bias[2] = raw_bias(vtest::Axis::z);
+ status.filtered_bias[0] = filtered_bias(vtest::Axis::x);
+ status.filtered_bias[1] = filtered_bias(vtest::Axis::y);
+ status.filtered_bias[2] = filtered_bias(vtest::Axis::z);
+ status.delta_norm = raw_bias_delta_norm;
+ _vte_bias_init_status_pub.publish(status);
+}
+
+bool VTEPosition::fuseActiveMeasurements(const matrix::Vector3f &vehicle_acc_ned, ObsValidMaskU &fusion_mask,
+ const TargetObs observations[kObsTypeCount])
+{
+ struct FusionJob {
+ hrt_abstime timestamp{};
+ ObsType type{ObsType::kTypeCount};
+ const TargetObs *obs{nullptr};
+ };
+
+ FusionJob jobs[kObsTypeCount] {};
+ size_t job_count = 0;
+
+ auto add_job = [&](ObsType type) {
+ const TargetObs &obs = observations[obsIndex(type)];
+
+ if ((job_count >= kObsTypeCount) || (obs.timestamp == 0)) {
+ return;
+ }
+
+ jobs[job_count++] = FusionJob{obs.timestamp, type, &obs};
+ };
+
+ bool position_fused = false;
+
+ if (fusion_mask.flags.fuse_target_gps_pos && !shouldBlockGnssFusionUntilBiasReady()) {
+ add_job(ObsType::kTargetGpsPos);
+ }
+
+ if (fusion_mask.flags.fuse_mission_pos && !shouldBlockGnssFusionUntilBiasReady()) {
+ add_job(ObsType::kMissionGpsPos);
+ }
+
+ if (fusion_mask.flags.fuse_vision && !shouldBlockVisionFusionUntilBiasReady()) {
+ add_job(ObsType::kFiducialMarker);
+ }
+
+ if (fusion_mask.flags.fuse_uav_gps_vel) {
+ add_job(ObsType::kUavGpsVel);
+ }
+
+#if defined(CONFIG_VTEST_MOVING)
+
+ if (fusion_mask.flags.fuse_target_gps_vel) {
+ add_job(ObsType::kTargetGpsVel);
+ }
+
+#endif // CONFIG_VTEST_MOVING
+
+ // Fuse strictly in timestamp order (oldest -> newest) to keep the OOSM approximation consistent.
+ for (size_t i = 1; i < job_count; ++i) {
+ FusionJob key = jobs[i];
+ size_t j = i;
+
+ while ((j > 0) && (jobs[j - 1].timestamp > key.timestamp)) {
+ jobs[j] = jobs[j - 1];
+ --j;
+ }
+
+ jobs[j] = key;
+ }
+
+ for (size_t i = 0; i < job_count; ++i) {
+ const FusionJob &job = jobs[i];
+
+ const bool fused = (job.obs != nullptr) && fuseMeas(vehicle_acc_ned, *job.obs);
+
+ switch (job.type) {
+ case ObsType::kTargetGpsPos:
+ case ObsType::kMissionGpsPos:
+ position_fused |= fused;
+
+ if (!_bias.set && fused) {
+ _pre_bias_reference = PreBiasReference::kGnss;
+ }
+
+ break;
+
+ case ObsType::kFiducialMarker:
+ position_fused |= fused;
+
+ if (!_bias.set && fused) {
+ _pre_bias_reference = PreBiasReference::kVision;
+ }
+
+ if (fused) {
+ _last_relative_meas_fused_time = hrt_absolute_time();
+ }
+
+ break;
+
+ case ObsType::kUavGpsVel:
+ case ObsType::kTargetGpsVel:
+ case ObsType::kTypeCount:
+ break;
+ }
+ }
+
+ return position_fused;
+}
+
+/*Vision observation: [rx, ry, rz]*/
+bool VTEPosition::processObsVision(TargetObs &obs)
+{
+ fiducial_marker_pos_report_s report;
+
+ if (!_fiducial_marker_pos_report_sub.update(&report) || !isVisionDataValid(report)) {
+ return false;
+ }
+
+ const matrix::Quaternionf quat_att(report.q);
+ const Vector3f vision_rel(report.rel_pos);
+ const Vector3f vision_ned = quat_att.rotateVector(vision_rel);
+ const float min_ev_pos_var = sq(fmaxf(_param_vte_ev_pos_noise.get(), kMinObservationNoise));
+
+ // Rotate covariance matrix to vc-NED
+ const SquareMatrix cov_mat = diag(Vector3f(
+ fmaxf(report.cov_rel_pos[0], min_ev_pos_var),
+ fmaxf(report.cov_rel_pos[1], min_ev_pos_var),
+ fmaxf(report.cov_rel_pos[2], min_ev_pos_var)));
+ const matrix::Dcmf r_att = matrix::Dcm(quat_att);
+ const SquareMatrix cov_rotated = r_att * cov_mat * r_att.transpose();
+
+ // Relative position
+ if (!PX4_ISFINITE(vision_ned(0)) || !PX4_ISFINITE(vision_ned(1)) || !PX4_ISFINITE(vision_ned(2))) {
+ if (shouldEmitWarning(_vision_pos_warn_last)) {
+ PX4_WARN("VTE: Vision position NED is corrupt!");
+ }
+
+ return false;
+ }
+
+ obs.timestamp = report.timestamp_sample;
+
+ obs.type = ObsType::kFiducialMarker;
+ obs.updated(vtest::Axis::x) = true;
+ obs.updated(vtest::Axis::y) = true;
+ obs.updated(vtest::Axis::z) = true;
+
+ // Assume noise correlation negligible:
+ obs.meas_h_xyz(vtest::Axis::x, vtest::State::pos_rel) = 1;
+ obs.meas_h_xyz(vtest::Axis::y, vtest::State::pos_rel) = 1;
+ obs.meas_h_xyz(vtest::Axis::z, vtest::State::pos_rel) = 1;
+
+ obs.meas_xyz = vision_ned;
+
+ // Assume off diag elements ~ 0
+ obs.meas_unc_xyz(vtest::Axis::x) = cov_rotated(vtest::Axis::x, vtest::Axis::x);
+ obs.meas_unc_xyz(vtest::Axis::y) = cov_rotated(vtest::Axis::y, vtest::Axis::y);
+ obs.meas_unc_xyz(vtest::Axis::z) = cov_rotated(vtest::Axis::z, vtest::Axis::z);
+
+ return true;
+}
+
+/*Drone GNSS velocity observation: [vel_uav_x, vel_uav_y ,vel_uav_z]*/
+bool VTEPosition::processObsGNSSVelUav(TargetObs &obs) const
+{
+ Vector3f vel_uav_ned{};
+
+ if (!getOffsetUavVel(_uav_gps_vel.timestamp, vel_uav_ned)) {
+ return false;
+ }
+
+ const float min_gps_vel_var = getMinGpsVelVar();
+
+ obs.meas_xyz = vel_uav_ned;
+
+ obs.meas_h_xyz(vtest::Axis::x, vtest::State::vel_uav) = 1;
+ obs.meas_h_xyz(vtest::Axis::y, vtest::State::vel_uav) = 1;
+ obs.meas_h_xyz(vtest::Axis::z, vtest::State::vel_uav) = 1;
+
+ const float unc = fmaxf(sq(_uav_gps_vel.uncertainty), min_gps_vel_var);
+ obs.meas_unc_xyz(vtest::Axis::x) = unc;
+ obs.meas_unc_xyz(vtest::Axis::y) = unc;
+ obs.meas_unc_xyz(vtest::Axis::z) = unc;
+
+ obs.timestamp = _uav_gps_vel.timestamp;
+
+ obs.type = ObsType::kUavGpsVel;
+
+ obs.updated(vtest::Axis::x) = true;
+ obs.updated(vtest::Axis::y) = true;
+ obs.updated(vtest::Axis::z) = true;
+
+ return true;
+}
+
+#if defined(CONFIG_VTEST_MOVING)
+
+/*Target GNSS velocity observation: [r_dotx, r_doty, r_dotz]*/
+bool VTEPosition::processObsGNSSVelTarget(const target_gnss_s &target_gnss, TargetObs &obs) const
+{
+ // If the target is moving, the relative velocity is expressed as the drone velocity minus the target velocity.
+ obs.meas_xyz(vtest::Axis::x) = target_gnss.vel_n_m_s;
+ obs.meas_xyz(vtest::Axis::y) = target_gnss.vel_e_m_s;
+ obs.meas_xyz(vtest::Axis::z) = target_gnss.vel_d_m_s;
+
+ const float unc = fmaxf(sq(target_gnss.s_acc_m_s), getMinGpsVelVar());
+
+ obs.meas_unc_xyz(vtest::Axis::x) = unc;
+ obs.meas_unc_xyz(vtest::Axis::y) = unc;
+ obs.meas_unc_xyz(vtest::Axis::z) = unc;
+
+ obs.meas_h_xyz(vtest::Axis::x, vtest::State::vel_target) = 1;
+ obs.meas_h_xyz(vtest::Axis::y, vtest::State::vel_target) = 1;
+ obs.meas_h_xyz(vtest::Axis::z, vtest::State::vel_target) = 1;
+
+ obs.timestamp = target_gnss.timestamp_sample;
+
+ obs.type = ObsType::kTargetGpsVel;
+
+ obs.updated(vtest::Axis::x) = true;
+ obs.updated(vtest::Axis::y) = true;
+ obs.updated(vtest::Axis::z) = true;
+
+ return true;
+}
+
+#endif // CONFIG_VTEST_MOVING
+
+/*Target GNSS mission observation: [rx + bx, ry + by, rz + bz]*/
+bool VTEPosition::processObsGNSSPosMission(TargetObs &obs)
+{
+ // Obtain GPS relative measurements in NED as target_global - uav_gps_global followed by global2local transformation
+ Vector3f gps_relative_pos;
+ get_vector_to_next_waypoint(_uav_gps_position.lat_deg, _uav_gps_position.lon_deg,
+ _mission_land_position.lat_deg, _mission_land_position.lon_deg,
+ &gps_relative_pos(0), &gps_relative_pos(1));
+
+ // Down direction (if the drone is above the target, the relative position is positive)
+ gps_relative_pos(2) = _uav_gps_position.alt_m - _mission_land_position.alt_m;
+
+ // Offset gps relative position to the center of mass:
+ if (_gps_pos_is_offset) {
+
+ if (!_gps_pos_offset_ned.valid
+ || !isTimeDifferenceWithin(_gps_pos_offset_ned.timestamp, _uav_gps_position.timestamp, _meas_updated_timeout_us)) {
+ return false;
+ }
+
+ gps_relative_pos += _gps_pos_offset_ned.xyz;
+ }
+
+ const float min_gps_pos_var = getMinGpsPosVar();
+ const float gps_unc_horizontal = fmaxf(sq(_uav_gps_position.eph), min_gps_pos_var);
+ const float gps_unc_vertical = fmaxf(sq(_uav_gps_position.epv), min_gps_pos_var);
+
+ // GPS already in NED, no rotation required.
+ // Obs: [pos_rel + bias]
+ obs.meas_h_xyz(vtest::Axis::x, vtest::State::pos_rel) = 1;
+ obs.meas_h_xyz(vtest::Axis::y, vtest::State::pos_rel) = 1;
+ obs.meas_h_xyz(vtest::Axis::z, vtest::State::pos_rel) = 1;
+
+ obs.timestamp = _uav_gps_position.timestamp;
+
+ obs.meas_xyz = gps_relative_pos;
+
+ obs.meas_unc_xyz(vtest::Axis::x) = gps_unc_horizontal;
+ obs.meas_unc_xyz(vtest::Axis::y) = gps_unc_horizontal;
+ obs.meas_unc_xyz(vtest::Axis::z) = gps_unc_vertical;
+
+ if (_bias.set) {
+ obs.meas_h_xyz(vtest::Axis::x, vtest::State::bias) = 1;
+ obs.meas_h_xyz(vtest::Axis::y, vtest::State::bias) = 1;
+ obs.meas_h_xyz(vtest::Axis::z, vtest::State::bias) = 1;
+ }
+
+ obs.type = ObsType::kMissionGpsPos;
+
+ obs.updated(vtest::Axis::x) = true;
+ obs.updated(vtest::Axis::y) = true;
+ obs.updated(vtest::Axis::z) = true;
+
+ // Keep track of the latest GNSS-relative position used for bias initialization.
+ // Mission and target GPS position fusion are mutually exclusive, so the mission
+ // path can refresh this cache unconditionally.
+ _pos_rel_gnss.timestamp = obs.timestamp;
+ _pos_rel_gnss.valid = (PX4_ISFINITE(gps_relative_pos(0)) && PX4_ISFINITE(gps_relative_pos(1))
+ && PX4_ISFINITE(gps_relative_pos(2)));
+ _pos_rel_gnss.xyz = gps_relative_pos;
+
+ return true;
+}
+
+/*Target GNSS observation: [rx + bx, ry + by, rz + bz]*/
+bool VTEPosition::processObsGNSSPosTarget(const target_gnss_s &target_gnss, TargetObs &obs)
+{
+ const int64_t time_diff_us = signedTimeDiffUs(target_gnss.timestamp_sample, _uav_gps_position.timestamp);
+ const float dt_sync_us = fabsf(static_cast(time_diff_us));
+
+ if (dt_sync_us > _meas_recent_timeout_us) {
+ PX4_DEBUG("Time diff between UAV GNSS and target GNSS too high: %.2f [ms] > timeout: %.2f [ms]",
+ static_cast(dt_sync_us * kMicrosecondsToMilliseconds),
+ static_cast(_meas_recent_timeout_us * kMicrosecondsToMilliseconds));
+ return false;
+ }
+
+ double uav_lat_deg = _uav_gps_position.lat_deg;
+ double uav_lon_deg = _uav_gps_position.lon_deg;
+ float uav_alt_m = _uav_gps_position.alt_m;
+
+ const float dt_sync_s = static_cast(time_diff_us) * kMicrosecondsToSeconds;
+ const float dt_sync_s_abs = fabsf(dt_sync_s);
+ bool uav_position_interpolated = false;
+
+ // Compensate UAV motion during the latency interval
+ static constexpr float kGnssSyncInterpolationMinTimeS = 0.1f;
+
+ Vector3f uav_vel_ned{};
+
+ if ((dt_sync_s_abs > kGnssSyncInterpolationMinTimeS)
+ && getOffsetUavVel(_uav_gps_position.timestamp, uav_vel_ned)) {
+
+ const float delta_n = uav_vel_ned(vtest::Axis::x) * dt_sync_s;
+ const float delta_e = uav_vel_ned(vtest::Axis::y) * dt_sync_s;
+
+ double lat_res = uav_lat_deg;
+ double lon_res = uav_lon_deg;
+ add_vector_to_global_position(uav_lat_deg, uav_lon_deg, delta_n, delta_e, &lat_res, &lon_res);
+ uav_lat_deg = lat_res;
+ uav_lon_deg = lon_res;
+ uav_alt_m = _uav_gps_position.alt_m - uav_vel_ned(vtest::Axis::z) * dt_sync_s;
+ uav_position_interpolated = true;
+ }
+
+ // Obtain GPS relative measurements in NED as target_global - uav_gps_global followed by global2local transformation
+ Vector3f gps_relative_pos;
+ get_vector_to_next_waypoint(uav_lat_deg, uav_lon_deg,
+ target_gnss.latitude_deg, target_gnss.longitude_deg,
+ &gps_relative_pos(0), &gps_relative_pos(1));
+
+ // Down direction (if the drone is above the target, the relative position is positive)
+ gps_relative_pos(2) = uav_alt_m - target_gnss.altitude_msl_m;
+
+ // Offset gps relative position to the center of mass:
+ if (_gps_pos_is_offset) {
+ if (!_gps_pos_offset_ned.valid
+ || !isTimeDifferenceWithin(_gps_pos_offset_ned.timestamp, _uav_gps_position.timestamp, _meas_updated_timeout_us)) {
+
+ return false;
+ }
+
+ gps_relative_pos += _gps_pos_offset_ned.xyz;
+ }
+
+ // Var(aX - bY) = a^2 Var(X) + b^2Var(Y) - 2ab Cov(X,Y)
+ const float propagation_unc = uav_position_interpolated
+ ? sq(_uav_gps_vel.uncertainty * dt_sync_s_abs) : 0.f;
+ const float min_gps_pos_var = getMinGpsPosVar();
+ const float gps_unc_horizontal = fmaxf(sq(_uav_gps_position.eph), min_gps_pos_var)
+ + fmaxf(sq(target_gnss.eph), min_gps_pos_var)
+ + propagation_unc;
+ const float gps_unc_vertical = fmaxf(sq(_uav_gps_position.epv), min_gps_pos_var)
+ + fmaxf(sq(target_gnss.epv), min_gps_pos_var)
+ + propagation_unc;
+
+ // GPS already in NED, no rotation required.
+ // Obs: [pos_rel + bias]
+ obs.meas_h_xyz(vtest::Axis::x, vtest::State::pos_rel) = 1;
+ obs.meas_h_xyz(vtest::Axis::y, vtest::State::pos_rel) = 1;
+ obs.meas_h_xyz(vtest::Axis::z, vtest::State::pos_rel) = 1;
+
+ if (_bias.set) {
+ obs.meas_h_xyz(vtest::Axis::x, vtest::State::bias) = 1;
+ obs.meas_h_xyz(vtest::Axis::y, vtest::State::bias) = 1;
+ obs.meas_h_xyz(vtest::Axis::z, vtest::State::bias) = 1;
+ }
+
+ obs.timestamp = target_gnss.timestamp_sample;
+
+ obs.meas_xyz = gps_relative_pos;
+
+ obs.meas_unc_xyz(vtest::Axis::x) = gps_unc_horizontal;
+ obs.meas_unc_xyz(vtest::Axis::y) = gps_unc_horizontal;
+ obs.meas_unc_xyz(vtest::Axis::z) = gps_unc_vertical;
+
+ obs.type = ObsType::kTargetGpsPos;
+
+ obs.updated(vtest::Axis::x) = true;
+ obs.updated(vtest::Axis::y) = true;
+ obs.updated(vtest::Axis::z) = true;
+
+ // Keep track of the gps relative position
+ _pos_rel_gnss.timestamp = obs.timestamp;
+ _pos_rel_gnss.valid = (PX4_ISFINITE(gps_relative_pos(0)) && PX4_ISFINITE(gps_relative_pos(1))
+ && PX4_ISFINITE(gps_relative_pos(2)));
+ _pos_rel_gnss.xyz = gps_relative_pos;
+
+ return true;
+}
+
+bool VTEPosition::fuseMeas(const Vector3f &vehicle_acc_ned, const TargetObs &target_obs)
+{
+ perf_begin(_vte_update_perf);
+ const float nis_threshold = fmaxf(_param_vte_pos_nis_thre.get(), kMinNisThreshold);
+
+ vte_aid_source3d_s target_innov = {};
+ target_innov.timestamp_sample = target_obs.timestamp;
+ target_innov.timestamp = hrt_absolute_time();
+ target_innov.time_last_predict = _last_predict;
+ target_innov.time_since_meas_ms = static_cast(signedTimeDiffUs(_last_predict, target_obs.timestamp))
+ * kMicrosecondsToMilliseconds;
+
+ bool any_axis_fused = false;
+ uint8_t history_steps = 0;
+
+ for (int j = 0; j < vtest::Axis::size; j++) {
+
+ if (!target_obs.updated(j)) {
+ target_innov.fusion_status[j] = static_cast(FusionStatus::kIdle);
+ continue; // nothing to do for this axis
+ }
+
+ const KF_position::ScalarMeas meas_input{target_obs.timestamp, target_obs.meas_xyz(j),
+ target_obs.meas_unc_xyz(j), target_obs.meas_h_xyz.row(j)};
+
+ target_innov.observation[j] = meas_input.val;
+ target_innov.observation_variance[j] = meas_input.unc;
+
+ perf_begin(_vte_fusion_perf);
+ const FusionResult result = _target_est_pos[j].fuseScalarAtTime(meas_input, _last_predict, nis_threshold);
+ perf_end(_vte_fusion_perf);
+
+ target_innov.innovation[j] = result.innov;
+ target_innov.innovation_variance[j] = result.innov_var;
+ target_innov.test_ratio[j] = result.test_ratio;
+ target_innov.fusion_status[j] = static_cast(result.status);
+ any_axis_fused |= (result.status == FusionStatus::kFusedCurrent ||
+ result.status == FusionStatus::kFusedOosm);
+
+ if (result.status == FusionStatus::kFusedOosm) {
+ history_steps = math::max(result.history_steps, history_steps);
+ }
+ }
+
+ target_innov.history_steps = history_steps;
+
+ perf_end(_vte_update_perf);
+ publishInnov(target_innov, target_obs.type);
+
+ return any_axis_fused;
+}
+
+void VTEPosition::publishInnov(const vte_aid_source3d_s &target_innov, const ObsType type)
+{
+ // Publish innovations
+ switch (type) {
+ case ObsType::kTargetGpsPos:
+ _vte_aid_gps_pos_target_pub.publish(target_innov);
+ break;
+
+ case ObsType::kMissionGpsPos:
+ _vte_aid_gps_pos_mission_pub.publish(target_innov);
+ break;
+
+ case ObsType::kUavGpsVel:
+ _vte_aid_gps_vel_uav_pub.publish(target_innov);
+ break;
+
+ case ObsType::kTargetGpsVel:
+ _vte_aid_gps_vel_target_pub.publish(target_innov);
+ break;
+
+ case ObsType::kFiducialMarker:
+ _vte_aid_fiducial_marker_pub.publish(target_innov);
+ break;
+
+ case ObsType::kTypeCount:
+ break;
+ }
+}
+
+void VTEPosition::publishTarget()
+{
+ _vte_state = {};
+ _target_pose = {};
+
+ _target_pose.timestamp = _last_predict;
+ _vte_state.timestamp = _last_predict;
+
+ _target_pose.rel_pos_valid = !hasTimedOut(_last_update, _target_valid_timeout_us);
+
+#if defined(CONFIG_VTEST_MOVING)
+ _target_pose.is_static = false;
+#else
+ _target_pose.is_static = true;
+#endif // CONFIG_VTEST_MOVING
+
+ // Get state
+ const EstimatorState &state_x = _target_est_pos[vtest::Axis::x].getState();
+ const EstimatorState &state_y = _target_est_pos[vtest::Axis::y].getState();
+ const EstimatorState &state_z = _target_est_pos[vtest::Axis::z].getState();
+
+ const EstimatorState state_var_x = _target_est_pos[vtest::Axis::x].getStateCovarianceDiag();
+ const EstimatorState state_var_y = _target_est_pos[vtest::Axis::y].getStateCovarianceDiag();
+ const EstimatorState state_var_z = _target_est_pos[vtest::Axis::z].getStateCovarianceDiag();
+
+ // Fill target relative pose
+ _target_pose.x_rel = state_x(vtest::State::pos_rel);
+ _target_pose.y_rel = state_y(vtest::State::pos_rel);
+ _target_pose.z_rel = state_z(vtest::State::pos_rel);
+
+ _target_pose.cov_x_rel = state_var_x(vtest::State::pos_rel);
+ _target_pose.cov_y_rel = state_var_y(vtest::State::pos_rel);
+ _target_pose.cov_z_rel = state_var_z(vtest::State::pos_rel);
+
+ // Fill target relative velocity
+ _target_pose.vx_rel = -state_x(vtest::State::vel_uav);
+ _target_pose.vy_rel = -state_y(vtest::State::vel_uav);
+ _target_pose.vz_rel = -state_z(vtest::State::vel_uav);
+
+ _target_pose.cov_vx_rel = state_var_x(vtest::State::vel_uav);
+ _target_pose.cov_vy_rel = state_var_y(vtest::State::vel_uav);
+ _target_pose.cov_vz_rel = state_var_z(vtest::State::vel_uav);
+
+#if defined(CONFIG_VTEST_MOVING)
+ const EstimatorStateCovariance &state_cov_x = _target_est_pos[vtest::Axis::x].getStateCovariance();
+ const EstimatorStateCovariance &state_cov_y = _target_est_pos[vtest::Axis::y].getStateCovariance();
+ const EstimatorStateCovariance &state_cov_z = _target_est_pos[vtest::Axis::z].getStateCovariance();
+
+ // If target is moving, relative velocity = vel_target - vel_uav
+ _target_pose.vx_rel += state_x(vtest::State::vel_target);
+ _target_pose.vy_rel += state_y(vtest::State::vel_target);
+ _target_pose.vz_rel += state_z(vtest::State::vel_target);
+
+ // Var(vel_target - vel_uav) = Var(vel_target) + Var(vel_uav) - 2 Cov(vel_uav, vel_target)
+ _target_pose.cov_vx_rel += state_var_x(vtest::State::vel_target)
+ - 2.f * state_cov_x(vtest::State::vel_uav, vtest::State::vel_target);
+ _target_pose.cov_vy_rel += state_var_y(vtest::State::vel_target)
+ - 2.f * state_cov_y(vtest::State::vel_uav, vtest::State::vel_target);
+ _target_pose.cov_vz_rel += state_var_z(vtest::State::vel_target)
+ - 2.f * state_cov_z(vtest::State::vel_uav, vtest::State::vel_target);
+
+#endif // CONFIG_VTEST_MOVING
+
+ // Fill vision target estimator state
+ _vte_state.rel_pos[0] = _target_pose.x_rel;
+ _vte_state.rel_pos[1] = _target_pose.y_rel;
+ _vte_state.rel_pos[2] = _target_pose.z_rel;
+
+ _vte_state.cov_rel_pos[0] = _target_pose.cov_x_rel;
+ _vte_state.cov_rel_pos[1] = _target_pose.cov_y_rel;
+ _vte_state.cov_rel_pos[2] = _target_pose.cov_z_rel;
+
+ // Fill uav velocity
+ _vte_state.vel_uav[0] = state_x(vtest::State::vel_uav);
+ _vte_state.vel_uav[1] = state_y(vtest::State::vel_uav);
+ _vte_state.vel_uav[2] = state_z(vtest::State::vel_uav);
+
+ _vte_state.cov_vel_uav[0] = state_var_x(vtest::State::vel_uav);
+ _vte_state.cov_vel_uav[1] = state_var_y(vtest::State::vel_uav);
+ _vte_state.cov_vel_uav[2] = state_var_z(vtest::State::vel_uav);
+
+ _vte_state.bias[0] = state_x(vtest::State::bias);
+ _vte_state.bias[1] = state_y(vtest::State::bias);
+ _vte_state.bias[2] = state_z(vtest::State::bias);
+
+ _vte_state.cov_bias[0] = state_var_x(vtest::State::bias);
+ _vte_state.cov_bias[1] = state_var_y(vtest::State::bias);
+ _vte_state.cov_bias[2] = state_var_z(vtest::State::bias);
+
+#if defined(CONFIG_VTEST_MOVING)
+
+ _vte_state.vel_target[0] = state_x(vtest::State::vel_target);
+ _vte_state.vel_target[1] = state_y(vtest::State::vel_target);
+ _vte_state.vel_target[2] = state_z(vtest::State::vel_target);
+
+ _vte_state.cov_vel_target[0] = state_var_x(vtest::State::vel_target);
+ _vte_state.cov_vel_target[1] = state_var_y(vtest::State::vel_target);
+ _vte_state.cov_vel_target[2] = state_var_z(vtest::State::vel_target);
+
+ _vte_state.acc_target[0] = state_x(vtest::State::acc_target);
+ _vte_state.acc_target[1] = state_y(vtest::State::acc_target);
+ _vte_state.acc_target[2] = state_z(vtest::State::acc_target);
+
+ _vte_state.cov_acc_target[0] = state_var_x(vtest::State::acc_target);
+ _vte_state.cov_acc_target[1] = state_var_y(vtest::State::acc_target);
+ _vte_state.cov_acc_target[2] = state_var_z(vtest::State::acc_target);
+
+#endif // CONFIG_VTEST_MOVING
+
+ // If the target is static, valid and vision obs was fused recently, use the relative to aid the EKF2 state estimation.
+ // Check performed in EKF2 to use target vel: if (landing_target_pose.is_static && landing_target_pose.rel_vel_valid)
+ const hrt_abstime now = hrt_absolute_time();
+ _target_pose.rel_vel_valid = _target_pose.is_static && _param_vte_ekf_aid.get() && _target_pose.rel_pos_valid &&
+ (_last_relative_meas_fused_time != 0) && (now >= _last_relative_meas_fused_time) &&
+ (now - _last_relative_meas_fused_time) < _meas_recent_timeout_us;
+ _vte_state.rel_pos_valid = _target_pose.rel_pos_valid;
+ _vte_state.rel_vel_valid = _target_pose.rel_vel_valid;
+
+ // Prec land does not check _target_pose.abs_pos_valid. Only send the target if abs pose valid.
+ if (_local_position.valid && _target_pose.rel_pos_valid) {
+ _target_pose.x_abs = _target_pose.x_rel + _local_position.xyz(0);
+ _target_pose.y_abs = _target_pose.y_rel + _local_position.xyz(1);
+ _target_pose.z_abs = _target_pose.z_rel + _local_position.xyz(2);
+ _target_pose.abs_pos_valid = true;
+
+#if defined(CONFIG_VTEST_MOVING)
+
+ // If the target is moving, move towards its expected location
+ float mpc_z_v_auto_dn = _mpc_z_v_auto_dn;
+
+ static constexpr float kMinAbsMpcZVAutoDnMps = 1e-3f;
+
+ if (fabsf(mpc_z_v_auto_dn) < kMinAbsMpcZVAutoDnMps) {
+ mpc_z_v_auto_dn = (mpc_z_v_auto_dn >= 0.f) ? kMinAbsMpcZVAutoDnMps : -kMinAbsMpcZVAutoDnMps;
+ }
+
+ float intersection_time_s = fabsf(_target_pose.z_rel / mpc_z_v_auto_dn);
+ intersection_time_s = math::constrain(intersection_time_s, _param_vte_moving_t_min.get(),
+ _param_vte_moving_t_max.get());
+
+ // Anticipate where the target will be
+ _target_pose.x_abs += _vte_state.vel_target[0] * intersection_time_s;
+ _target_pose.y_abs += _vte_state.vel_target[1] * intersection_time_s;
+ _target_pose.z_abs += _vte_state.vel_target[2] * intersection_time_s;
+
+#endif // CONFIG_VTEST_MOVING
+
+ _target_pose_pub.publish(_target_pose);
+
+ }
+
+ _target_estimator_state_pub.publish(_vte_state);
+
+ // TODO: decide what to do with Bias lim
+ float bias_lim = _param_vte_bias_lim.get();
+
+ if ((fabsf(_vte_state.bias[0]) > bias_lim)
+ || (fabsf(_vte_state.bias[1]) > bias_lim)
+ || (fabsf(_vte_state.bias[2]) > bias_lim)) {
+
+ PX4_DEBUG("Bias exceeds limit: %.2f bias x: %.2f bias y: %.2f bias z: %.2f", (double)bias_lim,
+ (double)_vte_state.bias[0], (double)_vte_state.bias[1], (double)_vte_state.bias[2]);
+
+ // resetFilter();
+ }
+
+}
+
+void VTEPosition::checkMeasurementInputs()
+{
+ // Make sure local position and local velocity are up to date.
+ if (_local_position.valid) {
+ _local_position.valid = isMeasUpdated(_local_position.timestamp);
+ }
+
+ if (_local_velocity.valid) {
+ _local_velocity.valid = isMeasUpdated(_local_velocity.timestamp);
+ }
+
+#if defined(CONFIG_VTEST_MOVING)
+
+ if (_target_gps_vel.valid) {
+ _target_gps_vel.valid = isMeasUpdated(_target_gps_vel.timestamp);
+ }
+
+#endif // CONFIG_VTEST_MOVING
+
+ if (_pos_rel_gnss.valid) {
+ _pos_rel_gnss.valid = isMeasUpdated(_pos_rel_gnss.timestamp);
+ }
+
+ if (_velocity_offset_ned.valid) {
+ _velocity_offset_ned.valid = isMeasUpdated(_velocity_offset_ned.timestamp);
+ }
+
+ if (_gps_pos_offset_ned.valid) {
+ _gps_pos_offset_ned.valid = isMeasUpdated(_gps_pos_offset_ned.timestamp);
+ }
+
+ if (_uav_gps_vel.valid) {
+ _uav_gps_vel.valid = isMeasUpdated(_uav_gps_vel.timestamp);
+ }
+}
+
+void VTEPosition::setGpsPosOffset(const matrix::Vector3f &xyz, const bool gps_is_offset)
+{
+ _gps_pos_is_offset = gps_is_offset;
+ _gps_pos_offset_ned.xyz = xyz;
+ _gps_pos_offset_ned.valid = PX4_ISFINITE(xyz(0))
+ && PX4_ISFINITE(xyz(1))
+ && PX4_ISFINITE(xyz(2));
+ _gps_pos_offset_ned.timestamp = hrt_absolute_time();
+}
+
+void VTEPosition::setVelOffset(const matrix::Vector3f &xyz)
+{
+ _velocity_offset_ned.xyz = xyz;
+ _velocity_offset_ned.valid = PX4_ISFINITE(xyz(0))
+ && PX4_ISFINITE(xyz(1))
+ && PX4_ISFINITE(xyz(2));
+ _velocity_offset_ned.timestamp = hrt_absolute_time();
+}
+
+void VTEPosition::setLocalVelocity(const matrix::Vector3f &vel_xyz, const bool vel_valid, hrt_abstime timestamp)
+{
+ _local_velocity.xyz = vel_xyz;
+ _local_velocity.valid = vel_valid && isMeasUpdated(timestamp);
+ _local_velocity.timestamp = timestamp;
+}
+
+void VTEPosition::setLocalPosition(const matrix::Vector3f &xyz, const bool pos_valid, hrt_abstime timestamp)
+{
+ _local_position.xyz = xyz;
+ _local_position.valid = pos_valid && isMeasUpdated(timestamp);
+ _local_position.timestamp = timestamp;
+}
+
+void VTEPosition::setMissionPosition(const double lat_deg, const double lon_deg, const float alt_m)
+{
+ if (_vte_aid_mask.flags.use_mission_pos) {
+ _mission_land_position.lat_deg = lat_deg;
+ _mission_land_position.lon_deg = lon_deg;
+ _mission_land_position.alt_m = alt_m;
+ _mission_land_position.valid = isLatLonAltValid(_mission_land_position.lat_deg, _mission_land_position.lon_deg,
+ _mission_land_position.alt_m, "Mission position ", &_mission_pos_warn_last);
+
+ if (_mission_land_position.valid) {
+ PX4_INFO("Mission position lat %.8f, lon %.8f [deg], alt %.1f [m]", lat_deg,
+ lon_deg, (double)(alt_m));
+
+ } else {
+ if (shouldEmitWarning(_mission_pos_status_warn_last)) {
+ PX4_WARN("Mission position not used because not valid");
+ }
+ }
+
+ } else {
+ _mission_land_position.valid = false;
+ }
+}
+
+void VTEPosition::updateParams()
+{
+ parameter_update_s pupdate;
+ _parameter_update_sub.copy(&pupdate);
+
+ ModuleParams::updateParams();
+
+#if defined(CONFIG_VTEST_MOVING)
+
+ if (_param_mpc_z_v_auto_dn != PARAM_INVALID) {
+ float new_mpc_z_v_auto_dn = _mpc_z_v_auto_dn;
+
+ if (param_get(_param_mpc_z_v_auto_dn, &new_mpc_z_v_auto_dn) == PX4_OK) {
+ _mpc_z_v_auto_dn = new_mpc_z_v_auto_dn;
+ }
+ }
+
+#endif // CONFIG_VTEST_MOVING
+}
+
+float VTEPosition::getMinGpsVelVar() const
+{
+ return sq(fmaxf(_param_vte_gps_vel_noise.get(), kMinObservationNoise));
+}
+
+float VTEPosition::getMinGpsPosVar() const
+{
+ return sq(fmaxf(_param_vte_gps_pos_noise.get(), kMinObservationNoise));
+}
+
+float VTEPosition::getBiasAveragingThreshold() const
+{
+ return fmaxf(_param_vte_bias_avg_thr.get(), 0.f);
+}
+
+uint32_t VTEPosition::getBiasAveragingTimeoutUs() const
+{
+ return static_cast(fmaxf(_param_vte_bias_avg_tout.get(), 0.f) * 1e6f);
+}
+
+bool VTEPosition::isLatLonAltValid(double lat_deg, double lon_deg, float alt_m, const char *who,
+ hrt_abstime *warn_last)
+{
+ // all finite
+ if (!PX4_ISFINITE(lat_deg) || !PX4_ISFINITE(lon_deg) || !PX4_ISFINITE(alt_m)) {
+ if (who && (!warn_last || shouldEmitWarning(*warn_last))) {
+ PX4_WARN("%s position not finite! lat: %.8f, lon: %.8f, alt: %.1f",
+ who, lat_deg, lon_deg, (double)alt_m);
+ }
+
+ return false;
+ }
+
+ // latitude/longitude within symmetric geographic ranges
+ if ((fabs(lat_deg) > kLatAbsMaxDeg) || (fabs(lon_deg) > kLonAbsMaxDeg)) {
+ if (who && (!warn_last || shouldEmitWarning(*warn_last))) {
+ PX4_WARN("%s lat/lon out of range! lat: %.8f, lon: %.8f", who, lat_deg, lon_deg);
+ }
+
+ return false;
+ }
+
+ // (0,0) sentinel is considered invalid
+ if ((fabs(lat_deg) < DBL_EPSILON) && (fabs(lon_deg) < DBL_EPSILON)) {
+ if (who && (!warn_last || shouldEmitWarning(*warn_last))) {
+ PX4_WARN("%s position near (0,0)!", who);
+ }
+
+ return false;
+ }
+
+ // altitude window
+ if ((alt_m < kAltMinM) || (alt_m > kAltMaxM)) {
+ if (who && (!warn_last || shouldEmitWarning(*warn_last))) {
+ PX4_WARN("%s altitude out of range! alt: %.1f [m] (limits: %.1f;%.1f)",
+ who, (double)alt_m, (double)kAltMinM, (double)kAltMaxM);
+ }
+
+ return false;
+ }
+
+ return true;
+}
+
+} // namespace vision_target_estimator
diff --git a/src/modules/vision_target_estimator/Position/VTEPosition.h b/src/modules/vision_target_estimator/Position/VTEPosition.h
new file mode 100644
index 000000000000..25eee2243492
--- /dev/null
+++ b/src/modules/vision_target_estimator/Position/VTEPosition.h
@@ -0,0 +1,419 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2025 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file VTEPosition.h
+ * @brief Estimate the state of a target by processing and fusing sensor data in
+ * a Kalman Filter.
+ *
+ * @author Jonas Perolini
+ *
+ */
+
+#pragma once
+
+#include "../common.h"
+#include "KF_position.h"
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+namespace vision_target_estimator
+{
+
+class VTEPosition : public ModuleParams
+{
+public:
+ VTEPosition();
+ virtual ~VTEPosition();
+
+ /*
+ * Get new measurements and update the state estimate
+ */
+ void update(const matrix::Vector3f &acc_ned);
+
+ bool init();
+
+ void resetFilter();
+
+ void setMissionPosition(double lat_deg, double lon_deg, float alt_m);
+ void setLocalVelocity(const matrix::Vector3f &vel_xyz, bool valid, hrt_abstime timestamp);
+ void setLocalPosition(const matrix::Vector3f &xyz, bool valid, hrt_abstime timestamp);
+ void setGpsPosOffset(const matrix::Vector3f &xyz, bool gps_is_offset);
+ void setVelOffset(const matrix::Vector3f &xyz);
+ void setVteTimeout(uint32_t tout) { _vte_timeout_us = tout; }
+ void setTargetValidTimeout(uint32_t tout)
+ {
+ _target_valid_timeout_us = tout;
+ }
+ void setMeasRecentTimeout(uint32_t tout)
+ {
+ _meas_recent_timeout_us = tout;
+ }
+ void setMeasUpdatedTimeout(uint32_t tout)
+ {
+ _meas_updated_timeout_us = tout;
+ }
+ void setVteAidMask(uint16_t mask_value)
+ {
+ _vte_aid_mask.value = mask_value;
+ }
+
+ bool timedOut() const
+ {
+ return _estimator_initialized && hasTimedOut(_last_update, _vte_timeout_us);
+ }
+ // TODO: Could be more strict and require a relative position meas (vision, GNSS)
+ bool fusionEnabled() const { return _vte_aid_mask.value != 0; }
+
+protected:
+ static constexpr double kLatAbsMaxDeg = 90.0;
+ static constexpr double kLonAbsMaxDeg = 180.0;
+ static constexpr float kAltMinM = -350.f;
+ static constexpr float kAltMaxM = 10000.f;
+
+ /*
+ * update parameters.
+ */
+ void updateParams() override;
+
+ uORB::Publication _target_pose_pub{ORB_ID(landing_target_pose)};
+ uORB::Publication _target_estimator_state_pub{ORB_ID(vision_target_est_position)};
+
+ // publish innovations target_estimator_gps_pos
+ uORB::Publication _vte_aid_gps_pos_target_pub{ORB_ID(vte_aid_gps_pos_target)};
+ uORB::Publication _vte_aid_gps_pos_mission_pub{ORB_ID(vte_aid_gps_pos_mission)};
+ uORB::Publication _vte_aid_gps_vel_target_pub{ORB_ID(vte_aid_gps_vel_target)};
+ uORB::Publication _vte_aid_gps_vel_uav_pub{ORB_ID(vte_aid_gps_vel_uav)};
+ uORB::Publication _vte_aid_fiducial_marker_pub{ORB_ID(vte_aid_fiducial_marker)};
+ uORB::Publication _vte_bias_init_status_pub{ORB_ID(vte_bias_init_status)};
+
+ uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
+
+private:
+ // Observation types used by the estimator. Keep ordering stable for array indexing.
+ enum class ObsType : uint8_t {
+ kTargetGpsPos,
+ kMissionGpsPos,
+ kUavGpsVel,
+ kTargetGpsVel,
+ kFiducialMarker,
+ kTypeCount
+ };
+
+ enum class PreBiasReference : uint8_t {
+ kUnknown,
+ kVision,
+ kGnss
+ };
+
+ static constexpr size_t kObsTypeCount = static_cast(ObsType::kTypeCount);
+ static constexpr size_t obsIndex(ObsType type)
+ {
+ return static_cast(type);
+ }
+
+ struct TargetObs {
+
+ ObsType type;
+ hrt_abstime timestamp = 0;
+
+ matrix::Vector updated{}; // Indicates if observations were updated.
+ matrix::Vector3f meas_xyz{}; // Measurements (meas_x, meas_y, meas_z)
+ matrix::Vector3f meas_unc_xyz{}; // Measurements' uncertainties
+ matrix::Matrix
+ meas_h_xyz{}; // Observation matrix where the rows correspond to the
+ // x,y,z observations and the columns to the state
+ };
+
+ union ObsValidMaskU {
+ struct {
+ uint8_t fuse_target_gps_pos : 1; ///< bit0: target GPS position ready to be fused
+ uint8_t fuse_uav_gps_vel : 1; ///< bit1: UAV GPS velocity ready to be fused
+ uint8_t fuse_vision : 1; ///< bit2: external vision-relative position ready to be fused
+ uint8_t fuse_mission_pos : 1; ///< bit3: mission position ready to be fused
+ uint8_t fuse_target_gps_vel : 1; ///< bit4: target GPS velocity ready to be fused
+ uint8_t reserved : 3; ///< bits5..7: reserved for future use
+ } flags;
+
+ uint8_t value{0};
+ };
+
+ static_assert(sizeof(ObsValidMaskU) == 1, "Unexpected masking size");
+
+ using EstimatorState = KF_position::VectorState;
+ using EstimatorStateCovariance = KF_position::SquareMatrixState;
+ using AxisEstimatorStates = px4::Array;
+
+ bool initEstimator(const AxisEstimatorStates &state_init);
+ bool performUpdateStep(const matrix::Vector3f &vehicle_acc_ned);
+ void predictionStep(const matrix::Vector3f &acc, float dt);
+ float getMinGpsVelVar() const;
+ float getMinGpsPosVar() const;
+ float getBiasAveragingThreshold() const;
+ uint32_t getBiasAveragingTimeoutUs() const;
+
+ inline bool isMeasRecent(hrt_abstime ts) const
+ {
+ return !hasTimedOut(ts, _meas_recent_timeout_us);
+ }
+
+ inline bool isMeasUpdated(hrt_abstime ts) const
+ {
+ return !hasTimedOut(ts, _meas_updated_timeout_us);
+ }
+
+ inline bool hasNewPositionSensorData(const ObsValidMaskU &fusion_mask) const
+ {
+ return fusion_mask.flags.fuse_mission_pos ||
+ fusion_mask.flags.fuse_target_gps_pos ||
+ fusion_mask.flags.fuse_vision;
+ }
+
+ inline bool isTimeDifferenceWithin(const hrt_abstime a, const hrt_abstime b, const uint32_t timeout_us) const
+ {
+ if ((a == 0) || (b == 0)) {
+ return false;
+ }
+
+ const hrt_abstime diff = (a > b) ? (a - b) : (b - a);
+ return diff <= timeout_us;
+ }
+
+ // Only estimate the GNSS bias if we have a GNSS estimation and a secondary source of position
+ inline bool shouldSetBias(const ObsValidMaskU &fusion_mask) const
+ {
+ return _pos_rel_gnss.valid && isMeasRecent(_pos_rel_gnss.timestamp) && fusion_mask.flags.fuse_vision;
+ };
+
+ inline bool isBiasAveragingEnabled() const
+ {
+ return getBiasAveragingThreshold() > 0.f && (getBiasAveragingTimeoutUs() > 0);
+ }
+
+ inline bool shouldAverageInitialBias(const ObsValidMaskU &fusion_mask,
+ const PreBiasReference reference) const
+ {
+ return shouldSetBias(fusion_mask) && (reference == PreBiasReference::kGnss)
+ && isBiasAveragingEnabled();
+ }
+
+ inline bool shouldAverageInitialBias(const ObsValidMaskU &fusion_mask) const
+ {
+ return shouldAverageInitialBias(fusion_mask, _pre_bias_reference);
+ }
+
+ inline bool shouldBlockVisionFusionUntilBiasReady() const
+ {
+ // Vision must wait until the GNSS bias is ready when the state is still
+ // referenced to GNSS.
+ return !_bias.set && (_pre_bias_reference == PreBiasReference::kGnss);
+ }
+
+ inline bool shouldBlockGnssFusionUntilBiasReady() const
+ {
+ // GNSS position must wait until the GNSS bias is ready when the state is
+ // still referenced to vision.
+ return !_bias.set && (_pre_bias_reference == PreBiasReference::kVision);
+ }
+
+ bool initializeEstimator(const ObsValidMaskU &fusion_mask,
+ const TargetObs observations[kObsTypeCount]);
+ void updateBiasIfObservable(const ObsValidMaskU &fusion_mask,
+ const TargetObs observations[kObsTypeCount]);
+ bool selectBiasGnssSample(hrt_abstime sample_time, matrix::Vector3f &gnss_sample);
+ PreBiasReference selectInitialPosition(const ObsValidMaskU &fusion_mask,
+ const TargetObs observations[kObsTypeCount], matrix::Vector3f &initial_position,
+ bool prefer_vision = true);
+ bool fuseActiveMeasurements(const matrix::Vector3f &vehicle_acc_ned, ObsValidMaskU &fusion_mask,
+ const TargetObs observations[kObsTypeCount]);
+ void processObservations(ObsValidMaskU &fusion_mask,
+ TargetObs observations[kObsTypeCount]);
+ bool getOffsetUavVel(hrt_abstime sample_time, matrix::Vector3f &uav_velocity_ned) const;
+ void startBiasAveraging(const matrix::Vector3f &bias_sample, hrt_abstime sample_time);
+ bool updateBiasAveraging(const matrix::Vector3f &bias_sample, hrt_abstime sample_time);
+ void resetBiasAveraging();
+ void activateBiasEstimate(const matrix::Vector3f &initial_position, const matrix::Vector3f &initial_bias);
+ void publishBiasInitStatus(const matrix::Vector3f &raw_bias, const matrix::Vector3f &filtered_bias,
+ float raw_bias_delta_norm, hrt_abstime sample_time);
+
+ bool isLatLonAltValid(double lat_deg, double lon_deg, float alt_m, const char *who = nullptr,
+ hrt_abstime *warn_last = nullptr);
+
+ /* Vision data */
+ bool isVisionDataValid(const fiducial_marker_pos_report_s &fiducial_marker_pose);
+ bool processObsVision(TargetObs &obs);
+
+ /* UAV GPS data */
+ bool updateUavGpsData();
+ bool isUavGpsPositionValid();
+ bool isUavGpsVelocityValid();
+ bool processObsGNSSPosMission(TargetObs &obs);
+ bool processObsGNSSVelUav(TargetObs &obs) const;
+
+ /* Target GPS data */
+ bool isTargetGpsPositionValid(const target_gnss_s &target_gnss);
+ bool isTargetGpsVelocityValid(const target_gnss_s &target_gnss);
+ bool processObsGNSSPosTarget(const target_gnss_s &target_gnss, TargetObs &obs);
+#if defined(CONFIG_VTEST_MOVING)
+ bool processObsGNSSVelTarget(const target_gnss_s &target_gnss, TargetObs &obs) const;
+ void updateTargetGpsVelocity(const target_gnss_s &target_gnss);
+#endif // CONFIG_VTEST_MOVING
+
+ bool fuseMeas(const matrix::Vector3f &vehicle_acc_ned, const TargetObs &target_pos_obs);
+ void publishTarget();
+ void publishInnov(const vte_aid_source3d_s &target_innov, const ObsType type);
+ void resetObservations();
+ bool shouldEmitWarning(hrt_abstime &last_warn);
+
+ uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
+ uORB::Subscription _fiducial_marker_pos_report_sub{ORB_ID(fiducial_marker_pos_report)};
+ uORB::Subscription _target_gnss_sub{ORB_ID(target_gnss)};
+
+ perf_counter_t _vte_predict_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": VTE prediction")};
+ perf_counter_t _vte_update_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": VTE update")};
+ perf_counter_t _vte_fusion_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": VTE fusion")};
+
+ TargetObs _obs_buffer[kObsTypeCount] {};
+ landing_target_pose_s _target_pose{};
+ vision_target_est_position_s _vte_state{};
+
+ struct GlobalPose {
+ hrt_abstime timestamp = 0;
+ bool valid = false;
+ double lat_deg = 0.0; // Latitude in degrees
+ double lon_deg = 0.0; // Longitude in degrees
+ float alt_m = 0.f; // Altitude in meters AMSL
+ float eph = 0.f;
+ float epv = 0.f;
+ };
+
+ GlobalPose _mission_land_position{};
+ GlobalPose _uav_gps_position{};
+
+ struct VelStamped {
+ hrt_abstime timestamp = 0;
+ bool valid = false;
+ matrix::Vector3f xyz{};
+ float uncertainty = 0.f;
+ };
+
+ VelStamped _uav_gps_vel{};
+
+ Vector3fStamped _local_position{};
+ Vector3fStamped _local_velocity{};
+ Vector3fStamped _pos_rel_gnss{};
+ Vector3fStamped _velocity_offset_ned{};
+ Vector3fStamped _gps_pos_offset_ned{};
+
+#if defined(CONFIG_VTEST_MOVING)
+ Vector3fStamped _target_gps_vel {};
+ float _mpc_z_v_auto_dn{0.f};
+ param_t _param_mpc_z_v_auto_dn{PARAM_INVALID};
+#endif // CONFIG_VTEST_MOVING
+
+ struct BiasState {
+ bool set{false};
+ bool averaging_active{false};
+ uint8_t stable_delta_count{0};
+ hrt_abstime averaging_start_time{0};
+ hrt_abstime last_sample_time{0};
+ AlphaFilter initial_lpf{};
+ };
+
+ bool _gps_pos_is_offset{false};
+ BiasState _bias{};
+ PreBiasReference _pre_bias_reference{PreBiasReference::kUnknown};
+
+ uint64_t _last_relative_meas_fused_time{0};
+ bool _estimator_initialized{false};
+
+ px4::Array _target_est_pos{};
+
+ hrt_abstime _last_predict{0}; // timestamp of last filter prediction
+ hrt_abstime _last_update{0}; // timestamp of last filter update (used to check timeout)
+ hrt_abstime _uav_gps_vel_warn_last{0};
+ hrt_abstime _target_gps_vel_warn_last{0};
+ hrt_abstime _vision_pos_warn_last{0};
+ hrt_abstime _uav_gps_pos_warn_last{0};
+ hrt_abstime _target_gps_pos_warn_last{0};
+ hrt_abstime _mission_pos_warn_last{0};
+ hrt_abstime _mission_pos_status_warn_last{0};
+ hrt_abstime _init_vel_warn_last{0};
+
+ /* parameters from vision_target_estimator_params.yaml */
+ void checkMeasurementInputs();
+
+ uint32_t _vte_timeout_us{3_s};
+ uint32_t _target_valid_timeout_us{2_s};
+ uint32_t _meas_recent_timeout_us{1_s};
+ uint32_t _meas_updated_timeout_us{100_ms};
+ SensorFusionMaskU _vte_aid_mask{};
+
+ DEFINE_PARAMETERS(
+ (ParamFloat) _param_vte_acc_d_unc,
+ (ParamFloat) _param_vte_acc_t_unc,
+ (ParamFloat) _param_vte_bias_lim,
+ (ParamFloat) _param_vte_bias_unc,
+ (ParamFloat) _param_vte_bias_avg_thr,
+ (ParamFloat) _param_vte_bias_avg_tout,
+ (ParamFloat) _param_vte_pos_unc_in,
+ (ParamFloat) _param_vte_vel_unc_in,
+ (ParamFloat) _param_vte_bias_unc_in,
+ (ParamFloat) _param_vte_acc_unc_in,
+ (ParamFloat) _param_vte_gps_vel_noise,
+ (ParamFloat) _param_vte_gps_pos_noise,
+ (ParamFloat) _param_vte_ev_pos_noise,
+ (ParamInt) _param_vte_ekf_aid,
+ (ParamFloat) _param_vte_moving_t_max,
+ (ParamFloat) _param_vte_moving_t_min,
+ (ParamFloat) _param_vte_pos_nis_thre
+ )
+};
+} // namespace vision_target_estimator
diff --git a/src/modules/vision_target_estimator/Position/vtest_derivation/derivation.py b/src/modules/vision_target_estimator/Position/vtest_derivation/derivation.py
new file mode 100755
index 000000000000..8e288680c092
--- /dev/null
+++ b/src/modules/vision_target_estimator/Position/vtest_derivation/derivation.py
@@ -0,0 +1,183 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+ Copyright (c) 2022-2026 PX4 Development Team
+ Redistribution and use in source and binary forms, with or without
+ modification, are permitted provided that the following conditions
+ are met:
+
+ 1. Redistributions of source code must retain the above copyright
+ notice, this list of conditions and the following disclaimer.
+ 2. Redistributions in binary form must reproduce the above copyright
+ notice, this list of conditions and the following disclaimer in
+ the documentation and/or other materials provided with the
+ distribution.
+ 3. Neither the name PX4 nor the names of its contributors may be
+ used to endorse or promote products derived from this software
+ without specific prior written permission.
+
+ THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ POSSIBILITY OF SUCH DAMAGE.
+
+File: derivation.py
+Description:
+ Derivation of KF
+"""
+
+import argparse
+
+import symforce
+symforce.set_epsilon_to_symbol()
+
+import symforce.symbolic as sf
+from symforce import typing as T
+from symforce import ops
+from symforce.values import Values
+
+import sympy as sp
+from utils.derivation_utils import *
+
+# Initialize parser
+parser = argparse.ArgumentParser()
+
+parser.add_argument("--moving", action='store_true', help="generate equations for moving filter")
+
+# Read arguments from command line
+args = parser.parse_args()
+moving = args.moving
+
+# The state vector is organized in an ordered dictionary
+State = Values(
+ pos_rel=sf.V1(),
+ vel_uav=sf.V1(),
+ bias=sf.V1(),
+ acc_target=sf.V1(),
+ vel_target=sf.V1()
+)
+
+if not moving:
+ del State["acc_target"]
+ del State["vel_target"]
+
+class IdxDof():
+ def __init__(self, idx, dof):
+ self.idx = idx
+ self.dof = dof
+
+def BuildTangentStateIndex():
+ tangent_state_index = {}
+ idx = 0
+ for key in State.keys_recursive():
+ dof = State[key].tangent_dim()
+ tangent_state_index[key] = IdxDof(idx, dof)
+ idx += dof
+ return tangent_state_index
+
+tangent_idx = BuildTangentStateIndex()
+
+class VState(sf.Matrix):
+ SHAPE = (State.storage_dim(), 1)
+
+class MState(sf.Matrix):
+ SHAPE = (State.storage_dim(), State.storage_dim())
+
+class VMeas(sf.Matrix):
+ SHAPE = (1, State.storage_dim())
+
+def symbolic_state(state_structure):
+ symbolic_state = Values()
+ for key in state_structure.keys_recursive():
+ var_type = type(state_structure[key])
+ if hasattr(var_type, 'symbolic'):
+ symbolic_state[key] = var_type.symbolic(key)
+
+ return symbolic_state
+
+def get_Phi_and_G(dt: sf.Scalar,
+) -> MState:
+
+ state = symbolic_state(State)
+
+ # Input acceleration (symbolic)
+ acc = sf.Symbol('acc')
+
+ state_pred = state.copy()
+ state_pred["pos_rel"] = state["pos_rel"] - dt * state["vel_uav"] - 0.5 * dt**2 * acc
+ state_pred["vel_uav"] = state["vel_uav"] + dt * acc
+ state_pred["bias"] = state["bias"]
+
+ if moving:
+ state_pred["pos_rel"] += dt * state["vel_target"] + 0.5 * dt**2 * state["acc_target"]
+ state_pred["vel_target"] = state["vel_target"] + dt * state["acc_target"]
+ state_pred["acc_target"] = state["acc_target"]
+
+ # State vector
+ state_pred_matrix = VState(state_pred.to_storage())
+
+ # Compute Phi = ∂f/∂x
+ Phi = state_pred_matrix.jacobian([state])
+ # Compute G = ∂f/∂w
+ G = state_pred_matrix.jacobian(acc)
+
+ return Phi, G
+
+def predictState(dt: sf.Scalar, state: VState, acc: sf.Scalar) -> VState:
+ Phi, G = get_Phi_and_G(dt)
+ return Phi * state + G * acc
+
+def getTransitionMatrix(dt: sf.Scalar) -> MState:
+ Phi, _ = get_Phi_and_G(dt)
+ return Phi
+
+def predictCov(dt: sf.Scalar, input_var: sf.Scalar, bias_var: sf.Scalar, acc_var: sf.Scalar, covariance: MState) -> MState:
+ Phi, G = get_Phi_and_G(dt)
+ Q = sf.Matrix.zeros(State.storage_dim(), State.storage_dim())
+
+ # Scale bias random walk by dt
+ idx_bias = tangent_idx['bias'].idx
+ Q[idx_bias, idx_bias] = bias_var * dt
+
+ if moving:
+ # Scale target acceleration random walk by dt
+ idx_a_target = tangent_idx['acc_target'].idx
+ Q[idx_a_target, idx_a_target] = acc_var * dt
+
+ return G * input_var * G.T + Q + Phi * covariance * Phi.T
+
+def computeInnovCov(meas_unc: sf.Scalar, covariance: MState, meas_matrix: VMeas) -> sf.Scalar:
+ return (meas_matrix * covariance * meas_matrix.T)[0, 0] + meas_unc
+
+# OOSM correction
+def applyCorrection(state: VState, cov: MState, K: VState, innov: sf.Scalar, S: sf.Scalar) -> (VState, MState):
+
+ # Update State
+ state_new = state + K * innov
+
+ # Update Covariance
+ cov_update = K * S * K.T
+ cov_new = cov - cov_update
+
+ # Force Symmetry
+ cov_new = 0.5 * (cov_new + cov_new.T)
+
+ return state_new, cov_new
+
+# Generate functions
+print(f"Derive VTEST equations for {'moving' if moving else 'static'} targets...")
+generate_px4_function(predictState, output_names=["predict_state"])
+generate_px4_function(getTransitionMatrix, output_names=["transition_matrix"])
+generate_px4_function(predictCov, output_names=["cov_updated"])
+generate_px4_function(computeInnovCov, output_names=["innov_cov_updated"])
+generate_px4_function(applyCorrection, output_names=["state_new", "cov_new"])
+
+generate_px4_state(State, tangent_idx)
diff --git a/src/modules/vision_target_estimator/Position/vtest_derivation/generated/applyCorrection.h b/src/modules/vision_target_estimator/Position/vtest_derivation/generated/applyCorrection.h
new file mode 100644
index 000000000000..d0bd6dbbf870
--- /dev/null
+++ b/src/modules/vision_target_estimator/Position/vtest_derivation/generated/applyCorrection.h
@@ -0,0 +1,76 @@
+// -----------------------------------------------------------------------------
+// This file was autogenerated by symforce from template:
+// function/FUNCTION.h.jinja
+// Do NOT modify by hand.
+// -----------------------------------------------------------------------------
+
+#pragma once
+
+#include
+
+namespace sym
+{
+
+/**
+ * This function was autogenerated from a symbolic function. Do not modify by hand.
+ *
+ * Symbolic function: applyCorrection
+ *
+ * Args:
+ * state: Matrix31
+ * cov: Matrix33
+ * K: Matrix31
+ * innov: Scalar
+ * S: Scalar
+ *
+ * Outputs:
+ * state_new: Matrix31
+ * cov_new: Matrix33
+ */
+template
+void Applycorrection(const matrix::Matrix &state,
+ const matrix::Matrix &cov, const matrix::Matrix &K,
+ const Scalar innov, const Scalar S,
+ matrix::Matrix *const state_new = nullptr,
+ matrix::Matrix *const cov_new = nullptr)
+{
+ // Total ops: 42
+
+ // Input arrays
+
+ // Intermediate terms (5)
+ const Scalar _tmp0 = 2 * S;
+ const Scalar _tmp1 =
+ -Scalar(0.5) * K(0, 0) * K(1, 0) * _tmp0 + Scalar(0.5) * cov(0, 1) + Scalar(0.5) * cov(1, 0);
+ const Scalar _tmp2 = K(2, 0) * _tmp0;
+ const Scalar _tmp3 =
+ -Scalar(0.5) * K(0, 0) * _tmp2 + Scalar(0.5) * cov(0, 2) + Scalar(0.5) * cov(2, 0);
+ const Scalar _tmp4 =
+ -Scalar(0.5) * K(1, 0) * _tmp2 + Scalar(0.5) * cov(1, 2) + Scalar(0.5) * cov(2, 1);
+
+ // Output terms (2)
+ if (state_new != nullptr) {
+ matrix::Matrix &_state_new = (*state_new);
+
+ _state_new(0, 0) = K(0, 0) * innov + state(0, 0);
+ _state_new(1, 0) = K(1, 0) * innov + state(1, 0);
+ _state_new(2, 0) = K(2, 0) * innov + state(2, 0);
+ }
+
+ if (cov_new != nullptr) {
+ matrix::Matrix &_cov_new = (*cov_new);
+
+ _cov_new(0, 0) = -Scalar(0.5) * std::pow(K(0, 0), Scalar(2)) * _tmp0 + Scalar(1.0) * cov(0, 0);
+ _cov_new(1, 0) = _tmp1;
+ _cov_new(2, 0) = _tmp3;
+ _cov_new(0, 1) = _tmp1;
+ _cov_new(1, 1) = -Scalar(0.5) * std::pow(K(1, 0), Scalar(2)) * _tmp0 + Scalar(1.0) * cov(1, 1);
+ _cov_new(2, 1) = _tmp4;
+ _cov_new(0, 2) = _tmp3;
+ _cov_new(1, 2) = _tmp4;
+ _cov_new(2, 2) = -Scalar(0.5) * std::pow(K(2, 0), Scalar(2)) * _tmp0 + Scalar(1.0) * cov(2, 2);
+ }
+} // NOLINT(readability/fn_size)
+
+// NOLINTNEXTLINE(readability/fn_size)
+} // namespace sym
diff --git a/src/modules/vision_target_estimator/Position/vtest_derivation/generated/computeInnovCov.h b/src/modules/vision_target_estimator/Position/vtest_derivation/generated/computeInnovCov.h
new file mode 100644
index 000000000000..fb44b42e5ee9
--- /dev/null
+++ b/src/modules/vision_target_estimator/Position/vtest_derivation/generated/computeInnovCov.h
@@ -0,0 +1,55 @@
+// -----------------------------------------------------------------------------
+// This file was autogenerated by symforce from template:
+// function/FUNCTION.h.jinja
+// Do NOT modify by hand.
+// -----------------------------------------------------------------------------
+
+#pragma once
+
+#include
+
+namespace sym
+{
+
+/**
+ * This function was autogenerated from a symbolic function. Do not modify by hand.
+ *
+ * Symbolic function: computeInnovCov
+ *
+ * Args:
+ * meas_unc: Scalar
+ * covariance: Matrix33
+ * meas_matrix: Matrix13
+ *
+ * Outputs:
+ * innov_cov_updated: Scalar
+ */
+template
+Scalar Computeinnovcov(const Scalar meas_unc, const matrix::Matrix &covariance,
+ const matrix::Matrix &meas_matrix)
+{
+ // Total ops: 21
+
+ // Input arrays
+
+ // Intermediate terms (0)
+
+ // Output terms (1)
+ Scalar _innov_cov_updated;
+
+ _innov_cov_updated = meas_matrix(0, 0) * (covariance(0, 0) * meas_matrix(0, 0) +
+ covariance(1, 0) * meas_matrix(0, 1) +
+ covariance(2, 0) * meas_matrix(0, 2)) +
+ meas_matrix(0, 1) * (covariance(0, 1) * meas_matrix(0, 0) +
+ covariance(1, 1) * meas_matrix(0, 1) +
+ covariance(2, 1) * meas_matrix(0, 2)) +
+ meas_matrix(0, 2) * (covariance(0, 2) * meas_matrix(0, 0) +
+ covariance(1, 2) * meas_matrix(0, 1) +
+ covariance(2, 2) * meas_matrix(0, 2)) +
+ meas_unc;
+
+ return _innov_cov_updated;
+} // NOLINT(readability/fn_size)
+
+// NOLINTNEXTLINE(readability/fn_size)
+} // namespace sym
diff --git a/src/modules/vision_target_estimator/Position/vtest_derivation/generated/getTransitionMatrix.h b/src/modules/vision_target_estimator/Position/vtest_derivation/generated/getTransitionMatrix.h
new file mode 100644
index 000000000000..99ea68d19f03
--- /dev/null
+++ b/src/modules/vision_target_estimator/Position/vtest_derivation/generated/getTransitionMatrix.h
@@ -0,0 +1,48 @@
+// -----------------------------------------------------------------------------
+// This file was autogenerated by symforce from template:
+// function/FUNCTION.h.jinja
+// Do NOT modify by hand.
+// -----------------------------------------------------------------------------
+
+#pragma once
+
+#include