fix(data_augmentation): quintic kinematic consistency

[rej55]

What

Fixes five kinematic inconsistencies in the quintic augmentation path
(StatePerturbation in utils/data_augmentation.py), one per commit:

#	commit	issue
1	use the current heading for the quintic start tangent	theta0 came from the chord to the GT point 1 s ahead, which lags the tangent and ignores the heading perturbation
2	use signed tangential terms in the quintic boundary conditions	v0/a0/vT/aT were vector magnitudes: sign dropped, and the centripetal term double-counted
3	keep ay consistent with the perturbed speed	ay = vx * yaw_rate broke by dvx * yaw_rate when vx was perturbed
4	stop the heading perturbation from injecting sideslip	the velocity vector kept pointing along the old heading, i.e. a sideslip equal to the heading perturbation
5	scale the past before building the interpolated future	the ego_past_noise_std scaling was applied after the target had already been built from the unscaled state

augment_type defaults to "quintic", so this is the path actually used in training.

Why this matters

interpolation_future_trajectory() was not the identity even with zero
perturbation. Feeding it a synthetic GT plus a current state that exactly matches
that GT should return the GT unchanged; instead it rewrote the first 2 s by up to
1.1 m. So the augmentation was corrupting the training target for accelerating,
braking and turning samples independently of the perturbation itself — every sample
with augment_prob hit and |vx| >= 2 m/s.

The two root causes were both in how the boundary conditions were built:

• torch.norm() on (ax, ay) dropped the sign, so a braking sample was handed a
  positive longitudinal acceleration.
• The same norm folded the lateral component in. ay is the centripetal term
  v * omega, and the quintic already expresses that through its
  -v0*sin(theta0)*omega0 term, so it was counted twice. On a constant-speed curve
  the true tangential acceleration is exactly 0 while ||(ax, ay)|| = v^2 * kappa
  — 3.84 m/s² at 8 m/s on a 0.06 1/m curve.

Verification

Zero-perturbation identity

Synthetic GT with a current state consistent with it, so the interpolation must be
the identity. Max position error over the bridged 0–2 s window:

case	before	after
8 m/s straight, a=0	0.000 m	0.000 m
8 m/s straight, a=−2 (braking)	0.553 m	0.031 m
8 m/s, kappa=0.03 constant-speed turn	0.533 m	0.030 m
8 m/s, kappa=0.06 constant-speed turn	1.102 m	0.065 m
3 m/s, kappa=0.08 low-speed turn	0.200 m	0.011 m
2.5 m/s, kappa=0.05, a=−1.5	0.235 m	0.042 m

Worst case 1.102 m → 0.065 m. The residual is the quintic polynomial approximating
a circular arc, which is inherent to the method.

Component consistency

quantity	before	after
sideslip from a 0.2 rad heading perturbation at 8 m/s	vy = −1.589 m/s (11.5°)	0.000 m/s
same at 12 m/s	vy = −2.384 m/s	0.000 m/s
ay − vx*yaw_rate at yaw_rate 0.48 rad/s, dvx = ±1	±0.480 m/s²	0.000 m/s²
state vx vs target initial speed, scale = 0.8 / 1.2	∓1.60 m/s	∓0.02 m/s

End-to-end

Batch of 64 with real random sampling (53 above the 2 m/s gate), smoothing on:

• no NaN / Inf in ego_current_state, ego_agent_future, ego_agent_past
• ego pose still exactly at the origin with heading 0 after centric_transform
• vy max 0.5324 m/s — exactly the ±0.5 explicit vy perturbation range, i.e. the
  heading-induced component is gone
• |ay − vx*yaw_rate| max 0.1032 m/s² — exactly the ±0.1 explicit ay noise range

Tests

tests/test_data_augmentation.py: 32 failed / 26 passed before and after — no
new failures. Those 32 are pre-existing on tier4-main: the tests
call StatePerturbation() with no arguments while the constructor requires five.

Behaviour changes to be aware of

• Training targets change, so a checkpoint trained before this cannot be compared
  to one trained after without retraining.
• The ego_past_noise_std semantics shift slightly. The scaled speed now shapes
  the first 2 s of the target, which is the point — the sample means "the vehicle
  really was travelling 10% faster", so its plan should start there too. The
  effective strength of that knob therefore changes.

Not in scope

Deliberately left alone, each worth a separate discussion:

• The ±0.5 m/s vy perturbation range. It is a tuning decision, not a consistency
  bug, but Autoware reports twist.linear.y ~ 0 so it is unreachable in deployment
  (12.8° of sideslip at 2.2 m/s). Worth revisiting.
• The |vx| >= 2.0 m/s gate that excludes the whole low-speed / standstill regime.
  Removing it needs a different formulation, not a different threshold: the method
  spreads the lateral offset over arc length, and a stopped vehicle has none.
• ego_agent_past coordinate transform being commented out in centric_transform.
• No mitigation for the history/recovery shortcut (copycat) that the upstream
  reference implementation addresses with M/N randomisation and a past-history bump.

Relation to the dev PR

The identical five commits are also proposed against dev; this branch is the same
set cherry-picked onto tier4-main (no conflicts, identical resulting diff).

Note the two branches already differ in this file for unrelated reasons, and this PR
touches neither:

• the 4-column neighbors_future branch in centric_transform — present on
  tier4-main, absent on dev

🤖 Generated with Claude Code

[Owen-Liuyuxuan]

@rej55
I noticed that there are several problem.

1. in order for directly scaling XY to match the velocity difference, there are several assumption:
   • Ego past's t=0 is already in (0, 0) -- this is correct in default PR
   • This is a bug that will also affect tau-augmentation from my PR
2. but self.augment(inputs) also modify the velocity with a diff, should we handle that, or just keep that discontinuity?