AC_Avoid: stop at fence incorrect if both circle and polygon are enabled

[dricketts]

Issue details

It is not safe to independently adjust the velocity to stop at the circle and the polygon fences. Whichever adjustment comes second (currently the polygon fence) can invalidate the adjustment from the first. Currently, this means that the circular fence can be violated, even if the position controller, sensors, etc work perfectly.

We should probably only allow one of the fences to be enabled at a time, unless there is a good reason to have both enabled at once.

[OXINARF]

Although you can do the same just with the polygon fence, I think that intercepting a circular and a polygon is easier for certain cases. Also the polygon fence applies to specific points while the circular fence is related to your arming location. So you may set a polygon fence that you don't want to exit but also don't want to get out of a circular fence related to your arming point.

Now, to solve this we would need to check circle and polygon at the same time right? Checking which one your current path is going to intercept first.

[dricketts]

@OXINARF Solving this issue is a bit more complicated than that. The problem is that the code in AC_Avoid adjusts not only your speed but also your direction. This is what allows you to slide at the boundaries of the fence rather than coming to an abrupt stop. However, this also means that the direction change induced by the polygon fence can cause a violation of the circular fence, and vice versa - whichever adjustment is computed second can cause the vehicle to "slide" outside the first fence.

I see three solutions:

1. Allow only one fence to be enabled at a time.
2. Change the AC_Avoid code to only change the magnitude of velocity, never the direction.
3. Make the adjustments dependent on each other. I'm not exactly sure how this would work.

In my opinion, (3) will be too complicated to implement. I'm not sure which of (1) and (2) is better. (1) seems nicer to me because sliding along the fence is nice. However, I don't really know what I'm talking about.

[OXINARF]

I think (3) would be the best to be implemented, but don't count on me to do that.

From (1) and (2), I would prefer (2) for two reasons:

• as I said, I do see value in having circular and polygon fence enabled at the same
• I see the "slide" feature more like a gimmick and I think that it may even be unwanted by some people (me included, when I hit the fence I want to stop, not keep moving)

And yeah, you don't know what you are talking about, you are just the genius that came up with this long time requested feature :smiley:

I'm going to assign this to @rmackay9 so he'll give his input here.

[dricketts]

@OXINARF What I like about the slide feature is that it provides a degree of robustness/smoothness. For example, without the slide feature, suppose you are very close to a polygon fence boundary and are traveling parallel to the boundary. Small oscillations in the pilot's roll/pitch inputs can cause you to oscillate between no intersection with the boundary and a close intersection with the boundary. This would result in the vehicle oscillating between exactly the commanded speed and jerking to a stop. The slide feature prevents this kind of behavior.

However, one thing to keep in mind is that we will run into this same issue when we add obstacle avoidance. The fundamental trade-off is the following: adjustments to the velocity magnitude are compositional - i.e. we can adjust for the circular fence, polygon fence, and obstacles independently. In fact, we can adjust for individual edges of the polygon fence independently. On the other hand, adjustments to velocity direction are not compositional - this makes all adjustments dependent. This is actually the cause of this issue: https://github.com/ArduPilot/ardupilot/issues/4429 - individual edge adjustments are not independent. All of this means that if people don't want the "slide" feature, then several issues go away, and the implementation becomes simpler. On the other hand, if people want the slide feature, then we need to be careful when having multiple things that the vehicle should avoid.

I still like the slide feature, but I really don't know what I'm talking about. My total experience flying any vehicle consists of no more than 20 flights with a 3DR Iris+, and Leonard Hall had all of the good ideas for AC_Avoid.

[dricketts]

I should also say that there is an in between solution - slide along some boundaries/obstacles and stop at others. For example, the vehicle could stop at obstacles and the circular fence but slide along the polygon fence. I don't know if that's a desirable behavior, but it's certainly possible to implement.

[rmackay9]

Really? Is this issue correct? I'm pretty sure I've checked this in the simulator and I thought it was ok. I think the vector shrinks in the direction of the fence but the vector itself can only get shorter. Because one axis (x or y) might be shortened more than the other, the direction of the vector changes but we never rotate the vector. The vector is just shortened. Because both circular fence and polygon fence adjustments are run one after the other, I don't think it will breach either fence.

[dricketts]

