From 3062a73e46bf79058f91ada00b2ada6438ffcf15 Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Sun, 7 Nov 2021 11:06:08 +0100 Subject: [PATCH 01/87] typedef EkfStates and EKfCovariance from Eigen lib --- AirLib/include/common/VectorMath.hpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/AirLib/include/common/VectorMath.hpp b/AirLib/include/common/VectorMath.hpp index 971728e086..382a092e58 100644 --- a/AirLib/include/common/VectorMath.hpp +++ b/AirLib/include/common/VectorMath.hpp @@ -37,6 +37,11 @@ namespace airlib typedef Eigen::Quaternion Quaterniond; typedef Eigen::Matrix Matrix3x3d; typedef Eigen::Matrix Matrix3x3f; + + // added by Suman + typedef Eigen::Matrix EkfStates; + typedef Eigen::Matrix EkfCovariance; + typedef Eigen::AngleAxisd AngleAxisd; typedef Eigen::AngleAxisf AngleAxisf; From fece6081097943b862a4755070fb48fc624eeab6 Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Sun, 7 Nov 2021 11:15:03 +0100 Subject: [PATCH 02/87] implement is_new flag for sensor models --- AirLib/include/sensors/SensorBase.hpp | 17 +++++++++++++++++ .../sensors/barometer/BarometerSimple.hpp | 6 +++++- AirLib/include/sensors/gps/GpsSimple.hpp | 6 +++++- AirLib/include/sensors/imu/ImuSimple.hpp | 3 +++ .../sensors/magnetometer/MagnetometerSimple.hpp | 6 +++++- 5 files changed, 35 insertions(+), 3 deletions(-) diff --git a/AirLib/include/sensors/SensorBase.hpp b/AirLib/include/sensors/SensorBase.hpp index 1e344e6f8e..55bf8c43a7 100644 --- a/AirLib/include/sensors/SensorBase.hpp +++ b/AirLib/include/sensors/SensorBase.hpp @@ -52,6 +52,18 @@ namespace airlib ground_truth_.environment = environment; } + // added by Suman + virtual void update() override + { + is_new_ = false; + } + + // added by Suman + const bool& checkIfNew() const + { + return is_new_; + } + const GroundTruth& getGroundTruth() const { return ground_truth_; @@ -68,6 +80,11 @@ namespace airlib //ground truth can be shared between many sensors GroundTruth ground_truth_ = { nullptr, nullptr }; std::string name_ = ""; + + protected: + // added by Suman + bool is_new_ = false; + }; } } //namespace diff --git a/AirLib/include/sensors/barometer/BarometerSimple.hpp b/AirLib/include/sensors/barometer/BarometerSimple.hpp index fb960e288c..735bcca9fe 100644 --- a/AirLib/include/sensors/barometer/BarometerSimple.hpp +++ b/AirLib/include/sensors/barometer/BarometerSimple.hpp @@ -63,8 +63,12 @@ namespace airlib delay_line_.update(); - if (freq_limiter_.isWaitComplete()) + if (freq_limiter_.isWaitComplete()){ setOutput(delay_line_.getOutput()); + + // added by Suman, isNew flag is set to true if the sensor signal updates + is_new_ = true; + } } //*** End: UpdatableState implementation ***// diff --git a/AirLib/include/sensors/gps/GpsSimple.hpp b/AirLib/include/sensors/gps/GpsSimple.hpp index 23170bf943..73a572b4f8 100644 --- a/AirLib/include/sensors/gps/GpsSimple.hpp +++ b/AirLib/include/sensors/gps/GpsSimple.hpp @@ -61,8 +61,12 @@ namespace airlib delay_line_.update(); - if (freq_limiter_.isWaitComplete()) + if (freq_limiter_.isWaitComplete()){ setOutput(delay_line_.getOutput()); + + // added by Suman, isNew flag is set to true if the sensor signal updates + is_new_ = true; + } } //*** End: UpdatableState implementation ***// diff --git a/AirLib/include/sensors/imu/ImuSimple.hpp b/AirLib/include/sensors/imu/ImuSimple.hpp index 7165682e14..3d37872e9a 100644 --- a/AirLib/include/sensors/imu/ImuSimple.hpp +++ b/AirLib/include/sensors/imu/ImuSimple.hpp @@ -43,6 +43,9 @@ namespace airlib ImuBase::update(); updateOutput(); + + // added by Suman, isNew flag is set to true if the sensor signal updates + is_new_ = true; } //*** End: UpdatableState implementation ***// diff --git a/AirLib/include/sensors/magnetometer/MagnetometerSimple.hpp b/AirLib/include/sensors/magnetometer/MagnetometerSimple.hpp index f76856de8a..70adad8c3d 100644 --- a/AirLib/include/sensors/magnetometer/MagnetometerSimple.hpp +++ b/AirLib/include/sensors/magnetometer/MagnetometerSimple.hpp @@ -59,8 +59,12 @@ namespace airlib delay_line_.update(); - if (freq_limiter_.isWaitComplete()) + if (freq_limiter_.isWaitComplete()){ setOutput(delay_line_.getOutput()); + + // added by Suman, isNew flag is set to true if the sensor signal updates + is_new_ = true; + } } //*** End: UpdatableObject implementation ***// From cfc8a402189161384eb1d8170db5369da57b13ce Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Sun, 7 Nov 2021 11:22:09 +0100 Subject: [PATCH 03/87] add creation of ekf unique pointer and passing a pointer into firmware in SimpleFlightApi, board gets vehicle_params_ --- .../firmwares/simple_flight/SimpleFlightApi.hpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightApi.hpp index 7ac34b52be..9baebd9686 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightApi.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightApi.hpp @@ -15,6 +15,7 @@ #include "AirSimSimpleFlightCommLink.hpp" #include "AirSimSimpleFlightEstimator.hpp" #include "AirSimSimpleFlightCommon.hpp" +#include "AirSimSimpleEKF.hpp" #include "physics/PhysicsBody.hpp" #include "common/AirSimSettings.hpp" @@ -38,12 +39,15 @@ 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()); + // added by Suman + ekf_.reset(new AirSimSimpleEkf(board_.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 @@ -421,6 +425,7 @@ namespace airlib unique_ptr comm_link_; unique_ptr estimator_; unique_ptr firmware_; + unique_ptr ekf_; MultirotorApiParams safety_params_; From afba251617a360881aee89c509804957b16fbcf1 Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Sun, 7 Nov 2021 11:24:03 +0100 Subject: [PATCH 04/87] add ekf in Firmware constructor and call of ekf update --- .../firmwares/simple_flight/firmware/Firmware.hpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) 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..c299c8ab30 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 "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), offboard_api_(params, board, board, state_estimator, comm_link), mixer_(params), ekf_(ekf) { switch (params->controller_type) { case Params::ControllerType::Cascade: @@ -53,6 +54,7 @@ class Firmware : public IFirmware IFirmware::update(); board_->update(); + ekf_->update(); offboard_api_.update(); controller_->update(); @@ -88,6 +90,7 @@ class Firmware : public IFirmware IBoard* board_; ICommLink* comm_link_; IStateEstimator* state_estimator_; + IEkf* ekf_; OffboardApi offboard_api_; Mixer mixer_; From 43a55266eacab96fb12131120168efdd89f05ccb Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Sun, 7 Nov 2021 11:28:11 +0100 Subject: [PATCH 05/87] implement interface to access sensor data from const board_, sensor models come from const vehicle_params_ --- .../simple_flight/AirSimSimpleFlightBoard.hpp | 130 +++++++++++++++++- .../firmware/interfaces/IBoardSensors.hpp | 6 + 2 files changed, 134 insertions(+), 2 deletions(-) diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightBoard.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightBoard.hpp index eedd390a28..c2e2b6608e 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,117 @@ namespace airlib //no op for now } + bool checkImuIfNew() const + { + return imu_->checkIfNew(); + } + + bool checkBarometerIfNew() const + { + return barometer_->checkIfNew(); + } + + bool checkMagnetometerIfNew() const + { + return magnetometer_->checkIfNew(); + } + + bool checkGpsIfNew() const + { + return gps_->checkIfNew(); + } + + // added by Suman + void readImuData(real_T accel[3], real_T gyro[3]) const + { + accel[0] = imu_->getOutput().linear_acceleration.x(); + accel[1] = imu_->getOutput().linear_acceleration.y(); + accel[3] = 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(); + } + + // added by Suman + void readBarometerData(real_T* altitude) const + { + *altitude = barometer_->getOutput().altitude; + } + + // added by Suman + void readMagnetometerData(real_T mag[3]) const + { + mag[0] = magnetometer_->getOutput().magnetic_field_body.x(); + mag[1] = magnetometer_->getOutput().magnetic_field_body.y(); + mag[2] = magnetometer_->getOutput().magnetic_field_body.z();; + } + + // added by Suman + void readGpsData(real_T geo[3], real_T vel[3]) const + { + 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: + // added by Suman + 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) + { + auto* imu_ = static_cast(findSensorByName(imu_name, SensorBase::SensorType::Imu)); + if (imu_ == nullptr) + { + //throw VehicleControllerException(Utils::stringf("No IMU with name %s exist on vehicle", imu_name.c_str())); + } + auto* barometer_ = static_cast(findSensorByName(barometer_name, SensorBase::SensorType::Barometer)); + if (barometer_ == nullptr) + { + //throw VehicleControllerException(Utils::stringf("No barometer with name %s exist on vehicle", barometer_name.c_str())); + } + auto* magnetometer_ = static_cast(findSensorByName(magnetometer_name, SensorBase::SensorType::Magnetometer)); + if (magnetometer_ == nullptr) + { + //throw VehicleControllerException(Utils::stringf("No magnetometer with name %s exist on vehicle", magnetometer_name.c_str())); + } + auto* gps_ = static_cast(findSensorByName(gps_name, SensorBase::SensorType::Gps)); + if (gps_ == nullptr) + { + //throw VehicleControllerException(Utils::stringf("No gps with name %s exist on vehicle", gps_name.c_str())); + } + } + + // added by Suman + 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 +264,14 @@ namespace airlib const simple_flight::Params* params_; const Kinematics::State* kinematics_; + + // added by Suman + 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/firmware/interfaces/IBoardSensors.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IBoardSensors.hpp index 791cd8591a..6ff881e8d1 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 @@ -8,5 +8,11 @@ class IBoardSensors public: 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 + + // // added by Suman + // virtual void readImuData(float accel[3], float gyro[3]) const = 0; + // virtual void readBarometerData(float* altitude) const = 0; + // virtual void readMagnetometerData(float mag[3]) const = 0; + // virtual void readGpsData(float geo[3], float vel[3]) const = 0; }; } \ No newline at end of file From 077352ac9e553559dd4bda59b3ded4d31467243a Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Sun, 7 Nov 2021 11:36:28 +0100 Subject: [PATCH 06/87] create four ekf hpp files and implement basic interfaces and function calls --- .../simple_flight/AirSimSimpleEkf.hpp | 248 ++++++++++++++++++ .../simple_flight/AirSimSimpleEkfBase.hpp | 41 +++ .../simple_flight/AirSimSimpleEkfModel.hpp | 83 ++++++ .../firmware/interfaces/IEkf.hpp | 16 ++ 4 files changed, 388 insertions(+) create mode 100644 AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp create mode 100644 AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfBase.hpp create mode 100644 AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfModel.hpp create mode 100644 AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IEkf.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..88a1ede893 --- /dev/null +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp @@ -0,0 +1,248 @@ +// Liscence info + +#ifndef msr_airlib_AirSimSimpleEkf_hpp +#define msr_airlib_AirSimSimpleEkf_hpp + +#include +#include +#include "interfaces/IBoard.hpp" +#include "common/FrequencyLimiter.hpp" +#include "AirSimSimpleEkfBase.hpp" + +// #include "firmware/Params.hpp" +// #include "common/Common.hpp" +// #include "common/ClockFactory.hpp" +// #include "physics/Kinematics.hpp" + +namespace msr +{ +namespace airlib +{ + + class AirSimSimpleEkf : public AirSimSimpleEkfBase + , private AirSimSimpleEkfModel + { + public: + // Constructor + AirSimSimpleEkf(const simple_flight::IBoard* board)//, const AirSimSettings::EKFSetting& setting = AirSimSettings::EKFSetting()) + : board_(board) + { + // params_ = // get the EKF params from airsim settings. Implement this as a struct for now with constant members ! inspired by sensor model + freq_limiter_.initialize(100); // physics engine and the imu refresh at period 3m ~ 333.33Hz. EKF tops at frequency 100Hz + } + + virtual void update() override + { + IUpdatable::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(); + + // updateEKFInternal(); + } + + private: + // --------------------------------------------------------------------- + // Internal functions + // --------------------------------------------------------------------- + + // this function updates at the frequency of EKF update + void updateEKFInternal() + { + predictionStep(); + measurementUpdateStep(); + } + + // 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]; + + // imu updates at higher frequency than the EKF. + // !Downsampling, needed or automatic? does it have any side effects? + + // 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(); + covariancePropagation(); + } + + // measurement update step + void measurementUpdateStep() + { + magnetometerUpdate(); + barometerUpdate(); + gpsUpdate(); + } + + // state propagtion + void statePropagation() + { + // auto Phi = calculatePhi(); + // auto Gamma_w = calculateGamma_w(); + } + + // co-variance propagtion + void covariancePropagation() + { + // auto Phi = calculatePhi(); + // auto Gamma_w = calculateGamma_w(); + } + + // magnetometer update + void magnetometerUpdate() + { + if(!board_->checkMagnetometerIfNew()) + return; + + // the updates at the frequency of magnetometer signal update + + real_T mag[3]; + + // check if the magnetometer gives new measurement and it is valid + bool is_valid = getMagnetometerData(mag); + + if(!is_valid){ + return; + } + + // auto C = dh_mag_dx(); + + // auto K = ... + } + + // barometer update + void barometerUpdate() + { + if(!board_->checkBarometerIfNew()) + return; + + // the updates at the frequency of barometer signal update + + real_T* altitude; + + // check if the barometer gives new measurement and it is valid + bool is_valid = getBarometerData(altitude); + + if(!is_valid) + { + return; + } + } + + // GPS update + void gpsUpdate() + { + if(!board_->checkGpsIfNew()) + return; + + // the updates at the frequency of GPS signal update + + real_T geo[3]; + real_T vel[3]; + + // check if the GPS gives new measurement and it is valid + bool is_valid = getGpsData(geo, vel); + + if(!is_valid) + { + return; + } + } + + void calculatePhi() + { + // calculate Phi based on the jacobian for some iteration + } + + void calculateGamma_w() + { + // calculate Gamma_w based on the jacobian for some iteration + } + + // --------------------------------------------------------------------- + // 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 + + 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 + + return true; + + } + + // reads barometer data + bool getBarometerData(real_T* altitude) + { + 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 + + return true; + + } + + // reads GPS data + bool getGpsData(real_T geo[3], + real_T vel[3]) + { + board_->readGpsData(geo, vel); + + // check if the signal has all data that is valid, else return false + // TODO: check if at least a subset of data is valid + + return true; + + } + + private: + // --------------------------------------------------------------------- + // Class attritubes + // --------------------------------------------------------------------- + // parameter struct instance + // params_; + // frequency limiter for EKF + FrequencyLimiter freq_limiter_; + // the flight board instance + const simple_flight::IBoard* board_; + + }; + +} +} //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..3c24e7400e --- /dev/null +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfBase.hpp @@ -0,0 +1,41 @@ +// Liscence info + +#ifndef msr_airlib_AirSimSimpleEkfBase_hpp +#define msr_airlib_AirSimSimpleEkfBase_hpp + +#include +#include "CommonStructs.hpp" +#include "common/Common.hpp" +#include "IEkf.hpp" + +namespace msr +{ +namespace airlib +{ + +class AirSimSimpleEkfBase : public simple_flight::IEkf +{ +public: + // setters + void setEkfStates() + { + + } + + // getters + const VectorMath::EkfStates& getEkfStates() const + { + + } + + +private: + // EKF states + VectorMath::EkfStates states_; + // EKF covariances + VectorMath::EkfCovariance covariance_; +}; + +} +} //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..dfad7f7555 --- /dev/null +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfModel.hpp @@ -0,0 +1,83 @@ +// Liscence info + +#ifndef msr_airlib_AirSimSimpleEkf_hpp +#define msr_airlib_AirSimSimpleEkf_hpp + +#include +#include +#include "interfaces/IUpdatable.hpp" +#include "interfaces/IBoard.hpp" +#include "common/FrequencyLimiter.hpp" +#include "IEkf.hpp" + +// #include "firmware/Params.hpp" +// #include "common/Common.hpp" +// #include "common/ClockFactory.hpp" +// #include "physics/Kinematics.hpp" + +namespace msr +{ +namespace airlib +{ + + class AirSimSimpleEkfModel : public simple_flight::IEkf + { + protected: + // --------------------------------------------------------------------- + // Mathematical functions + // --------------------------------------------------------------------- + + void f() + { + // the state_dot vector + } + + void h_mag() + { + // + } + + void h_baro() + { + // + } + + void h_GPS() + { + // + } + + // --------------------------------------------------------------------- + // Mathematical jacobians + // --------------------------------------------------------------------- + + void df_dx() + { + // jacobian wrt the states vector + } + + void df_dw() + { + // jacobian wrt the process noise vector + } + + void dh_mag_dx() + { + // jacobian wrt the states vector + } + + void dh_baro_dx() + { + // jacobian wrt the states vector + } + + void dh_GPS_dx() + { + // jacobian wrt the states vector + } + + }; + +} +} //namespace +#endif \ No newline at end of file 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..49c07c17a4 --- /dev/null +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IEkf.hpp @@ -0,0 +1,16 @@ +#pragma once + +#include +#include "IUpdatable.hpp" +#include "CommonStructs.hpp" +#include "common/Common.hpp" + +namespace simple_flight +{ + +class IEkf : public IUpdatable +{ + +}; + +} //namespace \ No newline at end of file From 083161d3780e3dc33e322f61f42557efa727750b Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Mon, 8 Nov 2021 14:51:16 +0100 Subject: [PATCH 07/87] add parser and can run simpleflighttest --- AirLibUnitTests/SimpleFlightTest.hpp | 32 +++++++++++- AirLibUnitTests/main.cpp | 2 +- AirLibUnitTests/settings_json_parser.hpp | 65 ++++++++++++++++++++++++ 3 files changed, 96 insertions(+), 3 deletions(-) create mode 100644 AirLibUnitTests/settings_json_parser.hpp diff --git a/AirLibUnitTests/SimpleFlightTest.hpp b/AirLibUnitTests/SimpleFlightTest.hpp index 304b7c32bc..90b0943704 100644 --- a/AirLibUnitTests/SimpleFlightTest.hpp +++ b/AirLibUnitTests/SimpleFlightTest.hpp @@ -10,6 +10,10 @@ #include "common/SteppableClock.hpp" #include "vehicles/multirotor/MultiRotorPhysicsBody.hpp" +#include "settings_json_parser.hpp" + +#include "vehicles/multirotor/firmwares/simple_flight/SimpleFlightQuadXParams.hpp" + namespace msr { namespace airlib @@ -24,6 +28,8 @@ namespace airlib ClockFactory::get(clock); SensorFactory sensor_factory; + // added by Suman, from https://github.com/microsoft/AirSim/pull/2558/commits/9c4e59d1a2b371ebc60cdc18f93b06cbe3e9d305 + SettingsLoader settings_loader; std::unique_ptr params = MultiRotorParamsFactory::createConfig( AirSimSettings::singleton().getVehicleSetting("SimpleFlight"), @@ -33,7 +39,7 @@ namespace airlib std::unique_ptr kinematics; std::unique_ptr environment; Kinematics::State initial_kinematic_state = Kinematics::State::zero(); - ; + initial_kinematic_state.pose = Pose(); kinematics.reset(new Kinematics(initial_kinematic_state)); @@ -47,6 +53,18 @@ namespace airlib std::vector vehicles = { &vehicle }; std::unique_ptr physics_engine(new FastPhysicsEngine()); PhysicsWorld physics_world(std::move(physics_engine), vehicles, static_cast(clock->getStepSize() * 1E9)); + // world.startAsyncUpdator(); called in the physics_world constructor + + // added by Suman, to fix calling update before reset https://github.com/microsoft/AirSim/issues/2773#issuecomment-703888477 + // TODO not sure if it should be here? see wrt to PawnSimApi + api->setSimulatedGroundTruth(&kinematics->getState(), environment.get()); + api->reset(); + kinematics->reset(); + + StateReporter reporter; + kinematics->reportState(reporter); // this writes the kinematics in reporter + std::cout << reporter.getOutput() << std::endl; + testAssert(api != nullptr, "api was null"); std::string message; @@ -60,11 +78,21 @@ namespace airlib api->armDisarm(true); api->takeoff(10); + // Vector3r pos = api->getMultirotorState().getPosition(); + // std::cout << pos << std::endl; + + kinematics->reportState(reporter); // this writes the kinematics in reporter + std::cout << reporter.getOutput() << std::endl; + + // TODO print some values OR log + clock->sleep_for(2.0f); Utils::getSetMinLogLevel(true); - api->moveToPosition(-5, -5, -5, 5, 1E3, DrivetrainType::MaxDegreeOfFreedom, YawMode(true, 0), -1, 0); + //api->moveToPosition(-5, -5, -5, 5, 1E3, DrivetrainType::MaxDegreeOfFreedom, YawMode(true, 0), -1, 0); + // pos = api->getMultirotorState().getPosition(); + // std::cout << pos << std::endl; clock->sleep_for(2.0f); diff --git a/AirLibUnitTests/main.cpp b/AirLibUnitTests/main.cpp index 5781050308..a3935bb7fc 100644 --- a/AirLibUnitTests/main.cpp +++ b/AirLibUnitTests/main.cpp @@ -12,7 +12,7 @@ int main() std::unique_ptr tests[] = { std::unique_ptr(new QuaternionTest()), - std::unique_ptr(new CelestialTest()), + // std::unique_ptr(new CelestialTest()), std::unique_ptr(new SettingsTest()), std::unique_ptr(new SimpleFlightTest()) //, diff --git a/AirLibUnitTests/settings_json_parser.hpp b/AirLibUnitTests/settings_json_parser.hpp new file mode 100644 index 0000000000..27682355b5 --- /dev/null +++ b/AirLibUnitTests/settings_json_parser.hpp @@ -0,0 +1,65 @@ +#include "common/common_utils/StrictMode.hpp" +STRICT_MODE_OFF +#ifndef RPCLIB_MSGPACK +#define RPCLIB_MSGPACK clmdep_msgpack +#endif // !RPCLIB_MSGPACK +// #include "rpc/rpc_error.h" +STRICT_MODE_ON + +#include "vehicles/multirotor/api/MultirotorRpcLibClient.hpp" +#include "common/common_utils/FileSystem.hpp" +#include +#include +#include "common/AirSimSettings.hpp" + +class SettingsLoader +{ +public: + SettingsLoader() + { + initializeSettings(); + } + + ~SettingsLoader() {}; + +private: + std::string getSimMode() + { + Settings& settings_json = Settings::loadJSonString(settingsText_); + return settings_json.getString("SimMode", ""); + } + + bool readSettingsTextFromFile(std::string settingsFilepath) + { + // check if path exists + bool found = std::ifstream(settingsFilepath.c_str()).good(); + if (found) + { + std::ifstream ifs(settingsFilepath); + std::stringstream buffer; + buffer << ifs.rdbuf(); + settingsText_ = buffer.str(); + } + return found; + } + + bool initializeSettings() + { + if (readSettingsTextFromFile(msr::airlib::Settings::Settings::getUserDirectoryFullPath("settings.json"))) + { + AirSimSettings::initializeSettings(settingsText_); + + Settings& settings_json = Settings::loadJSonString(settingsText_); + std::string simmode_name = settings_json.getString("SimMode", ""); + AirSimSettings::singleton().load(std::bind(&SettingsLoader::getSimMode, this)); + return true; + } + else + { + return false; + } + } + +private: + std::string settingsText_; +}; \ No newline at end of file From fd5d4c357a04d1e5234f109122f8432f37cdbdef Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Wed, 10 Nov 2021 10:52:09 +0100 Subject: [PATCH 08/87] add quick and dirty commlink log of vehicle altitude series in Firmware --- .../firmwares/simple_flight/firmware/Firmware.hpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) 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..f142464d71 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/Firmware.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/Firmware.hpp @@ -75,6 +75,19 @@ class Firmware : public IFirmware board_->writeOutput(motor_index, motor_outputs_.at(motor_index)); comm_link_->update(); + + // added by Suman, better to move this to OffboardApi.hpp + auto pos = state_estimator_->getPosition().z(); + + std::ostringstream ss; + ss << pos; + std::string s(ss.str()); + std::ostringstream tt; + tt << board_->millis(); + std::string t(tt.str()); + + std::string messgae = "Time: " + t + " Altitude: " + s; + comm_link_->log(messgae); } virtual IOffboardApi& offboardApi() override From fda8c9d1c02510c5b4ee718550d8368fed4593a9 Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Wed, 10 Nov 2021 13:02:22 +0100 Subject: [PATCH 09/87] update SimpleFlightTest.hpp --- .gitignore | 3 + AirLibUnitTests/SimpleFlightTest.hpp | 91 ++++++++++++++++++++-------- 2 files changed, 68 insertions(+), 26 deletions(-) diff --git a/.gitignore b/.gitignore index 85669e3f14..39ec707e2d 100644 --- a/.gitignore +++ b/.gitignore @@ -387,3 +387,6 @@ build_docs/ # api docs PythonClient/docs/_build + +# custom +log.txt diff --git a/AirLibUnitTests/SimpleFlightTest.hpp b/AirLibUnitTests/SimpleFlightTest.hpp index 90b0943704..305e710f13 100644 --- a/AirLibUnitTests/SimpleFlightTest.hpp +++ b/AirLibUnitTests/SimpleFlightTest.hpp @@ -18,17 +18,31 @@ namespace msr { namespace airlib { + void checkStatusMsg(MultirotorApiBase* api, std::ofstream* myfile) + { + + std::vector messages_; + api->getStatusMessages(messages_); + for (const auto& status_message : messages_) { + *myfile << status_message << std::endl; + // std::cout << status_message << std::endl; + } + messages_.clear(); + } class SimpleFlightTest : public TestBase { public: virtual void run() override { + std::cout << std::endl; auto clock = std::make_shared(3E-3f); ClockFactory::get(clock); SensorFactory sensor_factory; + // added by Suman, from https://github.com/microsoft/AirSim/pull/2558/commits/9c4e59d1a2b371ebc60cdc18f93b06cbe3e9d305 + // loads settings from settings.json or Default setting SettingsLoader settings_loader; std::unique_ptr params = MultiRotorParamsFactory::createConfig( @@ -36,6 +50,7 @@ namespace airlib std::make_shared()); auto api = params->createMultirotorApi(); + // create and initialize kinematics and environment std::unique_ptr kinematics; std::unique_ptr environment; Kinematics::State initial_kinematic_state = Kinematics::State::zero(); @@ -48,7 +63,8 @@ namespace airlib initial_environment.geo_point = GeoPoint(); environment.reset(new Environment(initial_environment)); - MultiRotorPhysicsBody vehicle(params.get(), api.get(), kinematics.get(), environment.get()); + // crete and initialize body and physics world + MultiRotorPhysicsBody vehicle(params.get(), api.get(), kinematics.get(), environment.get()); std::vector vehicles = { &vehicle }; std::unique_ptr physics_engine(new FastPhysicsEngine()); @@ -56,16 +72,19 @@ namespace airlib // world.startAsyncUpdator(); called in the physics_world constructor // added by Suman, to fix calling update before reset https://github.com/microsoft/AirSim/issues/2773#issuecomment-703888477 - // TODO not sure if it should be here? see wrt to PawnSimApi - api->setSimulatedGroundTruth(&kinematics->getState(), environment.get()); + // TODO not sure if it should be here? see wrt to PawnSimApi, no side effects so far + api->setSimulatedGroundTruth(&kinematics.get()->getState(), environment.get()); api->reset(); kinematics->reset(); - StateReporter reporter; - kinematics->reportState(reporter); // this writes the kinematics in reporter - std::cout << reporter.getOutput() << std::endl; + // set the vehicle as grounded, otherwise can not take off, needs to to be done after physics world construction! + vehicle.setGrounded(true); + // read intitial position + Vector3r pos = api->getMultirotorState().getPosition(); + std::cout << "starting position: " << pos << std::endl; + // test api if not null testAssert(api != nullptr, "api was null"); std::string message; testAssert(api->isReady(message), message); @@ -74,36 +93,56 @@ namespace airlib Utils::getSetMinLogLevel(true, 100); + std::ostringstream ss; + + std::ofstream myfile; + myfile.open("log.txt"); + myfile << "Writing this to a file.\n"; + + + // enable api control api->enableApiControl(true); + //checkStatusMsg(api.get(), &myfile); + + // arm api->armDisarm(true); - api->takeoff(10); + //checkStatusMsg(api.get(), &myfile); - // Vector3r pos = api->getMultirotorState().getPosition(); - // std::cout << pos << std::endl; + // take off + api->takeoff(50); + pos = api->getMultirotorState().getPosition(); + std::cout << "took-off position: " << pos << std::endl; + //checkStatusMsg(api.get(), &myfile); - kinematics->reportState(reporter); // this writes the kinematics in reporter - std::cout << reporter.getOutput() << std::endl; + clock->sleep_for(2.0f); - // TODO print some values OR log + // fly towards a waypoint + api->moveToPosition(-5, -5, -5, 5, 1E3, DrivetrainType::MaxDegreeOfFreedom, YawMode(true, 0), -1, 0); + pos = api->getMultirotorState().getPosition(); + std::cout << "waypoint position: " << pos << std::endl; + //checkStatusMsg(api.get(), &myfile); - clock->sleep_for(2.0f); + // clock->sleep_for(2.0f); - Utils::getSetMinLogLevel(true); + // land + api->land(10); + pos = api->getMultirotorState().getPosition(); + std::cout << "landed position: " << pos << std::endl; + checkStatusMsg(api.get(), &myfile); - //api->moveToPosition(-5, -5, -5, 5, 1E3, DrivetrainType::MaxDegreeOfFreedom, YawMode(true, 0), -1, 0); - // pos = api->getMultirotorState().getPosition(); - // std::cout << pos << std::endl; + // TODO print some values OR log - clock->sleep_for(2.0f); + // // report states + // std::cout << std::endl; + // StateReporter reporter; + // kinematics->reportState(reporter); // this writes the kinematics in reporter + // std::cout << reporter.getOutput() << std::endl; + + myfile.close(); + + /*while (true) { + }*/ - while (true) { - clock->sleep_for(0.1f); - api->getStatusMessages(messages_); - for (const auto& status_message : messages_) { - std::cout << status_message << std::endl; - } - messages_.clear(); - } } private: From fda73120299eab6203af71f215cfea45b7505630 Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Wed, 10 Nov 2021 22:23:30 +0100 Subject: [PATCH 10/87] fix ekf declaration --- .../firmwares/simple_flight/AirSimSimpleEkf.hpp | 11 ++++++----- .../firmwares/simple_flight/AirSimSimpleEkfBase.hpp | 4 ++-- .../firmwares/simple_flight/AirSimSimpleEkfModel.hpp | 10 +++++----- .../firmwares/simple_flight/firmware/Firmware.hpp | 2 +- .../simple_flight/firmware/interfaces/IEkf.hpp | 6 +++++- 5 files changed, 19 insertions(+), 14 deletions(-) diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp index 88a1ede893..d39589ec63 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp @@ -5,9 +5,10 @@ #include #include -#include "interfaces/IBoard.hpp" +#include "firmware/interfaces/IBoard.hpp" #include "common/FrequencyLimiter.hpp" #include "AirSimSimpleEkfBase.hpp" +#include "AirSimSimpleEkfModel.hpp" // #include "firmware/Params.hpp" // #include "common/Common.hpp" @@ -20,7 +21,7 @@ namespace airlib { class AirSimSimpleEkf : public AirSimSimpleEkfBase - , private AirSimSimpleEkfModel + //, private AirSimSimpleEkfModel { public: // Constructor @@ -135,7 +136,7 @@ namespace airlib // the updates at the frequency of barometer signal update - real_T* altitude; + real_T* altitude = nullptr; // check if the barometer gives new measurement and it is valid bool is_valid = getBarometerData(altitude); @@ -154,7 +155,7 @@ namespace airlib // the updates at the frequency of GPS signal update - real_T geo[3]; + double geo[3]; real_T vel[3]; // check if the GPS gives new measurement and it is valid @@ -218,7 +219,7 @@ namespace airlib } // reads GPS data - bool getGpsData(real_T geo[3], + bool getGpsData(double geo[3], real_T vel[3]) { board_->readGpsData(geo, vel); diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfBase.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfBase.hpp index 3c24e7400e..9a0c9b1ef0 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfBase.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfBase.hpp @@ -4,9 +4,9 @@ #define msr_airlib_AirSimSimpleEkfBase_hpp #include -#include "CommonStructs.hpp" +#include "common/CommonStructs.hpp" #include "common/Common.hpp" -#include "IEkf.hpp" +#include "firmware/interfaces/IEkf.hpp" namespace msr { diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfModel.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfModel.hpp index dfad7f7555..8478a75403 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfModel.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfModel.hpp @@ -1,14 +1,14 @@ // Liscence info -#ifndef msr_airlib_AirSimSimpleEkf_hpp -#define msr_airlib_AirSimSimpleEkf_hpp +#ifndef msr_airlib_AirSimSimpleEkfModel_hpp +#define msr_airlib_AirSimSimpleEkfModel_hpp #include #include -#include "interfaces/IUpdatable.hpp" -#include "interfaces/IBoard.hpp" +#include "firmware/interfaces/IUpdatable.hpp" +#include "firmware/interfaces/IBoard.hpp" #include "common/FrequencyLimiter.hpp" -#include "IEkf.hpp" +#include "firmware/interfaces/IEkf.hpp" // #include "firmware/Params.hpp" // #include "common/Common.hpp" 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 c299c8ab30..ace602ef03 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/Firmware.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/Firmware.hpp @@ -12,7 +12,7 @@ #include "Mixer.hpp" #include "CascadeController.hpp" #include "AdaptiveController.hpp" -#include "IEkf.hpp" +#include "interfaces/IEkf.hpp" namespace simple_flight { 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 index 49c07c17a4..9e054bde63 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IEkf.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IEkf.hpp @@ -10,7 +10,11 @@ namespace simple_flight class IEkf : public IUpdatable { - +public: + virtual void update() override + { + IUpdatable::update(); + } }; } //namespace \ No newline at end of file From 0a64ec75ab7428a04fce7a0bf67424cc15a8705f Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Wed, 10 Nov 2021 22:24:57 +0100 Subject: [PATCH 11/87] fix board_ methods' polymorphism --- .../simple_flight/AirSimSimpleFlightBoard.hpp | 26 +++++++++---------- .../firmware/interfaces/IBoardSensors.hpp | 14 ++++++---- 2 files changed, 22 insertions(+), 18 deletions(-) diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightBoard.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightBoard.hpp index c2e2b6608e..bda1c5d953 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightBoard.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightBoard.hpp @@ -130,28 +130,28 @@ namespace airlib //no op for now } - bool checkImuIfNew() const + virtual bool checkImuIfNew() const override { return imu_->checkIfNew(); } - bool checkBarometerIfNew() const + virtual bool checkBarometerIfNew() const override { return barometer_->checkIfNew(); } - bool checkMagnetometerIfNew() const + virtual bool checkMagnetometerIfNew() const override { return magnetometer_->checkIfNew(); } - bool checkGpsIfNew() const + virtual bool checkGpsIfNew() const override { return gps_->checkIfNew(); } // added by Suman - void readImuData(real_T accel[3], real_T gyro[3]) const + 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(); @@ -163,13 +163,13 @@ namespace airlib } // added by Suman - void readBarometerData(real_T* altitude) const + virtual void readBarometerData(real_T* altitude) const override { *altitude = barometer_->getOutput().altitude; } // added by Suman - void readMagnetometerData(real_T mag[3]) const + 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(); @@ -177,7 +177,7 @@ namespace airlib } // added by Suman - void readGpsData(real_T geo[3], real_T vel[3]) const + 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; @@ -185,7 +185,7 @@ namespace airlib vel[0] = gps_->getOutput().gnss.velocity.x(); vel[1] = gps_->getOutput().gnss.velocity.y(); - vel[2] = gps_->getOutput().gnss.velocity.z();; + vel[2] = gps_->getOutput().gnss.velocity.z(); } private: @@ -200,22 +200,22 @@ namespace airlib const std::string& magnetometer_name, const std::string& gps_name) { - auto* imu_ = static_cast(findSensorByName(imu_name, SensorBase::SensorType::Imu)); + imu_ = static_cast(findSensorByName(imu_name, SensorBase::SensorType::Imu)); if (imu_ == nullptr) { //throw VehicleControllerException(Utils::stringf("No IMU with name %s exist on vehicle", imu_name.c_str())); } - auto* barometer_ = static_cast(findSensorByName(barometer_name, SensorBase::SensorType::Barometer)); + barometer_ = static_cast(findSensorByName(barometer_name, SensorBase::SensorType::Barometer)); if (barometer_ == nullptr) { //throw VehicleControllerException(Utils::stringf("No barometer with name %s exist on vehicle", barometer_name.c_str())); } - auto* magnetometer_ = static_cast(findSensorByName(magnetometer_name, SensorBase::SensorType::Magnetometer)); + magnetometer_ = static_cast(findSensorByName(magnetometer_name, SensorBase::SensorType::Magnetometer)); if (magnetometer_ == nullptr) { //throw VehicleControllerException(Utils::stringf("No magnetometer with name %s exist on vehicle", magnetometer_name.c_str())); } - auto* gps_ = static_cast(findSensorByName(gps_name, SensorBase::SensorType::Gps)); + gps_ = static_cast(findSensorByName(gps_name, SensorBase::SensorType::Gps)); if (gps_ == nullptr) { //throw VehicleControllerException(Utils::stringf("No gps with name %s exist on vehicle", gps_name.c_str())); 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 6ff881e8d1..acc3d89e86 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,10 +9,14 @@ 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 - // // added by Suman - // virtual void readImuData(float accel[3], float gyro[3]) const = 0; - // virtual void readBarometerData(float* altitude) const = 0; - // virtual void readMagnetometerData(float mag[3]) const = 0; - // virtual void readGpsData(float geo[3], float vel[3]) const = 0; + // added by Suman + 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; }; } \ No newline at end of file From b42e47533cf4065510ba611699d16597b0f81ef6 Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Wed, 10 Nov 2021 22:25:26 +0100 Subject: [PATCH 12/87] update gitignore to ignore log.txt --- .gitignore | 3 +++ 1 file changed, 3 insertions(+) diff --git a/.gitignore b/.gitignore index 85669e3f14..09322dbc7c 100644 --- a/.gitignore +++ b/.gitignore @@ -387,3 +387,6 @@ build_docs/ # api docs PythonClient/docs/_build + +# ignore log +log.txt From 8b098bd3e00d73f67bb356d1c4d6c11d230ecafb Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Wed, 10 Nov 2021 23:16:14 +0100 Subject: [PATCH 13/87] bug fix, memory leak due to wrong declaration of message --- AirLibUnitTests/SimpleFlightTest.hpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/AirLibUnitTests/SimpleFlightTest.hpp b/AirLibUnitTests/SimpleFlightTest.hpp index 305e710f13..46d73e2f09 100644 --- a/AirLibUnitTests/SimpleFlightTest.hpp +++ b/AirLibUnitTests/SimpleFlightTest.hpp @@ -27,7 +27,6 @@ namespace airlib *myfile << status_message << std::endl; // std::cout << status_message << std::endl; } - messages_.clear(); } class SimpleFlightTest : public TestBase @@ -146,7 +145,7 @@ namespace airlib } private: - std::vector messages_; + //std::vector messages_; }; } } From ee848417c920a05bf699b0dac84c06b00f005966 Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Fri, 12 Nov 2021 09:53:25 +0100 Subject: [PATCH 14/87] Sync environment with kinematics in physics engine ! only for standalone AirSim usage! --- AirLib/include/physics/FastPhysicsEngine.hpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/AirLib/include/physics/FastPhysicsEngine.hpp b/AirLib/include/physics/FastPhysicsEngine.hpp index 39ee9c48f2..28ff62862d 100644 --- a/AirLib/include/physics/FastPhysicsEngine.hpp +++ b/AirLib/include/physics/FastPhysicsEngine.hpp @@ -110,8 +110,9 @@ namespace airlib //TODO: this is now being done in PawnSimApi::update. We need to re-think this sequence //with below commented out - Arducopter GPS may not work. - //body.getEnvironment().setPosition(next.pose.position); - //body.getEnvironment().update(); + //IMPORTANT! The following two lines should be commented out while using PawnSimApi!. It syncs environment from kinematics. + body.getEnvironment().setPosition(next.pose.position); + body.getEnvironment().update(); } static void updateCollisionResponseInfo(const CollisionInfo& collision_info, const Kinematics::State& next, From dec9f8bab96086576cb66cf19601c5b9b6f473a5 Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Fri, 12 Nov 2021 09:55:11 +0100 Subject: [PATCH 15/87] fix array out of bound in board --- .../firmwares/simple_flight/AirSimSimpleFlightBoard.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightBoard.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightBoard.hpp index bda1c5d953..356f436dfc 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightBoard.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightBoard.hpp @@ -155,7 +155,7 @@ namespace airlib { accel[0] = imu_->getOutput().linear_acceleration.x(); accel[1] = imu_->getOutput().linear_acceleration.y(); - accel[3] = imu_->getOutput().linear_acceleration.z(); + accel[2] = imu_->getOutput().linear_acceleration.z(); gyro[0] = imu_->getOutput().angular_velocity.x(); gyro[1] = imu_->getOutput().angular_velocity.y(); @@ -173,7 +173,7 @@ namespace airlib { mag[0] = magnetometer_->getOutput().magnetic_field_body.x(); mag[1] = magnetometer_->getOutput().magnetic_field_body.y(); - mag[2] = magnetometer_->getOutput().magnetic_field_body.z();; + mag[2] = magnetometer_->getOutput().magnetic_field_body.z(); } // added by Suman From 43c69455dbaf0f11743fc6fe240c0628c5b4dac1 Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Fri, 12 Nov 2021 09:55:57 +0100 Subject: [PATCH 16/87] change takeoff altituide for simple_flight --- .../multirotor/firmwares/simple_flight/firmware/Params.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/Params.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/Params.hpp index 3612b207aa..35e15b6703 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/Params.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/Params.hpp @@ -121,7 +121,7 @@ struct Params struct Takeoff { - float takeoff_z = -2.0f; + float takeoff_z = -10.0f; //float velocity = -1.0f; } takeoff; From 0da5d3d8f0d02667f83cd5a31d1b4cac42f7c540 Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Fri, 12 Nov 2021 09:58:34 +0100 Subject: [PATCH 17/87] transfer commlink log from Firmware to OffboardApi --- .../firmwares/simple_flight/firmware/Firmware.hpp | 13 +------------ .../simple_flight/firmware/OffboardApi.hpp | 8 ++++++++ 2 files changed, 9 insertions(+), 12 deletions(-) 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 117c926646..91294756c4 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/Firmware.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/Firmware.hpp @@ -43,6 +43,7 @@ class Firmware : public IFirmware board_->reset(); comm_link_->reset(); + ekf_->reset(); controller_->reset(); offboard_api_.reset(); @@ -78,18 +79,6 @@ class Firmware : public IFirmware comm_link_->update(); - // added by Suman, better to move this to OffboardApi.hpp - auto pos = state_estimator_->getPosition().z(); - - std::ostringstream ss; - ss << pos; - std::string s(ss.str()); - std::ostringstream tt; - tt << board_->millis(); - std::string t(tt.str()); - - std::string messgae = "Time: " + t + " Altitude: " + s; - comm_link_->log(messgae); } virtual IOffboardApi& offboardApi() override 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..e84e62b1db 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/OffboardApi.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/OffboardApi.hpp @@ -59,6 +59,14 @@ class OffboardApi : public IUpdatable detectLanding(); detectTakingOff(); + + // additional logging of vehicle states, TODO implement a separate log! + std::ostringstream alt_str; + alt_str << state_estimator_->getPosition().z()*-1.0; + std::ostringstream time_str; + time_str << clock_->millis(); + std::string messgae = "\nPhysics update: time (ms): " + time_str.str() + " altitude (m): " + alt_str.str(); + comm_link_->log(messgae); } /**************** IOffboardApi ********************/ From 1b304cdc6305121ab60d92702f063a413e4a2c10 Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Fri, 12 Nov 2021 10:00:42 +0100 Subject: [PATCH 18/87] make changes to ekf skeleton, log, and test: compiles and updates ekf functions correctly --- .../simple_flight/AirSimSimpleEkf.hpp | 49 ++++++++++++++----- .../simple_flight/AirSimSimpleEkfModel.hpp | 2 +- .../simple_flight/SimpleFlightApi.hpp | 2 +- AirLibUnitTests/SimpleFlightTest.hpp | 16 +++--- 4 files changed, 48 insertions(+), 21 deletions(-) diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp index d39589ec63..6045810c77 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp @@ -6,14 +6,10 @@ #include #include #include "firmware/interfaces/IBoard.hpp" +#include "firmware/interfaces/ICommLink.hpp" #include "common/FrequencyLimiter.hpp" #include "AirSimSimpleEkfBase.hpp" -#include "AirSimSimpleEkfModel.hpp" - -// #include "firmware/Params.hpp" -// #include "common/Common.hpp" -// #include "common/ClockFactory.hpp" -// #include "physics/Kinematics.hpp" +#include "AirSimSimpleEkfModel.hpp" namespace msr { @@ -21,20 +17,28 @@ namespace airlib { class AirSimSimpleEkf : public AirSimSimpleEkfBase - //, private AirSimSimpleEkfModel + , private AirSimSimpleEkfModel { public: // Constructor - AirSimSimpleEkf(const simple_flight::IBoard* board)//, const AirSimSettings::EKFSetting& setting = AirSimSettings::EKFSetting()) - : board_(board) + AirSimSimpleEkf(const simple_flight::IBoard* board, simple_flight::ICommLink* comm_link) //, const AirSimSettings::EKFSetting& setting = AirSimSettings::EKFSetting()) + : board_(board), comm_link_(comm_link) // commlink is only temporary here { // params_ = // get the EKF params from airsim settings. Implement this as a struct for now with constant members ! inspired by sensor model - freq_limiter_.initialize(100); // physics engine and the imu refresh at period 3m ~ 333.33Hz. EKF tops at frequency 100Hz + freq_limiter_.initialize(334); // physics engine and the imu refresh at period 3ms ~ 333.33Hz + } + + virtual void reset() override + { + IEkf::reset(); + + freq_limiter_.reset(); + } virtual void update() override { - IUpdatable::update(); + IEkf::update(); // update the frequency limiter freq_limiter_.update(); @@ -82,6 +86,11 @@ namespace airlib statePropagation(); covariancePropagation(); + + std::ostringstream imu_str; + imu_str << accel[2]; + std::string messgae = " prediction step / imu step called (z-accelerometer) " + imu_str.str(); + comm_link_->log(messgae); } // measurement update step @@ -126,6 +135,11 @@ namespace airlib // auto C = dh_mag_dx(); // auto K = ... + + std::ostringstream mag_str; + mag_str << mag[0]; + std::string messgae = " magnetometer step called (x-magnetic flux) " + mag_str.str(); + comm_link_->log(messgae); } // barometer update @@ -136,7 +150,7 @@ namespace airlib // the updates at the frequency of barometer signal update - real_T* altitude = nullptr; + real_T altitude[1]; // check if the barometer gives new measurement and it is valid bool is_valid = getBarometerData(altitude); @@ -145,6 +159,11 @@ namespace airlib { return; } + + std::ostringstream alt_str; + alt_str << altitude[0]; + std::string messgae = " barometer step called (baro altitude) " + alt_str.str(); + comm_link_->log(messgae); } // GPS update @@ -165,6 +184,11 @@ namespace airlib { return; } + + std::ostringstream gps_str; + gps_str << geo[2]; + std::string messgae = " gps step called (gps altitude) " + gps_str.str(); + comm_link_->log(messgae); } void calculatePhi() @@ -241,6 +265,7 @@ namespace airlib FrequencyLimiter freq_limiter_; // the flight board instance const simple_flight::IBoard* board_; + simple_flight::ICommLink* comm_link_; // commlink is only temporary here, later transfer logging to OffBoardApi via StateEstimator }; diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfModel.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfModel.hpp index 8478a75403..f1f4f092c7 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfModel.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfModel.hpp @@ -20,7 +20,7 @@ namespace msr namespace airlib { - class AirSimSimpleEkfModel : public simple_flight::IEkf + class AirSimSimpleEkfModel { protected: // --------------------------------------------------------------------- diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightApi.hpp index 9baebd9686..cd08f4b0db 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightApi.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightApi.hpp @@ -44,7 +44,7 @@ namespace airlib estimator_.reset(new AirSimSimpleFlightEstimator()); // added by Suman - ekf_.reset(new AirSimSimpleEkf(board_.get())); + ekf_.reset(new AirSimSimpleEkf(board_.get(), comm_link_.get())); //create firmware firmware_.reset(new simple_flight::Firmware(¶ms_, board_.get(), comm_link_.get(), estimator_.get(), ekf_.get())); diff --git a/AirLibUnitTests/SimpleFlightTest.hpp b/AirLibUnitTests/SimpleFlightTest.hpp index 46d73e2f09..90ca558144 100644 --- a/AirLibUnitTests/SimpleFlightTest.hpp +++ b/AirLibUnitTests/SimpleFlightTest.hpp @@ -96,7 +96,9 @@ namespace airlib std::ofstream myfile; myfile.open("log.txt"); - myfile << "Writing this to a file.\n"; + myfile << ">> Physics update frequency: 333.33 Hz.\n"; + myfile << ">> Barometer and magnetometer update frequency: 50 Hz.\n"; + myfile << ">> GPS update frequency: 50 Hz with startup delay.\n\n"; // enable api control @@ -113,20 +115,20 @@ namespace airlib std::cout << "took-off position: " << pos << std::endl; //checkStatusMsg(api.get(), &myfile); - clock->sleep_for(2.0f); + clock->sleep_for(10.0f); // fly towards a waypoint - api->moveToPosition(-5, -5, -5, 5, 1E3, DrivetrainType::MaxDegreeOfFreedom, YawMode(true, 0), -1, 0); - pos = api->getMultirotorState().getPosition(); - std::cout << "waypoint position: " << pos << std::endl; + //api->moveToPosition(-50, -50, -50, 50, 1E3, DrivetrainType::MaxDegreeOfFreedom, YawMode(true, 0), -1, 0); + //pos = api->getMultirotorState().getPosition(); + //std::cout << "waypoint position: " << pos << std::endl; //checkStatusMsg(api.get(), &myfile); // clock->sleep_for(2.0f); // land - api->land(10); + /*api->land(10); pos = api->getMultirotorState().getPosition(); - std::cout << "landed position: " << pos << std::endl; + std::cout << "landed position: " << pos << std::endl;*/ checkStatusMsg(api.get(), &myfile); // TODO print some values OR log From 610de43c11574c5abdff99dc4b1f962cd9a2af19 Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Tue, 16 Nov 2021 22:49:41 +0100 Subject: [PATCH 19/87] add ekf_ object as member of state_estimator_ --- .../AirSimSimpleFlightEstimator.hpp | 20 ++++++++++++++++++- .../simple_flight/SimpleFlightApi.hpp | 5 +++-- .../firmware/interfaces/IStateEstimator.hpp | 2 ++ 3 files changed, 24 insertions(+), 3 deletions(-) diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightEstimator.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightEstimator.hpp index 8e2f0a0c30..4a01da40c9 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightEstimator.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightEstimator.hpp @@ -18,7 +18,13 @@ namespace airlib class AirSimSimpleFlightEstimator : public simple_flight::IStateEstimator { public: - virtual ~AirSimSimpleFlightEstimator() {} + AirSimSimpleFlightEstimator(simple_flight::IEkf* ekf) + : ekf_(ekf) + { + + } + + virtual ~AirSimSimpleFlightEstimator(){} //for now we don't do any state estimation and use ground truth (i.e. assume perfect sensors) void setGroundTruthKinematics(const Kinematics::State* kinematics, const Environment* environment) @@ -97,9 +103,21 @@ namespace airlib return state; } + virtual simple_flight::Axis3r getEkfPostion() const override + { + simple_flight::Axis3r position; + auto ekf_states = ekf_->getEkfStates(); + position[0] = ekf_states[0]; + position[1] = ekf_states[1]; + position[2] = ekf_states[2]; + + return position; + } + private: const Kinematics::State* kinematics_; const Environment* environment_; + const simple_flight::IEkf* ekf_; }; } } //namespace diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightApi.hpp index cd08f4b0db..20e2a5b7f5 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightApi.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightApi.hpp @@ -41,10 +41,11 @@ namespace airlib //create sim implementations of board and commlink board_.reset(new AirSimSimpleFlightBoard(¶ms_, vehicle_params_)); comm_link_.reset(new AirSimSimpleFlightCommLink()); - estimator_.reset(new AirSimSimpleFlightEstimator()); - + // added by Suman ekf_.reset(new AirSimSimpleEkf(board_.get(), comm_link_.get())); + estimator_.reset(new AirSimSimpleFlightEstimator(ekf_.get())); + //create firmware firmware_.reset(new simple_flight::Firmware(¶ms_, board_.get(), comm_link_.get(), estimator_.get(), ekf_.get())); 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 9b63110aba..48ac9b9eff 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 @@ -20,5 +20,7 @@ class IStateEstimator virtual GeoPoint getHomeGeoPoint() const = 0; virtual Axis3r transformToBodyFrame(const Axis3r& world_frame_val) const = 0; + + virtual simple_flight::Axis3r getEkfPostion() const = 0; }; } \ No newline at end of file From dc097ca70117cbfeed96022ec3b589716fe6d616 Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Tue, 16 Nov 2021 22:51:31 +0100 Subject: [PATCH 20/87] change logging in OffBoardApi and log ekf position there --- .../simple_flight/firmware/OffboardApi.hpp | 20 ++++++++++++------- 1 file changed, 13 insertions(+), 7 deletions(-) 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 e84e62b1db..f7dea4b9a1 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/OffboardApi.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/OffboardApi.hpp @@ -60,13 +60,7 @@ class OffboardApi : public IUpdatable detectLanding(); detectTakingOff(); - // additional logging of vehicle states, TODO implement a separate log! - std::ostringstream alt_str; - alt_str << state_estimator_->getPosition().z()*-1.0; - std::ostringstream time_str; - time_str << clock_->millis(); - std::string messgae = "\nPhysics update: time (ms): " + time_str.str() + " altitude (m): " + alt_str.str(); - comm_link_->log(messgae); + logEkfStates(); } /**************** IOffboardApi ********************/ @@ -211,6 +205,18 @@ class OffboardApi : public IUpdatable } private: + + void logEkfStates() + { + // additional logging of vehicle states, TODO implement a separate log! + std::ostringstream log_msg; + log_msg << clock_->millis() << '\t' + << state_estimator_->getPosition().z() << '\t' + << state_estimator_->getEkfPostion().z() << '\t'; + std::string message = log_msg.str(); + comm_link_->log(message); + } + void updateGoalFromRc() { goal_ = rc_.getGoalValue(); From 9d2fdd068154d3fc8e9cae99c8e7b12a3c42cc45 Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Tue, 16 Nov 2021 22:54:55 +0100 Subject: [PATCH 21/87] transport member object and get/setters from Base to IEkf --- .../simple_flight/AirSimSimpleEkfBase.hpp | 18 ++++-------------- .../simple_flight/firmware/interfaces/IEkf.hpp | 15 +++++++++++++++ 2 files changed, 19 insertions(+), 14 deletions(-) diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfBase.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfBase.hpp index 9a0c9b1ef0..1bb80bb05e 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfBase.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfBase.hpp @@ -15,25 +15,15 @@ namespace airlib class AirSimSimpleEkfBase : public simple_flight::IEkf { -public: - // setters - void setEkfStates() - { - } - - // getters - const VectorMath::EkfStates& getEkfStates() const - { - } - - -private: +protected: // EKF states - VectorMath::EkfStates states_; + // VectorMath::EkfStates states_; // EKF covariances VectorMath::EkfCovariance covariance_; + // timestamps + TTimePoint last_statePropagation_time_; }; } 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 index 9e054bde63..b1e5e32175 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IEkf.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IEkf.hpp @@ -15,6 +15,21 @@ class IEkf : public IUpdatable { IUpdatable::update(); } + + // setters + void setEkfStates(VectorMath::EkfStates states) + { + states_ = states; + } + + // getters + const VectorMath::EkfStates& getEkfStates() const + { + return states_; + } + +protected: + VectorMath::EkfStates states_; }; } //namespace \ No newline at end of file From 68afd59301e48e5b266671db53219972d0d2aa52 Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Tue, 16 Nov 2021 22:57:02 +0100 Subject: [PATCH 22/87] implement evaluateStateDot function in ekf --- .../simple_flight/AirSimSimpleEkfModel.hpp | 103 +++++++++++++++++- 1 file changed, 98 insertions(+), 5 deletions(-) diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfModel.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfModel.hpp index f1f4f092c7..b8a454d600 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfModel.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfModel.hpp @@ -13,7 +13,10 @@ // #include "firmware/Params.hpp" // #include "common/Common.hpp" // #include "common/ClockFactory.hpp" -// #include "physics/Kinematics.hpp" +// #include "physics/Kinematics.hpp" + +constexpr float G_0 = 9.81f; +constexpr float R_E = 6357000.0f; namespace msr { @@ -27,9 +30,57 @@ namespace airlib // Mathematical functions // --------------------------------------------------------------------- - void f() + void evaluateStateDot(float f_local[10], float x[17], float u[6]) { - // the state_dot vector + // compute x_dot = f_local + /* + x[0] = x + x[1] = y + x[2] = z + x[3] = u + x[4] = v + x[5] = w + x[6] = q0 + x[7] = q1 + x[8] = q2 + x[9] = q3 + x[10] = bias_accel_x + x[11] = bias_accel_y + x[12] = bias_accel_z + x[13] = bias_gyro_x + x[14] = bias_gyro_x + x[15] = bias_gyro_x + x[16] = bias_baro + + u[0] = accel_x + u[1] = accel_y + u[2] = accel_z + u[3] = gyro_x + u[4] = gyro_y + u[5] = gyro_z + */ + + // f_pos + f_local[0] = x[3]; + f_local[1] = x[4]; + f_local[2] = x[5]; + // f_vel + f_local[3] = (x[6]*x[6] + x[7]*x[7] - x[8]*x[8] - x[9]*x[9])* (u[0] + x[10]) + + 2*(x[7]*x[8] - x[6]*x[9])* (u[1] + x[11]) + + 2*(x[6]*x[8] + x[7]*x[9])* (u[2] + x[12]); + f_local[4] = 2*(x[7]*x[8] + x[6]*x[9])* (u[0] + x[10]) + + (x[6]*x[6] - x[7]*x[7] + x[8]*x[8] - x[9]*x[9])* (u[1] + x[11]) + + 2*(x[8]*x[9] - x[6]*x[7])* (u[2] + x[12]); + f_local[5] = 2*(x[7]*x[9] - x[6]*x[8])* (u[0] + x[10]) + + 2*(x[8]*x[9] + x[6]*x[7])* (u[1] + x[11]) + + (x[6]*x[6] - x[7]*x[7] - x[8]*x[8] + x[9]*x[9])* (u[2] + x[12]) + + G_0*(1 + 2*x[2]/R_E); + // f_ori + f_local[6] = 0.5f * (-x[7]*(u[3] + x[13]) - x[8]*(u[4] + x[14]) - x[9]*(u[5] + x[15])); + f_local[7] = 0.5f * ( x[6]*(u[3] + x[13]) - x[9]*(u[4] + x[14]) - x[8]*(u[5] + x[15])); + f_local[8] = 0.5f * ( x[9]*(u[3] + x[13]) + x[6]*(u[4] + x[14]) - x[7]*(u[5] + x[15])); + f_local[9] = 0.5f * (-x[8]*(u[3] + x[13]) + x[7]*(u[4] + x[14]) + x[6]*(u[5] + x[15])); + } void h_mag() @@ -51,9 +102,51 @@ namespace airlib // Mathematical jacobians // --------------------------------------------------------------------- - void df_dx() + void df_dx(float A[17][17], float x[17], float u[6]) { - // jacobian wrt the states vector + // compute jacobian wrt the states vector df_dx = A + /* + x[0] = x + x[1] = y + x[2] = z + x[3] = u + x[4] = v + x[5] = w + x[6] = q0 + x[7] = q1 + x[8] = q2 + x[9] = q3 + x[10] = bias_accel_x + x[11] = bias_accel_y + x[12] = bias_accel_z + x[13] = bias_gyro_x + x[14] = bias_gyro_x + x[15] = bias_gyro_x + x[16] = bias_baro + + u[0] = accel_x + u[1] = accel_y + u[2] = accel_z + u[3] = gyro_x + u[4] = gyro_y + u[5] = gyro_z + */ + + // set all elements to zero + float A[17][17]; + memset(&A[0][0], 0, sizeof(A)); + + // 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*G_0/R_E; + // df_vel_q0 [3 4 5] + A[3][6] = 2*x[6](u[0] + x[10]) - 2*x[6](u[0] + x[10]) + 2*x[6](u[0] + x[10]) + + + } void df_dw() From 62a25d706576f8c26b899f8237a0c19cafb84346 Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Tue, 16 Nov 2021 22:58:55 +0100 Subject: [PATCH 23/87] implement statePropagation and initialization, remove logging in ekf --- .../simple_flight/AirSimSimpleEkf.hpp | 93 +++++++++++++++---- 1 file changed, 74 insertions(+), 19 deletions(-) diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp index 6045810c77..973e8af7b4 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp @@ -33,6 +33,7 @@ namespace airlib IEkf::reset(); freq_limiter_.reset(); + initialize(); } @@ -54,7 +55,15 @@ namespace airlib // --------------------------------------------------------------------- // Internal functions // --------------------------------------------------------------------- - + // initialize filter + void initialize() + { + // intiaalize with zero position and unity quaternion ! temporary + states_ = VectorMath::EkfStates::Zero(); + states_[6] = 1.0f; + last_statePropagation_time_ = board_->micros(); + } + // this function updates at the frequency of EKF update void updateEKFInternal() { @@ -84,13 +93,28 @@ namespace airlib return; } - statePropagation(); + statePropagation(accel, gyro); covariancePropagation(); - std::ostringstream imu_str; + /*std::ostringstream imu_str; imu_str << accel[2]; std::string messgae = " prediction step / imu step called (z-accelerometer) " + imu_str.str(); - comm_link_->log(messgae); + comm_link_->log(messgae);*/ + + // std::ostringstream prediction_str; + // prediction_str << "Prediction: " << states_[0] + // << '\t' << states_[1] + // << '\t' << states_[2] + // << '\t' << states_[3] + // << '\t' << states_[4] + // << '\t' << states_[5] + // << '\t' << states_[6] + // << '\t' << states_[7] + // << '\t' << states_[8] + // << '\t' << states_[9] + // << '\t' ; + // std::string messgae = prediction_str.str(); + // comm_link_->log(messgae); } // measurement update step @@ -102,10 +126,41 @@ namespace airlib } // state propagtion - void statePropagation() + void statePropagation(real_T* accel, real_T* gyro) { - // auto Phi = calculatePhi(); - // auto Gamma_w = calculateGamma_w(); + float x[17]; + float x_dot[10]; + float u[6]; + + // extract the states + for (int i=0; i<17; i++){ + x[i] = states_[i]; + } + + // extract the controls + for (int i=0; i<3; i++){ + u[i] = accel[i]; + u[i+3] = gyro[i]; + } + + evaluateStateDot(x_dot,x,u); + + TTimeDelta dt = (board_->micros() - last_statePropagation_time_) / 1.0E6; // in seconds + last_statePropagation_time_ = board_->micros(); + + // euler forward integration + float x_predicted[17]; + for (int i=0; i<10; i++){ + x_predicted[i] = x[i] + static_cast(dt*x_dot[i]); + } + for (int i=10; i<17; i++){ + x_predicted[i] = x[i]; + } + + // set the predicted states TODO: via an interface or after some checks + for (int i=0; i<17; i++){ + states_[i] = x_predicted[i]; + } } // co-variance propagtion @@ -136,10 +191,10 @@ namespace airlib // auto K = ... - std::ostringstream mag_str; - mag_str << mag[0]; - std::string messgae = " magnetometer step called (x-magnetic flux) " + mag_str.str(); - comm_link_->log(messgae); + // std::ostringstream mag_str; + // mag_str << mag[0]; + // std::string messgae = " magnetometer step called (x-magnetic flux) " + mag_str.str(); + // comm_link_->log(messgae); } // barometer update @@ -160,10 +215,10 @@ namespace airlib return; } - std::ostringstream alt_str; - alt_str << altitude[0]; - std::string messgae = " barometer step called (baro altitude) " + alt_str.str(); - comm_link_->log(messgae); + // std::ostringstream alt_str; + // alt_str << altitude[0]; + // std::string messgae = " barometer step called (baro altitude) " + alt_str.str(); + // comm_link_->log(messgae); } // GPS update @@ -185,10 +240,10 @@ namespace airlib return; } - std::ostringstream gps_str; - gps_str << geo[2]; - std::string messgae = " gps step called (gps altitude) " + gps_str.str(); - comm_link_->log(messgae); + // std::ostringstream gps_str; + // gps_str << geo[2]; + // std::string messgae = " gps step called (gps altitude) " + gps_str.str(); + // comm_link_->log(messgae); } void calculatePhi() From 80093b806f71d9fb4271dc49139fd77401ec6f8c Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Tue, 16 Nov 2021 23:00:24 +0100 Subject: [PATCH 24/87] running change in SimpleFlighttest.hpp, can run and log statePropagation in ekf --- AirLibUnitTests/SimpleFlightTest.hpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/AirLibUnitTests/SimpleFlightTest.hpp b/AirLibUnitTests/SimpleFlightTest.hpp index 90ca558144..fe5517515b 100644 --- a/AirLibUnitTests/SimpleFlightTest.hpp +++ b/AirLibUnitTests/SimpleFlightTest.hpp @@ -96,11 +96,11 @@ namespace airlib std::ofstream myfile; myfile.open("log.txt"); - myfile << ">> Physics update frequency: 333.33 Hz.\n"; + /*myfile << ">> Physics update frequency: 333.33 Hz.\n"; myfile << ">> Barometer and magnetometer update frequency: 50 Hz.\n"; - myfile << ">> GPS update frequency: 50 Hz with startup delay.\n\n"; + myfile << ">> GPS update frequency: 50 Hz with startup delay.\n\n";*/ + myfile << ">> timestamp (ms) \t GroundTruth altitude \t Estimated postiion (x,y,z) \n\n"; - // enable api control api->enableApiControl(true); //checkStatusMsg(api.get(), &myfile); @@ -115,7 +115,7 @@ namespace airlib std::cout << "took-off position: " << pos << std::endl; //checkStatusMsg(api.get(), &myfile); - clock->sleep_for(10.0f); + clock->sleep_for(20.0f); // fly towards a waypoint //api->moveToPosition(-50, -50, -50, 50, 1E3, DrivetrainType::MaxDegreeOfFreedom, YawMode(true, 0), -1, 0); @@ -126,9 +126,9 @@ namespace airlib // clock->sleep_for(2.0f); // land - /*api->land(10); + //api->land(10); pos = api->getMultirotorState().getPosition(); - std::cout << "landed position: " << pos << std::endl;*/ + std::cout << "final position: " << pos << std::endl; checkStatusMsg(api.get(), &myfile); // TODO print some values OR log From 1f2f45ea6d82ed5a2571c091fc7bae52c965d456 Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Fri, 19 Nov 2021 16:33:44 +0100 Subject: [PATCH 25/87] change and append Eigen matrix definitions --- AirLib/include/common/VectorMath.hpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/AirLib/include/common/VectorMath.hpp b/AirLib/include/common/VectorMath.hpp index 382a092e58..87783177e2 100644 --- a/AirLib/include/common/VectorMath.hpp +++ b/AirLib/include/common/VectorMath.hpp @@ -39,8 +39,10 @@ namespace airlib typedef Eigen::Matrix Matrix3x3f; // added by Suman - typedef Eigen::Matrix EkfStates; - typedef Eigen::Matrix EkfCovariance; + typedef Eigen::Matrix Vector17f; + typedef Eigen::Matrix Matrix17x17f; + typedef Eigen::Matrix Matrix17x13f; + typedef Eigen::Matrix Matrix13x13f; typedef Eigen::AngleAxisd AngleAxisd; typedef Eigen::AngleAxisf AngleAxisf; From e547bda3a4f91fda9c58601132fc58ba7d3cf405 Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Fri, 19 Nov 2021 16:36:10 +0100 Subject: [PATCH 26/87] change log in OffBoardApi.hpp --- .../firmwares/simple_flight/firmware/OffboardApi.hpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) 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 f7dea4b9a1..28c0273327 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/OffboardApi.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/OffboardApi.hpp @@ -210,9 +210,16 @@ class OffboardApi : public IUpdatable { // additional logging of vehicle states, TODO implement a separate log! std::ostringstream log_msg; - log_msg << clock_->millis() << '\t' + log_msg << clock_->millis() << '\t' + << state_estimator_->getPosition().x() << '\t' + << state_estimator_->getPosition().y() << '\t' << state_estimator_->getPosition().z() << '\t' - << state_estimator_->getEkfPostion().z() << '\t'; + << state_estimator_->getEkfPostion().x() << '\t' + << state_estimator_->getEkfPostion().y() << '\t' + << state_estimator_->getEkfPostion().z() << '\t' + << state_estimator_->getEkfPositionCovariance().x() << '\t' + << state_estimator_->getEkfPositionCovariance().y() << '\t' + << state_estimator_->getEkfPositionCovariance().z() << '\t'; std::string message = log_msg.str(); comm_link_->log(message); } From 2af227056b1156fa1df391092b8153a11473b417 Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Fri, 19 Nov 2021 16:40:22 +0100 Subject: [PATCH 27/87] add ekf position interface in flight estimator --- .../AirSimSimpleFlightEstimator.hpp | 48 +++++++++++++++++-- .../firmware/interfaces/IStateEstimator.hpp | 3 ++ 2 files changed, 48 insertions(+), 3 deletions(-) diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightEstimator.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightEstimator.hpp index 4a01da40c9..80a819f611 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightEstimator.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightEstimator.hpp @@ -107,13 +107,55 @@ namespace airlib { simple_flight::Axis3r position; auto ekf_states = ekf_->getEkfStates(); - position[0] = ekf_states[0]; - position[1] = ekf_states[1]; - position[2] = ekf_states[2]; + position.x() = ekf_states[0]; + position.y() = ekf_states[1]; + position.z() = ekf_states[2]; return position; } + virtual simple_flight::Axis3r getEkfAngles() const override + { + simple_flight::Axis3r angles; + Quaternionr orientation; + + auto ekf_states = ekf_->getEkfStates(); + orientation.x() = ekf_states[6]; + orientation.y() = ekf_states[7]; + orientation.z() = ekf_states[8]; + orientation.w() = ekf_states[9]; + + VectorMath::toEulerianAngle(orientation, + angles.pitch(), + angles.roll(), + angles.yaw()); + + return angles; + } + + virtual simple_flight::Axis3r getEkfPositionCovariance() const override + { + simple_flight::Axis3r position_cov; + auto ekf_covariance = ekf_->getEkfCovariance(); + position_cov.x() = ekf_covariance(0, 0); + position_cov.y() = ekf_covariance(1, 1); + position_cov.z() = ekf_covariance(2, 2); + + return position_cov; + } + + virtual simple_flight::Axis3r getEkfAngleCovariance() const override + { + simple_flight::Axis4r angle_cov; + auto ekf_covariance = ekf_->getEkfCovariance(); + angle_cov.x() = ekf_covariance(6, 6); + angle_cov.y() = ekf_covariance(7, 7); + angle_cov.z() = ekf_covariance(8, 8); + angle_cov.val4()= ekf_covariance(9, 9); + + return angle_cov; + } + private: const Kinematics::State* kinematics_; const Environment* environment_; 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 48ac9b9eff..76d85d5c25 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 @@ -22,5 +22,8 @@ class IStateEstimator virtual Axis3r transformToBodyFrame(const Axis3r& world_frame_val) const = 0; virtual simple_flight::Axis3r getEkfPostion() const = 0; + virtual simple_flight::Axis3r getEkfAngles() const = 0; + virtual simple_flight::Axis3r getEkfPositionCovariance() const = 0; + virtual simple_flight::Axis3r getEkfAngleCovariance() const = 0; }; } \ No newline at end of file From 9af2f543bd7660db2c1585fd64f7231a92932d02 Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Fri, 19 Nov 2021 16:43:30 +0100 Subject: [PATCH 28/87] add bash and python scripts to cleanup and plot log --- AirLibUnitTests/cleanup.sh | 8 ++++++++ AirLibUnitTests/plot.py | 40 ++++++++++++++++++++++++++++++++++++++ 2 files changed, 48 insertions(+) create mode 100644 AirLibUnitTests/cleanup.sh create mode 100644 AirLibUnitTests/plot.py diff --git a/AirLibUnitTests/cleanup.sh b/AirLibUnitTests/cleanup.sh new file mode 100644 index 0000000000..38d3d5bf50 --- /dev/null +++ b/AirLibUnitTests/cleanup.sh @@ -0,0 +1,8 @@ +#!/bin/bash +cleanup(){ + in_file="$1" + data_file="$2" + log_file="$3" + cat "$in_file" | sed -E '/^[^0-9]*$/d' > "$data_file" + cat "$in_file" | sed -E '/^[0-9]+.*$/d' > "$log_file" +} diff --git a/AirLibUnitTests/plot.py b/AirLibUnitTests/plot.py new file mode 100644 index 0000000000..3ef94b7eb3 --- /dev/null +++ b/AirLibUnitTests/plot.py @@ -0,0 +1,40 @@ +#!/usr/bin/env python3 +""" +Module Docstring +""" + +__author__ = "Suman Subedi" +__version__ = "0.1.0" +__license__ = "MIT" + +import argparse +import numpy as np +import matplotlib as mpl +import matplotlib.pyplot as plt + +def main(): + """ Main entry point of the app """ + + print("hello world") + parser = argparse.ArgumentParser() + parser.add_argument("--filename", help="filename of the txt file") + args = parser.parse_args() + + print(args.filename) + + with open('log.txt', 'rb') as raw_data: + data = np.loadtxt(raw_data, delimiter='\t') + + X = np.linspace(0, 2*np.pi, 100) + Y = np.cos(X) + + fig, ax = plt.subplots() + ax.plot(X, Y, color='C1') + + fig.savefig("figure.pdf") + fig.show() + + +if __name__ == "__main__": + """ This is executed when run from the command line """ + main() \ No newline at end of file From 4a536dad4469e8b8d7b315fccf4d67a8a94f5f01 Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Fri, 19 Nov 2021 16:45:29 +0100 Subject: [PATCH 29/87] implement covariance propagration in EKF --- .../simple_flight/AirSimSimpleEkf.hpp | 82 ++++-- .../simple_flight/AirSimSimpleEkfBase.hpp | 10 +- .../simple_flight/AirSimSimpleEkfModel.hpp | 247 ++++++++++++++++-- .../firmware/interfaces/IEkf.hpp | 20 +- 4 files changed, 315 insertions(+), 44 deletions(-) diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp index 973e8af7b4..98a207cf6b 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp @@ -58,10 +58,21 @@ namespace airlib // initialize filter void initialize() { - // intiaalize with zero position and unity quaternion ! temporary - states_ = VectorMath::EkfStates::Zero(); + // intiaalize with zero position and unity quaternion ! temporary | TODO do using setters + states_ = VectorMath::Vector17f::Zero(); states_[6] = 1.0f; - last_statePropagation_time_ = board_->micros(); + covariance_ = VectorMath::Matrix17x17f::Identity()*0.01f; + //covariance_(2,2) = 1.0f; + Q_ = VectorMath::Matrix13x13f::Identity()*0.01f; + Q_(0, 0) = 0.25f; + Q_(1, 1) = 0.25f; + Q_(2, 2) = 0.25f; + Q_(3, 3) = 0.25f; + Q_(4, 4) = 0.25f; + Q_(5, 5) = 0.25f; + + last_times_.state_propagation = board_->micros(); + last_times_.cov_propagation = board_->micros(); } // this function updates at the frequency of EKF update @@ -94,7 +105,7 @@ namespace airlib } statePropagation(accel, gyro); - covariancePropagation(); + covariancePropagation(accel, gyro); /*std::ostringstream imu_str; imu_str << accel[2]; @@ -128,8 +139,8 @@ namespace airlib // state propagtion void statePropagation(real_T* accel, real_T* gyro) { - float x[17]; float x_dot[10]; + float x[17]; float u[6]; // extract the states @@ -145,13 +156,13 @@ namespace airlib evaluateStateDot(x_dot,x,u); - TTimeDelta dt = (board_->micros() - last_statePropagation_time_) / 1.0E6; // in seconds - last_statePropagation_time_ = board_->micros(); + TTimeDelta delta_T = (board_->micros() - last_times_.state_propagation) / 1.0E6; // in seconds + last_times_.state_propagation = board_->micros(); // euler forward integration float x_predicted[17]; for (int i=0; i<10; i++){ - x_predicted[i] = x[i] + static_cast(dt*x_dot[i]); + x_predicted[i] = x[i] + static_cast(delta_T*x_dot[i]); } for (int i=10; i<17; i++){ x_predicted[i] = x[i]; @@ -164,10 +175,40 @@ namespace airlib } // co-variance propagtion - void covariancePropagation() + void covariancePropagation(real_T* accel, real_T* gyro) { - // auto Phi = calculatePhi(); - // auto Gamma_w = calculateGamma_w(); + VectorMath::Matrix17x17f A; + VectorMath::Matrix17x13f B_w; + float x[17]; + float u[6]; + + // extract the states + for (int i=0; i<17; i++){ + x[i] = states_[i]; + } + + // extract the controls + for (int i=0; i<3; i++){ + u[i] = accel[i]; + u[i+3] = gyro[i]; + } + + evaluateA(&A, x, u); + evaluateB_w(&B_w, x, u); + + TTimeDelta delta_T = (board_->micros() - last_times_.cov_propagation) / 1.0E6; // in seconds + last_times_.cov_propagation = board_->micros(); + + VectorMath::Matrix17x17f Phi; + VectorMath::Matrix17x13f GammaB_w; + evaluatePhiAndGamma_w(&Phi, &GammaB_w, &B_w, &A, delta_T); + + VectorMath::Matrix17x17f next_covariance; + next_covariance = Phi*covariance_*Phi.transpose() + GammaB_w*Q_*GammaB_w.transpose(); + + // set the new predicted covariance + covariance_ = next_covariance; + } // magnetometer update @@ -246,14 +287,23 @@ namespace airlib // comm_link_->log(messgae); } - void calculatePhi() + void evaluatePhiAndGamma_w( VectorMath::Matrix17x17f* Phi, + VectorMath::Matrix17x13f* GammaB_w, + VectorMath::Matrix17x13f* B_w, + VectorMath::Matrix17x17f* A, + TTimeDelta delta_T) { // calculate Phi based on the jacobian for some iteration - } + VectorMath::Matrix17x17f identity = VectorMath::Matrix17x17f::Identity(); + VectorMath::Matrix17x17f A_square = *A*(*A); - void calculateGamma_w() - { - // calculate Gamma_w based on the jacobian for some iteration + *Phi = identity + + *A * delta_T + + A_square * delta_T*delta_T/2; + + *GammaB_w = (identity + + identity * delta_T + + *A * delta_T*delta_T/2)*(*B_w); } // --------------------------------------------------------------------- diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfBase.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfBase.hpp index 1bb80bb05e..1c60573788 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfBase.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfBase.hpp @@ -15,15 +15,19 @@ namespace airlib class AirSimSimpleEkfBase : public simple_flight::IEkf { - +protected: + struct LastTimes + { + TTimePoint state_propagation; + TTimePoint cov_propagation; + }; protected: // EKF states // VectorMath::EkfStates states_; // EKF covariances - VectorMath::EkfCovariance covariance_; // timestamps - TTimePoint last_statePropagation_time_; + LastTimes last_times_; }; } diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfModel.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfModel.hpp index b8a454d600..418537d2b1 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfModel.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfModel.hpp @@ -66,15 +66,15 @@ namespace airlib f_local[2] = x[5]; // f_vel f_local[3] = (x[6]*x[6] + x[7]*x[7] - x[8]*x[8] - x[9]*x[9])* (u[0] + x[10]) - + 2*(x[7]*x[8] - x[6]*x[9])* (u[1] + x[11]) - + 2*(x[6]*x[8] + x[7]*x[9])* (u[2] + x[12]); - f_local[4] = 2*(x[7]*x[8] + x[6]*x[9])* (u[0] + x[10]) + + 2.0f*(x[7]*x[8] - x[6]*x[9])* (u[1] + x[11]) + + 2.0f*(x[6]*x[8] + x[7]*x[9])* (u[2] + x[12]); + f_local[4] = 2.0f*(x[7]*x[8] + x[6]*x[9])* (u[0] + x[10]) + (x[6]*x[6] - x[7]*x[7] + x[8]*x[8] - x[9]*x[9])* (u[1] + x[11]) - + 2*(x[8]*x[9] - x[6]*x[7])* (u[2] + x[12]); - f_local[5] = 2*(x[7]*x[9] - x[6]*x[8])* (u[0] + x[10]) - + 2*(x[8]*x[9] + x[6]*x[7])* (u[1] + x[11]) + + 2.0f*(x[8]*x[9] - x[6]*x[7])* (u[2] + x[12]); + f_local[5] = 2.0f*(x[7]*x[9] - x[6]*x[8])* (u[0] + x[10]) + + 2.0f*(x[8]*x[9] + x[6]*x[7])* (u[1] + x[11]) + (x[6]*x[6] - x[7]*x[7] - x[8]*x[8] + x[9]*x[9])* (u[2] + x[12]) - + G_0*(1 + 2*x[2]/R_E); + + G_0*(1 + 2.0f*x[2]/R_E); // f_ori f_local[6] = 0.5f * (-x[7]*(u[3] + x[13]) - x[8]*(u[4] + x[14]) - x[9]*(u[5] + x[15])); f_local[7] = 0.5f * ( x[6]*(u[3] + x[13]) - x[9]*(u[4] + x[14]) - x[8]*(u[5] + x[15])); @@ -102,7 +102,7 @@ namespace airlib // Mathematical jacobians // --------------------------------------------------------------------- - void df_dx(float A[17][17], float x[17], float u[6]) + void evaluateA(VectorMath::Matrix17x17f* A, float x[17], float u[6]) { // compute jacobian wrt the states vector df_dx = A /* @@ -132,26 +132,229 @@ namespace airlib u[5] = gyro_z */ - // set all elements to zero - float A[17][17]; - memset(&A[0][0], 0, sizeof(A)); - - // 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*G_0/R_E; - // df_vel_q0 [3 4 5] - A[3][6] = 2*x[6](u[0] + x[10]) - 2*x[6](u[0] + x[10]) + 2*x[6](u[0] + x[10]) + // // set all elements to zero + // memset(&A[0][0], 0, sizeof(A)); + // // 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*x[6]*(u[0] + x[10]) - 2.0f*x[9]*(u[1] + x[11]) + 2.0f*x[8]*(u[2] + x[12]); + // A[4][6] = 2.0f*x[9]*(u[0] + x[10]) + 2.0f*x[6]*(u[1] + x[11]) - 2.0f*x[7]*(u[2] + x[12]); + // A[5][6] = 2.0f*x[8]*(u[0] + x[10]) - 2.0f*x[7]*(u[1] + x[11]) + 2.0f*x[6]*(u[2] + x[12]); + // // df_vel_q1 [3 4 5][7] + // A[3][7] = 2.0f*x[7]*(u[0] + x[10]) + 2.0f*x[8]*(u[1] + x[11]) + 2.0f*x[9]*(u[2] + x[12]); + // A[4][7] = 2.0f*x[8]*(u[0] + x[10]) - 2.0f*x[7]*(u[1] + x[11]) - 2.0f*x[6]*(u[2] + x[12]); + // A[5][7] = 2.0f*x[9]*(u[0] + x[10]) + 2.0f*x[6]*(u[1] + x[11]) - 2.0f*x[7]*(u[2] + x[12]); + // // df_vel_q2 [3 4 5][8] + // A[3][8] = -2.0f*x[8]*(u[0] + x[10]) + 2.0f*x[7]*(u[1] + x[11]) + 2.0f*x[6]*(u[2] + x[12]); + // A[4][8] = 2.0f*x[7]*(u[0] + x[10]) + 2.0f*x[8]*(u[1] + x[11]) + 2.0f*x[9]*(u[2] + x[12]); + // A[5][8] = -2.0f*x[6]*(u[0] + x[10]) + 2.0f*x[9]*(u[1] + x[11]) - 2.0f*x[8]*(u[2] + x[12]); + // // df_vel_q3 [3 4 5][9] + // A[3][9] = -2.0f*x[9]*(u[0] + x[10]) - 2.0f*x[6]*(u[1] + x[11]) + 2.0f*x[7]*(u[2] + x[12]); + // A[4][9] = 2.0f*x[6]*(u[0] + x[10]) - 2.0f*x[9]*(u[1] + x[11]) + 2.0f*x[8]*(u[2] + x[12]); + // A[5][9] = 2.0f*x[7]*(u[0] + x[10]) + 2.0f*x[8]*(u[1] + x[11]) + 2.0f*x[9]*(u[2] + x[12]); + // // df_vel_xbias [3 4 5][10 11 12 ...] + // A[3][10] = x[6]*x[6] + x[7]*x[7] - x[8]*x[8] - x[9]*x[9]; + // A[3][11] = 2.0f*(x[7]*x[8] - x[6]*x[9]); + // A[3][12] = 2.0f*(x[6]*x[8] + x[7]*x[9]); + // A[4][10] = 2.0f*(x[7]*x[8] + x[6]*x[9]); + // A[4][11] = x[6]*x[6] - x[7]*x[7] + x[8]*x[8] - x[9]*x[9]; + // A[4][12] = 2.0f*(x[8]*x[9] - x[6]*x[7]); + // A[5][10] = 2.0f*(x[7]*x[9] - x[6]*x[8]); + // A[5][11] = 2.0f*(x[8]*x[9] + x[6]*x[7]); + // A[5][12] = x[6]*x[6] - x[7]*x[7] - x[8]*x[8] + x[9]*x[9]; + // // df_ori_[q0 q1 q2 q3] [6 7 8 9][6 7 8 9] + // A[6][7] = -0.5f*(u[3] + x[13]); + // A[6][8] = -0.5f*(u[4] + x[14]); + // A[6][9] = -0.5f*(u[5] + x[15]); + // A[7][6] = 0.5f*(u[3] + x[13]); + // A[7][8] = -0.5f*(u[5] + x[15]); + // A[7][9] = -0.5f*(u[4] + x[14]); + // A[8][6] = 0.5f*(u[4] + x[14]); + // A[8][7] = -0.5f*(u[5] + x[15]); + // A[8][9] = 0.5f*(u[3] + x[13]); + // A[9][6] = 0.5f*(u[5] + x[15]); + // A[9][7] = 0.5f*(u[4] + x[14]); + // A[9][8] = -0.5f*(u[3] + x[13]); + // // df_ori_x_bias [6 7 8 9][10 11 12 13 14 15 16 17] + // A[6][13] = -0.5f*x[7]; + // A[6][14] = -0.5f*x[8]; + // A[6][15] = -0.5f*x[9]; + // A[7][13] = 0.5f*x[6]; + // A[7][14] = -0.5f*x[9]; + // A[7][15] = -0.5f*x[8]; + // A[8][13] = 0.5f*x[9]; + // A[8][14] = 0.5f*x[6]; + // A[8][15] = -0.5f*x[7]; + // A[9][13] = -0.5f*x[8]; + // A[9][14] = 0.5f*x[7]; + // A[9][15] = 0.5f*x[6]; + *A = VectorMath::Matrix17x17f::Zero(); + // 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*x[6]*(u[0] + x[10]) - 2.0f*x[9]*(u[1] + x[11]) + 2.0f*x[8]*(u[2] + x[12]); + (*A)(4, 6) = 2.0f*x[9]*(u[0] + x[10]) + 2.0f*x[6]*(u[1] + x[11]) - 2.0f*x[7]*(u[2] + x[12]); + (*A)(5, 6) = 2.0f*x[8]*(u[0] + x[10]) - 2.0f*x[7]*(u[1] + x[11]) + 2.0f*x[6]*(u[2] + x[12]); + // df_vel_q1 [3 4 5][7] + (*A)(3, 7) = 2.0f*x[7]*(u[0] + x[10]) + 2.0f*x[8]*(u[1] + x[11]) + 2.0f*x[9]*(u[2] + x[12]); + (*A)(4, 7) = 2.0f*x[8]*(u[0] + x[10]) - 2.0f*x[7]*(u[1] + x[11]) - 2.0f*x[6]*(u[2] + x[12]); + (*A)(5, 7) = 2.0f*x[9]*(u[0] + x[10]) + 2.0f*x[6]*(u[1] + x[11]) - 2.0f*x[7]*(u[2] + x[12]); + // df_vel_q2 [3 4 5][8] + (*A)(3, 8) = -2.0f*x[8]*(u[0] + x[10]) + 2.0f*x[7]*(u[1] + x[11]) + 2.0f*x[6]*(u[2] + x[12]); + (*A)(4, 8) = 2.0f*x[7]*(u[0] + x[10]) + 2.0f*x[8]*(u[1] + x[11]) + 2.0f*x[9]*(u[2] + x[12]); + (*A)(5, 8) = -2.0f*x[6]*(u[0] + x[10]) + 2.0f*x[9]*(u[1] + x[11]) - 2.0f*x[8]*(u[2] + x[12]); + // df_vel_q3 [3 4 5][9] + (*A)(3, 9) = -2.0f*x[9]*(u[0] + x[10]) - 2.0f*x[6]*(u[1] + x[11]) + 2.0f*x[7]*(u[2] + x[12]); + (*A)(4, 9) = 2.0f*x[6]*(u[0] + x[10]) - 2.0f*x[9]*(u[1] + x[11]) + 2.0f*x[8]*(u[2] + x[12]); + (*A)(5, 9) = 2.0f*x[7]*(u[0] + x[10]) + 2.0f*x[8]*(u[1] + x[11]) + 2.0f*x[9]*(u[2] + x[12]); + // df_vel_xbias [3 4 5][10 11 12 ...] + (*A)(3, 10) = x[6]*x[6] + x[7]*x[7] - x[8]*x[8] - x[9]*x[9]; + (*A)(3, 11) = 2.0f*(x[7]*x[8] - x[6]*x[9]); + (*A)(3, 12) = 2.0f*(x[6]*x[8] + x[7]*x[9]); + (*A)(4, 10) = 2.0f*(x[7]*x[8] + x[6]*x[9]); + (*A)(4, 11) = x[6]*x[6] - x[7]*x[7] + x[8]*x[8] - x[9]*x[9]; + (*A)(4, 12) = 2.0f*(x[8]*x[9] - x[6]*x[7]); + (*A)(5, 10) = 2.0f*(x[7]*x[9] - x[6]*x[8]); + (*A)(5, 11) = 2.0f*(x[8]*x[9] + x[6]*x[7]); + (*A)(5, 12) = x[6]*x[6] - x[7]*x[7] - x[8]*x[8] + x[9]*x[9]; + // df_ori_[q0 q1 q2 q3] [6 7 8 9][6 7 8 9] + (*A)(6, 7)= -0.5f*(u[3] + x[13]); + (*A)(6, 8)= -0.5f*(u[4] + x[14]); + (*A)(6, 9)= -0.5f*(u[5] + x[15]); + (*A)(7, 6)= 0.5f*(u[3] + x[13]); + (*A)(7, 8)= -0.5f*(u[5] + x[15]); + (*A)(7, 9)= -0.5f*(u[4] + x[14]); + (*A)(8, 6)= 0.5f*(u[4] + x[14]); + (*A)(8, 7)= -0.5f*(u[5] + x[15]); + (*A)(8, 9)= 0.5f*(u[3] + x[13]); + (*A)(9, 6)= 0.5f*(u[5] + x[15]); + (*A)(9, 7)= 0.5f*(u[4] + x[14]); + (*A)(9, 8)= -0.5f*(u[3] + x[13]); + // df_ori_x_bias [6 7 8 9][10 11 12 13 14 15 16 17] + (*A)(6, 13) = -0.5f*x[7]; + (*A)(6, 14) = -0.5f*x[8]; + (*A)(6, 15) = -0.5f*x[9]; + (*A)(7, 13) = 0.5f*x[6]; + (*A)(7, 14) = -0.5f*x[9]; + (*A)(7, 15) = -0.5f*x[8]; + (*A)(8, 13) = 0.5f*x[9]; + (*A)(8, 14) = 0.5f*x[6]; + (*A)(8, 15) = -0.5f*x[7]; + (*A)(9, 13) = -0.5f*x[8]; + (*A)(9, 14) = 0.5f*x[7]; + (*A)(9, 15) = 0.5f*x[6]; } - void df_dw() + void evaluateB_w(VectorMath::Matrix17x13f* B_w, float x[17], float u[6]) { - // jacobian wrt the process noise vector + // compute jacobian wrt the process noise vector df_dw + /* + x[0] = x + x[1] = y + x[2] = z + x[3] = u + x[4] = v + x[5] = w + x[6] = q0 + x[7] = q1 + x[8] = q2 + x[9] = q3 + x[10] = bias_accel_x + x[11] = bias_accel_y + x[12] = bias_accel_z + x[13] = bias_gyro_x + x[14] = bias_gyro_x + x[15] = bias_gyro_x + x[16] = bias_baro + + u[0] = accel_x + u[1] = accel_y + u[2] = accel_z + u[3] = gyro_x + u[4] = gyro_y + u[5] = gyro_z + */ + + // // set all elements to zero + // memset(&B_w[0][0], 0, sizeof(B_w)); + + // // df_vel_w [3 4 5][0 1 2 ...] + // B_w[3][0] = x[6]*x[6] + x[7]*x[7] - x[8]*x[8] - x[9]*x[9]; + // B_w[3][1] = 2.0f*(x[7]*x[8] - x[6]*x[9]); + // B_w[3][2] = 2.0f*(x[6]*x[8] + x[7]*x[9]); + // B_w[4][0] = 2.0f*(x[7]*x[8] + x[6]*x[9]); + // B_w[4][1] = x[6]*x[6] - x[7]*x[7] + x[8]*x[8] - x[9]*x[9]; + // B_w[4][2] = 2.0f*(x[8]*x[9] - x[6]*x[7]); + // B_w[5][0] = 2.0f*(x[7]*x[9] - x[6]*x[8]); + // B_w[5][1] = 2.0f*(x[8]*x[9] + x[6]*x[7]); + // B_w[5][2] = x[6]*x[6] - x[7]*x[7] - x[8]*x[8] + x[9]*x[9]; + // // df_ori_x_bias [6 7 8 9][10 11 12 13 14 15 16 17] + // B_w[6][3] = -0.5f*x[7]; + // B_w[6][4] = -0.5f*x[8]; + // B_w[6][5] = -0.5f*x[9]; + // B_w[7][3] = 0.5f*x[6]; + // B_w[7][4] = -0.5f*x[9]; + // B_w[7][5] = -0.5f*x[8]; + // B_w[8][3] = 0.5f*x[9]; + // B_w[8][4] = 0.5f*x[6]; + // B_w[8][5] = -0.5f*x[7]; + // B_w[9][3] = -0.5f*x[8]; + // B_w[9][4] = 0.5f*x[7]; + // B_w[9][5] = 0.5f*x[6]; + // // 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; + + // set all elements to zero + *B_w = VectorMath::Matrix17x13f::Zero(); + + // df_vel_w [3 4 5][0 1 2 ...] + (*B_w)(3, 0) = x[6]*x[6] + x[7]*x[7] - x[8]*x[8] - x[9]*x[9]; + (*B_w)(3, 1) = 2.0f*(x[7]*x[8] - x[6]*x[9]); + (*B_w)(3, 2) = 2.0f*(x[6]*x[8] + x[7]*x[9]); + (*B_w)(4, 0) = 2.0f*(x[7]*x[8] + x[6]*x[9]); + (*B_w)(4, 1) = x[6]*x[6] - x[7]*x[7] + x[8]*x[8] - x[9]*x[9]; + (*B_w)(4, 2) = 2.0f*(x[8]*x[9] - x[6]*x[7]); + (*B_w)(5, 0) = 2.0f*(x[7]*x[9] - x[6]*x[8]); + (*B_w)(5, 1) = 2.0f*(x[8]*x[9] + x[6]*x[7]); + (*B_w)(5, 2) = x[6]*x[6] - x[7]*x[7] - x[8]*x[8] + x[9]*x[9]; + // df_ori_x_bias [6 7 8 9][10 11 12 13 14 15 16 17] + (*B_w)(6, 3) = -0.5f*x[7]; + (*B_w)(6, 4) = -0.5f*x[8]; + (*B_w)(6, 5) = -0.5f*x[9]; + (*B_w)(7, 3) = 0.5f*x[6]; + (*B_w)(7, 4) = -0.5f*x[9]; + (*B_w)(7, 5) = -0.5f*x[8]; + (*B_w)(8, 3) = 0.5f*x[9]; + (*B_w)(8, 4) = 0.5f*x[6]; + (*B_w)(8, 5) = -0.5f*x[7]; + (*B_w)(9, 3) = -0.5f*x[8]; + (*B_w)(9, 4) = 0.5f*x[7]; + (*B_w)(9, 5) = 0.5f*x[6]; + // 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; } void dh_mag_dx() 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 index b1e5e32175..42e5e8965d 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IEkf.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IEkf.hpp @@ -17,19 +17,33 @@ class IEkf : public IUpdatable } // setters - void setEkfStates(VectorMath::EkfStates states) + void setEkfStates(VectorMath::Vector17f states) { states_ = states; } // getters - const VectorMath::EkfStates& getEkfStates() const + const VectorMath::Vector17f& getEkfStates() const { return states_; } + // setters + void setEkfCovariance(VectorMath::Matrix17x17f covariance) + { + covariance_ = covariance; + } + + // getters + const VectorMath::Matrix17x17f& getEkfCovariance() const + { + return covariance_; + } + protected: - VectorMath::EkfStates states_; + VectorMath::Vector17f states_; + VectorMath::Matrix17x17f covariance_; + VectorMath::Matrix13x13f Q_; }; } //namespace \ No newline at end of file From a99b08bb4025efa0f08094fd95b509a63f0d7577 Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Sat, 25 Dec 2021 15:04:08 +0100 Subject: [PATCH 30/87] add geodetic2Ned interface for AirSim in GeodeticConverter --- AirLib/include/common/GeodeticConverter.hpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/AirLib/include/common/GeodeticConverter.hpp b/AirLib/include/common/GeodeticConverter.hpp index cbeb223931..f1de155713 100644 --- a/AirLib/include/common/GeodeticConverter.hpp +++ b/AirLib/include/common/GeodeticConverter.hpp @@ -134,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) { From 9de7f8e40e237f85dcb64271be7570f7d475a3ce Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Sat, 25 Dec 2021 15:07:12 +0100 Subject: [PATCH 31/87] add further matrix definitions in VectorMath --- AirLib/include/common/VectorMath.hpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/AirLib/include/common/VectorMath.hpp b/AirLib/include/common/VectorMath.hpp index 87783177e2..c67629d18d 100644 --- a/AirLib/include/common/VectorMath.hpp +++ b/AirLib/include/common/VectorMath.hpp @@ -43,6 +43,15 @@ namespace airlib 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 Matrix6x6f; + typedef Eigen::Matrix Matrix6x17f; + typedef Eigen::Matrix Matrix17x6f; + typedef Eigen::Matrix Vector13f; + typedef Eigen::Matrix Vector7f; typedef Eigen::AngleAxisd AngleAxisd; typedef Eigen::AngleAxisf AngleAxisf; From 99ec240d7403a3950d5f976838fd372c5400982a Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Sat, 25 Dec 2021 15:09:29 +0100 Subject: [PATCH 32/87] add white noise model in gps sensor model --- AirLib/include/sensors/gps/GpsSimple.hpp | 26 ++++++++++++++++++++++++ 1 file changed, 26 insertions(+) diff --git a/AirLib/include/sensors/gps/GpsSimple.hpp b/AirLib/include/sensors/gps/GpsSimple.hpp index 73a572b4f8..cb4e571c0d 100644 --- a/AirLib/include/sensors/gps/GpsSimple.hpp +++ b/AirLib/include/sensors/gps/GpsSimple.hpp @@ -38,6 +38,10 @@ namespace airlib //*** Start: UpdatableState implementation ***// virtual void resetImplementation() override { + // added + gauss_dist_pos.reset(); + gauss_dist_vel.reset(); + freq_limiter_.reset(); delay_line_.reset(); @@ -82,9 +86,27 @@ namespace airlib //GNSS output.gnss.time_utc = static_cast(clock()->nowNanos() / 1.0E3); output.gnss.geo_point = ground_truth.environment->getState().geo_point; + + // // added by Suman + // output.gnss.geo_point.longitude = ground_truth.kinematics->pose.position[0]; + // output.gnss.geo_point.latitude = ground_truth.kinematics->pose.position[1]; + // output.gnss.geo_point.altitude = ground_truth.kinematics->pose.position[2]; + + real_T gps_sigma_pos = 0.00001f; // 1/6378km rad + output.gnss.geo_point.longitude += gauss_dist_pos.next()[0] * gps_sigma_pos; + output.gnss.geo_point.latitude += gauss_dist_pos.next()[1] * gps_sigma_pos; + output.gnss.geo_point.altitude += gauss_dist_pos.next()[2] * 3.0f; + + // // + output.gnss.eph = eph; output.gnss.epv = epv; + output.gnss.velocity = ground_truth.kinematics->twist.linear; + output.gnss.velocity.x() += gauss_dist_vel.next()[0]*2.0f; + output.gnss.velocity.y() += gauss_dist_vel.next()[1]*2.0f; + output.gnss.velocity.z() += gauss_dist_vel.next()[2]*2.0f; + output.is_valid = true; output.gnss.fix_type = @@ -105,6 +127,10 @@ namespace airlib FirstOrderFilter eph_filter, epv_filter; FrequencyLimiter freq_limiter_; DelayLine delay_line_; + + //added + RandomVectorGaussianR gauss_dist_pos = RandomVectorGaussianR(0, 1); + RandomVectorGaussianR gauss_dist_vel = RandomVectorGaussianR(0, 1); }; } } //namespace From 8313e599974a4558ae9b4ff14f5ff86603ab78d5 Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Sat, 25 Dec 2021 15:13:42 +0100 Subject: [PATCH 33/87] change initialization order of ekf in Firmware --- .../multirotor/firmwares/simple_flight/firmware/Firmware.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 91294756c4..3bc31c8be7 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/Firmware.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/Firmware.hpp @@ -21,7 +21,7 @@ class Firmware : public IFirmware { public: 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), offboard_api_(params, board, board, state_estimator, comm_link), mixer_(params), ekf_(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: From 2ba0ca36df332c5b3c0281bb8bae27563bfc8ded Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Sat, 25 Dec 2021 15:15:01 +0100 Subject: [PATCH 34/87] add setting of ground truth in ekf --- .../multirotor/firmwares/simple_flight/SimpleFlightApi.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightApi.hpp index 20e2a5b7f5..171cae4cc2 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightApi.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightApi.hpp @@ -15,7 +15,7 @@ #include "AirSimSimpleFlightCommLink.hpp" #include "AirSimSimpleFlightEstimator.hpp" #include "AirSimSimpleFlightCommon.hpp" -#include "AirSimSimpleEKF.hpp" +#include "AirSimSimpleEkf.hpp" #include "physics/PhysicsBody.hpp" #include "common/AirSimSettings.hpp" @@ -119,6 +119,7 @@ namespace airlib { board_->setGroundTruthKinematics(kinematics); estimator_->setGroundTruthKinematics(kinematics, environment); + ekf_->setGroundTruthKinematics(kinematics, environment); } virtual bool setRCData(const RCData& rc_data) override { From e670538c02a4e2d2b7ecc64f82c4d3ad145e82d6 Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Sat, 25 Dec 2021 15:17:27 +0100 Subject: [PATCH 35/87] add further interfaces in flight estimator to log and use ekf results --- .../AirSimSimpleFlightEstimator.hpp | 219 +++++++++++++++++- .../firmware/interfaces/IStateEstimator.hpp | 20 +- 2 files changed, 226 insertions(+), 13 deletions(-) diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightEstimator.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightEstimator.hpp index 80a819f611..03648fdb2e 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightEstimator.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightEstimator.hpp @@ -103,27 +103,106 @@ namespace airlib return state; } - virtual simple_flight::Axis3r getEkfPostion() const override + // additional methods + + 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::SensorMeasurements getEkfMeasurements() const override + { + 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 getEkfPosition() const override { simple_flight::Axis3r position; auto ekf_states = ekf_->getEkfStates(); - position.x() = ekf_states[0]; - position.y() = ekf_states[1]; - position.z() = ekf_states[2]; + position.x() = ekf_states(0); + position.y() = ekf_states(1); + position.z() = ekf_states(2); return position; } + virtual simple_flight::Axis3r getEkfLinearVelocity() const override + { + 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::Axis4r getEkfOrientation() const override + { + simple_flight::Axis4r orientation; + auto ekf_states = ekf_->getEkfStates(); + + orientation.x() = ekf_states(6); + orientation.y() = ekf_states(7); + orientation.z() = ekf_states(8); + orientation.val4()= ekf_states(9); + + return orientation; + } + virtual simple_flight::Axis3r getEkfAngles() const override { simple_flight::Axis3r angles; Quaternionr orientation; auto ekf_states = ekf_->getEkfStates(); - orientation.x() = ekf_states[6]; - orientation.y() = ekf_states[7]; - orientation.z() = ekf_states[8]; - orientation.w() = ekf_states[9]; + 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(), @@ -133,10 +212,37 @@ namespace airlib return angles; } + virtual simple_flight::SensorBiases getEkfSensorBias() const override + { + 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 getEkfPositionCovariance() const override { simple_flight::Axis3r position_cov; - auto ekf_covariance = ekf_->getEkfCovariance(); + VectorMath::Matrix17x17f ekf_covariance = ekf_->getEkfCovariance(); position_cov.x() = ekf_covariance(0, 0); position_cov.y() = ekf_covariance(1, 1); position_cov.z() = ekf_covariance(2, 2); @@ -144,10 +250,21 @@ namespace airlib return position_cov; } - virtual simple_flight::Axis3r getEkfAngleCovariance() const override + virtual simple_flight::Axis3r getEkfLinearVelocityCovariance() const override + { + simple_flight::Axis3r velocity_cov; + VectorMath::Matrix17x17f ekf_covariance = ekf_->getEkfCovariance(); + velocity_cov.x() = ekf_covariance(3, 3); + velocity_cov.y() = ekf_covariance(4, 4); + velocity_cov.z() = ekf_covariance(5, 5); + + return velocity_cov; + } + + virtual simple_flight::Axis4r getEkfOrientationCovariance() const override { simple_flight::Axis4r angle_cov; - auto ekf_covariance = ekf_->getEkfCovariance(); + VectorMath::Matrix17x17f ekf_covariance = ekf_->getEkfCovariance(); angle_cov.x() = ekf_covariance(6, 6); angle_cov.y() = ekf_covariance(7, 7); angle_cov.z() = ekf_covariance(8, 8); @@ -156,6 +273,86 @@ namespace airlib return angle_cov; } + virtual simple_flight::Axis3r getEkfImuBiasCovariance() const override + { + simple_flight::Axis3r imu_bias_cov; + VectorMath::Matrix17x17f ekf_covariance = ekf_->getEkfCovariance(); + imu_bias_cov.x() = ekf_covariance(10, 10); + imu_bias_cov.y() = ekf_covariance(11, 11); + imu_bias_cov.z() = ekf_covariance(12, 12); + + return imu_bias_cov; + } + + virtual simple_flight::Axis3r getEkfGyroBiasCovariance() const override + { + simple_flight::Axis3r gyro_bias_cov; + VectorMath::Matrix17x17f ekf_covariance = ekf_->getEkfCovariance(); + gyro_bias_cov.x() = ekf_covariance(13, 13); + gyro_bias_cov.y() = ekf_covariance(14, 14); + gyro_bias_cov.z() = ekf_covariance(15, 15); + + return gyro_bias_cov; + } + + virtual float getEkfBaroBiasCovariance() const override + { + float baro_bias_cov; + VectorMath::Matrix17x17f ekf_covariance = ekf_->getEkfCovariance(); + baro_bias_cov = ekf_covariance(16, 16); + + return baro_bias_cov; + } + + 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; + } + + virtual std::array getEkfOrientationOffDiagCovariance() const override + { + std::array ori_offdiag_cov; + VectorMath::Matrix17x17f ekf_covariance = ekf_->getEkfCovariance(); + ori_offdiag_cov.at(0) = ekf_covariance(6, 7); + ori_offdiag_cov.at(1) = ekf_covariance(6, 8); + ori_offdiag_cov.at(2) = ekf_covariance(6, 9); + ori_offdiag_cov.at(3) = ekf_covariance(7, 8); + ori_offdiag_cov.at(4) = ekf_covariance(7, 9); + ori_offdiag_cov.at(5) = ekf_covariance(8, 9); + + return ori_offdiag_cov; + } + + virtual std::array getEkfOrientationGyroBiasCovariance() const override + { + std::array ori_gyro_bias_cov; + VectorMath::Matrix17x17f ekf_covariance = ekf_->getEkfCovariance(); + ori_gyro_bias_cov.at(0) = ekf_covariance(6, 13); + ori_gyro_bias_cov.at(1) = ekf_covariance(6, 14); + ori_gyro_bias_cov.at(2) = ekf_covariance(6, 15); + ori_gyro_bias_cov.at(3) = ekf_covariance(7, 13); + ori_gyro_bias_cov.at(4) = ekf_covariance(7, 14); + ori_gyro_bias_cov.at(5) = ekf_covariance(7, 15); + ori_gyro_bias_cov.at(6) = ekf_covariance(8, 13); + ori_gyro_bias_cov.at(7) = ekf_covariance(8, 14); + ori_gyro_bias_cov.at(8) = ekf_covariance(8, 15); + ori_gyro_bias_cov.at(9) = ekf_covariance(9, 13); + ori_gyro_bias_cov.at(10) = ekf_covariance(9, 14); + ori_gyro_bias_cov.at(11) = ekf_covariance(9, 15); + + return ori_gyro_bias_cov; + } + private: const Kinematics::State* kinematics_; const Environment* environment_; 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 76d85d5c25..8f59766547 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 @@ -21,9 +21,25 @@ class IStateEstimator virtual Axis3r transformToBodyFrame(const Axis3r& world_frame_val) const = 0; - virtual simple_flight::Axis3r getEkfPostion() 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 getEkfPositionCovariance() const = 0; - virtual simple_flight::Axis3r getEkfAngleCovariance() const = 0; + virtual simple_flight::Axis3r getEkfLinearVelocityCovariance() const = 0; + virtual simple_flight::Axis4r getEkfOrientationCovariance() const = 0; + virtual simple_flight::Axis3r getEkfImuBiasCovariance() const = 0; + virtual simple_flight::Axis3r getEkfGyroBiasCovariance() const = 0; + virtual float getEkfBaroBiasCovariance() const = 0; + + virtual float getEkfOrientationNorm() const = 0; + + virtual std::array getEkfOrientationOffDiagCovariance() const = 0; + virtual std::array getEkfOrientationGyroBiasCovariance() const = 0; }; } \ No newline at end of file From 7b15666c3877cbe60d4aa45b5bdc49f885c62eb3 Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Sat, 25 Dec 2021 15:20:37 +0100 Subject: [PATCH 36/87] add further ekf results data logging in OffboardApi --- .../simple_flight/firmware/OffboardApi.hpp | 109 ++++++++++++++++-- 1 file changed, 100 insertions(+), 9 deletions(-) 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 28c0273327..ea4c42703f 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/OffboardApi.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/OffboardApi.hpp @@ -60,7 +60,7 @@ class OffboardApi : public IUpdatable detectLanding(); detectTakingOff(); - logEkfStates(); + logEkfValues(); } /**************** IOffboardApi ********************/ @@ -206,20 +206,111 @@ class OffboardApi : public IUpdatable private: - void logEkfStates() + void logEkfValues() { // additional logging of vehicle states, TODO implement a separate log! std::ostringstream log_msg; log_msg << clock_->millis() << '\t' - << state_estimator_->getPosition().x() << '\t' - << state_estimator_->getPosition().y() << '\t' - << state_estimator_->getPosition().z() << '\t' - << state_estimator_->getEkfPostion().x() << '\t' - << state_estimator_->getEkfPostion().y() << '\t' - << state_estimator_->getEkfPostion().z() << '\t' + // ground truth mesurement signals + << state_estimator_->getTrueMeasurements().accel.x() << '\t' + << state_estimator_->getTrueMeasurements().accel.y() << '\t' + << state_estimator_->getTrueMeasurements().accel.z() << '\t' + << state_estimator_->getTrueMeasurements().gyro.x() << '\t' + << state_estimator_->getTrueMeasurements().gyro.y() << '\t' + << state_estimator_->getTrueMeasurements().gyro.z() << '\t' + // noisy mesurement signals + << state_estimator_->getEkfMeasurements().accel.x() << '\t' + << state_estimator_->getEkfMeasurements().accel.y() << '\t' + << state_estimator_->getEkfMeasurements().accel.z() << '\t' + << state_estimator_->getEkfMeasurements().gyro.x() << '\t' + << state_estimator_->getEkfMeasurements().gyro.y() << '\t' + << state_estimator_->getEkfMeasurements().gyro.z() << '\t' + << state_estimator_->getEkfMeasurements().gps_position.x() << '\t' + << state_estimator_->getEkfMeasurements().gps_position.y() << '\t' + << state_estimator_->getEkfMeasurements().gps_position.z() << '\t' + << state_estimator_->getEkfMeasurements().gps_velocity.x() << '\t' + << state_estimator_->getEkfMeasurements().gps_velocity.y() << '\t' + << state_estimator_->getEkfMeasurements().gps_velocity.z() << '\t' + << state_estimator_->getEkfMeasurements().baro_altitude << '\t' + << state_estimator_->getEkfMeasurements().magnetic_flux.x() << '\t' + << state_estimator_->getEkfMeasurements().magnetic_flux.y() << '\t' + << state_estimator_->getEkfMeasurements().magnetic_flux.z() << '\t' + // ground truth states + << state_estimator_->getKinematicsEstimated().position.x() << '\t' + << state_estimator_->getKinematicsEstimated().position.y() << '\t' + << state_estimator_->getKinematicsEstimated().position.z() << '\t' + << state_estimator_->getKinematicsEstimated().orientation.val4() << '\t' // ATTENSION val4 is w when converted to axis4r of simple_flight + << state_estimator_->getKinematicsEstimated().orientation.x() << '\t' + << state_estimator_->getKinematicsEstimated().orientation.y() << '\t' + << state_estimator_->getKinematicsEstimated().orientation.z() << '\t' + << state_estimator_->getAngles().pitch() << '\t' + << state_estimator_->getAngles().roll() << '\t' + << state_estimator_->getAngles().yaw() << '\t' + << state_estimator_->getKinematicsEstimated().linear_velocity.x() << '\t' + << state_estimator_->getKinematicsEstimated().linear_velocity.y() << '\t' + << state_estimator_->getKinematicsEstimated().linear_velocity.z() << '\t' + // estimated states + << state_estimator_->getEkfKinematicsEstimated().position.x() << '\t' + << state_estimator_->getEkfKinematicsEstimated().position.y() << '\t' + << state_estimator_->getEkfKinematicsEstimated().position.z() << '\t' + << state_estimator_->getEkfKinematicsEstimated().orientation.val4() << '\t' + << state_estimator_->getEkfKinematicsEstimated().orientation.x() << '\t' + << state_estimator_->getEkfKinematicsEstimated().orientation.y() << '\t' + << state_estimator_->getEkfKinematicsEstimated().orientation.z() << '\t' + << state_estimator_->getEkfAngles().pitch() << '\t' + << state_estimator_->getEkfAngles().roll() << '\t' + << state_estimator_->getEkfAngles().yaw() << '\t' + << state_estimator_->getEkfKinematicsEstimated().linear_velocity.x() << '\t' + << state_estimator_->getEkfKinematicsEstimated().linear_velocity.y() << '\t' + << state_estimator_->getEkfKinematicsEstimated().linear_velocity.z() << '\t' + << state_estimator_->getEkfKinematicsEstimated().sensor_bias.accel.x() << '\t' + << state_estimator_->getEkfKinematicsEstimated().sensor_bias.accel.y() << '\t' + << state_estimator_->getEkfKinematicsEstimated().sensor_bias.accel.z() << '\t' + << state_estimator_->getEkfKinematicsEstimated().sensor_bias.gyro.x() << '\t' + << state_estimator_->getEkfKinematicsEstimated().sensor_bias.gyro.y() << '\t' + << state_estimator_->getEkfKinematicsEstimated().sensor_bias.gyro.z() << '\t' + << state_estimator_->getEkfKinematicsEstimated().sensor_bias.barometer << '\t' + // covariances << state_estimator_->getEkfPositionCovariance().x() << '\t' << state_estimator_->getEkfPositionCovariance().y() << '\t' - << state_estimator_->getEkfPositionCovariance().z() << '\t'; + << state_estimator_->getEkfPositionCovariance().z() << '\t' + << state_estimator_->getEkfLinearVelocityCovariance().x() << '\t' + << state_estimator_->getEkfLinearVelocityCovariance().y() << '\t' + << state_estimator_->getEkfLinearVelocityCovariance().z() << '\t' + << state_estimator_->getEkfOrientationCovariance().x() << '\t' + << state_estimator_->getEkfOrientationCovariance().y() << '\t' + << state_estimator_->getEkfOrientationCovariance().z() << '\t' + << state_estimator_->getEkfOrientationCovariance().val4() << '\t' + << state_estimator_->getEkfImuBiasCovariance().x() << '\t' + << state_estimator_->getEkfImuBiasCovariance().y() << '\t' + << state_estimator_->getEkfImuBiasCovariance().z() << '\t' + << state_estimator_->getEkfGyroBiasCovariance().x() << '\t' + << state_estimator_->getEkfGyroBiasCovariance().y() << '\t' + << state_estimator_->getEkfGyroBiasCovariance().z() << '\t' + << state_estimator_->getEkfBaroBiasCovariance() << '\t' + // quaternion norm + << state_estimator_->getEkfOrientationNorm() << '\t' + // off-diag quaternion covariance + << state_estimator_->getEkfOrientationOffDiagCovariance().at(0) << '\t' + << state_estimator_->getEkfOrientationOffDiagCovariance().at(1) << '\t' + << state_estimator_->getEkfOrientationOffDiagCovariance().at(2) << '\t' + << state_estimator_->getEkfOrientationOffDiagCovariance().at(3) << '\t' + << state_estimator_->getEkfOrientationOffDiagCovariance().at(4) << '\t' + << state_estimator_->getEkfOrientationOffDiagCovariance().at(5) << '\t' + // gyro bias quaternion covariance + << state_estimator_->getEkfOrientationGyroBiasCovariance().at(0) << '\t' + << state_estimator_->getEkfOrientationGyroBiasCovariance().at(1) << '\t' + << state_estimator_->getEkfOrientationGyroBiasCovariance().at(2) << '\t' + << state_estimator_->getEkfOrientationGyroBiasCovariance().at(3) << '\t' + << state_estimator_->getEkfOrientationGyroBiasCovariance().at(4) << '\t' + << state_estimator_->getEkfOrientationGyroBiasCovariance().at(5) << '\t' + << state_estimator_->getEkfOrientationGyroBiasCovariance().at(6) << '\t' + << state_estimator_->getEkfOrientationGyroBiasCovariance().at(7) << '\t' + << state_estimator_->getEkfOrientationGyroBiasCovariance().at(8) << '\t' + << state_estimator_->getEkfOrientationGyroBiasCovariance().at(9) << '\t' + << state_estimator_->getEkfOrientationGyroBiasCovariance().at(10) << '\t' + << state_estimator_->getEkfOrientationGyroBiasCovariance().at(11) << '\t'; + std::string message = log_msg.str(); comm_link_->log(message); } From 50eb76741c4468d112b25c8f4c0a19e0592da010 Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Sat, 25 Dec 2021 15:22:00 +0100 Subject: [PATCH 37/87] add struct defintions for ekf --- .../firmware/interfaces/CommonStructs.hpp | 69 +++++++++++++++++++ 1 file changed, 69 insertions(+) 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 1c81b256c2..27a918c499 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 @@ -211,6 +211,75 @@ 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; +}; + +struct SensorCharacteristics +{ + Axis3r accel_std_dev; + Axis3r gyro_std_dev; + + Axis3r gps_pos_std_dev; + Axis3r gps_vel_std_dev; + + Axis3r mag_std_dev; + + real_T baro_std_dev; +}; + +struct EkfInitialStates +{ + Axis3r pos; + Axis3r vel; + + Axis4r quaternion; + + Axis3r accel_bias; + Axis3r gyro_bias; + real_T baro_bias; +}; + +struct EkfInitialStdDevs +{ + Axis3r pos; + Axis3r vel; + + Axis4r quaternion; + + Axis3r accel_bias; + Axis3r gyro_bias; + real_T baro_bias; +}; + enum class VehicleStateType { Unknown, From 097227ac5a3ab5cf5ddc4b5b41b2a53940ed4c57 Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Sat, 25 Dec 2021 15:26:32 +0100 Subject: [PATCH 38/87] bulk changes in ekf model: implement measurement models and refactor prediction models --- .../simple_flight/AirSimSimpleEkfModel.hpp | 744 ++++++++++++++++-- .../firmware/interfaces/IEkf.hpp | 7 +- 2 files changed, 677 insertions(+), 74 deletions(-) diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfModel.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfModel.hpp index 418537d2b1..550a6d15bd 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfModel.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfModel.hpp @@ -7,16 +7,9 @@ #include #include "firmware/interfaces/IUpdatable.hpp" #include "firmware/interfaces/IBoard.hpp" -#include "common/FrequencyLimiter.hpp" -#include "firmware/interfaces/IEkf.hpp" - -// #include "firmware/Params.hpp" -// #include "common/Common.hpp" -// #include "common/ClockFactory.hpp" -// #include "physics/Kinematics.hpp" - -constexpr float G_0 = 9.81f; -constexpr float R_E = 6357000.0f; + +float G_0 = EarthUtils::Gravity; // for less than 10000m const gravity is taken +float R_E = EARTH_RADIUS; namespace msr { @@ -25,12 +18,641 @@ namespace airlib class AirSimSimpleEkfModel { - protected: + public: // --------------------------------------------------------------------- // Mathematical functions // --------------------------------------------------------------------- - void evaluateStateDot(float f_local[10], float x[17], float u[6]) + 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]; + + // 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)); + attitude_dot[1] = 0.5f * ( q0*(omega_x + b_omega_x) - q3*(omega_y + b_omega_y) + q2*(omega_z + b_omega_z)); + attitude_dot[2] = 0.5f * ( q3*(omega_x + b_omega_x) + q0*(omega_y + b_omega_y) - q1*(omega_z + b_omega_z)); + attitude_dot[3] = 0.5f * (-q2*(omega_x + b_omega_x) + q1*(omega_y + b_omega_y) + q0*(omega_z + b_omega_z)); + } + + static void evaluateStateDot(float x_dot[17], float x[17], float u[6]) + { + 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(VectorMath::Matrix17x17f* A, float x[17], float u[6]) + { + *A = VectorMath::Matrix17x17f::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 + + // 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, 7) = -0.5f*(omega_x + b_omega_x); + (*A)(6, 8) = -0.5f*(omega_y + b_omega_y); + (*A)(6, 9) = -0.5f*(omega_z + b_omega_z); + (*A)(7, 6) = 0.5f*(omega_x + b_omega_x); + (*A)(7, 8) = 0.5f*(omega_z + b_omega_z); + (*A)(7, 9) = -0.5f*(omega_y + b_omega_y); + (*A)(8, 6) = 0.5f*(omega_y + b_omega_y); + (*A)(8, 7) = -0.5f*(omega_z + b_omega_z); + (*A)(8, 9) = 0.5f*(omega_x + b_omega_x); + (*A)(9, 6) = 0.5f*(omega_z + b_omega_z); + (*A)(9, 7) = 0.5f*(omega_y + b_omega_y); + (*A)(9, 8) = -0.5f*(omega_x + b_omega_x); + // 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(VectorMath::Matrix17x17f* A, float x[17], float u[6]) + { + *A = VectorMath::Matrix17x17f::Zero(); + + float derivative_perturbation = 1E-5f; + float x_plus[17]; + float x_minus[17]; + float f_plus[17]; + float f_minus[17]; + float df_dx_i_column[17]; + + for (int i=0; i<17; i++){ + for (int j=0; j<17; 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<17; j++){ + df_dx_i_column[j] = (f_plus[j] - f_minus[j])/(2*perturbation_i); + } + + for (int j=0; j<17; j++){ + (*A)(j,i) = df_dx_i_column[j]; + } + } + } + + static bool checkA(VectorMath::Matrix17x17f* A_error, VectorMath::Matrix17x17f* A, VectorMath::Matrix17x17f* A_finite, volatile float row[17], volatile 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] = i; + column[k] = 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(VectorMath::Matrix17x13f* B_w, float x[17], float u[6]) + { + // set all elements to zero + *B_w = VectorMath::Matrix17x13f::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(VectorMath::Matrix1x17f* C_baro) + { + // dh_baro_dx jacobian wrt the states vector + *C_baro = VectorMath::Matrix1x17f::Zero(); + (*C_baro)(2) = -1.0f; + (*C_baro)(16) = 1.0f; + } + + static void evaluateCGps(VectorMath::Matrix6x17f* C_gps) + { + // dh_gps_dx jacobian wrt the states vector + *C_gps = VectorMath::Matrix6x17f::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(VectorMath::Matrix3x17f* C_mag, float x[17], float earth_mag[3]) + { + // dh_mag_dx jacobian wrt the states vector + *C_mag = VectorMath::Matrix3x17f::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(VectorMath::Matrix1x17f* C_pseudo, float x[17]) + { + // dh_gps_dx jacobian wrt the states vector + *C_pseudo = VectorMath::Matrix1x17f::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 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[17], float x[17], float u[6], float up[6], 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; + + // x_predicted[10] = 0.0f; + // x_predicted[11] = 0.0f; + // x_predicted[12] = 0.0f; + // x_predicted[13] = 0.0f; + // x_predicted[14] = 0.0f; + // x_predicted[15] = 0.0f; + // x_predicted[16] = 0.0f; + } + + // OLD UNUSED METHODS BELOW!!! + // + // + + static void evaluateStateDotOld(float f_local[17], float x[17], float u[6]) { // compute x_dot = f_local /* @@ -65,44 +687,34 @@ namespace airlib f_local[1] = x[4]; f_local[2] = x[5]; // f_vel - f_local[3] = (x[6]*x[6] + x[7]*x[7] - x[8]*x[8] - x[9]*x[9])* (u[0] + x[10]) + f_local[3] = (x[6]*x[6] + x[7]*x[7] - x[8]*x[8] - x[9]*x[9])* (u[0] + x[10]) + 2.0f*(x[7]*x[8] - x[6]*x[9])* (u[1] + x[11]) + 2.0f*(x[6]*x[8] + x[7]*x[9])* (u[2] + x[12]); f_local[4] = 2.0f*(x[7]*x[8] + x[6]*x[9])* (u[0] + x[10]) - + (x[6]*x[6] - x[7]*x[7] + x[8]*x[8] - x[9]*x[9])* (u[1] + x[11]) + + (x[6]*x[6] - x[7]*x[7] + x[8]*x[8] - x[9]*x[9])* (u[1] + x[11]) + 2.0f*(x[8]*x[9] - x[6]*x[7])* (u[2] + x[12]); f_local[5] = 2.0f*(x[7]*x[9] - x[6]*x[8])* (u[0] + x[10]) + 2.0f*(x[8]*x[9] + x[6]*x[7])* (u[1] + x[11]) - + (x[6]*x[6] - x[7]*x[7] - x[8]*x[8] + x[9]*x[9])* (u[2] + x[12]) - + G_0*(1 + 2.0f*x[2]/R_E); + + (x[6]*x[6] - x[7]*x[7] - x[8]*x[8] + x[9]*x[9])* (u[2] + x[12]) + //+ G_0*(1 + 2.0f*x[2]/R_E); + + G_0;//+0.001f; // f_ori f_local[6] = 0.5f * (-x[7]*(u[3] + x[13]) - x[8]*(u[4] + x[14]) - x[9]*(u[5] + x[15])); - f_local[7] = 0.5f * ( x[6]*(u[3] + x[13]) - x[9]*(u[4] + x[14]) - x[8]*(u[5] + x[15])); + f_local[7] = 0.5f * ( x[6]*(u[3] + x[13]) - x[9]*(u[4] + x[14]) + x[8]*(u[5] + x[15])); f_local[8] = 0.5f * ( x[9]*(u[3] + x[13]) + x[6]*(u[4] + x[14]) - x[7]*(u[5] + x[15])); f_local[9] = 0.5f * (-x[8]*(u[3] + x[13]) + x[7]*(u[4] + x[14]) + x[6]*(u[5] + x[15])); + // f_bias + f_local[10] = 0.0f; + f_local[11] = 0.0f; + f_local[12] = 0.0f; + f_local[13] = 0.0f; + f_local[14] = 0.0f; + f_local[15] = 0.0f; + f_local[16] = 0.0f; } - - void h_mag() - { - // - } - - void h_baro() - { - // - } - - void h_GPS() - { - // - } - - // --------------------------------------------------------------------- - // Mathematical jacobians - // --------------------------------------------------------------------- - - void evaluateA(VectorMath::Matrix17x17f* A, float x[17], float u[6]) + + static void evaluateAOld(VectorMath::Matrix17x17f* A, float x[17], float u[6]) { // compute jacobian wrt the states vector df_dx = A /* @@ -201,11 +813,11 @@ namespace airlib (*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; + //(*A)(5, 2) = 2.0f*G_0/R_E; // df_vel_q0 [3 4 5][6] (*A)(3, 6) = 2.0f*x[6]*(u[0] + x[10]) - 2.0f*x[9]*(u[1] + x[11]) + 2.0f*x[8]*(u[2] + x[12]); (*A)(4, 6) = 2.0f*x[9]*(u[0] + x[10]) + 2.0f*x[6]*(u[1] + x[11]) - 2.0f*x[7]*(u[2] + x[12]); - (*A)(5, 6) = 2.0f*x[8]*(u[0] + x[10]) - 2.0f*x[7]*(u[1] + x[11]) + 2.0f*x[6]*(u[2] + x[12]); + (*A)(5, 6) = -2.0f*x[8]*(u[0] + x[10]) + 2.0f*x[7]*(u[1] + x[11]) + 2.0f*x[6]*(u[2] + x[12]); // df_vel_q1 [3 4 5][7] (*A)(3, 7) = 2.0f*x[7]*(u[0] + x[10]) + 2.0f*x[8]*(u[1] + x[11]) + 2.0f*x[9]*(u[2] + x[12]); (*A)(4, 7) = 2.0f*x[8]*(u[0] + x[10]) - 2.0f*x[7]*(u[1] + x[11]) - 2.0f*x[6]*(u[2] + x[12]); @@ -229,25 +841,25 @@ namespace airlib (*A)(5, 11) = 2.0f*(x[8]*x[9] + x[6]*x[7]); (*A)(5, 12) = x[6]*x[6] - x[7]*x[7] - x[8]*x[8] + x[9]*x[9]; // df_ori_[q0 q1 q2 q3] [6 7 8 9][6 7 8 9] - (*A)(6, 7)= -0.5f*(u[3] + x[13]); - (*A)(6, 8)= -0.5f*(u[4] + x[14]); - (*A)(6, 9)= -0.5f*(u[5] + x[15]); - (*A)(7, 6)= 0.5f*(u[3] + x[13]); - (*A)(7, 8)= -0.5f*(u[5] + x[15]); - (*A)(7, 9)= -0.5f*(u[4] + x[14]); - (*A)(8, 6)= 0.5f*(u[4] + x[14]); - (*A)(8, 7)= -0.5f*(u[5] + x[15]); - (*A)(8, 9)= 0.5f*(u[3] + x[13]); - (*A)(9, 6)= 0.5f*(u[5] + x[15]); - (*A)(9, 7)= 0.5f*(u[4] + x[14]); - (*A)(9, 8)= -0.5f*(u[3] + x[13]); + (*A)(6, 7) = -0.5f*(u[3] + x[13]); + (*A)(6, 8) = -0.5f*(u[4] + x[14]); + (*A)(6, 9) = -0.5f*(u[5] + x[15]); + (*A)(7, 6) = 0.5f*(u[3] + x[13]); + (*A)(7, 8) = 0.5f*(u[5] + x[15]); + (*A)(7, 9) = -0.5f*(u[4] + x[14]); + (*A)(8, 6) = 0.5f*(u[4] + x[14]); + (*A)(8, 7) = -0.5f*(u[5] + x[15]); + (*A)(8, 9) = 0.5f*(u[3] + x[13]); + (*A)(9, 6) = 0.5f*(u[5] + x[15]); + (*A)(9, 7) = 0.5f*(u[4] + x[14]); + (*A)(9, 8) = -0.5f*(u[3] + x[13]); // df_ori_x_bias [6 7 8 9][10 11 12 13 14 15 16 17] (*A)(6, 13) = -0.5f*x[7]; (*A)(6, 14) = -0.5f*x[8]; (*A)(6, 15) = -0.5f*x[9]; (*A)(7, 13) = 0.5f*x[6]; (*A)(7, 14) = -0.5f*x[9]; - (*A)(7, 15) = -0.5f*x[8]; + (*A)(7, 15) = 0.5f*x[8]; (*A)(8, 13) = 0.5f*x[9]; (*A)(8, 14) = 0.5f*x[6]; (*A)(8, 15) = -0.5f*x[7]; @@ -256,7 +868,7 @@ namespace airlib (*A)(9, 15) = 0.5f*x[6]; } - void evaluateB_w(VectorMath::Matrix17x13f* B_w, float x[17], float u[6]) + static void evaluateB_wOld(VectorMath::Matrix17x13f* B_w, float x[17], float u[6]) { // compute jacobian wrt the process noise vector df_dw /* @@ -340,7 +952,7 @@ namespace airlib (*B_w)(6, 5) = -0.5f*x[9]; (*B_w)(7, 3) = 0.5f*x[6]; (*B_w)(7, 4) = -0.5f*x[9]; - (*B_w)(7, 5) = -0.5f*x[8]; + (*B_w)(7, 5) = 0.5f*x[8]; (*B_w)(8, 3) = 0.5f*x[9]; (*B_w)(8, 4) = 0.5f*x[6]; (*B_w)(8, 5) = -0.5f*x[7]; @@ -348,29 +960,15 @@ namespace airlib (*B_w)(9, 4) = 0.5f*x[7]; (*B_w)(9, 5) = 0.5f*x[6]; // 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)(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; } - void dh_mag_dx() - { - // jacobian wrt the states vector - } - - void dh_baro_dx() - { - // jacobian wrt the states vector - } - - void dh_GPS_dx() - { - // jacobian wrt the states vector - } }; 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 index 42e5e8965d..5cdab8f5cd 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IEkf.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IEkf.hpp @@ -28,6 +28,11 @@ class IEkf : public IUpdatable return states_; } + const VectorMath::Vector17f& getEkfMeasurements() const + { + return measurement_; + } + // setters void setEkfCovariance(VectorMath::Matrix17x17f covariance) { @@ -43,7 +48,7 @@ class IEkf : public IUpdatable protected: VectorMath::Vector17f states_; VectorMath::Matrix17x17f covariance_; - VectorMath::Matrix13x13f Q_; + VectorMath::Vector17f measurement_ = VectorMath::Vector17f::Zero(); }; } //namespace \ No newline at end of file From 6cf0769dd08f8cd6a74a7c662e164d904a34d766 Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Sat, 25 Dec 2021 15:27:52 +0100 Subject: [PATCH 39/87] bulk changes in ekf: implement measurement updates and filter intialization --- .../simple_flight/AirSimSimpleEkf.hpp | 678 +++++++++++++++--- 1 file changed, 561 insertions(+), 117 deletions(-) diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp index 98a207cf6b..64ba24e6dc 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp @@ -9,7 +9,14 @@ #include "firmware/interfaces/ICommLink.hpp" #include "common/FrequencyLimiter.hpp" #include "AirSimSimpleEkfBase.hpp" -#include "AirSimSimpleEkfModel.hpp" +#include "AirSimSimpleEkfModel.hpp" +#include "common/GeodeticConverter.hpp" + +#define EKF_GROUND_TRUTH_MEAS_DIRECTIVE 0 +#define EKF_BARO_DIRECTIVE 1 +#define EKF_MAGNETO_DIRECTIVE 1 +#define EKF_GPS_DIRECTIVE 1 +#define EKF_PSEUDOMEAS_DIRECTIVE 1 namespace msr { @@ -25,7 +32,7 @@ namespace airlib : board_(board), comm_link_(comm_link) // commlink is only temporary here { // params_ = // get the EKF params from airsim settings. Implement this as a struct for now with constant members ! inspired by sensor model - freq_limiter_.initialize(334); // physics engine and the imu refresh at period 3ms ~ 333.33Hz + freq_limiter_.initialize(100); // physics engine and the imu refresh at period 3ms ~ 333.33Hz } virtual void reset() override @@ -33,7 +40,7 @@ namespace airlib IEkf::reset(); freq_limiter_.reset(); - initialize(); + initializeFilter(); } @@ -45,34 +52,203 @@ namespace airlib freq_limiter_.update(); // if the wait is complete and it is time to update EKF, update EKF - if (freq_limiter_.isWaitComplete()) - updateEKFInternal(); + // if (freq_limiter_.isWaitComplete()) + // updateEKFInternal(); - // updateEKFInternal(); + updateEKFInternal(); + } + + // only to debug and verify estimates + void setGroundTruthKinematics(const Kinematics::State* kinematics, const Environment* environment) + { + kinematics_ = kinematics; + environment_ = environment; } private: // --------------------------------------------------------------------- // Internal functions // --------------------------------------------------------------------- - // initialize filter - void initialize() + + // initialize filter + void initializeFilter() + { + simple_flight::SensorCharacteristics sensor_characteristics; + simple_flight::EkfInitialStates initial_states; + simple_flight::EkfInitialStdDevs initial_std_devs; + + // imu + sensor_characteristics.accel_std_dev.x() = 0.05f; // m/s^2 + sensor_characteristics.accel_std_dev.y() = 0.05f; // m/s^2 + sensor_characteristics.accel_std_dev.z() = 0.05f; // m/s^2 + sensor_characteristics.gyro_std_dev.x() = 0.1f; // deg/s + sensor_characteristics.gyro_std_dev.y() = 0.1f; // deg/s + sensor_characteristics.gyro_std_dev.z() = 0.1f; // deg/s + // gps + sensor_characteristics.gps_pos_std_dev.x() = 5.0f; // m + sensor_characteristics.gps_pos_std_dev.y() = 5.0f; // m + sensor_characteristics.gps_pos_std_dev.z() = 10.0f; // m + sensor_characteristics.gps_vel_std_dev.x() = 2.0f; // m/s + sensor_characteristics.gps_vel_std_dev.y() = 2.0f; // m/s + sensor_characteristics.gps_vel_std_dev.z() = 5.0f; // m/s + // barometer + sensor_characteristics.baro_std_dev = 1.0f; // m + // magnetomter + sensor_characteristics.mag_std_dev.x() = 0.01f; + sensor_characteristics.mag_std_dev.y() = 0.01f; + sensor_characteristics.mag_std_dev.z() = 0.01f; + + // initial position + initial_states.pos.x() = 2.0f; + initial_states.pos.y() = 2.0f; + initial_states.pos.z() = -10.0f + 2.0f; + // initial velocity + initial_states.vel.x() = 0.0f; + initial_states.vel.y() = 0.0f; + initial_states.vel.z() = 0.0f; + // initial quaternions + initial_states.quaternion.val4() = 0.98929; + initial_states.quaternion.x() = 0.0789265; + initial_states.quaternion.y() = 0.0940609; + initial_states.quaternion.z() = 0.0789265; + // initial biases + initial_states.accel_bias.x() = 0.0f; + initial_states.accel_bias.y() = 0.0f; + initial_states.accel_bias.z() = 0.0f; + initial_states.gyro_bias.x() = 0.0f; + initial_states.gyro_bias.y() = 0.0f; + initial_states.gyro_bias.z() = 0.0f; + initial_states.baro_bias = 0.0f; + + // initial position standard deviations + initial_std_devs.pos.x() = 5.0f; + initial_std_devs.pos.y() = 5.0f; + initial_std_devs.pos.z() = 10.0f; + // initial velocity standard deviations + initial_std_devs.vel.x() = 2.0f; + initial_std_devs.vel.y() = 2.0f; + initial_std_devs.vel.z() = 5.0f; + // initial quaternions standard deviations + initial_std_devs.quaternion.val4() = 0.1f; + initial_std_devs.quaternion.x() = 0.1f; + initial_std_devs.quaternion.y() = 0.1f; + initial_std_devs.quaternion.z() = 0.1f; + // initial biases standard deviations + initial_std_devs.accel_bias.x() = 0.05f; + initial_std_devs.accel_bias.y() = 0.05f; + initial_std_devs.accel_bias.z() = 0.05f; + initial_std_devs.gyro_bias.x() = 0.001f; + initial_std_devs.gyro_bias.y() = 0.001f; + initial_std_devs.gyro_bias.z() = 0.001f; + initial_std_devs.baro_bias = 0.1f; + + assignEkfMatrics(sensor_characteristics, initial_states, initial_std_devs); + resetIntialGlobalVariables(); + } + + void assignEkfMatrics(const simple_flight::SensorCharacteristics& sensor, + const simple_flight::EkfInitialStates& initial_states, + const simple_flight::EkfInitialStdDevs& initial_std_devs) { - // intiaalize with zero position and unity quaternion ! temporary | TODO do using setters - states_ = VectorMath::Vector17f::Zero(); - states_[6] = 1.0f; - covariance_ = VectorMath::Matrix17x17f::Identity()*0.01f; - //covariance_(2,2) = 1.0f; - Q_ = VectorMath::Matrix13x13f::Identity()*0.01f; - Q_(0, 0) = 0.25f; - Q_(1, 1) = 0.25f; - Q_(2, 2) = 0.25f; - Q_(3, 3) = 0.25f; - Q_(4, 4) = 0.25f; - Q_(5, 5) = 0.25f; + // imu + Q_(0, 0) = sensor.accel_std_dev.x()*sensor.accel_std_dev.x(); // accel_x + Q_(1, 1) = sensor.accel_std_dev.y()*sensor.accel_std_dev.y(); // accel_y + Q_(2, 2) = sensor.accel_std_dev.z()*sensor.accel_std_dev.z(); // accel_z + Q_(3, 3) = sensor.gyro_std_dev.x()*sensor.gyro_std_dev.x() * M_PI*M_PI/32400; // gyro_x + Q_(4, 4) = sensor.gyro_std_dev.y()*sensor.gyro_std_dev.y() * M_PI*M_PI/32400; // gyro_y + Q_(5, 5) = sensor.gyro_std_dev.z()*sensor.gyro_std_dev.z() * M_PI*M_PI/32400; // gyro_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_(0, 0) = sensor.gps_pos_std_dev.x()*sensor.gps_pos_std_dev.x(); // gps_pos_x + R_gps_(1, 1) = sensor.gps_pos_std_dev.y()*sensor.gps_pos_std_dev.y(); // gps_pos_y + R_gps_(2, 2) = sensor.gps_pos_std_dev.z()*sensor.gps_pos_std_dev.z(); // gps_pos_z + R_gps_(3, 3) = sensor.gps_vel_std_dev.x()*sensor.gps_vel_std_dev.x(); // gps_vel_x + R_gps_(4, 4) = sensor.gps_vel_std_dev.y()*sensor.gps_vel_std_dev.y(); // gps_vel_y + R_gps_(5, 5) = sensor.gps_vel_std_dev.z()*sensor.gps_vel_std_dev.z(); // gps_vel_z + + // magnetometer + R_mag_(0, 0) = sensor.mag_std_dev.x()*sensor.mag_std_dev.x(); // mag_x + R_mag_(1, 1) = sensor.mag_std_dev.y()*sensor.mag_std_dev.y(); // mag_y + R_mag_(2, 2) = sensor.mag_std_dev.z()*sensor.mag_std_dev.z(); // mag_z + + // barometer + R_baro_ = sensor.baro_std_dev*sensor.baro_std_dev; // baro_alt + + // intialize the ekf states + states_(0) = initial_states.pos.x(); + states_(1) = initial_states.pos.y(); + states_(2) = initial_states.pos.z(); + states_(3) = initial_states.vel.x(); + states_(4) = initial_states.vel.y(); + states_(5) = initial_states.vel.z(); + states_(6) = initial_states.quaternion.val4(); + states_(7) = initial_states.quaternion.x(); + states_(8) = initial_states.quaternion.y(); + states_(9) = initial_states.quaternion.z(); + states_(10) = initial_states.accel_bias.x(); + states_(11) = initial_states.accel_bias.y(); + states_(12) = initial_states.accel_bias.z(); + states_(13) = initial_states.gyro_bias.x(); + states_(14) = initial_states.gyro_bias.y(); + states_(15) = initial_states.gyro_bias.z(); + states_(16) = initial_states.baro_bias; + + // intitialize the ekf covariances + covariance_(0,0) = initial_std_devs.pos.x()*initial_std_devs.pos.x(); // x + covariance_(1,1) = initial_std_devs.pos.y()*initial_std_devs.pos.y(); // y + covariance_(2,2) = initial_std_devs.pos.z()*initial_std_devs.pos.z(); // z + covariance_(3,3) = initial_std_devs.vel.x()*initial_std_devs.vel.x(); // u + covariance_(4,4) = initial_std_devs.vel.y()*initial_std_devs.vel.y(); // v + covariance_(5,5) = initial_std_devs.vel.z()*initial_std_devs.vel.z(); // w + covariance_(6,6) = initial_std_devs.quaternion.val4()*initial_std_devs.quaternion.val4(); // q0 + covariance_(7,7) = initial_std_devs.quaternion.x()*initial_std_devs.quaternion.x(); // q1 + covariance_(8,8) = initial_std_devs.quaternion.y()*initial_std_devs.quaternion.y(); // q2 + covariance_(9,9) = initial_std_devs.quaternion.z()*initial_std_devs.quaternion.z(); // q3 + covariance_(10,10) = initial_std_devs.accel_bias.x()*initial_std_devs.accel_bias.x(); // + covariance_(11,11) = initial_std_devs.accel_bias.y()*initial_std_devs.accel_bias.y(); // + covariance_(12,12) = initial_std_devs.accel_bias.z()*initial_std_devs.accel_bias.z(); // + covariance_(13,13) = initial_std_devs.gyro_bias.x()*initial_std_devs.gyro_bias.x(); // + covariance_(14,14) = initial_std_devs.gyro_bias.y()*initial_std_devs.gyro_bias.y(); // + covariance_(15,15) = initial_std_devs.gyro_bias.z()*initial_std_devs.gyro_bias.z(); // + covariance_(16,16) = initial_std_devs.baro_bias*initial_std_devs.baro_bias; // + } + + void resetIntialGlobalVariables() + { + // reset last update times last_times_.state_propagation = board_->micros(); - last_times_.cov_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; + earth_mag_[0] = magnetic_field_true.x(); + earth_mag_[1] = magnetic_field_true.y(); + earth_mag_[2] = magnetic_field_true.z(); + + // reset imu data buffer + real_T accel[3]; + real_T gyro[3]; + bool is_new_and_valid = getImuData(accel, gyro); + 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 @@ -94,9 +270,6 @@ namespace airlib real_T accel[3]; real_T gyro[3]; - // imu updates at higher frequency than the EKF. - // !Downsampling, needed or automatic? does it have any side effects? - // check if the IMU gives new measurement and it is valid bool is_new_and_valid = getImuData(accel, gyro); @@ -106,83 +279,111 @@ namespace airlib statePropagation(accel, gyro); covariancePropagation(accel, gyro); - - /*std::ostringstream imu_str; - imu_str << accel[2]; - std::string messgae = " prediction step / imu step called (z-accelerometer) " + imu_str.str(); - comm_link_->log(messgae);*/ - - // std::ostringstream prediction_str; - // prediction_str << "Prediction: " << states_[0] - // << '\t' << states_[1] - // << '\t' << states_[2] - // << '\t' << states_[3] - // << '\t' << states_[4] - // << '\t' << states_[5] - // << '\t' << states_[6] - // << '\t' << states_[7] - // << '\t' << states_[8] - // << '\t' << states_[9] - // << '\t' ; - // std::string messgae = prediction_str.str(); - // comm_link_->log(messgae); } // measurement update step void measurementUpdateStep() { - magnetometerUpdate(); +#if EKF_BARO_DIRECTIVE == 1 barometerUpdate(); +#else +#endif +#if EKF_MAGNETO_DIRECTIVE == 1 + magnetometerUpdate(); +#else +#endif +#if EKF_GPS_DIRECTIVE == 1 gpsUpdate(); +#else +#endif +#if EKF_PSEUDOMEAS_DIRECTIVE == 1 + pseudoMeasurement(); +#else +#endif } // state propagtion void statePropagation(real_T* accel, real_T* gyro) { - float x_dot[10]; + // 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_dot[17]; float x[17]; float u[6]; + float uplus[6]; - // extract the states + // extract the current ekf states for (int i=0; i<17; i++){ x[i] = states_[i]; } - // extract the controls + // extract the current controls for (int i=0; i<3; i++){ - u[i] = accel[i]; - u[i+3] = gyro[i]; + uplus[i] = accel[i]; + u[i] = prev_imuData_.accel[i]; + uplus[i+3] = gyro[i]; + u[i+3] = prev_imuData_.gyro[i]; } - evaluateStateDot(x_dot,x,u); - - TTimeDelta delta_T = (board_->micros() - last_times_.state_propagation) / 1.0E6; // in seconds - last_times_.state_propagation = board_->micros(); + // 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]; - // euler forward integration + // do prediction float x_predicted[17]; - for (int i=0; i<10; i++){ - x_predicted[i] = x[i] + static_cast(delta_T*x_dot[i]); - } - for (int i=10; i<17; i++){ - x_predicted[i] = x[i]; - } + 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[17]; + // heun(x_predicted, x, u, uplus, dt_real); // set the predicted states TODO: via an interface or after some checks for (int i=0; i<17; i++){ - states_[i] = x_predicted[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) { - VectorMath::Matrix17x17f A; - VectorMath::Matrix17x13f B_w; + // 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[17]; float u[6]; + VectorMath::Matrix17x17f A; + VectorMath::Matrix17x17f A_finite; + VectorMath::Matrix17x13f B_w; + VectorMath::Matrix17x17f Phi; + VectorMath::Matrix17x13f GammaB_w; + VectorMath::Matrix17x17f P = covariance_; + VectorMath::Matrix17x17f next_covariance; - // extract the states + // extract the ekf states for (int i=0; i<17; i++){ x[i] = states_[i]; } @@ -193,22 +394,49 @@ namespace airlib 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); - TTimeDelta delta_T = (board_->micros() - last_times_.cov_propagation) / 1.0E6; // in seconds - last_times_.cov_propagation = board_->micros(); + evaluatePhiAndGamma_w(&Phi, &GammaB_w, &B_w, &A, dt_real); + // evaluatePhiAndGamma_w(&Phi, &GammaB_w, &B_w, &A_finite, dt_real); - VectorMath::Matrix17x17f Phi; - VectorMath::Matrix17x13f GammaB_w; - evaluatePhiAndGamma_w(&Phi, &GammaB_w, &B_w, &A, delta_T); - - VectorMath::Matrix17x17f next_covariance; - next_covariance = Phi*covariance_*Phi.transpose() + GammaB_w*Q_*GammaB_w.transpose(); + // evaluate next covariance matrix + next_covariance = Phi*P*Phi.transpose() + GammaB_w*Q_*GammaB_w.transpose(); // set the new predicted covariance covariance_ = next_covariance; + } + void evaluatePhiAndGamma_w( VectorMath::Matrix17x17f* Phi, + VectorMath::Matrix17x13f* GammaB_w, + VectorMath::Matrix17x13f* B_w, + VectorMath::Matrix17x17f* A, + float dt_real) + { + // declare local variables + VectorMath::Matrix17x17f identity = VectorMath::Matrix17x17f::Identity(); + VectorMath::Matrix17x17f A_square = (*A)*(*A); + VectorMath::Matrix17x17f A_cube = A_square*(*A); + VectorMath::Matrix17x17f A_forth = A_cube*(*A); + VectorMath::Matrix17x17f 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 @@ -217,8 +445,6 @@ namespace airlib if(!board_->checkMagnetometerIfNew()) return; - // the updates at the frequency of magnetometer signal update - real_T mag[3]; // check if the magnetometer gives new measurement and it is valid @@ -228,14 +454,46 @@ namespace airlib return; } - // auto C = dh_mag_dx(); + // 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 + VectorMath::Matrix3x17f C_mag; + evaluateCMag(&C_mag, x, earth_mag_); + + // calculate the Kalman gain matrix + VectorMath::Matrix17x17f P = covariance_; + VectorMath::Matrix3x3f inverse_term = (C_mag*P*C_mag.transpose() + R_mag_).inverse(); + VectorMath::Matrix17x3f kalman_gain = P * C_mag.transpose() * inverse_term; - // auto K = ... + // update states + float x_corrected[17]; + for (int i=0; i<17; 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 + VectorMath::Matrix17x17f P_corrected; + VectorMath::Matrix17x17f identity17x17 = VectorMath::Matrix17x17f::Identity(); + VectorMath::Matrix17x17f 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<17; i++){ + states_(i) = x_corrected[i]; + } + covariance_ = P_corrected; - // std::ostringstream mag_str; - // mag_str << mag[0]; - // std::string messgae = " magnetometer step called (x-magnetic flux) " + mag_str.str(); - // comm_link_->log(messgae); } // barometer update @@ -244,8 +502,6 @@ namespace airlib if(!board_->checkBarometerIfNew()) return; - // the updates at the frequency of barometer signal update - real_T altitude[1]; // check if the barometer gives new measurement and it is valid @@ -256,10 +512,38 @@ namespace airlib return; } - // std::ostringstream alt_str; - // alt_str << altitude[0]; - // std::string messgae = " barometer step called (baro altitude) " + alt_str.str(); - // comm_link_->log(messgae); + // extract the ekf states + float x[17]; + for (int i=0; i<17; i++){ + x[i] = states_(i); + } + + // evaluate the C matrix + VectorMath::Matrix1x17f C_baro; + evaluateCBaro(&C_baro); + + // calculate the Kalman gain matrix + VectorMath::Matrix17x17f P = covariance_; + float inverse_term = 1.0f/(C_baro*P*C_baro.transpose() + R_baro_); + VectorMath::Matrix17x1f kalman_gain = P * C_baro.transpose() * inverse_term; + + // update states + float x_corrected[17]; + for (int i=0; i<17; i++){ + x_corrected[i] = x[i] + kalman_gain[i]*(*altitude + x[2]); + } + + // update covariances + VectorMath::Matrix17x17f P_corrected; + VectorMath::Matrix17x17f identity17x17 = VectorMath::Matrix17x17f::Identity(); + VectorMath::Matrix17x17f 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<17; i++){ + states_(i) = x_corrected[i]; + } + covariance_ = P_corrected; } // GPS update @@ -268,42 +552,103 @@ namespace airlib if(!board_->checkGpsIfNew()) return; - // the updates at the frequency of GPS signal update - - double geo[3]; + double pos[3]; real_T vel[3]; // check if the GPS gives new measurement and it is valid - bool is_valid = getGpsData(geo, vel); + bool is_valid = getGpsData(pos, vel); if(!is_valid) { return; } - // std::ostringstream gps_str; - // gps_str << geo[2]; - // std::string messgae = " gps step called (gps altitude) " + gps_str.str(); - // comm_link_->log(messgae); + // extract the ekf states + float x[17]; + for (int i=0; i<17; i++){ + x[i] = states_(i); + } + + // evaluate the C matrix + VectorMath::Matrix6x17f C_gps; + evaluateCGps(&C_gps); + + // calculate the Kalman gain matrix + VectorMath::Matrix17x17f P = covariance_; + VectorMath::Matrix6x6f inverse_term = (C_gps*P*C_gps.transpose() + R_gps_).inverse(); + VectorMath::Matrix17x6f kalman_gain = P * C_gps.transpose() * inverse_term; + + // update the states + float x_corrected[17]; + for (int i=0; i<17; 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 + VectorMath::Matrix17x17f P_corrected; + VectorMath::Matrix17x17f identity17x17 = VectorMath::Matrix17x17f::Identity(); + VectorMath::Matrix17x17f 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<17; i++){ + states_(i) = x_corrected[i]; + } + covariance_ = P_corrected; + } - void evaluatePhiAndGamma_w( VectorMath::Matrix17x17f* Phi, - VectorMath::Matrix17x13f* GammaB_w, - VectorMath::Matrix17x13f* B_w, - VectorMath::Matrix17x17f* A, - TTimeDelta delta_T) + void pseudoMeasurement() { - // calculate Phi based on the jacobian for some iteration - VectorMath::Matrix17x17f identity = VectorMath::Matrix17x17f::Identity(); - VectorMath::Matrix17x17f A_square = *A*(*A); + if(!board_->checkGpsIfNew()) + return; - *Phi = identity - + *A * delta_T - + A_square * delta_T*delta_T/2; + // extract the states + float x[17]; + for (int i=0; i<17; 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 + VectorMath::Matrix1x17f C_pseudo = VectorMath::Matrix1x17f::Zero(); + evaluateCPseudo(&C_pseudo, x); + + // calculate the Kalman gain matrix + float R_pseudo = 0.0001f; + VectorMath::Matrix17x17f P = covariance_; + float inverse_term = 1.0f/(C_pseudo*P*C_pseudo.transpose() + R_pseudo); + VectorMath::Matrix17x1f kalman_gain = P* C_pseudo.transpose() * inverse_term; + + // update the ekf states + float x_corrected[17]; + for (int i=0; i<17; i++){ + x_corrected[i] = x[i] + kalman_gain[i]*(1.0f - norm_square); + } + + // covariance correction not done!!??? Is it correct?? + // VectorMath::Matrix17x17f P_corrected; + // VectorMath::Matrix17x17f identity17x17 = VectorMath::Matrix17x17f::Identity(); + // VectorMath::Matrix17x17f term = identity17x17 - kalman_gain*C_pseudo; + // P_corrected = term*P*term.transpose() + kalman_gain*R_pseudo*kalman_gain.transpose(); - *GammaB_w = (identity - + identity * delta_T - + *A * delta_T*delta_T/2)*(*B_w); + // write the states to the global variable + for (int i=0; i<17; i++){ + states_(i) = x_corrected[i]; + } + // covariance_ = P_corrected; } // --------------------------------------------------------------------- @@ -314,23 +659,89 @@ namespace airlib bool getImuData(real_T accel[3], real_T gyro[3]) { + +#if EKF_GROUND_TRUTH_MEAS_DIRECTIVE == 1 + // >>>>> only for verification, with ground truth 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); + accel[0] = linear_acceleration.x(); + accel[1] = linear_acceleration.y(); + accel[2] = linear_acceleration.z(); + gyro[0] = kinematics_->twist.angular.x(); + gyro[1] = kinematics_->twist.angular.y(); + gyro[2] = kinematics_->twist.angular.z(); + // >>>>> +#else board_->readImuData(accel, gyro); +#endif // 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 magnetometer data - bool getMagnetometerData(real_T mag[3]) + // reads GPS data + bool getGpsData(double pos[3], + real_T vel[3]) { - board_->readMagnetometerData(mag); + +#if EKF_GROUND_TRUTH_MEAS_DIRECTIVE == 1 + // >>>>> only for verification, with ground truth measurements + pos[0] = kinematics_->pose.position.x(); + pos[1] = kinematics_->pose.position.y(); + pos[2] = kinematics_->pose.position.z(); + // >>>>> +#else + double geo[3]; + board_->readGpsData(geo, vel); + + // GeoPoint geopoint; + // geopoint.latitude = geo[0]; + // geopoint.longitude = geo[1]; + // geopoint.altitude = geo[2]; + // GeoPoint geo_home = environment_->getHomeGeoPoint(); + + // Vector3r measured = EarthUtils::GeodeticToNedFast(geopoint, geo_home); + + GeoPoint geo_point; + Vector3r ned_pos; + geo_point.longitude = geo[0]; + geo_point.latitude = geo[1]; + geo_point.altitude = geo[2]; + geodetic_converter_.geodetic2Ned(geo_point, ned_pos); + + pos[0] = ned_pos[0]; + pos[1] = ned_pos[1]; + pos[2] = ned_pos[2]; + // pos[0] = geo[0]; + // pos[1] = geo[1]; + // pos[2] = geo[2]; +#endif // 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; } @@ -338,28 +749,49 @@ namespace airlib // reads barometer data bool getBarometerData(real_T* altitude) { +#if EKF_GROUND_TRUTH_MEAS_DIRECTIVE == 1 + altitude[0] = -1.0f*kinematics_->pose.position.z(); +#else board_->readBarometerData(altitude); +#endif // check if the signal has all data that is valid, else return false // TODO: check if at least a subset of data is valid + measurement_(12) = altitude[0]; + return true; } - - // reads GPS data - bool getGpsData(double geo[3], - real_T vel[3]) + + // reads magnetometer data + bool getMagnetometerData(real_T mag[3]) { - board_->readGpsData(geo, vel); + +#if EKF_GROUND_TRUTH_MEAS_DIRECTIVE == 1 + +#else + board_->readMagnetometerData(mag); +#endif // check if the signal has all data that is valid, else return false // TODO: check if at least a subset of data is valid + measurement_(13) = mag[0]; + measurement_(14) = mag[1]; + measurement_(15) = mag[2]; + return true; } + struct ImuDataBuffer + { + float accel[3]; + float gyro[3]; + float ang_acc[3]; + }; + private: // --------------------------------------------------------------------- // Class attritubes @@ -372,6 +804,18 @@ namespace airlib const simple_flight::IBoard* board_; simple_flight::ICommLink* comm_link_; // commlink is only temporary here, later transfer logging to OffBoardApi via StateEstimator + const Kinematics::State* kinematics_; + const Environment* environment_; + GeodeticConverter geodetic_converter_; + + VectorMath::Matrix13x13f Q_; + VectorMath::Matrix6x6f R_gps_; + VectorMath::Matrix3x3f R_mag_; + real_T R_baro_; + + ImuDataBuffer prev_imuData_; + float earth_mag_[3]; + }; } From 389ea387f6b432ebbad22241ccc31681f4ed3411 Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Sat, 25 Dec 2021 15:29:20 +0100 Subject: [PATCH 40/87] add running changes in peripheral scripts --- .gitignore | 7 +- AirLibUnitTests/JacobianTest.hpp | 126 +++++ AirLibUnitTests/SimpleFlightTest.hpp | 52 +- AirLibUnitTests/cleanup.sh | 6 +- AirLibUnitTests/compile.sh | 18 + AirLibUnitTests/main.cpp | 16 +- AirLibUnitTests/plot.py | 761 ++++++++++++++++++++++++++- AirLibUnitTests/style.mplstyle | 55 ++ 8 files changed, 1010 insertions(+), 31 deletions(-) create mode 100644 AirLibUnitTests/JacobianTest.hpp create mode 100644 AirLibUnitTests/compile.sh create mode 100644 AirLibUnitTests/style.mplstyle diff --git a/.gitignore b/.gitignore index 146d6d9169..db1a20574e 100644 --- a/.gitignore +++ b/.gitignore @@ -389,5 +389,8 @@ build_docs/ PythonClient/docs/_build -# custom: ignore log -log.txt +# custom: ignore +/AirLibUnitTests/data +/AirLibUnitTests/figures +/AirLibUnitTests/log.txt +/AirLibUnitTests/other.txt diff --git a/AirLibUnitTests/JacobianTest.hpp b/AirLibUnitTests/JacobianTest.hpp new file mode 100644 index 0000000000..b41469e53c --- /dev/null +++ b/AirLibUnitTests/JacobianTest.hpp @@ -0,0 +1,126 @@ + +#ifndef Jacobian_Test_hpp +#define Jacobian_Test_hpp + +#include "TestBase.hpp" +#include "vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfModel.hpp" + +namespace msr +{ +namespace airlib +{ + class JacobianTest : public TestBase + { + public: + virtual void run() override + { + float x[17] = {1.0f, 2.0f, 3.0f, + 1.2f, 2.3f, 3.4f, + 0.9f, 0.1f, 0.2f, 0.3f, + 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}; + float u[6] = {0.0f, 0.0f, -9.80665f, 1.0f, 2.0f, 3.0f}; + float u2[6] = {0.0f, 0.0f, -9.80665f, -3.23f, -1.38f, -0.2f}; + float x_dot[17]; + float x_dotdot[17]; + float x_dotdotdot[17]; + + AirSimSimpleEkfModel::evaluateStateDot(x_dot,x,u); + AirSimSimpleEkfModel::evaluateStateDot(x_dotdot,x_dot,u2); + AirSimSimpleEkfModel::evaluateStateDot(x_dotdotdot,x_dotdot,u2); + + float x_dotNew[17]; + float x_dotdotNew[17]; + float x_dotdotdotNew[17]; + + AirSimSimpleEkfModel::evaluateStateDotOld(x_dotNew,x,u); + AirSimSimpleEkfModel::evaluateStateDotOld(x_dotdotNew,x_dotNew,u2); + AirSimSimpleEkfModel::evaluateStateDotOld(x_dotdotdotNew,x_dotdotNew,u2); + + for (int i=0; i<17; i++) + { + std::cout << x[i] << '\t'; + std::cout << x_dot[i] << '\t'; + std::cout << x_dotdot[i] << '\t'; + std::cout << x_dotdotdot[i] << '\n'; + } + for (int i=0; i<17; i++) + { + std::cout << x[i] << '\t'; + std::cout << x_dotNew[i] << '\t'; + std::cout << x_dotdotNew[i] << '\t'; + std::cout << x_dotdotdotNew[i] << '\n'; + } + VectorMath::Matrix17x17f A; + VectorMath::Matrix17x17f ANew; + AirSimSimpleEkfModel::evaluateA(&A, x_dotdotdot,u2); + AirSimSimpleEkfModel::evaluateAOld(&ANew, x_dotdotdot,u2); + + for (int i=0; i<17; i++){ + for (int j=0; j<17; j++){ + std::cout << A(i,j) << "\t "; + } + std::cout << '\n'; + } + + for (int i=0; i<17; i++){ + for (int j=0; j<17; j++){ + std::cout << ANew(i,j) << "\t "; + } + std::cout << '\n'; + } + + // QuaternionT q = VectorMath::toQuaternion(RealT pitch, RealT roll, RealT yaw); + Quaternionr q = VectorMath::toQuaternion(10.0*M_PI/180, 10.0*M_PI/180, 10.0*M_PI/180); + std::cout << q.w() << '\n'; + std::cout << q.x() << '\n'; + std::cout << q.y() << '\n'; + std::cout << q.z() << '\n'; + + + // VectorMath::Matrix17x17f A_finite; + // VectorMath::Matrix17x17f A; + // VectorMath::Matrix17x17f A_error; + + // // evaluateA(&A, x, u); + // AirSimSimpleEkfModel::evaluateFiniteDifferenceA(&A_finite, x_dotdotdot,u2); + // AirSimSimpleEkfModel::evaluateA(&A, x_dotdotdot,u2); + + // volatile bool isOK; + // volatile float row[17]; + // volatile float column[17]; + // isOK = AirSimSimpleEkfModel::checkA(&A_error, &A, &A_finite, row, column); + + // std::cout << '\n'; + // for (int i=0; i<17; i++) + // { + // for (int j=0; j<17; j++) + // { + // std::cout << A_finite(i,j) << "\t "; + // } + // std::cout << '\n'; + // } + // std::cout << '\n'; + // for (int i=0; i<17; i++) + // { + // for (int j=0; j<17; j++) + // { + // std::cout << A(i,j) << "\t "; + // } + // std::cout << '\n'; + // } + // std::cout << '\n'; + // for (int i=0; i<17; i++) + // { + // for (int j=0; j<17; j++) + // { + // std::cout << A_error(i,j) << "\t "; + // } + // std::cout << '\n'; + // } + + } + + }; +} +} +#endif \ No newline at end of file diff --git a/AirLibUnitTests/SimpleFlightTest.hpp b/AirLibUnitTests/SimpleFlightTest.hpp index fe5517515b..a2fe7b44c5 100644 --- a/AirLibUnitTests/SimpleFlightTest.hpp +++ b/AirLibUnitTests/SimpleFlightTest.hpp @@ -36,6 +36,7 @@ namespace airlib { std::cout << std::endl; auto clock = std::make_shared(3E-3f); + // auto clock = std::make_shared(1E-1f); ClockFactory::get(clock); SensorFactory sensor_factory; @@ -55,11 +56,21 @@ namespace airlib Kinematics::State initial_kinematic_state = Kinematics::State::zero(); initial_kinematic_state.pose = Pose(); + // states_(6) = 0.9961946f; // q0 + // states_(7) = 0.0f; // q1 + // states_(8) = 0.08715574f; // q2 + + // initial_kinematic_state.pose.orientation.w() = 0.9961946f; + // initial_kinematic_state.pose.orientation.x() = 0.0f; + // initial_kinematic_state.pose.orientation.y() = 0.08715574f; + // initial_kinematic_state.pose.orientation.z() = 0.0f; + initial_kinematic_state.pose.position.z() = -10.0f; kinematics.reset(new Kinematics(initial_kinematic_state)); Environment::State initial_environment; initial_environment.position = initial_kinematic_state.pose.position; initial_environment.geo_point = GeoPoint(); + initial_environment.geo_point.altitude = 0.0f; // do not set it equal to kinematics z, this value goes into home geo point and acts as the ref for kinematics z environment.reset(new Environment(initial_environment)); // crete and initialize body and physics world @@ -67,7 +78,7 @@ namespace airlib std::vector vehicles = { &vehicle }; std::unique_ptr physics_engine(new FastPhysicsEngine()); - PhysicsWorld physics_world(std::move(physics_engine), vehicles, static_cast(clock->getStepSize() * 1E9)); + PhysicsWorld physics_world(std::move(physics_engine), vehicles);//, static_cast(clock->getStepSize() * 1E9)); // world.startAsyncUpdator(); called in the physics_world constructor // added by Suman, to fix calling update before reset https://github.com/microsoft/AirSim/issues/2773#issuecomment-703888477 @@ -88,7 +99,7 @@ namespace airlib std::string message; testAssert(api->isReady(message), message); - clock->sleep_for(0.04f); + clock->sleep_for(6.0f); Utils::getSetMinLogLevel(true, 100); @@ -115,21 +126,36 @@ namespace airlib std::cout << "took-off position: " << pos << std::endl; //checkStatusMsg(api.get(), &myfile); - clock->sleep_for(20.0f); - + // api->commandAngleRatesZ(0.0f, 0.0f, 0.1f, 0.0f); + // clock->sleep_for(180.0f); + // api->moveByAngleRatesZ(0.0f, 0.0f, 9.0f*M_PI/180, -1.53509f, 10.0f); + // clock->sleep_for(180.0f); + // api->moveByAngleRatesZ(0.0f, 4.0f*M_PI/180, 0.0f, -1.53509f, 10.0f); + // clock->sleep_for(60.0f); + // api->moveByAngleRatesZ(0.0f, 4.0f*M_PI/180, 0.0f, -1.53509f, 10.0f); // fly towards a waypoint - //api->moveToPosition(-50, -50, -50, 50, 1E3, DrivetrainType::MaxDegreeOfFreedom, YawMode(true, 0), -1, 0); - //pos = api->getMultirotorState().getPosition(); - //std::cout << "waypoint position: " << pos << std::endl; + // api->moveToPosition(-1, 0, -1.53509, 0.1, 1E3, DrivetrainType::MaxDegreeOfFreedom, YawMode(true, 0), -1, 0); + // pos = api->getMultirotorState().getPosition(); + // std::cout << "waypoint position: " << pos << std::endl; + // api->moveToPosition(0, -1, -1.53509, 0.1, 1E3, DrivetrainType::MaxDegreeOfFreedom, YawMode(true, 0), -1, 0); + // pos = api->getMultirotorState().getPosition(); + std::cout << "waypoint position: " << pos << std::endl; //checkStatusMsg(api.get(), &myfile); - // clock->sleep_for(2.0f); + // clock->sleep_for(60.0f); + // // fly towards a waypoint + // api->moveToPosition(15, 17, -20, 5, 1E3, DrivetrainType::MaxDegreeOfFreedom, YawMode(true, 0), -1, 0); + // pos = api->getMultirotorState().getPosition(); + // //std::cout << "waypoint position: " << pos << std::endl; + // //checkStatusMsg(api.get(), &myfile); - // land - //api->land(10); - pos = api->getMultirotorState().getPosition(); - std::cout << "final position: " << pos << std::endl; - checkStatusMsg(api.get(), &myfile); + // clock->sleep_for(60.0f); + + // // land + // //api->land(10); + // pos = api->getMultirotorState().getPosition(); + // std::cout << "final position: " << pos << std::endl; + checkStatusMsg(api.get(), &myfile); // TODO print some values OR log diff --git a/AirLibUnitTests/cleanup.sh b/AirLibUnitTests/cleanup.sh index 38d3d5bf50..99b05df8d4 100644 --- a/AirLibUnitTests/cleanup.sh +++ b/AirLibUnitTests/cleanup.sh @@ -3,6 +3,8 @@ cleanup(){ in_file="$1" data_file="$2" log_file="$3" - cat "$in_file" | sed -E '/^[^0-9]*$/d' > "$data_file" - cat "$in_file" | sed -E '/^[0-9]+.*$/d' > "$log_file" + # cat "$in_file" | sed -E '/^[^0-9]*$/d' > "$data_file" + # cat "$in_file" | sed -E '/^[0-9]+.*$/d' > "$log_file" + cat "log.txt" | sed -E '/^[^0-9]*$/d' > "data.csv" + cat "log.txt" | sed -E '/^[0-9]+.*$/d' > "other.txt" } diff --git a/AirLibUnitTests/compile.sh b/AirLibUnitTests/compile.sh new file mode 100644 index 0000000000..c072e56c94 --- /dev/null +++ b/AirLibUnitTests/compile.sh @@ -0,0 +1,18 @@ +#!/bin/bash +compile(){ + debug_flag="$1" + if [[ $debug_flag == 'd' ]]; then + cd ../build_debug/AirLibUnitTests + make + cd ../../AirLibUnitTests + echo "Debug mode, built only" + else + cd ../build_release/AirLibUnitTests + make + cd ../output/bin + ./AirLibUnitTests + cd ../../../AirLibUnitTests + echo "Release mode, built and run" + fi +} + diff --git a/AirLibUnitTests/main.cpp b/AirLibUnitTests/main.cpp index a3935bb7fc..cf384ed80a 100644 --- a/AirLibUnitTests/main.cpp +++ b/AirLibUnitTests/main.cpp @@ -5,23 +5,35 @@ #include "WorkerThreadTest.hpp" #include "QuaternionTest.hpp" #include "CelestialTests.hpp" +#include "JacobianTest.hpp" + +#include int main() { using namespace msr::airlib; std::unique_ptr tests[] = { - std::unique_ptr(new QuaternionTest()), + //std::unique_ptr(new QuaternionTest()), // std::unique_ptr(new CelestialTest()), - std::unique_ptr(new SettingsTest()), + //std::unique_ptr(new SettingsTest()), std::unique_ptr(new SimpleFlightTest()) + // std::unique_ptr(new JacobianTest()) //, //std::unique_ptr(new PixhawkTest()), //std::unique_ptr(new WorkerThreadTest()) }; + std::chrono::time_point start, end; + + start = std::chrono::system_clock::now(); for (auto& test : tests) test->run(); + end = std::chrono::system_clock::now(); + + std::chrono::duration elapsed_seconds = end - start; + std::cout << "elapsed time (s): " << elapsed_seconds.count() << "s\n"; + std::cout << "elapsed time (min): " << elapsed_seconds.count()/60.0f << "s\n"; return 0; } diff --git a/AirLibUnitTests/plot.py b/AirLibUnitTests/plot.py index 3ef94b7eb3..632b0ca930 100644 --- a/AirLibUnitTests/plot.py +++ b/AirLibUnitTests/plot.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python3 +#!/usr/bin python3 """ Module Docstring """ @@ -8,33 +8,770 @@ __license__ = "MIT" import argparse +import math import numpy as np +import pandas as pd import matplotlib as mpl import matplotlib.pyplot as plt def main(): """ Main entry point of the app """ - - print("hello world") parser = argparse.ArgumentParser() parser.add_argument("--filename", help="filename of the txt file") args = parser.parse_args() - print(args.filename) + if args.filename == None: + data = pd.read_csv('data.csv', delimiter='\t', header=None) + else: + data = pd.read_csv(args.filename, delimiter='\t', header=None) + + # print(data.head()) + + plotResults(data) + +def M_OB(q0, q1, q2, q3) -> np.array: + """ Direction cosine matrix """ + return np.array([[q0*q0 + q1*q1 - q2*q2 - q3*q3, 2.0*(q1*q2 - q0*q3), 2.0*(q0*q2 + q1*q3)], + [2.0*(q1*q2 + q0*q3), q0*q0 - q1*q1 + q2*q2 - q3*q3, 2.0*(q2*q3 - q0*q1)], + [2.0*(q1*q3 - q0*q2), 2.0*(q2*q3 + q0*q1), q0*q0 - q1*q1 - q2*q2 + q3*q3]]) + +def plotResults(data): + i=0 + # extract time stamps + timestamp = data[i].to_numpy() + i+=1 + start_time = timestamp[0] + timestamp = (timestamp - start_time)*0.001 + + # extract truth sensor measurements + true_accel_x = data[i].to_numpy(float) + i+=1 + true_accel_y = data[i].to_numpy(float) + i+=1 + true_accel_z = data[i].to_numpy(float) + i+=1 + true_gyro_x = data[i].to_numpy(float)*180/np.pi + i+=1 + true_gyro_y = data[i].to_numpy(float)*180/np.pi + i+=1 + true_gyro_z = data[i].to_numpy(float)*180/np.pi + i+=1 + + # extract sensor measurements + accel_x = data[i].to_numpy(float) + i+=1 + accel_y = data[i].to_numpy(float) + i+=1 + accel_z = data[i].to_numpy(float) + i+=1 + gyro_x = data[i].to_numpy(float)*180/np.pi + i+=1 + gyro_y = data[i].to_numpy(float)*180/np.pi + i+=1 + gyro_z = data[i].to_numpy(float)*180/np.pi + i+=1 + gps_pos_x = data[i].to_numpy(float) + i+=1 + gps_pos_y = data[i].to_numpy(float) + i+=1 + gps_pos_z = data[i].to_numpy(float) + i+=1 + gps_vel_x = data[i].to_numpy(float) + i+=1 + gps_vel_y = data[i].to_numpy(float) + i+=1 + gps_vel_z = data[i].to_numpy(float) + i+=1 + baro_alt = data[i].to_numpy(float) + i+=1 + mag_flux_x = data[i].to_numpy(float) + i+=1 + mag_flux_y = data[i].to_numpy(float) + i+=1 + mag_flux_z = data[i].to_numpy(float) + i+=1 + + # extract ground truth states + ground_truth_x = data[i].to_numpy(float) + i+=1 + ground_truth_y = data[i].to_numpy(float) + i+=1 + ground_truth_z = data[i].to_numpy(float) + i+=1 + ground_truth_q0 = data[i].to_numpy(float) + i+=1 + ground_truth_q1 = data[i].to_numpy(float) + i+=1 + ground_truth_q2 = data[i].to_numpy(float) + i+=1 + ground_truth_q3 = data[i].to_numpy(float) + i+=1 + ground_truth_pitch= data[i].to_numpy(float)*180/np.pi + i+=1 + ground_truth_roll = data[i].to_numpy(float)*180/np.pi + i+=1 + ground_truth_yaw = data[i].to_numpy(float)*180/np.pi + i+=1 + true_lin_vel_x = data[i].to_numpy(float) + i+=1 + true_lin_vel_y = data[i].to_numpy(float) + i+=1 + true_lin_vel_z = data[i].to_numpy(float) + i+=1 + + # estimated ekf states + estimated_x = data[i].to_numpy(float) + i+=1 + estimated_y = data[i].to_numpy(float) + i+=1 + estimated_z = data[i].to_numpy(float) + i+=1 + estimated_q0 = data[i].to_numpy(float) + i+=1 + estimated_q1 = data[i].to_numpy(float) + i+=1 + estimated_q2 = data[i].to_numpy(float) + i+=1 + estimated_q3 = data[i].to_numpy(float) + i+=1 + estimated_pitch = data[i].to_numpy(float)*180/np.pi + i+=1 + estimated_roll = data[i].to_numpy(float)*180/np.pi + i+=1 + estimated_yaw = data[i].to_numpy(float)*180/np.pi + i+=1 + esti_lin_vel_x = data[i].to_numpy(float) + i+=1 + esti_lin_vel_y = data[i].to_numpy(float) + i+=1 + esti_lin_vel_z = data[i].to_numpy(float) + i+=1 + bias_accel_x = data[i].to_numpy(float) + i+=1 + bias_accel_y = data[i].to_numpy(float) + i+=1 + bias_accel_z = data[i].to_numpy(float) + i+=1 + bias_gyro_x = data[i].to_numpy(float) + i+=1 + bias_gyro_y = data[i].to_numpy(float) + i+=1 + bias_gyro_z = data[i].to_numpy(float) + i+=1 + bias_baro = data[i].to_numpy(float) + i+=1 + + # variance of position + cov_x = (data[i].to_numpy(float)) + i+=1 + cov_y = (data[i].to_numpy(float)) + i+=1 + cov_z = (data[i].to_numpy(float)) + i+=1 + + # variance of linear velocity + cov_u = (data[i].to_numpy(float)) + i+=1 + cov_v = (data[i].to_numpy(float)) + i+=1 + cov_w = (data[i].to_numpy(float)) + i+=1 + + # extract covariance of angles + cov_q0 = (data[i].to_numpy(float)) + i+=1 + cov_q1 = (data[i].to_numpy(float)) + i+=1 + cov_q2 = (data[i].to_numpy(float)) + i+=1 + cov_q3 = (data[i].to_numpy(float)) + i+=1 + + # variance of imu bias + cov_b_f_x = (data[i].to_numpy(float)) + i+=1 + cov_b_f_y = (data[i].to_numpy(float)) + i+=1 + cov_b_f_z = (data[i].to_numpy(float)) + i+=1 + + # variance of gyro bias + cov_b_omega_x = (data[i].to_numpy(float)) + i+=1 + cov_b_omega_y = (data[i].to_numpy(float)) + i+=1 + cov_b_omega_z = (data[i].to_numpy(float)) + i+=1 + + # variance baro + cov_b_baro = data[i].to_numpy(float) + i+=1 + + # quaternion norm + quat_norm = data[i].to_numpy(float) + i+=1 + + # quaternion off diag cov + cov_q0_q1 = data[i].to_numpy(float) + i+=1 + cov_q0_q2 = data[i].to_numpy(float) + i+=1 + cov_q0_q3 = data[i].to_numpy(float) + i+=1 + cov_q1_q2 = data[i].to_numpy(float) + i+=1 + cov_q1_q3 = data[i].to_numpy(float) + i+=1 + cov_q2_q3 = data[i].to_numpy(float) + i+=1 + + # quaternion gyro bias cov + cov_q0_omega_x = data[i].to_numpy(float) + i+=1 + cov_q0_omega_y = data[i].to_numpy(float) + i+=1 + cov_q0_omega_z = data[i].to_numpy(float) + i+=1 + cov_q1_omega_x = data[i].to_numpy(float) + i+=1 + cov_q1_omega_y = data[i].to_numpy(float) + i+=1 + cov_q1_omega_z = data[i].to_numpy(float) + i+=1 + cov_q2_omega_x = data[i].to_numpy(float) + i+=1 + cov_q2_omega_y = data[i].to_numpy(float) + i+=1 + cov_q2_omega_z = data[i].to_numpy(float) + i+=1 + cov_q3_omega_x = data[i].to_numpy(float) + i+=1 + cov_q3_omega_y = data[i].to_numpy(float) + i+=1 + cov_q3_omega_z = data[i].to_numpy(float) + i+=1 + + # position error + err_estimated_x = ground_truth_x - estimated_x + err_estimated_y = ground_truth_y - estimated_y + err_estimated_z = ground_truth_z - estimated_z + + # angle error + err_estimated_pitch = ground_truth_pitch - estimated_pitch + err_estimated_roll = ground_truth_roll - estimated_roll + err_estimated_yaw = ground_truth_yaw - estimated_yaw + + # orientation error + error_q0 = ground_truth_q0 - estimated_q0 + error_q1 = ground_truth_q1 - estimated_q1 + error_q2 = ground_truth_q2 - estimated_q2 + error_q3 = ground_truth_q3 - estimated_q3 + + # orientation error + + + # velocity error + err_lin_vel_x = true_lin_vel_x - esti_lin_vel_x + err_lin_vel_y = true_lin_vel_y - esti_lin_vel_y + err_lin_vel_z = true_lin_vel_z - esti_lin_vel_z + + # sensor errors + err_accel_x = true_accel_x - accel_x + err_accel_y = true_accel_y - accel_y + err_accel_z = true_accel_z - accel_z + err_gyro_x = true_gyro_x - gyro_x + err_gyro_y = true_gyro_y - gyro_y + err_gyro_z = true_gyro_z - gyro_z + err_gps_x = ground_truth_x - gps_pos_x + err_gps_y = ground_truth_y - gps_pos_y + err_gps_z = ground_truth_z - gps_pos_z + err_gps_vel_x = true_lin_vel_x - gps_vel_x + err_gps_vel_y = true_lin_vel_y - gps_vel_y + err_gps_vel_z = true_lin_vel_z - gps_vel_z + err_baro_alt = -1*ground_truth_z - baro_alt + + # print(gps_vel_z) + + start_index = np.where(np.int_(timestamp) == 5)[0][0] + end_index = np.where(np.int_(timestamp) == 6)[0][0] + + # print("True pitch rate 1: ", true_gyro_x[start_index]) + # print("True pitch rate 2: ", true_gyro_x[end_index]) + + # print("True pitch angle 1: ", ground_truth_roll[start_index]) + # print("Estimated pitch angle 1: ", estimated_roll[start_index]) + + # print("True pitch angle 2: ", ground_truth_roll[end_index]) + # print("Estimated pitch angle 2: ", estimated_roll[end_index]) + + # print("Time difference: ", timestamp[end_index] - timestamp[start_index]) + print("Expected delta pitch angle: ", (true_gyro_x[start_index]+true_gyro_x[end_index])*0.5*(timestamp[end_index] - timestamp[start_index])) + print("AirSim delta pitch angle: ", (ground_truth_roll[end_index] - ground_truth_roll[start_index])) + print("Our delta pitch angle: ", (estimated_roll[end_index] - estimated_roll[start_index])) + + start_index = np.where(np.int_(timestamp) == 6)[0][0] + end_index = np.where(np.int_(timestamp) == 7)[0][0] + print("Expected delta pitch angle: ", (true_gyro_x[start_index]+true_gyro_x[end_index])*0.5*(timestamp[end_index] - timestamp[start_index])) + print("AirSim delta pitch angle: ", (ground_truth_roll[end_index] - ground_truth_roll[start_index])) + print("Our delta pitch angle: ", (estimated_roll[end_index] - estimated_roll[start_index])) + + # k = np.array([[1, 2, 3],[4, 5, 6],[7, 8, 9]]) + # print(k) + # print(k[2,1]) + + theta_error = np.zeros(len(timestamp)) + phi_error = np.zeros(len(timestamp)) + psi_error = np.zeros(len(timestamp)) + # print(psi_error) + + for i in range(0, len(timestamp)): + ground = M_OB(ground_truth_q0[i], + ground_truth_q1[i], + ground_truth_q2[i], + ground_truth_q3[i]) + estimated = M_OB(estimated_q0[i], + estimated_q1[i], + estimated_q2[i], + estimated_q3[i]) + # print(ground_error) + + error_direction_cosine = ground*estimated.transpose() + # print(error_direction_cosine[2, 0]) + + # euler = [atan2(mat(3,2),mat(3,3)); + # -asin(mat(3,1)); + # atan2(mat(2,1),mat(1,1))]; - with open('log.txt', 'rb') as raw_data: - data = np.loadtxt(raw_data, delimiter='\t') + phi_error[i] = np.arctan2(error_direction_cosine[2, 1],error_direction_cosine[2, 2])*180/np.pi + theta_error[i] = -np.arcsin(error_direction_cosine[2, 0])*180/np.pi + psi_error[i] = np.arctan2(error_direction_cosine[1, 0],error_direction_cosine[0, 0])*180/np.pi - X = np.linspace(0, 2*np.pi, 100) - Y = np.cos(X) - fig, ax = plt.subplots() - ax.plot(X, Y, color='C1') + # apply style + plt.style.use('./style.mplstyle') + + # define the plot + fig, ax = plt.subplots(25, 1) + + # begin plotting + i=0 + ax[i].plot(timestamp, accel_x, linestyle='dotted', color='C1', label="x") + ax[i].legend() + ax[i].plot(timestamp, accel_y, linestyle='solid', color='C3', label="y") + ax[i].legend() + ax[i].plot(timestamp, accel_z, linestyle='dotted', color='C5', label="z") + ax[i].legend() + ax[i].set_ylabel('accel (m/s2)') + # ax[i].set_title('Measurements') + # ax[i].set_xlim([10, 15]) + # ax[i].set_ylim([-15, 5]) + ax[i].grid(True) + + # i+=1 + # ax[i].plot(timestamp, gyro_x, linestyle='solid', color='C1', label="roll") + # ax[i].legend() + # ax[i].plot(timestamp, gyro_y, linestyle='solid', color='C2', label="pitch") + # ax[i].legend() + # ax[i].plot(timestamp, gyro_z, linestyle='dotted', color='C5', label="yaw") + # ax[i].legend() + # ax[i].set_ylabel('gyro error(deg/s)') + # ax[i].set_xlim([10, 15]) + # ax[i].set_ylim([-0.01, 0.01]) + # ax[i].grid(True) + + # i+=1 + # ax[i].plot(timestamp, err_baro_alt, linestyle='solid', color='C1') + # ax[i].legend() + # ax[i].set_ylabel('baro alt err(m)') + # ax[i].grid(True) + + i+=1 + ax[i].plot(timestamp, gyro_x, linestyle='dotted', color='C1', label="roll") + ax[i].legend() + ax[i].plot(timestamp, gyro_y, linestyle='solid', color='C2', label="pitch") + ax[i].legend() + ax[i].plot(timestamp, gyro_z, linestyle='dotted', color='C5', label="yaw") + ax[i].legend() + ax[i].set_ylabel('gyro rates (deg/s)') + # ax[i].set_xlim([10, 15]) + # ax[i].set_ylim([-1, 10]) + ax[i].grid(True) + + i+=1 + ax[i].plot(timestamp, err_gps_x, linestyle='solid', color='C1', label="x") + ax[i].legend() + ax[i].plot(timestamp, err_gps_y, linestyle='solid', color='C2', label="y") + ax[i].legend() + ax[i].plot(timestamp, err_gps_z, linestyle='solid', color='C5', label="z") + ax[i].legend() + ax[i].set_ylabel('gps position err(m)') + ax[i].grid(True) + + i+=1 + ax[i].plot(timestamp, err_gps_vel_x, linestyle='solid', color='C1', label="x") + ax[i].legend() + ax[i].plot(timestamp, err_gps_vel_y, linestyle='solid', color='C2', label="y") + ax[i].legend() + ax[i].plot(timestamp, err_gps_vel_z, linestyle='solid', color='C5', label="z") + ax[i].legend() + ax[i].set_ylabel('gps velocity err(m/s)') + ax[i].grid(True) + + i+=1 + ax[i].plot(timestamp, mag_flux_x, linestyle='solid', color='C1', label="x") + ax[i].legend() + ax[i].plot(timestamp, mag_flux_y, linestyle='solid', color='C3', label="y") + ax[i].legend() + ax[i].plot(timestamp, mag_flux_z, linestyle='solid', color='C5', label="z") + ax[i].legend() + ax[i].set_ylabel('Magnetic flux measurement (Gauss)') + ax[i].grid(True) + + i+=1 + ax[i].plot(timestamp, ground_truth_pitch, linestyle='solid', color='C1', label="ground_truth_pitch") + ax[i].legend(loc='upper left') + ax[i].plot(timestamp, estimated_pitch, linestyle='dotted', color='C6', label="estimated_pitch", lw='1.2') + ax[i].legend(loc='upper left') + ax[i].plot(timestamp, ground_truth_roll, linestyle='solid', color='C2', label="ground_truth_roll") + ax[i].legend(loc='upper left') + ax[i].plot(timestamp, estimated_roll, linestyle='dotted', color='C7', label="estimated_roll", lw='1.2') + ax[i].legend(loc='upper left') + ax[i].set_ylabel('angles(deg)') + # ax[i].set_xlim([10, 15]) + # ax[i].set_ylim([-1, 40]) + ax[i].grid(True) + + i+=1 + ax[i].plot(timestamp, ground_truth_yaw, linestyle='solid', color='C5', label="ground_truth_yaw") + ax[i].legend() + ax[i].plot(timestamp, estimated_yaw, linestyle='dotted', color='C8', label="estimated_yaw") + ax[i].legend() + ax[i].set_ylabel('yaw(deg)') + # ax[i].set_xlim([10, 15]) + # ax[i].set_ylim([-2, 2]) + ax[i].grid(True) + + i+=1 + ax[i].plot(timestamp, err_estimated_pitch, linestyle='solid', color='C1', label="pitch") + ax[i].legend() + ax[i].plot(timestamp, err_estimated_roll, linestyle='solid', color='C2', label="roll") + ax[i].legend() + ax[i].set_ylabel('angles error(deg)') + # ax[i].set_xlim([5, 6]) + # ax[i].set_ylim([-0.0001, 0.0001]) + ax[i].grid(True) + + i+=1 + ax[i].plot(timestamp, err_estimated_yaw, linestyle='dotted', color='C5', label="yaw") + ax[i].legend() + ax[i].set_ylabel('angles error(deg)') + # ax[i].set_xlim([5, 6]) + # ax[i].set_ylim([-0.0001, 0.0001]) + ax[i].grid(True) + + # i+=1 + # ax[i].plot(timestamp, theta_error, linestyle='solid', color='C1', label="pitch") + # ax[i].legend() + # ax[i].plot(timestamp, phi_error, linestyle='solid', color='C2', label="roll") + # ax[i].legend() + # ax[i].plot(timestamp, psi_error, linestyle='dotted', color='C5', label="yaw") + # ax[i].legend() + # ax[i].set_ylabel('angles error using Direc Cosine(deg)') + # # ax[i].set_xlim([5, 6]) + # # ax[i].set_ylim([-0.0001, 0.0001]) + # ax[i].grid(True) + + # i+=1 + # ax[i].plot(timestamp, err_estimated_pitch, linestyle='solid', color='C1', label="pitch") + # ax[i].legend() + # ax[i].plot(timestamp, err_estimated_roll, linestyle='solid', color='C2', label="roll") + # ax[i].legend() + # ax[i].plot(timestamp, err_estimated_yaw, linestyle='dotted', color='C5', label="yaw") + # ax[i].legend() + # ax[i].set_ylabel('angles error(deg)') + # ax[i].set_xlim([25, 27]) + # ax[i].set_ylim([-0.00001, 0.00007]) + # ax[i].grid(True) + + # i+=1 + # ax[i].plot(timestamp, err_estimated_pitch, linestyle='solid', color='C1', label="pitch") + # ax[i].legend() + # ax[i].plot(timestamp, err_estimated_roll, linestyle='solid', color='C2', label="roll") + # ax[i].legend() + # ax[i].plot(timestamp, err_estimated_yaw, linestyle='dotted', color='C5', label="yaw") + # ax[i].legend() + # ax[i].set_ylabel('angles error(deg)') + # ax[i].set_xlim([27, 29]) + # # ax[i].set_ylim([-0.0001, 0.0001]) + # ax[i].grid(True) + + + + i+=1 + ax[i].plot(timestamp, estimated_x, linestyle='solid', color='C1', label="x") + ax[i].legend() + ax[i].plot(timestamp, estimated_y, linestyle='solid', color='C2', label="y") + ax[i].legend() + ax[i].plot(timestamp, estimated_z, linestyle='solid', color='C5', label="z") + ax[i].legend() + ax[i].set_ylabel('estimated position(m)') + # ax[i].set_xlim([10, 15]) + ax[i].grid(True) + + i+=1 + ax[i].plot(timestamp, ground_truth_x, linestyle='solid', color='C1', label="x") + ax[i].legend() + ax[i].plot(timestamp, ground_truth_y, linestyle='solid', color='C2', label="y") + ax[i].legend() + ax[i].plot(timestamp, ground_truth_z, linestyle='solid', color='C5', label="z") + ax[i].legend() + ax[i].set_ylabel('true position(m)') + # ax[i].set_xlim([10, 15]) + # ax[i].set_ylim([-1, 10]) + ax[i].grid(True) + + i+=1 + ax[i].plot(timestamp, err_estimated_x, linestyle='solid', color='C1', label="x") + ax[i].legend() + ax[i].plot(timestamp, err_estimated_y, linestyle='solid', color='C2', label="y") + ax[i].legend() + ax[i].plot(timestamp, err_estimated_z, linestyle='solid', color='C5', label="z") + ax[i].legend() + ax[i].set_ylabel('position error(m)') + # ax[i].set_xlim([10, 15]) + # ax[i].set_ylim([-0.1, 0.1]) + ax[i].grid(True) + + # i+=1 + # ax[i].plot(timestamp, ground_truth_y, linestyle='solid', color='C1', label="ground_truth") + # ax[i].legend() + # ax[i].plot(timestamp, estimated_y, linestyle='solid', color='C2', label="estimated") + # ax[i].legend() + # # ax[i].plot(timestamp, gps_y, linestyle='dotted', color='C5', label="gps") + # # ax[i].legend() + # ax[i].set_ylabel('y_position (m)') + # ax[i].grid(True) + + # i+=1 + # ax[i].plot(timestamp, ground_truth_z, linestyle='solid', color='C1', label="ground_truth") + # ax[i].legend() + # ax[i].plot(timestamp, estimated_z, linestyle='solid', color='C2', label="estimated", lw='0.5') + # ax[i].legend() + # # ax[i].plot(timestamp, gps_z, linestyle='dotted', color='C5', label="gps") + # # ax[i].legend() + # ax[i].set_ylabel('z_position (m)') + # ax[i].grid(True) + + + + i+=1 + ax[i].plot(timestamp, esti_lin_vel_x, linestyle='solid', color='C1', label="x") + ax[i].legend() + ax[i].plot(timestamp, esti_lin_vel_y, linestyle='solid', color='C2', label="y") + ax[i].legend() + ax[i].plot(timestamp, esti_lin_vel_z, linestyle='solid', color='C5', label="z") + ax[i].legend() + ax[i].set_ylabel('estimated lin vel(m/s)') + # ax[i].set_xlim([10, 15]) + # ax[i].set_ylim([-1, 10]) + ax[i].grid(True) + + i+=1 + ax[i].plot(timestamp, true_lin_vel_x, linestyle='solid', color='C1', label="x") + ax[i].legend() + ax[i].plot(timestamp, true_lin_vel_y, linestyle='solid', color='C2', label="y") + ax[i].legend() + ax[i].plot(timestamp, true_lin_vel_z, linestyle='solid', color='C5', label="z") + ax[i].legend() + ax[i].set_ylabel('true lin vel(m/s)') + # ax[i].set_xlim([10, 15]) + # ax[i].set_ylim([-1, 10]) + ax[i].grid(True) + + i+=1 + ax[i].plot(timestamp, err_lin_vel_x, linestyle='solid', color='C1', label="x") + ax[i].legend() + ax[i].plot(timestamp, err_lin_vel_y, linestyle='solid', color='C2', label="y") + ax[i].legend() + ax[i].plot(timestamp, err_lin_vel_z, linestyle='solid', color='C5', label="z") + ax[i].legend() + ax[i].set_ylabel('lin vel error(m/s)') + # ax[i].set_xlim([10, 15]) + # ax[i].set_ylim([-0.01, 0.01]) + ax[i].grid(True) + + + + i+=1 + ax[i].plot(timestamp, quat_norm, linestyle='solid', color='C1', label="norm") + ax[i].legend() + ax[i].set_ylabel('quaternion norm') + # ax[i].set_xlim([10, 15]) + # ax[i].set_xlabel('Time (s)') + ax[i].grid(True) + + i+=1 + ax[i].plot(timestamp, bias_accel_x, linestyle='solid', color='C1', label="accel_x") + ax[i].legend() + ax[i].plot(timestamp, bias_accel_y, linestyle='solid', color='C2', label="accel_y") + ax[i].legend() + ax[i].plot(timestamp, bias_accel_z, linestyle='solid', color='C3', label="accel_z") + ax[i].legend() + ax[i].set_ylabel('biases') + ax[i].grid(True) + + i+=1 + ax[i].plot(timestamp, bias_gyro_x, linestyle='solid', color='C4', label="gyro_x") + ax[i].legend() + ax[i].plot(timestamp, bias_gyro_y, linestyle='solid', color='C5', label="gyro_y") + ax[i].legend() + ax[i].plot(timestamp, bias_gyro_z, linestyle='solid', color='C6', label="gyro_z") + ax[i].legend() + ax[i].plot(timestamp, bias_baro, linestyle='solid', color='C7', label="baro") + ax[i].legend() + ax[i].set_ylabel('biases') + ax[i].grid(True) + + # i+=1 + # ax[i].plot(timestamp, ground_truth_roll, linestyle='solid', color='C1', label="ground_truth") + # ax[i].legend() + # ax[i].plot(timestamp, estimated_roll, linestyle='dotted', color='black', label="estimated") + # ax[i].legend() + # ax[i].set_ylabel('roll') + # ax[i].grid(True) + + # i+=1 + # ax[i].plot(timestamp, ground_truth_yaw, linestyle='solid', color='C1', label="ground_truth") + # ax[i].legend() + # ax[i].plot(timestamp, estimated_yaw, linestyle='dotted', color='black', label="estimated") + # ax[i].legend() + # ax[i].set_ylabel('yaw') + # ax[i].grid(True) + + + # i+=1 + # ax[i].plot(timestamp, ground_truth_q0, linestyle='solid', color='C1', label="ground_truth_q0") + # ax[i].legend() + # ax[i].set_ylabel('ground_truth quaternions') + # ax[i].grid(True) + + # i+=1 + # ax[i].plot(timestamp, ground_truth_q1, linestyle='dotted', color='C2', label="ground_truth_q1") + # ax[i].legend() + # ax[i].plot(timestamp, ground_truth_q2, linestyle='solid', color='C4', label="ground_truth_q2") + # ax[i].legend() + # ax[i].plot(timestamp, ground_truth_q3, linestyle='dotted', color='C5', label="ground_truth_q3") + # ax[i].legend() + # ax[i].set_ylabel('ground_truth quaternions') + # ax[i].grid(True) + + # i+=1 + # ax[i].plot(timestamp, estimated_q0, linestyle='solid', color='C1', label="estimated_q0") + # ax[i].legend() + # ax[i].set_ylabel('estimated quaternions') + # ax[i].grid(True) + + # i+=1 + # ax[i].plot(timestamp, estimated_q1, linestyle='dotted', color='C2', label="estimated_q1") + # ax[i].legend() + # ax[i].plot(timestamp, estimated_q2, linestyle='solid', color='C4', label="estimated_q2") + # ax[i].legend() + # ax[i].plot(timestamp, estimated_q3, linestyle='dotted', color='C5', label="estimated_q3") + # ax[i].legend() + # ax[i].set_ylabel('estimated quaternions') + # ax[i].grid(True) + + # i+=1 + # ax[i].plot(timestamp, error_q0, linestyle='solid', color='C1', label="error_q0") + # ax[i].legend() + # ax[i].set_ylabel('error quaternions') + # ax[i].grid(True) + + # i+=1 + # ax[i].plot(timestamp, error_q1, linestyle='dotted', color='C2', label="error_q1") + # ax[i].legend() + # ax[i].plot(timestamp, error_q2, linestyle='solid', color='C4', label="error_q2") + # ax[i].legend() + # ax[i].plot(timestamp, error_q3, linestyle='dotted', color='C5', label="error_q3") + # ax[i].legend() + # ax[i].set_ylabel('error quaternions') + # ax[i].grid(True) + + i+=1 + ax[i].plot(timestamp, cov_x, linestyle='solid', color='C1', label="cov_x") + ax[i].legend() + ax[i].plot(timestamp, cov_y, linestyle='dotted', color='black', label="cov_y") + ax[i].legend() + ax[i].plot(timestamp, cov_z, linestyle='solid', color='C3', label="cov_z") + ax[i].legend() + ax[i].set_yscale("log") + ax[i].set_ylabel('cov_position') + ax[i].grid(True) + + i+=1 + ax[i].plot(timestamp, cov_u, linestyle='solid', color='C1', label="cov_u") + ax[i].legend() + ax[i].plot(timestamp, cov_v, linestyle='dotted', color='black', label="cov_v") + ax[i].legend() + ax[i].plot(timestamp, cov_w, linestyle='solid', color='C3', label="cov_w") + ax[i].legend() + ax[i].set_yscale("log") + ax[i].set_ylabel('cov_linear_velocity') + ax[i].grid(True) + + i+=1 + ax[i].plot(timestamp, cov_q0, linestyle='solid', color='C1', label="cov_q0") + ax[i].legend() + ax[i].legend() + ax[i].set_yscale("log") + ax[i].set_xlabel('Time (s)') + ax[i].set_ylabel('cov_orientation') + ax[i].grid(True) + + i+=1 + ax[i].plot(timestamp, cov_q1, linestyle='solid', color='C2', label="cov_q1") + ax[i].legend() + ax[i].plot(timestamp, cov_q2, linestyle='dotted', color='C3', label="cov_q2") + ax[i].legend() + ax[i].plot(timestamp, cov_q3, linestyle='dotted', color='C4', label="cov_q3") + ax[i].legend() + ax[i].set_yscale("log") + ax[i].set_xlabel('Time (s)') + ax[i].set_ylabel('cov_orientation') + ax[i].grid(True) + + i+=1 + ax[i].plot(timestamp, cov_b_f_x, linestyle='solid', color='C1', label="cov_b_f_x") + ax[i].legend() + ax[i].plot(timestamp, cov_b_f_y, linestyle='dotted', color='black', label="cov_b_f_y") + ax[i].legend() + ax[i].plot(timestamp, cov_b_f_z, linestyle='solid', color='C3', label="cov_b_f_z") + ax[i].legend() + ax[i].set_yscale("log") + ax[i].set_ylabel('cov_imu_bias') + ax[i].grid(True) + + i+=1 + ax[i].plot(timestamp, cov_b_omega_x, linestyle='solid', color='C1', label="cov_b_omega_x") + ax[i].legend() + ax[i].plot(timestamp, cov_b_omega_y, linestyle='dotted', color='black', label="cov_b_omega_y") + ax[i].legend() + ax[i].plot(timestamp, cov_b_omega_z, linestyle='solid', color='C3', label="cov_b_omega_z") + ax[i].legend() + ax[i].set_yscale("log") + ax[i].set_ylabel('cov_gyro_bias') + ax[i].grid(True) + + i+=1 + ax[i].plot(timestamp, cov_b_baro, linestyle='solid', color='C1', label="cov_b_baro") + ax[i].legend() + ax[i].set_yscale("log") + ax[i].set_ylabel('cov_baro_bias') + ax[i].grid(True) fig.savefig("figure.pdf") fig.show() - if __name__ == "__main__": """ This is executed when run from the command line """ - main() \ No newline at end of file + main() diff --git a/AirLibUnitTests/style.mplstyle b/AirLibUnitTests/style.mplstyle new file mode 100644 index 0000000000..4fde82193b --- /dev/null +++ b/AirLibUnitTests/style.mplstyle @@ -0,0 +1,55 @@ +# axes.titlesize : 24 +# axes.labelsize : 20 +# lines.linewidth : 3 +# lines.markersize : 10 +# xtick.labelsize : 16 +# ytick.labelsize : 16 + +# Matplotlib style for scientific plotting +# This is the base style for "SciencePlots" +# see: https://github.com/garrettj403/SciencePlots + +# Set color cycle: blue, green, yellow, red, violet, gray +axes.prop_cycle : cycler('color', ['0C5DA5', '00B945', 'FF9500', 'FF2C00', '845B97', '474747', '9e9e9e']) + +# Set default figure size +figure.figsize : 15, 80 + +# 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 : 1 + +# 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 + +# # Use LaTeX for math formatting +# text.usetex : True +# text.latex.preamble : \usepackage{amsmath} \usepackage{amssymb} \ No newline at end of file From b01d62479977480141437ff3e7f305c8bb627c4b Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Wed, 12 Jan 2022 14:01:11 +0100 Subject: [PATCH 41/87] add toggle for usage of true and estimated states --- .../AirSimSimpleFlightEstimator.hpp | 152 +++++++++++++----- .../simple_flight/firmware/OffboardApi.hpp | 31 ++-- .../firmware/interfaces/IStateEstimator.hpp | 8 + 3 files changed, 139 insertions(+), 52 deletions(-) diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightEstimator.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightEstimator.hpp index 03648fdb2e..9ba35654ba 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightEstimator.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightEstimator.hpp @@ -10,6 +10,8 @@ #include "physics/Environment.hpp" #include "common/Common.hpp" +#define EKF_ESTIMATED_DIRECTIVE 0 + namespace msr { namespace airlib @@ -35,52 +37,66 @@ namespace airlib virtual simple_flight::Axis3r getAngles() 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; +#if EKF_ESTIMATED_DIRECTIVE == 1 + return getEkfAngles(); +#else + return getTrueAngles(); +#endif } virtual simple_flight::Axis3r getAngularVelocity() const override { - const auto& anguler = kinematics_->twist.angular; - - simple_flight::Axis3r conv; - conv.x() = anguler.x(); - conv.y() = anguler.y(); - conv.z() = anguler.z(); - - return conv; +#if EKF_ESTIMATED_DIRECTIVE == 1 + auto ekf_measurements = getEkfMeasurements(); + return AirSimSimpleFlightCommon::toAxis3r(VectorMath::transformToWorldFrame(AirSimSimpleFlightCommon::toVector3r(ekf_measurements.gyro), + AirSimSimpleFlightCommon::toQuaternion(getOrientation()), + true)); +#else + return getTrueAngularVelocity(); +#endif } virtual simple_flight::Axis3r getPosition() const override { - return AirSimSimpleFlightCommon::toAxis3r(kinematics_->pose.position); +#if EKF_ESTIMATED_DIRECTIVE == 1 + return getEkfPosition(); +#else + return getTruePosition(); +#endif } virtual simple_flight::Axis3r transformToBodyFrame(const simple_flight::Axis3r& world_frame_val) const override { +#if EKF_ESTIMATED_DIRECTIVE == 1 + 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); +#endif } virtual simple_flight::Axis3r getLinearVelocity() const override { - return AirSimSimpleFlightCommon::toAxis3r(kinematics_->twist.linear); +#if EKF_ESTIMATED_DIRECTIVE == 1 + return getEkfLinearVelocity(); +#else + return getTrueLinearVelocity(); +#endif } virtual simple_flight::Axis4r getOrientation() const override { - return AirSimSimpleFlightCommon::toAxis4r(kinematics_->pose.orientation); +#if EKF_ESTIMATED_DIRECTIVE == 1 + return getEkfOrientation(); +#else + return getTrueOrientation(); +#endif } - virtual simple_flight::GeoPoint getGeoPoint() const override + virtual simple_flight::GeoPoint getGeoPoint() const override // TODO return this from measurement { return AirSimSimpleFlightCommon::toSimpleFlightGeoPoint(environment_->getState().geo_point); } @@ -97,13 +113,13 @@ namespace airlib 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); + state.linear_acceleration = AirSimSimpleFlightCommon::toAxis3r(kinematics_->accelerations.linear); // remove this + state.angular_acceleration = AirSimSimpleFlightCommon::toAxis3r(kinematics_->accelerations.angular); // remove this return state; } - // additional methods + // additional methods to get ground truth virtual simple_flight::SensorMeasurements getTrueMeasurements() const override { @@ -132,6 +148,54 @@ namespace airlib 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 getTrueAngularVelocity() const override + { + const auto& anguler = kinematics_->twist.angular; + + simple_flight::Axis3r conv; + conv.x() = anguler.x(); + conv.y() = anguler.y(); + conv.z() = anguler.z(); + + return conv; + } + 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 + + return state; + } + + // additional methods to get Ekf results virtual simple_flight::SensorMeasurements getEkfMeasurements() const override { @@ -242,7 +306,7 @@ namespace airlib virtual simple_flight::Axis3r getEkfPositionCovariance() const override { simple_flight::Axis3r position_cov; - VectorMath::Matrix17x17f ekf_covariance = ekf_->getEkfCovariance(); + auto ekf_covariance = ekf_->getEkfCovariance(); position_cov.x() = ekf_covariance(0, 0); position_cov.y() = ekf_covariance(1, 1); position_cov.z() = ekf_covariance(2, 2); @@ -253,7 +317,7 @@ namespace airlib virtual simple_flight::Axis3r getEkfLinearVelocityCovariance() const override { simple_flight::Axis3r velocity_cov; - VectorMath::Matrix17x17f ekf_covariance = ekf_->getEkfCovariance(); + auto ekf_covariance = ekf_->getEkfCovariance(); velocity_cov.x() = ekf_covariance(3, 3); velocity_cov.y() = ekf_covariance(4, 4); velocity_cov.z() = ekf_covariance(5, 5); @@ -263,20 +327,31 @@ namespace airlib virtual simple_flight::Axis4r getEkfOrientationCovariance() const override { - simple_flight::Axis4r angle_cov; - VectorMath::Matrix17x17f ekf_covariance = ekf_->getEkfCovariance(); - angle_cov.x() = ekf_covariance(6, 6); - angle_cov.y() = ekf_covariance(7, 7); - angle_cov.z() = ekf_covariance(8, 8); - angle_cov.val4()= ekf_covariance(9, 9); + simple_flight::Axis4r orientation_cov; + auto ekf_covariance = ekf_->getEkfCovariance(); + orientation_cov.val4()= ekf_covariance(6, 6); + orientation_cov.x() = ekf_covariance(7, 7); + orientation_cov.y() = ekf_covariance(8, 8); + orientation_cov.z() = ekf_covariance(9, 9); + + return orientation_cov; + } + + virtual simple_flight::Axis3r getEkfAnglesCovariance() const override + { + simple_flight::Axis3r angles_cov; + auto ekf_angles_covariance = ekf_->getEkfEulerAnglesCovariance(); + angles_cov.x() = ekf_angles_covariance(0, 0); + angles_cov.y() = ekf_angles_covariance(1, 1); + angles_cov.z() = ekf_angles_covariance(2, 2); - return angle_cov; + return angles_cov; } virtual simple_flight::Axis3r getEkfImuBiasCovariance() const override { simple_flight::Axis3r imu_bias_cov; - VectorMath::Matrix17x17f ekf_covariance = ekf_->getEkfCovariance(); + auto ekf_covariance = ekf_->getEkfCovariance(); imu_bias_cov.x() = ekf_covariance(10, 10); imu_bias_cov.y() = ekf_covariance(11, 11); imu_bias_cov.z() = ekf_covariance(12, 12); @@ -287,7 +362,7 @@ namespace airlib virtual simple_flight::Axis3r getEkfGyroBiasCovariance() const override { simple_flight::Axis3r gyro_bias_cov; - VectorMath::Matrix17x17f ekf_covariance = ekf_->getEkfCovariance(); + auto ekf_covariance = ekf_->getEkfCovariance(); gyro_bias_cov.x() = ekf_covariance(13, 13); gyro_bias_cov.y() = ekf_covariance(14, 14); gyro_bias_cov.z() = ekf_covariance(15, 15); @@ -298,8 +373,9 @@ namespace airlib virtual float getEkfBaroBiasCovariance() const override { float baro_bias_cov; - VectorMath::Matrix17x17f ekf_covariance = ekf_->getEkfCovariance(); + auto ekf_covariance = ekf_->getEkfCovariance(); baro_bias_cov = ekf_covariance(16, 16); + // baro_bias_cov = ekf_covariance(15, 15); return baro_bias_cov; } @@ -322,7 +398,7 @@ namespace airlib virtual std::array getEkfOrientationOffDiagCovariance() const override { std::array ori_offdiag_cov; - VectorMath::Matrix17x17f ekf_covariance = ekf_->getEkfCovariance(); + auto ekf_covariance = ekf_->getEkfCovariance(); ori_offdiag_cov.at(0) = ekf_covariance(6, 7); ori_offdiag_cov.at(1) = ekf_covariance(6, 8); ori_offdiag_cov.at(2) = ekf_covariance(6, 9); @@ -336,7 +412,7 @@ namespace airlib virtual std::array getEkfOrientationGyroBiasCovariance() const override { std::array ori_gyro_bias_cov; - VectorMath::Matrix17x17f ekf_covariance = ekf_->getEkfCovariance(); + auto ekf_covariance = ekf_->getEkfCovariance(); ori_gyro_bias_cov.at(0) = ekf_covariance(6, 13); ori_gyro_bias_cov.at(1) = ekf_covariance(6, 14); ori_gyro_bias_cov.at(2) = ekf_covariance(6, 15); 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 ea4c42703f..654c45060f 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/OffboardApi.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/OffboardApi.hpp @@ -236,19 +236,19 @@ class OffboardApi : public IUpdatable << state_estimator_->getEkfMeasurements().magnetic_flux.y() << '\t' << state_estimator_->getEkfMeasurements().magnetic_flux.z() << '\t' // ground truth states - << state_estimator_->getKinematicsEstimated().position.x() << '\t' - << state_estimator_->getKinematicsEstimated().position.y() << '\t' - << state_estimator_->getKinematicsEstimated().position.z() << '\t' - << state_estimator_->getKinematicsEstimated().orientation.val4() << '\t' // ATTENSION val4 is w when converted to axis4r of simple_flight - << state_estimator_->getKinematicsEstimated().orientation.x() << '\t' - << state_estimator_->getKinematicsEstimated().orientation.y() << '\t' - << state_estimator_->getKinematicsEstimated().orientation.z() << '\t' - << state_estimator_->getAngles().pitch() << '\t' - << state_estimator_->getAngles().roll() << '\t' - << state_estimator_->getAngles().yaw() << '\t' - << state_estimator_->getKinematicsEstimated().linear_velocity.x() << '\t' - << state_estimator_->getKinematicsEstimated().linear_velocity.y() << '\t' - << state_estimator_->getKinematicsEstimated().linear_velocity.z() << '\t' + << state_estimator_->getTrueKinematicsEstimated().position.x() << '\t' + << state_estimator_->getTrueKinematicsEstimated().position.y() << '\t' + << state_estimator_->getTrueKinematicsEstimated().position.z() << '\t' + << state_estimator_->getTrueKinematicsEstimated().orientation.val4() << '\t' // ATTENSION val4 is w when converted to axis4r of simple_flight + << state_estimator_->getTrueKinematicsEstimated().orientation.x() << '\t' + << state_estimator_->getTrueKinematicsEstimated().orientation.y() << '\t' + << state_estimator_->getTrueKinematicsEstimated().orientation.z() << '\t' + << state_estimator_->getTrueAngles().pitch() << '\t' + << state_estimator_->getTrueAngles().roll() << '\t' + << state_estimator_->getTrueAngles().yaw() << '\t' + << state_estimator_->getTrueKinematicsEstimated().linear_velocity.x() << '\t' + << state_estimator_->getTrueKinematicsEstimated().linear_velocity.y() << '\t' + << state_estimator_->getTrueKinematicsEstimated().linear_velocity.z() << '\t' // estimated states << state_estimator_->getEkfKinematicsEstimated().position.x() << '\t' << state_estimator_->getEkfKinematicsEstimated().position.y() << '\t' @@ -277,10 +277,13 @@ class OffboardApi : public IUpdatable << state_estimator_->getEkfLinearVelocityCovariance().x() << '\t' << state_estimator_->getEkfLinearVelocityCovariance().y() << '\t' << state_estimator_->getEkfLinearVelocityCovariance().z() << '\t' + << state_estimator_->getEkfOrientationCovariance().val4() << '\t' << state_estimator_->getEkfOrientationCovariance().x() << '\t' << state_estimator_->getEkfOrientationCovariance().y() << '\t' << state_estimator_->getEkfOrientationCovariance().z() << '\t' - << state_estimator_->getEkfOrientationCovariance().val4() << '\t' + << state_estimator_->getEkfAnglesCovariance().x() << '\t' + << state_estimator_->getEkfAnglesCovariance().y() << '\t' + << state_estimator_->getEkfAnglesCovariance().z() << '\t' << state_estimator_->getEkfImuBiasCovariance().x() << '\t' << state_estimator_->getEkfImuBiasCovariance().y() << '\t' << state_estimator_->getEkfImuBiasCovariance().z() << '\t' 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 8f59766547..1401362276 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 @@ -33,6 +33,7 @@ class IStateEstimator virtual simple_flight::Axis3r getEkfPositionCovariance() const = 0; virtual simple_flight::Axis3r getEkfLinearVelocityCovariance() const = 0; virtual simple_flight::Axis4r getEkfOrientationCovariance() const = 0; + virtual simple_flight::Axis3r getEkfAnglesCovariance() const = 0; virtual simple_flight::Axis3r getEkfImuBiasCovariance() const = 0; virtual simple_flight::Axis3r getEkfGyroBiasCovariance() const = 0; virtual float getEkfBaroBiasCovariance() const = 0; @@ -41,5 +42,12 @@ class IStateEstimator virtual std::array getEkfOrientationOffDiagCovariance() const = 0; virtual std::array getEkfOrientationGyroBiasCovariance() 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; }; } \ No newline at end of file From 0267b1a5292b5bdb14cd65ff95b6976f81be054f Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Wed, 12 Jan 2022 14:03:19 +0100 Subject: [PATCH 42/87] add EKF settings in AirSim settings --- AirLib/include/common/AirSimSettings.hpp | 45 +++++++++++++++++-- .../simple_flight/SimpleFlightApi.hpp | 4 +- 2 files changed, 43 insertions(+), 6 deletions(-) diff --git a/AirLib/include/common/AirSimSettings.hpp b/AirLib/include/common/AirSimSettings.hpp index a54334bc9f..3f7e99f673 100644 --- a/AirLib/include/common/AirSimSettings.hpp +++ b/AirLib/include/common/AirSimSettings.hpp @@ -207,6 +207,12 @@ namespace airlib float follow_distance = Utils::nan(); }; + struct EkfSetting + { + bool enabled = false; + Settings settings; + }; + struct SensorSetting { SensorBase::SensorType sensor_type; @@ -261,6 +267,7 @@ namespace airlib std::map cameras; std::map> sensors; + std::shared_ptr ekf_setting; RCSettings rc; @@ -398,6 +405,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; @@ -434,12 +444,14 @@ 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 loadRecordingSetting(settings_json); loadClockSettings(settings_json); + } static void initializeSettings(const std::string& json_settings_text) @@ -838,7 +850,8 @@ namespace airlib } 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(); @@ -852,6 +865,7 @@ 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; + simple_flight_setting->ekf_setting = ekf_setting; vehicles[simple_flight_setting->vehicle_name] = std::move(simple_flight_setting); } else if (simmode_name == kSimModeTypeCar) { @@ -875,9 +889,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)) { @@ -1306,6 +1321,28 @@ namespace airlib sensors[p.first] = p.second; } } + + 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, diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightApi.hpp index 171cae4cc2..61d419205a 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightApi.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightApi.hpp @@ -43,7 +43,7 @@ namespace airlib comm_link_.reset(new AirSimSimpleFlightCommLink()); // added by Suman - ekf_.reset(new AirSimSimpleEkf(board_.get(), comm_link_.get())); + ekf_.reset(new AirSimSimpleEkf(board_.get(), comm_link_.get(), vehicle_setting->ekf_setting.get())); estimator_.reset(new AirSimSimpleFlightEstimator(ekf_.get())); @@ -144,7 +144,7 @@ namespace airlib } return true; - } + } protected: virtual Kinematics::State getKinematicsEstimated() const override From d7b031c54afc100acfa1ead7db0ac95d18801111 Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Wed, 12 Jan 2022 14:07:22 +0100 Subject: [PATCH 43/87] add further matrix definitions --- AirLib/include/common/VectorMath.hpp | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/AirLib/include/common/VectorMath.hpp b/AirLib/include/common/VectorMath.hpp index c67629d18d..b5e65c21bb 100644 --- a/AirLib/include/common/VectorMath.hpp +++ b/AirLib/include/common/VectorMath.hpp @@ -40,6 +40,7 @@ namespace airlib // added by Suman typedef Eigen::Matrix Vector17f; + typedef Eigen::Matrix Vector16f; typedef Eigen::Matrix Matrix17x17f; typedef Eigen::Matrix Matrix17x13f; typedef Eigen::Matrix Matrix13x13f; @@ -47,11 +48,23 @@ namespace airlib typedef Eigen::Matrix Matrix17x1f; typedef Eigen::Matrix Matrix3x17f; typedef Eigen::Matrix Matrix17x3f; - typedef Eigen::Matrix Matrix6x6f; 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; From 4f21bf92728ffa707b8f79b340a8dd6cee7819de Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Wed, 12 Jan 2022 14:08:46 +0100 Subject: [PATCH 44/87] reset takeoff altitide to original value --- .../multirotor/firmwares/simple_flight/firmware/Params.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/Params.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/Params.hpp index 35e15b6703..3612b207aa 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/Params.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/Params.hpp @@ -121,7 +121,7 @@ struct Params struct Takeoff { - float takeoff_z = -10.0f; + float takeoff_z = -2.0f; //float velocity = -1.0f; } takeoff; From 65ae1c0f8b77fd5e89e1849b630909c7180b35fa Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Wed, 12 Jan 2022 14:10:37 +0100 Subject: [PATCH 45/87] add simple_flight matrix definitions for EKF --- .../firmware/interfaces/CommonStructs.hpp | 61 ++++++++----------- 1 file changed, 24 insertions(+), 37 deletions(-) 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 27a918c499..a013b3dfba 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 @@ -243,43 +243,6 @@ struct EkfKinematicsState SensorBiases sensor_bias; }; -struct SensorCharacteristics -{ - Axis3r accel_std_dev; - Axis3r gyro_std_dev; - - Axis3r gps_pos_std_dev; - Axis3r gps_vel_std_dev; - - Axis3r mag_std_dev; - - real_T baro_std_dev; -}; - -struct EkfInitialStates -{ - Axis3r pos; - Axis3r vel; - - Axis4r quaternion; - - Axis3r accel_bias; - Axis3r gyro_bias; - real_T baro_bias; -}; - -struct EkfInitialStdDevs -{ - Axis3r pos; - Axis3r vel; - - Axis4r quaternion; - - Axis3r accel_bias; - Axis3r gyro_bias; - real_T baro_bias; -}; - enum class VehicleStateType { Unknown, @@ -291,6 +254,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: From 3eb3bd8354b5800fa2c5b33ab8783fd1aa40b3c0 Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Wed, 12 Jan 2022 14:13:05 +0100 Subject: [PATCH 46/87] block changes in AirSim EKF, ekf settings, cleanups... --- .../simple_flight/AirSimSimpleEkf.hpp | 431 ++++++++--------- .../simple_flight/AirSimSimpleEkfBase.hpp | 56 ++- .../simple_flight/AirSimSimpleEkfModel.hpp | 443 ++++-------------- .../simple_flight/AirSimSimpleEkfParams.hpp | 258 ++++++++++ .../firmware/interfaces/IEkf.hpp | 41 +- 5 files changed, 595 insertions(+), 634 deletions(-) create mode 100644 AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfParams.hpp diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp index 64ba24e6dc..d45213f8a3 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp @@ -5,11 +5,13 @@ #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" #define EKF_GROUND_TRUTH_MEAS_DIRECTIVE 0 @@ -28,11 +30,11 @@ namespace airlib { public: // Constructor - AirSimSimpleEkf(const simple_flight::IBoard* board, simple_flight::ICommLink* comm_link) //, const AirSimSettings::EKFSetting& setting = AirSimSettings::EKFSetting()) + AirSimSimpleEkf(const simple_flight::IBoard* board, simple_flight::ICommLink* comm_link, const AirSimSettings::EkfSetting* setting = nullptr) : board_(board), comm_link_(comm_link) // commlink is only temporary here { - // params_ = // get the EKF params from airsim settings. Implement this as a struct for now with constant members ! inspired by sensor model - freq_limiter_.initialize(100); // physics engine and the imu refresh at period 3ms ~ 333.33Hz + params_.initializeParameters(setting); + freq_limiter_.initialize(334); // physics engine and the imu refresh at period 3ms ~ 333.33Hz } virtual void reset() override @@ -52,10 +54,10 @@ namespace airlib freq_limiter_.update(); // if the wait is complete and it is time to update EKF, update EKF - // if (freq_limiter_.isWaitComplete()) - // updateEKFInternal(); + if (freq_limiter_.isWaitComplete()) + updateEKFInternal(); - updateEKFInternal(); + // updateEKFInternal(); } // only to debug and verify estimates @@ -73,90 +75,20 @@ namespace airlib // initialize filter void initializeFilter() { - simple_flight::SensorCharacteristics sensor_characteristics; - simple_flight::EkfInitialStates initial_states; - simple_flight::EkfInitialStdDevs initial_std_devs; - - // imu - sensor_characteristics.accel_std_dev.x() = 0.05f; // m/s^2 - sensor_characteristics.accel_std_dev.y() = 0.05f; // m/s^2 - sensor_characteristics.accel_std_dev.z() = 0.05f; // m/s^2 - sensor_characteristics.gyro_std_dev.x() = 0.1f; // deg/s - sensor_characteristics.gyro_std_dev.y() = 0.1f; // deg/s - sensor_characteristics.gyro_std_dev.z() = 0.1f; // deg/s - // gps - sensor_characteristics.gps_pos_std_dev.x() = 5.0f; // m - sensor_characteristics.gps_pos_std_dev.y() = 5.0f; // m - sensor_characteristics.gps_pos_std_dev.z() = 10.0f; // m - sensor_characteristics.gps_vel_std_dev.x() = 2.0f; // m/s - sensor_characteristics.gps_vel_std_dev.y() = 2.0f; // m/s - sensor_characteristics.gps_vel_std_dev.z() = 5.0f; // m/s - // barometer - sensor_characteristics.baro_std_dev = 1.0f; // m - // magnetomter - sensor_characteristics.mag_std_dev.x() = 0.01f; - sensor_characteristics.mag_std_dev.y() = 0.01f; - sensor_characteristics.mag_std_dev.z() = 0.01f; - - // initial position - initial_states.pos.x() = 2.0f; - initial_states.pos.y() = 2.0f; - initial_states.pos.z() = -10.0f + 2.0f; - // initial velocity - initial_states.vel.x() = 0.0f; - initial_states.vel.y() = 0.0f; - initial_states.vel.z() = 0.0f; - // initial quaternions - initial_states.quaternion.val4() = 0.98929; - initial_states.quaternion.x() = 0.0789265; - initial_states.quaternion.y() = 0.0940609; - initial_states.quaternion.z() = 0.0789265; - // initial biases - initial_states.accel_bias.x() = 0.0f; - initial_states.accel_bias.y() = 0.0f; - initial_states.accel_bias.z() = 0.0f; - initial_states.gyro_bias.x() = 0.0f; - initial_states.gyro_bias.y() = 0.0f; - initial_states.gyro_bias.z() = 0.0f; - initial_states.baro_bias = 0.0f; - - // initial position standard deviations - initial_std_devs.pos.x() = 5.0f; - initial_std_devs.pos.y() = 5.0f; - initial_std_devs.pos.z() = 10.0f; - // initial velocity standard deviations - initial_std_devs.vel.x() = 2.0f; - initial_std_devs.vel.y() = 2.0f; - initial_std_devs.vel.z() = 5.0f; - // initial quaternions standard deviations - initial_std_devs.quaternion.val4() = 0.1f; - initial_std_devs.quaternion.x() = 0.1f; - initial_std_devs.quaternion.y() = 0.1f; - initial_std_devs.quaternion.z() = 0.1f; - // initial biases standard deviations - initial_std_devs.accel_bias.x() = 0.05f; - initial_std_devs.accel_bias.y() = 0.05f; - initial_std_devs.accel_bias.z() = 0.05f; - initial_std_devs.gyro_bias.x() = 0.001f; - initial_std_devs.gyro_bias.y() = 0.001f; - initial_std_devs.gyro_bias.z() = 0.001f; - initial_std_devs.baro_bias = 0.1f; - - assignEkfMatrics(sensor_characteristics, initial_states, initial_std_devs); - resetIntialGlobalVariables(); + assignEkfMatrics(); + resetGlobalVariables(); } - void assignEkfMatrics(const simple_flight::SensorCharacteristics& sensor, - const simple_flight::EkfInitialStates& initial_states, - const simple_flight::EkfInitialStdDevs& initial_std_devs) + void assignEkfMatrics() { + Q_ = simple_flight::MatrixNWxNWf::Zero(); // imu - Q_(0, 0) = sensor.accel_std_dev.x()*sensor.accel_std_dev.x(); // accel_x - Q_(1, 1) = sensor.accel_std_dev.y()*sensor.accel_std_dev.y(); // accel_y - Q_(2, 2) = sensor.accel_std_dev.z()*sensor.accel_std_dev.z(); // accel_z - Q_(3, 3) = sensor.gyro_std_dev.x()*sensor.gyro_std_dev.x() * M_PI*M_PI/32400; // gyro_x - Q_(4, 4) = sensor.gyro_std_dev.y()*sensor.gyro_std_dev.y() * M_PI*M_PI/32400; // gyro_y - Q_(5, 5) = sensor.gyro_std_dev.z()*sensor.gyro_std_dev.z() * M_PI*M_PI/32400; // gyro_z + 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; @@ -168,62 +100,69 @@ namespace airlib Q_(12, 12) = 0.0001f; // gps - R_gps_(0, 0) = sensor.gps_pos_std_dev.x()*sensor.gps_pos_std_dev.x(); // gps_pos_x - R_gps_(1, 1) = sensor.gps_pos_std_dev.y()*sensor.gps_pos_std_dev.y(); // gps_pos_y - R_gps_(2, 2) = sensor.gps_pos_std_dev.z()*sensor.gps_pos_std_dev.z(); // gps_pos_z - R_gps_(3, 3) = sensor.gps_vel_std_dev.x()*sensor.gps_vel_std_dev.x(); // gps_vel_x - R_gps_(4, 4) = sensor.gps_vel_std_dev.y()*sensor.gps_vel_std_dev.y(); // gps_vel_y - R_gps_(5, 5) = sensor.gps_vel_std_dev.z()*sensor.gps_vel_std_dev.z(); // gps_vel_z + 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_(0, 0) = sensor.mag_std_dev.x()*sensor.mag_std_dev.x(); // mag_x - R_mag_(1, 1) = sensor.mag_std_dev.y()*sensor.mag_std_dev.y(); // mag_y - R_mag_(2, 2) = sensor.mag_std_dev.z()*sensor.mag_std_dev.z(); // mag_z + 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_ = sensor.baro_std_dev*sensor.baro_std_dev; // baro_alt + R_baro_ = params_.baro.std_error*params_.baro.std_error; - // intialize the ekf states - states_(0) = initial_states.pos.x(); - states_(1) = initial_states.pos.y(); - states_(2) = initial_states.pos.z(); - states_(3) = initial_states.vel.x(); - states_(4) = initial_states.vel.y(); - states_(5) = initial_states.vel.z(); - states_(6) = initial_states.quaternion.val4(); - states_(7) = initial_states.quaternion.x(); - states_(8) = initial_states.quaternion.y(); - states_(9) = initial_states.quaternion.z(); - states_(10) = initial_states.accel_bias.x(); - states_(11) = initial_states.accel_bias.y(); - states_(12) = initial_states.accel_bias.z(); - states_(13) = initial_states.gyro_bias.x(); - states_(14) = initial_states.gyro_bias.y(); - states_(15) = initial_states.gyro_bias.z(); - states_(16) = initial_states.baro_bias; + // barometer + R_pseudo_ = params_.pseudo_meas.quaternion_norm_R; + // intialize the ekf states + states_ = simple_flight::VectorNXf::Zero(); + states_(0) = params_.initial_states.position.x(); + states_(1) = params_.initial_states.position.y(); + states_(2) = params_.initial_states.position.z(); + states_(3) = params_.initial_states.linear_velocity.x(); + states_(4) = params_.initial_states.linear_velocity.y(); + states_(5) = params_.initial_states.linear_velocity.z(); + states_(6) = params_.initial_states.quaternion.w(); + states_(7) = params_.initial_states.quaternion.x(); + states_(8) = params_.initial_states.quaternion.y(); + states_(9) = params_.initial_states.quaternion.z(); + states_(10) = params_.initial_states.accel_bias.x(); + states_(11) = params_.initial_states.accel_bias.y(); + states_(12) = params_.initial_states.accel_bias.z(); + states_(13) = params_.initial_states.gyro_bias.x(); + states_(14) = params_.initial_states.gyro_bias.y(); + states_(15) = params_.initial_states.gyro_bias.z(); + states_(16) = params_.initial_states.baro_bias; + // intitialize the ekf covariances - covariance_(0,0) = initial_std_devs.pos.x()*initial_std_devs.pos.x(); // x - covariance_(1,1) = initial_std_devs.pos.y()*initial_std_devs.pos.y(); // y - covariance_(2,2) = initial_std_devs.pos.z()*initial_std_devs.pos.z(); // z - covariance_(3,3) = initial_std_devs.vel.x()*initial_std_devs.vel.x(); // u - covariance_(4,4) = initial_std_devs.vel.y()*initial_std_devs.vel.y(); // v - covariance_(5,5) = initial_std_devs.vel.z()*initial_std_devs.vel.z(); // w - covariance_(6,6) = initial_std_devs.quaternion.val4()*initial_std_devs.quaternion.val4(); // q0 - covariance_(7,7) = initial_std_devs.quaternion.x()*initial_std_devs.quaternion.x(); // q1 - covariance_(8,8) = initial_std_devs.quaternion.y()*initial_std_devs.quaternion.y(); // q2 - covariance_(9,9) = initial_std_devs.quaternion.z()*initial_std_devs.quaternion.z(); // q3 - covariance_(10,10) = initial_std_devs.accel_bias.x()*initial_std_devs.accel_bias.x(); // - covariance_(11,11) = initial_std_devs.accel_bias.y()*initial_std_devs.accel_bias.y(); // - covariance_(12,12) = initial_std_devs.accel_bias.z()*initial_std_devs.accel_bias.z(); // - covariance_(13,13) = initial_std_devs.gyro_bias.x()*initial_std_devs.gyro_bias.x(); // - covariance_(14,14) = initial_std_devs.gyro_bias.y()*initial_std_devs.gyro_bias.y(); // - covariance_(15,15) = initial_std_devs.gyro_bias.z()*initial_std_devs.gyro_bias.z(); // - covariance_(16,16) = initial_std_devs.baro_bias*initial_std_devs.baro_bias; // + error_covariance_ = simple_flight::MatrixNXxNXf::Zero(); + error_covariance_(0,0) = pow(params_.initial_states_std_err.position.x(), 2); + error_covariance_(1,1) = pow(params_.initial_states_std_err.position.y(), 2); + error_covariance_(2,2) = pow(params_.initial_states_std_err.position.z(), 2); + error_covariance_(3,3) = pow(params_.initial_states_std_err.linear_velocity.x(), 2); + error_covariance_(4,4) = pow(params_.initial_states_std_err.linear_velocity.y(), 2); + error_covariance_(5,5) = pow(params_.initial_states_std_err.linear_velocity.z(), 2); + error_covariance_(6,6) = pow(params_.initial_states_std_err.quaternion.w(), 2); + error_covariance_(7,7) = pow(params_.initial_states_std_err.quaternion.x(), 2); + error_covariance_(8,8) = pow(params_.initial_states_std_err.quaternion.y(), 2); + error_covariance_(9,9) = pow(params_.initial_states_std_err.quaternion.z(), 2); + error_covariance_(10,10) = pow(params_.initial_states_std_err.accel_bias.x(), 2); + error_covariance_(11,11) = pow(params_.initial_states_std_err.accel_bias.y(), 2); + error_covariance_(12,12) = pow(params_.initial_states_std_err.accel_bias.z(), 2); + error_covariance_(13,13) = pow(params_.initial_states_std_err.gyro_bias.x(), 2); + error_covariance_(14,14) = pow(params_.initial_states_std_err.gyro_bias.y(), 2); + error_covariance_(15,15) = pow(params_.initial_states_std_err.gyro_bias.z(), 2); + error_covariance_(16,16) = pow(params_.initial_states_std_err.baro_bias, 2); } - void resetIntialGlobalVariables() + void resetGlobalVariables() { // reset last update times last_times_.state_propagation = board_->micros(); @@ -231,12 +170,12 @@ namespace airlib // reset geo and magnetic global variables geodetic_converter_.setHome(environment_->getHomeGeoPoint()); - VectorMath::Vector3f magnetic_field_true = EarthUtils::getMagField(environment_->getState().geo_point) * 1E4f; + 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 imu data buffer + // 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); @@ -300,6 +239,7 @@ namespace airlib pseudoMeasurement(); #else #endif + eulerAnglesCovariancePropagation(); } // state propagtion @@ -311,14 +251,14 @@ namespace airlib last_times_.state_propagation = board_->micros(); // declare local variables - float x_dot[17]; - float x[17]; - float u[6]; - float uplus[6]; + float x_dot[simple_flight::NX]; + 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<17; i++){ - x[i] = states_[i]; + for (int i=0; imicros(); // declare local variables - float x[17]; - float u[6]; - VectorMath::Matrix17x17f A; - VectorMath::Matrix17x17f A_finite; - VectorMath::Matrix17x13f B_w; - VectorMath::Matrix17x17f Phi; - VectorMath::Matrix17x13f GammaB_w; - VectorMath::Matrix17x17f P = covariance_; - VectorMath::Matrix17x17f next_covariance; + 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<17; i++){ + for (int i=0; i + +namespace msr +{ +namespace airlib +{ + + struct AirSimSimpleEkfParams + { + bool ekf_enabled = 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 InitialStates + { + 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; + + 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 refreshAndUnitConversion() + { + gyro.std_error = gyro.std_error * M_PI/180; // deg/s to rad/s + initial_states.attitude = initial_states.attitude * M_PI/180; //deg to rad + + initial_states.quaternion = VectorMath::toQuaternion(initial_states.attitude.y(), + initial_states.attitude.x(), + initial_states.attitude.z()); + } + + void initializeParameters(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; + } + 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 pseudo_meas_child; + if (json.getChild("PseudoMeasurement", pseudo_meas_child)){ + readRealT(pseudo_meas_child, "QuaternionNormR", pseudo_meas.quaternion_norm_R); + } + 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_child; + if (json.getChild("InitialStates", initial_states_child)){ + std::array pos_str = { + "PositionX", + "PositionY", + "PositionZ" + }; + readVector3r(initial_states_child, pos_str, initial_states.position); + std::array lin_vel_str = { + "LinearVelocityX", + "LinearVelocityY", + "LinearVelocityZ" + }; + readVector3r(initial_states_child, lin_vel_str, initial_states.linear_velocity); + std::array attitude_str = { + "AttitideRoll", + "AttitidePitch", + "AttitideYaw" + }; + readVector3r(initial_states_child, attitude_str, initial_states.attitude); + std::array accel_bias_str = { + "AccelBiasX", + "AccelBiasY", + "AccelBiasZ" + }; + readVector3r(initial_states_child, accel_bias_str, initial_states.accel_bias); + std::array gyro_bias_str = { + "GyroBiasX", + "GyroBiasY", + "GyroBiasZ" + }; + readVector3r(initial_states_child, gyro_bias_str, initial_states.gyro_bias); + readRealT(initial_states_child, "BaroBias", initial_states.baro_bias); + } + refreshAndUnitConversion(); + } + }; + +} +} //namespace +#endif 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 index 5cdab8f5cd..ec58b579fa 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IEkf.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IEkf.hpp @@ -11,44 +11,13 @@ namespace simple_flight class IEkf : public IUpdatable { public: - virtual void update() override - { - IUpdatable::update(); - } + virtual bool checkEkfEnabled() const = 0; - // setters - void setEkfStates(VectorMath::Vector17f states) - { - states_ = states; - } - // getters - const VectorMath::Vector17f& getEkfStates() const - { - return states_; - } - - const VectorMath::Vector17f& getEkfMeasurements() const - { - return measurement_; - } - - // setters - void setEkfCovariance(VectorMath::Matrix17x17f covariance) - { - covariance_ = covariance; - } - - // getters - const VectorMath::Matrix17x17f& getEkfCovariance() const - { - return covariance_; - } - -protected: - VectorMath::Vector17f states_; - VectorMath::Matrix17x17f covariance_; - VectorMath::Vector17f measurement_ = VectorMath::Vector17f::Zero(); + 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 From 46e89d4980eb1960b857f16485c593b849a76073 Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Wed, 12 Jan 2022 14:13:51 +0100 Subject: [PATCH 47/87] running changes in other supplementary files --- AirLibUnitTests/JacobianTest.hpp | 26 +- AirLibUnitTests/SimpleFlightTest.hpp | 39 +- AirLibUnitTests/cleanup.sh | 17 +- AirLibUnitTests/main.cpp | 4 +- AirLibUnitTests/plot.py | 655 ++++++++++++++------------- AirLibUnitTests/style.mplstyle | 6 +- 6 files changed, 408 insertions(+), 339 deletions(-) diff --git a/AirLibUnitTests/JacobianTest.hpp b/AirLibUnitTests/JacobianTest.hpp index b41469e53c..a2c4934e76 100644 --- a/AirLibUnitTests/JacobianTest.hpp +++ b/AirLibUnitTests/JacobianTest.hpp @@ -32,9 +32,9 @@ namespace airlib float x_dotdotNew[17]; float x_dotdotdotNew[17]; - AirSimSimpleEkfModel::evaluateStateDotOld(x_dotNew,x,u); - AirSimSimpleEkfModel::evaluateStateDotOld(x_dotdotNew,x_dotNew,u2); - AirSimSimpleEkfModel::evaluateStateDotOld(x_dotdotdotNew,x_dotdotNew,u2); + // AirSimSimpleEkfModel::evaluateStateDotOld(x_dotNew,x,u); + // AirSimSimpleEkfModel::evaluateStateDotOld(x_dotdotNew,x_dotNew,u2); + // AirSimSimpleEkfModel::evaluateStateDotOld(x_dotdotdotNew,x_dotdotNew,u2); for (int i=0; i<17; i++) { @@ -50,10 +50,10 @@ namespace airlib std::cout << x_dotdotNew[i] << '\t'; std::cout << x_dotdotdotNew[i] << '\n'; } - VectorMath::Matrix17x17f A; - VectorMath::Matrix17x17f ANew; + simple_flight::MatrixNXxNXf A; + simple_flight::MatrixNXxNXf ANew; AirSimSimpleEkfModel::evaluateA(&A, x_dotdotdot,u2); - AirSimSimpleEkfModel::evaluateAOld(&ANew, x_dotdotdot,u2); + // AirSimSimpleEkfModel::evaluateAOld(&ANew, x_dotdotdot,u2); for (int i=0; i<17; i++){ for (int j=0; j<17; j++){ @@ -62,15 +62,15 @@ namespace airlib std::cout << '\n'; } - for (int i=0; i<17; i++){ - for (int j=0; j<17; j++){ - std::cout << ANew(i,j) << "\t "; - } - std::cout << '\n'; - } + // for (int i=0; i<17; i++){ + // for (int j=0; j<17; j++){ + // std::cout << ANew(i,j) << "\t "; + // } + // std::cout << '\n'; + // } // QuaternionT q = VectorMath::toQuaternion(RealT pitch, RealT roll, RealT yaw); - Quaternionr q = VectorMath::toQuaternion(10.0*M_PI/180, 10.0*M_PI/180, 10.0*M_PI/180); + Quaternionr q = VectorMath::toQuaternion(0.2*M_PI/180, 0.0*M_PI/180, 0.2*M_PI/180); std::cout << q.w() << '\n'; std::cout << q.x() << '\n'; std::cout << q.y() << '\n'; diff --git a/AirLibUnitTests/SimpleFlightTest.hpp b/AirLibUnitTests/SimpleFlightTest.hpp index a2fe7b44c5..56d50d820f 100644 --- a/AirLibUnitTests/SimpleFlightTest.hpp +++ b/AirLibUnitTests/SimpleFlightTest.hpp @@ -64,7 +64,8 @@ namespace airlib // initial_kinematic_state.pose.orientation.x() = 0.0f; // initial_kinematic_state.pose.orientation.y() = 0.08715574f; // initial_kinematic_state.pose.orientation.z() = 0.0f; - initial_kinematic_state.pose.position.z() = -10.0f; + + // initial_kinematic_state.pose.position.z() = -10.0f; kinematics.reset(new Kinematics(initial_kinematic_state)); Environment::State initial_environment; @@ -99,7 +100,6 @@ namespace airlib std::string message; testAssert(api->isReady(message), message); - clock->sleep_for(6.0f); Utils::getSetMinLogLevel(true, 100); @@ -120,16 +120,21 @@ namespace airlib api->armDisarm(true); //checkStatusMsg(api.get(), &myfile); + clock->sleep_for(6.0f); + // take off api->takeoff(50); pos = api->getMultirotorState().getPosition(); std::cout << "took-off position: " << pos << std::endl; //checkStatusMsg(api.get(), &myfile); + // clock->sleep_for(60.0f); + // api->resetEkf(); + // api->commandAngleRatesZ(0.0f, 0.0f, 0.1f, 0.0f); - // clock->sleep_for(180.0f); + // clock->sleep_for(18.0f); // api->moveByAngleRatesZ(0.0f, 0.0f, 9.0f*M_PI/180, -1.53509f, 10.0f); - // clock->sleep_for(180.0f); + // clock->sleep_for(18.0f); // api->moveByAngleRatesZ(0.0f, 4.0f*M_PI/180, 0.0f, -1.53509f, 10.0f); // clock->sleep_for(60.0f); // api->moveByAngleRatesZ(0.0f, 4.0f*M_PI/180, 0.0f, -1.53509f, 10.0f); @@ -139,23 +144,33 @@ namespace airlib // std::cout << "waypoint position: " << pos << std::endl; // api->moveToPosition(0, -1, -1.53509, 0.1, 1E3, DrivetrainType::MaxDegreeOfFreedom, YawMode(true, 0), -1, 0); // pos = api->getMultirotorState().getPosition(); - std::cout << "waypoint position: " << pos << std::endl; //checkStatusMsg(api.get(), &myfile); + clock->sleep_for(6.0f); + // // // fly towards a waypoint + api->moveToPosition(10, 0, -2, 0.5, 1E3, DrivetrainType::MaxDegreeOfFreedom, YawMode(true, 0), -1, 0); + clock->sleep_for(18.0f); + api->moveToPosition(10, 10, -2, 0.5, 1E3, DrivetrainType::MaxDegreeOfFreedom, YawMode(true, 0), -1, 0); + clock->sleep_for(18.0f); + api->moveToPosition(10, 10, -20, 0.5, 1E3, DrivetrainType::MaxDegreeOfFreedom, YawMode(true, 0), -1, 0); + pos = api->getMultirotorState().getPosition(); + std::cout << "waypoint position: " << pos << std::endl; + // // api->commandVelocity(10.0f, 0.0f, 0.0f, YawMode(true, 0)); + // // pos = api->getMultirotorState().getPosition(); + // // //std::cout << "waypoint position: " << pos << std::endl; + // // //checkStatusMsg(api.get(), &myfile); + // clock->sleep_for(60.0f); - // // fly towards a waypoint - // api->moveToPosition(15, 17, -20, 5, 1E3, DrivetrainType::MaxDegreeOfFreedom, YawMode(true, 0), -1, 0); + // api->moveToPosition(-100, 100, -50, 15, 1E3, DrivetrainType::MaxDegreeOfFreedom, YawMode(true, 0), -1, 0); // pos = api->getMultirotorState().getPosition(); - // //std::cout << "waypoint position: " << pos << std::endl; - // //checkStatusMsg(api.get(), &myfile); - - // clock->sleep_for(60.0f); + // std::cout << "waypoint position: " << pos << std::endl; + clock->sleep_for(6.0f); // // land // //api->land(10); // pos = api->getMultirotorState().getPosition(); // std::cout << "final position: " << pos << std::endl; - checkStatusMsg(api.get(), &myfile); + checkStatusMsg(api.get(), &myfile); // TODO print some values OR log diff --git a/AirLibUnitTests/cleanup.sh b/AirLibUnitTests/cleanup.sh index 99b05df8d4..d43ac15295 100644 --- a/AirLibUnitTests/cleanup.sh +++ b/AirLibUnitTests/cleanup.sh @@ -1,10 +1,17 @@ #!/bin/bash cleanup(){ - in_file="$1" - data_file="$2" - log_file="$3" + debug_flag="$1" + # in_file="$1" + # data_file="$2" + # log_file="$3" # cat "$in_file" | sed -E '/^[^0-9]*$/d' > "$data_file" # cat "$in_file" | sed -E '/^[0-9]+.*$/d' > "$log_file" - cat "log.txt" | sed -E '/^[^0-9]*$/d' > "data.csv" - cat "log.txt" | sed -E '/^[0-9]+.*$/d' > "other.txt" + if [[ $debug_flag == 'd' ]]; then + cat "log.txt" | sed -E '/^[^0-9]*$/d' > "data.csv" + cat "log.txt" | sed -E '/^[0-9]+.*$/d' > "other.txt" + else + cat "../build_release/output/bin/log.txt" | sed -E '/^[^0-9]*$/d' > "data.csv" + cat "../build_release/output/bin/log.txt" | sed -E '/^[0-9]+.*$/d' > "other.txt" + fi + } diff --git a/AirLibUnitTests/main.cpp b/AirLibUnitTests/main.cpp index cf384ed80a..2043786768 100644 --- a/AirLibUnitTests/main.cpp +++ b/AirLibUnitTests/main.cpp @@ -32,8 +32,8 @@ int main() end = std::chrono::system_clock::now(); std::chrono::duration elapsed_seconds = end - start; - std::cout << "elapsed time (s): " << elapsed_seconds.count() << "s\n"; - std::cout << "elapsed time (min): " << elapsed_seconds.count()/60.0f << "s\n"; + std::cout << "elapsed time (s): " << elapsed_seconds.count() << "\n"; + std::cout << "elapsed time (min): " << elapsed_seconds.count()/60.0f << "\n"; return 0; } diff --git a/AirLibUnitTests/plot.py b/AirLibUnitTests/plot.py index 632b0ca930..209c4b4e22 100644 --- a/AirLibUnitTests/plot.py +++ b/AirLibUnitTests/plot.py @@ -13,6 +13,37 @@ import pandas as pd import matplotlib as mpl import matplotlib.pyplot as plt +from matplotlib.path import Path +from matplotlib.patches import PathPatch + +def draw_error_band(ax, x, y, err, **kwargs): + # Calculate normals via centered finite differences (except the first point + # which uses a forward difference and the last point which uses a backward + # difference). + + # dx = np.concatenate([[x[1] - x[0]], x[2:] - x[:-2], [x[-1] - x[-2]]]) + # dy = np.concatenate([[y[1] - y[0]], y[2:] - y[:-2], [y[-1] - y[-2]]]) + # l = np.hypot(dx, dy) + # nx = dy / l + # ny = -dx / l + + # end points of errors + # xp = x + nx * err + # yp = y + ny * err + # xn = x - nx * err + # yn = y - ny * err + xp = x + err + yp = y + err + xn = x - err + yn = y - err + + vertices = np.block([[xp, xn[::-1]], + [yp, yn[::-1]]]).T + codes = np.full(len(vertices), Path.LINETO) + codes[0] = Path.MOVETO + # codes[len(xp)] = Path.MOVETO + path = Path(vertices, codes) + ax.add_patch(PathPatch(path, **kwargs)) def main(): """ Main entry point of the app """ @@ -27,8 +58,33 @@ def main(): # print(data.head()) + # plotResults(data.head(100*300)) plotResults(data) + # N = 400 + # t = np.linspace(0, 2 * np.pi, N) + # r = 0.5 + np.cos(t) + # x, y = r * np.cos(t), r * np.sin(t) + + # fig, ax = plt.subplots() + # # ax.plot(x, y, "k") + # # ax.set(aspect=1) + + # # axs = (plt.figure(constrained_layout=True) + # # .subplots(1, 2, sharex=True, sharey=True)) + # # errs = [ + # # (axs[0], "constant error", 0.05), + # # (axs[1], "variable error", 0.05 * np.sin(2 * t) ** 2 + 0.04), + # # ] + + # ax.set(title=[], aspect=1, xticks=[], yticks=[]) + # ax.plot(x, y, "k") + # draw_error_band(ax, x, y, 0.05 * np.sin(2 * t) ** 2 + 0.04, + # facecolor=f"C{1}", edgecolor="none", alpha=.3) + + # plt.show() + # fig.savefig("plot.pdf") + def M_OB(q0, q1, q2, q3) -> np.array: """ Direction cosine matrix """ return np.array([[q0*q0 + q1*q1 - q2*q2 - q3*q3, 2.0*(q1*q2 - q0*q3), 2.0*(q0*q2 + q1*q3)], @@ -92,25 +148,25 @@ def plotResults(data): i+=1 # extract ground truth states - ground_truth_x = data[i].to_numpy(float) + true_x = data[i].to_numpy(float) i+=1 - ground_truth_y = data[i].to_numpy(float) + true_y = data[i].to_numpy(float) i+=1 - ground_truth_z = data[i].to_numpy(float) + true_z = data[i].to_numpy(float) i+=1 - ground_truth_q0 = data[i].to_numpy(float) + true_q0 = data[i].to_numpy(float) i+=1 - ground_truth_q1 = data[i].to_numpy(float) + true_q1 = data[i].to_numpy(float) i+=1 - ground_truth_q2 = data[i].to_numpy(float) + true_q2 = data[i].to_numpy(float) i+=1 - ground_truth_q3 = data[i].to_numpy(float) + true_q3 = data[i].to_numpy(float) i+=1 - ground_truth_pitch= data[i].to_numpy(float)*180/np.pi + true_pitch= data[i].to_numpy(float)*180/np.pi i+=1 - ground_truth_roll = data[i].to_numpy(float)*180/np.pi + true_roll = data[i].to_numpy(float)*180/np.pi i+=1 - ground_truth_yaw = data[i].to_numpy(float)*180/np.pi + true_yaw = data[i].to_numpy(float)*180/np.pi i+=1 true_lin_vel_x = data[i].to_numpy(float) i+=1 @@ -146,17 +202,17 @@ def plotResults(data): i+=1 esti_lin_vel_z = data[i].to_numpy(float) i+=1 - bias_accel_x = data[i].to_numpy(float) + bias_accel_x = data[i].to_numpy(float) * 1000 / 9.80665 i+=1 - bias_accel_y = data[i].to_numpy(float) + bias_accel_y = data[i].to_numpy(float) * 1000 / 9.80665 i+=1 - bias_accel_z = data[i].to_numpy(float) + bias_accel_z = data[i].to_numpy(float) * 1000 / 9.80665 i+=1 - bias_gyro_x = data[i].to_numpy(float) + bias_gyro_x = data[i].to_numpy(float) * 180/np.pi * 3600 i+=1 - bias_gyro_y = data[i].to_numpy(float) + bias_gyro_y = data[i].to_numpy(float) * 180/np.pi * 3600 i+=1 - bias_gyro_z = data[i].to_numpy(float) + bias_gyro_z = data[i].to_numpy(float) * 180/np.pi * 3600 i+=1 bias_baro = data[i].to_numpy(float) i+=1 @@ -186,21 +242,27 @@ def plotResults(data): i+=1 cov_q3 = (data[i].to_numpy(float)) i+=1 + cov_roll = (data[i].to_numpy(float))*180*180/np.pi/np.pi + i+=1 + cov_pitch = (data[i].to_numpy(float))*180*180/np.pi/np.pi + i+=1 + cov_yaw = (data[i].to_numpy(float))*180*180/np.pi/np.pi + i+=1 # variance of imu bias - cov_b_f_x = (data[i].to_numpy(float)) + cov_b_f_x = (data[i].to_numpy(float)) * (1000 / 9.80665)* (1000 / 9.80665) i+=1 - cov_b_f_y = (data[i].to_numpy(float)) + cov_b_f_y = (data[i].to_numpy(float)) * (1000 / 9.80665)* (1000 / 9.80665) i+=1 - cov_b_f_z = (data[i].to_numpy(float)) + cov_b_f_z = (data[i].to_numpy(float)) * (1000 / 9.80665)* (1000 / 9.80665) i+=1 # variance of gyro bias - cov_b_omega_x = (data[i].to_numpy(float)) + cov_b_omega_x = (data[i].to_numpy(float)) * (180/np.pi * 3600)* (180/np.pi * 3600) i+=1 - cov_b_omega_y = (data[i].to_numpy(float)) + cov_b_omega_y = (data[i].to_numpy(float)) * (180/np.pi * 3600)* (180/np.pi * 3600) i+=1 - cov_b_omega_z = (data[i].to_numpy(float)) + cov_b_omega_z = (data[i].to_numpy(float)) * (180/np.pi * 3600)* (180/np.pi * 3600) i+=1 # variance baro @@ -252,20 +314,20 @@ def plotResults(data): i+=1 # position error - err_estimated_x = ground_truth_x - estimated_x - err_estimated_y = ground_truth_y - estimated_y - err_estimated_z = ground_truth_z - estimated_z + err_estimated_x = true_x - estimated_x + err_estimated_y = true_y - estimated_y + err_estimated_z = true_z - estimated_z # angle error - err_estimated_pitch = ground_truth_pitch - estimated_pitch - err_estimated_roll = ground_truth_roll - estimated_roll - err_estimated_yaw = ground_truth_yaw - estimated_yaw + err_estimated_pitch = true_pitch - estimated_pitch + err_estimated_roll = true_roll - estimated_roll + err_estimated_yaw = true_yaw - estimated_yaw # orientation error - error_q0 = ground_truth_q0 - estimated_q0 - error_q1 = ground_truth_q1 - estimated_q1 - error_q2 = ground_truth_q2 - estimated_q2 - error_q3 = ground_truth_q3 - estimated_q3 + error_q0 = true_q0 - estimated_q0 + error_q1 = true_q1 - estimated_q1 + error_q2 = true_q2 - estimated_q2 + error_q3 = true_q3 - estimated_q3 # orientation error @@ -282,13 +344,13 @@ def plotResults(data): err_gyro_x = true_gyro_x - gyro_x err_gyro_y = true_gyro_y - gyro_y err_gyro_z = true_gyro_z - gyro_z - err_gps_x = ground_truth_x - gps_pos_x - err_gps_y = ground_truth_y - gps_pos_y - err_gps_z = ground_truth_z - gps_pos_z + err_gps_x = true_x - gps_pos_x + err_gps_y = true_y - gps_pos_y + err_gps_z = true_z - gps_pos_z err_gps_vel_x = true_lin_vel_x - gps_vel_x err_gps_vel_y = true_lin_vel_y - gps_vel_y err_gps_vel_z = true_lin_vel_z - gps_vel_z - err_baro_alt = -1*ground_truth_z - baro_alt + err_baro_alt = -1*true_z - baro_alt # print(gps_vel_z) @@ -298,99 +360,88 @@ def plotResults(data): # print("True pitch rate 1: ", true_gyro_x[start_index]) # print("True pitch rate 2: ", true_gyro_x[end_index]) - # print("True pitch angle 1: ", ground_truth_roll[start_index]) + # print("True pitch angle 1: ", true_roll[start_index]) # print("Estimated pitch angle 1: ", estimated_roll[start_index]) - # print("True pitch angle 2: ", ground_truth_roll[end_index]) + # print("True pitch angle 2: ", true_roll[end_index]) # print("Estimated pitch angle 2: ", estimated_roll[end_index]) # print("Time difference: ", timestamp[end_index] - timestamp[start_index]) - print("Expected delta pitch angle: ", (true_gyro_x[start_index]+true_gyro_x[end_index])*0.5*(timestamp[end_index] - timestamp[start_index])) - print("AirSim delta pitch angle: ", (ground_truth_roll[end_index] - ground_truth_roll[start_index])) - print("Our delta pitch angle: ", (estimated_roll[end_index] - estimated_roll[start_index])) + # print("Expected delta pitch angle: ", (true_gyro_x[start_index]+true_gyro_x[end_index])*0.5*(timestamp[end_index] - timestamp[start_index])) + # print("AirSim delta pitch angle: ", (true_roll[end_index] - true_roll[start_index])) + # print("Our delta pitch angle: ", (estimated_roll[end_index] - estimated_roll[start_index])) - start_index = np.where(np.int_(timestamp) == 6)[0][0] - end_index = np.where(np.int_(timestamp) == 7)[0][0] - print("Expected delta pitch angle: ", (true_gyro_x[start_index]+true_gyro_x[end_index])*0.5*(timestamp[end_index] - timestamp[start_index])) - print("AirSim delta pitch angle: ", (ground_truth_roll[end_index] - ground_truth_roll[start_index])) - print("Our delta pitch angle: ", (estimated_roll[end_index] - estimated_roll[start_index])) + # start_index = np.where(np.int_(timestamp) == 6)[0][0] + # end_index = np.where(np.int_(timestamp) == 7)[0][0] + # print("Expected delta pitch angle: ", (true_gyro_x[start_index]+true_gyro_x[end_index])*0.5*(timestamp[end_index] - timestamp[start_index])) + # print("AirSim delta pitch angle: ", (true_roll[end_index] - true_roll[start_index])) + # print("Our delta pitch angle: ", (estimated_roll[end_index] - estimated_roll[start_index])) # k = np.array([[1, 2, 3],[4, 5, 6],[7, 8, 9]]) # print(k) # print(k[2,1]) - theta_error = np.zeros(len(timestamp)) - phi_error = np.zeros(len(timestamp)) - psi_error = np.zeros(len(timestamp)) - # print(psi_error) + # theta_error = np.zeros(len(timestamp)) + # phi_error = np.zeros(len(timestamp)) + # psi_error = np.zeros(len(timestamp)) + # # print(psi_error) - for i in range(0, len(timestamp)): - ground = M_OB(ground_truth_q0[i], - ground_truth_q1[i], - ground_truth_q2[i], - ground_truth_q3[i]) - estimated = M_OB(estimated_q0[i], - estimated_q1[i], - estimated_q2[i], - estimated_q3[i]) - # print(ground_error) + # for i in range(0, len(timestamp)): + # ground = M_OB(true_q0[i], + # true_q1[i], + # true_q2[i], + # true_q3[i]) + # estimated = M_OB(estimated_q0[i], + # estimated_q1[i], + # estimated_q2[i], + # estimated_q3[i]) + # # print(ground_error) - error_direction_cosine = ground*estimated.transpose() - # print(error_direction_cosine[2, 0]) + # error_direction_cosine = ground*estimated.transpose() + # # print(error_direction_cosine[2, 0]) - # euler = [atan2(mat(3,2),mat(3,3)); - # -asin(mat(3,1)); - # atan2(mat(2,1),mat(1,1))]; + # # euler = [atan2(mat(3,2),mat(3,3)); + # # -asin(mat(3,1)); + # # atan2(mat(2,1),mat(1,1))]; - phi_error[i] = np.arctan2(error_direction_cosine[2, 1],error_direction_cosine[2, 2])*180/np.pi - theta_error[i] = -np.arcsin(error_direction_cosine[2, 0])*180/np.pi - psi_error[i] = np.arctan2(error_direction_cosine[1, 0],error_direction_cosine[0, 0])*180/np.pi + # phi_error[i] = np.arctan2(error_direction_cosine[2, 1],error_direction_cosine[2, 2])*180/np.pi + # theta_error[i] = -np.arcsin(error_direction_cosine[2, 0])*180/np.pi + # psi_error[i] = np.arctan2(error_direction_cosine[1, 0],error_direction_cosine[0, 0])*180/np.pi # apply style plt.style.use('./style.mplstyle') # define the plot - fig, ax = plt.subplots(25, 1) + fig, ax = plt.subplots(36, 1) # begin plotting i=0 - ax[i].plot(timestamp, accel_x, linestyle='dotted', color='C1', label="x") + ax[i].plot(timestamp, accel_x, linestyle='solid', color='C0', label="x") ax[i].legend() - ax[i].plot(timestamp, accel_y, linestyle='solid', color='C3', label="y") + ax[i].plot(timestamp, accel_y, linestyle='dotted', color='C1', label="y") ax[i].legend() - ax[i].plot(timestamp, accel_z, linestyle='dotted', color='C5', label="z") + ax[i].plot(timestamp, accel_z, linestyle='solid', color='C2', label="z") ax[i].legend() ax[i].set_ylabel('accel (m/s2)') - # ax[i].set_title('Measurements') - # ax[i].set_xlim([10, 15]) - # ax[i].set_ylim([-15, 5]) ax[i].grid(True) - # i+=1 - # ax[i].plot(timestamp, gyro_x, linestyle='solid', color='C1', label="roll") - # ax[i].legend() - # ax[i].plot(timestamp, gyro_y, linestyle='solid', color='C2', label="pitch") - # ax[i].legend() - # ax[i].plot(timestamp, gyro_z, linestyle='dotted', color='C5', label="yaw") - # ax[i].legend() - # ax[i].set_ylabel('gyro error(deg/s)') - # ax[i].set_xlim([10, 15]) - # ax[i].set_ylim([-0.01, 0.01]) - # ax[i].grid(True) - - # i+=1 - # ax[i].plot(timestamp, err_baro_alt, linestyle='solid', color='C1') - # ax[i].legend() - # ax[i].set_ylabel('baro alt err(m)') - # ax[i].grid(True) + i+=1 + ax[i].plot(timestamp, err_accel_x, linestyle='solid', color='C0', label="x") + ax[i].legend() + ax[i].plot(timestamp, err_accel_y, linestyle='dotted', color='C1', label="y") + ax[i].legend() + ax[i].plot(timestamp, err_accel_z, linestyle='solid', color='C2', label="z") + ax[i].legend() + ax[i].set_ylabel('accel err (m/s2)') + ax[i].grid(True) i+=1 - ax[i].plot(timestamp, gyro_x, linestyle='dotted', color='C1', label="roll") + ax[i].plot(timestamp, gyro_x, linestyle='solid', color='C0', label="roll") ax[i].legend() - ax[i].plot(timestamp, gyro_y, linestyle='solid', color='C2', label="pitch") + ax[i].plot(timestamp, gyro_y, linestyle='dotted', color='C1', label="pitch") ax[i].legend() - ax[i].plot(timestamp, gyro_z, linestyle='dotted', color='C5', label="yaw") + ax[i].plot(timestamp, gyro_z, linestyle='solid', color='C2', label="yaw") ax[i].legend() ax[i].set_ylabel('gyro rates (deg/s)') # ax[i].set_xlim([10, 15]) @@ -398,136 +449,144 @@ def plotResults(data): ax[i].grid(True) i+=1 - ax[i].plot(timestamp, err_gps_x, linestyle='solid', color='C1', label="x") + ax[i].plot(timestamp, err_gyro_x, linestyle='solid', color='C0', label="roll") ax[i].legend() - ax[i].plot(timestamp, err_gps_y, linestyle='solid', color='C2', label="y") + ax[i].plot(timestamp, err_gyro_y, linestyle='dotted', color='C1', label="pitch") ax[i].legend() - ax[i].plot(timestamp, err_gps_z, linestyle='solid', color='C5', label="z") + ax[i].plot(timestamp, err_gyro_z, linestyle='solid', color='C2', label="yaw") + ax[i].legend() + ax[i].set_ylabel('gyro error(deg/s)') + ax[i].grid(True) + + i+=1 + ax[i].plot(timestamp, err_gps_x, linestyle='solid', color='C0', label="x") + ax[i].legend() + ax[i].plot(timestamp, err_gps_y, linestyle='solid', color='C1', label="y") + ax[i].legend() + ax[i].plot(timestamp, err_gps_z, linestyle='solid', color='C2', label="z") ax[i].legend() ax[i].set_ylabel('gps position err(m)') ax[i].grid(True) i+=1 - ax[i].plot(timestamp, err_gps_vel_x, linestyle='solid', color='C1', label="x") + ax[i].plot(timestamp, err_gps_vel_x, linestyle='solid', color='C0', label="x") ax[i].legend() - ax[i].plot(timestamp, err_gps_vel_y, linestyle='solid', color='C2', label="y") + ax[i].plot(timestamp, err_gps_vel_y, linestyle='solid', color='C1', label="y") ax[i].legend() - ax[i].plot(timestamp, err_gps_vel_z, linestyle='solid', color='C5', label="z") + ax[i].plot(timestamp, err_gps_vel_z, linestyle='solid', color='C2', label="z") ax[i].legend() ax[i].set_ylabel('gps velocity err(m/s)') ax[i].grid(True) i+=1 - ax[i].plot(timestamp, mag_flux_x, linestyle='solid', color='C1', label="x") + ax[i].plot(timestamp, mag_flux_x, linestyle='solid', color='C0', label="x") ax[i].legend() - ax[i].plot(timestamp, mag_flux_y, linestyle='solid', color='C3', label="y") + ax[i].plot(timestamp, mag_flux_y, linestyle='solid', color='C1', label="y") ax[i].legend() - ax[i].plot(timestamp, mag_flux_z, linestyle='solid', color='C5', label="z") + ax[i].plot(timestamp, mag_flux_z, linestyle='solid', color='C2', label="z") ax[i].legend() - ax[i].set_ylabel('Magnetic flux measurement (Gauss)') + ax[i].set_ylabel('Mag. flux (Gauss)') ax[i].grid(True) i+=1 - ax[i].plot(timestamp, ground_truth_pitch, linestyle='solid', color='C1', label="ground_truth_pitch") + ax[i].plot(timestamp, true_pitch, linestyle='solid', color='k', label="true_pitch") ax[i].legend(loc='upper left') - ax[i].plot(timestamp, estimated_pitch, linestyle='dotted', color='C6', label="estimated_pitch", lw='1.2') + ax[i].plot(timestamp, estimated_pitch, linestyle='solid', color='C0', label="estimated_pitch") ax[i].legend(loc='upper left') - ax[i].plot(timestamp, ground_truth_roll, linestyle='solid', color='C2', label="ground_truth_roll") + ax[i].fill_between(timestamp, + estimated_pitch-3*np.sqrt(cov_pitch), + estimated_pitch+3*np.sqrt(cov_pitch), + alpha=0.2,linewidth=0, color='C5') + ax[i].set_ylabel('pitch(deg)') + # ax[i].set_xlim([10, 15]) + # ax[i].set_ylim([-1, 40]) + ax[i].grid(True) + + i+=1 + ax[i].plot(timestamp, true_roll, linestyle='solid', color='k', label="true_roll") ax[i].legend(loc='upper left') - ax[i].plot(timestamp, estimated_roll, linestyle='dotted', color='C7', label="estimated_roll", lw='1.2') + ax[i].plot(timestamp, estimated_roll, linestyle='solid', color='C1', label="estimated_roll") ax[i].legend(loc='upper left') - ax[i].set_ylabel('angles(deg)') + ax[i].fill_between(timestamp, + estimated_roll-3*np.sqrt(cov_roll), + estimated_roll+3*np.sqrt(cov_roll), + alpha=0.2,linewidth=0, color='C5') + ax[i].set_ylabel('roll(deg)') # ax[i].set_xlim([10, 15]) # ax[i].set_ylim([-1, 40]) ax[i].grid(True) i+=1 - ax[i].plot(timestamp, ground_truth_yaw, linestyle='solid', color='C5', label="ground_truth_yaw") + ax[i].plot(timestamp, true_yaw, linestyle='solid', color='k', label="true_yaw") ax[i].legend() - ax[i].plot(timestamp, estimated_yaw, linestyle='dotted', color='C8', label="estimated_yaw") + ax[i].plot(timestamp, estimated_yaw, linestyle='solid', color='C2', label="estimated_yaw") ax[i].legend() + ax[i].fill_between(timestamp, + estimated_yaw-3*np.sqrt(cov_yaw), + estimated_yaw+3*np.sqrt(cov_yaw), + alpha=0.2,linewidth=0, color='C5') ax[i].set_ylabel('yaw(deg)') # ax[i].set_xlim([10, 15]) # ax[i].set_ylim([-2, 2]) ax[i].grid(True) i+=1 - ax[i].plot(timestamp, err_estimated_pitch, linestyle='solid', color='C1', label="pitch") + ax[i].plot(timestamp, true_x, linestyle='solid', color='k', label="true_x") ax[i].legend() - ax[i].plot(timestamp, err_estimated_roll, linestyle='solid', color='C2', label="roll") + ax[i].plot(timestamp, estimated_x, linestyle='solid', color='C0', label="estimated_x") ax[i].legend() - ax[i].set_ylabel('angles error(deg)') - # ax[i].set_xlim([5, 6]) - # ax[i].set_ylim([-0.0001, 0.0001]) + ax[i].fill_between(timestamp, + estimated_x-3*np.sqrt(cov_x), + estimated_x+3*np.sqrt(cov_x), + alpha=0.3,linewidth=0, color='C5') + ax[i].set_ylabel('x position(m)') + # ax[i].set_xlim([10, 15]) ax[i].grid(True) i+=1 - ax[i].plot(timestamp, err_estimated_yaw, linestyle='dotted', color='C5', label="yaw") + ax[i].plot(timestamp, true_y, linestyle='solid', color='k', label="true_y") ax[i].legend() - ax[i].set_ylabel('angles error(deg)') - # ax[i].set_xlim([5, 6]) - # ax[i].set_ylim([-0.0001, 0.0001]) + ax[i].plot(timestamp, estimated_y, linestyle='solid', color='C1', label="estimated_y") + ax[i].legend() + ax[i].fill_between(timestamp, + estimated_y-3*np.sqrt(cov_y), + estimated_y+3*np.sqrt(cov_y), + alpha=0.3,linewidth=0, color='C5') + ax[i].set_ylabel('y position(m)') + # ax[i].set_xlim([10, 15]) ax[i].grid(True) - # i+=1 - # ax[i].plot(timestamp, theta_error, linestyle='solid', color='C1', label="pitch") - # ax[i].legend() - # ax[i].plot(timestamp, phi_error, linestyle='solid', color='C2', label="roll") - # ax[i].legend() - # ax[i].plot(timestamp, psi_error, linestyle='dotted', color='C5', label="yaw") - # ax[i].legend() - # ax[i].set_ylabel('angles error using Direc Cosine(deg)') - # # ax[i].set_xlim([5, 6]) - # # ax[i].set_ylim([-0.0001, 0.0001]) - # ax[i].grid(True) - - # i+=1 - # ax[i].plot(timestamp, err_estimated_pitch, linestyle='solid', color='C1', label="pitch") - # ax[i].legend() - # ax[i].plot(timestamp, err_estimated_roll, linestyle='solid', color='C2', label="roll") - # ax[i].legend() - # ax[i].plot(timestamp, err_estimated_yaw, linestyle='dotted', color='C5', label="yaw") - # ax[i].legend() - # ax[i].set_ylabel('angles error(deg)') - # ax[i].set_xlim([25, 27]) - # ax[i].set_ylim([-0.00001, 0.00007]) - # ax[i].grid(True) - - # i+=1 - # ax[i].plot(timestamp, err_estimated_pitch, linestyle='solid', color='C1', label="pitch") - # ax[i].legend() - # ax[i].plot(timestamp, err_estimated_roll, linestyle='solid', color='C2', label="roll") - # ax[i].legend() - # ax[i].plot(timestamp, err_estimated_yaw, linestyle='dotted', color='C5', label="yaw") - # ax[i].legend() - # ax[i].set_ylabel('angles error(deg)') - # ax[i].set_xlim([27, 29]) - # # ax[i].set_ylim([-0.0001, 0.0001]) - # ax[i].grid(True) - - - - i+=1 - ax[i].plot(timestamp, estimated_x, linestyle='solid', color='C1', label="x") - ax[i].legend() - ax[i].plot(timestamp, estimated_y, linestyle='solid', color='C2', label="y") - ax[i].legend() - ax[i].plot(timestamp, estimated_z, linestyle='solid', color='C5', label="z") - ax[i].legend() - ax[i].set_ylabel('estimated position(m)') + i+=1 + ax[i].plot(timestamp, true_z, linestyle='solid', color='k', label="true_z") + ax[i].legend() + ax[i].plot(timestamp, estimated_z, linestyle='solid', color='C2', label="estimated_z") + ax[i].legend() + ax[i].fill_between(timestamp, + estimated_z-3*np.sqrt(cov_z), + estimated_z+3*np.sqrt(cov_z), + alpha=0.3,linewidth=0, color='C5') + ax[i].set_ylabel('z position(m)') # ax[i].set_xlim([10, 15]) + ax[i].set_xlabel('Time (sec)') ax[i].grid(True) + i+=1 - ax[i].plot(timestamp, ground_truth_x, linestyle='solid', color='C1', label="x") + ax[i].plot(timestamp, err_estimated_pitch, linestyle='solid', color='C1', label="pitch") ax[i].legend() - ax[i].plot(timestamp, ground_truth_y, linestyle='solid', color='C2', label="y") + ax[i].plot(timestamp, err_estimated_roll, linestyle='solid', color='C2', label="roll") ax[i].legend() - ax[i].plot(timestamp, ground_truth_z, linestyle='solid', color='C5', label="z") + ax[i].set_ylabel('angles error(deg)') + # ax[i].set_xlim([5, 6]) + # ax[i].set_ylim([-0.0001, 0.0001]) + ax[i].grid(True) + + i+=1 + ax[i].plot(timestamp, err_estimated_yaw, linestyle='dotted', color='C5', label="yaw") ax[i].legend() - ax[i].set_ylabel('true position(m)') - # ax[i].set_xlim([10, 15]) - # ax[i].set_ylim([-1, 10]) + ax[i].set_ylabel('angles error(deg)') + # ax[i].set_xlim([5, 6]) + # ax[i].set_ylim([-0.0001, 0.0001]) ax[i].grid(True) i+=1 @@ -542,48 +601,44 @@ def plotResults(data): # ax[i].set_ylim([-0.1, 0.1]) ax[i].grid(True) - # i+=1 - # ax[i].plot(timestamp, ground_truth_y, linestyle='solid', color='C1', label="ground_truth") - # ax[i].legend() - # ax[i].plot(timestamp, estimated_y, linestyle='solid', color='C2', label="estimated") - # ax[i].legend() - # # ax[i].plot(timestamp, gps_y, linestyle='dotted', color='C5', label="gps") - # # ax[i].legend() - # ax[i].set_ylabel('y_position (m)') - # ax[i].grid(True) - - # i+=1 - # ax[i].plot(timestamp, ground_truth_z, linestyle='solid', color='C1', label="ground_truth") - # ax[i].legend() - # ax[i].plot(timestamp, estimated_z, linestyle='solid', color='C2', label="estimated", lw='0.5') - # ax[i].legend() - # # ax[i].plot(timestamp, gps_z, linestyle='dotted', color='C5', label="gps") - # # ax[i].legend() - # ax[i].set_ylabel('z_position (m)') - # ax[i].grid(True) - - - i+=1 - ax[i].plot(timestamp, esti_lin_vel_x, linestyle='solid', color='C1', label="x") + ax[i].plot(timestamp, true_lin_vel_x, linestyle='solid', color='k', label="true_x") ax[i].legend() - ax[i].plot(timestamp, esti_lin_vel_y, linestyle='solid', color='C2', label="y") + ax[i].plot(timestamp, esti_lin_vel_x, linestyle='solid', color='C0', label="estimated_x") ax[i].legend() - ax[i].plot(timestamp, esti_lin_vel_z, linestyle='solid', color='C5', label="z") - ax[i].legend() - ax[i].set_ylabel('estimated lin vel(m/s)') + ax[i].fill_between(timestamp, + esti_lin_vel_x-3*np.sqrt(cov_u), + esti_lin_vel_x+3*np.sqrt(cov_u), + alpha=0.3,linewidth=0, color='C5') + ax[i].set_ylabel('x lin vel(m/s)') # ax[i].set_xlim([10, 15]) # ax[i].set_ylim([-1, 10]) ax[i].grid(True) i+=1 - ax[i].plot(timestamp, true_lin_vel_x, linestyle='solid', color='C1', label="x") + ax[i].plot(timestamp, true_lin_vel_y, linestyle='solid', color='k', label="true_y") + ax[i].legend() + ax[i].plot(timestamp, esti_lin_vel_y, linestyle='solid', color='C1', label="estimated_y") ax[i].legend() - ax[i].plot(timestamp, true_lin_vel_y, linestyle='solid', color='C2', label="y") + ax[i].fill_between(timestamp, + esti_lin_vel_y-3*np.sqrt(cov_v), + esti_lin_vel_y+3*np.sqrt(cov_v), + alpha=0.3,linewidth=0, color='C5') + ax[i].set_ylabel('y lin vel(m/s)') + # ax[i].set_xlim([10, 15]) + # ax[i].set_ylim([-1, 10]) + ax[i].grid(True) + + i+=1 + ax[i].plot(timestamp, true_lin_vel_z, linestyle='solid', color='k', label="true_z") ax[i].legend() - ax[i].plot(timestamp, true_lin_vel_z, linestyle='solid', color='C5', label="z") + ax[i].plot(timestamp, esti_lin_vel_z, linestyle='solid', color='C2', label="estimated_z") ax[i].legend() - ax[i].set_ylabel('true lin vel(m/s)') + ax[i].fill_between(timestamp, + esti_lin_vel_z-3*np.sqrt(cov_w), + esti_lin_vel_z+3*np.sqrt(cov_w), + alpha=0.3,linewidth=0, color='C5') + ax[i].set_ylabel('estimated lin vel(m/s)') # ax[i].set_xlim([10, 15]) # ax[i].set_ylim([-1, 10]) ax[i].grid(True) @@ -600,8 +655,6 @@ def plotResults(data): # ax[i].set_ylim([-0.01, 0.01]) ax[i].grid(True) - - i+=1 ax[i].plot(timestamp, quat_norm, linestyle='solid', color='C1', label="norm") ax[i].legend() @@ -611,162 +664,156 @@ def plotResults(data): ax[i].grid(True) i+=1 - ax[i].plot(timestamp, bias_accel_x, linestyle='solid', color='C1', label="accel_x") + ax[i].plot(timestamp, bias_accel_x, linestyle='solid', color='C0', label="accel_x") ax[i].legend() - ax[i].plot(timestamp, bias_accel_y, linestyle='solid', color='C2', label="accel_y") + ax[i].fill_between(timestamp, + bias_accel_x-3*np.sqrt(cov_b_f_x), + bias_accel_x+3*np.sqrt(cov_b_f_x), + alpha=0.3,linewidth=0, color='C5') + ax[i].set_ylabel('x accel biases mili g') + ax[i].grid(True) + i+=1 + ax[i].plot(timestamp, bias_accel_y, linestyle='solid', color='C1', label="accel_y") ax[i].legend() - ax[i].plot(timestamp, bias_accel_z, linestyle='solid', color='C3', label="accel_z") + ax[i].fill_between(timestamp, + bias_accel_y-3*np.sqrt(cov_b_f_y), + bias_accel_y+3*np.sqrt(cov_b_f_y), + alpha=0.3,linewidth=0, color='C5') + ax[i].set_ylabel('y accel biases mili g') + ax[i].grid(True) + i+=1 + ax[i].plot(timestamp, bias_accel_z, linestyle='solid', color='C2', label="accel_z") ax[i].legend() - ax[i].set_ylabel('biases') + ax[i].fill_between(timestamp, + bias_accel_z-3*np.sqrt(cov_b_f_z), + bias_accel_z+3*np.sqrt(cov_b_f_z), + alpha=0.3,linewidth=0, color='C5') + ax[i].set_ylabel('z accel biases mili g') ax[i].grid(True) + i+=1 - ax[i].plot(timestamp, bias_gyro_x, linestyle='solid', color='C4', label="gyro_x") - ax[i].legend() - ax[i].plot(timestamp, bias_gyro_y, linestyle='solid', color='C5', label="gyro_y") + ax[i].plot(timestamp, bias_gyro_x, linestyle='solid', color='C0', label="gyro_x") ax[i].legend() - ax[i].plot(timestamp, bias_gyro_z, linestyle='solid', color='C6', label="gyro_z") - ax[i].legend() - ax[i].plot(timestamp, bias_baro, linestyle='solid', color='C7', label="baro") - ax[i].legend() - ax[i].set_ylabel('biases') + ax[i].fill_between(timestamp, + bias_gyro_x-3*np.sqrt(cov_b_omega_x), + bias_gyro_x+3*np.sqrt(cov_b_omega_x), + alpha=0.3,linewidth=0, color='C5') + ax[i].set_ylabel('x gyro biases deg/hr') ax[i].grid(True) - # i+=1 - # ax[i].plot(timestamp, ground_truth_roll, linestyle='solid', color='C1', label="ground_truth") - # ax[i].legend() - # ax[i].plot(timestamp, estimated_roll, linestyle='dotted', color='black', label="estimated") - # ax[i].legend() - # ax[i].set_ylabel('roll') - # ax[i].grid(True) + i+=1 + ax[i].plot(timestamp, bias_gyro_y, linestyle='solid', color='C1', label="gyro_y") + ax[i].legend() + ax[i].fill_between(timestamp, + bias_gyro_y-3*np.sqrt(cov_b_omega_y), + bias_gyro_y+3*np.sqrt(cov_b_omega_y), + alpha=0.3,linewidth=0, color='C5') + ax[i].set_ylabel('y gyro biases deg/hr') + ax[i].grid(True) - # i+=1 - # ax[i].plot(timestamp, ground_truth_yaw, linestyle='solid', color='C1', label="ground_truth") - # ax[i].legend() - # ax[i].plot(timestamp, estimated_yaw, linestyle='dotted', color='black', label="estimated") - # ax[i].legend() - # ax[i].set_ylabel('yaw') - # ax[i].grid(True) + i+=1 + ax[i].plot(timestamp, bias_gyro_z, linestyle='solid', color='C2', label="gyro_z") + ax[i].legend() + ax[i].fill_between(timestamp, + bias_gyro_z-3*np.sqrt(cov_b_omega_z), + bias_gyro_z+3*np.sqrt(cov_b_omega_z), + alpha=0.3,linewidth=0, color='C5') + ax[i].set_ylabel('z gyro biases deg/hr') + ax[i].grid(True) - - # i+=1 - # ax[i].plot(timestamp, ground_truth_q0, linestyle='solid', color='C1', label="ground_truth_q0") - # ax[i].legend() - # ax[i].set_ylabel('ground_truth quaternions') - # ax[i].grid(True) - - # i+=1 - # ax[i].plot(timestamp, ground_truth_q1, linestyle='dotted', color='C2', label="ground_truth_q1") - # ax[i].legend() - # ax[i].plot(timestamp, ground_truth_q2, linestyle='solid', color='C4', label="ground_truth_q2") - # ax[i].legend() - # ax[i].plot(timestamp, ground_truth_q3, linestyle='dotted', color='C5', label="ground_truth_q3") - # ax[i].legend() - # ax[i].set_ylabel('ground_truth quaternions') - # ax[i].grid(True) - - # i+=1 - # ax[i].plot(timestamp, estimated_q0, linestyle='solid', color='C1', label="estimated_q0") - # ax[i].legend() - # ax[i].set_ylabel('estimated quaternions') - # ax[i].grid(True) - - # i+=1 - # ax[i].plot(timestamp, estimated_q1, linestyle='dotted', color='C2', label="estimated_q1") - # ax[i].legend() - # ax[i].plot(timestamp, estimated_q2, linestyle='solid', color='C4', label="estimated_q2") - # ax[i].legend() - # ax[i].plot(timestamp, estimated_q3, linestyle='dotted', color='C5', label="estimated_q3") - # ax[i].legend() - # ax[i].set_ylabel('estimated quaternions') - # ax[i].grid(True) - - # i+=1 - # ax[i].plot(timestamp, error_q0, linestyle='solid', color='C1', label="error_q0") - # ax[i].legend() - # ax[i].set_ylabel('error quaternions') - # ax[i].grid(True) - - # i+=1 - # ax[i].plot(timestamp, error_q1, linestyle='dotted', color='C2', label="error_q1") - # ax[i].legend() - # ax[i].plot(timestamp, error_q2, linestyle='solid', color='C4', label="error_q2") - # ax[i].legend() - # ax[i].plot(timestamp, error_q3, linestyle='dotted', color='C5', label="error_q3") - # ax[i].legend() - # ax[i].set_ylabel('error quaternions') - # ax[i].grid(True) + i+=1 + ax[i].plot(timestamp, bias_baro, linestyle='solid', color='C3', label="baro") + ax[i].legend() + ax[i].fill_between(timestamp, + bias_baro-3*np.sqrt(cov_b_baro), + bias_baro+3*np.sqrt(cov_b_baro), + alpha=0.3,linewidth=0, color='C5') + ax[i].set_ylabel('baro biases deg/hr') + ax[i].grid(True) i+=1 - ax[i].plot(timestamp, cov_x, linestyle='solid', color='C1', label="cov_x") + ax[i].plot(timestamp, np.sqrt(cov_x), linestyle='solid', color='C0', label="std_dev_x") ax[i].legend() - ax[i].plot(timestamp, cov_y, linestyle='dotted', color='black', label="cov_y") + ax[i].plot(timestamp, np.sqrt(cov_y), linestyle='dotted', color='C1', label="std_dev_y") ax[i].legend() - ax[i].plot(timestamp, cov_z, linestyle='solid', color='C3', label="cov_z") + ax[i].plot(timestamp, np.sqrt(cov_z), linestyle='solid', color='C2', label="std_dev_z") ax[i].legend() ax[i].set_yscale("log") - ax[i].set_ylabel('cov_position') + ax[i].set_ylabel('std_dev_position(m)') ax[i].grid(True) i+=1 - ax[i].plot(timestamp, cov_u, linestyle='solid', color='C1', label="cov_u") + ax[i].plot(timestamp, np.sqrt(cov_u), linestyle='solid', color='C0', label="std_dev_u") + ax[i].legend() + ax[i].plot(timestamp, np.sqrt(cov_v), linestyle='dotted', color='C1', label="std_dev_v") + ax[i].legend() + ax[i].plot(timestamp, np.sqrt(cov_w), linestyle='solid', color='C2', label="std_dev_w") ax[i].legend() - ax[i].plot(timestamp, cov_v, linestyle='dotted', color='black', label="cov_v") + ax[i].set_yscale("log") + ax[i].set_ylabel('std_dev_lin_velocity(m/s)') + ax[i].grid(True) + + i+=1 + ax[i].plot(timestamp, np.sqrt(cov_q0), linestyle='solid', color='C3', label="std_dev_q0") ax[i].legend() - ax[i].plot(timestamp, cov_w, linestyle='solid', color='C3', label="cov_w") ax[i].legend() ax[i].set_yscale("log") - ax[i].set_ylabel('cov_linear_velocity') + ax[i].set_xlabel('Time (s)') + ax[i].set_ylabel('std_dev_orientation') ax[i].grid(True) i+=1 - ax[i].plot(timestamp, cov_q0, linestyle='solid', color='C1', label="cov_q0") + ax[i].plot(timestamp, np.sqrt(cov_q1), linestyle='solid', color='C0', label="std_dev_q1") + ax[i].legend() + ax[i].plot(timestamp, np.sqrt(cov_q2), linestyle='dotted', color='C1', label="std_dev_q2") ax[i].legend() + ax[i].plot(timestamp, np.sqrt(cov_q3), linestyle='dotted', color='C2', label="std_dev_q3") ax[i].legend() ax[i].set_yscale("log") ax[i].set_xlabel('Time (s)') - ax[i].set_ylabel('cov_orientation') + ax[i].set_ylabel('std_dev_orientation') ax[i].grid(True) i+=1 - ax[i].plot(timestamp, cov_q1, linestyle='solid', color='C2', label="cov_q1") + ax[i].plot(timestamp, np.sqrt(cov_roll), linestyle='solid', color='C0', label="std_dev_roll") ax[i].legend() - ax[i].plot(timestamp, cov_q2, linestyle='dotted', color='C3', label="cov_q2") + ax[i].plot(timestamp, np.sqrt(cov_pitch), linestyle='solid', color='C1', label="std_dev_pitch") ax[i].legend() - ax[i].plot(timestamp, cov_q3, linestyle='dotted', color='C4', label="cov_q3") + ax[i].plot(timestamp, np.sqrt(cov_yaw), linestyle='dotted', color='C2', label="std_dev_yaw") ax[i].legend() ax[i].set_yscale("log") ax[i].set_xlabel('Time (s)') - ax[i].set_ylabel('cov_orientation') + ax[i].set_ylabel('std_dev_angles(deg)') ax[i].grid(True) i+=1 - ax[i].plot(timestamp, cov_b_f_x, linestyle='solid', color='C1', label="cov_b_f_x") + ax[i].plot(timestamp, np.sqrt(cov_b_f_x), linestyle='solid', color='C0', label="std_dev_b_f_x") ax[i].legend() - ax[i].plot(timestamp, cov_b_f_y, linestyle='dotted', color='black', label="cov_b_f_y") + ax[i].plot(timestamp, np.sqrt(cov_b_f_y), linestyle='dotted', color='C1', label="std_dev_b_f_y") ax[i].legend() - ax[i].plot(timestamp, cov_b_f_z, linestyle='solid', color='C3', label="cov_b_f_z") + ax[i].plot(timestamp, np.sqrt(cov_b_f_z), linestyle='solid', color='C2', label="std_dev_b_f_z") ax[i].legend() ax[i].set_yscale("log") - ax[i].set_ylabel('cov_imu_bias') + ax[i].set_ylabel('std_dev_imu_bias(milli g)') ax[i].grid(True) i+=1 - ax[i].plot(timestamp, cov_b_omega_x, linestyle='solid', color='C1', label="cov_b_omega_x") + ax[i].plot(timestamp, np.sqrt(cov_b_omega_x), linestyle='solid', color='C0', label="std_dev_b_omega_x") ax[i].legend() - ax[i].plot(timestamp, cov_b_omega_y, linestyle='dotted', color='black', label="cov_b_omega_y") + ax[i].plot(timestamp, np.sqrt(cov_b_omega_y), linestyle='dotted', color='C1', label="std_dev_b_omega_y") ax[i].legend() - ax[i].plot(timestamp, cov_b_omega_z, linestyle='solid', color='C3', label="cov_b_omega_z") + ax[i].plot(timestamp, np.sqrt(cov_b_omega_z), linestyle='solid', color='C2', label="std_dev_b_omega_z") ax[i].legend() ax[i].set_yscale("log") - ax[i].set_ylabel('cov_gyro_bias') + ax[i].set_ylabel('std_dev_gyro_bias(deg/hr)') ax[i].grid(True) i+=1 - ax[i].plot(timestamp, cov_b_baro, linestyle='solid', color='C1', label="cov_b_baro") + ax[i].plot(timestamp, np.sqrt(cov_b_baro), linestyle='solid', color='C0', label="std_dev_b_baro") ax[i].legend() ax[i].set_yscale("log") - ax[i].set_ylabel('cov_baro_bias') + ax[i].set_ylabel('std_dev_baro_bias(m)') ax[i].grid(True) fig.savefig("figure.pdf") diff --git a/AirLibUnitTests/style.mplstyle b/AirLibUnitTests/style.mplstyle index 4fde82193b..e2c3732776 100644 --- a/AirLibUnitTests/style.mplstyle +++ b/AirLibUnitTests/style.mplstyle @@ -10,10 +10,10 @@ # see: https://github.com/garrettj403/SciencePlots # Set color cycle: blue, green, yellow, red, violet, gray -axes.prop_cycle : cycler('color', ['0C5DA5', '00B945', 'FF9500', 'FF2C00', '845B97', '474747', '9e9e9e']) +axes.prop_cycle : cycler('color', ['0C5DA5', '00B945','FF2C00', 'FF9500', '845B97', '474747', '9e9e9e']) # Set default figure size -figure.figsize : 15, 80 +figure.figsize : 5, 100 # Set x axis xtick.direction : in @@ -36,7 +36,7 @@ ytick.right : True # Set line widths axes.linewidth : 0.5 grid.linewidth : 0.5 -lines.linewidth : 1 +lines.linewidth : 0.8 # Remove legend frame legend.frameon : False From 5914289139e033db292e99999873df5e4e968b15 Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Wed, 12 Jan 2022 14:39:21 +0100 Subject: [PATCH 48/87] cosmetic cheanups --- AirLib/include/common/VectorMath.hpp | 1 - AirLib/include/sensors/SensorBase.hpp | 3 --- .../sensors/barometer/BarometerSimple.hpp | 3 +-- AirLib/include/sensors/gps/GpsSimple.hpp | 14 +++----------- AirLib/include/sensors/imu/ImuSimple.hpp | 1 - .../sensors/magnetometer/MagnetometerSimple.hpp | 1 - .../firmwares/simple_flight/AirSimSimpleEkf.hpp | 16 +++------------- .../simple_flight/AirSimSimpleFlightBoard.hpp | 7 ------- .../firmwares/simple_flight/SimpleFlightApi.hpp | 3 +-- .../firmware/interfaces/IBoardSensors.hpp | 1 - AirLibUnitTests/SimpleFlightTest.hpp | 2 +- 11 files changed, 9 insertions(+), 43 deletions(-) diff --git a/AirLib/include/common/VectorMath.hpp b/AirLib/include/common/VectorMath.hpp index b5e65c21bb..6901b7af2f 100644 --- a/AirLib/include/common/VectorMath.hpp +++ b/AirLib/include/common/VectorMath.hpp @@ -38,7 +38,6 @@ namespace airlib typedef Eigen::Matrix Matrix3x3d; typedef Eigen::Matrix Matrix3x3f; - // added by Suman typedef Eigen::Matrix Vector17f; typedef Eigen::Matrix Vector16f; typedef Eigen::Matrix Matrix17x17f; diff --git a/AirLib/include/sensors/SensorBase.hpp b/AirLib/include/sensors/SensorBase.hpp index 55bf8c43a7..f3a026519a 100644 --- a/AirLib/include/sensors/SensorBase.hpp +++ b/AirLib/include/sensors/SensorBase.hpp @@ -52,13 +52,11 @@ namespace airlib ground_truth_.environment = environment; } - // added by Suman virtual void update() override { is_new_ = false; } - // added by Suman const bool& checkIfNew() const { return is_new_; @@ -82,7 +80,6 @@ namespace airlib std::string name_ = ""; protected: - // added by Suman bool is_new_ = false; }; diff --git a/AirLib/include/sensors/barometer/BarometerSimple.hpp b/AirLib/include/sensors/barometer/BarometerSimple.hpp index 735bcca9fe..9bef833499 100644 --- a/AirLib/include/sensors/barometer/BarometerSimple.hpp +++ b/AirLib/include/sensors/barometer/BarometerSimple.hpp @@ -65,8 +65,7 @@ namespace airlib if (freq_limiter_.isWaitComplete()){ setOutput(delay_line_.getOutput()); - - // added by Suman, isNew flag is set to true if the sensor signal updates + is_new_ = true; } } diff --git a/AirLib/include/sensors/gps/GpsSimple.hpp b/AirLib/include/sensors/gps/GpsSimple.hpp index cb4e571c0d..d2852948a2 100644 --- a/AirLib/include/sensors/gps/GpsSimple.hpp +++ b/AirLib/include/sensors/gps/GpsSimple.hpp @@ -38,7 +38,6 @@ namespace airlib //*** Start: UpdatableState implementation ***// virtual void resetImplementation() override { - // added gauss_dist_pos.reset(); gauss_dist_vel.reset(); @@ -68,7 +67,6 @@ namespace airlib if (freq_limiter_.isWaitComplete()){ setOutput(delay_line_.getOutput()); - // added by Suman, isNew flag is set to true if the sensor signal updates is_new_ = true; } } @@ -86,22 +84,17 @@ namespace airlib //GNSS output.gnss.time_utc = static_cast(clock()->nowNanos() / 1.0E3); output.gnss.geo_point = ground_truth.environment->getState().geo_point; - - // // added by Suman - // output.gnss.geo_point.longitude = ground_truth.kinematics->pose.position[0]; - // output.gnss.geo_point.latitude = ground_truth.kinematics->pose.position[1]; - // output.gnss.geo_point.altitude = ground_truth.kinematics->pose.position[2]; - + + // add Gaussian white noise to the ground truth gps position outputs real_T gps_sigma_pos = 0.00001f; // 1/6378km rad output.gnss.geo_point.longitude += gauss_dist_pos.next()[0] * gps_sigma_pos; output.gnss.geo_point.latitude += gauss_dist_pos.next()[1] * gps_sigma_pos; output.gnss.geo_point.altitude += gauss_dist_pos.next()[2] * 3.0f; - // // - 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]*2.0f; output.gnss.velocity.y() += gauss_dist_vel.next()[1]*2.0f; @@ -128,7 +121,6 @@ namespace airlib FrequencyLimiter freq_limiter_; DelayLine delay_line_; - //added RandomVectorGaussianR gauss_dist_pos = RandomVectorGaussianR(0, 1); RandomVectorGaussianR gauss_dist_vel = RandomVectorGaussianR(0, 1); }; diff --git a/AirLib/include/sensors/imu/ImuSimple.hpp b/AirLib/include/sensors/imu/ImuSimple.hpp index 3d37872e9a..f18ce2049b 100644 --- a/AirLib/include/sensors/imu/ImuSimple.hpp +++ b/AirLib/include/sensors/imu/ImuSimple.hpp @@ -44,7 +44,6 @@ namespace airlib updateOutput(); - // added by Suman, isNew flag is set to true if the sensor signal updates is_new_ = true; } //*** End: UpdatableState implementation ***// diff --git a/AirLib/include/sensors/magnetometer/MagnetometerSimple.hpp b/AirLib/include/sensors/magnetometer/MagnetometerSimple.hpp index 70adad8c3d..568850abaf 100644 --- a/AirLib/include/sensors/magnetometer/MagnetometerSimple.hpp +++ b/AirLib/include/sensors/magnetometer/MagnetometerSimple.hpp @@ -62,7 +62,6 @@ namespace airlib if (freq_limiter_.isWaitComplete()){ setOutput(delay_line_.getOutput()); - // added by Suman, isNew flag is set to true if the sensor signal updates is_new_ = true; } } diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp index d45213f8a3..7002d3a1ed 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp @@ -56,8 +56,7 @@ namespace airlib // if the wait is complete and it is time to update EKF, update EKF if (freq_limiter_.isWaitComplete()) updateEKFInternal(); - - // updateEKFInternal(); + } // only to debug and verify estimates @@ -690,14 +689,6 @@ namespace airlib double geo[3]; board_->readGpsData(geo, vel); - // GeoPoint geopoint; - // geopoint.latitude = geo[0]; - // geopoint.longitude = geo[1]; - // geopoint.altitude = geo[2]; - // GeoPoint geo_home = environment_->getHomeGeoPoint(); - - // Vector3r measured = EarthUtils::GeodeticToNedFast(geopoint, geo_home); - GeoPoint geo_point; Vector3r ned_pos; geo_point.longitude = geo[0]; @@ -708,9 +699,6 @@ namespace airlib pos[0] = ned_pos[0]; pos[1] = ned_pos[1]; pos[2] = ned_pos[2]; - // pos[0] = geo[0]; - // pos[1] = geo[1]; - // pos[2] = geo[2]; #endif // check if the signal has all data that is valid, else return false @@ -740,6 +728,7 @@ namespace airlib // 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_(12) = altitude[0]; return true; @@ -759,6 +748,7 @@ namespace airlib // 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]; diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightBoard.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightBoard.hpp index 356f436dfc..adc7b50971 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightBoard.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightBoard.hpp @@ -150,7 +150,6 @@ namespace airlib return gps_->checkIfNew(); } - // added by Suman virtual void readImuData(real_T accel[3], real_T gyro[3]) const override { accel[0] = imu_->getOutput().linear_acceleration.x(); @@ -162,13 +161,11 @@ namespace airlib gyro[2] = imu_->getOutput().angular_velocity.z(); } - // added by Suman virtual void readBarometerData(real_T* altitude) const override { *altitude = barometer_->getOutput().altitude; } - // added by Suman virtual void readMagnetometerData(real_T mag[3]) const override { mag[0] = magnetometer_->getOutput().magnetic_field_body.x(); @@ -176,7 +173,6 @@ namespace airlib mag[2] = magnetometer_->getOutput().magnetic_field_body.z(); } - // added by Suman virtual void readGpsData(double geo[3], real_T vel[3]) const override { geo[0] = gps_->getOutput().gnss.geo_point.longitude; @@ -189,7 +185,6 @@ namespace airlib } private: - // added by Suman const SensorCollection& getSensors() { return vehicle_params_->getSensors(); @@ -222,7 +217,6 @@ namespace airlib } } - // added by Suman const SensorBase* findSensorByName(const std::string& sensor_name, const SensorBase::SensorType type) { const SensorBase* sensor = nullptr; @@ -265,7 +259,6 @@ namespace airlib const simple_flight::Params* params_; const Kinematics::State* kinematics_; - // added by Suman const MultiRotorParams* vehicle_params_; const ImuBase* imu_ = nullptr; const BarometerBase* barometer_ = nullptr; diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightApi.hpp index 61d419205a..8013b582cb 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightApi.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightApi.hpp @@ -42,7 +42,6 @@ namespace airlib board_.reset(new AirSimSimpleFlightBoard(¶ms_, vehicle_params_)); comm_link_.reset(new AirSimSimpleFlightCommLink()); - // added by Suman ekf_.reset(new AirSimSimpleEkf(board_.get(), comm_link_.get(), vehicle_setting->ekf_setting.get())); estimator_.reset(new AirSimSimpleFlightEstimator(ekf_.get())); @@ -144,7 +143,7 @@ namespace airlib } return true; - } + } protected: virtual Kinematics::State getKinematicsEstimated() const override 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 acc3d89e86..56eb06e856 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,7 +9,6 @@ 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 - // added by Suman virtual bool checkImuIfNew() const = 0; virtual bool checkBarometerIfNew() const = 0; virtual bool checkMagnetometerIfNew() const = 0; diff --git a/AirLibUnitTests/SimpleFlightTest.hpp b/AirLibUnitTests/SimpleFlightTest.hpp index 56d50d820f..c361efceba 100644 --- a/AirLibUnitTests/SimpleFlightTest.hpp +++ b/AirLibUnitTests/SimpleFlightTest.hpp @@ -41,7 +41,7 @@ namespace airlib SensorFactory sensor_factory; - // added by Suman, from https://github.com/microsoft/AirSim/pull/2558/commits/9c4e59d1a2b371ebc60cdc18f93b06cbe3e9d305 + // added from https://github.com/microsoft/AirSim/pull/2558/commits/9c4e59d1a2b371ebc60cdc18f93b06cbe3e9d305 // loads settings from settings.json or Default setting SettingsLoader settings_loader; From 5981512d89465073a47a9e69823d101364b69921 Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Wed, 12 Jan 2022 15:56:20 +0100 Subject: [PATCH 49/87] fixes --- .../firmwares/simple_flight/AirSimSimpleEkf.hpp | 2 +- .../firmwares/simple_flight/AirSimSimpleEkfParams.hpp | 3 --- AirLibUnitTests/SimpleFlightTest.hpp | 10 ++++++---- 3 files changed, 7 insertions(+), 8 deletions(-) diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp index 7002d3a1ed..3ca56ac8e4 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp @@ -580,7 +580,7 @@ namespace airlib 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(); + P_corrected = term*P*term.transpose() + kalman_gain*R_pseudo_*kalman_gain.transpose(); // write the states to the global variable for (int i=0; iarmDisarm(true); //checkStatusMsg(api.get(), &myfile); - clock->sleep_for(6.0f); + clock->sleep_for(10.0f); // take off api->takeoff(50); @@ -146,12 +146,14 @@ namespace airlib // pos = api->getMultirotorState().getPosition(); //checkStatusMsg(api.get(), &myfile); - clock->sleep_for(6.0f); + clock->sleep_for(10.0f); + api->moveByAngleRatesZ(0.0f, 0.0f, 4.0f*M_PI/180, -1.53509f, 4.0f); + clock->sleep_for(10.0f); // // // fly towards a waypoint api->moveToPosition(10, 0, -2, 0.5, 1E3, DrivetrainType::MaxDegreeOfFreedom, YawMode(true, 0), -1, 0); - clock->sleep_for(18.0f); + clock->sleep_for(10.0f); api->moveToPosition(10, 10, -2, 0.5, 1E3, DrivetrainType::MaxDegreeOfFreedom, YawMode(true, 0), -1, 0); - clock->sleep_for(18.0f); + clock->sleep_for(10.0f); api->moveToPosition(10, 10, -20, 0.5, 1E3, DrivetrainType::MaxDegreeOfFreedom, YawMode(true, 0), -1, 0); pos = api->getMultirotorState().getPosition(); std::cout << "waypoint position: " << pos << std::endl; From 20dd1c1324cab993a1013cb0c53f754e154fcfd3 Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Mon, 17 Jan 2022 09:34:26 +0100 Subject: [PATCH 50/87] fix euler angle covariance --- .../simple_flight/AirSimSimpleEkf.hpp | 40 +++++++++---------- 1 file changed, 20 insertions(+), 20 deletions(-) diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp index 3ca56ac8e4..633ca0c351 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp @@ -586,7 +586,7 @@ namespace airlib for (int i=0; i Date: Wed, 19 Jan 2022 12:48:18 +0100 Subject: [PATCH 51/87] refactor ekf params --- .../firmwares/simple_flight/AirSimSimpleEkfParams.hpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfParams.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfParams.hpp index ef003402cf..91f89fc16b 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfParams.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfParams.hpp @@ -109,6 +109,12 @@ namespace airlib } } + void initializeParameters(const AirSimSettings::EkfSetting* settings) + { + initializeFromSettigns(settings); + refreshAndUnitConversion(); + } + void refreshAndUnitConversion() { gyro.std_error = gyro.std_error * M_PI/180; // deg/s to rad/s @@ -119,7 +125,7 @@ namespace airlib initial_states.attitude.z()); } - void initializeParameters(const AirSimSettings::EkfSetting* settings) + void initializeFromSettigns(const AirSimSettings::EkfSetting* settings) { if (settings == nullptr){ return; @@ -246,7 +252,6 @@ namespace airlib readVector3r(initial_states_child, gyro_bias_str, initial_states.gyro_bias); readRealT(initial_states_child, "BaroBias", initial_states.baro_bias); } - refreshAndUnitConversion(); } }; From 3c1a07253fe0a45e4f487df146be6c4a12cf0c6e Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Wed, 19 Jan 2022 13:03:07 +0100 Subject: [PATCH 52/87] cleanup --- .gitignore | 5 ----- .../simple_flight/firmware/interfaces/IStateEstimator.hpp | 4 ---- 2 files changed, 9 deletions(-) diff --git a/.gitignore b/.gitignore index 64465a19c5..c4d20ea350 100644 --- a/.gitignore +++ b/.gitignore @@ -397,14 +397,9 @@ build_docs/ # api docs PythonClient/docs/_build -<<<<<<< HEAD # custom: ignore /AirLibUnitTests/data /AirLibUnitTests/figures /AirLibUnitTests/log.txt /AirLibUnitTests/other.txt -======= -# Docker -/docker/Blocks/ ->>>>>>> 0becf9148d88c19c3ffd69359ebe1f9c3ae43826 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 c68a1a613a..3bc7a4eb19 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 @@ -21,7 +21,6 @@ class IStateEstimator virtual Axis3r transformToBodyFrame(const Axis3r& world_frame_val) const = 0; -<<<<<<< HEAD virtual simple_flight::SensorMeasurements getTrueMeasurements() const = 0; virtual simple_flight::SensorMeasurements getEkfMeasurements() const = 0; virtual simple_flight::Axis3r getEkfPosition() const = 0; @@ -50,8 +49,5 @@ class IStateEstimator virtual simple_flight::Axis3r getTrueLinearVelocity() const = 0; virtual simple_flight::Axis4r getTrueOrientation() const = 0; virtual simple_flight::KinematicsState getTrueKinematicsEstimated() const = 0; -======= - virtual ~IStateEstimator() = default; ->>>>>>> 0becf9148d88c19c3ffd69359ebe1f9c3ae43826 }; } From 5d4a3550959c75dcbfb9419dd583e52aba599766 Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Wed, 19 Jan 2022 13:04:56 +0100 Subject: [PATCH 53/87] cleanup --- .../simple_flight/firmware/interfaces/IBoardSensors.hpp | 4 ---- 1 file changed, 4 deletions(-) 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 0b1b98a5cd..f882b753cb 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,7 +9,6 @@ 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 -<<<<<<< HEAD virtual bool checkImuIfNew() const = 0; virtual bool checkBarometerIfNew() const = 0; virtual bool checkMagnetometerIfNew() const = 0; @@ -18,8 +17,5 @@ class IBoardSensors 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; ->>>>>>> 0becf9148d88c19c3ffd69359ebe1f9c3ae43826 }; } From 1aa21f2a02808f49a83989abfe0283ecb0a388c7 Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Wed, 26 Jan 2022 16:21:20 +0100 Subject: [PATCH 54/87] fix a typo in ekfparams --- .../firmwares/simple_flight/AirSimSimpleEkfParams.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfParams.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfParams.hpp index 91f89fc16b..c46a94d5ce 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfParams.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfParams.hpp @@ -111,7 +111,7 @@ namespace airlib void initializeParameters(const AirSimSettings::EkfSetting* settings) { - initializeFromSettigns(settings); + initializeFromSettings(settings); refreshAndUnitConversion(); } @@ -125,7 +125,7 @@ namespace airlib initial_states.attitude.z()); } - void initializeFromSettigns(const AirSimSettings::EkfSetting* settings) + void initializeFromSettings(const AirSimSettings::EkfSetting* settings) { if (settings == nullptr){ return; From b40a3eb124adaca503fdd4b962136d01d15f0ab8 Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Mon, 14 Feb 2022 16:10:44 +0100 Subject: [PATCH 55/87] update gitignore --- .gitignore | 3 +++ 1 file changed, 3 insertions(+) diff --git a/.gitignore b/.gitignore index c4d20ea350..e13bd650dd 100644 --- a/.gitignore +++ b/.gitignore @@ -402,4 +402,7 @@ PythonClient/docs/_build /AirLibUnitTests/data /AirLibUnitTests/figures /AirLibUnitTests/log.txt +/AirLibUnitTests/data.csv +/AirLibUnitTests/figure.pdf /AirLibUnitTests/other.txt +build_AirSimEkfPod/ From 44135b5aa68d7e90034525e904c1c2846f3400d6 Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Mon, 14 Feb 2022 16:15:31 +0100 Subject: [PATCH 56/87] remove pure velocity zero condition before multirotor takeoff --- .../simple_flight/AirSimSimpleFlightEstimator.hpp | 10 +++++----- .../src/vehicles/multirotor/api/MultirotorApiBase.cpp | 10 +++++----- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightEstimator.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightEstimator.hpp index 9ba35654ba..42450e6f34 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightEstimator.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightEstimator.hpp @@ -10,7 +10,7 @@ #include "physics/Environment.hpp" #include "common/Common.hpp" -#define EKF_ESTIMATED_DIRECTIVE 0 +#define EKF_ESTIMATED_DIRECTIVE 1 namespace msr { @@ -249,10 +249,10 @@ namespace airlib simple_flight::Axis4r orientation; auto ekf_states = ekf_->getEkfStates(); - orientation.x() = ekf_states(6); - orientation.y() = ekf_states(7); - orientation.z() = ekf_states(8); - orientation.val4()= ekf_states(9); + orientation.val4()= ekf_states(6); + orientation.x() = ekf_states(7); + orientation.y() = ekf_states(8); + orientation.z() = ekf_states(9); return orientation; } 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(), From 0c8df519ef211cebb7e2d5e070ddd25526f0496f Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Mon, 14 Feb 2022 16:16:01 +0100 Subject: [PATCH 57/87] running changes in peripherals for ekf --- AirLibUnitTests/SimpleFlightTest.hpp | 4 +- AirLibUnitTests/plot.py | 62 ++++++++++++++++++---------- 2 files changed, 42 insertions(+), 24 deletions(-) diff --git a/AirLibUnitTests/SimpleFlightTest.hpp b/AirLibUnitTests/SimpleFlightTest.hpp index 99c9667c04..96b04473e8 100644 --- a/AirLibUnitTests/SimpleFlightTest.hpp +++ b/AirLibUnitTests/SimpleFlightTest.hpp @@ -120,7 +120,7 @@ namespace airlib api->armDisarm(true); //checkStatusMsg(api.get(), &myfile); - clock->sleep_for(10.0f); + clock->sleep_for(30.0f); // take off api->takeoff(50); @@ -166,7 +166,7 @@ namespace airlib // api->moveToPosition(-100, 100, -50, 15, 1E3, DrivetrainType::MaxDegreeOfFreedom, YawMode(true, 0), -1, 0); // pos = api->getMultirotorState().getPosition(); // std::cout << "waypoint position: " << pos << std::endl; - clock->sleep_for(6.0f); + clock->sleep_for(20.0f); // // land // //api->land(10); diff --git a/AirLibUnitTests/plot.py b/AirLibUnitTests/plot.py index 209c4b4e22..d242f8772d 100644 --- a/AirLibUnitTests/plot.py +++ b/AirLibUnitTests/plot.py @@ -381,39 +381,39 @@ def plotResults(data): # print(k) # print(k[2,1]) - # theta_error = np.zeros(len(timestamp)) - # phi_error = np.zeros(len(timestamp)) - # psi_error = np.zeros(len(timestamp)) + theta_error = np.zeros(len(timestamp)) + phi_error = np.zeros(len(timestamp)) + psi_error = np.zeros(len(timestamp)) # # print(psi_error) - # for i in range(0, len(timestamp)): - # ground = M_OB(true_q0[i], - # true_q1[i], - # true_q2[i], - # true_q3[i]) - # estimated = M_OB(estimated_q0[i], - # estimated_q1[i], - # estimated_q2[i], - # estimated_q3[i]) - # # print(ground_error) + for i in range(0, len(timestamp)): + ground = M_OB(true_q0[i], + true_q1[i], + true_q2[i], + true_q3[i]) + estimated = M_OB(estimated_q0[i], + estimated_q1[i], + estimated_q2[i], + estimated_q3[i]) + # print(ground_error) - # error_direction_cosine = ground*estimated.transpose() - # # print(error_direction_cosine[2, 0]) + error_direction_cosine = ground*estimated.transpose() + # print(error_direction_cosine[2, 0]) - # # euler = [atan2(mat(3,2),mat(3,3)); - # # -asin(mat(3,1)); - # # atan2(mat(2,1),mat(1,1))]; + # euler = [atan2(mat(3,2),mat(3,3)); + # -asin(mat(3,1)); + # atan2(mat(2,1),mat(1,1))]; - # phi_error[i] = np.arctan2(error_direction_cosine[2, 1],error_direction_cosine[2, 2])*180/np.pi - # theta_error[i] = -np.arcsin(error_direction_cosine[2, 0])*180/np.pi - # psi_error[i] = np.arctan2(error_direction_cosine[1, 0],error_direction_cosine[0, 0])*180/np.pi + phi_error[i] = np.arctan2(error_direction_cosine[2, 1],error_direction_cosine[2, 2])*180/np.pi + theta_error[i] = -np.arcsin(error_direction_cosine[2, 0])*180/np.pi + psi_error[i] = np.arctan2(error_direction_cosine[1, 0],error_direction_cosine[0, 0])*180/np.pi # apply style plt.style.use('./style.mplstyle') # define the plot - fig, ax = plt.subplots(36, 1) + fig, ax = plt.subplots(38, 1) # begin plotting i=0 @@ -589,6 +589,24 @@ def plotResults(data): # ax[i].set_ylim([-0.0001, 0.0001]) ax[i].grid(True) + i+=1 + ax[i].plot(timestamp, theta_error, linestyle='solid', color='C1', label="pitch") + ax[i].legend() + ax[i].plot(timestamp, phi_error, linestyle='solid', color='C2', label="roll") + ax[i].legend() + ax[i].set_ylabel('angles error dir cos(deg)') + # ax[i].set_xlim([5, 6]) + # ax[i].set_ylim([-0.0001, 0.0001]) + ax[i].grid(True) + + i+=1 + ax[i].plot(timestamp, psi_error, linestyle='dotted', color='C5', label="yaw") + ax[i].legend() + ax[i].set_ylabel('angles error dir cos(deg)') + # ax[i].set_xlim([5, 6]) + # ax[i].set_ylim([-0.0001, 0.0001]) + ax[i].grid(True) + i+=1 ax[i].plot(timestamp, err_estimated_x, linestyle='solid', color='C1', label="x") ax[i].legend() From 220337f3c21e3b9d36869c33f4c3bd535f77e49c Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Tue, 15 Feb 2022 13:57:49 +0100 Subject: [PATCH 58/87] add runtime selection if ekf estimates are used via state estimator --- .gitignore | 2 +- .../AirSimSimpleFlightEstimator.hpp | 78 +++++++++---------- 2 files changed, 40 insertions(+), 40 deletions(-) diff --git a/.gitignore b/.gitignore index e13bd650dd..a3c5a73484 100644 --- a/.gitignore +++ b/.gitignore @@ -403,6 +403,6 @@ PythonClient/docs/_build /AirLibUnitTests/figures /AirLibUnitTests/log.txt /AirLibUnitTests/data.csv -/AirLibUnitTests/figure.pdf +/AirLibUnitTests/figure*.pdf /AirLibUnitTests/other.txt build_AirSimEkfPod/ diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightEstimator.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightEstimator.hpp index 42450e6f34..54a00119f5 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightEstimator.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightEstimator.hpp @@ -10,7 +10,6 @@ #include "physics/Environment.hpp" #include "common/Common.hpp" -#define EKF_ESTIMATED_DIRECTIVE 1 namespace msr { @@ -21,7 +20,7 @@ namespace airlib { public: AirSimSimpleFlightEstimator(simple_flight::IEkf* ekf) - : ekf_(ekf) + : ekf_(ekf), ekf_enabled_(ekf_->checkEkfEnabled()) { } @@ -37,63 +36,63 @@ namespace airlib virtual simple_flight::Axis3r getAngles() const override { -#if EKF_ESTIMATED_DIRECTIVE == 1 - return getEkfAngles(); -#else - return getTrueAngles(); -#endif + if (ekf_enabled_) { + return getEkfAngles(); + } else { + return getTrueAngles(); + } } virtual simple_flight::Axis3r getAngularVelocity() const override { -#if EKF_ESTIMATED_DIRECTIVE == 1 - auto ekf_measurements = getEkfMeasurements(); - return AirSimSimpleFlightCommon::toAxis3r(VectorMath::transformToWorldFrame(AirSimSimpleFlightCommon::toVector3r(ekf_measurements.gyro), - AirSimSimpleFlightCommon::toQuaternion(getOrientation()), - true)); -#else - return getTrueAngularVelocity(); -#endif + 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_ESTIMATED_DIRECTIVE == 1 - return getEkfPosition(); -#else - return getTruePosition(); -#endif + if (ekf_enabled_) { + return getEkfPosition(); + } else { + return getTruePosition(); + } } virtual simple_flight::Axis3r transformToBodyFrame(const simple_flight::Axis3r& world_frame_val) const override { -#if EKF_ESTIMATED_DIRECTIVE == 1 - 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); -#endif + 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_ESTIMATED_DIRECTIVE == 1 - return getEkfLinearVelocity(); -#else - return getTrueLinearVelocity(); -#endif + if (ekf_enabled_) { + return getEkfLinearVelocity(); + } else { + return getTrueLinearVelocity(); + } } virtual simple_flight::Axis4r getOrientation() const override { -#if EKF_ESTIMATED_DIRECTIVE == 1 - return getEkfOrientation(); -#else - return getTrueOrientation(); -#endif + if (ekf_enabled_) { + return getEkfOrientation(); + } else { + return getTrueOrientation(); + } } virtual simple_flight::GeoPoint getGeoPoint() const override // TODO return this from measurement @@ -433,6 +432,7 @@ namespace airlib const Kinematics::State* kinematics_; const Environment* environment_; const simple_flight::IEkf* ekf_; + bool ekf_enabled_; }; } } //namespace From 76e46ace7c9778dfc06269c0cb1f39ba40e3adbf Mon Sep 17 00:00:00 2001 From: subedisuman Date: Wed, 16 Feb 2022 10:44:46 +0100 Subject: [PATCH 59/87] cleanup simple_flight state estimator (cov -> var), implement public bool to check if eekf is enabled --- .../AirSimSimpleFlightEstimator.hpp | 101 ++++++++++-------- .../simple_flight/firmware/OffboardApi.hpp | 50 +++++---- .../firmware/interfaces/IStateEstimator.hpp | 22 ++-- 3 files changed, 99 insertions(+), 74 deletions(-) diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightEstimator.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightEstimator.hpp index 54a00119f5..a000b4b65a 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightEstimator.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightEstimator.hpp @@ -20,9 +20,9 @@ namespace airlib { public: AirSimSimpleFlightEstimator(simple_flight::IEkf* ekf) - : ekf_(ekf), ekf_enabled_(ekf_->checkEkfEnabled()) + : ekf_(ekf) { - + ekf_enabled_ = ekf_->checkEkfEnabled(); } virtual ~AirSimSimpleFlightEstimator(){} @@ -302,81 +302,95 @@ namespace airlib return state; } - virtual simple_flight::Axis3r getEkfPositionCovariance() const override + virtual simple_flight::Axis3r getEkfPositionVariance() const override { - simple_flight::Axis3r position_cov; + simple_flight::Axis3r position_var; auto ekf_covariance = ekf_->getEkfCovariance(); - position_cov.x() = ekf_covariance(0, 0); - position_cov.y() = ekf_covariance(1, 1); - position_cov.z() = ekf_covariance(2, 2); + position_var.x() = ekf_covariance(0, 0); + position_var.y() = ekf_covariance(1, 1); + position_var.z() = ekf_covariance(2, 2); - return position_cov; + return position_var; } - virtual simple_flight::Axis3r getEkfLinearVelocityCovariance() const override + virtual simple_flight::Axis3r getEkfLinearVelocityVariance() const override { - simple_flight::Axis3r velocity_cov; + simple_flight::Axis3r velocity_var; auto ekf_covariance = ekf_->getEkfCovariance(); - velocity_cov.x() = ekf_covariance(3, 3); - velocity_cov.y() = ekf_covariance(4, 4); - velocity_cov.z() = ekf_covariance(5, 5); + velocity_var.x() = ekf_covariance(3, 3); + velocity_var.y() = ekf_covariance(4, 4); + velocity_var.z() = ekf_covariance(5, 5); - return velocity_cov; + return velocity_var; } - virtual simple_flight::Axis4r getEkfOrientationCovariance() const override + virtual simple_flight::Axis4r getEkfOrientationVariance() const override { - simple_flight::Axis4r orientation_cov; + simple_flight::Axis4r orientation_var; auto ekf_covariance = ekf_->getEkfCovariance(); - orientation_cov.val4()= ekf_covariance(6, 6); - orientation_cov.x() = ekf_covariance(7, 7); - orientation_cov.y() = ekf_covariance(8, 8); - orientation_cov.z() = ekf_covariance(9, 9); + 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_cov; + return orientation_var; } - virtual simple_flight::Axis3r getEkfAnglesCovariance() const override + virtual simple_flight::Axis3r getEkfAnglesVariance() const override { - simple_flight::Axis3r angles_cov; + simple_flight::Axis3r angles_var; auto ekf_angles_covariance = ekf_->getEkfEulerAnglesCovariance(); - angles_cov.x() = ekf_angles_covariance(0, 0); - angles_cov.y() = ekf_angles_covariance(1, 1); - angles_cov.z() = ekf_angles_covariance(2, 2); + 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_cov; + return angles_var; } - virtual simple_flight::Axis3r getEkfImuBiasCovariance() const override + virtual simple_flight::Axis3r getEkfAccelBiasVariance() const override { - simple_flight::Axis3r imu_bias_cov; + simple_flight::Axis3r accel_bias_var; auto ekf_covariance = ekf_->getEkfCovariance(); - imu_bias_cov.x() = ekf_covariance(10, 10); - imu_bias_cov.y() = ekf_covariance(11, 11); - imu_bias_cov.z() = ekf_covariance(12, 12); + 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 imu_bias_cov; + return accel_bias_var; } - virtual simple_flight::Axis3r getEkfGyroBiasCovariance() const override + virtual simple_flight::Axis3r getEkfGyroBiasVariance() const override { - simple_flight::Axis3r gyro_bias_cov; + simple_flight::Axis3r gyro_bias_var; auto ekf_covariance = ekf_->getEkfCovariance(); - gyro_bias_cov.x() = ekf_covariance(13, 13); - gyro_bias_cov.y() = ekf_covariance(14, 14); - gyro_bias_cov.z() = ekf_covariance(15, 15); + 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_cov; + return gyro_bias_var; } - virtual float getEkfBaroBiasCovariance() const override + virtual float getEkfBaroBiasVariance() const override { - float baro_bias_cov; + float gyro_bias_var; auto ekf_covariance = ekf_->getEkfCovariance(); - baro_bias_cov = ekf_covariance(16, 16); + gyro_bias_var = ekf_covariance(16, 16); // baro_bias_cov = ekf_covariance(15, 15); - return baro_bias_cov; + 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 @@ -432,7 +446,6 @@ namespace airlib const Kinematics::State* kinematics_; const Environment* environment_; const simple_flight::IEkf* ekf_; - bool ekf_enabled_; }; } } //namespace 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 654c45060f..7a3df46b77 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/OffboardApi.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/OffboardApi.hpp @@ -97,8 +97,12 @@ 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 { @@ -270,27 +274,27 @@ class OffboardApi : public IUpdatable << state_estimator_->getEkfKinematicsEstimated().sensor_bias.gyro.y() << '\t' << state_estimator_->getEkfKinematicsEstimated().sensor_bias.gyro.z() << '\t' << state_estimator_->getEkfKinematicsEstimated().sensor_bias.barometer << '\t' - // covariances - << state_estimator_->getEkfPositionCovariance().x() << '\t' - << state_estimator_->getEkfPositionCovariance().y() << '\t' - << state_estimator_->getEkfPositionCovariance().z() << '\t' - << state_estimator_->getEkfLinearVelocityCovariance().x() << '\t' - << state_estimator_->getEkfLinearVelocityCovariance().y() << '\t' - << state_estimator_->getEkfLinearVelocityCovariance().z() << '\t' - << state_estimator_->getEkfOrientationCovariance().val4() << '\t' - << state_estimator_->getEkfOrientationCovariance().x() << '\t' - << state_estimator_->getEkfOrientationCovariance().y() << '\t' - << state_estimator_->getEkfOrientationCovariance().z() << '\t' - << state_estimator_->getEkfAnglesCovariance().x() << '\t' - << state_estimator_->getEkfAnglesCovariance().y() << '\t' - << state_estimator_->getEkfAnglesCovariance().z() << '\t' - << state_estimator_->getEkfImuBiasCovariance().x() << '\t' - << state_estimator_->getEkfImuBiasCovariance().y() << '\t' - << state_estimator_->getEkfImuBiasCovariance().z() << '\t' - << state_estimator_->getEkfGyroBiasCovariance().x() << '\t' - << state_estimator_->getEkfGyroBiasCovariance().y() << '\t' - << state_estimator_->getEkfGyroBiasCovariance().z() << '\t' - << state_estimator_->getEkfBaroBiasCovariance() << '\t' + // variances + << state_estimator_->getEkfPositionVariance().x() << '\t' + << state_estimator_->getEkfPositionVariance().y() << '\t' + << state_estimator_->getEkfPositionVariance().z() << '\t' + << state_estimator_->getEkfLinearVelocityVariance().x() << '\t' + << state_estimator_->getEkfLinearVelocityVariance().y() << '\t' + << state_estimator_->getEkfLinearVelocityVariance().z() << '\t' + << state_estimator_->getEkfOrientationVariance().val4() << '\t' + << state_estimator_->getEkfOrientationVariance().x() << '\t' + << state_estimator_->getEkfOrientationVariance().y() << '\t' + << state_estimator_->getEkfOrientationVariance().z() << '\t' + << state_estimator_->getEkfAnglesVariance().x() << '\t' + << state_estimator_->getEkfAnglesVariance().y() << '\t' + << state_estimator_->getEkfAnglesVariance().z() << '\t' + << state_estimator_->getEkfAccelBiasVariance().x() << '\t' + << state_estimator_->getEkfAccelBiasVariance().y() << '\t' + << state_estimator_->getEkfAccelBiasVariance().z() << '\t' + << state_estimator_->getEkfGyroBiasVariance().x() << '\t' + << state_estimator_->getEkfGyroBiasVariance().y() << '\t' + << state_estimator_->getEkfGyroBiasVariance().z() << '\t' + << state_estimator_->getEkfBaroBiasVariance() << '\t' // quaternion norm << state_estimator_->getEkfOrientationNorm() << '\t' // off-diag quaternion covariance 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 3bc7a4eb19..a5fea3ea0e 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,10 @@ namespace simple_flight class IStateEstimator { public: + bool checkEkfEnabled() + { + return ekf_enabled_; + } virtual Axis3r getAngles() const = 0; virtual Axis3r getAngularVelocity() const = 0; virtual Axis3r getPosition() const = 0; @@ -30,13 +34,14 @@ class IStateEstimator virtual simple_flight::SensorBiases getEkfSensorBias() const = 0; virtual simple_flight::EkfKinematicsState getEkfKinematicsEstimated() const = 0; - virtual simple_flight::Axis3r getEkfPositionCovariance() const = 0; - virtual simple_flight::Axis3r getEkfLinearVelocityCovariance() const = 0; - virtual simple_flight::Axis4r getEkfOrientationCovariance() const = 0; - virtual simple_flight::Axis3r getEkfAnglesCovariance() const = 0; - virtual simple_flight::Axis3r getEkfImuBiasCovariance() const = 0; - virtual simple_flight::Axis3r getEkfGyroBiasCovariance() const = 0; - virtual float getEkfBaroBiasCovariance() 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; @@ -49,5 +54,8 @@ class IStateEstimator virtual simple_flight::Axis3r getTrueLinearVelocity() const = 0; virtual simple_flight::Axis4r getTrueOrientation() const = 0; virtual simple_flight::KinematicsState getTrueKinematicsEstimated() const = 0; + +public: + bool ekf_enabled_; }; } From 0642df1db188010ce5045db2a8a4520b5e185c9b Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Wed, 16 Feb 2022 18:23:04 +0100 Subject: [PATCH 60/87] add settings params for gps noise --- AirLib/include/sensors/gps/GpsSimple.hpp | 12 ++++++------ AirLib/include/sensors/gps/GpsSimpleParams.hpp | 15 +++++++++++++++ 2 files changed, 21 insertions(+), 6 deletions(-) diff --git a/AirLib/include/sensors/gps/GpsSimple.hpp b/AirLib/include/sensors/gps/GpsSimple.hpp index d2852948a2..38f550b3c7 100644 --- a/AirLib/include/sensors/gps/GpsSimple.hpp +++ b/AirLib/include/sensors/gps/GpsSimple.hpp @@ -87,18 +87,18 @@ namespace airlib // add Gaussian white noise to the ground truth gps position outputs real_T gps_sigma_pos = 0.00001f; // 1/6378km rad - output.gnss.geo_point.longitude += gauss_dist_pos.next()[0] * gps_sigma_pos; - output.gnss.geo_point.latitude += gauss_dist_pos.next()[1] * gps_sigma_pos; - output.gnss.geo_point.altitude += gauss_dist_pos.next()[2] * 3.0f; + 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]*2.0f; - output.gnss.velocity.y() += gauss_dist_vel.next()[1]*2.0f; - output.gnss.velocity.z() += gauss_dist_vel.next()[2]*2.0f; + 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; 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); From 4ac246ae829050356e4f81975a4167df03a1d0b3 Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Wed, 16 Feb 2022 18:24:27 +0100 Subject: [PATCH 61/87] fix baro altitude in ekf, introduce bool for sensor fusion --- .../simple_flight/AirSimSimpleEkf.hpp | 55 ++++++++++--------- .../simple_flight/AirSimSimpleEkfParams.hpp | 15 +++++ 2 files changed, 43 insertions(+), 27 deletions(-) diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp index 633ca0c351..43c306b012 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp @@ -14,11 +14,8 @@ #include "AirSimSimpleEkfParams.hpp" #include "common/GeodeticConverter.hpp" -#define EKF_GROUND_TRUTH_MEAS_DIRECTIVE 0 -#define EKF_BARO_DIRECTIVE 1 -#define EKF_MAGNETO_DIRECTIVE 1 -#define EKF_GPS_DIRECTIVE 1 -#define EKF_PSEUDOMEAS_DIRECTIVE 1 +#define AirSimSimpleEkf_GROUND_TRUTH_MEAS_DIRECTIVE 0 +#define AirSimSimpleEkf_PSEUDOMEAS_DIRECTIVE 1 namespace msr { @@ -174,6 +171,9 @@ namespace airlib 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]; @@ -222,19 +222,16 @@ namespace airlib // measurement update step void measurementUpdateStep() { -#if EKF_BARO_DIRECTIVE == 1 - barometerUpdate(); -#else -#endif -#if EKF_MAGNETO_DIRECTIVE == 1 - magnetometerUpdate(); -#else -#endif -#if EKF_GPS_DIRECTIVE == 1 - gpsUpdate(); -#else -#endif -#if EKF_PSEUDOMEAS_DIRECTIVE == 1 + if (params_.fuse_baro) { + barometerUpdate(); + } + if (params_.fuse_mag) { + magnetometerUpdate(); + } + if (params_.fuse_gps) { + gpsUpdate(); + } +#if AirSimSimpleEkf_PSEUDOMEAS_DIRECTIVE == 1 pseudoMeasurement(); #else #endif @@ -441,10 +438,10 @@ namespace airlib if(!board_->checkBarometerIfNew()) return; - real_T altitude[1]; + real_T ned_altitude[1]; // check if the barometer gives new measurement and it is valid - bool is_valid = getBarometerData(altitude); + bool is_valid = getBarometerData(ned_altitude); if(!is_valid) { @@ -469,7 +466,7 @@ namespace airlib // update states float x_corrected[simple_flight::NX]; for (int i=0; i>>>> only for verification, with ground truth measurements VectorMath::Vector3f linear_acceleration = kinematics_->accelerations.linear - environment_->getState().gravity; // acceleration is in world frame so transform to body frame @@ -679,7 +676,7 @@ namespace airlib real_T vel[3]) { -#if EKF_GROUND_TRUTH_MEAS_DIRECTIVE == 1 +#if AirSimSimpleEkf_GROUND_TRUTH_MEAS_DIRECTIVE == 1 // >>>>> only for verification, with ground truth measurements pos[0] = kinematics_->pose.position.x(); pos[1] = kinematics_->pose.position.y(); @@ -717,19 +714,22 @@ namespace airlib } // reads barometer data - bool getBarometerData(real_T* altitude) + bool getBarometerData(real_T* ned_altitude) { -#if EKF_GROUND_TRUTH_MEAS_DIRECTIVE == 1 +#if AirSimSimpleEkf_GROUND_TRUTH_MEAS_DIRECTIVE == 1 altitude[0] = -1.0f*kinematics_->pose.position.z(); #else + real_T altitude[1]; board_->readBarometerData(altitude); #endif // 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) = altitude[0]; + measurement_(12) = ned_altitude[0]; return true; @@ -739,7 +739,7 @@ namespace airlib bool getMagnetometerData(real_T mag[3]) { -#if EKF_GROUND_TRUTH_MEAS_DIRECTIVE == 1 +#if AirSimSimpleEkf_GROUND_TRUTH_MEAS_DIRECTIVE == 1 #else board_->readMagnetometerData(mag); @@ -782,6 +782,7 @@ namespace airlib const Environment* environment_; GeodeticConverter geodetic_converter_; float earth_mag_[3]; + float home_altitude_; LastTimes last_times_; ImuDataBuffer prev_imuData_; diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfParams.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfParams.hpp index c46a94d5ce..26014d047d 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfParams.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfParams.hpp @@ -14,6 +14,9 @@ namespace airlib struct AirSimSimpleEkfParams { bool ekf_enabled = false; + bool fuse_gps = false; + bool fuse_baro = false; + bool fuse_mag = false; struct Gyroscope { @@ -136,6 +139,18 @@ namespace airlib 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 = { From 8a8984d404fd771cc0a6d2b1cda374daade4038c Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Wed, 16 Feb 2022 18:26:03 +0100 Subject: [PATCH 62/87] fix virtual destructures warning --- .../simple_flight/AirSimSimpleFlightEstimator.hpp | 10 ++++++++-- .../firmware/interfaces/IBoardSensors.hpp | 2 ++ .../firmware/interfaces/IStateEstimator.hpp | 9 +++------ 3 files changed, 13 insertions(+), 8 deletions(-) diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightEstimator.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightEstimator.hpp index a000b4b65a..c9d8c41de9 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightEstimator.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightEstimator.hpp @@ -20,9 +20,9 @@ namespace airlib { public: AirSimSimpleFlightEstimator(simple_flight::IEkf* ekf) - : ekf_(ekf) + : ekf_(ekf), ekf_enabled_(ekf_->checkEkfEnabled()) { - ekf_enabled_ = ekf_->checkEkfEnabled(); + } virtual ~AirSimSimpleFlightEstimator(){} @@ -34,6 +34,11 @@ namespace airlib environment_ = environment; } + virtual bool checkEkfEnabled() const override + { + return ekf_enabled_; + } + virtual simple_flight::Axis3r getAngles() const override { if (ekf_enabled_) { @@ -446,6 +451,7 @@ namespace airlib 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/firmware/interfaces/IBoardSensors.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IBoardSensors.hpp index f882b753cb..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 @@ -17,5 +17,7 @@ class IBoardSensors 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/IStateEstimator.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IStateEstimator.hpp index a5fea3ea0e..7c4c64e7eb 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,10 +8,7 @@ namespace simple_flight class IStateEstimator { public: - bool checkEkfEnabled() - { - return ekf_enabled_; - } + virtual bool checkEkfEnabled() const = 0; virtual Axis3r getAngles() const = 0; virtual Axis3r getAngularVelocity() const = 0; virtual Axis3r getPosition() const = 0; @@ -55,7 +52,7 @@ class IStateEstimator virtual simple_flight::Axis4r getTrueOrientation() const = 0; virtual simple_flight::KinematicsState getTrueKinematicsEstimated() const = 0; -public: - bool ekf_enabled_; + virtual ~IStateEstimator() = default; + }; } From 6c081f00212c5575e8588ba9db4e948244208293 Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Wed, 16 Feb 2022 18:27:34 +0100 Subject: [PATCH 63/87] running change in simpleflighttest --- AirLibUnitTests/SimpleFlightTest.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/AirLibUnitTests/SimpleFlightTest.hpp b/AirLibUnitTests/SimpleFlightTest.hpp index 96b04473e8..857bf6d8db 100644 --- a/AirLibUnitTests/SimpleFlightTest.hpp +++ b/AirLibUnitTests/SimpleFlightTest.hpp @@ -70,8 +70,8 @@ namespace airlib Environment::State initial_environment; initial_environment.position = initial_kinematic_state.pose.position; - initial_environment.geo_point = GeoPoint(); - initial_environment.geo_point.altitude = 0.0f; // do not set it equal to kinematics z, this value goes into home geo point and acts as the ref for kinematics z + initial_environment.geo_point = GeoPoint(47.6415, -122.14, 121.173); // this becomes the home geo point with which ned to geo conversion takes place + // initial_environment.geo_point.altitude = 0.0f; // do not set it equal to kinematics z, this value goes into home geo point and acts as the ref for kinematics z environment.reset(new Environment(initial_environment)); // crete and initialize body and physics world From 6ce28d031cb8afbe79d50136cf7989394d3924f6 Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Thu, 17 Feb 2022 09:35:16 +0100 Subject: [PATCH 64/87] fix ekf settings not recognized by non-default vehicle --- AirLib/include/common/AirSimSettings.hpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/AirLib/include/common/AirSimSettings.hpp b/AirLib/include/common/AirSimSettings.hpp index 721770c2fa..59f0e13459 100644 --- a/AirLib/include/common/AirSimSettings.hpp +++ b/AirLib/include/common/AirSimSettings.hpp @@ -801,7 +801,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,6 +847,9 @@ 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; } @@ -865,7 +869,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) { @@ -906,7 +913,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); } } } From 9c60cce1c418faa3b028c367f82f5d09c1a1950b Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Fri, 18 Feb 2022 08:21:59 +0100 Subject: [PATCH 65/87] change ekf initial states setting to ekf initial state error --- .../simple_flight/AirSimSimpleEkf.hpp | 54 +++++++++---------- .../simple_flight/AirSimSimpleEkfParams.hpp | 28 +++++----- 2 files changed, 40 insertions(+), 42 deletions(-) diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp index 43c306b012..e1b2bc0442 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp @@ -56,7 +56,7 @@ namespace airlib } - // only to debug and verify estimates + // only in simulation void setGroundTruthKinematics(const Kinematics::State* kinematics, const Environment* environment) { kinematics_ = kinematics; @@ -71,11 +71,12 @@ namespace airlib // initialize filter void initializeFilter() { - assignEkfMatrics(); + assignEkfMeasurementMatrics(); + assignEkfStateMatrics(); resetGlobalVariables(); } - void assignEkfMatrics() + void assignEkfMeasurementMatrics() { Q_ = simple_flight::MatrixNWxNWf::Zero(); // imu @@ -115,26 +116,30 @@ namespace airlib // barometer R_pseudo_ = params_.pseudo_meas.quaternion_norm_R; - + } + + void assignEkfStateMatrics() + { // intialize the ekf states states_ = simple_flight::VectorNXf::Zero(); - states_(0) = params_.initial_states.position.x(); - states_(1) = params_.initial_states.position.y(); - states_(2) = params_.initial_states.position.z(); - states_(3) = params_.initial_states.linear_velocity.x(); - states_(4) = params_.initial_states.linear_velocity.y(); - states_(5) = params_.initial_states.linear_velocity.z(); - states_(6) = params_.initial_states.quaternion.w(); - states_(7) = params_.initial_states.quaternion.x(); - states_(8) = params_.initial_states.quaternion.y(); - states_(9) = params_.initial_states.quaternion.z(); - states_(10) = params_.initial_states.accel_bias.x(); - states_(11) = params_.initial_states.accel_bias.y(); - states_(12) = params_.initial_states.accel_bias.z(); - states_(13) = params_.initial_states.gyro_bias.x(); - states_(14) = params_.initial_states.gyro_bias.y(); - states_(15) = params_.initial_states.gyro_bias.z(); - states_(16) = params_.initial_states.baro_bias; + + 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) = 0.0f - params_.initial_states_err.accel_bias.x(); + states_(11) = 0.0f - params_.initial_states_err.accel_bias.y(); + states_(12) = 0.0f - params_.initial_states_err.accel_bias.z(); + states_(13) = 0.0f - params_.initial_states_err.gyro_bias.x(); + states_(14) = 0.0f - params_.initial_states_err.gyro_bias.y(); + states_(15) = 0.0f - params_.initial_states_err.gyro_bias.z(); + states_(16) = 0.0f - params_.initial_states_err.baro_bias; // intitialize the ekf covariances error_covariance_ = simple_flight::MatrixNXxNXf::Zero(); @@ -155,7 +160,6 @@ namespace airlib error_covariance_(14,14) = pow(params_.initial_states_std_err.gyro_bias.y(), 2); error_covariance_(15,15) = pow(params_.initial_states_std_err.gyro_bias.z(), 2); error_covariance_(16,16) = pow(params_.initial_states_std_err.baro_bias, 2); - } void resetGlobalVariables() @@ -429,7 +433,6 @@ namespace airlib states_(i) = x_corrected[i]; } error_covariance_ = P_corrected; - } // barometer update @@ -537,7 +540,6 @@ namespace airlib states_(i) = x_corrected[i]; } error_covariance_ = P_corrected; - } void pseudoMeasurement() @@ -668,7 +670,6 @@ namespace airlib measurement_(5) = gyro[2]; return true; - } // reads GPS data @@ -710,7 +711,6 @@ namespace airlib measurement_(11) = vel[2]; return true; - } // reads barometer data @@ -732,7 +732,6 @@ namespace airlib measurement_(12) = ned_altitude[0]; return true; - } // reads magnetometer data @@ -754,7 +753,6 @@ namespace airlib measurement_(15) = mag[2]; return true; - } private: diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfParams.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfParams.hpp index 26014d047d..86c0fa487e 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfParams.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfParams.hpp @@ -60,7 +60,7 @@ namespace airlib Quaternionr quaternion = Quaternionr(0.03f, 0.03f, 0.03f, 0.03f); } initial_states_std_err; - struct InitialStates + struct InitialStatesErr { Vector3r position = Vector3r(0.0f, 0.0f, 0.0f); Vector3r linear_velocity = Vector3r(0.0f, 0.0f, 0.0f); @@ -69,7 +69,7 @@ namespace airlib Vector3r gyro_bias = Vector3r(0.0f, 0.0f, 0.0f); real_T baro_bias = 0.0f; Quaternionr quaternion; - } initial_states; + } initial_states_err; void readVector3r(const Settings& json_child, const std::array& json_str, Vector3r& vector) { @@ -121,11 +121,11 @@ namespace airlib void refreshAndUnitConversion() { gyro.std_error = gyro.std_error * M_PI/180; // deg/s to rad/s - initial_states.attitude = initial_states.attitude * M_PI/180; //deg to rad + initial_states_err.attitude = initial_states_err.attitude * M_PI/180; //deg to rad - initial_states.quaternion = VectorMath::toQuaternion(initial_states.attitude.y(), - initial_states.attitude.x(), - initial_states.attitude.z()); + 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) @@ -233,39 +233,39 @@ namespace airlib 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_child; - if (json.getChild("InitialStates", initial_states_child)){ + Settings initial_states_err_child; + if (json.getChild("InitialStatesErr", initial_states_err_child)){ std::array pos_str = { "PositionX", "PositionY", "PositionZ" }; - readVector3r(initial_states_child, pos_str, initial_states.position); + readVector3r(initial_states_err_child, pos_str, initial_states_err.position); std::array lin_vel_str = { "LinearVelocityX", "LinearVelocityY", "LinearVelocityZ" }; - readVector3r(initial_states_child, lin_vel_str, initial_states.linear_velocity); + readVector3r(initial_states_err_child, lin_vel_str, initial_states_err.linear_velocity); std::array attitude_str = { "AttitideRoll", "AttitidePitch", "AttitideYaw" }; - readVector3r(initial_states_child, attitude_str, initial_states.attitude); + readVector3r(initial_states_err_child, attitude_str, initial_states_err.attitude); std::array accel_bias_str = { "AccelBiasX", "AccelBiasY", "AccelBiasZ" }; - readVector3r(initial_states_child, accel_bias_str, initial_states.accel_bias); + readVector3r(initial_states_err_child, accel_bias_str, initial_states_err.accel_bias); std::array gyro_bias_str = { "GyroBiasX", "GyroBiasY", "GyroBiasZ" }; - readVector3r(initial_states_child, gyro_bias_str, initial_states.gyro_bias); - readRealT(initial_states_child, "BaroBias", initial_states.baro_bias); + readVector3r(initial_states_err_child, gyro_bias_str, initial_states_err.gyro_bias); + readRealT(initial_states_err_child, "BaroBias", initial_states_err.baro_bias); } } }; From b9ba06af0c54857c2fee3724ba607d1708c79752 Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Fri, 18 Feb 2022 09:32:45 +0100 Subject: [PATCH 66/87] change sensor bias model to negative --- .../simple_flight/AirSimSimpleEkfModel.hpp | 160 +++++++++--------- 1 file changed, 80 insertions(+), 80 deletions(-) diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfModel.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfModel.hpp index b4a27cea73..a49c9e629f 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfModel.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfModel.hpp @@ -51,15 +51,15 @@ namespace airlib 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) + 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; } @@ -80,10 +80,10 @@ namespace airlib float b_omega_z = gyro_biases[2]; // 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)); - attitude_dot[1] = 0.5f * ( q0*(omega_x + b_omega_x) - q3*(omega_y + b_omega_y) + q2*(omega_z + b_omega_z)); - attitude_dot[2] = 0.5f * ( q3*(omega_x + b_omega_x) + q0*(omega_y + b_omega_y) - q1*(omega_z + b_omega_z)); - attitude_dot[3] = 0.5f * (-q2*(omega_x + b_omega_x) + q1*(omega_y + b_omega_y) + q0*(omega_z + b_omega_z)); + attitude_dot[0] = 0.5f * (-q1*(omega_x - b_omega_x) - q2*(omega_y - b_omega_y) - q3*(omega_z - b_omega_z)); + attitude_dot[1] = 0.5f * ( q0*(omega_x - b_omega_x) - q3*(omega_y - b_omega_y) + q2*(omega_z - b_omega_z)); + attitude_dot[2] = 0.5f * ( q3*(omega_x - b_omega_x) + q0*(omega_y - b_omega_y) - q1*(omega_z - b_omega_z)); + attitude_dot[3] = 0.5f * (-q2*(omega_x - b_omega_x) + q1*(omega_y - b_omega_y) + q0*(omega_z - b_omega_z)); } static void evaluateStateDot(float x_dot[simple_flight::NX], float x[simple_flight::NX], float u[simple_flight::NU]) @@ -198,57 +198,57 @@ namespace airlib // 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); + (*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); + (*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); + (*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); + (*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; + (*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, 7) = -0.5f*(omega_x + b_omega_x); - (*A)(6, 8) = -0.5f*(omega_y + b_omega_y); - (*A)(6, 9) = -0.5f*(omega_z + b_omega_z); - (*A)(7, 6) = 0.5f*(omega_x + b_omega_x); - (*A)(7, 8) = 0.5f*(omega_z + b_omega_z); - (*A)(7, 9) = -0.5f*(omega_y + b_omega_y); - (*A)(8, 6) = 0.5f*(omega_y + b_omega_y); - (*A)(8, 7) = -0.5f*(omega_z + b_omega_z); - (*A)(8, 9) = 0.5f*(omega_x + b_omega_x); - (*A)(9, 6) = 0.5f*(omega_z + b_omega_z); - (*A)(9, 7) = 0.5f*(omega_y + b_omega_y); - (*A)(9, 8) = -0.5f*(omega_x + b_omega_x); + (*A)(6, 7) = -0.5f*(omega_x - b_omega_x); + (*A)(6, 8) = -0.5f*(omega_y - b_omega_y); + (*A)(6, 9) = -0.5f*(omega_z - b_omega_z); + (*A)(7, 6) = 0.5f*(omega_x - b_omega_x); + (*A)(7, 8) = 0.5f*(omega_z - b_omega_z); + (*A)(7, 9) = -0.5f*(omega_y - b_omega_y); + (*A)(8, 6) = 0.5f*(omega_y - b_omega_y); + (*A)(8, 7) = -0.5f*(omega_z - b_omega_z); + (*A)(8, 9) = 0.5f*(omega_x - b_omega_x); + (*A)(9, 6) = 0.5f*(omega_z - b_omega_z); + (*A)(9, 7) = 0.5f*(omega_y - b_omega_y); + (*A)(9, 8) = -0.5f*(omega_x - b_omega_x); // 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; + (*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]) @@ -334,28 +334,28 @@ namespace airlib 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; + (*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; + (*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; @@ -376,7 +376,7 @@ namespace airlib // dh_baro_dx jacobian wrt the states vector *C_baro = simple_flight::Matrix1xNXf::Zero(); (*C_baro)(2) = -1.0f; - (*C_baro)(16) = 1.0f; + (*C_baro)(16) = -1.0f; } static void evaluateCGps(simple_flight::Matrix6xNXf* C_gps) From 39fbca72ebf4c725f19bb8e8519bbd1807f51d22 Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Fri, 18 Feb 2022 10:41:34 +0100 Subject: [PATCH 67/87] add turn on bias in imu from setting --- AirLib/include/sensors/imu/ImuSimpleParams.hpp | 6 ++++++ .../firmwares/simple_flight/AirSimSimpleEkf.hpp | 14 +++++++------- 2 files changed, 13 insertions(+), 7 deletions(-) diff --git a/AirLib/include/sensors/imu/ImuSimpleParams.hpp b/AirLib/include/sensors/imu/ImuSimpleParams.hpp index f2834e8e21..d869d8fd33 100644 --- a/AirLib/include/sensors/imu/ImuSimpleParams.hpp +++ b/AirLib/include/sensors/imu/ImuSimpleParams.hpp @@ -69,6 +69,12 @@ 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()); + gyro.turn_on_bias.x() = json.getFloat("GyroTurnOnBiasX", gyro.turn_on_bias.x()); + gyro.turn_on_bias.y() = json.getFloat("GyroTurnOnBiasY", gyro.turn_on_bias.y()); + gyro.turn_on_bias.z() = json.getFloat("GyroTurnOnBiasZ", gyro.turn_on_bias.z()); } }; } diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp index e1b2bc0442..774d9b46d0 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp @@ -133,13 +133,13 @@ namespace airlib 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) = 0.0f - params_.initial_states_err.accel_bias.x(); - states_(11) = 0.0f - params_.initial_states_err.accel_bias.y(); - states_(12) = 0.0f - params_.initial_states_err.accel_bias.z(); - states_(13) = 0.0f - params_.initial_states_err.gyro_bias.x(); - states_(14) = 0.0f - params_.initial_states_err.gyro_bias.y(); - states_(15) = 0.0f - params_.initial_states_err.gyro_bias.z(); - states_(16) = 0.0f - params_.initial_states_err.baro_bias; + 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(); From 60f563a469e5189a971da0772dd4f9e4da557a87 Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Fri, 18 Feb 2022 10:42:20 +0100 Subject: [PATCH 68/87] reset kinematics and environment before api in simpleflighttest --- AirLibUnitTests/SimpleFlightTest.hpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/AirLibUnitTests/SimpleFlightTest.hpp b/AirLibUnitTests/SimpleFlightTest.hpp index 857bf6d8db..daa3fc4c49 100644 --- a/AirLibUnitTests/SimpleFlightTest.hpp +++ b/AirLibUnitTests/SimpleFlightTest.hpp @@ -55,7 +55,7 @@ namespace airlib std::unique_ptr environment; Kinematics::State initial_kinematic_state = Kinematics::State::zero(); - initial_kinematic_state.pose = Pose(); + initial_kinematic_state.pose = Pose(Vector3r(1.0f, 2.0f, 3.0f), Quaternionr(1, 0, 0, 0)); // states_(6) = 0.9961946f; // q0 // states_(7) = 0.0f; // q1 // states_(8) = 0.08715574f; // q2 @@ -84,9 +84,11 @@ namespace airlib // added by Suman, to fix calling update before reset https://github.com/microsoft/AirSim/issues/2773#issuecomment-703888477 // TODO not sure if it should be here? see wrt to PawnSimApi, no side effects so far + // for the order of the reset see void MultirotorPawnSimApi::resetImplementation() api->setSimulatedGroundTruth(&kinematics.get()->getState(), environment.get()); + kinematics->reset(); // sets initial kinematics as current among other things + environment->reset(); api->reset(); - kinematics->reset(); // set the vehicle as grounded, otherwise can not take off, needs to to be done after physics world construction! vehicle.setGrounded(true); From 1410c7e3d8f89e95f70116a70fa502daaed5c41d Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Fri, 18 Feb 2022 12:13:30 +0100 Subject: [PATCH 69/87] correct unit of gyro bias in ekf and imu model --- .../include/sensors/imu/ImuSimpleParams.hpp | 16 +++++++++++--- .../simple_flight/AirSimSimpleEkfParams.hpp | 4 +++- AirLibUnitTests/plot.py | 22 +++++++++---------- 3 files changed, 27 insertions(+), 15 deletions(-) diff --git a/AirLib/include/sensors/imu/ImuSimpleParams.hpp b/AirLib/include/sensors/imu/ImuSimpleParams.hpp index d869d8fd33..1e218f1224 100644 --- a/AirLib/include/sensors/imu/ImuSimpleParams.hpp +++ b/AirLib/include/sensors/imu/ImuSimpleParams.hpp @@ -72,9 +72,19 @@ namespace airlib 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()); - gyro.turn_on_bias.x() = json.getFloat("GyroTurnOnBiasX", gyro.turn_on_bias.x()); - gyro.turn_on_bias.y() = json.getFloat("GyroTurnOnBiasY", gyro.turn_on_bias.y()); - gyro.turn_on_bias.z() = json.getFloat("GyroTurnOnBiasZ", gyro.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_PI/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_PI/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_PI/180; // deg/s to rad/s + } } }; } diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfParams.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfParams.hpp index 86c0fa487e..ed8f445eaa 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfParams.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfParams.hpp @@ -121,8 +121,10 @@ namespace airlib void refreshAndUnitConversion() { gyro.std_error = gyro.std_error * 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.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()); diff --git a/AirLibUnitTests/plot.py b/AirLibUnitTests/plot.py index d242f8772d..98a1ed2c9a 100644 --- a/AirLibUnitTests/plot.py +++ b/AirLibUnitTests/plot.py @@ -208,11 +208,11 @@ def plotResults(data): i+=1 bias_accel_z = data[i].to_numpy(float) * 1000 / 9.80665 i+=1 - bias_gyro_x = data[i].to_numpy(float) * 180/np.pi * 3600 + bias_gyro_x = data[i].to_numpy(float) * 180/np.pi i+=1 - bias_gyro_y = data[i].to_numpy(float) * 180/np.pi * 3600 + bias_gyro_y = data[i].to_numpy(float) * 180/np.pi i+=1 - bias_gyro_z = data[i].to_numpy(float) * 180/np.pi * 3600 + bias_gyro_z = data[i].to_numpy(float) * 180/np.pi i+=1 bias_baro = data[i].to_numpy(float) i+=1 @@ -258,11 +258,11 @@ def plotResults(data): i+=1 # variance of gyro bias - cov_b_omega_x = (data[i].to_numpy(float)) * (180/np.pi * 3600)* (180/np.pi * 3600) + cov_b_omega_x = (data[i].to_numpy(float)) * (180/np.pi)* (180/np.pi) i+=1 - cov_b_omega_y = (data[i].to_numpy(float)) * (180/np.pi * 3600)* (180/np.pi * 3600) + cov_b_omega_y = (data[i].to_numpy(float)) * (180/np.pi)* (180/np.pi) i+=1 - cov_b_omega_z = (data[i].to_numpy(float)) * (180/np.pi * 3600)* (180/np.pi * 3600) + cov_b_omega_z = (data[i].to_numpy(float)) * (180/np.pi)* (180/np.pi) i+=1 # variance baro @@ -717,7 +717,7 @@ def plotResults(data): bias_gyro_x-3*np.sqrt(cov_b_omega_x), bias_gyro_x+3*np.sqrt(cov_b_omega_x), alpha=0.3,linewidth=0, color='C5') - ax[i].set_ylabel('x gyro biases deg/hr') + ax[i].set_ylabel('x gyro biases deg/s') ax[i].grid(True) i+=1 @@ -727,7 +727,7 @@ def plotResults(data): bias_gyro_y-3*np.sqrt(cov_b_omega_y), bias_gyro_y+3*np.sqrt(cov_b_omega_y), alpha=0.3,linewidth=0, color='C5') - ax[i].set_ylabel('y gyro biases deg/hr') + ax[i].set_ylabel('y gyro biases deg/s') ax[i].grid(True) i+=1 @@ -737,7 +737,7 @@ def plotResults(data): bias_gyro_z-3*np.sqrt(cov_b_omega_z), bias_gyro_z+3*np.sqrt(cov_b_omega_z), alpha=0.3,linewidth=0, color='C5') - ax[i].set_ylabel('z gyro biases deg/hr') + ax[i].set_ylabel('z gyro biases deg/s') ax[i].grid(True) i+=1 @@ -747,7 +747,7 @@ def plotResults(data): bias_baro-3*np.sqrt(cov_b_baro), bias_baro+3*np.sqrt(cov_b_baro), alpha=0.3,linewidth=0, color='C5') - ax[i].set_ylabel('baro biases deg/hr') + ax[i].set_ylabel('baro bias m') ax[i].grid(True) i+=1 @@ -824,7 +824,7 @@ def plotResults(data): ax[i].plot(timestamp, np.sqrt(cov_b_omega_z), linestyle='solid', color='C2', label="std_dev_b_omega_z") ax[i].legend() ax[i].set_yscale("log") - ax[i].set_ylabel('std_dev_gyro_bias(deg/hr)') + ax[i].set_ylabel('std_dev_gyro_bias(deg/s)') ax[i].grid(True) i+=1 From 78ac3076a33a1058c052cc0a776f1b86ea2e73cb Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Wed, 23 Mar 2022 10:25:58 +0100 Subject: [PATCH 70/87] add quaternion normalization and lambda term in prediction step --- .../simple_flight/AirSimSimpleEkf.hpp | 16 ++++++-- .../simple_flight/AirSimSimpleEkfModel.hpp | 41 +++++++++++-------- 2 files changed, 38 insertions(+), 19 deletions(-) diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp index 774d9b46d0..508812f586 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp @@ -286,6 +286,19 @@ namespace airlib // 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; icheckGpsIfNew()) - return; - // extract the states float x[simple_flight::NX]; for (int i=0; i Date: Wed, 23 Mar 2022 14:13:29 +0100 Subject: [PATCH 71/87] delete codes not needed --- AirLib/include/physics/FastPhysicsEngine.hpp | 5 +- .../simple_flight/firmware/OffboardApi.hpp | 115 --- AirLibUnitTests/JacobianTest.hpp | 126 --- AirLibUnitTests/SimpleFlightTest.hpp | 161 +--- AirLibUnitTests/cleanup.sh | 17 - AirLibUnitTests/compile.sh | 18 - AirLibUnitTests/main.cpp | 20 +- AirLibUnitTests/plot.py | 842 ------------------ AirLibUnitTests/style.mplstyle | 55 -- 9 files changed, 30 insertions(+), 1329 deletions(-) delete mode 100644 AirLibUnitTests/JacobianTest.hpp delete mode 100644 AirLibUnitTests/cleanup.sh delete mode 100644 AirLibUnitTests/compile.sh delete mode 100644 AirLibUnitTests/plot.py delete mode 100644 AirLibUnitTests/style.mplstyle diff --git a/AirLib/include/physics/FastPhysicsEngine.hpp b/AirLib/include/physics/FastPhysicsEngine.hpp index 28ff62862d..39ee9c48f2 100644 --- a/AirLib/include/physics/FastPhysicsEngine.hpp +++ b/AirLib/include/physics/FastPhysicsEngine.hpp @@ -110,9 +110,8 @@ namespace airlib //TODO: this is now being done in PawnSimApi::update. We need to re-think this sequence //with below commented out - Arducopter GPS may not work. - //IMPORTANT! The following two lines should be commented out while using PawnSimApi!. It syncs environment from kinematics. - body.getEnvironment().setPosition(next.pose.position); - body.getEnvironment().update(); + //body.getEnvironment().setPosition(next.pose.position); + //body.getEnvironment().update(); } static void updateCollisionResponseInfo(const CollisionInfo& collision_info, const Kinematics::State& next, 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 7a3df46b77..b5092fcfa1 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/OffboardApi.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/OffboardApi.hpp @@ -59,8 +59,6 @@ class OffboardApi : public IUpdatable detectLanding(); detectTakingOff(); - - logEkfValues(); } /**************** IOffboardApi ********************/ @@ -209,119 +207,6 @@ class OffboardApi : public IUpdatable } private: - - void logEkfValues() - { - // additional logging of vehicle states, TODO implement a separate log! - std::ostringstream log_msg; - log_msg << clock_->millis() << '\t' - // ground truth mesurement signals - << state_estimator_->getTrueMeasurements().accel.x() << '\t' - << state_estimator_->getTrueMeasurements().accel.y() << '\t' - << state_estimator_->getTrueMeasurements().accel.z() << '\t' - << state_estimator_->getTrueMeasurements().gyro.x() << '\t' - << state_estimator_->getTrueMeasurements().gyro.y() << '\t' - << state_estimator_->getTrueMeasurements().gyro.z() << '\t' - // noisy mesurement signals - << state_estimator_->getEkfMeasurements().accel.x() << '\t' - << state_estimator_->getEkfMeasurements().accel.y() << '\t' - << state_estimator_->getEkfMeasurements().accel.z() << '\t' - << state_estimator_->getEkfMeasurements().gyro.x() << '\t' - << state_estimator_->getEkfMeasurements().gyro.y() << '\t' - << state_estimator_->getEkfMeasurements().gyro.z() << '\t' - << state_estimator_->getEkfMeasurements().gps_position.x() << '\t' - << state_estimator_->getEkfMeasurements().gps_position.y() << '\t' - << state_estimator_->getEkfMeasurements().gps_position.z() << '\t' - << state_estimator_->getEkfMeasurements().gps_velocity.x() << '\t' - << state_estimator_->getEkfMeasurements().gps_velocity.y() << '\t' - << state_estimator_->getEkfMeasurements().gps_velocity.z() << '\t' - << state_estimator_->getEkfMeasurements().baro_altitude << '\t' - << state_estimator_->getEkfMeasurements().magnetic_flux.x() << '\t' - << state_estimator_->getEkfMeasurements().magnetic_flux.y() << '\t' - << state_estimator_->getEkfMeasurements().magnetic_flux.z() << '\t' - // ground truth states - << state_estimator_->getTrueKinematicsEstimated().position.x() << '\t' - << state_estimator_->getTrueKinematicsEstimated().position.y() << '\t' - << state_estimator_->getTrueKinematicsEstimated().position.z() << '\t' - << state_estimator_->getTrueKinematicsEstimated().orientation.val4() << '\t' // ATTENSION val4 is w when converted to axis4r of simple_flight - << state_estimator_->getTrueKinematicsEstimated().orientation.x() << '\t' - << state_estimator_->getTrueKinematicsEstimated().orientation.y() << '\t' - << state_estimator_->getTrueKinematicsEstimated().orientation.z() << '\t' - << state_estimator_->getTrueAngles().pitch() << '\t' - << state_estimator_->getTrueAngles().roll() << '\t' - << state_estimator_->getTrueAngles().yaw() << '\t' - << state_estimator_->getTrueKinematicsEstimated().linear_velocity.x() << '\t' - << state_estimator_->getTrueKinematicsEstimated().linear_velocity.y() << '\t' - << state_estimator_->getTrueKinematicsEstimated().linear_velocity.z() << '\t' - // estimated states - << state_estimator_->getEkfKinematicsEstimated().position.x() << '\t' - << state_estimator_->getEkfKinematicsEstimated().position.y() << '\t' - << state_estimator_->getEkfKinematicsEstimated().position.z() << '\t' - << state_estimator_->getEkfKinematicsEstimated().orientation.val4() << '\t' - << state_estimator_->getEkfKinematicsEstimated().orientation.x() << '\t' - << state_estimator_->getEkfKinematicsEstimated().orientation.y() << '\t' - << state_estimator_->getEkfKinematicsEstimated().orientation.z() << '\t' - << state_estimator_->getEkfAngles().pitch() << '\t' - << state_estimator_->getEkfAngles().roll() << '\t' - << state_estimator_->getEkfAngles().yaw() << '\t' - << state_estimator_->getEkfKinematicsEstimated().linear_velocity.x() << '\t' - << state_estimator_->getEkfKinematicsEstimated().linear_velocity.y() << '\t' - << state_estimator_->getEkfKinematicsEstimated().linear_velocity.z() << '\t' - << state_estimator_->getEkfKinematicsEstimated().sensor_bias.accel.x() << '\t' - << state_estimator_->getEkfKinematicsEstimated().sensor_bias.accel.y() << '\t' - << state_estimator_->getEkfKinematicsEstimated().sensor_bias.accel.z() << '\t' - << state_estimator_->getEkfKinematicsEstimated().sensor_bias.gyro.x() << '\t' - << state_estimator_->getEkfKinematicsEstimated().sensor_bias.gyro.y() << '\t' - << state_estimator_->getEkfKinematicsEstimated().sensor_bias.gyro.z() << '\t' - << state_estimator_->getEkfKinematicsEstimated().sensor_bias.barometer << '\t' - // variances - << state_estimator_->getEkfPositionVariance().x() << '\t' - << state_estimator_->getEkfPositionVariance().y() << '\t' - << state_estimator_->getEkfPositionVariance().z() << '\t' - << state_estimator_->getEkfLinearVelocityVariance().x() << '\t' - << state_estimator_->getEkfLinearVelocityVariance().y() << '\t' - << state_estimator_->getEkfLinearVelocityVariance().z() << '\t' - << state_estimator_->getEkfOrientationVariance().val4() << '\t' - << state_estimator_->getEkfOrientationVariance().x() << '\t' - << state_estimator_->getEkfOrientationVariance().y() << '\t' - << state_estimator_->getEkfOrientationVariance().z() << '\t' - << state_estimator_->getEkfAnglesVariance().x() << '\t' - << state_estimator_->getEkfAnglesVariance().y() << '\t' - << state_estimator_->getEkfAnglesVariance().z() << '\t' - << state_estimator_->getEkfAccelBiasVariance().x() << '\t' - << state_estimator_->getEkfAccelBiasVariance().y() << '\t' - << state_estimator_->getEkfAccelBiasVariance().z() << '\t' - << state_estimator_->getEkfGyroBiasVariance().x() << '\t' - << state_estimator_->getEkfGyroBiasVariance().y() << '\t' - << state_estimator_->getEkfGyroBiasVariance().z() << '\t' - << state_estimator_->getEkfBaroBiasVariance() << '\t' - // quaternion norm - << state_estimator_->getEkfOrientationNorm() << '\t' - // off-diag quaternion covariance - << state_estimator_->getEkfOrientationOffDiagCovariance().at(0) << '\t' - << state_estimator_->getEkfOrientationOffDiagCovariance().at(1) << '\t' - << state_estimator_->getEkfOrientationOffDiagCovariance().at(2) << '\t' - << state_estimator_->getEkfOrientationOffDiagCovariance().at(3) << '\t' - << state_estimator_->getEkfOrientationOffDiagCovariance().at(4) << '\t' - << state_estimator_->getEkfOrientationOffDiagCovariance().at(5) << '\t' - // gyro bias quaternion covariance - << state_estimator_->getEkfOrientationGyroBiasCovariance().at(0) << '\t' - << state_estimator_->getEkfOrientationGyroBiasCovariance().at(1) << '\t' - << state_estimator_->getEkfOrientationGyroBiasCovariance().at(2) << '\t' - << state_estimator_->getEkfOrientationGyroBiasCovariance().at(3) << '\t' - << state_estimator_->getEkfOrientationGyroBiasCovariance().at(4) << '\t' - << state_estimator_->getEkfOrientationGyroBiasCovariance().at(5) << '\t' - << state_estimator_->getEkfOrientationGyroBiasCovariance().at(6) << '\t' - << state_estimator_->getEkfOrientationGyroBiasCovariance().at(7) << '\t' - << state_estimator_->getEkfOrientationGyroBiasCovariance().at(8) << '\t' - << state_estimator_->getEkfOrientationGyroBiasCovariance().at(9) << '\t' - << state_estimator_->getEkfOrientationGyroBiasCovariance().at(10) << '\t' - << state_estimator_->getEkfOrientationGyroBiasCovariance().at(11) << '\t'; - - std::string message = log_msg.str(); - comm_link_->log(message); - } - void updateGoalFromRc() { goal_ = rc_.getGoalValue(); diff --git a/AirLibUnitTests/JacobianTest.hpp b/AirLibUnitTests/JacobianTest.hpp deleted file mode 100644 index a2c4934e76..0000000000 --- a/AirLibUnitTests/JacobianTest.hpp +++ /dev/null @@ -1,126 +0,0 @@ - -#ifndef Jacobian_Test_hpp -#define Jacobian_Test_hpp - -#include "TestBase.hpp" -#include "vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfModel.hpp" - -namespace msr -{ -namespace airlib -{ - class JacobianTest : public TestBase - { - public: - virtual void run() override - { - float x[17] = {1.0f, 2.0f, 3.0f, - 1.2f, 2.3f, 3.4f, - 0.9f, 0.1f, 0.2f, 0.3f, - 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}; - float u[6] = {0.0f, 0.0f, -9.80665f, 1.0f, 2.0f, 3.0f}; - float u2[6] = {0.0f, 0.0f, -9.80665f, -3.23f, -1.38f, -0.2f}; - float x_dot[17]; - float x_dotdot[17]; - float x_dotdotdot[17]; - - AirSimSimpleEkfModel::evaluateStateDot(x_dot,x,u); - AirSimSimpleEkfModel::evaluateStateDot(x_dotdot,x_dot,u2); - AirSimSimpleEkfModel::evaluateStateDot(x_dotdotdot,x_dotdot,u2); - - float x_dotNew[17]; - float x_dotdotNew[17]; - float x_dotdotdotNew[17]; - - // AirSimSimpleEkfModel::evaluateStateDotOld(x_dotNew,x,u); - // AirSimSimpleEkfModel::evaluateStateDotOld(x_dotdotNew,x_dotNew,u2); - // AirSimSimpleEkfModel::evaluateStateDotOld(x_dotdotdotNew,x_dotdotNew,u2); - - for (int i=0; i<17; i++) - { - std::cout << x[i] << '\t'; - std::cout << x_dot[i] << '\t'; - std::cout << x_dotdot[i] << '\t'; - std::cout << x_dotdotdot[i] << '\n'; - } - for (int i=0; i<17; i++) - { - std::cout << x[i] << '\t'; - std::cout << x_dotNew[i] << '\t'; - std::cout << x_dotdotNew[i] << '\t'; - std::cout << x_dotdotdotNew[i] << '\n'; - } - simple_flight::MatrixNXxNXf A; - simple_flight::MatrixNXxNXf ANew; - AirSimSimpleEkfModel::evaluateA(&A, x_dotdotdot,u2); - // AirSimSimpleEkfModel::evaluateAOld(&ANew, x_dotdotdot,u2); - - for (int i=0; i<17; i++){ - for (int j=0; j<17; j++){ - std::cout << A(i,j) << "\t "; - } - std::cout << '\n'; - } - - // for (int i=0; i<17; i++){ - // for (int j=0; j<17; j++){ - // std::cout << ANew(i,j) << "\t "; - // } - // std::cout << '\n'; - // } - - // QuaternionT q = VectorMath::toQuaternion(RealT pitch, RealT roll, RealT yaw); - Quaternionr q = VectorMath::toQuaternion(0.2*M_PI/180, 0.0*M_PI/180, 0.2*M_PI/180); - std::cout << q.w() << '\n'; - std::cout << q.x() << '\n'; - std::cout << q.y() << '\n'; - std::cout << q.z() << '\n'; - - - // VectorMath::Matrix17x17f A_finite; - // VectorMath::Matrix17x17f A; - // VectorMath::Matrix17x17f A_error; - - // // evaluateA(&A, x, u); - // AirSimSimpleEkfModel::evaluateFiniteDifferenceA(&A_finite, x_dotdotdot,u2); - // AirSimSimpleEkfModel::evaluateA(&A, x_dotdotdot,u2); - - // volatile bool isOK; - // volatile float row[17]; - // volatile float column[17]; - // isOK = AirSimSimpleEkfModel::checkA(&A_error, &A, &A_finite, row, column); - - // std::cout << '\n'; - // for (int i=0; i<17; i++) - // { - // for (int j=0; j<17; j++) - // { - // std::cout << A_finite(i,j) << "\t "; - // } - // std::cout << '\n'; - // } - // std::cout << '\n'; - // for (int i=0; i<17; i++) - // { - // for (int j=0; j<17; j++) - // { - // std::cout << A(i,j) << "\t "; - // } - // std::cout << '\n'; - // } - // std::cout << '\n'; - // for (int i=0; i<17; i++) - // { - // for (int j=0; j<17; j++) - // { - // std::cout << A_error(i,j) << "\t "; - // } - // std::cout << '\n'; - // } - - } - - }; -} -} -#endif \ No newline at end of file diff --git a/AirLibUnitTests/SimpleFlightTest.hpp b/AirLibUnitTests/SimpleFlightTest.hpp index daa3fc4c49..304b7c32bc 100644 --- a/AirLibUnitTests/SimpleFlightTest.hpp +++ b/AirLibUnitTests/SimpleFlightTest.hpp @@ -10,189 +10,76 @@ #include "common/SteppableClock.hpp" #include "vehicles/multirotor/MultiRotorPhysicsBody.hpp" -#include "settings_json_parser.hpp" - -#include "vehicles/multirotor/firmwares/simple_flight/SimpleFlightQuadXParams.hpp" - namespace msr { namespace airlib { - void checkStatusMsg(MultirotorApiBase* api, std::ofstream* myfile) - { - - std::vector messages_; - api->getStatusMessages(messages_); - for (const auto& status_message : messages_) { - *myfile << status_message << std::endl; - // std::cout << status_message << std::endl; - } - } class SimpleFlightTest : public TestBase { public: virtual void run() override { - std::cout << std::endl; auto clock = std::make_shared(3E-3f); - // auto clock = std::make_shared(1E-1f); ClockFactory::get(clock); SensorFactory sensor_factory; - // added from https://github.com/microsoft/AirSim/pull/2558/commits/9c4e59d1a2b371ebc60cdc18f93b06cbe3e9d305 - // loads settings from settings.json or Default setting - SettingsLoader settings_loader; - std::unique_ptr params = MultiRotorParamsFactory::createConfig( AirSimSettings::singleton().getVehicleSetting("SimpleFlight"), std::make_shared()); auto api = params->createMultirotorApi(); - // create and initialize kinematics and environment std::unique_ptr kinematics; std::unique_ptr environment; Kinematics::State initial_kinematic_state = Kinematics::State::zero(); - - initial_kinematic_state.pose = Pose(Vector3r(1.0f, 2.0f, 3.0f), Quaternionr(1, 0, 0, 0)); - // states_(6) = 0.9961946f; // q0 - // states_(7) = 0.0f; // q1 - // states_(8) = 0.08715574f; // q2 - - // initial_kinematic_state.pose.orientation.w() = 0.9961946f; - // initial_kinematic_state.pose.orientation.x() = 0.0f; - // initial_kinematic_state.pose.orientation.y() = 0.08715574f; - // initial_kinematic_state.pose.orientation.z() = 0.0f; - - // initial_kinematic_state.pose.position.z() = -10.0f; + ; + initial_kinematic_state.pose = Pose(); kinematics.reset(new Kinematics(initial_kinematic_state)); Environment::State initial_environment; initial_environment.position = initial_kinematic_state.pose.position; - initial_environment.geo_point = GeoPoint(47.6415, -122.14, 121.173); // this becomes the home geo point with which ned to geo conversion takes place - // initial_environment.geo_point.altitude = 0.0f; // do not set it equal to kinematics z, this value goes into home geo point and acts as the ref for kinematics z + initial_environment.geo_point = GeoPoint(); environment.reset(new Environment(initial_environment)); - // crete and initialize body and physics world - MultiRotorPhysicsBody vehicle(params.get(), api.get(), kinematics.get(), environment.get()); + MultiRotorPhysicsBody vehicle(params.get(), api.get(), kinematics.get(), environment.get()); std::vector vehicles = { &vehicle }; std::unique_ptr physics_engine(new FastPhysicsEngine()); - PhysicsWorld physics_world(std::move(physics_engine), vehicles);//, static_cast(clock->getStepSize() * 1E9)); - // world.startAsyncUpdator(); called in the physics_world constructor - - // added by Suman, to fix calling update before reset https://github.com/microsoft/AirSim/issues/2773#issuecomment-703888477 - // TODO not sure if it should be here? see wrt to PawnSimApi, no side effects so far - // for the order of the reset see void MultirotorPawnSimApi::resetImplementation() - api->setSimulatedGroundTruth(&kinematics.get()->getState(), environment.get()); - kinematics->reset(); // sets initial kinematics as current among other things - environment->reset(); - api->reset(); - - // set the vehicle as grounded, otherwise can not take off, needs to to be done after physics world construction! - vehicle.setGrounded(true); + PhysicsWorld physics_world(std::move(physics_engine), vehicles, static_cast(clock->getStepSize() * 1E9)); - // read intitial position - Vector3r pos = api->getMultirotorState().getPosition(); - std::cout << "starting position: " << pos << std::endl; - - // test api if not null testAssert(api != nullptr, "api was null"); std::string message; testAssert(api->isReady(message), message); + clock->sleep_for(0.04f); Utils::getSetMinLogLevel(true, 100); - std::ostringstream ss; - - std::ofstream myfile; - myfile.open("log.txt"); - /*myfile << ">> Physics update frequency: 333.33 Hz.\n"; - myfile << ">> Barometer and magnetometer update frequency: 50 Hz.\n"; - myfile << ">> GPS update frequency: 50 Hz with startup delay.\n\n";*/ - myfile << ">> timestamp (ms) \t GroundTruth altitude \t Estimated postiion (x,y,z) \n\n"; - - // enable api control api->enableApiControl(true); - //checkStatusMsg(api.get(), &myfile); - - // arm api->armDisarm(true); - //checkStatusMsg(api.get(), &myfile); - - clock->sleep_for(30.0f); - - // take off - api->takeoff(50); - pos = api->getMultirotorState().getPosition(); - std::cout << "took-off position: " << pos << std::endl; - //checkStatusMsg(api.get(), &myfile); - - // clock->sleep_for(60.0f); - // api->resetEkf(); - - // api->commandAngleRatesZ(0.0f, 0.0f, 0.1f, 0.0f); - // clock->sleep_for(18.0f); - // api->moveByAngleRatesZ(0.0f, 0.0f, 9.0f*M_PI/180, -1.53509f, 10.0f); - // clock->sleep_for(18.0f); - // api->moveByAngleRatesZ(0.0f, 4.0f*M_PI/180, 0.0f, -1.53509f, 10.0f); - // clock->sleep_for(60.0f); - // api->moveByAngleRatesZ(0.0f, 4.0f*M_PI/180, 0.0f, -1.53509f, 10.0f); - // fly towards a waypoint - // api->moveToPosition(-1, 0, -1.53509, 0.1, 1E3, DrivetrainType::MaxDegreeOfFreedom, YawMode(true, 0), -1, 0); - // pos = api->getMultirotorState().getPosition(); - // std::cout << "waypoint position: " << pos << std::endl; - // api->moveToPosition(0, -1, -1.53509, 0.1, 1E3, DrivetrainType::MaxDegreeOfFreedom, YawMode(true, 0), -1, 0); - // pos = api->getMultirotorState().getPosition(); - //checkStatusMsg(api.get(), &myfile); - - clock->sleep_for(10.0f); - api->moveByAngleRatesZ(0.0f, 0.0f, 4.0f*M_PI/180, -1.53509f, 4.0f); - clock->sleep_for(10.0f); - // // // fly towards a waypoint - api->moveToPosition(10, 0, -2, 0.5, 1E3, DrivetrainType::MaxDegreeOfFreedom, YawMode(true, 0), -1, 0); - clock->sleep_for(10.0f); - api->moveToPosition(10, 10, -2, 0.5, 1E3, DrivetrainType::MaxDegreeOfFreedom, YawMode(true, 0), -1, 0); - clock->sleep_for(10.0f); - api->moveToPosition(10, 10, -20, 0.5, 1E3, DrivetrainType::MaxDegreeOfFreedom, YawMode(true, 0), -1, 0); - pos = api->getMultirotorState().getPosition(); - std::cout << "waypoint position: " << pos << std::endl; - // // api->commandVelocity(10.0f, 0.0f, 0.0f, YawMode(true, 0)); - // // pos = api->getMultirotorState().getPosition(); - // // //std::cout << "waypoint position: " << pos << std::endl; - // // //checkStatusMsg(api.get(), &myfile); - - // clock->sleep_for(60.0f); - // api->moveToPosition(-100, 100, -50, 15, 1E3, DrivetrainType::MaxDegreeOfFreedom, YawMode(true, 0), -1, 0); - // pos = api->getMultirotorState().getPosition(); - // std::cout << "waypoint position: " << pos << std::endl; - clock->sleep_for(20.0f); - - // // land - // //api->land(10); - // pos = api->getMultirotorState().getPosition(); - // std::cout << "final position: " << pos << std::endl; - checkStatusMsg(api.get(), &myfile); - - // TODO print some values OR log - - // // report states - // std::cout << std::endl; - // StateReporter reporter; - // kinematics->reportState(reporter); // this writes the kinematics in reporter - // std::cout << reporter.getOutput() << std::endl; - - myfile.close(); - - /*while (true) { - }*/ + api->takeoff(10); + + clock->sleep_for(2.0f); + Utils::getSetMinLogLevel(true); + + api->moveToPosition(-5, -5, -5, 5, 1E3, DrivetrainType::MaxDegreeOfFreedom, YawMode(true, 0), -1, 0); + + clock->sleep_for(2.0f); + + while (true) { + clock->sleep_for(0.1f); + api->getStatusMessages(messages_); + for (const auto& status_message : messages_) { + std::cout << status_message << std::endl; + } + messages_.clear(); + } } private: - //std::vector messages_; + std::vector messages_; }; } } diff --git a/AirLibUnitTests/cleanup.sh b/AirLibUnitTests/cleanup.sh deleted file mode 100644 index d43ac15295..0000000000 --- a/AirLibUnitTests/cleanup.sh +++ /dev/null @@ -1,17 +0,0 @@ -#!/bin/bash -cleanup(){ - debug_flag="$1" - # in_file="$1" - # data_file="$2" - # log_file="$3" - # cat "$in_file" | sed -E '/^[^0-9]*$/d' > "$data_file" - # cat "$in_file" | sed -E '/^[0-9]+.*$/d' > "$log_file" - if [[ $debug_flag == 'd' ]]; then - cat "log.txt" | sed -E '/^[^0-9]*$/d' > "data.csv" - cat "log.txt" | sed -E '/^[0-9]+.*$/d' > "other.txt" - else - cat "../build_release/output/bin/log.txt" | sed -E '/^[^0-9]*$/d' > "data.csv" - cat "../build_release/output/bin/log.txt" | sed -E '/^[0-9]+.*$/d' > "other.txt" - fi - -} diff --git a/AirLibUnitTests/compile.sh b/AirLibUnitTests/compile.sh deleted file mode 100644 index c072e56c94..0000000000 --- a/AirLibUnitTests/compile.sh +++ /dev/null @@ -1,18 +0,0 @@ -#!/bin/bash -compile(){ - debug_flag="$1" - if [[ $debug_flag == 'd' ]]; then - cd ../build_debug/AirLibUnitTests - make - cd ../../AirLibUnitTests - echo "Debug mode, built only" - else - cd ../build_release/AirLibUnitTests - make - cd ../output/bin - ./AirLibUnitTests - cd ../../../AirLibUnitTests - echo "Release mode, built and run" - fi -} - diff --git a/AirLibUnitTests/main.cpp b/AirLibUnitTests/main.cpp index 2043786768..e0c2f4d6f4 100644 --- a/AirLibUnitTests/main.cpp +++ b/AirLibUnitTests/main.cpp @@ -5,35 +5,23 @@ #include "WorkerThreadTest.hpp" #include "QuaternionTest.hpp" #include "CelestialTests.hpp" -#include "JacobianTest.hpp" - -#include int main() { using namespace msr::airlib; std::unique_ptr tests[] = { - //std::unique_ptr(new QuaternionTest()), - // std::unique_ptr(new CelestialTest()), - //std::unique_ptr(new SettingsTest()), + std::unique_ptr(new QuaternionTest()), + std::unique_ptr(new CelestialTest()), + std::unique_ptr(new SettingsTest()), std::unique_ptr(new SimpleFlightTest()) - // std::unique_ptr(new JacobianTest()) //, //std::unique_ptr(new PixhawkTest()), //std::unique_ptr(new WorkerThreadTest()) }; - std::chrono::time_point start, end; - - start = std::chrono::system_clock::now(); for (auto& test : tests) test->run(); - end = std::chrono::system_clock::now(); - - std::chrono::duration elapsed_seconds = end - start; - std::cout << "elapsed time (s): " << elapsed_seconds.count() << "\n"; - std::cout << "elapsed time (min): " << elapsed_seconds.count()/60.0f << "\n"; return 0; -} +} \ No newline at end of file diff --git a/AirLibUnitTests/plot.py b/AirLibUnitTests/plot.py deleted file mode 100644 index 98a1ed2c9a..0000000000 --- a/AirLibUnitTests/plot.py +++ /dev/null @@ -1,842 +0,0 @@ -#!/usr/bin python3 -""" -Module Docstring -""" - -__author__ = "Suman Subedi" -__version__ = "0.1.0" -__license__ = "MIT" - -import argparse -import math -import numpy as np -import pandas as pd -import matplotlib as mpl -import matplotlib.pyplot as plt -from matplotlib.path import Path -from matplotlib.patches import PathPatch - -def draw_error_band(ax, x, y, err, **kwargs): - # Calculate normals via centered finite differences (except the first point - # which uses a forward difference and the last point which uses a backward - # difference). - - # dx = np.concatenate([[x[1] - x[0]], x[2:] - x[:-2], [x[-1] - x[-2]]]) - # dy = np.concatenate([[y[1] - y[0]], y[2:] - y[:-2], [y[-1] - y[-2]]]) - # l = np.hypot(dx, dy) - # nx = dy / l - # ny = -dx / l - - # end points of errors - # xp = x + nx * err - # yp = y + ny * err - # xn = x - nx * err - # yn = y - ny * err - xp = x + err - yp = y + err - xn = x - err - yn = y - err - - vertices = np.block([[xp, xn[::-1]], - [yp, yn[::-1]]]).T - codes = np.full(len(vertices), Path.LINETO) - codes[0] = Path.MOVETO - # codes[len(xp)] = Path.MOVETO - path = Path(vertices, codes) - ax.add_patch(PathPatch(path, **kwargs)) - -def main(): - """ Main entry point of the app """ - parser = argparse.ArgumentParser() - parser.add_argument("--filename", help="filename of the txt file") - args = parser.parse_args() - - if args.filename == None: - data = pd.read_csv('data.csv', delimiter='\t', header=None) - else: - data = pd.read_csv(args.filename, delimiter='\t', header=None) - - # print(data.head()) - - # plotResults(data.head(100*300)) - plotResults(data) - - # N = 400 - # t = np.linspace(0, 2 * np.pi, N) - # r = 0.5 + np.cos(t) - # x, y = r * np.cos(t), r * np.sin(t) - - # fig, ax = plt.subplots() - # # ax.plot(x, y, "k") - # # ax.set(aspect=1) - - # # axs = (plt.figure(constrained_layout=True) - # # .subplots(1, 2, sharex=True, sharey=True)) - # # errs = [ - # # (axs[0], "constant error", 0.05), - # # (axs[1], "variable error", 0.05 * np.sin(2 * t) ** 2 + 0.04), - # # ] - - # ax.set(title=[], aspect=1, xticks=[], yticks=[]) - # ax.plot(x, y, "k") - # draw_error_band(ax, x, y, 0.05 * np.sin(2 * t) ** 2 + 0.04, - # facecolor=f"C{1}", edgecolor="none", alpha=.3) - - # plt.show() - # fig.savefig("plot.pdf") - -def M_OB(q0, q1, q2, q3) -> np.array: - """ Direction cosine matrix """ - return np.array([[q0*q0 + q1*q1 - q2*q2 - q3*q3, 2.0*(q1*q2 - q0*q3), 2.0*(q0*q2 + q1*q3)], - [2.0*(q1*q2 + q0*q3), q0*q0 - q1*q1 + q2*q2 - q3*q3, 2.0*(q2*q3 - q0*q1)], - [2.0*(q1*q3 - q0*q2), 2.0*(q2*q3 + q0*q1), q0*q0 - q1*q1 - q2*q2 + q3*q3]]) - -def plotResults(data): - i=0 - # extract time stamps - timestamp = data[i].to_numpy() - i+=1 - start_time = timestamp[0] - timestamp = (timestamp - start_time)*0.001 - - # extract truth sensor measurements - true_accel_x = data[i].to_numpy(float) - i+=1 - true_accel_y = data[i].to_numpy(float) - i+=1 - true_accel_z = data[i].to_numpy(float) - i+=1 - true_gyro_x = data[i].to_numpy(float)*180/np.pi - i+=1 - true_gyro_y = data[i].to_numpy(float)*180/np.pi - i+=1 - true_gyro_z = data[i].to_numpy(float)*180/np.pi - i+=1 - - # extract sensor measurements - accel_x = data[i].to_numpy(float) - i+=1 - accel_y = data[i].to_numpy(float) - i+=1 - accel_z = data[i].to_numpy(float) - i+=1 - gyro_x = data[i].to_numpy(float)*180/np.pi - i+=1 - gyro_y = data[i].to_numpy(float)*180/np.pi - i+=1 - gyro_z = data[i].to_numpy(float)*180/np.pi - i+=1 - gps_pos_x = data[i].to_numpy(float) - i+=1 - gps_pos_y = data[i].to_numpy(float) - i+=1 - gps_pos_z = data[i].to_numpy(float) - i+=1 - gps_vel_x = data[i].to_numpy(float) - i+=1 - gps_vel_y = data[i].to_numpy(float) - i+=1 - gps_vel_z = data[i].to_numpy(float) - i+=1 - baro_alt = data[i].to_numpy(float) - i+=1 - mag_flux_x = data[i].to_numpy(float) - i+=1 - mag_flux_y = data[i].to_numpy(float) - i+=1 - mag_flux_z = data[i].to_numpy(float) - i+=1 - - # extract ground truth states - true_x = data[i].to_numpy(float) - i+=1 - true_y = data[i].to_numpy(float) - i+=1 - true_z = data[i].to_numpy(float) - i+=1 - true_q0 = data[i].to_numpy(float) - i+=1 - true_q1 = data[i].to_numpy(float) - i+=1 - true_q2 = data[i].to_numpy(float) - i+=1 - true_q3 = data[i].to_numpy(float) - i+=1 - true_pitch= data[i].to_numpy(float)*180/np.pi - i+=1 - true_roll = data[i].to_numpy(float)*180/np.pi - i+=1 - true_yaw = data[i].to_numpy(float)*180/np.pi - i+=1 - true_lin_vel_x = data[i].to_numpy(float) - i+=1 - true_lin_vel_y = data[i].to_numpy(float) - i+=1 - true_lin_vel_z = data[i].to_numpy(float) - i+=1 - - # estimated ekf states - estimated_x = data[i].to_numpy(float) - i+=1 - estimated_y = data[i].to_numpy(float) - i+=1 - estimated_z = data[i].to_numpy(float) - i+=1 - estimated_q0 = data[i].to_numpy(float) - i+=1 - estimated_q1 = data[i].to_numpy(float) - i+=1 - estimated_q2 = data[i].to_numpy(float) - i+=1 - estimated_q3 = data[i].to_numpy(float) - i+=1 - estimated_pitch = data[i].to_numpy(float)*180/np.pi - i+=1 - estimated_roll = data[i].to_numpy(float)*180/np.pi - i+=1 - estimated_yaw = data[i].to_numpy(float)*180/np.pi - i+=1 - esti_lin_vel_x = data[i].to_numpy(float) - i+=1 - esti_lin_vel_y = data[i].to_numpy(float) - i+=1 - esti_lin_vel_z = data[i].to_numpy(float) - i+=1 - bias_accel_x = data[i].to_numpy(float) * 1000 / 9.80665 - i+=1 - bias_accel_y = data[i].to_numpy(float) * 1000 / 9.80665 - i+=1 - bias_accel_z = data[i].to_numpy(float) * 1000 / 9.80665 - i+=1 - bias_gyro_x = data[i].to_numpy(float) * 180/np.pi - i+=1 - bias_gyro_y = data[i].to_numpy(float) * 180/np.pi - i+=1 - bias_gyro_z = data[i].to_numpy(float) * 180/np.pi - i+=1 - bias_baro = data[i].to_numpy(float) - i+=1 - - # variance of position - cov_x = (data[i].to_numpy(float)) - i+=1 - cov_y = (data[i].to_numpy(float)) - i+=1 - cov_z = (data[i].to_numpy(float)) - i+=1 - - # variance of linear velocity - cov_u = (data[i].to_numpy(float)) - i+=1 - cov_v = (data[i].to_numpy(float)) - i+=1 - cov_w = (data[i].to_numpy(float)) - i+=1 - - # extract covariance of angles - cov_q0 = (data[i].to_numpy(float)) - i+=1 - cov_q1 = (data[i].to_numpy(float)) - i+=1 - cov_q2 = (data[i].to_numpy(float)) - i+=1 - cov_q3 = (data[i].to_numpy(float)) - i+=1 - cov_roll = (data[i].to_numpy(float))*180*180/np.pi/np.pi - i+=1 - cov_pitch = (data[i].to_numpy(float))*180*180/np.pi/np.pi - i+=1 - cov_yaw = (data[i].to_numpy(float))*180*180/np.pi/np.pi - i+=1 - - # variance of imu bias - cov_b_f_x = (data[i].to_numpy(float)) * (1000 / 9.80665)* (1000 / 9.80665) - i+=1 - cov_b_f_y = (data[i].to_numpy(float)) * (1000 / 9.80665)* (1000 / 9.80665) - i+=1 - cov_b_f_z = (data[i].to_numpy(float)) * (1000 / 9.80665)* (1000 / 9.80665) - i+=1 - - # variance of gyro bias - cov_b_omega_x = (data[i].to_numpy(float)) * (180/np.pi)* (180/np.pi) - i+=1 - cov_b_omega_y = (data[i].to_numpy(float)) * (180/np.pi)* (180/np.pi) - i+=1 - cov_b_omega_z = (data[i].to_numpy(float)) * (180/np.pi)* (180/np.pi) - i+=1 - - # variance baro - cov_b_baro = data[i].to_numpy(float) - i+=1 - - # quaternion norm - quat_norm = data[i].to_numpy(float) - i+=1 - - # quaternion off diag cov - cov_q0_q1 = data[i].to_numpy(float) - i+=1 - cov_q0_q2 = data[i].to_numpy(float) - i+=1 - cov_q0_q3 = data[i].to_numpy(float) - i+=1 - cov_q1_q2 = data[i].to_numpy(float) - i+=1 - cov_q1_q3 = data[i].to_numpy(float) - i+=1 - cov_q2_q3 = data[i].to_numpy(float) - i+=1 - - # quaternion gyro bias cov - cov_q0_omega_x = data[i].to_numpy(float) - i+=1 - cov_q0_omega_y = data[i].to_numpy(float) - i+=1 - cov_q0_omega_z = data[i].to_numpy(float) - i+=1 - cov_q1_omega_x = data[i].to_numpy(float) - i+=1 - cov_q1_omega_y = data[i].to_numpy(float) - i+=1 - cov_q1_omega_z = data[i].to_numpy(float) - i+=1 - cov_q2_omega_x = data[i].to_numpy(float) - i+=1 - cov_q2_omega_y = data[i].to_numpy(float) - i+=1 - cov_q2_omega_z = data[i].to_numpy(float) - i+=1 - cov_q3_omega_x = data[i].to_numpy(float) - i+=1 - cov_q3_omega_y = data[i].to_numpy(float) - i+=1 - cov_q3_omega_z = data[i].to_numpy(float) - i+=1 - - # position error - err_estimated_x = true_x - estimated_x - err_estimated_y = true_y - estimated_y - err_estimated_z = true_z - estimated_z - - # angle error - err_estimated_pitch = true_pitch - estimated_pitch - err_estimated_roll = true_roll - estimated_roll - err_estimated_yaw = true_yaw - estimated_yaw - - # orientation error - error_q0 = true_q0 - estimated_q0 - error_q1 = true_q1 - estimated_q1 - error_q2 = true_q2 - estimated_q2 - error_q3 = true_q3 - estimated_q3 - - # orientation error - - - # velocity error - err_lin_vel_x = true_lin_vel_x - esti_lin_vel_x - err_lin_vel_y = true_lin_vel_y - esti_lin_vel_y - err_lin_vel_z = true_lin_vel_z - esti_lin_vel_z - - # sensor errors - err_accel_x = true_accel_x - accel_x - err_accel_y = true_accel_y - accel_y - err_accel_z = true_accel_z - accel_z - err_gyro_x = true_gyro_x - gyro_x - err_gyro_y = true_gyro_y - gyro_y - err_gyro_z = true_gyro_z - gyro_z - err_gps_x = true_x - gps_pos_x - err_gps_y = true_y - gps_pos_y - err_gps_z = true_z - gps_pos_z - err_gps_vel_x = true_lin_vel_x - gps_vel_x - err_gps_vel_y = true_lin_vel_y - gps_vel_y - err_gps_vel_z = true_lin_vel_z - gps_vel_z - err_baro_alt = -1*true_z - baro_alt - - # print(gps_vel_z) - - start_index = np.where(np.int_(timestamp) == 5)[0][0] - end_index = np.where(np.int_(timestamp) == 6)[0][0] - - # print("True pitch rate 1: ", true_gyro_x[start_index]) - # print("True pitch rate 2: ", true_gyro_x[end_index]) - - # print("True pitch angle 1: ", true_roll[start_index]) - # print("Estimated pitch angle 1: ", estimated_roll[start_index]) - - # print("True pitch angle 2: ", true_roll[end_index]) - # print("Estimated pitch angle 2: ", estimated_roll[end_index]) - - # print("Time difference: ", timestamp[end_index] - timestamp[start_index]) - # print("Expected delta pitch angle: ", (true_gyro_x[start_index]+true_gyro_x[end_index])*0.5*(timestamp[end_index] - timestamp[start_index])) - # print("AirSim delta pitch angle: ", (true_roll[end_index] - true_roll[start_index])) - # print("Our delta pitch angle: ", (estimated_roll[end_index] - estimated_roll[start_index])) - - # start_index = np.where(np.int_(timestamp) == 6)[0][0] - # end_index = np.where(np.int_(timestamp) == 7)[0][0] - # print("Expected delta pitch angle: ", (true_gyro_x[start_index]+true_gyro_x[end_index])*0.5*(timestamp[end_index] - timestamp[start_index])) - # print("AirSim delta pitch angle: ", (true_roll[end_index] - true_roll[start_index])) - # print("Our delta pitch angle: ", (estimated_roll[end_index] - estimated_roll[start_index])) - - # k = np.array([[1, 2, 3],[4, 5, 6],[7, 8, 9]]) - # print(k) - # print(k[2,1]) - - theta_error = np.zeros(len(timestamp)) - phi_error = np.zeros(len(timestamp)) - psi_error = np.zeros(len(timestamp)) - # # print(psi_error) - - for i in range(0, len(timestamp)): - ground = M_OB(true_q0[i], - true_q1[i], - true_q2[i], - true_q3[i]) - estimated = M_OB(estimated_q0[i], - estimated_q1[i], - estimated_q2[i], - estimated_q3[i]) - # print(ground_error) - - error_direction_cosine = ground*estimated.transpose() - # print(error_direction_cosine[2, 0]) - - # euler = [atan2(mat(3,2),mat(3,3)); - # -asin(mat(3,1)); - # atan2(mat(2,1),mat(1,1))]; - - phi_error[i] = np.arctan2(error_direction_cosine[2, 1],error_direction_cosine[2, 2])*180/np.pi - theta_error[i] = -np.arcsin(error_direction_cosine[2, 0])*180/np.pi - psi_error[i] = np.arctan2(error_direction_cosine[1, 0],error_direction_cosine[0, 0])*180/np.pi - - - # apply style - plt.style.use('./style.mplstyle') - - # define the plot - fig, ax = plt.subplots(38, 1) - - # begin plotting - i=0 - ax[i].plot(timestamp, accel_x, linestyle='solid', color='C0', label="x") - ax[i].legend() - ax[i].plot(timestamp, accel_y, linestyle='dotted', color='C1', label="y") - ax[i].legend() - ax[i].plot(timestamp, accel_z, linestyle='solid', color='C2', label="z") - ax[i].legend() - ax[i].set_ylabel('accel (m/s2)') - ax[i].grid(True) - - i+=1 - ax[i].plot(timestamp, err_accel_x, linestyle='solid', color='C0', label="x") - ax[i].legend() - ax[i].plot(timestamp, err_accel_y, linestyle='dotted', color='C1', label="y") - ax[i].legend() - ax[i].plot(timestamp, err_accel_z, linestyle='solid', color='C2', label="z") - ax[i].legend() - ax[i].set_ylabel('accel err (m/s2)') - ax[i].grid(True) - - i+=1 - ax[i].plot(timestamp, gyro_x, linestyle='solid', color='C0', label="roll") - ax[i].legend() - ax[i].plot(timestamp, gyro_y, linestyle='dotted', color='C1', label="pitch") - ax[i].legend() - ax[i].plot(timestamp, gyro_z, linestyle='solid', color='C2', label="yaw") - ax[i].legend() - ax[i].set_ylabel('gyro rates (deg/s)') - # ax[i].set_xlim([10, 15]) - # ax[i].set_ylim([-1, 10]) - ax[i].grid(True) - - i+=1 - ax[i].plot(timestamp, err_gyro_x, linestyle='solid', color='C0', label="roll") - ax[i].legend() - ax[i].plot(timestamp, err_gyro_y, linestyle='dotted', color='C1', label="pitch") - ax[i].legend() - ax[i].plot(timestamp, err_gyro_z, linestyle='solid', color='C2', label="yaw") - ax[i].legend() - ax[i].set_ylabel('gyro error(deg/s)') - ax[i].grid(True) - - i+=1 - ax[i].plot(timestamp, err_gps_x, linestyle='solid', color='C0', label="x") - ax[i].legend() - ax[i].plot(timestamp, err_gps_y, linestyle='solid', color='C1', label="y") - ax[i].legend() - ax[i].plot(timestamp, err_gps_z, linestyle='solid', color='C2', label="z") - ax[i].legend() - ax[i].set_ylabel('gps position err(m)') - ax[i].grid(True) - - i+=1 - ax[i].plot(timestamp, err_gps_vel_x, linestyle='solid', color='C0', label="x") - ax[i].legend() - ax[i].plot(timestamp, err_gps_vel_y, linestyle='solid', color='C1', label="y") - ax[i].legend() - ax[i].plot(timestamp, err_gps_vel_z, linestyle='solid', color='C2', label="z") - ax[i].legend() - ax[i].set_ylabel('gps velocity err(m/s)') - ax[i].grid(True) - - i+=1 - ax[i].plot(timestamp, mag_flux_x, linestyle='solid', color='C0', label="x") - ax[i].legend() - ax[i].plot(timestamp, mag_flux_y, linestyle='solid', color='C1', label="y") - ax[i].legend() - ax[i].plot(timestamp, mag_flux_z, linestyle='solid', color='C2', label="z") - ax[i].legend() - ax[i].set_ylabel('Mag. flux (Gauss)') - ax[i].grid(True) - - i+=1 - ax[i].plot(timestamp, true_pitch, linestyle='solid', color='k', label="true_pitch") - ax[i].legend(loc='upper left') - ax[i].plot(timestamp, estimated_pitch, linestyle='solid', color='C0', label="estimated_pitch") - ax[i].legend(loc='upper left') - ax[i].fill_between(timestamp, - estimated_pitch-3*np.sqrt(cov_pitch), - estimated_pitch+3*np.sqrt(cov_pitch), - alpha=0.2,linewidth=0, color='C5') - ax[i].set_ylabel('pitch(deg)') - # ax[i].set_xlim([10, 15]) - # ax[i].set_ylim([-1, 40]) - ax[i].grid(True) - - i+=1 - ax[i].plot(timestamp, true_roll, linestyle='solid', color='k', label="true_roll") - ax[i].legend(loc='upper left') - ax[i].plot(timestamp, estimated_roll, linestyle='solid', color='C1', label="estimated_roll") - ax[i].legend(loc='upper left') - ax[i].fill_between(timestamp, - estimated_roll-3*np.sqrt(cov_roll), - estimated_roll+3*np.sqrt(cov_roll), - alpha=0.2,linewidth=0, color='C5') - ax[i].set_ylabel('roll(deg)') - # ax[i].set_xlim([10, 15]) - # ax[i].set_ylim([-1, 40]) - ax[i].grid(True) - - i+=1 - ax[i].plot(timestamp, true_yaw, linestyle='solid', color='k', label="true_yaw") - ax[i].legend() - ax[i].plot(timestamp, estimated_yaw, linestyle='solid', color='C2', label="estimated_yaw") - ax[i].legend() - ax[i].fill_between(timestamp, - estimated_yaw-3*np.sqrt(cov_yaw), - estimated_yaw+3*np.sqrt(cov_yaw), - alpha=0.2,linewidth=0, color='C5') - ax[i].set_ylabel('yaw(deg)') - # ax[i].set_xlim([10, 15]) - # ax[i].set_ylim([-2, 2]) - ax[i].grid(True) - - i+=1 - ax[i].plot(timestamp, true_x, linestyle='solid', color='k', label="true_x") - ax[i].legend() - ax[i].plot(timestamp, estimated_x, linestyle='solid', color='C0', label="estimated_x") - ax[i].legend() - ax[i].fill_between(timestamp, - estimated_x-3*np.sqrt(cov_x), - estimated_x+3*np.sqrt(cov_x), - alpha=0.3,linewidth=0, color='C5') - ax[i].set_ylabel('x position(m)') - # ax[i].set_xlim([10, 15]) - ax[i].grid(True) - - i+=1 - ax[i].plot(timestamp, true_y, linestyle='solid', color='k', label="true_y") - ax[i].legend() - ax[i].plot(timestamp, estimated_y, linestyle='solid', color='C1', label="estimated_y") - ax[i].legend() - ax[i].fill_between(timestamp, - estimated_y-3*np.sqrt(cov_y), - estimated_y+3*np.sqrt(cov_y), - alpha=0.3,linewidth=0, color='C5') - ax[i].set_ylabel('y position(m)') - # ax[i].set_xlim([10, 15]) - ax[i].grid(True) - - i+=1 - ax[i].plot(timestamp, true_z, linestyle='solid', color='k', label="true_z") - ax[i].legend() - ax[i].plot(timestamp, estimated_z, linestyle='solid', color='C2', label="estimated_z") - ax[i].legend() - ax[i].fill_between(timestamp, - estimated_z-3*np.sqrt(cov_z), - estimated_z+3*np.sqrt(cov_z), - alpha=0.3,linewidth=0, color='C5') - ax[i].set_ylabel('z position(m)') - # ax[i].set_xlim([10, 15]) - ax[i].set_xlabel('Time (sec)') - ax[i].grid(True) - - - i+=1 - ax[i].plot(timestamp, err_estimated_pitch, linestyle='solid', color='C1', label="pitch") - ax[i].legend() - ax[i].plot(timestamp, err_estimated_roll, linestyle='solid', color='C2', label="roll") - ax[i].legend() - ax[i].set_ylabel('angles error(deg)') - # ax[i].set_xlim([5, 6]) - # ax[i].set_ylim([-0.0001, 0.0001]) - ax[i].grid(True) - - i+=1 - ax[i].plot(timestamp, err_estimated_yaw, linestyle='dotted', color='C5', label="yaw") - ax[i].legend() - ax[i].set_ylabel('angles error(deg)') - # ax[i].set_xlim([5, 6]) - # ax[i].set_ylim([-0.0001, 0.0001]) - ax[i].grid(True) - - i+=1 - ax[i].plot(timestamp, theta_error, linestyle='solid', color='C1', label="pitch") - ax[i].legend() - ax[i].plot(timestamp, phi_error, linestyle='solid', color='C2', label="roll") - ax[i].legend() - ax[i].set_ylabel('angles error dir cos(deg)') - # ax[i].set_xlim([5, 6]) - # ax[i].set_ylim([-0.0001, 0.0001]) - ax[i].grid(True) - - i+=1 - ax[i].plot(timestamp, psi_error, linestyle='dotted', color='C5', label="yaw") - ax[i].legend() - ax[i].set_ylabel('angles error dir cos(deg)') - # ax[i].set_xlim([5, 6]) - # ax[i].set_ylim([-0.0001, 0.0001]) - ax[i].grid(True) - - i+=1 - ax[i].plot(timestamp, err_estimated_x, linestyle='solid', color='C1', label="x") - ax[i].legend() - ax[i].plot(timestamp, err_estimated_y, linestyle='solid', color='C2', label="y") - ax[i].legend() - ax[i].plot(timestamp, err_estimated_z, linestyle='solid', color='C5', label="z") - ax[i].legend() - ax[i].set_ylabel('position error(m)') - # ax[i].set_xlim([10, 15]) - # ax[i].set_ylim([-0.1, 0.1]) - ax[i].grid(True) - - i+=1 - ax[i].plot(timestamp, true_lin_vel_x, linestyle='solid', color='k', label="true_x") - ax[i].legend() - ax[i].plot(timestamp, esti_lin_vel_x, linestyle='solid', color='C0', label="estimated_x") - ax[i].legend() - ax[i].fill_between(timestamp, - esti_lin_vel_x-3*np.sqrt(cov_u), - esti_lin_vel_x+3*np.sqrt(cov_u), - alpha=0.3,linewidth=0, color='C5') - ax[i].set_ylabel('x lin vel(m/s)') - # ax[i].set_xlim([10, 15]) - # ax[i].set_ylim([-1, 10]) - ax[i].grid(True) - - i+=1 - ax[i].plot(timestamp, true_lin_vel_y, linestyle='solid', color='k', label="true_y") - ax[i].legend() - ax[i].plot(timestamp, esti_lin_vel_y, linestyle='solid', color='C1', label="estimated_y") - ax[i].legend() - ax[i].fill_between(timestamp, - esti_lin_vel_y-3*np.sqrt(cov_v), - esti_lin_vel_y+3*np.sqrt(cov_v), - alpha=0.3,linewidth=0, color='C5') - ax[i].set_ylabel('y lin vel(m/s)') - # ax[i].set_xlim([10, 15]) - # ax[i].set_ylim([-1, 10]) - ax[i].grid(True) - - i+=1 - ax[i].plot(timestamp, true_lin_vel_z, linestyle='solid', color='k', label="true_z") - ax[i].legend() - ax[i].plot(timestamp, esti_lin_vel_z, linestyle='solid', color='C2', label="estimated_z") - ax[i].legend() - ax[i].fill_between(timestamp, - esti_lin_vel_z-3*np.sqrt(cov_w), - esti_lin_vel_z+3*np.sqrt(cov_w), - alpha=0.3,linewidth=0, color='C5') - ax[i].set_ylabel('estimated lin vel(m/s)') - # ax[i].set_xlim([10, 15]) - # ax[i].set_ylim([-1, 10]) - ax[i].grid(True) - - i+=1 - ax[i].plot(timestamp, err_lin_vel_x, linestyle='solid', color='C1', label="x") - ax[i].legend() - ax[i].plot(timestamp, err_lin_vel_y, linestyle='solid', color='C2', label="y") - ax[i].legend() - ax[i].plot(timestamp, err_lin_vel_z, linestyle='solid', color='C5', label="z") - ax[i].legend() - ax[i].set_ylabel('lin vel error(m/s)') - # ax[i].set_xlim([10, 15]) - # ax[i].set_ylim([-0.01, 0.01]) - ax[i].grid(True) - - i+=1 - ax[i].plot(timestamp, quat_norm, linestyle='solid', color='C1', label="norm") - ax[i].legend() - ax[i].set_ylabel('quaternion norm') - # ax[i].set_xlim([10, 15]) - # ax[i].set_xlabel('Time (s)') - ax[i].grid(True) - - i+=1 - ax[i].plot(timestamp, bias_accel_x, linestyle='solid', color='C0', label="accel_x") - ax[i].legend() - ax[i].fill_between(timestamp, - bias_accel_x-3*np.sqrt(cov_b_f_x), - bias_accel_x+3*np.sqrt(cov_b_f_x), - alpha=0.3,linewidth=0, color='C5') - ax[i].set_ylabel('x accel biases mili g') - ax[i].grid(True) - i+=1 - ax[i].plot(timestamp, bias_accel_y, linestyle='solid', color='C1', label="accel_y") - ax[i].legend() - ax[i].fill_between(timestamp, - bias_accel_y-3*np.sqrt(cov_b_f_y), - bias_accel_y+3*np.sqrt(cov_b_f_y), - alpha=0.3,linewidth=0, color='C5') - ax[i].set_ylabel('y accel biases mili g') - ax[i].grid(True) - i+=1 - ax[i].plot(timestamp, bias_accel_z, linestyle='solid', color='C2', label="accel_z") - ax[i].legend() - ax[i].fill_between(timestamp, - bias_accel_z-3*np.sqrt(cov_b_f_z), - bias_accel_z+3*np.sqrt(cov_b_f_z), - alpha=0.3,linewidth=0, color='C5') - ax[i].set_ylabel('z accel biases mili g') - ax[i].grid(True) - - - i+=1 - ax[i].plot(timestamp, bias_gyro_x, linestyle='solid', color='C0', label="gyro_x") - ax[i].legend() - ax[i].fill_between(timestamp, - bias_gyro_x-3*np.sqrt(cov_b_omega_x), - bias_gyro_x+3*np.sqrt(cov_b_omega_x), - alpha=0.3,linewidth=0, color='C5') - ax[i].set_ylabel('x gyro biases deg/s') - ax[i].grid(True) - - i+=1 - ax[i].plot(timestamp, bias_gyro_y, linestyle='solid', color='C1', label="gyro_y") - ax[i].legend() - ax[i].fill_between(timestamp, - bias_gyro_y-3*np.sqrt(cov_b_omega_y), - bias_gyro_y+3*np.sqrt(cov_b_omega_y), - alpha=0.3,linewidth=0, color='C5') - ax[i].set_ylabel('y gyro biases deg/s') - ax[i].grid(True) - - i+=1 - ax[i].plot(timestamp, bias_gyro_z, linestyle='solid', color='C2', label="gyro_z") - ax[i].legend() - ax[i].fill_between(timestamp, - bias_gyro_z-3*np.sqrt(cov_b_omega_z), - bias_gyro_z+3*np.sqrt(cov_b_omega_z), - alpha=0.3,linewidth=0, color='C5') - ax[i].set_ylabel('z gyro biases deg/s') - ax[i].grid(True) - - i+=1 - ax[i].plot(timestamp, bias_baro, linestyle='solid', color='C3', label="baro") - ax[i].legend() - ax[i].fill_between(timestamp, - bias_baro-3*np.sqrt(cov_b_baro), - bias_baro+3*np.sqrt(cov_b_baro), - alpha=0.3,linewidth=0, color='C5') - ax[i].set_ylabel('baro bias m') - ax[i].grid(True) - - i+=1 - ax[i].plot(timestamp, np.sqrt(cov_x), linestyle='solid', color='C0', label="std_dev_x") - ax[i].legend() - ax[i].plot(timestamp, np.sqrt(cov_y), linestyle='dotted', color='C1', label="std_dev_y") - ax[i].legend() - ax[i].plot(timestamp, np.sqrt(cov_z), linestyle='solid', color='C2', label="std_dev_z") - ax[i].legend() - ax[i].set_yscale("log") - ax[i].set_ylabel('std_dev_position(m)') - ax[i].grid(True) - - i+=1 - ax[i].plot(timestamp, np.sqrt(cov_u), linestyle='solid', color='C0', label="std_dev_u") - ax[i].legend() - ax[i].plot(timestamp, np.sqrt(cov_v), linestyle='dotted', color='C1', label="std_dev_v") - ax[i].legend() - ax[i].plot(timestamp, np.sqrt(cov_w), linestyle='solid', color='C2', label="std_dev_w") - ax[i].legend() - ax[i].set_yscale("log") - ax[i].set_ylabel('std_dev_lin_velocity(m/s)') - ax[i].grid(True) - - i+=1 - ax[i].plot(timestamp, np.sqrt(cov_q0), linestyle='solid', color='C3', label="std_dev_q0") - ax[i].legend() - ax[i].legend() - ax[i].set_yscale("log") - ax[i].set_xlabel('Time (s)') - ax[i].set_ylabel('std_dev_orientation') - ax[i].grid(True) - - i+=1 - ax[i].plot(timestamp, np.sqrt(cov_q1), linestyle='solid', color='C0', label="std_dev_q1") - ax[i].legend() - ax[i].plot(timestamp, np.sqrt(cov_q2), linestyle='dotted', color='C1', label="std_dev_q2") - ax[i].legend() - ax[i].plot(timestamp, np.sqrt(cov_q3), linestyle='dotted', color='C2', label="std_dev_q3") - ax[i].legend() - ax[i].set_yscale("log") - ax[i].set_xlabel('Time (s)') - ax[i].set_ylabel('std_dev_orientation') - ax[i].grid(True) - - i+=1 - ax[i].plot(timestamp, np.sqrt(cov_roll), linestyle='solid', color='C0', label="std_dev_roll") - ax[i].legend() - ax[i].plot(timestamp, np.sqrt(cov_pitch), linestyle='solid', color='C1', label="std_dev_pitch") - ax[i].legend() - ax[i].plot(timestamp, np.sqrt(cov_yaw), linestyle='dotted', color='C2', label="std_dev_yaw") - ax[i].legend() - ax[i].set_yscale("log") - ax[i].set_xlabel('Time (s)') - ax[i].set_ylabel('std_dev_angles(deg)') - ax[i].grid(True) - - i+=1 - ax[i].plot(timestamp, np.sqrt(cov_b_f_x), linestyle='solid', color='C0', label="std_dev_b_f_x") - ax[i].legend() - ax[i].plot(timestamp, np.sqrt(cov_b_f_y), linestyle='dotted', color='C1', label="std_dev_b_f_y") - ax[i].legend() - ax[i].plot(timestamp, np.sqrt(cov_b_f_z), linestyle='solid', color='C2', label="std_dev_b_f_z") - ax[i].legend() - ax[i].set_yscale("log") - ax[i].set_ylabel('std_dev_imu_bias(milli g)') - ax[i].grid(True) - - i+=1 - ax[i].plot(timestamp, np.sqrt(cov_b_omega_x), linestyle='solid', color='C0', label="std_dev_b_omega_x") - ax[i].legend() - ax[i].plot(timestamp, np.sqrt(cov_b_omega_y), linestyle='dotted', color='C1', label="std_dev_b_omega_y") - ax[i].legend() - ax[i].plot(timestamp, np.sqrt(cov_b_omega_z), linestyle='solid', color='C2', label="std_dev_b_omega_z") - ax[i].legend() - ax[i].set_yscale("log") - ax[i].set_ylabel('std_dev_gyro_bias(deg/s)') - ax[i].grid(True) - - i+=1 - ax[i].plot(timestamp, np.sqrt(cov_b_baro), linestyle='solid', color='C0', label="std_dev_b_baro") - ax[i].legend() - ax[i].set_yscale("log") - ax[i].set_ylabel('std_dev_baro_bias(m)') - ax[i].grid(True) - - fig.savefig("figure.pdf") - fig.show() - -if __name__ == "__main__": - """ This is executed when run from the command line """ - main() diff --git a/AirLibUnitTests/style.mplstyle b/AirLibUnitTests/style.mplstyle deleted file mode 100644 index e2c3732776..0000000000 --- a/AirLibUnitTests/style.mplstyle +++ /dev/null @@ -1,55 +0,0 @@ -# axes.titlesize : 24 -# axes.labelsize : 20 -# lines.linewidth : 3 -# lines.markersize : 10 -# xtick.labelsize : 16 -# ytick.labelsize : 16 - -# Matplotlib style for scientific plotting -# This is the base style for "SciencePlots" -# see: https://github.com/garrettj403/SciencePlots - -# Set color cycle: blue, green, yellow, red, violet, gray -axes.prop_cycle : cycler('color', ['0C5DA5', '00B945','FF2C00', 'FF9500', '845B97', '474747', '9e9e9e']) - -# Set default figure size -figure.figsize : 5, 100 - -# 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 - -# # Use LaTeX for math formatting -# text.usetex : True -# text.latex.preamble : \usepackage{amsmath} \usepackage{amssymb} \ No newline at end of file From f3633e7e9973650ce090e60a1720e9d0bea505c4 Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Wed, 23 Mar 2022 14:30:15 +0100 Subject: [PATCH 72/87] cleanup more ekf code --- .../simple_flight/AirSimSimpleEkf.hpp | 87 +------------------ .../simple_flight/AirSimSimpleEkfBase.hpp | 5 -- .../simple_flight/AirSimSimpleFlightBoard.hpp | 8 +- .../AirSimSimpleFlightEstimator.hpp | 34 -------- .../firmware/interfaces/IStateEstimator.hpp | 3 - 5 files changed, 5 insertions(+), 132 deletions(-) diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp index 508812f586..8bcf16d0e8 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp @@ -12,10 +12,7 @@ #include "AirSimSimpleEkfBase.hpp" #include "AirSimSimpleEkfModel.hpp" #include "AirSimSimpleEkfParams.hpp" -#include "common/GeodeticConverter.hpp" - -#define AirSimSimpleEkf_GROUND_TRUTH_MEAS_DIRECTIVE 0 -#define AirSimSimpleEkf_PSEUDOMEAS_DIRECTIVE 1 +#include "common/GeodeticConverter.hpp" namespace msr { @@ -235,11 +232,7 @@ namespace airlib if (params_.fuse_gps) { gpsUpdate(); } -#if AirSimSimpleEkf_PSEUDOMEAS_DIRECTIVE == 1 pseudoMeasurement(); -#else -#endif - eulerAnglesCovariancePropagation(); } // state propagtion @@ -598,49 +591,6 @@ namespace airlib error_covariance_ = P_corrected; } - void eulerAnglesCovariancePropagation() - { - // extract the states - float x[simple_flight::NX]; - for (int i=0; i>>>> only for verification, with ground truth 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); - accel[0] = linear_acceleration.x(); - accel[1] = linear_acceleration.y(); - accel[2] = linear_acceleration.z(); - gyro[0] = kinematics_->twist.angular.x(); - gyro[1] = kinematics_->twist.angular.y(); - gyro[2] = kinematics_->twist.angular.z(); - // >>>>> -#else board_->readImuData(accel, gyro); -#endif // check if the signal has all data that is valid, else return false // TODO: check if at least a subset of data is valid @@ -686,14 +619,6 @@ namespace airlib bool getGpsData(double pos[3], real_T vel[3]) { - -#if AirSimSimpleEkf_GROUND_TRUTH_MEAS_DIRECTIVE == 1 - // >>>>> only for verification, with ground truth measurements - pos[0] = kinematics_->pose.position.x(); - pos[1] = kinematics_->pose.position.y(); - pos[2] = kinematics_->pose.position.z(); - // >>>>> -#else double geo[3]; board_->readGpsData(geo, vel); @@ -707,7 +632,6 @@ namespace airlib pos[0] = ned_pos[0]; pos[1] = ned_pos[1]; pos[2] = ned_pos[2]; -#endif // check if the signal has all data that is valid, else return false // TODO: check if at least a subset of data is valid @@ -726,12 +650,8 @@ namespace airlib // reads barometer data bool getBarometerData(real_T* ned_altitude) { -#if AirSimSimpleEkf_GROUND_TRUTH_MEAS_DIRECTIVE == 1 - altitude[0] = -1.0f*kinematics_->pose.position.z(); -#else real_T altitude[1]; board_->readBarometerData(altitude); -#endif // check if the signal has all data that is valid, else return false // TODO: check if at least a subset of data is valid @@ -747,12 +667,7 @@ namespace airlib // reads magnetometer data bool getMagnetometerData(real_T mag[3]) { - -#if AirSimSimpleEkf_GROUND_TRUTH_MEAS_DIRECTIVE == 1 - -#else board_->readMagnetometerData(mag); -#endif // check if the signal has all data that is valid, else return false // TODO: check if at least a subset of data is valid diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfBase.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfBase.hpp index 57a27ed4bf..9f2bf44f28 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfBase.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfBase.hpp @@ -35,10 +35,6 @@ namespace airlib { return error_covariance_; } - virtual const VectorMath::Matrix3x3f& getEkfEulerAnglesCovariance() const override - { - return euler_angles_error_covariance_; - } protected: // setters @@ -52,7 +48,6 @@ namespace airlib simple_flight::VectorNXf states_; simple_flight::MatrixNXxNXf error_covariance_; - VectorMath::Matrix3x3f euler_angles_error_covariance_; VectorMath::Vector17f measurement_ = VectorMath::Vector17f::Zero(); }; diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightBoard.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightBoard.hpp index adc7b50971..aadaf79af6 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightBoard.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightBoard.hpp @@ -198,22 +198,22 @@ namespace airlib imu_ = static_cast(findSensorByName(imu_name, SensorBase::SensorType::Imu)); if (imu_ == nullptr) { - //throw VehicleControllerException(Utils::stringf("No IMU with name %s exist on vehicle", imu_name.c_str())); + //TODO handle error } barometer_ = static_cast(findSensorByName(barometer_name, SensorBase::SensorType::Barometer)); if (barometer_ == nullptr) { - //throw VehicleControllerException(Utils::stringf("No barometer with name %s exist on vehicle", barometer_name.c_str())); + //TODO handle error } magnetometer_ = static_cast(findSensorByName(magnetometer_name, SensorBase::SensorType::Magnetometer)); if (magnetometer_ == nullptr) { - //throw VehicleControllerException(Utils::stringf("No magnetometer with name %s exist on vehicle", magnetometer_name.c_str())); + //TODO handle error } gps_ = static_cast(findSensorByName(gps_name, SensorBase::SensorType::Gps)); if (gps_ == nullptr) { - //throw VehicleControllerException(Utils::stringf("No gps with name %s exist on vehicle", gps_name.c_str())); + //TODO handle error } } diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightEstimator.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightEstimator.hpp index c9d8c41de9..54e58cde7f 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightEstimator.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightEstimator.hpp @@ -413,40 +413,6 @@ namespace airlib return norm; } - virtual std::array getEkfOrientationOffDiagCovariance() const override - { - std::array ori_offdiag_cov; - auto ekf_covariance = ekf_->getEkfCovariance(); - ori_offdiag_cov.at(0) = ekf_covariance(6, 7); - ori_offdiag_cov.at(1) = ekf_covariance(6, 8); - ori_offdiag_cov.at(2) = ekf_covariance(6, 9); - ori_offdiag_cov.at(3) = ekf_covariance(7, 8); - ori_offdiag_cov.at(4) = ekf_covariance(7, 9); - ori_offdiag_cov.at(5) = ekf_covariance(8, 9); - - return ori_offdiag_cov; - } - - virtual std::array getEkfOrientationGyroBiasCovariance() const override - { - std::array ori_gyro_bias_cov; - auto ekf_covariance = ekf_->getEkfCovariance(); - ori_gyro_bias_cov.at(0) = ekf_covariance(6, 13); - ori_gyro_bias_cov.at(1) = ekf_covariance(6, 14); - ori_gyro_bias_cov.at(2) = ekf_covariance(6, 15); - ori_gyro_bias_cov.at(3) = ekf_covariance(7, 13); - ori_gyro_bias_cov.at(4) = ekf_covariance(7, 14); - ori_gyro_bias_cov.at(5) = ekf_covariance(7, 15); - ori_gyro_bias_cov.at(6) = ekf_covariance(8, 13); - ori_gyro_bias_cov.at(7) = ekf_covariance(8, 14); - ori_gyro_bias_cov.at(8) = ekf_covariance(8, 15); - ori_gyro_bias_cov.at(9) = ekf_covariance(9, 13); - ori_gyro_bias_cov.at(10) = ekf_covariance(9, 14); - ori_gyro_bias_cov.at(11) = ekf_covariance(9, 15); - - return ori_gyro_bias_cov; - } - private: const Kinematics::State* kinematics_; const Environment* environment_; 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 7c4c64e7eb..701c47cbe1 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 @@ -42,9 +42,6 @@ class IStateEstimator virtual float getEkfOrientationNorm() const = 0; - virtual std::array getEkfOrientationOffDiagCovariance() const = 0; - virtual std::array getEkfOrientationGyroBiasCovariance() const = 0; - virtual simple_flight::Axis3r getTrueAngles() const = 0; virtual simple_flight::Axis3r getTrueAngularVelocity() const = 0; virtual simple_flight::Axis3r getTruePosition() const = 0; From b19f8d10214403bf5494ae71714516bd8d2bf99c Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Wed, 23 Mar 2022 15:00:46 +0100 Subject: [PATCH 73/87] add euler angle covariance computation and getter --- .../simple_flight/AirSimSimpleEkf.hpp | 44 +++++++++++++++++++ .../simple_flight/AirSimSimpleEkfBase.hpp | 5 +++ 2 files changed, 49 insertions(+) diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp index 8bcf16d0e8..c02623e887 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp @@ -195,6 +195,7 @@ namespace airlib { predictionStep(); measurementUpdateStep(); + eulerAnglesCovariancePropagation(); } // prediction step @@ -591,6 +592,49 @@ namespace airlib error_covariance_ = P_corrected; } + void eulerAnglesCovariancePropagation() + { + // extract the states + float x[simple_flight::NX]; + for (int i=0; i Date: Wed, 23 Mar 2022 15:51:20 +0100 Subject: [PATCH 74/87] cleanup ekf --- .../firmwares/simple_flight/AirSimSimpleEkf.hpp | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp index 508812f586..bf17360c0b 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp @@ -15,7 +15,6 @@ #include "common/GeodeticConverter.hpp" #define AirSimSimpleEkf_GROUND_TRUTH_MEAS_DIRECTIVE 0 -#define AirSimSimpleEkf_PSEUDOMEAS_DIRECTIVE 1 namespace msr { @@ -28,7 +27,7 @@ namespace airlib public: // Constructor AirSimSimpleEkf(const simple_flight::IBoard* board, simple_flight::ICommLink* comm_link, const AirSimSettings::EkfSetting* setting = nullptr) - : board_(board), comm_link_(comm_link) // commlink is only temporary here + : board_(board) { params_.initializeParameters(setting); freq_limiter_.initialize(334); // physics engine and the imu refresh at period 3ms ~ 333.33Hz @@ -198,6 +197,7 @@ namespace airlib { predictionStep(); measurementUpdateStep(); + eulerAnglesCovariancePropagation(); } // prediction step @@ -235,11 +235,7 @@ namespace airlib if (params_.fuse_gps) { gpsUpdate(); } -#if AirSimSimpleEkf_PSEUDOMEAS_DIRECTIVE == 1 pseudoMeasurement(); -#else -#endif - eulerAnglesCovariancePropagation(); } // state propagtion @@ -784,7 +780,6 @@ namespace airlib // --------------------------------------------------------------------- FrequencyLimiter freq_limiter_; const simple_flight::IBoard* board_; - simple_flight::ICommLink* comm_link_; const Kinematics::State* kinematics_; const Environment* environment_; From 5a3b26dcb75e3b10aa3cb5c62772e4046c242c9c Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Wed, 23 Mar 2022 17:51:42 +0100 Subject: [PATCH 75/87] clang formating --- AirLib/include/common/AirSimSettings.hpp | 3 +- AirLib/include/common/VectorMath.hpp | 2 +- AirLib/include/sensors/SensorBase.hpp | 3 +- .../sensors/barometer/BarometerSimple.hpp | 4 +- AirLib/include/sensors/gps/GpsSimple.hpp | 10 +- .../include/sensors/imu/ImuSimpleParams.hpp | 6 +- .../magnetometer/MagnetometerSimple.hpp | 2 +- .../simple_flight/AirSimSimpleEkf.hpp | 312 +++++++------- .../simple_flight/AirSimSimpleEkfBase.hpp | 2 +- .../simple_flight/AirSimSimpleEkfModel.hpp | 384 +++++++++--------- .../simple_flight/AirSimSimpleEkfParams.hpp | 70 ++-- .../simple_flight/AirSimSimpleFlightBoard.hpp | 15 +- .../AirSimSimpleFlightEstimator.hpp | 85 ++-- .../simple_flight/SimpleFlightApi.hpp | 3 +- .../simple_flight/firmware/Firmware.hpp | 3 +- .../simple_flight/firmware/OffboardApi.hpp | 20 +- .../firmware/interfaces/IEkf.hpp | 2 +- .../firmware/interfaces/IStateEstimator.hpp | 3 +- AirLibUnitTests/JacobianTest.hpp | 38 +- AirLibUnitTests/SimpleFlightTest.hpp | 16 +- AirLibUnitTests/main.cpp | 6 +- AirLibUnitTests/settings_json_parser.hpp | 17 +- 22 files changed, 471 insertions(+), 535 deletions(-) diff --git a/AirLib/include/common/AirSimSettings.hpp b/AirLib/include/common/AirSimSettings.hpp index b641213009..71fab2ae3d 100644 --- a/AirLib/include/common/AirSimSettings.hpp +++ b/AirLib/include/common/AirSimSettings.hpp @@ -463,7 +463,6 @@ namespace airlib //this should be done last because it depends on vehicles (and/or their type) we have loadRecordingSetting(settings_json); loadClockSettings(settings_json); - } static void initializeSettings(const std::string& json_settings_text) @@ -1368,7 +1367,7 @@ namespace airlib sensors[p.first] = p.second; } } - + static std::shared_ptr createEkfSettings(bool enabled) { std::shared_ptr ekf_setting; diff --git a/AirLib/include/common/VectorMath.hpp b/AirLib/include/common/VectorMath.hpp index 6901b7af2f..4a983811fa 100644 --- a/AirLib/include/common/VectorMath.hpp +++ b/AirLib/include/common/VectorMath.hpp @@ -37,7 +37,7 @@ 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; diff --git a/AirLib/include/sensors/SensorBase.hpp b/AirLib/include/sensors/SensorBase.hpp index f3a026519a..dd46861d67 100644 --- a/AirLib/include/sensors/SensorBase.hpp +++ b/AirLib/include/sensors/SensorBase.hpp @@ -81,8 +81,7 @@ namespace airlib protected: bool is_new_ = false; - - }; + }; } } //namespace #endif diff --git a/AirLib/include/sensors/barometer/BarometerSimple.hpp b/AirLib/include/sensors/barometer/BarometerSimple.hpp index 9bef833499..28654daa1d 100644 --- a/AirLib/include/sensors/barometer/BarometerSimple.hpp +++ b/AirLib/include/sensors/barometer/BarometerSimple.hpp @@ -63,9 +63,9 @@ namespace airlib delay_line_.update(); - if (freq_limiter_.isWaitComplete()){ + if (freq_limiter_.isWaitComplete()) { setOutput(delay_line_.getOutput()); - + is_new_ = true; } } diff --git a/AirLib/include/sensors/gps/GpsSimple.hpp b/AirLib/include/sensors/gps/GpsSimple.hpp index 38f550b3c7..2c8a60186d 100644 --- a/AirLib/include/sensors/gps/GpsSimple.hpp +++ b/AirLib/include/sensors/gps/GpsSimple.hpp @@ -64,7 +64,7 @@ namespace airlib delay_line_.update(); - if (freq_limiter_.isWaitComplete()){ + if (freq_limiter_.isWaitComplete()) { setOutput(delay_line_.getOutput()); is_new_ = true; @@ -84,12 +84,12 @@ 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 - real_T gps_sigma_pos = 0.00001f; // 1/6378km rad + real_T gps_sigma_pos = 0.00001f; // 1/6378km rad 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.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; diff --git a/AirLib/include/sensors/imu/ImuSimpleParams.hpp b/AirLib/include/sensors/imu/ImuSimpleParams.hpp index 1e218f1224..1495155eef 100644 --- a/AirLib/include/sensors/imu/ImuSimpleParams.hpp +++ b/AirLib/include/sensors/imu/ImuSimpleParams.hpp @@ -75,15 +75,15 @@ namespace airlib 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_PI/180; // deg/s to rad/s + gyro.turn_on_bias.x() = gyro_turn_on_bias_x * M_PI / 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_PI/180; // deg/s to rad/s + gyro.turn_on_bias.y() = gyro_turn_on_bias_y * M_PI / 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_PI/180; // deg/s to rad/s + gyro.turn_on_bias.z() = gyro_turn_on_bias_z * M_PI / 180; // deg/s to rad/s } } }; diff --git a/AirLib/include/sensors/magnetometer/MagnetometerSimple.hpp b/AirLib/include/sensors/magnetometer/MagnetometerSimple.hpp index 568850abaf..6f0d939a2e 100644 --- a/AirLib/include/sensors/magnetometer/MagnetometerSimple.hpp +++ b/AirLib/include/sensors/magnetometer/MagnetometerSimple.hpp @@ -59,7 +59,7 @@ namespace airlib delay_line_.update(); - if (freq_limiter_.isWaitComplete()){ + if (freq_limiter_.isWaitComplete()) { setOutput(delay_line_.getOutput()); is_new_ = true; diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp index bf17360c0b..bdcb621322 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp @@ -12,7 +12,7 @@ #include "AirSimSimpleEkfBase.hpp" #include "AirSimSimpleEkfModel.hpp" #include "AirSimSimpleEkfParams.hpp" -#include "common/GeodeticConverter.hpp" +#include "common/GeodeticConverter.hpp" #define AirSimSimpleEkf_GROUND_TRUTH_MEAS_DIRECTIVE 0 @@ -39,7 +39,6 @@ namespace airlib freq_limiter_.reset(); initializeFilter(); - } virtual void update() override @@ -52,7 +51,6 @@ namespace airlib // if the wait is complete and it is time to update EKF, update EKF if (freq_limiter_.isWaitComplete()) updateEKFInternal(); - } // only in simulation @@ -66,7 +64,7 @@ namespace airlib // --------------------------------------------------------------------- // Internal functions // --------------------------------------------------------------------- - + // initialize filter void initializeFilter() { @@ -79,59 +77,59 @@ namespace airlib { 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(); + 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_(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(); + 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(); + 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; + 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_(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(); @@ -139,36 +137,36 @@ namespace airlib 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) = pow(params_.initial_states_std_err.position.x(), 2); - error_covariance_(1,1) = pow(params_.initial_states_std_err.position.y(), 2); - error_covariance_(2,2) = pow(params_.initial_states_std_err.position.z(), 2); - error_covariance_(3,3) = pow(params_.initial_states_std_err.linear_velocity.x(), 2); - error_covariance_(4,4) = pow(params_.initial_states_std_err.linear_velocity.y(), 2); - error_covariance_(5,5) = pow(params_.initial_states_std_err.linear_velocity.z(), 2); - error_covariance_(6,6) = pow(params_.initial_states_std_err.quaternion.w(), 2); - error_covariance_(7,7) = pow(params_.initial_states_std_err.quaternion.x(), 2); - error_covariance_(8,8) = pow(params_.initial_states_std_err.quaternion.y(), 2); - error_covariance_(9,9) = pow(params_.initial_states_std_err.quaternion.z(), 2); - error_covariance_(10,10) = pow(params_.initial_states_std_err.accel_bias.x(), 2); - error_covariance_(11,11) = pow(params_.initial_states_std_err.accel_bias.y(), 2); - error_covariance_(12,12) = pow(params_.initial_states_std_err.accel_bias.z(), 2); - error_covariance_(13,13) = pow(params_.initial_states_std_err.gyro_bias.x(), 2); - error_covariance_(14,14) = pow(params_.initial_states_std_err.gyro_bias.y(), 2); - error_covariance_(15,15) = pow(params_.initial_states_std_err.gyro_bias.z(), 2); - error_covariance_(16,16) = pow(params_.initial_states_std_err.baro_bias, 2); + error_covariance_(0, 0) = pow(params_.initial_states_std_err.position.x(), 2); + error_covariance_(1, 1) = pow(params_.initial_states_std_err.position.y(), 2); + error_covariance_(2, 2) = pow(params_.initial_states_std_err.position.z(), 2); + error_covariance_(3, 3) = pow(params_.initial_states_std_err.linear_velocity.x(), 2); + error_covariance_(4, 4) = pow(params_.initial_states_std_err.linear_velocity.y(), 2); + error_covariance_(5, 5) = pow(params_.initial_states_std_err.linear_velocity.z(), 2); + error_covariance_(6, 6) = pow(params_.initial_states_std_err.quaternion.w(), 2); + error_covariance_(7, 7) = pow(params_.initial_states_std_err.quaternion.x(), 2); + error_covariance_(8, 8) = pow(params_.initial_states_std_err.quaternion.y(), 2); + error_covariance_(9, 9) = pow(params_.initial_states_std_err.quaternion.z(), 2); + error_covariance_(10, 10) = pow(params_.initial_states_std_err.accel_bias.x(), 2); + error_covariance_(11, 11) = pow(params_.initial_states_std_err.accel_bias.y(), 2); + error_covariance_(12, 12) = pow(params_.initial_states_std_err.accel_bias.z(), 2); + error_covariance_(13, 13) = pow(params_.initial_states_std_err.gyro_bias.x(), 2); + error_covariance_(14, 14) = pow(params_.initial_states_std_err.gyro_bias.y(), 2); + error_covariance_(15, 15) = pow(params_.initial_states_std_err.gyro_bias.z(), 2); + error_covariance_(16, 16) = 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(); + last_times_.cov_propagation = board_->micros(); // reset geo and magnetic global variables - geodetic_converter_.setHome(environment_->getHomeGeoPoint()); + 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(); @@ -206,7 +204,7 @@ namespace airlib // the entire prediction step updates at the frequency of imu update // TODO later state propagation and covariance propagation can be decoupled! - if(!board_->checkImuIfNew()) + if (!board_->checkImuIfNew()) return; real_T accel[3]; @@ -215,7 +213,7 @@ namespace airlib // check if the IMU gives new measurement and it is valid bool is_new_and_valid = getImuData(accel, gyro); - if(!is_new_and_valid){ + if (!is_new_and_valid) { return; } @@ -253,16 +251,16 @@ namespace airlib float uplus[simple_flight::NU]; // extract the current ekf states - for (int i=0; icheckMagnetometerIfNew()) + 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){ + if (!is_valid) { return; } // extract the ekf states float x[17]; - for (int i=0; i<17; i++){ + for (int i = 0; i < 17; i++) { x[i] = states_(i); } @@ -419,26 +403,24 @@ namespace airlib // calculate the Kalman gain matrix simple_flight::MatrixNXxNXf P = error_covariance_; - VectorMath::Matrix3x3f inverse_term = (C_mag*P*C_mag.transpose() + R_mag_).inverse(); + 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; icheckBarometerIfNew()) + if (!board_->checkBarometerIfNew()) return; real_T ned_altitude[1]; @@ -455,14 +437,13 @@ namespace airlib // check if the barometer gives new measurement and it is valid bool is_valid = getBarometerData(ned_altitude); - if(!is_valid) - { + if (!is_valid) { return; } // extract the ekf states float x[simple_flight::NX]; - for (int i=0; icheckGpsIfNew()) + if (!board_->checkGpsIfNew()) return; double pos[3]; @@ -506,14 +487,13 @@ namespace airlib // check if the GPS gives new measurement and it is valid bool is_valid = getGpsData(pos, vel); - if(!is_valid) - { + if (!is_valid) { return; } // extract the ekf states float x[simple_flight::NX]; - for (int i=0; ipose.position.z(); + altitude[0] = -1.0f * kinematics_->pose.position.z(); #else real_T altitude[1]; board_->readBarometerData(altitude); @@ -739,11 +711,11 @@ namespace airlib return true; } - + // reads magnetometer data bool getMagnetometerData(real_T mag[3]) { - + #if AirSimSimpleEkf_GROUND_TRUTH_MEAS_DIRECTIVE == 1 #else @@ -773,7 +745,7 @@ namespace airlib TTimePoint state_propagation; TTimePoint cov_propagation; }; - + private: // --------------------------------------------------------------------- // Class attritubes @@ -786,7 +758,7 @@ namespace airlib GeodeticConverter geodetic_converter_; float earth_mag_[3]; float home_altitude_; - + LastTimes last_times_; ImuDataBuffer prev_imuData_; diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfBase.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfBase.hpp index 57a27ed4bf..25d4f6f547 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfBase.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfBase.hpp @@ -15,7 +15,7 @@ namespace airlib { class AirSimSimpleEkfBase : public simple_flight::IEkf - { + { public: virtual bool checkEkfEnabled() const override { diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfModel.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfModel.hpp index c4fd115447..b514331f17 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfModel.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfModel.hpp @@ -7,8 +7,8 @@ #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 G_0 = EarthUtils::Gravity; // for less than 10000m const gravity is taken constexpr float R_E = EARTH_RADIUS; namespace msr @@ -51,17 +51,11 @@ namespace airlib 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; + 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]) @@ -79,14 +73,14 @@ namespace airlib 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 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); + 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]) @@ -155,17 +149,17 @@ namespace airlib void h_mag() { - // + // } void h_baro() { - // + // } void h_GPS() { - // + // } // --------------------------------------------------------------------- @@ -203,61 +197,61 @@ namespace airlib // 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); + (*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); + (*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); + (*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); + (*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); + (*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); + (*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; + (*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]) @@ -271,60 +265,60 @@ namespace airlib float f_minus[simple_flight::NX]; float df_dx_i_column[simple_flight::NX]; - for (int i=0; i derivative_test_tolerance){ + for (int i = 0; i < 17; i++) { + for (int j = 0; j < 17; j++) { + if ((*A_error)(i, j) > derivative_test_tolerance) { row[k] = i; column[k] = j; k++; } } } - for (int i=0; i<17; i++){ - for (int j=0; j<17; j++){ - if ((*A_error)(i, j) > derivative_test_tolerance){ + for (int i = 0; i < 17; i++) { + for (int j = 0; j < 17; j++) { + if ((*A_error)(i, j) > derivative_test_tolerance) { return false; } } @@ -335,7 +329,7 @@ namespace airlib 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(); + *B_w = simple_flight::MatrixNXxNWf::Zero(); float q0 = x[6]; // quaternions float q1 = x[7]; // quaternions @@ -343,36 +337,36 @@ namespace airlib 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); + (*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; + (*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; + (*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() @@ -383,15 +377,15 @@ namespace airlib 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 = 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 = simple_flight::Matrix6xNXf::Zero(); (*C_gps)(0, 0) = 1.0f; (*C_gps)(1, 1) = 1.0f; (*C_gps)(2, 2) = 1.0f; @@ -411,21 +405,15 @@ namespace airlib 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; + 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(); + *C_mag = simple_flight::Matrix3xNXf::Zero(); float q0 = x[6]; float q1 = x[7]; @@ -437,27 +425,27 @@ namespace airlib 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; + (*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; + (*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; + (*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; + (*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(); + *C_pseudo = simple_flight::Matrix1xNXf::Zero(); float q0 = x[6]; float q1 = x[7]; @@ -465,16 +453,16 @@ namespace airlib 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; + (*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(); + *C_euler = VectorMath::Matrix3x4f::Zero(); float q0 = x[6]; float q1 = x[7]; @@ -482,30 +470,30 @@ namespace airlib 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; + 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; + 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; + 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; @@ -527,25 +515,25 @@ namespace airlib float k2[17] = { 0 }; float k3[17] = { 0 }; float k4[17] = { 0 }; - float x_dot[17] = { 0 }; + float x_dot[17] = { 0 }; float x_temp[17] = { 0 }; - evaluateStateDot(x_dot,x,u); + 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); + 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); + 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); + 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; @@ -556,15 +544,15 @@ namespace airlib { float k1[17] = { 0 }; float k2[17] = { 0 }; - float x_dot[17] = { 0 }; + float x_dot[17] = { 0 }; float x_temp[17] = { 0 }; - evaluateStateDot(x_dot,x,u); + 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); + 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; @@ -572,10 +560,10 @@ namespace airlib } 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 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]; @@ -593,17 +581,17 @@ namespace airlib // 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]; + 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]); + 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]; @@ -615,10 +603,10 @@ namespace airlib // // 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++){ + 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] + 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]); } @@ -642,8 +630,8 @@ namespace airlib 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]); + 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]; @@ -651,11 +639,10 @@ namespace airlib 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]); + 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]; @@ -674,11 +661,10 @@ namespace airlib // 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]) + 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]; + 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]; diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfParams.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfParams.hpp index ed8f445eaa..bfde1d55f3 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfParams.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfParams.hpp @@ -74,42 +74,42 @@ namespace airlib 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; - } + 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; - } + 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; - } + 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; - } + 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; - } + 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; - } + 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; - } + if (!std::isnan(element_z)) { + quaternion.z() = element_z; + } } void initializeParameters(const AirSimSettings::EkfSetting* settings) @@ -120,19 +120,19 @@ namespace airlib 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 + 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 = 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){ + if (settings == nullptr) { return; } const auto& json = settings->settings; @@ -154,7 +154,7 @@ namespace airlib fuse_mag = mag_fusion; } Settings imu_child; - if (json.getChild("Imu", imu_child)){ + if (json.getChild("Imu", imu_child)) { std::array gyro_str = { "GyroErrorStdDevX", "GyroErrorStdDevY", @@ -169,7 +169,7 @@ namespace airlib readVector3r(imu_child, accel_str, accel.std_error); } Settings gps_child; - if (json.getChild("Gps", gps_child)){ + if (json.getChild("Gps", gps_child)) { std::array gps_pos_str = { "PositionErrorStdDevX", "PositionErrorStdDevY", @@ -184,11 +184,11 @@ namespace airlib readVector3r(gps_child, gps_vel_str, gps.std_error_velocity); } Settings baro_child; - if (json.getChild("Barometer", baro_child)){ + if (json.getChild("Barometer", baro_child)) { readRealT(baro_child, "PositionErrorStdDevZ", baro.std_error); } Settings mag_child; - if (json.getChild("Magnetometer", mag_child)){ + if (json.getChild("Magnetometer", mag_child)) { std::array mag_str = { "MagFluxErrorStdDevX", "MagFluxErrorStdDevY", @@ -197,11 +197,11 @@ namespace airlib readVector3r(mag_child, mag_str, mag.std_error); } Settings pseudo_meas_child; - if (json.getChild("PseudoMeasurement", 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)){ + if (json.getChild("InitialStatesStdErr", initial_states_std_err_child)) { std::array pos_str = { "PositionX", "PositionY", @@ -236,7 +236,7 @@ namespace airlib 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)){ + if (json.getChild("InitialStatesErr", initial_states_err_child)) { std::array pos_str = { "PositionX", "PositionY", diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightBoard.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightBoard.hpp index adc7b50971..40c219168f 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightBoard.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightBoard.hpp @@ -190,29 +190,25 @@ namespace airlib return vehicle_params_->getSensors(); } - void setSensors(const std::string& imu_name, + 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) - { + if (imu_ == nullptr) { //throw VehicleControllerException(Utils::stringf("No IMU with name %s exist on vehicle", imu_name.c_str())); } barometer_ = static_cast(findSensorByName(barometer_name, SensorBase::SensorType::Barometer)); - if (barometer_ == nullptr) - { + if (barometer_ == nullptr) { //throw VehicleControllerException(Utils::stringf("No barometer with name %s exist on vehicle", barometer_name.c_str())); } magnetometer_ = static_cast(findSensorByName(magnetometer_name, SensorBase::SensorType::Magnetometer)); - if (magnetometer_ == nullptr) - { + if (magnetometer_ == nullptr) { //throw VehicleControllerException(Utils::stringf("No magnetometer with name %s exist on vehicle", magnetometer_name.c_str())); } gps_ = static_cast(findSensorByName(gps_name, SensorBase::SensorType::Gps)); - if (gps_ == nullptr) - { + if (gps_ == nullptr) { //throw VehicleControllerException(Utils::stringf("No gps with name %s exist on vehicle", gps_name.c_str())); } } @@ -264,7 +260,6 @@ namespace airlib const BarometerBase* barometer_ = nullptr; const MagnetometerBase* magnetometer_ = nullptr; const GpsBase* gps_ = nullptr; - }; } } //namespace diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightEstimator.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightEstimator.hpp index c9d8c41de9..0b6429a1a9 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightEstimator.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleFlightEstimator.hpp @@ -10,7 +10,6 @@ #include "physics/Environment.hpp" #include "common/Common.hpp" - namespace msr { namespace airlib @@ -20,12 +19,11 @@ namespace airlib { public: AirSimSimpleFlightEstimator(simple_flight::IEkf* ekf) - : ekf_(ekf), ekf_enabled_(ekf_->checkEkfEnabled()) + : ekf_(ekf), ekf_enabled_(ekf_->checkEkfEnabled()) { - } - virtual ~AirSimSimpleFlightEstimator(){} + virtual ~AirSimSimpleFlightEstimator() {} //for now we don't do any state estimation and use ground truth (i.e. assume perfect sensors) void setGroundTruthKinematics(const Kinematics::State* kinematics, const Environment* environment) @@ -43,7 +41,8 @@ namespace airlib { if (ekf_enabled_) { return getEkfAngles(); - } else { + } + else { return getTrueAngles(); } } @@ -55,7 +54,8 @@ namespace airlib return AirSimSimpleFlightCommon::toAxis3r(VectorMath::transformToWorldFrame(AirSimSimpleFlightCommon::toVector3r(ekf_measurements.gyro), AirSimSimpleFlightCommon::toQuaternion(getOrientation()), true)); - } else { + } + else { return getTrueAngularVelocity(); } } @@ -64,7 +64,8 @@ namespace airlib { if (ekf_enabled_) { return getEkfPosition(); - } else { + } + else { return getTruePosition(); } } @@ -75,7 +76,8 @@ namespace airlib const Vector3r& vec = AirSimSimpleFlightCommon::toVector3r(world_frame_val); const Vector3r& trans = VectorMath::transformToBodyFrame(vec, AirSimSimpleFlightCommon::toQuaternion(getOrientation())); return AirSimSimpleFlightCommon::toAxis3r(trans); - } else { + } + else { const Vector3r& vec = AirSimSimpleFlightCommon::toVector3r(world_frame_val); const Vector3r& trans = VectorMath::transformToBodyFrame(vec, kinematics_->pose.orientation); return AirSimSimpleFlightCommon::toAxis3r(trans); @@ -86,7 +88,8 @@ namespace airlib { if (ekf_enabled_) { return getEkfLinearVelocity(); - } else { + } + else { return getTrueLinearVelocity(); } } @@ -95,7 +98,8 @@ namespace airlib { if (ekf_enabled_) { return getEkfOrientation(); - } else { + } + else { return getTrueOrientation(); } } @@ -148,7 +152,7 @@ namespace airlib 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(); + true_measurements.baro_altitude = -1.0f * kinematics_->pose.position.z(); return true_measurements; } @@ -253,10 +257,10 @@ namespace airlib 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); + orientation.val4() = ekf_states(6); + orientation.x() = ekf_states(7); + orientation.y() = ekf_states(8); + orientation.z() = ekf_states(9); return orientation; } @@ -333,10 +337,10 @@ namespace airlib { 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); + 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; } @@ -345,9 +349,9 @@ namespace airlib { 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); + 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; } @@ -356,9 +360,9 @@ namespace airlib { 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); + 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; } @@ -367,9 +371,9 @@ namespace airlib { 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); + 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; } @@ -404,10 +408,7 @@ namespace airlib 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 = 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; @@ -431,16 +432,16 @@ namespace airlib { std::array ori_gyro_bias_cov; auto ekf_covariance = ekf_->getEkfCovariance(); - ori_gyro_bias_cov.at(0) = ekf_covariance(6, 13); - ori_gyro_bias_cov.at(1) = ekf_covariance(6, 14); - ori_gyro_bias_cov.at(2) = ekf_covariance(6, 15); - ori_gyro_bias_cov.at(3) = ekf_covariance(7, 13); - ori_gyro_bias_cov.at(4) = ekf_covariance(7, 14); - ori_gyro_bias_cov.at(5) = ekf_covariance(7, 15); - ori_gyro_bias_cov.at(6) = ekf_covariance(8, 13); - ori_gyro_bias_cov.at(7) = ekf_covariance(8, 14); - ori_gyro_bias_cov.at(8) = ekf_covariance(8, 15); - ori_gyro_bias_cov.at(9) = ekf_covariance(9, 13); + ori_gyro_bias_cov.at(0) = ekf_covariance(6, 13); + ori_gyro_bias_cov.at(1) = ekf_covariance(6, 14); + ori_gyro_bias_cov.at(2) = ekf_covariance(6, 15); + ori_gyro_bias_cov.at(3) = ekf_covariance(7, 13); + ori_gyro_bias_cov.at(4) = ekf_covariance(7, 14); + ori_gyro_bias_cov.at(5) = ekf_covariance(7, 15); + ori_gyro_bias_cov.at(6) = ekf_covariance(8, 13); + ori_gyro_bias_cov.at(7) = ekf_covariance(8, 14); + ori_gyro_bias_cov.at(8) = ekf_covariance(8, 15); + ori_gyro_bias_cov.at(9) = ekf_covariance(9, 13); ori_gyro_bias_cov.at(10) = ekf_covariance(9, 14); ori_gyro_bias_cov.at(11) = ekf_covariance(9, 15); diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightApi.hpp index 8013b582cb..fe04785db6 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightApi.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightApi.hpp @@ -41,11 +41,10 @@ namespace airlib //create sim implementations of board and commlink board_.reset(new AirSimSimpleFlightBoard(¶ms_, vehicle_params_)); comm_link_.reset(new AirSimSimpleFlightCommLink()); - + 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(), ekf_.get())); } 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 3bc31c8be7..e414480cf6 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/Firmware.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/Firmware.hpp @@ -21,7 +21,7 @@ class Firmware : public IFirmware { public: 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) + : 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: @@ -78,7 +78,6 @@ class Firmware : public IFirmware board_->writeOutput(motor_index, motor_outputs_.at(motor_index)); comm_link_->update(); - } virtual IOffboardApi& offboardApi() override 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 7a3df46b77..ff143821bc 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/OffboardApi.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/OffboardApi.hpp @@ -99,7 +99,8 @@ class OffboardApi : public IUpdatable if (state_estimator_->checkEkfEnabled()) { message = string("requestApiControl was successful. ") + string("Ekf enabled: ") + string("True"); - } else { + } + else { message = string("requestApiControl was successful. ") + string("Ekf enabled: ") + string("False"); } comm_link_->log(message, ICommLink::kLogLevelInfo); @@ -209,20 +210,19 @@ class OffboardApi : public IUpdatable } private: - void logEkfValues() { // additional logging of vehicle states, TODO implement a separate log! std::ostringstream log_msg; log_msg << clock_->millis() << '\t' - // ground truth mesurement signals + // ground truth mesurement signals << state_estimator_->getTrueMeasurements().accel.x() << '\t' << state_estimator_->getTrueMeasurements().accel.y() << '\t' << state_estimator_->getTrueMeasurements().accel.z() << '\t' << state_estimator_->getTrueMeasurements().gyro.x() << '\t' << state_estimator_->getTrueMeasurements().gyro.y() << '\t' << state_estimator_->getTrueMeasurements().gyro.z() << '\t' - // noisy mesurement signals + // noisy mesurement signals << state_estimator_->getEkfMeasurements().accel.x() << '\t' << state_estimator_->getEkfMeasurements().accel.y() << '\t' << state_estimator_->getEkfMeasurements().accel.z() << '\t' @@ -239,7 +239,7 @@ class OffboardApi : public IUpdatable << state_estimator_->getEkfMeasurements().magnetic_flux.x() << '\t' << state_estimator_->getEkfMeasurements().magnetic_flux.y() << '\t' << state_estimator_->getEkfMeasurements().magnetic_flux.z() << '\t' - // ground truth states + // ground truth states << state_estimator_->getTrueKinematicsEstimated().position.x() << '\t' << state_estimator_->getTrueKinematicsEstimated().position.y() << '\t' << state_estimator_->getTrueKinematicsEstimated().position.z() << '\t' @@ -253,7 +253,7 @@ class OffboardApi : public IUpdatable << state_estimator_->getTrueKinematicsEstimated().linear_velocity.x() << '\t' << state_estimator_->getTrueKinematicsEstimated().linear_velocity.y() << '\t' << state_estimator_->getTrueKinematicsEstimated().linear_velocity.z() << '\t' - // estimated states + // estimated states << state_estimator_->getEkfKinematicsEstimated().position.x() << '\t' << state_estimator_->getEkfKinematicsEstimated().position.y() << '\t' << state_estimator_->getEkfKinematicsEstimated().position.z() << '\t' @@ -274,7 +274,7 @@ class OffboardApi : public IUpdatable << state_estimator_->getEkfKinematicsEstimated().sensor_bias.gyro.y() << '\t' << state_estimator_->getEkfKinematicsEstimated().sensor_bias.gyro.z() << '\t' << state_estimator_->getEkfKinematicsEstimated().sensor_bias.barometer << '\t' - // variances + // variances << state_estimator_->getEkfPositionVariance().x() << '\t' << state_estimator_->getEkfPositionVariance().y() << '\t' << state_estimator_->getEkfPositionVariance().z() << '\t' @@ -295,16 +295,16 @@ class OffboardApi : public IUpdatable << state_estimator_->getEkfGyroBiasVariance().y() << '\t' << state_estimator_->getEkfGyroBiasVariance().z() << '\t' << state_estimator_->getEkfBaroBiasVariance() << '\t' - // quaternion norm + // quaternion norm << state_estimator_->getEkfOrientationNorm() << '\t' - // off-diag quaternion covariance + // off-diag quaternion covariance << state_estimator_->getEkfOrientationOffDiagCovariance().at(0) << '\t' << state_estimator_->getEkfOrientationOffDiagCovariance().at(1) << '\t' << state_estimator_->getEkfOrientationOffDiagCovariance().at(2) << '\t' << state_estimator_->getEkfOrientationOffDiagCovariance().at(3) << '\t' << state_estimator_->getEkfOrientationOffDiagCovariance().at(4) << '\t' << state_estimator_->getEkfOrientationOffDiagCovariance().at(5) << '\t' - // gyro bias quaternion covariance + // gyro bias quaternion covariance << state_estimator_->getEkfOrientationGyroBiasCovariance().at(0) << '\t' << state_estimator_->getEkfOrientationGyroBiasCovariance().at(1) << '\t' << state_estimator_->getEkfOrientationGyroBiasCovariance().at(2) << '\t' 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 index ec58b579fa..aebda5533c 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IEkf.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IEkf.hpp @@ -9,7 +9,7 @@ namespace simple_flight { class IEkf : public IUpdatable -{ +{ public: virtual bool checkEkfEnabled() const = 0; 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 7c4c64e7eb..d54e40d248 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 @@ -39,7 +39,7 @@ class IStateEstimator 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 std::array getEkfOrientationOffDiagCovariance() const = 0; @@ -53,6 +53,5 @@ class IStateEstimator virtual simple_flight::KinematicsState getTrueKinematicsEstimated() const = 0; virtual ~IStateEstimator() = default; - }; } diff --git a/AirLibUnitTests/JacobianTest.hpp b/AirLibUnitTests/JacobianTest.hpp index a2c4934e76..a24e975b22 100644 --- a/AirLibUnitTests/JacobianTest.hpp +++ b/AirLibUnitTests/JacobianTest.hpp @@ -14,19 +14,16 @@ namespace airlib public: virtual void run() override { - float x[17] = {1.0f, 2.0f, 3.0f, - 1.2f, 2.3f, 3.4f, - 0.9f, 0.1f, 0.2f, 0.3f, - 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}; - float u[6] = {0.0f, 0.0f, -9.80665f, 1.0f, 2.0f, 3.0f}; - float u2[6] = {0.0f, 0.0f, -9.80665f, -3.23f, -1.38f, -0.2f}; + float x[17] = { 1.0f, 2.0f, 3.0f, 1.2f, 2.3f, 3.4f, 0.9f, 0.1f, 0.2f, 0.3f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f }; + float u[6] = { 0.0f, 0.0f, -9.80665f, 1.0f, 2.0f, 3.0f }; + float u2[6] = { 0.0f, 0.0f, -9.80665f, -3.23f, -1.38f, -0.2f }; float x_dot[17]; float x_dotdot[17]; float x_dotdotdot[17]; - AirSimSimpleEkfModel::evaluateStateDot(x_dot,x,u); - AirSimSimpleEkfModel::evaluateStateDot(x_dotdot,x_dot,u2); - AirSimSimpleEkfModel::evaluateStateDot(x_dotdotdot,x_dotdot,u2); + AirSimSimpleEkfModel::evaluateStateDot(x_dot, x, u); + AirSimSimpleEkfModel::evaluateStateDot(x_dotdot, x_dot, u2); + AirSimSimpleEkfModel::evaluateStateDot(x_dotdotdot, x_dotdot, u2); float x_dotNew[17]; float x_dotdotNew[17]; @@ -36,15 +33,13 @@ namespace airlib // AirSimSimpleEkfModel::evaluateStateDotOld(x_dotdotNew,x_dotNew,u2); // AirSimSimpleEkfModel::evaluateStateDotOld(x_dotdotdotNew,x_dotdotNew,u2); - for (int i=0; i<17; i++) - { + for (int i = 0; i < 17; i++) { std::cout << x[i] << '\t'; std::cout << x_dot[i] << '\t'; std::cout << x_dotdot[i] << '\t'; std::cout << x_dotdotdot[i] << '\n'; } - for (int i=0; i<17; i++) - { + for (int i = 0; i < 17; i++) { std::cout << x[i] << '\t'; std::cout << x_dotNew[i] << '\t'; std::cout << x_dotdotNew[i] << '\t'; @@ -52,12 +47,12 @@ namespace airlib } simple_flight::MatrixNXxNXf A; simple_flight::MatrixNXxNXf ANew; - AirSimSimpleEkfModel::evaluateA(&A, x_dotdotdot,u2); + AirSimSimpleEkfModel::evaluateA(&A, x_dotdotdot, u2); // AirSimSimpleEkfModel::evaluateAOld(&ANew, x_dotdotdot,u2); - for (int i=0; i<17; i++){ - for (int j=0; j<17; j++){ - std::cout << A(i,j) << "\t "; + for (int i = 0; i < 17; i++) { + for (int j = 0; j < 17; j++) { + std::cout << A(i, j) << "\t "; } std::cout << '\n'; } @@ -70,13 +65,12 @@ namespace airlib // } // QuaternionT q = VectorMath::toQuaternion(RealT pitch, RealT roll, RealT yaw); - Quaternionr q = VectorMath::toQuaternion(0.2*M_PI/180, 0.0*M_PI/180, 0.2*M_PI/180); + Quaternionr q = VectorMath::toQuaternion(0.2 * M_PI / 180, 0.0 * M_PI / 180, 0.2 * M_PI / 180); std::cout << q.w() << '\n'; std::cout << q.x() << '\n'; std::cout << q.y() << '\n'; std::cout << q.z() << '\n'; - // VectorMath::Matrix17x17f A_finite; // VectorMath::Matrix17x17f A; // VectorMath::Matrix17x17f A_error; @@ -86,10 +80,10 @@ namespace airlib // AirSimSimpleEkfModel::evaluateA(&A, x_dotdotdot,u2); // volatile bool isOK; - // volatile float row[17]; + // volatile float row[17]; // volatile float column[17]; // isOK = AirSimSimpleEkfModel::checkA(&A_error, &A, &A_finite, row, column); - + // std::cout << '\n'; // for (int i=0; i<17; i++) // { @@ -117,9 +111,7 @@ namespace airlib // } // std::cout << '\n'; // } - } - }; } } diff --git a/AirLibUnitTests/SimpleFlightTest.hpp b/AirLibUnitTests/SimpleFlightTest.hpp index daa3fc4c49..095186d8ee 100644 --- a/AirLibUnitTests/SimpleFlightTest.hpp +++ b/AirLibUnitTests/SimpleFlightTest.hpp @@ -54,7 +54,7 @@ namespace airlib std::unique_ptr kinematics; std::unique_ptr environment; Kinematics::State initial_kinematic_state = Kinematics::State::zero(); - + initial_kinematic_state.pose = Pose(Vector3r(1.0f, 2.0f, 3.0f), Quaternionr(1, 0, 0, 0)); // states_(6) = 0.9961946f; // q0 // states_(7) = 0.0f; // q1 @@ -75,11 +75,11 @@ namespace airlib environment.reset(new Environment(initial_environment)); // crete and initialize body and physics world - MultiRotorPhysicsBody vehicle(params.get(), api.get(), kinematics.get(), environment.get()); + MultiRotorPhysicsBody vehicle(params.get(), api.get(), kinematics.get(), environment.get()); std::vector vehicles = { &vehicle }; std::unique_ptr physics_engine(new FastPhysicsEngine()); - PhysicsWorld physics_world(std::move(physics_engine), vehicles);//, static_cast(clock->getStepSize() * 1E9)); + PhysicsWorld physics_world(std::move(physics_engine), vehicles); //, static_cast(clock->getStepSize() * 1E9)); // world.startAsyncUpdator(); called in the physics_world constructor // added by Suman, to fix calling update before reset https://github.com/microsoft/AirSim/issues/2773#issuecomment-703888477 @@ -87,7 +87,7 @@ namespace airlib // for the order of the reset see void MultirotorPawnSimApi::resetImplementation() api->setSimulatedGroundTruth(&kinematics.get()->getState(), environment.get()); kinematics->reset(); // sets initial kinematics as current among other things - environment->reset(); + environment->reset(); api->reset(); // set the vehicle as grounded, otherwise can not take off, needs to to be done after physics world construction! @@ -102,7 +102,6 @@ namespace airlib std::string message; testAssert(api->isReady(message), message); - Utils::getSetMinLogLevel(true, 100); std::ostringstream ss; @@ -113,7 +112,7 @@ namespace airlib myfile << ">> Barometer and magnetometer update frequency: 50 Hz.\n"; myfile << ">> GPS update frequency: 50 Hz with startup delay.\n\n";*/ myfile << ">> timestamp (ms) \t GroundTruth altitude \t Estimated postiion (x,y,z) \n\n"; - + // enable api control api->enableApiControl(true); //checkStatusMsg(api.get(), &myfile); @@ -149,7 +148,7 @@ namespace airlib //checkStatusMsg(api.get(), &myfile); clock->sleep_for(10.0f); - api->moveByAngleRatesZ(0.0f, 0.0f, 4.0f*M_PI/180, -1.53509f, 4.0f); + api->moveByAngleRatesZ(0.0f, 0.0f, 4.0f * M_PI / 180, -1.53509f, 4.0f); clock->sleep_for(10.0f); // // // fly towards a waypoint api->moveToPosition(10, 0, -2, 0.5, 1E3, DrivetrainType::MaxDegreeOfFreedom, YawMode(true, 0), -1, 0); @@ -183,12 +182,11 @@ namespace airlib // StateReporter reporter; // kinematics->reportState(reporter); // this writes the kinematics in reporter // std::cout << reporter.getOutput() << std::endl; - + myfile.close(); /*while (true) { }*/ - } private: diff --git a/AirLibUnitTests/main.cpp b/AirLibUnitTests/main.cpp index 2043786768..c2e0e6a5e0 100644 --- a/AirLibUnitTests/main.cpp +++ b/AirLibUnitTests/main.cpp @@ -25,15 +25,15 @@ int main() }; std::chrono::time_point start, end; - + start = std::chrono::system_clock::now(); for (auto& test : tests) test->run(); end = std::chrono::system_clock::now(); - + std::chrono::duration elapsed_seconds = end - start; std::cout << "elapsed time (s): " << elapsed_seconds.count() << "\n"; - std::cout << "elapsed time (min): " << elapsed_seconds.count()/60.0f << "\n"; + std::cout << "elapsed time (min): " << elapsed_seconds.count() / 60.0f << "\n"; return 0; } diff --git a/AirLibUnitTests/settings_json_parser.hpp b/AirLibUnitTests/settings_json_parser.hpp index 27682355b5..f2983be831 100644 --- a/AirLibUnitTests/settings_json_parser.hpp +++ b/AirLibUnitTests/settings_json_parser.hpp @@ -20,7 +20,7 @@ class SettingsLoader initializeSettings(); } - ~SettingsLoader() {}; + ~SettingsLoader(){}; private: std::string getSimMode() @@ -29,24 +29,22 @@ class SettingsLoader return settings_json.getString("SimMode", ""); } - bool readSettingsTextFromFile(std::string settingsFilepath) + bool readSettingsTextFromFile(std::string settingsFilepath) { // check if path exists - bool found = std::ifstream(settingsFilepath.c_str()).good(); - if (found) - { + bool found = std::ifstream(settingsFilepath.c_str()).good(); + if (found) { std::ifstream ifs(settingsFilepath); std::stringstream buffer; buffer << ifs.rdbuf(); - settingsText_ = buffer.str(); + settingsText_ = buffer.str(); } return found; } bool initializeSettings() { - if (readSettingsTextFromFile(msr::airlib::Settings::Settings::getUserDirectoryFullPath("settings.json"))) - { + if (readSettingsTextFromFile(msr::airlib::Settings::Settings::getUserDirectoryFullPath("settings.json"))) { AirSimSettings::initializeSettings(settingsText_); Settings& settings_json = Settings::loadJSonString(settingsText_); @@ -54,8 +52,7 @@ class SettingsLoader AirSimSettings::singleton().load(std::bind(&SettingsLoader::getSimMode, this)); return true; } - else - { + else { return false; } } From aba2f1673ec5464ae47bb5a57c4ba7c06f327ac0 Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Wed, 23 Mar 2022 17:57:24 +0100 Subject: [PATCH 76/87] fix double float conversion warning in sensors --- AirLib/include/sensors/gps/GpsSimple.hpp | 1 - AirLib/include/sensors/imu/ImuSimpleParams.hpp | 6 +++--- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/AirLib/include/sensors/gps/GpsSimple.hpp b/AirLib/include/sensors/gps/GpsSimple.hpp index 2c8a60186d..3075d6ba2c 100644 --- a/AirLib/include/sensors/gps/GpsSimple.hpp +++ b/AirLib/include/sensors/gps/GpsSimple.hpp @@ -86,7 +86,6 @@ namespace airlib output.gnss.geo_point = ground_truth.environment->getState().geo_point; // add Gaussian white noise to the ground truth gps position outputs - real_T gps_sigma_pos = 0.00001f; // 1/6378km rad 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; diff --git a/AirLib/include/sensors/imu/ImuSimpleParams.hpp b/AirLib/include/sensors/imu/ImuSimpleParams.hpp index 1495155eef..42d564d364 100644 --- a/AirLib/include/sensors/imu/ImuSimpleParams.hpp +++ b/AirLib/include/sensors/imu/ImuSimpleParams.hpp @@ -75,15 +75,15 @@ namespace airlib 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_PI / 180; // deg/s to rad/s + 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_PI / 180; // deg/s to rad/s + 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_PI / 180; // deg/s to rad/s + gyro.turn_on_bias.z() = gyro_turn_on_bias_z * M_PIf / 180; // deg/s to rad/s } } }; From 8d47e356d8030fb79eebb1a91ebe013ee325fe5b Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Wed, 23 Mar 2022 18:07:02 +0100 Subject: [PATCH 77/87] clang formating --- AirLib/include/physics/FastPhysicsEngine.hpp | 2 +- AirLib/include/sensors/SensorBase.hpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/AirLib/include/physics/FastPhysicsEngine.hpp b/AirLib/include/physics/FastPhysicsEngine.hpp index 28ff62862d..b2c34d4799 100644 --- a/AirLib/include/physics/FastPhysicsEngine.hpp +++ b/AirLib/include/physics/FastPhysicsEngine.hpp @@ -110,7 +110,7 @@ namespace airlib //TODO: this is now being done in PawnSimApi::update. We need to re-think this sequence //with below commented out - Arducopter GPS may not work. - //IMPORTANT! The following two lines should be commented out while using PawnSimApi!. It syncs environment from kinematics. + //IMPORTANT! The following two lines should be commented out while using PawnSimApi!. It syncs environment from kinematics. body.getEnvironment().setPosition(next.pose.position); body.getEnvironment().update(); } diff --git a/AirLib/include/sensors/SensorBase.hpp b/AirLib/include/sensors/SensorBase.hpp index dd46861d67..ce650ba4a0 100644 --- a/AirLib/include/sensors/SensorBase.hpp +++ b/AirLib/include/sensors/SensorBase.hpp @@ -81,7 +81,7 @@ namespace airlib protected: bool is_new_ = false; - }; + }; } } //namespace #endif From 84fc11bc470273ed1a8d33144aeb85134fdfc455 Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Wed, 23 Mar 2022 19:01:25 +0100 Subject: [PATCH 78/87] fix further double to float warnings --- .../simple_flight/AirSimSimpleEkf.hpp | 42 +++++++++---------- .../simple_flight/AirSimSimpleEkfModel.hpp | 6 +-- 2 files changed, 24 insertions(+), 24 deletions(-) diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp index bdcb621322..0ea8f42ff1 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkf.hpp @@ -140,23 +140,23 @@ namespace airlib // intitialize the ekf covariances error_covariance_ = simple_flight::MatrixNXxNXf::Zero(); - error_covariance_(0, 0) = pow(params_.initial_states_std_err.position.x(), 2); - error_covariance_(1, 1) = pow(params_.initial_states_std_err.position.y(), 2); - error_covariance_(2, 2) = pow(params_.initial_states_std_err.position.z(), 2); - error_covariance_(3, 3) = pow(params_.initial_states_std_err.linear_velocity.x(), 2); - error_covariance_(4, 4) = pow(params_.initial_states_std_err.linear_velocity.y(), 2); - error_covariance_(5, 5) = pow(params_.initial_states_std_err.linear_velocity.z(), 2); - error_covariance_(6, 6) = pow(params_.initial_states_std_err.quaternion.w(), 2); - error_covariance_(7, 7) = pow(params_.initial_states_std_err.quaternion.x(), 2); - error_covariance_(8, 8) = pow(params_.initial_states_std_err.quaternion.y(), 2); - error_covariance_(9, 9) = pow(params_.initial_states_std_err.quaternion.z(), 2); - error_covariance_(10, 10) = pow(params_.initial_states_std_err.accel_bias.x(), 2); - error_covariance_(11, 11) = pow(params_.initial_states_std_err.accel_bias.y(), 2); - error_covariance_(12, 12) = pow(params_.initial_states_std_err.accel_bias.z(), 2); - error_covariance_(13, 13) = pow(params_.initial_states_std_err.gyro_bias.x(), 2); - error_covariance_(14, 14) = pow(params_.initial_states_std_err.gyro_bias.y(), 2); - error_covariance_(15, 15) = pow(params_.initial_states_std_err.gyro_bias.z(), 2); - error_covariance_(16, 16) = pow(params_.initial_states_std_err.baro_bias, 2); + 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() @@ -179,6 +179,7 @@ namespace airlib 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; @@ -245,7 +246,6 @@ namespace airlib last_times_.state_propagation = board_->micros(); // declare local variables - float x_dot[simple_flight::NX]; float x[simple_flight::NX]; float u[simple_flight::NU]; float uplus[simple_flight::NU]; @@ -481,7 +481,7 @@ namespace airlib if (!board_->checkGpsIfNew()) return; - double pos[3]; + float pos[3]; real_T vel[3]; // check if the GPS gives new measurement and it is valid @@ -651,7 +651,7 @@ namespace airlib } // reads GPS data - bool getGpsData(double pos[3], + bool getGpsData(float pos[3], real_T vel[3]) { @@ -669,7 +669,7 @@ namespace airlib Vector3r ned_pos; geo_point.longitude = geo[0]; geo_point.latitude = geo[1]; - geo_point.altitude = geo[2]; + geo_point.altitude = static_cast(geo[2]); geodetic_converter_.geodetic2Ned(geo_point, ned_pos); pos[0] = ned_pos[0]; diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfModel.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfModel.hpp index b514331f17..7fc5a779af 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfModel.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfModel.hpp @@ -287,7 +287,7 @@ namespace airlib } } - static bool checkA(VectorMath::Matrix17x17f* A_error, VectorMath::Matrix17x17f* A, VectorMath::Matrix17x17f* A_finite, volatile float row[17], volatile float column[17]) + 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(); @@ -310,8 +310,8 @@ namespace airlib for (int i = 0; i < 17; i++) { for (int j = 0; j < 17; j++) { if ((*A_error)(i, j) > derivative_test_tolerance) { - row[k] = i; - column[k] = j; + row[k] = static_cast(i); + column[k] = static_cast(j); k++; } } From a861f6aba3d12e0e18e51951f0a25546949ab344 Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Wed, 23 Mar 2022 19:18:56 +0100 Subject: [PATCH 79/87] fix further double to float warnings --- AirLibUnitTests/JacobianTest.hpp | 2 +- AirLibUnitTests/SimpleFlightTest.hpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/AirLibUnitTests/JacobianTest.hpp b/AirLibUnitTests/JacobianTest.hpp index a24e975b22..5cd0caa4aa 100644 --- a/AirLibUnitTests/JacobianTest.hpp +++ b/AirLibUnitTests/JacobianTest.hpp @@ -65,7 +65,7 @@ namespace airlib // } // QuaternionT q = VectorMath::toQuaternion(RealT pitch, RealT roll, RealT yaw); - Quaternionr q = VectorMath::toQuaternion(0.2 * M_PI / 180, 0.0 * M_PI / 180, 0.2 * M_PI / 180); + Quaternionr q = VectorMath::toQuaternion(0.2 * M_PIf / 180, 0.0 * M_PIf / 180, 0.2 * M_PIf / 180); std::cout << q.w() << '\n'; std::cout << q.x() << '\n'; std::cout << q.y() << '\n'; diff --git a/AirLibUnitTests/SimpleFlightTest.hpp b/AirLibUnitTests/SimpleFlightTest.hpp index 095186d8ee..2fa15267b3 100644 --- a/AirLibUnitTests/SimpleFlightTest.hpp +++ b/AirLibUnitTests/SimpleFlightTest.hpp @@ -70,7 +70,7 @@ namespace airlib Environment::State initial_environment; initial_environment.position = initial_kinematic_state.pose.position; - initial_environment.geo_point = GeoPoint(47.6415, -122.14, 121.173); // this becomes the home geo point with which ned to geo conversion takes place + initial_environment.geo_point = GeoPoint(47.6415, -122.14, 121.173f); // this becomes the home geo point with which ned to geo conversion takes place // initial_environment.geo_point.altitude = 0.0f; // do not set it equal to kinematics z, this value goes into home geo point and acts as the ref for kinematics z environment.reset(new Environment(initial_environment)); @@ -148,7 +148,7 @@ namespace airlib //checkStatusMsg(api.get(), &myfile); clock->sleep_for(10.0f); - api->moveByAngleRatesZ(0.0f, 0.0f, 4.0f * M_PI / 180, -1.53509f, 4.0f); + api->moveByAngleRatesZ(0.0f, 0.0f, 4.0f * M_PIf / 180, -1.53509f, 4.0f); clock->sleep_for(10.0f); // // // fly towards a waypoint api->moveToPosition(10, 0, -2, 0.5, 1E3, DrivetrainType::MaxDegreeOfFreedom, YawMode(true, 0), -1, 0); From dc008fe3241103e3ee04774209374a06e08aec06 Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Wed, 23 Mar 2022 20:04:35 +0100 Subject: [PATCH 80/87] fix further double to float warning --- AirLibUnitTests/JacobianTest.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/AirLibUnitTests/JacobianTest.hpp b/AirLibUnitTests/JacobianTest.hpp index 5cd0caa4aa..97f817c228 100644 --- a/AirLibUnitTests/JacobianTest.hpp +++ b/AirLibUnitTests/JacobianTest.hpp @@ -65,7 +65,7 @@ namespace airlib // } // QuaternionT q = VectorMath::toQuaternion(RealT pitch, RealT roll, RealT yaw); - Quaternionr q = VectorMath::toQuaternion(0.2 * M_PIf / 180, 0.0 * M_PIf / 180, 0.2 * M_PIf / 180); + Quaternionr q = VectorMath::toQuaternion(0.2f * M_PIf / 180.0f, 0.0f * M_PIf / 180.0f, 0.2f * M_PIf / 180.0f); std::cout << q.w() << '\n'; std::cout << q.x() << '\n'; std::cout << q.y() << '\n'; From 5654d19c616f162ca9e3039cd3fa7dc82c2ee2ec Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Wed, 23 Mar 2022 20:29:42 +0100 Subject: [PATCH 81/87] clean airsimunittest --- AirLibUnitTests/main.cpp | 8 -------- 1 file changed, 8 deletions(-) diff --git a/AirLibUnitTests/main.cpp b/AirLibUnitTests/main.cpp index c2e0e6a5e0..5b84fb0fdc 100644 --- a/AirLibUnitTests/main.cpp +++ b/AirLibUnitTests/main.cpp @@ -24,16 +24,8 @@ int main() //std::unique_ptr(new WorkerThreadTest()) }; - std::chrono::time_point start, end; - - start = std::chrono::system_clock::now(); for (auto& test : tests) test->run(); - end = std::chrono::system_clock::now(); - - std::chrono::duration elapsed_seconds = end - start; - std::cout << "elapsed time (s): " << elapsed_seconds.count() << "\n"; - std::cout << "elapsed time (min): " << elapsed_seconds.count() / 60.0f << "\n"; return 0; } From 1ef34beb539c00e1f4922d1c7e94a173eb2d1e4c Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Wed, 23 Mar 2022 20:56:03 +0100 Subject: [PATCH 82/87] clean airsimunittest --- AirLibUnitTests/main.cpp | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/AirLibUnitTests/main.cpp b/AirLibUnitTests/main.cpp index 5b84fb0fdc..e0c2f4d6f4 100644 --- a/AirLibUnitTests/main.cpp +++ b/AirLibUnitTests/main.cpp @@ -5,20 +5,16 @@ #include "WorkerThreadTest.hpp" #include "QuaternionTest.hpp" #include "CelestialTests.hpp" -#include "JacobianTest.hpp" - -#include int main() { using namespace msr::airlib; std::unique_ptr tests[] = { - //std::unique_ptr(new QuaternionTest()), - // std::unique_ptr(new CelestialTest()), - //std::unique_ptr(new SettingsTest()), + std::unique_ptr(new QuaternionTest()), + std::unique_ptr(new CelestialTest()), + std::unique_ptr(new SettingsTest()), std::unique_ptr(new SimpleFlightTest()) - // std::unique_ptr(new JacobianTest()) //, //std::unique_ptr(new PixhawkTest()), //std::unique_ptr(new WorkerThreadTest()) @@ -28,4 +24,4 @@ int main() test->run(); return 0; -} +} \ No newline at end of file From 0a8ddc7541ce6c4158299ab039c31ebf97f48583 Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Wed, 23 Mar 2022 21:47:34 +0100 Subject: [PATCH 83/87] remove jacobian test --- AirLibUnitTests/JacobianTest.hpp | 118 ----------------------- AirLibUnitTests/main.cpp | 2 +- AirLibUnitTests/settings_json_parser.hpp | 62 ------------ 3 files changed, 1 insertion(+), 181 deletions(-) delete mode 100644 AirLibUnitTests/JacobianTest.hpp delete mode 100644 AirLibUnitTests/settings_json_parser.hpp diff --git a/AirLibUnitTests/JacobianTest.hpp b/AirLibUnitTests/JacobianTest.hpp deleted file mode 100644 index 97f817c228..0000000000 --- a/AirLibUnitTests/JacobianTest.hpp +++ /dev/null @@ -1,118 +0,0 @@ - -#ifndef Jacobian_Test_hpp -#define Jacobian_Test_hpp - -#include "TestBase.hpp" -#include "vehicles/multirotor/firmwares/simple_flight/AirSimSimpleEkfModel.hpp" - -namespace msr -{ -namespace airlib -{ - class JacobianTest : public TestBase - { - public: - virtual void run() override - { - float x[17] = { 1.0f, 2.0f, 3.0f, 1.2f, 2.3f, 3.4f, 0.9f, 0.1f, 0.2f, 0.3f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f }; - float u[6] = { 0.0f, 0.0f, -9.80665f, 1.0f, 2.0f, 3.0f }; - float u2[6] = { 0.0f, 0.0f, -9.80665f, -3.23f, -1.38f, -0.2f }; - float x_dot[17]; - float x_dotdot[17]; - float x_dotdotdot[17]; - - AirSimSimpleEkfModel::evaluateStateDot(x_dot, x, u); - AirSimSimpleEkfModel::evaluateStateDot(x_dotdot, x_dot, u2); - AirSimSimpleEkfModel::evaluateStateDot(x_dotdotdot, x_dotdot, u2); - - float x_dotNew[17]; - float x_dotdotNew[17]; - float x_dotdotdotNew[17]; - - // AirSimSimpleEkfModel::evaluateStateDotOld(x_dotNew,x,u); - // AirSimSimpleEkfModel::evaluateStateDotOld(x_dotdotNew,x_dotNew,u2); - // AirSimSimpleEkfModel::evaluateStateDotOld(x_dotdotdotNew,x_dotdotNew,u2); - - for (int i = 0; i < 17; i++) { - std::cout << x[i] << '\t'; - std::cout << x_dot[i] << '\t'; - std::cout << x_dotdot[i] << '\t'; - std::cout << x_dotdotdot[i] << '\n'; - } - for (int i = 0; i < 17; i++) { - std::cout << x[i] << '\t'; - std::cout << x_dotNew[i] << '\t'; - std::cout << x_dotdotNew[i] << '\t'; - std::cout << x_dotdotdotNew[i] << '\n'; - } - simple_flight::MatrixNXxNXf A; - simple_flight::MatrixNXxNXf ANew; - AirSimSimpleEkfModel::evaluateA(&A, x_dotdotdot, u2); - // AirSimSimpleEkfModel::evaluateAOld(&ANew, x_dotdotdot,u2); - - for (int i = 0; i < 17; i++) { - for (int j = 0; j < 17; j++) { - std::cout << A(i, j) << "\t "; - } - std::cout << '\n'; - } - - // for (int i=0; i<17; i++){ - // for (int j=0; j<17; j++){ - // std::cout << ANew(i,j) << "\t "; - // } - // std::cout << '\n'; - // } - - // QuaternionT q = VectorMath::toQuaternion(RealT pitch, RealT roll, RealT yaw); - Quaternionr q = VectorMath::toQuaternion(0.2f * M_PIf / 180.0f, 0.0f * M_PIf / 180.0f, 0.2f * M_PIf / 180.0f); - std::cout << q.w() << '\n'; - std::cout << q.x() << '\n'; - std::cout << q.y() << '\n'; - std::cout << q.z() << '\n'; - - // VectorMath::Matrix17x17f A_finite; - // VectorMath::Matrix17x17f A; - // VectorMath::Matrix17x17f A_error; - - // // evaluateA(&A, x, u); - // AirSimSimpleEkfModel::evaluateFiniteDifferenceA(&A_finite, x_dotdotdot,u2); - // AirSimSimpleEkfModel::evaluateA(&A, x_dotdotdot,u2); - - // volatile bool isOK; - // volatile float row[17]; - // volatile float column[17]; - // isOK = AirSimSimpleEkfModel::checkA(&A_error, &A, &A_finite, row, column); - - // std::cout << '\n'; - // for (int i=0; i<17; i++) - // { - // for (int j=0; j<17; j++) - // { - // std::cout << A_finite(i,j) << "\t "; - // } - // std::cout << '\n'; - // } - // std::cout << '\n'; - // for (int i=0; i<17; i++) - // { - // for (int j=0; j<17; j++) - // { - // std::cout << A(i,j) << "\t "; - // } - // std::cout << '\n'; - // } - // std::cout << '\n'; - // for (int i=0; i<17; i++) - // { - // for (int j=0; j<17; j++) - // { - // std::cout << A_error(i,j) << "\t "; - // } - // std::cout << '\n'; - // } - } - }; -} -} -#endif \ No newline at end of file diff --git a/AirLibUnitTests/main.cpp b/AirLibUnitTests/main.cpp index e0c2f4d6f4..5781050308 100644 --- a/AirLibUnitTests/main.cpp +++ b/AirLibUnitTests/main.cpp @@ -24,4 +24,4 @@ int main() test->run(); return 0; -} \ No newline at end of file +} diff --git a/AirLibUnitTests/settings_json_parser.hpp b/AirLibUnitTests/settings_json_parser.hpp deleted file mode 100644 index f2983be831..0000000000 --- a/AirLibUnitTests/settings_json_parser.hpp +++ /dev/null @@ -1,62 +0,0 @@ -#include "common/common_utils/StrictMode.hpp" -STRICT_MODE_OFF -#ifndef RPCLIB_MSGPACK -#define RPCLIB_MSGPACK clmdep_msgpack -#endif // !RPCLIB_MSGPACK -// #include "rpc/rpc_error.h" -STRICT_MODE_ON - -#include "vehicles/multirotor/api/MultirotorRpcLibClient.hpp" -#include "common/common_utils/FileSystem.hpp" -#include -#include -#include "common/AirSimSettings.hpp" - -class SettingsLoader -{ -public: - SettingsLoader() - { - initializeSettings(); - } - - ~SettingsLoader(){}; - -private: - std::string getSimMode() - { - Settings& settings_json = Settings::loadJSonString(settingsText_); - return settings_json.getString("SimMode", ""); - } - - bool readSettingsTextFromFile(std::string settingsFilepath) - { - // check if path exists - bool found = std::ifstream(settingsFilepath.c_str()).good(); - if (found) { - std::ifstream ifs(settingsFilepath); - std::stringstream buffer; - buffer << ifs.rdbuf(); - settingsText_ = buffer.str(); - } - return found; - } - - bool initializeSettings() - { - if (readSettingsTextFromFile(msr::airlib::Settings::Settings::getUserDirectoryFullPath("settings.json"))) { - AirSimSettings::initializeSettings(settingsText_); - - Settings& settings_json = Settings::loadJSonString(settingsText_); - std::string simmode_name = settings_json.getString("SimMode", ""); - AirSimSettings::singleton().load(std::bind(&SettingsLoader::getSimMode, this)); - return true; - } - else { - return false; - } - } - -private: - std::string settingsText_; -}; \ No newline at end of file From 19a541c141f4ed2c4d508bbe12b35de2bb6c0a2c Mon Sep 17 00:00:00 2001 From: Subedi Suman Date: Fri, 25 Mar 2022 13:25:10 +0100 Subject: [PATCH 84/87] add ekf data as line in RecordFileLine --- AirLib/include/common/EkfStructs.hpp | 46 ++++++++++++ .../multirotor/api/MultirotorApiBase.hpp | 1 + .../AirSimSimpleFlightCommon.hpp | 36 +++++++++ .../simple_flight/SimpleFlightApi.hpp | 40 ++++++++++ .../Multirotor/MultirotorPawnSimApi.cpp | 74 +++++++++++++++++++ .../Multirotor/MultirotorPawnSimApi.h | 2 + 6 files changed, 199 insertions(+) create mode 100644 AirLib/include/common/EkfStructs.hpp 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/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/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/SimpleFlightApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightApi.hpp index fe04785db6..bd172c0050 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightApi.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightApi.hpp @@ -10,6 +10,7 @@ #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" @@ -390,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) 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_; From a5e0e794f5790b7a3e87896ed4df4774f5835865 Mon Sep 17 00:00:00 2001 From: subedisuman Date: Mon, 4 Apr 2022 13:45:10 +0200 Subject: [PATCH 85/87] add vehicleresetapi --- AirLib/src/api/RpcLibServerBase.cpp | 4 ++++ PythonClient/airsim/client.py | 9 +++++++++ 2 files changed, 13 insertions(+) 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/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 From 2a27eb397a434d66a8392396226efeec1ce9a839 Mon Sep 17 00:00:00 2001 From: subedisuman Date: Tue, 19 Apr 2022 15:57:09 +0200 Subject: [PATCH 86/87] add python client scripts to run and plot ekf --- PythonClient/multirotor/hello_drone_ekf.py | 29 ++ PythonClient/multirotor/plot.py | 334 +++++++++++++++++++++ PythonClient/multirotor/style.mplstyle | 38 +++ 3 files changed, 401 insertions(+) create mode 100644 PythonClient/multirotor/hello_drone_ekf.py create mode 100644 PythonClient/multirotor/plot.py create mode 100644 PythonClient/multirotor/style.mplstyle 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..d4359ab9b2 --- /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 From 5b82314d8f5ad95f26ad6d9aa83ba86da8a25f95 Mon Sep 17 00:00:00 2001 From: subedisuman Date: Tue, 19 Apr 2022 16:24:48 +0200 Subject: [PATCH 87/87] fix the plot.py doc string --- PythonClient/multirotor/plot.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/PythonClient/multirotor/plot.py b/PythonClient/multirotor/plot.py index d4359ab9b2..2b9585c144 100644 --- a/PythonClient/multirotor/plot.py +++ b/PythonClient/multirotor/plot.py @@ -2,7 +2,7 @@ Module to plot the recorded drone data. Args: - --recording= + --recording= e.g. --recording=2022-04-19-14-57-09 """