Starred: Not in GPS folder (but commonly used by GPS VI’s)

Blue: Commonly used typedefs (all located in LabVIEW  Common 

TypeDefs)
Red: VI contains errors

RepositoryCASELabVIEWCommonGPS
*GPS Coordinate.ctl
A class for keeping GPS coordinates

*Line Object.ctl
A class of lines defined by two GPS coordinates

*deg to rad.vi
rad = pi/180 * deg, mod 2pi

*DegreesToRadians.vi
Unlike deg to rad.vi, output not necessarily between 0 and 2pi. Used by
IsLineCrossed.vi

*rad to deg.vi
rad * 180/pi, then coerce to 360

Angles to Intersection Entrances.vi

Input: Description of target, including GPS coordinates and Waypoint ID;
numeric control.
Output: GPS coordinates of intersection entrance; same numeric as input;
difference of headings.
Method: Unbundles Target cluster (error lies here) for array of Relevant
Intersection Points, and takes the entry at the index of the entered
numeric. Unbundles that entry for the Intersection Entrance GPS. Gets
the GPS difference of headings between that and the readings from the
physical state observer.
Direct Sub-VI’s: Read Physical State.vi; GPS_Difference of Headings.vi;
Heading_From_GPS_Location_Pair.vi

Build Clockwise Intersection Polygon.vi

Input: Description of target; size.
Output: Clockwise intersection array of GPS coordinates.
Method: Not working since Angles to Intersection Entrances.vi has error. For
size “n” many times, takes the Angles to Intersection Entrances.vi output,
sorts and reorders by angle headings, producing a new array of
intersection GPS coordinates in the clockwise order,
starting with the physical state GPS coordinate.
Direct Sub-VI’s: Angles to Intersection Entrances.vi; Read Physical State.vi

Calculate GPS Scaling Factor.vi

Input: Current GPS Location
Output: Latitude scaling factor; longitude scaling factor
Method: Using differential increments (dx/d phi, dy/d phi), "returns the number
of meters in one degree of latitude and longitude"
Direct Sub-VI’s: Exact GPS Point Distance.vi

Closest GPS Endpoint.vi

Input: GPS coordinates of physical state; GPS point 1; GPS point 2
Output: Closest GPS endpoint
Method: Calculates distance in meters between physical state and point 1,
physical state and point 2. Takes the minimum of the 2 distances.
Calculates distance in meters between physical state and midpoint. If that
is less than the previous minimum, outputs GPS of midpoint; otherwise
outputs GPS of the closer of the 2 points.
Direct Sub-VI’s: Distance Between GPS Coordinates in Meters.vi; GPS
Midpoint.vi

Closest GPS in Array to Given GPS Point.vi

Input: GPS point to match; array of GPS points
Output: Closest distance; index of match; GPS coordinates of match
Method: Calculates distances between GPS points, then finds the coordinate in
the array of GPS coordinates that is closest to the coordinate to match.
Direct Sub-VI’s: Distance Between GPS Coordinates in Meters.vi

Coerce Heading to Range.vi

Input: Heading In (degrees)
Output: Coerced Heading (0 to 360)
Method: If < 0, add 360 until > 0 --> if >360, subtract 360 until < 360 but check
again for > 0.

Convert GPS to Local Vehicle Coordinate Frame.vi

Input: Current GPS point, target GPS point, heading (degrees)
Output: Distance ahead and lateral offset, in meters. Positive distance ahead
means target is ahead of vehicle, positive lateral offset means target is to
the left of vehicle, along current heading
Method: Uses "Head_From_GPS_Location_Pair.vi" to calculate heading from
current point to target, and "Distance Between GPS Coordinates in
Meters.vi" to calculate distance between current point and target.
Subtracts calculated heading from input heading, changes it from degrees
to radians. Computes sine and cosine (radians) of the difference;
multiplies sine and cosine by the distance between coordinates to get
lateral distance (positive means target is to the left of vehicle) and
distance ahead (positive means target is ahead of vehicle). Headings: 0 is
north, 90 is east, etc.
Direct Sub-VI’s: Heading_From_GPS_Location_Pair.vi; Distance Between GPS
Coordinates in Meters.vi; deg to rad.vi