@rmackay9 I'm pretty sure this issue is correct. It is essentially the same as this issue: https://github.com/ArduPilot/ardupilot/issues/4429. The below (poorly drawn) image depicts the problem. On the left is the adjustment caused by one edge of the polygon fence (suppose the other edges are far enough away to be irrelevant). The red vector is the pilot's desired velocity, and the blue is the adjusted velocity vector. The image on the right depicts the subsequent adjustment from the circular fence. The yellow vector is the adjusted velocity from the circular fence Since the circular fence is at an acute angle to the edge, the yellow vector is unsafe for the polygon edge.

[rmackay9]

I just did a quick test in SITL and it seems to work. I didn't test a very complicated situation so if anyone has an idea of a complicated situation I can test, I will.

[rmackay9]

@dricketts, ok, thanks for the diagram. If the diagram for what happens with the circle is accurate then I think the issue is that the yellow vector should be pulled back so that it meets the blue dotted line.

I think we want to keep the sliding behaviour but I don't think the vector should ever get longer in either axis. I think in the circle example you've given, if it was drawn so the black line was perfectly horizontal, the vector would have grown in the horizontal axis (y-axis I think).

In any case, I guess it wouldn't occur in my example because there's no acute angles. TBH, I can live with this for now anyway. A fix would be great but perhaps we can handle it for now (i.e. for the AC-3.4 release) with some documentation on the wiki.

[OXINARF]

What I like about the slide feature is that it provides a degree of robustness/smoothness. For example, without the slide feature, suppose you are very close to a polygon fence boundary and are traveling parallel to the boundary. Small oscillations in the pilot's roll/pitch inputs can cause you to oscillate between no intersection with the boundary and a close intersection with the boundary. This would result in the vehicle oscillating between exactly the commanded speed and jerking to a stop. The slide feature prevents this kind of behavior.

I totally understand this is desirable in that case, but I think, that's life. If you are intersecting with the boundary you should stop. Maybe some kind of filtering could be added, I don't know.

Anyway, I'll comment if needed but I'll let you and Randy discuss this one :smiley:

[rmackay9]

One solution to the acute angle problem is perhaps to do this after we run each fence check:

• save the original desired x-y vector input but rotate into the vehicle's body frame (i.e. turn the north-east vector into a vehicle's forward-right vector).
• run the 1st fence check as it is now (using the original non-rotated north-east vector)
• take the resulting x-y vector and rotate it into the vehicle's body frame
• compare with the original, shorten x and/or y if they've gotten longer for some reason
• rotate back into the north-east frame and feed into the 2nd fence check (and we will perform another check after it's run too)

I'm not sure if it's a hack or not but I think it might work.

@OXINARF, I think in practice it will be annoying to fly if the walls are essentially sticky. The vehicle will stop at the fence and you'll always have to backup to un-stick it. Anyway, I'm sure, no matter what, we haven't reached nirvana yet with the object avoidance look-and-fell.

[rmackay9]

Sorry, the solution above, isn't going to work as-is. It clearly doesn't matter which way the vehicle is facing so the whole rotate-to-body-frame is a dud idea.

[olliw42]

I have not looked at the code just read this post and thought over it while jogging, the result: The issue has IMHO nothing to do with polygon or circle, it's all just a collection of surfaces defining a singly connected convex hull (I think/assume that's the limitation you imply, correct me if I'm wrong). I think the issue is because a linear algorithm is attempted, which I think isn't possible (I don't have the skills to proof that but I'm sure someone did many decades ago LOL). E.g., currently it is not guaranteed that the result is within the valid area. Maybe this could be an approach: Move along the LOS path from the current position to the expected position (i.e. along the red arrow), and find the point where the first fence surface is pierced. Now move along that fence surface until you either (a) reach another fence surface or (b) the distance from the starting point (i.e. velocity) has reached a certain maximum length (which could be the length of the blue arrow). Algorithmically you're of course not moving, but just find piercing points and edge points (the latter could be even pre-calculated) (the math is highschool, so I'm sure folks have developed efficient algorithms many decades ago). What this will do is to ensure that the target point is within the valid area, and will slide as much as possible. It will trap the copter at a nearby corner, the operator can "slide" out of that, or one could implement an additional action in that case, e.g. that one also moves along that 2nd fence surface until (a) or (b). :)

[dricketts]

@olliw42 I think that your analysis of the issue is pretty accurate, with one point of clarification: the polygon boundary is not necessarily convex. However, each time the fence logic runs, we do compute a convex polygon that is contained by the polygon fence. The exact construction is described here: http://math.stackexchange.com/questions/1852720/inscribing-convex-polygon-within-simple-polyon.

