[TASK] Implement ESKF for sensor fusion of DVL and IMU

[Andeshog]

Description of task

To get better pose and velocity estimates, we can fuse the DVL measurements and the IMU measurements. Structure of the DVL measurements not yet decided, but the IMU measurements are given in an IMU message.

The IMU can be tested in the office by:

• Finding the IMU and plugging it into your computer.
• Make a new workspace, clone this repo and build:
  • mkdir -p stim300_ws/src ; cd stim300_ws/src
  • git clone [email protected]:vortexntnu/stim300-driver.git -b feature/ros2-port
  • cd .. ; colcon build ; source install/setup.bash
• Launch the driver:
  • ros2 launch driver_stim300 stim300_driver.launch.py

Suggested Workflow

• Define the state vector to be estimated. Must include 6 DOF pose (NED) and velocity (body).
• Run and read the output from the IMU by doing the steps in the description.
• Look at this template for inspiration/starting point.

Specifications

• Output message type: Odometry
• Output topic name: TBD

Contacts

• @Andeshog
• @chrstrom

Code Quality

• Every function in header files are documented (inputs/returns/exceptions)
• The project has automated tests that cover MOST of the functions and branches in functions (pytest/gtest)
• The code is documented on the wiki (provide link)

[Talhanc]

We are thinking of implementing the error state kalman filter (ESKF). We will leave this for now, and try to learn c++ more, and learning the ESKF from the sensorfusjon course, and then trying to implement it in late january/february. Hopefully until then we have more info and structured how we want to program it,