feat(gps): split Doppler paths for ESKF and PPS

2026-07-26 01:59:08 +03:00
parent 49b8609bb4
commit 23650be635
35 changed files with 3884 additions and 82 deletions
@@ -78,6 +78,14 @@ static void test_cn0_and_mailbox_decode(void)
 		measurement.pseudorange_rate_mps,
 		expected_range_rate,
 		1.0e-9));
+	assert(close_to(
+		measurement.filtered_doppler_hz,
+		measurement.doppler_hz,
+		1.0e-12));
+	assert(close_to(
+		measurement.filtered_pseudorange_rate_mps,
+		measurement.pseudorange_rate_mps,
+		1.0e-12));

 	registers.MEASUREMENT_META &=
 		~(1u << GPS_TRCK_MEAS_META_LOCK_POS);
@@ -180,6 +188,86 @@ static void test_velocity_wls(void)
 		solved_velocity_covariance[0][1],
 		solved_velocity_covariance[1][0],
 		1.0e-12));
+	{
+		double clean_velocity[3];
+		double clean_covariance[3][3];
+		double clean_clock_drift;
+		double clean_residual_rms;
+		uint32_t clean_excluded_mask = UINT32_MAX;
+		assert(gps_tracking_velocity_wls_robust(
+			receiver_position,
+			satellite_positions,
+			satellite_velocities,
+			satellite_clock_drift,
+			measurements,
+			TEST_SATELLITES,
+			clean_velocity,
+			clean_covariance,
+			&clean_clock_drift,
+			&clean_residual_rms,
+			&clean_excluded_mask));
+		assert(clean_excluded_mask == 0u);
+		for(unsigned int component = 0u; component < 3u; component++)
+		{
+			assert(close_to(
+				clean_velocity[component],
+				solved_velocity[component],
+				1.0e-12));
+		}
+	}
+
+	/* One gross carrier burst must be isolated before aggregate ESKF fusion. */
+	measurements[2].pseudorange_rate_mps += 12.0;
+	double plain_velocity[3];
+	double plain_covariance[3][3];
+	double plain_clock_drift;
+	double plain_residual_rms;
+	assert(gps_tracking_velocity_wls(
+		receiver_position,
+		satellite_positions,
+		satellite_velocities,
+		satellite_clock_drift,
+		measurements,
+		TEST_SATELLITES,
+		plain_velocity,
+		plain_covariance,
+		&plain_clock_drift,
+		&plain_residual_rms));
+	double robust_velocity[3];
+	double robust_covariance[3][3];
+	double robust_clock_drift;
+	double robust_residual_rms;
+	uint32_t excluded_mask = 0u;
+	assert(gps_tracking_velocity_wls_robust(
+		receiver_position,
+		satellite_positions,
+		satellite_velocities,
+		satellite_clock_drift,
+		measurements,
+		TEST_SATELLITES,
+		robust_velocity,
+		robust_covariance,
+		&robust_clock_drift,
+		&robust_residual_rms,
+		&excluded_mask));
+	assert(excluded_mask == (1u << 2));
+	double plain_error_square = 0.0;
+	for(unsigned int component = 0u; component < 3u; component++)
+	{
+		double plain_error =
+			plain_velocity[component] - expected_velocity[component];
+		plain_error_square += plain_error * plain_error;
+		assert(close_to(
+			robust_velocity[component],
+			expected_velocity[component],
+			1.0e-8));
+	}
+	assert(sqrt(plain_error_square) > 1.0);
+	assert(close_to(
+		robust_clock_drift,
+		expected_clock_drift_mps,
+		1.0e-8));
+	assert(robust_residual_rms < 1.0e-8);
 }

 int main(void)