pyGPSBehaviour/GPS_module.py at master · alexblaessle/pyGPSBehaviour · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
"""GPS module of seal project. Contains all functions necessary to read and convert GPS coordinates."""

# Import modules
import numpy as np

def toCartesian(lat,lon,r=6378137,e=8.1819190842622e-2,h=0):

	"""Converts the GPS data to cartesian coordinates.

	Args:
		lat (numpy.ndarray): Latitude values.
		lon (numpy.ndarray): Longitude values.

	Keyword Args:
		r (float): Radius.
		e (float): Excentriciity.
		h (float): Height of location. Normally assumed to be sea-level.

	Returns:

		tuple: Tuple containing:

			* x (numpy.ndarray): x coordinates.
			* y (numpy.ndarray): y coordinates.
			* z (numpy.ndarray): z coordinates.

	"""

	# To radians
	latRad=lat/180.*np.pi
	lonRad=lon/180.*np.pi

	N = r/np.sqrt(1 - e**2 * np.sin(latRad)**2)

	x = -1*(N+h)*np.cos(latRad)*np.cos(lonRad)
	y = -1*(N+h)*np.cos(latRad)*np.sin(lonRad)
	z = ((1-e**2)*N+h)*np.sin(latRad)

	return x,y,z

def computeDistances(x,y):

	"""Computes distances of all steps.

	Args:
		x (numpy.ndarray): x coordinates.
		y (numpy.ndarray): y coordinates.

	Returns:
		numpy.ndarray: Distances.

	"""

	d=np.sqrt(abs(np.diff(x))**2+abs(np.diff(y))**2)

	return d