Position calculations – simple and exact solutions
- by means of n-vector

This web-page contains solutions to various geographical position calculations, 10 examples are given below.

Written by the navigation group at FFI (The Norwegian Defence Research Establishment).
Most of the content is based on the following article:

Gade, K. (2010). A Non-singular Horizontal Position Representation, The Journal of Navigation, Volume 63, Issue 03, pp 395-417, July 2010.

Thus this article should be cited in publications using this page or the downloaded program code.

Figure colors: Red=Given. Green=Find this.

Example 1: A and B to delta

Given two positions A and B. Find the exact vector from A to B in meters north, east and down, and find the direction (azimuth/bearing) to B, relative to north. Use WGS-84 ellipsoid.

Solution example 1
Example one
Example 2: B and delta to C

Given the position of vehicle B and a bearing and distance to an object C. Find the exact position of C. Use WGS-72 ellipsoid.

Solution example 2
Example Two
Example 3: ECEF-vector to geodetic latitude

Given an ECEF-vector of a position. Find geodetic latitude, longitude and height.

Solution example 3
Example three
Example 4: Geodetic latitude to ECEF-vector

Given geodetic latitude, longitude and height. Find the ECEF-vector.

Solution example 4
Example four
Example 5: Surface distance

Given position A and B. Find the surface distance (i.e. great circle distance) and the Euclidean distance.

Solution example 5
Example five
Example 6: Interpolated position

Given the position of B at time t0 and t1. Find an interpolated position at time ti.

Solution example 6
Example six
Example 7: Mean position (center/midpoint)

Given three positions A, B, and C. Find the mean position (center/midpoint).

Solution example 7
Example seven
Example 8: A and azimuth/distance to B

Given position A and an azimuth/bearing and a (great circle) distance. Find the destination point B.

Solution example 8
Example eight
Example 9: Intersection of two paths

Given path A going through A1 and A2, and path B going through B1 and B2. Find the intersection of the two paths.

Solution example 9
Example nine
Example 10: Cross track distance (cross track error)

Given path A going through A1 and A2, and a point B. Find the cross track distance/cross track error between B and the path.

Solution example 10
Example ten

 

 

About the solutions

The four first examples are simply solved by using the functions provided for download here. The functions are exact, and can use ellipsoidal or spherical Earth model. They are without iterations (i.e. on closed form), and WGS-84 reference ellipsoid is used as default. Custom ellipsoids/spheres can be specified. The functions are based on n-vector (described below) instead of latitude and longitude.

The last six examples are solved for spherical Earth, and then no functions are needed. Using n-vector instead of latitude and longitude, these examples are quite straight forward to solve by simple vector algebra.

The solutions to all 10 examples are non-singular, and they work equally well for all global positions (i.e. they work at the Poles, close to the Poles, and across the Date Line). The solutions also have full numerical accuracy for both short and long distances (they are numerically well-conditioned).

Download

