This page documents how I kept developing my project from UCSD ECE 276B: Planning & Learning in Robotics after taking the course.  ·  Home

The assignment, environment and starter code are the work of Prof. Nikolay Atanasov and the ECE 276B course staff, who hold the copyright. Because this is a graded assignment that students still complete, the code is not public. This page shares only the approach and the results.

Safe Trajectory Tracking under Motion Uncertainty

Junsung Park
July 2026

A differential-drive robot has to follow a figure-eight past four circular obstacles, and every half-second Gaussian noise pushes it off the path it predicted. A certainty-equivalent controller (CEC) replans ten steps ahead as if there were no noise. Generalized policy iteration (GPI) solves a discretized MDP that includes the noise. After the course I kept working on one question: how much does it help a controller to know about the noise?

The answer turned out to depend less on the controllers than on a five-centimeter gap in the environment, and on when collisions are counted. CEC tracks the reference most closely and collides in every noisy episode. GPI keeps a margin, and an ablation says the margin comes from its calibrated collision probability. But giving CEC that same probability made it worse. While chasing the difference that remained, I found that part of GPI's safety was a loophole: it had learned to cross the gap at full speed, so that its two samples land on either side of it. This page follows the investigation in the order it happened.

The setup

The robot state is \(x_t = (p_t, \theta_t)\) and the control is \(u_t = (v_t, \omega_t) \in [0.1, 1] \times [-1, 1]\). The kinematics are integrated exactly over \(\Delta = 0.5\) s, then noise \(w_t \sim \mathcal N(0, \mathrm{diag}(\sigma)^2)\) with \(\sigma = (0.04, 0.04, 0.004)\) is added. With the tracking error \(e_t = (p_t - r_t,\ \theta_t - \alpha_t)\), the objective is

\[ V^*(\tau, e) = \min_\pi \ \mathbb E \Big[\sum_{t \ge \tau} \gamma^{t-\tau} \big(\tilde p_t^\top Q \tilde p_t + q (1-\cos\tilde\theta_t)^2 + u_t^\top R u_t\big)\Big] \quad \text{s.t.}\ \ \tilde p_t + r_t \in \mathcal F , \]

where \(\mathcal F\) is the square \([-3,3]^2\) minus the obstacles inflated by the robot radius (0.5 + 0.3 = 0.8 m). Every method uses \(Q = 10 I\), \(q = 10\), \(R = 0.1 I\) and \(\gamma = 0.95\). I scale the noise by \(k \in \{0, 0.25, 0.5, 1, 2\}\) and run 200 episodes of 240 steps at each non-zero level. For a given seed, every controller sees exactly the same noise draws, so all comparisons are paired. GPI is trained separately for each noise level, with that level's noise model. A collision means a sampled robot position inside an inflated obstacle, as in the assignment. This definition comes back later.

A five-centimeter gap

Before running any controller, I checked the geometry, and it turned out to matter more than anything else in the project. The reference passes inside the inflated obstacles C1 and C2, so perfect tracking means collision. Worse, the gap between inflated C1 and inflated C3 is only 5.1 cm, and C2 and C4 leave the same gap. At noise ×1, a single sample at the middle of that gap collides with probability 51%. A controller can either keep some margin inside the gap or leave the reference and take a longer route around C3.

Where does GPI's margin come from?

Tabular GPI uses the noise model in two places: in the Bellman transition (the expected value over noisy next states) and in the one-step collision probability. To see which one produces the margin, I retrained the noise ×1 policy with each part switched independently and evaluated all four on the same 200 episodes.

Bellman transitionrisk σcollision rate (%)collision steps
Gaussian4 cm41.50.51
deterministic4 cm38.50.43
Gaussian1 cm97.03.30
deterministic1 cm98.53.74
Noise ×1, 200 paired episodes. The true position noise is 4 cm per step; a 1 cm risk σ underestimates it.

Switching the transition noise on or off makes no measurable difference (paired exact McNemar p = 0.53). Setting the collision probability to the true noise level changes the selected route and removes most collisions (p < 10−30). With 10 cm grid cells and 4 cm of noise, averaging the noise over a piecewise-linear value barely changes it. So, at least on this grid, what matters is a correctly calibrated collision probability, propagated backward in time by dynamic programming.

CEC, of course, has no collision probability at all. The usual way to make it more careful is a safety margin: tighten every obstacle constraint by m.

