def export_kml(self, name, tx_id):
    # TODO I've changed some stuff ... make sure this output is still sensible. 
  
    # E.g.: https://developers.google.com/kml/documentation/kmlreference#gxtrack 
    # TODO The file is way longer than it needs to be, since I wanted to display
    # the coordinates and datetime in the tooltip that appears in Google Earth.
    # Perhaps what we want is not a gx:track, but something fucnctionally 
    # similar.

    # TODO Add northing, easting to output.
    try:
      import utm
    except ImportError:
      print "function export_kml() requires \"utm\" library, please install"
      raise

    fd = open('%s_track.kml' % name, 'w')
    fd.write('<?xml version="1.0" encoding="UTF-8"?>\n')
    fd.write('<kml xmlns="http://www.opengis.net/kml/2.2"\n')
    fd.write(' xmlns:gx="http://www.google.com/kml/ext/2.2">\n')
    fd.write('<Folder>\n')
    fd.write('  <Placemark>\n')
    fd.write('    <name>%s (deploymentID=%d)</name>\n' % (name, tx_id))
    fd.write('    <gx:Track>\n')
    for (pos_id, dep_id, t, easting, northing, utm_number, utm_letter, ll, activity) in self.table: 
      tm = time.gmtime(t)
      t = '%04d-%02d-%02dT%02d:%02d:%02dZ' % (tm.tm_year, tm.tm_mon, tm.tm_mday,
                                              tm.tm_hour, tm.tm_min, tm.tm_sec)
      fd.write('      <when>%s</when>\n' % t)
    for (pos_id, dep_id, t, easting, northing, utm_number, utm_letter, ll, activity) in self.table: 
      (lat, lon) = utm.to_latlon(easting, northing, utm_number, utm_letter) 
      fd.write('      <gx:coord>%f %f 0</gx:coord>\n' % (lon, lat))
    fd.write('      <ExtendedData>\n')
    fd.write('        <SchemaData schemaUrl="#schema">\n')
    fd.write('          <gx:SimpleArrayData name="Time">\n')
    for (pos_id, dep_id, t, easting, northing, utm_number, utm_letter, ll, activity) in self.table: 
      tm = time.gmtime(t)
      t = '%04d-%02d-%02d %02d:%02d:%02d' % (tm.tm_year, tm.tm_mon, tm.tm_mday,
                                              tm.tm_hour, tm.tm_min, tm.tm_sec)
      fd.write('          <gx:value>%s</gx:value>\n' % t)
    fd.write('          </gx:SimpleArrayData>\n')
    fd.write('          <gx:SimpleArrayData name="(lat, long)">\n')
    for (pos_id, dep_id, t, easting, northing, utm_number, utm_letter, ll, activity) in self.table: 
      (lat, lon) = utm.to_latlon(easting, northing, utm_number, utm_letter) 
      fd.write('          <gx:value>%fN, %fW</gx:value>\n' % (lat, lon))
    fd.write('          </gx:SimpleArrayData>\n')
    fd.write('          <gx:SimpleArrayData name="positionID">\n')
    for (pos_id, dep_id, t, easting, northing, utm_number, utm_letter, ll, activity) in self.table: 
      tm = time.gmtime(t)
      t = '%04d-%02d-%02d %02d:%02d:%02d' % (tm.tm_year, tm.tm_mon, tm.tm_mday,
                                              tm.tm_hour, tm.tm_min, tm.tm_sec)
      fd.write('          <gx:value>%d</gx:value>\n' % pos_id)
    fd.write('          </gx:SimpleArrayData>\n')
    fd.write('        </SchemaData>\n')
    fd.write('      </ExtendedData>\n')
    fd.write('    </gx:Track>\n')
    fd.write('  </Placemark>\n')
    fd.write('</Folder>\n')
    fd.write('</kml>')
    fd.close() 

    def test_utm_latlon_conversion(self):
        "check utm to latlon conversion of Dry Creek actually maps there"
        # bbox corners from upper left clockwise around
        east0 = 569029.6
        east1 = 569452.1
        north0 = 4842544.9
        north1 = 4842177.4

        utm_zone = 11
        utm_letter = 'T'

        latlons = utm2latlon(bsamp=east0, bline=north0, dsamp=east1-east0,
                             dline=north1-north0, nsamp=2, nline=2,
                             utm_zone=utm_zone, utm_letter=utm_letter)

        from utm import to_latlon
        ll0 = to_latlon(east0, north0, utm_zone, utm_letter)
        ll1 = to_latlon(east1, north0, utm_zone, utm_letter)
        ll2 = to_latlon(east1, north1, utm_zone, utm_letter)
        ll3 = to_latlon(east0, north1, utm_zone, utm_letter)

        assert (ll0 == latlons[0]).all
        assert (ll1 == latlons[1]).all
        assert (ll2 == latlons[2]).all
        assert (ll3 == latlons[3]).all

    def get_LOS_vector(self, locations):
        """
            calculate beta - the angle at earth center between reference point
            and satellite nadir
        """

        utmZone = self.location_UTM_zone
        refPoint = vmath.Vector3(self.ref[0], self.ref[1], 0)
        satAltitude = self.satellite_altitude
        satAzimuth = self.satellite_azimuth
        satIncidence = self.ref_incidence
        earthRadius = self.local_earth_radius

        DEG2RAD = np.pi / 180.
        alpha = satIncidence * DEG2RAD
        beta = (earthRadius / (satAltitude + earthRadius)) * np.sin(alpha)
        beta = alpha - np.arcsin(beta)
        beta = beta / DEG2RAD

        # calculate angular separation of (x,y) from satellite track passing
        # through (origx, origy) with azimuth satAzimuth

        # Long lat **NOT** lat long
        origy, origx = utm.to_latlon(
            refPoint.x, refPoint.y, np.abs(utmZone), northern=utmZone > 0
        )

        xy = np.array([
            utm.to_latlon(u[0], u[1], np.abs(utmZone), northern=utmZone > 0)
            for u in locations
        ])
        y = xy[:, 0]
        x = xy[:, 1]

        angdist = self._ang_to_gc(x, y, origx, origy, satAzimuth)

        # calculate beta2, the angle at earth center between roaming point and
        # satellite nadir track, assuming right-looking satellite

        beta2 = beta - angdist
        beta2 = beta2 * DEG2RAD

        # calculate alpha2, the new incidence angle

        alpha2 = np.sin(beta2) / (
            np.cos(beta2) - (earthRadius / (earthRadius + satAltitude))
        )
        alpha2 = np.arctan(alpha2)
        alpha2 = alpha2 / DEG2RAD

        # calculate pointing vector

        satIncidence = 90 - alpha2
        satAzimuth = 360 - satAzimuth

        los_x = -np.cos(satAzimuth * DEG2RAD) * np.cos(satIncidence * DEG2RAD)
        los_y = -np.sin(satAzimuth * DEG2RAD) * np.cos(satIncidence * DEG2RAD)
        los_z = np.sin(satIncidence * DEG2RAD)

        return vmath.Vector3(los_x, los_y, los_z)

