initial commit. Added code for InEKF propagation and correction using…

… landmarks. Untested.

.gitignore

@@ -1,2 +1,4 @@
 slprj/
 *.asv
+bin/
+build/

.vscode/c_cpp_properties.json

@@ -0,0 +1,23 @@
+{
+    "configurations": [
+        {
+            "name": "Win32",
+            "includePath": [
+                "${workspaceFolder}/**",
+                "/usr/include/c++/5.4.0/",
+                "/usr/include/eigen3/**",
+                "/usr/include/boost/**"
+            ],
+            "defines": [
+                "_DEBUG",
+                "UNICODE",
+                "_UNICODE"
+            ],
+            "windowsSdkVersion": "8.1",
+            "compilerPath": "/usr/bin/gcc",
+            "cStandard": "c11",
+            "intelliSenseMode": "msvc-x64"
+        }
+    ],
+    "version": 4
+}

.vscode/settings.json

@@ -0,0 +1,52 @@
+{
+    "files.associations": {
+        "iostream": "cpp",
+        "dense": "cpp",
+        "*.tcc": "cpp",
+        "fstream": "cpp",
+        "sstream": "cpp",
+        "array": "cpp",
+        "functional": "cpp",
+        "type_traits": "cpp",
+        "tuple": "cpp",
+        "utility": "cpp",
+        "core": "cpp",
+        "ostream": "cpp",
+        "iosfwd": "cpp",
+        "cmath": "cpp",
+        "complex": "cpp",
+        "deque": "cpp",
+        "vector": "cpp",
+        "cstddef": "cpp",
+        "cstdlib": "cpp",
+        "cctype": "cpp",
+        "clocale": "cpp",
+        "cstdint": "cpp",
+        "cstdio": "cpp",
+        "cstring": "cpp",
+        "ctime": "cpp",
+        "cwchar": "cpp",
+        "cwctype": "cpp",
+        "list": "cpp",
+        "unordered_map": "cpp",
+        "exception": "cpp",
+        "initializer_list": "cpp",
+        "istream": "cpp",
+        "limits": "cpp",
+        "memory": "cpp",
+        "new": "cpp",
+        "numeric": "cpp",
+        "stdexcept": "cpp",
+        "streambuf": "cpp",
+        "system_error": "cpp",
+        "typeinfo": "cpp",
+        "map": "cpp",
+        "atomic": "cpp",
+        "strstream": "cpp",
+        "chrono": "cpp",
+        "ratio": "cpp",
+        "thread": "cpp",
+        "cinttypes": "cpp",
+        "typeindex": "cpp"
+    }
+}

CMakeLists.txt

@@ -0,0 +1,49 @@
+# Specify the minimum version for CMake
+cmake_minimum_required(VERSION 2.8)
+set(CMAKE_CXX_STANDARD 11)
+#set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g") # Debugging
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}  -Wl,--no-as-needed") 
+
+# Project's name
+project(inekf)
+
+# Set the output folder where your program will be created
+set(CMAKE_BINARY_DIR ${CMAKE_SOURCE_DIR}/bin)
+set(EXECUTABLE_OUTPUT_PATH ${CMAKE_BINARY_DIR})
+set(LIBRARY_OUTPUT_PATH ${CMAKE_BINARY_DIR}/lib)
+
+############################################
+#  The following folders will be included  #
+############################################
+include_directories("${PROJECT_SOURCE_DIR}/include/")
+
+# Threading
+find_package (Threads)
+
+# Boost
+find_package(Boost 1.58 REQUIRED COMPONENTS system)
+include_directories(${Boost_INCLUDE_DIR})
+
+# Eigen 
+find_package(Eigen3 REQUIRED)
+include_directories(${EIGEN3_INCLUDE_DIR})
+
+# Adding all classes
+file(GLOB src_files
+    "${PROJECT_SOURCE_DIR}/src/*.cpp"
+)
+
+######################
+#  Add Execuatables  #
+######################
+link_directories(${PROJECT_SOURCE_DIR}/${Boost_LIBRARY_DIRS})
+
+add_executable(landmarks ${PROJECT_SOURCE_DIR}/src/examples/landmarks.cpp ${src_files})
+add_executable(propagation_speed ${PROJECT_SOURCE_DIR}/src/tests/propagation_speed.cpp ${src_files})
+
+target_link_libraries(landmarks ${Boost_LIBRARIES})
+target_link_libraries(propagation_speed ${Boost_LIBRARIES})
+
+
+
+

include/InEKF.h