Collision rate versus tracking error for CEC with safety margins and tabular GPI with different risk penalties
Each point is a separately configured controller evaluated on the same 200 episodes at noise ×1.

It does not help at this noise level: every margin still collides in all 200 episodes. Beyond 2.5 cm (half the gap), the gap becomes infeasible, and the ten-step NLP either pushes through on its slack variables or stalls at the entrance, reaching 99 cm of tracking error at m = 10 cm. Sweeping GPI's risk weight λ instead moves the policy smoothly along a trade-off, from every gap at λ = 10 (83% collisions) to no observed collision at λ = 10,000 (47 cm of tracking error).

At this point the explanation seemed simple: GPI is safer because it knows the noise. That suggests a direct test, and it is where the project stopped going as I expected.

Giving CEC the noise model

The comparison so far mixes two differences: GPI knows the noise level, and GPI plans over an infinite horizon. To separate them, I gave CEC exactly GPI's stage cost, including the calibrated collision probability with the same \(\lambda = 1000\), and kept the rest of CEC: noise-free predictions, ten steps, and IPOPT warm-started from the previous plan. Along its prediction, this is the objective of the ablation's GPI with a deterministic transition, which was about as safe as the full policy (38.5% against 41.5%). So if knowing the noise is what matters, this risk-aware CEC should be about as safe as GPI.

The probability enters as a penalty, not as a constraint. Chance-constrained planners bound it instead (Blackmore et al., 2011; Zhu & Alonso-Mora, 2019), but with a fixed per-step noise level such a bound is just a margin, which had already failed: inside the gap no bound below 51% can be met.

collision rate (%)tracking error (cm)
×0.25×0.5×1×2×0.25×0.5×1×2
CEC1001001001005.66.99.615.8
risk-aware CEC10081.510096.52.231.580.2101
+ constraints0.579100966.441.479.398.3
+ GPI value054.578346.635.234.649.0
tabular GPI05.541.520.57.19.827.949.7
Collisions at the samples; 200 paired episodes per cell. Each "+" row adds to the row above it: "+ constraints" also keeps CEC's obstacle constraints on the noise-free positions, and "+ GPI value" also uses GPI's value function as the terminal cost.

It did not work. The first version made CEC worse: it collided in every episode at noise ×0.25, even though its tracking error fell to 2.2 cm, and in 81.5–100% of episodes at higher noise, with tracking errors up to a meter. Two separate problems turned out to be behind this.

Problem 1: the probability has no slope where the plan is. The collision probability of a sample rises from zero to one within about three standard deviations of the obstacle's edge, a 6 cm band at noise ×0.25. Near C1, consecutive predicted positions are about 17 cm apart, so a plan can put one position just before the band and the next deep inside it, where the probability is one and its gradient is zero. Nothing then pushes the plan back out, and the controller follows the reference straight through C1, 40 cm deep in the median episode; it does the same without any noise.

The risk-aware CEC's ten-step plan into C1, the plan with obstacle constraints, and the planned positions on the collision-probability curve
Noise ×0.25, seed 0, step 13, six steps before the robot enters C1. Circles are the ten planned positions, shaded by the collision probability each one is assigned (white 0, black 1), and the thin dotted orange line is where the robot then went. Left: the plan steps from just outside the inflated C1 to deep inside it and follows the reference through. Middle: with the obstacle constraints kept, the plan stays outside and bends into the C1–C3 gap. Right: the left plan's positions on the probability curve; none of them lies in the ±3σ band where the curve has a slope.

Keeping CEC's obstacle constraints on the noise-free positions restores a gradient everywhere, and with them the controller collides in 1 of 200 episodes at ×0.25, at 6.4 cm of tracking error.

Problem 2: ten steps are too short. At noise ×1, going through the gap costs about \(0.51\,\lambda\) per sample. Within a ten-step window, circling at the minimum speed looks cheaper, because falling behind the reference costs little at first. The robot falls up to 2.5 m behind at the passes of C1 and C2 and collides while catching up, in every episode. A longer window helps only partly (48.5% collisions with 20 steps, 80% with 40), because what happens after the window is still missing from the cost, and that missing cost is exactly GPI's value function. With the value as the terminal cost, as in POLO (Lowrey et al., 2019), the stall disappears and the tracking error falls to 35 cm.

