Truth System - What Really Happened?

Hamid Mokhtarzadeh -

The design of autonomous systems involves a chain of design iterations. There is a continuum of work from pure simulation to real-time outdoor testing. In order to evaluate the system under design or testing, it is either required or helpful to know the answer to the question:

"What really happened?"

This may seem obvious, but when in the thick of the details, it can easily be overlooked. After all, you are tasked with delivering a working system. Wouldn't you rather be picking sensors, writing code, developing simulations, and designing controllers or estimators? But giving the truth some thought early in the development can provide compounding advantages as a project continues.

Options

There are actually multiple options when it comes to a truth system:

  1. Reference System - an independently developed system with much better accuracy than system under design.
  2. Vehicle Dynamics and Sensor Simulation - use a vehicle simulation to simulate sensor measurements
  3. Test Along Known Trajectory - testing in controlled environment where the vehicle state will be known confidently
  4. Bundle Adjustment - us an optimization routine to find best-fit of vehicle state given multitude of sensor data.

Reference System

This is probably the most expensive option, but has lots of utility. There are two parts to this:

  1. Hardware/Sensors with superior characteristics than you plan to use
    • This data can be compared with the lower-cost sensors you plan to use
  2. Independently-developed sensor fusion software used to derive vehicle state
    • This can be used to define the true vehicle state as well as evaluate any sensor fusion algorithms under development

Notice that the quality of sensor and state information that can serve as the truth is relative to the desired system accuracy. For example, if the system being designed uses hardware that costs less than $$50, then a $1,000 solution from a company like VectorNav may be an acceptable reference system.
In contrast, higher accuracy or reliability requirements may drive you to a reference system like Novatel SPAN, Oxford Technical Solutions, or Geodetics. At this point the prices are likely between $20-50k, of which, only about half is the hardware costs. The other portion is the complexity of the sensor-fusion software as well as the interface for using the system. Do not neglect the importance of user-friendly software. After all, shouldn't the "truth" be easy?

Vehicle Dynamics and Sensor Simulation

Simulation software can go a long ways in the life of a guidance, navigation, and control project.

Unlike other truth systems which measure what really happened, in this case we start with the truth.

The input to such simulation software is the true vehicle trajectory. This includes position, velocity, orientation, as well as any other system states like deterministic sensor errors. The simulation software takes the true state and derives corresponding sensor measurements (e.g. gyro measurements or GPS pseudoranges), each corrupted by the sensor quality specified. Hence, you end with three sets of time-history data:

  1. true vehicle state
  2. (simulated) true or uncorrupted sensor measurements
  3. (simulated) sensor measurements corrupted by noise and other sensor errors

The last two sets of data can be used to evaluate the navigation system under design by comparing the estimated results with the truth vehicle states. All this, without the price tag of a physical reference system. Sounds great, no?

Maybe. I wouldn't discount this option and you may need it regardless of having a physical reference system. However, simulating vehicle dynamics and corresponding sensor measurements can be complex. Either you'll need to purchase existing tools and learn to use them or spend considerable time implementing them yourself. If I was to guess, you will likely find yourself spending considerable time debugging the truth system. Who said this was supposed to be easy!?

Test Along Known Trajectory

The measure-and-iterate no-nonsense engineers, this is for you. Simulate? I've got no time or interest. Pay for the "truth"!? Yeah, right.

Actually this approach is used most commonly:

  • Testing an Attitude and Heading Reference System (AHRS)? Collect data along a straight and level path.
  • Checking a dead reckoning system? Travel a known distance.
  • Validating a short-baseline RTK-GNSS system? Start and end at a known distance from the base station.

With planning and careful consideration the truth will be designed into the experiment. An extension of this would be using a calibration table or another system that can faithfully reproduce a known condition.

The advantage of this approach is that it makes sense, is easy to explain, and has little extra cost involved. The downside is that the tests rarely reflect the final conditions under which the system designed must perform. For example, an AHRS tested at straight and level will not uncover potential issues only observable at non-level angles.

Hence, testing along a known trajectory is often a necessary, but not sufficient, means of system evaluation.

Bundle Adjustment

The system under design may need to work in real-time on an embedded processor. However, during testing we have an abundance of time and processing power. The name bundle adjustment comes from the computer vision community, but here we are using the name to reference a global optimization problem. Data is collected using all sensors available to the system under test: accelerometers, gyros, camera images, GPS measurements, etc... Then a best-fit optimization problem is defined:

Given the time-history of sensor data, find the time-history of vehicle state that minimizes the defined cost function.

The cost function will codify vehicle constraints and inter-state constraints. For example, they will define a relationship between position, velocity, and orientation, or orientation and sequences of camera images. This can be further improved if several knowns are given to the optimization problem. For example, that the vehicle travelled a known total distance.

Setting up and solving such a global optimization problem is not trivial and often will require taking numerical issues into account. But learning the tools and setting up the software to conduct this optimization can pay other dividends for system design. For example, a tool to evaluate the impact of using a subset of the original sensors.