Error-state Kalman Filter implementation #596
Conversation
…ses msg imu/data_raw and /orca/pose as the dvl info
for more information, see https://pre-commit.ci
for more information, see https://pre-commit.ci
for more information, see https://pre-commit.ci
idk Anders told me to
for more information, see https://pre-commit.ci
| imu_topic: /imu/data_raw | ||
| dvl_topic: /orca/twist | ||
| odom_topic: odom |
There was a problem hiding this comment.
These can go to orca.yaml in auv_setup
There was a problem hiding this comment.
newdrone.yaml 👀 name not decided yet
There was a problem hiding this comment.
Let this be a thing we do semi last
|
|
||
| struct state_quat { |
There was a problem hiding this comment.
Use same style as for classes, i.e.
struct StateQuat| std_msgs::msg::Float64 nis_msg; | ||
| nis_msg.data = eskf_->NIS_; | ||
| nis_pub_->publish(nis_msg); |
There was a problem hiding this comment.
If this value is not used/always needed, could it be put under a debug flag?
Codecov Report❌ Patch coverage is Additional details and impacted files@@ Coverage Diff @@
## main #596 +/- ##
==========================================
- Coverage 18.77% 16.92% -1.85%
==========================================
Files 38 42 +4
Lines 2376 2635 +259
Branches 426 428 +2
==========================================
Hits 446 446
- Misses 1815 2074 +259
Partials 115 115
Flags with carried forward coverage won't be shown. Click here to find out more.
🚀 New features to boost your workflow:
|
There was a problem hiding this comment.
Pull Request Overview
This PR implements an Error-state Kalman Filter (ESKF) for navigation, adding a complete ROS 2 node implementation with support for IMU and DVL sensor fusion. The implementation provides odometry estimation using quaternion-based state representation for the nominal state and Euler angles for the error state.
- Implements core ESKF algorithm with IMU prediction and DVL measurement updates
- Adds ROS 2 integration with configurable parameters and topic subscriptions
- Includes automated testing infrastructure for the new ESKF node
Reviewed Changes
Copilot reviewed 14 out of 14 changed files in this pull request and generated 5 comments.
Show a summary per file
| File | Description |
|---|---|
| navigation/eskf/src/eskf.cpp | Core ESKF algorithm implementation with state prediction and measurement updates |
| navigation/eskf/src/eskf_ros.cpp | ROS 2 node wrapper providing sensor callbacks and odometry publishing |
| navigation/eskf/src/eskf_node.cpp | Main entry point for the ESKF ROS node |
| navigation/eskf/src/eskf_utils.cpp | Utility functions for quaternion operations and mathematical helpers |
| navigation/eskf/include/eskf/*.hpp | Header files defining classes, typedefs, and function declarations |
| navigation/eskf/config/eskf_params.yaml | Configuration parameters for noise models and sensor topics |
| navigation/eskf/launch/eskf.launch.py | Launch file for starting the ESKF node with parameters |
| navigation/eskf/package.xml | ROS package definition with dependencies |
| navigation/eskf/CMakeLists.txt | Build configuration for the ESKF package |
| tests/ros_node_tests/eskf_node_test.sh | Integration test script for ESKF node functionality |
| .github/workflows/ros-node-tests.yml | CI workflow update to include ESKF node testing |
Tip: Customize your code reviews with copilot-instructions.md. Create the file or learn how to get started.
| imu_topic: /imu/data_raw | ||
| dvl_topic: /orca/twist | ||
| odom_topic: odom |
There was a problem hiding this comment.
newdrone.yaml 👀 name not decided yet
| // @brief Calculate the NIS | ||
| // @param innovation: Innovation vector | ||
| // @param S: Innovation covariance matrix | ||
| void NIS(const Eigen::Vector3d& innovation, const Eigen::Matrix3d& S); |
There was a problem hiding this comment.
I suggest thinking about visualization from the start. Don't know if you want to implement it seperately or inside this library e.g calculate ANIS function, print covariance elipse function ... , but you should not neglect it. Would be nice to have some nice visuals on propagation of error state and maybe when / how covariances blow up.
"I've probably spent 80% of my time on visualization and understanding and 20% of my time doing math and programming" -Edmund
Probably helps with debugging later if you have put thought into testing from the start. But then again i am no expert on this (yet, give me 4 weeks ¯_(ツ)_/¯ )
There was a problem hiding this comment.
I think we need to look a this later on, but yes a good way to plot would be nice. The issue though is that i would like to have a plot of the estimates with there uncertainty, NEES and NIS given that we have ground truth. This will make it easier for you to test. The issue is that NIS upper and lower bounds depends on the amount of measurements, which can become a problem ( i feel so ) if we have different dimensions and different measurements.
|
@Talhanc any reason for making the ros-eskf class be so very stateful? I'd expect that the non-ros eskf class maintained most of the state, only exposing things through getters (leaving the ros-layer mostly for conversions etc) |
4 participants
Purpose
This branch is created as a clean starting point for the ESKF implementation from earlier this year.
The goal is to complete the ESKF and potentially improve its performance.
Objectives