Speed and Heading calculation starting from GPS coordinate

Thu, 12/10/2009 - 07:34 am

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