├── .gitignore ├── Makefile ├── GYRO.h ├── GYRO.cpp ├── README.md ├── util.h ├── Captor.cpp ├── MAGNETOMETER.h ├── Captor.h ├── GPS.h ├── GPS_Filter.h ├── MAGNETOMETER.cpp ├── ACCELEROMETER.h ├── GPS_Filter.cpp ├── GPS.cpp ├── ACCELEROMETER.cpp ├── EKF.h ├── main.cpp └── EKF.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | 2 | main 3 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | # Makefile for INS_EKF 2 | 3 | 4 | .PHONY: clean 5 | .SUFFIXES: 6 | 7 | # Définitions des variables 8 | CC = gcc 9 | CFLAGS = -v 10 | CXX = g++ 11 | CXXFLAGS = -g 12 | 13 | 14 | # Compilation 15 | # 16 | main: EKF.o Captor.o ACCELEROMETER.o GYRO.o GPS_Filter.o GPS.o MAGNETOMETER.o main.o 17 | $(CXX) $^ -o main $(CXXFLAGS) 18 | 19 | %.o: %.cpp 20 | $(CXX) -c $< -o $@ $(CXXFLAGS) 21 | 22 | clean : 23 | rm -rf *.o 24 | 25 | #Test 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /GYRO.h: -------------------------------------------------------------------------------- 1 | // 2 | // GYRO.h 3 | // INS_EKF 4 | // 5 | // Created by Louis Faury on 12/07/2015. 6 | // Copyright (c) 2015 Louis Faury. All rights reserved. 7 | // 8 | 9 | #ifndef __INS_EKF__GYRO__ 10 | #define __INS_EKF__GYRO__ 11 | 12 | #include 13 | #include "Captor.h" 14 | 15 | class GYRO : public Captor { 16 | public: 17 | 18 | GYRO(); 19 | 20 | GYRO(const std::string, STYLE); 21 | 22 | 23 | }; 24 | 25 | #endif /* defined(__INS_EKF__GYRO__) */ 26 | -------------------------------------------------------------------------------- /GYRO.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // GYRO.cpp 3 | // INS_EKF 4 | // 5 | // Created by Louis Faury on 12/07/2015. 6 | // Copyright (c) 2015 Louis Faury. All rights reserved. 7 | // 8 | 9 | #include 10 | #include "GYRO.h" 11 | 12 | 13 | GYRO::GYRO(){ 14 | } 15 | 16 | GYRO::GYRO(const std::string gyro_file_name, STYLE style) : Captor(gyro_file_name){ 17 | std::cout << "Gyro file open" << std::endl; 18 | captor_id = "GYRO"; 19 | LINE_MARK = "$G"; 20 | OFFSET_MARK = "$GO"; 21 | 22 | if (style != COLD_START){ 23 | // TODO 24 | } 25 | } 26 | 27 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # INS_EKF 2 | 3 | 4 | Program reading from log files implementing an extended Kalman filter for the inertial navigation. 5 | 6 | To compile, you'll need to install the Eigen library. Simply go on their website to download it and then run : 7 | 8 | sudo -s 9 | mv Eigen/ /usr/include 10 | 11 | 12 | You'll then simply need to run : 13 | 14 | make && make clean 15 | 16 | The 'make clean' command being optional. 17 | 18 | You'll need to change the path to the log you want to treat, log that would have been stored thanks to the APM_READER programm. 19 | 20 | -------------------------------------------------------------------------------- /util.h: -------------------------------------------------------------------------------- 1 | // 2 | // util.h 3 | // INS_EKF 4 | // 5 | // Created by Louis Faury on 05/08/2015. 6 | // Copyright (c) 2015 Louis Faury. All rights reserved. 7 | // 8 | 9 | #ifndef INS_EKF_util_h 10 | #define INS_EKF_util_h 11 | 12 | #include 13 | #include 14 | 15 | 16 | #define PI 3.14159265 17 | #define SPQ_DECLINATION_RAD 0.0052 18 | #define HMC5843L_GAIN 660; 19 | #define TODEG 57.295791 20 | #define RT 6371000 21 | #define GRAVITY_INT_PARIS 9.80665 22 | 23 | #define ADCALIBRATION // To compile all the code used for acc advanced calibration 24 | // which take some non-negligable space (this is why we use pre-processors here) 25 | 26 | 27 | typedef Eigen::Matrix Matrix6f; 28 | typedef Eigen::Matrix Vector6f; 29 | typedef Eigen::Matrix Matrix6_10d; 30 | typedef Eigen::Matrix Vector16f; 31 | typedef Eigen::Matrix Vector9f; 32 | typedef Eigen::Matrix Matrix16f; 33 | 34 | 35 | 36 | #endif 37 | -------------------------------------------------------------------------------- /Captor.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Captors.cpp 3 | // INS_EKF 4 | // 5 | // Created by Louis Faury on 12/07/2015. 6 | // Copyright (c) 2015 Louis Faury. All rights reserved. 7 | // 8 | 9 | #include "Captor.h" 10 | 11 | 12 | Captor::Captor(){}; 13 | 14 | 15 | Captor::Captor(const std::string cap_file_name) throw(std::exception) : captor_file(cap_file_name) 16 | { 17 | if (!captor_file) throw std::domain_error("Couldn't open file"); 18 | } 19 | 20 | 21 | 22 | int Captor::initOffsets(){ 23 | int count = 0; 24 | std::string::size_type sz; 25 | if (std::getline(captor_file,line)){ 26 | if (line[2] == 'O'){ 27 | std::stringstream ss(line); 28 | while (getline(ss,line,',')){ 29 | if (line == OFFSET_MARK); 30 | else { 31 | captor_offsets[count] = std::stof(line,&sz); 32 | count++; 33 | } 34 | } 35 | //std::cout << captor_offsets.transpose() << std::endl; 36 | return 1; 37 | } 38 | else return 0; 39 | } 40 | return -1; 41 | } 42 | 43 | 44 | 45 | Eigen::Vector3f Captor::getOffsets() const{ 46 | return captor_offsets; 47 | } 48 | 49 | 50 | 51 | int Captor::initGain(){ 52 | captor_gain << 1,1,1; 53 | return 1; 54 | } 55 | 56 | 57 | 58 | void Captor::getOutput(Eigen::Vector3f* captor_vector_buffer){ 59 | int count = 0; 60 | std::string::size_type sz; 61 | if (std::getline(captor_file,line)){ 62 | line_end = false; 63 | std::stringstream ss(line); 64 | while (getline(ss,line,',')){ 65 | if (line == LINE_MARK); 66 | else { 67 | (*captor_vector_buffer)[count] = std::stof(line,&sz); 68 | count++; 69 | } 70 | } 71 | std::cout << captor_id << " : line read" << std::endl; 72 | } 73 | else{ 74 | std::cout << "Couldn't read from captor : " << captor_id << std::endl; 75 | line_end = true; 76 | } 77 | } 78 | 79 | 80 | 81 | void Captor::correctOutput(Eigen::Vector3f* captor_buffer){ 82 | (*captor_buffer) -= captor_offsets; 83 | for (int i=0; i<3; ++i){ 84 | (*captor_buffer)(i) /= captor_gain(i); 85 | } 86 | } 87 | 88 | 89 | 90 | 91 | Captor::~Captor(){ 92 | } 93 | 94 | 95 | 96 | 97 | 98 | 99 | -------------------------------------------------------------------------------- /MAGNETOMETER.h: -------------------------------------------------------------------------------- 1 | // 2 | // MAGNETOMETER.h 3 | // INS_EKF 4 | // 5 | // Created by Louis Faury on 12/07/2015. 6 | // Copyright (c) 2015 Louis Faury. All rights reserved. 7 | // 8 | 9 | #ifndef __INS_EKF__MAGNETOMETER__ 10 | #define __INS_EKF__MAGNETOMETER__ 11 | 12 | #include 13 | #include "Captor.h" 14 | #include "util.h" 15 | 16 | class MAGNETOMETER : public Captor{ 17 | 18 | 19 | public: 20 | 21 | MAGNETOMETER(); 22 | 23 | 24 | MAGNETOMETER(const std::string, STYLE); 25 | 26 | 27 | 28 | /* \brief Returns the inertial unit heading in RAD 29 | * \param Roll and Pitch angle in RAD 30 | */ 31 | double getHeading(float,float) const; 32 | 33 | 34 | 35 | /* \brief Returns the inertial unit heading assuming that the IMU in on even surface 36 | * \brief In RAD 37 | */ 38 | double getHeading() const; 39 | 40 | 41 | 42 | /* \brief Update magnetometer 43 | */ 44 | void update(); 45 | 46 | 47 | /* \brief Initialize _earth_magnetic_field_even 48 | */ 49 | void _init_earth_even_magnetic_field(); 50 | 51 | 52 | /* \brief Returns _earth_even_magnetic_field as a Vector3d 53 | */ 54 | Eigen::Vector3d getEEMF(); 55 | 56 | 57 | /* \brief Correct magnetometer (offsets) 58 | */ 59 | void correct(); 60 | 61 | 62 | /* \brief Update and correct magnetometer (offsets) 63 | */ 64 | void update_correct(); 65 | 66 | 67 | /* \brief Print the magnetometer state (magx,magy,magz) 68 | */ 69 | void printState() const; 70 | 71 | 72 | /* \brief Returns the state (mag_x,mag_y,mag_z) 73 | */ 74 | Eigen::Vector3d getState() const; 75 | 76 | 77 | 78 | /* \brief Returns the magnetometer noises' covariance matrix 79 | * which is initalised when constructing MAGNETOMETER mag. 80 | */ 81 | Eigen::Matrix3f getMagCovarianceMatrix(); 82 | 83 | 84 | private: 85 | 86 | double mag_x, mag_y, mag_z; 87 | double _declination; // Declination angle with the North in RAD. 88 | // Need to be specified in preprocessor and modified in the initialiser 89 | Eigen::Vector3d _earth_even_magnetic_field; // The magnetic field measured by the magnetometer when on even surface 90 | // Is initialized when the first line is read 91 | 92 | Eigen::Matrix3f R; // Noise covariance matrix 93 | }; 94 | 95 | #endif /* defined(__INS_EKF__MAGNETOMETER__) */ 96 | -------------------------------------------------------------------------------- /Captor.h: -------------------------------------------------------------------------------- 1 | // 2 | // Captors.h 3 | // INS_EKF 4 | // 5 | // Created by Louis Faury on 12/07/2015. 6 | // Copyright (c) 2015 Louis Faury. All rights reserved. 7 | // 8 | 9 | #ifndef __INS_EKF__Captor__ 10 | #define __INS_EKF__Captor__ 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include "GPS.h" 20 | 21 | 22 | 23 | class Captor{ 24 | 25 | public: 26 | 27 | // Basic constructor of the class 28 | Captor(); 29 | 30 | 31 | // Name for mark syle, see the captors constructors 32 | enum STYLE{ 33 | COLD_START, 34 | CALIBRATED 35 | }; 36 | 37 | 38 | /* Basic constructor of the class 39 | * \brief Instanciate a stream to a file given the file path name 40 | * \param The file path name 41 | */ 42 | Captor(const std::string) throw(std::exception); 43 | 44 | 45 | 46 | /* \brief Store offset read in the reading file into a vector of float. 47 | * \brief Return 1 if succeeded in opening file and 0 if failed to detect the initial offset line 48 | * \brief Return -1 if couldn't get any line 49 | */ 50 | int initOffsets(); 51 | 52 | 53 | 54 | /* \brief Initialize the captor's gain vector. 55 | * \brief For now, we'll assume the gain vector starts at {1,1,1} 56 | * \brief Return 1 (success) for now 57 | */ 58 | int initGain(); 59 | 60 | 61 | 62 | 63 | /* \brief Returns the captor's offset 64 | */ 65 | Eigen::Vector3f getOffsets() const; 66 | 67 | 68 | 69 | /* \brief Reads line from captor file and return it as an output vector 70 | */ 71 | void getOutput(Eigen::Vector3f*); 72 | 73 | 74 | 75 | /* \brief Correct the captor's output with its offset 76 | * \param The outbut buffer you want to correct 77 | */ 78 | void correctOutput(Eigen::Vector3f*); 79 | 80 | 81 | 82 | 83 | // TODO 84 | void setOffsets(Eigen::Vector3f); 85 | 86 | 87 | 88 | ~Captor(); 89 | 90 | bool line_end = false; 91 | 92 | 93 | 94 | 95 | protected: 96 | 97 | std::ifstream captor_file; 98 | Eigen::Vector3f captor_offsets; 99 | Eigen::Vector3f captor_gain; 100 | std::string line; 101 | std::string OFFSET_MARK; // Offset mark for the setOffsets method 102 | std::string captor_id; 103 | std::string LINE_MARK; // Line mark for the getOutput method 104 | std::string ADVANCED_OFFSET_MARK; // Only serves for acc (GN calibration method) 105 | 106 | }; 107 | 108 | 109 | #endif /* defined(__INS_EKF__Captor__) */ 110 | -------------------------------------------------------------------------------- /GPS.h: -------------------------------------------------------------------------------- 1 | // 2 | // GPS.h 3 | // INS_EKF 4 | // 5 | // Created by Louis Faury on 12/07/2015. 6 | // Copyright (c) 2015 Louis Faury. All rights reserved. 7 | // 8 | 9 | #ifndef __INS_EKF__GPS__ 10 | #define __INS_EKF__GPS__ 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include "/usr/local/include/Eigen/Dense" 17 | #include 18 | #include 19 | #include "Captor.h" 20 | #include "util.h" 21 | 22 | 23 | 24 | 25 | class GPS{ 26 | 27 | public: 28 | 29 | // Enum for the mode the GPS would have automatically chosen given the available satelites. TO DO : Automatic detection of the style 30 | enum style{ 31 | NMEA, 32 | UBLOX 33 | }; 34 | 35 | 36 | 37 | GPS(); 38 | 39 | 40 | /* \brief Constructor of GPS object 41 | * \brief throw exception if file couldn't be opened 42 | * \param string of the gps file path 43 | */ 44 | GPS(std::string,style) throw(std::exception); 45 | 46 | 47 | 48 | /* \brief Set the HOME of the INU by giving the first GPS value 49 | */ 50 | int setHome(); 51 | 52 | 53 | 54 | /* \brief Update the GPS position 55 | */ 56 | int update(Eigen::Vector3d*); 57 | 58 | 59 | 60 | /* \brief Transform LLA data to local frame coordinates and stores it into actual_position 61 | * \brief To match with the IMU when we'll perform Kalman filtering, this function calculates distance on EAST NORTH and ALTITUDE axis from the HOME position 62 | * \brief We consider the following formulas for calculating such distances 63 | * \brief See source code for formulas 64 | * \param The gps output you want to transform - the pointer to buffer of the gps datas 65 | */ 66 | void calculatePositionFromHome(Eigen::Vector3d*); 67 | 68 | 69 | 70 | /* \brief Checks if GPS received available datas 71 | */ 72 | bool isAvailable(); 73 | 74 | 75 | /* \brief Store in a Vector3d the last gps update and the current_time 76 | * \param Pointer to the buffer vector for GPS position 77 | */ 78 | void actualizeInternDatas(Eigen::Vector3d*); 79 | 80 | 81 | 82 | 83 | /* \brief Returns actual GPS Position from HOME as calculated in calculatePositionFromHome 84 | */ 85 | Eigen::Vector3d getActualPosition(); 86 | 87 | 88 | friend class GPS_Filter; 89 | 90 | private: 91 | 92 | const std::string HOME_MARK = "$GPSI"; 93 | const std::string LINE_MARK = "$GPS"; 94 | std::ifstream gps_file; 95 | Eigen::Vector3d HOME; 96 | Eigen::Vector3d actual_position; 97 | Eigen::Vector3d last_update; 98 | double current_time; 99 | double time_since_last_update; 100 | double time_of_last_update; 101 | double time_init; 102 | Eigen::Vector3d ground_speed; 103 | std::string line; 104 | bool available; 105 | int time_scale; //Since the GPS can have to style of fonctionnement with two different time_scale 106 | 107 | 108 | 109 | 110 | }; 111 | 112 | #endif /* defined(__INS_EKF__GPS__) */ 113 | -------------------------------------------------------------------------------- /GPS_Filter.h: -------------------------------------------------------------------------------- 1 | // 2 | // GPS_Filter.h 3 | // INS_EKF 4 | // 5 | // Created by Louis Faury on 25/07/2015. 6 | // Copyright (c) 2015 Louis Faury. All rights reserved. 7 | // 8 | 9 | #ifndef __INS_EKF__GPS_Filter__ 10 | #define __INS_EKF__GPS_Filter__ 11 | 12 | #include 13 | #include 14 | #include "/usr/include/Eigen/Dense" 15 | #include 16 | #include "GPS.h" 17 | 18 | 19 | class GPS_Filter{ 20 | 21 | public: 22 | 23 | GPS_Filter(); 24 | 25 | 26 | /* \brief Constructor of the class 27 | * \param Pointer to the current GPS object 28 | */ 29 | GPS_Filter(GPS*); 30 | 31 | 32 | 33 | /* \brief Used for the predict step of the Kalman filter 34 | * \brief Calculate the state matrix at incrementation time k 35 | */ 36 | void calculateTransitionMatrix(); 37 | 38 | 39 | 40 | /* \brief Used for the predict step of the Kalman filter 41 | * \brief Main method for the predict step. 42 | */ 43 | void predict(); 44 | 45 | 46 | 47 | /* \brief Used for the predict step of the Kalman filter, using the speed calculated from the filter (coupled Kalman filtering) 48 | * \brief Main method for the predict step. 49 | * \param The Vector3f of the speed as calculated by the Kalman filter 50 | * WARNING : Deprecated, accelerometer is too noisy 51 | */ 52 | void predict(Eigen::Vector3f); 53 | 54 | 55 | 56 | /* \brief Used for the update step of the Kalman filter 57 | * \brief Calculate Z vector from the gps datas 58 | */ 59 | void updateMesure(); 60 | 61 | 62 | 63 | /* \brief Used for the update step of the Kalman filter 64 | * \brief Main method for the update step 65 | */ 66 | void updateFilter(); 67 | 68 | 69 | /* \brief Used for the update step of the Kalman filter 70 | * \brief Implement R matrix adaptative evolution 71 | * \brief : Taken from Ali Almagbile, Jinling Wang, and Weidong Ding paper : Evaluating the Performances of Adaptive Kalman Filter Methods in GPS/INS Integration 72 | */ 73 | void adaptRMatrix(); 74 | 75 | 76 | 77 | /* \brief Used to update speed of GPS_Filter by the one given by the filter 78 | * \param The Vector3f of the speed as calculated by the Kalman filter 79 | */ 80 | void updateSpeedEKF(Eigen::Vector3f); 81 | 82 | 83 | /* \brief Returns the current position in a Vector3f 84 | */ 85 | Eigen::Vector3f getCurrentPosition() const; 86 | 87 | 88 | /* \brief Returns the current speed in a Vector3f 89 | */ 90 | Eigen::Vector3f getCurrentSpeed() const; 91 | 92 | 93 | /* \brief Returns the actual state vector in the form of a Vector6f 94 | */ 95 | Vector6f getState() const; 96 | 97 | 98 | 99 | /* \brief Return the speed part of P_ covariance matrix 100 | */ 101 | Eigen::Matrix3f getSpeedCovMatrix() const; 102 | 103 | 104 | /* \brief Return the pos part of P_ covariance matrix 105 | */ 106 | Eigen::Matrix3f getPositionCovMatrix() const; 107 | 108 | 109 | ~GPS_Filter(); 110 | 111 | 112 | 113 | private: 114 | 115 | Matrix6f P_; // P(k)+ 116 | Matrix6f _P; // P(k)- 117 | Matrix6f R; 118 | Matrix6f Q; 119 | Matrix6f H; 120 | Matrix6f F; 121 | Matrix6f K; 122 | Vector6f _X; 123 | Vector6f X_; // WARNING : the GPS state vector is state_vector=(ground_speed,position) 124 | Vector6f Z; 125 | Matrix6f I6; 126 | float dt; 127 | GPS* gps; 128 | Eigen::Vector3f actual_position; 129 | Eigen::Vector3f actual_speed; 130 | Matrix6f C; // For R adaptative method 131 | Matrix6_10d mu; // For R adpatative method 132 | 133 | 134 | }; 135 | 136 | 137 | 138 | 139 | 140 | 141 | #endif /* defined(__INS_EKF__GPS_Filter__) */ 142 | -------------------------------------------------------------------------------- /MAGNETOMETER.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // MAGNETOMETER.cpp 3 | // INS_EKF 4 | // 5 | // Created by Louis Faury on 12/07/2015. 6 | // Copyright (c) 2015 Louis Faury. All rights reserved. 7 | // 8 | 9 | #include "MAGNETOMETER.h" 10 | 11 | 12 | 13 | 14 | MAGNETOMETER::MAGNETOMETER(){ 15 | } 16 | 17 | 18 | MAGNETOMETER::MAGNETOMETER(const std::string mag_file_name, STYLE style) : Captor(mag_file_name){ 19 | std::cout << "Magnetometer file open" << std::endl; 20 | captor_id = "MAG"; 21 | LINE_MARK = "$M"; 22 | OFFSET_MARK = "$MO"; 23 | _declination = SPQ_DECLINATION_RAD; // Setting the declination angle for the tests in Qro,Mexico 24 | R.setZero(); 25 | R(0,0) = 0.0009; 26 | R(1,1) = 0.0004; 27 | R(2,2) = 0.02; 28 | if (style != COLD_START){ 29 | // TODO 30 | } 31 | } 32 | 33 | 34 | double MAGNETOMETER::getHeading(float pitch, float roll) const{ 35 | double heading_x = mag_x*cos(pitch)-mag_y*sin(roll)*sin(pitch)-mag_z*cos(roll)*sin(pitch); 36 | //int heading_x = mag_x*cos(PRY(1))-mag_y*sin(PRY(0))*sin(PRY(1))-mag_z*cos(PRY(0))*sin(PRY(1)); 37 | double heading_y = -mag_y*cos(roll)+mag_z*sin(roll); 38 | //int heading_y = mag_y*cos(PRY(0))-mag_z*sin(PRY(0)); 39 | 40 | double heading = (double) atan2(-heading_y, heading_x); 41 | if( _declination > 0.0f ) 42 | { 43 | heading = heading + _declination; 44 | if (heading > PI) // We normalize the angle, so that it is between -PI and +PI 45 | heading -= (2.0f * PI); 46 | else if (heading < -PI) 47 | heading += (2.0f * PI); 48 | } 49 | 50 | return heading; 51 | 52 | } 53 | 54 | 55 | 56 | double MAGNETOMETER::getHeading() const{ 57 | double heading = (double) atan2(mag_y, mag_x); 58 | if( _declination > 0.0f ) 59 | { 60 | heading = heading + _declination; 61 | if (heading > PI) // We normalize the angle, so that it is between -PI and +PI 62 | heading -= (2.0f * PI); 63 | else if (heading < -PI) 64 | heading += (2.0f * PI); 65 | } 66 | 67 | return heading; 68 | } 69 | 70 | 71 | 72 | void MAGNETOMETER::update(){ 73 | int count = 0; 74 | std::string::size_type sz; 75 | if (std::getline(captor_file,line)){ 76 | line_end = false; 77 | std::stringstream ss(line); 78 | while (getline(ss,line,',')){ 79 | if (line == LINE_MARK); 80 | else { 81 | switch (count) { 82 | case 0: 83 | mag_x = std::stod(line,&sz); 84 | break; 85 | case 1: 86 | mag_y = std::stod(line,&sz); 87 | break; 88 | case 2: 89 | mag_z = std::stod(line,&sz); 90 | break; 91 | } 92 | count++; 93 | } 94 | } 95 | std::cout << captor_id << " : line read" << std::endl; 96 | } 97 | else{ 98 | std::cout << "Couldn't read from captor : " << captor_id << std::endl; 99 | line_end = true; 100 | } 101 | 102 | } 103 | 104 | 105 | 106 | void MAGNETOMETER::_init_earth_even_magnetic_field(){ 107 | this->update_correct(); 108 | _earth_even_magnetic_field << mag_x,mag_y,mag_z; 109 | //std::cout << _earth_even_magnetic_field.transpose() << std::endl; 110 | } 111 | 112 | 113 | Eigen::Vector3d MAGNETOMETER::getEEMF(){ 114 | return _earth_even_magnetic_field; 115 | } 116 | 117 | 118 | void MAGNETOMETER::correct(){ 119 | mag_x = (mag_x+captor_offsets(0))/HMC5843L_GAIN; 120 | mag_y = (mag_y+captor_offsets(1))/HMC5843L_GAIN; 121 | mag_z = (mag_z+captor_offsets(2))/HMC5843L_GAIN; 122 | } 123 | 124 | 125 | 126 | void MAGNETOMETER::update_correct(){ 127 | this->update(); 128 | this->correct(); 129 | } 130 | 131 | 132 | 133 | void MAGNETOMETER::printState() const{ 134 | std::cout << mag_x << "\t" << mag_y << "\t" << mag_z << std::endl; 135 | } 136 | 137 | 138 | Eigen::Vector3d MAGNETOMETER::getState() const{ 139 | Eigen::Vector3d result; 140 | result << mag_x, mag_y, mag_z; 141 | return result; 142 | } 143 | 144 | 145 | 146 | Eigen::Matrix3f MAGNETOMETER::getMagCovarianceMatrix(){ 147 | return R; 148 | } 149 | 150 | 151 | 152 | 153 | -------------------------------------------------------------------------------- /ACCELEROMETER.h: -------------------------------------------------------------------------------- 1 | // 2 | // ACCELEROMETER.h 3 | // INS_EKF 4 | // 5 | // Created by Louis Faury on 12/07/2015. 6 | // Copyright (c) 2015 Louis Faury. All rights reserved. 7 | // 8 | 9 | #ifndef __INS_EKF__ACCELEROMETER__ 10 | #define __INS_EKF__ACCELEROMETER__ 11 | 12 | #include 13 | #include "Captor.h" 14 | #include "util.h" 15 | 16 | 17 | 18 | 19 | class ACCELEROMETER : public Captor{ 20 | 21 | 22 | public: 23 | 24 | ACCELEROMETER(); 25 | 26 | 27 | ACCELEROMETER(const std::string, STYLE); 28 | 29 | 30 | /*\brief Init accelerometer offsets 31 | * WARNING : SUPPOSING THE IMU IS PLACED ON EVEN SURFACE 32 | * \brief Calls Captors::initOffsets() method and add g vector for the Z-axis offset 33 | */ 34 | int initOffsets(); 35 | 36 | 37 | /* \brief Corrects acc output 38 | * \brief Calls the Captors::correctOutput() method and add to the buffer vector the rotated g acceleration of gravity vector 39 | * \param The buffer of the acc_output and the rotation matrix of the EKF 40 | */ 41 | void correctOutput(Eigen::Vector3f*,Eigen::Matrix3f); 42 | 43 | 44 | 45 | /* \brief General method for the Gauss-Newton calibration of the accelerometer 46 | * \brief This method (and the followings) is inspired from Rolfe Schmidt's website and work : 47 | * \brief http://chionophilous.wordpress.com/2011/10/24/accelerometer-calibration-iv-1-implementing-gauss-newton-on-an-atmega/ 48 | * \brief The original script was found here : 49 | * \brief http://rolfeschmidt.com/mathtools/skimetrics/adxl_gn_calibration.pde 50 | * \brief Returns 1 if succeeded. 51 | * \brief Returns -1 if couldn't capture the calibration routines acceleration datas 52 | * \brief Returns 0 if the method couln't converge in less than 10 iterations 53 | */ 54 | int performAdvancedAccCalibration(); 55 | 56 | 57 | 58 | /* \brief Is called in order to capture the acc values when performing the GN algorithm for acc advanced calibration 59 | * \brief Part of acc advanced calibration routine 60 | * \brief Returns 1 if succeeded 61 | * \brief Returns 0 if line wasn't part of the good routine and -1 if there is no line to read in the acc data file. 62 | */ 63 | int captureAdvancedCalibrationRoutine(); 64 | 65 | 66 | 67 | /* \brief Initialize the Gauss-Newton algorithm 68 | * \brief Part of acc advanced calibration routine 69 | */ 70 | void initGNMethod(); 71 | 72 | 73 | 74 | /* \brief Calculate and returns the sum square of the residual 75 | * Part of acc advanced calibration routine 76 | */ 77 | float calculateSumSquareDiff(); 78 | 79 | 80 | 81 | 82 | /* \brief Update the matrix of the GN algorithm 83 | * Part of acc advanced calibration routine 84 | */ 85 | void updateGNAlgorithm(); 86 | 87 | 88 | 89 | 90 | /* \brief Find and returns the direction in which the state vector should be moved to minimize the square distance sum. 91 | * \brief Part of acc advanced calibration routine 92 | */ 93 | Vector6f findIncrement(); 94 | 95 | 96 | 97 | 98 | /* \brief Correct the _beta vector 99 | * \brief Part of acc advanced calibration routine 100 | */ 101 | void correct_beta(); 102 | 103 | 104 | /* \brief We then replace the offsets and the gains by those calculated by the GN method. 105 | * \brief Part of acc advanced calibration routine 106 | */ 107 | void correct_GN(); 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | private: 119 | 120 | 121 | #ifdef ADCALIBRATION 122 | Eigen::Matrix _ad_acc_data; // Matrix used to store the advanced acc calibration method readings 123 | Vector6f _r_vector; // Vector of the residuals for the Gauss-Newton method 124 | Vector6f _beta; // Vector of offsets and gains 125 | // In order : offset_x,offset_y,offset_z,gain_x,gain_y,gain_Z 126 | Matrix6f _jacobian; // The jacobian matrix for the Gauss-Newton method 127 | #endif 128 | 129 | }; 130 | 131 | #endif /* defined(__INS_EKF__ACCELEROMETER__) */ 132 | -------------------------------------------------------------------------------- /GPS_Filter.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // GPS_Filter.cpp 3 | // INS_EKF 4 | // 5 | // Created by Louis Faury on 25/07/2015. 6 | // Copyright (c) 2015 Louis Faury. All rights reserved. 7 | // 8 | 9 | #include "GPS_Filter.h" 10 | 11 | 12 | 13 | Vector6f getColumn(Matrix6_10d input, int i){ 14 | Vector6f result; 15 | for (int j=0; jtime_since_last_update; 76 | F(3,0) = dt; 77 | F(4,1) = dt; 78 | F(5,2) = dt; 79 | } 80 | 81 | 82 | void GPS_Filter::predict(){ 83 | calculateTransitionMatrix(); 84 | _X = F*X_; 85 | _P = F*P_*(F.transpose()) + Q; 86 | } 87 | 88 | 89 | void GPS_Filter::predict(Eigen::Vector3f ekf_speed){ 90 | calculateTransitionMatrix(); 91 | for (int i=0; i<3; ++i){ 92 | X_(i) = ekf_speed(i); 93 | } 94 | _X = F*X_; 95 | _P = F*P_*(F.transpose()) + Q; 96 | } 97 | 98 | 99 | void GPS_Filter::updateMesure(){ 100 | for (int i=0; i<3; ++i){ 101 | Z(i) = (gps->ground_speed)(i); 102 | } 103 | for (int i=3; i<6; ++i){ 104 | Z(i) = (gps->actual_position)(i-3); 105 | } 106 | } 107 | 108 | 109 | 110 | void GPS_Filter::updateFilter(){ 111 | updateMesure(); 112 | adaptRMatrix(); 113 | K = _P * (H.transpose())*((H*_P*(H.transpose())+R).inverse()); 114 | X_ = _X + K*(Z-H*_X); 115 | P_ = (I6 - K*H)*_P; 116 | actual_position << X_(3),X_(4),X_(5); 117 | actual_speed << X_(0),X_(1),X_(2); 118 | } 119 | 120 | 121 | Eigen::Vector3f GPS_Filter::getCurrentPosition() const{ 122 | return actual_position; 123 | } 124 | 125 | 126 | Eigen::Vector3f GPS_Filter::getCurrentSpeed() const{ 127 | return actual_speed; 128 | } 129 | 130 | 131 | void GPS_Filter::adaptRMatrix(){ 132 | 133 | for (int j = 9; j > 0; j--) { 134 | for (int i=0; i<6; ++i){ 135 | mu(i,j) = mu(i,j-1); 136 | } 137 | } 138 | 139 | for (int i=0; i<6; i++){ 140 | mu(i,0) = (Z-H*_X)(i); 141 | } 142 | C.setZero(); 143 | for (int i=0; i<10; ++i){ 144 | C = C + (getColumn(mu,i)*(getColumn(mu,i).transpose()))/10; 145 | } 146 | R = C + H*_P*(H.transpose()); 147 | } 148 | 149 | 150 | 151 | void GPS_Filter::updateSpeedEKF(Eigen::Vector3f ekf_speed){ 152 | for (int i=0; i<3; ++i){ 153 | X_(i) = (X_(i) +0.1*ekf_speed(i))/1.1; // We're doing it the way of a low pass filter since captor is really noisy 154 | // Strong change in speed will then be detected while only speed drift will be ignored 155 | } 156 | } 157 | 158 | 159 | Vector6f GPS_Filter::getState() const{ 160 | return X_; 161 | } 162 | 163 | 164 | 165 | Eigen::Matrix3f GPS_Filter::getSpeedCovMatrix() const{ 166 | Eigen::Matrix3f result = P_.block<3,3>(0,0); 167 | return result; 168 | } 169 | 170 | 171 | Eigen::Matrix3f GPS_Filter::getPositionCovMatrix() const{ 172 | Eigen::Matrix3f result = P_.block<3,3>(3,3); 173 | return result; 174 | } 175 | 176 | 177 | GPS_Filter::~GPS_Filter(){ 178 | 179 | } 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | -------------------------------------------------------------------------------- /GPS.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // GPS.cpp 3 | // INS_EKF 4 | // 5 | // Created by Louis Faury on 12/07/2015. 6 | // Copyright (c) 2015 Louis Faury. All rights reserved. 7 | // 8 | 9 | #include "GPS.h" 10 | 11 | 12 | 13 | 14 | GPS::GPS(){ 15 | } 16 | 17 | 18 | 19 | GPS::GPS(std::string gps_file_path, style style) throw(std::exception) : gps_file(gps_file_path.c_str()) { 20 | if (style == NMEA) time_scale = 100; 21 | if (style == UBLOX) time_scale = 1000; 22 | if (!gps_file) throw std::domain_error("Couldn't open GPS file !"); 23 | std::cout << "GPS file open" << std::endl; 24 | } 25 | 26 | 27 | 28 | int GPS::setHome(){ 29 | int count = 0; 30 | std::string::size_type sz; 31 | if (std::getline(gps_file,line)){ 32 | std::stringstream ss(line); 33 | while (getline(ss,line,',')){ 34 | if (line == HOME_MARK); 35 | else { 36 | if (count < 3) { 37 | HOME[count] = (std::stod(line,&sz)); 38 | count++; 39 | } 40 | else time_init = std::stod(line,&sz)/time_scale; 41 | } 42 | } 43 | //std::cout << HOME << std::endl; 44 | last_update << 0,0,0; 45 | time_of_last_update = time_init; 46 | return 1; 47 | } 48 | return -1; 49 | } 50 | 51 | 52 | 53 | int GPS::update(Eigen::Vector3d* gps_buffer){ 54 | int count = 0; 55 | std::string::size_type sz; 56 | if (std::getline(gps_file,line)){ 57 | std::stringstream ss(line); 58 | while (getline(ss,line,',') && count<4){ 59 | if (line == LINE_MARK); 60 | else { 61 | if (line == "NA"){ 62 | available = false; 63 | return -1; 64 | } 65 | else { 66 | if (count < 3){ 67 | (*gps_buffer)[count] = std::stod(line,&sz); 68 | count++; 69 | } 70 | else current_time = std::stod(line,&sz)/time_scale; 71 | available = true; 72 | } 73 | } 74 | } 75 | std::cout << "GPS : line read" << std::endl; 76 | return 1; 77 | } 78 | std::cout << "Couldn't read from GPS" << std::endl; 79 | return 0; 80 | } 81 | 82 | 83 | 84 | void GPS::calculatePositionFromHome(Eigen::Vector3d *source){ 85 | // We first calculate the true distance from the earth's center 86 | // in meter 87 | double _distance_from_earth_center = RT + (*source)(2); 88 | // We then calculate the distance we are from HOME on the EAST axis 89 | // x = (RT+h)*(actual_long - HOME_long)*cos(actual_lat) 90 | // in meter 91 | double x = _distance_from_earth_center*(((*source)(1)-HOME(1))/TODEG)*cos((*source)(0)/TODEG); 92 | // We then calculate the distance we are from HOME on the NORTH axis 93 | // y = (RT+h)*(actual_lat - HOME_lat) 94 | // in meter 95 | double y = _distance_from_earth_center*((*source)(0)-HOME(0))/TODEG; 96 | // We then calculate the distance we are from HOME regarding altitude in meter 97 | // z = actual_altitude - HOME_altitude 98 | // in meter 99 | double z = (*source)(2)-HOME(2); 100 | // We eventually update the actual_position as NORTH / EAST / ALTITUDE to match with IMU 101 | actual_position << y,x,z; 102 | } 103 | 104 | 105 | 106 | Eigen::Vector3d GPS::getActualPosition(){ 107 | return actual_position; 108 | } 109 | 110 | 111 | 112 | bool GPS::isAvailable(){ 113 | return available; 114 | } 115 | 116 | 117 | 118 | void GPS::actualizeInternDatas(Eigen::Vector3d* gps_buffer_vector){ 119 | // We update the time since the last update 120 | time_since_last_update = current_time - time_of_last_update; 121 | 122 | // Prevent the anomaly that happens when functionning in NMEA : multiple array with same GPS time are given out by GPS 123 | // Thus we test if the time since the last update in non-zero 124 | // either way the speed will not be valid. 125 | if (time_since_last_update != 0){ 126 | time_of_last_update = current_time; 127 | for (int i=0; i<3; i++){ 128 | ground_speed(i) = (actual_position(i)-last_update(i))/time_since_last_update; // Actualisation of the ground_speed 129 | // v = (actual_pos - last_pos) / dt 130 | } 131 | last_update = actual_position; // Update last_update 132 | // now that we've actualized the datas 133 | } 134 | else { 135 | actual_position = last_update; // This is in case of NMEA mode as it appears it prints multiple data for same timing 136 | ground_speed *= 0; // We assume speed is then 0 and position stays the same 137 | } 138 | 139 | } 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | -------------------------------------------------------------------------------- /ACCELEROMETER.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // ACCELEROMETER.cpp 3 | // INS_EKF 4 | // 5 | // Created by Louis Faury on 12/07/2015. 6 | // Copyright (c) 2015 Louis Faury. All rights reserved. 7 | // 8 | 9 | #include 10 | #include "ACCELEROMETER.h" 11 | 12 | ACCELEROMETER::ACCELEROMETER(){ 13 | } 14 | 15 | 16 | ACCELEROMETER::ACCELEROMETER(const std::string acc_file_name, STYLE style) : Captor(acc_file_name){ 17 | std::cout << "Acceleration file open" << std::endl; 18 | captor_id = "ACC"; 19 | LINE_MARK = "$A"; 20 | OFFSET_MARK = "$AO"; 21 | ADVANCED_OFFSET_MARK = "$AAC"; 22 | 23 | if (style != COLD_START){ 24 | // TO DO 25 | } 26 | } 27 | 28 | 29 | int ACCELEROMETER::initOffsets(){ 30 | int ret = Captor::initOffsets(); 31 | if (ret == 1){ 32 | captor_offsets[2] += GRAVITY_INT_PARIS; // We assume here that calibration was performed on an even surface 33 | } 34 | return ret; 35 | } 36 | 37 | 38 | 39 | void ACCELEROMETER::correctOutput(Eigen::Vector3f* acc_buffer, Eigen::Matrix3f _C){ 40 | Captor::correctOutput(acc_buffer); 41 | Eigen::Vector3f _gravity; 42 | _gravity << 0,0,-GRAVITY_INT_PARIS; 43 | Eigen::Vector3f _rotate_gravity = (_C.transpose())*_gravity; 44 | *acc_buffer -= _rotate_gravity; 45 | } 46 | 47 | 48 | 49 | // What follows is the source code for the function used for the Gauss-Newton method calibration (see header). 50 | #ifdef ADCALIBRATION 51 | 52 | int ACCELEROMETER::captureAdvancedCalibrationRoutine(){ 53 | int row(0),column(0),num_pos(6); // num_pos is the number of position we will put the IMU in, 54 | // thus is the number of rows of the storage matrix 55 | _ad_acc_data.setZero(); 56 | std::string::size_type sz; 57 | while (row < num_pos){ 58 | if (std::getline(captor_file,line)){ 59 | if (line[2] == 'A'){ 60 | std::stringstream ss(line); 61 | while (getline(ss,line,',') && column <3){ 62 | if (line == ADVANCED_OFFSET_MARK); 63 | else { 64 | _ad_acc_data(row,column) = std::stof(line,&sz); 65 | column++; 66 | } 67 | } 68 | } 69 | else return 0; 70 | } 71 | else return -1; 72 | column = 0; // Re - initializing column to zero 73 | row++; 74 | } 75 | //std::cout << _ad_acc_data << std::endl; 76 | return 1; 77 | } 78 | 79 | 80 | void ACCELEROMETER::initGNMethod(){ 81 | // We set the first beta to be the one captured on length surface, should not be far from the final result 82 | for (int i=0; i<3; ++i){ 83 | _beta(i) = captor_offsets(i); 84 | _beta(i+3) = captor_gain(i)/GRAVITY_INT_PARIS; 85 | } 86 | } 87 | 88 | 89 | float ACCELEROMETER::calculateSumSquareDiff(){ 90 | float result(0); 91 | for (int i=0; i<6; ++i){ 92 | result += _r_vector(i)*_r_vector(i); // S = sum(r_{i}^{2}) : the sum we're trying to minimize 93 | } 94 | return result; 95 | } 96 | 97 | 98 | void ACCELEROMETER::updateGNAlgorithm(){ 99 | // We start by updating the residual vector 100 | _r_vector.setZero(); 101 | for (int i=0; i<6; ++i){ 102 | for (int j=0; j<3; j++){ 103 | _r_vector(i) -= ((_ad_acc_data(i,j)-_beta(j))*_beta(j+3))*(_ad_acc_data(i,j)-_beta(j))*_beta(j+3); 104 | } 105 | _r_vector(i) += 1; // Each line of _r_vector is the difference between unity and the measured gravity vector norm normalized. 106 | } 107 | //std::cout << _r_vector.transpose() << std::endl; 108 | 109 | // We then update the jacobian matrix 110 | _jacobian.setZero(); 111 | for (int i=0; i<6; ++i){ 112 | for (int j=0; j<3; ++j){ 113 | _jacobian(i,j) = -2*(_ad_acc_data(i,j)-_beta(j))*_beta(j+3)*_beta(j+3); 114 | _jacobian(i,j+3) = 2*(_ad_acc_data(i,j)-_beta(j))*(_ad_acc_data(i,j)-_beta(j))*_beta(j+3); 115 | } 116 | } 117 | 118 | } 119 | 120 | 121 | Vector6f ACCELEROMETER::findIncrement(){ 122 | // Gauss-Newton method enable to calculate directly the miniisation direction 123 | Vector6f delta = (_jacobian.transpose()*_jacobian).inverse()*(_jacobian.transpose())*_r_vector; 124 | return delta; 125 | } 126 | 127 | 128 | void ACCELEROMETER::correct_beta(){ 129 | _beta += findIncrement(); 130 | } 131 | 132 | 133 | int ACCELEROMETER::performAdvancedAccCalibration(){ 134 | // Capturing data for the algorithm to work 135 | if (captureAdvancedCalibrationRoutine() != 1) return -1; 136 | else { 137 | int counter(0); // Counter so that it won't calculate for ever if fails 138 | initGNMethod(); 139 | while (calculateSumSquareDiff() > 0.0000001 && counter <10){ 140 | updateGNAlgorithm(); 141 | correct_beta(); 142 | counter++; 143 | } 144 | if (counter < 10) { 145 | std::cout << _beta.transpose() << std::endl; 146 | return 1; 147 | } 148 | } 149 | return 0; 150 | } 151 | 152 | 153 | 154 | void ACCELEROMETER::correct_GN(){ 155 | for (int i=0; i<3; ++i){ 156 | captor_offsets(i) = _beta(i); // Correcting the offsets 157 | captor_gain(i) = _beta(i+3)*GRAVITY_INT_PARIS; // Correcting the gain in m.s^-2 (they are in g's in the GN algorithm) 158 | } 159 | } 160 | 161 | 162 | #endif 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | -------------------------------------------------------------------------------- /EKF.h: -------------------------------------------------------------------------------- 1 | // 2 | // EKF.h 3 | // INS_EKF 4 | // 5 | // Created by Louis Faury on 13/07/2015. 6 | // Copyright (c) 2015 Louis Faury. All rights reserved. 7 | // 8 | 9 | #ifndef __INS_EKF__EKF__ 10 | #define __INS_EKF__EKF__ 11 | 12 | #include 13 | #include "/usr/local/include/Eigen/Dense" 14 | #include 15 | #include 16 | #include 17 | #include "GPS_Filter.h" 18 | #include "ACCELEROMETER.h" 19 | #include "GYRO.h" 20 | #include "MAGNETOMETER.h" 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | 27 | extern char* IMUport; 28 | 29 | 30 | class EKF : public Runnable{ 31 | 32 | //The state vector : {pos_vector NORTH,EAST,ALTITUDE 33 | // speed_vector NORTH,EAST,ATLITUDE 34 | // quaternion_vector, 35 | // acc_bias_random_walk, 36 | // gyro_bias_random_walk, 37 | // acc_white_gaussian_noise, 38 | // gyro_white_gaussian_noise} 39 | 40 | 41 | 42 | 43 | class EKF : public Runnable{ 44 | 45 | 46 | public: 47 | 48 | 49 | EKF(Drone* drone_); 50 | 51 | void start(); 52 | 53 | void* run(); 54 | 55 | 56 | /* Constructor of the class. 57 | * \param A pointer to a GPS_Filter Object - used for actualizing the general filter 58 | */ 59 | EKF(Drone*, GPS_Filter*,ACCELEROMETER*,GYRO*,MAGNETOMETER*); 60 | 61 | 62 | void start(); 63 | 64 | void* run(); 65 | 66 | void interpret(char* msg); 67 | 68 | 69 | /* \brief Initialize the state vector 70 | * \brief WARNING : For now, initialize in the inertial frame 71 | * \brief Position, speed and quaternion 72 | */ 73 | void init_state_vector(); 74 | 75 | 76 | 77 | /* \brief Initalize the state_vector given an initial heading 78 | * \brief Assume the IMU is on an even surface, with only a non zero heading 79 | * \brief Allow the EKF to evolve in N/E frame 80 | * \param heading (int) 81 | */ 82 | void init_state_vector(double heading); 83 | 84 | 85 | 86 | /* \brief Return the state vector 87 | */ 88 | Vector16f get_state_vector() const; 89 | 90 | 91 | /* \brief Construct the jacobian matrix for the prediction step of the Kalman filter 92 | * \param Pointer to a Vector3f (buffer for the acc) 93 | * \param Pointer to a Vector3f (buffer for the gyro) 94 | */ 95 | void build_jacobian_matrix(Eigen::Vector3f*, Eigen::Vector3f*); 96 | 97 | 98 | 99 | void interpret(char* msg); 100 | 101 | 102 | 103 | Eigen::Vector3f toPRY(Vector10f vector); 104 | 105 | 106 | /* \brief Prediction step for the EKF 107 | */ 108 | void predict(); 109 | 110 | 111 | 112 | /* \brief Correcting step for the EKF 113 | */ 114 | void correct(); 115 | 116 | 117 | 118 | /* DEPRECATED 119 | * \brief Converts the quaternion vector inside the state_vector into a Vector3f of PRY (Pitch, Roll, and Yaw) in RAD. 120 | * \param The state vector 121 | */ 122 | Eigen::Vector3f toRPY(Vector16f vector); 123 | 124 | 125 | 126 | /* \brief Converts the quaternion vector inside the state_vector into a Vector3f of PRY (Pitch, Roll, and Yaw) in RAD and returns it in form of a Vector3f. 127 | * \param none 128 | */ 129 | Eigen::Vector3f getRPY() const; 130 | 131 | 132 | 133 | /* \brief Return current position from the Extended Kalman Filter 134 | */ 135 | Eigen::Vector3f getCurrentPos() const; 136 | 137 | 138 | 139 | /* \brief Return current speed from the Extended Kalman Filter 140 | */ 141 | Eigen::Vector3f getCurrentSpeed() const; 142 | 143 | 144 | /* \brief Returns as a Vector6f the Offsets calculated by the Kalman filter 145 | */ 146 | Vector6f getActualInertialOffsets() const; 147 | 148 | 149 | /* Update the measure vector Z from the gps_filter datas 150 | */ 151 | void updateMeasure(); 152 | 153 | 154 | 155 | /* Computes the DCM matrix from the quaternion vector 156 | */ 157 | void calculateDCM(); 158 | 159 | 160 | /* \brief Returns actual rotation matrix from inertial body to earth body frame 161 | */ 162 | Eigen::Matrix3f getDCM(); 163 | 164 | 165 | ~EKF(); 166 | 167 | 168 | 169 | 170 | 171 | 172 | private: 173 | 174 | Drone* drone; 175 | 176 | 177 | //Pour la communication avec l'APM 178 | Listener* IMUlistener; 179 | BlockingQueue* received; 180 | int working; 181 | 182 | 183 | GPS_Filter* gps_filter; 184 | ACCELEROMETER* acc; 185 | GYRO* gyro; 186 | MAGNETOMETER* mag; 187 | Vector16f _X; // For X- 188 | Vector16f X_; // For X+ 189 | Vector9f Z; // Measurement vector : Position, Speed and Magnetic Field 190 | Matrix16f J; // The jacobian Matrix for integration of inertial datas // In Extended Kalman Filter, would be state matrix. 191 | double dt; // Sampling time in seconds 192 | Matrix16f P_; // P(k)+ 193 | Matrix16f _P; // P(k)- 194 | Matrix16f R; // Covariance matrix for the measure noises 195 | Matrix16f Q; // Covariance matrix for the model noises 196 | Eigen::Matrix K; 197 | Eigen::Matrix H_GPS_POS; 198 | Eigen::Matrix H_GPS_SPEED; 199 | Eigen::Matrix H_MAG; 200 | Eigen::Matrix3f _C; // DCM Matrix from inertial body to earth body 201 | 202 | 203 | }; 204 | 205 | #endif /* defined(__INS_EKF__EKF__) */ 206 | -------------------------------------------------------------------------------- /main.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // main.cpp 3 | // INS_EKF 4 | // 5 | // Created by Louis Faury on 12/07/2015. 6 | // Copyright (c) 2015 Louis Faury. All rights reserved. 7 | // 8 | 9 | /*#include 10 | #include "ACCELEROMETER.h" 11 | #include "GYRO.h" 12 | #include "GPS.h" 13 | #include "MAGNETOMETER.h" 14 | #include 15 | #include 16 | #include "EKF.h" 17 | #include "GPS_Filter.h" 18 | #include "util.h" 19 | 20 | 21 | 22 | 23 | // Path to reading files 24 | const std::string acc_file_path = "/Users/louisfaury/Documents/C++/APM_LOG/test/acc.txt"; 25 | const std::string gyro_file_path = "/Users/louisfaury/Documents/C++/APM_LOG/test/gyro.txt"; 26 | const std::string gps_file_path = "/Users/louisfaury/Documents/C++/APM_LOG/test/gps.txt"; 27 | const std::string mag_file_path = "/Users/louisfaury/Documents/C++/APM_LOG/test/mag.txt"; 28 | 29 | // Path to output file 30 | const std::string to_log_file_path = "/Users/louisfaury/Documents/C++/APM_LOG/test/log_out.txt"; 31 | const std::string rpy_file_path = "/Users/louisfaury/Documents/C++/APM_LOG/test/rpy.txt"; 32 | const std::string gps_out_path = "/Users/louisfaury/Documents/C++/APM_LOG/test/gps_out.txt"; 33 | const std::string gps_filter_out_path = "/Users/louisfaury/Documents/C++/APM_LOG/test/gps_filter.txt"; 34 | const std::string mag_out_path = "/Users/louisfaury/Documents/C++/APM_LOG/test/mag_out.txt"; 35 | 36 | 37 | /* 38 | // Path to reading files (immobile testing for magnetometer and quaternions) 39 | const std::string acc_file_path = "/Users/louisfaury/Documents/C++/APM_LOG/immobile1/acc.txt"; 40 | const std::string gyro_file_path = "/Users/louisfaury/Documents/C++/APM_LOG/immobile1/gyro.txt"; 41 | const std::string gps_file_path = "/Users/louisfaury/Documents/C++/APM_LOG/immobile1/gps.txt"; 42 | const std::string mag_file_path = "/Users/louisfaury/Documents/C++/APM_LOG/immobile1/mag.txt"; 43 | 44 | // Path to output file 45 | const std::string to_log_file_path = "/Users/louisfaury/Documents/C++/APM_LOG/test/log_out.txt"; 46 | const std::string rpy_file_path = "/Users/louisfaury/Documents/C++/APM_LOG/test/rpy.txt"; 47 | const std::string gps_out_path = "/Users/louisfaury/Documents/C++/APM_LOG/test/gps_out.txt"; 48 | const std::string gps_filter_out_path = "/Users/louisfaury/Documents/C++/APM_LOG/test/gps_filter.txt"; 49 | const std::string mag_out_path = "/Users/louisfaury/Documents/C++/APM_LOG/test/mag_out.txt"; 50 | */ 51 | 52 | 53 | 54 | int main(int argc, const char * argv[]) { 55 | 56 | 57 | std::ofstream to_log(to_log_file_path.c_str()); 58 | std::ofstream to_rpy(rpy_file_path.c_str()); 59 | std::ofstream gps_out(gps_out_path.c_str()); 60 | std::ofstream gps_filter_out(gps_filter_out_path.c_str()); 61 | std::ofstream mag_out(mag_out_path.c_str()); 62 | 63 | try { 64 | 65 | // Declaring objects used for inertial navigation 66 | ACCELEROMETER acc(acc_file_path, Captor::COLD_START); 67 | GYRO gyro(gyro_file_path, Captor::COLD_START); 68 | MAGNETOMETER mag(mag_file_path, Captor::COLD_START); 69 | GPS gps(gps_file_path,GPS::UBLOX); 70 | GPS_Filter gps_filter(&gps); 71 | EKF ekf(&gps_filter,&acc,&gyro,&mag); 72 | 73 | 74 | 75 | 76 | // Getting offset and gain for accelerometer, gyroscope and magnetometer. Getting GPS HOME field. 77 | // This routine will warn you if one of the offset couldn't be updated 78 | int ret = acc.initOffsets(); 79 | if (ret != 1) std::cout << "Couldn't get acc offsets !" << std::endl; 80 | else std::cout << "Acc offsets updated" << std::endl; 81 | ret = acc.initGain(); 82 | if (ret != 1) std::cout << "Couldn't get acc gain !" << std::endl; 83 | else std::cout << "Acc gain updated" << std::endl; 84 | 85 | // Advanced calibration for acc (offset and gain) 86 | #ifdef ADCALIBRATION 87 | ret = acc.performAdvancedAccCalibration(); 88 | if (ret !=1) std::cout << "Couldn't perform acc advanced calibration method" << std::endl; 89 | else { 90 | std::cout << "Acc advanced calibration performed" << std::endl; 91 | acc.correct_GN(); 92 | } 93 | #endif 94 | 95 | ret = gyro.initOffsets(); 96 | if (ret != 1) std::cout << "Couldn't get gyro offsets !" << std::endl; 97 | else std::cout << "Gyro offsets updated" << std::endl; 98 | ret = gyro.initGain(); 99 | if (ret != 1) std::cout << "Couldn't get gyro gain !" << std::endl; 100 | else std::cout << "Gyro gain updated" << std::endl; 101 | 102 | ret = mag.initOffsets(); 103 | if (ret != 1) std::cout << "Couldn't get magnetometer offsets !" << std::endl; 104 | else std::cout << "Mag offsets updated" << std::endl; 105 | 106 | ret = gps.setHome(); 107 | if (ret != 1) std::cout << "Couldn't get HOME !" << std::endl; 108 | else std::cout << "HOME updated" << std::endl; 109 | 110 | 111 | 112 | // Reading lines 113 | // We create the buffer for the sensor's output values 114 | Eigen::Vector3f acc_vector_buffer; 115 | Eigen::Vector3f gyro_vector_buffer; 116 | Eigen::Vector3d gps_buffer_vector; 117 | Eigen::Vector3d gps_position_vector; 118 | 119 | // We update and correct the magnetometer 120 | mag._init_earth_even_magnetic_field(); 121 | mag.update_correct(); 122 | // We initialize the state vector giving the initial heading 123 | ekf.init_state_vector(mag.getHeading()); 124 | //ekf.init_state_vector(); 125 | to_rpy << TODEG*(ekf.toRPY(ekf.get_state_vector())).transpose() << std::endl; // Storing operation 126 | 127 | 128 | 129 | while (!acc.line_end && !gyro.line_end){ 130 | 131 | // Reading from inertial captors and correcting the outputs 132 | acc.getOutput(&acc_vector_buffer); 133 | acc.correctOutput(&acc_vector_buffer, ekf.getDCM()); 134 | gyro.getOutput(&gyro_vector_buffer); 135 | gyro.correctOutput(&gyro_vector_buffer); 136 | // Updating and correcting magnetometer 137 | mag.update_correct(); 138 | // Updating GPS and storing into the &gps_buffer_vector 139 | gps.update(&gps_buffer_vector); 140 | 141 | 142 | 143 | // Predicting the state vector 144 | // Extended Kalman Filter according step is the prediction step. We first build the Jacobian matrix linearized around the current state and we then multiply the currrent state vector by such a matrix 145 | ekf.build_jacobian_matrix(&acc_vector_buffer, &gyro_vector_buffer); 146 | ekf.predict(); 147 | 148 | to_rpy << TODEG*(ekf.getRPY()).transpose() << std::endl; // Storing operation for data exploitation 149 | to_log << ekf.get_state_vector().transpose() << std::endl; // Storing operation for data exploitation 150 | //mag_out << TODEG*mag.getHeading() << std::endl; // Storing operation for data exploitation 151 | mag_out << TODEG*mag.getHeading((ekf.getRPY())(1),(ekf.getRPY())(0)) << std::endl; // Storing operation for data exploitation 152 | 153 | 154 | 155 | 156 | 157 | // Checking inf a new gps data is availaible 158 | if (gps.isAvailable()){ 159 | // If yes, we first update the GPS filter 160 | gps.calculatePositionFromHome(&gps_buffer_vector); 161 | gps_out << gps.getActualPosition().transpose() << std::endl; 162 | gps.actualizeInternDatas(&gps_buffer_vector); 163 | gps_filter.predict(); 164 | gps_filter.updateFilter(); 165 | gps_filter_out << gps_filter.getState().transpose() << std::endl; // Storing operation for data exploitation 166 | // And we then correct the EKF filter by correcting in order position, speed and quaternion attitude vector 167 | //ekf.correct(); 168 | //gps_filter.updateSpeedEKF(ekf.getCurrentSpeed()); 169 | } 170 | 171 | } 172 | 173 | } 174 | catch (std::exception const& e) 175 | { std::cout << e.what() << " file " << std::endl; } 176 | 177 | 178 | 179 | 180 | return 0; 181 | }*/ 182 | -------------------------------------------------------------------------------- /EKF.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // EKF.cpp 3 | // INS_EKF 4 | // 5 | // Created by Louis Faury on 13/07/2015. 6 | // Copyright (c) 2015 Louis Faury. All rights reserved. 7 | // 8 | 9 | #include "EKF.h" 10 | 11 | 12 | 13 | 14 | 15 | EKF::EKF(){ 16 | } 17 | 18 | 19 | 20 | EKF::EKF(Drone* drone_, GPS_Filter* gps_filter, ACCELEROMETER* acc, GYRO* gyro, MAGNETOMETER* mag) : gps_filter(gps_filter), acc(acc), gyro(gyro), mag(mag), drone(drone_) { 21 | 22 | IMUlistener = new Listener(IMUport, received); 23 | received = new BlockingQueue(); 24 | 25 | // Initialisation of the sampling time rate 26 | dt = 0.02; 27 | // Initialisation of _P 28 | _P.setZero(); 29 | // Initialisation of P_ 30 | P_.setZero(); 31 | for (int i=0; i<6; ++i){ 32 | P_(i,i) = 0.5; 33 | } 34 | for (int i=6; i<10 ; ++i){ 35 | P_(i,i) = 0.00001; 36 | } 37 | for (int i=10; i<15; ++i){ 38 | P_(i,i) = 0.000003; 39 | } 40 | // Initialisation of Q 41 | Q.setZero(); 42 | Q(0,0) = 0.00001; 43 | Q(1,1) = 0.00001; 44 | Q(2,2) = 0.000001; 45 | Q(3,3) = 0.038; 46 | Q(4,4) = 0.036; 47 | Q(5,5) = 0.0031; 48 | Q(10,10) = 0.0023; 49 | Q(11,11) = 0.0017; 50 | Q(12,12) = 0.0027; 51 | Q(13,13) = 0.0001; 52 | Q(14,14) = 0.0001; 53 | Q(15,15) = 0.0001; 54 | // Initialisation of H_GPS_POS 55 | H_GPS_POS.setZero(); 56 | for (int i=0; i<3; ++i){ 57 | H_GPS_POS(i,i) = 1; 58 | } 59 | // Initialisation of H_GPS_SPEED 60 | H_GPS_SPEED.setZero(); 61 | for (int i=3; i<6; ++i){ 62 | H_GPS_SPEED(i-3,i) = 1; 63 | } 64 | // Initialisation of H_MAG 65 | H_MAG.setZero(); 66 | // Initialisation of K 67 | K.setZero(); 68 | // Initialisation of Z 69 | Z.setZero(); 70 | // Initialisation of C 71 | _C.setZero(); 72 | } 73 | 74 | void EKF::start(){ 75 | drone->startThread(this, eKFthread); 76 | } 77 | 78 | 79 | EKF::EKF(Drone* drone_): drone(drone_){ 80 | IMUlistener = new Listener(IMUport, received); 81 | received = new BlockingQueue(); 82 | 83 | } 84 | 85 | void EKF::start(){ 86 | drone->startThread(this, eKFthread); 87 | } 88 | 89 | void* EKF::run(){ 90 | //ébauche de protocole de communication 91 | while(true){ 92 | if(working == 0){ 93 | drone->sendMsg(new Message(Message::SYSTEM, "Démarrage de l'IMU.", 0)); 94 | IMUlistener->write("Boot \n"); 95 | interpret(received->pop(10)); 96 | //mettre un timeout dans le pop 97 | 98 | } 99 | else{ 100 | interpret(received->pop(100)); 101 | //ici le temps entre deux actualisations (pour détecter un plantage de la ardupilot) 102 | } 103 | } 104 | } 105 | 106 | void EKF::interpret(char* Msg){ 107 | //interprétation du message reçu depuis l'APM 108 | } 109 | 110 | 111 | void EKF::init_state_vector(){ 112 | X_.setZero(); 113 | X_(6) = 1; 114 | // std::cout << X.transpose() << std::endl; 115 | for (int i=0; i<3; ++i){ 116 | X_(10+i) = (acc->getOffsets())(i); 117 | } 118 | for (int i=0; i<3; ++i){ 119 | X_(12+i) = (gyro->getOffsets())(i); 120 | } 121 | calculateDCM(); 122 | } 123 | 124 | 125 | 126 | void EKF::init_state_vector(double heading){ 127 | // We first calculate the quaternion linked with the heading 128 | float q0 = cos(heading/2); 129 | float q1 = 0; 130 | float q2 = 0; 131 | float q3 = sin(heading/2); 132 | X_ << 0,0,0,0,0,0,q0,q1,q2,q3,0,0,0,0,0,0; 133 | // Then we replace the 0 values by the actual offsets values 134 | for (int i=0; i<3; ++i){ 135 | X_(10+i) = (acc->getOffsets())(i); 136 | } 137 | for (int i=0; i<3; ++i){ 138 | X_(13+i) = (gyro->getOffsets())(i); 139 | } 140 | calculateDCM(); 141 | } 142 | 143 | 144 | 145 | Vector16f EKF::get_state_vector() const{ 146 | return X_; 147 | } 148 | 149 | 150 | 151 | void EKF::build_jacobian_matrix(Eigen::Vector3f* acc, Eigen::Vector3f* gyro){ 152 | 153 | // Position 154 | J.setZero(); 155 | 156 | for (int i=0; i<3; i++){ 157 | J(i,i+3) = dt; // Position = dt*speed 158 | } 159 | 160 | // Vitesse 161 | J(3,6) = 2*dt*(X_(6)*(*acc)(0)-X_(9)*(*acc)(1)+X_(8)*(*acc)(2)); 162 | J(3,7) = 2*dt*(X_(7)*(*acc)(0)+X_(8)*(*acc)(1)+X_(9)*(*acc)(2)); 163 | J(3,8) = 2*dt*(-X_(8)*(*acc)(0)+X_(7)*(*acc)(1)+X_(6)*(*acc)(2)); 164 | J(3,9) = 2*dt*(-X_(9)*(*acc)(0)-X_(6)*(*acc)(1)+X_(7)*(*acc)(2)); 165 | J(4,6) = 2*dt*(X_(9)*(*acc)(0)+X_(6)*(*acc)(1)-X_(7)*(*acc)(2)); 166 | J(4,7) = 2*dt*(X_(8)*(*acc)(0)-X_(7)*(*acc)(1)-X_(6)*(*acc)(2)); 167 | J(4,8) = 2*dt*(X_(7)*(*acc)(0)+X_(8)*(*acc)(1)+X_(9)*(*acc)(2)); 168 | J(4,9) = 2*dt*(X_(6)*(*acc)(0)-X_(9)*(*acc)(1)+X_(8)*(*acc)(2)); 169 | J(5,6) = 2*dt*(-X_(8)*(*acc)(0)+X_(7)*(*acc)(1)+X_(6)*(*acc)(2)); 170 | J(5,7) = 2*dt*(X_(9)*(*acc)(0)+X_(6)*(*acc)(1)-X_(7)*(*acc)(2)); 171 | J(5,8) = 2*dt*(-X_(6)*(*acc)(0)+X_(9)*(*acc)(1)-X_(8)*(*acc)(2)); 172 | J(5,9) = 2*dt*(X_(7)*(*acc)(0)+X_(8)*(*acc)(1)+X_(9)*(*acc)(2)); 173 | 174 | 175 | // Quaternion 176 | J(6,6) = 0; 177 | J(6,7) = -dt*(*gyro)(0)/2; 178 | J(6,8) = -dt*(*gyro)(1)/2; 179 | J(6,9) = -dt*(*gyro)(2)/2; 180 | J(7,6) = dt*(*gyro)(0)/2; 181 | J(7,7) = 0; 182 | J(7,8) = dt*(*gyro)(2)/2; 183 | J(7,9) = -dt*(*gyro)(1)/2; 184 | J(8,6) = dt*(*gyro)(1)/2; 185 | J(8,7) = -dt*(*gyro)(2)/2; 186 | J(8,8) = 0; 187 | J(8,9) = dt*(*gyro)(0)/2; 188 | J(9,6) = dt*(*gyro)(2)/2; 189 | J(9,7) = dt*(*gyro)(1)/2; 190 | J(9,8) = -dt*(*gyro)(0)/2; 191 | J(9,9) = 0; 192 | 193 | 194 | 195 | for (int i=0; i<16; i++){ 196 | J(i,i) += 1; 197 | } 198 | 199 | } 200 | 201 | 202 | 203 | Eigen::Vector3f EKF::toRPY(Vector16f vector){ 204 | float roll = atan2(2*(vector(6)*vector(7)+vector(8)*vector(9)),1-2*(vector(7)*vector(7)+vector(8)*vector(8))); 205 | float pitch = asin(2*(vector(6)*vector(8)-vector(9)*vector(7))); 206 | float yaw = atan2(2*(vector(6)*vector(9)+vector(7)*vector(8)),1-2*(vector(8)*vector(8)+vector(9)*vector(9))); 207 | 208 | Eigen::Vector3f RPY(roll,pitch,yaw); 209 | return RPY; 210 | } 211 | 212 | 213 | 214 | Eigen::Vector3f EKF::getRPY() const{ 215 | float roll = atan2(2*(X_(6)*X_(7)+X_(8)*X_(9)),1-2*(X_(7)*X_(7)+X_(8)*X_(8))); 216 | float pitch = asin(2*(X_(6)*X_(8)-X_(9)*X_(7))); 217 | float yaw = atan2(2*(X_(6)*X_(9)+X_(7)*X_(8)),1-2*(X_(8)*X_(8)+X_(9)*X_(9))); 218 | 219 | Eigen::Vector3f RPY(roll,pitch,yaw); 220 | return RPY; 221 | } 222 | 223 | 224 | 225 | Eigen::Vector3f EKF::getCurrentPos() const{ 226 | Eigen::Vector3f result; 227 | result << X_(0),X_(1),X_(2); 228 | return result; 229 | } 230 | 231 | 232 | 233 | Eigen::Vector3f EKF::getCurrentSpeed() const{ 234 | Eigen::Vector3f result; 235 | result << X_(3),X_(4),X_(5); 236 | return result; 237 | } 238 | 239 | 240 | 241 | Vector6f EKF::getActualInertialOffsets() const{ 242 | Vector6f result; 243 | result.setZero(); 244 | result << X_(10),X_(11),X_(12),X_(13),X_(14),X_(15); 245 | return result; 246 | } 247 | 248 | 249 | void EKF::predict(){ 250 | _X = J*X_; 251 | _P = J*P_*(J.transpose())+Q; 252 | X_=_X; 253 | P_ = _P; 254 | 255 | // We also actualize the rotation matrix 256 | calculateDCM(); 257 | } 258 | 259 | 260 | void EKF::updateMeasure(){ 261 | for (int i=0; i<3;++i){ 262 | Z(i) = (gps_filter->getState())(i+3)-_X(i); // Position 263 | } 264 | for (int i=3; i<6;++i){ 265 | Z(i) = (gps_filter->getState())(i-3)-_X(i); // Speed 266 | } 267 | for (int i=6; i<9;++i){ 268 | Z(i) = (mag->getState())(i-6); // Magnetic field 269 | } 270 | } 271 | 272 | 273 | void EKF::correct(){ 274 | 275 | //We actualize the measurement vector with the gps_filter datas 276 | updateMeasure(); 277 | //We create dimensionnaly acceptable alias 278 | Eigen::Vector3f Z_alias; 279 | Z_alias.setZero(); 280 | 281 | // We first correct the position 282 | for (int i=0; i<3; ++i){ 283 | Z_alias(i) = Z(i); 284 | } 285 | if ((H_GPS_POS*_P*(H_GPS_POS.transpose())+gps_filter->getPositionCovMatrix()).inverse().determinant() != 0){ 286 | K = _P*(H_GPS_POS.transpose())*((H_GPS_POS*_P*(H_GPS_POS.transpose())+gps_filter->getPositionCovMatrix()).inverse()); //Computes K 287 | } 288 | P_ = _P - K*H_GPS_POS*_P; //Computes P 289 | //Correct prediction 290 | X_ = _X + K*Z_alias; 291 | 292 | // We then correct the speed : 293 | for (int i=0; i<3; ++i){ 294 | Z_alias(i) = Z(i+3); 295 | } 296 | if ((H_GPS_SPEED*_P*(H_GPS_SPEED.transpose())+gps_filter->getSpeedCovMatrix()).inverse().determinant() != 0){ 297 | K = _P*(H_GPS_SPEED.transpose())*((H_GPS_SPEED*_P*(H_GPS_SPEED.transpose())+gps_filter->getSpeedCovMatrix()).inverse()); //Computes K 298 | } 299 | P_ = _P - K*H_GPS_SPEED*_P; //Computes P 300 | //Correct prediction 301 | X_ = _X + K*Z_alias; 302 | 303 | // Eventually we correct attitude via the magnetometer 304 | for (int i=0; i<3; ++i){ 305 | Z_alias(i) = Z(i+6); 306 | } 307 | // Computing H_MAG 308 | H_MAG(0,6) = (X_(6)*(mag->getEEMF())(0)+X_(9)*(mag->getEEMF())(1)-X_(8)*(mag->getEEMF())(2)); 309 | H_MAG(0,7) = (X_(7)*(mag->getEEMF())(0)+X_(8)*(mag->getEEMF())(1)+X_(9)*(mag->getEEMF())(2)); 310 | H_MAG(0,8) = (-X_(8)*(mag->getEEMF())(0)+X_(7)*(mag->getEEMF())(1)-X_(6)*(mag->getEEMF())(2)); 311 | H_MAG(0,9) = (-X_(9)*(mag->getEEMF())(0)+X_(6)*(mag->getEEMF())(1)+X_(7)*(mag->getEEMF())(2)); 312 | H_MAG(1,6) = (-X_(9)*(mag->getEEMF())(0)+X_(6)*(mag->getEEMF())(1)-X_(7)*(mag->getEEMF())(2)); 313 | H_MAG(1,7) = (X_(8)*(mag->getEEMF())(0)-X_(7)*(mag->getEEMF())(1)+X_(6)*(mag->getEEMF())(2)); 314 | H_MAG(1,8) = (X_(7)*(mag->getEEMF())(0)+X_(8)*(mag->getEEMF())(1)+X_(9)*(mag->getEEMF())(2)); 315 | H_MAG(1,9) = (-X_(6)*(mag->getEEMF())(0)-X_(9)*(mag->getEEMF())(1)+X_(8)*(mag->getEEMF())(2)); 316 | H_MAG(2,6) = (X_(8)*(mag->getEEMF())(0)-X_(7)*(mag->getEEMF())(1)+X_(6)*(mag->getEEMF())(2)); 317 | H_MAG(2,7) = (X_(9)*(mag->getEEMF())(0)+X_(6)*(mag->getEEMF())(1)-X_(7)*(mag->getEEMF())(2)); 318 | H_MAG(2,8) = (X_(6)*(mag->getEEMF())(0)+X_(9)*(mag->getEEMF())(1)-X_(8)*(mag->getEEMF())(2)); 319 | H_MAG(2,9) = (X_(7)*(mag->getEEMF())(0)+X_(8)*(mag->getEEMF())(1)+X_(9)*(mag->getEEMF())(2)); 320 | 321 | 322 | 323 | K = _P*(H_MAG.transpose())*((H_MAG*_P*(H_MAG.transpose())+mag->getMagCovarianceMatrix()).inverse()); 324 | //P_ = _P - K*H_MAG*_P; // Computes P TODO : need to understand why this affects so much the state vector correction for speed and position .. 325 | //Correct prediction 326 | //std::cout << Z_alias.transpose() << std::endl; 327 | //std::cout << (H_MAG*_X).transpose() << std::endl; 328 | for (int i=6; i<11; ++i){ 329 | X_(i) = (_X + K*(Z_alias-H_MAG*_X))(i); 330 | } 331 | } 332 | 333 | 334 | void EKF::calculateDCM(){ 335 | _C(0,0) = X_(6)*X_(6) + X_(7)*X_(7) - X_(8)*X_(8) + X_(9)*X_(9); 336 | _C(0,1) = 2*(X_(7)*X_(8) - X_(6)*X_(9)); 337 | _C(0,2) = 2*(X_(7)*X_(9) + X_(6)*X_(8)); 338 | _C(1,0) = 2*(X_(7)*X_(8) + X_(6)*X_(9)); 339 | _C(1,1) = X_(6)*X_(6) - X_(7)*X_(7) + X_(8)*X_(8) - X_(9)*X_(9); 340 | _C(1,2) = 2*(X_(8)*X_(9) - X_(6)*X_(7)); 341 | _C(2,0) = 2*(X_(7)*X_(9) - X_(6)*X_(8)); 342 | _C(2,1) = 2*(X_(8)*X_(9) + X_(6)*X_(7)); 343 | _C(2,2) = X_(6)*X_(6) - X_(7)*X_(7) - X_(8)*X_(8) + X_(9)*X_(9); 344 | } 345 | 346 | 347 | Eigen::Matrix3f EKF::getDCM(){ 348 | return _C; 349 | } 350 | 351 | 352 | 353 | 354 | 355 | EKF::~EKF(){ 356 | 357 | delete drone; 358 | delete IMUlistener; 359 | delete received; 360 | } 361 | 362 | 363 | --------------------------------------------------------------------------------