├── .devcontainer ├── Dockerfile.devcontainer ├── README.md ├── devcontainer.json ├── docker-compose.devcontainer.yaml ├── setup_base_env.sh └── setup_container.sh ├── .gitignore ├── .vscode └── .bashrc ├── CMakeLists.txt ├── LICENSE ├── README.md ├── bags └── .gitkeep ├── config ├── alphasense.yaml ├── anymal_b.yaml ├── anymal_c.yaml ├── ouster.yaml ├── realsense_d425i.yaml ├── realsense_t265.yaml ├── sim.yaml └── simulation │ └── imu_simulator.yaml ├── figs ├── realsense_acceleration.png └── realsense_gyro.png ├── include └── allan_variance_ros │ ├── AllanVarianceComputor.hpp │ ├── ImuMeasurement.hpp │ └── yaml_parsers.hpp ├── launch └── record.launch ├── package.xml ├── scripts ├── analysis.py └── cookbag.py └── src ├── AllanVarianceComputor.cpp ├── ImuMeasurement.cpp ├── ImuSimulator.cpp ├── allan_variance.cpp └── yaml_parsers.cpp /.devcontainer/Dockerfile.devcontainer: -------------------------------------------------------------------------------- 1 | # Use the official Ubuntu 20.04 as a base image 2 | FROM ubuntu:20.04 3 | 4 | # Set environment variables for non-interactive installation 5 | ENV DEBIAN_FRONTEND=noninteractive 6 | 7 | ARG uid=1002:1002 8 | ARG gid=1002:1002 9 | ARG username=local 10 | 11 | # Update and install necessary packages 12 | RUN apt-get update && apt-get install -y \ 13 | lsb-release \ 14 | curl \ 15 | gnupg2 \ 16 | build-essential \ 17 | cmake \ 18 | git \ 19 | wget \ 20 | && rm -rf /var/lib/apt/lists/* 21 | 22 | # Setup sources for ROS Noetic 23 | RUN sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' 24 | RUN curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | apt-key add - 25 | 26 | # Install ROS Noetic 27 | RUN apt-get update && apt-get install -y \ 28 | ros-noetic-desktop-full \ 29 | python3-rosdep \ 30 | python3-rosinstall \ 31 | python3-rosinstall-generator \ 32 | python3-wstool \ 33 | python3-rosdep \ 34 | python3-catkin-tools \ 35 | python3-pip \ 36 | python3-rosbag \ 37 | && rm -rf /var/lib/apt/lists/* 38 | 39 | # Install VIM 40 | RUN apt-get update && apt-get install -y vim 41 | 42 | # Initialize rosdep 43 | RUN rosdep init && rosdep update 44 | 45 | # Install pip requirements 46 | RUN pip3 install scipy numpy matplotlib rosbags 47 | 48 | # Setup environment 49 | RUN echo "source /opt/ros/noetic/setup.bash" >> ~/.bashrc 50 | SHELL ["/bin/bash", "-c"] 51 | 52 | # User setup 53 | RUN useradd -m $username -l -u "$(echo $uid | cut -d: -f1)" \ 54 | && usermod -u "$(echo $uid | cut -d: -f1)" $username \ 55 | && groupmod -g "$(echo $gid | cut -d: -f2)" $username \ 56 | && usermod -aG sudo,video,dialout,plugdev,audio $username \ 57 | && echo $username ' ALL=(ALL:ALL) NOPASSWD:ALL' >> /etc/sudoers \ 58 | && mkdir -p /home/$username/.cache /home/$username/.ssh \ 59 | && ssh-keyscan github.com >> /home/$username/.ssh/known_hosts \ 60 | && chown -R $username:$username /home/$username 61 | 62 | # install ssh 63 | RUN apt-get update && apt-get install -y openssh-server 64 | 65 | # Create a workspace 66 | USER $username 67 | RUN mkdir -p /home/local/catkin_ws/src 68 | WORKDIR /home/local/catkin_ws 69 | 70 | # Build the workspace 71 | RUN /bin/bash -c "source /opt/ros/noetic/setup.bash && catkin_make" 72 | RUN rm -rf /home/local/catkin_ws/build /home/local/catkin_ws/devel 73 | 74 | # Set the default command to run when starting the container 75 | CMD ["bash"] -------------------------------------------------------------------------------- /.devcontainer/README.md: -------------------------------------------------------------------------------- 1 | # How to use a devcontainer for development 2 | 3 | 1. Install the [Remote - Containers](https://marketplace.visualstudio.com/items?itemName=ms-vscode-remote.remote-containers) extension in VSCode 4 | 2. Install Docker on local machine (https://docs.docker.com/desktop/setup/install/linux/ubuntu/) 5 | 3. Open the allan_variance_ros repository in VSCode 6 | 4. Reopen in Container: Use the Command Palette (Ctrl+Shift+P or Cmd+Shift+P on macOS) and select "Remote-Containers: Reopen in Container". This will build the container using the Dockerfile and open your project inside the container 7 | 5. Run `catkin build allan_variance_ros` to build the package 8 | 6. Run `rosrun allan_variance_ros allan_variance /path/to/rosbag.bag /path/to/config.yaml` to run the tool 9 | 10 | Once the repository is open in the container, you can develop as normal following the instructions in the [README.md](../README.md) file. 11 | -------------------------------------------------------------------------------- /.devcontainer/devcontainer.json: -------------------------------------------------------------------------------- 1 | // For format details, see https://aka.ms/devcontainer.json. For config options, see the README at: 2 | // https://github.com/microsoft/vscode-dev-containers/tree/v0.241.1/containers/docker-from-docker-compose 3 | { 4 | "name": "ROS Dev Container", 5 | "dockerComposeFile": [ 6 | "${localWorkspaceFolder}/.devcontainer/docker-compose.devcontainer.yaml" 7 | ], 8 | "service": "devcontainer", 9 | "containerUser": "local", 10 | "workspaceFolder": "/home/local/catkin_ws/src/${localWorkspaceFolderBasename}", 11 | 12 | // Setup the environment 13 | "initializeCommand": "${localWorkspaceFolder}/.devcontainer/setup_base_env.sh", 14 | 15 | "onCreateCommand": "${containerWorkspaceFolder}/.devcontainer/setup_container.sh", 16 | 17 | // Sets or overrides environment variables for the container for supporting services/tools (or sub-processes like terminals) 18 | "remoteEnv": { 19 | "DISPLAY": "${localEnv:DISPLAY}", 20 | "XAUTHORITY": "/home/${containerEnv:USER_DOCKER}/.Xauthority", 21 | "XDG_RUNTIME_DIR": "/run/user/${containerEnv:UID}" 22 | }, 23 | 24 | // Add ptrace capability and configure security to facilitate debugging and display forwarding 25 | "capAdd": [ 26 | "SYS_PTRACE", 27 | "NET_ADMIN" 28 | ], 29 | "securityOpt": [ 30 | "seccomp=unconfined", 31 | "apparmor=unconfined" // This allows RViz and other dbus-based apps to run 32 | ], 33 | 34 | // Configure tool-specific properties. 35 | "customizations": { 36 | // Configure properties specific to VS Code. 37 | "vscode": { 38 | // Add the IDs of extensions you want installed when the container is created. 39 | "extensions": [ 40 | "augustocdias.tasks-shell-input@1.9.1", 41 | "eamodio.gitlens", 42 | "hbenl.vscode-test-explorer", 43 | "llvm-vs-code-extensions.vscode-clangd", 44 | "matepek.vscode-catch2-test-adapter", 45 | "ms-python.python", 46 | "ms-python.vscode-pylance", 47 | "ms-vscode.cpptools", 48 | "ms-vscode.cpptools-themes", 49 | "ms-vscode-remote.remote-containers", 50 | "rioj7.command-variable", 51 | "streetsidesoftware.code-spell-checker", 52 | "twxs.cmake", 53 | "ms-python.debugpy" 54 | ], 55 | "settings": { 56 | "bazel.queriesShareServer": false, 57 | "C_Cpp.intelliSenseEngine": "disabled", // Using clangd now! 58 | "C_Cpp.default.cppStandard": "c++17", 59 | "C_Cpp.default.cStandard": "gnu17", 60 | "[cpp]": { 61 | "editor.defaultFormatter": "llvm-vs-code-extensions.vscode-clangd" 62 | }, 63 | "dev.containers.dockerCredentialHelper": false, 64 | "files.exclude": { 65 | "**/bazel-*": true, 66 | ".devcontainer/.vscode-server": true 67 | }, 68 | "remote.localPortHost": "allInterfaces", 69 | "search.exclude": { 70 | "**/bazel-*": true, 71 | ".devcontainer/.vscode-server": true 72 | }, 73 | "testExplorer.useNativeTesting": true, 74 | "testMate.cpp.test.advancedExecutables": [ 75 | { 76 | "pattern": "bazel-bin/**/*.runfiles/**/*{test,Test,TEST,tests}", 77 | "cwd": "${absDirpath}", 78 | "executionWrapper": { 79 | "path": "${workspaceFolder}/.vscode/run_unit_test.sh", 80 | "args": [ "${workspaceFolder}", "${cmd}", "${argsFlat}" ] 81 | } 82 | } 83 | ], 84 | "testMate.cpp.debug.configTemplate": { 85 | "type": "cppvsdbg", 86 | "linux": { 87 | "type": "cppdbg", "MIMode": "gdb", 88 | "setupCommands": [ 89 | { 90 | "description": "Enable pretty-printing for gdb", 91 | "text": "-enable-pretty-printing", 92 | "ignoreFailures": true 93 | }, 94 | { 95 | "description": "Set Disassembly Flavor to Intel", 96 | "text": "-gdb-set disassembly-flavor intel", 97 | "ignoreFailures": true 98 | } 99 | ] 100 | }, 101 | "darwin": { "type": "cppdbg", "MIMode": "lldb" }, 102 | "win32": { "type": "cppvsdbg" }, 103 | "program": "${exec}", 104 | "args": "${argsArray}", 105 | "cwd": "${absDirpath}", 106 | "env": "${envObj}", 107 | "environment": "${envObjArray}", 108 | "sourceFileMap": {"/proc/self/cwd" : "${workspaceFolder}"} 109 | }, 110 | "terminal.integrated.defaultProfile.linux": "custom-bash", 111 | // This setting prevents characters in the terminal from getting jumbled when characters are sent too fast 112 | "terminal.integrated.localEchoLatencyThreshold": -1, 113 | "terminal.integrated.profiles.linux": { 114 | "custom-bash": { 115 | "path": "bash", 116 | "args": ["--rcfile", "${workspaceFolder}/.vscode/.bashrc"] 117 | } 118 | }, 119 | "clangd.path": "/usr/bin/clangd", 120 | "clangd.arguments": [//"--log=verbose", // for debugging, disabled for now 121 | // "--pretty", // for debugging, disabled for now 122 | "--background-index", 123 | //"--query-driver=/bin/arm-buildroot-linux-gnueabihf-g++", //for cross compile usage 124 | "--clang-tidy", 125 | "--completion-style=detailed", 126 | "--header-insertion=never", 127 | "--compile-commands-dir=${workspaceFolder}/"] 128 | } 129 | } 130 | } 131 | } -------------------------------------------------------------------------------- /.devcontainer/docker-compose.devcontainer.yaml: -------------------------------------------------------------------------------- 1 | version: "3.3" 2 | services: 3 | devcontainer: 4 | build: 5 | context: ./../ 6 | dockerfile: ./.devcontainer/${DOCKERFILE_BUILD} 7 | args: 8 | build_apt_deps: ${BUILD_APT_DEPS} 9 | username: ${USER_DOCKER} 10 | uid: ${CURRENT_UID} 11 | ssh_port: ${SSH_PORT} 12 | devices: 13 | - "/dev/bus/usb:/dev/bus/usb" 14 | - "/dev/video*:/dev/video*" 15 | - "/dev:/dev" 16 | ports: 17 | - "9876:9876" 18 | privileged: true 19 | user: ${USER_DOCKER} 20 | volumes: 21 | - ${WORKSPACE}:${WORKSPACE_DOCKER} 22 | - ${NETRC_FILE:-/dev/null}:${NETRC_FILE_DOCKER:-/home/$USER/.netrc-null} 23 | - ${AWS_FILE:-/dev/null}:${AWS_FILE_DOCKER:-/home/$USER/.aws-null} 24 | - ${SSH_FILE:-/dev/null}:${SSH_FILE_DOCKER:-/home/$USER/.ssh/ssh.null} 25 | - ${DOCKER_DIR:-/dev/null}:${DOCKER_DIR_DOCKER:-/home/$USER/.docker-null} 26 | - ${BAZEL_LOCAL_CACHE:-/dev/null}:${BAZEL_LOCAL_CACHE_DOCKER:-/home/$USER/.cache/bazel-null} 27 | - ${HOME}:${HOME} 28 | - ${HOME}/.config:${HOME_DOCKER}/.config 29 | - ${HOME}/.gitconfig:${HOME_DOCKER}/.gitconfig 30 | - ${WORKSPACE}/.devcontainer/.vscode-server:${HOME_DOCKER}/.vscode-server 31 | - ${SSH_AUTH_SOCK:-/dev/null}:${SSH_AUTH_SOCK:-/home/$USER_DOCKER/.ssh-auth-sock-null} 32 | - /tmp/.X11-unix:/tmp/.X11-unix 33 | - ${XAUTHORITY:-~/.Xauthority}:/home/${USER_DOCKER}/.Xauthority 34 | - ${HOME}/.Xauthority:/root/.Xauthority 35 | env_file: .env 36 | environment: 37 | ARCH: ${ARCH:-amd64} 38 | VCS_REF: ${VCS_REF:-dirty} 39 | VCS_SHORT_REF: ${VCS_SHORT_REF:-dirty} 40 | VCS_URL: ${VCS_URL:-dirty} 41 | VCS_DESCRIBE: ${GIT_DESCRIBE:-none} 42 | BUILD_NUMBER: ${BUILD_NUMBER:-non-ci} 43 | BUILD_DATE: ${BUILD_DATE:-non-ci} 44 | BRANCH_NAME: ${BRANCH_NAME:-non-ci} 45 | DOCKER_TAG: ${DOCKER_TAG:-non-ci} 46 | SSH_AUTH_SOCK: ${SSH_AUTH_SOCK} 47 | HOST_SSH_AUTH_SOCK: ${SSH_AUTH_SOCK} 48 | WORKSPACE_DOCKER: ${WORKSPACE_DOCKER} 49 | USER_HOST: ${USER} 50 | USER_DOCKER: ${USER_DOCKER} 51 | DISPLAY: ${DISPLAY} 52 | XAUTHORITY: /home/${USER_DOCKER}/.Xauthority 53 | command: /bin/sh -c "while sleep 1000; do :; done" 54 | working_dir: ${WORKSPACE_DOCKER} 55 | network_mode: host 56 | ipc: host 57 | pid: host -------------------------------------------------------------------------------- /.devcontainer/setup_base_env.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Exit on failures 4 | set -e 5 | 6 | USER_DOCKER="local" 7 | HOME_DOCKER="/home/${USER_DOCKER}" 8 | 9 | # fix .gitconfig causing issues when the file doesn't exist before creating devcontainer 10 | GITCONFIG=${HOME}/.gitconfig 11 | 12 | if [ -d "$GITCONFIG" ]; then 13 | # Directory exists, remove directory 14 | rm -rf $GITCONFIG 15 | fi 16 | 17 | if [ ! -f "$GITCONFIG" ]; then 18 | # Check if file doesn't exist, then create one 19 | touch $GITCONFIG 20 | fi 21 | 22 | # Get the workspace path without expanding symlinks 23 | WORKSPACE_PATH=$(pwd)/$(git rev-parse --show-cdup) 24 | WORKSPACE_BASENAME=$(basename $WORKSPACE_PATH) 25 | WORKSPACE_DOCKER="${HOME_DOCKER}/catkin_ws/src/${WORKSPACE_BASENAME}" 26 | 27 | DEVCONTAINER_PATH=${DEVCONTAINER_PATH:=${WORKSPACE_PATH}/.devcontainer} 28 | USER_CONFIG_PATH=$DEVCONTAINER_PATH/user_config 29 | DOCKERFILE_BUILD="Dockerfile.devcontainer" 30 | 31 | # Load the user config for the container, which could have the following options: 32 | # ADDITIONAL_BUILD_APT_DEPS - additional APT packages to install when building the container 33 | # HOST_HOME_MOUNT_PATH - location in the container to mount the user's home folder from the host machine, defaults to /home/$USER/host_home 34 | # DOCKERFILE_BUILD - target an alternate Dockerfile 35 | if [ -f $USER_CONFIG_PATH ]; then 36 | . $USER_CONFIG_PATH 37 | fi 38 | 39 | # Create the .env file 40 | ENV_TARGET=${DEVCONTAINER_PATH}/.env 41 | rm -f $ENV_TARGET 42 | 43 | # Add additional variables needed in the target environment 44 | echo "DOCKERFILE_BUILD=${DOCKERFILE_BUILD}" >> $ENV_TARGET 45 | echo "WORKSPACE=$WORKSPACE_PATH" >> $ENV_TARGET 46 | echo "WORKSPACE_DOCKER=$WORKSPACE_DOCKER" >> $ENV_TARGET 47 | echo "WORKSPACE_BASENAME=${WORKSPACE_BASENAME}" >> $ENV_TARGET 48 | echo "HOST_HOME_MOUNT_PATH=${HOST_HOME_MOUNT_PATH:=/home/$USER/host_home}" >> $ENV_TARGET 49 | echo "DEVCONTAINER_PATH=$DEVCONTAINER_PATH" >> $ENV_TARGET 50 | echo "HOME_DOCKER=${HOME_DOCKER}" >> $ENV_TARGET 51 | echo "USER_DOCKER=${USER_DOCKER}" >> $ENV_TARGET 52 | 53 | # Define which nvidia drivers/libs to load into the container 54 | # See https://docs.nvidia.com/datacenter/cloud-native/container-toolkit/1.10.0/user-guide.html#driver-capabilities 55 | echo "NVIDIA_DRIVER_CAPABILITIES=${NVIDIA_DRIVER_CAPABILITIES:=}" >> $ENV_TARGET 56 | 57 | # Set env variable for netrc file 58 | NETRC_FILE=${NETRC_FILE:="${HOME}/.netrc"} 59 | NETRC_FILE_DOCKER=${NETRC_FILE_DOCKER:="${HOME_DOCKER}/.netrc"} 60 | if [ ! -f "${NETRC_FILE}" ]; then 61 | NETRC_FILE=/dev/null 62 | NETRC_FILE_DOCKER="${HOME_DOCKER}/.netrc-null" 63 | fi 64 | 65 | echo "NETRC_FILE=$NETRC_FILE" >> $ENV_TARGET 66 | echo "NETRC_FILE_DOCKER=$NETRC_FILE_DOCKER" >> $ENV_TARGET 67 | 68 | # Set env variable for ssh file 69 | SSH_FILE_NAME=${SSH_FILE_NAME} 70 | SSH_FILE=${SSH_FILE:-"${HOME}/.ssh/${SSH_FILE_NAME}"} 71 | SSH_FILE_DOCKER=${SSH_FILE_DOCKER:-"${HOME_DOCKER}/.ssh/${SSH_FILE_NAME}"} 72 | 73 | if [ ! -f {$SSH_FILE} ]; then 74 | SSH_FILE=/dev/null 75 | SSH_FILE_DOCKER="${HOME_DOCKER}/.ssh/ssh.null" 76 | 77 | for key_name in "id_rsa" "id_ed25519" 78 | do 79 | TEMP_SSH_FILE="${HOME}/.ssh/${key_name}" 80 | 81 | if [ -f "$TEMP_SSH_FILE" ]; then 82 | export SSH_FILE_NAME="${key_name}" 83 | export SSH_FILE="${TEMP_SSH_FILE}" 84 | export SSH_FILE_DOCKER="${HOME_DOCKER}/.ssh/${SSH_FILE_NAME}" 85 | fi 86 | done 87 | fi 88 | 89 | echo "SSH_FILE=$SSH_FILE" >> $ENV_TARGET 90 | echo "SSH_FILE_DOCKER=$SSH_FILE_DOCKER" >> $ENV_TARGET 91 | 92 | # Set env variable for docker config dir 93 | DOCKER_DIR=${DOCKER_DIR:="${HOME}/.docker"} 94 | DOCKER_DIR_DOCKER=${DOCKER_DIR_DOCKER:="${HOME_DOCKER}/.docker"} 95 | if [ ! -d "${DOCKER_DIR}" ]; then 96 | DOCKER_DIR=/dev/null 97 | DOCKER_DIR_DOCKER="${HOME_DOCKER}/.docker-null" 98 | fi 99 | 100 | echo "DOCKER_DIR=$DOCKER_DIR" >> $ENV_TARGET 101 | echo "DOCKER_DIR_DOCKER=$DOCKER_DIR_DOCKER" >> $ENV_TARGET 102 | 103 | # Add the user's ID to the environment 104 | echo "CURRENT_UID=$(id -u):$(id -g)" >> $ENV_TARGET 105 | 106 | # Ensure the .vscode-server folder exists 107 | VSCODE_SERVER_PATH=${DEVCONTAINER_PATH}/.vscode-server 108 | if [ ! -d $VSCODE_SERVER_PATH ]; then 109 | mkdir $VSCODE_SERVER_PATH 110 | else 111 | # Remove the machine settings file to ensure the latest is used from the devcontainer.json 112 | rm -f $VSCODE_SERVER_PATH/data/Machine/settings.json 113 | rm -f $VSCODE_SERVER_PATH/data/Machine/.writeMachineSettingsMarker 114 | 115 | # Remove the install extensions marker file to ensure the extensions in the devcontainer.json are installed 116 | rm -f $VSCODE_SERVER_PATH/data/Machine/.installExtensionsMarker 117 | fi 118 | 119 | SSH_PORT=${SSH_PORT:-8888} # Can override in user_config 120 | echo "SSH_PORT=${SSH_PORT}" >> $ENV_TARGET 121 | -------------------------------------------------------------------------------- /.devcontainer/setup_container.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | set -e 3 | 4 | sudo service ssh start 5 | 6 | # Enable any ssh key that can ssh into machine to be able to ssh into dev container 7 | if [ -f /home/"$USER_HOST"/.ssh/authorized_keys ]; then 8 | cp /home/"$USER_HOST"/.ssh/authorized_keys /home/"$USER_DOCKER"/.ssh/authorized_keys 9 | fi 10 | 11 | # Allow local machine to ssh into dev container with existing key 12 | if [ -f /home/"$USER_HOST"/.ssh/id_rsa.pub ]; then 13 | cat /home/"$USER_HOST"/.ssh/id_rsa.pub >> /home/"$USER_DOCKER"/.ssh/authorized_keys 14 | elif [ -f /home/"$USER_HOST"/.ssh/id_ed25519.pub ]; then 15 | cat /home/"$USER_HOST"/.ssh/id_ed25519.pub >> /home/"$USER_DOCKER"/.ssh/authorized_keys 16 | fi 17 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Ignore ROS build directories 2 | build/ 3 | devel/ 4 | install/ 5 | 6 | # Ignore log files 7 | log/ 8 | 9 | # Ignore ROS bag files 10 | *.bag 11 | 12 | # Ignore CSV files autogenerated by AllanVarianceComputor 13 | *.csv 14 | 15 | # Ignore development environment files 16 | .catkin_tools/ 17 | .vscode-server/ 18 | .env -------------------------------------------------------------------------------- /.vscode/.bashrc: -------------------------------------------------------------------------------- 1 | # Get the directory containing this script, assuming this script is being sourced 2 | SCRIPT_DIR=$( dirname "${BASH_SOURCE[0]}" ) 3 | 4 | # Include .bashrc if it exists 5 | if [ -f $HOME/.bashrc ]; then 6 | . $HOME/.bashrc 7 | fi 8 | 9 | # Include .bashrc if it exists 10 | if [ -f $SCRIPT_DIR/user.bashrc ]; then 11 | . $SCRIPT_DIR/user.bashrc 12 | fi 13 | 14 | # Provide git/bazel/etc.. completion for everyone 15 | # source /usr/share/bash-completion/bash_completion 16 | 17 | source /opt/ros/noetic/setup.bash -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(allan_variance_ros) 3 | 4 | set(CATKIN_PACKAGE_DEPENDENCIES 5 | geometry_msgs 6 | sensor_msgs 7 | rospy 8 | tf2_geometry_msgs 9 | tf2_ros 10 | rosbag) 11 | 12 | ## Find catkin macros and libraries 13 | find_package(catkin REQUIRED COMPONENTS 14 | ${CATKIN_PACKAGE_DEPENDENCIES} 15 | ) 16 | 17 | find_package(yaml-cpp REQUIRED) 18 | 19 | find_package(OpenMP) 20 | if (OPENMP_FOUND) 21 | message(STATUS "OpenMP found will try to link!") 22 | set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 23 | set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 24 | set (CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}") 25 | endif() 26 | 27 | catkin_package( 28 | INCLUDE_DIRS include 29 | LIBRARIES ${PROJECT_NAME} 30 | CATKIN_DEPENDS 31 | ${CATKIN_PACKAGE_DEPENDENCIES} 32 | DEPENDS YAML_CPP 33 | ) 34 | 35 | # Enable compile optimizations 36 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -fsee -fomit-frame-pointer -fno-signed-zeros -fno-math-errno -funroll-loops") 37 | 38 | # Enable debug flags (use if you want to debug in gdb) 39 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g3 -Wall -Wuninitialized -fno-omit-frame-pointer") 40 | 41 | include_directories( 42 | include 43 | ${EIGEN3_INCLUDE_DIRS} 44 | ${catkin_INCLUDE_DIRS} 45 | ) 46 | 47 | add_library(${PROJECT_NAME} SHARED 48 | src/ImuMeasurement.cpp 49 | src/yaml_parsers.cpp 50 | src/AllanVarianceComputor.cpp 51 | ) 52 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES}) 53 | 54 | add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) 55 | 56 | 57 | add_executable(allan_variance src/allan_variance.cpp) 58 | 59 | target_link_libraries(allan_variance ${PROJECT_NAME} ) 60 | 61 | add_executable(imu_simulator src/ImuSimulator.cpp) 62 | 63 | target_link_libraries(imu_simulator ${PROJECT_NAME} ) 64 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2021, Russell Buchanan, University of Oxford 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | 3. Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Allan Variance ROS 2 | ## ROS package which loads a rosbag of IMU data and computes Allan Variance parameters 3 | The purpose of this tool is to read a long sequence of IMU data and compute the Angle Random Walk (ARW), Bias Instability and Gyro Random Walk for the gyroscope as well as Velocity Random Walk (VRW), Bias Instability and Accel Random Walk for the accelerometer. 4 | 5 | While there are many open source tools which do the same thing, this package has the following features: 6 | 7 | - Fully ROS compatable. Simply record a `rosbag` and provide it as input. No conversion required. 8 | - Written in C++ making use of rosbag::View means the `rosbag` is processed at maximum speed. No need to play back the bag file. 9 | - Designed for [Kalibr](https://github.com/ethz-asl/kalibr). Will produce an `imu.yaml` file. 10 | 11 | This tool is designed for Ubuntu 20.04. Attempting to use on another distro or version may require some code changes. 12 | 13 | If you do not have Ubuntu 20.04, you can use the devcontainer to build and run the tool. See the [devcontainer README](.devcontainer/README.md) for more information. 14 | 15 | ## How to build 16 | 17 | The allan_variance_ros folder should be in the src folder of the catkin workspace. 18 | For example: ~/catkin_ws/src/allan_variance_ros 19 | 20 | From the catkin workspace directory (e.g. ~/catkin_ws), run: 21 | ``catkin build allan_variance_ros`` 22 | ``source devel/setup.bash`` 23 | 24 | ## How to use 25 | 26 | 1. Ensure rosmaster is running by running ``roscore`` in a terminal. 27 | 28 | 2. 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. 29 | 30 | 3. **Recommended** Reorganize ROS messages by timestamp: 31 | 32 | ``rosrun allan_variance_ros cookbag.py --input original_rosbag --output cooked_rosbag`` 33 | 34 | 4. Run the Allan Variance computation tool (example config files provided): 35 | 36 | ``rosrun allan_variance_ros allan_variance [path_to_folder_containing_bag] [path_to_config_file]`` 37 | 38 | 5. 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: 39 | 40 | ``rosrun allan_variance_ros analysis.py --data allan_variance.csv`` 41 | 42 | If you have a config file to set the topic and update rate, you can pass it in as well: 43 | 44 | ``rosrun allan_variance_ros analysis.py --data allan_variance.csv --config config/realsense_d425i.yaml`` 45 | 46 | Press `space` to go to next figure. 47 | 48 | 49 | 50 | ### Example Log 51 | 52 | 3 hour log of [Realsense D435i IMU](https://drive.google.com/file/d/1ovI2NvYR52Axt-KuRs5HjVk7-57ky72H/view?usp=sharing) with timestamps already re-arranged. 53 | 54 | ![Acceleration](/figs/realsense_acceleration.png) 55 | ![Gyroscope](/figs/realsense_gyro.png) 56 | 57 | Example terminal output: 58 | 59 | ``` 60 | ACCELEROMETER: 61 | X Velocity Random Walk: 0.00333 m/s/sqrt(s) 0.19983 m/s/sqrt(hr) 62 | Y Velocity Random Walk: 0.01079 m/s/sqrt(s) 0.64719 m/s/sqrt(hr) 63 | Z Velocity Random Walk: 0.00481 m/s/sqrt(s) 0.28846 m/s/sqrt(hr) 64 | X Bias Instability: 0.00055 m/s^2 7173.28800 m/hr^2 65 | Y Bias Instability: 0.00153 m/s^2 19869.01200 m/hr^2 66 | Z Bias Instability: 0.00052 m/s^2 6701.58000 m/hr^2 67 | X Accel Random Walk: 0.00008 m/s^2/sqrt(s) 68 | Y Accel Random Walk: 0.00020 m/s^2/sqrt(s) 69 | Z Accel Random Walk: 0.00007 m/s^2/sqrt(s) 70 | GYROSCOPE: 71 | X Angle Random Walk: 0.00787 deg/sqrt(s) 0.47215 deg/sqrt(hr) 72 | Y Angle Random Walk: 0.00987 deg/sqrt(s) 0.59204 deg/sqrt(hr) 73 | Z Angle Random Walk: 0.00839 deg/sqrt(s) 0.50331 deg/sqrt(hr) 74 | X Bias Instability: 0.00049 deg/s 1.76568 deg/hr 75 | Y Bias Instability: 0.00136 deg/s 4.88153 deg/hr 76 | Z Bias Instability: 0.00088 deg/s 3.15431 deg/hr 77 | X Rate Random Walk: 0.00007 deg/s/sqrt(s) 78 | Y Rate Random Walk: 0.00028 deg/s/sqrt(s) 79 | Z Rate Random Walk: 0.00011 deg/s/sqrt(s) 80 | 81 | ``` 82 | 83 | ## Kalibr 84 | 85 | [Kalibr](https://github.com/ethz-asl/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`: 86 | 87 | ``` 88 | #Accelerometer 89 | accelerometer_noise_density: 0.006308226052016165 90 | accelerometer_random_walk: 0.00011673723527962174 91 | 92 | #Gyroscope 93 | gyroscope_noise_density: 0.00015198973532354657 94 | gyroscope_random_walk: 2.664506559330434e-06 95 | 96 | rostopic: '/sensors/imu' #Make sure this is correct 97 | update_rate: 400.0 #Make sure this is correct 98 | 99 | ``` 100 | ## Allan Variance ROS Evaluation 101 | 102 | ### IMU Noise Simulator 103 | 104 | Thanks to [@kekeliu-whu](https://github.com/kekeliu-whu) who contributed an IMU noise simulator is based on the [Kalibr IMU noise model](https://github.com/ethz-asl/kalibr/wiki/IMU-Noise-Model). You can generate a rosbag of simulated IMU noise and run allan_variance_ros to verify the tool is working. 105 | As shown in PR https://github.com/ori-drs/allan_variance_ros/pull/24 accuracy is quite good. 106 | 107 | 108 | ### To generate simulated noise 109 | 110 | `rosrun allan_variance_ros imu_simulator [path_to_output_bag_file] [path_to_simulation_config_file]` 111 | 112 | A simulation config file is provided in `allan_variance_ros/config/simulation/imu_simulator.yaml` 113 | 114 | ### To test Allan Variance ROS on simulated rosbag 115 | 116 | ``rosrun allan_variance_ros allan_variance [path_to_folder_containing_bag] [path_to_config_file]`` 117 | 118 | A config file is provided in `allan_variance_ros/config/sim.yaml` 119 | 120 | ### Additional Example bags 121 | 122 | Some additional rosbags of real IMU data for different sensors is available [here](https://drive.google.com/drive/folders/1a3Es85JDKl7tSpVWEUZryOwtsXB8793o). Thanks to Patrick Geneva. 123 | 124 | ## Author 125 | 126 | [Russell Buchanan](https://www.ripl-lab.com/) 127 | 128 | If you use this package for academic work, please consider using the citation below: 129 | 130 | ``` 131 | @software{AllanVarianceRos, 132 | author = {Russell Buchanan}, 133 | title = {Allan Variance ROS}, 134 | month = Nov, 135 | year = 2021, 136 | publisher = {Oxford Robotics Institute, DRS Lab}, 137 | version = {1.2}, 138 | url = {https://github.com/ori-drs/allan_variance_ros}} 139 | } 140 | ``` 141 | 142 | ## References 143 | 144 | - [Indirect Kalman Filter for 3D Attitude Estimation, Trawny & Roumeliotis](http://mars.cs.umn.edu/tr/reports/Trawny05b.pdf) 145 | - [An introduction to inertial navigation, Oliver Woodman](https://www.cl.cam.ac.uk/techreports/UCAM-CL-TR-696.pdf) 146 | - [Characterization of Errors and Noises in MEMS Inertial Sensors Using Allan Variance Method, Leslie Barreda Pupo](https://upcommons.upc.edu/bitstream/handle/2117/103849/MScLeslieB.pdf?sequence=1&isAllowed=y) 147 | - [Kalibr IMU Noise Documentation](https://github.com/ethz-asl/kalibr/wiki/IMU-Noise-Model) 148 | -------------------------------------------------------------------------------- /bags/.gitkeep: -------------------------------------------------------------------------------- 1 | # Place ros1 bags here -------------------------------------------------------------------------------- /config/alphasense.yaml: -------------------------------------------------------------------------------- 1 | imu_topic: "/alphasense_driver_ros/imu" 2 | imu_rate: 200 3 | measure_rate: 200 # Rate to which imu data is subsampled 4 | sequence_time: 10800 # 3 hours in seconds -------------------------------------------------------------------------------- /config/anymal_b.yaml: -------------------------------------------------------------------------------- 1 | imu_topic: "/sensors/imu" 2 | imu_rate: 400 3 | measure_rate: 100 # Rate to which imu data is subsampled 4 | sequence_time: 3500 # 1 hour in seconds -------------------------------------------------------------------------------- /config/anymal_c.yaml: -------------------------------------------------------------------------------- 1 | imu_topic: "/sensors/imu" 2 | imu_rate: 400 3 | measure_rate: 100 # Rate to which imu data is subsampled 4 | sequence_time: 10800 # 3 hours in seconds -------------------------------------------------------------------------------- /config/ouster.yaml: -------------------------------------------------------------------------------- 1 | imu_topic: "/os_cloud_node/imu" 2 | imu_rate: 100 3 | measure_rate: 100 # Rate to which imu data is subsampled 4 | sequence_time: 10800 # 3 hours in seconds -------------------------------------------------------------------------------- /config/realsense_d425i.yaml: -------------------------------------------------------------------------------- 1 | imu_topic: "/camera/imu" 2 | imu_rate: 400 3 | measure_rate: 100 # Rate to which imu data is subsampled 4 | sequence_time: 10800 # 3 hours in seconds -------------------------------------------------------------------------------- /config/realsense_t265.yaml: -------------------------------------------------------------------------------- 1 | imu_topic: "/camera/imu" 2 | imu_rate: 200 3 | measure_rate: 200 # Rate to which imu data is subsampled 4 | sequence_time: 10800 # 3 hours in seconds -------------------------------------------------------------------------------- /config/sim.yaml: -------------------------------------------------------------------------------- 1 | imu_topic: '/sensors/imu' 2 | imu_rate: 400 3 | measure_rate: 400 # Rate to which imu data is subsampled 4 | sequence_time: 11000 # 3 hours in seconds 5 | -------------------------------------------------------------------------------- /config/simulation/imu_simulator.yaml: -------------------------------------------------------------------------------- 1 | #Accelerometer 2 | accelerometer_noise_density: 0.0025019929573561175 3 | accelerometer_random_walk: 6.972435158192731e-05 4 | accelerometer_bias_init: 0.007 5 | 6 | #Gyroscope 7 | gyroscope_noise_density: 0.0001888339269965301 8 | gyroscope_random_walk: 2.5565313322052523e-06 9 | gyroscope_bias_init: 0.006 10 | 11 | rostopic: '/sensors/imu' 12 | update_rate: 400.0 13 | 14 | sequence_time: 11000 15 | -------------------------------------------------------------------------------- /figs/realsense_acceleration.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ori-drs/allan_variance_ros/1d54b602ee7f2ba0427865d63afe4945d913ed24/figs/realsense_acceleration.png -------------------------------------------------------------------------------- /figs/realsense_gyro.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ori-drs/allan_variance_ros/1d54b602ee7f2ba0427865d63afe4945d913ed24/figs/realsense_gyro.png -------------------------------------------------------------------------------- /include/allan_variance_ros/AllanVarianceComputor.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // ROS 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | // allan_variance_ros 13 | #include "allan_variance_ros/ImuMeasurement.hpp" 14 | #include "allan_variance_ros/yaml_parsers.hpp" 15 | 16 | namespace allan_variance_ros { 17 | 18 | template 19 | using EigenVector = std::vector>; 20 | 21 | uint64_t s2ns(double t) { return static_cast(t * 1000000000); } 22 | double ns2s(uint64_t t) { return static_cast(t * 0.000000001); } 23 | 24 | struct ImuFormat { 25 | double time; 26 | double accX; 27 | double accY; 28 | double accZ; 29 | double gyroX; 30 | double gyroY; 31 | double gyroZ; 32 | double qx; 33 | double qy; 34 | double qz; 35 | double qw; 36 | 37 | void writeOnFile(std::ofstream& file) { 38 | file << std::setprecision(19) << time << std::setprecision(7) << " " << accX << " " << accY << " " << accZ << " " 39 | << gyroX << " " << gyroY << " " << gyroZ << " " << qx << " " << qy << " " << qz << " " << qw << std::endl; 40 | } 41 | }; 42 | 43 | struct AllanVarianceFormat { 44 | double period; 45 | double accX; 46 | double accY; 47 | double accZ; 48 | double gyroX; 49 | double gyroY; 50 | double gyroZ; 51 | 52 | void writeOnFile(std::ofstream& file) { 53 | file << std::setprecision(19) << period << std::setprecision(7) << " " << accX << " " << accY << " " << accZ << " " 54 | << gyroX << " " << gyroY << " " << gyroZ << " " << std::endl; 55 | } 56 | }; 57 | 58 | class AllanVarianceComputor { 59 | public: 60 | AllanVarianceComputor(ros::NodeHandle& nh, std::string config_file, std::string output_path); 61 | 62 | virtual ~AllanVarianceComputor() {closeOutputs();} 63 | 64 | void run(std::string bag_path); 65 | void closeOutputs(); 66 | void allanVariance(); 67 | void writeAllanDeviation(std::vector variance, double period); 68 | 69 | private: 70 | // ROS 71 | ros::NodeHandle& nh_; 72 | 73 | // Data 74 | AllanVarianceFormat aVRecorder_{}; 75 | std::ofstream av_output_; 76 | std::string imu_output_file_; 77 | 78 | // Config 79 | int sequence_time_{}; 80 | int measure_rate_{}; 81 | std::vector input_topics_; 82 | double imu_rate_ = 100.0; 83 | 84 | int skipped_imu_{}; 85 | int imu_skip_; 86 | uint64_t tCurrNanoSeconds_{}; 87 | uint64_t lastImuTime_{}; 88 | uint64_t firstTime_{}; 89 | EigenVector imuBuffer_; 90 | bool firstMsg_; 91 | float overlap_; // Percent to overlap bins 92 | }; 93 | } // namespace allan_variance_ros -------------------------------------------------------------------------------- /include/allan_variance_ros/ImuMeasurement.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | 7 | /// 8 | /// \brief Contains data from the IMU mesaurements. 9 | /// 10 | class ImuMeasurement{ 11 | public: 12 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 13 | 14 | uint64_t t{}; ///< ROS time message received (nanoseconds). 15 | 16 | Eigen::Vector3d I_a_WI; ///< Raw acceleration from the IMU (m/s/s) 17 | Eigen::Vector3d I_w_WI; ///< Raw angular velocity from the IMU (deg/s) 18 | 19 | ~ImuMeasurement() {} 20 | ImuMeasurement(); 21 | ImuMeasurement(const uint64_t _t, 22 | const Eigen::Vector3d& _I_a_WI, 23 | const Eigen::Vector3d& _I_w_WI); 24 | friend std::ostream& operator<< (std::ostream& stream, const ImuMeasurement& meas); 25 | }; 26 | 27 | -------------------------------------------------------------------------------- /include/allan_variance_ros/yaml_parsers.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | 11 | /// Update the value only if it can be found in the YAML file. 12 | template static 13 | bool get(const YAML::Node &node, const std::string& param, T& value) { 14 | if (!node[param]) { return false; } 15 | value = node[param].as(); 16 | return true; 17 | } 18 | 19 | YAML::Node loadYamlFile(const std::string &filename); 20 | 21 | -------------------------------------------------------------------------------- /launch/record.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | allan_variance_ros 4 | 0.0.1 5 | 6 | Allan Variance ROS 7 | 8 | 9 | Russell Buchanan 10 | 11 | Russell Buchanan 12 | 13 | Proprietary 14 | 15 | https://github.com/ori-drs/kins 16 | 17 | catkin 18 | 19 | geometry_msgs 20 | sensor_msgs 21 | rospy 22 | tf2_geometry_msgs 23 | tf2_ros 24 | rosbag 25 | yaml-cpp 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /scripts/analysis.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | """ 4 | @file analysis.py 5 | @brief Plotting and analysis tool to determine IMU parameters. 6 | @author Russell Buchanan 7 | """ 8 | 9 | import argparse 10 | import csv 11 | 12 | import matplotlib.pyplot as plt 13 | import numpy as np 14 | from scipy.optimize import curve_fit 15 | 16 | 17 | def line_func(x, m, b): 18 | """ 19 | Linear function to fit the data. 20 | 21 | Args: 22 | x (np.array): The x values to fit. 23 | m (float): The slope of the line. 24 | b (float): The y-intercept of the line. 25 | 26 | Returns: 27 | np.array: The y values of the line. 28 | """ 29 | return m * x + b 30 | 31 | def get_intercept(x, y, m, b): 32 | """ 33 | Get the intercept of the line. 34 | 35 | Args: 36 | x (np.array): The x values to fit. 37 | y (np.array): The y values to fit. 38 | m (float): The slope of the line. 39 | b (float): The y-intercept of the line. 40 | 41 | Returns: 42 | float: The y-intercept of the line. 43 | """ 44 | 45 | logx = np.log(x) 46 | logy = np.log(y) 47 | coeffs, _ = curve_fit(line_func, logx, logy, bounds=([m, -np.inf], [m + 0.001, np.inf])) 48 | poly = np.poly1d(coeffs) 49 | yfit = lambda x: np.exp(poly(np.log(x))) 50 | 51 | return yfit(b), yfit 52 | 53 | 54 | def generate_prediction(tau, q_quantization=0, q_white=0, q_bias_instability=0, q_walk=0, q_ramp=0): 55 | """ 56 | Generate a prediction of the Allan deviation. 57 | 58 | Args: 59 | tau (np.array): The time periods to predict. 60 | q_quantization (float): The quantization noise. 61 | q_white (float): The white noise. 62 | q_bias_instability (float): The bias instability. 63 | q_walk (float): The random walk. 64 | q_ramp (float): The ramp. 65 | """ 66 | n = len(tau) 67 | 68 | # Create the matrix A 69 | A = np.empty((n, 5)) 70 | A[:, 0] = 3 / tau**2 71 | A[:, 1] = 1 / tau 72 | A[:, 2] = 2 * np.log(2) / np.pi 73 | A[:, 3] = tau / 3 74 | A[:, 4] = tau**2 / 2 75 | 76 | # Create the parameters vector 77 | params = np.array([q_quantization ** 2, q_white ** 2, q_bias_instability ** 2, q_walk ** 2, q_ramp ** 2]) 78 | 79 | # Calculate the Allan deviation 80 | return np.sqrt(A.dot(params)) 81 | 82 | # Parse command line arguments 83 | parser = argparse.ArgumentParser() 84 | parser.add_argument('--data', metavar='STR', type=str, help='TUM data files to plot') 85 | parser.add_argument('--config', metavar='STR', type=str, help='yaml config file') 86 | parser.add_argument("--skip", type=int, default=1) 87 | parser.add_argument("--output", type=str, default="imu.yaml") 88 | args = parser.parse_args() 89 | 90 | # Load config file if provided 91 | rostopic = "/sensors/imu" 92 | update_rate = 400.0 93 | if args.config: 94 | import yaml #pip install pyyaml 95 | with open(args.config, "r") as stream: 96 | config = yaml.safe_load(stream) 97 | rostopic = config["imu_topic"] 98 | update_rate = config["imu_rate"] 99 | 100 | # Assumes tum format 101 | # Initialize arrays 102 | period = np.array([]) 103 | acceleration = np.empty((0,3), float) 104 | rotation_rate = np.empty((0,3), float) 105 | 106 | # Read data from file 107 | with open(args.data) as input_file: 108 | csv_reader = csv.reader(input_file, delimiter=' ') 109 | counter = 0 110 | 111 | for row in csv_reader: 112 | counter = counter + 1 113 | if (counter % args.skip != 0): 114 | continue 115 | # Data is in the format: 116 | # period, accel_x_allan_deviation, accel_y_allan_deviation, accel_z_allan_deviation, gyro_x_allan_deviation, gyro_y_allan_deviation, gyro_z_allan_deviation 117 | t = float(row[0]) 118 | period = np.append(period, [t], axis=0) 119 | acceleration = np.append(acceleration, np.array([float(row[1]), float(row[2]), float(row[3])]).reshape(1,3), axis=0) 120 | rotation_rate = np.append(rotation_rate, np.array([float(row[4]), float(row[5]), float(row[6])]).reshape(1,3), axis=0) 121 | 122 | # Calculate intercepts and minimums for accelerometer 123 | white_noise_break_point = np.where(period == 10)[0][0] 124 | random_rate_break_point = np.where(period == 10)[0][0] 125 | 126 | # White Noise Intercept with a line of the form y = -0.5 * x + 1.0 127 | 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) 128 | accel_wn_intercept_y, yfit_wn = get_intercept(period[0:white_noise_break_point], acceleration[0:white_noise_break_point,1], -0.5, 1.0) 129 | accel_wn_intercept_z, zfit_wn = get_intercept(period[0:white_noise_break_point], acceleration[0:white_noise_break_point,2], -0.5, 1.0) 130 | 131 | # Random Walk Intercept with a line of the form y = 0.5 * x + 3.0 132 | accel_rr_intercept_x, xfit_rr = get_intercept(period, acceleration[:,0], 0.5, 3.0) 133 | accel_rr_intercept_y, yfit_rr = get_intercept(period, acceleration[:,1], 0.5, 3.0) 134 | accel_rr_intercept_z, zfit_rr = get_intercept(period, acceleration[:,2], 0.5, 3.0) 135 | 136 | accel_min_x = np.amin(acceleration[:,0]) 137 | accel_min_y = np.amin(acceleration[:,1]) 138 | accel_min_z = np.amin(acceleration[:,2]) 139 | 140 | accel_min_x_index = np.argmin(acceleration[:,0]) 141 | accel_min_y_index = np.argmin(acceleration[:,1]) 142 | accel_min_z_index = np.argmin(acceleration[:,2]) 143 | 144 | # Use worst value among all axes x,y,z 145 | worst_accel_white_noise = np.amax([accel_wn_intercept_x, accel_wn_intercept_y, accel_wn_intercept_z]) 146 | worst_accel_random_walk = np.amax([accel_rr_intercept_x, accel_rr_intercept_y, accel_rr_intercept_z]) 147 | 148 | # Write to yaml file 149 | yaml_file = open(args.output, "w") 150 | yaml_file.write("#Accelerometer\n") 151 | yaml_file.write("accelerometer_noise_density: " + repr(worst_accel_white_noise) + " \n") 152 | yaml_file.write("accelerometer_random_walk: " + repr(worst_accel_random_walk) + " \n") 153 | yaml_file.write("\n") 154 | 155 | print("ACCELEROMETER:") 156 | print(f"X Velocity Random Walk: {accel_wn_intercept_x: .5f} m/s/sqrt(s) {accel_wn_intercept_x*60: .5f} m/s/sqrt(hr)") 157 | print(f"Y Velocity Random Walk: {accel_wn_intercept_y: .5f} m/s/sqrt(s) {accel_wn_intercept_y*60: .5f} m/s/sqrt(hr)") 158 | print(f"Z Velocity Random Walk: {accel_wn_intercept_z: .5f} m/s/sqrt(s) {accel_wn_intercept_z*60: .5f} m/s/sqrt(hr)") 159 | 160 | print(f"X Bias Instability: {accel_min_x: .5f} m/s^2 {accel_min_x*3600*3600: .5f} m/hr^2") 161 | print(f"Y Bias Instability: {accel_min_y: .5f} m/s^2 {accel_min_y*3600*3600: .5f} m/hr^2") 162 | print(f"Z Bias Instability: {accel_min_z: .5f} m/s^2 {accel_min_z*3600*3600: .5f} m/hr^2") 163 | 164 | print(f"X Accel Random Walk: {accel_rr_intercept_x: .5f} m/s^2/sqrt(s)") 165 | print(f"Y Accel Random Walk: {accel_rr_intercept_y: .5f} m/s^2/sqrt(s)") 166 | print(f"Z Accel Random Walk: {accel_rr_intercept_z: .5f} m/s^2/sqrt(s)") 167 | 168 | average_acc_white_noise = (accel_wn_intercept_x + accel_wn_intercept_y + accel_wn_intercept_z) / 3 169 | average_acc_bias_instability = (accel_min_x + accel_min_y + accel_min_z) / 3 170 | average_acc_random_walk = (accel_rr_intercept_x + accel_rr_intercept_y + accel_rr_intercept_z) / 3 171 | 172 | # Plot accelerometer data 173 | dpi = 90 174 | figsize = (16, 9) 175 | fig1 = plt.figure(num="Acceleration", dpi=dpi, figsize=figsize) 176 | 177 | plt.loglog(period, acceleration[:,0], "r--" , label='X') 178 | plt.loglog(period, acceleration[:,1], "g--" , label='Y') 179 | plt.loglog(period, acceleration[:,2], "b--" , label='Z') 180 | 181 | plt.loglog(period, xfit_wn(period), "m-") 182 | plt.loglog(period, yfit_wn(period), "m-") 183 | plt.loglog(period, zfit_wn(period), "m-", label="White noise fit line") 184 | 185 | plt.loglog(period, xfit_rr(period), "y-",) 186 | plt.loglog(period, yfit_rr(period), "y-",) 187 | plt.loglog(period, zfit_rr(period), "y-", label="Random Rate fit line") 188 | 189 | plt.loglog(1.0, accel_wn_intercept_x, "ro", markersize=20) 190 | plt.loglog(1.0, accel_wn_intercept_y, "go", markersize=20) 191 | plt.loglog(1.0, accel_wn_intercept_z, "bo", markersize=20) 192 | 193 | plt.loglog(3.0, accel_rr_intercept_x, "r*", markersize=20) 194 | plt.loglog(3.0, accel_rr_intercept_y, "g*", markersize=20) 195 | plt.loglog(3.0, accel_rr_intercept_z, "b*", markersize=20) 196 | 197 | plt.loglog(period[accel_min_x_index], accel_min_x, "r^", markersize=20) 198 | plt.loglog(period[accel_min_y_index], accel_min_y, "g^", markersize=20) 199 | plt.loglog(period[accel_min_z_index], accel_min_z, "b^", markersize=20) 200 | 201 | fitted_model = generate_prediction(period, q_white=average_acc_white_noise, 202 | q_bias_instability=average_acc_bias_instability, q_walk=average_acc_random_walk) 203 | plt.loglog(period, fitted_model, "-k", label='fitted model') 204 | 205 | plt.title("Accelerometer", fontsize=30) 206 | plt.ylabel("Allan Deviation m/s^2", fontsize=30) 207 | plt.legend(fontsize=25) 208 | plt.grid(True) 209 | plt.xlabel("Period (s)", fontsize=30) 210 | plt.tight_layout() 211 | 212 | plt.draw() 213 | plt.pause(1) 214 | w = plt.waitforbuttonpress(timeout=5) 215 | plt.close() 216 | 217 | fig1.savefig('acceleration.png', dpi=600, bbox_inches = "tight") 218 | 219 | # Calculate intercepts and minimums for gyroscope 220 | gyro_wn_intercept_x, xfit_wn = get_intercept(period[0:white_noise_break_point], rotation_rate[0:white_noise_break_point,0], -0.5, 1.0) 221 | gyro_wn_intercept_y, yfit_wn = get_intercept(period[0:white_noise_break_point], rotation_rate[0:white_noise_break_point,1], -0.5, 1.0) 222 | gyro_wn_intercept_z, zfit_wn = get_intercept(period[0:white_noise_break_point], rotation_rate[0:white_noise_break_point,2], -0.5, 1.0) 223 | 224 | gyro_rr_intercept_x, xfit_rr = get_intercept(period, rotation_rate[:,0], 0.5, 3.0) 225 | gyro_rr_intercept_y, yfit_rr = get_intercept(period, rotation_rate[:,1], 0.5, 3.0) 226 | gyro_rr_intercept_z, zfit_rr = get_intercept(period, rotation_rate[:,2], 0.5, 3.0) 227 | 228 | gyro_min_x = np.amin(rotation_rate[:,0]) 229 | gyro_min_y = np.amin(rotation_rate[:,1]) 230 | gyro_min_z = np.amin(rotation_rate[:,2]) 231 | 232 | gyro_min_x_index = np.argmin(rotation_rate[:,0]) 233 | gyro_min_y_index = np.argmin(rotation_rate[:,1]) 234 | gyro_min_z_index = np.argmin(rotation_rate[:,2]) 235 | 236 | # use worst value 237 | worst_gyro_white_noise = np.amax([gyro_wn_intercept_x, gyro_wn_intercept_y, gyro_wn_intercept_z]) 238 | worst_gyro_random_walk = np.amax([gyro_rr_intercept_x, gyro_rr_intercept_y, gyro_rr_intercept_z]) 239 | 240 | # Write gyroscope parameters to yaml file 241 | yaml_file.write("#Gyroscope\n") 242 | # Convert back to radians here 243 | yaml_file.write("gyroscope_noise_density: " + repr(worst_gyro_white_noise * np.pi / 180) + " \n") 244 | yaml_file.write("gyroscope_random_walk: " + repr(worst_gyro_random_walk * np.pi / 180) + " \n") 245 | yaml_file.write("\n") 246 | 247 | print("GYROSCOPE:") 248 | print(f"X Angle Random Walk: {gyro_wn_intercept_x: .5f} deg/sqrt(s) {gyro_wn_intercept_x * 60: .5f} deg/sqrt(hr)") 249 | print(f"Y Angle Random Walk: {gyro_wn_intercept_y: .5f} deg/sqrt(s) {gyro_wn_intercept_y * 60: .5f} deg/sqrt(hr)") 250 | print(f"Z Angle Random Walk: {gyro_wn_intercept_z: .5f} deg/sqrt(s) {gyro_wn_intercept_z * 60: .5f} deg/sqrt(hr)") 251 | 252 | print(f"X Bias Instability: {gyro_min_x: .5f} deg/s {gyro_min_x*60*60: .5f} deg/hr") 253 | print(f"Y Bias Instability: {gyro_min_y: .5f} deg/s {gyro_min_y*60*60: .5f} deg/hr") 254 | print(f"Z Bias Instability: {gyro_min_z: .5f} deg/s {gyro_min_z*60*60: .5f} deg/hr") 255 | 256 | print(f"X Rate Random Walk: {gyro_rr_intercept_x: .5f} deg/s/sqrt(s)") 257 | print(f"Y Rate Random Walk: {gyro_rr_intercept_y: .5f} deg/s/sqrt(s)") 258 | print(f"Z Rate Random Walk: {gyro_rr_intercept_z: .5f} deg/s/sqrt(s)") 259 | 260 | average_gyro_white_noise = (gyro_wn_intercept_x + gyro_wn_intercept_y + gyro_wn_intercept_z) / 3 261 | average_gyro_bias_instability = (gyro_min_x + gyro_min_y + gyro_min_z) / 3 262 | average_gyro_random_walk = (gyro_rr_intercept_x + gyro_rr_intercept_y + gyro_rr_intercept_z) / 3 263 | 264 | # Plot gyroscope data 265 | fig2 = plt.figure(num="Gyro", dpi=dpi, figsize=figsize) 266 | 267 | plt.loglog(period, rotation_rate[:,0], "r-" , label='X') 268 | plt.loglog(period, rotation_rate[:,1], "g-" , label='Y') 269 | plt.loglog(period, rotation_rate[:,2], "b-" , label='Z') 270 | 271 | plt.loglog(period, xfit_wn(period), "m-") 272 | plt.loglog(period, yfit_wn(period), "m-") 273 | plt.loglog(period, zfit_wn(period), "m-", label="White noise fit line") 274 | 275 | plt.loglog(period, xfit_rr(period), "y-") 276 | plt.loglog(period, yfit_rr(period), "y-") 277 | plt.loglog(period, zfit_rr(period), "y-", label="Random rate fit line") 278 | 279 | plt.loglog(1.0, gyro_wn_intercept_x, "ro", markersize=20) 280 | plt.loglog(1.0, gyro_wn_intercept_y, "go", markersize=20) 281 | plt.loglog(1.0, gyro_wn_intercept_z, "bo", markersize=20) 282 | 283 | plt.loglog(3.0, gyro_rr_intercept_x, "r*", markersize=20) 284 | plt.loglog(3.0, gyro_rr_intercept_y, "g*", markersize=20) 285 | plt.loglog(3.0, gyro_rr_intercept_z, "b*", markersize=20) 286 | 287 | plt.loglog(period[gyro_min_x_index], gyro_min_x, "r^", markersize=20) 288 | plt.loglog(period[gyro_min_y_index], gyro_min_y, "g^", markersize=20) 289 | plt.loglog(period[gyro_min_z_index], gyro_min_z, "b^", markersize=20) 290 | 291 | fitted_model = generate_prediction(period, q_white=average_gyro_white_noise, 292 | q_bias_instability=average_gyro_bias_instability, q_walk=average_gyro_random_walk) 293 | plt.loglog(period, fitted_model, "-k", label='fitted model') 294 | 295 | plt.title("Gyroscope", fontsize=30) 296 | plt.ylabel("Allan Deviation deg/s", fontsize=30) 297 | plt.legend(fontsize=25) 298 | plt.grid(True) 299 | plt.xlabel("Period (s)", fontsize=30) 300 | plt.tight_layout() 301 | 302 | plt.draw() 303 | plt.pause(1) 304 | w = plt.waitforbuttonpress(timeout=5) 305 | plt.close() 306 | 307 | fig2.savefig('gyro.png', dpi=600, bbox_inches = "tight") 308 | 309 | # Write rostopic and update rate to yaml file 310 | if args.config: 311 | yaml_file.write("rostopic: " + repr(rostopic) + " \n") 312 | yaml_file.write("update_rate: " + repr(update_rate) + " \n") 313 | else: 314 | yaml_file.write("rostopic: " + repr(rostopic) + " #Make sure this is correct\n") 315 | yaml_file.write("update_rate: " + repr(update_rate) + " #Make sure this is correct\n") 316 | yaml_file.write("\n") 317 | yaml_file.close() 318 | 319 | print("Writing Kalibr imu.yaml file.") 320 | print("Make sure to update the rostopic and rate in the file if a config file was not provided.") 321 | -------------------------------------------------------------------------------- /scripts/cookbag.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Taken from http://wiki.ros.org/rosbag/Cookbook 3 | 4 | """ 5 | @file cookbag.py 6 | @brief Script to reorganize a ROS bag file by message timestamps. 7 | @author Russell Buchanan 8 | """ 9 | 10 | import rosbag 11 | import argparse 12 | 13 | parser = argparse.ArgumentParser() 14 | 15 | parser.add_argument("--input", type=str, default=None) 16 | parser.add_argument("--output", type=str, default=None) 17 | 18 | args = parser.parse_args() 19 | 20 | with rosbag.Bag(args.output, 'w', compression='lz4') as outbag: 21 | for topic, msg, t in rosbag.Bag(args.input).read_messages(): 22 | # This also replaces tf timestamps under the assumption 23 | # that all transforms in the message share the same timestamp 24 | if topic == "/tf" and msg.transforms: 25 | outbag.write(topic, msg, msg.transforms[0].header.stamp) 26 | else: 27 | outbag.write(topic, msg, msg.header.stamp if msg._has_header else t) 28 | -------------------------------------------------------------------------------- /src/AllanVarianceComputor.cpp: -------------------------------------------------------------------------------- 1 | 2 | /** 3 | * @file AllanVarianceComputor.cpp 4 | * @brief Implementation of the AllanVarianceComputor class. 5 | * @author Russell Buchanan 6 | */ 7 | 8 | #include "allan_variance_ros/AllanVarianceComputor.hpp" 9 | 10 | namespace allan_variance_ros { 11 | 12 | AllanVarianceComputor::AllanVarianceComputor(ros::NodeHandle& nh, std::string config_file, std::string output_path) 13 | : nh_(nh), firstMsg_(true), overlap_(0.0) { 14 | YAML::Node node = loadYamlFile(config_file); 15 | 16 | std::string imu_topic; 17 | 18 | get(node, "imu_topic", imu_topic); 19 | ROS_INFO_STREAM("imu_topic: " << imu_topic); 20 | get(node, "imu_rate", imu_rate_); 21 | ROS_INFO_STREAM("imu_rate: " << imu_rate_); 22 | get(node, "measure_rate", measure_rate_); 23 | ROS_INFO_STREAM("measure_rate: " << measure_rate_); 24 | get(node, "sequence_time", sequence_time_); 25 | ROS_INFO_STREAM("sequence_time: " << sequence_time_); 26 | 27 | input_topics_.push_back(imu_topic); 28 | 29 | imu_skip_ = int(imu_rate_ / measure_rate_); 30 | 31 | imu_output_file_ = output_path + "/" + "allan_variance" + ".csv"; 32 | } 33 | 34 | // write_imu_only assumes batch optimization and that an optimization run had already happened 35 | void AllanVarianceComputor::run(std::string bag_path) { 36 | ROS_INFO_STREAM("Processing " << bag_path << " ..."); 37 | 38 | av_output_ = std::ofstream(imu_output_file_.c_str(), std::ofstream::out); 39 | 40 | int imu_counter = 0; 41 | 42 | try { 43 | rosbag::Bag bag; 44 | bag.open(bag_path, rosbag::bagmode::Read); 45 | rosbag::View view(bag, rosbag::TopicQuery(input_topics_)); 46 | 47 | 48 | // Check to make sure we have data to play 49 | if (view.size() == 0) { 50 | ROS_ERROR_STREAM("Unable to parse any messages..."); 51 | return; 52 | } 53 | ROS_INFO_STREAM("Bag has " << view.size() << " messages, parsing..."); 54 | 55 | // Loop through data 56 | time_t start = clock(); 57 | for (const rosbag::MessageInstance &msg : view) { 58 | // Fill IMU buffer 59 | if (msg.isType()) { 60 | sensor_msgs::ImuConstPtr imu_msg = msg.instantiate(); 61 | tCurrNanoSeconds_ = imu_msg->header.stamp.toNSec(); 62 | 63 | imu_counter++; 64 | 65 | // Subsample IMU measurements 66 | if (imu_counter % imu_skip_ != 0 || imu_counter / imu_rate_ > sequence_time_) { 67 | continue; 68 | } 69 | 70 | if (difftime(clock(), start) / CLOCKS_PER_SEC >= 2.0) { 71 | ROS_INFO_STREAM(imu_counter / imu_rate_ << " / " << sequence_time_ << " seconds loaded"); 72 | start = clock(); 73 | } 74 | 75 | if (firstMsg_) { 76 | firstMsg_ = false; 77 | firstTime_ = tCurrNanoSeconds_; 78 | lastImuTime_ = tCurrNanoSeconds_; 79 | } 80 | 81 | if (tCurrNanoSeconds_ < lastImuTime_) { 82 | skipped_imu_++; 83 | ROS_ERROR_STREAM("IMU out of order. Current(ns): " 84 | << tCurrNanoSeconds_ - firstTime_ << " Last(ns): " 85 | << lastImuTime_ - firstTime_ << " (" << skipped_imu_ << " dropped)"); 86 | continue; 87 | } 88 | lastImuTime_ = tCurrNanoSeconds_; 89 | 90 | ImuMeasurement input; 91 | input.t = imu_msg->header.stamp.toNSec(); 92 | input.I_a_WI = Eigen::Vector3d(imu_msg->linear_acceleration.x, imu_msg->linear_acceleration.y, 93 | imu_msg->linear_acceleration.z); 94 | input.I_w_WI = 95 | Eigen::Vector3d(imu_msg->angular_velocity.x, imu_msg->angular_velocity.y, imu_msg->angular_velocity.z); 96 | 97 | imuBuffer_.push_back(input); 98 | } 99 | if (!nh_.ok()) { 100 | ROS_ERROR_STREAM("Stop requested, closing the bag!"); 101 | bag.close(); 102 | return; 103 | } 104 | } 105 | bag.close(); 106 | 107 | } catch (rosbag::BagIOException &e) { 108 | ROS_WARN_STREAM("Captured rosbag::BagIOException " << e.what()); 109 | } catch (rosbag::BagUnindexedException &e) { 110 | ROS_WARN_STREAM("Captured rosbag::BagUnindexedException " << e.what()); 111 | } catch (std::exception &e) { 112 | ROS_ERROR_STREAM(e.what()); 113 | } catch (...) { 114 | ROS_ERROR("Captured unknown exception"); 115 | } 116 | 117 | ROS_INFO_STREAM("Finished collecting data. " << imuBuffer_.size() << " measurements"); 118 | 119 | // Compute Allan Variance here 120 | if(!imuBuffer_.empty()) { 121 | allanVariance(); 122 | } else { 123 | ROS_ERROR("No IMU messages to process, is your topic right?"); 124 | } 125 | } 126 | 127 | void AllanVarianceComputor::closeOutputs() { av_output_.close(); } 128 | 129 | void AllanVarianceComputor::allanVariance() { 130 | 131 | std::mutex mtx; 132 | bool stop_early = false; 133 | std::map>> averages_map; 134 | 135 | // Range we will sample from (0.1s to 1000s) 136 | int period_min = 1; 137 | int period_max = 10000; 138 | 139 | // Overlapping method 140 | #pragma omp parallel for 141 | for (int period = period_min; period < period_max; period++) { 142 | 143 | if (!nh_.ok() || stop_early) { 144 | stop_early = true; 145 | continue; 146 | } 147 | 148 | std::vector> averages; 149 | double period_time = period * 0.1; // Sampling periods from 0.1s to 1000s 150 | 151 | int max_bin_size = period_time * measure_rate_; 152 | int overlap = floor(max_bin_size * overlap_); 153 | 154 | std::vector current_average = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; 155 | 156 | // Compute Averages 157 | for (int j = 0; j < ((int)imuBuffer_.size() - max_bin_size); j += (max_bin_size - overlap)) { 158 | // get average for current bin 159 | for (int m = 0; m < max_bin_size; m++) { 160 | // ROS_INFO_STREAM("j + m: " << j + m); 161 | // // Acceleration 162 | current_average[0] += imuBuffer_[j + m].I_a_WI[0]; 163 | current_average[1] += imuBuffer_[j + m].I_a_WI[1]; 164 | current_average[2] += imuBuffer_[j + m].I_a_WI[2]; 165 | 166 | // Gyro - assumes measurements in radians and convert to degrees 167 | current_average[3] += imuBuffer_[j + m].I_w_WI[0] * 180 / M_PI; 168 | current_average[4] += imuBuffer_[j + m].I_w_WI[1] * 180 / M_PI; 169 | current_average[5] += imuBuffer_[j + m].I_w_WI[2] * 180 / M_PI; 170 | } 171 | 172 | current_average[0] /= max_bin_size; 173 | current_average[1] /= max_bin_size; 174 | current_average[2] /= max_bin_size; 175 | current_average[3] /= max_bin_size; 176 | current_average[4] /= max_bin_size; 177 | current_average[5] /= max_bin_size; 178 | 179 | averages.push_back(current_average); 180 | current_average = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; 181 | } 182 | 183 | 184 | { 185 | std::lock_guard lck(mtx); 186 | int num_averages = averages.size(); 187 | ROS_INFO_STREAM("Computed " << num_averages << " averages for period " << period_time 188 | << " (" << (10000 - averages_map.size()) << " left)"); 189 | averages_map.insert({period, averages}); 190 | } 191 | } 192 | 193 | if(!nh_.ok() || stop_early) { 194 | ROS_ERROR_STREAM("Stop requested, stopping calculation!"); 195 | return; 196 | } 197 | 198 | 199 | std::vector> allan_variances; 200 | for (int period = period_min; period < period_max; period++) { 201 | 202 | std::vector> averages = averages_map.at(period); 203 | double period_time = period * 0.1; // Sampling periods from 0.1s to 1000s 204 | int num_averages = averages.size(); 205 | ROS_INFO_STREAM("Computed " << num_averages << " bins for sampling period " << period_time << " out of " 206 | << imuBuffer_.size() << " measurements."); 207 | 208 | // Compute Allan Variance 209 | std::vector allan_variance = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; 210 | for (int k = 0; k < num_averages - 1; k++) { 211 | allan_variance[0] += std::pow(averages[k + 1][0] - averages[k][0], 2); 212 | allan_variance[1] += std::pow(averages[k + 1][1] - averages[k][1], 2); 213 | allan_variance[2] += std::pow(averages[k + 1][2] - averages[k][2], 2); 214 | allan_variance[3] += std::pow(averages[k + 1][3] - averages[k][3], 2); 215 | allan_variance[4] += std::pow(averages[k + 1][4] - averages[k][4], 2); 216 | allan_variance[5] += std::pow(averages[k + 1][5] - averages[k][5], 2); 217 | } 218 | std::vector avar = { 219 | allan_variance[0] / (2 * (num_averages - 1)), 220 | allan_variance[1] / (2 * (num_averages - 1)), 221 | allan_variance[2] / (2 * (num_averages - 1)), 222 | allan_variance[3] / (2 * (num_averages - 1)), 223 | allan_variance[4] / (2 * (num_averages - 1)), 224 | allan_variance[5] / (2 * (num_averages - 1))}; 225 | 226 | std::vector allan_deviation = {std::sqrt(avar[0]), std::sqrt(avar[1]), std::sqrt(avar[2]), 227 | std::sqrt(avar[3]), std::sqrt(avar[4]), std::sqrt(avar[5])}; 228 | 229 | writeAllanDeviation(allan_deviation, period_time); 230 | 231 | allan_variances.push_back(avar); 232 | 233 | } 234 | 235 | 236 | } 237 | 238 | void AllanVarianceComputor::writeAllanDeviation(std::vector variance, double period) { 239 | aVRecorder_.period = period; 240 | aVRecorder_.accX = variance[0]; 241 | aVRecorder_.accY = variance[1]; 242 | aVRecorder_.accZ = variance[2]; 243 | aVRecorder_.gyroX = variance[3]; 244 | aVRecorder_.gyroY = variance[4]; 245 | aVRecorder_.gyroZ = variance[5]; 246 | aVRecorder_.writeOnFile(av_output_); 247 | } 248 | 249 | } // namespace allan_variance_ros 250 | -------------------------------------------------------------------------------- /src/ImuMeasurement.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "allan_variance_ros/ImuMeasurement.hpp" 3 | 4 | 5 | ImuMeasurement::ImuMeasurement(): 6 | I_a_WI(0,0,0), 7 | I_w_WI(0,0,0) 8 | { 9 | } 10 | 11 | ImuMeasurement::ImuMeasurement(const uint64_t _t, 12 | const Eigen::Vector3d& _I_a_WI, 13 | const Eigen::Vector3d& _I_w_WI){ 14 | t = _t; 15 | } 16 | 17 | std::ostream& operator<< (std::ostream& stream, const ImuMeasurement& meas) { 18 | stream << "IMU Measurement at time = " << meas.t << " : \n" 19 | << "I_a_WI: " << meas.I_a_WI.transpose() << "\n" 20 | << "I_w_WI: " << meas.I_w_WI.transpose() << "\n"; 21 | return stream; 22 | } 23 | -------------------------------------------------------------------------------- /src/ImuSimulator.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ImuSimulator.cpp 3 | * @brief Tool to simulate imu data, ref: 4 | * https://github.com/ethz-asl/kalibr/wiki/IMU-Noise-Model. 5 | * @author Rick Liu 6 | */ 7 | 8 | // std, eigen and boost 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | // ROS 17 | #include 18 | #include 19 | #include 20 | 21 | #include "allan_variance_ros/yaml_parsers.hpp" 22 | 23 | using Vec3d = Eigen::Vector3d; 24 | 25 | Vec3d RandomNormalDistributionVector(double sigma) { 26 | static boost::mt19937 rng; 27 | static boost::normal_distribution<> nd(0, 1); 28 | return {sigma * nd(rng), sigma * nd(rng), sigma * nd(rng)}; 29 | } 30 | 31 | template void FillROSVector3d(const S &from, T &to) { 32 | to.x = from.x(); 33 | to.y = from.y(); 34 | to.z = from.z(); 35 | } 36 | 37 | class ImuSimulator { 38 | public: 39 | ImuSimulator(std::string config_file, std::string output_path) 40 | : bag_output_(output_path, rosbag::bagmode::Write) { 41 | auto yaml_config = loadYamlFile(config_file); 42 | 43 | get(yaml_config, "accelerometer_noise_density", 44 | accelerometer_noise_density_); 45 | get(yaml_config, "accelerometer_random_walk", accelerometer_random_walk_); 46 | get(yaml_config, "accelerometer_bias_init", accelerometer_bias_init_); 47 | 48 | get(yaml_config, "gyroscope_noise_density", gyroscope_noise_density_); 49 | get(yaml_config, "gyroscope_random_walk", gyroscope_random_walk_); 50 | get(yaml_config, "gyroscope_bias_init", gyroscope_bias_init_); 51 | 52 | get(yaml_config, "rostopic", rostopic_); 53 | ROS_INFO_STREAM("rostopic: " << rostopic_); 54 | get(yaml_config, "update_rate", update_rate_); 55 | ROS_INFO_STREAM("update_rate: " << update_rate_); 56 | get(yaml_config, "sequence_time", sequence_time_); 57 | ROS_INFO_STREAM("sequence_time: " << sequence_time_); 58 | } 59 | 60 | virtual ~ImuSimulator() { bag_output_.close(); } 61 | 62 | void run() { 63 | ROS_INFO_STREAM("Generating IMU data ..."); 64 | 65 | double dt = 1 / update_rate_; 66 | 67 | // clang-format off 68 | ros::Time start_time(1.0); 69 | Vec3d accelerometer_bias = Vec3d::Constant(accelerometer_bias_init_); 70 | Vec3d gyroscope_bias = Vec3d::Constant(gyroscope_bias_init_); 71 | Vec3d accelerometer_real = Vec3d::Zero(); 72 | Vec3d gyroscope_real = Vec3d::Zero(); 73 | 74 | 75 | for (int64_t i = 0; i < sequence_time_ * update_rate_; ++i) { 76 | 77 | // Break if requested by user 78 | if (!ros::ok()) { 79 | break; 80 | } 81 | 82 | // Reference: https://github.com/ethz-asl/kalibr/wiki/IMU-Noise-Model 83 | accelerometer_bias += RandomNormalDistributionVector(accelerometer_random_walk_) * sqrt(dt); 84 | gyroscope_bias += RandomNormalDistributionVector(gyroscope_random_walk_) * sqrt(dt); 85 | 86 | Vec3d acc_measure = accelerometer_real + accelerometer_bias + RandomNormalDistributionVector(accelerometer_noise_density_) / sqrt(dt); 87 | Vec3d gyro_measure = gyroscope_real + gyroscope_bias + RandomNormalDistributionVector(gyroscope_noise_density_) / sqrt(dt); 88 | 89 | sensor_msgs::Imu msg; 90 | msg.header.stamp = start_time + ros::Duration(1, 0) * (i / update_rate_); 91 | msg.header.seq = i; 92 | FillROSVector3d(acc_measure, msg.linear_acceleration); 93 | FillROSVector3d(gyro_measure, msg.angular_velocity); 94 | 95 | bag_output_.write(rostopic_, msg.header.stamp, msg); 96 | } 97 | // clang-format on 98 | 99 | ROS_INFO_STREAM("Finished generating data. "); 100 | } 101 | 102 | private: 103 | // ROS 104 | rosbag::Bag bag_output_; 105 | 106 | private: 107 | double accelerometer_noise_density_; 108 | double accelerometer_random_walk_; 109 | double accelerometer_bias_init_; 110 | 111 | double gyroscope_noise_density_; 112 | double gyroscope_random_walk_; 113 | double gyroscope_bias_init_; 114 | 115 | std::string rostopic_; 116 | double update_rate_; 117 | 118 | double sequence_time_; 119 | }; 120 | 121 | int main(int argc, char **argv) { 122 | ros::init(argc, argv, "allan_variance_ros"); 123 | ros::NodeHandle nh; 124 | std::string rosbag_filename; 125 | std::string config_file; 126 | 127 | if (argc >= 2) { 128 | rosbag_filename = argv[1]; 129 | config_file = argv[2]; 130 | ROS_INFO_STREAM("Bag filename = " << rosbag_filename); 131 | ROS_INFO_STREAM("Config File = " << config_file); 132 | } else { 133 | ROS_WARN("Usage: ./imu_simulator /path/to/output/bag_filename /path/to/simulation/config_filename"); 134 | return 1; 135 | } 136 | 137 | auto start = std::clock(); 138 | 139 | ImuSimulator simulator(config_file, rosbag_filename); 140 | ROS_INFO_STREAM("IMU simulator constructed"); 141 | simulator.run(); 142 | 143 | double durationTime = (std::clock() - start) / (double)CLOCKS_PER_SEC; 144 | ROS_INFO("Total computation time: %f s", durationTime); 145 | return 0; 146 | } 147 | -------------------------------------------------------------------------------- /src/allan_variance.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file allan_variance.cpp 3 | * @brief Tool to compute Allan Variance and Deviation from rosbag. 4 | * @author Russell Buchanan 5 | */ 6 | 7 | // std, eigen and boost 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | // ROS 14 | #include 15 | 16 | // allan_variance_ros 17 | #include "allan_variance_ros/AllanVarianceComputor.hpp" 18 | 19 | int main(int argc, char** argv) { 20 | ros::init(argc, argv, "allan_variance_ros"); 21 | ros::NodeHandle n("~"); 22 | std::string bags_folder = "."; 23 | std::string config_file; 24 | 25 | if (argc >= 2) { 26 | bags_folder = argv[1]; 27 | config_file = argv[2]; 28 | ROS_INFO_STREAM("Bag Folder = " << bags_folder); 29 | ROS_INFO_STREAM("Config File = " << config_file); 30 | } else { 31 | ROS_WARN("Rosbag folder and/or config file not provided!"); 32 | } 33 | 34 | namespace fs = boost::filesystem; 35 | fs::path path = fs::absolute(fs::path(bags_folder)); 36 | 37 | std::set bag_filenames_sorted; 38 | for (const auto& entry : fs::directory_iterator(path)) { 39 | if (entry.path().extension() == ".bag") { 40 | bag_filenames_sorted.insert(entry.path().string()); 41 | } 42 | } 43 | ROS_INFO_STREAM("Bag filenames count: " << bag_filenames_sorted.size()); 44 | 45 | std::clock_t start = std::clock(); 46 | 47 | allan_variance_ros::AllanVarianceComputor computor(n, config_file, bags_folder); 48 | ROS_INFO_STREAM("Batch computor constructed"); 49 | for (const auto& it : bag_filenames_sorted) { 50 | ROS_INFO_STREAM("Processing rosbag " << it); 51 | computor.run(it); 52 | if (!n.ok()) { 53 | break; 54 | } 55 | // For now just do one rosbag 56 | break; 57 | } 58 | 59 | double durationTime = (std::clock() - start) / (double)CLOCKS_PER_SEC; 60 | ROS_INFO("Total computation time: %f s", durationTime); 61 | ROS_INFO("Data written to allan_variance.csv"); 62 | return 0; 63 | } 64 | -------------------------------------------------------------------------------- /src/yaml_parsers.cpp: -------------------------------------------------------------------------------- 1 | #include "allan_variance_ros/yaml_parsers.hpp" 2 | #include 3 | #include 4 | 5 | YAML::Node loadYamlFile(const std::string &filename) { 6 | if (filename.empty()) { throw std::invalid_argument("Filename is empty!"); } 7 | 8 | YAML::Node node; 9 | 10 | try { 11 | node = YAML::LoadFile(filename); 12 | } catch (...) { 13 | throw std::invalid_argument("Error reading config file: " + filename); 14 | } 15 | 16 | if (node.IsNull()) { throw std::invalid_argument("Error reading config file: " + filename); } 17 | 18 | ROS_INFO_STREAM("Successfully read config file: " << filename); 19 | 20 | return node; 21 | } 22 | 23 | --------------------------------------------------------------------------------