Coordinate Matrix Meters to GPS.vi

Input: Meters matrix; physical state
Output: GPS matrix
Method: Given a 4x4 coordinate matrix in meters units, converts it to the same
value in GPS coordinates, based on the physical state observations. The
Z translation component is lost when doing this.
Direct Sub-VI’s: Y-X to Lat-Long.vi

Distance Between GPS Coordinates In Meters.vi

Input: GPS coordinates for 2 points (degrees)
Output: Distance between GPS coordinates in meters
Method: Uses scaling factors and Pythagorean Theorem to give distance
between two points in meters.

Exact GPS Point Distance.vi

Input: GPS of point 1; GPS of point 2
Output: Radius of earth in meters (indicator), distance between points 1 and 2 in
meters
Method: Uses mean of the latitudes as input for Get Earth Radius.vi, outputs
Earth radius. Uses geometry of the curvature of the earth (geodesics?) to
find shortest distance between the two points, in km, then converts to m.
For long distances, harder than Pythagorean, apparently "not to be used
often"
Direct Sub-VI’s: Get Earth Radius.vi; deg to rad.vi

Find GPS Coordinate On Line That Is Given Distance From GPS Location.vi
(See revised version of VI)
Get Earth Radius.vi
Input: Latitude
Output: Radius of the earth at given latitude (in kilometers)
Method: Uses math formula since equatorial radius and polar radius are
different; since earth is not a perfect sphere...

Get GPS Box Corners.vi

Input: GPS coordinate; Heading; Distance forward; Distance backwards; Box
half width
Output: Array of 4 box corner points
Method: Ultimately creates an array of GPS coordinates for the 4 corner points.
Uses "GPS Location from Given Position.vi" with current location,
heading, and distance forward for top 2 points; heading + 180 degrees
and distance backward for bottom 2 points. Then uses "GPS Location
from Given Position.vi" on the new positions with heading + 90 for left,
heading - 90 for right, and box half-width as the new distance traveled.
Direct Sub-VI’s: GPS Location from Given Position.vi"

GPS Component Distance.vi

Input: GPS coordinate of current position; target heading; GPS target
Output: Returns the distance in meters ahead of the target along the given
heading from the current position and the lateral offset. Positive "Ahead"
indicates that the target is ahead of the current position along the current
heading. Positive "Lateral' indicates that the position is to the left of the
target
Method: Calculates the angle heading for the GPS target, subtracts target
heading, coerces to 0-360 degree range, converts to radians. Calculates
distance between GPS coordinate and target in meters. Sine angle *
distance = lateral, cosine angle * distance = ahead.
Direct Sub-VI’s: Heading_From_GPS_Location_Pair.vi; Distance Between GPS
Coordinates in Meters.vi; Coerce Heading to Range.vi; deg to rad.vi

GPS Coordinates to X-Y Coordinates.vi

Input: Array of GPS points; GPS coordinate of origin
Output: Array of X-Y coordinates (in meters) of points with respect to given
origin
Method: Multiplies difference between latitudes and distance between
longitudes by GPS scale factor.
Direct Sub-VI’s: GPS Get Scale Factors.vi

GPS Distance (scale in).vi

Input: GPS coordinates of two points; latitude scale; longitude scale
Output: Distance in meters between points
Method: Returns the distance in meters between two passed in GPS points.
There is a computationally expensive calibration routine that goes on here
when large changes in GPS position occur, so the latitude and longitude
scales are used

GPS Get Scale Factors.vi

Input: GPS coordinates for point
Output: Latitude scale, longitude scale (meters/degree)
Method: Apparently the scaling for Cleveland…don’t have the logic behind it…
Direct Sub-VI: Calculate GPS Scaling Factor.vi

