Math Help Trig

RoboTuna · 2013-03-06 15:23

I am in need of some help I am using two GPS sensors and taking Long and Lat from both and calculating distance. I found an equation for such a feet and have gotten it to work in excel. I have not been able to get it to work in Spin though.

OBJ

[SIZE=1]  M    : "Float32Full"[/SIZE]
[SIZE=1]  PST  : "FullDuplexSerial"[/SIZE]
[SIZE=1]  FS   : "FloatString"[/SIZE]
[SIZE=1]  [/SIZE]
[SIZE=1]CON[/SIZE]
[SIZE=1]
[/SIZE]
[SIZE=1]  _clkmode = xtal1 + pll16x[/SIZE]
[SIZE=1]  _xinfreq = 5_000_000[/SIZE]
[SIZE=1]  [/SIZE]
[SIZE=1]PUB Distance | D, ULat, ULong, BLat, BLong, DLong, R[/SIZE]
[SIZE=1]
[/SIZE]
[SIZE=1]  PST.Start(31, 30, 0, 9600)[/SIZE]
[SIZE=1]  M.Start[/SIZE]
[SIZE=1]  ULat   := M.Radians(40.74860)     'Lat  2[/SIZE]
[SIZE=1]  ULong  := M.Radians(-73.98640)   'Long 2[/SIZE]
[SIZE=1]  BLat   := M.Radians(40.74860)     'Lat  1[/SIZE]
[SIZE=1]  BLong  := M.Radians(-73.98640)   'Long 1[/SIZE]
[SIZE=1]  DLong  := ULong - BLong[/SIZE]
[SIZE=1]  R      := float(6371)      'In KM radius of Earth[/SIZE]
[SIZE=1]  Repeat[/SIZE]
[SIZE=1]    PST.tx(1)[/SIZE]
[SIZE=1]    PST.Str(String("Lets Do Math"))[/SIZE]
[SIZE=1]    PST.TX(13)[/SIZE]
[SIZE=1]    PST.Str(FS.FloatToString(ULat))[/SIZE]
[SIZE=1]    PST.TX(13)[/SIZE]
[SIZE=1]    PST.Str(FS.FloatToString(ULong))[/SIZE]
[SIZE=1]    PST.TX(13) [/SIZE]
[SIZE=1]    PST.Str(FS.FloatToString(BLat))[/SIZE]
[SIZE=1]    PST.TX(13) [/SIZE]
[SIZE=1]    PST.Str(FS.FloatToString(BLong))[/SIZE]
[SIZE=1]    PST.TX(13) [/SIZE]
[SIZE=1]    PST.Str(FS.FloatToString(DLong))[/SIZE]
[SIZE=1]    PST.TX(13) [/SIZE]
[SIZE=1]    PST.str(FS.Floattostring(R))[/SIZE]
[SIZE=1]    D := M.ACos(M.Sin(BLat) * M.Sin(ULat)+ M.Cos(BLat) * M.Cos(ULat) * M.Cos(DLong)) * R[/SIZE]
[SIZE=1]    PST.tx(13)[/SIZE]
[SIZE=1]    PST.Str(FS.FloatToString(D))[/SIZE]

Any help with this would be greatly appreciated. I am probably missing something easy but I cant find it.FloatString.spin Float32Full.spin Float32A.spin FullDuplexSerial.spin

Duane Degn · 2013-03-06 15:33

You're doing integer math on a floating point number.

  DLong  := ULong - BLong

You'll need to use the "FSub" method.

You do it again here:

D := M.ACos(M.Sin(BLat) * M.Sin(ULat)+ M.Cos(BLat) * M.Cos(ULat) * M.Cos(DLong)) * R

You'll need the "FMul" method to multiply floats.

BTW, F32 does all the FloatFull does but faster and with one cog. It does have a bug in its ATan2 method. There's another recent Prop trig thread that has an archive containing a patched version of F32 (attached to one of my posts). You might want to take a look at the other thread for some extra tips about using floating point numbers.