The mentioned function names (in blue) are available for download as Matlab code (as well as C#, C++, Python, and JavaScript) here. The download also includes an example file solving the 10 examples above.

Background

Performing global position calculations often involves one or more of the following concerns:

1.       Complex implementations (many, and often complex lines of program code needed)

2.       Approximations, e.g.

a.       distortion in map projections

b.      assuming spherical Earth when ellipsoidal should be used

           Errors often increase with increasing distances

3.       Equation/code not valid/accurate for all Earth positions, e.g.

a.       Latitude/longitude:

                                                               i.      Singularities at Poles

                                                             ii.      Discontinuity at International Date Line (±180° meridian)

b.      Map projections: usually valid for a limited area, e.g. UTM

4.       Iterations (need to iterate to achieve needed accuracy)


By using the solutions provided here, these concerns can be avoided for a majority of position calculations.

What can go wrong if using latitude and longitude?

Even the simple discontinuity of longitude at the International Date Line (±180° meridian) can give serious consequences if not handled correctly in all the code. An example illustrating this was the computer crash experienced by 12 fighter aircrafts (F-22 Raptors) when they crossed this meridian in 2007.

The discontinuity can be quite easily handled by adding specific code, while the singularities at the Poles are much more challenging to deal with. Examples of how the singularities can affect calculations with latitude and longitude are found in Section 6 of the reference Gade (2010).

F-22 Raptors

The discontinuity of longitude caused serious problems for the F-22 Raptors when the longitude instantly changed from -180° to +180° (when crossing the International Date Line).

n-vector

As discussed above, using latitude and longitude in calculations may lead to several problems due to the singularities at the Poles and complex behavior near the Poles, and due to the discontinuity at the Date Line (±180° meridian).

Instead of latitude and longitude, we represent position with an “n-vector”, which is the normal vector to the Earth model (the same reference ellipsoid that is used for latitude and longitude). When using n-vector, all Earth-positions are treated equally, and there is no need to worry about singularities or discontinuities. An additional benefit with using n-vector is that many position calculations can be solved with simple vector algebra (e.g. dot product and cross product).

Math equation example

The n-vector is the normal vector to the Earth model (reference ellipsoid).

 

Converting between n-vector and latitude/longitude is done with the simple equations (3) to (5) in Gade (2010). Those equations are implemented in two of the files available for download here. Going from latitude and longitude to n-vector is done with the function lat_long2n_E, while n_E2lat_long does the opposite conversion.  n_E is n-vector in the program code, while in documents we use n E MathType@MTEF@5@5@+= feaagKart1ev2aqatCvAUfeBSjuyZL2yd9gzLbvyNv2CaerbuLwBLn hiov2DGi1BTfMBaeXatLxBI9gBaerbd9wDYLwzYbItLDharqqtubsr 4rNCHbGeaGqiVu0Je9sqqrpepC0xbbL8F4rqqrFfpeea0xe9Lq=Jc9 vqaqpepm0xbba9pwe9Q8fs0=yqaqpepae9pg0FirpepeKkFr0xfr=x fr=xb9adbaqaaeGaciGaaiaabmqaamaabaabaaGcbaGaaCOBamaaCa aaleqabaGaamyraaaaaaa@37E7@ . E denotes an Earth-fixed coordinate frame, and it indicates that the three components of n-vector are along the three axes of E. More details about the notation used are found here. The position of the North Pole given as n-vector is

n E =[ 0 0 1 ] MathType@MTEF@5@5@+= feaagKart1ev2aqatCvAUfeBSjuyZL2yd9gzLbvyNv2CaerbuLwBLn hiov2DGi1BTfMBaeXatLxBI9gBaerbd9wDYLwzYbItLDharqqtubsr 4rNCHbGeaGqiVu0Je9sqqrpepC0xbbL8F4rqqrFfpeea0xe9Lq=Jc9 vqaqpepm0xbba9pwe9Q8fs0=yqaqpepae9pg0FirpepeKkFr0xfr=x fr=xb9adbaqaaeGaciGaaiaabmqaamaabaabaaGcbaGaaCOBamaaCa aaleqabaGaamyraaaakiabg2da9maadmaabaqbaeqabmqaaaqaaiaa icdaaeaacaaIWaaabaGaaGymaaaaaiaawUfacaGLDbaaaaa@3D27@

(assuming the coordinate frame E has its z-axis pointing to the North Pole). For all the details about n-vector, see the reference Gade (2010).

 

Many position calculations can be solved with only two functions

It turns out that very often, the problem to be solved is one of these:

1.       A and B => delta: Given two positions, A and B (e.g. as latitudes/longitudes/heights), find the (delta) vector from A to B in meters. See also Example 1.

2.       A and delta => B: Given position A (e.g. as lat/long/height) and a vector in meters from A to B, find position B. See also Example 2.

Two functions performing these calculations are available here, using n-vectors for position A and B as input/output. The n-vectors for position A and B are written n EA E MathType@MTEF@5@5@+= feaagKart1ev2aaatCvAUfeBSjuyZL2yd9gzLbvyNv2CaerbuLwBLn hiov2DGi1BTfMBaeXatLxBI9gBaebbnrfifHhDYfgasaacH8srps0l bbf9q8WrFfeuY=Hhbbf9v8qqaqFr0xc9pk0xbba9q8WqFfea0=yr0R Yxir=Jbba9q8aq0=yq=He9q8qqQ8frFve9Fve9Ff0dmeaabaqaciaa caGaaeWabaGabeaadaaakeaacaWHUbWaa0baaSqaaiaadweacaWGbb aabaGaamyraaaaaaa@353A@  and n EB E MathType@MTEF@5@5@+= feaagKart1ev2aaatCvAUfeBSjuyZL2yd9gzLbvyNv2CaerbuLwBLn hiov2DGi1BTfMBaeXatLxBI9gBaebbnrfifHhDYfgasaacH8srps0l bbf9q8WrFfeuY=Hhbbf9v8qqaqFr0xc9pk0xbba9q8WqFfea0=yr0R Yxir=Jbba9q8aq0=yq=He9q8qqQ8frFve9Fve9Ff0dmeaabaqaciaa caGaaeWabaGabeaadaaakeaacaWHUbWaa0baaSqaaiaadweacaWGcb aabaGaamyraaaaaaa@353B@ , while in program code we use n_EA_E and n_EB_E. The (delta) position vector from A to B is written p AB E MathType@MTEF@5@5@+= feaagKart1ev2aaatCvAUfeBSjuyZL2yd9gzLbvyNv2CaerbuLwBLn hiov2DGi1BTfMBaeXatLxBI9gBaebbnrfifHhDYfgasaacH8srps0l bbf9q8WrFfeuY=Hhbbf9v8qqaqFr0xc9pk0xbba9q8WqFfea0=yr0R Yxir=Jbba9q8aq0=yq=He9q8qqQ8frFve9Fve9Ff0dmeaabaqaciaa caGaaeWabaGabeaadaaakeaacaWHWbWaa0baaSqaaiaadgeacaWGcb aabaGaamyraaaaaaa@3539@  (p_AB_E). More info about the notation is found here.

Based on the above variable names, the two above functions are called:

1.  n_EA_E_and_n_EB_E2p_AB_E.m

2.  n_EA_E_and_p_AB_E2n_EB_E.m

 

Solving the 10 examples

We will now show how to solve the 10 examples listed above with the use of n-vector. The solutions are also available as Matlab code in the file examples.m, and as C#, Python and JavaScript code here. The color coding input (red) is used for the initial input, and the final solution is colored output (green).

Example 1: “A and B to delta

Example one

Problem 1:

Given two positions, A and B as latitudes, longitudes and depths (relative to Earth, E):

-        latEA, longEA and zEA (depth = - height)

-        latEB, longEB and zEB

Find the exact vector between the two positions, given in meters north, east, and down, and find the direction (azimuth) to B, relative to north.

Details:

·         Assume WGS-84 ellipsoid. The given depths are from the ellipsoid surface.

·         Use position A to define north, east, and down directions. (Due to the curvature of Earth and different directions to the North Pole, the north, east, and down directions will change (relative to Earth) for different places. A must be outside the poles for the north and east directions to be defined.)

 

Solution 1:

1.       First, the given latitudes and longitudes are converted to n-vectors:

n EA E =lat_long2n_ E(la t EA ,lon g EA ) n EB E = lat_long2n_ E (la t EB ,lon g EB ) MathType@MTEF@5@5@+= feaagKart1ev2aaatCvAUfeBSjuyZL2yd9gzLbvyNv2CaerbuLwBLn hiov2DGi1BTfMBaeXatLxBI9gBaebbnrfifHhDYfgasaacH8srps0l bbf9q8WrFfeuY=Hhbbf9v8qqaqFr0xc9pk0xbba9q8WqFfea0=yr0R Yxir=Jbba9q8aq0=yq=He9q8qqQ8frFve9Fve9Ff0dmeaabaqaciaa caGaaeWabaGabeaadaaakqaabeqaaiaah6gadaqhaaWcbaGaamyrai aadgeaaeaacaWGfbaaaOGaeyypa0tefm0B1jxALjhiov2DaGabbKaa abbbaaaaaG+acXwDLbWdbiaa=XgacaWFHbGaa8hDaiaa=9facaWFSb Gaa83Baiaa=5gacaWFNbGaa8Nmaiaa=5gacaWFFbGaa8xraOWdaiaa =Hcaqqa6daaaaaGuLrgapiGaamiBaiaadggacaWG0bWaaSbaaSqaai aadweacaWGbbaabeaak8aacaWFSaWdciaadYgacaWGVbGaamOBaiaa dEgadaWgaaWcbaGaamyraiaadgeaaeqaaOWdaiaaysW7caWFPaaaba GaaCOBamaaDaaaleaacaWGfbGaamOqaaqaaiaadweaaaGccqGH9aqp jaaqpeGaa8hBaiaa=fgacaWF0bGaa83xaiaa=XgacaWFVbGaa8NBai aa=DgacaWFYaGaa8NBaiaa=9facaWFfbGcpaGaa8hka8GacaWGSbGa amyyaiaadshadaWgaaWcbaGaamyraiaadkeaaeqaaOWdaiaa=Xcapi GaamiBaiaad+gacaWGUbGaam4zamaaBaaaleaacaWGfbGaamOqaaqa baGcpaGaaGjbVlaa=Lcaaaaa@75C7@

2.       When the positions are given as n-vectors (and depths), it is easy to find the delta vector decomposed in E (using the first function mentioned previously):

p AB E =n_EA_E_and _n_EB_E2p_AB_ E( n EA E , n EB E , z EA , z EB ) MathType@MTEF@5@5@+= feaagKart1ev2aaatCvAUfeBSjuyZL2yd9gzLbvyNv2CaerbuLwBLn hiov2DGi1BTfMBaeXatLxBI9gBaebbnrfifHhDYfgasaacH8srps0l bbf9q8WrFfeuY=Hhbbf9v8qqaqFr0xc9pk0xbba9q8WqFfea0=yr0R Yxir=Jbba9q8aq0=yq=He9q8qqQ8frFve9Fve9Ff0dmeaabaqaciaa caGaaeWabaGabeaadaaakeaacaWHWbWaa0baaSqaaiaadgeacaWGcb aabaGaamyraaaakiabg2da9erbd9wDYLwzYbItLDhaiqqajaaqqqaa aaaaOpGqSvxza8qacaWFUbGaa83xaiaa=veacaWFbbGaa83xaiaa=v eacaWFFbGaa8xyaiaa=5gacaWFKbGaa83xaiaa=5gacaWFFbGaa8xr aiaa=jeacaWFFbGaa8xraiaa=jdacaWFWbGaa83xaiaa=feacaWFcb Gaa83xaiaa=veak8aacaWFOaGaaCOBamaaDaaaleaacaWGfbGaamyq aaqaaiaadweaaaGccaWFSaGaaGjbVlaah6gadaqhaaWcbaGaamyrai aadkeaaeaacaWGfbaaaOGaa8hlaiaaysW7qqa6daaaaaGuLrgapiGa amOEamaaBaaaleaacaWGfbGaamyqaaqabaGcpaGaa8hlaiaaysW7pi GaamOEamaaBaaaleaacaWGfbGaamOqaaqabaGcpaGaa8xkaaaa@689F@

No ellipsoid is specified when calling the function, thus WGS-84 (default) is used.

3.       We now have the delta vector from A to B, but (as seen from the superscript) the three coordinates of the vector are along the Earth coordinate frame E, while we need the coordinates to be north, east and down. To get this, we define a North-East-Down coordinate frame called N, and then we need the rotation matrix (direction cosine matrix) R EN MathType@MTEF@5@5@+= feaagKart1ev2aaatCvAUfeBSjuyZL2yd9gzLbvyNv2CaerbuLwBLn hiov2DGi1BTfMBaeXatLxBI9gBaebbnrfifHhDYfgasaacH8srps0l bbf9q8WrFfeuY=Hhbbf9v8qqaqFr0xc9pk0xbba9q8WqFfea0=yr0R Yxir=Jbba9q8aq0=yq=He9q8qqQ8frFve9Fve9Ff0dmeaabaqaciaa caGaaeWabaGabeaadaaakeaacaWHsbWaa0baaSqaaiaadweacaWGob aabaaaaaaa@3461@  to go between E and N (as explained here). In our math library we have a simple function that calculates R EN MathType@MTEF@5@5@+= feaagKart1ev2aaatCvAUfeBSjuyZL2yd9gzLbvyNv2CaerbuLwBLn hiov2DGi1BTfMBaeXatLxBI9gBaebbnrfifHhDYfgasaacH8srps0l bbf9q8WrFfeuY=Hhbbf9v8qqaqFr0xc9pk0xbba9q8WqFfea0=yr0R Yxir=Jbba9q8aq0=yq=He9q8qqQ8frFve9Fve9Ff0dmeaabaqaciaa caGaaeWabaGabeaadaaakeaacaWHsbWaa0baaSqaaiaadweacaWGob aabaaaaaaa@3461@  from n-vector, and we use this function (using n-vector at A-position):

 

R EN =n_E2R_EN( n EA E ) MathType@MTEF@5@5@+= feaagKart1ev2aaatCvAUfeBSjuyZL2yd9gzLbvyNv2CaerbuLwBLn hiov2DGi1BTfMBaeXatLxBI9gBaebbnrfifHhDYfgasaacH8srps0l bbf9q8WrFfeuY=Hhbbf9v8qqaqFr0xc9pk0xbba9q8WqFfea0=yr0R Yxir=Jbba9q8aq0=yq=He9q8qqQ8frFve9Fve9Ff0dmeaabaqaciaa caGaaeWabaGabeaadaaakeaacaWHsbWaa0baaSqaaiaadweacaWGob aabaaaaOGaeyypa0tefm0B1jxALjhiov2DaGabbKaaabbbaaaaaG+a cXwDLbWdbiaa=5gacaWFFbGaa8xraiaa=jdacaWFsbGaa83xaiaa=v eacaWFobGcpaGaa8hkaiaah6gadaqhaaWcbaGaamyraiaadgeaaeaa caWGfbaaaOGaa8xkaaaa@481B@

 

4.       Now the delta vector is easily decomposed in N. When deciding if we should premultiply p AB E MathType@MTEF@5@5@+= feaagKart1ev2aaatCvAUfeBSjuyZL2yd9gzLbvyNv2CaerbuLwBLn hiov2DGi1BTfMBaeXatLxBI9gBaebbnrfifHhDYfgasaacH8srps0l bbf9q8WrFfeuY=Hhbbf9v8qqaqFr0xc9pk0xbba9q8WqFfea0=yr0R Yxir=Jbba9q8aq0=yq=He9q8qqQ8frFve9Fve9Ff0dmeaabaqaciaa caGaaeWabaGabeaadaaakeaacaWHWbWaa0baaSqaaiaadgeacaWGcb aabaGaamyraaaaaaa@3539@  with R EN MathType@MTEF@5@5@+= feaagKart1ev2aaatCvAUfeBSjuyZL2yd9gzLbvyNv2CaerbuLwBLn hiov2DGi1BTfMBaeXatLxBI9gBaebbnrfifHhDYfgasaacH8srps0l bbf9q8WrFfeuY=Hhbbf9v8qqaqFr0xc9pk0xbba9q8WqFfea0=yr0R Yxir=Jbba9q8aq0=yq=He9q8qqQ8frFve9Fve9Ff0dmeaabaqaciaa caGaaeWabaGabeaadaaakeaacaWHsbWaa0baaSqaaiaadweacaWGob aabaaaaaaa@3461@  or R NE MathType@MTEF@5@5@+= feaagKart1ev2aaatCvAUfeBSjuyZL2yd9gzLbvyNv2CaerbuLwBLn hiov2DGi1BTfMBaeXatLxBI9gBaebbnrfifHhDYfgasaacH8srps0l bbf9q8WrFfeuY=Hhbbf9v8qqaqFr0xc9pk0xbba9q8WqFfea0=yr0R Yxir=Jbba9q8aq0=yq=He9q8qqQ8frFve9Fve9Ff0dmeaabaqaciaa caGaaeWabaGabeaadaaakeaacaWHsbWaa0baaSqaaiaad6eacaWGfb aabaaaaaaa@3461@ , we use the "closest-rule" saying that the subscript that is closest to the vector should be the frame where the vector is decomposed (also explained here). Since the vector is decomposed in E, we must use R NE MathType@MTEF@5@5@+= feaagKart1ev2aaatCvAUfeBSjuyZL2yd9gzLbvyNv2CaerbuLwBLn hiov2DGi1BTfMBaeXatLxBI9gBaebbnrfifHhDYfgasaacH8srps0l bbf9q8WrFfeuY=Hhbbf9v8qqaqFr0xc9pk0xbba9q8WqFfea0=yr0R Yxir=Jbba9q8aq0=yq=He9q8qqQ8frFve9Fve9Ff0dmeaabaqaciaa caGaaeWabaGabeaadaaakeaacaWHsbWaa0baaSqaaiaad6eacaWGfb aabaaaaaaa@3461@  ( R NE MathType@MTEF@5@5@+= feaagKart1ev2aaatCvAUfeBSjuyZL2yd9gzLbvyNv2CaerbuLwBLn hiov2DGi1BTfMBaeXatLxBI9gBaebbnrfifHhDYfgasaacH8srps0l bbf9q8WrFfeuY=Hhbbf9v8qqaqFr0xc9pk0xbba9q8WqFfea0=yr0R Yxir=Jbba9q8aq0=yq=He9q8qqQ8frFve9Fve9Ff0dmeaabaqaciaa caGaaeWabaGabeaadaaakeaacaWHsbWaa0baaSqaaiaad6eacaWGfb aabaaaaaaa@3461@  is the transpose of R EN MathType@MTEF@5@5@+= feaagKart1ev2aaatCvAUfeBSjuyZL2yd9gzLbvyNv2CaerbuLwBLn hiov2DGi1BTfMBaeXatLxBI9gBaebbnrfifHhDYfgasaacH8srps0l bbf9q8WrFfeuY=Hhbbf9v8qqaqFr0xc9pk0xbba9q8WqFfea0=yr0R Yxir=Jbba9q8aq0=yq=He9q8qqQ8frFve9Fve9Ff0dmeaabaqaciaa caGaaeWabaGabeaadaaakeaacaWHsbWaa0baaSqaaiaadweacaWGob aabaaaaaaa@3461@  ):

 

p AB N = R NE p AB E MathType@MTEF@5@5@+= feaagKart1ev2aaatCvAUfeBSjuyZL2yd9gzLbvyNv2CaerbuLwBLn hiov2DGi1BTfMBaeXatLxBI9gBaebbnrfifHhDYfgasaacH8srps0l bbf9q8WrFfeuY=Hhbbf9v8qqaqFr0xc9pk0xbba9q8WqFfea0=yr0R Yxir=Jbba9q8aq0=yq=He9q8qqQ8frFve9Fve9Ff0dmeaabaqaciaa caGaaeWabaGabeaadaaakeaaqaaaauFaaaWdbiaahchadaqhaaWcba GaamyqaiaadkeaaeaacaWGobaaaOWdaiabg2da9iaahkfadaqhaaWc baGaamOtaiaadweaaeaaaaGccaWHWbWaa0baaSqaaiaadgeacaWGcb aabaGaamyraaaaaaa@3DA3@

 

5.       The three components of p AB N MathType@MTEF@5@5@+= feaagKart1ev2aaatCvAUfeBSjuyZL2yd9gzLbvyNv2CaerbuLwBLn hiov2DGi1BTfMBaeXatLxBI9gBaebbnrfifHhDYfgasaacH8srps0l bbf9q8WrFfeuY=Hhbbf9v8qqaqFr0xc9pk0xbba9q8WqFfea0=yr0R Yxir=Jbba9q8aq0=yq=He9q8qqQ8frFve9Fve9Ff0dmeaabaqaciaa caGaaeWabaGabeaadaaakeaaqaaaauFaaaWdbiaahchadaqhaaWcba GaamyqaiaadkeaaeaacaWGobaaaaaa@3658@  are the north, east and down displacements from A to B in meters. The azimuth is simply found from element 1 and 2 of the vector (the north and east components):

 

azimuth=atan2( p AB N (2), p AB N (1) ) MathType@MTEF@5@5@+= feaagKart1ev2aaatCvAUfeBSjuyZL2yd9gzLbvyNv2CaerbuLwBLn hiov2DGi1BTfMBaeXatLxBI9gBaebbnrfifHhDYfgasaacH8srps0l bbf9q8WrFfeuY=Hhbbf9v8qqaqFr0xc9pk0xbba9q8WqFfea0=yr0R Yxir=Jbba9q8aq0=yq=He9q8qqQ8frFve9Fve9Ff0dmeaabaqaciaa caGaaeWabaGabeaadaaakeaaqaaaauFaaaWdbiaadggacaWG6bGaam yAaiaad2gacaWG1bGaamiDaiaadIgapaGaeyypa0Jaaeyyaiaabsha caqGHbGaaeOBaiaabkdadaqadaqaaiaahchadaqhaaWcbaGaamyqai aadkeaaeaacaWGobaaaOGaaiikaiaaikdacaGGPaGaaiilaiaahcha daqhaaWcbaGaamyqaiaadkeaaeaacaWGobaaaOGaaiikaiaaigdaca GGPaaacaGLOaGaayzkaaaaaa@4C73@

 

A Matlab code version of this solution is found in examples.m, while solutions written in C#, Python and JavaScript are available here.


Example 2: “B and delta to C

Example Two

Problem 2:

A radar or sonar attached to a vehicle B (Body coordinate frame) measures the distance and direction to an object C. We assume that the distance and two angles (typically bearing and elevation relative to B) are already combined to the vector p BC B MathType@MTEF@5@5@+= feaagKart1ev2aaatCvAUfeBSjuyZL2yd9gzLbvyNv2CaerbuLwBLn hiov2DGi1BTfMBaeXatLxBI9gBaebbnrfifHhDYfgasaacH8srps0l bbf9q8WrFfeuY=Hhbbf9v8qqaqFr0xc9pk0xbba9q8WqFfea0=yr0R Yxir=Jbba9q8aq0=yq=He9q8qqQ8frFve9Fve9Ff0dmeaabaqaciaa caGaaeWabaGabeaadaaakeaaqqa6daaaaaGuLrgapeGaaCiCamaaDa aaleaacaWGcbGaam4qaaqaaiaadkeaaaaaaa@3762@  (i.e. the vector from B to C, decomposed in B). The position of B is given as n EB E MathType@MTEF@5@5@+= feaagKart1ev2aaatCvAUfeBSjuyZL2yd9gzLbvyNv2CaerbuLwBLn hiov2DGi1BTfMBaeXatLxBI9gBaebbnrfifHhDYfgasaacH8srps0l bbf9q8WrFfeuY=Hhbbf9v8qqaqFr0xc9pk0xbba9q8WqFfea0=yr0R Yxir=Jbba9q8aq0=yq=He9q8qqQ8frFve9Fve9Ff0dmeaabaqaciaa caGaaeWabaGabeaadaaakeaaqqa6daaaaaGuLrgapeGaaCOBamaaDa aaleaacaWGfbGaamOqaaqaaiaadweaaaaaaa@3765@  and z EB MathType@MTEF@5@5@+= feaagKart1ev2aaatCvAUfeBSjuyZL2yd9gzLbvyNv2CaerbuLwBLn hiov2DGi1BTfMBaeXatLxBI9gBaebbnrfifHhDYfgasaacH8srps0l bbf9q8WrFfeuY=Hhbbf9v8qqaqFr0xc9pk0xbba9q8WqFfea0=yr0R Yxir=Jbba9q8aq0=yq=He9q8qqQ8frFve9Fve9Ff0dmeaabaqaciaa caGaaeWabaGabeaadaaakeaaqqa6daaaaaGuLrgapeGaamOEamaaDa aaleaacaWGfbGaamOqaaqaaaaaaaa@36A3@ , and the orientation (attitude) of B is given as R NB MathType@MTEF@5@5@+= feaagKart1ev2aaatCvAUfeBSjuyZL2yd9gzLbvyNv2CaerbuLwBLn hiov2DGi1BTfMBaeXatLxBI9gBaebbnrfifHhDYfgasaacH8srps0l bbf9q8WrFfeuY=Hhbbf9v8qqaqFr0xc9pk0xbba9q8WqFfea0=yr0R Yxir=Jbba9q8aq0=yq=He9q8qqQ8frFve9Fve9Ff0dmeaabaqaciaa caGaaeWabaGabeaadaaakeaaqqa6daaaaaGuLrgapeGaaCOuamaaDa aaleaacaWGobGaamOqaaqaaaaaaaa@3688@  (this rotation matrix can be found from roll/pitch/yaw by using zyx2R.m).

Find the exact position of object C as n-vector and depth ( n EC E MathType@MTEF@5@5@+= feaagKart1ev2aaatCvAUfeBSjuyZL2yd9gzLbvyNv2CaerbuLwBLn hiov2DGi1BTfMBaeXatLxBI9gBaebbnrfifHhDYfgasaacH8srps0l bbf9q8WrFfeuY=Hhbbf9v8qqaqFr0xc9pk0xbba9q8WqFfea0=yr0R Yxir=Jbba9q8aq0=yq=He9q8qqQ8frFve9Fve9Ff0dmeaabaqaciaa caGaaeWabaGabeaadaaakeaaqaaaauFaaaWdbiaah6gadaqhaaWcba GaamyraiaadoeaaeaacaWGfbaaaaaa@3652@  and z EC MathType@MTEF@5@5@+= feaagKart1ev2aaatCvAUfeBSjuyZL2yd9gzLbvyNv2CaerbuLwBLn hiov2DGi1BTfMBaeXatLxBI9gBaebbnrfifHhDYfgasaacH8srps0l bbf9q8WrFfeuY=Hhbbf9v8qqaqFr0xc9pk0xbba9q8WqFfea0=yr0R Yxir=Jbba9q8aq0=yq=He9q8qqQ8frFve9Fve9Ff0dmeaabaqaciaa caGaaeWabaGabeaadaaakeaaqaaaauFaaaWdbiaadQhadaqhaaWcba Gaamyraiaadoeaaeaaaaaaaa@3590@  ), assuming Earth ellipsoid with semi-major axis a and flattening f. For WGS-72, use a = 6 378 135 m and f = 1/298.26.

Solution 2:

1.       The delta vector is given in B. It should be decomposed in E before using it, and thus we need R EB MathType@MTEF@5@5@+= feaagKart1ev2aaatCvAUfeBSjuyZL2yd9gzLbvyNv2CaerbuLwBLn hiov2DGi1BTfMBaeXatLxBI9gBaebbnrfifHhDYfgasaacH8srps0l bbf9q8WrFfeuY=Hhbbf9v8qqaqFr0xc9pk0xbba9q8WqFfea0=yr0R Yxir=Jbba9q8aq0=yq=He9q8qqQ8frFve9Fve9Ff0dmeaabaqaciaa caGaaeWabaGabeaadaaakeaacaWHsbWaa0baaSqaaiaadweacaWGcb aabaaaaaaa@3455@ . This matrix is found from the matrixes R EN MathType@MTEF@5@5@+= feaagKart1ev2aaatCvAUfeBSjuyZL2yd9gzLbvyNv2CaerbuLwBLn hiov2DGi1BTfMBaeXatLxBI9gBaebbnrfifHhDYfgasaacH8srps0l bbf9q8WrFfeuY=Hhbbf9v8qqaqFr0xc9pk0xbba9q8WqFfea0=yr0R Yxir=Jbba9q8aq0=yq=He9q8qqQ8frFve9Fve9Ff0dmeaabaqaciaa caGaaeWabaGabeaadaaakeaacaWHsbWaa0baaSqaaiaadweacaWGob aabaaaaaaa@3461@  and R NB MathType@MTEF@5@5@+= feaagKart1ev2aaatCvAUfeBSjuyZL2yd9gzLbvyNv2CaerbuLwBLn hiov2DGi1BTfMBaeXatLxBI9gBaebbnrfifHhDYfgasaacH8srps0l bbf9q8WrFfeuY=Hhbbf9v8qqaqFr0xc9pk0xbba9q8WqFfea0=yr0R Yxir=Jbba9q8aq0=yq=He9q8qqQ8frFve9Fve9Ff0dmeaabaqaciaa caGaaeWabaGabeaadaaakeaaqqa6daaaaaGuLrgapeGaaCOuamaaDa aaleaacaWGobGaamOqaaqaaaaaaaa@3688@ , and we need to find R EN MathType@MTEF@5@5@+= feaagKart1ev2aaatCvAUfeBSjuyZL2yd9gzLbvyNv2CaerbuLwBLn hiov2DGi1BTfMBaeXatLxBI9gBaebbnrfifHhDYfgasaacH8srps0l bbf9q8WrFfeuY=Hhbbf9v8qqaqFr0xc9pk0xbba9q8WqFfea0=yr0R Yxir=Jbba9q8aq0=yq=He9q8qqQ8frFve9Fve9Ff0dmeaabaqaciaa caGaaeWabaGabeaadaaakeaacaWHsbWaa0baaSqaaiaadweacaWGob aabaaaaaaa@3461@ , as in Example 1:

R EN =n_E2R_EN( n EB E ) MathType@MTEF@5@5@+= feaagKart1ev2aaatCvAUfeBSjuyZL2yd9gzLbvyNv2CaerbuLwBLn hiov2DGi1BTfMBaeXatLxBI9gBaebbnrfifHhDYfgasaacH8srps0l bbf9q8WrFfeuY=Hhbbf9v8qqaqFr0xc9pk0xbba9q8WqFfea0=yr0R Yxir=Jbba9q8aq0=yq=He9q8qqQ8frFve9Fve9Ff0dmeaabaqaciaa caGaaeWabaGabeaadaaakeaacaWHsbWaa0baaSqaaiaadweacaWGob aabaaaaOGaeyypa0tefm0B1jxALjhiov2DaGabbKaaabbbaaaaaG+a cXwDLbWdbiaa=5gacaWFFbGaa8xraiaa=jdacaWFsbGaa83xaiaa=v eacaWFobGcpaGaa8hkaabbOpaaaaaasvgza8GacaWHUbWaa0baaSqa aiaadweacaWGcbaabaGaamyraaaak8aacaWFPaaaaa@4A56@

2.       Now, we can find R EB MathType@MTEF@5@5@+= feaagKart1ev2aaatCvAUfeBSjuyZL2yd9gzLbvyNv2CaerbuLwBLn hiov2DGi1BTfMBaeXatLxBI9gBaebbnrfifHhDYfgasaacH8srps0l bbf9q8WrFfeuY=Hhbbf9v8qqaqFr0xc9pk0xbba9q8WqFfea0=yr0R Yxir=Jbba9q8aq0=yq=He9q8qqQ8frFve9Fve9Ff0dmeaabaqaciaa caGaaeWabaGabeaadaaakeaacaWHsbWaa0baaSqaaiaadweacaWGcb aabaaaaaaa@3455@  by using that the closest frames cancel when multiplying two rotation matrixes (i.e. N is cancelled here):

R EB = R EN R NB MathType@MTEF@5@5@+= feaagKart1ev2aaatCvAUfeBSjuyZL2yd9gzLbvyNv2CaerbuLwBLn hiov2DGi1BTfMBaeXatLxBI9gBaebbnrfifHhDYfgasaacH8srps0l bbf9q8WrFfeuY=Hhbbf9v8qqaqFr0xc9pk0xbba9q8WqFfea0=yr0R Yxir=Jbba9q8aq0=yq=He9q8qqQ8frFve9Fve9Ff0dmeaabaqaciaa caGaaeWabaGabeaadaaakeaacaWHsbWaa0baaSqaaiaadweacaWGcb aabaaaaOGaeyypa0JaaCOuamaaDaaaleaacaWGfbGaamOtaaqaaaaa kabbOpaaaaaasvgza8qacaWHsbWaa0baaSqaaiaad6eacaWGcbaaba aaaaaa@3CE0@

3.       The delta vector is now decomposed in E (details about decomposing are found here):

p BC E = R EB p BC B MathType@MTEF@5@5@+= feaagKart1ev2aaatCvAUfeBSjuyZL2yd9gzLbvyNv2CaerbuLwBLn hiov2DGi1BTfMBaeXatLxBI9gBaebbnrfifHhDYfgasaacH8srps0l bbf9q8WrFfeuY=Hhbbf9v8qqaqFr0xc9pk0xbba9q8WqFfea0=yr0R Yxir=Jbba9q8aq0=yq=He9q8qqQ8frFve9Fve9Ff0dmeaabaqaciaa caGaaeWabaGabeaadaaakeaacaWHWbWaa0baaSqaaiaadkeacaWGdb aabaGaamyraaaakiabg2da9iaahkfadaqhaaWcbaGaamyraiaadkea aeaaaaGcqqa6daaaaaGuLrgapeGaaCiCamaaDaaaleaacaWGcbGaam 4qaaqaaiaadkeaaaaaaa@3E94@

4.       It is now easy to find the position of C using the second function mentioned previously (with custom ellipsoid overriding the default WGS-84).

[ n EC E , z EC ]=n_EA_E_and_p_AB_E2n_EB_E( n EB E , p BC E , z EB ,a,f) MathType@MTEF@5@5@+= feaagKart1ev2aaatCvAUfeBSjuyZL2yd9gzLbvyNv2CaerbuLwBLn hiov2DGi1BTfMBaeXatLxBI9gBaebbnrfifHhDYfgasaacH8srps0l bbf9q8WrFfeuY=Hhbbf9v8qqaqFr0xc9pk0xbba9q8WqFfea0=yr0R Yxir=Jbba9q8aq0=yq=He9q8qqQ8frFve9Fve9Ff0dmeaabaqaciaa caGaaeWabaGabeaadaaakeaacaGGBbaeaaaa1haaa8qacaWHUbWaa0 baaSqaaiaadweacaWGdbaabaGaamyraaaak8aacaGGSaWdbiaadQha daWgaaWcbaGaamyraiaadoeaaeqaaOWdaiaac2facqGH9aqpruWqVv NCPvMCG4uz3baceeqcaaKaa8NBaiaa=9facaWFfbGaa8xqaiaa=9fa caWFfbGaa83xaiaa=fgacaWFUbGaa8hzaiaa=9facaWFWbGaa83xai aa=feacaWFcbGaa83xaiaa=veacaWFYaGaa8NBaiaa=9facaWFfbGa a8Nqaiaa=9facaWFfbGccaWFOaaeeG+aaaaaaivzKbWdciaah6gada qhaaWcbaGaamyraiaadkeaaeaacaWGfbaaaOaeeaaaaaa6dieB1vga pmGaaGjcVlaayIW7paGaa8hlaiaaysW7caWHWbWaa0baaSqaaiaadk eacaWGdbaabaGaamyraaaakiaayIW7caaMi8Uaa8hlaiaaysW7piGa amOEamaaBaaaleaacaWGfbGaamOqaaqabaGcpmGaaGPaV=aacaWFSa WdciaadggapaGaa8hlaiaaykW7piGaamOza8aacaWFPaaaaa@76E9@

A Matlab code version of this solution is found in examples.m, while solutions written in C#, Python and JavaScript are available here.

Details: For simplicity, the orientation of the vehicle in this example was given relative to N. If the navigation system of the vehicle was using the non-singular coordinate frame L instead, the solution would be the same, but replacing the function n_E2R_EN.m with n_E_and_wa2R_EL.m . See Table 2 in Gade (2010) and the help text in n_E_and_wa2R_EL.m for details.


Example 3: “ECEF-vector to geodetic latitude”


Example Three

Problem 3:

Position B is given as an “ECEF-vector” p EB E MathType@MTEF@5@5@+= feaagKart1ev2aaatCvAUfeBSjuyZL2yd9gzLbvyNv2CaerbuLwBLn hiov2DGi1BTfMBaeXatLxBI9gBaebbnrfifHhDYfgasaacH8srps0l bbf9q8WrFfeuY=Hhbbf9v8qqaqFr0xc9pk0xbba9q8WqFfea0=yr0R Yxir=Jbba9q8aq0=yq=He9q8qqQ8frFve9Fve9Ff0dmeaabaqaciaa caGaaeWabaGabeaadaaakeaaqqa6daaaaaGuLrgapeGaaCiCamaaDa aaleaacaWGfbGaamOqaaqaaiaadweaaaaaaa@3767@  (i.e. a vector from E, the center of the Earth, to B, decomposed in E).

Find the geodetic latitude, longitude and height, latEB, longEB and hEB, assuming WGS-84 ellipsoid.

Solution 3:

1.       We have a function that converts p-vector to n-vector, see Appendix B in Gade (2010):

[ n EB E , z EB ]=p_EB_E2n_EB_E( p EB E ) MathType@MTEF@5@5@+= feaagKart1ev2aaatCvAUfeBSjuyZL2yd9gzLbvyNv2CaerbuLwBLn hiov2DGi1BTfMBaeXatLxBI9gBaebbnrfifHhDYfgasaacH8srps0l bbf9q8WrFfeuY=Hhbbf9v8qqaqFr0xc9pk0xbba9q8WqFfea0=yr0R Yxir=Jbba9q8aq0=yq=He9q8qqQ8frFve9Fve9Ff0dmeaabaqaciaa caGaaeWabaGabeaadaaakeaadaWadaqaaiaah6gadaqhaaWcbaGaam yraiaadkeaaeaacaWGfbaaaerbd9wDYLwzYbItLDhaiqqakiaa=Xca caaMe8UaamOEamaaBaaaleaacaWGfbGaamOqaaqabaaakiaawUfaca GLDbaacqGH9aqpjaaqqqaaaaaaOpGqSvxza8qacaWFWbGaa83xaiaa =veacaWFcbGaa83xaiaa=veacaWFYaGaa8NBaiaa=9facaWFfbGaa8 Nqaiaa=9facaWFfbGcpaGaa8hkaabbOpaaaaaasvgza8GacaWHWbWa a0baaSqaaiaadweacaWGcbaabaGaamyraaaak8aacaWFPaaaaa@5645@

2.       Find latitude, longitude and height from n EB E MathType@MTEF@5@5@+= feaagKart1ev2aaatCvAUfeBSjuyZL2yd9gzLbvyNv2CaerbuLwBLn hiov2DGi1BTfMBaeXatLxBI9gBaebbnrfifHhDYfgasaacH8srps0l bbf9q8WrFfeuY=Hhbbf9v8qqaqFr0xc9pk0xbba9q8WqFfea0=yr0R Yxir=Jbba9q8aq0=yq=He9q8qqQ8frFve9Fve9Ff0dmeaabaqaciaa caGaaeWabaGabeaadaaakeaacaWHUbWaa0baaSqaaiaadweacaWGcb aabaGaamyraaaaaaa@353B@  and zEB.

[ la t EB ,lon g EB ]= n_E2lat_long( n EB E ) MathType@MTEF@5@5@+= feaagKart1ev2aaatCvAUfeBSjuyZL2yd9gzLbvyNv2CaerbuLwBLn hiov2DGi1BTfMBaeXatLxBI9gBaebbnrfifHhDYfgasaacH8srps0l bbf9q8WrFfeuY=Hhbbf9v8qqaqFr0xc9pk0xbba9q8WqFfea0=yr0R Yxir=Jbba9q8aq0=yq=He9q8qqQ8frFve9Fve9Ff0dmeaabaqaciaa caGaaeWabaGabeaadaaakeaadaWadaqaaabaaaq9baaapeGaamiBai aadggacaWG0bWaaSbaaSqaaiaadweacaWGcbaabeaaruWqVvNCPvMC G4uz3baceeGcpaGaa8hla8qacaWGSbGaam4Baiaad6gacaWGNbWaaS baaSqaaiaadweacaWGcbaabeaaaOWdaiaawUfacaGLDbaacqGH9aqp jaaqqqaaaaaaOpGqSvxza8GacaWFUbGaa83xaiaa=veacaWFYaGaa8 hBaiaa=fgacaWF0bGaa83xaiaa=XgacaWFVbGaa8NBaiaa=Dgak8aa caWFOaGaaCOBamaaDaaaleaacaWGfbGaamOqaaqaaiaadweaaaGcca WFPaaaaa@5783@

hEB MathType@MTEF@5@5@+= feaagKart1ev2aqatCvAUfeBSjuyZL2yd9gzLbvyNv2CaerbuLwBLn hiov2DGi1BTfMBaeXatLxBI9gBaerbd9wDYLwzYbItLDharqqtubsr 4rNCHbGeaGqiVu0Je9sqqrpepC0xbbL8F4rqqrFfpeea0xe9Lq=Jc9 vqaqpepm0xbba9pwe9Q8fs0=yqaqpepae9pg0FirpepeKkFr0xfr=x fr=xb9adbaqaaeGaciGaaiaabeqaamaabaabaaGcbaqefmuySLMyYL gaiuaajugybabaaaaaaaaapeGaa83eGaaa@3A73@  zEB

A Matlab code version of this solution is found in examples.m, while solutions written in C#, Python and JavaScript are available here.


Example 4: “Geodetic latitude to ECEF-vector”


Example four

Problem 4:

Geodetic latitude, longitude and height are given for position B as latEB, longEB and hEB, find the ECEF-vector for this position, p EB E MathType@MTEF@5@5@+= feaagKart1ev2aaatCvAUfeBSjuyZL2yd9gzLbvyNv2CaerbuLwBLn hiov2DGi1BTfMBaeXatLxBI9gBaebbnrfifHhDYfgasaacH8srps0l bbf9q8WrFfeuY=Hhbbf9v8qqaqFr0xc9pk0xbba9q8WqFfea0=yr0R Yxir=Jbba9q8aq0=yq=He9q8qqQ8frFve9Fve9Ff0dmeaabaqaciaa caGaaeWabaGabeaadaaakeaaqaaaauFaaaWdbiaahchadaqhaaWcba GaamyraiaadkeaaeaacaWGfbaaaaaa@3653@ .

Solution 4:

1.       First, the given latitude and longitude are converted to n-vector:

n EB E = lat_long2n_E( la t EB ,lon g EB ) MathType@MTEF@5@5@+= feaagKart1ev2aaatCvAUfeBSjuyZL2yd9gzLbvyNv2CaerbuLwBLn hiov2DGi1BTfMBaeXatLxBI9gBaebbnrfifHhDYfgasaacH8srps0l bbf9q8WrFfeuY=Hhbbf9v8qqaqFr0xc9pk0xbba9q8WqFfea0=yr0R Yxir=Jbba9q8aq0=yq=He9q8qqQ8frFve9Fve9Ff0dmeaabaqaciaa caGaaeWabaGabeaadaaakeaacaWHUbWaa0baaSqaaiaadweacaWGcb aabaGaamyraaaakiabg2da9erbd9wDYLwzYbItLDhaiqqajaaqqqaa aaaaOpGqSvxza8qacaWFSbGaa8xyaiaa=rhacaWFFbGaa8hBaiaa=9 gacaWFUbGaa83zaiaa=jdacaWFUbGaa83xaiaa=veak8aacaWFOaae eG+aaaaaaivzKbWdciaadYgacaWGHbGaamiDamaaBaaaleaacaWGfb GaamOqaaqabaGcpaGaa8hla8GacaWGSbGaam4Baiaad6gacaWGNbWa aSbaaSqaaiaadweacaWGcbaabeaak8aacaaMe8Uaa8xkaaaa@5833@

2.       We want p EB E MathType@MTEF@5@5@+= feaagKart1ev2aaatCvAUfeBSjuyZL2yd9gzLbvyNv2CaerbuLwBLn hiov2DGi1BTfMBaeXatLxBI9gBaebbnrfifHhDYfgasaacH8srps0l bbf9q8WrFfeuY=Hhbbf9v8qqaqFr0xc9pk0xbba9q8WqFfea0=yr0R Yxir=Jbba9q8aq0=yq=He9q8qqQ8frFve9Fve9Ff0dmeaabaqaciaa caGaaeWabaGabeaadaaakeaacaWHWbWaa0baaSqaaiaadweacaWGcb aabaGaamyraaaaaaa@353D@  from n EB E MathType@MTEF@5@5@+= feaagKart1ev2aaatCvAUfeBSjuyZL2yd9gzLbvyNv2CaerbuLwBLn hiov2DGi1BTfMBaeXatLxBI9gBaebbnrfifHhDYfgasaacH8srps0l bbf9q8WrFfeuY=Hhbbf9v8qqaqFr0xc9pk0xbba9q8WqFfea0=yr0R Yxir=Jbba9q8aq0=yq=He9q8qqQ8frFve9Fve9Ff0dmeaabaqaciaa caGaaeWabaGabeaadaaakeaacaWHUbWaa0baaSqaaiaadweacaWGcb aabaGaamyraaaaaaa@353B@ , and have functions that convert between these two. Now we need n_EB_E2p_EB_E.m . See also Appendix B in Gade (2010).

p EB E = n_EB_E2p_EB_E( n EB E , h EB ) MathType@MTEF@5@5@+= feaagKart1ev2aaatCvAUfeBSjuyZL2yd9gzLbvyNv2CaerbuLwBLn hiov2DGi1BTfMBaeXatLxBI9gBaebbnrfifHhDYfgasaacH8srps0l bbf9q8WrFfeuY=Hhbbf9v8qqaqFr0xc9pk0xbba9q8WqFfea0=yr0R Yxir=Jbba9q8aq0=yq=He9q8qqQ8frFve9Fve9Ff0dmeaabaqaciaa caGaaeWabaGabeaadaaakeaaqaaaauFaaaWdbiaahchadaqhaaWcba GaamyraiaadkeaaeaacaWGfbaaaOWdaiabg2da9erbd9wDYLwzYbIt LDhaiqqajaaqqqaaaaaaOpGqSvxza8GacaWFUbGaa83xaiaa=veaca WFcbGaa83xaiaa=veacaWFYaGaa8hCaiaa=9facaWFfbGaa8Nqaiaa =9facaWFfbGcpaGaa8hkaiaah6gadaqhaaWcbaGaamyraiaadkeaae aacaWGfbaaaOGaa8hlaiaaysW7cqGHsislqqa6daaaaaGuLrgapmGa amiAamaaBaaaleaacaWGfbGaamOqaaqabaGcpaGaa8xkaaaa@5655@

A Matlab code version of this solution is found in examples.m, while solutions written in C#, Python and JavaScript are available here.


Example 5: Surface distance

Example five

In the remaining examples, we assume that a spherical Earth model is sufficiently accurate, and then the position functions are not needed. The examples are included to demonstrate the simplicity of these calculations when using n-vector instead of latitude and longitude. The n-vector solutions are also non-singular for all Earth-positions and work across the Date Line.

Problem 5:

Given two positions A and B as n EA E MathType@MTEF@5@5@+= feaagKart1ev2aaatCvAUfeBSjuyZL2yd9gzLbvyNv2CaerbuLwBLn hiov2DGi1BTfMBaeXatLxBI9gBaebbnrfifHhDYfgasaacH8srps0l bbf9q8WrFfeuY=Hhbbf9v8qqaqFr0xc9pk0xbba9q8WqFfea0=yr0R Yxir=Jbba9q8aq0=yq=He9q8qqQ8frFve9Fve9Ff0dmeaabaqaciaa caGaaeWabaGabeaadaaakeaaqqa6daaaaaGuLrgapeGaaCOBamaaDa aaleaacaWGfbGaamyqaaqaaiaadweaaaaaaa@3764@ , n EB E MathType@MTEF@5@5@+= feaagKart1ev2aaatCvAUfeBSjuyZL2yd9gzLbvyNv2CaerbuLwBLn hiov2DGi1BTfMBaeXatLxBI9gBaebbnrfifHhDYfgasaacH8srps0l bbf9q8WrFfeuY=Hhbbf9v8qqaqFr0xc9pk0xbba9q8WqFfea0=yr0R Yxir=Jbba9q8aq0=yq=He9q8qqQ8frFve9Fve9Ff0dmeaabaqaciaa caGaaeWabaGabeaadaaakeaaqqa6daaaaaGuLrgapeGaaCOBamaaDa aaleaacaWGfbGaamOqaaqaaiaadweaaaaaaa@3765@ .

Find the surface distance sAB (i.e. great circle distance). The heights of A and B are not relevant (i.e. if they don’t have zero height, we seek the distance between the points that are at the surface of the Earth, directly above/below A and B). The Euclidean distance (chord length) dAB should also be found (for dAB, nonzero heights can be used). Use Earth radius rEarth.

Solution 5:

The surface distance can be found in three different ways; by utilizing the dot product of the vectors, the cross product, or a combination:

  s AB =acos( n EA E n EB E ) r Earth =asin( | n EA E × n EB E | ) r Earth =atan2( | n EA E × n EB E |, n EA E n EB E ) r Earth MathType@MTEF@5@5@+= feaagKart1ev2aaatCvAUfeBSjuyZL2yd9gzLbvyNv2CaerbuLwBLn hiov2DGi1BTfMBaeXatLxBI9gBaebbnrfifHhDYfgasaacH8srps0l bbf9q8WrFfeuY=Hhbbf9v8qqaqFr0xc9pk0xbba9q8WqFfea0=yr0R Yxir=Jbba9q8aq0=yq=He9q8qqQ8frFve9Fve9Ff0dmeaabaqaciaa caGaaeWabaGabeaadaaakeaaqaaaauFaaaWdbiaadohadaqhaaWcba GaamyqaiaadkeaaeaaaaGcpaGaeyypa0tefm0B1jxALjhiov2DaGab bKaaabbbaaaaaG+acXwDLbWdciaa=fgacaWFJbGaa83Baiaa=nhak8 aadaqadaqaaabbOpaaaaaasvgza8WacaWHUbWaa0baaSqaaiaadwea caWGbbaabaGaamyraaaak8aacqGHflY1pmGaaCOBamaaDaaaleaaca WGfbGaamOqaaqaaiaadweaaaaak8aacaGLOaGaayzkaaGaeyyXIC9d diaadkhadaWgaaWcbaGaamyraiaadggacaWGYbGaamiDaiaadIgaae qaaOWdaiabg2da9Kaaa9GacaWFHbGaa83Caiaa=LgacaWFUbGcpaWa aeWaaeaadaabdaqaa8WacaWHUbWaa0baaSqaaiaadweacaWGbbaaba Gaamyraaaak8aacqGHxdaTpmGaaCOBamaaDaaaleaacaWGfbGaamOq aaqaaiaadweaaaaak8aacaGLhWUaayjcSdaacaGLOaGaayzkaaGaey yXIC9ddiaadkhadaWgaaWcbaGaamyraiaadggacaWGYbGaamiDaiaa dIgaaeqaaOWdaiabg2da9Kaaa9GacaWFHbGaa8hDaiaa=fgacaWFUb Gaa8NmaOWdamaabmaabaWaaqWaaeaapmGaaCOBamaaDaaaleaacaWG fbGaamyqaaqaaiaadweaaaGcpaGaey41aq7ddiaah6gadaqhaaWcba GaamyraiaadkeaaeaacaWGfbaaaaGcpaGaay5bSlaawIa7aiaacYca pmGaaCOBamaaDaaaleaacaWGfbGaamyqaaqaaiaadweaaaGcpaGaey yXIC9ddiaah6gadaqhaaWcbaGaamyraiaadkeaaeaacaWGfbaaaaGc paGaayjkaiaawMcaaiabgwSix=WacaWGYbWaaSbaaSqaaiaadweaca WGHbGaamOCaiaadshacaWGObaabeaaaaa@9896@

The atan2 expression is numerically well-conditioned for all distances, see also equation (16) in Gade (2010).

The Euclidean distance is found by

d AB =| n EA E n EB E | r Earth MathType@MTEF@5@5@+= feaagKart1ev2aaatCvAUfeBSjuyZL2yd9gzLbvyNv2CaerbuLwBLn hiov2DGi1BTfMBaeXatLxBI9gBaebbnrfifHhDYfgasaacH8srps0l bbf9q8WrFfeuY=Hhbbf9v8qqaqFr0xc9pk0xbba9q8WqFfea0=yr0R Yxir=Jbba9q8aq0=yq=He9q8qqQ8frFve9Fve9Ff0dmeaabaqaciaa caGaaeWabaGabeaadaaakeaaqaaaauFaaaWdbiaadsgadaqhaaWcba GaamyqaiaadkeaaeaaaaGcpaGaeyypa0ZaaqWaaeaaqqa6daaaaaGu LrgapiGaaCOBamaaDaaaleaacaWGfbGaamyqaaqaaiaadweaaaGcpa GaeyOeI0Ydciaah6gadaqhaaWcbaGaamyraiaadkeaaeaacaWGfbaa aaGcpaGaay5bSlaawIa7aiabgwSix=GacaWGYbWaaSbaaSqaaiaadw eacaWGHbGaamOCaiaadshacaWGObaabeaaaaa@4C19@

If nonzero heights for A and B are wanted for the Euclidean distance, each n-vector should be multiplied with rEarth + hi, where hi is the corresponding height of each n-vector. If ellipsoidal Earth model is wanted, use n_EA_E_and_n_EB_E2p_AB_E.m from Example 1, and take the norm of the output vector.

A Matlab code version of this solution is found in examples.m, while solutions written in C#, Python and JavaScript are available here.


Example 6: Interpolated position

Example six

Problem 6:

Given the position of B at time t0 and t1, n EB E ( t 0 ) MathType@MTEF@5@5@+= feaagKart1ev2aaatCvAUfeBSjuyZL2yd9gzLbvyNv2CaerbuLwBLn hiov2DGi1BTfMBaeXatLxBI9gBaebbnrfifHhDYfgasaacH8srps0l bbf9q8WrFfeuY=Hhbbf9v8qqaqFr0xc9pk0xbba9q8WqFfea0=yr0R Yxir=Jbba9q8aq0=yq=He9q8qqQ8frFve9Fve9Ff0dmeaabaqaciaa caGaaeWabaGabeaadaaakeaaqqa6daaaaaGuLrgapeGaaCOBamaaDa aaleaacaWGfbGaamOqaaqaaiaadweaaaGccaGGOaGaamiDamaaBaaa leaacaaIWaaabeaakiaacMcaaaa@3AB1@  and n EB E ( t 1 ) MathType@MTEF@5@5@+= feaagKart1ev2aaatCvAUfeBSjuyZL2yd9gzLbvyNv2CaerbuLwBLn hiov2DGi1BTfMBaeXatLxBI9gBaebbnrfifHhDYfgasaacH8srps0l bbf9q8WrFfeuY=Hhbbf9v8qqaqFr0xc9pk0xbba9q8WqFfea0=yr0R Yxir=Jbba9q8aq0=yq=He9q8qqQ8frFve9Fve9Ff0dmeaabaqaciaa caGaaeWabaGabeaadaaakeaaqqa6daaaaaGuLrgapeGaaCOBamaaDa aaleaacaWGfbGaamOqaaqaaiaadweaaaGccaGGOaGaamiDamaaBaaa leaacaaIXaaabeaakiaacMcaaaa@3AB2@ .

Find an interpolated position at time ti, n EB E ( t i ) MathType@MTEF@5@5@+= feaagKart1ev2aaatCvAUfeBSjuyZL2yd9gzLbvyNv2CaerbuLwBLn hiov2DGi1BTfMBaeXatLxBI9gBaebbnrfifHhDYfgasaacH8srps0l bbf9q8WrFfeuY=Hhbbf9v8qqaqFr0xc9pk0xbba9q8WqFfea0=yr0R Yxir=Jbba9q8aq0=yq=He9q8qqQ8frFve9Fve9Ff0dmeaabaqaciaa caGaaeWabaGabeaadaaakeaaqaaaauFaaaWdbiaah6gadaqhaaWcba GaamyraiaadkeaaeaacaWGfbaaaOGaaiikaiaadshadaWgaaWcbaGa amyAaaqabaGccaGGPaaaaa@39D1@ . All positions are given as n-vectors.

Solution 6:

Standard interpolation can be used directly with n-vector:

n EB E ( t i )=unit( n EB E ( t 0 )+( t i t 0 ) n EB E ( t 1 ) n EB E ( t 0 ) t 1 t 0 ) MathType@MTEF@5@5@+= feaagKart1ev2aaatCvAUfeBSjuyZL2yd9gzLbvyNv2CaerbuLwBLn hiov2DGi1BTfMBaeXatLxBI9gBaebbnrfifHhDYfgasaacH8srps0l bbf9q8WrFfeuY=Hhbbf9v8qqaqFr0xc9pk0xbba9q8WqFfea0=yr0R Yxir=Jbba9q8aq0=yq=He9q8qqQ8frFve9Fve9Ff0dmeaabaqaciaa caGaaeWabaGabeaadaaakeGabaaLrabaaaq9baaapeGaaCOBamaaDa aaleaacaWGfbGaamOqaaqaaiaadweaaaGccaGGOaGaamiDamaaBaaa leaacaWGPbaabeaakiaacMcapaGaeyypa0tefm0B1jxALjhiov2DaG abbKaaabbbaaaaaG+acXwDLbWdciaa=vhacaWFUbGaa8xAaiaa=rha k8aadaqadaqaaabbOpaaaaaasvgza8WacaWHUbWaa0baaSqaaiaadw eacaWGcbaabaGaamyraaaakiaacIcacaWG0bWaaSbaaSqaaiaaicda aeqaaOGaaiyka8aacqGHRaWkdaqadaqaa8WacaWG0bWaaSbaaSqaai aadMgaaeqaaOWdaiabgkHiT8WacaWG0bWaaSbaaSqaaiaaicdaaeqa aaGcpaGaayjkaiaawMcaamaalaaabaWddiaah6gadaqhaaWcbaGaam yraiaadkeaaeaacaWGfbaaaOGaaiikaiaadshadaWgaaWcbaGaaGym aaqabaGccaGGPaWdaiabgkHiT8WacaWHUbWaa0baaSqaaiaadweaca WGcbaabaGaamyraaaakiaacIcacaWG0bWaaSbaaSqaaiaaicdaaeqa aOGaaiykaaWdaeaapmGaamiDamaaBaaaleaacaaIXaaabeaak8aacq GHsislpmGaamiDamaaBaaaleaacaaIWaaabeaaaaaak8aacaGLOaGa ayzkaaaaaa@6C99@

A Matlab code version of this solution is found in examples.m, while solutions written in C#, Python and JavaScript are available here.


Example 7: Mean position (center/midpoint)

Example seven

Problem 7:

Three positions A, B, and C are given as n-vectors n EA E MathType@MTEF@5@5@+= feaagKart1ev2aaatCvAUfeBSjuyZL2yd9gzLbvyNv2CaerbuLwBLn hiov2DGi1BTfMBaeXatLxBI9gBaebbnrfifHhDYfgasaacH8srps0l bbf9q8WrFfeuY=Hhbbf9v8qqaqFr0xc9pk0xbba9q8WqFfea0=yr0R Yxir=Jbba9q8aq0=yq=He9q8qqQ8frFve9Fve9Ff0dmeaabaqaciaa caGaaeWabaGabeaadaaakeaaqqa6daaaaaGuLrgapeGaaCOBamaaDa aaleaacaWGfbGaamyqaaqaaiaadweaaaaaaa@3764@ , n EB E MathType@MTEF@5@5@+= feaagKart1ev2aaatCvAUfeBSjuyZL2yd9gzLbvyNv2CaerbuLwBLn hiov2DGi1BTfMBaeXatLxBI9gBaebbnrfifHhDYfgasaacH8srps0l bbf9q8WrFfeuY=Hhbbf9v8qqaqFr0xc9pk0xbba9q8WqFfea0=yr0R Yxir=Jbba9q8aq0=yq=He9q8qqQ8frFve9Fve9Ff0dmeaabaqaciaa caGaaeWabaGabeaadaaakeaaqqa6daaaaaGuLrgapeGaaCOBamaaDa aaleaacaWGfbGaamOqaaqaaiaadweaaaaaaa@3765@ , and n EC E MathType@MTEF@5@5@+= feaagKart1ev2aaatCvAUfeBSjuyZL2yd9gzLbvyNv2CaerbuLwBLn hiov2DGi1BTfMBaeXatLxBI9gBaebbnrfifHhDYfgasaacH8srps0l bbf9q8WrFfeuY=Hhbbf9v8qqaqFr0xc9pk0xbba9q8WqFfea0=yr0R Yxir=Jbba9q8aq0=yq=He9q8qqQ8frFve9Fve9Ff0dmeaabaqaciaa caGaaeWabaGabeaadaaakeaaqqa6daaaaaGuLrgapeGaaCOBamaaDa aaleaacaWGfbGaam4qaaqaaiaadweaaaaaaa@3766@ . Find the mean position, M, given as n EM E MathType@MTEF@5@5@+= feaagKart1ev2aaatCvAUfeBSjuyZL2yd9gzLbvyNv2CaerbuLwBLn hiov2DGi1BTfMBaeXatLxBI9gBaebbnrfifHhDYfgasaacH8srps0l bbf9q8WrFfeuY=Hhbbf9v8qqaqFr0xc9pk0xbba9q8WqFfea0=yr0R Yxir=Jbba9q8aq0=yq=He9q8qqQ8frFve9Fve9Ff0dmeaabaqaciaa caGaaeWabaGabeaadaaakeaaqaaaauFaaaWdbiaah6gadaqhaaWcba Gaamyraiaad2eaaeaacaWGfbaaaaaa@365C@ . Note that the calculation is independent of the depths of the positions.

Solution 7:

The mean position is simply given by the mean n-vector:

n EM E = unit( n EA E + n EB E + n EC E )= n EA E + n EB E + n EC E | n EA E + n EB E + n EC E | MathType@MTEF@5@5@+= feaagKart1ev2aaatCvAUfeBSjuyZL2yd9gzLbvyNv2CaerbuLwBLn hiov2DGi1BTfMBaeXatLxBI9gBaebbnrfifHhDYfgasaacH8srps0l bbf9q8WrFfeuY=Hhbbf9v8qqaqFr0xc9pk0xbba9q8WqFfea0=yr0R Yxir=Jbba9q8aq0=yq=He9q8qqQ8frFve9Fve9Ff0dmeaabaqaciaa caGaaeWabaGabeaadaaakeaaqaaaauFaaaWdbiaah6gadaqhaaWcba Gaamyraiaad2eaaeaacaWGfbaaaOWdaiabg2da9erbd9wDYLwzYbIt LDhaiqqajaaqqqaaaaaaOpGqSvxza8GacaWF1bGaa8NBaiaa=Lgaca WF0bGcpaWaaeWaaeaaqqa6daaaaaGuLrgapmGaaCOBamaaDaaaleaa caWGfbGaamyqaaqaaiaadweaaaGcpaGaey4kaSYddiaah6gadaqhaa WcbaGaamyraiaadkeaaeaacaWGfbaaaOWdaiabgUcaR8WacaWHUbWa a0baaSqaaiaadweacaWGdbaabaGaamyraaaaaOWdaiaawIcacaGLPa aacqGH9aqpdaWcaaqaa8WacaWHUbWaa0baaSqaaiaadweacaWGbbaa baGaamyraaaak8aacqGHRaWkpmGaaCOBamaaDaaaleaacaWGfbGaam OqaaqaaiaadweaaaGcpaGaey4kaSYddiaah6gadaqhaaWcbaGaamyr aiaadoeaaeaacaWGfbaaaaGcpaqaamaaemaabaWddiaah6gadaqhaa WcbaGaamyraiaadgeaaeaacaWGfbaaaOWdaiabgUcaR8WacaWHUbWa a0baaSqaaiaadweacaWGcbaabaGaamyraaaak8aacqGHRaWkpmGaaC OBamaaDaaaleaacaWGfbGaam4qaaqaaiaadweaaaaak8aacaGLhWUa ayjcSdaaaaaa@70A0@

See also equation (17) in Gade (2010), and a Matlab code version in examples.m (solutions written in C#, Python and JavaScript are available here). Details about the definition of the horizontal geographical mean position are found here.

Figure of mean position is from examples.m. Red: n-vectors for positions A, B, and C. Green: n-vector for the mean position.


Example 8: A and azimuth/distance to B

Example eight

Problem 8:

Position A is given as n-vector . We also have an initial direction of travel given as an azimuth (bearing) relative to north (clockwise), and finally the distance to travel along a great circle is given as sAB. Use Earth radius rEarth.

Find the destination point B, given as .

In geodesy this is known as "The first geodetic problem" or "The direct geodetic problem" for a sphere, and we see that this is similar to Example 2, but now the delta is given as an azimuth and a great circle distance. ("The second/inverse geodetic problem" for a sphere is already solved in Examples 1 and 5.)

Solution 8:

The azimuth (relative to north) is a singular quantity (undefined at the Poles), but from this angle we can find a quantity that is more convenient when working with vector algebra: a vector  that points in the initial direction. We find this from azimuth by first finding the north and east vectors at the start point, with unit lengths (see also equations (9) and (10) in Gade (2010), and the figure below):

Here we have assumed that our coordinate frame E has it's z-axis along the rotational axis of the Earth, pointing towards the North Pole. Hence, this axis is given by .

Math graph

The north and east vectors are simple to find from the n-vector. The gray Earth-rotation-axis vector is also drawn at position A for convenience (vectors can be drawn at any position since they only have direction and magnitude).

The two vectors  and  are horizontal, orthogonal, and span the tangent plane at the initial position. A unit vector  in the direction of the azimuth is now given by:

Math graph

With the initial direction given as  instead of azimuth, it is now quite simple to find . We know that  and  are orthogonal, and they will span the plane where  will lie. Thus, we can use sin and cos in the same manner as above, with the angle travelled given by sAB/rEarth:

Math graph

A Matlab code version of this solution is found in examples.m, while solutions written in C#, Python and JavaScript are available here.


Example 9: Intersection of two paths

Example nine

Credits: Some readers of the paper Gade (2010) have contacted the author and reported new examples (other than those included in the paper) they have solved by means of n-vector. The following example has been reported twice, and is thus included here. Thanks to Even Børhaug at Kongsberg Maritime and Chris Veness at Movable Type Ltd, for this example. Børhaug reported that the solution he found that was based on latitude and longitude used multiple pages of code, while Veness says that such a solution is "horribly complex". Using n-vector, the intersection is very simple to find.

Problem 9:

Define a path from two given positions (at the surface of a spherical Earth), as the great circle that goes through the two points.

Path A is given by  and , while path B is given by  and .

Find the position C where the two paths intersect, as .

Solution 9:

A convenient way to represent a great circle is by its normal vector (i.e. the normal vector to the plane containing the great circle). This normal vector is simply found by taking the cross product of the two n-vectors defining the great circle (path). Having the normal vectors to both paths, the intersection is now simply found by taking the cross product of the two normal vectors:

 

Note that there will be two places where the great circles intersect, and thus two solutions are found. Selecting the solution that is closest to e.g.  can be achieved by selecting the solution that has a positive dot product with  (or the mean position from Example 7 could be used instead of  ). In program code, this can be implemented by selecting any of the two solutions above (+ or MathType@MTEF@5@5@+= feaagKart1ev2aqatCvAUfeBSjuyZL2yd9gzLbvyNv2CaerbuLwBLn hiov2DGi1BTfMBaeXatLxBI9gBaerbd9wDYLwzYbItLDharqqtubsr 4rNCHbGeaGqiVu0Je9sqqrpepC0xbbL8F4rqqrFfpeea0xe9Lq=Jc9 vqaqpepm0xbba9pwe9Q8fs0=yqaqpepae9pg0FirpepeKkFr0xfr=x fr=xb9adbaqaaeGaciGaaiaabeqaamaabaabaaGcbaqefmuySLMyYL gaiuaajugybabaaaaaaaaapeGaa83eGaaa@3A73@ ), called C', and calculate:

A Matlab code version of this solution is found in examples.m, while solutions written in C#, Python and JavaScript are available here.

 

Variant: If each of the two paths is specified by start point and azimuth instead of two points, the normal vectors to the great circles can be found by the following:

1.       Find the direction vector for each path in the same manner as  in Example 8.

2.       Take the cross product of the direction vector and the n-vector for the initial position.


Example 10: Cross track distance (cross track error)

Example ten

Credits: Thanks to Chris Veness at Movable Type Ltd for this example.

Problem 10:

Path A is given by the two n-vectors  and (as in the previous example), and a position B is given by .

Find the cross track distance sxt between the path A (i.e. the great circle through   and ) and the position B (i.e. the shortest distance at the surface, between the great circle and B). Also find the Euclidean distance dxt between B and the plane defined by the great circle. Use Earth radius rEarth.

Solution 10:

First, find the normal to the great circle, with direction given by the right hand rule and the direction of travel:

Math graph

We now see that the cross track distance to B is the great circle distance from  minus 90° (π/2 radians):

 

We have used the acos() variant of the great circle distance in Example 5, since it is numerically well-conditioned in the relevant interval.

Finding the Euclidean distance is even simpler, since it is the projection of  onto , thus simply the dot product:

 

For both sxt and dxt, positive answers means that B is to the right of the track.

A Matlab code version of this solution is found in examples.m, while solutions written in C#, Python and JavaScript are available here.

 

Variant: If the path is specified by start point and azimuth instead of two points, the normal vector to the great circle can be found by the following:

1.       Find the direction vector for the path in the same manner as  in Example 8.

2.       Take the cross product of the direction vector and the n-vector for the initial position.

 

Mathematical notation used (vector symbols explained)

The notation system used for the n-vector page and the files for download, is presented very briefly in Section 2 of Gade (2010), but here we will present it in much more detail, and here we will also skip the usage of coordinate free vectors.

Coordinate frame

A coordinate frame has a position (origin), and three axes (basis vectors) x, y and z (orthonormal). Thus a coordinate frame can represent both position and orientation, i.e. 6 degrees of freedom. It can be used to represent a rigid body, such as a vehicle or the Earth, and it can also be used to represent a "virtual" coordinate frame such as North-East-Down.

Coordinate frames are designated with capital letters, e.g. the three generic coordinate frames A, B, and C.

We also have specific names for some common coordinate frames:

Coordinate frame

Description

E

Earth

N

North-East-Down

B

Body, i.e. the vehicle

 

General vector

A 3D vector given with numbers is written e.g. . The three numbers are the vector components along the x, y and z

axes of a coordinate frame. If the name of the vector is k, and the coordinate frame is A, we will use bold k and A as trailing superscript, i.e.:

Thus  is the 3D vector that is constructed by going 2 units along the x-axis of coordinate frame A, 4 units along the y-axis, and 6 along the z-axis. We say that the vector k is decomposed in A.

Position vector

Instead of the general vector k, we can have a specific vector that goes from A to B. This vector can be decomposed in C. A, B, and C are three arbitrary coordinate frames. We would write this vector:

In program code: p_AB_C

The letter p is used since this is a position vector.

Example a):

From the subscript we see that this is the vector that goes from E (center of the Earth) to B (the vehicle). The superscript tells us that it is decomposed in E, which we now assume has its z-axis pointing towards the North Pole. From the values we see that the vector goes 6371 km towards the North Pole, and zero in the x and y directions. If we assume that the Earth is a sphere with radius 6371 km, we see that B is at the North Pole.

Example b):

