Robotics Algorithm Cookbook

Working, tested implementations of the algorithms that show up over and over in robotics, mechatronics and motion control — written to be readable at 2am with no internet connection.

📖 Interactive HTML Textbook: Open docs/index.html in any browser for human-friendly explanations, pedagogical derivations, interactive visualisations, and firmware guides designed for software engineers.

Every recipe is self-contained, dependency-free, and comes with a test suite that measures what it claims rather than asserting that it feels right. The READMEs carry the derivations, the tuning procedures, the numbers those tests actually produce, and — importantly — the failure modes.

The constraint that shaped everything

This library exists to be useful when you cannot look anything up and cannot ask anyone. That forced three rules:

1. No dependencies. Python recipes are stdlib-only — no numpy, because pip install needs a network. C recipes need nothing but libm.
2. Every directory stands alone. Copy one folder into your project and it works. Where several recipes need small-matrix maths, linalg.py is duplicated verbatim rather than shared. That is deliberate.
3. The explanation lives next to the code. If you have to guess why a constant is 0.676, the recipe has failed.

How to use it

make test          # build and run everything (~2 minutes)

Or run any single recipe:

cd control/pid-controller && make test
cd estimation/kalman-filter && python3 demo_kalman.py

Every test prints measured results — step responses, error tables, ASCII plots — so the output is itself documentation. make test in motion-profile draws you the S-curve; imu-calibration prints C constants ready to paste into firmware.

There is also a textbook at docs/index.html — open it in a browser. It teaches the theory behind these recipes for a software-engineering audience; the READMEs here are the reference next to the code. MESSAGES.md is the shared mailbox the two authors use to send each other corrections.

Checking the maths against a reference library

No recipe needs numpy, and none ever will — that is the premise. But "is your hand-rolled linear algebra actually right?" is a fair question, so there is an optional cross-check:

./tools/fetch-offline-wheels.sh --venv    # do this while you have internet
make verify

It runs 21 comparisons against numpy and scipy — linalg.py against LAPACK, dare() against solve_discrete_are, c2d() against cont2discrete, the Kalman filter against a numpy reference implementation, the UKF's sigma points against numpy moments, and dls_step against lstsq. Everything agrees to between 10⁻¹⁶ and 10⁻¹⁰.

make verify exits 0 with an explanation when numpy is absent, so it is safe in a Makefile that has to work offline. The wheels land in vendor/wheels/ (gitignored, ~50 MB) so the check still works during the outage this repo exists for.

Target platforms

Task	Platform	Why
Hard real-time firmware	Pico C SDK	deterministic timing, no scheduler between you and the pin
Anything where task-switching is fine	Python 3.11+ asyncio	you get to think about the problem instead of about interrupts

C recipes are split in two: a portable C11 core that compiles and is unit-tested on your laptop, and Pico SDK glue isolated in a pico/ subdirectory. So you can test the algorithm without flashing anything, and the algorithm never depends on the SDK.

---

Recipes

Control — making something do what you asked

Recipe	Lang	What it's for
pid-controller	C + Pico	Hold one value steady. Anti-windup, derivative filtering, feed-forward, bumpless transfer.
motion-profile	C + Pico	Trapezoidal and S-curve point-to-point moves. Fixes more "bad PID tune" complaints than tuning does.
lqr-regulator	Python	Optimal feedback when states are coupled and one actuator drives several of them. Balancing robots.
model-predictive-control	Python	When the actuator saturates and you need the controller to know that.
foc-svm	C + Pico	Field-oriented control: turn a BLDC into a torque source. Clarke/Park, space-vector PWM, current loops.
stepper-ramp	C + Pico	Integer-only acceleration ramps + multi-axis Bresenham. The core of every 3D printer.

Estimation — figuring out what's actually going on

Recipe	Lang	What it's for
kalman-filter	Python	Measure position, get velocity — 30× better than differencing. Joseph form, NIS gating, steady-state gains.
extended-kalman-filter	Python	Nonlinear models. Differential-drive localisation from landmarks.
unscented-kalman-filter	Python	Nonlinear without Jacobians, and more accurate. Works on models with no derivative at all.
particle-filter	Python	Global localisation and kidnapping recovery — the things Gaussian filters structurally cannot do.
complementary-filter	C + Pico	Tilt from gyro + accelerometer. One tuning knob, 40 lines, very hard to beat.
mahony-ahrs	C + Pico	Full 3D attitude on a quaternion, no gimbal lock, ~60 flops.
imu-calibration	Python	Ellipsoid fitting for magnetometers. Do this before tuning any filter.

