END_TEST
START_TEST(test_wgsecef2llh2ecef)
{
double llh[3];
double ecef[3];
wgsecef2llh(ecefs[_i], llh);
wgsllh2ecef(llh, ecef);
for (int n=0; n<3; n++) {
fail_unless(!isnan(llh[n]), "NaN in LLH output from conversion.");
fail_unless(!isnan(ecef[n]), "NaN in ECEF output from conversion.");
}
for (int n=0; n<3; n++) {
double err = fabs(ecef[n] - ecefs[_i][n]);
fail_unless(err < MAX_DIST_ERROR_M,
"Converting WGS84 ECEF to LLH and back again does not return the "
"original values.\n"
"Initial ECEF: %f, %f, %f\n"
"LLH: %f, %f, %f\n"
"Final ECEF: %f, %f, %f\n"
"X error (mm): %g\nY error (mm): %g\nZ error (mm): %g",
ecefs[_i][0], ecefs[_i][1], ecefs[_i][2],
R2D*llh[0], R2D*llh[1], llh[2],
ecef[0], ecef[1], ecef[2],
(ecef[0] - ecefs[_i][0])*1e3,
(ecef[1] - ecefs[_i][1])*1e3,
(ecef[2] - ecefs[_i][2])*1e3
);
}
}
END_TEST
START_TEST(test_wgsecef2llh)
{
double llh[3];
wgsecef2llh(ecefs[_i], llh);
for (int n=0; n<3; n++) {
fail_unless(!isnan(llh[n]), "NaN in output from wgsecef2llh.");
}
double lat_err = fabs(llh[0] - llhs[_i][0]);
double lon_err = fabs(llh[1] - llhs[_i][1]);
double hgt_err = fabs(llh[2] - llhs[_i][2]);
fail_unless((lat_err < MAX_ANGLE_ERROR_RAD) &&
(lon_err < MAX_ANGLE_ERROR_RAD) &&
(hgt_err < MAX_DIST_ERROR_M),
"Conversion from WGS84 ECEF to LLH has >1e-6 {rad, m} error:\n"
"ECEF: %f, %f, %f\n"
"Lat error (arc sec): %g\nLon error (arc sec): %g\nH error (mm): %g",
ecefs[_i][0], ecefs[_i][1], ecefs[_i][2],
(llh[0] - llhs[_i][0])*(R2D*3600),
(llh[1] - llhs[_i][1])*(R2D*3600),
(llh[2] - llhs[_i][2])*1e3
);
}
END_TEST
START_TEST(test_wgsllh2ecef2llh)
{
double ecef[3];
double llh[3];
wgsllh2ecef(llhs[_i], ecef);
wgsecef2llh(ecef, llh);
for (int n=0; n<3; n++) {
fail_unless(!isnan(llh[n]), "NaN in LLH output from conversion.");
fail_unless(!isnan(ecef[n]), "NaN in ECEF output from conversion.");
}
double lat_err = fabs(llh[0] - llhs[_i][0]);
double lon_err = fabs(llh[1] - llhs[_i][1]);
double hgt_err = fabs(llh[2] - llhs[_i][2]);
fail_unless((lat_err < MAX_ANGLE_ERROR_RAD) &&
(lon_err < MAX_ANGLE_ERROR_RAD) &&
(hgt_err < MAX_DIST_ERROR_M),
"Converting WGS84 LLH to ECEF and back again does not return the "
"original values.\n"
"Initial LLH: %f, %f, %f\n"
"ECEF: %f, %f, %f\n"
"Final LLH: %f, %f, %f\n"
"Lat error (arc sec): %g\nLon error (arc sec): %g\nH error (mm): %g",
R2D*llhs[_i][0], R2D*llhs[_i][1], llhs[_i][2],
ecef[0], ecef[1], ecef[2],
R2D*llh[0], R2D*llh[1], llh[2],
(llh[0] - llhs[_i][0])*(R2D*3600),
(llh[1] - llhs[_i][1])*(R2D*3600),
(llh[2] - llhs[_i][2])*1e3
);
}
/**
* Performs a simulation step for the given duration, by moving
* our simulated position in a circle at a given radius and speed
* around the simulation's center point.
*/
void simulation_step_position_in_circle(double elapsed)
{
/* Update the angle, making a small angle approximation. */
sim_state.angle += (sim_settings.speed * elapsed) / sim_settings.radius;
if (sim_state.angle > 2*M_PI) {
sim_state.angle = 0;
}
double pos_ned[3] = {
sim_settings.radius * sin(sim_state.angle),
sim_settings.radius * cos(sim_state.angle),
0
};
/* Fill out position simulation's gnss_solution pos_ECEF, pos_LLH structures */
wgsned2ecef_d(pos_ned,
sim_settings.base_ecef,
sim_state.pos);
/* Calculate an accurate baseline for simulating RTK */
vector_subtract(3,
sim_state.pos,
sim_settings.base_ecef,
sim_state.baseline);
/* Add gaussian noise to PVT position */
double* pos_ecef = sim_state.noisy_solution.pos_ecef;
double pos_variance = sim_settings.pos_sigma * sim_settings.pos_sigma;
pos_ecef[0] = sim_state.pos[0] + rand_gaussian(pos_variance);
pos_ecef[1] = sim_state.pos[1] + rand_gaussian(pos_variance);
pos_ecef[2] = sim_state.pos[2] + rand_gaussian(pos_variance);
wgsecef2llh(sim_state.noisy_solution.pos_ecef, sim_state.noisy_solution.pos_llh);
/* Calculate Velocity vector tangent to the sphere */
double noisy_speed = sim_settings.speed +
rand_gaussian(sim_settings.speed_sigma *
sim_settings.speed_sigma);
sim_state.noisy_solution.vel_ned[0] = noisy_speed * cos(sim_state.angle);
sim_state.noisy_solution.vel_ned[1] = noisy_speed * -1.0 * sin(sim_state.angle);
sim_state.noisy_solution.vel_ned[2] = 0.0;
wgsned2ecef(sim_state.noisy_solution.vel_ned,
sim_state.noisy_solution.pos_ecef,
sim_state.noisy_solution.vel_ecef);
}
s8 calc_PVT(const u8 n_used,
const navigation_measurement_t const nav_meas[n_used],
gnss_solution *soln,
dops_t *dops)
{
/* Initial state is the center of the Earth with zero velocity and zero
* clock error, if we have some a priori position estimate we could use
* that here to speed convergence a little on the first iteration.
*
* rx_state format:
* pos[3], clock error, vel[3], intermediate freq error
*/
static double rx_state[8];
double H[4][4];
soln->valid = 0;
soln->n_used = n_used; // Keep track of number of working channels
/* reset state to zero !? */
for(u8 i=4; i<8; i++) {
rx_state[i] = 0;
}
double update;
u8 iters;
/* Newton-Raphson iteration. */
for (iters=0; iters<PVT_MAX_ITERATIONS; iters++) {
if ((update = pvt_solve(rx_state, n_used, nav_meas, H)) > 0) {
break;
}
}
/* Compute various dilution of precision metrics. */
compute_dops((const double(*)[4])H, rx_state, dops);
soln->err_cov[6] = dops->gdop;
/* Populate error covariances according to layout in definition
* of gnss_solution struct.
*/
soln->err_cov[0] = H[0][0];
soln->err_cov[1] = H[0][1];
soln->err_cov[2] = H[0][2];
soln->err_cov[3] = H[1][1];
soln->err_cov[4] = H[1][2];
soln->err_cov[5] = H[2][2];
if (iters >= PVT_MAX_ITERATIONS) {
/* Reset state if solution fails */
rx_state[0] = 0;
rx_state[1] = 0;
rx_state[2] = 0;
return -4;
}
/* Save as x, y, z. */
for (u8 i=0; i<3; i++) {
soln->pos_ecef[i] = rx_state[i];
soln->vel_ecef[i] = rx_state[4+i];
}
wgsecef2ned(soln->vel_ecef, soln->pos_ecef, soln->vel_ned);
/* Convert to lat, lon, hgt. */
wgsecef2llh(rx_state, soln->pos_llh);
soln->clock_offset = rx_state[3] / GPS_C;
soln->clock_bias = rx_state[7] / GPS_C;
/* Time at receiver is TOT plus time of flight. Time of flight is eqaul to
* the pseudorange minus the clock bias. */
soln->time = nav_meas[0].tot;
soln->time.tow += nav_meas[0].pseudorange / GPS_C;
/* Subtract clock offset. */
soln->time.tow -= rx_state[3] / GPS_C;
soln->time = normalize_gps_time(soln->time);
u8 ret;
if ((ret = filter_solution(soln, dops))) {
memset(soln, 0, sizeof(*soln));
/* Reset state if solution fails */
rx_state[0] = 0;
rx_state[1] = 0;
rx_state[2] = 0;
return -ret;
}
soln->valid = 1;
return 0;
}
END_TEST
START_TEST(test_random_wgsecef2llh2ecef)
{
double ecef_init[3];
double llh[3];
double ecef[3];
seed_rng();
ecef_init[0] = frand(-4*EARTH_A, 4*EARTH_A);
ecef_init[1] = frand(-4*EARTH_A, 4*EARTH_A);
ecef_init[2] = frand(-4*EARTH_A, 4*EARTH_A);
wgsecef2llh(ecef_init, llh);
wgsllh2ecef(llh, ecef);
for (int n=0; n<3; n++) {
fail_unless(!isnan(llh[n]), "NaN in LLH output from conversion.");
fail_unless(!isnan(ecef[n]), "NaN in ECEF output from conversion.");
}
for (int n=0; n<3; n++) {
double err = fabs(ecef[n] - ecef_init[n]);
fail_unless(err < MAX_DIST_ERROR_M,
"Converting random WGS84 ECEF to LLH and back again does not return the "
"original values.\n"
"Initial ECEF: %f, %f, %f\n"
"LLH: %f, %f, %f\n"
"Final ECEF: %f, %f, %f\n"
"X error (mm): %g\nY error (mm): %g\nZ error (mm): %g",
ecef_init[0], ecef_init[1], ecef_init[2],
R2D*llh[0], R2D*llh[1], llh[2],
ecef[0], ecef[1], ecef[2],
(ecef[0] - ecef_init[0])*1e3,
(ecef[1] - ecef_init[1])*1e3,
(ecef[2] - ecef_init[2])*1e3
);
}
fail_unless((R2D*llh[0] >= -90) && (R2D*llh[0] <= 90),
"Converting random WGS84 ECEF gives latitude out of bounds.\n"
"Initial ECEF: %f, %f, %f\n"
"LLH: %f, %f, %f\n",
ecef_init[0], ecef_init[1], ecef_init[2],
R2D*llh[0], R2D*llh[1], llh[2]
);
fail_unless((R2D*llh[1] >= -180) && (R2D*llh[1] <= 180),
"Converting random WGS84 ECEF gives longitude out of bounds.\n"
"Initial ECEF: %f, %f, %f\n"
"LLH: %f, %f, %f\n",
ecef_init[0], ecef_init[1], ecef_init[2],
R2D*llh[0], R2D*llh[1], llh[2]
);
fail_unless(llh[2] > -EARTH_A,
"Converting random WGS84 ECEF gives height out of bounds.\n"
"Initial ECEF: %f, %f, %f\n"
"LLH: %f, %f, %f\n",
ecef_init[0], ecef_init[1], ecef_init[2],
R2D*llh[0], R2D*llh[1], llh[2]
);
}
END_TEST
START_TEST(test_random_wgsllh2ecef2llh)
{
double ecef[3];
double llh_init[3];
double llh[3];
seed_rng();
llh_init[0] = D2R*frand(-90, 90);
llh_init[1] = D2R*frand(-180, 180);
llh_init[2] = frand(-0.5 * EARTH_A, 4 * EARTH_A);
wgsllh2ecef(llh_init, ecef);
wgsecef2llh(ecef, llh);
for (int n=0; n<3; n++) {
fail_unless(!isnan(llh[n]), "NaN in LLH output from conversion.");
fail_unless(!isnan(ecef[n]), "NaN in ECEF output from conversion.");
}
double lat_err = fabs(llh[0] - llh_init[0]);
double lon_err = fabs(llh[1] - llh_init[1]);
double hgt_err = fabs(llh[2] - llh_init[2]);
fail_unless((lat_err < MAX_ANGLE_ERROR_RAD) &&
(lon_err < MAX_ANGLE_ERROR_RAD) &&
(hgt_err < MAX_DIST_ERROR_M),
"Converting random WGS84 LLH to ECEF and back again does not return the "
"original values.\n"
"Initial LLH: %f, %f, %f\n"
"ECEF: %f, %f, %f\n"
"Final LLH: %f, %f, %f\n"
"Lat error (arc sec): %g\nLon error (arc sec): %g\nH error (mm): %g",
R2D*llh_init[0], R2D*llh_init[1], llh_init[2],
ecef[0], ecef[1], ecef[2],
R2D*llh[0], R2D*llh[1], llh[2],
(llh[0] - llh_init[0])*(R2D*3600),
(llh[1] - llh_init[1])*(R2D*3600),
(llh[2] - llh_init[2])*1e3
);
fail_unless((R2D*llh[0] >= -90) && (R2D*llh[0] <= 90),
"Converting random WGS84 ECEF gives latitude out of bounds.\n"
"ECEF: %f, %f, %f\n"
"LLH: %f, %f, %f\n",
ecef[0], ecef[1], ecef[2],
R2D*llh[0], R2D*llh[1], llh[2]
);
fail_unless((R2D*llh[1] >= -180) && (R2D*llh[1] <= 180),
"Converting random WGS84 ECEF gives longitude out of bounds.\n"
"ECEF: %f, %f, %f\n"
"LLH: %f, %f, %f\n",
ecef[0], ecef[1], ecef[2],
R2D*llh[0], R2D*llh[1], llh[2]
);
fail_unless(llh[2] > -EARTH_A,
"Converting random WGS84 ECEF gives height out of bounds.\n"
"ECEF: %f, %f, %f\n"
"LLH: %f, %f, %f\n",
ecef[0], ecef[1], ecef[2],
R2D*llh[0], R2D*llh[1], llh[2]
);
}
static void test_wgs_conversion_functions(void)
{
#define D2R DEG_TO_RAD_DOUBLE
/* Maximum allowable error in quantities with units of length (in meters). */
#define MAX_DIST_ERROR_M 1e-6
/* Maximum allowable error in quantities with units of angle (in sec of arc).
* 1 second of arc on the equator is ~31 meters. */
#define MAX_ANGLE_ERROR_SEC 1e-7
#define MAX_ANGLE_ERROR_RAD (MAX_ANGLE_ERROR_SEC*(D2R/3600.0))
/* Semi-major axis. */
#define EARTH_A 6378137.0
/* Semi-minor axis. */
#define EARTH_B 6356752.31424517929553985595703125
#define NUM_COORDS 10
Vector3d llhs[NUM_COORDS];
llhs[0] = Vector3d(0, 0, 0); /* On the Equator and Prime Meridian. */
llhs[1] = Vector3d(0, 180*D2R, 0); /* On the Equator. */
llhs[2] = Vector3d(0, 90*D2R, 0); /* On the Equator. */
llhs[3] = Vector3d(0, -90*D2R, 0); /* On the Equator. */
llhs[4] = Vector3d(90*D2R, 0, 0); /* North pole. */
llhs[5] = Vector3d(-90*D2R, 0, 0); /* South pole. */
llhs[6] = Vector3d(90*D2R, 0, 22); /* 22m above the north pole. */
llhs[7] = Vector3d(-90*D2R, 0, 22); /* 22m above the south pole. */
llhs[8] = Vector3d(0, 0, 22); /* 22m above the Equator and Prime Meridian. */
llhs[9] = Vector3d(0, 180*D2R, 22); /* 22m above the Equator. */
Vector3d ecefs[NUM_COORDS];
ecefs[0] = Vector3d(EARTH_A, 0, 0);
ecefs[1] = Vector3d(-EARTH_A, 0, 0);
ecefs[2] = Vector3d(0, EARTH_A, 0);
ecefs[3] = Vector3d(0, -EARTH_A, 0);
ecefs[4] = Vector3d(0, 0, EARTH_B);
ecefs[5] = Vector3d(0, 0, -EARTH_B);
ecefs[6] = Vector3d(0, 0, (EARTH_B+22));
ecefs[7] = Vector3d(0, 0, -(EARTH_B+22));
ecefs[8] = Vector3d((22+EARTH_A), 0, 0);
ecefs[9] = Vector3d(-(22+EARTH_A), 0, 0);
hal.console->printf("TESTING wgsllh2ecef\n");
for (int i = 0; i < NUM_COORDS; i++) {
Vector3d ecef;
wgsllh2ecef(llhs[i], ecef);
double x_err = fabs(ecef[0] - ecefs[i][0]);
double y_err = fabs(ecef[1] - ecefs[i][1]);
double z_err = fabs(ecef[2] - ecefs[i][2]);
if ((x_err < MAX_DIST_ERROR_M) &&
(y_err < MAX_DIST_ERROR_M) &&
(z_err < MAX_DIST_ERROR_M)) {
hal.console->printf("passing llh to ecef test %d\n", i);
} else {
hal.console->printf("failed llh to ecef test %d: ", i);
hal.console->printf("(%f - %f) (%f - %f) (%f - %f) => %.10f %.10f %.10f\n", ecef[0], ecefs[i][0], ecef[1], ecefs[i][1], ecef[2], ecefs[i][2], x_err, y_err, z_err);
}
}
hal.console->printf("TESTING wgsecef2llh\n");
for (int i = 0; i < NUM_COORDS; i++) {
Vector3d llh;
wgsecef2llh(ecefs[i], llh);
double lat_err = fabs(llh[0] - llhs[i][0]);
double lon_err = fabs(llh[1] - llhs[i][1]);
double hgt_err = fabs(llh[2] - llhs[i][2]);
if ((lat_err < MAX_ANGLE_ERROR_RAD) &&
(lon_err < MAX_ANGLE_ERROR_RAD) &&
(hgt_err < MAX_DIST_ERROR_M)) {
hal.console->printf("passing exef to llh test %d\n", i);
} else {
hal.console->printf("failed ecef to llh test %d: ", i);
hal.console->printf("%.10f %.10f %.10f\n", lat_err, lon_err, hgt_err);
}
}
}
/** Try to calculate a single point gps solution
*
* \param n_used number of measurments
* \param nav_meas array of measurements
* \param disable_raim passing True will omit raim check/repair functionality
* \param soln output solution struct
* \param dops output doppler information
* \return Non-negative values indicate a valid solution.
* - `2`: Solution converged but RAIM unavailable or disabled
* - `1`: Solution converged, failed RAIM but was successfully repaired
* - `0`: Solution converged and verified by RAIM
* - `-1`: PDOP is too high to yield a good solution.
* - `-2`: Altitude is unreasonable.
* - `-3`: Velocity is greater than or equal to 1000 kts.
* - `-4`: RAIM check failed and repair was unsuccessful
* - `-5`: RAIM check failed and repair was impossible (not enough measurements)
* - `-6`: pvt_iter didn't converge
* - `-7`: < 4 measurements
*/
s8 calc_PVT(const u8 n_used,
const navigation_measurement_t nav_meas[n_used],
bool disable_raim,
gnss_solution *soln,
dops_t *dops)
{
/* Initial state is the center of the Earth with zero velocity and zero
* clock error, if we have some a priori position estimate we could use
* that here to speed convergence a little on the first iteration.
*
* rx_state format:
* pos[3], clock error, vel[3], intermediate freq error
*/
static double rx_state[8];
double H[4][4];
if (n_used < 4) {
return -7;
}
soln->valid = 0;
soln->n_used = n_used; // Keep track of number of working channels
gnss_signal_t removed_sid;
s8 raim_flag = pvt_solve_raim(rx_state, n_used, nav_meas, disable_raim,
H, &removed_sid, 0);
if (raim_flag < 0) {
/* Didn't converge or least squares integrity check failed. */
return raim_flag - 3;
}
/* Initial solution failed, but repair was successful. */
if (raim_flag == 1) {
soln->n_used--;
}
/* Compute various dilution of precision metrics. */
compute_dops((const double(*)[4])H, rx_state, dops);
soln->err_cov[6] = dops->gdop;
/* Populate error covariances according to layout in definition
* of gnss_solution struct.
*/
soln->err_cov[0] = H[0][0];
soln->err_cov[1] = H[0][1];
soln->err_cov[2] = H[0][2];
soln->err_cov[3] = H[1][1];
soln->err_cov[4] = H[1][2];
soln->err_cov[5] = H[2][2];
/* Save as x, y, z. */
for (u8 i=0; i<3; i++) {
soln->pos_ecef[i] = rx_state[i];
soln->vel_ecef[i] = rx_state[4+i];
}
wgsecef2ned(soln->vel_ecef, soln->pos_ecef, soln->vel_ned);
/* Convert to lat, lon, hgt. */
wgsecef2llh(rx_state, soln->pos_llh);
soln->clock_offset = rx_state[3] / GPS_C;
soln->clock_bias = rx_state[7] / GPS_C;
/* Time at receiver is TOT plus time of flight. Time of flight is eqaul to
* the pseudorange minus the clock bias. */
soln->time = nav_meas[0].tot;
soln->time.tow += nav_meas[0].pseudorange / GPS_C;
/* Subtract clock offset. */
soln->time.tow -= rx_state[3] / GPS_C;
soln->time = normalize_gps_time(soln->time);
u8 ret;
if ((ret = filter_solution(soln, dops))) {
memset(soln, 0, sizeof(*soln));
/* Reset position elements of state if solution fails. */
rx_state[0] = 0;
rx_state[1] = 0;
rx_state[2] = 0;
return -ret;
}
soln->valid = 1;
return raim_flag;
}
