Lab-Scale Vision-Based Relative Navigation Testbed

A hardware-in-the-loop testbed for optical relative navigation around small-body analogs. A Raspberry Pi drives a camera along a linear stage toward a 3D-printed tri-axial ellipsoid, tracks landmarks on its surface, and recovers relative pose with a multiplicative EKF, benchmarked against stage-derived ground truth.

Build status. Software is complete and validated in simulation. Hardware assembly is in progress; CAD and photographs are placeholders marked below.

Rig layout in both views. Dimensions are drawn from config.yaml, so the diagram cannot drift away from what the software actually commands. Regenerate with python make_diagram.py after changing rail travel or mounting geometry.

---

Contents

• What it does
• Why a testbed
• Hardware
• Software architecture
• The estimator
• Getting started
• Calibration
• Running a test
• Results
• Ground truth accuracy
• CAD and fabrication
• Known limitations
• References

---

What it does

The system solves for the position and attitude of a camera relative to an uncooperative body, using only images of that body, and reports how well it did against an independent measurement of the truth.

A run sweeps the camera along the rail through a series of stand-off distances. At each station it captures frames, extracts landmarks, and folds them into an EKF. Because the stage position is known from homed step counts, every estimate has a truth value attached, so the output is not a trajectory, it is a characterization: error against range, against illumination angle, against how many surface features are visible.

Two detection paths are supported and the pairing is deliberate:

Path	Data association	Purpose
Fiducial (ArUco)	Solved by marker ID	Validate the estimator in isolation
Natural features (ORB + KLT)	By appearance, tracks break	Characterize the realistic case

Bringing the estimator up on fiducials first means that when the natural-feature run degrades, the filter is already ruled out. The difference between the two runs is the measurement of what data association costs.

---

Why a testbed

Optical navigation around a small body is dominated by effects that are hard to model and easy to get wrong in simulation: illumination that grazes the surface and moves the apparent centroid of every feature, self-shadowing that removes landmarks non-uniformly, a body whose feature distribution is not isotropic, and a camera whose real noise is not the Gaussian the filter assumes.

---

Hardware

Bill of materials

Item	Notes
Raspberry Pi 5 (4 GB)	Pi 4 works; expect roughly 2× the per-frame processing time
Camera Module 3 (standard, 66° HFOV)	Global shutter module is better if available, see limitations
Linear rail with leadscrew or belt, ≥ 600 mm	Travel sets the achievable range sweep
NEMA 17 stepper × 2	Rail and turntable
A4988 / DRV8825 / TMC2209 driver × 2	TMC2209 is quieter and holds microsteps better
Mechanical limit switch × 2	Homing
12 V 3 A supply + buck to 5 V	Do not run steppers off the Pi rail
Diffuse LED panel on an adjustable arm	PWM dimmable, on a protractor mount
Matte black backdrop	Cardstock is fine; kills background features
3D-printed ellipsoid body	See CAD

Wiring

Default GPIO assignments (BCM numbering) live in config.yaml:

Function	Pin	Function	Pin
Rail STEP	17	Turntable STEP	23
Rail DIR	27	Turntable DIR	24
Rail ENABLE	22	Turntable ENABLE	25
Rail limit	5	Turntable limit	6
Light PWM	18

Limit switches wire normally-open to ground with the internal pull-up enabled. Driver grounds must be common with the Pi ground, and the motor supply must not be.

Two layout constraints are worth reading off the diagram before building. The light arm rises behind the body, on the backdrop side, so no part of its structure enters the camera's field of view, a post in frame is a high-contrast static feature that a tracker will lock onto and report as zero relative motion. And at these stand-offs a 66° lens is far wider than the body: it underfills the frame badly at the far stations, which is why the backdrop matters and why body_mask() exists.

---

Software architecture

├── testbed/
│   ├── geometry.py       Quaternion algebra, projection, analytic Jacobians
│   ├── ekf.py            Multiplicative EKF for relative pose
│   ├── vision.py         Camera capture, ArUco + KLT landmark extraction
│   ├── stage.py          Stepper axes, turntable, light, ground-truth pose
│   └── simulator.py      Synthetic scene for offline testing
├── run_test.py           Sweep orchestrator, CSV logging
├── analyze.py            Benchmark figures from a run CSV
├── make_diagram.py       Layout diagram, drawn from config.yaml
├── calibrate_camera.py   ChArUco intrinsic calibration
├── make_landmark_map.py  Landmark map from body geometry
├── tests_math.py         Jacobian and quaternion convention checks
├── tests_ekf.py          Closed-loop convergence check
├── config.yaml           All hardware and estimator parameters
├── CAD/                  Body and bracket models (placeholder)
└── Images/               Figures and photographs

stage.py and vision.py both degrade gracefully off-hardware: absent gpiozero or picamera2 they fall back to simulated backends, so the whole pipeline including logging and analysis runs on a laptop. That is not a convenience feature, it means estimator changes get regression tested without booking bench time.

