New Feature: Extended Kalman Filter State Estimator for SimpleFlight

.gitignore

@@ -397,5 +397,12 @@ build_docs/
 # api docs
 PythonClient/docs/_build
 
-# Docker
-/docker/Blocks/
+
+# custom: ignore
+/AirLibUnitTests/data
+/AirLibUnitTests/figures
+/AirLibUnitTests/log.txt
+/AirLibUnitTests/data.csv
+/AirLibUnitTests/figure*.pdf
+/AirLibUnitTests/other.txt
+build_AirSimEkfPod/

AirLib/include/common/AirSimSettings.hpp

@@ -219,6 +219,12 @@ namespace airlib
             float follow_distance = Utils::nan<float>();
         };
 
+        struct EkfSetting
+        {
+            bool enabled = false;
+            Settings settings;
+        };
+
         struct SensorSetting
         {
             SensorBase::SensorType sensor_type;
@@ -273,6 +279,7 @@ namespace airlib
 
             std::map<std::string, CameraSetting> cameras;
             std::map<std::string, std::shared_ptr<SensorSetting>> sensors;
+            std::shared_ptr<EkfSetting> ekf_setting;
 
             RCSettings rc;
 
@@ -410,6 +417,9 @@ namespace airlib
         float speed_unit_factor = 1.0f;
         std::string speed_unit_label = "m\\s";
         std::map<std::string, std::shared_ptr<SensorSetting>> sensor_defaults;
+
+        std::shared_ptr<EkfSetting> ekf_setting;
+
         Vector3r wind = Vector3r::Zero();
         std::map<std::string, CameraSetting> external_cameras;
 
@@ -446,7 +456,8 @@ namespace airlib
             loadPawnPaths(settings_json, pawn_paths);
             loadOtherSettings(settings_json);
             loadDefaultSensorSettings(simmode_name, settings_json, sensor_defaults);
-            loadVehicleSettings(simmode_name, settings_json, vehicles, sensor_defaults);
+            loadEkfSettings(settings_json);
+            loadVehicleSettings(simmode_name, settings_json, vehicles, sensor_defaults, ekf_setting);
             loadExternalCameraSettings(settings_json, external_cameras);
 
             //this should be done last because it depends on vehicles (and/or their type) we have
@@ -801,7 +812,8 @@ namespace airlib
 
         static std::unique_ptr<VehicleSetting> createVehicleSetting(const std::string& simmode_name, const Settings& settings_json,
                                                                     const std::string vehicle_name,
-                                                                    std::map<std::string, std::shared_ptr<SensorSetting>>& sensor_defaults)
+                                                                    std::map<std::string, std::shared_ptr<SensorSetting>>& sensor_defaults,
+                                                                    std::shared_ptr<EkfSetting> ekf_setting)
         {
             auto vehicle_type = Utils::toLower(settings_json.getString("VehicleType", ""));
 
@@ -846,11 +858,15 @@ namespace airlib
             loadCameraSettings(settings_json, vehicle_setting->cameras);
             loadSensorSettings(settings_json, "Sensors", vehicle_setting->sensors, sensor_defaults);
 
+            // add ekf setting
+            vehicle_setting->ekf_setting = ekf_setting;
+
             return vehicle_setting;
         }
 
         static void createDefaultVehicle(const std::string& simmode_name, std::map<std::string, std::unique_ptr<VehicleSetting>>& vehicles,
-                                         const std::map<std::string, std::shared_ptr<SensorSetting>>& sensor_defaults)
+                                         const std::map<std::string, std::shared_ptr<SensorSetting>>& sensor_defaults,
+                                         std::shared_ptr<EkfSetting> ekf_setting)
         {
             vehicles.clear();
 
@@ -864,6 +880,10 @@ namespace airlib
                 // currently keyboard is not supported so use rc as default
                 simple_flight_setting->rc.remote_control_id = 0;
                 simple_flight_setting->sensors = sensor_defaults;
+
+                // add ekf setting
+                simple_flight_setting->ekf_setting = ekf_setting;
+
                 vehicles[simple_flight_setting->vehicle_name] = std::move(simple_flight_setting);
             }
             else if (simmode_name == kSimModeTypeCar) {
@@ -887,9 +907,10 @@ namespace airlib
 
         static void loadVehicleSettings(const std::string& simmode_name, const Settings& settings_json,
                                         std::map<std::string, std::unique_ptr<VehicleSetting>>& vehicles,
-                                        std::map<std::string, std::shared_ptr<SensorSetting>>& sensor_defaults)
+                                        std::map<std::string, std::shared_ptr<SensorSetting>>& sensor_defaults,
+                                        std::shared_ptr<EkfSetting> ekf_setting)
         {
-            createDefaultVehicle(simmode_name, vehicles, sensor_defaults);
+            createDefaultVehicle(simmode_name, vehicles, sensor_defaults, ekf_setting);
 
             msr::airlib::Settings vehicles_child;
             if (settings_json.getChild("Vehicles", vehicles_child)) {
@@ -903,7 +924,7 @@ namespace airlib
                 for (const auto& key : keys) {
                     msr::airlib::Settings child;
                     vehicles_child.getChild(key, child);
-                    vehicles[key] = createVehicleSetting(simmode_name, child, key, sensor_defaults);
+                    vehicles[key] = createVehicleSetting(simmode_name, child, key, sensor_defaults, ekf_setting);
                 }
             }
         }