RoboTuna · 2013-03-06 15:50

D := M.FMul(M.ACos(M.FAdd(M.FMul(M.Sin(BLat),M.Sin(ULat)),M.FMul(M.FMul(M.Cos(BLat),M.Cos(ULat)),M.Cos(DLong)))), R)

So this would make more sense to the Cog

F32 does Acos because I thought I looked thru it and I did not see that function called out.

Duane Degn · 2013-03-06 17:46

RoboTuna wrote: »

F32 does Acos because I thought I looked thru it and I did not see that function called out.

F32 does have an ACos method.

A section taking from summary view:

PUB ASin(a) | b
PUB ACos(a) | b
PUB ATan(a) | b
PUB ATan2(a, b)

I didn't check your code carefully but it looks like you have the right idea.

RoboTuna · 2013-03-06 18:56

Okay I am now using F32 thanks for that.

I run the program and it gives me a different result then when I used Float32Full.

Also when I change a variable I do not get a change in Result.

Duane Degn · 2013-03-06 19:11

RoboTuna wrote: »

Okay I am now using F32 thanks for that.

I run the program and it gives me a different result then when I used Float32Full.

Also when I change a variable I do not get a change in Result.

What where the results?

What where you expecting? What happened?

What variable did you change? What where you expecting? What happened?

More information would help a lot. It would also help if you posted the code you used.

RoboTuna · 2013-03-06 19:21

Lets Do Math0.7111972 "Lat2
-1.291306 "Long2
0.7111972 "Lat1
-1.291306 "long1
0 "Dlong
6371
6.956075 "Result of equation.

If all the variables match meaning the positions are the same you should get 0 its a distance calc.

I changed the Lat2 from 40.74860 to 40.74861 which should get me around .0011 Km

I only get a change when I change 40 to 41 or 40.7 to 40.8 for lat2

OBJ

  M    : "F32"
  PST  : "FullDuplexSerial"
  FS   : "FloatString"
  
CON


  _clkmode = xtal1 + pll16x
  _xinfreq = 5_000_000
  
PUB Distance | D, ULat, ULong, BLat, BLong, DLong, R


  PST.Start(31, 30, 0, 9600)
  M.Start
  ULat   := M.Radians(40.74860)     'Lat  2
  ULong  := M.Radians(-73.98640)   'Long 2
  BLat   := M.Radians(40.74860)     'Lat  1
  BLong  := M.Radians(-73.98640)   'Long 1
  DLong  := M.FSub(ULong,BLong)
  R      := float(6371)     'In KM radius of Earth
  Repeat
    PST.tx(1)
    PST.Str(String("Lets Do Math"))
    PST.TX(13)
    PST.Str(FS.FloatToString(ULat))
    PST.TX(13)
    PST.Str(FS.FloatToString(ULong))
    PST.TX(13) 
    PST.Str(FS.FloatToString(BLat))
    PST.TX(13) 
    PST.Str(FS.FloatToString(BLong))
    PST.TX(13) 
    PST.Str(FS.FloatToString(DLong))
    PST.TX(13) 
    PST.str(FS.Floattostring(R))
    D := M.FMul(M.ACos(M.FAdd(M.FMul(M.Sin(BLat),M.Sin(ULat)),M.FMul(M.FMul(M.Cos(BLat),M.Cos(ULat)),M.Cos(DLong)))), R)
    PST.tx(13)
    PST.Str(FS.FloatToString(D))

Sorry for the confusion

Duane Degn · 2013-03-06 20:10

I think you're running into the limit of 32-bit floats.

I'm pretty sure there are other algorithms for calculating distance that assume a flat surface that don't require as much math. I think travelling 1.1 meters it's safe to assume the earth is flat.

I know these alternate equations have been discussed on the forum recently. Maybe someone will provide a link?

