[AUD-017][High] Correct AHRS magnetic-gradient Jacobian terms

[gabrielsantosphilips]

Audit finding

• ID: AUD-017
• Status: Verified defect
• Severity: High
• Confidence: High
• Audited revision: 2f479320d805a1f9f35ebe4afaaeeded48913a94

Problem

GradientMag() contains two Jacobian terms inconsistent with its own sixth magnetic residual. For

f6 = 2 bx (qw qy + qx qz) + bz (1 - 2 qx^2 - 2 qy^2) - mz,

correct derivatives include df6/dqx = 2 bx qz - 4 bz qx and df6/dqy = 2 bx qw - 4 bz qy. The implementation uses different terms.

Source:

numerical-toolbox-cpp/numerical/filters/active/AhrsMadgwickMahony.hpp

Lines 149 to 181 in 2f47932

	T gx{ T(2) * bz * qz * f4 + (T(2) * bx * qy + T(2) * bz * qw) * f5 + (T(2) * bx * qz - T(2) * bz * qw) * f6 };
	T gy{ (-T(4) * bx * qy - T(2) * bz * qw) * f4 + (T(2) * bx * qx + T(2) * bz * qz) * f5 + (T(2) * bx * qw + T(2) * bz * qy) * f6 };
	T gz{ (-T(4) * bx * qz + T(2) * bz * qx) * f4 + (-T(2) * bx * qw + T(2) * bz * qy) * f5 + T(2) * bx * qx * f6 };
	T invNorm{ SafeInvSqrt(gw * gw + gx * gx + gy * gy + gz * gz) };
	return math::Quaternion<T>{ gw * invNorm, gx * invNorm, gy * invNorm, gz * invNorm };
	}
	template<typename T, AhrsMode M>
	void AhrsFilter<T, M>::IntegrateGyro(const math::Vector3<T>& gyro)
	{
	math::Quaternion<T> qGyro{ T{}, gyro.at(0, 0), gyro.at(1, 0), gyro.at(2, 0) };
	math::Quaternion<T> qDot{ q * qGyro };
	q.w += T(0.5) * qDot.w * Ts;
	q.x += T(0.5) * qDot.x * Ts;
	q.y += T(0.5) * qDot.y * Ts;
	q.z += T(0.5) * qDot.z * Ts;
	q.Normalize();
	}
	template<typename T, AhrsMode M>
	void AhrsFilter<T, M>::MahonyStep(const math::Vector3<T>& gyro, const math::Vector3<T>& e)
	{
	integralFb.at(0, 0) += Ki * e.at(0, 0) * Ts;
	integralFb.at(1, 0) += Ki * e.at(1, 0) * Ts;
	integralFb.at(2, 0) += Ki * e.at(2, 0) * Ts;
	T wx{ gyro.at(0, 0) + Kp * e.at(0, 0) + integralFb.at(0, 0) };
	T wy{ gyro.at(1, 0) + Kp * e.at(1, 0) + integralFb.at(1, 0) };
	T wz{ gyro.at(2, 0) + Kp * e.at(2, 0) + integralFb.at(2, 0) };
	math::Quaternion<T> qGyro{ T{}, wx, wy, wz };
	math::Quaternion<T> qDot{ q * qGyro };
	q.w += T(0.5) * qDot.w * Ts;
	q.x += T(0.5) * qDot.x * Ts;

Axis-aligned magnetic-field tests mask the error; arbitrary tilted MARG updates follow the wrong descent direction.

Acceptance criteria

• Correct the analytic Jacobian.
• Verify every gradient component against bounded finite differences.
• Add deterministic tilted quaternion/magnetic-field cases.
• Confirm convergence against an independent orientation reference.

[gabrielfrasantos]

Fixed and merged in #325 (d71b05f on main).

Checked all twelve partials against the filter's own residuals before changing anything. The nine for f4 and f5 are correct; exactly the two reported terms were wrong, and they match Madgwick's published Jacobian once corrected:

term	required	was
df6/dqx	2*bx*qz - 4*bz*qx	2*bx*qz - 2*bz*qw
df6/dqy	2*bx*qw - 4*bz*qy	2*bx*qw + 2*bz*qy

On verifying against bounded finite differences: my first attempt was a MARG convergence test on a tilted field, and it passed with the old Jacobian too, so it was worthless as a regression test. A fixed point is where the residual vanishes, which does not depend on the gradient being right — only the path there does. So the gradient itself has to be the thing under test.

GradientGravity and GradientMag are therefore public statics now, and the test compares the analytic gradient against central differences of the magnetic objective over several non-axis-aligned orientations. Confirmed it discriminates: it fails against the previous coefficients and passes against the corrected ones. Across 2000 random tilted orientations the worst relative error is 5.1e-10, against roughly the magnitude of the gradient itself before.

Tests added: magnetic_gradient_matches_finite_differences and converges_for_arbitrary_tilted_magnetic_field. The second does not catch the defect on its own, for the reason above; it is kept as an end-to-end check, not as the regression guard.

---

Generated by Claude Code