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

[JonasPerolini]

MVP of #22008

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

Changelog Entry

• TBD

Test coverage

• TBD

[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.
• Sort out CONSTRAINED_FLASH
• Check changes in Mavlink receiver
• Finish UWB observation processing.
• Testing: both simulation and real flights.
• Documentation

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

[bresch]

Could you rather write down the equations of motions using the states directly, please?

[JonasPerolini]

Done here: 23dcda0 for position filter and here b2bcc0c for orientation filter. Is this what you meant @bresch?