GPS Location From Given Position.vi

Input: Current GPS; Heading in polar degrees; Distance in meters
Output: New GPS location after traveling a specified distance in a specified
direction (0 degrees = North, positive change is clockwise.)
Method: Calculates dY and dX from heading and distance, scales from meters
to latitude and longitude, adds this to the current location.
Direct Sub-VI’s: Map_Angle-Heading_Converter.vi; GPS Get Scale Factors.vi

GPS Midpoint.vi
Input: Two GPS coordinates
Output: GPS coordinate of midpoint
Method: Takes GPS coordinates of two points and averages the latitudes and
longitudes (midpoint rule)

GPS time between two GPS points.vi

Input: GPS coordinates for A and B, speed
Output: Distance between A and B in meters, time needed to travel distance
Method: Uses "Distance Between GPS Coordinates In Meters.vi" to give
distance; time = distance/velocity
Direct Sub-VI: Distance Between GPS Coordinates in Meters.vi

GPS Average of Headings (radians).vi

Input: Headings (in radians) of points A and B
Output: Average of headings (in radians)
Method: Makes an array with A-B, A-B+2pi, A-B-2pi; first part exactly like with
"GPS Difference of Headings.vi." Then divides difference by 2, adds to the
subtracted heading (B) and mods by 2pi, giving the average of A and B.
This handles wrap-around, giving the smallest angle between A and B.
GPS Difference of Headings.vi
Input: Headings of points A and B (headings are 0 to 360)
Output: Difference in headings (smallest angle between 0 and 360 degrees)
Method: Using arrays, takes the absolute values of A-B, A-B+360, A-B-360 and
returns the minimum (the latter 2 cases handle wrap around/obtuse
angles) Note: Why take the index of the min, then look up index? Why not
directly return minimum using Array Max & Min function?

Harlie local to GPS coordinates.vi

Input: X-Y coordinate
Output: GPS coordinate; heading
Method: Takes in robot's current GPS and heading, and an X-Y location along
that heading in Harlie's local frame. Uses rotation of axes to a zero
heading to get new local coordinates. Then uses GPS scale factors to
convert the local coordinates to GPS, and calculates the new heading
toward that GPS point.
Direct Sub-VI’s: Physical_State_read.vi; GPS Get Scale Factors.vi;
Heading_From_GPS_Location_Pair.vi

Heading_From_GPS_Location_Pair.vi
Input: GPS Coordinate A, GPS Coordinate B
Output: Returns heading for direction A faces to reach B (0 to 360); North would
be 0, East 90, etc (also dy, dx, and GPS of temp point)
Method: Temporary point takes the latitude of A and the longitude of B. Gets
the distance in meters between the temporary point and A, temporary
point and B. If longitude of B is greater than that of A, dx is the former
distance, else negative the former distance. If latitude of B is greater than
that of A, dy is the latter distance, else negative the latter distance. Uses
the inverse tangent function to compute heading in radians, then converts
to degrees, then converts to polar heading and returns result.
Direct Sub-VI’s: Map_Angle-Heading_Converter.vi; Distance Between GPS
Coordinates in Meters.vi

Is A Ahead of B.vi
Input: GPS coordinates of A and B, heading of B(?)
Output: Boolean true or false
Method: Use "Heading_From_GPS_Location_Pair.vi" to get the heading from B
to A. Change from degrees to radians. Take the sine and cosine of both
headings. Think of the sines and cosines as x- and y-components as the
entries of 2-dimensional vectors, for the given heading and the calculated
heading. We can think of the second vector as the vector from B to A.
 We then multiply the sines and cosines together, then sum them up, aka
taking the dot product of the two vectors. If the dot product is positive,
then the angle between the two vectors is between 0 and 90, or between
270 and 360, then the dot product is positive (and geometrically, the
vectors point in roughly the same direction), so point A is ahead of B
Direct Sub-VI: Heading_From_GPS_Location_Pair.vi