Kinematics — joint space, task space, and the map between them

Recipe	Lang	What it's for
planar-arm-ik	Python	Exact closed-form IK for 2R/3R planar arms. Both elbow branches, honest reachability, no iteration.
jacobian-ik	Python	Numerical IK for any chain. Damped least squares, redundancy resolution, singularity-safe.
wheel-kinematics	C + Pico	Diff-drive, mecanum and swerve, plus odometry with exact arc integration.
quaternion-toolkit	C	3D orientation done carefully: SLERP, conversions, exact integration, and the double-cover guards.

Planning — deciding where to go and how to get there

Recipe	Lang	What it's for
astar-grid	Python	Optimal paths on an occupancy grid. Admissible heuristics, tie-breaking, obstacle inflation, and a definite "no path".
path-smoothing	Python	Grid path in, drivable path out. 45° heading jumps down to 2.5°, with a fallback when smoothing cuts a corner.
pure-pursuit	Python	Follow the path. Speed-scheduled lookahead, because a fixed one trades oscillation against corner-cutting and loses both at speed.
rrt-star	Python	Sampling-based planning for spaces too big to enumerate. Rewiring, the shrinking ball, and the cost bug that makes a tree lie about itself.
dynamic-window-approach	Python	Local avoidance in velocity space. Acceleration limits, the braking constraint, and the U-trap it cannot escape alone.

Planned

Scaffolded but not yet written. Directories exist; see each chapter README.

Chapter	Recipes
planning	rrt-star · dynamic-window-approach
signal	digital-filters · quadrature-decoder · cordic-fixed-point · goertzel-tone-detect
systems	cobs-crc-framing · realtime-scheduler · asyncio-supervisor

---

Picking the right tool

"I need to hold a value steady." One value, one actuator → PID. Coupled states → LQR. Hard limits that matter → MPC.

"My PID overshoots / the machine clunks." Before touching gains, stop feeding it step commands. Give it a motion profile, and put the steady-state effort in feed-forward so the integrator has almost nothing to do.

"I need to know where I am." Roughly know already, linear model → Kalman. Have a heading in the state → EKF. Jacobians painful → UKF. No idea where you are, or the map has identical-looking places → particle filter.

"I need to know which way up I am." Two-axis tilt, never near vertical → complementary filter. Full 3D → Mahony. Either way, calibrate first.

"I need to spin a motor." Brushed → PID on current or speed. Brushless, smooth torque → FOC. Stepper → stepper-ramp.

"I need to move the tip of an arm to a point." Planar arm → planar-arm-ik: exact, instant, and it hands you both elbow branches so you can pick the one that does not slam. Any other chain, or a redundant one → jacobian-ik. Either way, profile each joint afterwards — IK gives you waypoints, not a trajectory.

"My arm goes berserk near full extension." That is a singularity, and an undamped pseudo-inverse is doing exactly what you asked: computing the enormous joint motion that best approximates a tool motion the arm cannot make. Use damped least squares (jacobian-ik), and keep planned paths above a manipulability floor rather than grazing the boundary.

"My robot's reported position drifts outward on every curve." That is almost certainly the odometry integrator, not the wheel radius. The straight-line form x += v·cos(θ)·dt reports a larger circle than you drove — a signed scale error indistinguishable from a calibration problem. Use exact arc integration (wheel-kinematics); at four ticks per quarter turn it is 278 mm better.

"I need to store or interpolate a 3D orientation." Use a quaternion (quaternion-toolkit) and stay in it. Convert to Euler angles at the last possible moment, for humans only — the round-trip error through Euler is a decade worse than through any other representation, and it is worst exactly where you are least able to tolerate it. If you are estimating attitude rather than commanding it, that is Mahony.

"I need to get from here to there without hitting anything." Grid map, 2D → astar-grid, then path-smoothing, then pure-pursuit. That is the whole stack, in that order, and each one exists because the previous one's output is not yet drivable. Inflate the map by the robot radius before planning, not after — it turns "the robot fits" from a cost term you can be bargained out of into a property of the map.