def distance(UTM_ini, UTM_fin):
    """
    distance returns the calculated distance (in kms) between two points
    defined in the UTM coordinate system
    """

    UTM_ini_x = UTM_ini[0]
    UTM_ini_y = UTM_ini[1]
    UTM_ini_zone = UTM_ini[2]

    UTM_fin_x = UTM_fin[0]
    UTM_fin_y = UTM_fin[1]
    UTM_fin_zone = UTM_fin[2]

    [LAT_INI, LONG_INI] = utm.to_latlon(
        UTM_ini_x, UTM_ini_y, int(UTM_ini_zone[0:2]), str(UTM_ini_zone[3])
    )  # to get dd.dd from utm
    [LAT_FIN, LONG_FIN] = utm.to_latlon(
        UTM_fin_x, UTM_fin_y, int(UTM_fin_zone[0:2]), str(UTM_fin_zone[3])
    )  # to get dd.dd from utm

    point_i = (LAT_INI, LONG_INI)
    point_f = (LAT_FIN, LONG_FIN)

    distance = great_circle(
        point_i, point_f
    ).kilometers  # gives you a distance (in kms) between two coordinate in dd.dd

    return distance

def to_gps(x = 0.0, y = 0.0):
    #print x, y, type(x), type(y)
    if x < 400000:
        lat, lon = utm.to_latlon(x, y, 51,'R')
        return lon, lat
    elif x > 400000:
        lat, lon = utm.to_latlon(x, y, 16,'T')
        return lon, lat

    def test_to_latlon(self):
        '''to_latlon should give known result with known input'''
        for latlon, utm, utm_kw in self.known_values:
            result = UTM.to_latlon(*utm)
            self.assert_latlon_equal(latlon, result)

            result = UTM.to_latlon(*utm[0:3], **utm_kw)
            self.assert_latlon_equal(latlon, result)

    def test_to_latlon_range_checks(self):
        '''to_latlon should fail with out-of-bounds input'''
        self.assertRaises(UTM.OutOfRangeError, UTM.to_latlon, 0, 5000000, 32, 'U')
        self.assertRaises(UTM.OutOfRangeError, UTM.to_latlon, 99999, 5000000, 32, 'U')
        for i in range(100000, 999999, 1000):
            UTM.to_latlon(i, 5000000, 32, 'U')
        self.assertRaises(UTM.OutOfRangeError, UTM.to_latlon, 1000000, 5000000, 32, 'U')
        self.assertRaises(UTM.OutOfRangeError, UTM.to_latlon, 100000000000, 5000000, 32, 'U')

        self.assertRaises(UTM.OutOfRangeError, UTM.to_latlon, 500000, -100000, 32, 'U')
        self.assertRaises(UTM.OutOfRangeError, UTM.to_latlon, 500000, -1, 32, 'U')
        for i in range(10, 10000000, 1000):
            UTM.to_latlon(500000, i, 32, 'U')
        self.assertRaises(UTM.OutOfRangeError, UTM.to_latlon, 500000, 10000001, 32, 'U')
        self.assertRaises(UTM.OutOfRangeError, UTM.to_latlon, 500000, 50000000, 32, 'U')

        self.assertRaises(UTM.OutOfRangeError, UTM.to_latlon, 500000, 5000000, 0, 'U')
        for i in range(1, 60):
            UTM.to_latlon(500000, 5000000, i, 'U')
        self.assertRaises(UTM.OutOfRangeError, UTM.to_latlon, 500000, 5000000, 61, 'U')
        self.assertRaises(UTM.OutOfRangeError, UTM.to_latlon, 500000, 5000000, 1000, 'U')

        self.assertRaises(UTM.OutOfRangeError, UTM.to_latlon, 500000, 5000000, 32, 'A')
        self.assertRaises(UTM.OutOfRangeError, UTM.to_latlon, 500000, 5000000, 32, 'B')
        self.assertRaises(UTM.OutOfRangeError, UTM.to_latlon, 500000, 5000000, 32, 'I')
        self.assertRaises(UTM.OutOfRangeError, UTM.to_latlon, 500000, 5000000, 32, 'O')
        for i in range(ord('C'), ord('X')):
            i = chr(i)
            if i != 'I' and i != 'O':
                UTM.to_latlon(500000, 5000000, 32, i)
        self.assertRaises(UTM.OutOfRangeError, UTM.to_latlon, 500000, 5000000, 32, 'Y')
        self.assertRaises(UTM.OutOfRangeError, UTM.to_latlon, 500000, 5000000, 32, 'Z')