Is GPS Point Inside Box.vi

Input: Method; Test point; Top left corner; Top right corner; Bottom left corner;
Bottom right corner
Output: Inside the test point inside the quadrilateral specified by the four GPS
coordinates? (True or false)
Method: User chooses from “Exact Box” or “Bounding Box”. “Bounding Box”
checks that the test point is between the max and min latitudes of the four
corners, and max and min longitudes of the four corners. “Exact Box” gets
the four sides of the box using a corner point and a heading for each line,
and checks that the test point has not crossed any of the lines.
Direct Sub-VI’s: Heading_From_GPS_Location_Pair.vi; IsLineCrossed.vi

Is GPS Point Inside Region.vi

Input: GPS point array; test point
Output: Inside region specified by point array? (True or false)
Method: Returns true if the passed-in GPS point is inside a region bounded by
the passed-in GPS point array. Points must be passed in clockwise order.
Makes sure there are at least three boundary points. Gets heading from
array point to next array point and adds 90 degrees. Setting the new
angle as the heading for the former array point, checks if passed-in point
is ahead of the array point. If so, the passed-in point is to the right of the
approximating polygon side connecting the two array points. Continue this
for all the array points, so that all the polygon sides are checked, or until
the passed-in point is outside a line.
Direct Sub-VI’s: Heading_From_GPS_Location_Pair.vi; Is A Ahead of B.vi

Is Line Inside Region.vi

Input: Line object; GPS point
Output: Inside region (True/False)
Method: Uses “Is Line Inside Region_Robust.vi”. Returns true if the passed-in
line intersects the region formed by the GPS points or is entirely inside the
region. Points must be passed in clockwise order.
Direct Sub-VI’s: Is Line Inside Region_Robust.vi

IsLineCrossed.vi
Input: GPS coordinate; Line Crossed Query (line is determined by GPS location
of a point on the line and a compass direction from -180 degrees to 180
degrees, with 0 degrees as north, > 0 as east, < 0 as west)
Output: Boolean (true for crossed, false for not crossed)
Note: Here angles are between -180 and 180 degrees, compared to 0 and 360 in
other VI's, which could be potentially confusing

Linear Fit from 2 GPS.vi

Input: GPS coordinate of point 1 and point 2
Output: Slope m and intercept b of the linear fit y=mx+b. If slope m is inf, returns
x-intercept instead
Method: Calculate (lat1-lat2) and (long1-long2). Using case structure, if (long1-
long2) aka x1-x2 is equal to 0, then slope = inf and intercept = long2 = x2.
Otherwise, slope = (lat1 - lat2)/(long1 - long2) and b = y2 - m*x2

Map_Angle-Heading_Converter.vi
Input: HeadDegIn, AnglDegIn
Output: AngDegOut, HeadDegOut
Method: Polar Heading --> Cartesian Angle: 360 - ((phi mod 360)+270) = 90 –
phi mod 360
(if < 0, 450 - phi mod 360)
Cartesian Angle --> Polar Heading: 90 - phi mod 360
(if < 0, 450 - phi mod 360)

Meters to GPS Coordinates (Given Origin).vi

Input: Array X-Y coordinates, GPS coordinate of origin
Output: Array of GPS coordinates
Method: Takes an array of X-Y coordinates of points and the GPS coordinate of
the origin. Divides the X (east is +) and Y (north is +) distances by the
GPS scale factor to get latitudinal and longitudinal offsets. Adds offsets to
GPS coordinate of origin to get array of converted GPS coordinates.
Direct Sub-VI’s: GPS Get Scale Factors.vi

Move NE.vi
Input: Distance north, distance east, GPS coordinate of cluster
Output: Latitude and longitude after move
Method: Uses "GPS Location From Given Position.vi" to find latitude and
longitude of cluster after moving NE. First uses sub-vi on cluster with
distance north and heading 0, then plugs in new coordinates for cluster with
distance east and heading 90, then unbundles coordinates to
output the latitude and longitude separately
Point to Line Distance.vi
Input: GPS of a point, GPS of two points determining line
Output: Distance to segment, distance to infinite line
Method: Math looks correct

