Merge pull request #1 from OmarHarib/devel-c98

Devel c98

.gitignore

@@ -1,4 +1,6 @@
-<<<<<<< HEAD
+# Temp files
+*~
+
 # Prerequisites
 *.d
 

CMakeLists.txt

@@ -3,10 +3,26 @@ cmake_minimum_required(VERSION 2.8.3)
 # Project's name
 project(inekf)
 
+option(USE_CPP11 "Use flag -std=c++11" ON)
+option(USE_MUTEX "Use mutex in code" ON)
+
+if (USE_CPP11 EQUAL OFF AND USE_MUTEX EQUAL ON)
+    message(FATAL_ERROR "You cannot have USE_CPP11 = OFF AND USE_MUTEX = ON")
+endif (USE_CPP11 EQUAL OFF AND USE_MUTEX EQUAL ON)
+
 # Set compiler flags
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -O3 -DEIGEN_NO_DEBUG -march=native -Wl,--no-as-needed") 
+if (USE_CPP11)
+    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+endif(USE_CPP11)
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -DEIGEN_NO_DEBUG -march=native -Wl,--no-as-needed")
+if (USE_MUTEX)
+    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DINEKF_USE_MUTEX=true")
+else (USE_MUTEX)
+    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DINEKF_USE_MUTEX=false")
+endif(USE_MUTEX)
 SET(CMAKE_CXX_COMPILER /usr/bin/g++)
 
+message("CMAKE_CXX_FLAGS: " ${CMAKE_CXX_FLAGS})
 
 # Set the output folder where your program will be created  
 set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/bin)
@@ -21,7 +37,7 @@ include_directories("${PROJECT_SOURCE_DIR}/include/")
 find_package (Threads)
 
 # Boost
-find_package(Boost 1.58 REQUIRED COMPONENTS system)
+find_package(Boost 1.57 REQUIRED COMPONENTS system)
 include_directories(${Boost_INCLUDE_DIR})
 message("Boost_INCLUDE_DIR: " ${Boost_INCLUDE_DIR})
 