def utm_to_latlong(input_data_file=None, output_data_file=None, log_file=None, log_level=DEFAULT_LOG_LEVEL):
    """Converts UTM coordinates into latitude/longitude.
    assumes rows are easting, northing, zone number, either 'N' for northern
    hemisphere or 'S' for southern hemisphere
    """
    logger = logger_message(__name__, log_file, log_level)

    # Check required input and output data file names were given.
    assert input_data_file is not None, 'An input CSV file with columns of values.'
    assert output_data_file is not None, 'An output CSV file to write new values.'

    _in = open(input_data_file, 'r')
    try:
        _out = open(output_data_file, 'w')
        try:
            data = csv.reader(_in)
            output = csv.writer(_out)
            for row_ind, row in enumerate(data):
                east = float(row[0])
                north = float(row[1])
                zone = int(row[2])

                latlong = utm.to_latlon(east, north, zone, northern=('N' == row[3]))
                logger.info('Changed row {} from: {}  to: {}'.format(row_ind,
                                                                     (row[0], row[1]), latlong))

                output.writerow(latlong)
        finally:
            _out.close()
    finally:
        _in.close()

def get_data(path='data/Historical Marker_20150521_145030_254.csv'):
    with open(path, 'r') as fp:
        reader = csv.DictReader(fp)
        for row in reader:
            text = row['markertext']
            years = find_years(text)
            row['address'] = row['address'].strip()

            # add our own data
            row['years'] = years
            row['classifications'] = Classifier(years, text=text).classify()
            try:
                lat, lon = utm.to_latlon(
                    int(row['utm_east']),
                    int(row['utm_north']),
                    int(row['utm_zone']),
                    northern=True,
                )
                row['location'] = {
                    "lat": lat,
                    "lon": lon,
                }
            except ValueError:
                # log warn missing
                pass
            yield row

  def insert_db(self, db_con, dep_id, bearing_ids, zone):
    ''' Insert position and bearings into the database. 
    
      Inputs: 
        
        db_con, dep_id, zone

        bearing_ids -- bearingIDs (serial identifiers in the database) of the bearings
                       corresponding to bearing data. These are used for provenance 
                       in the database. 
    ''' 
    if self.p is None: 
      return None
    number, letter = zone
    lat, lon = utm.to_latlon(self.p.imag, self.p.real, number, letter)
    cur = db_con.cursor()
    cur.execute('''INSERT INTO position
                     (deploymentID, timestamp, latitude, longitude, easting, northing, 
                      utm_zone_number, utm_zone_letter, likelihood, 
                      activity, number_est_used)
                   VALUES (%s, %s, %s, %s, %s, %s, %s, %s, %s, %s, %s)''',
                     (dep_id, self.t, round(lat,6), round(lon,6),
                      self.p.imag, self.p.real, number, letter, 
                      self.likelihood, self.activity,
                      self.num_est))
    pos_id = cur.lastrowid
    handle_provenance_insertion(cur, {'bearing': tuple(bearing_ids)}, 
                                     {'position' : (pos_id,)})
    return pos_id