Straight Line Interpolate.vi

Input: GPS of start point, GPS of end point, number of points (including
endpoints)
Output: GPS path
Method: Calculates distance and heading from start to end point with sub VI's.
Divides the overall distance by (number of points - 1) to get
the increment distance. For loop uses "GPS location from Given
Location.vi" with start point, heading, and change in distance starting at 0
and adding another increment distance each n-1 iteration of the loop (the
shift register adds an increment distance each iteration).
Returns the coordinates from the starting point (index 0) to the last point
before the end (for n-1 points)

RepositoryCASELabVIEWPlanner
Best_Path_Given_GPS_Location.vi
Converting a mapped area to a graph (in the math sense) representation: Each
intersection of roads is a “node,” and each segment of road between two nodes
constitutes an “edge.” The length of an edge is the “weight” of the edge.
Input: GPS point of Harlie; target GPS; number of nodes; adjacency matrix
(Format: Entry a_ij is the length of the path from node i to node j. If nodes
i and j are not connected, enter a very large number unlikely to be
confused with a real distance to serve as “infinity.” Also note that for
regular non-directed graphs, the matrix will be symmetric.);
edge table array
(Format: Entry a_i contains the initial and terminal nodes describing the
edge, the length/”weight” of the edge, and the GPS coordinates of
waypoints on the edge, including the two node endpoints).
Output: Closest edge and waypoint to Harlie (within threshold); the two nodal
endpoints of said edge; closest edge and waypoint to GPS target (within
threshold); the two nodal endpoints of said edge; array of nodes on the
shortest path from Harlie to target
Method: Looks up closest edges/waypoints to Harlie and target. For both Harlie
 and the target, there are two nodal endpoints for their incident edges. In
both cases, the best path could take either node, so there are 2*2=4
choices. Once the nodes are chosen, then Dijkstra’s algorithm can be
used for the best node-to-node path. Thus the total length of the path is
Harlie-to-Node + Node-to-Node+Node-to-Target, and the shortest of the
four choices is chosen as the best path.
Direct Sub-VI’s: Distance Between GPS Coordinates in Meters.vi;
Dijkstra_Algorithm_for_Node_to_Node.vi; Closest_Edge_Given_
GPS_Location.vi

Dijkstra_Algorithm_for_Node_to_Node.vi
Input: Number of nodes; source node; target node; adjacency matrix; edge table
array
Output: Value table of shortest distances from source to all nodes; shortest
node-to-node path from source node to target node.
Method: Uses Dijkstra’s algorithm to find the shortest path (see pseudo-code on
Wikipedia). Makes use of three main arrays during operation: “Distance
List,” which is updated each iteration to produce the final value table;
“Previous Vertex List,” which is updated each iteration to produce the list
of immediate predecessor nodes for each node, based on its optimal path
from the source node; and the “List of Remaining Nodes,” which is
initialized the same as the distance list and updated the same as the
Distance List each iteration, but for the node to be deleted, which is
reassigned the value of infinity, and when all nodes are set to infinity, all
nodes have been optimized.

Closest_Edge_Given_GPS_Location.vi
Input: GPS point; edge table array
Output: Edge and waypoint closest to GPS point, within a two meter radius of
the given point; the two nodes describing said edge.
Method: Running outer loop, for each edge, looks up its waypoints. Running
inner loop, for each waypoint on the edge, get distance in meters from
GPS point; keep value and edge/waypoint information if distance is
smaller than two meters and smaller than that of previous waypoint. End
of the loops will result in optimal distance, unless there is no waypoint
closer than two meters, in which case the VI returns an error.
Direct Sub-VI: Distance Between GPS Coordinates in Meters.vi