Gyro filtering: apply IMU_GYRO_CUTOFF also to D-term

[bkueng]

• flying master lately I noticed the D-term is considerably more suspectible to noise on different vehicles. Initially I attributed that to the removal of the on-chip DLPF.
• trying to fly the Kopis 2 on master, it was just unflyable. And no matter how much PID and filter tuning I did, I could not get it to fly.

I then made the filter change here, and it immediately improved things a lot (so apparently the ordering matters). With some further tuning it's flyable again.

Log: https://logs.px4.io/plot_app?log=65497f73-7350-4965-aec1-5ccdce1c628c

@dakejahl I'm pretty sure this helps on your S500 as well.

Some feedback if other people experience the same would be appreciated.

[dagar]

@jlecoeur any comment on the filtering? 7e9b10f

[dagar]

Do you know if wq:rate_ctrl (filtering -> mc_rate_control -> dshot) was always this high (~31%)?

 PID COMMAND                   CPU(ms) CPU(%)  USED/STACK PRIO(BASE) STATE FD
   0 Idle Task                    2815  5.882   232/  512   0 (  0)  READY  3
   1 hpwork                          0  0.000   344/ 1260 249 (249)  w:sig  3
   2 lpwork                          2  0.000   608/ 1612  50 ( 50)  w:sig  3
   3 init                          250  0.000  1984/ 2924 100 (100)  w:sem  3
   4 wq:manager                      0  0.000   376/ 1252 255 (255)  w:sem  5
  16 dataman                        14  0.000   744/ 1204  90 ( 90)  w:sem  4
  18 wq:lp_default                   5  0.000   780/ 1700 205 (205)  w:sem  5
  22 wq:hp_default                 143  1.096  1072/ 1900 240 (240)  w:sem  5
 127 wq:SPI4                      2224 17.746  1840/ 2492 250 (250)  w:sem  5
 134 wq:I2C1                        25  0.099   828/ 1468 246 (246)  w:sem  5
 158 wq:navigation_and_contro     1517 11.764  1184/ 7196 241 (241)  w:sem  5
 159 wq:rate_ctrl                 3727 31.306  1096/ 1660 255 (255)  w:sem  5
 164 commander                     132  0.697  1248/ 3212 140 (140)  READY  6
 165 commander_low_prio              0  0.000   768/ 2996  50 ( 50)  w:sem  6
 217 mavlink_if1                   524  4.685  1824/ 2484 100 (100)  READY  4
 218 mavlink_rcv_if1                48  0.398  2536/ 4060 175 (175)  w:sem  4
 232 wq:UART3                      100  0.797   764/ 1396 235 (235)  w:sem  5
 245 wq:attitude_ctrl               67  0.498   600/ 1532 242 (242)  w:sem  5
 251 navigator                       5  0.000   984/ 1764 105 (105)  w:sem  5
 270 wq:SPI2                       328  2.991   696/ 2492 252 (252)  w:sem  5
 284 logger                        204  3.988  2896/ 3644 230 (230)  RUN    4
 285 log_writer_file               185 14.755   560/ 1164  60 ( 60)  READY  4

Processes: 22 total, 5 running, 17 sleeping, max FDs: 15
CPU usage: 90.83% tasks, 3.29% sched, 5.88% idle
DMA Memory: 5120 total, 1536 used 1536 peak
Uptime: 12.721s total, 2.815s idle

[jlecoeur]

I am surprised that it improves the noise level on the D term. I would expect it to have similar noise below the cutting frequencies and larger delay.
@bkueng do you have logs before & after with the same cutoff frequencies?

[bkueng]

Do you know if wq:rate_ctrl (filtering -> mc_rate_control -> dshot) was always this high (~31%)?

Yes that is pretty much how it is and has been @2khz.

---

@bkueng do you have logs before & after with the same cutoff frequencies?

I have many logs. This is a direct comparison:

• master, IMU_GYRO_CUTOFF=60, IMU_DGYRO_CUTOFF=60: https://logs.px4.io/plot_app?log=bfc43644-fab5-489d-98a9-54a8038131c9
• this PR, IMU_GYRO_CUTOFF=60, IMU_DGYRO_CUTOFF=off: https://logs.px4.io/plot_app?log=0d7cd17a-1aa8-4844-99d2-1164c9ef8b28