def ChangeXYView(request):
    if request.method == "POST":
        form = ChangeXYForm(request.POST)
        if form.is_valid():
            obs = Occurrence.objects.get(pk=request.POST["DB_id"])
            coordinates = utm.to_latlon(int(request.POST["new_easting"]), int(request.POST["new_northing"]), 37, "N")
            pnt = GEOSGeometry("POINT (" + str(coordinates[1]) + " " + str(coordinates[0]) + ")", 4326)  # WKT
            obs.geom = pnt
            obs.save()
            messages.add_message(request, messages.INFO, 'Successfully Updated Coordinates For %s.' % obs.catalog_number)
            return redirect("/admin/mlp/occurrence")
    else:
        selected = list(request.GET.get("ids", "").split(","))
        if len(selected) > 1:
            messages.error(request, "You can't change the coordinates of multiple points at once.")
            return redirect("/admin/mlp/occurrence")
        selected_object = Occurrence.objects.get(pk=int(selected[0]))
        initial_data = {"DB_id": selected_object.id,
                        "barcode": selected_object.barcode,
                        "old_easting": selected_object.easting,
                        "old_northing": selected_object.northing,
                        "item_scientific_name": selected_object.item_scientific_name,
                        "item_description": selected_object.item_description
                        }
        the_form = ChangeXYForm(initial=initial_data)
        return render_to_response('projects/changeXY.html', {"theForm": the_form}, RequestContext(request))

