Code: Select all
vel += accZ * ACC_VelScale * dTime;
vel = vel * 0.985f + baroVel * 0.015f;
should be changed to
Code: Select all
vel = vel * 0.985f + baroVel * 0.015f;
vel += accZ * ACC_VelScale * dTime;
so we dont actually delay the acc effect on alt.
This should result in much better althold and less sensitivity to suboptimal althold PID values.
my implementation of the stuff
Code: Select all
float PredictedAlt;
float EstimatedAlt;
float EstAltVel = 0.0;
float KP = 0.040; // Position factor
float KV = 0.004; // Velocity factor
float KZ = 0.995; // velocity decrease factor to supress ringing
// input baroalt is totally unfiltered, output EstAlt
float diff = BaroAlt - PredictedAlt;
// estalt in cm
EstimatedAlt = PredictedAlt + diff * KP;
// estaltvel in cm/s
EstAltVel = EstAltVel * KZ + diff * KV + accZ * 0.025; // 0.025 = 25ms
PredictedAlt = EstimatedAlt + EstAltVel*0.025;
EstAlt = (int32_t)EstimatedAlt;
Right?