Why is altitude error used for vertical speed correction?

[avsaase]

In estimationCalculateCorrection_Z(), when having a valid baro reading, the difference between the estimated altitude from estimationPredict() and the baro is used to correct the altitude estimation. That makes sense. But for the vertical speed estimation, the correction is also based on this altitude residual, with the square of the position weight, instead of using some sort of baro vertical speed measure. I don't understand why that is.

inav/src/main/navigation/navigation_pos_estimator.c

Lines 563 to 566 in 1356480

	// Altitude
	const float baroAltResidual = (isAirCushionEffectDetected ? posEstimator.state.baroGroundAlt : posEstimator.baro.alt) - posEstimator.est.pos.z;
	ctx->estPosCorr.z += baroAltResidual * positionEstimationConfig()->w_z_baro_p * ctx->dt;
	ctx->estVelCorr.z += baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p) * ctx->dt;

The same applies to the GPS when you don't have a baro and are using use_gps_no_baro, but here there is a double correction using both the altitude residual and the residual from the GPS vertical speed.

inav/src/main/navigation/navigation_pos_estimator.c

Lines 597 to 599 in 1356480

	ctx->estPosCorr.z += gpsAltResudual * positionEstimationConfig()->w_z_gps_p * ctx->dt;
	ctx->estVelCorr.z += gpsAltResudual * sq(positionEstimationConfig()->w_z_gps_p) * ctx->dt;
	ctx->estVelCorr.z += (posEstimator.gps.vel.z - posEstimator.est.vel.z) * positionEstimationConfig()->w_z_gps_v * ctx->dt;

And a more general question, why are the altitude and vertical speed estimation are so decoupled from each other? Wouldn't it make more sense to have one estimation for altitude and take the difference with the previous value for the vertical speed? This would avoid inconsistencies where the altitude is increasing but the vario indicates negative vertical speed and vice versa.

Thanks.

[DzikuVx]

@digitalentity ?

[digitalentity]

This is WAI and based on Alpha-Beta filter math: https://en.wikipedia.org/wiki/Alpha_beta_filter

The filter provides both a coordinate and speed consuming a single observation (coordinate). We add an additional observation (acceleration) to improve response times for disturbances, but it has a drawback of causing the filter to disconverge occasionally (i.e. increasing coordinate, but negative speed). However, the Alpha-Beta filter if numerically stabile (unlike Kalman) and will converge again so this is deemed to be safe.

[avsaase]

Thanks, this explains a lot. But isn't there a scaling issue, with both the altitude and vertical speed being corrected with * ctx->dt?

EDIT: actually not necessarily a scaling issue, the velocity correction is just very small. beta = alpha^2 * dT to be exact. With the default w_z_baro_p = 0.35 and a 2kHz loop frequency this means beta = 6E-5. Here's my math: alpha-beta-filter.pdf.

[digitalentity]

Both coordinate and velocity correction is scaled by dT to make weights independent of the update rate. We can apply correction every 1 second with full weight, every 0.5 seconds with 1/2 weight and so on. Otherwise we'll have to adjust the weights every time we change the update rate (this is how it was back in the early days when INAV was just a PR to Cleanflight 😆)

[digitalentity]

But you're probably right about lack of ∆T for beta 🤔
Feel free to experiment with that and see if that improves velocity tracking.

[avsaase]

I will experiment with 47b84d6. I just hope my quad doesn't fly away because of this 😅

[avsaase]

Okay, that was a disaster. As soon as I switched into POS HOLD the quad tried to take off with pulses of 100% throttle. Looking at the log I see the vertical speed is all over the place. Makes sense since I increased the correction by a factor of 2000. A well, you live and learn.

[DzikuVx]

@avsaase on a off-topic note, could you drop me an email? You should find my address and I can't find yours :)

[avsaase]

Done. I used the address on your GitHub profile.