def sync_gis():
  request_cookie = requests.get(app.config['GIS_URL_COOKIE'])
  request_data = requests.get(app.config['GIS_URL_DATA'], cookies=request_cookie.cookies)
  data = request_data.json()['features']
  for dataset in data:
    if Tree.query.filter_by(external_id=dataset['attributes']['OBJECTID']).count() == 0:
      print "Add new Dataset with external ID %s" % dataset['attributes']['OBJECTID']
      new_tree = Tree()
      new_tree.external_id = dataset['attributes']['OBJECTID']
      lat_lng = utm.to_latlon(dataset['geometry']['x'], dataset['geometry']['y'], 32, 'U')
      new_tree.lat = lat_lng[0]
      new_tree.lng = lat_lng[1]
      if len(dataset['attributes']['ADRESSE'].strip()):
        new_tree.address = dataset['attributes']['ADRESSE']
      if len(dataset['attributes'][u'GEH\xf6lZFL\xe4c'].strip()):
        new_tree.tree_type_old = dataset['attributes'][u'GEH\xf6lZFL\xe4c']
      if len(dataset['attributes'][u'F\xe4lLGRUND'].strip()):
        new_tree.chop_reason = dataset['attributes'][u'F\xe4lLGRUND']
      descr = ''
      if len(dataset['attributes'][u'F\xe4lLUNG'].strip()):
        descr += u'Zeitpunkt der Fällung: ' + dataset['attributes'][u'F\xe4lLUNG'].strip()
      new_tree.descr = descr
      new_tree.created_at = datetime.datetime.now()
      new_tree.updated_at = datetime.datetime.now()
      new_tree.author = 'Stadt Bochum'
      new_tree.email = '[email protected]'
      new_tree.city = 'Bochum'
      new_tree.public = 1
      new_tree.postalcode = ''
      new_tree.type = 4
      new_tree.source = 'Stadt Bochum'
      db.session.add(new_tree)
      db.session.commit()

 def rectilinear(self, nx, ny):
     """
     function that generates a rectilinear grid
     """
     #### Step 1) define a regular grid ####
     #NW = (29.017842, -95.174746)
     NW = (29.036487, -95.131658)
     #NE = (29.459124, -94.413252)
     NE = (29.439981, -94.465266)
     #SW = (28.777523, -94.979306)
     SW = (28.807300, -95.005893)
     
     #### Calculate the length in x- and y- direction ####
     Lx = self.distance_on_unit_sphere(NW[0], NW[1], NE[0], NE[1])
     Ly = self.distance_on_unit_sphere(NW[0], NW[1], SW[0], SW[1])
     
     new_NW = utm.from_latlon(NW[0], NW[1])[0:2]
     #new_SW = (new_NW[0]-Ly, new_NW[1])
     #new_NE = (new_NW[0], new_NW[1]+Lx)
     
     y = np.linspace(new_NW[1]-Ly, new_NW[1], ny)
     x = np.linspace(new_NW[0], new_NW[0]+Lx, nx)
     
     xv, yv = np.meshgrid(x, y)        
     #origin = (xv[0,-1], yv[0,-1])
     origin = new_NW
     
     tem_xv = xv - origin[0]
     tem_yv = yv - origin[1]
     
     #### Step 2) rotate the grid from an angle ####                    
     def rotate(yv, xv, theta):
         """Rotates the given polygon which consists of corners represented as (x,y),
         around the ORIGIN, clock-wise, theta degrees"""
         theta = math.radians(theta)
         out_yv = np.zeros_like(yv)
         out_xv = np.zeros_like(xv)
         (nx, ny) = xv.shape
         for i in range(nx):
             for j in range(ny):
                 out_yv[i,j] = yv[i,j]*math.cos(theta)-xv[i,j]*math.sin(theta)
                 out_xv[i,j] = yv[i,j]*math.sin(theta)+xv[i,j]*math.cos(theta)
         
         return out_yv, out_xv
         
     tem_yv, tem_xv = rotate(tem_yv, tem_xv, -35)
     
     new_xv = tem_xv + origin[0]   #lon 
     new_yv = tem_yv + origin[1]   #lat
     
     lon = np.zeros_like(new_xv)
     lat = np.zeros_like(new_yv)
     #pdb.set_trace()
     for i in range(ny):
         for j in range(nx):
             (lat[i,j], lon[i,j]) = utm.to_latlon(new_xv[i,j], new_yv[i,j],15,'U')[0:2]
     
     dx = Lx / nx
     dy = Ly / ny                  
     return lon, lat, dx, dy   

    def ProcessLocalization(self, msg):
        """Process Localization, stat mileages and save driving path."""
        localization = LocalizationEstimate()
        localization.ParseFromString(msg)
        timestamp = localization.header.timestamp_sec
        cur_pos = localization.pose.position

        # Stat mileages.
        if (self._last_position is not None and
            self._current_driving_mode is not None):
            driving_mode = Chassis.DrivingMode.Name(self._current_driving_mode)
            meters = utm_distance_meters(self._last_position, cur_pos)
            if driving_mode in self.record.stat.mileages:
                self.record.stat.mileages[driving_mode] += meters
            else:
                self.record.stat.mileages[driving_mode] = meters

        # Sample driving path.
        G = gflags.FLAGS
        if (self._last_position_sampled is None or
            (timestamp - self._last_position_sampled_time >
                 G.pos_sample_min_duration and
             utm_distance_meters(self._last_position_sampled, cur_pos) >
                 G.pos_sample_min_distance)):
            self._last_position_sampled = cur_pos
            self._last_position_sampled_time = timestamp
            lat, lon = utm.to_latlon(cur_pos.x, cur_pos.y,
                                     G.utm_zone_id, G.utm_zone_letter)
            self.record.stat.driving_path.add(lat=lat, lon=lon)
        # Update position.
        self._last_position = cur_pos

