ROS compatible tool to generate Allan Deviation plots

Overview

Allan Variance ROS

ROS package which loads a rosbag of IMU data and computes Allan Variance parameters

The purpose of this tool is to read a long sequence of IMU data and return the Angle Random Walk (ARW), Bias Instability and Random Walk for the gyroscope as well as Velocity Random Walk (VRW), Bias Instability and Random Walk for the accelerometer.

How to use

Build the package:

catkin build allan_variance_ros

Recommended: Reorganize ROS messages by timestamp using ROS Cookbook:

rosrun allan_variance_ros cookbag.py --input original_rosbag --output cooked_rosbag

Run the Allan Variance computation tool:

rosrun allan_variance_ros allan_variance [path_to_rosbags] [path_to_config_file]

This will compute the Allan Deviation for the IMU and generate a CSV. The next step is to visualize the plots and get parameters. For this run:

rosrun allan_variance_ros analysis.py --data allan_variance.csv

Press space to go to next figure.

How to collect sequence

Place your IMU on some damped surface and record your IMU data to a rosbag. You must record at least 3 hours of data. The longer the sequence, the more accurate the results.

Example Log

3 hour log of Realsense D435i IMU already "cooked".

Acceleration Gyroscope

Example terminal output:

ACCELEROMETER:
X Velocity Random Walk:  0.00333 m/s/sqrt(s)  0.19983 m/s/sqrt(hr)
Y Velocity Random Walk:  0.01079 m/s/sqrt(s)  0.64719 m/s/sqrt(hr)
Z Velocity Random Walk:  0.00481 m/s/sqrt(s)  0.28846 m/s/sqrt(hr)
X Bias Instability:  0.00055 m/s^2  1.99258 m/hr^2
Y Bias Instability:  0.00153 m/s^2  5.51917 m/hr^2
Z Bias Instability:  0.00052 m/s^2  1.86155 m/hr^2
X Accel Random Walk:  0.00008 m/s^2
Y Accel Random Walk:  0.00020 m/s^2
Z Accel Random Walk:  0.00007 m/s^2
GYROSCOPE:
X Angle Random Walk:  0.00787 deg/sqrt(s)  0.47215 deg/sqrt(hr)
Y Angle Random Walk:  0.00987 deg/sqrt(s)  0.59204 deg/sqrt(hr)
Z Angle Random Walk:  0.00839 deg/sqrt(s)  0.50331 deg/sqrt(hr)
X Bias Instability:  0.00049 deg/s  1.76568 deg/hr
Y Bias Instability:  0.00136 deg/s	 4.88153 deg/hr
Z Bias Instability:  0.00088 deg/s	 3.15431 deg/hr
X Rate Random Walk:  0.00007 deg/s
Y Rate Random Walk:  0.00028 deg/s
Z Rate Random Walk:  0.00011 deg/s

Kalibr

Kalibr is a useful collection of tools for calibrating cameras and IMUs. For IMU calibration it needs the noise parameters of the IMU generated in a yaml file. allan_variance_ros automatically generates this file file as imu.yaml:

#Accelerometer
accelerometer_noise_density: 0.006308226052016165 
accelerometer_random_walk: 0.00011673723527962174 

#Gyroscope
gyroscope_noise_density: 0.00015198973532354657 
gyroscope_random_walk: 2.664506559330434e-06 

rostopic: '/sensors/imu' #Make sure this is correct
update_rate: 400.0 #Make sure this is correct

References