@@ -1347,6 +1368,28 @@ namespace airlib
             }
         }
 
+        static std::shared_ptr<EkfSetting> createEkfSettings(bool enabled)
+        {
+            std::shared_ptr<EkfSetting> ekf_setting;
+
+            ekf_setting = std::shared_ptr<EkfSetting>(new EkfSetting());
+            ekf_setting->enabled = enabled;
+
+            return ekf_setting;
+        }
+
+        // creates and intializes Extended Kalman Filter (ekf) settings from json
+        void loadEkfSettings(const Settings& settings_json)
+        {
+            msr::airlib::Settings child;
+            if (settings_json.getChild("EkfSetting", child)) {
+                auto enabled = child.getBool("Enabled", false);
+                ekf_setting = createEkfSettings(enabled);
+                ekf_setting->enabled = enabled;
+                ekf_setting->settings = child;
+            }
+        }
+
         // creates default sensor list when none specified in json
         static void createDefaultSensorSettings(const std::string& simmode_name,
                                                 std::map<std::string, std::shared_ptr<SensorSetting>>& sensors)

AirLib/include/common/EkfStructs.hpp

@@ -0,0 +1,46 @@
+
+#ifndef msr_airlib_EkfStructs_hpp
+#define msr_airlib_EkfStructs_hpp
+
+#include "common/Common.hpp"
+
+namespace msr
+{
+namespace airlib
+{
+
+struct SensorMeasurements
+{
+    Vector3r accel;
+    Vector3r gyro;
+
+    Vector3r gps_position;
+    Vector3r gps_velocity;
+
+    real_T baro_altitude;
+
+    Vector3r magnetic_flux;
+};
+
+struct SensorBiases
+{
+    Vector3r accel;
+    Vector3r gyro;
+
+    real_T barometer;
+};
+
+struct EkfKinematicsState
+{
+    Vector3r position;
+    Quaternionr orientation;
+    Vector3r angles;
+
+    Vector3r linear_velocity;
+
+    SensorBiases sensor_bias;
+};
+
+}
+} //namespace
+#endif

AirLib/include/common/GeodeticConverter.hpp

@@ -39,10 +39,8 @@ namespace airlib
             geodetic2Ecef(home_latitude_, home_longitude_, home_altitude_, &home_ecef_x_, &home_ecef_y_, &home_ecef_z_);
 
             // Compute ECEF to NED and NED to ECEF matrices
-            double phiP = atan2(home_ecef_z_, sqrt(pow(home_ecef_x_, 2) + pow(home_ecef_y_, 2)));
-
-            ecef_to_ned_matrix_ = nRe(phiP, home_longitude_rad_);
-            ned_to_ecef_matrix_ = nRe(home_latitude_rad_, home_longitude_rad_).transpose();
+            ecef_to_ned_matrix_ = nRe(home_latitude_rad_, home_longitude_rad_);
+            ned_to_ecef_matrix_ = ecef_to_ned_matrix_.inverse();
         }
 
         void setHome(const GeoPoint& home_geopoint)
@@ -136,17 +134,22 @@ namespace airlib
             ecef2Ned(x, y, z, north, east, down);
         }
 
+        void geodetic2Ned(const GeoPoint& geopoint, Vector3r& ned_pos)
+        {
+            double north, east, down;
+            geodetic2Ned(geopoint.latitude, geopoint.longitude, geopoint.altitude, &north, &east, &down);
+            ned_pos[0] = static_cast<float>(north);
+            ned_pos[1] = static_cast<float>(east);
+            ned_pos[2] = static_cast<float>(down);
+        }
+
         void ned2Geodetic(const double north, const double east, const float down, double* latitude,
                           double* longitude, float* altitude)
         {
             // Local NED position to geodetic coordinates
             double x, y, z;
             ned2Ecef(north, east, down, &x, &y, &z);
             ecef2Geodetic(x, y, z, latitude, longitude, altitude);
-
-            //TODO: above returns wrong altitude if down was positive. This is because sqrt return value would be -ve
-            //but normal sqrt only return +ve. For now we just override it.
-            *altitude = home_altitude_ - down;
         }
 
         void ned2Geodetic(const Vector3r& ned_pos, GeoPoint& geopoint)

AirLib/include/common/VectorMath.hpp

@@ -37,6 +37,34 @@ namespace airlib
         typedef Eigen::Quaternion<double, Eigen::DontAlign> Quaterniond;
         typedef Eigen::Matrix<double, 3, 3> Matrix3x3d;
         typedef Eigen::Matrix<float, 3, 3> Matrix3x3f;
