1d-position-estimation

Small Python simulation for experimenting with position estimation from noisy, delayed sensors and using that estimate in a PID-controlled feedback loop.

Overview

The project simulates a moving vehicle, reads its position through two asynchronous sensors, estimates the current position, and drives the vehicle toward a target destination.

• The simulation updates the true position in a background thread.
• A fast sensor reports quickly but with more noise.
• A slow sensor reports more slowly but with less noise.
• An estimator turns those sensor readings into a position estimate.
• A PID controller uses the estimate to set vehicle speed.
• A logger records true vs estimated position and shows a plot at the end of the run.

Project Layout

• main.py: entry point and control loop
• params.yaml: runtime configuration
• src/sim.py: vehicle simulation
• src/sensors.py: noisy, delayed sensor model
• src/estimator.py: estimation strategies
• src/pid.py: PID controller
• src/utils.py: shared helper functions
• src/logger.py: plotting and run logging

Available Estimators

Set estimation_approach in params.yaml to one of these:

• SimpleFastEstimator: uses the latest fast-sensor reading directly
• FastSmoothEstimator: exponentially smooths fast-sensor readings
• SwitchSmoothEstimator: prefers the slow sensor when available, otherwise smooths the fast sensor
• FastSmoothPredictEstimator: smooths the fast sensor and projects position forward using estimated speed
• ProjectThenFuseEstimator: predicts current position independently for both sensors, then fuses the two estimates using inverse-noise weighting

The default configuration uses ProjectThenFuseEstimator.

Setup

This repository does not currently include a requirements.txt, so install the packages imported by the code:

python3 -m venv .venv
source .venv/bin/activate
pip install pyyaml pandas matplotlib seaborn

Run

python3 main.py

During execution, the program prints the true simulated position periodically. When the session ends, it opens a Matplotlib plot comparing true position and estimated position.

Configuration

Runtime settings live in params.yaml.

verbose: true
speed_limit: 10
session_duration_s: 20
sim_steps_per_second: 100
control_steps_per_second: 50
speed_initial: 1
target_destination: 50
estimation_approach: ProjectThenFuseEstimator

pid_params:
  Kp: 2
  Ki: 0.05
  Kd: 1
  integral_err_min: -50
  integral_err_max: 50
  integral_overshoot_zeroout: false
  integral_cap_increment: true
  integral_cap_increment_radius: 10

Meaning of the main settings:

• verbose: prints PID terms and periodic simulation state during the run
• speed_limit: max absolute vehicle speed allowed by the simulation
• session_duration_s: total run time
• sim_steps_per_second: internal simulation update rate
• control_steps_per_second: PID/control loop frequency
• speed_initial: initial speed before PID corrections dominate
• target_destination: desired final position
• estimation_approach: estimator class name to use
• pid_params: PID gains plus anti-windup controls for the integral term

PID-specific settings:

• Kp, Ki, Kd: proportional, integral, and derivative gains
• integral_err_min / integral_err_max: lower and upper bounds for the accumulated integral state
• integral_overshoot_zeroout: when true, clears the integral state when the position error changes sign
• integral_cap_increment: when true, clamps each integration step before it is added to the accumulated integral
• integral_cap_increment_radius: symmetric bound used for the per-step integral clamp; this should be a positive value

When pid_params.integral_cap_increment is true, large instantaneous errors no longer dump their full value into the integral term on a single control step. This makes the integrator build up more gradually while still preserving the overall proportional and derivative response.

When pid_params.integral_overshoot_zeroout is true, the controller clears the accumulated integral term when the position error changes sign. This helps the controller shed built-up integral action after crossing the target instead of pushing further past it.

Notes

• Sensor noise and latency are currently hard-coded in main.py.
• The logger ends with plt.show(), so you need a graphical environment to see the chart.

Example

example terminal output:

Simulation, true position 0.0
[PID] Current loc estimate 0.000
[PID] Error components P: 100.000, I: 0.300, D: 0.000; total 100.300
[PID] Current loc estimate 0.000
[PID] Error components P: 100.000, I: 0.600, D: 0.000; total 100.600
[PID] Current loc estimate -0.007
[PID] Error components P: 100.014, I: 0.900, D: 0.350; total 101.264
[PID] Current loc estimate -0.007
[PID] Error components P: 100.014, I: 1.200, D: 0.002; total 101.216
[PID] Current loc estimate -0.007
[PID] Error components P: 100.014, I: 1.500, D: 0.000; total 101.515
[PID] Current loc estimate 0.094
[PID] Error components P: 99.812, I: 1.800, D: -5.063; total 96.548
[PID] Current loc estimate 0.106
[PID] Error components P: 99.789, I: 2.099, D: -0.571; total 101.317
[PID] Current loc estimate 0.111
[PID] Error components P: 99.778, I: 2.398, D: -0.278; total 101.898
[PID] Current loc estimate 0.143
[PID] Error components P: 99.714, I: 2.697, D: -1.601; total 100.810
[PID] Current loc estimate 0.146
[PID] Error components P: 99.708, I: 2.997, D: -0.132; total 102.572
[PID] Current loc estimate 0.720
...
[PID] Current loc estimate 50.030
[PID] Error components P: -0.059, I: -0.001, D: 0.008; total -0.053
[PID] Current loc estimate 50.057
[PID] Error components P: -0.114, I: -0.001, D: -1.361; total -1.476
[PID] Current loc estimate 50.072
[PID] Error components P: -0.143, I: -0.002, D: -0.737; total -0.881
[PID] Current loc estimate 50.077
[PID] Error components P: -0.154, I: -0.002, D: -0.275; total -0.431
[PID] Current loc estimate 50.059
[PID] Error components P: -0.119, I: -0.002, D: 0.882; total 0.761
Simulation, true position 49.95030448101059