GPS/INS/Wheel CountGPS/INS/Wheel Count

Sensor Acquisition and FusionSensor Acquisition and Fusion

What do GPS, INS, and Wheel What do GPS, INS, and Wheel Counts do?Counts do?

Report and record the position of the Report and record the position of the robotrobot• Position relative to a starting position for Position relative to a starting position for

INS and wheel count.INS and wheel count.• GPS gives location in Latitude and GPS gives location in Latitude and

LongitudeLongitude

Why do we need three methods to Why do we need three methods to determine position?determine position?

All three systems use different All three systems use different methods to calculate location.methods to calculate location.

By looking at all three, a better By looking at all three, a better approximation of position can be approximation of position can be established.established.

RedundancyRedundancy The weaknesses of each system can The weaknesses of each system can

be offset by the other systems.be offset by the other systems.

Sensor Function OverviewSensor Function Overview

GPS: Triangulates position by GPS: Triangulates position by sending signals to a satellite array. sending signals to a satellite array. Time to receive signals from different Time to receive signals from different satellites allows calculation of satellites allows calculation of position.position.• Strengths: gives latittude/longitude Strengths: gives latittude/longitude

readings anywhere in the world. No readings anywhere in the world. No drifting of measurement.drifting of measurement.

• Weaknesses: inaccuracies, outages Weaknesses: inaccuracies, outages

Sensor Function OverviewSensor Function Overview

INS (Inertial Navigation System) uses a set INS (Inertial Navigation System) uses a set of rate gyroscopes and accelerometers of rate gyroscopes and accelerometers mounted on each of the three axis. mounted on each of the three axis. • Strengths: fast refresh, accurate values, fairly Strengths: fast refresh, accurate values, fairly

reliable with no outages.reliable with no outages.• Weaknesses: drifts over timeWeaknesses: drifts over time

Wheel Count measures the number of Wheel Count measures the number of rotations of each wheel.rotations of each wheel.• Strengths: Works consistently, simple systemStrengths: Works consistently, simple system• Weaknesses: No way to measure wheel slip. Weaknesses: No way to measure wheel slip.

Modeling System Clock to GPS TimeModeling System Clock to GPS Time

GPS has relatively large GPS has relatively large update timeupdate time

Unacceptable for Speed Unacceptable for Speed based GPS estimatesbased GPS estimates

dt(GPS) = loop_time * adt(GPS) = loop_time * a

A = A = 30.179407176330.1794071763

Modeling Dead Reckoning Angle to Modeling Dead Reckoning Angle to Gyroscope Angle Gyroscope Angle

Both initialize to –pi/2Both initialize to –pi/2

Differences are related Differences are related to wheel slipto wheel slip

Simple degrees to Simple degrees to radians conversionradians conversion

Implementation Implementation requires infrequent requires infrequent linkinglinking

Wheel speed to GPS speedWheel speed to GPS speed Sporadic GPS speedSporadic GPS speed

Conversion based on Conversion based on experimental dataexperimental data

GPS speed = GPS speed = wheel_speed*bwheel_speed*b

b =.628734687354b =.628734687354

(X, Y) Coordinates to (X, Y) Coordinates to GPS Longitude and LatitudeGPS Longitude and Latitude

latlen = m1 + (m2.*cos(2.*gps_lat))+ latlen = m1 + (m2.*cos(2.*gps_lat))+ (m3.*cos(4.*gps_lat)) + (m3.*cos(4.*gps_lat)) + (m4.*cos(6.*gps_lat))(m4.*cos(6.*gps_lat))

longlen = (p1 * cos(gps_lat)) + longlen = (p1 * cos(gps_lat)) + (p2.*cos(3.*gps_lat)) + (p2.*cos(3.*gps_lat)) + (p3.*cos(5.*gps_lat))(p3.*cos(5.*gps_lat))

Two sets of (x,y) coordinatesTwo sets of (x,y) coordinates• (x,y) and (x+dx/dt,y+dy/dt)(x,y) and (x+dx/dt,y+dy/dt)

Difference between points as a vectorDifference between points as a vector

Rotate Vector to match GPS angleRotate Vector to match GPS angle• New_angle = original angle-New_angle = original angle-

GPS_angle_offsetGPS_angle_offset

Convert from meters to degreesConvert from meters to degrees

Add to current GPS latitude and Add to current GPS latitude and longitude to get new valuelongitude to get new value

Inertial Navigation System Inertial Navigation System Implementation (ins_conv.c)Implementation (ins_conv.c)

get_gps_est(double est_long, double est_lat, double get_gps_est(double est_long, double est_lat, double est_speed, est_speed, double est_angle, double est_loop_time, int double est_angle, double est_loop_time, int sys_num, double x, double y,double link_offset_angle, sys_num, double x, double y,double link_offset_angle, rstatus r)rstatus r)

• System I: GPS onlySystem I: GPS only• System II: Dead Reckoning to GPSSystem II: Dead Reckoning to GPS • System III: Wheel Speed and System III: Wheel Speed and

Gyroscope to GPSGyroscope to GPS• System IV: GPS Speed and System IV: GPS Speed and

Gyroscope to GPSGyroscope to GPS link_gyro_angle_to_gpslink_gyro_angle_to_gps

Sensor FusionSensor Fusion

By combining all sensor inputs and By combining all sensor inputs and finding “average”, weaknesses of finding “average”, weaknesses of each system can be factored out.each system can be factored out.

Kalman filter is the most common Kalman filter is the most common method to integrate this sensor data.method to integrate this sensor data.

Kalman FilterKalman Filter

A digital filter A digital filter which uses a recursive which uses a recursive process to determine a process to determine a least-squares fit to data least-squares fit to data obtained over time. obtained over time. MIMO, filter for noisy MIMO, filter for noisy sensor data.sensor data.

Works on a Works on a theory of states with theory of states with uncertainties added in uncertainties added in at each recursion. The at each recursion. The uncertainties and uncertainties and weights are updated weights are updated and improved during and improved during each cycle.each cycle.

Source: http://www.cs.unc.edu/~welch/kalman/Levy1997/fig6.htmThe Kalman Filter: Navigation's Integration Workhorse by Larry J Levy

Where would we go from here?Where would we go from here?

Add full INS system to robot.Add full INS system to robot. Implement real-time Kalman filter Implement real-time Kalman filter

including full INS and GPS data.including full INS and GPS data.