feat(fusion): fuse Doppler velocity in ESKF

soc/software/gps_system/tests/test_gps_tracking_measurement.c

@@ -143,6 +143,7 @@ static void test_velocity_wls(void)
 	}
 
 	double solved_velocity[3];
+	double solved_velocity_covariance[3][3];
 	double solved_clock_drift;
 	double residual_rms;
 	assert(gps_tracking_velocity_wls(
@@ -153,6 +154,7 @@ static void test_velocity_wls(void)
 		measurements,
 		TEST_SATELLITES,
 		solved_velocity,
+		solved_velocity_covariance,
 		&solved_clock_drift,
 		&residual_rms));
 
@@ -168,6 +170,16 @@ static void test_velocity_wls(void)
 		expected_clock_drift_mps,
 		1.0e-9));
 	assert(residual_rms < 1.0e-9);
+	for(unsigned int component = 0u; component < 3u; component++)
+	{
+		assert(isfinite(
+			solved_velocity_covariance[component][component]));
+		assert(solved_velocity_covariance[component][component] > 0.0);
+	}
+	assert(close_to(
+		solved_velocity_covariance[0][1],
+		solved_velocity_covariance[1][0],
+		1.0e-12));
 }
 
 int main(void)