"A grid would work, but the space is an arm, not a floor." rrt-star. The test is whether you can allocate the grid: 2D or 3D, enumerate it and use A*, which on a 2D map beats RRT* on speed, optimality and determinism at once. A 6-DOF arm at a coarse 5° per joint is 139 billion configurations and 130 GB, so the question of which is better never arises. Sampling planners never build the space; their memory is the number of samples you chose to draw.

"The path was fine but the robot still hit something." dynamic-window-approach. A geometric planner cannot know what your motors can do in the next 100 ms. DWA searches the velocity rectangle reachable in one tick, so every candidate is executable by construction, and admits only speeds it can still brake from: v <= sqrt(2·a_max·d). Without that line the robot hits the wall at 0.60 m/s whenever its rollout is shorter than its stopping distance — a boundary that falls exactly where the physics puts it, with no tuning. Note what it is not: a bound on kinetic energy, not a keep-out zone. For a keep-out zone, inflate the map.

"I tuned the cost weights for days and nothing changed." Check whether the terms are normalised before they are weighted. dynamic-window-approach scores heading (0..π), clearance (0..1 m) and speed (0..0.8 m/s) — summed raw, heading outranks speed four to one before any weight applies, and the robot never arrives. Scaling every weight by ten changes nothing, which is the tell: the bug is the ratio of the ranges, not the weights.

"My planner returns a valid path but converges to the wrong cost." Recompute the cost from the geometry and compare it to the one you stored. rrt-star shows a rewiring implementation that forgets to push cost changes down the subtree: it still returns collision-free paths, it still converges, and it is 3.5% worse than it should be, with individual nodes off by 1.9 m on a 10 m path. Any stored quantity that is derived from geometry can drift from it, and the check is always the same — recompute and compare.

"My estimator is lying to me." Check consistency, don't eyeball plots. Average the NIS — it should equal the measurement dimension. In simulation, average the NEES — it should equal the state dimension. Both are far sharper instruments than looking at a graph, and they work on a robot with no ground truth.

---

What "tested" means here

The tests are the reason to trust any of this, so they try to be adversarial:

• Closed-form checks. The scalar LQR is verified against the golden ratio. c2d is checked against exp(a·dt) to 1e-12.

• Cross-implementation agreement. The UKF reproduces the Kalman filter to 3e-15 on a linear model — which pins down the weights, the matrix square root and the cross-covariance at once. Unconstrained MPC reproduces LQR to 6e-10.

• Empirical optimality. The LQR gain is pitted against 300 perturbed gains; none may beat it.

• Statistical consistency. NIS ≈ 1.0 and NEES ≈ 3.002, with deliberate mistuning shown to move them the right way.

• Derivative checks. Analytic Jacobians are compared against central differences, and then one sign is deliberately flipped to prove the check has teeth.

• Numerical honesty. The Joseph-form covariance update is compared against the short form in emulated float32 — the short form drives a variance to exactly 0.0, which is a filter that has stopped listening.

• Failure modes are demonstrated, not described. The EKF is shown diverging to 29 m of error while reporting 10 cm of uncertainty. The UKF is shown failing on the same problem, so the comparison can't oversell it.

• Sampling is not coverage. A 20 000-sample random round trip through the quaternion Euler conversion reported 9·10⁻⁴ degrees of error while the gimbal-lock branch was thirty degrees wrong — uniform sampling essentially never lands within 10⁻⁵ of a pole. A branch your random inputs cannot reach needs a test that goes there on purpose.

• The test must be able to fail. The first odometry test drove a full circle and checked it closed. Both integrators passed, because an N-gon with N equal turns always closes; the broken one simply closed around a bigger circle. Measuring a partial arc against a known endpoint exposed 278 mm.

• Cross-check against a real library where one exists. make verify compares the hand-rolled maths against numpy and scipy — 21 checks, all agreeing to between 10⁻¹⁶ and 10⁻¹⁰. Optional, and no recipe depends on it.

Several bugs found this way are documented in the recipes that had them, including a sensor model that scored an impossible pose above the true one, a sign error that only a magnitude test could catch, and the two above.

Conventions

• C: C11, -Wall -Wextra, no dependency beyond <math.h>. M_PI is not defined under -std=c11, so files that need it define it.
• Python: 3.11+, stdlib only, type hints, dataclasses.
• Angles are radians internally and degrees only at the edges. Every angle difference is wrapped.
• dt is measured, never assumed. Firmware examples time each tick.
• Units are SI unless a comment says otherwise.

Licence

Do whatever you like with it.