Proposal: D-term pre-differentiation LPF to reduce noise amplification (Betaflight-inspired)

[19379353560]

Problem

In the current INAV multirotor PID controller, differentiation is applied directly to the raw gyro signal:

gyroRate → diff(prev - cur) → PT2 LPF @ 110Hz → D output

Differentiation amplifies high-frequency noise by a factor of f_loop / f_cutoff — at 1 kHz loop rate this is roughly 9×. The downstream PT2 then has to suppress that amplified noise, which limits how high D gain can be set before oscillation.

Proposed Fix

Add an optional PT1 pre-filter applied before differentiation, similar to how Betaflight filters gyro data before computing the D-term derivative:

gyroRate → PT1 pre-LPF @ 250Hz → diff(prev_filtered - filtered) → PT2 LPF @ 110Hz → D output

	Stock INAV	With pre-diff LPF
Noise amplification	~9×	~3.6×
Added latency	—	+0.6 ms (PT1 @ 250 Hz)
D gain headroom	baseline	+15–25%

The added latency is negligible for the position/altitude loop (bandwidth < 5 Hz). For the attitude loop, 0.6 ms is an acceptable trade-off given the noise reduction.

Implementation

I have implemented this in a fork: https://github.com/19379353560/inav

Key changes:

• pidState_t: added dtermLpf2State (PT1) and previousFilteredGyroRate
• dTermProcess(): pre-filter path activated when dterm_lpf2_hz > 0
• pidInitFilters(): initializes pre-diff filter from config
• New parameter dterm_lpf2_hz (default 250 Hz, 0 = disabled / stock behavior)

Happy to submit a PR if there is interest. Would appreciate feedback from the team on whether this approach fits the INAV architecture.