In any case, I think that the solution you propose is essentially the right approach. Taken to the limit, the idea is to compute the point within the convex shape that is closest to the pilot's desired velocity. Conceptually, this means sliding along as many fence boundaries as necessary until we find the closest point.

There is a relatively simple algorithm for computing this. The algorithm iterates through all constraints (e.g. polygon edges, circular fence, detected objects, etc.) and maintains a best velocity v. For each new constraint C processed, if v satisfies C, then continue. If v does not satisfy C, iterate through all prior constraints, and find the closest point to v on C that satisfies all prior constraints.

In the worst case, this algorithm is quadratic in the number of constraints. However, I believe that in practice, it will almost always be linear because there will almost always be at most one "relevant" constraint.

I do think that a linear algorithm is possible, using something like this algorithm (linear programming in two dimensions): http://theory.stanford.edu/~megiddo/pdf/lplin.pdf. However, this algorithm is fairly complicated and might not be faster in practice than the simple one that I proposed.

[olliw42]

thx for the links, especially the last one yeah, at the time of posting I was thinking that the polyfence is described by faces, but I then realized that it's much easier to just give the edge points (which I thus guess you in fact do), and that thus convexity isn't of any relevance I think this doesn't affect the proposed approach, since in that case the red vector wouldn't pierce a fence surface, and no correction would be needed (it algorithmically however means that one needs not only to calculate the piercing point, but also check that it is outside the allowed fencesurface, i.e. within the valid area, which should however be quite simple with essentially zero extra cost) I don't see a need for the inscribed convex polygon. The only thing I could see a need for is a method to check if a point is inside or outside. I probably however miss the point :). The "relatively simple algorithm" you describe is obviously the brut force approach, and obviously can be improved by simple means, making it faster than quadratic. I must admit that I would not have expected it possible to get it down to linear. Interesting. Anyway, the proposed approach is sub-quadratic, and nearly linear, i.e. doesn't grow terribly faster than linear. cheers :)

[dricketts]

@olliw42 The reason for the inscribed convex polygon is described here: https://gist.github.com/dricketts/dd27d50ac69ed2e2f949050bf7f54366. It is certainly not necessary, but I think it provides a degree of robustness.

I think I'm a bit unclear on your propose approach. I think you're trying to compute the closest visible point to the desired stopping point of the pilot. Is that correct? If so, there are linear algorithms but they're not entirely trivial. However, my bigger concern with this approach is related to a requirement that you may not be aware of: the code tries to maintain a margin between the vehicle and the fence. This means that when the vehicles is within this margin, the fence code should push it quickly back inside. What would your approach do in this case?

[olliw42]

It's not clear to me what "$q'$ is continuous in the position of $q$" is supposed to mean, continuity for discrete points isn't an obvious or well defined concept.

The reason why I don't get the need of the convex polygon $I$ is that if you're far away from edge E or F it's irrelevant and if you're close to E or F it cuts out paths which actually would be perfectly allowed to fly. It's robust, sure, but it would be even more robust to shrink the fly-zone to a point :).

I like this chicken fat margin thing, and it's good to hear you have something like that. However, that's unrelated to the issue which was the starting point of this thread, i.e. it's an addition to any algorithm. The answer what to do would strongly depend on what this margin is exactly supposed to achieve, it could be just a shrunken fly-safe zone, or it could be a layer in which the vehicle's behavior is changed, e.g. slows down, or whatever. My suggestion was related to the above graph. Now, if that graph does not represent what the code is doing, but was just - so to say - an illustrative example to illustrate a particular point, without any further deeper meaning, then everything I have said is probably totally irrelevant. If that graph is however a reflection of what is being done, then I would think the answer would be to just do exactly the same thing you're doing now when the point $q$ is outside of the fence/margin. Not sure what meaningful I actually could respond.

