/** Populate a navigation_measurement_t structure with simulated data for
* the almanac_i satellite, currently dist away from simulated point at given elevation.
*
*/
void populate_nav_meas(navigation_measurement_t *nav_meas, double dist, double elevation, int almanac_i)
{
nav_meas->prn = simulation_almanacs[almanac_i].prn + SIM_PRN_OFFSET;
nav_meas->raw_pseudorange = dist;
nav_meas->raw_pseudorange += rand_gaussian(sim_settings.pseudorange_sigma);
nav_meas->carrier_phase = dist / (GPS_C / GPS_L1_HZ);
nav_meas->carrier_phase += simulation_fake_carrier_bias[almanac_i];
nav_meas->carrier_phase += rand_gaussian(sim_settings.phase_sigma);
nav_meas->snr = lerp(elevation, 0, M_PI/2, 4, 12) + rand_gaussian(sim_settings.cn0_sigma);
}
/**
* 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);
}
// Given a 3-array of pointers to n-arrays representing the x, y, and z components of vectors,
// place the vectors in a Maxwell-Boltzmann distribution.
void rand_maxboltz(size_t n, double *arr[3]) {
size_t i, j;
for(i = 0; i < 3; i += 1) {
for(j = 0; j < n; j += 1) {
// In a Maxwell-Boltzmann distribution, the components of the velocity vectors are Gaussian distributed:
// http://en.wikipedia.org/wiki/Maxwell%E2%80%93Boltzmann_distribution#Distribution_for_the_velocity_vector
arr[i][j] = rand_gaussian();
}
}
}
int rand_poisson
( double lambda
)
{ int v;
if (lambda>10000)
{ v = (int) (lambda + rand_gaussian()*sqrt(lambda) + 0.5);
}
else
{ v = 0;
for (;;)
{ lambda -= rand_exp();
if (lambda<=0) break;
v += 1;
}
}
return v;
}
void randvec_gaussian(double* v, int d) {
for(int i=0; i<d; i++) v[i] = rand_gaussian(0,1);
}
void randvec_gaussian(float* v, int d) {
for(int i=0; i<d; i++) v[i] = rand_gaussian(0,1);
}
/***********************************************************************
* Generates randomized series with normal gaussian sources of given
* mean and std.dev. Puts the series to output.
***********************************************************************/
int main (int argc, char* argv[])
{
if(argc != 3 && argc != 4 && argc != 5)
{
fprintf(stderr, "Error: need 2, 3 or 4 arguments\n");
fprintf(stderr,
"Usage: drand Length Mean StdDev [DiscrTrFunc]\n"\
" or drand InputSeries DiscrTrFunc\n"\
"DRAND_SAFE=something to avoid the same seed in srand()\n");
return 1;
}
NaOpenLogFile("drand.log");
int length;
float mean;
float stddev;
char *dtf_file = NULL;
NaDataFile *ins = NULL;
if(argc == 3)
{
ins = OpenInputDataFile(argv[1]);
dtf_file = argv[2];
}
else
{
length = atol(argv[1]);
mean = atof(argv[2]);
stddev = atof(argv[3]);
if(argc == 5)
dtf_file = argv[4];
}
try{
int i;
NaTransFunc dtf; /* K=1 */
if(NULL != dtf_file)
{
NaConfigPart *conf_list[] = { &dtf };
NaConfigFile conf_file(";NeuCon transfer", 1, 0);
conf_file.AddPartitions(NaNUMBER(conf_list), conf_list);
conf_file.LoadFromFile(dtf_file);
}
if(NULL == ins)
{
// See DRAND_SAFE to prevent dependent random series
reset_rand();
}
else
{
ins->GoStartRecord();
length = ins->CountOfRecord();
}
dtf.Reset();
for(i = 0; i < length; ++i)
{
NaReal fRand, fOut;
if(NULL == ins)
fRand = rand_gaussian(mean, stddev);
else
{
fRand = ins->GetValue();
ins->GoNextRecord();
}
dtf.Function(&fRand, &fOut);
printf("%g\n", fOut);
}
delete ins;
}
catch(NaException& ex){
NaPrintLog("EXCEPTION: %s\n", NaExceptionMsg(ex));
}
return 0;
}
