C++/ROS 2 Navigation Filter

Author:

• Maximilian Nitsch [email protected]

Affiliation: Institute of Automatic Control - RWTH Aachen University

Maintainer:

• Maximilian Nitsch [email protected]

Description

This project provides a loosely-coupled navigation filter written in C++ for the state estimation of the TRIPLE-nanoAUV.

The navigation filter implements the following features:

• State-of-the-art Error-State Kalman Filter (ES-EKF) and Unscented Kalman Filter (UKF)
• Prediction with strapdown inertial navigation system (SINS) model by integrating measurement of an inertial measurement unit (IMU)
• Aiding with further navigation sensors:
  • Ultra-Short Baseline (USBL) system (position measurement)
  • Doppler-Velocity Log (DVL) (velocity measurement)
  • Depth Pressure Sensor (DPTH) (z-position measurement)
  • Magnetometer (MAG) (attitude measurement)
  • GNSS PVT (geodetic position, NED velocity, time measurement)
  • GNSS compass (attitude measurement)
• The filter can be easily extended for further sensors
• All parameters can be configured in a YAML file
• All sensor updates can be enabled/disabled separately

Table of Contents

• Dependencies
• Installation
• Usage
• ROS 2 Node
  • Publishers
  • Subscribers
• Coding Guidelines
• References
• Reports
• Contributing
• License

Dependencies

This project depends on the following literature and libraries:

• Eigen3: Eigen is a C++ template library for linear algebra: Eigen website.
• ROS 2 Humble: ROS 2 is a set of software libraries and tools for building robot applications: ROS 2 Installation page).
• Navigation-Interfaces: Custom ROS 2 interfaces repository: Navigation-Interfaces.
• Navigation-Utilities: Navigation utilities repository: Navigation-Utilities.

Installation

To install the navigation_filter_package, you need to follow these steps:

1. Install Eigen3: Eigen3 is a dependency for your package. You can install it using your package manager. For example, on Ubuntu, you can install it using the following command:

   sudo apt-get install libeigen3-dev

2. Install ROS 2 Humble: Make sure you have ROS 2 (Humble) installed. You can follow the official installation instructions provided by ROS 2. Visit ROS 2 Humble Installation page for detailed installation instructions tailored to your platform.

3. Clone the Package: Clone the package repository to your ROS 2 workspace. If you don't have a ROS 2 workspace yet, you can create one using the following commands:

   mkdir -p /path/to/ros2_workspace/src
   cd /path/to/ros2_workspace/src

   Now, clone the package repository:

   git clone <repository_url>

   Replace <repository_url> with the URL of your package repository.

4. Build the Package: Once the package is cloned, you need to build it using colcon, the default build system for ROS 2. Navigate to your ROS 2 workspace and run the following command:

   cd /path/to/ros2_workspace
   colcon build

   This command will build all the packages in your workspace, including the newly added package.

5. Source the Workspace: After building the package, you need to source your ROS 2 workspace to make the package available in your ROS 2 environment. Run the following command:

   source /path/to/ros2_workspace/install/setup.bash

   Replace /path/to/ros2_workspace with the actual path to your ROS 2 workspace.

That's it! Your navigation_filter_package should now be installed along with its dependencies and ready to use in your ROS 2 environment.

Usage

1. Configure a YAML file for your navigation filter or use the default file.

2. Start the navigation sensor publishers for your vehicle simulation. You can launch all navigation sensors using:

   ros2 launch sensors_bringup_package sensor_simulators.launch.py

3. Start the navigation filter with the launch file:

   ros2 launch navigation_filter_package navigation_filter.launch.py 

The navigation filter prints your settings and starts to estimate the state by fusing the published sensor data.

4. The odometry message with the estimated states and associated covariances should now be published.

ROS 2 Node

The navigation filter node implements one publisher and subscribes to five topics. Furthermore, one action is provided. Services are not provided.

Publishers

This node publishes the following topics:

Topic Name	Message Type	Description
/navigation/odometry	nav_msgs/Odometry.msg	Publishes odometry message (estimated state with covariance).

Subscribers

This node subscribes to the following topics:

Topic Name	Message Type	Description
/imu/inertial_data	nanoauv_sensor_driver_interfaces/msg/inertial_measurement_unit.msg	Subscribes to custom IMU message.
/usbl/usbllong	nanoauv_sensor_driver_interfaces/msg/usbl_long.msg	Subscribes custom USBL message (USBLLONG).
/dvl/velocity_and_transducer	nanoauv_sensor_driver_interfaces/msg/dvl_velocity.msg	Subscribes custom DVL message.
/depth/depth_pressure	nanoauv_sensor_driver_interfaces/msg/depth_pressure.msg	Subscribes to custom DPTH message.
/magnetometer/magnetic_field	nanoauv_sensor_driver_interfaces/msg/magnetometer.msg	Subscribes to custom MAG message.
/gnss/pvt	nanoauv_sensor_driver_interfaces/msg/gnss_pvt.msg	Subscribes to custom GNSS PVT message.
/gnss/attitude	nanoauv_sensor_driver_interfaces/msg/gnss_attitude.msg	Subscribes to custom GNSS attitude message.

It is recommend to subscribe at least to the IMU + DVL + DPTH or IMU + GNSS PVT (geodetic) measurements to ensure that the navigation filter converges.

Actions

This node provides the following actions:

Action Name	Message Type	Description
/navigation/initialize_navigation_filter	nanoauv_navigation_interfaces/action/navigation_filter_initialization.action	Activates the dynamic initialization of the navigation filter for a specified time.

Coding Guidelines

This project follows these coding guidelines:

• https://google.github.io/styleguide/cppguide.html
• http://docs.ros.org/en/humble/The-ROS2-Project/Contributing/Code-Style-Language-Versions.html

References

The navigation filter implementation closely follows the work:

• M. Nitsch, "Navigation of a miniaturized autonomous underwater vehicle exploring waters under ice," Dissertation, Rheinisch-Westfälische Technische Hochschule Aachen, Aachen, RWTH Aachen University, 2024. DOI: 10.18154/RWTH-2024-05964.

Contributing

If you would like to contribute to the project, see the CONTRIBUTING file for details.

License

This project is licensed under the BSD-3-Clause License. See the LICENSE file for details.