Copter: failover to 2nd GPS can cause sudden movement

[andrea25]

Few day ago I had a crash. Unfortunately no log and no video.

I was using double GPS on PixHawk. 2 Ublox LEA-6H. I was flying with DCM, no EKF. I was using arducopter 3.2.

I deliberately created some interference with one of the two Ublox. I had to do this for testing purposes.

What happened is this: Copter switch to the GPS with high HDOP. During the switch PixHawk emit some melodic tones. And the this is all right.

Then I started the interferences tests. At a certain point PixHawk switch the GPS and the copter sudden move itself forward. The copter repeat some strange movements every time I go to "create interference" with one of the two GPS. But it not do immeditaly, it to it after the gps switch (I can hear the tones).

So, practically: the copter siwtch to the GPS with high HDOP but do not check if the position is too differnt from the starting position, and if the position reported by the GPS with high HDOP is too far away, you can have a little fly away. By the way this is what I think.

I'am not sure at 100%. This is what I think. Can anyone go to check this?

I had a crash on a tree after hearing the GPS switching tones.

[rmackay9]

@andrea25, Thanks very much for testing this. I believe you are the first. So what must be happening is that it's failing over to the 2nd GPS but we're not doing anything intelligent about that in the inertial nav position estimate. We really should probably try to adjust the position a bit to account for the sudden shift instead of just forcing the shift down through the filter. The behaviour will certainly change somewhat when we move to use the EKF by default for AC3.3 but there are often lessons to be learned from how inertial nav deals with a situation.

[andrea25]

@rmackay9 I'm the second =P My Italian friend Antonio gave my the idea =P

If the behavior will change in the nexts relases write it on the change log: when I read that is changed I do some other tests =) Hopefully this time whitout a crash XD

[Akshat-]

We can use the same EKF GPS glitch protection logic where if the reported GPS position jumps beyond the GPS glitch radius then we use the last valid position from the primary GPS and try to calculate the current position using IMU measurements and then compare the data from the secondary GPS to see if its reported positions falls within the GPS Glitch radius centered around the current estimated position, if yes then we should use these new values and the copter should hold position or navigate accordingly, if in case the position reported by the secondary GPS is outside the GPS glitch radius then the copter should continue estimating its position using IMU measurements while at the same time expand the GPS radius by some acceleration..and once the new GPS positions reported by either the primary or the secondary GPS's falls within this circle we again start using these values and accelerate the copter slowly towards this newer position...if both GPS positions lie within the glitch radius we must use the one closer to the current estimated position....and if subsequent positions from both the GPS's lie outside the Glitch radius the we can trigger GPS failsafe after some threshold time and enter land or Althold depending on what the user specifies.... I also wanted to suggest that GPS failsafe should have another option to enter Althold with simple mode..this will help people flying without FPV or video equipment to recover their copter more easily in case of a GPS failsafe situation, also MP should display a line pointing in the direction in which the copter was facing while arming..this way even if the user forgets or is too lazy to remember the direction in which the copter was facing when armed...he can always refer to the MP to find out which direction the copter will head when in simple mode..this will make manual copter recovery more easy...but that is for another issue...I hope the GPS switchover method specified above makes sense

[R-Lefebvre]

@priseborough do you think this issue is resolved with the latest EKF code?

[rmackay9]

No, it's not resolved yet. There was a demo video by Hugues on the AC3.3 beta testing thread that shows the copter doing the controlled but fast move after the GPS switches.

[R-Lefebvre]

What is the desired behaviour? A controlled but slow move?

[rmackay9]

I think for Loiter and PosHold we would want to reset the target so that the vehicle doesn't move at all. In all the other modes it's probably fine as it is. I wonder if drift sees a hard lean as well..?

[R-Lefebvre]

Oh, I see. So maybe we need to maintain a number that is an offset between the two GPS, and if it switches, add that offset to the target?

[rmackay9]

Yes, exactly. I think if we polled the EKF constantly we could get it to tell us which GPS it's using.. or maybe we only do that after each GPS update (because presumably it would only switch over after a GPS update).

[R-Lefebvre]