@@ -0,0 +1,99 @@
+#ifndef INEKF_H
+#define INEKF_H 
+#include <Eigen/Dense>
+#include <Eigen/StdVector>
+#include <iostream>
+#include <vector>
+#include <map>
+#include "RobotState.h"
+#include "LieGroup.h"
+
+typedef std::map<int,Eigen::Vector3d,std::less<int>,Eigen::aligned_allocator<std::pair<const int,Eigen::Vector3d>>> mapIntVector3d;
+typedef std::vector<std::pair<int,Eigen::Vector3d>> vectorPairIntVector3d;
+
+class NoiseParams {
+
+    public:
+        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+        NoiseParams();
+
+        void setGyroscopeNoise(double std);
+        void setGyroscopeNoise(Eigen::Vector3d std);
+        void setGyroscopeNoise(Eigen::Matrix3d cov);
+
+        void setAccelerometerNoise(double std);
+        void setAccelerometerNoise(Eigen::Vector3d std);
+        void setAccelerometerNoise(Eigen::Matrix3d cov);  
+
+        void setGyroscopeBiasNoise(double std);
+        void setGyroscopeBiasNoise(Eigen::Vector3d std);
+        void setGyroscopeBiasNoise(Eigen::Matrix3d cov);
+
+        void setAccelerometerBiasNoise(double std);
+        void setAccelerometerBiasNoise(Eigen::Vector3d std);
+        void setAccelerometerBiasNoise(Eigen::Matrix3d cov);  
+
+        void setLandmarkNoise(double std);
+        void setLandmarkNoise(Eigen::Vector3d std);
+        void setLandmarkNoise(Eigen::Matrix3d cov);
+
+        Eigen::Matrix3d getGyroscopeCov();
+        Eigen::Matrix3d getAccelerometerCov();
+        Eigen::Matrix3d getGyroscopeBiasCov();
+        Eigen::Matrix3d getAccelerometerBiasCov();
+        Eigen::Matrix3d getLandmarkCov();
+
+    private:
+        Eigen::Matrix3d Qg_;
+        Eigen::Matrix3d Qa_;
+        Eigen::Matrix3d Qbg_;
+        Eigen::Matrix3d Qba_;
+        Eigen::Matrix3d Ql_;
+};
+
+
+
+
+class Observation {
+
+    public:
+        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+        Observation(Eigen::VectorXd& Y, Eigen::VectorXd& b, Eigen::MatrixXd& H, Eigen::MatrixXd& N, Eigen::MatrixXd& PI);
+        bool empty();
+
+        Eigen::VectorXd Y;
+        Eigen::VectorXd b;
+        Eigen::MatrixXd H;
+        Eigen::MatrixXd N;
+        Eigen::MatrixXd PI;
+
+        friend std::ostream& operator<<(std::ostream& os, const Observation& o);  
+
+
+};
+
+
+class InEKF {
+
+    public:
+        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+        InEKF();
+        InEKF(RobotState state);
+        InEKF(RobotState state, mapIntVector3d prior_landmarks);
+
+        RobotState getState();
+        void Propagate(const Eigen::Matrix<double,6,1>& m, double dt);
+        void Correct(const Observation& obs);
+        void CorrectLandmarks(const vectorPairIntVector3d measured_landmarks);
+
+    private:
+        RobotState state_;
+        NoiseParams noise_params_;
+        const Eigen::Vector3d g_ = (Eigen::VectorXd(3) << 0,0,-9.81).finished(); // Gravity
+        mapIntVector3d prior_landmarks_;
+        std::map<int,int> estimated_landmarks_;
+};
+
+
+
+#endif 

include/LieGroup.h

@@ -0,0 +1,13 @@
+#ifndef LIEGROUP_H
+#define LIEGROUP_H 
+#include <Eigen/Dense>
+#include <iostream>
+
+#define TOLERANCE 1e-10 
+
+Eigen::Matrix3d skew(const Eigen::Vector3d& v);
+Eigen::Matrix3d Exp_SO3(const Eigen::Vector3d& w);
+Eigen::MatrixXd Exp_SEK3(const Eigen::VectorXd& v);
+Eigen::MatrixXd Adjoint_SEK3(const Eigen::MatrixXd& X);
+
+#endif 

include/RobotState.h

@@ -0,0 +1,47 @@
+#ifndef ROBOTSTATE_H
+#define ROBOTSTATE_H 
+#include <Eigen/Dense>
+#include <iostream>
+
+class RobotState {
+
+    public:
+        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+        RobotState();
+        RobotState(const Eigen::MatrixXd& X);
+        RobotState(const Eigen::MatrixXd& X, const Eigen::VectorXd& Theta);
+        RobotState(const Eigen::MatrixXd& X, const Eigen::VectorXd& Theta, const Eigen::MatrixXd& P);
+        const Eigen::MatrixXd getX();
+        const Eigen::VectorXd getTheta();
+        const Eigen::MatrixXd getP();
+        const Eigen::Matrix3d getRotation();
+        const Eigen::Vector3d getVelocity();
+        const Eigen::Vector3d getPosition();
+        const Eigen::Vector3d getAngularVelocityBias();
+        const Eigen::Vector3d getLinearAccelerationBias();
+        const int dimX();
+        const int dimTheta();
+        const int dimP();
+
+        void setX(Eigen::MatrixXd& X);
+        void setP(Eigen::MatrixXd& P);
+        void setTheta(Eigen::VectorXd& Theta);
+        void setRotation(Eigen::Matrix3d& R);
+        void setVelocity(Eigen::Vector3d& v);
+        void setPosition(Eigen::Vector3d& p);
+        void setAngularVelocityBias(Eigen::Vector3d& bg);
+        void setLinearAccelerationBias(Eigen::Vector3d& ba);
+
+        void copyDiagX(int n, Eigen::MatrixXd& BigX);
+
+        friend class InEKF;
+        friend std::ostream& operator<<(std::ostream& os, const RobotState& s);  
+
+    private:
+        Eigen::MatrixXd X_;
+        Eigen::VectorXd Theta_;
+        Eigen::MatrixXd P_;
+
+};
+
+#endif