From 0238147261b7a0c52d9e3e1a2223366fe7b82de6 Mon Sep 17 00:00:00 2001 From: RossHartley Date: Mon, 10 Sep 2018 17:05:41 -0400 Subject: [PATCH] initial commit. Added code for InEKF propagation and correction using landmarks. Untested. --- .gitignore | 2 + .vscode/c_cpp_properties.json | 23 + .vscode/settings.json | 52 + CMakeLists.txt | 49 + include/InEKF.h | 99 + include/LieGroup.h | 13 + include/RobotState.h | 47 + src/InEKF.cpp | 319 + src/LieGroup.cpp | 64 + src/RobotState.cpp | 65 + src/data/landmark_measurements.txt | 16 + src/data/propagation_speed_test_data.txt | 10000 +++++++++++++++++++++ src/examples/landmarks.cpp | 95 + src/tests/propagation_speed.cpp | 83 + 14 files changed, 10927 insertions(+) create mode 100644 .vscode/c_cpp_properties.json create mode 100644 .vscode/settings.json create mode 100644 CMakeLists.txt create mode 100644 include/InEKF.h create mode 100644 include/LieGroup.h create mode 100644 include/RobotState.h create mode 100644 src/InEKF.cpp create mode 100644 src/LieGroup.cpp create mode 100644 src/RobotState.cpp create mode 100644 src/data/landmark_measurements.txt create mode 100644 src/data/propagation_speed_test_data.txt create mode 100644 src/examples/landmarks.cpp create mode 100644 src/tests/propagation_speed.cpp diff --git a/.gitignore b/.gitignore index 2025580..159f469 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,4 @@ slprj/ *.asv +bin/ +build/ \ No newline at end of file diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json new file mode 100644 index 0000000..fb53d18 --- /dev/null +++ b/.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 +} \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..ea5ce21 --- /dev/null +++ b/.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" + } +} \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..8c0b4eb --- /dev/null +++ b/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}) + + + + diff --git a/include/InEKF.h b/include/InEKF.h new file mode 100644 index 0000000..474d42d --- /dev/null +++ b/include/InEKF.h @@ -0,0 +1,99 @@ +#ifndef INEKF_H +#define INEKF_H +#include +#include +#include +#include +#include +#include "RobotState.h" +#include "LieGroup.h" + +typedef std::map,Eigen::aligned_allocator>> mapIntVector3d; +typedef std::vector> 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& 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 estimated_landmarks_; +}; + + + +#endif diff --git a/include/LieGroup.h b/include/LieGroup.h new file mode 100644 index 0000000..caaf968 --- /dev/null +++ b/include/LieGroup.h @@ -0,0 +1,13 @@ +#ifndef LIEGROUP_H +#define LIEGROUP_H +#include +#include + +#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 diff --git a/include/RobotState.h b/include/RobotState.h new file mode 100644 index 0000000..dfe6c3e --- /dev/null +++ b/include/RobotState.h @@ -0,0 +1,47 @@ +#ifndef ROBOTSTATE_H +#define ROBOTSTATE_H +#include +#include + +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 diff --git a/src/InEKF.cpp b/src/InEKF.cpp new file mode 100644 index 0000000..a387735 --- /dev/null +++ b/src/InEKF.cpp @@ -0,0 +1,319 @@ +#include "InEKF.h" +using namespace std; + + +// ------------ NoiseParams ------------- +// Default Constructor +NoiseParams::NoiseParams() { + setGyroscopeNoise(0.01); + setAccelerometerNoise(0.1); + setGyroscopeBiasNoise(0.00001); + setAccelerometerBiasNoise(0.0001); + setLandmarkNoise(0.1); +} + +void NoiseParams::setGyroscopeNoise(double std) { Qg_ = std*std*Eigen::Matrix3d::Identity(); } +void NoiseParams::setGyroscopeNoise(Eigen::Vector3d std) { Qg_ << std(0)*std(0),0,0, 0,std(1)*std(1),0, 0,0,std(2)*std(2); } +void NoiseParams::setGyroscopeNoise(Eigen::Matrix3d cov) { Qg_ = cov; } + +void NoiseParams::setAccelerometerNoise(double std) { Qa_ = std*std*Eigen::Matrix3d::Identity(); } +void NoiseParams::setAccelerometerNoise(Eigen::Vector3d std) { Qa_ << std(0)*std(0),0,0, 0,std(1)*std(1),0, 0,0,std(2)*std(2); } +void NoiseParams::setAccelerometerNoise(Eigen::Matrix3d cov) { Qa_ = cov; } + +void NoiseParams::setGyroscopeBiasNoise(double std) { Qbg_ = std*std*Eigen::Matrix3d::Identity(); } +void NoiseParams::setGyroscopeBiasNoise(Eigen::Vector3d std) { Qbg_ << std(0)*std(0),0,0, 0,std(1)*std(1),0, 0,0,std(2)*std(2); } +void NoiseParams::setGyroscopeBiasNoise(Eigen::Matrix3d cov) { Qbg_ = cov; } + +void NoiseParams::setAccelerometerBiasNoise(double std) { Qbg_ = std*std*Eigen::Matrix3d::Identity(); } +void NoiseParams::setAccelerometerBiasNoise(Eigen::Vector3d std) { Qba_ << std(0)*std(0),0,0, 0,std(1)*std(1),0, 0,0,std(2)*std(2); } +void NoiseParams::setAccelerometerBiasNoise(Eigen::Matrix3d cov) { Qba_ = cov; } + +void NoiseParams::setLandmarkNoise(double std) { Ql_ = std*std*Eigen::Matrix3d::Identity(); } +void NoiseParams::setLandmarkNoise(Eigen::Vector3d std) { Ql_ << std(0)*std(0),0,0, 0,std(1)*std(1),0, 0,0,std(2)*std(2); } +void NoiseParams::setLandmarkNoise(Eigen::Matrix3d cov) { Ql_ = cov; } + +Eigen::Matrix3d NoiseParams::getGyroscopeCov() { return Qg_; } +Eigen::Matrix3d NoiseParams::getAccelerometerCov() { return Qa_; } +Eigen::Matrix3d NoiseParams::getGyroscopeBiasCov() { return Qbg_; } +Eigen::Matrix3d NoiseParams::getAccelerometerBiasCov() { return Qba_; } +Eigen::Matrix3d NoiseParams::getLandmarkCov() { return Ql_; } + + + +// ------------ Observation ------------- +// Default constructor +Observation::Observation(Eigen::VectorXd& Y, Eigen::VectorXd& b, Eigen::MatrixXd& H, Eigen::MatrixXd& N, Eigen::MatrixXd& PI) : + Y(Y), b(b), H(H), N(N), PI(PI) {} + +// Check if empty +bool Observation::empty() { return Y.rows() == 0; } + +ostream& operator<<(ostream& os, const Observation& o) { + os << "---------- Observation ------------" << endl; + os << "Y:\n" << o.Y << endl << endl; + os << "b:\n" << o.b << endl << endl; + os << "H:\n" << o.H << endl << endl; + os << "N:\n" << o.N << endl << endl; + os << "PI:\n" << o.PI << endl; + os << "-----------------------------------"; + return os; +} + + + +// ------------ InEKF ------------- +// Default constructor +InEKF::InEKF() {} + +// Constructor with initial state +InEKF::InEKF(RobotState state) : state_(state) {} + +// Constructor wtih initial state and prior landmarks +InEKF::InEKF(RobotState state, mapIntVector3d prior_landmarks) : state_(state), prior_landmarks_(prior_landmarks) {} + +// Return robot's current state +RobotState InEKF::getState() { return state_; } + +// InEKF Propagation - Inertial Data +void InEKF::Propagate(const Eigen::Matrix& m, double dt) { + Eigen::Vector3d w = m.head(3) - state_.getAngularVelocityBias(); // Angular Velocity + Eigen::Vector3d a = m.tail(3) - state_.getLinearAccelerationBias(); // Linear Acceleration + + Eigen::MatrixXd X = state_.getX(); + Eigen::MatrixXd P = state_.getP(); + + // Extract State + Eigen::Matrix3d R = state_.getRotation(); + Eigen::Vector3d v = state_.getVelocity(); + Eigen::Vector3d p = state_.getPosition(); + + // Strapdown IMU motion model + Eigen::Vector3d phi = w*dt; + Eigen::Matrix3d R_pred = R * Exp_SO3(phi); + Eigen::Vector3d v_pred = v + (R*a + g_)*dt; + Eigen::Vector3d p_pred = p + v*dt + 0.5*(R*a + g_)*dt*dt; + + // Set new state (bias has constant dynamics) + state_.setRotation(R_pred); + state_.setVelocity(v_pred); + state_.setPosition(p_pred); + + // ---- Linearized invariant error dynamics ----- + int dimX = state_.dimX(); + int dimP = state_.dimP(); + int dimTheta = state_.dimTheta(); + Eigen::MatrixXd A = Eigen::MatrixXd::Zero(dimP,dimP); + // Inertial terms + A.block<3,3>(3,0) = skew(g_); // TODO: Efficiency could be improved by not computing the constant terms every time + A.block<3,3>(6,3) = Eigen::Matrix3d::Identity(); + // Bias terms + A.block<3,3>(0,dimP-dimTheta) = -R; + A.block<3,3>(3,dimP-dimTheta+3) = -R; + for (int i=3; i(3*i-6,dimP-dimTheta) = -skew(X.block<3,1>(0,i))*R; + } + + // Noise terms + Eigen::MatrixXd Qk = Eigen::MatrixXd::Zero(dimP,dimP); + Qk.block<3,3>(0,0) = noise_params_.getGyroscopeCov(); + Qk.block<3,3>(3,3) = noise_params_.getAccelerometerCov(); + Qk.block<3,3>(dimP-dimTheta,dimP-dimTheta) = noise_params_.getGyroscopeBiasCov(); + Qk.block<3,3>(dimP-dimTheta+3,dimP-dimTheta+3) = noise_params_.getAccelerometerBiasCov(); + + // Discretization + Eigen::MatrixXd I = Eigen::MatrixXd::Identity(dimP,dimP); + Eigen::MatrixXd Phi = I + A*dt; // Fast approximation of exp(A*dt). TODO: explore using the full exp() instead + Eigen::MatrixXd Adj = I; + Adj.block(0,0,dimP-dimTheta,dimP-dimTheta) = Adjoint_SEK3(X); + Eigen::MatrixXd Qk_hat = Phi * Adj * Qk * Adj.transpose() * Phi.transpose() * dt; // Approximated discretized noise matrix + + // Propagate Covariance + Eigen::MatrixXd P_pred = Phi * P * Phi.transpose() + Qk_hat; + + // Set new covariance + state_.setP(P_pred); + + return; +} + +// Correct State: Right-Invariant Observation +void InEKF::Correct(const Observation& obs) { + // Compute Kalman Gain + Eigen::MatrixXd P = state_.getP(); + Eigen::MatrixXd PHT = P * obs.H.transpose(); + Eigen::MatrixXd S = obs.H * PHT; + Eigen::MatrixXd K = PHT * S.inverse(); + + // Copy X along the diagonals if more than one measurement + Eigen::MatrixXd BigX; + state_.copyDiagX(obs.Y.rows()/state_.dimX(), BigX); + + // Compute correction terms + Eigen::MatrixXd Z = BigX*obs.Y - obs.b; + Eigen::VectorXd delta = K*obs.PI*Z; + Eigen::MatrixXd dX = Exp_SEK3(delta.segment(0,delta.rows()-state_.dimTheta())); + Eigen::VectorXd dTheta = delta.segment(delta.rows()-state_.dimTheta(), state_.dimTheta()); + + // Update state + Eigen::MatrixXd X_new = dX*state_.getX(); // Right-Invariant Update + Eigen::VectorXd Theta_new = state_.getTheta() + dTheta; + state_.setX(X_new); + state_.setTheta(Theta_new); + + // Update Covariance + Eigen::MatrixXd IKH = Eigen::MatrixXd::Identity(state_.dimP(),state_.dimP()) - K*obs.H; + Eigen::MatrixXd P_new = IKH * P * IKH.transpose() + K*obs.N*K.transpose(); // Joseph update form + state_.setP(P_new); +} + +// Create Observation from vector of landmark measurements +void InEKF::CorrectLandmarks(const vectorPairIntVector3d measured_landmarks) { + Eigen::VectorXd Y; + Eigen::VectorXd b; + Eigen::MatrixXd H; + Eigen::MatrixXd N; + Eigen::MatrixXd PI; + + Eigen::Matrix3d R = state_.getRotation(); + vectorPairIntVector3d new_landmarks; + + for (auto it=measured_landmarks.begin(); it!=measured_landmarks.end(); ++it) { + cout << "measured_landmark: " << it->first << ", " << it->second.transpose() << endl; + // See if we can find id in prior_landmarks or estimated_landmarks + auto it_prior = prior_landmarks_.find(it->first); + auto it_estimated = estimated_landmarks_.find(it->first); + if (it_prior!=prior_landmarks_.end()) { + // Found in prior landmark set + cout << "Found in prior landmarks\n"; + int dimX = state_.dimX(); + int dimP = state_.dimP(); + int startIndex; + + // Fill out Y + startIndex = Y.rows(); + Y.conservativeResize(startIndex+dimX, Eigen::NoChange); + Y.segment(startIndex,dimX) = Eigen::VectorXd::Zero(dimX); + Y.segment(startIndex,3) = it->second; // p_bl + Y(startIndex+4) = 1; + + // Fill out b + startIndex = b.rows(); + b.conservativeResize(startIndex+dimX, Eigen::NoChange); + b.segment(startIndex,dimX) = Eigen::VectorXd::Zero(dimX); + b.segment(startIndex,3) = it_prior->second; // p_wl + b(startIndex+4) = 1; + + // Fill out H + startIndex = H.rows(); + H.conservativeResize(startIndex+3, dimP); + H.block(startIndex,0,3,dimP) = Eigen::MatrixXd::Zero(3,dimP); + H.block(startIndex,0,3,3) = skew(it_prior->second); // skew(p_wl) + H.block(startIndex,6,3,3) = -Eigen::Matrix3d::Identity(); // -I + + // Fill out N + startIndex = N.rows(); + N.conservativeResize(startIndex+3, startIndex+3); + N.block(startIndex,0,3,startIndex) = Eigen::MatrixXd::Zero(3,startIndex); + N.block(0,startIndex,startIndex,3) = Eigen::MatrixXd::Zero(startIndex,3); + N.block(startIndex,startIndex,3,3) = R * noise_params_.getLandmarkCov() * R.transpose(); + + // Fill out PI + startIndex = PI.rows(); + int startIndex2 = PI.cols(); + PI.conservativeResize(startIndex+3, startIndex2+dimX); + PI.block(startIndex,0,3,startIndex2) = Eigen::MatrixXd::Zero(3,startIndex2); + PI.block(0,startIndex2,startIndex,dimX) = Eigen::MatrixXd::Zero(startIndex,dimX); + PI.block(startIndex,startIndex2,3,dimX) = Eigen::MatrixXd::Zero(3,dimX); + PI.block(startIndex,startIndex2,3,3) = Eigen::Matrix3d::Identity(); + + } else if (it_estimated!=estimated_landmarks_.end()) {; + // Found in estimated landmark set + cout << "Found in estimated landmarks\n"; + int dimX = state_.dimX(); + int dimP = state_.dimP(); + int startIndex; + + // Fill out Y + startIndex = Y.rows(); + Y.conservativeResize(startIndex+dimX, Eigen::NoChange); + Y.segment(startIndex,dimX) = Eigen::VectorXd::Zero(dimX); + Y.segment(startIndex,3) = it->second; // p_bl + Y(startIndex+4) = 1; + Y(startIndex+it_estimated->second) = -1; + + // Fill out b + startIndex = b.rows(); + b.conservativeResize(startIndex+dimX, Eigen::NoChange); + b.segment(startIndex,dimX) = Eigen::VectorXd::Zero(dimX); + b(startIndex+4) = 1; + b(startIndex+it_estimated->second) = -1; + + // Fill out H + startIndex = H.rows(); + H.conservativeResize(startIndex+3, dimP); + H.block(startIndex,0,3,dimP) = Eigen::MatrixXd::Zero(3,dimP); + H.block(startIndex,6,3,3) = -Eigen::Matrix3d::Identity(); // -I + H.block(startIndex,3*it_estimated->second-6,3,3) = Eigen::Matrix3d::Identity(); // I + + // Fill out N + startIndex = N.rows(); + N.conservativeResize(startIndex+3, startIndex+3); + N.block(startIndex,0,3,startIndex) = Eigen::MatrixXd::Zero(3,startIndex); + N.block(0,startIndex,startIndex,3) = Eigen::MatrixXd::Zero(startIndex,3); + N.block(startIndex,startIndex,3,3) = R * noise_params_.getLandmarkCov() * R.transpose(); + + // Fill out PI + startIndex = PI.rows(); + int startIndex2 = PI.cols(); + PI.conservativeResize(startIndex+3, startIndex2+dimX); + PI.block(startIndex,0,3,startIndex2) = Eigen::MatrixXd::Zero(3,startIndex2); + PI.block(0,startIndex2,startIndex,dimX) = Eigen::MatrixXd::Zero(startIndex,dimX); + PI.block(startIndex,startIndex2,3,dimX) = Eigen::MatrixXd::Zero(3,dimX); + PI.block(startIndex,startIndex2,3,3) = Eigen::Matrix3d::Identity(); + + + } else { + // First time landmark as been detected (add to list for later state augmentation) + cout << "New landmark, adding to list for state augmentation\n"; + new_landmarks.push_back(*it); + } + } + + // Correct state using stacked observation + Observation obs(Y,b,H,N,PI); + if (!obs.empty()) { + this->Correct(obs); + } + + // Augment state with newly detected landmarks + if (new_landmarks.size() > 0) { + Eigen::MatrixXd X_aug = state_.getX(); + Eigen::MatrixXd P_aug = state_.getP(); + Eigen::Vector3d p = state_.getPosition(); + for (auto it=new_landmarks.begin(); it!=new_landmarks.end(); ++it) { + // Initialize new landmark mean + int startIndex = X_aug.rows(); + X_aug.conservativeResize(startIndex+1, startIndex+1); + X_aug.block(startIndex,0,1,startIndex) = Eigen::MatrixXd::Zero(1,startIndex); + X_aug.block(0,startIndex,startIndex,1) = Eigen::MatrixXd::Zero(startIndex,1); + X_aug(startIndex, startIndex) = 1; + X_aug.block(0,startIndex,3,1) = p + R*it->second; + + // Initialize new landmark covariance - TODO:speed up + Eigen::MatrixXd F = Eigen::MatrixXd::Zero(state_.dimP()+3,state_.dimP()); + F.block(0,0,state_.dimP()-state_.dimTheta(),state_.dimP()-state_.dimTheta()) = Eigen::MatrixXd::Identity(state_.dimP()-state_.dimTheta(),state_.dimP()-state_.dimTheta()); // for old X + F.block(state_.dimP()-state_.dimTheta(),6,3,3) = Eigen::Matrix3d::Identity(); // for new landmark + F.block(state_.dimP()-state_.dimTheta()+3,state_.dimP()-state_.dimTheta(),state_.dimTheta(),state_.dimTheta()) = Eigen::MatrixXd::Identity(state_.dimTheta(),state_.dimTheta()); // for theta + Eigen::MatrixXd G = Eigen::MatrixXd::Zero(F.rows(),3); + G.block(G.rows()-state_.dimTheta()-3,0,3,3) = R; + P_aug = (F*P_aug*F.transpose() + G*noise_params_.getLandmarkCov()*G.transpose()).eval(); + + // Add to list of estimated landmarks + estimated_landmarks_.insert(pair (it->first, startIndex)); + } + state_.setX(X_aug); + state_.setP(P_aug); + } + + return; +} \ No newline at end of file diff --git a/src/LieGroup.cpp b/src/LieGroup.cpp new file mode 100644 index 0000000..e48d3ec --- /dev/null +++ b/src/LieGroup.cpp @@ -0,0 +1,64 @@ +#include "LieGroup.h" +using namespace std; + +Eigen::Matrix3d skew(const Eigen::Vector3d& v) { + // Convert vector to skew-symmetric matrix + Eigen::Matrix3d M = Eigen::Matrix3d::Zero(); + M << 0, -v[2], v[1], + v[2], 0, -v[0], + -v[1], v[0], 0; + return M; +} + +Eigen::Matrix3d Exp_SO3(const Eigen::Vector3d& w) { + // Computes the vectorized exponential map for SO(3) + Eigen::Matrix3d A = skew(w); + double theta = w.norm(); + if (theta < TOLERANCE) { + return Eigen::Matrix3d::Identity(); + } + Eigen::Matrix3d R = Eigen::Matrix3d::Identity() + (sin(theta)/theta)*A + ((1-cos(theta))/(theta*theta))*A*A; + return R; +} + +Eigen::MatrixXd Exp_SEK3(const Eigen::VectorXd& v) { + // Computes the vectorized exponential map for SE_K(3) + int K = (v.size()-3)/3; + Eigen::MatrixXd X = Eigen::MatrixXd::Identity(3+K,3+K); + Eigen::Matrix3d R; + Eigen::Matrix3d Jl; + Eigen::Vector3d w = v.head(3); + double theta = w.norm(); + Eigen::Matrix3d I = Eigen::Matrix3d::Identity(); + if (theta < TOLERANCE) { + R = I; + Jl = I; + } else { + Eigen::Matrix3d A = skew(w); + double theta2 = theta*theta; + double stheta = sin(theta); + double ctheta = cos(theta); + double oneMinusCosTheta2 = (1-ctheta)/(theta2); + Eigen::Matrix3d A2 = A*A; + R = I + (stheta/theta)*A + oneMinusCosTheta2*A2; + Jl = I + oneMinusCosTheta2*A + ((theta-stheta)/(theta2*theta))*A2; + } + X.block<3,3>(0,0) = R; + for (int i=0; i(0,3+i) = Jl * v.segment<3>(3+3*i); + } + return X; +} + +Eigen::MatrixXd Adjoint_SEK3(const Eigen::MatrixXd& X) { + // Compute Adjoint(X) for X in SE_K(3) + int K = X.cols()-3; + Eigen::MatrixXd Adj = Eigen::MatrixXd::Zero(3+3*K, 3+3*K); + Eigen::Matrix3d R = X.block<3,3>(0,0); + Adj.block<3,3>(0,0) = R; + for (int i=0; i(3+3*i,3+3*i) = R; + Adj.block<3,3>(3+3*i,0) = skew(X.block<3,1>(0,3+i))*R; + } + return Adj; +} diff --git a/src/RobotState.cpp b/src/RobotState.cpp new file mode 100644 index 0000000..5482da3 --- /dev/null +++ b/src/RobotState.cpp @@ -0,0 +1,65 @@ +#include "RobotState.h" +#include "LieGroup.h" +using namespace std; + +// Default constructor +RobotState::RobotState() : + X_(Eigen::MatrixXd::Identity(5,5)), Theta_(Eigen::MatrixXd::Zero(6,1)), P_(Eigen::MatrixXd::Identity(15,15)) {} +// Initialize with X +RobotState::RobotState(const Eigen::MatrixXd& X) : + X_(X), Theta_(Eigen::MatrixXd::Zero(6,1)) { + P_ = Eigen::MatrixXd::Identity(3*this->dimX()+this->dimTheta()-6, 3*this->dimX()+this->dimTheta()-6); +} +// Initialize with X and Theta +RobotState::RobotState(const Eigen::MatrixXd& X, const Eigen::VectorXd& Theta) : + X_(X), Theta_(Theta) { + P_ = Eigen::MatrixXd::Identity(3*this->dimX()+this->dimTheta()-6, 3*this->dimX()+this->dimTheta()-6); +} +// Initialize with X, Theta and P +RobotState::RobotState(const Eigen::MatrixXd& X, const Eigen::VectorXd& Theta, const Eigen::MatrixXd& P) : + X_(X), Theta_(Theta), P_(P) {} +// TODO: error checking to make sure dimensions are correct and supported + + +const Eigen::MatrixXd RobotState::getX() { return X_; } +const Eigen::VectorXd RobotState::getTheta() { return Theta_; } +const Eigen::MatrixXd RobotState::getP() { return P_; } +const Eigen::Matrix3d RobotState::getRotation() { return X_.block<3,3>(0,0); } +const Eigen::Vector3d RobotState::getVelocity() { return X_.block<3,1>(0,3); } +const Eigen::Vector3d RobotState::getPosition() { return X_.block<3,1>(0,4); } +const Eigen::Vector3d RobotState::getAngularVelocityBias() { return Theta_.head(3); } +const Eigen::Vector3d RobotState::getLinearAccelerationBias() { return Theta_.tail(3); } +const int RobotState::dimX() { return X_.cols(); } +const int RobotState::dimTheta() { return Theta_.rows(); } +const int RobotState::dimP() { return P_.cols(); } + +void RobotState::setX(Eigen::MatrixXd& X) { X_ = X; } +void RobotState::setTheta(Eigen::VectorXd& Theta) { Theta_ = Theta; } +void RobotState::setP(Eigen::MatrixXd& P) { P_ = P; } +void RobotState::setRotation(Eigen::Matrix3d& R) { X_.block<3,3>(0,0) = R; } +void RobotState::setVelocity(Eigen::Vector3d& v) { X_.block<3,1>(0,3) = v; } +void RobotState::setPosition(Eigen::Vector3d& p) { X_.block<3,1>(0,4) = p; } +void RobotState::setAngularVelocityBias(Eigen::Vector3d& bg) { Theta_.head(3) = bg; } +void RobotState::setLinearAccelerationBias(Eigen::Vector3d& ba) { Theta_.tail(3) = ba; } + + +void RobotState::copyDiagX(int n, Eigen::MatrixXd& BigX) { + int dimX = this->dimX(); + for(int i=0; i +#include +#include +#include +#include +#include "InEKF.h" + +#define DT_MIN 1e-6 +#define DT_MAX 1 + +using namespace std; + +int main() +{ + // Initialize robot's state + RobotState initial_state; + + // Initialize prior landmarks + mapIntVector3d prior_landmarks; + Eigen::Vector3d p_wl; + int id; + + id = 0; + p_wl << 1,2,3; + prior_landmarks.insert(pair (id, p_wl)); + + id = 1; + p_wl << 4,5,6; + prior_landmarks.insert(pair (id, p_wl)); + + id = 2; + p_wl << 7,8,9; + prior_landmarks.insert(pair (id, p_wl)); + + // Initialize filter + InEKF filter(initial_state, prior_landmarks); + cout << "Robot's state is initialized to: \n"; + cout << filter.getState() << endl; + + ifstream infile("../src/data/landmark_measurements.txt"); + string line; + Eigen::Matrix m, m_last; + double t, t_last; + m_last << 0,0,0,0,0,0; + t_last = 0; + + // Loop through data file and read in measurements line by line + while (getline(infile, line)){ + vector measurement; + boost::split(measurement,line,boost::is_any_of(" ")); + // Handle measurements + if (measurement[0].compare("IMU")==0){ + cout << "Received IMU Data, propagating state\n"; + t = stod(measurement[1]); + m << stod(measurement[2]), + stod(measurement[3]), + stod(measurement[4]), + stod(measurement[5]), + stod(measurement[6]), + stod(measurement[7]); + + // Propagate using IMU data + double dt = t - t_last; + if (dt > DT_MIN && dt < DT_MAX) { + filter.Propagate(m_last, dt); + //cout << filter.getState() << endl; + } + + } + else if (measurement[0].compare("LANDMARK")==0){ + cout << "Received LANDMARK observation, correcting state\n"; + t = stod(measurement[1]); + vectorPairIntVector3d measured_landmarks; + for (int i=2; i (landmark_id, p_bl)); + } + + // Correct state using landmark measurements + filter.CorrectLandmarks(measured_landmarks); + //cout << filter.getState() << endl; + + } + + // Store previous timestamp + t_last = t; + m_last = m; + } + + return 0; +} \ No newline at end of file diff --git a/src/tests/propagation_speed.cpp b/src/tests/propagation_speed.cpp new file mode 100644 index 0000000..2c3a10f --- /dev/null +++ b/src/tests/propagation_speed.cpp @@ -0,0 +1,83 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include "InEKF.h" + +#define DT_MIN 1e-6 +#define DT_MAX 1 + +using namespace std; + +typedef vector>> vectorPairIntVector6d; + +int main() +{ + // Initialize filter + Eigen::MatrixXd X = Eigen::MatrixXd::Identity(10,10); + RobotState state(X); + InEKF filter(state); + + cout << "Robot's state is initialized to: \n"; + cout << filter.getState() << endl; + + ifstream infile("../src/data/propagation_speed_test_data.txt"); + string line; + Eigen::Matrix m, m_last; + double t, t_last; + m_last << 0,0,0,0,0,0; + t_last = 0; + vectorPairIntVector6d measurements_vec; + + // Loop through data file and read in measurements line by line + while (getline(infile, line)){ + vector measurement; + boost::split(measurement,line,boost::is_any_of(" ")); + // Handle measurements + if (measurement[0].compare("IMU")==0){ + t = stod(measurement[1]); + m << stod(measurement[2]), + stod(measurement[3]), + stod(measurement[4]), + stod(measurement[5]), + stod(measurement[6]), + stod(measurement[7]); + measurements_vec.push_back(pair> (t, m)); + } + } + + // Propagate all IMU data + cout << "Propagating " << measurements_vec.size() << " IMU measurements...\n"; + int64_t max_duration = 0; + int64_t sum_duration = 0; + for (auto it=measurements_vec.begin(); it!=measurements_vec.end(); ++it) { + // Propagate using IMU data + t = it->first; + m = it->second; + double dt = t - t_last; + if (dt > DT_MIN && dt < DT_MAX) { + chrono::high_resolution_clock::time_point start_time = chrono::high_resolution_clock::now(); + filter.Propagate(m_last, dt); + chrono::high_resolution_clock::time_point end_time = chrono::high_resolution_clock::now(); + auto duration = chrono::duration_cast( end_time - start_time ).count(); + //cout << "duration: " << duration << endl; + sum_duration += duration; + if (duration > max_duration) + max_duration = duration; + + //cout << filter.getState() << endl; + } + // Store previous timestamp + t_last = t; + m_last = m; + } + + cout << "max duration: " << max_duration << endl; + cout << "average duration: " << double(sum_duration)/measurements_vec.size() << endl; + + return 0; +} \ No newline at end of file