Vehicle_odometry: protect angular_velocity field against aliasing

[bresch]

Solved Problem

External system using vehicle_odometry might want to use the angular_velocity field as a source of low-rate gyro data for another estimator. Currently, this signal is subject to aliasing as the last imu data is used to fill the message instead of properly down-sampling the gyro data.

Solution

Accumulate delta-angles between each publication to get the correct angular velocity between two attitudes.

The resulting angular velocity is not the average of the gyro measurements, but the time derivative of shortest rotation (delta_angle) between two unaided attitudes. The angular velocity is bias-corrected (using the bias estimate of the EKF) but otherwise not directly affected by other aiding sources.

Changelog Entry

For release notes:

Bugfix: Avoid aliasing of angular vel in vehicle_odometry uORB topic
New parameter: -
Documentation: -

Test coverage

Tested on hardware (sensor_combined @ 200Hz, vehicle_odometry @ 100Hz)

[github-actions]

🔎 FLASH Analysis

px4_fmu-v5x [Total VM Diff: -48 byte (-0 %)]

    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.0%     +70  [ = ]       0    .debug_abbrev
+0.0%     +32  [ = ]       0    .debug_aranges
+0.0%    +112  [ = ]       0    .debug_frame
+0.0% +2.42Ki  [ = ]       0    .debug_info
+0.0%     +73  [ = ]       0    .debug_line
+0.0%    +474  [ = ]       0    .debug_loclists
-0.0%     -78  [ = ]       0    .debug_rnglists
  [NEW]      +2  [ = ]       0    [Unmapped]
  -0.0%     -80  [ = ]       0    [section .debug_rnglists]
+0.0%    +188  [ = ]       0    .debug_str
-0.8%      -2  [ = ]       0    .shstrtab
+0.0%    +106  [ = ]       0    .strtab
  [NEW]     +62  [ = ]       0    OutputPredictor::getAngularVelocityAndResetAccumulator()
   +67%     +16  [ = ]       0    __hrt_call_enter_veneer
 -33.3%     -16  [ = ]       0    __nxsched_add_readytorun_veneer
   +20%     +44  [ = ]       0    matrix::Matrix<>::setIdentity()
+0.0%     +64  [ = ]       0    .symtab
 -50.0%     -16  [ = ]       0    EKF2::PublishAidSourceStatus()
  +100%     +16  [ = ]       0    EKF2::PublishGpsStatus()
 -16.7%     -16  [ = ]       0    EKF2::PublishLocalPosition()
  +100%     +16  [ = ]       0    EKF2::PublishOdometry()
 -50.0%     -16  [ = ]       0    EKF2::PublishWindEstimate()
   +50%     +16  [ = ]       0    EKF2::PublishYawEstimatorStatus()
   +33%     +16  [ = ]       0    EKF2::VerifyParams()
 -33.3%     -16  [ = ]       0    EKF2::print_usage()
 -66.7%     -32  [ = ]       0    Ekf::controlBetaFusion()
 -25.0%     -16  [ = ]       0    Ekf::controlEvHeightFusion()
 -33.3%     -32  [ = ]       0    Ekf::controlEvPosFusion()
  +100%     +16  [ = ]       0    Ekf::controlGnssPosFusion()
  +100%     +16  [ = ]       0    Ekf::controlGnssVelFusion()
  +100%     +16  [ = ]       0    Ekf::resetAccelBias()
 -33.3%     -16  [ = ]       0    Ekf::resetGyroBias()
 -33.3%     -16  [ = ]       0    Ekf::resetVelocityToGnss()
  +100%     +16  [ = ]       0    Ekf::resetWindCov()
 -25.0%     -16  [ = ]       0    Ekf::resetWindToExternalObservation()
 -40.0%     -32  [ = ]       0    Ekf::runGnssChecks()
 -50.0%     -16  [ = ]       0    Ekf::shouldResetGpsFusion()
 -98.6%    +176  [ = ]       0    [10 Others]
+0.6%     +48  [ = ]       0    [Unmapped]
-0.0%     -48  -0.0%     -48    .text
  [NEW]    +104  [NEW]    +104    OutputPredictor::getAngularVelocityAndResetAccumulator()
  +5.1%     +40  +5.1%     +40    OutputPredictor::calculateOutputStates()
  +9.5%     +16  +9.5%     +16    EKF2::PublishBaroBias()
  +2.9%     +16  +2.9%     +16    OutputPredictor::OutputPredictor()
  +9.4%     +16  +9.4%     +16    matrix::Matrix<>::setIdentity()
  +1.9%     +12  +1.9%     +12    Ekf::checkMagHeadingConsistency()
  +0.8%     +12  +0.8%     +12    Ekf::controlBaroHeightFusion()
  +4.0%     +12  +4.0%     +12    OutputPredictor::reset()
  +0.3%      +8  +0.3%      +8    EKF2::PublishAidSourceStatus()
  +0.8%      +8  +0.8%      +8    EKF2::PublishStatusFlags()
  +3.6%      +8  +3.6%      +8    Ekf::computeDeltaHorizontalPosition()
  -1.3%      -8  -1.3%      -8    Ekf::checkMagField()
  -2.5%      -8  -2.5%      -8    Ekf::checkVerticalAccelerationHealth()
  -0.2%     -12  -0.2%     -12    Ekf::controlMagFusion()
  -0.7%     -16  -0.7%     -16    Ekf::fuseMag()
  -3.0%     -16  -3.0%     -16    Ekf::reset()
 -100.0%     -16 -100.0%     -16    [53 Others]
  -2.0%     -40  -2.0%     -40    Ekf::controlOpticalFlowFusion()
  -2.3%     -48  -2.3%     -48    Ekf::controlRangeHaglFusion()
 -13.5%     -52 -13.5%     -52    EKF2::PublishOdometry()
  -2.5%     -84  -2.5%     -84    Ekf::Ekf()
