In part 1 I looked at the reasons why we use a state estimation filter for Savage Solder’s localization system and what the options were. This time, I’ll look at the specific measurements we take and the resulting states that we estimate along with some implementation details.

Measurements

Our localization system uses the following measurements:

• GPS: UTM (Universal Transverse Mercator) referenced position from a u-blox 6 GPS sampled at 4Hz
• Gyroscope: We use the yaw rate from a Pololu MiniIMU-9 sampled at 50Hz
• Odometer: We feed the sensors from our sensored brushless motor to an AVR microcontroller and count motor steps to measure odometry at 50Hz. This is not perfect, (for example, it suffers from backlash), but works reasonably well.

Each of these measurements is integrated into the state estimation filter at their natural rate. The filter only emits a solution on each IMU update, just to reduce the variability in the rest of the system.

Estimated States

The Unscented Kalman Filter (UKF) estimates the following system states:

• X/Y Position: The position is estimated in a UTM coordinate reference. We nominally estimate where a position under the center of the rear axle is, but the car is small enough that lever arms don’t really come into consideration.
• Velocity Velocity is estimated assuming no lateral slip.
• Heading: We assume that the car is always perfectly flat, and that the only orientation degree of freedom which matters is heading.
• Curvature: We assume that the car moves in a car like way, and thus, given no change in steering input, it will drive in a circle.
• Odometer Scale Factor: The only error term we estimate for the odometer is a scale factor. This is largely determined by errors in the diameter of the tires, but also is affected by the texture of the terrain that is being traversed.
• Gyroscope Bias: Gyroscopes have many error factors which can be modeled. For our purposes, since we have accurate GPS most of the time, we get away with only estimating the bias of the gyroscope.

Constant Selection and Heuristics

When designing a state estimation filter, there are a lot of constants that need to be selected. For Kalman filters, you need an initial covariance and process noise for all estimated states, and measurement noise for all dimensions of the measured values. By choosing a specific selection of constants, you can trade off between the bandwidth (performance or accuracy) of the filter, and its stability. Generally, if you make the filter more accurate, you increase the risk that the filter will become unstable if the system model or measurement models do not accurately track the real world.

For Savage Solder, we used a largely ad-hoc approach, with no rigorous backing for our constant selection. We picked process noises that roughly corresponded to the amount of system error we expected to see, and did the same with measurement noises. One exception was GPS measurement error. Since we are using a naive total state filter, and GPS measurements from cheap receivers are highly time correlated (because of their own internal estimation filters), there are a number of other factors to consider. If the GPS measurement is set too high in this formulation, heading cannot be estimated accurately. If it is set too low, the global position will jump around a lot with every GPS update. In no case is the filter’s assumption, that of a measurement corrupted purely by white noise, satisfied, so measurements noises that are too low also run the risk of filter divergence. We just picked a happy medium that tends to work well in our simulations and practice.

There are also a fair number of heuristics included. For instance, if we know the car is stopped from the odometer, we artificially fix the heading and curvature estimates and covariances. At least when it is driving under its own power, Savage Solder rarely rotates in place. Without holding these fixed, noise in the GPS and gyroscope can cause the estimated heading to drift with no bounds and the heading uncertainty to grow arbitrarily large.

Global and Local Coordinate System

As I mentioned early in part 1, different subsystems in Savage Solder have differing requirements for the state estimates they consume. Some, like global driving, want the estimate to be close to the globally correct position so that the car can drive close to a pre-surveyed path. Others, like cone tracking, would much rather have a locally consistent frame of reference that is divorced from global coordinates entirely.

We solve this by deriving a separate local coordinate estimate from the global estimate emitted by the UKF. This local estimate always starts at position 0, 0 with a heading of 0. At each time step, the velocity, heading, and curvature of the global solution are integrated forward. This results in an estimate which is by definition smooth, yet still is relatively accurate over short distances, as the velocity and heading inputs have full access to the sensor suite of the car.

Coming Up…

