Thanks @oursland for the article.
As I understand increasing EKF2_ACC_NOISE with lower the cutoff frequency of the EKF. So the high frequencies will be more smoothed out. However this parameter is clipped (hard coded) to 1.0 m/s/s. And checking my log
the standard deviation of the accelerometer noise is closer to let’s say 5 m/s/s. Although this value of 5 is the optimal, I’m a bit concerned the resulting “cutoff frequency” will be too low for the controller to stabilize the vehicle. Maybe someone knows why this parameter is clipped to 1.0?
I’m aware this (100+ kg) vehicle has a lot of noise. I believe the controller can operate with high frequency noise once it is filtered.
Regarding the position controller, the D term multiplies the derivative of the velocity. So I added a 2nd order low pass filter in mc_pos_control to smooth out the velocity before taking its derivative. That seems to work well and respected the idea of avoiding filtering before the EKF 
Is that something worth doing a PR upstream?
Digging in the code, it seems the discrete differentiation of the local inertial velocity into local inertial acceleration is equipped with a first order low pass filter. On which when I call getLP() returns me 5… 5Hz I imagine. Any idea where this is initialized, and how I can change this cutoff frequency?
- I found the answer to this sub-question, this is MPC_VELD_LP