Exemplo n.º 1
0
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
Exemplo n.º 2
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
Exemplo n.º 3
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
Exemplo n.º 4
0
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
Exemplo n.º 5
0
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
Exemplo n.º 6
0
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
Exemplo n.º 7
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
Exemplo n.º 8
0
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))
Exemplo n.º 9
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
Exemplo n.º 10
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))
Exemplo n.º 11
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))