changed mag to use dynamic model, added baro noise, example code for …

…sensors

AirLib/include/sensors/barometer/BarometerSimple.hpp

@@ -22,7 +22,7 @@ class BarometerSimple  : public BarometerBase {
         pressure_factor.initialize(params_.pressure_factor_tau, params_.pressure_factor_sigma, Utils::nan<real_T>());
 
         uncorrelated_noise = RandomGeneratorGausianR(0.0f, params_.unnorrelated_noise_sigma);
-        correlated_noise.initialize(params_.correlated_noise_tau, params_.correlated_noise_sigma, 0.0f);
+        //correlated_noise.initialize(params_.correlated_noise_tau, params_.correlated_noise_sigma, 0.0f);
     }
 
     //*** Start: UpdatableState implementation ***//
@@ -55,16 +55,13 @@ class BarometerSimple  : public BarometerBase {
         pressure += pressure * pressure_factor.getOutput();
         //add user specified offset
         pressure += EarthUtils::SeaLevelPressure - params_.qnh*100.0f;
+        pressure += uncorrelated_noise.next();
 
         output.pressure = pressure;
         //apply altimeter formula
         //https://en.wikipedia.org/wiki/Pressure_altitude
+        //TODO: use same formula as in driver code?
         output.altitude = (1 - pow(pressure / EarthUtils::SeaLevelPressure, 0.190284f)) * 145366.45f * 0.3048f;
-
-        //apply noise model
-        //correlated_noise.update(dt);
-        //output.altitude += correlated_noise.getOutput();
-        //output.altitude += uncorrelated_noise.next();
         output.qnh = params_.qnh;
 
         setOutput(output);

AirLib/include/sensors/barometer/BarometerSimpleParams.hpp

@@ -15,22 +15,28 @@ struct BarometerSimpleParams {
     real_T qnh = EarthUtils::SeaLevelPressure / 100.0f; // hPa
 
     //sea level min,avh,max = 950,1013,1050 ie approx 3.65% variation
-    //put this in 2 std deviations to get sigma = ~18 hPa
-    //stddevs for various seasons are at 
+    //regular pressure changes in quite conditions are 1/4th of this
     //Mariner's Pressure Atlas, David Burch, 2014
     //https://www.starpath.com/ebooksamples/9780914025382_sample.pdf
-    real_T pressure_factor_sigma = 0.0365f / 2;
+    real_T pressure_factor_sigma = 0.0365f / 2 / 4;
     real_T pressure_factor_tau = 3600;
 
     /*
     Ref: A Stochastic Approach to Noise Modeling for Barometric Altimeters
          Angelo Maria Sabatini* and Vincenzo Genovese
          Sample values are from Table 1
          https://www.ncbi.nlm.nih.gov/pmc/articles/PMC3871085/
+         This is however not used because numbers mentioned in paper doesn't match experiments.
+
+         real_T correlated_noise_sigma = 0.27f;
+         real_T correlated_noise_tau = 0.87f;
+         real_T unnorrelated_noise_sigma = 0.24f;
     */
-    real_T correlated_noise_sigma = 0.27f;
-    real_T correlated_noise_tau = 0.87f;
-    real_T unnorrelated_noise_sigma = 0.24f;
+
+    //Experiments for MEAS MS56112 sensor shows 0.021mbar, datasheet has resoultion of 0.027mbar @ 1024
+    //http://www.te.com/commerce/DocumentDelivery/DDEController?Action=srchrtrv&DocNm=MS5611-01BA03&DocType=Data+Sheet&DocLang=English
+    real_T unnorrelated_noise_sigma = 0.027f * 100;
+
 };
 
 

AirLib/include/sensors/magnetometer/MagnetometerSimpleParams.hpp

@@ -22,10 +22,10 @@ struct MagnetometerSimpleParams {
     float ref_update_frequency = 0.2f;    //Hz
 
     //use dipole model is there is enough compute power available
-    //bool dynamic_reference_source = true;
-    //ReferenceSource ref_source = ReferenceSource::ReferenceSource_DipoleModel;
-    bool dynamic_reference_source = false;
-    ReferenceSource ref_source = ReferenceSource::ReferenceSource_Constant;
+    bool dynamic_reference_source = true;
+    ReferenceSource ref_source = ReferenceSource::ReferenceSource_DipoleModel;
+    //bool dynamic_reference_source = false;
+    //ReferenceSource ref_source = ReferenceSource::ReferenceSource_Constant;
 };
 
 

Examples/StandAloneSensors.hpp

@@ -1,6 +1,8 @@
 #pragma once
 
 #include "sensors/imu/ImuSimple.hpp"
+#include "sensors/Barometer/BarometerSimple.hpp"
+#include "sensors/Magnetometer/MagnetometerSimple.hpp"
 #include "common/Common.hpp"
 #include <thread>
 #include <ostream>
@@ -10,7 +12,7 @@ namespace msr { namespace airlib {
 
 class StandALoneSensors {
 public:
-    static void createStaticData(std::ostream& output, float period, float total_duration)
+    static void generateImuStaticData(std::ostream& output_stream, float period, float total_duration)
     {
         auto kinematics = Kinematics::State::zero();
         msr::airlib::Environment::State initial_environment(kinematics.pose.position, GeoPoint(), 0);
@@ -22,14 +24,14 @@ class StandALoneSensors {
 
         float interations = total_duration / period;
 
-        output << std::fixed;
+        output_stream << std::fixed;
 
         double last = Utils::getTimeSinceEpoch();
         for (auto i = 0; i < interations; ++i) {
-            const auto& imu_output = imu.getOutput();
-            output << Utils::getTimeSinceEpoch() << "\t";
-            output << imu_output.angular_velocity.x() << "\t" << imu_output.angular_velocity.y() << "\t" << imu_output.angular_velocity.z() << "\t";
-            output << imu_output.linear_acceleration.x() << "\t" << imu_output.linear_acceleration.y() << "\t" << imu_output.linear_acceleration.z() << "\n";
+            const auto& output = imu.getOutput();
+            output_stream << Utils::getTimeSinceEpoch() << "\t";
+            output_stream << output.angular_velocity.x() << "\t" << output.angular_velocity.y() << "\t" << output.angular_velocity.z() << "\t";
+            output_stream << output.linear_acceleration.x() << "\t" << output.linear_acceleration.y() << "\t" << output.linear_acceleration.z() << "\n";
 
             std::this_thread::sleep_for(std::chrono::duration<double>(period - (Utils::getTimeSinceEpoch() - last))); 
 
@@ -39,6 +41,108 @@ class StandALoneSensors {
             imu.update(dt);
         }
     }
+
+    static void generateBarometerStaticData(std::ostream& output_stream, float period, float total_duration)
+    {
+        auto kinematics = Kinematics::State::zero();
+        msr::airlib::Environment::State initial_environment(kinematics.pose.position, GeoPoint(), 0);
+        msr::airlib::Environment environment(initial_environment);
+
+        BarometerSimple baro;
+        baro.initialize(&kinematics, &environment);
+
+
+        float interations = total_duration / period;
+
+        output_stream << std::fixed;
+
+        double last = Utils::getTimeSinceEpoch();
+        for (auto i = 0; i < interations; ++i) {
+            const auto& output = baro.getOutput();
+            output_stream << Utils::getTimeSinceEpoch() << "\t";
+            output_stream << output.pressure << "\t" << output.altitude << std::endl;
+
+
+            std::this_thread::sleep_for(std::chrono::duration<double>(period - (Utils::getTimeSinceEpoch() - last))); 
+
+            float dt = static_cast<float>(Utils::getTimeSinceEpoch() - last);
+            last = Utils::getTimeSinceEpoch();
+            environment.update(dt);
+            baro.update(dt);
+        }
+    }
+
+
+    static void generateBarometerDynamicData(std::ostream& output_stream, float period, float total_duration)
+    {
+        auto kinematics = Kinematics::State::zero();
+        msr::airlib::Environment::State initial_environment(kinematics.pose.position, GeoPoint(), 0);
+        msr::airlib::Environment environment(initial_environment);
+
+        BarometerSimple baro;
+        baro.initialize(&kinematics, &environment);
+
+
+        float interations_20s = 20.0f / period;
+
+        output_stream << std::fixed;
+
+        double last = Utils::getTimeSinceEpoch();
+        for(auto j = 0; j < 10; ++j) {
+            for (auto i = 0; i < interations_20s; ++i) {
+                const auto& output = baro.getOutput();
+                output_stream << Utils::getTimeSinceEpoch() << "\t";
+                output_stream << output.pressure << "\t" << output.altitude << std::endl;
+
+
+                std::this_thread::sleep_for(std::chrono::duration<double>(period - (Utils::getTimeSinceEpoch() - last))); 
+
+                float dt = static_cast<float>(Utils::getTimeSinceEpoch() - last);
+                last = Utils::getTimeSinceEpoch();
+                environment.update(dt);
+                baro.update(dt);
+            }
+
+            environment.setPosition(Vector3r(0, 0, environment.getState().geo_point.altitude <= 1.0f ? 1.78f : 0));
+        }
+    }
+
+
+    static void generateMagnetometerDataLoc(std::ostream& output_stream, float period, float total_duration)
+    {
+        output_stream << std::fixed;
+        float interations = total_duration / period;
+        double last = Utils::getTimeSinceEpoch();
+        for (float pitch = 0; pitch < 2.1*M_PIf; pitch += M_PIf/2) {
+        for (float roll = 0; roll < 2.1*M_PIf; roll += M_PIf/2) {
+        for (float yaw = 0; yaw < 2.1*M_PIf; yaw += M_PIf/2) {
+
+            auto kinematics = Kinematics::State::zero();
+            kinematics.pose.orientation = VectorMath::toQuaternion(pitch, roll, yaw);
+            //msr::airlib::Environment::State initial_environment(kinematics.pose.position, GeoPoint(47.7631699f, -122.0685655f, 111.208f), 0);
+            msr::airlib::Environment::State initial_environment(kinematics.pose.position, GeoPoint(47.6628040f, -122.1167039f, 7.564f), 0);
+            msr::airlib::Environment environment(initial_environment);
+            MagnetometerSimple mag;
+            mag.initialize(&kinematics, &environment);
+
+            for (auto i = 0; i < interations; ++i) {
+                const auto& output = mag.getOutput();
+                const auto& geo = environment.getState().geo_point;
+
+                output_stream << Utils::getTimeSinceEpoch() << "\t";
+                output_stream << output.magnetic_field_body.x() << "\t" << output.magnetic_field_body.y() <<  "\t" << output.magnetic_field_body.z();
+                output_stream << "\t" << geo.latitude << "\t" << geo.longitude << "\t" << geo.altitude;
+                output_stream << "\t" << kinematics.pose.orientation.w() << "\t" << kinematics.pose.orientation.x()<< "\t" << kinematics.pose.orientation.y() << "\t" << kinematics.pose.orientation.z();
+                output_stream << std::endl;
+
+                std::this_thread::sleep_for(std::chrono::duration<double>(period - (Utils::getTimeSinceEpoch() - last))); 
+                float dt = static_cast<float>(Utils::getTimeSinceEpoch() - last);
+                last = Utils::getTimeSinceEpoch();
+                environment.update(dt);
+                mag.update(dt);
+            }
+        }}}
+    }
 };
 
 

Examples/main.cpp

@@ -24,5 +24,8 @@ int main(int argc, const char *argv[])
     using namespace msr::airlib;
 
     std::ofstream out_file(argv[1]);
-    StandALoneSensors::createStaticData(out_file, period, total_duration);
+    //StandALoneSensors::createStaticData(out_file, period, total_duration);
+    //StandALoneSensors::generateBarometerStaticData(out_file, period, total_duration);
+    //StandALoneSensors::generateBarometerDynamicData(out_file, period, total_duration);
+    StandALoneSensors::generateMagnetometerDataLoc(out_file, period, total_duration);
 }