150 lines
4.5 KiB
C
150 lines
4.5 KiB
C
#include "gps/NavEquations.h"
|
|
|
|
#include <assert.h>
|
|
#include <math.h>
|
|
#include <stdio.h>
|
|
#include <string.h>
|
|
|
|
static void test_broadcast_ephemeris_velocity(void)
|
|
{
|
|
gps_ephemeris_t ephemeris;
|
|
memset(&ephemeris, 0, sizeof(ephemeris));
|
|
ephemeris.tow.week = 2400;
|
|
ephemeris.tow.sec = 345700.0;
|
|
ephemeris.toe.week = 2400;
|
|
ephemeris.toe.sec = 345600.0;
|
|
ephemeris.toc.week = 2400;
|
|
ephemeris.toc.sec = 345600.0;
|
|
ephemeris.sqrta = 5153.7954775;
|
|
ephemeris.A = ephemeris.sqrta * ephemeris.sqrta;
|
|
ephemeris.ecc = 0.0093;
|
|
ephemeris.m0 = 0.75;
|
|
ephemeris.deltan = 4.5e-9;
|
|
ephemeris.cuc = 1.0e-6;
|
|
ephemeris.cus = -3.0e-6;
|
|
ephemeris.crc = 180.0;
|
|
ephemeris.crs = -75.0;
|
|
ephemeris.cic = 1.2e-7;
|
|
ephemeris.cis = -8.0e-8;
|
|
ephemeris.inc0 = 0.96;
|
|
ephemeris.idot = -2.0e-10;
|
|
/* Legacy names: omgdot=Omega0, omg0=omega, omgkdot=Omega-dot. */
|
|
ephemeris.omgdot = 1.1;
|
|
ephemeris.omg0 = -0.4;
|
|
ephemeris.omgkdot = -8.0e-9;
|
|
ephemeris.af0 = 2.3e-5;
|
|
ephemeris.af1 = -4.0e-12;
|
|
ephemeris.af2 = 1.0e-19;
|
|
ephemeris.tgd = -2.0e-9;
|
|
|
|
double position[3];
|
|
double velocity[3];
|
|
double clock[2];
|
|
double correction;
|
|
satpos(&ephemeris, position, velocity, clock, &correction);
|
|
|
|
const double dt = 0.05;
|
|
gps_ephemeris_t before = ephemeris;
|
|
gps_ephemeris_t after = ephemeris;
|
|
before.tow.sec -= dt;
|
|
after.tow.sec += dt;
|
|
double position_before[3], position_after[3];
|
|
double velocity_unused[3], clock_before[2], clock_after[2];
|
|
double correction_before, correction_after;
|
|
satpos(&before, position_before, velocity_unused,
|
|
clock_before, &correction_before);
|
|
satpos(&after, position_after, velocity_unused,
|
|
clock_after, &correction_after);
|
|
|
|
double radius_squared = 0.0;
|
|
double speed_squared = 0.0;
|
|
for(unsigned int component = 0u; component < 3u; component++)
|
|
{
|
|
double finite_difference =
|
|
(position_after[component] - position_before[component]) /
|
|
(2.0 * dt);
|
|
assert(fabs(velocity[component] - finite_difference) < 0.05);
|
|
radius_squared += position[component] * position[component];
|
|
speed_squared += velocity[component] * velocity[component];
|
|
}
|
|
double finite_clock_drift =
|
|
(correction_after - correction_before) / (2.0 * dt);
|
|
assert(fabs(clock[1] - finite_clock_drift) < 1.0e-13);
|
|
assert(fabs(clock[0] - correction) < 1.0e-18);
|
|
assert(sqrt(radius_squared) > 25000000.0);
|
|
assert(sqrt(radius_squared) < 28000000.0);
|
|
assert(sqrt(speed_squared) > 1000.0);
|
|
assert(sqrt(speed_squared) < 5000.0);
|
|
}
|
|
|
|
static void test_cn0_changes_position_weight(void)
|
|
{
|
|
const unsigned int satellite_count = 6u;
|
|
const double receiver[3] = {6378137.0, 0.0, 0.0};
|
|
const double directions[6][3] = {
|
|
{ 1.0, 0.0, 0.0},
|
|
{ 0.2, 0.9, 0.3872983346207417},
|
|
{ 0.3, -0.4, 0.8660254037844386},
|
|
{-0.2, 0.5, 0.8426149773176358},
|
|
{ 0.4, -0.8, -0.4472135954999579},
|
|
{-0.5, -0.5, 0.7071067811865475}
|
|
};
|
|
mat_type* satellite_positions =
|
|
create_matrix((int)satellite_count, 3, "testSatPos");
|
|
mat_type* observations =
|
|
create_matrix(1, (int)satellite_count, "testObs");
|
|
mat_type* az = create_matrix(1, (int)satellite_count, "testAz");
|
|
mat_type* el = create_matrix(1, (int)satellite_count, "testEl");
|
|
assert(satellite_positions != NULL);
|
|
assert(observations != NULL);
|
|
assert(az != NULL);
|
|
assert(el != NULL);
|
|
initialize_all(0.0, satellite_positions);
|
|
initialize_all(0.0, observations);
|
|
initialize_all(0.0, az);
|
|
initialize_all(0.0, el);
|
|
|
|
unsigned char ura[6] = {0u, 0u, 0u, 0u, 0u, 0u};
|
|
double cn0_dbhz[6] = {52.0, 22.0, 45.0, 45.0, 45.0, 45.0};
|
|
for(unsigned int satellite = 0u;
|
|
satellite < satellite_count;
|
|
satellite++)
|
|
{
|
|
double range_squared = 0.0;
|
|
for(unsigned int component = 0u; component < 3u; component++)
|
|
{
|
|
satellite_positions->matrix[satellite][component] =
|
|
receiver[component] +
|
|
directions[satellite][component] * 21000000.0;
|
|
double difference =
|
|
satellite_positions->matrix[satellite][component] -
|
|
receiver[component];
|
|
range_squared += difference * difference;
|
|
}
|
|
observations->matrix[0][satellite] =
|
|
sqrt(range_squared) + 125.0;
|
|
}
|
|
|
|
gps_nav_solution_t solution;
|
|
assert(leastSquaresSTD(
|
|
satellite_positions, observations, (int)satellite_count,
|
|
az, el, ura, cn0_dbhz, NULL, 345700.0, &solution));
|
|
assert(isfinite(solution.measurement_sigma_m[0]));
|
|
assert(isfinite(solution.measurement_sigma_m[1]));
|
|
assert(solution.measurement_sigma_m[1] >
|
|
solution.measurement_sigma_m[0] + 20.0);
|
|
|
|
free_matrix(satellite_positions);
|
|
free_matrix(observations);
|
|
free_matrix(az);
|
|
free_matrix(el);
|
|
}
|
|
|
|
int main(void)
|
|
{
|
|
test_broadcast_ephemeris_velocity();
|
|
test_cn0_changes_position_weight();
|
|
puts("test_gps_satellite_velocity PASS");
|
|
return 0;
|
|
}
|