+
+        typedef Eigen::Matrix<float, 17, 1> Vector17f;
+        typedef Eigen::Matrix<float, 16, 1> Vector16f;
+        typedef Eigen::Matrix<float, 17, 17> Matrix17x17f;
+        typedef Eigen::Matrix<float, 17, 13> Matrix17x13f;
+        typedef Eigen::Matrix<float, 13, 13> Matrix13x13f;
+        typedef Eigen::Matrix<float, 1, 17> Matrix1x17f;
+        typedef Eigen::Matrix<float, 17, 1> Matrix17x1f;
+        typedef Eigen::Matrix<float, 3, 17> Matrix3x17f;
+        typedef Eigen::Matrix<float, 17, 3> Matrix17x3f;
+        typedef Eigen::Matrix<float, 6, 17> Matrix6x17f;
+        typedef Eigen::Matrix<float, 17, 6> Matrix17x6f;
+        typedef Eigen::Matrix<float, 13, 1> Vector13f;
+        typedef Eigen::Matrix<float, 7, 1> Vector7f;
+        typedef Eigen::Matrix<float, 6, 6> Matrix6x6f;
+        typedef Eigen::Matrix<float, 3, 4> Matrix3x4f;
+        typedef Eigen::Matrix<float, 4, 4> Matrix4x4f;
+
+        typedef Eigen::Matrix<float, 16, 16> Matrix16x16f;
+        typedef Eigen::Matrix<float, 16, 12> Matrix16x12f;
+        typedef Eigen::Matrix<float, 12, 12> Matrix12x12f;
+        typedef Eigen::Matrix<float, 1, 16> Matrix1x16f;
+        typedef Eigen::Matrix<float, 16, 1> Matrix16x1f;
+        typedef Eigen::Matrix<float, 3, 16> Matrix3x16f;
+        typedef Eigen::Matrix<float, 16, 3> Matrix16x3f;
+        typedef Eigen::Matrix<float, 6, 16> Matrix6x16f;
+        typedef Eigen::Matrix<float, 16, 6> Matrix16x6f;
+
         typedef Eigen::AngleAxisd AngleAxisd;
         typedef Eigen::AngleAxisf AngleAxisf;
 

AirLib/include/physics/Environment.hpp

@@ -8,6 +8,7 @@
 #include "common/UpdatableObject.hpp"
 #include "common/CommonStructs.hpp"
 #include "common/EarthUtils.hpp"
+#include "common/GeodeticConverter.hpp"
 
 namespace msr
 {
@@ -53,12 +54,13 @@ namespace airlib
 
             setHomeGeoPoint(initial_.geo_point);
 
-            updateState(initial_, home_geo_point_);
+            updateState(initial_);
         }
 
         void setHomeGeoPoint(const GeoPoint& home_geo_point)
         {
             home_geo_point_ = HomeGeoPoint(home_geo_point);
+            geodetic_converter_.setHome(home_geo_point);
         }
 
         GeoPoint getHomeGeoPoint() const
@@ -87,7 +89,7 @@ namespace airlib
 
         virtual void update() override
         {
-            updateState(current_, home_geo_point_);
+            updateState(current_);
         }
 
     protected:
@@ -106,9 +108,9 @@ namespace airlib
         }
 
     private:
-        static void updateState(State& state, const HomeGeoPoint& home_geo_point)
+        void updateState(State& state)
         {
-            state.geo_point = EarthUtils::nedToGeodetic(state.position, home_geo_point);
+            geodetic_converter_.ned2Geodetic(state.position, state.geo_point);
 
             real_T geo_pot = EarthUtils::getGeopotential(state.geo_point.altitude / 1000.0f);
             state.temperature = EarthUtils::getStandardTemperature(geo_pot);
@@ -122,6 +124,7 @@ namespace airlib
     private:
         State initial_, current_;
         HomeGeoPoint home_geo_point_;
+        GeodeticConverter geodetic_converter_;
     };
 }
 } //namespace

AirLib/include/sensors/SensorBase.hpp

@@ -52,6 +52,16 @@ namespace airlib
             ground_truth_.environment = environment;
         }
 
+        virtual void update() override
+        {
+            is_new_ = false;
+        }
+
+        const bool& checkIfNew() const
+        {
+            return is_new_;
+        }
+
         const GroundTruth& getGroundTruth() const
         {
             return ground_truth_;
@@ -68,6 +78,9 @@ namespace airlib
         //ground truth can be shared between many sensors
         GroundTruth ground_truth_ = { nullptr, nullptr };
         std::string name_ = "";
+
+    protected:
+        bool is_new_ = false;
     };
 }
 } //namespace

AirLib/include/sensors/barometer/BarometerSimple.hpp

@@ -63,8 +63,11 @@ namespace airlib
 
             delay_line_.update();
 
-            if (freq_limiter_.isWaitComplete())
+            if (freq_limiter_.isWaitComplete()) {
                 setOutput(delay_line_.getOutput());
+
+                is_new_ = true;
+            }
         }
         //*** End: UpdatableState implementation ***//