delay line and freq limit in baro and mag, fix bug in baro, add groun…

…d lock

AirLib/include/common/ClockBase.hpp

@@ -31,6 +31,10 @@ class ClockBase {
     {
         return (second - first) / 1.0E9;
     }
+    TTimePoint addTo(TTimePoint t, TTimeDelta dt)
+    {
+        return static_cast<TTimePoint>(t + dt * 1.0E9);
+    }
     TTimeDelta updateSince(TTimePoint& since) const
     {
         TTimePoint cur = nowNanos();

AirLib/include/common/DebugClock.hpp

@@ -11,27 +11,36 @@ namespace msr { namespace airlib {
 
 class DebugClock : public ClockBase {
 public:
-    DebugClock(TTimePoint start = 1000, TTimePoint step = 20E6)
+    //Debug clock allows to advance the clock manually in arbitrary way
+    //TTimePoint is nano seconds passed since some fixed point in time
+    //step is how much we would advance the clock by default when calling step()
+    //by default we advance by 20ms
+    DebugClock(TTimeDelta step = 20E-3f, TTimePoint start = 1000)
         : current_(start), step_(step)
     {
         current_ = start;
     }
 
-    void add(TTimePoint amount)
+    TTimePoint step(TTimeDelta amount)
     {
-        current_ += amount;
+        current_ = addTo(current_, amount);
+        return current_;
     }
 
-    void stepNext()
+    TTimePoint step()
     {
-        current_ += step_;
+        current_ = addTo(current_, step_);
+        return current_;
     }
 
     virtual TTimePoint nowNanos() const override
     {
         return current_;
     }
 
+    //convert wall clock interval to this clock interval
+    //below functions should normally be used in things like thread.sleep 
+    //which usually requires wall time (i.e. clock used by system)
     virtual TTimeDelta fromWallDelta(TTimeDelta dt) const override
     {
         return dt;
@@ -43,7 +52,8 @@ class DebugClock : public ClockBase {
 
 
 private:
-    TTimePoint current_, step_;
+    TTimePoint current_;
+    TTimeDelta step_;
 };
 
 }} //namespace

AirLib/include/common/EarthUtils.hpp

@@ -143,7 +143,8 @@ class EarthUtils {
         //Above 51km: http://www.braeunig.us/space/atmmodel.htm
         //Validation data: https://www.avs.org/AVS/files/c7/c7edaedb-95b2-438f-adfb-36de54f87b9e.pdf
         if (geopot_height <= 11)
-            return  101325 * powf(288.15f / std_temperature, -5.255877f);
+            //at alt 0, return sea level pressure
+            return  SeaLevelPressure * powf(288.15f / std_temperature, -5.255877f);
         else if (geopot_height <= 20)
             return 22632.06f * expf(-0.1577f * (geopot_height - 11));
         else if (geopot_height <= 32)

AirLib/include/common/GaussianMarkov.hpp

@@ -15,11 +15,11 @@ class GaussianMarkov : UpdatableObject {
 public:
     GaussianMarkov()
     {}
-    GaussianMarkov(real_T tau, real_T sigma, real_T initial_output) //in seconds
+    GaussianMarkov(real_T tau, real_T sigma, real_T initial_output = 0) //in seconds
     {
         initialize(tau, sigma, initial_output);
     }
-    void initialize(real_T tau, real_T sigma, real_T initial_output)  //in seconds
+    void initialize(real_T tau, real_T sigma, real_T initial_output = 0)  //in seconds
     {
         tau_ = tau;
         sigma_ = sigma;

AirLib/include/physics/FastPhysicsEngine.hpp

@@ -122,12 +122,17 @@ class FastPhysicsEngine : public PhysicsEngineBase {
 
         //see if impact is straight at body's surface (assuming its box)
         const Vector3r normal_body = VectorMath::transformToBodyFrame(collison_info.normal, current.pose.orientation);
-        if (Utils::isApproximatelyEqual(std::abs(normal_body.x()), 1.0f, kAxisTolerance) 
-            || Utils::isApproximatelyEqual(std::abs(normal_body.y()), 1.0f, kAxisTolerance)
-            || Utils::isApproximatelyEqual(std::abs(normal_body.z()), 1.0f, kAxisTolerance)) {
-
+        const bool is_ground_normal = Utils::isApproximatelyEqual(std::abs(normal_body.z()), 1.0f, kAxisTolerance);
+        bool ground_lock = false;
+        if (is_ground_normal
+            || Utils::isApproximatelyEqual(std::abs(normal_body.x()), 1.0f, kAxisTolerance) 
+            || Utils::isApproximatelyEqual(std::abs(normal_body.y()), 1.0f, kAxisTolerance) 
+           ) {
             //think of collison occured along the surface, not at point
             r = Vector3r::Zero();
+
+            //we have collided with ground straight on, we will fix orientation later
+            ground_lock = is_ground_normal;
         }
 
         //velocity at contact point
@@ -174,7 +179,13 @@ class FastPhysicsEngine : public PhysicsEngineBase {
         next.accelerations.angular = Vector3r::Zero();
 
         next.pose = current.pose;
-        //overwrite position, keep the orientation
+        if (ground_lock) {
+            float pitch, roll, yaw;
+            VectorMath::toEulerianAngle(next.pose.orientation, pitch, roll, yaw);
+            pitch = roll = 0;
+            next.pose.orientation = VectorMath::toQuaternion(pitch, roll, yaw);
+        }
+        //else keep the orientation
         next.pose.position = collison_info.position + (collison_info.normal * collison_info.penetration_depth) + next.twist.linear * (dt_real * kCollisionResponseCycles);
 
 
@@ -363,7 +374,7 @@ class FastPhysicsEngine : public PhysicsEngineBase {
 
 private:
     static constexpr uint kCollisionResponseCycles = 1;
-    static constexpr float kAxisTolerance = 0.05f;
+    static constexpr float kAxisTolerance = 0.25f;
     static constexpr float kRestingVelocityMax = 0.1f;
     static constexpr float kDragMinVelocity = 0.1f;
 

AirLib/include/sensors/barometer/BarometerSimple.hpp

@@ -10,6 +10,8 @@
 #include "BarometerSimpleParams.hpp"
 #include "BarometerBase.hpp"
 #include "common/GaussianMarkov.hpp"
+#include "common/DelayLine.hpp"
+#include "common/FrequencyLimiter.hpp"
 
 
 namespace msr { namespace airlib {
@@ -19,62 +21,83 @@ class BarometerSimple  : public BarometerBase {
     BarometerSimple(const BarometerSimpleParams& params = BarometerSimpleParams())
         : params_(params)
     {
-        pressure_factor.initialize(params_.pressure_factor_tau, params_.pressure_factor_sigma, Utils::nan<real_T>());
+        //GM process that would do random walk for pressure factor
+        pressure_factor_.initialize(params_.pressure_factor_tau, params_.pressure_factor_sigma, 0);
 
-        uncorrelated_noise = RandomGeneratorGausianR(0.0f, params_.unnorrelated_noise_sigma);
-        //correlated_noise.initialize(params_.correlated_noise_tau, params_.correlated_noise_sigma, 0.0f);
+        uncorrelated_noise_ = RandomGeneratorGausianR(0.0f, params_.unnorrelated_noise_sigma);
+        //correlated_noise_.initialize(params_.correlated_noise_tau, params_.correlated_noise_sigma, 0.0f);
+
+        //initialize frequency limiter
+        freq_limiter_.initialize(params_.update_frequency, params_.startup_delay);
+        delay_line_.initialize(params_.update_latency);
     }
 
     //*** Start: UpdatableState implementation ***//
     virtual void reset() override
     {
-        updateOutput();
-        pressure_factor.reset();
-        correlated_noise.reset();
-        uncorrelated_noise.reset();
+        pressure_factor_.reset();
+        correlated_noise_.reset();
+        uncorrelated_noise_.reset();
+
+        freq_limiter_.reset();
+        delay_line_.reset();
+
+        delay_line_.push_back(getOutputInternal());
     }
 
     virtual void update() override
     {
-        updateOutput();
+        freq_limiter_.update();
+
+        if (freq_limiter_.isWaitComplete()) { 
+            delay_line_.push_back(getOutputInternal());
+        }
+
+        delay_line_.update();
+
+        if (freq_limiter_.isWaitComplete())
+            setOutput(delay_line_.getOutput());
     }
     //*** End: UpdatableState implementation ***//
 
     virtual ~BarometerSimple() = default;
 
 private: //methods
-    void updateOutput()
+    Output getOutputInternal()
     {
         Output output;
         const GroundTruth& ground_truth = getGroundTruth();
 
         auto altitude = ground_truth.environment->getState().geo_point.altitude;
         auto pressure = EarthUtils::getStandardPressure(altitude);
 
-        //TODO: below drift is producing big swings of +/- 5 meters in 1 ms, need to reevaluate params
-        //add drift in pressure
-        //pressure_factor.update();
-        //pressure += pressure * pressure_factor.getOutput();
-        //add user specified offset
-        //pressure -= params_.qnh*100.0f;
-        pressure += uncorrelated_noise.next();
+        //add drift in pressure, about 10m change per hour
+        pressure_factor_.update();
+        pressure += pressure * pressure_factor_.getOutput();
+
+        //add noise in pressure (about 0.2m sigma)
+        pressure += uncorrelated_noise_.next();
+
+        output.pressure = pressure - EarthUtils::SeaLevelPressure + params_.qnh*100.0f;
 
-        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;
         output.qnh = params_.qnh;
 
-        setOutput(output);
+        return output;
     }
 
 private:
     BarometerSimpleParams params_;
 
-    GaussianMarkov pressure_factor;
-    GaussianMarkov correlated_noise;
-    RandomGeneratorGausianR uncorrelated_noise;
+    GaussianMarkov pressure_factor_;
+    GaussianMarkov correlated_noise_;
+    RandomGeneratorGausianR uncorrelated_noise_;
+
+    FrequencyLimiter freq_limiter_;
+    DelayLine<Output> delay_line_;
 };
 
 }} //namespace

AirLib/include/sensors/barometer/BarometerSimpleParams.hpp

@@ -11,14 +11,17 @@ namespace msr { namespace airlib {
 
 
 struct BarometerSimpleParams {
-    //user specified sea level pressure 
+    //user specified sea level pressure is specified in hPa units
     real_T qnh = EarthUtils::SeaLevelPressure / 100.0f; // hPa
 
     //sea level min,avh,max = 950,1013,1050 ie approx 3.65% variation
-    //regular pressure changes in quiet conditions are 1/10th of this
+    //regular pressure changes in quiet conditions are taken as 1/20th of this
     //Mariner's Pressure Atlas, David Burch, 2014
+    //GM process may generate ~70% of sigma in tau interval
+    //This means below config may produce ~10m variance per hour
+
     //https://www.starpath.com/ebooksamples/9780914025382_sample.pdf
-    real_T pressure_factor_sigma = 0.0365f / 2 / 10;
+    real_T pressure_factor_sigma = 0.0365f / 20;
     real_T pressure_factor_tau = 3600;
 
     /*
@@ -35,9 +38,14 @@ struct BarometerSimpleParams {
 
     //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;
-    //TODO: above seems to be too high noise - need to reevaluate experimentally
-    real_T unnorrelated_noise_sigma = 0.1f;
+    real_T unnorrelated_noise_sigma = 0.027f * 100;
+    //jMavSim uses below
+    //real_T unnorrelated_noise_sigma = 0.1f;
+
+    //see PX4 param reference for EKF: https://dev.px4.io/en/advanced/parameter_reference.html
+    real_T update_latency = 0.0f;    //sec
+    real_T update_frequency = 50;    //Hz
+    real_T startup_delay = 0;        //sec
 
 };
 

AirLib/include/sensors/magnetometer/MagnetometerSimple.hpp

@@ -21,6 +21,10 @@ class MagnetometerSimple : public MagnetometerBase {
     {
         noise_vec_ = RandomVectorGaussianR(Vector3r::Zero(), params_.noise_sigma);
         bias_vec_ = RandomVectorR(-params_.noise_bias, params_.noise_bias).next();
+
+        //initialize frequency limiter
+        freq_limiter_.initialize(params_.update_frequency, params_.startup_delay);
+        delay_line_.initialize(params_.update_latency);
     }
 
     //*** Start: UpdatableObject implementation ***//
@@ -29,12 +33,25 @@ class MagnetometerSimple : public MagnetometerBase {
         //Ground truth is reset before sensors are reset
         updateReference(getGroundTruth());
         noise_vec_.reset();
-        updateOutput();
+
+        freq_limiter_.reset();
+        delay_line_.reset();
+
+        delay_line_.push_back(getOutputInternal());
     }
 
     virtual void update() override
     {
-        updateOutput();
+        freq_limiter_.update();
+
+        if (freq_limiter_.isWaitComplete()) { 
+            delay_line_.push_back(getOutputInternal());
+        }
+
+        delay_line_.update();
+
+        if (freq_limiter_.isWaitComplete())
+            setOutput(delay_line_.getOutput());
     }
     //*** End: UpdatableObject implementation ***//
 
@@ -56,7 +73,7 @@ class MagnetometerSimple : public MagnetometerBase {
             throw std::invalid_argument("magnetic reference source type is not recognized");
         }
     }
-    void updateOutput()
+    Output getOutputInternal()
     {
         Output output;
         const GroundTruth& ground_truth = getGroundTruth();
@@ -71,7 +88,7 @@ class MagnetometerSimple : public MagnetometerBase {
             + noise_vec_.next()
             + bias_vec_;
 
-        setOutput(output);
+        return output;
     }
 
 private:
@@ -80,6 +97,10 @@ class MagnetometerSimple : public MagnetometerBase {
 
     Vector3r magnetic_field_true_;
     MagnetometerSimpleParams params_;
+
+
+    FrequencyLimiter freq_limiter_;
+    DelayLine<Output> delay_line_;
 };
 
 }} //namespace

AirLib/include/sensors/magnetometer/MagnetometerSimpleParams.hpp

@@ -26,6 +26,11 @@ struct MagnetometerSimpleParams {
     ReferenceSource ref_source = ReferenceSource::ReferenceSource_DipoleModel;
     //bool dynamic_reference_source = false;
     //ReferenceSource ref_source = ReferenceSource::ReferenceSource_Constant;
+
+    //see PX4 param reference for EKF: https://dev.px4.io/en/advanced/parameter_reference.html
+    real_T update_latency = 0.0f;    //sec: from PX4 doc
+    real_T update_frequency = 50;    //Hz
+    real_T startup_delay = 0;        //sec
 };
 
 

Examples/Examples.vcxproj

@@ -185,6 +185,7 @@
     <ClCompile Include="main.cpp" />
   </ItemGroup>
   <ItemGroup>
+    <ClInclude Include="GaussianMarkovTest.hpp" />
     <ClInclude Include="RandomPointPoseGenerator.hpp" />
     <ClInclude Include="StandAlonePhysics.hpp" />
     <ClInclude Include="StandAloneSensors.hpp" />

Examples/Examples.vcxproj.filters

@@ -32,5 +32,8 @@
     <ClInclude Include="RandomPointPoseGenerator.hpp">
       <Filter>Header Files</Filter>
     </ClInclude>
+    <ClInclude Include="GaussianMarkovTest.hpp">
+      <Filter>Header Files</Filter>
+    </ClInclude>
   </ItemGroup>
 </Project>