Draft: Static Vision-based target esitmator (Kalman Filter)

[JonasPerolini]

Vision Target Estimator

Objectives: Extend the current landing target estimator to:

• Estimate the full pose of the target (position + orientation)
• Fuse GNSS (vel / position) and vision.
• Create a general Kalman Filter framework in which it is simple to include other observations e.g. uwb, irlock
• Make it simple to use the Kalman Filter for other purposes than precision landing e.g. precision delivery
• Be robust to temporary vision losses: use the GNSS position of the target (or the mission's landing position) and estimate the GNSS bias using vision
• Update the precision landing algorithm to also control the orientation if enabled

Documentation:

• docs/en/advanced_features/vision_target_estimator.md and
• docs/en/advanced_features/vision_target_estimator_advanced.md

Changelog Entry

• TBD

Test coverage

• TBD

Linked to #22008

[bresch]

I guess you just started so I converted it to draft (needs to remove the "moving" filters and general cleanup).

[JonasPerolini]

Let's see if we can find a solution that does not require removing the moving target filter.

Main changes from #22008:

• Change the state of the static filter. Instead of relative velocity, use the UAV's velocity (since target velocity is zero, UAV_vel = -relative_vel). The goal is to have the same basic states as the moving target filter.

  • Static filter: pos_rel, vel_uav, bias
  • Moving filter: pos_rel, vel_uav, bias, target_vel, target_acc

• As discussed with @bresch , auto-generate the state of the filter depending on whether the target is moving or not (similar to EKF2 e.g. CONFIG_EKF2_WIND), This allows to have only one KF for both moving and static targets. No need for virtual classes anymore. All KF equations are auto-generated using symforce. Also unified the symforce generation of the position, orientation, static, and moving KFs. (Still requires file renaming and avoiding duplicate use of derivation_utils.py)

  • Done in these commits for position and orientation filters.

• Update VTEPosition to use the unified filter 6d2ab35

  • Simplifies init of filters.
  • Clarifies state vs augmented state

• Better observation processing processObservations: 7d21f16

• Refactor: Split main functions into smaller ones to enhance modularity and readability. Mainly:

  • VisionTargetEst::Run(): d7ade2b
  • VTEPosition::update_step: 40ff4e8

• Include support for UWB. @NXPLoic please note that I've only worked on the main structure (cc97967 and 9680dc4). Work is still required to properly get the NED position of the target and the variance

TODO:

• File renaming
• Orientation KF, only keep moving filter --> b2bcc0c
• Flash is better now but still issues (including uwb did not help)
• Handle #if defined(CONFIG_VTEST_MOVING) in Cmake
• Extend observation checks.
• Check changes in Mavlink receiver
• Sort out CONSTRAINED_FLASH
• Documentation
• Finish UWB observation processing.
• Testing simulation
• Testing real flights.
• Once merged, the TARGET_RELATIVE and TARGET_ABSOLUTE MAVLINK messages must be moved from development.xml to common.xml. Then update the custom build images and remove CONFIG_MAVLINK_DIALECT="development"

Side note: to include a new sensor e.g. irlock, follow steps from cc97967 and 9680dc4

[JonasPerolini]

Good evening @bresch @dagar , could you please review this PR when you get a chance?

I've updated the documentation, plelase see docs/en/advanced_features/vision_target_estimator.md and docs/en/advanced_features/vision_target_estimator_advanced.md for the necessary context of the PR.

[DronecodeBot]

This pull request has been mentioned on Discussion Forum for PX4, Pixhawk, QGroundControl, MAVSDK, MAVLink. There might be relevant details there:

https://discuss.px4.io/t/px4-dev-call-sep-17-2025-team-sync-and-community-q-a/47445/3

[DronecodeBot]

This pull request has been mentioned on Discussion Forum for PX4, Pixhawk, QGroundControl, MAVSDK, MAVLink. There might be relevant details there:

https://discuss.px4.io/t/px4-dev-call-sep-24-2025-team-sync-and-community-q-a/47494/1

[DronecodeBot]

This pull request has been mentioned on Discussion Forum for PX4, Pixhawk, QGroundControl, MAVSDK, MAVLink. There might be relevant details there:

https://discuss.px4.io/t/precision-moving-landing-development-status/47613/1

[JonasPerolini]

Thank you for the review @bresch I've applied all the requested changes in c58be72

[JonasPerolini]

@dagar please note that as discussed during the dev call I've reverted the changes inside: Tools/msg/px_generate_uorb_topic_files.py and Tools/zenoh/px_generate_zenoh_topic_files.py and created new Uorb messages specifically for the vision target estimator 6a68951

[JonasPerolini]

Hi @bresch, this commit: a21ade6 moves the EKF2 ring buffer class to the lib folder to be used by other modules. Since the there is already a FIFO RingBuffer in the lib folder, I've renamed the EKF2 ring buffer to TimestampedRingBuffer because it contains the field time_us

For my specific use case, I know the size of the ring buffer so I created a static variant. I also need a couple of new functions which I've included in both the static and dynamic version. Here's a detail of the changes:

1. New ring buffer static version
2. Inside reset() : change _buffer[i] = {}; to _buffer[i].time_us = 0;
3. A protection againts size > UINT8_MAX
4. New functions:
   • bool empty() const { return entries() == 0; }
   • 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; }
   • const data_type &get_newest() const { return _buffer[_head]; }

Unit tests passed:

Start 62: unit-TimestampedRingBufferDynamic
1/2 Test #62: unit-TimestampedRingBufferDynamic ...   Passed    0.00 sec
Start 63: unit-TimestampedRingBufferStatic
2/2 Test #63: unit-TimestampedRingBufferStatic ....   Passed    0.00 sec
100% tests passed, 0 tests failed out of 2

[DronecodeBot]

This pull request has been mentioned on Discussion Forum for PX4, Pixhawk, QGroundControl, MAVSDK, MAVLink. There might be relevant details there:

https://discuss.px4.io/t/landmark-precision-landing-system/47408/9

[bresch]

Did you check the CPU load for this? If the 25 samples of the buffer are updated for every measurement, this can start to become significant.