From: Walter Fetter Lages Date: Mon, 14 Dec 2020 06:12:01 +0000 (-0300) Subject: Change initialization to compute magnetic field and accelaration at rest in ENU frame. X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=4bf6e4f37f3eee5bbf15311fc1118ce7f94f41e0;p=imufusion.git Change initialization to compute magnetic field and accelaration at rest in ENU frame. --- diff --git a/Makefile b/Makefile index dbb5c20..d401c32 100644 --- a/Makefile +++ b/Makefile @@ -1,4 +1,5 @@ DIRS=lib test #doc +PREFIX?=/usr/local all: for i in $(DIRS); do $(MAKE) -C $$i; done @@ -25,10 +26,10 @@ distclean: rm -rf *~ *.bak install: - for i in $(DIRS); do $(MAKE) -C $$i install; done + for i in $(DIRS); do $(MAKE) -C $$i PREFIX=$(PREFIX) install; done uninstall: - for i in $(DIRS); do $(MAKE) -C $$i uninstall; done + for i in $(DIRS); do $(MAKE) -C $$i PREFIX=$(PREFIX) uninstall; done unpack: for i in $(DIRS); do $(MAKE) -C $$i unpack; done diff --git a/include/ekf.hpp b/include/ekf.hpp index 3a2bd69..faf53ad 100644 --- a/include/ekf.hpp +++ b/include/ekf.hpp @@ -1,7 +1,7 @@ /****************************************************************************** IMU Fusion Library Extended Kalman Filter - Copyright (C) 2019 Walter Fetter Lages + Copyright (C) 2019,2020 Walter Fetter Lages This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -38,12 +38,18 @@ namespace imufusion class Ekf { private: - const double Gz_; ///< Gravity acceleration const double T_; ///< Sampling period + int initSamples_; ///< Number of samples to use for initialization + int nSamples_; ///< Number of initialization samples executed Eigen::VectorXd x_; ///< Filter state Eigen::MatrixXd P_; ///< Covariance a priori Eigen::MatrixXd Pw_; ///< Covariance of process noise Eigen::MatrixXd Pv_; ///< Covariance of observation noise + Eigen::Vector3d accel_; ///< Acceleration average while initializing + Eigen::Vector3d gyro_; ///< Gyroscope average while initializeing + Eigen::Vector3d mag_; ///< Magnetic field average while initializing + Eigen::Vector3d accel0_; ///< Accleration at rest in ENU frame + Eigen::Vector3d mag0_; ///< Magnetic field in ENU frame /** @brief Quaternion transition function \f${}^0\dot{Q}_g(t)=\frac{1}{2} \Xi\left({}^0Q_g(t)\right) {}^g\omega\f$ @@ -76,14 +82,14 @@ class Ekf public: + /** @brief Constructs an extended Kalman filter. - @param x0 Inital orientation. - @param P0 Initial a priori covariance of estimation error @param Pw Covariance of process noise @param Pv Covariance of observation noise @param T Sampling period + @param initSamples Number of samples to use to compute initial estimate and covariance */ - Ekf(const Eigen::Quaterniond &x0,const Eigen::MatrixXd &P0,const Eigen::MatrixXd &Pw,const Eigen::MatrixXd &Pv,double T); + Ekf(const Eigen::MatrixXd &Pw,const Eigen::MatrixXd &Pv,double T,int initSamples=50); /** @brief Destructor of an extended Kalman filter. */ diff --git a/include/imu.hpp b/include/imu.hpp index a72db0e..c6e1dac 100644 --- a/include/imu.hpp +++ b/include/imu.hpp @@ -85,6 +85,9 @@ class Imu */ Exception(const std::string& message): std::runtime_error(message) {} }; + + // Force 16-byte aligment of fixed-size vectorizable Eigen types, + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; diff --git a/include/lsm9ds1.hpp b/include/lsm9ds1.hpp index 0629285..1bc76bf 100644 --- a/include/lsm9ds1.hpp +++ b/include/lsm9ds1.hpp @@ -81,6 +81,9 @@ class Lsm9ds1: public Imu with magnetometer axis. */ Eigen::Vector3d gyro(void); + + // Force 16-byte aligment of fixed-size vectorizable Eigen types, + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; } diff --git a/lib/Makefile b/lib/Makefile index 3c298f6..1cb0432 100644 --- a/lib/Makefile +++ b/lib/Makefile @@ -1,7 +1,7 @@ TARGET=libimufusion.a SRCS=imu.cpp bno055.cpp lsm9ds1.cpp bno080.cpp ekf.cpp -FLAGS=-O2 -Wall -MMD -std=c++11 +FLAGS=-O2 -Wall -MMD -std=c++11 -fPIC INCLUDE=-I../include \ -I../../bno055/BNO055_driver -I../../bno055/bno055/include \ -I../../lsm9ds1/lsm9ds1_STdC/driver -I../../lsm9ds1/lsm9ds1/include \ @@ -9,6 +9,7 @@ INCLUDE=-I../include \ -I /usr/include/eigen3 LIBDIR= LIBS= +PREFIX?=/usr/local CXX=$(CROSS_COMPILE)g++ CCAR=$(CROSS_COMPILE)ar @@ -31,3 +32,8 @@ clean: distclean: clean rm -f $(TARGET) + +install: + install -d $(PREFIX)/lib$(BITS) $(PREFIX)/include + install -m 644 $(TARGET) $(PREFIX)/lib$(BITS) + install -m 644 ../include/ekf.hpp $(PREFIX)/include diff --git a/lib/ekf.cpp b/lib/ekf.cpp index 9889afe..4e157dd 100644 --- a/lib/ekf.cpp +++ b/lib/ekf.cpp @@ -1,7 +1,7 @@ /****************************************************************************** IMU Fusion Library Extended Kalman Filter - Copyright (C) 2019 Walter Fetter Lages + Copyright (C) 2019, 2020 Walter Fetter Lages This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -24,15 +24,21 @@ namespace imufusion { -Ekf::Ekf(const Eigen::Quaterniond &q0,const Eigen::MatrixXd &P0,const Eigen::MatrixXd &Pw,const Eigen::MatrixXd &Pv,double T): Gz_(-9.81), T_(T), x_(7), P_(P0), Pw_(Pw),Pv_(Pv) +Ekf::Ekf(const Eigen::MatrixXd &Pw,const Eigen::MatrixXd &Pv,double T,int initSamples): T_(T), initSamples_(initSamples), nSamples_(1), x_(7), P_(7,7), Pw_(Pw),Pv_(Pv) { - Eigen::Vector3d gyroBias; - gyroBias.setZero(); - x_ << q0.w(), - q0.x(), - q0.y(), - q0.z(), - gyroBias; + P_.setIdentity(); + P_*=1e6; + + x_.setZero(); + x_[0]=1.0; + + mag0_ << 0, + 1, + 0; + + accel_.setZero(); + gyro_.setZero(); + mag_.setZero(); } Ekf::~Ekf(void) @@ -42,6 +48,70 @@ Ekf::~Ekf(void) void Ekf::update(const Eigen::Vector3d &acc,const Eigen::Vector3d &mag,const Eigen::Vector3d &gyro) { + if(nSamples_ <= initSamples_) + { + Eigen::Vector3d z=acc; + z.normalize(); + Eigen::Vector3d x=mag.cross(z); + x.normalize(); + Eigen::Vector3d y=z.cross(x); + y.normalize(); + + Eigen::Matrix3d R; + R << x.transpose(), + y.transpose(), + z.transpose(); + + Eigen::Quaterniond q=Eigen::Quaterniond(R); + + Eigen::VectorXd xi(7); + + xi << q.w(), + q.x(), + q.y(), + q.z(), + gyro; + + if(nSamples_ >=2) P_=(nSamples_-2.0)/(nSamples_-1.0)*P_+1.0/nSamples_*(xi-x_)*(xi-x_).transpose(); + + x_=(nSamples_-1.0)/nSamples_*x_+1.0/nSamples_*xi; + + accel_=(nSamples_-1.0)/nSamples_*accel_+1.0/nSamples_*acc; + gyro_=(nSamples_-1.0)/nSamples_*gyro_+1.0/nSamples_*gyro; + mag_=(nSamples_-1.0)/nSamples_*mag_+1.0/nSamples_*mag; + + accel0_=R*accel_; + mag0_=R*mag_; + + if(nSamples_==initSamples_) + { + z=accel_; + z.normalize(); + x=mag_.cross(z); + x.normalize(); + y=z.cross(x); + y.normalize(); + + R << x.transpose(), + y.transpose(), + z.transpose(); + + q=Eigen::Quaterniond(R); + + x_ << q.w(), + q.x(), + q.y(), + q.z(), + gyro_; + + mag0_=R*mag_; + accel0_=R*accel_; + } + + nSamples_++; + return; + } + // Observation Eigen::VectorXd y(acc.rows()+mag.rows()); y << acc, @@ -52,6 +122,7 @@ void Ekf::update(const Eigen::Vector3d &acc,const Eigen::Vector3d &mag,const Eig // State estimation Eigen::VectorXd x=x_+K*(y-h()); + x.head<4>().normalize(); // Covariance a posteriori Eigen::MatrixXd P=(Eigen::MatrixXd::Identity(7,7)-K*H())*P_; @@ -62,11 +133,12 @@ void Ekf::update(const Eigen::Vector3d &acc,const Eigen::Vector3d &mag,const Eig // State prediction x_=f(u); // Should be the last operation because it changes x_ + x_.head<4>().normalize(); } Eigen::Quaterniond Ekf::quaternion(void) { - Eigen::Quaterniond q(x_(0),x_(1),x_(2),x_(3)); + Eigen::Quaterniond q(x_[0],x_[1],x_[2],x_[3]); // Do not use x_.head<>(). Internal order of elements is different. q.normalize(); return q; } @@ -122,25 +194,58 @@ Eigen::MatrixXd Ekf::F(const Eigen::Vector3d u) Eigen::VectorXd Ekf::h(void) { + Eigen::Matrix3d R=Eigen::Quaterniond(x_[0],x_[1],x_[2],x_[3]).matrix(); // Do not use x_.head<>(). Internal order of elements is different. + Eigen::Vector3d ya=R.transpose()*accel0_; + Eigen::Vector3d ym=R.transpose()*mag0_; + Eigen::VectorXd h(6); - h << -2*Gz_*(x_(1)*x_(3)-x_(2)*x_(0)), - -2*Gz_*(x_(2)*x_(3)+x_(1)*x_(0)), - -Gz_*(x_(0)*x_(0)-x_(1)*x_(1)-x_(2)*x_(2)+x_(3)*x_(3)), - 2*(x_(1)*x_(2)+x_(3)*x_(0)), - x_(0)*x_(0)-x_(1)*x_(1)+x_(2)*x_(2)-x_(3)*x_(3), - 2*(x_(2)*x_(3)-x_(1)*x_(0)); + h << ya, + ym; + return h; } Eigen::MatrixXd Ekf::H(void) { Eigen::MatrixXd H(6,7); - H << 2*Gz_*x_(2), -2*Gz_*x_(3), 2*Gz_*x_(0), -2*Gz_*x_(1), Eigen::MatrixXd::Zero(1,3), - -2*Gz_*x_(1), -2*Gz_*x_(0), -2*Gz_*x_(3), -2*Gz_*x_(2), Eigen::MatrixXd::Zero(1,3), - -2*Gz_*x_(0), 2*Gz_*x_(1), 2*Gz_*x_(2), -2*Gz_*x_(3), Eigen::MatrixXd::Zero(1,3), - 2*x_(3), 2*x_(2), 2*x_(1), 2*x_(0), Eigen::MatrixXd::Zero(1,3), - 2*x_(0), -2*x_(1), 2*x_(2), -2*x_(3), Eigen::MatrixXd::Zero(1,3), - -2*x_(1), -2*x_(0), 2*x_(3), 2*x_(2), Eigen::MatrixXd::Zero(1,3); + + H << 2*x_(0)*accel0_(0)+2*x_(3)*accel0_(1)-2*x_(2)*accel0_(2), + 2*x_(1)*accel0_(0)+2*x_(2)*accel0_(1)+2*x_(3)*accel0_(2), + -2*x_(2)*accel0_(0)+2*x_(1)*accel0_(1)-2*x_(0)*accel0_(2), + -2*x_(3)*accel0_(0)+2*x_(0)*accel0_(1)+2*x_(1)*accel0_(2), + Eigen::MatrixXd::Zero(1,3), + + -2*x_(3)*accel0_(0)+2*x_(0)*accel0_(1)+2*x_(1)*accel0_(2), + 2*x_(2)*accel0_(0)-2*x_(1)*accel0_(1)+2*x_(0)*accel0_(2), + 2*x_(1)*accel0_(0)+2*x_(2)*accel0_(1)+2*x_(3)*accel0_(2), + -2*x_(0)*accel0_(0)-2*x_(3)*accel0_(1)+2*x_(2)*accel0_(2), + Eigen::MatrixXd::Zero(1,3), + + 2*x_(2)*accel0_(0)-2*x_(1)*accel0_(1)+2*x_(0)*accel0_(2), + 2*x_(3)*accel0_(0)-2*x_(0)*accel0_(1)-2*x_(1)*accel0_(2), + 2*x_(0)*accel0_(0)+2*x_(3)*accel0_(1)-2*x_(2)*accel0_(2), + 2*x_(1)*accel0_(0)+2*x_(2)*accel0_(1)+2*x_(3)*accel0_(2), + Eigen::MatrixXd::Zero(1,3), + + + 2*x_(0)*mag0_(0)+2*x_(3)*mag0_(1)-2*x_(2)*mag0_(2), + 2*x_(1)*mag0_(0)+2*x_(2)*mag0_(1)+2*x_(3)*mag0_(2), + -2*x_(2)*mag0_(0)+2*x_(1)*mag0_(1)-2*x_(0)*mag0_(2), + -2*x_(3)*mag0_(0)+2*x_(0)*mag0_(1)+2*x_(1)*mag0_(2), + Eigen::MatrixXd::Zero(1,3), + + -2*x_(3)*mag0_(0)+2*x_(0)*mag0_(1)+2*x_(1)*mag0_(2), + 2*x_(2)*mag0_(0)-2*x_(1)*mag0_(1)+2*x_(0)*mag0_(2), + 2*x_(1)*mag0_(0)+2*x_(2)*mag0_(1)+2*x_(3)*mag0_(2), + -2*x_(0)*mag0_(0)-2*x_(3)*mag0_(1)+2*x_(2)*mag0_(2), + Eigen::MatrixXd::Zero(1,3), + + 2*x_(2)*mag0_(0)-2*x_(1)*mag0_(1)+2*x_(0)*mag0_(2), + 2*x_(3)*mag0_(0)-2*x_(0)*mag0_(1)-2*x_(1)*mag0_(2), + 2*x_(0)*mag0_(0)+2*x_(3)*mag0_(1)-2*x_(2)*mag0_(2), + 2*x_(1)*mag0_(0)+2*x_(2)*mag0_(1)+2*x_(3)*mag0_(2), + Eigen::MatrixXd::Zero(1,3); + return H; } diff --git a/test/Makefile b/test/Makefile index bf6b844..06d27b4 100644 --- a/test/Makefile +++ b/test/Makefile @@ -35,3 +35,5 @@ clean: distclean: clean rm -f $(TARGET) + +install: diff --git a/test/ekf_test.cpp b/test/ekf_test.cpp index eee57ba..c515582 100644 --- a/test/ekf_test.cpp +++ b/test/ekf_test.cpp @@ -1,7 +1,7 @@ /****************************************************************************** IMU Fusion Library Extended Kalman Filter Test - Copyright (C) 2019 Walter Fetter Lages + Copyright (C) 2019, 2020 Walter Fetter Lages This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -177,13 +177,6 @@ int main(int argc,char *argv[]) return -1; } - Eigen::Quaterniond q0; - q0.setIdentity(); - - Eigen::MatrixXd P0(7,7); - P0.setIdentity(); - P0*=1e5; - const double T=0.02; // 0.01 is too fast for an UART at 115200 bps Eigen::MatrixXd Pw(7,7); @@ -237,7 +230,7 @@ int main(int argc,char *argv[]) break; } - imufusion::Ekf ekf(q0,P0,Pw,Pv,T); + imufusion::Ekf ekf(Pw,Pv,T); struct timeval tv0; gettimeofday(&tv0,NULL);