GPS waypoint proximity

[Buriedcode]

Him

I have a basic understanding of how GPS works, but just thinking about using an actual module for a small project.  I'll make it brief.

According to the NMEA spec, proximity waypoints can be set along with a radius around that point.  I'm aware of the relative accuracy (looking at a few metres, ideal) would it be possible to set a waypoint at ones current position, set the radius to say, 100 metres, and use the proximity alarm to indicate whether one is within that zone?  So rather than using the alarm to see when the area is entered, the inverse is used to indicate one has left that zone?

I was hoping for a small low power embedded app, but if it doesn't work the way I described, one would have to compared current position to the fixed position and use trig to calculate the distance between the points - doable on micro's, but always nice if one can avoid doing heavy maths on 8/16/32 (cortex m0).  Bearing is unimportant, but of course would be handy to point to the centre of the fixed position.

I've googled a fair bit, and whilst there is information for complete devices, I haven't found much info on using modules for waypoints.  Ideally (perhaps ignorantly) I would set the module up, wait for a fix, set the update rate to fairly low (1hz or less) to save power, set a way point and just look for certain data spat out of the module indicating whether or not one is in the 'zone'.  The sentences I'm looking at are:

Code: [Select]

$GPBEC - Bearing & Distance to Waypoint, Dead Reckoning
$GPBOD - Bearing, Origin to Destination
$GPBWC - Bearing & Distance to Waypoint, Great Circle
$GPBWR - Bearing & Distance to Waypoint, Rhumb Line
$GPBWW - Bearing, Waypoint to Waypoint

And it appears the distance to waypiont is given in miles.  Would like this in metres but of course can convert.  Dead reckoning wouldn't be accurate and from what I understand the difference between Rhumb line and great circle is that a Rhumb line takes into consideration the fact one travels across the surface of a sphere.  For small distances I imagine there won't be much difference between the two.

I have a module on order to use with an arduino (or PIC, or cortex-m0, Xmega etc.. ) that uses the ublox-6 core.  But I realised that even with the hardware and a test app I'm not sure where to start, thus, a post!  Anyone have experience with this?

[rs20]

I have no relevant experience; I had no idea that (some) GPSs were capable of having waypoints set (in fact, I've never written a single byte to a GPS at all, I've always used them as read-only devices).

I will contribute that if you go down this road, you're reliant on how your particular module works, and you never know if different modules will have different bugs/features. On the flip side, computing a dozen trig functions per second is not "heavy maths". Even an 8-bit AtTiny running at 1 MHz could easily handle that. It's only when you're doing trig on audio streams at 44100 sample/second that things become hairy, GPS is 1 sample per second. So I'd suggest you weigh that up against potentially (?) vendor specific NMEA sentence.

Yes, rhumb line and great circle are almost identical over short distances. For two antipodean points (the most extreme worst case), rhumb line is the diameter of the earth ("as the neutrino flies", straight through the earth), whereas the great circle is half the circumference ("as the plane flies", going around the earth).

EDIT: Looks like I was wrong about the definition of rhumb line. The Rhumb line is (a portion of) the crazy spiral path you take if you maintain constant bearing. It is always longer than the great circle, but as before of course it's extremely close for nearby points.

[Harvs]

For calculating great circle stuff, instead of dealing with spherical trigonometry it can be done using vector math.

I did some stuff about 2 yrs ago, calculating distances to intersecting paths and it was much simpler once I found out the whole vector math thing.

[Kilrah]

+1, never seen that in use anywhere. It may be part of the NMEA spec but it would be good  to look for support on the actual module you're planning to use. Wouldn't be surprised if pretty much nobody implemented it.

And yes doing that even on an 8-bit micro is a non-issue. We've got a device that runs on an atmega64, does a ton of real-time control stuff, AND runs GPS distance calculations once a second without problems.
There are some "optimized" algorithms around too.

[lincoln]

