MC rate controller: add low-pass filter for D-term #8920
Conversation
- use the same value for both - lower control latency allows increasing these gains
and call hrt_absolute_time() only once.
…he D-term The filter is disabled by default, thus the behavior is unchanged.
| _lp_filters_d[0].set_cutoff_frequency(_loop_update_rate_hz, _params.d_term_cutoff_freq); | ||
| _lp_filters_d[1].set_cutoff_frequency(_loop_update_rate_hz, _params.d_term_cutoff_freq); | ||
| _lp_filters_d[2].set_cutoff_frequency(_loop_update_rate_hz, _params.d_term_cutoff_freq); | ||
| _lp_filters_d[0].reset(_rates_prev(0)); |
There was a problem hiding this comment.
Right, I changed it to update only when the frequency changes.
| _rates_int + | ||
| rates_d_scaled.emult(_rates_prev - rates) / dt + | ||
| _rates_int - | ||
| rates_d_scaled.emult(rates_filtered - _rates_prev_filtered) / dt + | ||
| _params.rate_ff.emult(_rates_sp); | ||
|
|
||
| _rates_sp_prev = _rates_sp; |
There was a problem hiding this comment.
I don't think _rates_sp_prev is used anywhere?
|
Asking out of curiosity, did you consider BlockDerivative? |
| math::Vector<3> _rates_prev; /**< angular rates on previous step */ | ||
| math::Vector<3> _rates_sp_prev; /**< previous rates setpoint */ | ||
| math::LowPassFilter2p _lp_filters_d[3]; /**< low-pass filters for D-term (roll, pitch & yaw) */ | ||
| static constexpr const float initial_update_rate_hz = 250.f; /**< loop update rate used for initialization */ |
There was a problem hiding this comment.
I think we're soon going to need a mechanism to set this system wide (single param or programmatically).
There was a problem hiding this comment.
This is only for initialization. A module needs to be able to determine this on its own and at runtime.
I've seen it before, but forgot about it. It's using a different filter, and comes with the typical Block* overhead. I think it can be useful to have a separate derivative class, but since the derivative is only used in one place, I don't see it being useful in that case. |
bkueng
force-pushed
the
d-term_filter
branch
from
029ab64
to
96cb48d
Compare
bkueng
force-pushed
the
d-term_filter
branch
from
96cb48d
to
41cdf41
Compare
|
I'm seeing an incredible performance increase on my racer after applying this PR and adjusting cutoff frequencies! Thanks @bkueng! |
|
Awesome! Do you have a video & log? 😬 |
|
@bkueng Is this something we could usefully add docs for in devguide? Perhaps part of a rate controller diagram? |
|
@hamishwillee yes it should be part of the diagram. But do we already have one for the attitude & rate controller? I only see the one for position. |
|
@bkueng No, not yet (FYI we do have an FW attitude controller going in soon). Perhaps worth talking to @bresch about whether you can work together on that? |
|
@bkueng You're the man making it happen! This filtering process is super important, thanks a lot! |
Simpler version of #7698.
This adds a butterworth low-pass filter to the D-term of the rate controller.
The idea behind this is: The D-term uses the derivative of the rate and thus is the most susceptible to noise. Therefore, using a D-term filter allows to decrease the driver-level filtering, which leads to reduced control latency and permits to increase the P gains.
Output Noise
I was using
IMU_GYRO_CUTOFF=90for my testing. I think I could go even higher, but I should also reduce the on-chip filtering (which I did not do for these tests).With
IMU_GYRO_CUTOFF=90, I have visible noise on the outputs, but the quad still flies. I tested different values for the D-term cutoff filter frequency, and it reduces the noise on the output indeed. I found for my quad a cutoff frequency of 70 removes most of the noise.This is the FFT for actuator outputs without D-term filter (first 3 motors):
This is using D-term filter frequency = 30:
Tuning Gains
I was flying with a P gain of 0.18 before, and was able to increase it to 0.28 now. I tested the same gains against the state of master (
IMU_GYRO_CUTOFF=30,MC_DTERM_CUTOFF=0), and it has strong, visible oscillations, meaning it doesn't fly:The filter is disabled by default, thus it should not be problematic to merge. We should evaluate the whole filtering pipeline and default values once we have #8731.
In future, we can also experiment with different lp filters.
@AndreasAntener @bresch @MaEtUgR @RomanBapst @Stifael