As regards the algorithm, I think I would not phrase it in terms of "closest" or such extremum concepts. It probably could be made to be so, but it has some flexibility in it (e.g. to slow down near the fence) which would break that. I just think that extremal is not a very relevant concept. Don't know how to add a figure, so it's here: www.olliw.eu/drop/fence.JPG. The suggestion is: You have N edges and thus N-1 faces, either lines or arcs. For each face you calculate the piercing point, which gives a list of N-1 $\lambda$ values, the smallest positive is of interest. If it's >1 then there is no fence pierced. Done. If it's <1 it's the first fence, or point 1, when find a point 3, by whatever length criteria you want. If point 3 is closer than point 2, take it, DONE. If not take point 2, DONE. As suggested one could also move along the next fence surface, to not get trapped in a corner, but I'm not sure that's desirable. In case of a reflex angle at point 2 one could continue the line and check for a fence, or use point 3. If $q$ is outside of the fly-zone do whatever you want to bring it back, e.g. what your doing now. If there should be a margin, shrink the zone by moving the edge points inside. And so on. It's N-1 calculations plus some more or less constant overhead, so not linear but sub-square. Very simple. Hope that helps better.

[dricketts]

It's not clear to me what "$q'$ is continuous in the position of $q$" is supposed to mean, continuity for discrete points isn't an obvious or well defined concept.

@olliw42 $q'$ is a function of $q$ (the desired velocity) and $p$ (the current position). I believe that it should be continuous in both of its inputs. In other words, the adjust_velocity function in AC_Avoid should be continuous in its inputs.

It's robust, sure, but it would be even more robust to shrink the fly-zone to a point :).

It is more conservative than necessary in certain scenarios, but flight tests suggest that it is sufficiently permissive. I think that robustness is important in this domain, and this justifies the conservativeness. Of course, that's just my humble opinion, and there certainly could be much better approaches.

I like this chicken fat margin thing, and it's good to hear you have something like that. However, that's unrelated to the issue which was the starting point of this thread, i.e. it's an addition to any algorithm.

It is not orthogonal to the algorithm you choose. As with the previous point, this algorithm should be continuous in positions close to the boundary, and in particular when the vehicle passes into the margin area. Keep in mind that this happens quite frequently, sometimes by just a few inches. You don't want the code to cause a big change in velocity simply because the vehicle went into the margin by 1 inch. This means that the margin behavior must be designed along with the rest of the algorithm.

In any case, I believe I understand your algorithm now. There's one issue that I see: after adjusting the velocity based on the first pierced edge, you have to check every other edge for a violation (because the fence can be concave). That is, the line segment from q to 3 can intersect an edge that was not intersected by the line segment from q to q'. However, it seems straightforward to move back along the pierced segment to a safe point.

I do think that the algorithm should not necessarily only consider the originally pierced fence segment. It should be able to adjust the vector over arbitrarily many segments. The reason is that there should be no difference between a fence consisting of two segments, AB and BC, and one segment AC, if A, B, and C are collinear.

[olliw42]

as regards the margin, I don't think it's really a biggy, not sure what further to say at the moment

I still can't see any reason for $I$. I of course can't object to a statement that robustness is important, but by that line of argumentation I have won: You can't beat my suggestion to shrink the zone to a point ;). This doesn't lead to anything.

" $q'$ is a function of $q$ (the desired velocity) and $p$ (the current position). I believe that it should be continuous in both of its inputs." well, then "my" algorithm is continuous. If you move p, then q' will never jump (as long as it is within it's definition area ;)). I think what you want to say is that it should not jump when crossing the fence. I of course agree with that - in principle. However I don't think that this is a criterion of much practical relevance. The algorithm is applied at discrete time steps, and you produce a sequence of jumps anyhow. So your method can be formally continuous but nevertheless produce a huge jump between to steps. You would need a limiting criteria on the jump size, i.e. rather something like a maximal gradient. Not sure such formal efforts will lead to anywhere. Also, in the forbidden area I would NOT think it's a question of how to find a q' from a projected q, but how to affect the velocity vector in the first place. This is fundamentally different, and can't be easily connected by the concept of continuity of points.

"There's one issue that I see: after adjusting the velocity based on the first pierced edge, you have to check every other edge for a violation" no, because if it could you would have chosen point 2 instead of point 3: "If point 3 is closer than point 2, take it, DONE. If not take point 2, DONE." See the two middle figures.

"It should be able to adjust the vector over arbitrarily many segments." I've suggested that the algorithm could be extended to also the next face. However, unless you allow the operator to put arbitrarily many fence edges at arbitrarily close distances, I don't see any reason to extend it to arbitrarily many segments. Correct, connected collinear fences I've not considered, but this should be really trivial.