Hello,
   Those messages are only really implemented in GPS systems rather than the embedded modules, you will have to do the math on your own.

[Buriedcode]

Thanks for the replies!

So it looks like I do have to calculate this.  I suppose in a way that is better as  I'm not relying on a 'black box' being accurate, also allows almost any gps module to be used (given other restrictions).

As this is going to be rather small and low power (because its small) every milliamp counts.  This means low duty.  I iwll have to run tests as to whether it is better to run a uC at a relatively low clock, and have it constantly runt he calculations, or wake it up and run it in bursts at full speed.  I was apprehensive about doing trig calculations with floats because the main micro *may* have do be filtering other sensors - I was going to go for a PIC because of the different power/osc modes, but looks like I may have to bump this up to a cortex M0.

I have some webpages bookmarked that explain the algorithms used by 'tinyGPS' so I don't think I'll ahve much trouble.  Just have to time the calculations on different devices to see which is the best (or rather fastest/lowest power).

Cheers

[SL4P]

Remember, if your reference receiver is static most of the time, you can average the lat/lon over time to derive a very accurate home position, which will eliminate half the error when comparing the live location with another continuously mobile receiver.

[LazyJack]

Get a handheld small gps, like a Garmin or such. Set up the waypoint/route in that and it will provide these NMEA senteces. A standaline GPS module will not usually provide waypoint data, nor it is possible to set up wpts in them. They only give position and fix related data.
Grear circle, rhumb line data is usually only given in marine and aviation related GPS devices, because these are the use cases where it makes sense.

[Ian.M]

You don't need to do spherical trig for this.  For a local area where the difference between a flat plane and the geoid is insignificant, you can treat the GPS coordinates as if they were on a rectangular grid. 

* 1' difference in Latitude == 1 Nautical Mile
* 1' difference in Longitude == Cos(Latitude) Nautical Miles.

As Cos(Latitude) is not sensitive to small changes of position, you can simply use a 90 entry lookup table for it, and store the value when the reference position is set

You then simply use Pythagoras and compute the inequality:

   dLat²+dLong²*Cos(Lat)²<R²

with precalculated Cos(Lat)² and R².

If you've got more processing power, one can get into stuff like determining if a point is inside or outside an arbitrary polygon (stored as a list of the Lat, Long coordinates of the vertices).  See http://geomalgorithms.com/a03-_inclusion.html

Edit: I got confused about the Cos(Lat) term.

[Buriedcode]

Wow thanks Ian.  Whilst I was thinking just a radius (circle, with little regard for 'height', so probably a cylinder zone) I suppose an octagon or hexagon could work just as well.  The grid idea sounds ideal, as accuracy isn't necessarily paramount, assuming the GPS has ideal conditions, even 3-4 metres is fine for processing, although I imagine combined with real life this would equate to 10-12 metres.

It looks like this is going to be a good introductory project for me for GPS.  I'll start with using an Arduino (just for testing) with a button that stores the current point, then constantly compares the current location.  As I wander a round, if it detects I've entered the zone, it'll beep, and store that point in flash, and do the same when I leave a zone.   Then download those co-ordinates and plot them on a map to see the accuracy - hopefully a lot of dots around a rough circle that is the radius I set.  Then use different algorithms or varying complexity to see which one is the most accurate, and/or efficient.

I often get bogged down with 'example projects' trying to perfect them and losing sight of the fact its a stepping stone.  But its a nice side project - one that I could use on different platforms.

[Ian.M]

Sounds possible.  The circular proximity area can certainly be calculated easly on an 8 bit MCU.  The polygonal area is a lot harder to calculate and you'll probably need a more powerful MCU to (a) fit it in and (b) calculate it with a reasonable update rate.

N.B. I had an error in the above formula (now fixed), Cos(Lat) starts at 1 at the equator and goes to a limit of zero at the north or south poles, as does the distance represented by 1 minute of Longitude.  To convert from dLong to a distance in NM, you need to divide by Cos(Lat).  As division is costly on 8 bit MCUs, precompute Cos(Lat)² when you store the center position.   (x^-2 is 1/x²)