This seems like it will be really hard to do. Let's say that once the GPS switches from GPS1 to GPS2, we apply the offset. I expect the copter will move in the direction of GPS1 first, then return to GPS2. This would be because the GPS position is offset immediately, but the EKF will "dampen" the position movement a bit. The GPS position has to slowly filter through the inertial solution.

I don't have any answers, just something to think about. Seems to me, we'd almost need two full EKF estimates, one with each EKF, and move between those.

I think we should talk about this on the dev call tonight. Maybe Paul will have more ideas?

[magicrub]

I think the offset is a horrible idea. When moving to a different GPS we should blend to it slowly, perhaps over 10 second. So more like an offset that decays. On May 18, 2015 5:37 AM, "Robert Lefebvre" notifications@github.com wrote:

This seems like it will be really hard to do. Let's say that once the GPS switches from GPS1 to GPS2, we apply the offset. I expect the copter will move in the direction of GPS1 first, then return to GPS2. This would be because the GPS position is offset immediately, but the EKF will "dampen" the position movement a bit. The GPS position has to slowly filter through the inertial solution.

I don't have any answers, just something to think about. Seems to me, we'd almost need two full EKF estimates, one with each EKF, and move between those.

I think we should talk about this on the dev call tonight. Maybe Paul will have more ideas?

— Reply to this email directly or view it on GitHub https://github.com/diydrones/ardupilot/issues/1875#issuecomment-103045155 .

[andyp1per]

This is the dual compass/IMU problem all over again. There is a "correct" answer but with only two datapoints you have no idea which one is right. In the first instance I think you should do the safe thing, i.e. land. I'm struggling to think of any other safe option - maybe RTL, but even that has issues. Carrying on seems dangerous.

[magicrub]

the bottom line is if you are falling back to a different sensor you should not induce a long-term error offset into that sensor's data. Fo all you know this backup sensor is better than the first and adding any offset is horrible. An offset should be used to transition from one sensor to the other but leaving that offset in long-term is nasty

On Tue, May 19, 2015 at 1:33 AM, Andy Piper notifications@github.com wrote:

This is the dual compass/IMU problem all over again. There is a "correct" answer but with only two datapoints you have no idea which one is right. In the first instance I think you should do the safe thing, i.e. land. I'm struggling to think of any other safe option - maybe RTL, but even that has issues. Carrying on seems dangerous.

— Reply to this email directly or view it on GitHub https://github.com/diydrones/ardupilot/issues/1875#issuecomment-103398676 .

[Gerporgl]

I am not sure what the status of the dual GPS support is. I got 2 GPS (both are UBLOX M8N), but I got scared by the auto switch behavior reported, so I did not even use it for real flight and turned it off (I am using 3.3 RC8) What I noticed when testing (with the copter landed and not moving), is that both GPS will report slightly different position (could be 0.5-5 meters I think) even though they have about the same HDOP and same SAT count (or +/- 2). I was thinking doing an average of both GPS location when the 2 are close in term of HDOP/SAT count would make sense... or in fact this could be a function that gives a different weight to each GPS based on their difference in SAT count and/or HDOP. For instance, if one GPS had 18 sat count, and the other 16, then an almost 50/50% average might be ok. But then the weight of the weaker one should quickly go down (exponentially or something) as its SAT count / HDOP becomes worst. Another way to see it: if HDOPs are very close (within the “noise” of HDOP report) then both should weight equal, but then as soon as one starts to have a higher HDOP, then it quickly (but still gradual/smooth) lose any weight in the average. The algorithm would have to remove any GPS from the average if its HDOP is high (2.5 or greater?), at least when the other one is very good / low. This would replace the current, and brutal, auto switch: “if sat count diff >= 2 then switch” To me, this could smooth out the transition between the 2 GPS. I know I should try to simulate this myself to see if it would be better or not, but I was wondering if a similar algorithm was already considered. One could think doing the average may not make sense, as it is making up a new lat/long position, but then if both GPSs of same brand/model have a 0.5-5 meters difference when they both have same SAT count and HDOP, and each one still don’t drift much by themselves and each provide a reliable position, then which one has the right position anyway? Finally, I know if the 2 GPS constantly drift a lot in HDOP and SAT count (one goes a lot higher than the other and vice versa), then doing the average would still make the copter move from one place to the other, but the case I have seen the most with my 2 GPS is that both are always around the same SAT count and HDOP, just that they have an “offset” which would cause the movement as they constantly auto switch.