The vector goes from B, e.g. an aircraft, to C, e.g. an object. The vector is decomposed in N (which has North-East-Down axes), i.e. C is 50 m north of B and 60 m east, and C is also 5 m above B.

 

Properties of the position vector

For the general position vector , we have the property:

I.e. swapping the coordinate frames in the subscript gives a vector that goes in the opposite direction. We also have:

Math equation image

I.e., going from A to D is the same as first going from A to B, then from B to D. From the equation, we see that B is cancelled out. A, B, C, and D are arbitrary coordinate frames.

Rotation matrix

If we return to the general vector , we could also have a coordinate frame B, with different orientation than A. The same vector k could be expressed by components along the x, y and z-axes of B instead A, i.e. it can also be decomposed in B, written . Note that the length of  equals the length of . We will now have the relation:

 is the 9 element (3x3) rotation matrix (also called direction cosine matrix) that transforms vectors decomposed in B to vectors decomposed in A. Note that the B in  should be closest to the vector decomposed in B (the "closest rule"). If we need to go in the other direction we have:

Now we see that A is closest to A.

 

Properties of the rotation matrix

We have that

where the T means matrix transpose. We also have the following property (closest frames are cancelled):

If we compare these properties with the position vector, we see that they are very similar: minus is replaced by transpose, and plus is replaced by matrix multiplication. A, B, and C are three arbitrary coordinate frames.