Issues
  • About units of params Accel Random Walk and Rate Random Walk

    About units of params Accel Random Walk and Rate Random Walk

    Hi author, Thanks for your sharing so convient tool to community very much. In code we average three axis Accel Random Walk to get accelerometer_random_walk of kalibr. accelerometer_random_walk's unit is m/s^3/sqrt(HZ), namely m/s^2/sqrt(s), but the Accel Random Walk's unit is m/s^2, is there a typo ? The same question exist about Rate Random Walk. Velocity Random Walk and Angle Random Walk units are equal to accelerometer_noise_density and gyroscope_noise_densityof kalibr. Thanks for your help and time! https://github.com/ori-drs/allan_variance_ros/blob/25a33882f1fafddc910dbbdb0e2d4858cde009fc/scripts/analysis.py#L121

    opened by narutojxl 6
  • analysis.py:25: RuntimeWarning: divide by zero encountered in log

    analysis.py:25: RuntimeWarning: divide by zero encountered in log

    Hello author, I have recorded 3hr 37:31s (13051s) 2021-11-18-14-38-03.bag, it only contains imu data which published by gazebo libgazebo_ros_imu_sensor.so. cooked_rosbag_2021-11-18-14-38-03.bag is located in /home/jxl/jxl/allan_ws/. gazebo_husky.yaml is

    imu_topic: "/imu/data"
    imu_rate: 50
    measure_rate: 50
    sequence_time: 13050 #bag in seconds
    
    rosrun allan_variance_ros cookbag.py  --input /home/jxl/2021-11-18-14-38-03.bag  --output  cooked_rosbag_2021-11-18-14-38-03.bag
    rosrun allan_variance_ros allan_variance  /home/jxl/jxl/allan_ws/ /home/jxl/jxl/allan_ws/src/allan_variance_ros/config/gazebo_husky.yaml
    

    When createing allan_variance.csv, it reminds me many imu msgs timestamp have somewrong, this is only part of the information output by the terminal.

    [ERROR] [1637290175.093369303]: Skipped imu messages 614547
    [ INFO] [1637290175.093390054]: 12291 / 13050 seconds loaded
    [ERROR] [1637290175.093418470]: IMU message before last imu time. IMU time: 13077039999998 last imu time 93877317622759
    [ERROR] [1637290175.093438070]: Skipped imu messages 614548
    [ INFO] [1637290175.093457149]: 12291 / 13050 seconds loaded
    [ERROR] [1637290175.093483640]: IMU message before last imu time. IMU time: 13077059999998 last imu time 93877317622759
    [ERROR] [1637290175.093503903]: Skipped imu messages 614549
    [ERROR] [1637290175.093530664]: IMU message before last imu time. IMU time: 13077079999998 last imu time 93877317622759
    [ERROR] [1637290175.093551317]: Skipped imu messages 614550
    [ INFO] [1637290175.093572197]: 12291 / 13050 seconds loaded
    [ERROR] [1637290175.093600659]: IMU message before last imu time. IMU time: 13077099999998 last imu time 93877317622759
    
    [ INFO] [1637290176.968499108]: Computed 0 averages for period 985.5
    [ INFO] [1637290176.968519090]: Computed 0 averages for period 985.6
    [ INFO] [1637290176.968539519]: Computed 0 averages for period 985.7
    [ INFO] [1637290176.968559882]: Computed 0 averages for period 985.8
    [ INFO] [1637290176.968580278]: Computed 0 averages for period 985.9
    [ INFO] [1637290176.968600411]: Computed 0 averages for period 986
    [ INFO] [1637290176.968620740]: Computed 0 averages for period 986.1
    [ INFO] [1637290176.968641282]: Computed 0 averages for period 986.2
    [ INFO] [1637290176.968661284]: Computed 0 averages for period 986.3
    [ INFO] [1637290176.968681675]: Computed 0 averages for period 986.4
    [ INFO] [1637290176.968701987]: Computed 0 averages for period 986.5
    [ INFO] [1637290176.968721859]: Computed 0 averages for period 986.6
    [ INFO] [1637290176.968742431]: Computed 0 averages for period 986.7
    [ INFO] [1637290176.968763563]: Computed 0 averages for period 986.8
    [ INFO] [1637290176.968783902]: Computed 0 averages for period 986.9
    [ INFO] [1637290176.968804266]: Computed 0 averages for period 987
    [ INFO] [1637290176.968824155]: Computed 0 averages for period 987.1
    [ INFO] [1637290176.968844767]: Computed 0 averages for period 987.2
    [ INFO] [1637290176.968865573]: Computed 0 averages for period 987.3
    

    When i rosrun allan_variance_ros analysis.py --data /home/jxl/jxl/allan_ws/allan_variance.csv, program is terminated. Is there something i missing? Every imu msg has a timestamp, does it mean cookbag.py do nothing actually ? Thanks for any your suggestion and help.

    ➜  [/home/jxl/jxl/allan_ws] rosrun allan_variance_ros analysis.py --data  /home/jxl/allan_variance.csv 
    /home/jxl/jxl/allan_ws/src/allan_variance_ros/scripts/analysis.py:25: RuntimeWarning: divide by zero encountered in log
      logy = np.log(y)
    Traceback (most recent call last):
      File "/home/jxl/jxl/allan_ws/src/allan_variance_ros/scripts/analysis.py", line 84, in <module>
        accel_wn_intercept_x, xfit_wn = get_intercept(period[0:white_noise_break_point], acceleration[0:white_noise_break_point,0], -0.5, 1.0)
      File "/home/jxl/jxl/allan_ws/src/allan_variance_ros/scripts/analysis.py", line 26, in get_intercept
        coeffs, _ = curve_fit(line_func, logx, logy, bounds=([m, -np.inf], [m + 0.001, np.inf]))
      File "/home/jxl/.local/lib/python3.8/site-packages/scipy/optimize/minpack.py", line 735, in curve_fit
        ydata = np.asarray_chkfinite(ydata, float)
      File "/usr/lib/python3/dist-packages/numpy/lib/function_base.py", line 495, in asarray_chkfinite
        raise ValueError(
    ValueError: array must not contain infs or NaNs
    ➜  [/home/jxl/jxl/allan_ws] 
    

    This is csv file's part info.

    0.5 -0 -0 -0 -0 -0 -0 
    0.6000000238418579102 -0 -0 -0 -0 -0 -0 
    0.6999999880790710449 -0 -0 -0 -0 -0 -0 
    0.8000000119209289551 -0 -0 -0 -0 -0 -0 
    0.8999999761581420898 -0 -0 -0 -0 -0 -0 
    1 -0 -0 -0 -0 -0 -0 
    1.10000002384185791 -0 -0 -0 -0 -0 -0 
    1.20000004768371582 -0 -0 -0 -0 -0 -0 
    1.29999995231628418 -0 -0 -0 -0 -0 -0 
    1.39999997615814209 -0 -0 -0 -0 -0 -0 
    1.5 -0 -0 -0 -0 -0 -0 
    
    opened by narutojxl 4
  • How long should this take to run?

    How long should this take to run?

    Not complaining - trying to plan - but approx. how long should the allan_variance computation process take for say ~3hr long data?

    I have a rosbag ~3hrs (11214s) long being processed. The message Computed X averages for period Y is progressing through 1-unit of "period" every ~3.5s real-time. If I take "period" to mean the seconds in the rosbag it looks like my computer will take >10hrs to process... Is just how it is or am I missing something?

    Obviously this is dependant on my hardware. I'm on a Ryzen 3600X based machine 16GB if that helps.

    opened by seajayshore 1
  • path_to_config_file help

    path_to_config_file help

    Hi,

    thanks for sharing your work. I'd really like to use this toolset but I'm not familiar with ROS - can you help me with the "path_to_config_file" arg mentioned in the following line from your instructions?

    rosrun allan_variance_ros allan_variance [path_to_rosbags] [path_to_config_file]

    Thanks!

    opened by kevoc69 1
  • Fix uninitialized variables + clang-tidy warnings

    Fix uninitialized variables + clang-tidy warnings

    The only really important commit is the last one ("Initialize all variables"). This fixes a bug caused by lastImuTime_ not being initialized, leading to all IMU messages being skipped with error message "IMU message before last imu time."

    The remaining commits just fix some clang-tidy warnings and are more aesthetic than functional.

    Please review each commit separately.

    opened by mintar 1
  • Only getting Accelerometer data not gyroscope

    Only getting Accelerometer data not gyroscope

    Hello I am using this package to generate config file for the kalibr imu-camera calibration. But when I run the package rosrun allan_variance_ros allan_variance bags imu.config

    and when I analyze the .csv file with rosrun allan_variance_ros analysis.py --data allan_variance.csv

    It only shows the Accelerometer values. ACCELEROMETER: X Velocity Random Walk: 0.00214 m/s/sqrt(s) 0.12869 m/s/sqrt(hr) Y Velocity Random Walk: 0.00285 m/s/sqrt(s) 0.17109 m/s/sqrt(hr) Z Velocity Random Walk: 0.00211 m/s/sqrt(s) 0.12644 m/s/sqrt(hr) X Bias Instability: 0.00090 m/s^2 11706.92482 m/hr^2 Y Bias Instability: 0.00117 m/s^2 15211.83888 m/hr^2 Z Bias Instability: 0.00059 m/s^2 7607.42798 m/hr^2 X Accel Random Walk: 0.00058 m/s^2/sqrt(s) Y Accel Random Walk: 0.00076 m/s^2/sqrt(s) Z Accel Random Walk: 0.00039 m/s^2/sqrt(s)

    What could be the problem ?

    opened by Avi241 0
  • Fix units again

    Fix units again

    I made a mistake in #4 (ee29d7b5). The tables I posted there were correct, but my code changes did not reflect what was in the tables.

    Thanks @narutojxl for spotting this! (https://github.com/ori-drs/allan_variance_ros/issues/2#issuecomment-980492911).

    This will hopefully be the final edit to the units...

    opened by mintar 0
  • Plot predicted model

    Plot predicted model

    This PR adds the fitted model to the plots (data from a Realsense D455):

    acceleration

    gyro

    generate_prediction adapted from https://github.com/nmayorov/allan-variance/

    Please review each commit in this PR separately.

    opened by mintar 0
  • Units from my IMU

    Units from my IMU

    Hi,

    I record a video and extract IMU metadata like shown below. I need to know the units so i can easily use your tool. Please help !!

    "Doc1961:Accelerometer": { "id": "Accelerometer", "table": "QuickTime::Stream", "val": "0.97705078125 -0.775634765625 -0.63134765625" }, "Doc1961:AngularVelocity": { "id": "AngularVelocity", "table": "QuickTime::Stream", "val": "-0.711596667766571 -0.207726567983627 -0.145941227674484"

    opened by sadsasuke 0
  • Config Analysis

    Config Analysis

    This allows a user to pass --config file.yaml so the IMU config file generated is completely valid. Additionally, tfixed building on my system due to Eigen3.

    opened by goldbattle 0
  • .bag format

    .bag format

    hi,

    I want to find values of my own IMU data and i need to know 1- what is the format needed to make a .bag file that can be used by the tool. 2- how to convert my own file into bag format suggestions.

    Thanks

    opened by sadsasuke 0
  • about analysis.py

    about analysis.py

    I have a problem.

    I ran the analysis.py

    but the biases about accelerometer and gyroscope x,y, and z have nan value.

    What should I do???

    results . . acceleration . . gyro . . . . . . And I modified the code in analysis.py

    logx = np.log(x) and logy = np.log(y) ----> add logx = np.nan_to_num(logx) and logy = np.nan_to_num(logy)

    coeffs, _ = curve_fit(line_func, logx, logy, bounds=~~~~~~ ----> coeffs, _ = curve_fit(line_func, logx, logy)

    opened by boeun98 0
  • compile err

    compile err

    1. uint64_t catkin_build/src/allan_variance_ros/include/allan_variance_ros/ImuMeasurement.hpp:14:3: error: ‘uint64_t’ does not name a type uint64_t t{}; ///< ROS time message received (nanoseconds).

    add #include <stdint.h> in ImuMeasurement.hpp file.

    1. error: expected unqualified-id before ‘using’ catkin_build/src/allan_variance_ros/include/allan_variance_ros/AllanVarianceComputor.hpp:17:1: error: expected unqualified-id before ‘using’ using EigenVector = std::vector<T, Eigen::aligned_allocator>;
    opened by improve100 1
Owner
Oxford Dynamic Robot Systems Group
Oxford Robotics Institute, University of Oxford
Oxford Dynamic Robot Systems Group
Matryoshka loader is a tool that red team operators can leverage to generate shellcode for Microsoft Office document phishing payloads.

Overview Matryoshka loader is a tool that red team operators can leverage to generate shellcode for an egghunter to bypass size-limitations and perfor

Praetorian 24 Jun 26, 2022
LLpatch: Userspace tool to generate Linux kernel livepatch

LLpatch: LLVM-based Kernel Livepatch Generation LLpatch generates, from a source patch, a kernel loadable module or binary package, that can update Li

Google 24 May 30, 2022
D2R mod generator. Provide quick tool to generate .txt files to change game balance: increase drop, monster density or even randomize items.

Diablo 2 mod generator Generator is inspired by d2modmaker. It provides fast and easy way to create mod without any modding knowledge. Features includ

Smirnov Vladimir 18 Jun 25, 2022
A tool to generate elegant UML-like class/object diagrams for C++ header files

Diagrams for C++ header files Note: This is a PoC project; Issues will drive the development What's this all about We strive for a tool to generate el

Mohammed Elwardi Fadeli 2 Mar 19, 2022
image_projection is a ROS package to create various projections from multiple calibrated cameras.

image_projection Overview image_projection is a ROS package to create various projections from multiple calibrated cameras. Use cases involve: Rectify

Technische Universität Darmstadt ROS Packages 104 Jun 23, 2022
A ROS based Open Source Simulation Environment for Robotics Beginners

A ROS based Open Source Simulation Environment for Robotics Beginners

Sulegeyixiu 101 Jun 25, 2022
Isaac ROS image_pipeline package for hardware-accelerated image processing in ROS2.

isaac_ros_image_pipeline Overview This metapackage offers similar functionality as the standard, CPU-based image_pipeline metapackage, but does so by

NVIDIA AI IOT 30 Apr 1, 2022
This project contains the main ROS 2 packages of Xiaomi CyberDog®.

Xiaomi CyberDog ROS 2 文档包含简体中文和English 简介 - Introduction 本项目包含小米铁蛋®的ROS 2主要功能包. This project contains the main ROS 2 packages of Xiaomi CyberDog®. 基本信

null 333 Jun 27, 2022
Developing a Drone from scratch in ROS Gazebo

Project Building a drone and developing its control system. Table of Contents Project Table of Contents About The Project Tech Stack File Structure Ge

null 15 Mar 22, 2022
ROS packages for Scout 2.0

ROS packages for Scout 2.0

ROAS 3 Jun 14, 2022
Dobot MG400 callaborative robot with ROS

MG400Robot Chinese version of the README -> please click here Building ubuntu16.04 cd $HOME/catkin_ws/src git clone https://github.com/Dobot-Arm/mg40

Dobot 5 Jun 15, 2022
ROS driver for the ICM-20948

ROS driver for the ICM-20948 1 Installation 1.1 Install ROS package on PC $ cd catkin_ws/src $ git clone https://github.com/Alpaca-zip/icm_20948.git

null 3 Apr 11, 2022
This repository uses a ROS node to subscribe to camera (hikvision) and lidar (livox) data. After the node merges the data, it publishes the colored point cloud and displays it in rviz.

fusion-lidar-camera-ROS 一、介绍 本仓库是一个ROS工作空间,其中ws_fusion_camera/src有一个工具包color_pc ws_fusion_camera │ README.md │ └───src │ └───package: c

hongyu wang 14 Jun 15, 2022
A driver for u-blox receiver (ZED-F9P) with ros support

ublox_driver Authors/Maintainers: CAO Shaozu (shaozu.cao AT gmail.com) The ublox_driver provides essential functionalities for u-blox GNSS receivers.

HKUST Aerial Robotics Group 30 Jun 27, 2022
Simple ROS mobile robot

Do-it-yourself differential drive ROS robot This project started out when my wife bought me a Raspberry-Pi 4 (w/ 8GB RAM) for my birthday. At the time

Doug Blanding 14 May 21, 2022
Boilerplate to create a project with: STM32 + Ethernet + micro-ROS + FreeRTOS + Arduino + PlatformIO

micro_ros_stm32_template Boilerplate to create a project with: STM32 + Ethernet + micro-ROS + FreeRTOS + Arduino + PlatformIO Default config STM32F407

Husarion 11 May 7, 2022
A ros package for robust odometry and mapping using LiDAR with aid of different sensors

W-LOAM A ros package for robust odometry and mapping using LiDAR with aid of different sensors Demo Video https://www.bilibili.com/video/BV1Fy4y1L7kZ?

Saki-Chen 45 Jun 14, 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
The ROS version of ICP Mapping with QPEP Solver (Quadratic Pose Estimation Problems)

The ROS version of ICP Mapping with QPEP Solver (Quadratic Pose Estimation Problems) The project is based on https://github.com/ethz-asl/ethzasl_icp_m

Jin Wu 10 Feb 25, 2022