In the next, and final installment, I’ll look at some techniques we are, or want to be exploring to improve the localization solution on Savage Solder.

Today was the first day Savage Solder was firing on all cylinders in our Sparkfun AVC 2013 practice. We ran a pretty large number of circuits, and all systems seemed to be working pretty well, missing the hoop and ramp only a couple of times. Here is a video of a flawless run through the course at 11mph.

When it isn’t crashing into trash cans, Savage Solder uses a combination of several sensors to form accurate estimates about where it is in the world. This series describes the sensors and techniques that it uses to identify where it is in the world, providing data that is useful for the driving and cone tracking subsystems.

Principles

First, why does the car need to know where it is? Several other subsystems in Savage Solder rely on having accurate estimates of where the car is, measured in different ways. First, when driving to try and find a cone, it follows a path surveyed by GPS. In that case, the driving algorithm needs to know where the car is relative to the pre-surveyed path in order to select the appropriate steering and throttle commands. Here, absolute accuracy is most useful. We rely on surveyed paths to avoid obstacles, and the driving algorithm is relatively insensitive to position estimates that move around as GPS measurements change.

When moving in proximity to cones, the car needs a locally stable coordinate system in order to identify where the cone is relative to the car itself. For this, an estimate that is smooth is most important, as the car’s position relative to a cone can only change in a continuous manner.

For both of these cases, the localization subsystem on Savage Solder takes measurements from various sensors, and produces an estimate of the current position of the car. This estimate is updated on a regular basis, and fed to each other subsystem which needs to know where the car is.

Estimation

At the top level, Savage Solder uses a total-state unscented Kalman filter to incorporate measurements and produce localization estimates. Kalman filters are a class of algorithms which, given a series of noisy measurements of a system, estimate what the internal state variables the system are. In the localization problem, the noisy measurements are things like a GPS reading, a gyroscope measurement, or the value from an odometer, while the estimated state is perhaps the latitude and longitude of the vehicle along with its heading, heading rate and speed.

The Unscented Kalman filter is just one type of estimation algorithm. Traditionally, a designer would select it over other non-linear estimators when higher accuracy is required. As compared to estimators like the Extended Kalman Filter, it achieves this higher accuracy through an increase in computational cost.

For Savage Solder, our PC based computer platform has power to spare, so we chose the unscented Kalman filter (UKF) for a different reason. We used the UKF because it only requires numerical system models. Most non-linear Kalman filters, like the EKF, require analytical derivations of the partial derivatives of the system and measurement functions. These can be involved to create. When you want to change your design, and modify the estimated states or measurement variables, the process is slow and error prone. The UKF only requires a numerical definition of the system and measurement functions themselves. Because of this, we can experiment with different filter structures much more rapidly.

Next Installment

In the next installment, I’ll look in more detail at the measurements we use, and the states that we estimate.

We got in our first day of autonomous practice this weekend for the Sparkfun AVC 2013. Some simple issues gave us some problems in the morning, but by the end of the day we had enough either resolved or worked around enough of them that we were achieving pretty good runs on a practice course. I’ve got video of our best run below, but first a couple of caveats:

• We have a debugging creep at the start of the run to verify heading.
• Our practice hoop wasn’t on the course. We don’t yet have a visual detector for the hoop, or our “hoop deflector” mounted on the car.
• There is a debugging pause after it slaloms through the barrels, before it moves on to the ramp.

Enjoy!

Today we took Savage Solder out to take some calibration image data of the various Sparkfun AVC obstacles and to experiment with manual navigation through an AVC style course. Unfortunately, we had some electronics failures with our replacement ESC that will take some repair. Before that though, we managed to collect a fair amount of useful data, and did a few jumps off the ramp. While not impossible, it looks to be pretty challenging. For small cars, the end of the ramp is actually pretty high. For large cars, the ramp is surprisingly narrow and was not trivial to hit when driving manually. This video is of the slowest run we dared do, higher speeds made better landings.