[Paparazzi-devel] Quadrotor, Lia 1.1. + PWM actuators - adaptive thrust estimation does not converge

[Michal Podhradsky]

Hello Paparazzi team,

I am using Lia 1.1. board (based on Lisa M 2.0), PWM controlled motors and adaptive thrust estimation for altitude control (Z_HOLD mode).
What I noticed is that the Kalman filter does not converge, thus it does not provide suitable values for nominal hover thrust.

First thing, if we switch to V_MODE_HOVER (comes with Z_HOLD), the zd_ref is set to zero. (Seehttp://github.com/paparazzi/paparazzi/blob/v4.0/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c#L138 ) The zd_ref is then zero all the time we are in Z_HOLD or ATT mode.
If you call the filter routine: http://github.com/paparazzi/paparazzi/blob/v4.0/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c#L170
then the last argument is zero (zd_ref). So our reference is zero and covariance error is constant. 
Also, the Kalman gain stays zero (assume because covariance error E is too large) so covariance is not updated and stays also constant. 

Only measured state is changing, but it does not propagate into our estimated state (gv_adapt_X) so the inv_m in http://github.com/paparazzi/paparazzi/blob/v4.0/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c#L280 stays constant and in our case too high, so the quad just shoots up.

Do you have any explanation for this behavior or any suggestions what we might be doing wrong?

Attached a log of inv_m, gv_adapt_Xmeas and covariance.

With kind regards
Michal

adapt_est.png
Description: PNG image