+0.0% +3.43Ki  -0.0%     -48    TOTAL

px4_fmu-v6x [Total VM Diff: -56 byte (-0 %)]

    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.0%     +70  [ = ]       0    .debug_abbrev
+0.0%     +32  [ = ]       0    .debug_aranges
+0.0%    +112  [ = ]       0    .debug_frame
+0.0% +2.42Ki  [ = ]       0    .debug_info
+0.0%     +73  [ = ]       0    .debug_line
+0.0%    +475  [ = ]       0    .debug_loclists
-0.0%     -79  [ = ]       0    .debug_rnglists
   +50%      +1  [ = ]       0    [Unmapped]
  -0.0%     -80  [ = ]       0    [section .debug_rnglists]
+0.0%    +188  [ = ]       0    .debug_str
-0.8%      -2  [ = ]       0    .shstrtab
+0.0%    +106  [ = ]       0    .strtab
  [NEW]     +62  [ = ]       0    OutputPredictor::getAngularVelocityAndResetAccumulator()
   +20%     +44  [ = ]       0    matrix::Matrix<>::setIdentity()
+0.0%     +64  [ = ]       0    .symtab
 -50.0%     -16  [ = ]       0    EKF2::PublishAidSourceStatus()
  +100%     +16  [ = ]       0    EKF2::PublishGpsStatus()
 -16.7%     -16  [ = ]       0    EKF2::PublishLocalPosition()
  +100%     +16  [ = ]       0    EKF2::PublishOdometry()
 -50.0%     -16  [ = ]       0    EKF2::PublishWindEstimate()
   +50%     +16  [ = ]       0    EKF2::PublishYawEstimatorStatus()
   +33%     +16  [ = ]       0    EKF2::VerifyParams()
 -33.3%     -16  [ = ]       0    EKF2::print_usage()
 -66.7%     -32  [ = ]       0    Ekf::controlBetaFusion()
 -40.0%     -32  [ = ]       0    Ekf::controlEvHeightFusion()
 -33.3%     -32  [ = ]       0    Ekf::controlEvPosFusion()
  +100%     +16  [ = ]       0    Ekf::controlGnssPosFusion()
  +100%     +16  [ = ]       0    Ekf::controlGnssVelFusion()
  +100%     +16  [ = ]       0    Ekf::resetAccelBias()
 -33.3%     -16  [ = ]       0    Ekf::resetGyroBias()
 -33.3%     -16  [ = ]       0    Ekf::resetVelocityToGnss()
  +100%     +16  [ = ]       0    Ekf::resetWindCov()
 -25.0%     -16  [ = ]       0    Ekf::resetWindToExternalObservation()
 -40.0%     -32  [ = ]       0    Ekf::runGnssChecks()
 -50.0%     -16  [ = ]       0    Ekf::shouldResetGpsFusion()
 -98.4%    +192  [ = ]       0    [7 Others]
+0.8%     +56  [ = ]       0    [Unmapped]
-0.0%     -56  -0.0%     -56    .text
  [NEW]    +104  [NEW]    +104    OutputPredictor::getAngularVelocityAndResetAccumulator()
  +5.1%     +40  +5.1%     +40    OutputPredictor::calculateOutputStates()
  +9.5%     +16  +9.5%     +16    EKF2::PublishBaroBias()
  +2.9%     +16  +2.9%     +16    OutputPredictor::OutputPredictor()
  +9.4%     +16  +9.4%     +16    matrix::Matrix<>::setIdentity()
  +1.9%     +12  +1.9%     +12    Ekf::checkMagHeadingConsistency()
  +0.8%     +12  +0.8%     +12    Ekf::controlBaroHeightFusion()
  +4.0%     +12  +4.0%     +12    OutputPredictor::reset()
  +0.3%      +8  +0.3%      +8    EKF2::PublishAidSourceStatus()
  +0.8%      +8  +0.8%      +8    EKF2::PublishStatusFlags()
  +3.6%      +8  +3.6%      +8    Ekf::computeDeltaHorizontalPosition()
  -1.3%      -8  -1.3%      -8    Ekf::checkMagField()
  -2.5%      -8  -2.5%      -8    Ekf::checkVerticalAccelerationHealth()
  -0.2%     -12  -0.2%     -12    Ekf::controlMagFusion()
  -0.7%     -16  -0.7%     -16    Ekf::fuseMag()
  -3.0%     -16  -3.0%     -16    Ekf::reset()
 -100.1%     -24 -100.1%     -24    [52 Others]
  -2.0%     -40  -2.0%     -40    Ekf::controlOpticalFlowFusion()
  -2.3%     -48  -2.3%     -48    Ekf::controlRangeHaglFusion()
 -13.5%     -52 -13.5%     -52    EKF2::PublishOdometry()
  -2.5%     -84  -2.5%     -84    Ekf::Ekf()
+0.0% +3.43Ki  -0.0%     -56    TOTAL

Updated: 2025-05-27T14:19:41

[haumarco]

nice