n-vector

The n-vector is in almost all cases decomposed in E, and in the simplest form we will write it

This simple form can be used in cases where it is no doubt about what n-vector expresses the position of. In such cases we can also express the position using the variables lat and long, without further specification.

However, if we are interested in the position of multiple objects, e.g. A and B, we must specify which of the two, both for n-vector and for latitude/longitude. In this case we will write:

 and  (program code: n_EA_E and n_EB_E)

And

 and

The subscript E might seem redundant here, it could be sufficient to use only A or B. However, we have chosen to also include the E, since both n-vector and latitude/longitude are depending on the reference ellipsoid that is associated with E (see Section 4.1. in Gade (2010) for more about this). Note however, that the subscript rules (swapping and canceling) we had for  and  cannot be used for n-vector or lat/long.

For spherical Earth, we have a simple relation between  and :

where  is the radius of the Earth,  is the height of B and  is the depth. For more information about how to use n-vector in various calculations, see the 10 examples and Gade (2010).

 

Download

Functions for performing various position calculations are available for download here. Currently, the functions are available in the following programming languages: Matlab, C#, C++, Python, and JavaScript.

Matlab:

The following 19 files are included in the downloadable zip-file (n_vector_library_Matlab.zip):

#

Filename

Content

 

Convert between lat/long and n-vector:

1.        

lat_long2n_E.m

Converts latitude and longitude to n-vector

2.        

n_E2lat_long.m

Converts n-vector to latitude and longitude

 

Convert between delta and  n-vectors:

3.        

n_EA_E_and_n_EB_E2p_AB_E.m

From two positions A and B, finds the delta position

4.        

n_EA_E_and_p_AB_E2n_EB_E.m

From position A and delta, finds position B

 

Convert between n-vector and ECEF vector:

5.        

n_EB_E2p_EB_E.m

Converts n-vector to position vector in meters

6.        

p_EB_E2n_EB_E.m

Converts position vector in meters to n-vector

 

Convert between n-vector and rotation matrix:

7.        

R_EN2n_E.m

Finds n-vector from

8.        

n_E2R_EN.m

Finds  from n-vector

9.        

R_EL2n_E.m

Finds n-vector from

10.    

n_E_and_wa2R_EL.m

Finds  from n-vector and wander azimuth angle

 

Convert between Euler angles and rotation matrix:

11.    

xyz2R.m

Creates a rotation matrix from 3 angles about new axes in the xyz order

12.    

R2xyz.m

Three angles about new axes in the xyz order are found from a rotation matrix

13.    

zyx2R.m

Creates a rotation matrix from 3 angles about new axes in the zyx order (e.g. yaw-pitch-roll)

