def convert_azm_aacgm2geo(azM, latG, lonG, dTime, refAlt=300):
# Convert azimuths from AACGM to geodetic
refZ = -refAlt * 1E3 # nvector uses z = height in metres down
wgs84 = nv.FrameE(name='WGS84')
nPole = aacgmv2.convert_latlon(90, 0, refAlt, dTime, method_code="A2G")
geoAzimuths = np.ones(azM.shape) * np.nan
for ind, mAzm in enumerate(azM):
pointA = wgs84.GeoPoint(
latitude=latG[ind],
longitude=lonG[ind],
z=refZ,
degrees=True,
)
pointPole = wgs84.GeoPoint(
latitude=nPole[0],
longitude=nPole[1],
z=refZ,
degrees=True,
)
p_AB_N = pointA.delta_to(pointPole)
azimuth_offset = p_AB_N.azimuth_deg[0]
geoAzimuths[ind] = mAzm + azimuth_offset
geoAzimuths[geoAzimuths > 180] -= 360.
geoAzimuths[geoAzimuths < -180] += 360.
return geoAzimuths
def convert_latlong_to_xyz(latitude: float, longitude: float) -> np.ndarray:
# See https://github.com/pbrod/nvector#example-4-geodetic-latitude-to-ecef-vector
xyz = nv.FrameE(name='WGS84') \
.GeoPoint(latitude=latitude, longitude=longitude, z=0.0, degrees=True) \
.to_ecef_vector() \
.pvector.ravel()
return xyz
def convert_xyz_to_latlong(xyz: np.ndarray) -> Tuple[float, float]:
# See https://github.com/pbrod/nvector#example-3-ecef-vector-to-geodetic-latitude
position_B = 6371e3 * np.vstack((xyz[0], xyz[1], xyz[2]))
latitude, longitude, _ = nv.FrameE(name='WGS84') \
.ECEFvector(position_B) \
.to_geo_point().latlon_deg
return float(latitude), float(longitude)
def calculate_distance(lat1, lon1, lat2, lon2):
wgs84 = nvector.FrameE(name='WGS84')
point1 = wgs84.GeoPoint(latitude=lat1, longitude=lon1, degrees=True)
point2 = wgs84.GeoPoint(latitude=lat2, longitude=lon2, degrees=True)
s_12, _azi1, _azi2 = point1.distance_and_azimuth(point2)
return s_12 / 1000 / 1.852 # nm
def surface_distance(p1, p2):
frame = nv.FrameE(name='WGS84')
a1 = frame.GeoPoint(p1[0], p1[1], degrees=True)
a2 = frame.GeoPoint(p2[0], p2[1], degrees=True)
a_12_e = a1.to_ecef_vector() - a2.to_ecef_vector()
d = norm(a_12_e.pvector, axis=0)[0]
return d
def __init__(self, name):
self.name = name
self.frame = nvector.FrameE(name='WGS84')
#self.adc_lock = threading.Lock()
self.timeout_neutral = 2
self.timeout_kill = 10
self._enabled = False
self.gps_reading = None
self.joy_data_prev = None
#self.steering = Setting_Publisher('/steering/setting', Scaler(+1, -1, 64, 192))
##self.steering = Setting_Publisher('/steering/setting', Scaler(+1, -1, 96, 160))
#self.transmission = Setting_Publisher('/transmission/setting', Scaler(+1, -1, 64, 192))
# 2016.10.10
self.steering = Setting_Publisher('/steering/put',
Scaler(+1, -1, 104, 152))
self.transmission = Setting_Publisher('/transmission/put',
Scaler(+1, -1, 64, 192))
self.throttle = Setting_Publisher('/throttle/put',
Scaler(+1, 0, 20, 230))
if False:
for i in range(-10, 10):
j = i / 10.
self.steering(j)
self.steering(0)
# Stop...
sys.exit()
#self.steer_for_bearing = PID.PID(P=0.01, I=0.02, D=0.01, Derivator=0, Integrator=0, Integrator_max=2, Integrator_min=-2, output_min = -1.0, output_max=+1.0)
self.steer_for_bearing = PID.PID(P=0.02,
I=0.02,
D=0.02,
Derivator=0,
Integrator=0,
Integrator_max=2,
Integrator_min=-2,
output_min=-0.60,
output_max=+0.60)
#self.steer_for_bearing = PID.PID(P=0.02, I=0.03, D=0.02, Derivator=0, Integrator=0, Integrator_max=2, Integrator_min=-2, output_min = -0.60, output_max=+0.60)
print self.steer_for_bearing(30)
self.engine = None
#self.engine.disable()
self.navigator = None
rospy.loginfo('sto')
self.disable()
rospy.loginfo('%s initialized.' % (self.name))
if False:
sys.exit()
signal.signal(signal.SIGALRM, self.timeout)
def __init__(self, name=None):
if name:
self.name = name
print 'starting %s' % (self)
self.captured = 0
self.captured_distance = 0.50
self.offset = 0.0
self.throttle = 0.50
# 000.027, 000.018 at 7 MPH!
#self.xtd_pid = PID.PID(P=12.0, I=1.0, D=1.0, Derivator=0, Integrator=0, Integrator_max=1, Integrator_min=-1)
# 000.020, 000.017 at 7
#self.pid_captured = PID.PID(P=20.0, I=1.0, D=1.0, Derivator=0, Integrator=0, Integrator_max=1, Integrator_min=-1)
# Used for drilling
#self.pid_captured = PID.PID(P=20.0, I=8.0, D=4.0, Derivator=0, Integrator=0, Integrator_max=1, Integrator_min=-1)
#self.pid_seeking = PID.PID(P=6.0, I=0.0, D=4.0, Derivator=0, Integrator=0, Integrator_max=1, Integrator_min=-1)
# Used for cart.
self.pid_captured = PID.PID(P=100.0, I=70.0, D=50, Derivator=0, Integrator=0, Integrator_max=50, Integrator_min=-50)
self.pid_seeking = PID.PID(P=10.0, I=0.0, D=4.0, Derivator=0, Integrator=0, Integrator_max=1, Integrator_min=-1)
# Yikes! Hunting...
#self.pid_captured = PID.PID(P=30.0, I=30.0, D=10.0, Derivator=0, Integrator=0, Integrator_max=1, Integrator_min=-1)
#self.pid_seeking = PID.PID(P=30.0, I=0.0, D=4.0, Derivator=0, Integrator=0, Integrator_max=1, Integrator_min=-1)
self.frame = nvector.FrameE(name='WGS84')
self.pub_xtd = rospy.Publisher('/navigation/xtd', std_msgs.msg.Float64, queue_size=10)
def calc_dist(
alt1,
lat1,
lon1,
alt2,
lat2,
lon2,
):
import nvector as nv
wgs84 = nv.FrameE(name='WGS84')
pt1 = wgs84.GeoPoint(latitude=lat1, longitude=lon1, z=alt1, degrees=True)
pt2 = wgs84.GeoPoint(latitude=lat2, longitude=lon2, z=alt2, degrees=True)
# Great circle dist
# dist_gc = np.sqrt(np.sum(pt1.delta_to(pt2).pvector ** 2)) / 1E3
# Straight-line dist
dist_strt = np.sqrt(
np.sum((pt1.to_ecef_vector().pvector - pt2.to_ecef_vector().pvector)**
2)) / 1E3
# midpoint between the two
midpt = nv.GeoPath(pt1, pt2).interpolate(0.5).to_geo_point()
midpt_ll = [midpt.latitude, midpt.longitude]
# dist. from straight-line midpoint up to ground level
hypot = np.sqrt(
np.sum((pt1.to_ecef_vector().pvector - midpt.to_ecef_vector().pvector)
**2)) / 1E3
midpt_depth = np.sqrt(hypot**2 - (dist_strt / 2)**2)
return dist_strt, midpt_depth
def translateLongLatAlt_to_XYZ(longitude, latitude, altitude):
wgs84 = nv.FrameE(name='WGS84')
pointB = wgs84.GeoPoint(longitude, latitude, altitude, degrees=True)
p_EB_E = pointB.to_ecef_vector()
myvector = p_EB_E.pvector.ravel()
return p_EB_E.pvector.ravel()
def dist_to_point(self, loc, dis, ang):
frame = nv.FrameE(name='WGS84')
pointA = frame.GeoPoint(latitude=loc[0],
longitude=loc[1],
degrees=True)
pointB, _azimuthb = pointA.geo_point(dis, azimuth=ang, degrees=True)
lat, lon = pointB.latitude_deg, pointB.longitude_deg
loc = [lat, lon]
return loc
def crosstrack_distance(a1, a2, b):
frame = nv.FrameE(a=EARTH_RADIUS, f=0)
pointA1 = frame.GeoPoint(a1[0], a1[1], degrees=True)
pointA2 = frame.GeoPoint(a2[0], a2[1], degrees=True)
pointB = frame.GeoPoint(b[0], b[1], degrees=True)
pathA = nv.GeoPath(pointA1, pointA2)
cr_distance = pathA.cross_track_distance(pointB,
method='greatcircle').ravel()
return cr_distance[0]
def dist(self, loc1, loc2):
wgs84 = nv.FrameE(name='WGS84')
point1 = wgs84.GeoPoint(latitude=loc1[0],
longitude=loc1[1],
degrees=True)
point2 = wgs84.GeoPoint(latitude=loc2[0],
longitude=loc2[1],
degrees=True)
s_12, _azi1, _azi2 = point1.distance_and_azimuth(point2)
return s_12
def closest_point_on_circle(a1, a2, b):
frame = nv.FrameE(a=EARTH_RADIUS, f=0)
pointA1 = frame.GeoPoint(a1[0], a1[1], degrees=True)
pointA2 = frame.GeoPoint(a2[0], a2[1], degrees=True)
pointB = frame.GeoPoint(b[0], b[1], degrees=True)
pathA = nv.GeoPath(pointA1, pointA2)
closest_point = pathA.closest_point_on_great_circle(pointB).to_geo_point()
lat, lon = closest_point.latitude_deg.tolist(
)[0], closest_point.longitude_deg.tolist()[0]
return lat, lon
def intersection_point(a1, a2, b1, b2):
frame = nv.FrameE(a=EARTH_RADIUS, f=0)
pointA1 = frame.GeoPoint(a1[0], a1[1], degrees=True)
pointA2 = frame.GeoPoint(a2[0], a2[1], degrees=True)
pointB1 = frame.GeoPoint(b1[0], b1[1], degrees=True)
pointB2 = frame.GeoPoint(b2[0], b2[1], degrees=True)
pathA = nv.GeoPath(pointA1, pointA2)
pathB = nv.GeoPath(pointB1, pointB2)
ipoint = pathA.intersect(pathB).to_geo_point()
return float(ipoint.latitude_deg[0]), float(ipoint.longitude_deg[0])
def azimuth(Lon1, Lat1, Lon2, Lat2):
wgs84 = nv.FrameE(name='WGS84')
pointA = wgs84.GeoPoint(latitude=Lat1, longitude=Lon1, z=0, degrees=True)
pointB = wgs84.GeoPoint(latitude=Lat2, longitude=Lon2, z=0, degrees=True)
p_AB_N = pointA.delta_to(pointB)
azim = p_AB_N.azimuth_deg[0]
if azim < 0:
azim += 360
else:
pass
return azim
def __init__(self, name, geo=None):
self.name = name
self.geo = geo
self.enu = None
grs80 = nv.FrameE(name='GRS80')
lat, lon, height = geo
geoPoint = grs80.GeoPoint(latitude=lat,
longitude=lon,
z=-height,
degrees=True)
self.ecef = geoPoint.to_ecef_vector().pvector.ravel()
def position_on_path(a1, a2, b):
frame = nv.FrameE(a=EARTH_RADIUS, f=0)
pointA1 = frame.GeoPoint(a1[0], a1[1], degrees=True)
pointA2 = frame.GeoPoint(a2[0], a2[1], degrees=True)
pointB = frame.GeoPoint(b[0], b[1], degrees=True)
pathA = nv.GeoPath(pointA1, pointA2)
if pathA.on_great_circle(pointB):
if pathA.on_path(pointB):
return PositionOnPath.OnPath
else:
return PositionOnPath.NotOnPathOnCircle
else:
return PositionOnPath.NotOnCircle
def convertGodeticToECEF(geodetics):
'''http://itrf.ensg.ign.fr/faq.php?type=answer#question2'''
grs80 = nv.FrameE(name='GRS80')
ecefPoints = np.empty((0, 3))
for lat, lon, height in geodetics:
geoPoint = grs80.GeoPoint(latitude=lat,
longitude=lon,
z=-height,
degrees=True)
ecefPoint = geoPoint.to_ecef_vector().pvector.ravel()
ecefPoints = np.append(ecefPoints, [ecefPoint], axis=0)
return ecefPoints
def findstrike(v, alt, az):
alt = math.radians(alt)
G = 6.673 * (10**-11)
M = 5.97219 * (10**24)
Re = 6371000.0
Ri = 2 * Re * G * M / (2 * G * M - ((v * math.sin(alt))**2) * Re)
t = ((2 * (Re**3) / (G * M))**0.5) * (math.atan(
(Re / (Ri - Re))**0.5) - ((Re * (Ri - Re)) / Ri)**0.5 + math.pi / 2)
d = v * math.cos(alt) * t
frame = nv.FrameE(a=6371e3, f=0)
pointA = frame.GeoPoint(latitude=27.9881, longitude=86.9250, degrees=True)
pointB, _azimuthb = pointA.displace(distance=d, azimuth=az, degrees=True)
lat, lon = pointB.latitude_deg, pointB.longitude_deg
return (lat, lon)
def __init__(self, ROI):
bounds = [(ROI[0], ROI[2], 0), (ROI[1], ROI[2], 0),
(ROI[0], ROI[3], 0), (ROI[1], ROI[3], 0)]
self.wgs84 = nv.FrameE(name='WGS84')
lon, lat, hei = np.array(bounds).T
geo_points = self.wgs84.GeoPoint(longitude=lon,
latitude=lat,
z=-hei,
degrees=True)
P = geo_points.to_ecef_vector().pvector.T
dx = normed(P[1] - P[0])
dy = P[2] - P[0]
dy -= dx * dy.dot(dx)
dy = normed(dy)
dz = np.cross(dx, dy)
self.rotation = np.array([dx, dy, dz]).T
self.mu = np.mean(P.dot(self.rotation), axis=0)[np.newaxis, :]
def arc(poi1, poi2):
points = []
frame_E = nv.FrameE(a=e_radius * 1000, f=0)
gp1 = frame_E.GeoPoint(latitude=poi1.latitude,
longitude=poi1.longitude,
degrees=True)
gp2 = frame_E.GeoPoint(latitude=poi2.latitude,
longitude=poi2.longitude,
degrees=True)
distance, _azia, _azib = gp1.distance_and_azimuth(gp2)
distance = max(1, int(distance / 50000))
path = nv.GeoPath(gp1, gp2)
for i in range(0, distance + 1):
geo = path.interpolate((1.0 / distance) * i).to_geo_point()
points += [poi(geo.latitude_deg, geo.longitude_deg, 10)]
return points
def way_points_interp(location_block):
lat_lon1 = location_block[0]
lat_lon2 = location_block[1]
n = location_block[2]
wgs84 = nv.FrameE(name='WGS84')
lat1, lon1 = lat_lon1
lat2, lon2 = lat_lon2
n_EB_E_t0 = wgs84.GeoPoint(lat1, lon1, degrees=True).to_nvector()
n_EB_E_t1 = wgs84.GeoPoint(lat2, lon2, degrees=True).to_nvector()
path = nv.GeoPath(n_EB_E_t0, n_EB_E_t1)
interpolate_coor = [[lat1, lon1]]
piece_fraction = 1 / n
for n in range(n - 1):
g_EB_E_ti = path.interpolate(piece_fraction * (n + 1)).to_geo_point()
interpolate_coor.append(
[g_EB_E_ti.latitude_deg[0], g_EB_E_ti.longitude_deg[0]]
)
return interpolate_coor
def get_distance_from_boundaries(animal_position, perimeter):
north_west_corner = perimeter['north_west_corner']
north_east_corner = perimeter['north_east_corner']
south_west_corner = perimeter['south_west_corner']
south_east_corner = perimeter['south_east_corner']
earth_radius = 6371e3
frame = nv.FrameE(a=earth_radius, f=0)
# Convert the positions to the format used by the nvector library
animal_geo_point = frame.GeoPoint(animal_position['latitude'], animal_position['longitude'], degrees=True)
north_west_geo_point = frame.GeoPoint(north_west_corner['latitude'], north_west_corner['longitude'], degrees=True)
north_east_geo_point = frame.GeoPoint(north_east_corner['latitude'], north_east_corner['longitude'], degrees=True)
south_west_geo_point = frame.GeoPoint(south_west_corner['latitude'], south_west_corner['longitude'], degrees=True)
south_east_geo_point = frame.GeoPoint(south_east_corner['latitude'], south_east_corner['longitude'], degrees=True)
# Get the four boundaries
west_boundary = nv.GeoPath(north_west_geo_point, south_west_geo_point)
east_boundary = nv.GeoPath(north_east_geo_point, south_east_geo_point)
north_boundary = nv.GeoPath(north_west_geo_point, north_east_geo_point)
south_boundary = nv.GeoPath(south_west_geo_point, south_east_geo_point)
# Caclulate the absolute distance of the animal from each of the four boundaries
dist_to_west_boundary = abs((west_boundary.cross_track_distance(animal_geo_point, method='greatcircle').ravel())[0])
dist_to_east_boundary = abs((east_boundary.cross_track_distance(animal_geo_point, method='greatcircle').ravel())[0])
dist_to_north_boundary = abs((north_boundary.cross_track_distance(animal_geo_point, method='greatcircle').ravel())[0])
dist_to_south_boundary = abs((south_boundary.cross_track_distance(animal_geo_point, method='greatcircle').ravel())[0])
# Put the distances in a dictonary and return
distance_to_boundaries = {}
distance_to_boundaries['west'] = dist_to_west_boundary
distance_to_boundaries['east'] = dist_to_east_boundary
distance_to_boundaries['north'] = dist_to_north_boundary
distance_to_boundaries['south'] = dist_to_south_boundary
return distance_to_boundaries
def __init__(
self,
port,
baudrate=115200,
):
self.port = port
self.baudrate = baudrate
self.uart = serial.Serial(port=self.port, baudrate=self.baudrate)
self.streamreader = pynmea2.NMEAStreamReader(self.uart)
#self.thread = threading.Thread(target=self.consumer)
self.prev_heading = 0.0
self.clear_position()
self.frame = nvector.FrameE(name='WGS84')
self.update_time = time.time()
# It's not running, but it hasn't been started either.
self.stopped = False
# restart
if False:
#self.send_command('$PNVGRST,W')
self.send_command('$PNVGRST,F')
sys.exit()
if True:
#send_command(receiver_nmea, '$PNVGRZB,PNVGBLS,1,GGA,5,PNVGIMU,1,RMC,1,PNVGBSS,1')
#self.send_command('$PNVGVER')
#self.send_command('$PNVGRZB')
#self.send_command('$PNVGRZB,PNVGBLS,1,GGA,5,PNVGIMU,1,RMC,1,PNVGBSS,1,VTG,1,HDT,1')
self.send_command('$PNVGRZB,PNVGBLS,1,GGA,5,PNVGIMU,1,RMC,1,PNVGBSS,1,VTG,1,HDT,1')
def calculateEuclideanDistance(self, startPoint=Point, endPoint=Point):
"""
Calculate the distances between two points in meters.
:param startPoint: latitude and longitud of the first point, must contain the CRS in which is given the coordinates
:param endPoint: latitude and longitud of the second point, must contain the CRS in which is given the coordinates
:return: Euclidean distance between the two points in meters.
"""
startPointTransformed = self.transformPoint(startPoint)
endPointTransformed = self.transformPoint(endPoint)
wgs84 = nv.FrameE(name='WGS84')
point1 = wgs84.GeoPoint(latitude=startPointTransformed.getLatitude(),
longitude=startPointTransformed.getLongitude(),
degrees=True)
point2 = wgs84.GeoPoint(latitude=endPointTransformed.getLatitude(),
longitude=endPointTransformed.getLongitude(),
degrees=True)
ellipsoidalDistance, _azi1, _azi2 = point1.distance_and_azimuth(point2)
p_12_E = point2.to_ecef_vector() - point1.to_ecef_vector()
euclideanDistance = np.linalg.norm(p_12_E.pvector, axis=0)[0]
return euclideanDistance
from std_msgs.msg import Header
from std_srvs.srv import Trigger, TriggerResponse
from geometry_msgs.msg import (
TransformStamped,
Transform,
PoseStamped,
Pose,
Quaternion,
Vector3,
Point,
PointStamped
)
NODE_NAME = 'rtk_to_cartesian_coord'
WGS84 = nv.FrameE(name='WGS84')
""" TODO(asiron) convert into parameter """
HISTORY_LENGTH = 30
EXPECTED_FREQ = 5.0
LAST_MEASUREMENT_DELAY_THRESHOLD = 0.5
MEASUREMENT_UPDATE_FREQUENCY_TOLERANCE = 1.0
LAT_VAR_TH = 1e-10
LON_VAR_TH = 1e-10
ALT_VAR_TH = 1e-3
def logged_service_callback(f, throttled = True):
if throttled:
loginfo = partial(rospy.loginfo_throttle, 1)
def nvector_cell_corners(lat, lon):
"""
Given the central lat/lon of a 2D grid of pixels, calculates the
lat/lon coordinates of the corners around the cells. (The result
will be an array one larger than the input in each direction). Uses
the nvector module to perform interpolation.
Firstly, the input arrays are extended by one from each side. The
new points are extrapolated along a geocidic line from the grid. The
corners are extrapolated from the new edges. Secondly, each set of
four adjacent points are averaged to find their centre. To
illustrate,
u<-a--b c--d->v
^ ^ ^ ^ ^ ^
| | | | | |
w<-A--B C--D->x
| | | | | |
y<-E--F G--H->z
A-H are the original data points. Point a is extrapolated from the
line connecting A and E while point w comes from AB. Similarly, b is
from BF and y is from ER. Point u is then extrapolated from the
intersection of lines ab and wy.
"""
import nvector as nv
wgs84 = nv.FrameE(name='WGS84')
op_flags = [['writeonly'], ['readonly'], ['readonly']]
# Build a larger array and convert all coords to nvectors
points = np.empty((lon.shape[0] + 2, lon.shape[1] + 2), dtype=nv.Nvector)
for point, ln, lt in np.nditer([points[1:-1, 1:-1], lon, lat],
flags=['refs_ok'],
op_flags=op_flags):
point[...] = wgs84.GeoPoint(lt, ln, degrees=True).to_nvector()
def extrap_edge(edge_line, near_line, far_line):
"""Extrapolate points to edges of grid."""
for edge, near, far in np.nditer([edge_line, near_line, far_line],
flags=['refs_ok'],
op_flags=op_flags):
path = nv.GeoPath(far[()], near[()])
edge[()] = path.interpolate(2.)
extrap_edge(points[1:-1, 0], points[1:-1, 1], points[1:-1, 2])
extrap_edge(points[1:-1, -1], points[1:-1, -2], points[1:-1, -3])
extrap_edge(points[0, 1:-1], points[1, 1:-1], points[2, 1:-1])
extrap_edge(points[-1, 1:-1], points[-2, 1:-1], points[-3, 1:-1])
def extrap_corner(horz_near, horz_far, vert_near, vert_far):
"""Extrapolate grid corners to the edges of the grid."""
horz_path = nv.GeoPath(horz_far, horz_near)
vert_path = nv.GeoPath(vert_far, vert_near)
return horz_path.intersection(vert_path).to_nvector()
points[0, 0] = extrap_corner(points[1, 0], points[2, 0], points[0, 1],
points[0, 2])
points[-1, 0] = extrap_corner(points[-2, 0], points[-3, 0], points[-1, 1],
points[-1, 2])
points[0, -1] = extrap_corner(points[1, -1], points[2, -1], points[0, -2],
points[0, -3])
points[-1, -1] = extrap_corner(points[-2, -1], points[-3, -1],
points[-1, -2], points[-1, -3])
# Form output arrays
shape = (lon.shape[0] + 1, lon.shape[1] + 1)
crnrlon = np.empty(shape)
crnrlat = np.empty(shape)
# GeoPoints/Nvectors aren't easily merged so make a container
nvecs = wgs84.Nvector([[np.ones(4)], [np.zeros(4)], [np.zeros(4)]])
# Corners are the midpoint of each 2x2 box of points
for (i, j), _ in np.ndenumerate(crnrlon):
# Copy 2x2 box of points into container (it's a list of arrays)
for k in range(3):
nvecs.normal[k] = [
pnt.normal[k] for pnt in points[i:i + 2, j:j + 2].flat
]
# Use nvector to find average of each square
mean = nvecs.mean_horizontal_position().to_geo_point()
crnrlon[i, j] = mean.longitude_deg
crnrlat[i, j] = mean.latitude_deg
return crnrlat, crnrlon
import math
import nvector
earth_radius = 6371e3
n_scans = 1354 # samples per scan
n_tracks = 2030 # samples per scan
swath_angel = 55 # degrees
orbit_height = 705e3 # km
nadir_pixel_size = 1e3 # km
n_scan_groups = 203
earth_surface_sphere = 4 * math.pi * (earth_radius / 1000)**2
pixel_angel = 2 * math.degrees(math.atan(
0.5 * nadir_pixel_size / orbit_height))
earth = nvector.FrameE(a=earth_radius, f=0)
import best
import best.plot
from pymc import MCMC
import statsmodels.stats as stats
MAX_DISTANCE = 50.0
if __name__ == '__main__':
# read main replacements
cip = pd.read_csv('cip.csv', index_col='bes', parse_dates=['start', 'end'])
# read water quality tests
wq = pd.read_csv('wq.csv', index_col='date', parse_dates=['date'])
# nav frame
frame = nv.FrameE(a=6371e3, f=0)
data = []
for j in range(len(wq)):
# get water test point
p1 = frame.GeoPoint(wq.iloc[j]['lat'], wq.iloc[j]['lng'], degrees=True)
for i in range(len(cip)):
# get main replacement path
l1 = frame.GeoPoint(cip.iloc[i]['from_lat'],
cip.iloc[i]['from_lng'],
degrees=True)
l2 = frame.GeoPoint(cip.iloc[i]['to_lat'],
cip.iloc[i]['to_lng'],
degrees=True)
import pandas as pd
import nvector as nv
from datetime import datetime
from haversine import haversine
from functools import reduce
# Custom classes
import link_classes as lc
import dist_functions as dist
# Constants
DATA_DIR = '../data'
# DATA_DIR = 'probe_data_map_matching'
FRAME = nv.FrameE(a=6371e3, f=0)
# Utility functions
def bearing(start, end):
"""
Computes the bearing in degrees between two geopoints
Inputs:
start (tuple of lat, long): starting geolocation
end (tuple of lat, long): ending geolocation
Outputs:
(float): bearing in degrees between start and end
"""
phi_1 = math.radians(start[0])
