paparazzi-devel
[Top][All Lists]

## [Paparazzi-devel] HFF FILTER

 From: Walid Subject: [Paparazzi-devel] HFF FILTER Date: Fri, 5 Oct 2012 20:20:10 +0200

Hi Felix,
How are you?

I'm totally agree with you regarding RTOS/scheduler and it will be fine if you have it for INS / GPS filter.

<<If you work on the INS, it really makes senses to do this based on master where we already started refactoring it...>>  ->OK :)...I'm agree...

For the moment, I try to run the HFF filter correctly. I started by analyzing the hf_float.c but I see some errors or approximations:

PROPAGATION step:

by comparing b2_hff_propagate_x and b2_hff_propagate_y, we have:

equation_1: hff_work->x = hff_work->x + DT_HFILTER * hff_work->xdot;

equation_2: hff_work->y = hff_work->y + DT_HFILTER * hff_work->ydot + DT_HFILTER*DT_HFILTER/2 * hff_work->ydotdot

The equation 1 is false because if we refer to Winner process acceleration we have:

hff_work->x = hff_work->x + DT_HFILTER * hff_work->xdot + DT_HFILTER*DT_HFILTER/2 * hff_work->xdotdot -> it's the same as equation_2

UPDATE step:

Position update:
x position:
b2_hff_update_x( ) {
b2_hff_x_meas = x_meas;

const float y  = x_meas - hff_work->x;
const float S  = hff_work->xP[0][0] + Rpos;
const float K1 = hff_work->xP[0][0] * 1/S;
const float K2 = hff_work->xP[1][0] * 1/S;

hff_work->x     = hff_work->x          + K1 * y;
hff_work->xdot  = hff_work->xdot  + K2 * y;  -> I think this equation is wrong because the x velocity is updated is according to the error between the propagated x position and the measurement of x and not on the error between the measurement and the propagation of the x speed. ( I think the true equation is:  hff_work->xdot  = hff_work->xdot  + K2 * (vel.x - hff_work->xdot) )

->the same error in y position update

Velocity update:
vx speed  b2_hff_update_xdot()
b2_hff_xd_meas = vel;

const float yd = vel - hff_work->xdot;
const float S  = hff_work->xP[1][1] + Rvel;
const float K1 = hff_work->xP[0][1] * 1/S;
const float K2 = hff_work->xP[1][1] * 1/S;

hff_work->x     = hff_work->x          + K1 * yd; -> the x position is not correctly updated ->  I think the true equation is:  hff_work->x = hff_work->x   + K1*(x_meas - hff_work->x)
hff_work->xdot  = hff_work->xdot  + K2 * yd;

Are you aggree with me :)?

Regards.

Walid.

Le 27/09/2012 19:35, Felix Ruess a écrit :
Hi Walid,

If you work on the INS, it really makes senses to do this based on master where we already started refactoring it...

Also for a full INS/GPS filter it would really be best if we had a RTOS/scheduler.
The strapdown can run at full imu rate without problems, but the filter prediction/updates don't need to run at such a high rate.
The filter also won't be able to run at the full main loop frequency (512Hz for quads), so we need a scheduler to run the filter as a preemptable task.
(Unless you want to start splitting this up by hand, which you really don't want to do ;-)

I have no idea about wookong or unmanned innovation autopilots...

Cheers, Felix
:
Hi Felix,
Excuse me for not writing to you before to thank you for your last email that you sent me.
Thank you very much for your help! Very good recommandation!

To answer your questions: our aim is  to have a good navigation and a good Hover_Z_hold. I think the data fusion (ins.c and the ins sub system) is not well made, so we want to improve this part. Whatsoever implement a full INS/GPS filter or make some changes in ins.c and ins sub system. Your recommandation has guided me to the indirect kalman filter with using quaternion formulation. I understood with the theory formulation but the integration of the algorithm in paparazzi software is a very difficult mission. I hope to reach one day:)!!!!

Felix,
have you any idea about the methods used by autopilots that work well as wookong or unmanned innovation?
I searched in internet some informations  of these autopilots but nothing :)
...

Thank you again for your help.

Walid.