Last time I discussed the challenges when operating the mjbots quad A1 on sloped surfaces. While there are a number of possible means of tackling this, the approach I’ve gone with for now is to estimate the slope of the terrain under the robot, and use that to determine how to position the center of mass. Here’ll I’ll cover the estimation part of this solution.
On paper, the quad A1 has plenty of information to estimate the terrain under its feet. Between the IMU with attitude estimator, the proprioceptive feedback from the joints, and the ability to move the feet around, it would be obvious to a human whether the ground under them was sloped or level. The challenge here is to devise an algorithm to do so, despite the noise in the IMU, the fact that the feet are not always on the ground, and that as the robot moves, the terrain under it changes.
Approach
My basic approach can be summarized in the follow flow chart / block diagram:
First, a brief description of the 3 pertinent reference frames:
B Frame: The body frame (or B frame), is centered on the robot body, and rigidly fixed to the robot body. The proprioceptive system eventually calculates each of the 4 feet positions in this frame.
M Frame: The CoM frame (or M frame), is centered at the robot’s idle center of mass and oriented such that positive Z points along gravity toward the ground with a heading that is arbitrary at start up, but that tracks the robot’s changing heading.
T Frame: The terrain frame (or T frame), is referenced to the M frame at the average height of the legs with a slope that aligns with the average slope of the terrain under the robot.
The algorithm works in roughly the following steps:
First, project all the feet positions into the M frame.
For any in-flight legs, reset the Z value to one calculated from the current TM transform and a 0 T frame Z height.
Fit a plane to these “on-ground” M frame points.
Update the slope of the T frame using this plane with an exponential filter along the X and Y axes.
Properties
This algorithm has the benefit that it will converge on the terrain underneath the robot as long as feet touch the ground with regularity, which is a somewhat necessary condition for a robot supported by its legs. The rate at which the estimate converges can be controlled by the filter constant. Selecting that to be the same order as the step frequency does a decent job of rejecting spurious noise while responding in a timely manner to updated terrain.
Next up we’ll see how to use this information to balance, and watch the results in simulation.
In Part 1 and Part 2, I described why we’re trying to measure localization accuracy, and the properties of a GPS receiver that allows us to do so. In this post, I’ll describe the technique we used to measure accuracy of our solution purely from recorded data, without needing to go back out to the field every time a change was made.
Our solution
The technique we used to measure localization accuracy is somewhat similar to the Allan Variance plots used in part 2. Here, we take a large corpus of pre-recorded sensor data from the vehicle and re-run it through the localization solution. The trick is that for a given time window, the GPS updates are witheld from the filter, then at the end of the window, the difference between the estimated position and the measured GPS position is recorded. The cycle then starts anew at the current time, with the estimate being reset to the best possible one, and GPS denied until the next window end. Each sampled error is one data point showing how far off the localization solution can be after that much time with no GPS.
We expect this to be effective because, as the plots in part 2 showed, over short time windows, the average drift in the GPS is actually pretty small. For instance, the u-blox 6 on savage solder, within a 5s time window, will have drifted only about 0.6m with 95% confidence.
Once the results have been collated for a given time window, say 1s, we repeat the entire process for 2s, then 3s, etc. The curves this produces show how rapidly the position error in localization grows with time. The lower the value is at longer time intervals, that means the vehicle is more robust to GPS outages or drifts.
Results on Savage Solder’s 2013 AVC software
A plot of our 2013 AVC localization solution’s accuracy is shown below. It was measured over about 30 minutes of autonomous driving, mostly recorded in the weeks leading up to the 2013 AVC. I have superimposed on it the 68% and 95% confidence in the u-blox drift for reference. If the localization solution were perfect, we would expect the measured errors to approximately line up with the GPS drift over the same time interval.
Savage Solder AVC 2013 Localization Accuracy
This shows that the accuracy isn’t terrible, but isn’t particularly great either. After 15 seconds, it is off by less than 2m two thirds of the time. However, in order to capture the best 95% of results, we have to look all the way out to 7.5m, which clearly isn’t too usable. For a course like the Sparkfun AVC one, you can roughly say that errors larger than 2 or 3 meters will result in a collision with something. This implies that Savage Solder can run for about 3 to 5 seconds with no GPS and be not terrible.
We have a couple of theories for where the largest sources of error are in the system as shown in the above plot:
Initial heading error: For all of these data sets, the car has only a very rough knowledge of its heading when starting out and all information about the heading comes from GPS. Even a small initial heading error will result in large position errors early in each run.
Total state filter:As described before the localization solution used during 2013 was a total state Kalman filter. I expect that switching to an error state can improve the performance.
Improved inertial sensors: This can’t strictly be tested after the fact, but there now exist easily obtainable higher quality gyroscopes and accelerometers than the Pololu MiniIMU v2 we used in 2013.
Recap of measuring localization accuracy
Looking back at part 1, this technique measures up pretty well. It:
requires only data recorded on the robot, it
provides hard numeric results (within the limits of the GPS’s short term drift), and it
requires no additional sensors
You can tweak the localization algorithms in software as many times as necessary, each time accurately assessing the results, and never once need to go out and actually drive the robot around.
In Part 1, I discussed how measuring the accuracy of a localization solution in a mobile robot is challenging, and some properties an ideal solution would have. This time, I’ll describe some of the properties of the GPS receiver Savage Solder uses, to motivate our mechanism for using it to measure localization accuracy.
Principles
The basic idea behind our approach is that the GPS mounted on Savage Solder, while relatively inaccurate in general, rarely has a very large error. And even when the error is large, it is usually only for a short window of time. Over time, these periods where the GPS has a lot of error come and go semi-randomly, which means that with enough data, they will tend to average out. To see how this works in a little more detail, let’s talk about the major sources of error that a GPS receiver can have.
NASA rendering of GPS satellite
Geometry and clock error: At any given instant, only a subset of the GPS satellites are visible to a receiver, and those that are visible will have a configuration which introduces a source of error due to the process of triangulation. For instance, if all the visible satellites are in the same part of the sky, measuring ranges to the satellites will not tell you much about your absolute position. Secondly, each satellite may have differing errors in their onboard clocks, each of which translates directly in range measurement errors. Both of these error sources change relatively slowly with time.
Ephemeris and atmospheric effects: To estimate its location, a receiver must have precise knowledge of each satellite’s orbit, or ephemeris. While this orbit is known relatively precisely, every centimeter of error directly corresponds to positioning error on the ground. Ephemeris errors typically change slowly over time, as space weather isn’t as drastic as Boston weather. Atmospheric effects have similar properties when visible from the receiver, the ionosphere is the primary factor, as it causes delays in the signals propagating from the satellites to each receiver. Its effects also change relatively slowly with time.
Multipath and obstructions: When the line of sight to a receiver is blocked by a tree, building, person, vehicle, or the horizon, that can cause the signal to weaken enough to mis-register. The receiver can also pick up reflections of the actual satellite signal from any of the above. These reflected signals are called “multi-path”, and they cause the receiver to measure the additional length in the reflected path, instead of the true shortest path. As new satellites become available or are hidden, they can join or fall out of the solution. These errors can change rapidly for ground vehicles, where the line of sight to satellites can rapidly become clear or obstructed as it moves around.
Noise: Each measurement has some amount of random noise associated with it. Consumer receivers typically only measure the code phase, and not the carrier phase, so this measurement noise can be on the order of a meter or so for each satellite. It has mostly high frequency components.
Filtering: In order for the output to look more “reasonable”, most low-cost consumer receivers implement some sort of state estimation filtering before emitting any outputs. This smooths out noise components, and also smooths out rapid changes in multi-path or which satellites are used in the solution. As a result, the final position can often seem smooth, but as a result has more absolute error at any given instant.
Windowed error measurement
To get an idea of the magnitude of each of these error types, we used a technique similar to Allan Variance to see the magnitude of error from the GPS solution in differing time domains. A long recording of reported GPS positions is made while the receiver is stationary. Then, it is divided up first into say consecutive 0.2s windows. Within each window the position is averaged, after which the change between consecutive windows is measured. These deltas represent how much the receiver’s absolute offset has drifted in that time period. For the 0.2s size, you can then see how much the offset changes on average, or how much it changes 95% of the time.
Once you’ve done that for the first window size, you increase the window size, say to 0.3s and repeat the whole process. You keep increasing the window size until you can only fit a few bins into the recorded trace.
What we expect to see is something like the following:
Typical GPS relative error plot
At very high frequencies (short time intervals), the filtering on the receiver renders the errors small. This means that on average, the position doesn’t change much over short intervals. Then, as the time interval gets up into the 5 to 60 minutes range, the error rapidly increases as we see the effects of atmospheric, ephemeris, and multipath errors become realized. Eventually, the error will peak, at a time interval which depends upon what the worst error contributor is. Finally, as the time grows to infinity, we would expect to see the error drop off, as averaging over such large time intervals tends to reveal the zero-drift property of GPS.
We ran this experiment on the u-blox 6 GPS used on Savage Solder and a high quality dual frequency receiver outfitted with Omnistar G2 as a reference. The u-blox was very crudely weatherized for long term outdoor recording with a disposable tupperware container. A recording at full data rate for each GPS was made over about 16 days of operation. Each GPS’s plot shows the median error and the maximum expected error for differing probabilities, which equate to about 1, 2, and 3 sigma on a normal distribution. (The non-weatherized u-blox was tested over a shorter duration and appeared to produce equivalent results.)
Time stability of u-blox 6 with WAAS versus Omnistar G2.
Analysis
The data was taken while stationary on a rooftop with clear 360 degree view of the sky, and thus has best case visibility. Results on an AVC style course will be worse, since multi-path and obstructions will be constantly changing. Despite that, we can get some lower bounds on how good the system could possibly be from these results.
For instance, for a time commensurate with a Sparkfun AVC course run (about 45 seconds for a fast vehicle), the u-blox can be expected to drift around 2.2 meters with 95% confidence. The maximum drift over any interval with 95% confidence is around 3.3m, which implies that it is dicey to survey the course in ahead of time and expect the measurements to be useful. Also, the time required before averaging measurements actually starts to improve stability is pretty long. For the u-blox, it is around 1 hour, and even after looking at an entire day, the stability only gets down to around 24cm.
It is important to note that while the u-blox reports a GPS accuracy metric at any given time, it is usually extremely optimistic. For most of the above trace, the accuracy was reported as about 0.5m with a 1 sigma probability, when the measured absolute 1 sigma accuracy was clearly around 2m or more.
As a reference, the Omnistar G2 trace shows that yes, its performance is about 2 orders of magnitude better than the low-cost u-blox receiver. In these near-ideal conditions, it has a 95% confident maximum error of around 12cm, which means that it could be viable for hitting the hoop and ramp. However, as this is in ideal conditions, shading and multipath from the course, spectators, and other vehicles will certainly make actual results even worse.
Using this
In the next post, I’ll show how we used this knowledge of our GPS receiver’s error properties to measure the quality of our localization solution over short to medium time intervals.
One of the challenges in developing a localization solution for a mobile robot is knowing when you are making things better versus making things worse. We felt this acutely when developing Savage Solder for the Sparkfun AVC event, especially as we were having intermittent GPS issues. Specifically, we are interested in knowing how well we can localize in the absence of GPS, or with poor GPS quality. In this series I’ll describe a new technique we’re using to make the process go a little faster as well as have a better understanding of just how accurate we are.
As a short refresher, the localization software in a mobile robot incorporates measurements from the robot’s sensors, and produces an estimate of where the robot is in one or more reference frames. Each of the sensors has errors that cause them to report non-ideal values. There can be many types of error, and the magnitudes of error can be relatively large. If a solution is good, it will report an accurate position even when the sensors aren’t so good.
The “simple” way
A simple technique is make a change, let the car drive the course, and visually observe how close it was to the path that was planned. Lather, rinse, and repeat. Once you get into it, this technique breaks down rapidly:
“How good is it” is very subjective: As the car drives an entire course, it can be hard to remember exactly how close it was at every point. You also can’t get much in the way of numerical results, just a gut feel.
Visual observation incorporates many more error sources than localization: Trajectory tracking errors, as well as remote sensor positioning errors (if the vehicle is avoiding observed obstacles) can all cause the robot to drive not where you expect, but still know accurately where it is.
Time: One of the easiest ways to waste a lot of time developing a robot is to drive it in real life after any software change. This causes development to slow to a crawl.
Recordkeeping: Somewhat a corollary of the first item, once you have tried a few techniques, and a few constant tunings for each technique, unless your recordkeeping is meticulous, you’ll rapidly lose track of how each variant performed.
Canceling errors: One common variant is to measure where the vehicle stops at the end of a run. Many sources of error can cancel each other out going in opposite directions, so sometimes this can over-state the actual accuracy of a localization solution mid-run.
It isn’t hard to see how the “simple” technique for improving a localization isn’t so simple after all.
What would be better?
Any solution you may want to use should have the following properties:
Operate on recorded data: By working on purely recorded data, this means you can iterate rapidly, and even tune constants in a script if need be.
Consistent metric: A solution should provide a hard number or set of numbers that describe the quality of the localization solution.
Onboard sensors only: While it is possible to use an auxiliary, higher quality positioning solution than could otherwise be placed on the robot, it is preferable if only the onboard sensors are required.
Next steps…
In the next post, I’ll begin developing the technique we’re using to measure localization accuracy and how it measures up to these properties.
In part 1 and part 2, I covered the details of the localization solution we use on Savage Solder. To finish off, I’ll look at the areas we are exploring for future improvement.
u-blox 6 Position Errors
While in motion, our u-blox GPS can occasionally slew to an erroneous position that may be 20m away from ground truth, then slew back 20s later, even while reporting WAAS corrections, good HDOP and a large number of satellites. These slews of course confuse the localization algorithm, as they do not match the system dynamics at all. The estimation filter’s heading and position can diverge during these events.
One possible indication of these events can be found in the pseudo-range residuals the u-blox reports. The GPS receiver operates by measuring “pseudo-ranges” to each of the satellites it is tracking in the sky. In general, there are more ranges than necessary to determine both the receiver’s position and its clock bias at any point in time. Thus for any given position the receiver reports, there will be some error between what it thinks the range should be to each satellite, and what the actual measured pseudo-range was. This is reported as the “pseudo-range residual” via the NMEA GPGRS message.
During these error slew events, we have identified that the u-blox does generally report one or more of the residuals growing very large (sometimes more than 100m), then shrinking back down as the position corrects itself. This is even as it reports the same approximately constant horizontal position accuracy! For future competitions, such as Sparkfun AVC, we’re experimenting with using these residual measurements to de-weight GPS readings which are suspect. I’ve also put together the below video to describe the problem in more detail:
Error State Filter
The Kalman filter configuration we use now is a total state filter. In a total state localization filter, quantities such as the yaw rate are estimated as part of the filter, in addition to their error parameters. This makes the filter formulation straightforward, but results in an effective low pass filter applied on the yaw rate, as it is only updated through the Kalman filter measurement update step. Because of this filtering, in high dynamic maneuvers the heading can suffer from accuracy problems until the GPS is able to correct matters.
One alternate filter configuration, the “error-state filter”, estimates the error between a forward integrated solution and reality rather than estimating the solution directly. In this formulation, the error model parameters of the yaw rate gyro are included as states, but the yaw rate itself is not. This removes the low-pass filtering on the yaw rate, potentially improving filter response during fast maneuvering.
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
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.
Savage Solder Localization Sensors
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
Kalman Filter Tuning Tradeoffs
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.
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.
Inaccurate Localization
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.
Block diagram of Savage Solder’s localization system
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.
Options for Savage Solder’s localization estimation filter
Next Installment
In the next installment, I’ll look in more detail at the measurements we use, and the states that we estimate.
Last time I covered the techniques we use on Savage Solder to pick out orange traffic cones from webcam images in the “Cone Detector”. This time, I’ll look at the next stage in that pipeline, turning the range and bearing reported by the cone tracker into Cartesian coordinate estimates of where any given cone is.
Cone Tracker Overview
As mentioned last time, our “Cone Tracker” takes as input the range and bearing, (along with their uncertainties), from the cone detector. It also receives the current navigation state of the car. This includes things like the current estimate of geodetic position (latitude and longitude), current map position, (UTM easting and northing), and an unreferenced local coordinate system (x, y). For each of these, it reports the vehicle’s speed and heading.
I won’t go into the details of each of these coordinate systems here, but since the cone tracker actually only uses the local one, I can discuss it a bit. The local coordinate system on Savage Solder starts out at x,y=(0m,0m) with a heading of 0 degrees at the beginning of every run. As time progresses, the heading and position are updated primarily based on our onboard dead reckoning sensors. The local coordinate system is smooth, and never jumps for GPS updates. As a consequence, it isn’t really useful to compare positions in it from more than a minute or two apart, nor is it useful to do GPS based navigation. Fortunately, for cone tracking, from the time to when we see the cone to when we touch it is usually only a couple of seconds total over which time the local solution is very accurate.
Now, let’s cover some broad details of the implementation. The guts of our cone tracker consists of a bank of identical Kalman Filters. Each Kalman filter estimates the position of one particular cone and the error in that estimate. This lets the car keep around several possible cones that could be in sight at one time while still distinguishing them. By storing the cones in a local coordinate system, it allows for easy driving towards, or alongside, the cone and accurate speed control leading up to it. The position uncertainty could be used to control behavior, but we don’t bother in our current implementation.
Cone Tracker Internals
New and Old Cones
Additionally, the tracker has to handle seeing new cones for the first time, and discarding cones that maybe were false detections in the cone detector. It does this by assigning each cone a “likelihood”, which is just a probability that the cone is a true detection. When data arrives that match the cone well, its likelihood is increased. When the available data doesn’t match the cone very well, or no cone is observed at all when one is expected, the likelihood is decreased.
If a range and bearing arrive which corresponds to no known cones, a new one is created with a relatively low likelihood. Then once it has reached a certain threshold, it is reported to the outside world as a valid target. Conversely, when an existing cone’s likelihood reaches a level which is too low, it is removed entirely on the thesis that it was actually a false detection to begin with.
More specifically, the likelihood update is handled using Bayes theorem. We have an empirically derived table showing, for our detector, the odds that a cone will be detected or a false one will be seen at various ranges. These are used to fill in the various probabilities in the equations.
Incorporating New Measurements
A “measurement” in this problem is simply the range and bearing that the cone detector reports. To incorporate a new measurement, the tracker looks through all the cones currently in its filter bank. For each, it computes a measure of the likelihood that the given cone could produce the current measurement. This is done using what’s called the Mahalanobis distance. The Mahalanobis distance is just a measure of how far away you are from an uncertain target in a multi-dimensional space.
If the best matching cone has a Mahalanobis distance small enough to count as valid, then the standard Kalman filter update equation is applied to that cone. If no cones match, then we create a new one as described above.
Scale Factor
Cone Tracker Flowchart
One final detail, is that in addition to estimating the position of each cone, the tracker also estimates its “scale” as seen by the cone detector. The image based detector we use has the property that the range values are likely to have a fixed scale error for a number of reasons. One, the cones could actually be bigger or smaller than the regulation sized ones. Second, lighting conditions can sometimes cause a fraction of the cone to be not detected, which will result in the cone being seen as smaller than it actually is.
Since the range values are not correct, the position will be similarly off. This error isn’t as bad as it seems, since we (and most every RoboMagellan entrant), approach the cone in a straight line. Thus the position errors will always be directly towards or away from the robot, and as long as you keep moving, you’ll hit it eventually.
Savage Solder has two reasons to care. First, is that we decelerate from a high speed to an appropriate “bumping” speed with just a half meter to spare. Thus, if the cone is estimated as too close, we run the risk of colliding with it at an unsafe speed and damaging our front end. Secondly, we have a mode where the car can just “clip” the cone by driving alongside it and tapping it with a protruding stick. This allows us to avoid stopping entirely when the geometry allows it. However, if the position estimate is incorrect here, we might miss the cone entirely.
Final Process
To summarize, the cone tracker handles each new range and bearing with the following steps:
A cone detection results in a new range, bearing, and uncertainty measurement.
Existing cones in the Kalman filter bank are checked to see what the likelihood is each could have produced this measurement.
If one was likely enough, then:
The position and scale factor are updated using the Kalman filter equations.
The likelihood estimate is updated according to our current false positive and false negative estimates.
If none was likely enough, then a new cone is created with a position fixed at the exact position implied by the local navigation solution and the range and bearing.
Any cones which we had expected to see but didn’t have their likelihood decreased.
Any cones which have too low of a likelihood are removed.
Finally, all the cones which are close to the vehicle, and have a high enough likelihood are reported to the high level logic.
Caveats and Next Steps
One of the challenges with this approach is that there are a lot of constants to tune. I’ll cover the details in a later post, but for most of them we tried to find some way to empirically measure reasonable values for our application. Another problem was debugging. printf doesn’t cut it when you have this much geometry. For that, we created a number of custom debugging and visualization tools which help show what is going on in the system, some of which I’ll cover later too.
A Modicum of Fun 