def local_to_global(origin, x, y, theta=0.0):
    """
    Converts from a local frame in meters into a global frame in lon/lat.
    Note that heading is in degrees!

    @param origin (lon, lat, heading)
    @param point shapely.geometry.Point() in local frame
    @return (longitude, latitude, heading) in WGS84
    """
    # Create local transformation frame.
    import utm
    ox, oy, zone, hemi = utm.from_latlon(origin[1], origin[0])
    o_theta = (90 - origin[2]) * (math.pi/180.0)

    # Translate and rotate point to origin.
    point = shapely.geometry.Point(x, y)
    point = shapely.affinity.rotate(point, o_theta,
                                    origin=ORIGIN, use_radians=True)
    point = shapely.affinity.translate(point, ox, oy)
    p_theta = theta + o_theta
    heading = 90 - (p_theta * 180.0/math.pi)

    # Return transformed point.
    lat, lon = utm.to_latlon(point.x, point.y, zone, hemi)
    return lon, lat, heading

def derive_taz_attributes(cur, list_taz):
    
    description_taz=[]
    for taz_id in list_taz:
        
        s='select ST_AsText(ST_Centroid(TAZ.geom)) from taz_boundary_scag_2009 TAZ where TAZ.gid = '+str(taz_id)+';'
        cur.execute(s)
        rows=cur.fetchall()
        for row in rows:
            pos_centroid_UTM = (str(row[0]))
            pos_centroid_UTM = pos_centroid_UTM[6:len(pos_centroid_UTM)-1]
            pos_centroid_UTM = pos_centroid_UTM.split()
            pos_centroid_UTM = map(float, pos_centroid_UTM)
            lat_centroid, lng_centroid = utm.to_latlon(pos_centroid_UTM[0], pos_centroid_UTM[1] , 11, 'N') #The shapefiles are in the UTM "11N" coordinates
    
        if is_in_LA_Box(lat_centroid, lng_centroid):
            s='select count(*) from taz_boundary_scag_2009 TAZ, la_network_nodes_v2 nodes where TAZ.gid = '+str(taz_id)+' AND ST_Contains(TAZ.geom, nodes.geom);'
            cur.execute(s)
            rows=cur.fetchall()
            for row in rows:
                nb_nodes_in_taz = int(str(row[0]))
            s='select TAZ.area_ from taz_boundary_scag_2009 TAZ where TAZ.gid = '+str(taz_id)+';'
            cur.execute(s)
            rows=cur.fetchall()
            for row in rows:
                area_taz = float(str(row[0]))
        
            description_taz.append([taz_id, lat_centroid, lng_centroid, nb_nodes_in_taz, area_taz])
            
    return description_taz

