Pose estimation error analysis and closed-loop visual docking for a differential-drive AMR, measured against exactly-known ground truth.
I shipped ArUco marker tracking on a Jetson-based AMR using the apparent-width method — measure the marker in pixels, divide, get a distance. This project rebuilds that estimator properly, measures where it breaks, and then runs both versions closed-loop to see which one actually lands the robot on the dock.
A synthetic frame: marker at 2 m, yawed 35°. Every image is rendered through a known camera model at a chosen pose, so the ground truth is exact to machine precision and any error an estimator reports is genuinely its own.
You cannot measure a pose estimator by photographing a printed marker, because then you are comparing one estimate against another — the tape-measure reading is itself uncertain, and at 3 m a 2 cm placement error is already larger than the effect you are trying to see.
So the marker is rendered: a pose is chosen, its corners are projected through the camera model, and the marker bitmap is warped onto the image plane by the homography between them. The pose that produced the pixels is known exactly. Detection then recovers the corners to better than 0.1 px at 2 m, so the residual is the estimator's, not the harness's.
| Marker yaw | known-width error | solvePnP error | predicted |
|---|---|---|---|
| 0° | −0.1% | −0.07% | 0.0% |
| 15° | 3.0% | 0.05% | 3.5% |
| 30° | 15.1% | 0.05% | 15.5% |
| 45° | 40.6% | −0.42% | 41.4% |
| 60° | 99.8% | 0.35% | 100.0% |
| 70° | 185.1% | 0.40% | 192.4% |
The known-width method is not randomly wrong — it is wrong in a shape you can derive. A marker rotated
by yaw
At 60° the marker is reported at twice its true distance. solvePnP stays under 0.42% across the entire range.
Bearing, by contrast, survives: both estimators hold bearing error under 0.06°, because horizontal image position is unaffected by the marker's rotation. The naive method fails in range, not in direction — which is precisely why it feels like it works right up until it doesn't.
Static error curves show which estimator is more accurate. They do not show whether a controller can absorb the error. Only closed-loop testing answers that, so both estimators drive a real docking run: every control step renders what the camera would see from the robot's current pose, detects, estimates, and steers on that estimate. Nothing consults ground truth except the scoring.
| Estimator | Docked | Final position error | Final heading error | Lost frames |
|---|---|---|---|---|
| solvePnP | yes | 0.039 m | 9.9° | 0 / 401 |
| known-width | no | 0.237 m | 27.3° | 0 / 401 |
Both see the marker the whole way. The difference is entirely estimation quality, and it is a 6× gap in final position error — the difference between docking and missing.
There is a second, deeper failure. The known-width method returns a distance and a bearing but cannot recover the marker's orientation at all. A docking controller needs that: the goal pose is a standoff point out along the marker's normal, not just somewhere near the marker. With no yaw estimate the controller must assume it is already on the marker's axis — an assumption that is only true at the end of a successful approach. That is why its heading error is 27° at the dock.
The obvious next step was an EKF fusing wheel odometry with the vision measurements, so the robot could coast through the dropouts measured above. Building it produced two results I did not expect.
Range and bearing to one point are two measurements for a three-degree-of-freedom pose. They constrain the robot to a circle around the marker and fix its heading relative to the line of sight, but say nothing about where on that circle it sits. Measured: the filter settles at a 0.230 m error that never shrinks, no matter how many clean detections arrive, because a stationary robot never excites the unobservable direction.
The fix was already in hand. solvePnP recovers the marker's apparent yaw — the angle between its
normal and the line of sight — which is exactly the missing coordinate. Adding it as a third
measurement takes the measurement Jacobian from rank 2 to rank 3, and the standing error to under 1 cm.
Both facts are asserted in the tests.
That is also the second reason the known-width estimator is a dead end: it returns no yaw at all, so it cannot supply the one measurement that would make the problem well posed. It cannot be rescued by filtering.
My first fusion experiment used random frame dropout, and the filter lost. At 10 Hz, even 70% loss still delivers three corrections a second — and the unfiltered controller re-derives everything relative to the marker each frame, so it accumulates no drift to correct. It simply does not need help.
The realistic failure is a sustained occlusion: someone walks in front, or the marker leaves the frame mid-turn. A safe-stop controller halts for exactly the duration; a filter dead-reckons through it.
| Occlusion | vision only | vision + EKF | stall recovered |
|---|---|---|---|
| none | 5/5, dock 8.9 s | 5/5, dock 8.9 s | — |
| 3 s | 5/5, dock 11.9 s | 5/5, dock 9.1 s | 93% |
| 7 s | 5/5, dock 15.9 s | 5/5, dock 10.5 s | 77% |
| 13 s | 5/5, dock 21.9 s | 5/5, dock 15.1 s | 52% |
| 21 s | 5/5, dock 29.9 s | 5/5, dock 21.5 s | 40% |
| 31 s | 4/5, dock 39.9 s | 5/5, dock 29.5 s | 34% |
The unfiltered controller loses precisely the occlusion duration — 8.9 s plus the blackout, every time. The filter recovers most of that, and the fraction it recovers decays as odometry drift accumulates, which is exactly what dead reckoning should do.
Final accuracy is unchanged at 4 cm either way. That is the honest headline: the EKF does not make docking more accurate, it stops the robot from stopping. Those are different benefits and it is worth being precise about which one you are claiming.
One caveat stated plainly: the filter treats the dock as surveyed and estimates the robot's absolute pose, while the unfiltered baseline works purely relative and assumes nothing. That is how real docking works — an AMR knows where its charger is — but the two are not strictly like-for-like.
Calibration. Neither estimator survives a wrong focal length; the error passes through essentially 1:1, so a focal length 10% too long puts every distance 10% too far. No amount of clever estimation recovers it — this is the one input you cannot afford to guess.
Detection envelope. 79% of a 0.3–12 m × 0–85° grid detects at all. The limit is not gradual: past roughly 60° of yaw, or 8 m at a 0.15 m marker, the marker stops being found rather than being found inaccurately.
Blur and noise. These are what actually end a docking run on a moving AMR — the pose is not wrong, it is simply absent for a few frames. Detection is robust to sensor noise alone, but collapses to zero at 4 px of blur regardless of noise. On a real robot that sets an upper bound on approach speed for a given exposure time.
The interesting parts of this project were the failures, all of which are now regression tests.
1. Every single detection failed at first. The renderer placed the marker's "top" at
2. SOLVEPNP_IPPE_SQUARE is not usable here. It is the solver advertised for exactly this case —
a planar square of known size — but on OpenCV 5.0 it returns NaN at some yaw angles even with
OpenCV's own documented point ordering, and a degenerate near-zero translation with any other. A sweep
over solver and ordering combinations gave:
| Solver | non-finite results | worst finite range error |
|---|---|---|
IPPE_SQUARE |
fails at multiple angles | — |
IPPE |
14 / 686 (2%) | 2.63% |
ITERATIVE |
0 | 53.99% |
IPPE + ITERATIVE fallback |
0 / 686 | 2.63% |
Neither is usable alone: IPPE is accurate but intermittently NaN, ITERATIVE is always finite but
occasionally lands in the wrong local minimum — the classic planar pose ambiguity. Running IPPE
first and using ITERATIVE only to rescue the frames it drops gives both properties at once.
3. One NaN cost 386 frames. An unguarded non-finite estimate did not just lose its own frame: it
propagated into the controller, corrupted the robot's state, and every subsequent render then had
nothing to detect. One bad estimate turned into 386 lost frames out of 401 and a completely failed
run. Estimators now validate finiteness at the boundary, and a test asserts the trajectory stays
finite.
41 unit tests on Python 3.10-3.13 in CI. The ones that matter check claims, not code paths:
| Test | What it protects |
|---|---|
| Known-width error follows |
The headline claim itself, asserted rather than just written down |
| solvePnP accurate at 0–60° yaw | The comparison is fair and not a tuning artefact |
| Marker renders upright, not mirrored | The bug that silently broke every detection |
| solvePnP never returns non-finite values | The NaN that cost 386 frames |
Known-width returns None for yaw |
The limitation cannot quietly disappear |
| Projected size falls as |
The renderer obeys perspective |
| Docking gains satisfy |
Stability condition, checked not assumed |
| Trajectory stays finite | State corruption cannot recur |
| Wrong focal length scales range 1:1 | The calibration finding |
| EKF Jacobians vs. finite differences | A sign error here would not crash, just quietly degrade the filter |
| Range+bearing Jacobian is rank 2, with yaw rank 3 | The observability finding, as linear algebra |
| Range+bearing alone leaves a standing error | The 0.23 m that never converges |
| Wheel slip actually perturbs the trajectory | Stops the filter winning because odometry is secretly perfect |
CI additionally re-runs every experiment and fails the build if no estimator manages to dock.
git clone https://github.com/yongjunmun/ArUco-AMR-Docking.git
cd ArUco-AMR-Docking
pip install -r requirements.txt
python run.py # simplest: open run.py in an editor and press Run
python -m arucodock.cli errors # estimator accuracy sweeps
python -m arucodock.cli envelope # detection limits, blur and noise
python -m arucodock.cli dock # closed-loop docking runs
python -m arucodock.cli fusion # EKF under sustained occlusion
python -m arucodock.cli all # everything into results/
python -m unittest discover -s tests -t . -vAdding your own estimator takes one class:
import numpy as np
from arucodock import DEFAULT_CAMERA, MarkerPose, PoseEstimate, PoseEstimator, detect_corners, render
class MyEstimator(PoseEstimator):
name = "mine"
def estimate(self, corners, camera):
width = np.linalg.norm(corners[1] - corners[0])
return PoseEstimate(range_m=0.15 * camera.focal_px / width, bearing_deg=0.0)
pose = MarkerPose(distance_m=2.0, yaw_deg=30.0)
print(MyEstimator().estimate(detect_corners(render(pose), 7), DEFAULT_CAMERA))run.py click-to-run entry point
arucodock/
camera.py pinhole intrinsics, distortion, deliberate miscalibration
scene.py synthetic marker rendering at exactly known poses
detect.py ArUco detection with sub-pixel corner refinement
estimators.py known-width vs solvePnP, with the solver fallback
experiments.py sweeps over yaw, distance, calibration, blur and noise
robot.py differential-drive AMR model
ekf.py EKF fusing wheel odometry with range/bearing/yaw
docking.py closed-loop visual servoing, perception in the loop
plotting.py figures
cli.py command line entry point
tests/ 41 unit tests
results/ committed figures and CSVs
Stated plainly, because a simulation result is not a hardware result:
- No hardware validation. Rendering is a perfect pinhole camera. A real sensor adds rolling shutter, auto-exposure hunting, chromatic aberration and lens distortion that this models only crudely.
- The lighting model is trivial. Uniform illumination, no specular reflection off the marker, no shadow across it. On a real floor, glare on a laminated marker is a common detection failure that does not appear here at all.
- The robot model is kinematic. No wheel slip, no acceleration limits, no controller latency between capturing a frame and acting on it — all of which matter at speed.
- One marker, one dictionary. No occlusion, no multiple markers, no false positives from marker-like clutter in the background.
- Blur is isotropic Gaussian. Real motion blur is directional and depends on the velocity, so the 4 px cliff is indicative rather than a number to design against.
Camera distortion recovery and a calibration routine, a marker-board pose estimate rather than a single tag, multiple markers so the pose stays observable when one is occluded, and validation against a real webcam with a printed marker on a measured rail.
MIT — free to use, modify and distribute, with attribution and no warranty.