Fix yaw gradient bug

[vegard-solum-4ss]

fix: yaw gradient wrt. the attitude error state

Summary

_yaw_gradient returned the gradient of the yaw angle wrt. the global scaled
Gibbs parameterization, a = 2 * q_v / q_w. The filters use it as the heading row of
the measurement matrix H, where the gradient must be taken wrt. the local
body-frame attitude error — the error state dx[6:9] that _reset applies
multiplicatively via _update_quaternion_with_gibbs2.

The heading innovation itself was always correct, so heading stayed observable and the
filters converged. What was wrong is how that innovation is distributed into the other
states, which mainly cost roll and pitch accuracy.

Affects heading aiding in PVAMEKF, VAMEKF and AMEKF.

The bug

The two parameterizations coincide only at the identity attitude. At roll = pitch = 0
they differ by a scalar factor cos(yaw / 2)**2 — so at level attitude and yaw = 90°
the old gradient was exactly half the correct value. Away from level they disagree in
direction as well:

q = [0.1865, -0.1960, 0.9500, 0.1556]

  _yaw_gradient (old)  : [-0.196688, -0.029604, -0.046914]
  finite difference    : [ 0.0,       0.268996, -1.065915]

The old gradient also has a spurious x-component. The correct gradient has none: yaw is
atan2(R_nb[1, 0], R_nb[0, 0]), a function of the body x-axis alone, and a rotation
about that same axis does not move it.

The fix

The gradient wrt. the local body-frame error is the bottom row of the Euler angle rate
transformation matrix:

d(yaw)/da = [0, sin(roll) / cos(pitch), cos(roll) / cos(pitch)]

Implemented in terms of the quaternion, to avoid computing Euler angles:

cos_pitch_sq = r_00**2 + r_10**2

dhda_y = -(r_00 * r_12 - r_10 * r_02) / cos_pitch_sq  # sin(roll) / cos(pitch)
dhda_z = (r_00 * r_11 - r_10 * r_01) / cos_pitch_sq   # cos(roll) / cos(pitch)

return np.array([0.0, dhda_y, dhda_z])

Conditioning is unchanged: in the old expression the 1 / u_x**2 factor cancelled
against 1 / (1 + u**2), leaving the same cos(pitch)**2 denominator. Both are
singular only at cos(pitch) = 0, where the yaw angle itself is undefined.

Impact

Forward-filter attitude RMSE [rad] on benchmark_full_pva_beat_202311A with heading and
gravity-reference aiding, 10 Hz, 30 min, 600 s warmup discarded:

	roll	pitch	yaw
before	0.00891	0.01176	0.00127
after	0.00729	0.00273	0.00127

A 4.3x improvement in pitch and 1.2x in roll. Yaw is unchanged, as expected — it was
directly observable either way.

Tests

test__dhda hardcoded the old values, so it is replaced by two tests:

• test__yaw_gradient — six attitudes checked against the closed form above.
• test__yaw_gradient_vs_finite_difference — five random quaternions checked against a
  central difference of _yaw_from_quaternion under _update_quaternion_with_gibbs2.
  This one pins the parameterization that was wrong, so the bug cannot return silently.

Full suite passes (263 tests). black and isort clean.

Notes

• Found while investigating a separate issue, where the RTS smoother produced worse
  estimates than the forward filter. This fix is not the cause of that; the two are
  independent and that work is on its own branch.
• The # type: ignore[no-any-return] is no longer needed, as the function now returns a
  concrete np.array.