/**
* 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);
}
/** For a point given in the local North, East, Down (NED) frame of a provided
* ECEF reference point, return the vector to that point in ECEF coordinates.
*
* Intended for calculating a new ECEF position. For converting velocity vectors,
* \see wgsned2ecef.
*
* \param ned The North, East, Down vector is passed as [N, E, D], all in
* meters.
* \param ref_ecef Cartesian coordinates of the reference point, passed as
* [X, Y, Z], all in meters.
* \param ecef Cartesian coordinates of the point written into this array,
* [X, Y, Z], all in meters.
*/
void wgsned2ecef_d(const double ned[3], const double ref_ecef[3],
double ecef[3]) {
double tempv[3];
wgsned2ecef(ned, ref_ecef, tempv);
vector_add(3, tempv, ref_ecef, ecef);
}