Change initialization to compute magnetic field and accelaration at rest in ENU frame.

diff --git a/Makefile b/Makefile
index dbb5c20..d401c32 100644 (file)
--- 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 (file)
--- a/include/ekf.hpp
+++ b/include/ekf.hpp
@@ -1,7 +1,7 @@
 /******************************************************************************
                                IMU Fusion Library
                              Extended Kalman Filter
-          Copyright (C) 2019 Walter Fetter Lages <w.fetter@ieee.org>
+          Copyright (C) 2019,2020 Walter Fetter Lages <w.fetter@ieee.org>
 
         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 (file)
--- 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 (file)
--- 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 (file)
--- 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 (file)
--- a/lib/ekf.cpp
+++ b/lib/ekf.cpp
@@ -1,7 +1,7 @@
 /******************************************************************************
                                IMU Fusion Library
                              Extended Kalman Filter
-          Copyright (C) 2019 Walter Fetter Lages <w.fetter@ieee.org>
+          Copyright (C) 2019, 2020 Walter Fetter Lages <w.fetter@ieee.org>
 
         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 (file)
--- 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 (file)
--- 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 <w.fetter@ieee.org>
+          Copyright (C) 2019, 2020 Walter Fetter Lages <w.fetter@ieee.org>
 
         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);