Merge pull request #1773 from scott-eddy/HIL_add_sensor_noise

Hil add sensor noise

src/uas/UAS.cc

@@ -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,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<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] */
+    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();
     }

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

tools/RandomNumberVerification/gaussian_noise.cpp

+/****************************************************************************
+ *
+ *   Copyright (C) 2015 PX4 Development Team. All rights reserved.
+ *   Author: Eddy Scott <scott.edward@aurora.aero>
+ *
+ * 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 <cstdlib>
+#include <cmath>
+#include <ctime>
+#include <limits>
+#include <iostream> // not needed
+#include <fstream> // 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<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] */
+	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<num_runs;j++){
+  	if(j!=0){
+  		fid <<"\n";
+  	}
+  	for(int i=0;i<num_samples;i++){
+    	fid << generateGaussianNoise(mu, variance) << ",";
+  	}
+}
+  fid.close();
+  return 0;
+}

tools/RandomNumberVerification/noise_characteristics.m

+% ****************************************************************************
+%  *
+%  *   Copyright (C) 2015 PX4 Development Team. All rights reserved.
+%  *   Author: Eddy Scott <scott.edward@aurora.aero>
+%  *
+%  * 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
+

tools/RandomNumberVerification/verify_noise_characteristics.m

+% ****************************************************************************
+%  *
+%  *   Copyright (C) 2015 PX4 Development Team. All rights reserved.
+%  *   Author: Eddy Scott <scott.edward@aurora.aero>
+%  *
+%  * 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)
+
+
+
+