EKF2: rotate vel on heading reset

[haumarco]

Solved Problem

After a heading reset, the velocity vector was not adjusted accordingly, leading to inconsistent body-velocity behavior. For example, if a fixed-wing aircraft receives a 90° heading reset, the velocity vector remains in its original orientation. This causes the aircraft to appear to fly sideways relative to its new heading, triggering false airspeed failures.

Solution

When no NE-aiding is active and a heading reset exceeds 0.3 rad (~17°), the velocity vector is now rotated to match the new heading orientation. This maintains consistency between the vehicle's heading and velocity estimates.

Changelog Entry

For release notes:

Bugfix: Fixed velocity vector orientation after heading reset
- Velocity is now rotated to match heading when reset exceeds 0.3 rad without NE-aiding active
- Prevents false airspeed failures and erratic velocity estimates after heading corrections

Test Coverage

Added unit test to verify velocity rotation during heading reset. Hardware testing suggestions welcome.

[github-actions]

🔎 FLASH Analysis

px4_fmu-v5x [Total VM Diff: 328 byte (0.02 %)]

    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.0%    +328  +0.0%    +328    .text
  [NEW]    +168  [NEW]    +168    Ekf::resetHorizontalVelocityToMatchYaw()
   +19%    +136   +19%    +136    Ekf::resetQuatStateYaw()
  +4.9%     +16  +4.9%     +16    Ekf::resetYawByFusion()
  +0.0%      +8  +0.0%      +8    [section .text]
+0.0%     +55  [ = ]       0    .debug_abbrev
+0.0%      +8  [ = ]       0    .debug_aranges
+0.0%     +68  [ = ]       0    .debug_frame
+0.0% +3.90Ki  [ = ]       0    .debug_info
+0.0% +1.10Ki  [ = ]       0    .debug_line
  [NEW]      +1  [ = ]       0    [Unmapped]
  +0.0% +1.10Ki  [ = ]       0    [section .debug_line]
+0.0% +1.69Ki  [ = ]       0    .debug_loclists
+0.1%    +321  [ = ]       0    .debug_rnglists
 -50.0%      -1  [ = ]       0    [Unmapped]
  +0.1%    +322  [ = ]       0    [section .debug_rnglists]
+0.0%     +97  [ = ]       0    .debug_str
-0.4%      -1  [ = ]       0    .shstrtab
+0.0%     +45  [ = ]       0    .strtab
  [NEW]     +45  [ = ]       0    Ekf::resetHorizontalVelocityToMatchYaw()
+0.0%     +48  [ = ]       0    .symtab
  [NEW]     +48  [ = ]       0    Ekf::resetHorizontalVelocityToMatchYaw()
 -25.0%     -16  [ = ]       0    __devif_loopback_veneer
 -33.3%     -16  [ = ]       0    __nxsem_restore_baseprio_irq_veneer
   +50%     +16  [ = ]       0    __nxsig_ismember_veneer
   +33%     +16  [ = ]       0    __stm32_ep0out_setup_veneer
-3.9%    -328  [ = ]       0    [Unmapped]
+0.0% +7.32Ki  +0.0%    +328    TOTAL

px4_fmu-v6x [Total VM Diff: 328 byte (0.02 %)]

    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.0%    +328  +0.0%    +328    .text
  [NEW]    +168  [NEW]    +168    Ekf::resetHorizontalVelocityToMatchYaw()
   +19%    +136   +19%    +136    Ekf::resetQuatStateYaw()
  +4.9%     +16  +4.9%     +16    Ekf::resetYawByFusion()
  +0.0%      +8  +0.0%      +8    [section .text]
+0.0%     +55  [ = ]       0    .debug_abbrev
+0.0%      +8  [ = ]       0    .debug_aranges
+0.0%     +68  [ = ]       0    .debug_frame
+0.0% +3.90Ki  [ = ]       0    .debug_info
+0.0% +1.10Ki  [ = ]       0    .debug_line
  [NEW]      +1  [ = ]       0    [Unmapped]
  +0.0% +1.10Ki  [ = ]       0    [section .debug_line]
+0.1% +1.80Ki  [ = ]       0    .debug_loclists
+0.1%    +321  [ = ]       0    .debug_rnglists
 -50.0%      -1  [ = ]       0    [Unmapped]
  +0.1%    +322  [ = ]       0    [section .debug_rnglists]
+0.0%     +97  [ = ]       0    .debug_str
-0.4%      -1  [ = ]       0    .shstrtab
+0.0%     +45  [ = ]       0    .strtab
  [NEW]     +45  [ = ]       0    Ekf::resetHorizontalVelocityToMatchYaw()
+0.0%     +48  [ = ]       0    .symtab
  [NEW]     +48  [ = ]       0    Ekf::resetHorizontalVelocityToMatchYaw()
-7.2%    -328  [ = ]       0    [Unmapped]
+0.0% +7.43Ki  +0.0%    +328    TOTAL

Updated: 2025-10-13T20:37:10