Invariant-ekf - C++ library to implement invariant extended Kalman filtering for aided inertial navigation.

Overview

inekf

This repository contains a C++ library that implements an invariant extended Kalman filter (InEKF) for 3D aided inertial navigation.

InEKF LiDAR Mapping

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".

Inclusion of kinematic and contact measurements is presented in: "Contact-aided Invariant Extended Kalman Filtering for Legged Robot State Estimation".

A ROS wrapper for the filter is available at https://github.com/RossHartley/invariant-ekf-ros.

Setup

Requirements

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}
}
Comments
  • Derivation of left error derivative with noise.

    Derivation of left error derivative with noise.

    Trying to derive left error with noise (equation 30 in paper - left version). Similar to how paper does noise free derivative of error.

    d/dt n = g(n) - w^ n <- this is what we seek and is the left version of equation 30 in the paper.

    Definitions: d/dt x = f(x) + x w^ n = x^-1 xbar x n = xbar g(n) = f(n) - f(I) n

    Start by taking derivative: d/dt(n) = d/dt (x^-1) xbar + x^-1 d/dt xbar Substitute d/dt(x ^x-1) = d/dt I = 0 -> d/dt x^-1 = -x^-1 (d/dt x) x^-1 = -x^-1 d/dt x x^-1 xbar + x^-1 d/dt xbar Insert dynamics "f": = -x^-1 (f(x) + x w^) x^-1 xbar + x^-1 (f(xbar) + xbar w^) = -x^-1 f(x)x^-1 xbar + -x^-1 x w^ x^-1 xbar + (x^-1 f(xbar) + x^-1 xbar w^) Put n back in = -x^-1 f(x)n -w^ n + x^-1 f(xbar) + n w^ Eliminte xbar = -x^-1 f(x)n -w^ n + x^-1 f(x n) + n w^ Set x = I = -f(I)n -w^ n + f(n) + n w^ Insert defintion of g = f(n) - f(I)n -w^ n + n w^ = g(n) -w^ n + n w^

    Almost but not quite. I can't get rid of "n w^". What am I missing? Paper derives noise free error derivative but only states results of the noisey version.

    opened by MontyTHall2 2
  • Has this repository compiled on Mac?

    Has this repository compiled on Mac?

    Has this repository compiled on Mac? I'm using Mac to compile this repository and I got following error message:clang: error: linker command failed with exit code 1 (use -v to see invocation) make[2]: *** [../lib/libinekf.dylib] Error 1 make[1]: *** [CMakeFiles/inekf.dir/all] Error 2 make: *** [all] Error 2 I have installed the requirement package already. Thanks in advance!

    opened by yuan0623 2
  • A Question about `Continuous RI-EKF Equations'

    A Question about `Continuous RI-EKF Equations'

    The IEKF system noise Qt=Adx*Cov (wt)*Adx^T . where Adx=[R 0 0 0] [(vt)xR R 0 0] [(pt)xR 0 R 0] [(dt)xR 0 0 R] if the position pt is large, for example the robot moves to a certain location (1000,1000,1000),and its velocity is (100,100,100) ,then Qt is very large. Namely IEKF's system noise is very large and this is unreasonable. Why is this happening?  

    opened by Erensu 2
  • Landmarks from apriltag markers.

    Landmarks from apriltag markers.

    Hi, thanks for making this code available.

    I am trying to use it with a pre determined map of ceiling mounted apriltag markers. (corner positions known).

    Would you be able to give me a few tips on how to use your example and add marker corners?

    Would I give each corner an id , and set them as landmarks?

    Where is the camera / robot pose added?

    Thanks!

    opened by antithing 1
  • matlab /examples_matlab

    matlab /examples_matlab

    hello,When I run simulink, I meet the following problems. Simulink cannot propagate the variable-size mode from the 'Output Port 1' of 'RIEKF_test/Landmark Spoofer' to the 'Input Port 1' of 'RIEKF_test/Orientation EtherCAT Rate Transition6'. This input port expects a fixed-size mode. The variable-size mode originates from 'RIEKF_test/Landmark Spoofer/MATLAB Function1'. Examine the configurations of 'RIEKF_test/Orientation EtherCAT Rate Transition6' for one of the following scenarios: 1) the block does not support variable-size signals; 2) the block supports variable-size signals but needs to be configured for them.

    opened by lxy-mini 1
  • question about the sensor measurement of kinematic example

    question about the sensor measurement of kinematic example

    Hi, I'm trying to reproduce your results on my own to make sure that I really understand your paper Contact-Aided Invariant Extended Kalman Filtering for Robot State Estimation*. When I look into the KINEMATIC measurement in imu_kinematic_measurements.txt, I find it is quite strange. Here is my understanding: instead of measuring the joint encoder of the Cassie, the imu_kinematic_measurements.txt provides the relative position between the foot and the robot directly, which I can understand. However, The imu_kinematic_measurements.txt also provides the covariance matrix, which I really don't understand. Can the covariance matrix be measured? This is really strange to me. I would be really appreciated if you can answer this question for me. Thank you so much in advance for your time! Best Yuan

    opened by yuan0623 2
  • Implementing ZUPT

    Implementing ZUPT

    Hello,

    I want to implement ZUPT (zero velocity updates). I first thought it would be a piece of cake but I came to the realisation that I have to master the theory first... which is not out of reach, but certainly going to take least a couple of days. Still, the solution might be really simple so I'm taking a chance here. Perhaps someone could provide the solution in just a few key lines of code.

    Thanks to anyone! And a big thanks to Ross Hartley for sharing.

    opened by Atlis 0
