experiment with low-pass filtering GPS float solution

[greymfm]

Currently, robot wobbles around tracked path if using GPS float solution. Try out if low-pass filtering GPS float solution would help.

Idea:
In function 'void computeRobotState()' (robot.cpp, line 931), try out a low-pass filter for GPS float solution (similar to what we do to low-pass filter the GPS yaw angle) :

small low-pass filtering:

if (maps.useGPSfloatForPosEstimation){ // allows planner to use float solution?
  if  (stateGroundSpeed > 0.03) {
    stateX = 0.99 * stateX + 0.01 * posE;  // this will only use GPS float positions as 'long-term' values
    stateY = 0.99 * stateY + 0.01 * posN;
  } else {
    stateX = posE;
    stateY = posN; 
  }
}

[ShadedSelf]

Is GPS float data normally distributed around the actual antenna?
If so, averaging over time should give a very good aproximation, if it isnt the mower would not wobble but the position estimation would be biased.

I might give this a shot and see what happens.

[greymfm]

I think in float mode, the raw measurements the GPS receiver computes (and that e.g. you can see with a custom RTK engine like RTKLIB) could be normally distributed around the actual antenna. However, the ublox GPS receiver already runs a filter and that will make the output signal no longer normally distributed around the actual antenna. I think you cannot turn of this float filter...(one could use ublox ucenter to find out or study the F9P receiver protocol manual)

In fact, the float solution typically happens due to multipath-errors (in float mode, the GPS receiver struggles to understand what is a direct satellite signal and what is a satellite reflection coming from a wall, so the signal satellite distance measurements are longer than in reality. The result is that the path is deviated from the ground truth into some direction. When the RTK is fixed again typically a GPS jump happens...

Example float recordings:

[ShadedSelf]

I see.

Well, I made a couple of changes:

My mower allways goes into invalid for a sec when it loses fix, so it stops, does the whole motor spin up, and then goes, so I added a 10 second timeout for invalid gps, only using imu + odometry, now the mower just keeps going when it goes into invalid and quickly finds either a float or fix solution. Seems accurate enough.

Then the suggested change of mixing odometry and heading with float, I dont use float gps data to calculate heading here, it seems to work pretty well, I dont know if this is just because the imu is better short term or because mixing those values is actually doing anything though, but seems better than just flloat.

Ill test further, but it seems to have improved things.

[disaster123]

@ShadedSelf this sounds interesting - i would like to test this as well. Do you already have some commits or a branch ready?

[ShadedSelf]

Im working with my local dirty files.
Ive made a new branch if you want to test it, hopefully I didnt miss anything.

[disaster123]

thanks will have a look

[ShadedSelf]

This is working fine for me, today I had the mower drive on invalid for 5 seconds or so, then switch to float for another 3 seconds and finally go back to fixed and be exactly where it was supposed to be.
I also had over 20 invalid and float recoveries and didnt even notice.
I dont really get longer float times, so I cant really test much longer than that, but its definatelly an improvement for me.

Ive merged this into #145 since both share the vector/quaternion library.

Code could be improved using float accuracy to control the imu+odometry/gps fusion rates, but seems good enough as it is.