har-in-air / kalmanfilter_altimeter_vario Goto Github PK

Hi I think something wrong in " Update state covariance matrix"
Paz -= ka * Pzz;
Pav -= ka * Pzv;
Paa -= ka * Pza;

Pvz -= kv * Pzz;
Pvv -= kv * Pzv;
Pva -= kv * Pza;

Pzz -= kz * Pzz;
Pzv -= kz * Pzv;
Pza -= kz * Pza;

must be
Paz_ -= ka * Pzz_;
Pav_ -= ka * Pzv_;
Paa_ -= ka * Pza_;

Pvz_ -= kv * Pzz_;
Pvv_ -= kv * Pzv_;
Pva_ -= kv * Pza_;

Pzz_ -= kz * Pzz_;
Pzv_ -= kz * Pzv_;
Pza_ -= kz * Pza_;

I am using your code in my flight controller. I believe the state covariance matrix update should be reversed as the new P values are being overwritten e.g. Pzz is computed and then used to update Paz.

I think the ordering should be:

Paz -= ka * Pzz;
Pav -= ka * Pzv;
Paa -= ka * Pza;

Pvz -= kv * Pzz;
Pvv -= kv * Pzv;
Pva -= kv * Pza;

Pzz -= kz * Pzz;
Pzv -= kz * Pzv;
Pza -= kz * Pza;

Cheers

Hi
I have tested your code , it has good precision in difference of altitude but absolute altitude is drift because of barometer . Is there any way fix this without GPS?