def start_simulator_spin():

    rospy.init_node("Controller_Tester", anonymous=True)
    simulator_pub = rospy.Publisher('Pose', Pose, queue_size=10)
    c_tester = ControllerTester()
    command_listener = rospy.Subscriber("Command", Command, c_tester.command_callback)

    #initialize()


    while not rospy.is_shutdown():
        #apply the model
        #use np.dot(a, b) for matrix product of two 2d-lists a and b
        #print(model)
        c_tester.x = np.dot(c_tester.model, c_tester.x)

        #publish a new pose message using simulator_pub
        simulated_pose = Pose()

        #lat_long is a 2-tuple
        #for syntax of the utm package see python package index
        #The syntax is utm.to_latlon(EASTING, NORTHING, ZONE NUMBER, ZONE LETTER).
        #The return has the form (LATITUDE, LONGITUDE).
        
        lat_long = utm.to_latlon(c_tester.x[0][0], c_tester.x[1][0], c_tester.utm_zone_number, c_tester.utm_zone_letter)
        simulated_pose.latitude_deg = lat_long[0]
        simulated_pose.longitude_deg = lat_long[1]
        simulated_pose.heading_rad = c_tester.x[3][0]

        simulator_pub.publish(simulated_pose)
        c_tester.updateModel(c_tester.x)
        #rospy.spin()

        c_tester.rate.sleep()

def calc_list( l_points, EP, func_fretch ):
    if not calc_list_precheck(l_points, func_fretch):
        return None
    pr, level = calc_list_core(l_points, EP, func_fretch)
    if pr:
        latitude, longitude = utm.to_latlon(pr.x, pr.y, pr.zone, pr.mark)
        return latitude, longitude, pr.r, level
    return None

    def eval(self, values, X):
        if self.tnci_time != self.t:    
            self.tnci.set_time(self.t)
            self.tnci_time = self.t

        latlon = utm.to_latlon(X[0], X[1], utm_zone, utm_band)
        # OTPS has lon, lat coordinates!
        values[0] = self.tnci.get_val((latlon[1], latlon[0]), allow_extrapolation=True)

  def convert_to_lonlat(self,zone_number,northern=False):
    import utm
    lon=np.zeros(self.x.size,dtype=self.x.dtype)
    lat=np.zeros(self.x.size,dtype=self.x.dtype)
    for i in range(self.x.size):
      lat[i],lon[i]=utm.to_latlon(self.x[i], self.y[i], zone_number=zone_number, northern=northern)

    self.lon=lon
    self.lat=lat