Commit 5b79aa21 authored by Eddy Scott's avatar Eddy Scott

Added gaussian noise function. Currently noise will not be added to...

Added gaussian noise function.  Currently noise will not be added to measurements unless a user changes the initialization value and builds from source.
parent cf20a404
......@@ -18,6 +18,9 @@
#include <cmath>
#include <qmath.h>
#include <limits>
#include <cstdlib>
#include "UAS.h"
#include "LinkInterface.h"
#include "UASManager.h"
......@@ -143,6 +146,7 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(),
// Initialize HIL sensor noise variances to 0. If user wants corrupted sensor values they will need to set them
......@@ -2993,6 +2997,42 @@ void UAS::sendHilState(quint64 time_us, float roll, float pitch, float yaw, floa
#ifndef __mobile__
float UAS::addZeroMeanNoise(float truth_meas, float noise_var)
/* Calculate normally distributed variable noise with mean = 0 and variance = noise_var. Calculated according to
Box-Muller transform */
static const float epsilon = std::numeric_limits<float>::min(); //used to ensure non-zero uniform numbers
static const float two_pi = 2.0*3.14159265358979323846; // 2*pi
static float z0; //calculated normal distribution random variables with mu = 0, var = 1;
float u1, u2; //random variables generated from c++ rand();
/*Generate random variables in range (0 1] */
//TODO seed rand() with srand(time) but srand(time should be called once on startup)
//currently this will generate repeatable random noise
u1 = rand() * (1.0 / RAND_MAX);
u2 = rand() * (1.0 / RAND_MAX);
while ( u1 <= epsilon ); //Have a catch to ensure non-zero for log()
z0 = sqrt(-2.0 * log(u1)) * cos(two_pi * u2); //calculate normally distributed variable with mu = 0, var = 1
//TODO add bias term that changes randomly to simulate accelerometer and gyro bias the exf should handle these
//as well
float noise = z0 * (noise_var*noise_var); //calculate normally distributed variable with mu = 0, std = var^2
//Finally gaurd against any case where the noise is not real
return truth_meas + noise;
return truth_meas;
* @param abs_pressure Absolute Pressure (hPa)
* @param diff_pressure Differential Pressure (hPa)
......@@ -3003,13 +3043,25 @@ void UAS::sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, fl
if (this->base_mode & MAV_MODE_FLAG_HIL_ENABLED)
float xacc_corrupt = addZeroMeanNoise(xacc, xacc_var);
float yacc_corrupt = addZeroMeanNoise(yacc, yacc_var);
float zacc_corrupt = addZeroMeanNoise(zacc, zacc_var);
float rollspeed_corrupt = addZeroMeanNoise(rollspeed,rollspeed_var);
float pitchspeed_corrupt = addZeroMeanNoise(pitchspeed,pitchspeed_var);
float yawspeed_corrupt = addZeroMeanNoise(yawspeed,yawspeed_var);
float xmag_corrupt = addZeroMeanNoise(xmag, xmag_var);
float ymag_corrupt = addZeroMeanNoise(ymag, ymag_var);
float zmag_corrupt = addZeroMeanNoise(zmag, zmag_var);
float abs_pressure_corrupt = addZeroMeanNoise(abs_pressure,abs_pressure_var);
float diff_pressure_corrupt = addZeroMeanNoise(diff_pressure, diff_pressure_var);
float pressure_alt_corrupt = addZeroMeanNoise(pressure_alt, pressure_alt_var);
float temperature_corrupt = addZeroMeanNoise(temperature,temperature_var);
mavlink_message_t msg;
mavlink_msg_hil_sensor_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg,
time_us, xacc, yacc, zacc, rollspeed, pitchspeed, yawspeed,
xmag, ymag, zmag, abs_pressure, diff_pressure, pressure_alt, temperature,
time_us, xacc_corrupt, yacc_corrupt, zacc_corrupt, rollspeed_corrupt, pitchspeed_corrupt,
yawspeed_corrupt, xmag_corrupt, ymag_corrupt, zmag_corrupt, abs_pressure_corrupt,
diff_pressure_corrupt, pressure_alt_corrupt, temperature_corrupt, fields_changed);
lastSendTimeSensors = QGC::groundTimeMilliseconds();
......@@ -3022,13 +3074,6 @@ void UAS::sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, fl
#ifndef __mobile__
float UAS::addZeroMeanNoise(float truth_meas, noise_var)
#ifndef __mobile__
void UAS::sendHilOpticalFlow(quint64 time_us, qint16 flow_x, qint16 flow_y, float flow_comp_m_x,
float flow_comp_m_y, quint8 quality, float ground_distance)
......@@ -39,13 +39,13 @@ clc
system('g++ gaussian_noise.cpp -o generate_noise_csv');
mu_des = 0.0;
std_des = 0.02;
var_desired = sqrt(std_des);
var_des = 0.2;%sqrt(std_des);
num_samples = 10000;
num_runs = 100;
if exist('generated_noise.csv','file')
system('rm generated_noise.csv')
system(sprintf('./generate_noise_csv %s %s %s %s',num2str(mu_des),num2str(var_desired),num2str(num_runs),num2str(num_samples)))
system(sprintf('./generate_noise_csv %s %s %s %s',num2str(mu_des),num2str(var_des),num2str(num_runs),num2str(num_samples)))
%% Load the generated noise values
noise_vals = load('generated_noise.csv');
for i = 1:size(noise_vals,1);
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment