diff --git a/.gitignore b/.gitignore index 64d80d637e..a3c5a73484 100644 --- a/.gitignore +++ b/.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/ diff --git a/AirLib/include/common/AirSimSettings.hpp b/AirLib/include/common/AirSimSettings.hpp index 25108b4b7d..71fab2ae3d 100644 --- a/AirLib/include/common/AirSimSettings.hpp +++ b/AirLib/include/common/AirSimSettings.hpp @@ -219,6 +219,12 @@ namespace airlib float follow_distance = Utils::nan(); }; + struct EkfSetting + { + bool enabled = false; + Settings settings; + }; + struct SensorSetting { SensorBase::SensorType sensor_type; @@ -273,6 +279,7 @@ namespace airlib std::map cameras; std::map> sensors; + std::shared_ptr 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> sensor_defaults; + + std::shared_ptr ekf_setting; + Vector3r wind = Vector3r::Zero(); std::map 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 createVehicleSetting(const std::string& simmode_name, const Settings& settings_json, const std::string vehicle_name, - std::map>& sensor_defaults) + std::map>& sensor_defaults, + std::shared_ptr 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>& vehicles, - const std::map>& sensor_defaults) + const std::map>& sensor_defaults, + std::shared_ptr 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>& vehicles, - std::map>& sensor_defaults) + std::map>& sensor_defaults, + std::shared_ptr 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 createEkfSettings(bool enabled) + { + std::shared_ptr ekf_setting; + + ekf_setting = std::shared_ptr(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>& sensors) diff --git a/AirLib/include/common/EkfStructs.hpp b/AirLib/include/common/EkfStructs.hpp new file mode 100644 index 0000000000..034edce048 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/AirLib/include/common/GeodeticConverter.hpp b/AirLib/include/common/GeodeticConverter.hpp index cffd886625..f1de155713 100644 --- a/AirLib/include/common/GeodeticConverter.hpp +++ b/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,6 +134,15 @@ 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(north); + ned_pos[1] = static_cast(east); + ned_pos[2] = static_cast(down); + } + void ned2Geodetic(const double north, const double east, const float down, double* latitude, double* longitude, float* altitude) { @@ -143,10 +150,6 @@ namespace airlib 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) diff --git a/AirLib/include/common/VectorMath.hpp b/AirLib/include/common/VectorMath.hpp index 971728e086..4a983811fa 100644 --- a/AirLib/include/common/VectorMath.hpp +++ b/AirLib/include/common/VectorMath.hpp @@ -37,6 +37,34 @@ namespace airlib typedef Eigen::Quaternion Quaterniond; typedef Eigen::Matrix Matrix3x3d; typedef Eigen::Matrix Matrix3x3f; + + typedef Eigen::Matrix Vector17f; + typedef Eigen::Matrix Vector16f; + typedef Eigen::Matrix Matrix17x17f; + typedef Eigen::Matrix Matrix17x13f; + typedef Eigen::Matrix Matrix13x13f; + typedef Eigen::Matrix Matrix1x17f; + typedef Eigen::Matrix Matrix17x1f; + typedef Eigen::Matrix Matrix3x17f; + typedef Eigen::Matrix Matrix17x3f; + typedef Eigen::Matrix Matrix6x17f; + typedef Eigen::Matrix Matrix17x6f; + typedef Eigen::Matrix Vector13f; + typedef Eigen::Matrix Vector7f; + typedef Eigen::Matrix Matrix6x6f; + typedef Eigen::Matrix Matrix3x4f; + typedef Eigen::Matrix Matrix4x4f; + + typedef Eigen::Matrix Matrix16x16f; + typedef Eigen::Matrix Matrix16x12f; + typedef Eigen::Matrix Matrix12x12f; + typedef Eigen::Matrix Matrix1x16f; + typedef Eigen::Matrix Matrix16x1f; + typedef Eigen::Matrix Matrix3x16f; + typedef Eigen::Matrix Matrix16x3f; + typedef Eigen::Matrix Matrix6x16f; + typedef Eigen::Matrix Matrix16x6f; + typedef Eigen::AngleAxisd AngleAxisd; typedef Eigen::AngleAxisf AngleAxisf; diff --git a/AirLib/include/physics/Environment.hpp b/AirLib/include/physics/Environment.hpp index f3e7837ccf..9649ea0a8f 100644 --- a/AirLib/include/physics/Environment.hpp +++ b/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 diff --git a/AirLib/include/sensors/SensorBase.hpp b/AirLib/include/sensors/SensorBase.hpp index 1e344e6f8e..ce650ba4a0 100644 --- a/AirLib/include/sensors/SensorBase.hpp +++ b/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 diff --git a/AirLib/include/sensors/barometer/BarometerSimple.hpp b/AirLib/include/sensors/barometer/BarometerSimple.hpp index fb960e288c..28654daa1d 100644 --- a/AirLib/include/sensors/barometer/BarometerSimple.hpp +++ b/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 ***// diff --git a/AirLib/include/sensors/gps/GpsSimple.hpp b/AirLib/include/sensors/gps/GpsSimple.hpp index 23170bf943..3075d6ba2c 100644 --- a/AirLib/include/sensors/gps/GpsSimple.hpp +++ b/AirLib/include/sensors/gps/GpsSimple.hpp @@ -38,6 +38,9 @@ namespace airlib //*** Start: UpdatableState implementation ***// virtual void resetImplementation() override { + gauss_dist_pos.reset(); + gauss_dist_vel.reset(); + freq_limiter_.reset(); delay_line_.reset(); @@ -61,8 +64,11 @@ namespace airlib delay_line_.update(); - if (freq_limiter_.isWaitComplete()) + if (freq_limiter_.isWaitComplete()) { setOutput(delay_line_.getOutput()); + + is_new_ = true; + } } //*** End: UpdatableState implementation ***// @@ -78,9 +84,21 @@ namespace airlib //GNSS output.gnss.time_utc = static_cast(clock()->nowNanos() / 1.0E3); output.gnss.geo_point = ground_truth.environment->getState().geo_point; + + // add Gaussian white noise to the ground truth gps position outputs + output.gnss.geo_point.longitude += gauss_dist_pos.next()[0] * params_.sigma_long; + output.gnss.geo_point.latitude += gauss_dist_pos.next()[1] * params_.sigma_lat; + output.gnss.geo_point.altitude += gauss_dist_pos.next()[2] * params_.sigma_alt; + output.gnss.eph = eph; output.gnss.epv = epv; + + // add Gaussian white noise to the ground truth gps position outputs output.gnss.velocity = ground_truth.kinematics->twist.linear; + output.gnss.velocity.x() += gauss_dist_vel.next()[0] * params_.sigma_vel_x; + output.gnss.velocity.y() += gauss_dist_vel.next()[1] * params_.sigma_vel_y; + output.gnss.velocity.z() += gauss_dist_vel.next()[2] * params_.sigma_vel_z; + output.is_valid = true; output.gnss.fix_type = @@ -101,6 +119,9 @@ namespace airlib FirstOrderFilter eph_filter, epv_filter; FrequencyLimiter freq_limiter_; DelayLine delay_line_; + + RandomVectorGaussianR gauss_dist_pos = RandomVectorGaussianR(0, 1); + RandomVectorGaussianR gauss_dist_vel = RandomVectorGaussianR(0, 1); }; } } //namespace diff --git a/AirLib/include/sensors/gps/GpsSimpleParams.hpp b/AirLib/include/sensors/gps/GpsSimpleParams.hpp index 74cb9a834e..27942d6016 100644 --- a/AirLib/include/sensors/gps/GpsSimpleParams.hpp +++ b/AirLib/include/sensors/gps/GpsSimpleParams.hpp @@ -18,6 +18,13 @@ namespace airlib real_T eph_final = 0.1f, epv_final = 0.1f; // PX4 won't converge GPS sensor fusion until we get to 10% accuracty. real_T eph_min_3d = 3.0f, eph_min_2d = 4.0f; + double sigma_long = 0.000001; // deg + double sigma_lat = 0.000001; // deg + real_T sigma_alt = 1.0f; // m + real_T sigma_vel_x = 0.1f; // m/s + real_T sigma_vel_y = 0.1f; // m/s + real_T sigma_vel_z = 0.1f; // m/s + real_T update_latency = 0.2f; //sec real_T update_frequency = 50; //Hz real_T startup_delay = 1; //sec @@ -33,6 +40,14 @@ namespace airlib epv_final = json.getFloat("EpvFinal", epv_final); eph_min_3d = json.getFloat("EphMin3d", eph_min_3d); eph_min_2d = json.getFloat("EphMin2d", eph_min_2d); + + sigma_long = json.getDouble("SigmaLong", sigma_long); + sigma_lat = json.getDouble("SigmaLat", sigma_lat); + sigma_alt = json.getFloat("SigmaAlt", sigma_alt); + sigma_vel_x = json.getFloat("SigmaVelX", sigma_vel_x); + sigma_vel_y = json.getFloat("SigmaVelY", sigma_vel_y); + sigma_vel_z = json.getFloat("SigmaVelZ", sigma_vel_z); + update_latency = json.getFloat("UpdateLatency", update_latency); update_frequency = json.getFloat("UpdateFrequency", update_frequency); startup_delay = json.getFloat("StartupDelay", startup_delay); diff --git a/AirLib/include/sensors/imu/ImuSimple.hpp b/AirLib/include/sensors/imu/ImuSimple.hpp index 7165682e14..f18ce2049b 100644 --- a/AirLib/include/sensors/imu/ImuSimple.hpp +++ b/AirLib/include/sensors/imu/ImuSimple.hpp @@ -43,6 +43,8 @@ namespace airlib ImuBase::update(); updateOutput(); + + is_new_ = true; } //*** End: UpdatableState implementation ***// diff --git a/AirLib/include/sensors/imu/ImuSimpleParams.hpp b/AirLib/include/sensors/imu/ImuSimpleParams.hpp index f2834e8e21..42d564d364 100644 --- a/AirLib/include/sensors/imu/ImuSimpleParams.hpp +++ b/AirLib/include/sensors/imu/ImuSimpleParams.hpp @@ -69,6 +69,22 @@ namespace airlib if (!std::isnan(bias_stability)) { accel.bias_stability = bias_stability * 1E-6f * EarthUtils::Gravity; //ug converted to m/s^2 } + accel.turn_on_bias.x() = json.getFloat("AccelTurnOnBiasX", accel.turn_on_bias.x()); + accel.turn_on_bias.y() = json.getFloat("AccelTurnOnBiasY", accel.turn_on_bias.y()); + accel.turn_on_bias.z() = json.getFloat("AccelTurnOnBiasZ", accel.turn_on_bias.z()); + + auto gyro_turn_on_bias_x = json.getFloat("GyroTurnOnBiasX", Utils::nan()); + if (!std::isnan(gyro_turn_on_bias_x)) { + gyro.turn_on_bias.x() = gyro_turn_on_bias_x * M_PIf / 180; // deg/s to rad/s + } + auto gyro_turn_on_bias_y = json.getFloat("GyroTurnOnBiasY", Utils::nan()); + if (!std::isnan(gyro_turn_on_bias_y)) { + gyro.turn_on_bias.y() = gyro_turn_on_bias_y * M_PIf / 180; // deg/s to rad/s + } + auto gyro_turn_on_bias_z = json.getFloat("GyroTurnOnBiasZ", Utils::nan()); + if (!std::isnan(gyro_turn_on_bias_z)) { + gyro.turn_on_bias.z() = gyro_turn_on_bias_z * M_PIf / 180; // deg/s to rad/s + } } }; } diff --git a/AirLib/include/sensors/magnetometer/MagnetometerSimple.hpp b/AirLib/include/sensors/magnetometer/MagnetometerSimple.hpp index f76856de8a..6f0d939a2e 100644 --- a/AirLib/include/sensors/magnetometer/MagnetometerSimple.hpp +++ b/AirLib/include/sensors/magnetometer/MagnetometerSimple.hpp @@ -59,8 +59,11 @@ namespace airlib delay_line_.update(); - if (freq_limiter_.isWaitComplete()) + if (freq_limiter_.isWaitComplete()) { setOutput(delay_line_.getOutput()); + + is_new_ = true; + } } //*** End: UpdatableObject implementation ***// diff --git a/AirLib/include/vehicles/multirotor/api/MultirotorApiBase.hpp b/AirLib/include/vehicles/multirotor/api/MultirotorApiBase.hpp index 01a8108d37..667d6a05cd 100644 --- a/AirLib/include/vehicles/multirotor/api/MultirotorApiBase.hpp +++ b/AirLib/include/vehicles/multirotor/api/MultirotorApiBase.hpp @@ -5,6 +5,7 @@ #define air_DroneControlServer_hpp #include "common/Common.hpp" +#include "common/EkfStructs.hpp" #include "MultirotorCommon.hpp" #include "safety/SafetyEval.hpp" #include "physics/Kinematics.hpp" diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp new file mode 100644 index 0000000000..9290f1a3cc --- /dev/null +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp @@ -0,0 +1,737 @@ +// Liscence info + +#ifndef msr_airlib_AirSimSimpleEkf_hpp +#define msr_airlib_AirSimSimpleEkf_hpp + +#include +#include +#include +#include "firmware/interfaces/IBoard.hpp" +#include "firmware/interfaces/ICommLink.hpp" +#include "common/FrequencyLimiter.hpp" +#include "AirSimSimpleEkfBase.hpp" +#include "AirSimSimpleEkfModel.hpp" +#include "AirSimSimpleEkfParams.hpp" +#include "common/GeodeticConverter.hpp" + +namespace msr +{ +namespace airlib +{ + + class AirSimSimpleEkf : public AirSimSimpleEkfBase + , private AirSimSimpleEkfModel + { + public: + // Constructor + AirSimSimpleEkf(const simple_flight::IBoard* board, simple_flight::ICommLink* comm_link, const AirSimSettings::EkfSetting* setting = nullptr) + : board_(board) + { + params_.initializeParameters(setting); + freq_limiter_.initialize(334); // physics engine and the imu refresh at period 3ms ~ 333.33Hz + } + + virtual void reset() override + { + IEkf::reset(); + + freq_limiter_.reset(); + initializeFilter(); + } + + virtual void update() override + { + IEkf::update(); + + // update the frequency limiter + freq_limiter_.update(); + + // if the wait is complete and it is time to update EKF, update EKF + if (freq_limiter_.isWaitComplete()) + updateEKFInternal(); + } + + // only in simulation + void setGroundTruthKinematics(const Kinematics::State* kinematics, const Environment* environment) + { + kinematics_ = kinematics; + environment_ = environment; + } + + private: + // --------------------------------------------------------------------- + // Internal functions + // --------------------------------------------------------------------- + + // initialize filter + void initializeFilter() + { + assignEkfMeasurementMatrics(); + assignEkfStateMatrics(); + resetGlobalVariables(); + } + + void assignEkfMeasurementMatrics() + { + Q_ = simple_flight::MatrixNWxNWf::Zero(); + // imu + Q_(0, 0) = params_.accel.std_error.x() * params_.accel.std_error.x(); + Q_(1, 1) = params_.accel.std_error.y() * params_.accel.std_error.y(); + Q_(2, 2) = params_.accel.std_error.z() * params_.accel.std_error.z(); + Q_(3, 3) = params_.gyro.std_error.x() * params_.gyro.std_error.x(); + Q_(4, 4) = params_.gyro.std_error.y() * params_.gyro.std_error.y(); + Q_(5, 5) = params_.gyro.std_error.z() * params_.gyro.std_error.z(); + + // biases + Q_(6, 6) = 0.0001f; + Q_(7, 7) = 0.0001f; + Q_(8, 8) = 0.0001f; + Q_(9, 9) = 0.0001f; + Q_(10, 10) = 0.0001f; + Q_(11, 11) = 0.0001f; + Q_(12, 12) = 0.0001f; + + // gps + R_gps_ = VectorMath::Matrix6x6f::Zero(); + R_gps_(0, 0) = params_.gps.std_error_position.x() * params_.gps.std_error_position.x(); + R_gps_(1, 1) = params_.gps.std_error_position.y() * params_.gps.std_error_position.y(); + R_gps_(2, 2) = params_.gps.std_error_position.z() * params_.gps.std_error_position.z(); + R_gps_(3, 3) = params_.gps.std_error_velocity.x() * params_.gps.std_error_velocity.x(); + R_gps_(4, 4) = params_.gps.std_error_velocity.y() * params_.gps.std_error_velocity.y(); + R_gps_(5, 5) = params_.gps.std_error_velocity.z() * params_.gps.std_error_velocity.z(); + + // magnetometer + R_mag_ = VectorMath::Matrix3x3f::Zero(); + R_mag_(0, 0) = params_.mag.std_error.x() * params_.mag.std_error.x(); + R_mag_(1, 1) = params_.mag.std_error.y() * params_.mag.std_error.y(); + R_mag_(2, 2) = params_.mag.std_error.z() * params_.mag.std_error.z(); + + // barometer + R_baro_ = params_.baro.std_error * params_.baro.std_error; + + // barometer + R_pseudo_ = params_.pseudo_meas.quaternion_norm_R; + } + + void assignEkfStateMatrics() + { + // intialize the ekf states + states_ = simple_flight::VectorNXf::Zero(); + + states_(0) = kinematics_->pose.position.x() - params_.initial_states_err.position.x(); + states_(1) = kinematics_->pose.position.y() - params_.initial_states_err.position.y(); + states_(2) = kinematics_->pose.position.z() - params_.initial_states_err.position.z(); + states_(3) = kinematics_->twist.linear.x() - params_.initial_states_err.linear_velocity.x(); + states_(4) = kinematics_->twist.linear.y() - params_.initial_states_err.linear_velocity.y(); + states_(5) = kinematics_->twist.linear.z() - params_.initial_states_err.linear_velocity.z(); + states_(6) = params_.initial_states_err.quaternion.w(); + states_(7) = params_.initial_states_err.quaternion.x(); + states_(8) = params_.initial_states_err.quaternion.y(); + states_(9) = params_.initial_states_err.quaternion.z(); + states_(10) = params_.initial_states_err.accel_bias.x(); + states_(11) = params_.initial_states_err.accel_bias.y(); + states_(12) = params_.initial_states_err.accel_bias.z(); + states_(13) = params_.initial_states_err.gyro_bias.x(); + states_(14) = params_.initial_states_err.gyro_bias.y(); + states_(15) = params_.initial_states_err.gyro_bias.z(); + states_(16) = params_.initial_states_err.baro_bias; + + // intitialize the ekf covariances + error_covariance_ = simple_flight::MatrixNXxNXf::Zero(); + error_covariance_(0, 0) = static_cast(pow(params_.initial_states_std_err.position.x(), 2)); + error_covariance_(1, 1) = static_cast(pow(params_.initial_states_std_err.position.y(), 2)); + error_covariance_(2, 2) = static_cast(pow(params_.initial_states_std_err.position.z(), 2)); + error_covariance_(3, 3) = static_cast(pow(params_.initial_states_std_err.linear_velocity.x(), 2)); + error_covariance_(4, 4) = static_cast(pow(params_.initial_states_std_err.linear_velocity.y(), 2)); + error_covariance_(5, 5) = static_cast(pow(params_.initial_states_std_err.linear_velocity.z(), 2)); + error_covariance_(6, 6) = static_cast(pow(params_.initial_states_std_err.quaternion.w(), 2)); + error_covariance_(7, 7) = static_cast(pow(params_.initial_states_std_err.quaternion.x(), 2)); + error_covariance_(8, 8) = static_cast(pow(params_.initial_states_std_err.quaternion.y(), 2)); + error_covariance_(9, 9) = static_cast(pow(params_.initial_states_std_err.quaternion.z(), 2)); + error_covariance_(10, 10) = static_cast(pow(params_.initial_states_std_err.accel_bias.x(), 2)); + error_covariance_(11, 11) = static_cast(pow(params_.initial_states_std_err.accel_bias.y(), 2)); + error_covariance_(12, 12) = static_cast(pow(params_.initial_states_std_err.accel_bias.z(), 2)); + error_covariance_(13, 13) = static_cast(pow(params_.initial_states_std_err.gyro_bias.x(), 2)); + error_covariance_(14, 14) = static_cast(pow(params_.initial_states_std_err.gyro_bias.y(), 2)); + error_covariance_(15, 15) = static_cast(pow(params_.initial_states_std_err.gyro_bias.z(), 2)); + error_covariance_(16, 16) = static_cast(pow(params_.initial_states_std_err.baro_bias, 2)); + } + + void resetGlobalVariables() + { + // reset last update times + last_times_.state_propagation = board_->micros(); + last_times_.cov_propagation = board_->micros(); + + // reset geo and magnetic global variables + geodetic_converter_.setHome(environment_->getHomeGeoPoint()); + VectorMath::Vector3f magnetic_field_true = EarthUtils::getMagField(environment_->getState().geo_point) * 1E4f; // in Gauss + earth_mag_[0] = magnetic_field_true.x(); + earth_mag_[1] = magnetic_field_true.y(); + earth_mag_[2] = magnetic_field_true.z(); + + // reset home altitude + home_altitude_ = environment_->getHomeGeoPoint().altitude; + + // reset imu data buffer todo manage this in better way + real_T accel[3]; + real_T gyro[3]; + bool is_new_and_valid = getImuData(accel, gyro); + unused(is_new_and_valid); + prev_imuData_.accel[0] = 0.0f; + prev_imuData_.accel[1] = 0.0f; + prev_imuData_.accel[2] = -9.80665f; + prev_imuData_.gyro[0] = 0.0f; + prev_imuData_.gyro[1] = 0.0f; + prev_imuData_.gyro[2] = 0.0f; + prev_imuData_.ang_acc[0] = 0.0f; + prev_imuData_.ang_acc[1] = 0.0f; + prev_imuData_.ang_acc[2] = 0.0f; + } + + // this function updates at the frequency of EKF update + void updateEKFInternal() + { + predictionStep(); + measurementUpdateStep(); + eulerAnglesCovariancePropagation(); + } + + // prediction step + void predictionStep() + { + // the entire prediction step updates at the frequency of imu update + // TODO later state propagation and covariance propagation can be decoupled! + + if (!board_->checkImuIfNew()) + return; + + real_T accel[3]; + real_T gyro[3]; + + // check if the IMU gives new measurement and it is valid + bool is_new_and_valid = getImuData(accel, gyro); + + if (!is_new_and_valid) { + return; + } + + statePropagation(accel, gyro); + covariancePropagation(accel, gyro); + } + + // measurement update step + void measurementUpdateStep() + { + if (params_.fuse_baro) { + barometerUpdate(); + } + if (params_.fuse_mag) { + magnetometerUpdate(); + } + if (params_.fuse_gps) { + gpsUpdate(); + } + pseudoMeasurement(); + } + + // state propagtion + void statePropagation(real_T* accel, real_T* gyro) + { + // extract time step and update time + TTimeDelta delta_T = (board_->micros() - last_times_.state_propagation) / 1.0E6; // in seconds + float dt_real = static_cast(delta_T); + last_times_.state_propagation = board_->micros(); + + // declare local variables + float x[simple_flight::NX]; + float u[simple_flight::NU]; + float uplus[simple_flight::NU]; + + // extract the current ekf states + for (int i = 0; i < simple_flight::NX; i++) { + x[i] = states_(i); + } + + // extract the current controls + for (int i = 0; i < 3; i++) { + uplus[i] = accel[i]; + u[i] = prev_imuData_.accel[i]; + uplus[i + 3] = gyro[i]; + u[i + 3] = prev_imuData_.gyro[i]; + } + + // extract angular acceleration data + float ang_accel[3]; + ang_accel[0] = prev_imuData_.ang_acc[0]; + ang_accel[1] = prev_imuData_.ang_acc[1]; + ang_accel[2] = prev_imuData_.ang_acc[2]; + + // do prediction + float x_predicted[simple_flight::NX]; + inertialNavigation(x_predicted, x, u, uplus, ang_accel, dt_real); + + // runge kutta integration + // float x_predicted[17]; + // rungeKutta(x_predicted, x, u, dt_real); + + // heun integration + // float x_predicted[simple_flight::NX]; + // heun(x_predicted, x, u, uplus, dt_real); + + // normalize the quaternions + float norm; + norm = sqrt( + x_predicted[6] * x_predicted[6] + x_predicted[7] * x_predicted[7] + x_predicted[8] * x_predicted[8] + x_predicted[9] * x_predicted[9]); + x_predicted[6] = x_predicted[6] / norm; + x_predicted[7] = x_predicted[7] / norm; + x_predicted[8] = x_predicted[8] / norm; + x_predicted[9] = x_predicted[9] / norm; + + // set the predicted states TODO: via an interface or after some checks + for (int i = 0; i < simple_flight::NX; i++) { + states_(i) = x_predicted[i]; + } + + // update imu data buffer + prev_imuData_.accel[0] = accel[0]; + prev_imuData_.accel[1] = accel[1]; + prev_imuData_.accel[2] = accel[2]; + prev_imuData_.gyro[0] = gyro[0]; + prev_imuData_.gyro[1] = gyro[1]; + prev_imuData_.gyro[2] = gyro[2]; + prev_imuData_.ang_acc[0] = kinematics_->accelerations.angular.x(); + prev_imuData_.ang_acc[1] = kinematics_->accelerations.angular.y(); + prev_imuData_.ang_acc[2] = kinematics_->accelerations.angular.z(); + } + + // co-variance propagtion + void covariancePropagation(real_T* accel, real_T* gyro) + { + // extract time step and update time + TTimeDelta delta_T = (board_->micros() - last_times_.cov_propagation) / 1.0E6; // in seconds + float dt_real = static_cast(delta_T); + last_times_.cov_propagation = board_->micros(); + + // declare local variables + float x[simple_flight::NX]; + float u[simple_flight::NU]; + simple_flight::MatrixNXxNXf A; + simple_flight::MatrixNXxNXf A_finite; + simple_flight::MatrixNXxNWf B_w; + simple_flight::MatrixNXxNXf Phi; + simple_flight::MatrixNXxNWf GammaB_w; + simple_flight::MatrixNXxNXf P = error_covariance_; + simple_flight::MatrixNXxNXf next_covariance; + + // extract the ekf states + for (int i = 0; i < simple_flight::NX; i++) { + x[i] = states_[i]; + } + + // extract the controls + for (int i = 0; i < 3; i++) { + u[i] = accel[i]; + u[i + 3] = gyro[i]; + } + + // evaluate A and B matrix + evaluateA(&A, x, u); + // evaluateFiniteDifferenceA(&A_finite, x, u); + evaluateB_w(&B_w, x, u); + + evaluatePhiAndGamma_w(&Phi, &GammaB_w, &B_w, &A, dt_real); + // evaluatePhiAndGamma_w(&Phi, &GammaB_w, &B_w, &A_finite, dt_real); + + // evaluate next covariance matrix + next_covariance = Phi * P * Phi.transpose() + GammaB_w * Q_ * GammaB_w.transpose(); + + // set the new predicted covariance + error_covariance_ = next_covariance; + } + + void evaluatePhiAndGamma_w(simple_flight::MatrixNXxNXf* Phi, + simple_flight::MatrixNXxNWf* GammaB_w, + simple_flight::MatrixNXxNWf* B_w, + simple_flight::MatrixNXxNXf* A, + float dt_real) + { + // declare local variables + simple_flight::MatrixNXxNXf identity = simple_flight::MatrixNXxNXf::Identity(); + simple_flight::MatrixNXxNXf A_square = (*A) * (*A); + simple_flight::MatrixNXxNXf A_cube = A_square * (*A); + simple_flight::MatrixNXxNXf A_forth = A_cube * (*A); + simple_flight::MatrixNXxNXf A_fifth = A_forth * (*A); + + // calculate Phi matrix + *Phi = identity + (*A) * dt_real + A_square * dt_real * dt_real / 2 + A_cube * dt_real * dt_real * dt_real / 6 + A_forth * dt_real * dt_real * dt_real * dt_real / 24 + A_fifth * dt_real * dt_real * dt_real * dt_real * dt_real / 120; + + // calculate GammaB_w matrix + *GammaB_w = (identity * dt_real + (*A) * dt_real * dt_real / 2 + A_square * dt_real * dt_real * dt_real / 6 + A_cube * dt_real * dt_real * dt_real * dt_real / 24 + A_forth * dt_real * dt_real * dt_real * dt_real * dt_real / 120 + A_fifth * dt_real * dt_real * dt_real * dt_real * dt_real * dt_real / 720) * (*B_w); + } + + // magnetometer update + void magnetometerUpdate() + { + if (!board_->checkMagnetometerIfNew()) + return; + + real_T mag[3]; + + // check if the magnetometer gives new measurement and it is valid + bool is_valid = getMagnetometerData(mag); + + if (!is_valid) { + return; + } + + // extract the ekf states + float x[17]; + for (int i = 0; i < 17; i++) { + x[i] = states_(i); + } + + // evaluate current measurement model + float h_mag[3]; + evaluatehMag(h_mag, x, earth_mag_); + + // evaluate the C matrix + simple_flight::Matrix3xNXf C_mag; + evaluateCMag(&C_mag, x, earth_mag_); + + // calculate the Kalman gain matrix + simple_flight::MatrixNXxNXf P = error_covariance_; + VectorMath::Matrix3x3f inverse_term = (C_mag * P * C_mag.transpose() + R_mag_).inverse(); + simple_flight::MatrixNXx3f kalman_gain = P * C_mag.transpose() * inverse_term; + + // update states + float x_corrected[simple_flight::NX]; + for (int i = 0; i < simple_flight::NX; i++) { + float correction = kalman_gain(i, 0) * (mag[0] - h_mag[0]) + kalman_gain(i, 1) * (mag[1] - h_mag[1]) + kalman_gain(i, 2) * (mag[2] - h_mag[2]); + x_corrected[i] = x[i] + correction; + } + + // update covariance + simple_flight::MatrixNXxNXf P_corrected; + simple_flight::MatrixNXxNXf identity17x17 = simple_flight::MatrixNXxNXf::Identity(); + simple_flight::MatrixNXxNXf term = identity17x17 - kalman_gain * C_mag; + P_corrected = term * P * term.transpose() + kalman_gain * R_mag_ * kalman_gain.transpose(); + + // write the new states and covariance matrix to global variables + for (int i = 0; i < simple_flight::NX; i++) { + states_(i) = x_corrected[i]; + } + error_covariance_ = P_corrected; + } + + // barometer update + void barometerUpdate() + { + if (!board_->checkBarometerIfNew()) + return; + + real_T ned_altitude[1]; + + // check if the barometer gives new measurement and it is valid + bool is_valid = getBarometerData(ned_altitude); + + if (!is_valid) { + return; + } + + // extract the ekf states + float x[simple_flight::NX]; + for (int i = 0; i < simple_flight::NX; i++) { + x[i] = states_(i); + } + + // evaluate the C matrix + simple_flight::Matrix1xNXf C_baro; + evaluateCBaro(&C_baro); + + // calculate the Kalman gain matrix + simple_flight::MatrixNXxNXf P = error_covariance_; + float inverse_term = 1.0f / (C_baro * P * C_baro.transpose() + R_baro_); + simple_flight::MatrixNXx1f kalman_gain = P * C_baro.transpose() * inverse_term; + + // update states + float x_corrected[simple_flight::NX]; + for (int i = 0; i < simple_flight::NX; i++) { + x_corrected[i] = x[i] + kalman_gain[i] * (*ned_altitude + x[2]); + } + + // update covariances + simple_flight::MatrixNXxNXf P_corrected; + simple_flight::MatrixNXxNXf identity17x17 = simple_flight::MatrixNXxNXf::Identity(); + simple_flight::MatrixNXxNXf term = identity17x17 - kalman_gain * C_baro; + P_corrected = term * P * term.transpose() + kalman_gain * R_baro_ * kalman_gain.transpose(); + + // write the new states and cavariance matrix to the global variables + for (int i = 0; i < simple_flight::NX; i++) { + states_(i) = x_corrected[i]; + } + error_covariance_ = P_corrected; + } + + // GPS update + void gpsUpdate() + { + if (!board_->checkGpsIfNew()) + return; + + float pos[3]; + real_T vel[3]; + + // check if the GPS gives new measurement and it is valid + bool is_valid = getGpsData(pos, vel); + + if (!is_valid) { + return; + } + + // extract the ekf states + float x[simple_flight::NX]; + for (int i = 0; i < simple_flight::NX; i++) { + x[i] = states_(i); + } + + // evaluate the C matrix + simple_flight::Matrix6xNXf C_gps; + evaluateCGps(&C_gps); + + // calculate the Kalman gain matrix + simple_flight::MatrixNXxNXf P = error_covariance_; + VectorMath::Matrix6x6f inverse_term = (C_gps * P * C_gps.transpose() + R_gps_).inverse(); + simple_flight::MatrixNXx6f kalman_gain = P * C_gps.transpose() * inverse_term; + + // update the states + float x_corrected[simple_flight::NX]; + for (int i = 0; i < simple_flight::NX; i++) { + float correction = kalman_gain(i, 0) * (pos[0] - x[0]) + kalman_gain(i, 1) * (pos[1] - x[1]) + kalman_gain(i, 2) * (pos[2] - x[2]) + kalman_gain(i, 3) * (vel[0] - x[3]) + kalman_gain(i, 4) * (vel[1] - x[4]) + kalman_gain(i, 5) * (vel[2] - x[5]); + x_corrected[i] = x[i] + correction; + } + + // update the covariance matrix + simple_flight::MatrixNXxNXf P_corrected; + simple_flight::MatrixNXxNXf identity17x17 = simple_flight::MatrixNXxNXf::Identity(); + simple_flight::MatrixNXxNXf term = identity17x17 - kalman_gain * C_gps; + P_corrected = term * P * term.transpose() + kalman_gain * R_gps_ * kalman_gain.transpose(); + + // write the new states and covariance matrix to the global variables + for (int i = 0; i < simple_flight::NX; i++) { + states_(i) = x_corrected[i]; + } + error_covariance_ = P_corrected; + } + + void pseudoMeasurement() + { + // extract the states + float x[simple_flight::NX]; + for (int i = 0; i < simple_flight::NX; i++) { + x[i] = states_(i); + } + + // evaluate the current quaternion norm square + float norm_square; + norm_square = x[6] * x[6] + x[7] * x[7] + x[8] * x[8] + x[9] * x[9]; + + // evaluate the C matrix + simple_flight::Matrix1xNXf C_pseudo; + evaluateCPseudo(&C_pseudo, x); + + // calculate the Kalman gain matrix + simple_flight::MatrixNXxNXf P = error_covariance_; + float inverse_term = 1.0f / (C_pseudo * P * C_pseudo.transpose() + R_pseudo_); + simple_flight::MatrixNXx1f kalman_gain = P * C_pseudo.transpose() * inverse_term; + + // update the ekf states + float x_corrected[simple_flight::NX]; + for (int i = 0; i < simple_flight::NX; i++) { + x_corrected[i] = x[i] + kalman_gain[i] * (1.0f - norm_square); + } + + // covariance correction not done!!??? Is it correct?? + simple_flight::MatrixNXxNXf P_corrected; + simple_flight::MatrixNXxNXf identity17x17 = simple_flight::MatrixNXxNXf::Identity(); + simple_flight::MatrixNXxNXf term = identity17x17 - kalman_gain * C_pseudo; + P_corrected = term * P * term.transpose() + kalman_gain * R_pseudo_ * kalman_gain.transpose(); + + // write the states to the global variable + for (int i = 0; i < simple_flight::NX; i++) { + states_(i) = x_corrected[i]; + } + error_covariance_ = P_corrected; + } + + void eulerAnglesCovariancePropagation() + { + // extract the states + float x[simple_flight::NX]; + for (int i = 0; i < simple_flight::NX; i++) { + x[i] = states_(i); + } + + // evaluate the C matrix + VectorMath::Matrix3x4f C_euler; + evaluateCEulerAngles(&C_euler, x); + + // define the matrices + simple_flight::MatrixNXxNXf P = error_covariance_; + VectorMath::Matrix3x3f P_euler_angles; + VectorMath::Matrix4x4f P_quaternions; + + // map P onto P_quaternions + P_quaternions(0, 0) = P(0 + 6, 0 + 6); + P_quaternions(0, 1) = P(0 + 6, 1 + 6); + P_quaternions(0, 2) = P(0 + 6, 2 + 6); + P_quaternions(0, 3) = P(0 + 6, 3 + 6); + + P_quaternions(1, 0) = P(1 + 6, 0 + 6); + P_quaternions(1, 1) = P(1 + 6, 1 + 6); + P_quaternions(1, 2) = P(1 + 6, 2 + 6); + P_quaternions(1, 3) = P(1 + 6, 3 + 6); + + P_quaternions(2, 0) = P(2 + 6, 0 + 6); + P_quaternions(2, 1) = P(2 + 6, 1 + 6); + P_quaternions(2, 2) = P(2 + 6, 2 + 6); + P_quaternions(2, 3) = P(2 + 6, 3 + 6); + + P_quaternions(3, 0) = P(3 + 6, 0 + 6); + P_quaternions(3, 1) = P(3 + 6, 1 + 6); + P_quaternions(3, 2) = P(3 + 6, 2 + 6); + P_quaternions(3, 3) = P(3 + 6, 3 + 6); + + P_euler_angles = C_euler * P_quaternions * C_euler.transpose(); + + euler_angles_error_covariance_ = P_euler_angles; + } + + // --------------------------------------------------------------------- + // Measurement functions, reads measurement signal from board_ + // --------------------------------------------------------------------- + + // reads IMU data + bool getImuData(real_T accel[3], + real_T gyro[3]) + { + board_->readImuData(accel, gyro); + + // check if the signal has all data that is valid, else return false + // TODO: check if at least a subset of data is valid + + // record the measurement signals + measurement_(0) = accel[0]; + measurement_(1) = accel[1]; + measurement_(2) = accel[2]; + measurement_(3) = gyro[0]; + measurement_(4) = gyro[1]; + measurement_(5) = gyro[2]; + + return true; + } + + // reads GPS data + bool getGpsData(float pos[3], + real_T vel[3]) + { + double geo[3]; + board_->readGpsData(geo, vel); + + GeoPoint geo_point; + Vector3r ned_pos; + geo_point.longitude = geo[0]; + geo_point.latitude = geo[1]; + geo_point.altitude = static_cast(geo[2]); + geodetic_converter_.geodetic2Ned(geo_point, ned_pos); + + pos[0] = ned_pos[0]; + pos[1] = ned_pos[1]; + pos[2] = ned_pos[2]; + + // check if the signal has all data that is valid, else return false + // TODO: check if at least a subset of data is valid + + // record the measurement signals + measurement_(6) = pos[0]; + measurement_(7) = pos[1]; + measurement_(8) = pos[2]; + measurement_(9) = vel[0]; + measurement_(10) = vel[1]; + measurement_(11) = vel[2]; + + return true; + } + + // reads barometer data + bool getBarometerData(real_T* ned_altitude) + { + real_T altitude[1]; + board_->readBarometerData(altitude); + + // check if the signal has all data that is valid, else return false + // TODO: check if at least a subset of data is valid + + ned_altitude[0] = altitude[0] - home_altitude_; + + // record the measurement signals + measurement_(12) = ned_altitude[0]; + + return true; + } + + // reads magnetometer data + bool getMagnetometerData(real_T mag[3]) + { + board_->readMagnetometerData(mag); + + // check if the signal has all data that is valid, else return false + // TODO: check if at least a subset of data is valid + + // record the measurement signals + measurement_(13) = mag[0]; + measurement_(14) = mag[1]; + measurement_(15) = mag[2]; + + return true; + } + + private: + struct ImuDataBuffer + { + float accel[3]; + float gyro[3]; + float ang_acc[3]; + }; + struct LastTimes + { + TTimePoint state_propagation; + TTimePoint cov_propagation; + }; + + private: + // --------------------------------------------------------------------- + // Class attritubes + // --------------------------------------------------------------------- + FrequencyLimiter freq_limiter_; + const simple_flight::IBoard* board_; + + const Kinematics::State* kinematics_; + const Environment* environment_; + GeodeticConverter geodetic_converter_; + float earth_mag_[3]; + float home_altitude_; + + LastTimes last_times_; + ImuDataBuffer prev_imuData_; + + simple_flight::MatrixNWxNWf Q_; + VectorMath::Matrix6x6f R_gps_; + VectorMath::Matrix3x3f R_mag_; + real_T R_baro_; + real_T R_pseudo_; + }; + +} +} //namespace +#endif \ No newline at end of file diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfBase.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfBase.hpp new file mode 100644 index 0000000000..25d4f6f547 --- /dev/null +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfBase.hpp @@ -0,0 +1,61 @@ +// Liscence info + +#ifndef msr_airlib_AirSimSimpleEkfBase_hpp +#define msr_airlib_AirSimSimpleEkfBase_hpp + +#include +#include "common/CommonStructs.hpp" +#include "common/Common.hpp" +#include "firmware/interfaces/IEkf.hpp" +#include "AirSimSimpleEkfParams.hpp" + +namespace msr +{ +namespace airlib +{ + + class AirSimSimpleEkfBase : public simple_flight::IEkf + { + public: + virtual bool checkEkfEnabled() const override + { + return params_.ekf_enabled; + } + + // getters + virtual const simple_flight::VectorNXf& getEkfStates() const override + { + return states_; + } + virtual const VectorMath::Vector17f& getEkfMeasurements() const override + { + return measurement_; + } + virtual const simple_flight::MatrixNXxNXf& getEkfCovariance() const override + { + return error_covariance_; + } + virtual const VectorMath::Matrix3x3f& getEkfEulerAnglesCovariance() const override + { + return euler_angles_error_covariance_; + } + + protected: + // setters + void setEkfCovariance(simple_flight::MatrixNXxNXf error_covariance) + { + error_covariance_ = error_covariance; + } + + protected: + AirSimSimpleEkfParams params_; + + simple_flight::VectorNXf states_; + simple_flight::MatrixNXxNXf error_covariance_; + VectorMath::Matrix3x3f euler_angles_error_covariance_; + VectorMath::Vector17f measurement_ = VectorMath::Vector17f::Zero(); + }; + +} +} //namespace +#endif \ No newline at end of file diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfModel.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfModel.hpp new file mode 100644 index 0000000000..7fc5a779af --- /dev/null +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfModel.hpp @@ -0,0 +1,695 @@ +// Liscence info + +#ifndef msr_airlib_AirSimSimpleEkfModel_hpp +#define msr_airlib_AirSimSimpleEkfModel_hpp + +#include +#include +#include "firmware/interfaces/IUpdatable.hpp" +#include "firmware/interfaces/IBoard.hpp" + +constexpr float G_0 = EarthUtils::Gravity; // for less than 10000m const gravity is taken +constexpr float R_E = EARTH_RADIUS; + +namespace msr +{ +namespace airlib +{ + + class AirSimSimpleEkfModel + { + public: + // --------------------------------------------------------------------- + // Mathematical functions + // --------------------------------------------------------------------- + + static void insPositionKinematics(float position_dot[3], float lin_velocity[3]) + { + float u = lin_velocity[0]; + float v = lin_velocity[1]; + float w = lin_velocity[2]; + + // position_dot + position_dot[0] = u; // x_dot + position_dot[1] = v; // y_dot + position_dot[2] = w; // z_dot + } + + static void insVelocityKinematics(float lin_velocity_dot[3], float quaternions[4], float specific_forces[3], float accel_biases[3]) + { + float q0 = quaternions[0]; + float q1 = quaternions[1]; + float q2 = quaternions[2]; + float q3 = quaternions[3]; + + float f_x = specific_forces[0]; + float f_y = specific_forces[1]; + float f_z = specific_forces[2]; + + float b_f_x = accel_biases[0]; + float b_f_y = accel_biases[1]; + float b_f_z = accel_biases[2]; + + // velocity_dot. Transform specific forces from B frame to O frame and evaluate velocity_dot + lin_velocity_dot[0] = (q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * (f_x - b_f_x) + 2.0f * (q1 * q2 - q0 * q3) * (f_y - b_f_y) + 2.0f * (q0 * q2 + q1 * q3) * (f_z - b_f_z); + lin_velocity_dot[1] = 2.0f * (q1 * q2 + q0 * q3) * (f_x - b_f_x) + (q0 * q0 - q1 * q1 + q2 * q2 - q3 * q3) * (f_y - b_f_y) + 2.0f * (q2 * q3 - q0 * q1) * (f_z - b_f_z); + lin_velocity_dot[2] = 2.0f * (q1 * q3 - q0 * q2) * (f_x - b_f_x) + 2.0f * (q2 * q3 + q0 * q1) * (f_y - b_f_y) + (q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) * (f_z - b_f_z) + //+ G_0*(1 + 2.0f*x[2]/R_E); + + G_0; //+0.001f; + } + + static void insAttitudeKinematics(float attitude_dot[4], float quaternions[4], float gyro_rates[3], float gyro_biases[3]) + { + float q0 = quaternions[0]; + float q1 = quaternions[1]; + float q2 = quaternions[2]; + float q3 = quaternions[3]; + + float omega_x = gyro_rates[0]; + float omega_y = gyro_rates[1]; + float omega_z = gyro_rates[2]; + + float b_omega_x = gyro_biases[0]; + float b_omega_y = gyro_biases[1]; + float b_omega_z = gyro_biases[2]; + + float lambda = 1.0f - (q0 * q0 + q1 * q1 + q3 * q2 + q3 * q3); + float k = 300.0f; // k*h <= 1, h = 0.003 + + // attitude_dot. Transform specific forces from B frame to O frame and evaluate velocity_dot + attitude_dot[0] = 0.5f * (-q1 * (omega_x - b_omega_x) - q2 * (omega_y - b_omega_y) - q3 * (omega_z - b_omega_z) + q0 * 2 * k * lambda); + attitude_dot[1] = 0.5f * (q0 * (omega_x - b_omega_x) - q3 * (omega_y - b_omega_y) + q2 * (omega_z - b_omega_z) + q1 * 2 * k * lambda); + attitude_dot[2] = 0.5f * (q3 * (omega_x - b_omega_x) + q0 * (omega_y - b_omega_y) - q1 * (omega_z - b_omega_z) + q2 * 2 * k * lambda); + attitude_dot[3] = 0.5f * (-q2 * (omega_x - b_omega_x) + q1 * (omega_y - b_omega_y) + q0 * (omega_z - b_omega_z) + q3 * 2 * k * lambda); + } + + static void evaluateStateDot(float x_dot[simple_flight::NX], float x[simple_flight::NX], float u[simple_flight::NU]) + { + float lin_velocity[3]; + lin_velocity[0] = x[3]; + lin_velocity[1] = x[4]; + lin_velocity[2] = x[5]; + + // position kinematics + float position_dot[3]; + insPositionKinematics(position_dot, lin_velocity); + + float quaternions[4]; + quaternions[0] = x[6]; + quaternions[1] = x[7]; + quaternions[2] = x[8]; + quaternions[3] = x[9]; + + float specific_forces[3]; + specific_forces[0] = u[0]; + specific_forces[1] = u[1]; + specific_forces[2] = u[2]; + + float accel_biases[3]; + accel_biases[0] = x[10]; + accel_biases[1] = x[11]; + accel_biases[2] = x[12]; + + // linear velocity kinematics + float lin_velocity_dot[3]; + insVelocityKinematics(lin_velocity_dot, quaternions, specific_forces, accel_biases); + + float gyro_rates[3]; + gyro_rates[0] = u[3]; + gyro_rates[1] = u[4]; + gyro_rates[2] = u[5]; + + float gyro_biases[3]; + gyro_biases[0] = x[13]; + gyro_biases[1] = x[14]; + gyro_biases[2] = x[15]; + + // attitude kinematics + float quaternions_dot[4]; + insAttitudeKinematics(quaternions_dot, quaternions, gyro_rates, gyro_biases); + + x_dot[0] = position_dot[0]; + x_dot[1] = position_dot[1]; + x_dot[2] = position_dot[2]; + x_dot[3] = lin_velocity_dot[0]; + x_dot[4] = lin_velocity_dot[1]; + x_dot[5] = lin_velocity_dot[2]; + x_dot[6] = quaternions_dot[0]; + x_dot[7] = quaternions_dot[1]; + x_dot[8] = quaternions_dot[2]; + x_dot[9] = quaternions_dot[3]; + x_dot[10] = 0.0f; + x_dot[11] = 0.0f; + x_dot[12] = 0.0f; + x_dot[13] = 0.0f; + x_dot[14] = 0.0f; + x_dot[15] = 0.0f; + x_dot[16] = 0.0f; + } + + void h_mag() + { + // + } + + void h_baro() + { + // + } + + void h_GPS() + { + // + } + + // --------------------------------------------------------------------- + // Mathematical jacobians + // --------------------------------------------------------------------- + + static void evaluateA(simple_flight::MatrixNXxNXf* A, float x[simple_flight::NX], float u[simple_flight::NU]) + { + *A = simple_flight::MatrixNXxNXf::Zero(); + + float q0 = x[6]; // quaternions + float q1 = x[7]; // quaternions + float q2 = x[8]; // quaternions + float q3 = x[9]; // quaternions + float b_f_x = x[10]; // specific forces + float b_f_y = x[11]; // specific forces + float b_f_z = x[12]; // specific forces + float b_omega_x = x[13]; // gyro_biases + float b_omega_y = x[14]; // gyro_biases + float b_omega_z = x[15]; // gyro_biases + + float f_x = u[0]; // specific forces + float f_y = u[1]; // specific forces + float f_z = u[2]; // specific forces + float omega_x = u[3]; // gyro rates + float omega_y = u[4]; // gyro rates + float omega_z = u[5]; // gyro rates + + float k = 300.0f; + + // df_pos_dx [0 1 2][:] + (*A)(0, 3) = 1.0f; + (*A)(1, 4) = 1.0f; + (*A)(2, 5) = 1.0f; + // df_vel_dx [3 4 5][:] + //(*A)(5, 2) = 2.0f*G_0/R_E; + // df_vel_q0 [3 4 5][6] + (*A)(3, 6) = 2.0f * q0 * (f_x - b_f_x) - 2.0f * q3 * (f_y - b_f_y) + 2.0f * q2 * (f_z - b_f_z); + (*A)(4, 6) = 2.0f * q3 * (f_x - b_f_x) + 2.0f * q0 * (f_y - b_f_y) - 2.0f * q1 * (f_z - b_f_z); + (*A)(5, 6) = -2.0f * q2 * (f_x - b_f_x) + 2.0f * q1 * (f_y - b_f_y) + 2.0f * q0 * (f_z - b_f_z); + // df_vel_q1 [3 4 5][7] + (*A)(3, 7) = 2.0f * q1 * (f_x - b_f_x) + 2.0f * q2 * (f_y - b_f_y) + 2.0f * q3 * (f_z - b_f_z); + (*A)(4, 7) = 2.0f * q2 * (f_x - b_f_x) - 2.0f * q1 * (f_y - b_f_y) - 2.0f * q0 * (f_z - b_f_z); + (*A)(5, 7) = 2.0f * q3 * (f_x - b_f_x) + 2.0f * q0 * (f_y - b_f_y) - 2.0f * q1 * (f_z - b_f_z); + // df_vel_q2 [3 4 5][8] + (*A)(3, 8) = -2.0f * q2 * (f_x - b_f_x) + 2.0f * q1 * (f_y - b_f_y) + 2.0f * q0 * (f_z - b_f_z); + (*A)(4, 8) = 2.0f * q1 * (f_x - b_f_x) + 2.0f * q2 * (f_y - b_f_y) + 2.0f * q3 * (f_z - b_f_z); + (*A)(5, 8) = -2.0f * q0 * (f_x - b_f_x) + 2.0f * q3 * (f_y - b_f_y) - 2.0f * q2 * (f_z - b_f_z); + // df_vel_q3 [3 4 5][9] + (*A)(3, 9) = -2.0f * q3 * (f_x - b_f_x) - 2.0f * q0 * (f_y - b_f_y) + 2.0f * q1 * (f_z - b_f_z); + (*A)(4, 9) = 2.0f * q0 * (f_x - b_f_x) - 2.0f * q3 * (f_y - b_f_y) + 2.0f * q2 * (f_z - b_f_z); + (*A)(5, 9) = 2.0f * q1 * (f_x - b_f_x) + 2.0f * q2 * (f_y - b_f_y) + 2.0f * q3 * (f_z - b_f_z); + // df_vel_xbias [3 4 5][10 11 12 ...] + (*A)(3, 10) = -(q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3); + (*A)(3, 11) = -2.0f * (q1 * q2 - q0 * q3); + (*A)(3, 12) = -2.0f * (q0 * q2 + q1 * q3); + (*A)(4, 10) = -2.0f * (q1 * q2 + q0 * q3); + (*A)(4, 11) = -(q0 * q0 - q1 * q1 + q2 * q2 - q3 * q3); + (*A)(4, 12) = -2.0f * (q2 * q3 - q0 * q1); + (*A)(5, 10) = -2.0f * (q1 * q3 - q0 * q2); + (*A)(5, 11) = -2.0f * (q2 * q3 + q0 * q1); + (*A)(5, 12) = -(q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3); + // df_ori_[q0 q1 q2 q3] [6 7 8 9][6 7 8 9] + (*A)(6, 6) = 0.5f * (2 * k - 6 * k * q0 * q0 - 2 * k * q1 * q1 - 2 * k * q2 * q2 - 2 * k * q3 * q3); + (*A)(6, 7) = 0.5f * (-(omega_x - b_omega_x) - 4 * k * q0 * q1); + (*A)(6, 8) = 0.5f * (-(omega_y - b_omega_y) - 4 * k * q0 * q2); + (*A)(6, 9) = 0.5f * (-(omega_z - b_omega_z) - 4 * k * q0 * q3); + (*A)(7, 6) = 0.5f * (omega_x - b_omega_x - 4 * k * q1 * q0); + (*A)(7, 7) = 0.5f * (2 * k - 2 * k * q0 * q0 - 6 * k * q1 * q1 - 2 * k * q2 * q2 - 2 * k * q3 * q3); + (*A)(7, 8) = 0.5f * (omega_z - b_omega_z - 4 * k * q1 * q2); + (*A)(7, 9) = 0.5f * (-(omega_y - b_omega_y) - 4 * k * q1 * q3); + (*A)(8, 6) = 0.5f * (omega_y - b_omega_y - 4 * k * q2 * q0); + (*A)(8, 7) = 0.5f * (-(omega_z - b_omega_z) - 4 * k * q2 * q1); + (*A)(8, 8) = 0.5f * (2 * k - 2 * k * q0 * q0 - 2 * k * q1 * q1 - 6 * k * q2 * q2 - 2 * k * q3 * q3); + (*A)(8, 9) = 0.5f * (omega_x - b_omega_x - 4 * k * q2 * q3); + (*A)(9, 6) = 0.5f * (omega_z - b_omega_z - 4 * k * q3 * q0); + (*A)(9, 7) = 0.5f * (omega_y - b_omega_y - 4 * k * q3 * q1); + (*A)(9, 8) = 0.5f * (-(omega_x - b_omega_x) - 4 * k * q3 * q2); + (*A)(9, 9) = 0.5f * (2 * k - 2 * k * q0 * q0 - 2 * k * q1 * q1 - 2 * k * q2 * q2 - 6 * k * q3 * q3); + // df_ori_x_bias [6 7 8 9][10 11 12 13 14 15 16 17] + (*A)(6, 13) = 0.5f * q1; + (*A)(6, 14) = 0.5f * q2; + (*A)(6, 15) = 0.5f * q3; + (*A)(7, 13) = -0.5f * q0; + (*A)(7, 14) = 0.5f * q3; + (*A)(7, 15) = -0.5f * q2; + (*A)(8, 13) = -0.5f * q3; + (*A)(8, 14) = -0.5f * q0; + (*A)(8, 15) = 0.5f * q1; + (*A)(9, 13) = 0.5f * q2; + (*A)(9, 14) = -0.5f * q1; + (*A)(9, 15) = -0.5f * q0; + } + + static void evaluateFiniteDifferenceA(simple_flight::MatrixNXxNXf* A, float x[simple_flight::NX], float u[simple_flight::NU]) + { + *A = simple_flight::MatrixNXxNXf::Zero(); + + float derivative_perturbation = 1E-5f; + float x_plus[simple_flight::NX]; + float x_minus[simple_flight::NX]; + float f_plus[simple_flight::NX]; + float f_minus[simple_flight::NX]; + float df_dx_i_column[simple_flight::NX]; + + for (int i = 0; i < simple_flight::NX; i++) { + for (int j = 0; j < simple_flight::NX; j++) { + x_plus[j] = x[j]; + x_minus[j] = x[j]; + } + auto perturbation_i = std::max(1.0f, std::abs(x[i])) * derivative_perturbation; + x_plus[i] = x[i] + perturbation_i; + x_minus[i] = x[i] - perturbation_i; + + evaluateStateDot(f_plus, x_plus, u); + evaluateStateDot(f_minus, x_minus, u); + + for (int j = 0; j < simple_flight::NX; j++) { + df_dx_i_column[j] = (f_plus[j] - f_minus[j]) / (2 * perturbation_i); + } + + for (int j = 0; j < simple_flight::NX; j++) { + (*A)(j, i) = df_dx_i_column[j]; + } + } + } + + static bool checkA(VectorMath::Matrix17x17f* A_error, VectorMath::Matrix17x17f* A, VectorMath::Matrix17x17f* A_finite, float row[17], float column[17]) + { + *A_error = VectorMath::Matrix17x17f::Zero(); + + float derivative_test_tolerance = 0.01f; + + for (int i = 0; i < 17; i++) { + for (int j = 0; j < 17; j++) { + float a = (*A)(i, j); + float a_finite = (*A_finite)(i, j); + (*A_error)(i, j) = std::abs(a - a_finite) / std::max(std::abs(a_finite), derivative_test_tolerance); + } + } + int k = 0; + for (int i = 0; i < 17; i++) { + row[i] = -1; + } + for (int i = 0; i < 17; i++) { + column[i] = -1; + } + for (int i = 0; i < 17; i++) { + for (int j = 0; j < 17; j++) { + if ((*A_error)(i, j) > derivative_test_tolerance) { + row[k] = static_cast(i); + column[k] = static_cast(j); + k++; + } + } + } + for (int i = 0; i < 17; i++) { + for (int j = 0; j < 17; j++) { + if ((*A_error)(i, j) > derivative_test_tolerance) { + return false; + } + } + } + return true; + } + + static void evaluateB_w(simple_flight::MatrixNXxNWf* B_w, float x[simple_flight::NX], float u[simple_flight::NU]) + { + // set all elements to zero + *B_w = simple_flight::MatrixNXxNWf::Zero(); + + float q0 = x[6]; // quaternions + float q1 = x[7]; // quaternions + float q2 = x[8]; // quaternions + float q3 = x[9]; // quaternions + + // df_vel_w [3 4 5][0 1 2 ...] + (*B_w)(3, 0) = -(q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3); + (*B_w)(3, 1) = -2.0f * (q1 * q2 - q0 * q3); + (*B_w)(3, 2) = -2.0f * (q0 * q2 + q1 * q3); + (*B_w)(4, 0) = -2.0f * (q1 * q2 + q0 * q3); + (*B_w)(4, 1) = -(q0 * q0 - q1 * q1 + q2 * q2 - q3 * q3); + (*B_w)(4, 2) = -2.0f * (q2 * q3 - q0 * q1); + (*B_w)(5, 0) = -2.0f * (q1 * q3 - q0 * q2); + (*B_w)(5, 1) = -2.0f * (q2 * q3 + q0 * q1); + (*B_w)(5, 2) = -(q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3); + // df_ori_x_bias [6 7 8 9][10 11 12 13 14 15 16 17] + (*B_w)(6, 3) = 0.5f * q1; + (*B_w)(6, 4) = 0.5f * q2; + (*B_w)(6, 5) = 0.5f * q3; + (*B_w)(7, 3) = -0.5f * q0; + (*B_w)(7, 4) = 0.5f * q3; + (*B_w)(7, 5) = -0.5f * q2; + (*B_w)(8, 3) = -0.5f * q3; + (*B_w)(8, 4) = -0.5f * q0; + (*B_w)(8, 5) = 0.5f * q1; + (*B_w)(9, 3) = 0.5f * q2; + (*B_w)(9, 4) = -0.5f * q1; + (*B_w)(9, 5) = -0.5f * q0; + // df_bias_w + (*B_w)(10, 6) = 1.0f; + (*B_w)(11, 7) = 1.0f; + (*B_w)(12, 8) = 1.0f; + (*B_w)(13, 9) = 1.0f; + (*B_w)(14, 10) = 1.0f; + (*B_w)(15, 11) = 1.0f; + (*B_w)(16, 12) = 1.0f; + } + + static void dh_mag_dx() + { + // jacobian wrt the states vector + } + + static void evaluateCBaro(simple_flight::Matrix1xNXf* C_baro) + { + // dh_baro_dx jacobian wrt the states vector + *C_baro = simple_flight::Matrix1xNXf::Zero(); + (*C_baro)(2) = -1.0f; + (*C_baro)(16) = -1.0f; + } + + static void evaluateCGps(simple_flight::Matrix6xNXf* C_gps) + { + // dh_gps_dx jacobian wrt the states vector + *C_gps = simple_flight::Matrix6xNXf::Zero(); + (*C_gps)(0, 0) = 1.0f; + (*C_gps)(1, 1) = 1.0f; + (*C_gps)(2, 2) = 1.0f; + (*C_gps)(3, 3) = 1.0f; + (*C_gps)(4, 4) = 1.0f; + (*C_gps)(5, 5) = 1.0f; + } + + static void evaluatehMag(float h_mag[3], float x[17], float earth_mag[3]) + { + float q0 = x[6]; + float q1 = x[7]; + float q2 = x[8]; + float q3 = x[9]; + + float tau_x = earth_mag[0]; + float tau_y = earth_mag[1]; + float tau_z = earth_mag[2]; + + h_mag[0] = (q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * tau_x + (2.0f * (q1 * q2 + q0 * q3)) * tau_y + (2.0f * (q1 * q3 - q0 * q2)) * tau_z; + h_mag[1] = (2.0f * (q1 * q2 - q0 * q3)) * tau_x + (q0 * q0 - q1 * q1 + q2 * q2 - q3 * q3) * tau_y + (2.0f * (q2 * q3 + q0 * q1)) * tau_z; + h_mag[2] = (2.0f * (q0 * q2 + q1 * q3)) * tau_x + (2.0f * (q2 * q3 - q0 * q1)) * tau_y + (q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) * tau_z; + } + + static void evaluateCMag(simple_flight::Matrix3xNXf* C_mag, float x[17], float earth_mag[3]) + { + // dh_mag_dx jacobian wrt the states vector + *C_mag = simple_flight::Matrix3xNXf::Zero(); + + float q0 = x[6]; + float q1 = x[7]; + float q2 = x[8]; + float q3 = x[9]; + + float tau_x = earth_mag[0]; + float tau_y = earth_mag[1]; + float tau_z = earth_mag[2]; + + // dh_mag_q0 + (*C_mag)(0, 6) = 2.0f * q0 * tau_x + 2.0f * q3 * tau_y - 2.0f * q2 * tau_z; + (*C_mag)(1, 6) = -2.0f * q3 * tau_x + 2.0f * q0 * tau_y + 2.0f * q1 * tau_z; + (*C_mag)(2, 6) = 2.0f * q2 * tau_x - 2.0f * q1 * tau_y + 2.0f * q0 * tau_z; + // dh_mag_q1 + (*C_mag)(0, 7) = 2.0f * q1 * tau_x + 2.0f * q2 * tau_y + 2.0f * q3 * tau_z; + (*C_mag)(1, 7) = 2.0f * q2 * tau_x - 2.0f * q1 * tau_y + 2.0f * q0 * tau_z; + (*C_mag)(2, 7) = 2.0f * q3 * tau_x - 2.0f * q0 * tau_y - 2.0f * q1 * tau_z; + // dh_mag_q2 + (*C_mag)(0, 8) = -2.0f * q2 * tau_x + 2.0f * q1 * tau_y - 2.0f * q0 * tau_z; + (*C_mag)(1, 8) = 2.0f * q1 * tau_x + 2.0f * q2 * tau_y + 2.0f * q3 * tau_z; + (*C_mag)(2, 8) = 2.0f * q0 * tau_x + 2.0f * q3 * tau_y - 2.0f * q2 * tau_z; + // dh_mag_q3 + (*C_mag)(0, 9) = -2.0f * q3 * tau_x + 2.0f * q0 * tau_y + 2.0f * q1 * tau_z; + (*C_mag)(1, 9) = -2.0f * q0 * tau_x - 2.0f * q3 * tau_y + 2.0f * q2 * tau_z; + (*C_mag)(2, 9) = 2.0f * q1 * tau_x + 2.0f * q2 * tau_y + 2.0f * q3 * tau_z; + } + + static void evaluateCPseudo(simple_flight::Matrix1xNXf* C_pseudo, float x[17]) + { + // dh_gps_dx jacobian wrt the states vector + *C_pseudo = simple_flight::Matrix1xNXf::Zero(); + + float q0 = x[6]; + float q1 = x[7]; + float q2 = x[8]; + float q3 = x[9]; + + // dh_mag_dq + (*C_pseudo)(6) = 2.0f * q0; + (*C_pseudo)(7) = 2.0f * q1; + (*C_pseudo)(8) = 2.0f * q2; + (*C_pseudo)(9) = 2.0f * q3; + } + + static void evaluateCEulerAngles(VectorMath::Matrix3x4f* C_euler, float x[17]) + { + // dattitude_dq jacobian of euler angles wrt the quaternions + *C_euler = VectorMath::Matrix3x4f::Zero(); + + float q0 = x[6]; + float q1 = x[7]; + float q2 = x[8]; + float q3 = x[9]; + + // d_phi_dq + float term_1 = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3; + float term_2 = q2 * q3 + q0 * q1; + float denominator = term_1 * term_1 + 4 * term_2 * term_2; + float dphi_q0 = (2 * q1 * term_1 - 4 * q0 * term_2) / denominator; + float dphi_q1 = (2 * q0 * term_1 + 4 * q1 * term_2) / denominator; + float dphi_q2 = (2 * q3 * term_1 + 4 * q2 * term_2) / denominator; + float dphi_q3 = (2 * q2 * term_1 - 4 * q3 * term_2) / denominator; + + // d_theta_dq + term_1 = q1 * q3 - q0 * q2; + denominator = sqrt(1 - 4 * term_1 * term_1); + float dtheta_q0 = 2 * q2 / denominator; + float dtheta_q1 = -2 * q3 / denominator; + float dtheta_q2 = 2 * q0 / denominator; + float dtheta_q3 = -2 * q1 / denominator; + + // d_psi_dq + term_1 = q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3; + term_2 = q1 * q2 + q0 * q3; + denominator = term_1 * term_1 + 4 * term_2 * term_2; + float dpsi_q0 = (2 * q3 * term_1 - 4 * q0 * term_2) / denominator; + float dpsi_q1 = (2 * q2 * term_1 - 4 * q1 * term_2) / denominator; + float dpsi_q2 = (2 * q1 * term_1 + 4 * q2 * term_2) / denominator; + float dpsi_q3 = (2 * q0 * term_1 + 4 * q3 * term_2) / denominator; + + (*C_euler)(0, 0) = dphi_q0; + (*C_euler)(0, 1) = dphi_q1; + (*C_euler)(0, 2) = dphi_q2; + (*C_euler)(0, 3) = dphi_q3; + (*C_euler)(1, 0) = dtheta_q0; + (*C_euler)(1, 1) = dtheta_q1; + (*C_euler)(1, 2) = dtheta_q2; + (*C_euler)(1, 3) = dtheta_q3; + (*C_euler)(2, 0) = dpsi_q0; + (*C_euler)(2, 1) = dpsi_q1; + (*C_euler)(2, 2) = dpsi_q2; + (*C_euler)(2, 3) = dpsi_q3; + } + + static void rungeKutta(float xp[17], float x[17], float u[6], float dt, int size = 17) + { + float k1[17] = { 0 }; + float k2[17] = { 0 }; + float k3[17] = { 0 }; + float k4[17] = { 0 }; + float x_dot[17] = { 0 }; + float x_temp[17] = { 0 }; + + evaluateStateDot(x_dot, x, u); + for (int n = 0; n < size; n++) { + k1[n] = x_dot[n] * dt; + x_temp[n] = x[n] + k1[n] / 2; + } + evaluateStateDot(x_dot, x_temp, u); + for (int n = 0; n < size; n++) { + k2[n] = x_dot[n] * dt; + x_temp[n] = x[n] + k2[n] / 2; + } + evaluateStateDot(x_dot, x_temp, u); + for (int n = 0; n < size; n++) { + k3[n] = x_dot[n] * dt; + x_temp[n] = x[n] + k3[n]; + } + evaluateStateDot(x_dot, x_temp, u); + for (int n = 0; n < size; n++) { + k4[n] = x_dot[n] * dt; + xp[n] = x[n] + k1[n] / 6 + k2[n] / 3 + k3[n] / 4 + k4[n] / 6; + } + } + + static void heun(float xp[17], float x[17], float u[6], float up[6], float dt, int size = 17) + { + float k1[17] = { 0 }; + float k2[17] = { 0 }; + float x_dot[17] = { 0 }; + float x_temp[17] = { 0 }; + + evaluateStateDot(x_dot, x, u); + for (int n = 0; n < size; n++) { + k1[n] = x_dot[n] * dt; + x_temp[n] = x[n] + k1[n]; + } + evaluateStateDot(x_dot, x_temp, up); + for (int n = 0; n < size; n++) { + k2[n] = x_dot[n] * dt; + xp[n] = x[n] + k1[n] / 2 + k2[n] / 2; + } + } + + static void inertialNavigation(float x_predicted[simple_flight::NX], + float x[simple_flight::NX], + float u[simple_flight::NU], + float up[simple_flight::NU], + float ang_acc[3], + float dt_real) + { + float curr_quaternions[4]; + curr_quaternions[0] = x[6]; + curr_quaternions[1] = x[7]; + curr_quaternions[2] = x[8]; + curr_quaternions[3] = x[9]; + + float curr_gyro_biases[3]; + curr_gyro_biases[0] = x[13]; + curr_gyro_biases[1] = x[14]; + curr_gyro_biases[2] = x[15]; + + float curr_gyro_rates[3]; + // curr_gyro_rates[0] = u[3]; + // curr_gyro_rates[1] = u[4]; + // curr_gyro_rates[2] = u[5]; + curr_gyro_rates[0] = u[3] + 0.5f * dt_real * ang_acc[0]; + curr_gyro_rates[1] = u[4] + 0.5f * dt_real * ang_acc[1]; + curr_gyro_rates[2] = u[5] + 0.5f * dt_real * ang_acc[2]; + + // attitude propagation + float curr_attitude_dot[4]; + insAttitudeKinematics(curr_attitude_dot, curr_quaternions, curr_gyro_rates, curr_gyro_biases); + float next_quaternions[4]; + float next_quaternions_minus[4]; + for (int i = 0; i < 4; i++) { + next_quaternions_minus[i] = curr_quaternions[i] + dt_real * (curr_attitude_dot[i]); + } + float next_gyro_rates[3]; + next_gyro_rates[0] = up[3]; + next_gyro_rates[1] = up[4]; + next_gyro_rates[2] = up[5]; + float next_attitude_dot[4]; + // insAttitudeKinematics(curr_attitude_dot, curr_quaternions, next_gyro_rates, curr_gyro_biases); + insAttitudeKinematics(next_attitude_dot, next_quaternions_minus, next_gyro_rates, curr_gyro_biases); + // // for (int i=0; i<4; i++){ + // // next_quaternions[i] = curr_quaternions[i] + dt_real*(attitude_dot[i] + attitude_dot_next[i])/2; + // // } + for (int i = 0; i < 4; i++) { + // next_quaternions[i] = curr_quaternions[i] + dt_real*0.5f*(curr_attitude_dot[i] + next_attitude_dot[i]); + next_quaternions[i] = curr_quaternions[i] + dt_real * (curr_attitude_dot[i]); + // next_quaternions[i] = curr_quaternions[i] + dt_real*curr_attitude_dot[i] + // + dt_real*0.5f*(next_attitude_dot[i] - curr_attitude_dot[i]); + } + + float curr_lin_velocity[3]; + curr_lin_velocity[0] = x[3]; + curr_lin_velocity[1] = x[4]; + curr_lin_velocity[2] = x[5]; + + float curr_accel_biases[3]; + curr_accel_biases[0] = x[10]; + curr_accel_biases[1] = x[11]; + curr_accel_biases[2] = x[12]; + + float curr_specific_forces[3]; + curr_specific_forces[0] = u[0]; + curr_specific_forces[1] = u[1]; + curr_specific_forces[2] = u[2]; + + // velocity propagation + float curr_lin_velocity_dot[3]; + insVelocityKinematics(curr_lin_velocity_dot, curr_quaternions, curr_specific_forces, curr_accel_biases); + float next_lin_velocity_minus[3]; + float next_lin_velocity[3]; + for (int i = 0; i < 3; i++) { + next_lin_velocity_minus[i] = curr_lin_velocity[i] + dt_real * (curr_lin_velocity_dot[i]); + } + float next_specific_forces[3]; + next_specific_forces[0] = up[0]; + next_specific_forces[1] = up[1]; + next_specific_forces[2] = up[2]; + float next_lin_velocity_dot[3]; + insVelocityKinematics(next_lin_velocity_dot, next_quaternions, next_specific_forces, curr_accel_biases); + for (int i = 0; i < 3; i++) { + next_lin_velocity[i] = curr_lin_velocity[i] + dt_real * 0.5f * (curr_lin_velocity_dot[i] + next_lin_velocity_dot[i]); + } + + float curr_position[3]; + curr_position[0] = x[0]; + curr_position[1] = x[1]; + curr_position[2] = x[2]; + + // position propagation + float next_position[3]; + // float position_dot[3]; + // float position_dot_next[3]; + // insPositionKinematics(position_dot, next_lin_velocity_minus); + // float next_position_minus[3]; + // for (int i=0; i<3; i++){ + // next_position_minus[i] = curr_position[i] + dt_real*(position_dot[i]); + // } + // insPositionKinematics(position_dot_next, next_lin_velocity); + // for (int i=0; i<3; i++){ + // next_position[i] = curr_position[i] + dt_real*(position_dot[i] + position_dot_next[i])/2; + // } + for (int i = 0; i < 3; i++) { + // next_position[i] = curr_position[i] + dt_real*0.5f*(0.5f*(curr_lin_velocity[i] + next_lin_velocity[i]) + // + 0.25f*dt_real*(curr_lin_velocity_dot[i] + next_lin_velocity_dot[i])); + next_position[i] = curr_position[i] + dt_real * curr_lin_velocity[i] + 0.5f * dt_real * dt_real * curr_lin_velocity_dot[i]; + } + + float curr_baro_bias = x[16]; + + x_predicted[0] = next_position[0]; + x_predicted[1] = next_position[1]; + x_predicted[2] = next_position[2]; + x_predicted[3] = next_lin_velocity[0]; + x_predicted[4] = next_lin_velocity[1]; + x_predicted[5] = next_lin_velocity[2]; + x_predicted[6] = next_quaternions[0]; + x_predicted[7] = next_quaternions[1]; + x_predicted[8] = next_quaternions[2]; + x_predicted[9] = next_quaternions[3]; + + x_predicted[10] = curr_accel_biases[0]; + x_predicted[11] = curr_accel_biases[1]; + x_predicted[12] = curr_accel_biases[2]; + x_predicted[13] = curr_gyro_biases[0]; + x_predicted[14] = curr_gyro_biases[1]; + x_predicted[15] = curr_gyro_biases[2]; + x_predicted[16] = curr_baro_bias; + } + }; + +} +} //namespace +#endif \ No newline at end of file diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfParams.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfParams.hpp new file mode 100644 index 0000000000..bfde1d55f3 --- /dev/null +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfParams.hpp @@ -0,0 +1,277 @@ + +#ifndef msr_airlib_AirSimSimpleEkfParams_hpp +#define msr_airlib_AirSimSimpleEkfParams_hpp + +#include "common/Common.hpp" +#include "common/AirSimSettings.hpp" +#include + +namespace msr +{ +namespace airlib +{ + + struct AirSimSimpleEkfParams + { + bool ekf_enabled = false; + bool fuse_gps = false; + bool fuse_baro = false; + bool fuse_mag = false; + + struct Gyroscope + { + Vector3r std_error = Vector3r(0.2f, 0.2f, 0.2f); + } gyro; + + struct Accelerometer + { + Vector3r std_error = Vector3r(0.1f, 0.1f, 0.1f); + } accel; + + struct Gps + { + Vector3r std_error_position = Vector3r(5.0f, 5.0f, 10.0f); + Vector3r std_error_velocity = Vector3r(2.0f, 2.0f, 5.0f); + } gps; + + struct Barometer + { + real_T std_error = 1.0f; + } baro; + + struct Magnetometer + { + Vector3r std_error = Vector3r(0.1f, 0.1f, 0.1f); + } mag; + + struct PseudoMeasurement + { + real_T quaternion_norm_R = 0.00001f; + } pseudo_meas; + + struct InitialStatesStdErr + { + Vector3r position = Vector3r(5.0f, 5.0f, 5.0f); + Vector3r linear_velocity = Vector3r(2.0, 2.0, 5.0); + // Vector3r attitude = Vector3r(2.0, 2.0, 5.0); + Vector3r accel_bias = Vector3r(0.1f, 0.1f, 0.1f); + Vector3r gyro_bias = Vector3r(0.005f, 0.005f, 0.005f); + real_T baro_bias = 0.1f; + Quaternionr quaternion = Quaternionr(0.03f, 0.03f, 0.03f, 0.03f); + } initial_states_std_err; + + struct InitialStatesErr + { + Vector3r position = Vector3r(0.0f, 0.0f, 0.0f); + Vector3r linear_velocity = Vector3r(0.0f, 0.0f, 0.0f); + Vector3r attitude = Vector3r(0.0, 0.0, 0.0); + Vector3r accel_bias = Vector3r(0.0f, 0.0f, 0.0f); + Vector3r gyro_bias = Vector3r(0.0f, 0.0f, 0.0f); + real_T baro_bias = 0.0f; + Quaternionr quaternion; + } initial_states_err; + + void readVector3r(const Settings& json_child, const std::array& json_str, Vector3r& vector) + { + float element_x = json_child.getFloat(json_str.at(0), Utils::nan()); + if (!std::isnan(element_x)) { + vector.x() = element_x; + } + float element_y = json_child.getFloat(json_str.at(1), Utils::nan()); + if (!std::isnan(element_y)) { + vector.y() = element_y; + } + float element_z = json_child.getFloat(json_str.at(2), Utils::nan()); + if (!std::isnan(element_z)) { + vector.z() = element_z; + } + } + + void readRealT(const Settings& json_child, const std::string json_str, real_T& destination) + { + float element = json_child.getFloat(json_str, Utils::nan()); + if (!std::isnan(element)) { + destination = element; + } + } + + void readQuaternionr(const Settings& json_child, const std::array& json_str, Quaternionr& quaternion) + { + readRealT(json_child, json_str.at(0), quaternion.w()); + float element_x = json_child.getFloat(json_str.at(1), Utils::nan()); + if (!std::isnan(element_x)) { + quaternion.x() = element_x; + } + float element_y = json_child.getFloat(json_str.at(2), Utils::nan()); + if (!std::isnan(element_y)) { + quaternion.y() = element_y; + } + float element_z = json_child.getFloat(json_str.at(3), Utils::nan()); + if (!std::isnan(element_z)) { + quaternion.z() = element_z; + } + } + + void initializeParameters(const AirSimSettings::EkfSetting* settings) + { + initializeFromSettings(settings); + refreshAndUnitConversion(); + } + + void refreshAndUnitConversion() + { + gyro.std_error = gyro.std_error * M_PI / 180; // deg/s to rad/s + initial_states_err.gyro_bias = initial_states_err.gyro_bias * M_PI / 180; // deg/s to rad/s + initial_states_std_err.gyro_bias = initial_states_std_err.gyro_bias * M_PI / 180; // deg/s to rad/s + + initial_states_err.attitude = initial_states_err.attitude * M_PI / 180; //deg to rad + initial_states_err.quaternion = VectorMath::toQuaternion(initial_states_err.attitude.y(), + initial_states_err.attitude.x(), + initial_states_err.attitude.z()); + } + + void initializeFromSettings(const AirSimSettings::EkfSetting* settings) + { + if (settings == nullptr) { + return; + } + const auto& json = settings->settings; + + float enabled = json.getBool("Enabled", Utils::nan()); + if (!std::isnan(enabled)) { + ekf_enabled = enabled; + } + float gps_fusion = json.getBool("GpsFusion", Utils::nan()); + if (!std::isnan(gps_fusion)) { + fuse_gps = gps_fusion; + } + float baro_fusion = json.getBool("BaroFusion", Utils::nan()); + if (!std::isnan(baro_fusion)) { + fuse_baro = baro_fusion; + } + float mag_fusion = json.getBool("MagnetoFusion", Utils::nan()); + if (!std::isnan(mag_fusion)) { + fuse_mag = mag_fusion; + } + Settings imu_child; + if (json.getChild("Imu", imu_child)) { + std::array gyro_str = { + "GyroErrorStdDevX", + "GyroErrorStdDevY", + "GyroErrorStdDevZ" + }; + readVector3r(imu_child, gyro_str, gyro.std_error); + std::array accel_str = { + "AccelErrorStdDevX", + "AccelErrorStdDevY", + "AccelErrorStdDevZ" + }; + readVector3r(imu_child, accel_str, accel.std_error); + } + Settings gps_child; + if (json.getChild("Gps", gps_child)) { + std::array gps_pos_str = { + "PositionErrorStdDevX", + "PositionErrorStdDevY", + "PositionErrorStdDevZ" + }; + readVector3r(gps_child, gps_pos_str, gps.std_error_position); + std::array gps_vel_str = { + "VelocityErrorStdDevX", + "VelocityErrorStdDevY", + "VelocityErrorStdDevZ" + }; + readVector3r(gps_child, gps_vel_str, gps.std_error_velocity); + } + Settings baro_child; + if (json.getChild("Barometer", baro_child)) { + readRealT(baro_child, "PositionErrorStdDevZ", baro.std_error); + } + Settings mag_child; + if (json.getChild("Magnetometer", mag_child)) { + std::array mag_str = { + "MagFluxErrorStdDevX", + "MagFluxErrorStdDevY", + "MagFluxErrorStdDevZ" + }; + readVector3r(mag_child, mag_str, mag.std_error); + } + Settings pseudo_meas_child; + if (json.getChild("PseudoMeasurement", pseudo_meas_child)) { + readRealT(pseudo_meas_child, "QuaternionNormR", pseudo_meas.quaternion_norm_R); + } + Settings initial_states_std_err_child; + if (json.getChild("InitialStatesStdErr", initial_states_std_err_child)) { + std::array pos_str = { + "PositionX", + "PositionY", + "PositionZ" + }; + readVector3r(initial_states_std_err_child, pos_str, initial_states_std_err.position); + std::array lin_vel_str = { + "LinearVelocityX", + "LinearVelocityY", + "LinearVelocityZ" + }; + readVector3r(initial_states_std_err_child, lin_vel_str, initial_states_std_err.linear_velocity); + std::array quaternion_str = { + "QuaternionW", + "QuaternionX", + "QuaternionY", + "QuaternionZ" + }; + readQuaternionr(initial_states_std_err_child, quaternion_str, initial_states_std_err.quaternion); + std::array accel_bias_str = { + "AccelBiasX", + "AccelBiasY", + "AccelBiasZ" + }; + readVector3r(initial_states_std_err_child, accel_bias_str, initial_states_std_err.accel_bias); + std::array gyro_bias_str = { + "GyroBiasX", + "GyroBiasY", + "GyroBiasZ" + }; + readVector3r(initial_states_std_err_child, gyro_bias_str, initial_states_std_err.gyro_bias); + readRealT(initial_states_std_err_child, "BaroBias", initial_states_std_err.baro_bias); + } + Settings initial_states_err_child; + if (json.getChild("InitialStatesErr", initial_states_err_child)) { + std::array pos_str = { + "PositionX", + "PositionY", + "PositionZ" + }; + readVector3r(initial_states_err_child, pos_str, initial_states_err.position); + std::array lin_vel_str = { + "LinearVelocityX", + "LinearVelocityY", + "LinearVelocityZ" + }; + readVector3r(initial_states_err_child, lin_vel_str, initial_states_err.linear_velocity); + std::array attitude_str = { + "AttitideRoll", + "AttitidePitch", + "AttitideYaw" + }; + readVector3r(initial_states_err_child, attitude_str, initial_states_err.attitude); + std::array accel_bias_str = { + "AccelBiasX", + "AccelBiasY", + "AccelBiasZ" + }; + readVector3r(initial_states_err_child, accel_bias_str, initial_states_err.accel_bias); + std::array gyro_bias_str = { + "GyroBiasX", + "GyroBiasY", + "GyroBiasZ" + }; + readVector3r(initial_states_err_child, gyro_bias_str, initial_states_err.gyro_bias); + readRealT(initial_states_err_child, "BaroBias", initial_states_err.baro_bias); + } + } + }; + +} +} //namespace +#endif diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightBoard.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightBoard.hpp index eedd390a28..074bb4e6d7 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightBoard.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightBoard.hpp @@ -20,9 +20,17 @@ namespace airlib class AirSimSimpleFlightBoard : public simple_flight::IBoard { public: - AirSimSimpleFlightBoard(const simple_flight::Params* params) - : params_(params) + AirSimSimpleFlightBoard(const simple_flight::Params* params, const MultiRotorParams* vehicle_params) + : params_(params), vehicle_params_(vehicle_params) { + const std::string& imu_name = ""; + const std::string& barometer_name = ""; + const std::string& magnetometer_name = ""; + const std::string& gps_name = ""; + setSensors(imu_name, + barometer_name, + magnetometer_name, + gps_name); } //interface for simulator -------------------------------------------------------------------------------- @@ -122,7 +130,107 @@ namespace airlib //no op for now } + virtual bool checkImuIfNew() const override + { + return imu_->checkIfNew(); + } + + virtual bool checkBarometerIfNew() const override + { + return barometer_->checkIfNew(); + } + + virtual bool checkMagnetometerIfNew() const override + { + return magnetometer_->checkIfNew(); + } + + virtual bool checkGpsIfNew() const override + { + return gps_->checkIfNew(); + } + + virtual void readImuData(real_T accel[3], real_T gyro[3]) const override + { + accel[0] = imu_->getOutput().linear_acceleration.x(); + accel[1] = imu_->getOutput().linear_acceleration.y(); + accel[2] = imu_->getOutput().linear_acceleration.z(); + + gyro[0] = imu_->getOutput().angular_velocity.x(); + gyro[1] = imu_->getOutput().angular_velocity.y(); + gyro[2] = imu_->getOutput().angular_velocity.z(); + } + + virtual void readBarometerData(real_T* altitude) const override + { + *altitude = barometer_->getOutput().altitude; + } + + virtual void readMagnetometerData(real_T mag[3]) const override + { + mag[0] = magnetometer_->getOutput().magnetic_field_body.x(); + mag[1] = magnetometer_->getOutput().magnetic_field_body.y(); + mag[2] = magnetometer_->getOutput().magnetic_field_body.z(); + } + + virtual void readGpsData(double geo[3], real_T vel[3]) const override + { + geo[0] = gps_->getOutput().gnss.geo_point.longitude; + geo[1] = gps_->getOutput().gnss.geo_point.latitude; + geo[2] = gps_->getOutput().gnss.geo_point.altitude; + + vel[0] = gps_->getOutput().gnss.velocity.x(); + vel[1] = gps_->getOutput().gnss.velocity.y(); + vel[2] = gps_->getOutput().gnss.velocity.z(); + } + private: + const SensorCollection& getSensors() + { + return vehicle_params_->getSensors(); + } + + void setSensors(const std::string& imu_name, + const std::string& barometer_name, + const std::string& magnetometer_name, + const std::string& gps_name) + { + imu_ = static_cast(findSensorByName(imu_name, SensorBase::SensorType::Imu)); + if (imu_ == nullptr) { + //TODO handle error + } + barometer_ = static_cast(findSensorByName(barometer_name, SensorBase::SensorType::Barometer)); + if (barometer_ == nullptr) { + //TODO handle error + } + magnetometer_ = static_cast(findSensorByName(magnetometer_name, SensorBase::SensorType::Magnetometer)); + if (magnetometer_ == nullptr) { + //TODO handle error + } + gps_ = static_cast(findSensorByName(gps_name, SensorBase::SensorType::Gps)); + if (gps_ == nullptr) { + //TODO handle error + } + } + + const SensorBase* findSensorByName(const std::string& sensor_name, const SensorBase::SensorType type) + { + const SensorBase* sensor = nullptr; + + // Find sensor with the given name (for empty input name, return the first one found) + // Not efficient but should suffice given small number of sensors + uint count_sensors = getSensors().size(type); + for (uint i = 0; i < count_sensors; i++) { + const SensorBase* current_sensor = getSensors().getByType(type, i); + if (current_sensor != nullptr && (current_sensor->getName() == sensor_name || sensor_name == "")) { + sensor = current_sensor; + break; + } + } + + return sensor; + } + void sleep(double msec) { clock()->sleep_for(msec * 1000.0); @@ -146,6 +254,12 @@ namespace airlib const simple_flight::Params* params_; const Kinematics::State* kinematics_; + + const MultiRotorParams* vehicle_params_; + const ImuBase* imu_ = nullptr; + const BarometerBase* barometer_ = nullptr; + const MagnetometerBase* magnetometer_ = nullptr; + const GpsBase* gps_ = nullptr; }; } } //namespace diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightCommon.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightCommon.hpp index 9f34a424ef..d7d96b68e0 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightCommon.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightCommon.hpp @@ -6,6 +6,7 @@ #include "physics/Kinematics.hpp" #include "common/Common.hpp" +#include "common/EkfStructs.hpp" namespace msr { @@ -47,6 +48,41 @@ namespace airlib return state3r; } + static SensorMeasurements toSensorMeasurements(const simple_flight::SensorMeasurements& sensor_meas) + { + SensorMeasurements airlib_sensor_meas; + airlib_sensor_meas.accel = toVector3r(sensor_meas.accel); + airlib_sensor_meas.gyro = toVector3r(sensor_meas.gyro); + airlib_sensor_meas.gps_position = toVector3r(sensor_meas.gps_position); + airlib_sensor_meas.gps_velocity = toVector3r(sensor_meas.gps_velocity); + airlib_sensor_meas.magnetic_flux = toVector3r(sensor_meas.magnetic_flux); + airlib_sensor_meas.baro_altitude = sensor_meas.baro_altitude; + + return airlib_sensor_meas; + } + + static SensorBiases toSensorBiases(const simple_flight::SensorBiases& sensor_bias) + { + SensorBiases airlib_sensor_bias; + airlib_sensor_bias.accel = toVector3r(sensor_bias.accel); + airlib_sensor_bias.gyro = toVector3r(sensor_bias.gyro); + airlib_sensor_bias.barometer = sensor_bias.barometer; + + return airlib_sensor_bias; + } + + static EkfKinematicsState toEkfKinematicsState(const simple_flight::EkfKinematicsState& ekf_states) + { + EkfKinematicsState airlib_ekf_states; + airlib_ekf_states.position = toVector3r(ekf_states.position); + airlib_ekf_states.orientation = toQuaternion(ekf_states.orientation); + airlib_ekf_states.angles = toVector3r(ekf_states.angles); + airlib_ekf_states.linear_velocity = toVector3r(ekf_states.linear_velocity); + airlib_ekf_states.sensor_bias = toSensorBiases(ekf_states.sensor_bias); + + return airlib_ekf_states; + } + static simple_flight::Axis4r toAxis4r(const Quaternionr& q) { simple_flight::Axis4r conv; diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightEstimator.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightEstimator.hpp index 8e2f0a0c30..11da4de87d 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightEstimator.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightEstimator.hpp @@ -18,6 +18,11 @@ namespace airlib class AirSimSimpleFlightEstimator : public simple_flight::IStateEstimator { public: + AirSimSimpleFlightEstimator(simple_flight::IEkf* ekf) + : ekf_(ekf), ekf_enabled_(ekf_->checkEkfEnabled()) + { + } + virtual ~AirSimSimpleFlightEstimator() {} //for now we don't do any state estimation and use ground truth (i.e. assume perfect sensors) @@ -27,20 +32,142 @@ namespace airlib environment_ = environment; } + virtual bool checkEkfEnabled() const override + { + return ekf_enabled_; + } + virtual simple_flight::Axis3r getAngles() const override + { + if (ekf_enabled_) { + return getEkfAngles(); + } + else { + return getTrueAngles(); + } + } + + virtual simple_flight::Axis3r getAngularVelocity() const override + { + if (ekf_enabled_) { + auto ekf_measurements = getEkfMeasurements(); + return AirSimSimpleFlightCommon::toAxis3r(VectorMath::transformToWorldFrame(AirSimSimpleFlightCommon::toVector3r(ekf_measurements.gyro), + AirSimSimpleFlightCommon::toQuaternion(getOrientation()), + true)); + } + else { + return getTrueAngularVelocity(); + } + } + + virtual simple_flight::Axis3r getPosition() const override + { + if (ekf_enabled_) { + return getEkfPosition(); + } + else { + return getTruePosition(); + } + } + + virtual simple_flight::Axis3r transformToBodyFrame(const simple_flight::Axis3r& world_frame_val) const override + { + if (ekf_enabled_) { + const Vector3r& vec = AirSimSimpleFlightCommon::toVector3r(world_frame_val); + const Vector3r& trans = VectorMath::transformToBodyFrame(vec, AirSimSimpleFlightCommon::toQuaternion(getOrientation())); + return AirSimSimpleFlightCommon::toAxis3r(trans); + } + else { + const Vector3r& vec = AirSimSimpleFlightCommon::toVector3r(world_frame_val); + const Vector3r& trans = VectorMath::transformToBodyFrame(vec, kinematics_->pose.orientation); + return AirSimSimpleFlightCommon::toAxis3r(trans); + } + } + + virtual simple_flight::Axis3r getLinearVelocity() const override + { + if (ekf_enabled_) { + return getEkfLinearVelocity(); + } + else { + return getTrueLinearVelocity(); + } + } + + virtual simple_flight::Axis4r getOrientation() const override + { + if (ekf_enabled_) { + return getEkfOrientation(); + } + else { + return getTrueOrientation(); + } + } + + virtual simple_flight::GeoPoint getGeoPoint() const override // TODO return this from measurement + { + return AirSimSimpleFlightCommon::toSimpleFlightGeoPoint(environment_->getState().geo_point); + } + + virtual simple_flight::GeoPoint getHomeGeoPoint() const override + { + return AirSimSimpleFlightCommon::toSimpleFlightGeoPoint(environment_->getHomeGeoPoint()); + } + + virtual simple_flight::KinematicsState getKinematicsEstimated() const override + { + simple_flight::KinematicsState state; + state.position = getPosition(); + state.orientation = getOrientation(); + state.linear_velocity = getLinearVelocity(); + state.angular_velocity = getAngularVelocity(); + state.linear_acceleration = AirSimSimpleFlightCommon::toAxis3r(kinematics_->accelerations.linear); // remove this + state.angular_acceleration = AirSimSimpleFlightCommon::toAxis3r(kinematics_->accelerations.angular); // remove this + + return state; + } + + // additional methods to get ground truth + + virtual simple_flight::SensorMeasurements getTrueMeasurements() const override + { + simple_flight::SensorMeasurements true_measurements; + + VectorMath::Vector3f linear_acceleration = kinematics_->accelerations.linear - environment_->getState().gravity; + + //acceleration is in world frame so transform to body frame + linear_acceleration = VectorMath::transformToBodyFrame(linear_acceleration, + kinematics_->pose.orientation, + true); + + true_measurements.accel.x() = linear_acceleration.x(); + true_measurements.accel.y() = linear_acceleration.y(); + true_measurements.accel.z() = linear_acceleration.z(); + true_measurements.gyro.x() = kinematics_->twist.angular.x(); + true_measurements.gyro.y() = kinematics_->twist.angular.y(); + true_measurements.gyro.z() = kinematics_->twist.angular.z(); + true_measurements.gps_position.x() = kinematics_->pose.position.x(); + true_measurements.gps_position.y() = kinematics_->pose.position.y(); + true_measurements.gps_position.z() = kinematics_->pose.position.z(); + true_measurements.gps_velocity.x() = kinematics_->twist.linear.x(); + true_measurements.gps_velocity.y() = kinematics_->twist.linear.y(); + true_measurements.gps_velocity.z() = kinematics_->twist.linear.z(); + true_measurements.baro_altitude = -1.0f * kinematics_->pose.position.z(); + + return true_measurements; + } + virtual simple_flight::Axis3r getTrueAngles() const override { simple_flight::Axis3r angles; VectorMath::toEulerianAngle(kinematics_->pose.orientation, angles.pitch(), angles.roll(), angles.yaw()); - //Utils::log(Utils::stringf("Ang Est:\t(%f, %f, %f)", angles.pitch(), angles.roll(), angles.yaw())); return angles; } - - virtual simple_flight::Axis3r getAngularVelocity() const override + virtual simple_flight::Axis3r getTrueAngularVelocity() const override { const auto& anguler = kinematics_->twist.angular; @@ -51,55 +178,247 @@ namespace airlib return conv; } - - virtual simple_flight::Axis3r getPosition() const override + virtual simple_flight::Axis3r getTruePosition() const override { return AirSimSimpleFlightCommon::toAxis3r(kinematics_->pose.position); } + virtual simple_flight::Axis3r getTrueLinearVelocity() const override + { + return AirSimSimpleFlightCommon::toAxis3r(kinematics_->twist.linear); + } + virtual simple_flight::Axis4r getTrueOrientation() const override + { + return AirSimSimpleFlightCommon::toAxis4r(kinematics_->pose.orientation); + } + virtual simple_flight::KinematicsState getTrueKinematicsEstimated() const override + { + simple_flight::KinematicsState state; + state.position = getTruePosition(); + state.orientation = getTrueOrientation(); + state.linear_velocity = getTrueLinearVelocity(); + state.angular_velocity = getTrueAngularVelocity(); + state.linear_acceleration = AirSimSimpleFlightCommon::toAxis3r(kinematics_->accelerations.linear); // remove this + state.angular_acceleration = AirSimSimpleFlightCommon::toAxis3r(kinematics_->accelerations.angular); // remove this - virtual simple_flight::Axis3r transformToBodyFrame(const simple_flight::Axis3r& world_frame_val) const override + return state; + } + + // additional methods to get Ekf results + + virtual simple_flight::SensorMeasurements getEkfMeasurements() const override { - const Vector3r& vec = AirSimSimpleFlightCommon::toVector3r(world_frame_val); - const Vector3r& trans = VectorMath::transformToBodyFrame(vec, kinematics_->pose.orientation); - return AirSimSimpleFlightCommon::toAxis3r(trans); + simple_flight::SensorMeasurements ekf_measurements; + auto ekf_measurements_vector = ekf_->getEkfMeasurements(); + + ekf_measurements.accel.x() = ekf_measurements_vector(0); + ekf_measurements.accel.y() = ekf_measurements_vector(1); + ekf_measurements.accel.z() = ekf_measurements_vector(2); + ekf_measurements.gyro.x() = ekf_measurements_vector(3); + ekf_measurements.gyro.y() = ekf_measurements_vector(4); + ekf_measurements.gyro.z() = ekf_measurements_vector(5); + ekf_measurements.gps_position.x() = ekf_measurements_vector(6); + ekf_measurements.gps_position.y() = ekf_measurements_vector(7); + ekf_measurements.gps_position.z() = ekf_measurements_vector(8); + ekf_measurements.gps_velocity.x() = ekf_measurements_vector(9); + ekf_measurements.gps_velocity.y() = ekf_measurements_vector(10); + ekf_measurements.gps_velocity.z() = ekf_measurements_vector(11); + ekf_measurements.baro_altitude = ekf_measurements_vector(12); + ekf_measurements.magnetic_flux.x() = ekf_measurements_vector(13); + ekf_measurements.magnetic_flux.y() = ekf_measurements_vector(14); + ekf_measurements.magnetic_flux.z() = ekf_measurements_vector(15); + + return ekf_measurements; } - virtual simple_flight::Axis3r getLinearVelocity() const override + virtual simple_flight::Axis3r getEkfPosition() const override { - return AirSimSimpleFlightCommon::toAxis3r(kinematics_->twist.linear); + simple_flight::Axis3r position; + auto ekf_states = ekf_->getEkfStates(); + position.x() = ekf_states(0); + position.y() = ekf_states(1); + position.z() = ekf_states(2); + + return position; } - virtual simple_flight::Axis4r getOrientation() const override + virtual simple_flight::Axis3r getEkfLinearVelocity() const override { - return AirSimSimpleFlightCommon::toAxis4r(kinematics_->pose.orientation); + simple_flight::Axis3r velocity; + auto ekf_states = ekf_->getEkfStates(); + velocity.x() = ekf_states(3); + velocity.y() = ekf_states(4); + velocity.z() = ekf_states(5); + + return velocity; } - virtual simple_flight::GeoPoint getGeoPoint() const override + virtual simple_flight::Axis4r getEkfOrientation() const override { - return AirSimSimpleFlightCommon::toSimpleFlightGeoPoint(environment_->getState().geo_point); + simple_flight::Axis4r orientation; + auto ekf_states = ekf_->getEkfStates(); + + orientation.val4() = ekf_states(6); + orientation.x() = ekf_states(7); + orientation.y() = ekf_states(8); + orientation.z() = ekf_states(9); + + return orientation; } - virtual simple_flight::GeoPoint getHomeGeoPoint() const override + virtual simple_flight::Axis3r getEkfAngles() const override { - return AirSimSimpleFlightCommon::toSimpleFlightGeoPoint(environment_->getHomeGeoPoint()); + simple_flight::Axis3r angles; + Quaternionr orientation; + + auto ekf_states = ekf_->getEkfStates(); + orientation.w() = ekf_states(6); + orientation.x() = ekf_states(7); + orientation.y() = ekf_states(8); + orientation.z() = ekf_states(9); + + VectorMath::toEulerianAngle(orientation, + angles.pitch(), + angles.roll(), + angles.yaw()); + + return angles; } - virtual simple_flight::KinematicsState getKinematicsEstimated() const override + virtual simple_flight::SensorBiases getEkfSensorBias() const override { - simple_flight::KinematicsState state; - state.position = getPosition(); - state.orientation = getOrientation(); - state.linear_velocity = getLinearVelocity(); - state.angular_velocity = getAngularVelocity(); - state.linear_acceleration = AirSimSimpleFlightCommon::toAxis3r(kinematics_->accelerations.linear); - state.angular_acceleration = AirSimSimpleFlightCommon::toAxis3r(kinematics_->accelerations.angular); + simple_flight::SensorBiases bias; + auto ekf_states = ekf_->getEkfStates(); + bias.accel.x() = ekf_states(10); + bias.accel.y() = ekf_states(11); + bias.accel.z() = ekf_states(12); + bias.gyro.x() = ekf_states(13); + bias.gyro.y() = ekf_states(14); + bias.gyro.z() = ekf_states(15); + bias.barometer = ekf_states(16); + + return bias; + } + + virtual simple_flight::EkfKinematicsState getEkfKinematicsEstimated() const override + { + simple_flight::EkfKinematicsState state; + state.position = getEkfPosition(); + state.orientation = getEkfOrientation(); + state.angles = getEkfAngles(); + state.linear_velocity = getEkfLinearVelocity(); + state.sensor_bias = getEkfSensorBias(); return state; } + virtual simple_flight::Axis3r getEkfPositionVariance() const override + { + simple_flight::Axis3r position_var; + auto ekf_covariance = ekf_->getEkfCovariance(); + position_var.x() = ekf_covariance(0, 0); + position_var.y() = ekf_covariance(1, 1); + position_var.z() = ekf_covariance(2, 2); + + return position_var; + } + + virtual simple_flight::Axis3r getEkfLinearVelocityVariance() const override + { + simple_flight::Axis3r velocity_var; + auto ekf_covariance = ekf_->getEkfCovariance(); + velocity_var.x() = ekf_covariance(3, 3); + velocity_var.y() = ekf_covariance(4, 4); + velocity_var.z() = ekf_covariance(5, 5); + + return velocity_var; + } + + virtual simple_flight::Axis4r getEkfOrientationVariance() const override + { + simple_flight::Axis4r orientation_var; + auto ekf_covariance = ekf_->getEkfCovariance(); + orientation_var.val4() = ekf_covariance(6, 6); + orientation_var.x() = ekf_covariance(7, 7); + orientation_var.y() = ekf_covariance(8, 8); + orientation_var.z() = ekf_covariance(9, 9); + + return orientation_var; + } + + virtual simple_flight::Axis3r getEkfAnglesVariance() const override + { + simple_flight::Axis3r angles_var; + auto ekf_angles_covariance = ekf_->getEkfEulerAnglesCovariance(); + angles_var.x() = ekf_angles_covariance(0, 0); + angles_var.y() = ekf_angles_covariance(1, 1); + angles_var.z() = ekf_angles_covariance(2, 2); + + return angles_var; + } + + virtual simple_flight::Axis3r getEkfAccelBiasVariance() const override + { + simple_flight::Axis3r accel_bias_var; + auto ekf_covariance = ekf_->getEkfCovariance(); + accel_bias_var.x() = ekf_covariance(10, 10); + accel_bias_var.y() = ekf_covariance(11, 11); + accel_bias_var.z() = ekf_covariance(12, 12); + + return accel_bias_var; + } + + virtual simple_flight::Axis3r getEkfGyroBiasVariance() const override + { + simple_flight::Axis3r gyro_bias_var; + auto ekf_covariance = ekf_->getEkfCovariance(); + gyro_bias_var.x() = ekf_covariance(13, 13); + gyro_bias_var.y() = ekf_covariance(14, 14); + gyro_bias_var.z() = ekf_covariance(15, 15); + + return gyro_bias_var; + } + + virtual float getEkfBaroBiasVariance() const override + { + float gyro_bias_var; + auto ekf_covariance = ekf_->getEkfCovariance(); + gyro_bias_var = ekf_covariance(16, 16); + // baro_bias_cov = ekf_covariance(15, 15); + + return gyro_bias_var; + } + + virtual simple_flight::EkfKinematicsState getEkfStateVariance() const override + { + simple_flight::EkfKinematicsState state_var; + state_var.position = getEkfPositionVariance(); + state_var.orientation = getEkfOrientationVariance(); + state_var.angles = getEkfAnglesVariance(); + state_var.linear_velocity = getEkfLinearVelocityVariance(); + state_var.sensor_bias.accel = getEkfAccelBiasVariance(); + state_var.sensor_bias.gyro = getEkfGyroBiasVariance(); + state_var.sensor_bias.barometer = getEkfBaroBiasVariance(); + + return state_var; + } + + virtual float getEkfOrientationNorm() const override + { + float norm; + Quaternionr orientation; + + auto ekf_states = ekf_->getEkfStates(); + norm = ekf_states(6) * ekf_states(6) + ekf_states(7) * ekf_states(7) + ekf_states(8) * ekf_states(8) + ekf_states(9) * ekf_states(9); + norm = sqrt(norm); + + return norm; + } + private: const Kinematics::State* kinematics_; const Environment* environment_; + const simple_flight::IEkf* ekf_; + const bool ekf_enabled_; }; } } //namespace diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightApi.hpp index 7ac34b52be..bd172c0050 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightApi.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightApi.hpp @@ -10,11 +10,13 @@ #include "physics/Kinematics.hpp" #include "vehicles/multirotor/MultiRotorParams.hpp" #include "common/Common.hpp" +#include "common/EkfStructs.hpp" #include "firmware/Firmware.hpp" #include "AirSimSimpleFlightBoard.hpp" #include "AirSimSimpleFlightCommLink.hpp" #include "AirSimSimpleFlightEstimator.hpp" #include "AirSimSimpleFlightCommon.hpp" +#include "AirSimSimpleEkf.hpp" #include "physics/PhysicsBody.hpp" #include "common/AirSimSettings.hpp" @@ -38,12 +40,14 @@ namespace airlib safety_params_.vel_to_breaking_dist = safety_params_.min_breaking_dist = 0; //create sim implementations of board and commlink - board_.reset(new AirSimSimpleFlightBoard(¶ms_)); + board_.reset(new AirSimSimpleFlightBoard(¶ms_, vehicle_params_)); comm_link_.reset(new AirSimSimpleFlightCommLink()); - estimator_.reset(new AirSimSimpleFlightEstimator()); + + ekf_.reset(new AirSimSimpleEkf(board_.get(), comm_link_.get(), vehicle_setting->ekf_setting.get())); + estimator_.reset(new AirSimSimpleFlightEstimator(ekf_.get())); //create firmware - firmware_.reset(new simple_flight::Firmware(¶ms_, board_.get(), comm_link_.get(), estimator_.get())); + firmware_.reset(new simple_flight::Firmware(¶ms_, board_.get(), comm_link_.get(), estimator_.get(), ekf_.get())); } public: //VehicleApiBase implementation @@ -114,6 +118,7 @@ namespace airlib { board_->setGroundTruthKinematics(kinematics); estimator_->setGroundTruthKinematics(kinematics, environment); + ekf_->setGroundTruthKinematics(kinematics, environment); } virtual bool setRCData(const RCData& rc_data) override { @@ -386,6 +391,45 @@ namespace airlib //*** End: MultirotorApiBase implementation ***// + public: // additional interface for ekf data recording + // Ground Truth quantities + Kinematics::State getTrueKinematicsEstimated() const + { + return AirSimSimpleFlightCommon::toKinematicsState3r(firmware_->offboardApi().getStateEstimator().getTrueKinematicsEstimated()); + } + + Vector3r getTrueAngles() const + { + const auto& val = firmware_->offboardApi().getStateEstimator().getTrueAngles(); + return AirSimSimpleFlightCommon::toVector3r(val); + } + + SensorMeasurements getTrueMeasurements() const + { + return AirSimSimpleFlightCommon::toSensorMeasurements(firmware_->offboardApi().getStateEstimator().getTrueMeasurements()); + } + + // Estimated quantities + EkfKinematicsState getEkfKinematicsEstimated() const + { + return AirSimSimpleFlightCommon::toEkfKinematicsState(firmware_->offboardApi().getStateEstimator().getEkfKinematicsEstimated()); + } + + SensorMeasurements getEkfMeasurements() const + { + return AirSimSimpleFlightCommon::toSensorMeasurements(firmware_->offboardApi().getStateEstimator().getEkfMeasurements()); + } + + EkfKinematicsState getEkfStateVariance() const + { + return AirSimSimpleFlightCommon::toEkfKinematicsState(firmware_->offboardApi().getStateEstimator().getEkfStateVariance()); + } + + float getEkfOrientationNorm() const + { + return firmware_->offboardApi().getStateEstimator().getEkfOrientationNorm(); + } + private: //convert pitch, roll, yaw from -1 to 1 to PWM static uint16_t angleToPwm(float angle) @@ -421,6 +465,7 @@ namespace airlib unique_ptr comm_link_; unique_ptr estimator_; unique_ptr firmware_; + unique_ptr ekf_; MultirotorApiParams safety_params_; diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/Firmware.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/Firmware.hpp index a612b7e4d7..e414480cf6 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/Firmware.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/Firmware.hpp @@ -12,6 +12,7 @@ #include "Mixer.hpp" #include "CascadeController.hpp" #include "AdaptiveController.hpp" +#include "interfaces/IEkf.hpp" namespace simple_flight { @@ -19,8 +20,8 @@ namespace simple_flight class Firmware : public IFirmware { public: - Firmware(Params* params, IBoard* board, ICommLink* comm_link, IStateEstimator* state_estimator) - : params_(params), board_(board), comm_link_(comm_link), state_estimator_(state_estimator), offboard_api_(params, board, board, state_estimator, comm_link), mixer_(params) + Firmware(Params* params, IBoard* board, ICommLink* comm_link, IStateEstimator* state_estimator, IEkf* ekf) + : params_(params), board_(board), comm_link_(comm_link), state_estimator_(state_estimator), ekf_(ekf), offboard_api_(params, board, board, state_estimator, comm_link), mixer_(params) { switch (params->controller_type) { case Params::ControllerType::Cascade: @@ -42,6 +43,7 @@ class Firmware : public IFirmware board_->reset(); comm_link_->reset(); + ekf_->reset(); controller_->reset(); offboard_api_.reset(); @@ -53,6 +55,7 @@ class Firmware : public IFirmware IFirmware::update(); board_->update(); + ekf_->update(); offboard_api_.update(); controller_->update(); @@ -88,6 +91,7 @@ class Firmware : public IFirmware IBoard* board_; ICommLink* comm_link_; IStateEstimator* state_estimator_; + IEkf* ekf_; OffboardApi offboard_api_; Mixer mixer_; diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/OffboardApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/OffboardApi.hpp index 81ea9e7e56..b7f138686f 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/OffboardApi.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/OffboardApi.hpp @@ -95,8 +95,13 @@ class OffboardApi : public IUpdatable //initial value from RC for smooth transition updateGoalFromRc(); - comm_link_->log("requestApiControl was successful", ICommLink::kLogLevelInfo); - + if (state_estimator_->checkEkfEnabled()) { + message = string("requestApiControl was successful. ") + string("Ekf enabled: ") + string("True"); + } + else { + message = string("requestApiControl was successful. ") + string("Ekf enabled: ") + string("False"); + } + comm_link_->log(message, ICommLink::kLogLevelInfo); return true; } else { diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/CommonStructs.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/CommonStructs.hpp index 0add31cbdd..7e463bfd7b 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/CommonStructs.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/CommonStructs.hpp @@ -213,6 +213,38 @@ struct KinematicsState Axis3r angular_acceleration; }; +struct SensorMeasurements +{ + Axis3r accel; + Axis3r gyro; + + Axis3r gps_position; + Axis3r gps_velocity; + + TReal baro_altitude; + + Axis3r magnetic_flux; +}; + +struct SensorBiases +{ + Axis3r accel; + Axis3r gyro; + + TReal barometer; +}; + +struct EkfKinematicsState +{ + Axis3r position; + Axis4r orientation; + Axis3r angles; + + Axis3r linear_velocity; + + SensorBiases sensor_bias; +}; + enum class VehicleStateType { Unknown, @@ -224,6 +256,30 @@ enum class VehicleStateType Disarmed }; +// Ekf typedefs +constexpr int NX = 17; +constexpr int NU = 6; +constexpr int NW = 13; +typedef VectorMath::Vector17f VectorNXf; +// typedef VectorMath::Matrix16x16f MatrixNXxNXf; +// typedef VectorMath::Matrix12x12f MatrixNWxNWf; +// typedef VectorMath::Matrix16x12f MatrixNXxNWf; +// typedef VectorMath::Matrix1x16f Matrix1xNXf; +// typedef VectorMath::Matrix16x1f MatrixNXx1f; +// typedef VectorMath::Matrix3x16f Matrix3xNXf; +// typedef VectorMath::Matrix16x3f MatrixNXx3f; +// typedef VectorMath::Matrix6x16f Matrix6xNXf; +// typedef VectorMath::Matrix16x6f MatrixNXx6f; +typedef VectorMath::Matrix17x17f MatrixNXxNXf; +typedef VectorMath::Matrix13x13f MatrixNWxNWf; +typedef VectorMath::Matrix17x13f MatrixNXxNWf; +typedef VectorMath::Matrix1x17f Matrix1xNXf; +typedef VectorMath::Matrix17x1f MatrixNXx1f; +typedef VectorMath::Matrix3x17f Matrix3xNXf; +typedef VectorMath::Matrix17x3f MatrixNXx3f; +typedef VectorMath::Matrix6x17f Matrix6xNXf; +typedef VectorMath::Matrix17x6f MatrixNXx6f; + class VehicleState { public: diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IBoardSensors.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IBoardSensors.hpp index d19e65102f..c98e73d76d 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IBoardSensors.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IBoardSensors.hpp @@ -9,6 +9,15 @@ class IBoardSensors virtual void readAccel(float accel[3]) const = 0; //accel in m/s^2 virtual void readGyro(float gyro[3]) const = 0; //angular velocity vector rad/sec + virtual bool checkImuIfNew() const = 0; + virtual bool checkBarometerIfNew() const = 0; + virtual bool checkMagnetometerIfNew() const = 0; + virtual bool checkGpsIfNew() const = 0; + virtual void readImuData(real_T accel[3], real_T gyro[3]) const = 0; + virtual void readBarometerData(real_T* altitude) const = 0; + virtual void readMagnetometerData(real_T mag[3]) const = 0; + virtual void readGpsData(double geo[3], real_T vel[3]) const = 0; + virtual ~IBoardSensors() = default; }; } diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IEkf.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IEkf.hpp new file mode 100644 index 0000000000..aebda5533c --- /dev/null +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IEkf.hpp @@ -0,0 +1,23 @@ +#pragma once + +#include +#include "IUpdatable.hpp" +#include "CommonStructs.hpp" +#include "common/Common.hpp" + +namespace simple_flight +{ + +class IEkf : public IUpdatable +{ +public: + virtual bool checkEkfEnabled() const = 0; + + // getters + virtual const VectorNXf& getEkfStates() const = 0; + virtual const VectorMath::Vector17f& getEkfMeasurements() const = 0; + virtual const MatrixNXxNXf& getEkfCovariance() const = 0; + virtual const VectorMath::Matrix3x3f& getEkfEulerAnglesCovariance() const = 0; +}; + +} //namespace \ No newline at end of file diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IStateEstimator.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IStateEstimator.hpp index 0f19ad83ae..2d798a982d 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IStateEstimator.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IStateEstimator.hpp @@ -8,6 +8,7 @@ namespace simple_flight class IStateEstimator { public: + virtual bool checkEkfEnabled() const = 0; virtual Axis3r getAngles() const = 0; virtual Axis3r getAngularVelocity() const = 0; virtual Axis3r getPosition() const = 0; @@ -21,6 +22,33 @@ class IStateEstimator virtual Axis3r transformToBodyFrame(const Axis3r& world_frame_val) const = 0; + virtual simple_flight::SensorMeasurements getTrueMeasurements() const = 0; + virtual simple_flight::SensorMeasurements getEkfMeasurements() const = 0; + virtual simple_flight::Axis3r getEkfPosition() const = 0; + virtual simple_flight::Axis3r getEkfLinearVelocity() const = 0; + virtual simple_flight::Axis4r getEkfOrientation() const = 0; + virtual simple_flight::Axis3r getEkfAngles() const = 0; + virtual simple_flight::SensorBiases getEkfSensorBias() const = 0; + virtual simple_flight::EkfKinematicsState getEkfKinematicsEstimated() const = 0; + + virtual simple_flight::Axis3r getEkfPositionVariance() const = 0; + virtual simple_flight::Axis3r getEkfLinearVelocityVariance() const = 0; + virtual simple_flight::Axis4r getEkfOrientationVariance() const = 0; + virtual simple_flight::Axis3r getEkfAnglesVariance() const = 0; + virtual simple_flight::Axis3r getEkfAccelBiasVariance() const = 0; + virtual simple_flight::Axis3r getEkfGyroBiasVariance() const = 0; + virtual float getEkfBaroBiasVariance() const = 0; + virtual simple_flight::EkfKinematicsState getEkfStateVariance() const = 0; + + virtual float getEkfOrientationNorm() const = 0; + + virtual simple_flight::Axis3r getTrueAngles() const = 0; + virtual simple_flight::Axis3r getTrueAngularVelocity() const = 0; + virtual simple_flight::Axis3r getTruePosition() const = 0; + virtual simple_flight::Axis3r getTrueLinearVelocity() const = 0; + virtual simple_flight::Axis4r getTrueOrientation() const = 0; + virtual simple_flight::KinematicsState getTrueKinematicsEstimated() const = 0; + virtual ~IStateEstimator() = default; }; } diff --git a/AirLib/src/api/RpcLibServerBase.cpp b/AirLib/src/api/RpcLibServerBase.cpp index 6d1fc7b50a..9a21d2156c 100644 --- a/AirLib/src/api/RpcLibServerBase.cpp +++ b/AirLib/src/api/RpcLibServerBase.cpp @@ -140,6 +140,10 @@ namespace airlib return getVehicleApi(vehicle_name)->armDisarm(arm); }); + pimpl_->server.bind("resetVehicleApi", [&](const std::string& vehicle_name) -> void { + getVehicleApi(vehicle_name)->reset(); + }); + pimpl_->server.bind("simRunConsoleCommand", [&](const std::string& command) -> bool { return getWorldSimApi()->runConsoleCommand(command); }); diff --git a/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp b/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp index 3329798373..d27df5d129 100644 --- a/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp +++ b/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp @@ -27,11 +27,11 @@ namespace airlib SingleTaskCall lock(this); auto kinematics = getKinematicsEstimated(); - if (kinematics.twist.linear.norm() > approx_zero_vel_) { - throw VehicleMoveException(Utils::stringf( - "Cannot perform takeoff because vehicle is already moving with velocity %f m/s", - kinematics.twist.linear.norm())); - } + // if (kinematics.twist.linear.norm() > approx_zero_vel_) { + // throw VehicleMoveException(Utils::stringf( + // "Cannot perform takeoff because vehicle is already moving with velocity %f m/s", + // kinematics.twist.linear.norm())); + // } bool ret = moveToPosition(kinematics.pose.position.x(), kinematics.pose.position.y(), diff --git a/PythonClient/airsim/client.py b/PythonClient/airsim/client.py index bcda726478..05bed454c6 100644 --- a/PythonClient/airsim/client.py +++ b/PythonClient/airsim/client.py @@ -84,6 +84,15 @@ def armDisarm(self, arm, vehicle_name = ''): """ return self.client.call('armDisarm', arm, vehicle_name) + def resetVehicleApi(self, vehicle_name = ''): + """ + Resets vehicle api for vehicle corresponding to vehicle_name + + Args: + vehicle_name (str, optional): Name of the vehicle to send this command to + """ + self.client.call('resetVehicleApi', vehicle_name) + def simPause(self, is_paused): """ Pauses simulation diff --git a/PythonClient/multirotor/hello_drone_ekf.py b/PythonClient/multirotor/hello_drone_ekf.py new file mode 100644 index 0000000000..7bca717f16 --- /dev/null +++ b/PythonClient/multirotor/hello_drone_ekf.py @@ -0,0 +1,29 @@ +# ready to run example: PythonClient/multirotor/hello_drone_ekf.py +import airsim +import time + +# connect to the AirSim simulator +client = airsim.MultirotorClient() +client.confirmConnection() + +client.startRecording() +client.resetVehicleApi() +client.enableApiControl(True) +client.armDisarm(True) + +# Async methods returns Future. Call join() to wait for task to complete. +client.takeoffAsync().join() +client.moveToPositionAsync(-10, 10, -10, 5).join() + +# sleep +time.sleep(10) + +# land +client.moveToPositionAsync(0, 0, -2, 5).join() +client.landAsync().join() + +# stop +time.sleep(5) +client.stopRecording() +client.armDisarm(False) +client.enableApiControl(False) diff --git a/PythonClient/multirotor/plot.py b/PythonClient/multirotor/plot.py new file mode 100644 index 0000000000..2b9585c144 --- /dev/null +++ b/PythonClient/multirotor/plot.py @@ -0,0 +1,334 @@ +""" +Module to plot the recorded drone data. + +Args: + --recording= + e.g. --recording=2022-04-19-14-57-09 + +""" +import argparse +import numpy as np +import pandas as pd +import matplotlib.pyplot as plt +from os.path import join, dirname, abspath, expanduser +from matplotlib.figure import Figure + + +def convert_unit(data) -> pd.DataFrame: + data.TRUE_ANGLE_ROLL = data.TRUE_ANGLE_ROLL * 180 / np.pi + data.TRUE_ANGLE_PITCH = data.TRUE_ANGLE_PITCH * 180 / np.pi + data.TRUE_ANGLE_YAW = data.TRUE_ANGLE_YAW * 180 / np.pi + data.EKF_ANGLE_ROLL = data.EKF_ANGLE_ROLL * 180 / np.pi + data.EKF_ANGLE_PITCH = data.EKF_ANGLE_PITCH * 180 / np.pi + data.EKF_ANGLE_YAW = data.EKF_ANGLE_YAW * 180 / np.pi + data.EKF_ANGLE_ROLL_VAR = data.EKF_ANGLE_ROLL_VAR * 180 / np.pi * 180 / np.pi + data.EKF_ANGLE_PITCH_VAR = data.EKF_ANGLE_PITCH_VAR * 180 / np.pi * 180 / np.pi + data.EKF_ANGLE_YAW_VAR = data.EKF_ANGLE_YAW_VAR * 180 / np.pi * 180 / np.pi + + return data + + +def read_data_error(filename): + data = pd.read_csv(filename, delimiter="\t", header=0) + data = convert_unit(data) + error = pd.DataFrame(data.TRUE_POS_X - data.EKF_POS_X, columns=["POS_X"]) + error = error.join( + pd.DataFrame(data.TRUE_POS_Y - data.EKF_POS_Y, columns=["POS_Y"]) + ) + error = error.join( + pd.DataFrame(data.TRUE_POS_Z - data.EKF_POS_Z, columns=["POS_Z"]) + ) + + timestamp = data.TimeStamp + start_time = timestamp[0] + time = np.array(timestamp - start_time) * 0.001 + data = data.join(pd.DataFrame(time, columns=["Time"])) + + return (time, data, error) + + +def plot_with_confidence(ax, time, array, confidence, label, title) -> None: + ax.plot(time, array[0], linestyle="solid", color="k", label=label[0]) + ax.legend() + ax.plot(time, array[1], linestyle="solid", color="C0", label=label[1]) + ax.legend() + ax.fill_between( + time, + array[1] - 3 * np.sqrt(confidence), + array[1] + 3 * np.sqrt(confidence), + alpha=0.2, + linewidth=0, + color="C5", + ) + ax.set_ylabel(title) + # ax.set_xlim([25, 30]) + # ax.set_ylim([2, 15]) + ax.grid(True) + + +def plot_with_confidence_error(ax, time, array, confidence, title, ylim=None) -> None: + ax.plot(time, array, linestyle="solid", color="k") # , label="error") + # ax.legend() + ax.fill_between( + time, + array - 3 * np.sqrt(confidence), + array + 3 * np.sqrt(confidence), + alpha=0.2, + linewidth=0, + color="C5", + ) + ax.set_ylabel(title) + # ax.set_xlim(xlim) + ax.set_ylim(ylim) + ax.grid(True) + + +def plot_three_items(ax, time, array, label, title) -> None: + ax.plot(time, array[0], linestyle="solid", color="C0", label=label[0]) + ax.legend() + ax.plot(time, array[1], linestyle="dotted", color="C1", label=label[1]) + ax.legend() + ax.plot(time, array[2], linestyle="solid", color="C2", label=label[2]) + ax.legend() + ax.set_ylabel(title) + ax.grid(True) + + +def make_figure(time, data, error) -> Figure: + fig, ax = plt.subplots(nrows=26, ncols=1, figsize=(6, 50)) + + idx = 0 + plot_three_items( + ax[idx], + time, + (data.TRUE_POS_X, data.TRUE_POS_Y, data.TRUE_POS_Z), + ["x", "y", "z"], + "True Position(m)", + ) + idx += 1 + plot_three_items( + ax[idx], + time, + (data.EKF_POS_X, data.EKF_POS_Y, data.EKF_POS_Z), + ["x", "y", "z"], + "Estimated Position(m)", + ) + idx += 1 + plot_three_items( + ax[idx], + time, + (error.POS_X, error.POS_Y, error.POS_Z), + ["x", "y", "z"], + "Error Position(m)", + ) + + idx += 1 + plot_with_confidence_error( + ax[idx], + time, + (error.POS_X), + data.EKF_POS_X_VAR, + "x Position Error(m)", + ylim=[-2, 2] + # ylim=[0,] + ) + + idx += 1 + plot_with_confidence_error( + ax[idx], + time, + (error.POS_Y), + data.EKF_POS_Y_VAR, + "y Position Error(m)", + ylim=[-2, 2], + ) + idx += 1 + plot_with_confidence( + ax[idx], + time, + (data.TRUE_POS_Z, data.EKF_POS_Z), + data.EKF_POS_Z_VAR, + ["true", "est"], + "z Position(m)", + ) + idx += 1 + plot_with_confidence( + ax[idx], + time, + (data.TRUE_ANGLE_ROLL, data.EKF_ANGLE_ROLL), + data.EKF_ANGLE_ROLL_VAR, + ["true", "est"], + "Roll(deg)", + ) + idx += 1 + plot_with_confidence( + ax[idx], + time, + (data.TRUE_ANGLE_PITCH, data.EKF_ANGLE_PITCH), + data.EKF_ANGLE_PITCH_VAR, + ["true", "est"], + "Pitch(deg)", + ) + idx += 1 + plot_with_confidence( + ax[idx], + time, + (data.TRUE_ANGLE_YAW, data.EKF_ANGLE_YAW), + data.EKF_ANGLE_YAW_VAR, + ["true", "est"], + "Yaw(deg)", + ) + idx += 1 + plot_with_confidence( + ax[idx], + time, + (data.TRUE_VEL_X, data.EKF_VEL_X), + data.EKF_VEL_X_VAR, + ["true", "est"], + "x Vel(m/s)", + ) + idx += 1 + plot_with_confidence( + ax[idx], + time, + (data.TRUE_VEL_Y, data.EKF_VEL_Y), + data.EKF_VEL_Y_VAR, + ["true", "est"], + "y Vel(m/s)", + ) + idx += 1 + plot_with_confidence( + ax[idx], + time, + (data.TRUE_VEL_Z, data.EKF_VEL_Z), + data.EKF_VEL_Z_VAR, + ["true", "est"], + "z Vel(m/s)", + ) + idx += 1 + ax[idx].plot(time, data.QUAT_NORM, linestyle="solid", color="C2") + ax[idx].set_ylabel("Quat Norm") + ax[idx].grid(True) + + idx += 1 + plot_with_confidence_error( + ax[idx], time, (data.BIAS_ACCEL_Y), data.BIAS_ACCEL_Y_VAR, "x bias accel(m/s2)" + ) + idx += 1 + plot_with_confidence_error( + ax[idx], time, (data.BIAS_ACCEL_Y), data.BIAS_ACCEL_Y_VAR, "y bias accel(m/s2)" + ) + idx += 1 + plot_with_confidence_error( + ax[idx], time, (data.BIAS_ACCEL_Z), data.BIAS_ACCEL_Z_VAR, "z bias accel(m/s2)" + ) + idx += 1 + plot_with_confidence_error( + ax[idx], + time, + (data.BIAS_GYRO_Y * 180 * np.pi), + data.BIAS_GYRO_Y_VAR * 180 * np.pi * 180 * np.pi, + "x bias gyro(deg/s)", + ) + idx += 1 + plot_with_confidence_error( + ax[idx], + time, + (data.BIAS_GYRO_Y * 180 * np.pi), + data.BIAS_GYRO_Y_VAR * 180 * np.pi * 180 * np.pi, + "y bias gyro(deg/s)", + ) + idx += 1 + plot_with_confidence_error( + ax[idx], + time, + (data.BIAS_GYRO_Z * 180 * np.pi), + data.BIAS_GYRO_Z_VAR * 180 * np.pi * 180 * np.pi, + "z bias gyro(deg/s)", + ) + idx += 1 + plot_with_confidence_error( + ax[idx], time, (data.BIAS_BARO), data.BIAS_BARO_VAR, "bias baro(m)" + ) + + idx += 1 + plot_three_items( + ax[idx], + time, + (data.ACCEL_X, data.ACCEL_Y, data.ACCEL_Z), + ["x", "y", "z"], + "accel(m/s2)", + ) + idx += 1 + plot_three_items( + ax[idx], + time, + ( + data.GYRO_X * 180 * np.pi, + data.GYRO_Y * 180 * np.pi, + data.GYRO_Z * 180 * np.pi, + ), + ["x", "y", "z"], + "gyro(deg/s)", + ) + idx += 1 + plot_three_items( + ax[idx], + time, + (data.GPS_POS_X, data.GPS_POS_Y, data.GPS_POS_Z), + ["x", "y", "z"], + "gps pos(m)", + ) + idx += 1 + plot_three_items( + ax[idx], + time, + (data.GPS_VEL_X, data.GPS_VEL_Y, data.GPS_VEL_Z), + ["x", "y", "z"], + "gps vel(m/s)", + ) + idx += 1 + plot_three_items( + ax[idx], + time, + (data.MAG_X, data.MAG_Y, data.MAG_Z), + ["x", "y", "z"], + "mag flux(Gauss)", + ) + idx += 1 + ax[idx].plot(time, data.BARO_ALT, linestyle="solid", color="C2") + ax[idx].set_ylabel("baro alt(m)") + ax[idx].grid(True) + ax[idx].set_xlabel("Time(s)") + + return fig + + + +def read_and_plot(recording: str, path) -> pd.DataFrame: + plt.style.use(join(path, "style.mplstyle")) + filename = join("~/Documents/AirSim", recording, "airsim_rec.txt") + print(f"Plotting the recorded data: {filename}") + + (time, data, error) = read_data_error(filename) + fig = make_figure(time, data, error) + + fig_name = join(expanduser("~/Documents/AirSim"), recording, "figure.pdf") + fig.savefig(fig_name) + print(f"Plot saved in: {fig_name}") + + +def main() -> None: + """Main entry point""" + parser = argparse.ArgumentParser() + parser.add_argument( + "--recording", help="name of the recording folder", required=True + ) + args = parser.parse_args() + + path = dirname(abspath(__file__)) + + read_and_plot(args.recording, path) + +if __name__ == "__main__": + main() diff --git a/PythonClient/multirotor/style.mplstyle b/PythonClient/multirotor/style.mplstyle new file mode 100644 index 0000000000..b359d80075 --- /dev/null +++ b/PythonClient/multirotor/style.mplstyle @@ -0,0 +1,38 @@ +# Set color cycle: blue, green, yellow, red, violet, gray +axes.prop_cycle : cycler('color', ['0C5DA5', '00B945','FF2C00', 'FF9500', '845B97', '474747', '9e9e9e']) + + +# Set x axis +xtick.direction : in +xtick.major.size : 3 +xtick.major.width : 0.5 +xtick.minor.size : 1.5 +xtick.minor.width : 0.5 +xtick.minor.visible : True +xtick.top : True + +# Set y axis +ytick.direction : in +ytick.major.size : 3 +ytick.major.width : 0.5 +ytick.minor.size : 1.5 +ytick.minor.width : 0.5 +ytick.minor.visible : True +ytick.right : True + +# Set line widths +axes.linewidth : 0.5 +grid.linewidth : 0.5 +lines.linewidth : 0.8 + +# Remove legend frame +legend.frameon : False + +# Always save as 'tight' +savefig.bbox : tight +savefig.pad_inches : 0.05 + +# Use serif fonts +# font.serif : Times +font.family : sans-serif +mathtext.fontset : dejavuserif diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/MultirotorPawnSimApi.cpp b/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/MultirotorPawnSimApi.cpp index 0f27009068..e9814f84b6 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/MultirotorPawnSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/MultirotorPawnSimApi.cpp @@ -1,6 +1,7 @@ #include "MultirotorPawnSimApi.h" #include "AirBlueprintLib.h" #include "vehicles/multirotor/MultiRotorParamsFactory.hpp" +#include "vehicles/multirotor/firmwares/simple_flight/SimpleFlightApi.hpp" #include "UnrealSensors/UnrealSensorFactory.h" #include @@ -183,3 +184,76 @@ MultirotorPawnSimApi::UpdatableObject* MultirotorPawnSimApi::getPhysicsBody() return multirotor_physics_body_->getPhysicsBody(); } //*** End: UpdatableState implementation ***// + +std::string MultirotorPawnSimApi::getRecordFileLine(bool is_header_line) const +{ + std::ostringstream ss; + if (getVehicleSetting()->vehicle_type == "" || //default config + getVehicleSetting()->vehicle_type == msr::airlib::AirSimSettings::kVehicleTypeSimpleFlight) { + + std::string common_line = PawnSimApi::getRecordFileLine(is_header_line); + if (is_header_line) { + return common_line + + "TRUE_POS_X\tTRUE_POS_Y\tTRUE_POS_Z\tTRUE_VEL_X\tTRUE_VEL_Y\tTRUE_VEL_Z\tTRUE_Q_W\tTRUE_Q_X\tTRUE_Q_Y\tTRUE_Q_Z\tTRUE_ANGLE_ROLL\tTRUE_ANGLE_PITCH\tTRUE_ANGLE_YAW\t" + + "EKF_POS_X\tEKF_POS_Y\tEKF_POS_Z\tEKF_VEL_X\tEKF_VEL_Y\tEKF_VEL_Z\tEKF_Q_W\tEKF_Q_X\tEKF_Q_Y\tEKF_Q_Z\tEKF_ANGLE_ROLL\tEKF_ANGLE_PITCH\tEKF_ANGLE_YAW\t" + + "EKF_POS_X_VAR\tEKF_POS_Y_VAR\tEKF_POS_Z_VAR\tEKF_VEL_X_VAR\tEKF_VEL_Y_VAR\tEKF_VEL_Z_VAR\tEKF_Q_W_VAR\tEKF_Q_X_VAR\tEKF_Q_Y_VAR\tEKF_Q_Z_VAR\tEKF_ANGLE_ROLL_VAR\tEKF_ANGLE_PITCH_VAR\tEKF_ANGLE_YAW_VAR\t" + + "QUAT_NORM\t" + + "ACCEL_X\tACCEL_Y\tACCEL_Z\t" + + "GYRO_X\tGYRO_Y\tGYRO_Z\t" + + "GPS_POS_X\tGPS_POS_Y\tGPS_POS_Z\t" + + "GPS_VEL_X\tGPS_VEL_Y\tGPS_VEL_Z\t" + + "BARO_ALT\t" + + "MAG_X\tMAG_Y\tMAG_Z\t" + + "BIAS_ACCEL_X\tBIAS_ACCEL_Y\tBIAS_ACCEL_Z\t" + + "BIAS_GYRO_X\tBIAS_GYRO_Y\tBIAS_GYRO_Z\t" + + "BIAS_BARO\t" + + "BIAS_ACCEL_X_VAR\tBIAS_ACCEL_Y_VAR\tBIAS_ACCEL_Z_VAR\t" + + "BIAS_GYRO_X_VAR\tBIAS_GYRO_Y_VAR\tBIAS_GYRO_Z_VAR\t" + + "BIAS_BARO_VAR\t"; + } + + const SimpleFlightApi* simple_flight_vehicle_api = static_cast(getVehicleApi()); + const auto& true_kin_state = simple_flight_vehicle_api->getTrueKinematicsEstimated(); + const auto& true_angles = simple_flight_vehicle_api->getTrueAngles(); + const auto& ekf_state = simple_flight_vehicle_api->getEkfKinematicsEstimated(); + const auto& ekf_state_variance = simple_flight_vehicle_api->getEkfStateVariance(); + const auto& ekf_meas = simple_flight_vehicle_api->getEkfMeasurements(); + const auto& quaternion_norm = simple_flight_vehicle_api->getEkfOrientationNorm(); + + ss << common_line; + + ss << true_kin_state.pose.position.x() << "\t" << true_kin_state.pose.position.y() << "\t" << true_kin_state.pose.position.z() << "\t"; + ss << true_kin_state.twist.linear.x() << "\t" << true_kin_state.twist.linear.y() << "\t" << true_kin_state.twist.linear.z() << "\t"; + ss << true_kin_state.pose.orientation.w() << '\t' << true_kin_state.pose.orientation.x() << "\t" << true_kin_state.pose.orientation.y() << "\t" << true_kin_state.pose.orientation.z() << "\t"; + ss << true_angles.x() << "\t" << true_angles.y() << "\t" << true_angles.z() << "\t"; + + ss << ekf_state.position.x() << "\t" << ekf_state.position.y() << "\t" << ekf_state.position.z() << "\t"; + ss << ekf_state.linear_velocity.x() << "\t" << ekf_state.linear_velocity.y() << "\t" << ekf_state.linear_velocity.z() << "\t"; + ss << ekf_state.orientation.w() << '\t' << ekf_state.orientation.x() << "\t" << ekf_state.orientation.y() << "\t" << ekf_state.orientation.z() << "\t"; + ss << ekf_state.angles.x() << "\t" << ekf_state.angles.y() << "\t" << ekf_state.angles.z() << "\t"; + + ss << ekf_state_variance.position.x() << "\t" << ekf_state_variance.position.y() << "\t" << ekf_state_variance.position.z() << "\t"; + ss << ekf_state_variance.linear_velocity.x() << "\t" << ekf_state_variance.linear_velocity.y() << "\t" << ekf_state_variance.linear_velocity.z() << "\t"; + ss << ekf_state_variance.orientation.w() << '\t' << ekf_state_variance.orientation.x() << "\t" << ekf_state_variance.orientation.y() << "\t" << ekf_state_variance.orientation.z() << "\t"; + ss << ekf_state_variance.angles.x() << "\t" << ekf_state_variance.angles.y() << "\t" << ekf_state_variance.angles.z() << "\t"; + + ss << quaternion_norm << "\t"; + + ss << ekf_meas.accel.x() << "\t" << ekf_meas.accel.y() << "\t" << ekf_meas.accel.z() << "\t"; + ss << ekf_meas.gyro.x() << "\t" << ekf_meas.gyro.y() << "\t" << ekf_meas.gyro.z() << "\t"; + ss << ekf_meas.gps_position.x() << "\t" << ekf_meas.gps_position.y() << "\t" << ekf_meas.gps_position.z() << "\t"; + ss << ekf_meas.gps_velocity.x() << "\t" << ekf_meas.gps_velocity.y() << "\t" << ekf_meas.gps_velocity.z() << "\t"; + ss << ekf_meas.baro_altitude << "\t"; + ss << ekf_meas.magnetic_flux.x() << "\t" << ekf_meas.magnetic_flux.y() << "\t" << ekf_meas.magnetic_flux.z() << "\t"; + + ss << ekf_state.sensor_bias.accel.x() << "\t" << ekf_state.sensor_bias.accel.y() << "\t" << ekf_state.sensor_bias.accel.z() << "\t"; + ss << ekf_state.sensor_bias.gyro.x() << "\t" << ekf_state.sensor_bias.gyro.y() << "\t" << ekf_state.sensor_bias.gyro.z() << "\t"; + ss << ekf_state.sensor_bias.barometer << "\t"; + + ss << ekf_state_variance.sensor_bias.accel.x() << "\t" << ekf_state_variance.sensor_bias.accel.y() << "\t" << ekf_state_variance.sensor_bias.accel.z() << "\t"; + ss << ekf_state_variance.sensor_bias.gyro.x() << "\t" << ekf_state_variance.sensor_bias.gyro.y() << "\t" << ekf_state_variance.sensor_bias.gyro.z() << "\t"; + ss << ekf_state_variance.sensor_bias.barometer << "\t"; + } + + return ss.str(); +} \ No newline at end of file diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/MultirotorPawnSimApi.h b/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/MultirotorPawnSimApi.h index f7db8a8fda..6c24ab3534 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/MultirotorPawnSimApi.h +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/MultirotorPawnSimApi.h @@ -56,6 +56,8 @@ class MultirotorPawnSimApi : public PawnSimApi return vehicle_api_.get(); } + virtual std::string getRecordFileLine(bool is_header_line) const override; + private: std::unique_ptr vehicle_api_; std::unique_ptr vehicle_params_;