def course(lat1, lon1, lat2, lon2):
"""Return course in degrees between two lat/lon points relative to grid north.
"""
delta_y = trig.sind(lon2 - lon1) * trig.cosd(lat2)
delta_x = (trig.cosd(lat1) * trig.sind(lat2) -
trig.sind(lat1) * trig.cosd(lat2) * trig.cosd(lon2 - lon1))
return trig.atan2d(delta_y, delta_x) % 360.0
def get_measurement_positions(measurements, acc_dx, acc_dy, heading):
"""Return the x and y coordinates of the measurements as two parallel lists.
"""
xs = [-m.surface_range * trig.sind(m.hor_angle) for m in measurements]
ys = [m.surface_range * trig.cosd(m.hor_angle) for m in measurements]
xs_global, ys_global = [], []
for x, y in zip(xs, ys):
xg = x * trig.cosd(heading) + y * trig.sind(heading) + acc_dx
yg = -x * trig.sind(heading) + y * trig.cosd(heading) + acc_dy
xs_global.append(xg)
ys_global.append(yg)
return xs_global, ys_global
def get_measurement_positions(measurements, acc_dx, acc_dy, heading):
"""Return the x and y coordinates of the measurements as two parallel lists.
"""
xs = [-m.surface_range * trig.sind(m.hor_angle) for m in measurements]
ys = [m.surface_range * trig.cosd(m.hor_angle) for m in measurements]
xs_global, ys_global = [], []
for x, y in zip(xs, ys):
xg = x * trig.cosd(heading) + y * trig.sind(heading) + acc_dx
yg = -x * trig.sind(heading) + y * trig.cosd(heading) + acc_dy
xs_global.append(xg)
ys_global.append(yg)
return xs_global, ys_global
def get_particle_positions(particles, acc_dx, acc_dy, heading):
"""Return the x and y coordinates of the particles as two parallel lists.
"""
xs = [-p.surface_range * trig.sind(p.hor_angle) for p in particles]
ys = [p.surface_range * trig.cosd(p.hor_angle) for p in particles]
xs_global, ys_global = [], []
for x, y in zip(xs, ys):
xg = x * trig.cosd(heading) + y * trig.sind(heading) + acc_dx
yg = -x * trig.sind(heading) + y * trig.cosd(heading) + acc_dy
xs_global.append(xg)
ys_global.append(yg)
return xs_global, ys_global
def get_particle_positions(particles, acc_dx, acc_dy, heading):
"""Return the x and y coordinates of the particles as two parallel lists.
"""
xs = [-p.surface_range * trig.sind(p.hor_angle) for p in particles]
ys = [p.surface_range * trig.cosd(p.hor_angle) for p in particles]
xs_global, ys_global = [], []
for x, y in zip(xs, ys):
xg = x * trig.cosd(heading) + y * trig.sind(heading) + acc_dx
yg = -x * trig.sind(heading) + y * trig.cosd(heading) + acc_dy
xs_global.append(xg)
ys_global.append(yg)
return xs_global, ys_global
def add_offsets_to_latlons(geo_position, x_particle, y_particle): """Return latitude and longitude after adding x (positive along east) and y (positive along north) offsets in meters to input latitude and longitude. From http://gis.stackexchange.com/questions/2951/algorithm-for-offsetting-a-latitude-longitude-by-some-amount-of-meters """ lat, lon, hdg = geo_position.lat, geo_position.lon, geo_position.heading x_sonar = SONAR_X_RE_GPS * trig.cosd(hdg) + SONAR_Y_RE_GPS * trig.sind(hdg) y_sonar = -SONAR_X_RE_GPS * trig.sind(hdg) + SONAR_Y_RE_GPS * trig.cosd(hdg) x_meters = x_sonar + x_particle y_meters = y_sonar + y_particle lat_out = lat + math.degrees(y_meters / EARTH_RADIUS) lon_out = lon + math.degrees(x_meters / (EARTH_RADIUS * trig.cosd(lat))) return lat_out, lon_out
def compute_sensor_movement(last_position, current_position):
# Calculate how the sensor moved.
course = geo.course(last_position.lat, last_position.lon,
current_position.lat, current_position.lon)
displacement = geo.distance(last_position.lat, last_position.lon,
current_position.lat, current_position.lon)
dx = displacement * trig.sind(course)
dy = displacement * trig.cosd(course)
return dx, dy
def lon_degree_len(lat):
"""Return the length of a degree of longitude at a given latitude.
From http://en.wikipedia.org/wiki/Longitude#Length_of_a_degree_of_longitude
"""
a = 6378137.0
b = 6356752.3142
e = math.sqrt((a**2 - b**2) / a**2)
return ((math.pi * a * trig.cosd(lat)) /
180.0 * (1.0 - e**2 * trig.sind(lat)**2)**(1.0/2.0))
def compute_sensor_movement(last_position, current_position):
# Calculate how the sensor moved.
course = geo.course(
last_position.lat, last_position.lon,
current_position.lat, current_position.lon)
displacement = geo.distance(
last_position.lat, last_position.lon,
current_position.lat, current_position.lon)
dx = displacement * trig.sind(course)
dy = displacement * trig.cosd(course)
return dx, dy
def move(self, last_position, curr_position, e1, n1):
"""Given sensor motion, move the relative location of the particle.
"""
# Calculate how to move the particle relative to the sensor.
target_bearing = last_position.heading - self.hor_angle
# In Cartesian coordinates, let (0, 0) represent the sensor's previous
# position, (e1, n1) represent the sensor's current position, and (e2, n2)
# represent the target's position.
e2 = self.surface_range * trig.sind(target_bearing)
n2 = self.surface_range * trig.cosd(target_bearing)
predicted_target_bearing = trig.atan2d(e2 - e1, n2 - n1) % 360.0
self.hor_angle = (curr_position.heading - predicted_target_bearing +
random.gauss(0, self.angle_noise))
self.surface_range = (math.sqrt((e2 - e1)**2 + (n2 - n1)**2) +
random.gauss(0, self.range_noise))
def move(self, last_position, curr_position, e1, n1):
"""Given sensor motion, move the relative location of the particle.
"""
# Calculate how to move the particle relative to the sensor.
target_bearing = last_position.heading - self.hor_angle
# In Cartesian coordinates, let (0, 0) represent the sensor's previous
# position, (e1, n1) represent the sensor's current position, and (e2, n2)
# represent the target's position.
e2 = self.surface_range * trig.sind(target_bearing)
n2 = self.surface_range * trig.cosd(target_bearing)
predicted_target_bearing = trig.atan2d(e2 - e1, n2 - n1) % 360.0
self.hor_angle = (curr_position.heading - predicted_target_bearing +
random.gauss(0, self.angle_noise))
self.surface_range = (math.sqrt((e2 - e1)**2 + (n2 - n1)**2) +
random.gauss(0, self.range_noise))