---

The estimator

State and formulation

A multiplicative EKF (MEKF), estimating camera pose in the body frame:

	Contents
Nominal	position $p \in \mathbb{R}^3$, velocity $v \in \mathbb{R}^3$, quaternion $q$ (body → camera)
Error	$[\delta p,\ \delta v,\ \delta\theta] \in \mathbb{R}^9$

A quaternion carries four parameters for three degrees of freedom. A naive EKF that puts all four in the state ends up with a singular attitude covariance and a quaternion that drifts off the unit sphere. The MEKF keeps the quaternion outside the filter and estimates a three-parameter error rotation against it, so the covariance stays full rank and normalization is exact by construction.

Measurement model

Each landmark at known body-frame position $L_i$ projects to

$$p_{\text{cam}} = R(q),(L_i - p), \qquad \begin{bmatrix} u \ v \end{bmatrix} = \begin{bmatrix} f_x , x_c/z_c + c_x \ f_y , y_c/z_c + c_y \end{bmatrix}$$

With the local (right-multiplied) attitude error convention $q_{\text{true}} = q_{\text{nom}} \otimes \delta q$, the Jacobian blocks are

$$\frac{\partial p_{\text{cam}}}{\partial \delta p} = -R, \qquad \frac{\partial p_{\text{cam}}}{\partial \delta \theta} = -R , [L_i - p]_\times$$

tests_math.py checks these against central finite differences on every run, the convention is easy to get subtly wrong, and a sign error there produces a filter that converges slowly and biased rather than failing outright, which is far harder to diagnose from the outside.

Three departures from the textbook filter

Each fixes a failure that appears immediately in practice, and each is documented at its implementation site:

Iterated update. A single linearization about a pose ten degrees off produces hundred-pixel residuals and an overshooting correction. Relinearizing several times inside the update is Gauss–Newton on the same cost the EKF minimizes, and it converts a filter that needs a good initial guess into one that does not. In simulation this pulls a 114 mm / 10° initial error to 3 mm in the first update.

Chi-square gating. A mis-associated landmark is a large, structured error. Ungated, one bad match drags the pose several centimetres while the filter reports tight covariance, confidently wrong, which is the worst failure mode for anything downstream.

Starvation recovery. Gating plus a batch of eighty landmarks is a trap, and it is the bug this filter was actually built around. The batch collapses the covariance so far that any model mismatch makes every residual look like an outlier; the filter then accepts nothing and dead-reckons on whatever velocity it last held. Consecutive empty updates now progressively widen the gate and inflate the covariance, and a floor keeps the formal uncertainty from dropping below what the hardware can physically resolve.

---

Getting started

git clone https://github.com/JCarroll-OU/vision-nav-testbed.git
cd vision-nav-testbed
python -m pip install -r requirements.txt

On the Pi, install the hardware packages from apt rather than pip, the pip builds of picamera2 and the GPIO stack are frequently broken:

sudo apt install -y python3-picamera2 python3-gpiozero python3-lgpio

Verify the estimator without touching hardware:

python tests_math.py        # quaternion conventions, Jacobian vs finite differences
python tests_ekf.py         # closed-loop convergence from a poor initial guess
python run_test.py --simulate && python analyze.py data/run.csv

---

Calibration

Three things must be measured before a run means anything. All three feed the ground truth or the measurement model directly, and an error in any of them appears as estimator error, sending you to debug the wrong thing.

1. Camera intrinsics. ChArUco board, 20+ views with good coverage into the image corners:

python calibrate_camera.py --capture
python calibrate_camera.py --solve      # writes back into config.yaml

Aim for reprojection RMS under 0.5 px. A 1% focal-length error is a 1% range bias that no amount of filtering removes.

2. Stage geometry. mount_offset, rail_axis, and rail_zero_m in config.yaml describe where the camera's optical centre sits relative to the body origin when the rail is homed. Measure these on the assembled rig.

3. Pixel noise. Park the stage, capture a few hundred frames, and take the standard deviation of each landmark's detected position. That number goes into sigma_pixel. Guessing it makes the reported covariance meaningless, which defeats the purpose of running a filter at all.

---

Running a test

python make_landmark_map.py --out data/landmark_map.csv   # from body geometry
python run_test.py --mode aruco --out data/run_01         # fiducial baseline
python run_test.py --mode natural --out data/run_02       # natural features
python analyze.py data/run_01/run.csv --out Images

The sweep — rail stations, turntable angles, light levels, frames per station, is defined under run: in config.yaml. Sweeping turntable angle varies which surface features are visible; sweeping light level and the light arm angle varies illumination. Those are the two sensitivity axes the testbed exists to characterize.

Every CSV row carries both the estimate and the truth, never just the derived error. When a run looks wrong you need to know whether the estimate moved or the truth did, and you cannot recover that from a difference.

