Re: [Paparazzi-devel] INS subsystem

[Loic Drumettaz]

Hi Felix,

I tried again to use HFF filter yesterday. I used the option "USE_GPS_ACC4R", together with default values of the HFF filter:

 <define name="ACCEL_NOISE"  value="0.5"/>

  <define name="R_POS"        value="8."/>

  <define name="R_POS_MIN"    value="3."/>

  <define name="R_SPEED"      value="2."/>

 <define name="R_SPEED_MIN"  value="1."/>

I have also recorded that Pacc was always higher than 4.5 meters during flight, so R_POS_MIN is never reached. That's the same for SACC and R_SPEED_MIN.

I tried hovering with HFF filter and without. The hovering is less stable with the filter: it shows more oscillations around the setpoint. To best describe this, i fly my quadrotor in hover mode and i use RC inputs to move the quadrotor away from the setpoint. When I release the sticks, the quadrotor goes back to the setpoint. With HFF, the quadrotor oscillates around the setpoint, more than without. I tried to tune again the nav PID settings to use the HFF, without success. This behaviour makes me believe that the HFF filter adds more delay than GPS alone. 

One thing I still haven't tried, is to tune ACCEL_NOISE. The variance of accels measurements is around 10 m/s² to 25 m/s² (standard deviation 3 to 5 m/s²). This level of vibration is much higher than the default value for HFF filter, which could explain why HFF didn't improve my hover test. So I will try:

<define name="ACCEL_NOISE"  value="10"/>

And I'll report my experiments later.

Regards

Loïc

Hi Loic,

disclaimer first: I wrote the hff a couple of years ago trying to deal with some gps delays. It is really not how you would properly do it (but we are quite constrained regarding computational resources). I haven't really used it recently (had a really crappy gps back then, that was the main reason for writing it).

I'm not sure if the current implementation really gives you any benefits if your gps is good.

E.g. if you have high noise (due to vibrations, etc.) on your accels (or a significant bias, as that is not estimated) you might make things worse.

I tried to add HFF filter a few times without success: the hovering becomes unstable.

Can you describe this a bit more precisely?

 

I suppose the HFF filter adds some delay to the position and speed estimation, leading to this unstable behaviour.

It *should* not add any delay, on the contrary it *should* have less delay, since the acceleration and velocity is propagated to a position (by default at 32Hz). Hence there should be less delay in the position than with GPS only (normally 4Hz).

 

I'm working in V3.9 branch, with lisa m V2.0 and ublox lea 6H GPS

Is it possible to tune the HFF filter parameters in order to improve the filtering, and maybe obtain better position and speed updates? I noticed the flag "USE_GPS_ACC4R" which adapts the filter coefficients to the GPS speed and position accuracy. Does it help?

That should help quite a bit, yes. But not all GPSs provide the position accuracy that is used for this (but the ublox do). Maybe you should even try to define a smaller HFF_R_POS_MIN in your airframe file (default is 3m accuracy).

 

Or, does anyone have recommendations to tune the HFF filter, in particular:

ACCEL_NOISE

This is the accel noise (variance of a gaussian) in m/s². Best would be to take some measurements (with running motors) and compute the variance of the scaled accel measurements.

If you have a lot of noise on the accels, increase this value.

 

 R_POS     

 R_SPEED

Should be better with USE_GPS_ACC4R as long as the pos and vel accuracty values reported by the GPS are ok (should be good for the LEA6, but check them via messages).

Is it efficicient to specifiy the GPS delay in the filter, with "GPS_LAG" parameter? What is this delay for let's say, a Lea 6 GPS?

That is hard to say as the values are already filtered inside the GPS and we have no clue what exactly is going on there... the delay is definitely less than 1/gps_output_frequency.

Probably better to ignore the delay on a lea6, but you could experiment with 0.1s, much more does not make sense due to the gps internal filtering...

One the other side, I also understood that the IMU sensor filtering is affected by periodic frequency: https://github.com/paparazzi/paparazzi/issues/240. Is it the same problem for HFF filter?

To some extent, yes.. it is not completely correct if you only get new accel values at 500Hz instead of 512Hz.

I pushed a small fix to take this into account. Assuming you have an aspirin2 with the MPU6000 you should add

<configure name="AHRS_PROPAGATE_FREQUENCY" value="500"/>

to the ap firmware in your airframe file.

But I don't think this has a big effect here, there are other approximations that have a higher influence:

* simply take the mean of all the accel measurements since the last propagation as measurement

* assumption is that the attitude was the same during that time

* assumption is that the attitude estimate was correct

Also make sure that these calculations don't result in a delay of the rest of the main loop. Check with the sys_mon module.

So to summarize: it is easy to get worse position estimates with hff if it the configuration does not match your actual sensors and you have high noise on the accels.

On my old quad it really helped (compared to using my crappy GPS directly), but on the other hand the hff probably hasn't really been used since...

In the end hff is sort of a hack and should be completely rewritten (or better even replaced by a full INS)... but to do it properly we would really benefit from a floating point unit and more importantly from a RTOS where we can let this run as a lower priority task without corrupting the timing of the rest.

So much for now...

Cheers, Felix