diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index e51de03ff449e72b0b02687eccd1c53a37429217..780bc8db5f27a6a8ba8c07eadb32a73e0e3d4e69 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -18,6 +18,9 @@ #include #include +#include +#include + #include "UAS.h" #include "LinkInterface.h" #include "UASManager.h" @@ -143,6 +146,23 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(), blockHomePositionChanges(false), receivedMode(false), + // Initialize HIL sensor noise variances to 0. If user wants corrupted sensor values they will need to set them + // Note variances calculated from flight case from this log: http://dash.oznet.ch/view/MRjW8NUNYQSuSZkbn8dEjY + // TODO: calibrate stand-still pixhawk variances + xacc_var(1.2914), + yacc_var(0.7048), + zacc_var(1.9577), + rollspeed_var(0.8126), + pitchspeed_var(0.6145), + yawspeed_var(0.5852), + xmag_var(0.4786), + ymag_var(0.4566), + zmag_var(0.3333), + abs_pressure_var(1.1604), + diff_pressure_var(1.1604), + pressure_alt_var(1.1604), + temperature_var(1.4290), + #ifndef __mobile__ simulation(0), #endif @@ -2979,6 +2999,42 @@ void UAS::sendHilState(quint64 time_us, float roll, float pitch, float yaw, floa } #endif +#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::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] */ + do + { + //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 + if(std::isfinite(noise)){ + return truth_meas + noise; + } + else{ + return truth_meas; + } +} +#endif + /* * @param abs_pressure Absolute Pressure (hPa) * @param diff_pressure Differential Pressure (hPa) @@ -2989,11 +3045,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, - fields_changed); + 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); sendMessage(msg); lastSendTimeSensors = QGC::groundTimeMilliseconds(); } diff --git a/src/uas/UAS.h b/src/uas/UAS.h index 85a515f6d959c7f5232187633a0de5befa70120c..a7ff66e08059b894161a133ab89c4958db9ef07c 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -332,6 +332,60 @@ public: return nedAttGlobalOffset; } + + // Setters for HIL noise variance + void setXaccVar(float var){ + xacc_var = var; + } + + void setYaccVar(float var){ + yacc_var = var; + } + + void setZaccVar(float var){ + zacc_var = var; + } + + void setRollSpeedVar(float var){ + rollspeed_var = var; + } + + void setPitchSpeedVar(float var){ + pitchspeed_var = var; + } + + void setYawSpeedVar(float var){ + pitchspeed_var = var; + } + + void setXmagVar(float var){ + xmag_var = var; + } + + void setYmagVar(float var){ + ymag_var = var; + } + + void setZmagVar(float var){ + zmag_var = var; + } + + void setAbsPressureVar(float var){ + abs_pressure_var = var; + } + + void setDiffPressureVar(float var){ + diff_pressure_var = var; + } + + void setPressureAltVar(float var){ + pressure_alt_var = var; + } + + void setTemperatureVar(float var){ + temperature_var = var; + } + bool isRotaryWing(); bool isFixedWing(); @@ -465,6 +519,21 @@ protected: //COMMENTS FOR TEST UNIT bool blockHomePositionChanges; ///< Block changes to the home position bool receivedMode; ///< True if mode was retrieved from current conenction to UAS + /// SIMULATION NOISE + float xacc_var; ///< variance of x acclerometer noise for HIL sim (mg) + float yacc_var; ///< variance of y acclerometer noise for HIL sim (mg) + float zacc_var; ///< variance of z acclerometer noise for HIL sim (mg) + float rollspeed_var; ///< variance of x gyroscope noise for HIL sim (rad/s) + float pitchspeed_var; ///< variance of y gyroscope noise for HIL sim (rad/s) + float yawspeed_var; ///< variance of z gyroscope noise for HIL sim (rad/s) + float xmag_var; ///< variance of x magnatometer noise for HIL sim (???) + float ymag_var; ///< variance of y magnatometer noise for HIL sim (???) + float zmag_var; ///< variance of z magnatometer noise for HIL sim (???) + float abs_pressure_var; ///< variance of absolute pressure noise for HIL sim (hPa) + float diff_pressure_var; ///< variance of differential pressure noise for HIL sim (hPa) + float pressure_alt_var; ///< variance of altitude pressure noise for HIL sim (hPa) + float temperature_var; ///< variance of temperature noise for HIL sim (C) + /// SIMULATION #ifndef __mobile__ QGCHilLink* simulation; ///< Hardware in the loop simulation link @@ -728,6 +797,8 @@ public slots: void 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); + float addZeroMeanNoise(float truth_meas, float noise_var); + /** * @param time_us * @param lat diff --git a/tools/RandomNumberVerification/gaussian_noise.cpp b/tools/RandomNumberVerification/gaussian_noise.cpp new file mode 100644 index 0000000000000000000000000000000000000000..ebb1073cf154593913b6be12dfad6fa0a5081b1d --- /dev/null +++ b/tools/RandomNumberVerification/gaussian_noise.cpp @@ -0,0 +1,87 @@ +/**************************************************************************** + * + * Copyright (C) 2015 PX4 Development Team. All rights reserved. + * Author: Eddy Scott + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + + +#include +#include +#include +#include +#include // not needed +#include // not needed +using namespace std; +float generateGaussianNoise(float mu, float variance) +{ + /* Calculate normally distributed variable noise with mean = mu and variance = variance. Calculated according to + Box-Muller transform */ + static const float epsilon = std::numeric_limits::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] */ + do + { + 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 + float noise = z0 * (variance*variance) + mu; //calculate normally distributed variable with mu = mu, std = var^2 + return noise +} +int main(int argc, char *argv[]) +{ + ofstream fid; + fid.open ("generated_noise.csv"); + float mu = atof(argv[1]); // Define the mean of the noise, for gaussian = 0 + float variance = atof(argv[2]); //Define the variance of the noise + int num_runs = atoi(argv[3]); //Define number of runs + int num_samples = atoi(argv[4]); + srand(time(NULL)); //Seed rand() function so same random variables are not calculated + cout << "Desired Mean: " << mu << "\n"; + cout << "Desired Variance: " << variance << "\n"; + cout << "Desired number of runs: " << num_runs << "\n"; + cout << "Desired number of samples per run: " << num_samples << "\n"; + for(int j=0;j +% * +% * Redistribution and use in source and binary forms, with or without +% * modification, are permitted provided that the following conditions +% * are met: +% * +% * 1. Redistributions of source code must retain the above copyright +% * notice, this list of conditions and the following disclaimer. +% * 2. Redistributions in binary form must reproduce the above copyright +% * notice, this list of conditions and the following disclaimer in +% * the documentation and/or other materials provided with the +% * distribution. +% * 3. Neither the name PX4 nor the names of its contributors may be +% * used to endorse or promote products derived from this software +% * without specific prior written permission. +% * +% * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +% * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +% * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +% * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +% * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +% * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +% * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +% * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +% * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +% * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +% * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +% * POSSIBILITY OF SUCH DAMAGE. +% * +% ****************************************************************************/ +%% Clean work environment +clear all +close all +clc +%% Load the generated noise values +noise_val = load('generated_noise.csv'); +for i = 1:size(noise_val + diff --git a/tools/RandomNumberVerification/verify_noise_characteristics.m b/tools/RandomNumberVerification/verify_noise_characteristics.m new file mode 100644 index 0000000000000000000000000000000000000000..9d660f8e6cb96db82316ddc14d6d0faa498ae126 --- /dev/null +++ b/tools/RandomNumberVerification/verify_noise_characteristics.m @@ -0,0 +1,79 @@ +% **************************************************************************** +% * +% * Copyright (C) 2015 PX4 Development Team. All rights reserved. +% * Author: Eddy Scott +% * +% * Redistribution and use in source and binary forms, with or without +% * modification, are permitted provided that the following conditions +% * are met: +% * +% * 1. Redistributions of source code must retain the above copyright +% * notice, this list of conditions and the following disclaimer. +% * 2. Redistributions in binary form must reproduce the above copyright +% * notice, this list of conditions and the following disclaimer in +% * the documentation and/or other materials provided with the +% * distribution. +% * 3. Neither the name PX4 nor the names of its contributors may be +% * used to endorse or promote products derived from this software +% * without specific prior written permission. +% * +% * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +% * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +% * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +% * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +% * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +% * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +% * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +% * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +% * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +% * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +% * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +% * POSSIBILITY OF SUCH DAMAGE. +% * +% ****************************************************************************/ +%% Clean work environment +clear all +close all +clc +%% Compile and run the noise cpp program +system('g++ gaussian_noise.cpp -o generate_noise_csv'); +mu_des = 0.0; +std_des = 0.02; +var_des = 0.2;%sqrt(std_des); +num_samples = 10000; +num_runs = 100; +if exist('generated_noise.csv','file') + system('rm generated_noise.csv') +end +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); + run_mean = mean(noise_vals(i,:)); + run_std = std(noise_vals(i,:)); + mean_array(i) = run_mean; + std_array(i) = run_std; +end +%%%%% Display statisitcs of means generated +max_mean = max(mean_array); +min_mean = min(mean_array); +max_std = max(std_array); +min_std = min(std_array); + +figure +hist(mean_array) +ylabel('Occurances') +xlabel('Mean Value') +title('Histogram of mean values') + +figure +hist(std_array) +ylabel('Occurances') +xlabel('Std Value') +title('Histogram of std values') + +table(min_mean, mu_des, max_mean, min_std, std_des, max_std) + + + +