C++ (Cpp) get_gaussian_noise Examples

Example #1

File: ahrs_on_synth.c Project: EwoudSmeur/paparazzi

void aos_compute_sensors(void)
{

  struct FloatRates gyro;
  RATES_SUM(gyro, aos.imu_rates, aos.gyro_bias);
  //  printf("#aos.gyro_bias %f\n",DegOfRad( aos.gyro_bias.r));

  float_rates_add_gaussian_noise(&gyro, &aos.gyro_noise);

  RATES_BFP_OF_REAL(imu.gyro, gyro);
  RATES_BFP_OF_REAL(imu.gyro_prev, gyro);

  struct FloatVect3 g_ltp = {0., 0., 9.81};
  struct FloatVect3 accelero_ltp;
  VECT3_DIFF(accelero_ltp, aos.ltp_accel, g_ltp);
  struct FloatVect3 accelero_imu;
  float_quat_vmult(&accelero_imu, &aos.ltp_to_imu_quat, &accelero_ltp);

  float_vect3_add_gaussian_noise(&accelero_imu, &aos.accel_noise);
  ACCELS_BFP_OF_REAL(imu.accel, accelero_imu);

#ifndef DISABLE_MAG_UPDATE
  struct FloatVect3 h_earth = {AHRS_H_X, AHRS_H_Y, AHRS_H_Z};
  struct FloatVect3 h_imu;
  float_quat_vmult(&h_imu, &aos.ltp_to_imu_quat, &h_earth);
  MAGS_BFP_OF_REAL(imu.mag, h_imu);
#endif

  aos.heading_meas = aos.ltp_to_imu_euler.psi + get_gaussian_noise() * aos.heading_noise;

#ifdef AHRS_GRAVITY_UPDATE_COORDINATED_TURN
#if AHRS_TYPE == AHRS_TYPE_FCQ || AHRS_TYPE == AHRS_TYPE_FLQ
  ahrs_impl.ltp_vel_norm = float_vect3_norm(&aos.ltp_vel);
  ahrs_impl.ltp_vel_norm_valid = true;
#endif
#if AHRS_TYPE == AHRS_TYPE_FCR2
  ahrs_impl.ltp_vel_norm = float_vect3_norm(&aos.ltp_vel);
  ahrs_impl.ltp_vel_norm_valid = true;
#endif
#if AHRS_TYPE == AHRS_TYPE_FCR
  ahrs_impl.gps_speed = float_vect3_norm(&aos.ltp_vel);
  ahrs_impl.gps_age = 0;
  ahrs_update_gps();
  //RunOnceEvery(100,printf("# gps accel: %f\n", ahrs_impl.gps_acceleration));
#endif
#if AHRS_TYPE == AHRS_TYPE_ICQ
  ahrs_impl.ltp_vel_norm = SPEED_BFP_OF_REAL(float_vect3_norm(&aos.ltp_vel));
  ahrs_impl.ltp_vel_norm_valid = true;
#endif
#endif

}

Example #2

File: nps_sensor_sonar.c Project: davidmobach/paparazzi

void nps_sensor_sonar_run_step(struct NpsSensorSonar* sonar, double time) {

    if (time < sonar->next_update)
        return;

    /* agl in meters */
    sonar->value = fdm.agl + NPS_SONAR_OFFSET;
    /* add noise with std dev meters */
    sonar->value += get_gaussian_noise() * NPS_SONAR_NOISE_STD_DEV;

    sonar->next_update += NPS_SONAR_DT;
    sonar->data_available = TRUE;
}

Example #3

File: nps_sensor_temperature.c Project: 2seasuav/paparuzzi

void nps_sensor_temperature_run_step(struct NpsSensorTemperature *temperature, double time)
{
  if (time < temperature->next_update) {
    return;
  }

  /* termperature in degrees Celcius */
  temperature->value = fdm.temperature;
  /* add noise with std dev */
  temperature->value += get_gaussian_noise() * temperature->noise_std_dev;

  temperature->next_update += NPS_TEMPERATURE_DT;
  temperature->data_available = TRUE;
}

Example #4

File: nps_sensor_sonar.c Project: 2seasuav/paparuzzi

void nps_sensor_sonar_run_step(struct NpsSensorSonar *sonar, double time)
{

  if (time < sonar->next_update) {
    return;
  }

  /* agl in meters */
  sonar->value = fdm.agl + sonar->offset;
  /* add noise with std dev meters */
  sonar->value += get_gaussian_noise() * sonar->noise_std_dev;

  sonar->next_update += NPS_SONAR_DT;
  sonar->data_available = TRUE;
}

Example #5

File: nps_sensor_baro.c Project: AshuLara/lisa

void nps_sensor_baro_run_step(struct NpsSensorBaro* baro, double time) {

  if (time < baro->next_update)
    return;

  /*if (time < 10.)
    baro->value = rint(time*90);
    else {*/
    double z = fdm.ltpprz_pos.z + get_gaussian_noise()*NPS_BARO_NOISE_STD_DEV;
    double baro_reading = NPS_BARO_QNH + z * NPS_BARO_SENSITIVITY;
    baro_reading = rint(baro_reading);
    baro->value = baro_reading;
    Bound(baro->value, 0, 1024);
    //}

  baro->next_update += NPS_BARO_DT;
  baro->data_available = TRUE;
}

Example #6

File: nps_sensor_airspeed.c Project: 2seasuav/paparuzzi

void nps_sensor_airspeed_run_step(struct NpsSensorAirspeed *airspeed, double time)
{

  if (time < airspeed->next_update) {
    return;
  }

  /* equivalent airspeed + sensor offset */
  airspeed->value = fdm.airspeed + airspeed->offset;
  /* add noise with std dev meters/second */
  airspeed->value += get_gaussian_noise() * airspeed->noise_std_dev;
  /* can't be negative, min is zero */
  if (airspeed->value < 0) {
    airspeed->value = 0.0;
  }

  airspeed->next_update += NPS_AIRSPEED_DT;
  airspeed->data_available = TRUE;
}

Example #7

File: nps_random.c Project: BrandoJS/paparazzi

void double_vect3_get_gaussian_noise(struct DoubleVect3* vect, struct DoubleVect3* std_dev) {
  vect->x = get_gaussian_noise() * std_dev->x;
  vect->y = get_gaussian_noise() * std_dev->y;
  vect->z = get_gaussian_noise() * std_dev->z;
}

Example #8

File: nps_random.c Project: BrandoJS/paparazzi

void float_rates_add_gaussian_noise(struct FloatRates* vect, struct FloatRates* std_dev) {
  vect->p += get_gaussian_noise() * std_dev->p;
  vect->q += get_gaussian_noise() * std_dev->q;
  vect->r += get_gaussian_noise() * std_dev->r;
}

Example #9

File: nps_random.c Project: BrandoJS/paparazzi

void float_vect3_add_gaussian_noise(struct FloatVect3* vect, struct FloatVect3* std_dev) {
  vect->x += get_gaussian_noise() * std_dev->x;
  vect->y += get_gaussian_noise() * std_dev->y;
  vect->z += get_gaussian_noise() * std_dev->z;
}

Example #10

File: nps_random.c Project: BrandoJS/paparazzi

void double_vect3_add_gaussian_noise(struct DoubleVect3* vect, struct DoubleVect3* std_dev) {
  vect->x += get_gaussian_noise() * std_dev->x;
  vect->y += get_gaussian_noise() * std_dev->y;
  vect->z += get_gaussian_noise() * std_dev->z;
}