Owner
Ross Hartley
Ross Hartley
Homework for Computer Aided Geometric Design Course

Computer Aided Geometric Design Introduction MATH6110P: Computer Aided Geometric Design (Autumn-Winter 2021-2022) Instructors: Renjie Chen Webpage: ht

Wenbo Chen 3 Sep 10, 2022
EKF-based late fusion

深蓝学院多传感器融合感知课程 项目实现了Lidar与Camera的后融合感知算法,融合的算法基于扩展卡尔曼滤波(Extended Kalman Filter,EKF)。输入数据为Lidar检测结果以及Camera检测结果,检测算法与Apollo 6.0一致,Lidar检测算法为PointPillar

深蓝学院 8 Nov 24, 2022
Kalman Filter Implementation for MPU6050 with STM32-Nucleo. Via CubeIDE

Kalman Filter Implementation for MPU6050 on STM32 Nucleo Board Kalman Filter Implementation for MPU6050 with STM32-Nucleo I implemented a Kalman Filte

Ibrahim Ozdemir 25 Nov 27, 2022
DirectX 11 library that provides convenient access to compute-based triangle filtering (CTF)

AMD GeometryFX The GeometryFX library provides convenient access to compute-based triangle filtering (CTF), which improves triangle throughput by filt

GPUOpen Effects 218 Oct 6, 2022
Xtl - eXtended Template Library

eXtended Template Library Open Hub Linux Windows Coverage Technical Debt Code Quality License Contribute with Gratipay Contribute with Beerpay View th

David Mott 4 Sep 26, 2022
A small XM (FastTracker II Extended Module) player library.

libxm A small XM (FastTracker II Extended Module) player library. Main features: Small size in mind; many features can be disabled at compile-time, or

Romain D. 108 Nov 18, 2022
Application firewall PoC with filtering performed in the kernel, for Linux.

Voi Application firewall with filtering performed in the kernel, for Linux. Status Currently just scaffolding code No where near ready for a productio

Marc 7 Sep 15, 2022
Project to create a teensy based gamecube controller with hall effect sensors, snapback filtering, and notch calibration

PhobGCC Gamecube controller motherboard using a teensy as the microcontroller. Aim is to make an accessible and consistent controller. Has the option

null 93 Dec 1, 2022
Arduino library for basic aerial navigation functions used for processing Euler angles, direction cosine matrices, quaternions, frame conversions, and more.

navduino Arduino library for basic aerial navigation functions used for Euler angles Direction cosine matrices Quaternions Rodrigues Rotation Vectors

PB2 7 Oct 24, 2022
Obfuscator refactored and extended from OLLVM.

OLLVM++ Obfuscator refactored and extended from OLLVM. Environment Ubuntu 18.04.5 LTS LLVM 12.0.1 Clang 12.0.1 CMake 3.21.1 Usage Compile Obfuscation

34r7hm4n 500 Nov 24, 2022
The core engine forked from NVidia's Q2RTX. Heavily modified and extended to allow for a nicer experience all-round.

Polyhedron - A Q2RTX Fork A fork of the famous Q2RTX project by NVIDIA™ that strives to improve all of its other factors of what was once upon a time

Polyhedron Studio 19 Nov 22, 2022
Half-Life : Extended main branch for developing purposes

Half Life : Extended SDK Source Code of Half Life : Extended as a open source modbase for everyone publicly, make your own mod with alot of features e

Bacontsu 14 Nov 22, 2022
A CUDA-accelerated cloth simulation engine based on Extended Position Based Dynamics (XPBD).

Velvet Velvet is a CUDA-accelerated cloth simulation engine based on Extended Position Based Dynamics (XPBD). Why another cloth simulator? There are a

Vital Chen 37 Nov 21, 2022
An Open Source tripmaster for navigation rallies

Open Rally Computer An open source tripmaster for navigation rallies Description The Open Rally Computer (previously known as Baja Pro) is a complete

Matias Godoy 47 Sep 7, 2022
2D/3D Registration and system integration for image-based navigation of orthopedic robotic applications, inculding femoroplasty, osteonecrosis, etc.

Registration and System Integration Software for Orthopedic Surgical Robotic System This repository contains software programs for image-based registr

Cong Gao 15 Sep 3, 2022
Small Robot, with LIDAR and DepthCamera. Using ROS for Maping and Navigation

?? RoboCop ?? Small Robot, with LIDAR and DepthCamera. Using ROS for Maping and Navigation Made by Clemente Donoso, ?? Chile ???? RoboCop Lateral Fron

Clemente Donoso Krauss 2 Jan 4, 2022
Local Navigation Planner for Legged Robots

ANYmal Rough Terrain Planner Sampling based path planning for ANYmal, based on 2.5D height maps. More detailed instructions still to come. The paper d

Robotic Systems Lab - Legged Robotics at ETH Zürich 35 Nov 25, 2022
Optimization-Based GNSS/INS Integrated Navigation System

OB_GINS Optimization-Based GNSS/INS Integrated Navigation System We open-source OB_GINS, an optimization-based GNSS/INS integrated navigation system.

i2Nav-WHU 281 Nov 24, 2022
Tightly coupled GNSS-Visual-Inertial system for locally smooth and globally consistent state estimation in complex environment.

GVINS GVINS: Tightly Coupled GNSS-Visual-Inertial Fusion for Smooth and Consistent State Estimation. paper link Authors: Shaozu CAO, Xiuyuan LU and Sh

HKUST Aerial Robotics Group 572 Nov 27, 2022