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.