Dead reckoning with a Kalman filter#

One-axis dead reckoning with several accelerometers and a multi-rate Kalman filter.

A robot moves along a line with an unknown, smoothly changing acceleration \(a(t)\) (an Ornstein-Uhlenbeck process). Each Sensor reports \(a + b_k + n\) at its own rate, where \(b_k\) is a random-walk bias. A Kalman filter with state \((p, v, a, b_1, \dots, b_K)\) fuses whichever sensors are in use and, optionally, occasional noisy position fixes (a landmark, a wheel-odometry reset). Integrating a biased acceleration twice makes position error grow, so what matters is how well the filter can estimate the bias: a slow, almost drift-free sensor can calibrate a fast, drifting one. compare_sensor_suites() runs the same simulated trajectories through several sensor suites and reports position error, so the benefit of the quantum sensor (and where it vanishes) can be measured instead of assumed.

The filter is the standard linear Kalman filter with the true noise parameters (a matched filter, an upper bound on what a real tuned filter gets). The simulation is one axis and illustrative; it is not a navigation system.

Classes

DeadReckoningData

A simulated trajectory with the measurements of every sensor.

Functions

simulate_dead_reckoning

Simulate a trajectory and the readings of every sensor.

run_filter

Run the multi-rate Kalman filter on a simulated trajectory.

compare_sensor_suites

Run the same simulated trajectories through several sensor suites.
class DeadReckoningData(dt, pos, accel, readings, fixes, sensors, accel_std, accel_tau, fix_period, fix_std)[source]#

Bases: object

A simulated trajectory with the measurements of every sensor.

Variables:

  • dt (float)

  • pos (numpy.ndarray) – True position at every step.

  • accel (numpy.ndarray) – True acceleration at every step.

  • readings (dict) – {sensor name: array} of what the sensor outputs at every step (only the sampled steps are used).

  • fixes (numpy.ndarray) – Noisy position at every step (used only at the fix instants).

  • sensors (list of Sensor)

  • fix_std (accel_std, accel_tau, fix_period,) – Trajectory and fix parameters, needed by the filter.

Parameters:
simulate_dead_reckoning(sensors, duration=300.0, dt=0.01, accel_std=0.5, accel_tau=2.0, fix_period=None, fix_std=0.1, seed=0)[source]#

Simulate a trajectory and the readings of every sensor.

Parameters:

  • sensors (list of Sensor)

  • duration (float, optional) – Seconds.

  • dt (float, optional) – Time step; must be at most the shortest sensor period.

  • accel_std (float, optional) – Standard deviation (m/s^2) and correlation time (s) of the true acceleration.

  • accel_tau (float, optional) – Standard deviation (m/s^2) and correlation time (s) of the true acceleration.

  • fix_period (float, optional) – Seconds between position fixes (None for none).

  • fix_std (float, optional) – Standard deviation of a position fix in metres.

  • seed (int, optional)

Returns:

DeadReckoningData

run_filter(data, use=None, use_fixes=False)[source]#

Run the multi-rate Kalman filter on a simulated trajectory.

Parameters:

  • data (DeadReckoningData)

  • use (list of str, optional) – Names of the sensors the filter may use (default: all).

  • use_fixes (bool, optional) – Whether to use the position fixes (requires data.fix_period).

Returns:

numpy.ndarray – Position error (estimate minus truth) at every step.

compare_sensor_suites(sensors, suites, duration=300.0, dt=0.01, accel_std=0.5, accel_tau=2.0, fix_period=None, fix_std=0.1, seeds=range(0, 10))[source]#

Run the same simulated trajectories through several sensor suites.

Parameters:

  • sensors (list of Sensor) – Every sensor that exists in the simulation.

  • suites (dict) – {label: list of sensor names} to give the filter.

  • duration (optional) – As in simulate_dead_reckoning(); the filter uses position fixes whenever fix_period is set.

  • dt (optional) – As in simulate_dead_reckoning(); the filter uses position fixes whenever fix_period is set.

  • accel_std (optional) – As in simulate_dead_reckoning(); the filter uses position fixes whenever fix_period is set.

  • accel_tau (optional) – As in simulate_dead_reckoning(); the filter uses position fixes whenever fix_period is set.

  • fix_period (optional) – As in simulate_dead_reckoning(); the filter uses position fixes whenever fix_period is set.

  • fix_std (optional) – As in simulate_dead_reckoning(); the filter uses position fixes whenever fix_period is set.

  • seeds (iterable of int, optional) – One simulated trajectory per seed.

Returns:

list of dict – One row per suite with suite, rmse and final_error as (mean, std) over seeds (metres), and rmse_per_seed (the list), so a paired comparison is possible.