(yes both don't look great, as I adjusted the filters for the sake of testing - still, there is a big difference). You can clearly see the difference in flight as well, in form of random twitches.

This is also confirmed to improve things on bit a larger quad.

[jlecoeur]

Thanks @bkueng for the logs, however both logs are with commit c02bdf4. Also, the one with IMU_DGYRO_CUTOFF=0 (i.e. a single LPF on the D term) looks better than the one with IMU_DGYRO_CUTOFF=60 (i.e. two LPFs on the D term). This is very strange.

Ideally we would compare logs with

• c02bdf4, with IMU_GYRO_CUTOFF=80, IMU_DGYRO_CUTOFF=50
• c02bdf4 + reverted 7e9b10f, with IMU_GYRO_CUTOFF=80, IMU_DGYRO_CUTOFF=50

I see several explanations:

• we need a higher order filter for the D term to cut the high frequencies created by the differentiation. This is effectively what was done before with the two 2nd order LPF in series (one in the sensor module, one in the rate controller), and what you have reverted to with this PR
• extra noise is introduced in the differentiation, is it jitter on the dt @dagar? Applying the filter after the differentiation may be helping.
• we have separate filters for the D term, i.e. the feedback is filtered by the LPF at IMU_DGYRO_CUTOFF, while the reference is not (it is computed from the attitude loop which has its own filtering). Maybe the delay difference is lower with 2 LPFs in series, hence the better performance. If this is the case, probably the best move would be to filter the D term error so that both setpoint and feedback are filtered the same way.

In the end, if performance is better and CPU usage the same, I am ok with this PR. Although I think it is worth digging a little bit more to understand what is the best thing to do.

[bkueng]

Sorry, noticed I used IMU_GYRO_CUTOFF twice in the description, I fixed this now.

Thanks @bkueng for the logs, however both logs are with commit c02bdf4

I did the change locally to minimize diff. In particular:

diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
index d2fa7a441f..f7fd82b62d 100644
--- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
+++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
@@ -291,8 +291,8 @@ void VehicleAngularVelocity::Run()

             const Vector3f angular_velocity{_lp_filter_velocity.apply(angular_velocity_notched)};

-            const Vector3f angular_acceleration_raw = (angular_velocity - _angular_velocity_prev) / dt;
-            _angular_velocity_prev = angular_velocity;
+            const Vector3f angular_acceleration_raw = (angular_velocity_notched - _angular_velocity_prev) / dt;
+            _angular_velocity_prev = angular_velocity_notched;
             _angular_acceleration_prev = angular_acceleration_raw;
             const Vector3f angular_acceleration{_lp_filter_acceleration.apply(angular_acceleration_raw)};

IMU_DGYRO_CUTOFF=60 (i.e. two LPFs on the D term)

Note that on master IMU_GYRO_CUTOFF is not applied to the D term, so the comparison changes only a single thing, namely the order. Your suggested test would change 2 things, the ordering + filter.

Applying the filter after the differentiation may be helping.

What the test shows is exactly the opposite: you need a filter before.

Based on what I see, the difference is definitely noise, not signal latency differences (I had that suspicion as well, so I tested with filter configs as close as possible).
There are alternative ways for implementation (and I agree it's worth looking into), this just reverts to a known working state.

[jlecoeur]

Ok I understand more now.

What about two separate filters, with differentiation after the DTERM filter? This would keep a single filter per signal. Something like:

gyro -> notch -> LPF @ IMU_GYRO_CUTOFF -> angular_velocity
             |-> LPF @ IMU_DGYRO_CUTOFF -> derivative -> angular_acceleration

[bkueng]

Yes I had that in mind as well. Potential downside is that the differentiation might add noise (dt).

[dagar]

I'll merge this in the meantime.

[RomanBapst]

@bkueng I remember to have seen this before. Continuous time theory says that the order of linear elements should not matter but I think the effect we see here is due to discreteness of the system.
I think you see a similar effect when providing a discrete low pass filter an extremely large sample.
Such a large value with a certain dt can blow up the filter state and make it take quite some time to recover. I think I remember that in certain applications people actually apply a median filter in front of the lowpass.
Maybe we are seeing similar effects here since differentiating before filtering might give a similar effect (increasing amplitude considerably.)