Animated rollouts at noise 1: the risk-aware CEC stalls, the version with GPI's value keeps up, and tabular GPI
Noise ×1, seed 0. Open circles are the plan each controller executes at that step, and the white triangle is the reference. With only the ten-step window (left), the plan curls up at the minimum speed at the passes of C1 and C2, and the robot falls far behind; with GPI's value as the terminal cost (middle), it keeps up.
Making GPI's value usable by IPOPT

GPI's table is multilinear between grid nodes, so its gradient jumps at every cell face, and with it as the terminal cost IPOPT mostly ran into its iteration limit. Instead, at every step I sample GPI's value on a window of 17 × 17 × 46 nodes around the planned end state (0.1 m and 9°, GPI's own spacing near the reference) and use the samples as the control points of a cubic B-spline. That spline has continuous second derivatives and stays within the range of the nearby table values, so the optimizer cannot exploit overshoot. A quadratic penalty outside the window works as a trust region, and the window moves with the plan.

With both fixes, the risk-aware CEC has GPI's objective, GPI's value beyond its horizon, and the same noise model. It still collides much more often than GPI: 54.5% against 5.5% at noise ×0.5 and 78% against 41.5% at ×1 (paired exact McNemar p < 10−13 at both levels). I expected the remaining difference to be about the route, but it was about speed. Both controllers cross the gaps at full speed; GPI gets there directly, while the risk-aware CEC first creeps up to the entrance, at 0.13 m/s on average over the three steps before a crossing against 0.54 m/s for GPI (noise ×0.5). So why does GPI cross the gaps at full speed?

What the samples miss

At noise ×0.5 and ×1, tabular GPI crosses every gap it uses at the maximum speed of 1 m/s, which is half a meter per step. Its two samples then land on either side of the 5 cm gap, and the collision check, which looks only at the samples, sees two safe positions. The gap itself is never checked.

To see what happens between the samples, I checked every step along its path: the noise-free arc from the sample, with that step's noise blended in linearly, at 21 points. Counted this way, GPI collides in 72.5% of episodes at noise ×0.5 instead of 5.5%, and in 84% at ×1 instead of 41.5%. At both levels, in every episode that collides along the path but never at a sample, the first such collision is at a gap crossing. The conclusion does not hinge on how the noise is blended in: the commanded arcs alone, without any noise, enter an obstacle in 62.5% of episodes at ×0.5 and 77% at ×1.

GPI's samples and paths at the C2-C4 gap, and collision rates counted at samples and along the path
Left: twelve crossings of the C2–C4 gap by tabular GPI at noise ×0.5. Dots are the samples, lines the paths between them, and black marks where a path is inside an inflated obstacle. Right: collision rate over 200 episodes, counted at the samples and along the path; shading shows 95% intervals.

In hindsight, GPI optimized exactly what I asked for: its risk term scores only the next sample, and the policy found the loophole. Checking constraints only at discrete points is a known pitfall in trajectory optimization; TrajOpt, for example, checks the swept volume between waypoints (Schulman et al., 2014), and risk-bounded planners bound the collision probability over continuous time (Jasour et al., 2021). It also explains the speed gap from the previous section: dashing was not a quirk of GPI, it was the safest-looking thing to do under the rules.

The fix on GPI's side is a risk term that looks along the path. For every step it takes the largest collision probability at four points of the noise-free arc, with the noise growing linearly over the step (standard deviation \(s\sigma\) at fraction \(s\) of the step). For a full-speed dash through the middle of the gap, this doubles the step's risk at noise ×1 and multiplies it by nine at ×0.5. I retrained GPI with it at every noise level, about three minutes per policy.

Tracking error, collision rate along the path and steps until the first collision for CEC, GPI with each risk, and the hybrid
Collisions counted along the path; 200 paired episodes per point, shading shows 95% intervals. CEC here also constrains four points along each step of its plan. The hybrid is introduced in the next section.

With the path-aware risk, GPI changes its decision where the loophole mattered most: at noise ×1 it detours around both lobes and collides along the path in none of the 200 episodes (against 84%), at 44 cm of tracking error instead of 28. At ×0.5 it keeps the gaps, but now it prices them honestly: at a crossing it predicts a 12.5% chance of a collision along the path and realizes 18%, and over an episode it collides along the path in 64%. At this noise level, a penalty of \(\lambda = 1000\) simply does not make the detour worth it. CEC, with its constraints extended to four points along each step, still collides along the path in every episode.

Handing CEC the route

With the loophole closed, I went back to the original question and gave the risk-aware CEC the path-aware risk in both its stage cost and GPI's value. At noise ×1 it now takes the same detours as GPI and collides along the path in 8% of episodes, at 41 cm of tracking error. The remaining failures are on the left-hand lobe, where it follows the reference down the outer side before turning back, while GPI turns at the top. GPI's value says the outer side is a dead end, but a local optimizer only compares plans near its warm start, and turning at the top is a different homotopy class. Topology-aware planners keep several such candidates (Rösmann et al., 2017); here the natural second candidate is GPI's own plan. At every step, IPOPT also starts from ten steps of the GPI policy, and the controller keeps the cheaper solution. I'll call this the hybrid.

Animated left-hand lobe at noise 1 for the risk-aware CEC with GPI's value, the hybrid, and path-aware GPI
Noise ×1, the left-hand lobe, all with the path-aware risk; seed 0 is animated over nineteen other seeds. Open circles are the executed plan, and the dashed gray plan is the hybrid's other start. The two CEC variants move identically until the top of the lobe, where the plan started from GPI becomes much cheaper; the hybrid switches to it and turns back like GPI.

At noise ×1 the hybrid collides along the path in 4% of episodes and tracks 9 cm closer to the reference than path-aware GPI (35 against 44 cm, closer in every one of the 200 paired episodes). At ×0.25 and ×0.5 it is both safer and closer: 0.5% against 11% and 52% against 64% of episodes with a collision along the path, at 6.5 against 10.0 cm and 8.4 against 12.9 cm. The noise ×1 result holds on seeds used nowhere else on this page.

seeds 0–199seeds 1000–1199
noise ×1samples (%)path (%)error (cm)path (%)error (cm)
CEC1001009.61009.6
GPI, sample risk41.58427.98528.0
GPI, path risk0044.3044.2
hybrid3.5435.2235.2
Collision rates counted at the samples and along the path, and tracking error, at noise ×1; 200 paired episodes per cell.

At ×2, however, the hybrid fails: 72% against 20.5%. The reason is that its predictions are noise-free, like every CEC here. GPI with noise-free transitions and the same path-aware risk shows the same pattern, 42% at ×2 against 20.5% with noisy transitions (paired p = 6 × 10−7), and starting IPOPT only from GPI's plan makes it worse, not better (81%). This is the case the earlier ablation could not see: once the risk looks along the path and the noise reaches 8 cm per step, the noise in the transition matters after all.

Collision rate along the path versus tracking error at noise 1 for CEC margins, GPI with each risk, and the hybrid
Each point is a separately configured controller evaluated on the same 200 episodes at noise ×1, with collisions counted along the path. The dashed curve is GPI's earlier λ sweep, recounted along the path.

Sweeping the penalty compares them at matched safety. With \(\lambda = 10\) or 100, every controller takes the gaps and collides along the path in 98–100% of episodes. At \(\lambda = 1000\), the hybrid and path-aware GPI are nearly equally safe (4% and 0%), and the hybrid tracks 9 cm closer; at \(\lambda = 10{,}000\) the hybrid falls behind (35% against 0%). The refinement is not cheap either: 272 ms per step on average, within the 500 ms control period, against 2.7 ms for path-aware GPI.

So, does knowing the noise help?

For CEC, knowing the noise was not enough on its own. It needed the calibrated risk, a constraint to give that risk a gradient, GPI's value to see past its horizon, and GPI's plan to change route. With all four it is safer than path-aware GPI at low noise and nearly as safe at noise ×1, and it tracks closer at every noise level; at noise ×2 its noise-free predictions cost it that safety. In the end, global planning chose the route and local optimization refined it.

The part I did not expect was how much of the original story depended on the collision check. GPI's low collision rates at moderate noise looked like the payoff of a well-calibrated risk, and they partly were, but they also came from a policy that learned exactly where the check does not look. None of the aggregate numbers showed this. It only surfaced because two controllers with the same objective crossed the same gap at very different speeds, and I asked why.

What this does and does not show


Built on the starter code of ECE 276B (Nikolay Atanasov, UC San Diego).