You might want to look at some of the other GPS projects to see how they did it. I think I have some GPS links in my index. I do, they're in post #5.

Batang · 2013-03-07 02:08

If you want a less processing intensive method to calculate the distance between 2 points below is a function I wrote a while back.

The function is in C but is easy to convert to spin.

This function is accurate if the distance is less than 100km, the result is in kilometers.

The value 111.226264 is 1 degree in kilometers of longitude (or latitude, I can't remember which) at the equator, for more accuracy just plug the value at your location.

Cheers.

double Distance(double lat1, double lon1, double lat2, double lon2) // Get Distance Between 2 Points
{
double lat = lat2-lat1;
double lon = lon2-lon1;

return sqrt( (lat * lat) + (lon * lon) ) * 111.226264;
}

RoboTuna · 2013-03-07 16:02

[QUOTE=
double lat = lat2-lat1;
double lon = lon2-lon1;

return sqrt( (lat * lat) + (lon * lon) ) * 111.226264;
}
[/QUOTE]

Okay so is it "sqrt(Lat1*Lat2)+(Lon1+Lon2)*111.226264" correct also what are you using the double lat and long for.

Do you have a source on were to find the more accurate 1 deg to km number.

Thanks.

Heater. · 2013-03-07 16:26

Have you not heard of Pythagoras?
http://en.wikipedia.org/wiki/Pythagorean_theorem

What you have written is not the same as what Robo Tuna wrote.

lat2-lat1 is the "height" of a right angle triangle. lon2-lon1 is the "width" or base of the triangle.
The lengh of the "diagonal" side is given by squaring, adding and square rooting as wikipedia shows.

Now, this is only an approximation as it assumes that the world is flat. It may be sufficiently accurate for short distances. You will need to change that 111.22... depending on your latitude. For example where I am, at 60 degrees north, it's only 55.60Km per degree.

You can get an accurate degree to km number with the calculator here http://www.movable-type.co.uk/scripts/latlong.html where they also have more accurate formulas you can use.

Duane Degn · 2013-03-07 17:19

Let's call the 111.22. . . number y. This gives you the kilometers per degree latitude.

Let's call the kilometers per degree longitude x. The value of x would be the same as y at the equator. Everywhere else in the world the value of x will be less than the value of y. At the poles the value of x would be zero.

To find y at your latitude use the equation:

x = y * cos(latitude)

You'll probably want to compute x on a PC or calculator and use it as a constant.

The the distance equation is:

d = sqrt( ( ( (Lat2 - Lat1) * y) ^ 2) + ( ( (Lon2 - Lon1) * x) ^ 2))

At least that's my theory. Someone please correct me if I'm wrong.

Edit: Sorry, I had x and y switched in several places. I started the post using the latitude multiplier x but decided it would make more sense to use x for east west distances and y for north south distances. I think I've fixed it.

RoboTuna · 2013-03-07 18:45

What is X in that equation is x the long and then you take cos of lat

RoboTuna · 2013-03-07 18:46

Heater. wrote: »

Have you not heard of Pythagoras?
http://en.wikipedia.org/wiki/Pythagorean_theorem

What you have written is not the same as what Robo Tuna wrote.

lat2-lat1 is the "height" of a right angle triangle. lon2-lon1 is the "width" or base of the triangle.
The lengh of the "diagonal" side is given by squaring, adding and square rooting as wikipedia shows.

Now, this is only an approximation as it assumes that the world is flat. It may be sufficiently accurate for short distances. You will need to change that 111.22... depending on your latitude. For example where I am, at 60 degrees north, it's only 55.60Km per degree.

You can get an accurate degree to km number with the calculator here http://www.movable-type.co.uk/scripts/latlong.html where they also have more accurate formulas you can use.

I have tried those equations in Spin no luck as you can see above in my original posts.

Duane Degn · 2013-03-07 19:39

RoboTuna wrote: »

I have tried those equations in Spin no luck as you can see above in my original posts.

I had several of my x's and y's switched in my earlier post. I've since fixed the post (I think).

It will take some thought on how to manipulate these numbers. You may want to take advantage of Spin's "**" operator and do some of the calculation with 64-bit integer math. I don't know if there's an integer square root method or not. If not, you would probably want to wait to convert numbers to floats until after the multiplication and addition have been completed.
I've heard if you think you need floating point numbers to solve a problem then you probably don't understand the problem.
It may be a good idea to use the "Programmer" mode of Windows' calculator to check how many bits the various steps in the calculation need.

It would probably also be a good idea to see how others have solved this problem with the Propeller.

Batang · 2013-03-07 21:30

If you have a desktop C compiler you can use the function Distance1 below to get the value for kilometres / degree.

Also you can compare the 2 functions i.e. the earlier example.

Cheers.

double Distance1(double lat1, double lon1, double lat2, double lon2)
{
#define EARTHRADIUS 6372.795477598
#define RADIAN 0.0174532925

double dx, dy, dz;
lon1 -= lon2;
lon1 *= RADIAN, lat1 *= RADIAN, lat2 *= RADIAN;

dz = sin(lat1) - sin(lat2);
dx = cos(lon1) * cos(lat1) - cos(lat2);
dy = sin(lon1) * cos(lat1);

return asin(sqrt(dx * dx + dy * dy + dz * dz) / 2) * 2 * EARTHRADIUS;
}

double Distance2(double lat1, double lon1, double lat2, double lon2)
{
#define DEGREEKM 111.226264 // 1 Degree In Kilometres Approx

double lat = lat2-lat1;
double lon = lon2-lon1;

return sqrt( (lat*lat) + (lon*lon) ) * DEGREEKM;
}

main()
{

double lat1, lat2, lon1, lon2;

lat1 =0; lon1 =0;
lat2 =0; lon2 =1;

printf("Distance1 %f km \n",Distance1(lat1,lon1,lat2,lon2) );
printf("Distance2 %f km \n",Distance2(lat1,lon1,lat2,lon2) );

return 0;
}

Duane Degn · 2013-03-08 01:40

Batang wrote: »

If you have a desktop C compiler you can use the function Distance1 below to get the value for kilometres / degree.

@Batang, Your program is fine for those living in Malaysia of somewhere else near the equator. For Heater, the computed distance using your equation would be double the real distance when travelling north/south. Edit: I didn't read Batang's post carefully enough.

Us non-equatorial dwellers need to adjust our longitudinal kilometers per degree by the cosine of our latitude.

@RoboTuna, After thinking about this a bit, I don't think using 32-bit floating point numbers will be a problem. I think the precision of the GPS unit will likely be the limiting factor in being to accurately measure distances using GPS coordinates.

As I mention previously, the equation I gave earlier, is only useful over small distances since it assumes the earth is flat.

Andrew Williams has GPS project he has posted to the forum. The object "Navigation_System" has a method "calculate_distance" you may want to look at. I'm guessing the method "calculate_distance" takes into account the curvature of the earth.

I decided to write a little program to try out the equation I gave in post #12.

Here is the method (and helper method) that calculates the distance between two coordinates.

PUB CalculateDistance(fLat1, fLon1, fLat2, fLon2) | fAverageLat

  fAverageLat := FMath.FAdd(fLat1, fLat2)
  fAverageLat := FMath.FDiv(fAverageLat, 2.0)
  
  fLat1 := FMath.FSub(fLat2, fLat1)
  fLon1 := FMath.FSub(fLon2, fLon1)


  fLat1 := FMath.FMul(fLat1, F_KILOMETERS_PER_DEGREE_LAT)
  fLon1 := FMath.FMul(fLon1, KilometersPerDegLongitude(fAverageLat))


  fLat1 := FMath.FMul(fLat1, fLat1)
  fLon1 := FMath.FMul(fLon1, fLon1)


  
  result := FMath.FSqr(FMath.FAdd(fLat1, fLon1))


PUB KilometersPerDegLongitude(localFLatitude)


  result := FMath.FMul(F_KILOMETERS_PER_DEGREE_LAT, FMath.Cos(FMath.Radians(localFLatitude)))

You could try changing these constants to test out the algorithm.

START_LATITUDE = 40.00000  START_LONGITUDE = -112.0000
  END_LATITUDE = 40.010000
  END_LONGITUDE = -112.0100

Here's the simple output from the program.

Distance = 1.400 km

I tested the above coordinates on the site Heater linked to and their solution for the above distance was 1.401 km. So the two programs disagreed by less than a tenth of a percent.

The difference become much larger when you use larger distances. I think the above equation should be safe to use if the distances traveled are only a couple of km.

max72 · 2013-03-08 01:43

I posted in the past this:
http://forums.parallax.com/showthread.php/128884-integer-only-GPS-navigation-object

It the same approach. You use a cartesian planar projection. distance is ((deltalat)^2+(deltalong*cos(latitude))^2)^0.5
It is good until the distance small enough (tens or hundreds of kilometers) and you are not near the pole.

Using floats will not work because you loose too much precision, so if you are navigating with a bot the rounding errors will kill you.

The object I posted converts everything to integer, and it is not really intuitive, but woks well, with the exception of the out of line error (still rounding errors).
I used it on a sailing boat so it is tailored to nautical miles and knots. One prime over the equator is one nautical mile, so I adapted everything to this "unit".
You have distance, absolute angle to target, and if you enter you heading you have the angular correction.

Massimo

Duane Degn · 2013-03-08 01:58

max72 wrote: »

I posted in the past this:
http://forums.parallax.com/showthread.php/128884-integer-only-GPS-navigation-object

It the same approach. You use a cartesian planar projection. distance is ((deltalat)^2+(deltalong*cos(latitude))^2)^0.5
It is good until the distance small enough (tens or hundreds of kilometers) and you are not near the pole.

I'm pretty sure it was your post here that I was thinking of when I mentioned, in post #8, a recent forum discussion about flat earth math.

My recent experiments with a digital compass got me thinking about your earlier GPS discussion. Thanks for posting the link to your object.

Batang · 2013-03-08 02:09

@Batang, Your program is fine for those living in Malaysia of somewhere else near the equator. For Heater, the computed distance using your equation would be double the real distance when travelling north/south.

As I indicated earlier the Degree In Kilometres would need to change, for the examples values you used i.e.

START_LATITUDE = 40.00000 START_LONGITUDE = -112.0000
END_LATITUDE = 40.010000 END_LONGITUDE = -112.0100

Will require a value of #define DEGREEMETERS 98.85213.

The picture below gives the results (using above values) of three functions i.e. the 2 I posted earlier plus the following function:

double Distance3(double lat1, double lon1, double lat2, double lon2)
{
#define RADIAN 0.0174532925
#define EARTHRADIUS 6372.795477598

double dlat = (lat2 - lat1) * RADIAN;
double dlng = (lon2 - lon1) * RADIAN;

double tmp = pow(sin(dlat / 2), 2) + cos(lat1 * RADIAN) * cos(lat2 * RADIAN) * pow(sin(dlng / 2), 2);

return ((2 * atan2(sqrt(tmp), sqrt(1 - tmp))) * EARTHRADIUS);
}

Anyway as per my original post if you want a simple less processing intensive function and working over short distances then the Pythagorean method is ideal.

In the case of embedded C programming the function I posted only requires the sqrt function from the maths library and only subtraction and multiplication, executes fast.

Cheers.

Duane Degn · 2013-03-08 02:26

@Batang, You're right. Sorry, it's been too long since I've used C and I only saw the equation that would work at the equator.

Thanks for the clarification.

Math Help Trig

Comments