[magicrub]

What you have described is sensor fusion and that is the main job of the EKF. However, right now only one gps input to the EKF is supported and adding another is already on Paul's long to-do list. On Aug 10, 2015 7:28 PM, "Gerporgl" notifications@github.com wrote:

I am not sure what the status of the dual GPS support is. I got 2 GPS (both are UBLOX M8N), but I got scared by the auto switch behavior reported, so I did not even use it for real flight and turned it off (I am using 3.3 RC8) What I noticed when testing (with the copter landed and not moving), is that both GPS will report slightly different position (could be 0.5-5 meters I think) even though they have about the same HDOP and same SAT count (or +/- 2). I was thinking doing an average of both GPS location when the 2 are close in term of HDOP/SAT count would make sense... or in fact this could be a function that gives a different weight to each GPS based on their difference in SAT count and/or HDOP. For instance, if one GPS had 18 sat count, and the other 16, then an almost 50/50% average might be ok. But then the weight of the weaker one should quickly go down (exponentially or something) as its SAT count / HDOP becomes worst. Another way to see it: if HDOPs are very close (within the “noise” of HDOP report) then both should weight equal, but then as soon as one starts to have a higher HDOP, then it quickly (but still gradual/smooth) lose any weight in the average. The algorithm would have to remove any GPS from the average if its HDOP is high (2.5 or greater?), at least when the other one is very good / low. This would replace the current, and brutal, auto switch: “if sat count diff >= 2 then switch” To me, this could smooth out the transition between the 2 GPS. I know I should try to simulate this myself to see if it would be better or not, but I was wondering if a similar algorithm was already considered. One could think doing the average may not make sense, as it is making up a new lat/long position, but then if both GPSs of same brand/model have a 0.5-5 meters difference when they both have same SAT count and HDOP, and each one still don’t drift much by themselves and each provide a reliable position, then which one has the right position anyway? Finally, I know if the 2 GPS constantly drift a lot in HDOP and SAT count (one goes a lot higher than the other and vice versa), then doing the average would still make the copter move from one place to the other, but the case I have seen the most with my 2 GPS is that both are always around the same SAT count and HDOP, just that they have an “offset” which would cause the movement as they constantly auto switch.

— Reply to this email directly or view it on GitHub https://github.com/diydrones/ardupilot/issues/1875#issuecomment-129681963 .

[Gerporgl]

Thanks for the clarification. I thought it was the role of EKF but wasn't sure how it works... Good to hear this is on the to-do list. Also, my understanding when we deactivate the AUTO SWITCH is that the second GPS is not used at all right? I mean not even for automatic failover to second GPS if the first one was to fail reporting completely? In that case, I'll remove the second GPS for now.

[magicrub]

Correct. You can also set GPS_type2=0 On Aug 10, 2015 8:17 PM, "Gerporgl" notifications@github.com wrote:

Thanks for the clarification. I thought it was the role of EKF but wasn't sure how it works... Good to hear this is on the to-do list. Also, my understanding when we deactivate the AUTO SWITCH is that the second GPS is not used at all right? I mean not even for automatic failover to second GPS if the first one was to fail reporting completely? In that case, I'll remove the second GPS for now.

— Reply to this email directly or view it on GitHub https://github.com/diydrones/ardupilot/issues/1875#issuecomment-129689709 .

[rmackay9]

Here's the EKF part of the patch from Paul. This still needs to be tested and merged to master: https://github.com/priseborough/ardupilot/commits/ekfPublishPosVelReset

The work to add the warning & handling in the vehicle's position controller still needs to be done.

[triolus]

I think eveyrone has the wrong idea. Invert the situation. 1. Gps 1 goes faulty. Gps 2 marks current position as the new hold position. Then the copter dont move from its current position ....

[magicrub]

@triolus the problem is two GPS's never have the exact same location. So, when switching there is a step input to the system that is not currently handled well.

[R-Lefebvre]

I think what he's suggesting is at the instant the primary gps goes "bad", mark the current position of the second gps as the new location. This would prevent the step change.

Sounds like a reasonable solution, but I'm sure there are lots of cases it won't work.