---

Results

The figures below are from the simulated pipeline (run_test.py --simulate), which establishes that the estimator and analysis path are correct. They are placeholders for hardware results and will be replaced once the rig is assembled. Simulated numbers are a lower bound on error, not a prediction.

Error against the filter's own reported 3σ. The consistency check is that the error stays inside the envelope, an estimator that is accurate but overconfident is unusable downstream, since anything consuming its covariance will be misled.

Error against stand-off. Landmark angular separation shrinks with range, so the geometry degrades, this curve is the headline characterization the testbed produces.

Residual RMS against the assumed pixel noise, plus accepted and gated landmark counts. Residuals well above sigma_pixel, or a rising rejection count, both indicate the measurement model no longer matches reality.

Simulated performance (140 landmarks, 0.7 px noise, 5% dropout, 0.2–0.6 m range):

Metric	Value
Steady-state position error	0.31 ± 0.19 mm
Steady-state attitude error	0.085°
Convergence from 114 mm / 10° initial error	first update
Residual RMS vs. assumed 0.7 px	0.67 px

---

Ground truth accuracy

The stage is the measurement, so its error bounds every claim the testbed makes. Two design choices carry most of the budget:

Homing on every run. Open-loop step counting is only as good as its zero, and the zero is lost on every power cycle. Homing is two-speed — fast seek, back off, slow re-approach, because switch trip points depend on approach speed by a few tenths of a millimetre.

Unidirectional approach. Every commanded position is reached from the same direction, overshooting and coming back. Backlash in a leadscrew or belt stage is typically 50–200 µm and is usually the single largest error term; approaching consistently removes it from the budget entirely at the cost of some travel time.

Preliminary budget, to be replaced with measured values:

Source	Estimate
Step resolution (200 × 16, 8 mm lead)	2.5 µm
Homing repeatability	~50 µm, MEASURE with a dial indicator
Residual backlash after unidirectional approach	< 20 µm
Rail straightness and carriage yaw	MEASURE
Mount offset measurement	~0.5 mm, likely dominant

The mount offset is measured with calipers against a printed part, so it is the weakest link. If the estimator ever reports errors near 0.5 mm, that number needs a better method, not a better filter.

---

CAD and fabrication

Placeholder: models to be added.

CAD/
├── body/              Tri-axial ellipsoid analog with surface features
├── carriage/          Camera mount and rail interface
├── turntable/         Body mount and gear reduction
└── light_arm/         Adjustable illuminator mount with protractor

Notes for when the models land:

• The body's semi-axes and marker placement must match SEMI_AXES and the placement list in make_landmark_map.py. The landmark map is a design output for the fiducial path, the markers go where you put them, so the coordinates follow from CAD rather than from measurement.
• Print the body in a matte filament and avoid glossy finishes. Specular highlights move with the camera rather than sitting on the surface, so a feature tracker will happily lock onto one and report motion the body did not make.
• Surface features should be non-uniformly distributed. An evenly textured body makes the estimator look better than it is; the interesting failure is what happens when the camera faces a bald patch.

---

Known limitations

• Rolling shutter. The Camera Module 3 is rolling-shutter, so a moving carriage skews the image and biases landmark positions along the scan direction. Current mitigation is to capture only at rest, which caps the achievable frame rate and prevents characterizing the estimator under continuous motion. A global-shutter module removes this.

• Attitude propagates as a random walk. There is no gyro on the carriage, so between measurements the filter has nothing better to do with attitude. Real angular motion shows up as process noise that the measurements must pull back. An IMU on the carriage would tighten this considerably.

• Constant-velocity process model. The stage moves and stops; acceleration transients at each station are absorbed as process noise. Feeding the commanded stage velocity forward would be a straightforward improvement.

• Natural-feature map building is unsolved. The fiducial path gets its map from CAD. The natural-feature path needs landmark positions from either a structured-light scan of the printed body or bootstrapping off a fiducial run, and neither is implemented.

• Single-body, no occlusion modeling. Self-occlusion is handled by the detector simply failing to see the far side. There is no explicit visibility prediction, so the filter cannot distinguish "landmark occluded" from "landmark missed".

• Simulated results only. Everything under Results comes from the synthetic pipeline. It has no lens distortion, no motion blur, no detector dropout beyond a uniform random rate, and perfect data association. It validates the math; it does not predict hardware performance.

---

References

1. Markley, F. L. — Attitude error representations for Kalman filtering (the MEKF error-state formulation).
2. Lucas, B. D. and Kanade, T. — An iterative image registration technique with an application to stereo vision.
3. Baker, S. and Matthews, I. — Lucas-Kanade 20 years on: a unifying framework.
4. Garrido-Jurado, S. et al. — Automatic generation and detection of highly reliable fiducial markers under occlusion.
5. Bell, B. M. and Cathey, F. W. — The iterated Kalman filter update as a Gauss-Newton method.