EKF2: constrain max variance by zero innovation update

[bresch]

Solved Problem

We cannot let the covariance of a temporarily unobservable state grow for ever due to numerical issues that will arise when the the matrix is getting ill-conditioned but the current method of clipping the variance has a destabilizing effect as it increases the correlation between the states. (recall: correlation = cov_xy / (sqrt(var_x) * sqrt(var_y) so if cov_xy grows but var_x or var_y increases, correlation increases)

This can be observed on the position state when flying without GNSS (e.g.: navigating with optical flow or airspeed data). When the position variance reaches the limit of 1e6, clipping it causes the correlation with the velocity state to increase and when a single position observation is fused (here sent via a manual position update), the velocity variance collapses (over-correction).
In reality, the calculated velocity variance is negative but artificially clipped to a minimum value of 1e-6.

Note that there is no problem in artificially limiting the minimum value of a state variance as this is similar to having a higher process noise and reduces the correlation with other states without causing instabilities.

Solution

Run a "zero innovation update" to keep the variance of a state below the "numerical stability threshold".
We can see that after the manual position update, the velocity variance is updated to a reasonable value.

Alternatives

The UDU formulation should avoid the need for an upper variance limit.

Test coverage

SITL tests

[github-actions]

🔎 FLASH Analysis

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

    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.0%     +46  [ = ]       0    .debug_abbrev
   +11%     +56  [ = ]       0    ../../src/lib/version/version.c
  -0.2%     -10  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
-0.0%      -8  [ = ]       0    .debug_aranges
  -5.0%      -8  [ = ]       0    ../../src/lib/version/version.c
+0.0%     +28  [ = ]       0    .debug_frame
-0.0%    -166  [ = ]       0    .debug_info
  -0.2%      -4  [ = ]       0    ../../src/lib/version/version.c
  -0.0%    -162  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
-0.0%    -408  [ = ]       0    .debug_line
  -1.3%     -25  [ = ]       0    ../../src/lib/version/version.c
  -0.1%    -382  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
  -0.1%      -1  [ = ]       0    task/task_cancelpt.c
-0.0%     -46  [ = ]       0    .debug_loc
  -0.1%    -172  [ = ]       0    [section .debug_loc]
  +0.0%    +126  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
-0.0%    -162  [ = ]       0    .debug_ranges
  -2.6%      -8  [ = ]       0    ../../src/lib/version/version.c
  -0.0%     -24  [ = ]       0    [section .debug_ranges]
  -0.1%    -128  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
  -3.0%      -2  [ = ]       0    task/task_cancelpt.c
+0.5%      +1  [ = ]       0    .shstrtab
+0.0%    +139  [ = ]       0    .strtab
  -8.1%     -32  [ = ]       0    ../../src/lib/version/version.c
  +0.0%     +32  [ = ]       0    [section .strtab]
  +0.9%    +139  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
+0.0%    +144  [ = ]       0    .symtab
  -7.0%     -64  [ = ]       0    ../../src/lib/version/version.c
  +0.3%     +16  [ = ]       0    ../../src/modules/fw_pos_control/FixedwingPositionControl.cpp
  +0.1%     +48  [ = ]       0    [section .symtab]
  +0.9%    +144  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
+0.7%     +64  [ = ]       0    [Unmapped]
-0.0%     -64  -0.0%     -64    .text
  -0.0%     -64  -0.0%     -64    src/modules/ekf2/modules__ekf2_unity.cpp
-0.0%    -432  -0.0%     -64    TOTAL

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

    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.0%     +46  [ = ]       0    .debug_abbrev
   +11%     +56  [ = ]       0    ../../src/lib/version/version.c
  -0.2%     -10  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
-0.0%      -8  [ = ]       0    .debug_aranges
  -5.0%      -8  [ = ]       0    ../../src/lib/version/version.c
+0.0%     +28  [ = ]       0    .debug_frame
-0.0%    -166  [ = ]       0    .debug_info
  -0.2%      -4  [ = ]       0    ../../src/lib/version/version.c
  -0.0%    -162  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
-0.0%    -400  [ = ]       0    .debug_line
  -1.3%     -25  [ = ]       0    ../../src/lib/version/version.c
  -0.1%    -382  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
  +0.6%      +7  [ = ]       0    task/task_cancelpt.c
-0.0%    -192  [ = ]       0    .debug_loc
  -0.1%    -170  [ = ]       0    [section .debug_loc]
  -0.0%     -22  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
-0.0%    -160  [ = ]       0    .debug_ranges
  -2.6%      -8  [ = ]       0    ../../src/lib/version/version.c
  -0.0%     -24  [ = ]       0    [section .debug_ranges]
  -0.1%    -128  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
+0.5%      +1  [ = ]       0    .shstrtab
+0.0%    +139  [ = ]       0    .strtab
  -8.1%     -32  [ = ]       0    ../../src/lib/version/version.c
  +0.0%     +32  [ = ]       0    [section .strtab]
  +0.9%    +139  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
+0.0%    +144  [ = ]       0    .symtab
  -7.0%     -64  [ = ]       0    ../../src/lib/version/version.c
  +0.3%     +16  [ = ]       0    ../../src/modules/fw_pos_control/FixedwingPositionControl.cpp
  +0.1%     +48  [ = ]       0    [section .symtab]
  +0.9%    +144  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
+0.1%     +64  [ = ]       0    [Unmapped]
-0.0%     -64  -0.0%     -64    .text
  -0.0%     -64  -0.0%     -64    src/modules/ekf2/modules__ekf2_unity.cpp
-0.0%    -568  -0.0%     -64    TOTAL

Updated: 2025-01-14T15:34:06

[haumarco]

So if I understand correctly, the filter would artificially decrease the correlation when we enforce the minimal variance? I guess this would not happen since we've chosen those limits rand the measurement uncertainty reasonably.

[bresch]

So if I understand correctly, the filter would artificially decrease the correlation when we enforce the minimal variance? I guess this would not happen since we've chosen those limits and the measurement uncertainty reasonably.

It could still occur in the accel bias case for example where a maximum condition number between the accel bias states is enforced and that all 3 axes are not observable at the same time. But it's not really harmful, just sub-optimal

[dagar]

Zero innovation heading update?

[bresch]

Zero innovation heading update?

Good point, I think there is room for improvement in general in the yaw fusion code as the measurement jacobian is now trivial and can be considered as a "direct state measurement" (correct in the context of the error state formulation).
Next time we need flash to merge something into the EKF -> yaw fusion cleanup.