[magicrub]

It's still a step input. The real solution is a blend by the ekf and this is covered by another issue.

On Aug 8, 2016 10:52 AM, "Robert Lefebvre" notifications@github.com wrote:

I think what he's suggesting is at the instant the primary gps goes "bad", mark the current position of the second gps as the new location. This would prevent the step change.

Sounds like a reasonable solution, but I'm sure there are lots of cases it won't work.

— You are receiving this because you commented. Reply to this email directly, view it on GitHub https://github.com/ArduPilot/ardupilot/issues/1875#issuecomment-238320335, or mute the thread https://github.com/notifications/unsubscribe-auth/AEj7G6d-tq6df3a7trdZjXaBpY-J6Trvks5qd2zogaJpZM4DgPbK .

[triolus]

So your saying that you are trying to do two things: Using both gps's for accuracy and a safety redundant in case one goes bad. Are you sure the gps's are stable enough to be combined in an EKF to provide good data? Just inquiring/learning.

[R-Lefebvre]

I'm not sure where you're seeing the step input Tom?

It would however, offset all waypoints, including the RTL point.

[priseborough]

The problem with blending inside the back end of the EKF, is that we would need to add additional states for the estimated offset of each receiver. This would be expensive computationally given the scaling of computational load with number of states has an exponent ~= 2

One approach would be a Bayes filter or equivalent implemented inside the GPS library that used the receivers reported position, height and speed accuracy and combined data to estimate the offset for each receiver. This could also enable the difference in receiver position in body frame to be compensated for which is something we need to do for larger airframes

Another approach is to use a different GPS for each instance of the EKF, however this means the control loops would need to be able to handle the switch over between EKF instances or the GPS data would need to be corrected for offset before being used by the EKF, which still requires an estimator)

There are other approaches that could be used, but the solution adopted will depend on whether the dual GPS capability is seen as a backup, or something that is exercised every flight (eg tailsitter UAV's where one GPS is used in hover, and another in forward flight)

[R-Lefebvre]

Would love if the solution could use GPS positional difference to be used for heading estimation on larger aircraft. :) (ie: GPS on front and back of a heli, or wingtips on a plane)

[triolus]

I thought switching gps's with different locations could be as simple as a 2d proportional problem. guess theres more math involved..

[priseborough]

GPS height can also be used by the EKF depending on parameter settings so it has to be 3D. Then there is what to do with the GPs 3D velocity data that is also fused.

Hard switching is easier than blending, but not by much. A simple algorithm with an offset that collapses after the switch could be used, but then the switch criteria will need enough hysteresis to ensure switching won't occur too frequently. There is also the problem of what to do if the GPS solution has degraded and is bad before the switch, do you offset to match the EKF solution at switchover (creates a vulnerability to bad IMU data) or do you rate limit the change in offset for the secondary GPS and work on the assumption that the switch over from the degraded primary will happen before the offset is corrupted. The other design questions is how fast to decay the offset. The offset decay will also require the GPS velocity data to be adjusted to be consistent with the decay rate.

Getting the corner cases right will be 90% of the work.

[triolus]

how about this: Gps 1 goes out. 2. craft reverts back to alt hold. 3. after gps 2 comes online with ekf, it switches back to the previous flight mode and resumes... I don't mind a stop/start in the middle of using auto. The others shouldn't be bothered since the craft is moving.

[magicrub]

related: https://github.com/ArduPilot/ardupilot/issues/3547

On Wed, Aug 10, 2016 at 2:12 PM, Charles C. Spencer III < notifications@github.com> wrote:

how about this: Gps 1 goes out. 2. craft reverts back to alt hold. 3. after gps 2 comes online with ekf, it switches back to the previous flight mode and resumes... I don't mind a stop/start in the middle of using auto. The others shouldn't be bothered since the craft is moving.

— You are receiving this because you commented. Reply to this email directly, view it on GitHub https://github.com/ArduPilot/ardupilot/issues/1875#issuecomment-239005967, or mute the thread https://github.com/notifications/unsubscribe-auth/AEj7G6kqyyXlliNOpEwtW1aZh7vmKh58ks5qej6ogaJpZM4DgPbK .

[rmackay9]

This has been resolved and is included in Copter-3.5