State estimation

[Jared-Chan]

Create a state estimator for Triton that publishes the position, orientation, and linear velocity of the AUV.

The state estimator would be subscribed to the IMU topic, which provides linear acceleration and angular velocity. The various detectors (e.g. gate detector) may give information on position relative to objects of known position.

From sound localization, we may get a position estimation as well as a depth reading. (Talk to the sound localization team)

While we could just integrate acceleration to get velocity and displacement, we would have more and more inaccurate results and we would not be taking advantage of measurements from multiple sensors.

Tasks

• [x] Investigate state estimators. (Suggestion: the Extended Kalman Filter)
• [x] Look into our sensors and how to best utilize them. Also look into how to use the output of various object detectors to aid state estimation
• [ ] Communicate with the sound localization team on what position information they can give us, and how to incorporate it into the state estimator
• [x] Create a new node in triton_controls and implements the state estimator
• [x] Test the state estimator in Gazebo and compare the state produced to triton/state, which is produced by a Gazebo plugin.

[Jared-Chan]

• Using the Unscented Kalman Filter provided by the robot_localization package
• Fusing IMU data with Pose data (x, y, and z coordinates), could add another source of positional data deduced from camera images
• Created a launch file and config file
• Tested with Gazebo and RVIZ. It works under ideal situations