14.    

R2zyx.m

Three angles about new axes in the zyx order (e.g. yaw-pitch-roll) are found from a rotation matrix

 

Miscellaneous simple utilities:

15.    

unit.m

Makes input vector unit length (i.e. norm=1)

16.    

rad.m

Converts angle in degrees to radians

17.    

deg.m

Converts angle in radians to degrees

18.    

R_Ee.m

Selects axes of the coordinate frame E

 

Solutions to the 10 examples:

19.    

examples.m

Contains solutions to the 10 examples given here

 

C# (C Sharp):

A C#-version of the n-vector library and the 10 examples are available for download here.

Author: Jørn Inge Vestgården at FFI (responsible for translation from Matlab and adaption to C#).

 

C++:

A C++-version of the n-vector library is available for download here (currently, functions 1 to 8 in the table above are included).

Author: Magnus Baksaas at FFI (responsible for translation from Matlab and adaption to C++).

Python:

A Python-version of the entire n-vector library is available at https://pypi.python.org/pypi/nvector. Python solutions to the 10 examples are also included there.

Author: Per A. Brodtkorb at FFI (responsible for translation from Matlab and adaption to Python).

JavaScript:

A JavaScript-version of the n-vector library is available here, while JavaScript solutions to the 10 examples are available here.

Author: Chris Veness at Movable Type (responsible for translation from Matlab and adaption to JavaScript).

Java, LabVIEW etc.:

If you translate the functions to another programming language, and want to make your solution available for others, we would be happy to add your files (with credits) to this site (or add a link to a site where the functions are available).

 


Reference:

Kenneth Gade (2010): A Non-singular Horizontal Position Representation, The Journal of Navigation, Volume 63, Issue 03, pp 395-417, July 2010. www.navlab.net/Publications/A_Nonsingular_Horizontal_Position_Representation.pdf

 

If you have comments or suggestions regarding this web page or the downloadable code, please send an e-mail to nvector@navlab.net

For more fundamental navigation theory, see the following article: The Seven Ways to Find Heading, in The Journal of Navigation, September 2016.