Speed and Heading calculation starting from GPS coordinate

 Simple tool for the calculation of Speed and heading starting from Latitude and Longitude.




//Thanks to darcas.net/index.php/2008/10/12/calcolo-della-distanza-tra-due-punti-geografici/

Function SpeedAndHeading(TimeWv, LatWv, LonWv) 

Wave TimeWv, LatWv, LonWv

Duplicate/O TimeWv, Speed_Knot
Duplicate/O TimeWv, Speed_m_s
Duplicate/O TimeWv, Heading

Variable i,  z1, z2, HeadTmp

For(i=0;i<=NumPnts(TimeWv);i+=1)
	
	//Speed Calculation
	Speed_m_s[i] = (1000 * ( ACOS(SIN(Rad(latWv[i]))*SIN(Rad(latWv[i+1]))+COS(Rad(latWv[i]))*COS(Rad(latWv[i+1]))*COS(Rad(lonWv[i+1])-Rad(lonWv[i])))*6372.795477598))/(timeWv[i+1]-TimeWv[i])		
	Speed_Knot[i] = Speed_m_s[i] / 0.514444
	
	//heading Calculation
	Z1 = SIN(Rad(lonWv[i+1]) - Rad(lonWv[i])) * cos(Rad(latWv[i+1]))	
	Z2 = Cos(Rad(LatWv[i])) * Sin(Rad(LatWv[i+1])) - Sin(Rad(LatWv[i])) * Cos(Rad(LatWv[i+1])) * Cos(Rad(LonWv[i+1]) - Rad(LonWv[i]))	
	HeadTmp = (Grad( Atan2(Z1, Z2)))
	Heading[i] = (HeadTmp < 0) ? HeadTmp+360 : HeadTmp
	
EndFor

End

Function Rad(Gr)
Variable Gr

Return (PI * Gr /180)

End

Function Grad(Rad)
Variable Rad

Return (180 * Rad / Pi)

End
Cool. You are aware though that NMEA sentences already include speed and heading information?

regards,
Andy

December 10, 2009 at 05:56 pm - Permalink

Forum

Support

Gallery

Igor Pro 10

Learn More

Igor XOP Toolkit

Learn More

Igor NIDAQ Tools MX

Learn More