Edit: its multiply by Cos(Lat)² not divide!

[Buriedcode]

Hi Ian,

Yes, I was thinking, given the 'zone' will have a maximum radius of say 100 metres, I was hoping to treat the area as pretty much flat.  So when the centre point is set, use look up tables, or just calculate the distance per minute for both long and lat at that particular location (the device should work anywhere on the planet, but I doubt it'll be used near the poles).  Then hopefully it would just be a case of Pythagoras: distance = (dlong^2 + dlat^2) ^ 1/2.   Some hysteresis is probably required, so there will be two limits set: threshold distance to enter the zone,  and threshold distance to leave it.

As I'm comparing the distance to a fixed number, when the radius is set, I could just square this - negating the need for a square root, store it, meaning I would only have to convert current long/lat to a distance in metres, then compare my stored distance(^2) to (dlong^2 + dlat^2). 

So continuous measurements would only require two multiplications, one addition, and two comparisons.  Set radius requires two multiplications (one square for each threshold), and set point would require a tweak to the latitude distance (based on absolute latitude), and a calculation of the longitude distance.  As the set point is only done once in a while (literally every few hours) it can be as complicated as needed.  I think this also has the benefit of converting the long/lat I get from the GPS to metres.  Given the largest value is 100 metres, squared that is only 10000 - which would fit in an int, and wouldn't be very accurate. Perhaps I could use centimeters instead, giving a maximum stored distance in an unsigned long as (2^32)^1/2 cm, or 655.3 metres.

So there seems to be several levels of complexity to try.  With the simplest perhaps accurate but very specific to the application (limited range, assumptions made about where it will be used setting limits on lattitude); and the most complicated being the Haversine  formula.

Note:  I just noticed the above is pretty much exactly what you suggested in your post yesterday...

As Cos(Latitude) is not sensitive to small changes of position, you can simply use a 90 entry lookup table for it, and store the value when the reference position is set

You then simply use Pythagoras and compute the inequality:

   dLat2+dLong2*Cos(Lat)-2<R2

with precalculated Cos(Lat)-2 and R2.

Apologies, it didn't quite sink in when I read it >.<

[Ian.M]

Working in metres isn't so easy.   1' of arc at the poles of a WGS84 geoid is approx 1861m and at the equator is approx 1843m, a variation of nearly 1%.

Taking the original 1791 Paris half-meridian/10,000 definition of the Nautical Mile using modern measurements gives a mean historic nautical mile of 1852.216m.  (The N.M. is currently defined as 1852m exactly.)  Unless you want to have a lookup table for 1' of arc with respect to latitude, you'll have an error of +/-5m per Km depending on your latitude even using the historic N.M.

If the GPS receiver outputs 5 decimal places of the minutes of Lat and Long in the NMEA0183 GLL sentence, you'd need 28 bits for the longitude as a fixed point binary number and 27 bits for the latitude.   5 decimal places is a resolution of 1.85 cm. However few receivers are anywhere near that accurate, and a typical consumer grade receiver will have a horizontal accuracy (95% confidence) of about 3.5m

Therefore there is no point in trying to work to finer resolution than 1m.  It may even be worth doing the initial coordinate subtraction in BCD using the raw ASCII numbers so you can reduce the number range before converting to binary for the multiplications.

[Buriedcode]

Hi Ian, thanks for the reply.  Sorry to continue with questions but I'm learning more from you that I have in a week of googling (might be losing my googling skills here..).

Alas the distance, radius set, should be in metres.  I can of course do calculations in miles and then convert - I'm going to have to use floats of fixed-point maths at some point, I just thought it might make calculations easier to deal with integers.

The GPS module I'm planning to use is based on a u-blox 6, and the manual indicates it spits out NMEA sentences with minutes to 6 decimal places. Latitude ddmm.mmmm, but the examples have 6 numbers after the decimal place, the same with longitude, just with  three digits for degrees. 

Whilst I understand it won't be that accurate in absolute terms a quick play on google maps shows there is indeed a distance of 1853.85m per minute of latitude, at 51 degree's.  So a lookup table to adjust this distance would be prudent.  If I use just 1853, and plug the numbers back in (difference in decimal minutes * 1853) I get a distance about 10cm out from google maps distance (not bad!) so perhaps I could get away with using integers for latitude 'distance per minute', storing the difference as an integer of minutes to 6 decimal places, then dividing by a million, or 100k to get units of 0.1m.

I'm still confused about the longitude distance. I know this clearly varies much more, and that the 'distance per minute' depends on cos(latitude), but not sure why the difference in longitude should be divided by it. If we are to get the distance from the difference in longitude, then surely the difference is 1855.35 (distance per minute at the equator) multiplied by cos(latitude) ?

So, our two values to plug into Pythagoras are (dlat * latdistance) and (dlong * longdistance) where lat distance is adjusted from 1853, and long distance is 1853 * cos(lat) ?  These are of course calculated at set point.

I think by working in minutes I'm confusing myself no end..

[Ian.M]

Yes. I had the Cos(Lat) stuff upside-down.    Sorry, its multiply by Cos(Lat) to go from dLong to distance!

Consider a point 1NM from the south pole (Latitude S 89° 59.0'). 
Cos(Lat) is very small. 
Travel 1° East or West and you only go 2*Pi/360  NM, which is about 32.5m.

I would suggest only supporting the range 80° N to 80° S as the 'plane sailing' rectangular grid approximation is not valid near the poles.  That will only exclude a handful of permanently inhabited high Arctic and Antarctic bases and reduces the risk of firmware crashes due to unexpected number range problems.

[Buriedcode]

Once again, thank you so much.  I'm not just learning about the algorithm itself, but it's now 'clicked' where-as before, I was on wikipedia, and even did some pencil and paper trig and wasn't getting that far.   You can probably tell i like to know exactly how things work.. and why.

So, on the agenda is:
-generate a lookup table to adjust the latitude distance per minute, based on lattitude - a small variation, but would only require maybe 80-90 entries.  And will only have to store the offset from equatorial latitude.

-Look at how compliers do cos(x) its input format, and output format (radians, degrees, and if the output is a float, or can be a fixed point number).

-Attempt to simplify things down to only require multiplications.

- Create a test unit, set a zone and radius, and wander around grabbing absolute points when entering/leaving the zone to check relative accuracy, and also points every few seconds for reference.  The accuracy of these points is purely down to the GPS, as I'll store the raw long/lat, rather than anything calculated - as well as points every few seconds.  This way I'll have a rather large flash chip with a set of GPS points, some tagged with 'entered zone', some 'left zone' and ones based on a timer.  Plotting those on a map, with the zone ones in red.. should see how good it is.

I'll have some 'fun' with excel to set this up. And the 'distance per long lat' figures, because they are rather small (1852, 1855.325) I'll calculate (or look up) these to two decimal places, so they'll fit into a 16-bit number, ergo treating 1855.325 as 185533 (metres x 100). Then multiply this by the long/lat differences (which will inevitably be millionths of a minute), and scale this down to 16-bit, as this then has to be squared.  This keeps some accuracy of (that 0.325 can add up for larger differences) but should allow it to fit inside a long when squared.    Apologies it that sounds confusing, the multiplying by 100, and multiplying minutes by a 1000000 will have to be scaled back down, its just an attempt to deal exclusively with integers for most of the calculations - I realise this probably won't work though.

[Ian.M]

Trig functions tend to require a *LOT* of code on 8 bit embedded systems.  They are worse than floating point, which one generally tries to avoid for efficiency reasons. You are usually best off going for 'pre-cooked' #included lookup tables generated out of Excel or by a C console program running in your build environment, + linear interpolation if the table would otherwise be too large.

I'd have to sit down and pick through your fixed point algorithm in pencil on squared paper to see if it will work without overflowing.  Based on my current ability to confuse / and *, I don't think that would be a profitable use of time for either of us!   Maybe sometime when life isn't stressing me so much . . . . .

You should familiarize yourself with the algorithm before trying to code an optimised embedded version. What I'd do is start off with a NMEA logger that logs selected sentences, + a couple of buttons logged as a '$PZZZB' sentence (proprietary, manufacturer code ZZZ and B for buttons ) so you can do your walkaround logging to SD card.  You can take that data file and feed it into a PC C console application implementation of the algorithm, (which can cheerfully use trig functions and doubles with little impact) to test it.  That also gives you test files you can stream to the embedded version when you write it, replacing the GPS input so you can compare the results.

[JimRemington]

The most straightforward approach to local GPS distance calculations on a micro is to use 32 bit integers to represent location, with degrees*1E6 as the basic unit. As suggested above, you can use the "equirectangular approximation" (Pythagoras theorem and cos(latitude) as a correction factor). This flat Earth approximation is valid up to several kilometers, for an overall accuracy of about +/-1 meter. I've implemented this on an 8-bit AVR for autonomous vehicles, and it works very, very well.

A great resource on all sorts of GPS related calculations is http://www.movable-type.co.uk/scripts/latlong.html

[Buriedcode]

Trig functions tend to require a *LOT* of code on 8 bit embedded systems.  They are worse than floating point, which one generally tries to avoid for efficiency reasons. You are usually best off going for 'pre-cooked' #included lookup tables generated out of Excel or by a C console program running in your build environment, + linear interpolation if the table would otherwise be too large.

Ahh yes, I considered a lookup table for the 'cos(lat) * nautical mile in metres' where the input will be latitude minutes*10^6, and the output metres*100.  That's a 16-bit word in (the difference in minutes wont' be more than 0.06, I'll cap it if it is), and a 16-bit word out.  The linear interpolation sounds like a good idea too - as always the compromise between execution time/speed, and memory.

I'd have to sit down and pick through your fixed point algorithm in pencil on squared paper to see if it will work without overflowing.  Based on my current ability to confuse / and *, I don't think that would be a profitable use of time for either of us!   Maybe sometime when life isn't stressing me so much . . . . .

Oh don't worry about it, I wasn't asking for you to go over everything   I just rambled on, using my posts as a sort of scratchpad for myself that I can refer too.  I too resort to pencil and paper often, I guess some do this out of principle, but even with scripts, clever excel functions, and various bits of software to do the donkey work for me.. often just writing it all down can make things clearer... a cup of a coffee and a pencil sharpener are all that is required.

You should familiarize yourself with the algorithm before trying to code an optimised embedded version. What I'd do is start off with a NMEA logger that logs selected sentences, + a couple of buttons logged as a '$PZZZB' sentence (proprietary, manufacturer code ZZZ and B for buttons ) so you can do your walkaround logging to SD card.  You can take that data file and feed it into a PC C console application implementation of the algorithm, (which can cheerfully use trig functions and doubles with little impact) to test it.  That also gives you test files you can stream to the embedded version when you write it, replacing the GPS input so you can compare the results.

Excellent suggestion!  I am prone to 'do everything at once' which makes debugging an absolute nightmare (stings me with FPGA work too..).  A nice lot of raw GPS data would indeed be a handy source for testing algorithms - also gives me a chance to brush up on windows and android apps, because side projects force one to knock up little bits of software, something to do over morning coffee.   So, first step, basic arduino (because its a piece of piss to knock up test apps) GPS logger.  I might even put all this on a blog... its good to have an interesting project for something new eh?

[Ian.M]

Adafruit have a GPS logger shield ready to go: https://www.adafruit.com/product/1272   Just add a two cell LiPO pack (charged externally) and some buttons and you'll be ready to write your logger.