@@ -47,6 +63,7 @@ add_executable(correction_speed ${PROJECT_SOURCE_DIR}/src/tests/correction_speed
 
 target_link_libraries(landmarks ${Boost_LIBRARIES})
 target_link_libraries(kinematics ${Boost_LIBRARIES})
+target_link_libraries(propagation_speed ${Boost_LIBRARIES})
 target_link_libraries(correction_speed ${Boost_LIBRARIES})
 
 # Create Library

README.md

@@ -1,2 +1,67 @@
-# invariant-ekf
-C++ library to implement invariant extended Kalman filtering for aided inertial navigation.  
+# inekf
+This repository contains a C++ library that implements an invariant extended Kalman filter (InEKF) for 3D aided inertial navigation. 
+
+This filter can be used to estimate a robot's 3D pose and velocity using an IMU motion model for propagation. The following measurements are currently supported:
+* Prior landmark position measurements (localization)
+* Estiamted landmark position measurements (SLAM)
+* Kinematic and contact measurements
+
+The core theory was developed by Barrau and Bonnabel and is presented in:
+["The Invariant Extended Kalman filter as a Stable Observer"](https://arxiv.org/abs/1410.1465).
+
+Inclusion of kinematic and contact measurements is presented in:
+["Contact-aided Invariant Extended Kalman Filtering for Legged Robot State Estimation"](https://arxiv.org/pdf/1805.10410.pdf).
+
+A ROS wrapper for the filter is available at [https://github.com/RossHartley/invariant-ekf-ros](https://github.com/RossHartley/invariant-ekf-ros).
+
+## Setup
+### Requirements
+* CMake 2.8.3 or later
+* g++ 5.4.0 or later
+* [Eigen3 C++ Library](http://eigen.tuxfamily.org/index.php?title=Main_Page)
+
+### Installation Using CMake
+```
+mkdir build
+cd build 
+cmake .. 
+make
+``` 
+invariant-ekf can be easily included in your cmake project by adding the following to your CMakeLists.txt:
+```
+find_package(inekf) 
+include_directories(${inekf_INCLUDE_DIRS})
+```
+
+## Examples
+1. A landmark-aided inertial navigation example is provided at `src/examples/landmarks.cpp`
+2. A contact-aided inertial navigation example is provided at `src/examples/kinematics.cpp`
+
+## Citations
+The contact-aided invariant extended Kalman filter is described in: 
+* R. Hartley, M. G. Jadidi, J. Grizzle, and R. M. Eustice, “Contact-aided invariant extended kalman filtering for legged robot state estimation,” in Proceedings of Robotics: Science and Systems, Pittsburgh, Pennsylvania, June 2018.
+```
+@INPROCEEDINGS{Hartley-RSS-18, 
+    AUTHOR    = {Ross Hartley AND Maani Ghaffari Jadidi AND Jessy Grizzle AND Ryan M Eustice}, 
+    TITLE     = {Contact-Aided Invariant Extended Kalman Filtering for Legged Robot State Estimation}, 
+    BOOKTITLE = {Proceedings of Robotics: Science and Systems}, 
+    YEAR      = {2018}, 
+    ADDRESS   = {Pittsburgh, Pennsylvania}, 
+    MONTH     = {June}, 
+    DOI       = {10.15607/RSS.2018.XIV.050} 
+} 
+```
+The core theory of invariant extended Kalman filtering is presented in:
+* Barrau, Axel, and Silvère Bonnabel. "The invariant extended Kalman filter as a stable observer." IEEE Transactions on Automatic Control 62.4 (2017): 1797-1812.
+```
+@article{barrau2017invariant,
+  title={The invariant extended Kalman filter as a stable observer},
+  author={Barrau, Axel and Bonnabel, Silv{\`e}re},
+  journal={IEEE Transactions on Automatic Control},
+  volume={62},
+  number={4},
+  pages={1797--1812},
+  year={2017},
+  publisher={IEEE}
+}
+```

include/InEKF.h

@@ -18,8 +18,9 @@
 #include <iostream>
 #include <vector>
 #include <map>
-#include <tuple>
+#if INEKF_USE_MUTEX
 #include <mutex>
+#endif
 #include <algorithm>
 #include "RobotState.h"
 #include "NoiseParams.h"
@@ -88,7 +89,7 @@ class InEKF {
         void setState(RobotState state);
         void setNoiseParams(NoiseParams params);
         void setPriorLandmarks(const mapIntVector3d& prior_landmarks);
-        void setContacts(std::vector<std::pair<int,bool>> contacts);
+        void setContacts(std::vector<std::pair<int,bool> > contacts);
 
         void Propagate(const Eigen::Matrix<double,6,1>& m, double dt);
         void Correct(const Observation& obs);
@@ -98,13 +99,15 @@ class InEKF {
     private:
         RobotState state_;
         NoiseParams noise_params_;
-        const Eigen::Vector3d g_ = (Eigen::VectorXd(3) << 0,0,-9.81).finished(); // Gravity
+        const Eigen::Vector3d g_; // Gravity
         mapIntVector3d prior_landmarks_;
         std::map<int,int> estimated_landmarks_;
-        std::mutex estimated_landmarks_mutex_;
         std::map<int,bool> contacts_;
         std::map<int,int> estimated_contact_positions_;
+#if INEKF_USE_MUTEX
         std::mutex estimated_contacts_mutex_;
+        std::mutex estimated_landmarks_mutex_;
+#endif
 };
 
 } // end inekf namespace

include/RobotState.h

@@ -15,9 +15,9 @@
 #define ROBOTSTATE_H 
 #include <Eigen/Dense>
 #include <iostream>
-#include <thread>
+#if INEKF_USE_MUTEX
 #include <mutex>
-#include <condition_variable>
+#endif
 
 namespace inekf {
 
@@ -30,10 +30,12 @@ class RobotState {
         RobotState(const Eigen::MatrixXd& X, const Eigen::VectorXd& Theta);
         RobotState(const Eigen::MatrixXd& X, const Eigen::VectorXd& Theta, const Eigen::MatrixXd& P);
 
+#if INEKF_USE_MUTEX
         // RobotState(RobotState&& other); // Move initialization
         RobotState(const RobotState& other); // Copy initialization
         // RobotState& operator=(RobotState&& other); // Move assignment
         RobotState& operator=(const RobotState& other); // Copy assignment
+#endif
 
         const Eigen::MatrixXd getX();
         const Eigen::VectorXd getTheta();
@@ -64,7 +66,9 @@ class RobotState {
         Eigen::MatrixXd X_;
         Eigen::VectorXd Theta_;
         Eigen::MatrixXd P_;
+#if INEKF_USE_MUTEX
         mutable std::mutex mutex_;
+#endif
 
 };
 

matlab_example/README.md

@@ -0,0 +1,44 @@
+## Running the Matlab example
+1. Open Matlab to the "matlab_example" folder.
+
+2. Run the scipt "run_RIEKF_test.m". This will open and run a simulink model with the measurement data stored in the "/data" folder. After the simulation finishes, a few plots will appear analyzing the results of the state estimator.   
+
+3. The filter parameters can be changed in the "example_code/RIEKF_InitFcn.m" script. This script is automatically executed when the simulink model is run.
+
+### Tunable Parameters
+The following parameters will affect the actual noisy measurements coming into the filter:
+* `gyro_true_bias_init` - Initial gyroscope bias 
+* `accel_true_bias_init` - Initial accelerometer bias 
+* `gyro_true_noise_std` - Standard deviation of noise added to the gyroscope measurement 
+* `gyro_true_bias_noise_std` - Standard deviation of noise added to the gyroscope bias 
+* `accel_true_noise_std` - Standard deviation of noise added to the accelerometer measurement 
+* `accel_true_bias_noise_std` - Standard deviation of noise added to the accelerometer bias 
+* `landmark_true_noise_std` - Standard deviation of noise added to the landmark measurements 
+* `landmark_positions` - List of landmark positions with an associated ID
+* `landmark_measurement_frequency` - Frequency of incoming landmark measurements
+
+The following parameters will affect how the filter is run:
+* `static_bias_initialization` - Flag that enables static bias initialization, where the initial bias estimate is obtained from the first few seconds of data assuming the base pose remains fixed. Keep this flag to false for the included dataset.
+* `ekf_update_enabled` - Flag that enables the update phase of the Kalman filter.
+* `enable_kinematic_measurements` - Flag that enables kinematic measurements.
+* `enable_landmark_measurements` - Flag that enables landmark measurements.
+* `enable_static_landmarks` - Flag that enables static landmarks. If false, the landmark positions will be estimated along with the rest of the state variables.
+
+The following parameters affect the initial condition and covariances used for the process and measurement models:
+* `gyro_bias_init` - Initial gyroscope bias estimate
+* `accel_bias_init` - Initial accelerometer bias estimate
+* `gyro_noise_std` - Standard deviation of the gyroscope measurement noise
+* `gyro_bias_noise_std` - Standard deviation of the gyroscope bias noise
+* `accel_noise_std` - Standard deviation of the accelerometer measurement noise
+* `accel_bias_noise_std` - Standard deviation the accelerometer bias noise
+* `contact_noise_std` - Standard deviation of the contact frame velocity measurement noise
+* `encoder_noise_std` - Standard deviation of the joint encoder measurement noise
+* `landmark_noise_std` - Standard deviation of the landmark measurement noise
+
+The following parameters set the initial covariance for the state estimate:
+* `prior_base_pose_std` - Initial base orientation and position standard deviation
+* `prior_base_velocity_std` - Initial base velocity standard deviation
+* `prior_contact_position_std` - Initial contact position standard deviation
+* `prior_gyro_bias_std` - Initial gyroscope bias standard deviation
+* `prior_accel_bias_std` - Initial accelerometer bias standard deviation
+* `prior_forward_kinematics_std` - Additional noise term that is added to increase the forward kinematics measurement covariance

src/InEKF.cpp

@@ -40,16 +40,16 @@ ostream& operator<<(ostream& os, const Observation& o) {
 
 // ------------ InEKF -------------
 // Default constructor
-InEKF::InEKF() {}
+InEKF::InEKF() : g_((Eigen::VectorXd(3) << 0,0,-9.81).finished()){}
 
 // Constructor with noise params
-InEKF::InEKF(NoiseParams params) : noise_params_(params) {}
+InEKF::InEKF(NoiseParams params) : g_((Eigen::VectorXd(3) << 0,0,-9.81).finished()), noise_params_(params) {}
 
 // Constructor with initial state
-InEKF::InEKF(RobotState state) : state_(state) {}
+InEKF::InEKF(RobotState state) : g_((Eigen::VectorXd(3) << 0,0,-9.81).finished()), state_(state) {}
 
 // Constructor with initial state and noise params
-InEKF::InEKF(RobotState state, NoiseParams params) : state_(state), noise_params_(params) {}
+InEKF::InEKF(RobotState state, NoiseParams params) : g_((Eigen::VectorXd(3) << 0,0,-9.81).finished()), state_(state), noise_params_(params) {}
 
 // Return robot's current state
 RobotState InEKF::getState() { 
@@ -83,19 +83,25 @@ void InEKF::setPriorLandmarks(const mapIntVector3d& prior_landmarks) {
 
 // Return filter's estimated landmarks
 map<int,int> InEKF::getEstimatedLandmarks() { 
+#if INEKF_USE_MUTEX
     lock_guard<mutex> mlock(estimated_landmarks_mutex_);
+#endif
     return estimated_landmarks_; 
 }
 
 // Return filter's estimated landmarks
 map<int,int> InEKF::getEstimatedContactPositions() { 
+#if INEKF_USE_MUTEX
     lock_guard<mutex> mlock(estimated_contacts_mutex_);
+#endif
     return estimated_contact_positions_; 
 }
 
 // Set the filter's contact state
 void InEKF::setContacts(vector<pair<int,bool> > contacts) {
+#if INEKF_USE_MUTEX
     lock_guard<mutex> mlock(estimated_contacts_mutex_);
+#endif
     // Insert new measured contact states
     for (vector<pair<int,bool> >::iterator it=contacts.begin(); it!=contacts.end(); ++it) {
         pair<map<int,bool>::iterator,bool> ret = contacts_.insert(*it);
@@ -109,7 +115,9 @@ void InEKF::setContacts(vector<pair<int,bool> > contacts) {
 
 // Return the filter's contact state
 std::map<int,bool> InEKF::getContacts() {
+#if INEKF_USE_MUTEX
     lock_guard<mutex> mlock(estimated_contacts_mutex_);
+#endif
     return contacts_; 
 }
 
@@ -214,7 +222,9 @@ void InEKF::Correct(const Observation& obs) {
 
 // Create Observation from vector of landmark measurements
 void InEKF::CorrectLandmarks(const vectorLandmarks& measured_landmarks) {
+#if INEKF_USE_MUTEX
     lock_guard<mutex> mlock(estimated_landmarks_mutex_);
+#endif
     Eigen::VectorXd Y;
     Eigen::VectorXd b;
     Eigen::MatrixXd H;
@@ -371,7 +381,9 @@ void InEKF::CorrectLandmarks(const vectorLandmarks& measured_landmarks) {
 
 // Correct state using kinematics measured between imu and contact point
 void InEKF::CorrectKinematics(const vectorKinematics& measured_kinematics) {
+#if INEKF_USE_MUTEX
     lock_guard<mutex> mlock(estimated_contacts_mutex_);
+#endif
     Eigen::VectorXd Y;
     Eigen::VectorXd b;
     Eigen::MatrixXd H;
@@ -545,4 +557,4 @@ void removeRowAndColumn(Eigen::MatrixXd& M, int index) {
     M.conservativeResize(dimX-1,dimX-1);
 }
 
-} // end inekf namespace
+} // end inekf namespace