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 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))