as regards the margin, I guess I actually would approach that a bit differently and make the velocity dependent on the distance to the piercing point, E.g. if lambda is in the range 1...2 it is reduced, even if the fence is still away. Or one could shrink the edges dependent on the velocity. The more I think about it, I think the approach can also be well used for obstacle avoidance.

Anyway, I'm not saying that I have produced a final answer, in no way, some edge cases certainly need to be considered and better worked out, but I think the approach is quite flexible and extensible to accommodate them. Also, it was ONLY a suggestion for solving a specific issue ;). Maybe I'll think over the margin the next time I'm going jogging LOL.

[dricketts]

I of course can't object to a statement that robustness is important, but by that line of argumentation I have won: You can't beat my suggestion to shrink the zone to a point ;). This doesn't lead to anything.

@olliw42 Robustness is not the only important criteria. Of course, runtime and permissiveness are other important criteria. A constant function (i.e. shrinking the zone to the point) obviously has a better runtime but less permissiveness. Your legitimate proposal has a better runtime, better or incomparable permissiveness (depending on whether you consider multiple faces), and less robustness. These are all different points in the design space.

I think what you want to say is that it should not jump when crossing the fence.

No, I want it to be continuous. It might be even better, as you said, to enforce a maximum derivative as well, but this would require more conservativeness.

I should have mentioned earlier that my actual algorithm for the polygon fence operates on the velocity space. In particular, I solve the following optimization problem:

Find the vector safe_vel that is closest to the pilot's desired velocity, subject to the following constraints for each edge:

• Let P be the closest point to the current position of the vehicle.
• Let U be the unit vector in the direction of P from the current position.
• Let D be the distance from the current position to P.

safe_vel must satisfy safe_vel * P <= max_speed(D), where max_speed takes into account the margin. More precisely, max_speed(D) gives the maximum speed such that the vehicle can stop within D - margin distance.

Obviously, this doesn't immediately accommodate the circular fence, which is the purpose of this issue. My plan is to conservatively approximate the circle with a polygon. This polygon will be dependent on the vehicle's current position, so that the approximation will be better for parts of the fence that are closer to the vehicle. While this is again more conservative than necessary, code reuse is good, and I expect that the approximation will be good enough in practice.

no, because if it could you would have chosen point 2 instead of point 3: "If point 3 is closer than point 2, take it, DONE. If not take point 2, DONE." See the two middle figures.

Thanks for the clarification. I didn't understand the purpose of point 2.

Anyway, I'm not saying that I have produced a final answer, in no way, some edge cases certainly need to be considered and better worked out, but I think the approach is quite flexible and extensible to accommodate them. Also, it was ONLY a suggestion for solving a specific issue ;). Maybe I'll think over the margin the next time I'm going jogging LOL.

For now, I've implemented my approach, but it would be great to see your idea implemented as well. The more options, the better.

[lthall]

Hi all, So I think the issue here is we are only treating the case where two fence segments intersect. We don't check and treat the situation where a line segment intersects with a circular segment. In particular this causes a problem where the correct behaviour would be to stop at the intersection of the line and circular segment.

Sliding is really the only way to go when you run through all the options and tradeoffs. I wanted to do the stop at fence option but there are lots of issues. They can all be summarised by "how do we go again".

The other thing to keep in mind is "what happens if this goes wrong?" In this case we initialize a RTL or land. This doesn't cause a crash or fly away. I seem to remember seeing this as a potential issue but I said that we can tolerate the predictable response from the copter this will cause.

[olliw42]

robustness: Maybe we have different definitions of that word, To me robustness is that it garantes to keep the copter in the fly-safe, that the algorithm can't "hang up", and has a defined runtime. Not allowing flight paths which actually would be perfectly healthy is then not correctly described as robustness. continuous.: I accept you want it to be that, but I continue to find that of only formal relevance :). (BTW, the approach in your PR does break continuity).

"I solve the following optimization problem" I think I have to think a bit longer about what you're describing, it's not all obvious (e.g. what is safe_vel * P when P is a point,...)

"We don't check and treat the situation where a line segment intersects with a circular segment." not quite

• the issue in the above figure occurs also for acute line segments
• the proposed algorithm does treat lines and arcs alike (it's just that I have present the "math" for lines, to spare me the effort to write down a quadratic equation ;))
• with the current approach it can be, in principle, be solved simply by some further recurrences, which I think is what the recent PR is doing

Since a fence is not object avoidance I think it's the appropriate behavior to have the copter stop in a corner, the algorithm should only allow the operator to get easily out of the corner, by pulling back or sliding along the fence with a command which has a tangential component. As mentioned, the proposed algorithm could easily be extended to have the copter sliding itself out of a corner (by looking also at the 2nd fence surface), but as said I think this is more avoidance and not fence like.

[dricketts]

robustness: Maybe we have different definitions of that word, To me robustness is that it garantes to keep the copter in the fly-safe, that the algorithm can't "hang up", and has a defined runtime. Not allowing flight paths which actually would be perfectly healthy is then not correctly described as robustness.

Fair enough. Just to clarify, not allowing flight paths is not what I mean by robustness. Instead, this is a way to achieve robustness. I've been defining as continuity of the velocity adjustment induced by the fence. You can also imagine strengthening this to differentiability or even bounded derivative of the adjustment.

BTW, the approach in your PR does break continuity

@olliw42 How so?

I think I have to think a bit longer about what you're describing, it's not all obvious (e.g. what is safe_vel * P when P is a point,...)

Sorry, * in this case is dot-product (that's the syntax used in the code).

[olliw42]

PR: it is recursive, with a stop criteria which is not a convergence to a point, so it's not continuous (in general, and in the strict formal sense)(the case you sketched in the above should give an example)(t's not a concern, its nicely continuous on "my" level)

I got the * (if dot means scalar), but P is a point, measured from some origin, so - e.g. - that product depends on the origin, I have troubles seeing why that quantity should be relevant. I'm more along safe_velU \ D

[dricketts]

it is recursive, with a stop criteria which is not a convergence to a point, so it's not continuous

Could you elaborate? What is a stop criteria? Why is convergence to a point relevant?

I'm more along safe_velU \ D

Ah thanks - you're right.

[olliw42]

"Could you elaborate? " I probably wasn't really clear. If I got the description in the PR correct, what you're doing is to just repeat doing the corrections until you're satisfied. I.e in a first loop you go sequentially through all fence surfaces and apply the correction, as in the original code, but now run the loop again (maybe limited to only the relevant surfaces), run the loop again, and so forth. It's what I know as iterative. In case of a corner (e.g. the sketch in the above) this brings the point q' (which is now actually q'''''''''') to the corner. However, since it approaches that corner from outside the fly-safe zone it formally never gets into it, only numerical inaccuracies will bring it on spot. More troublesome, one has to repeat that indefinitely, so one has to define a stopping criteria, to stop that within reasonable time. This means that the resulting point is always outside of the fence and also that it jumps around within the stopping criteria limit. I have not looked at your code, but I'm sure you have seen that you have to add a condition to make the process to stop after not too many iterations, but for calculation time reasons this can't have been too many, so the limit isn't quite strong and the point this relatively jumpy. I might however also misunderstand what you're doing.

[dricketts]

@olliw42 I intended to implement a variant on the algorithm described at the beginning of the lecture: https://people.csail.mit.edu/indyk/6.838-old/handouts/lec3.pdf. There is no need for an infinite number of repetitions - the algorithm finds the optimum in worst case O(n^2).

Of course, that lecture describes an algorithm for linear objective functions, while my objective function is Euclidean distance. However, I convinced myself that this just requires a small tweak - the optimum does not necessarily lie at a vertex of the feasible space.

[olliw42]

thx for the link, I'll have to look at it, seems I didn't got your PR correct (as said I'm guided by the picture in the above, maybe I should get me a LP book) anyway, it occurred to me that one "only" needs a "corner detector", and if it triggers one can use the edge point right away ... this would keep you essentially linear, continuous, and would be just a minor addition to your previously existing code, sounds great ... I've not further thought on that, but maybe the corner could be detected by the alternating correction vectors (green vectors), or if the resulting vector swaps across an edge point ...

[dricketts]

@olliw42 A corner detector seems like a reasonable optimization. The algorithm I implemented is probably the simplest possible thing you could do. For now, I'm just trying to get something correct. I'm hoping that someone smarter than me can optimize it in the future - definitely take a crack at it if you have the time.

[dricketts]

I submitted a pull request for this: https://github.com/ArduPilot/ardupilot/issues/4829. I somewhat simplified the problem by changing the circular fence to a regular polygon fence centered at home.

[amilcarlucas]

Maybe #15550 fixes some of this?