/AuvEstimator

Primary LanguagePython

AuvEstimator

Unmanned under water vehicle state estimator using EKF and FGO

*** WORK IN PROGRESS ***

GTSAM

Graph formulation

factorGraph

Sensors

  • DVL
  • barometer
  • IMU

Input measurements into GTSAM

  • Robot body frame velocity (DVL)
  • Navigation frame linear position z (barometer)
  • Robot body frame linear acceleration + angular velocity (IMU)

Output data from GTSAM

  • Navigation frame linear position x, y, z (IMU preintegration - NavState)
  • Navigation frame linear velocity x, y, z (DVL)
  • Navigation frame angular position (IMU preintegration - NavState)
  • Robot body frame acceleration bias (IMU preintegration - NavState)
  • Robot body frame angular velocity bias position (IMU preintegration - NavState)

List of relevant program files:

File Description
processBag.launch File to run gtsam core files on bag data
uuvGtsam.py GTSAM state estimator script.
uuvGtsamRosNode.py Ros wrapper for GTSAM base code (uuvGtsam.py)
plots.py File to generate plots.
errorAnalysis.py Computes RMSE between the EKF, FGO, and dead reckoning against ground truth.
commons.py General helpful functions.
transformations.py File for frame transformations (e.g., quaternion, rotations, euler). Had trouble importing tf2_ros (probably because python 2.7), so generated hardcoded file.
PriorFactorPose3Z.h Barometer custom factor.
PriorFactorVel.h DVL custom factor.

EKF

Position calculation

  1. Dead reckoning where position is computed by performing euler integration on velocity measurements obtained by DVL
  2. Estimator position is computed by performing trapezoidal integration method on velocity measurements obtained by DVL and IMU

Tranforming between frames (robot body to map frame) using Fixed xyz rotation

FrameTransformation

EKF detail

6 DoF loosely coupled EKF

EKF

15 state

   x - map
   y - map
   z - map
   roll - map
   pitch - map
   yaw - map
   x velocity (u) - map
   y velocity (v) - map
   z velocity (w) - map
   x accel bias - robot
   y accel bias - robot
   z accel bias - robot
   x gyro bias - robot
   y gyro bias - robot
   z gyro bias - robot

Predict step

xPredictStep pPredictStep

List of relevant program files:

File Description
ekfTest.launch File to launch the SensorNode.py, deadReckoning.py, and computeError.py. This file also contains ros parameters that are used in the python files.
uuvEkfEstimator.py Script that setup and performs the state estimation of the vehicle with EKF using DVL, IMU, and barometer.
uuvEkfRosNode.py Ros wrapper for EKF base code (uuvEkfEstimator.py)
deadReckon.py Performs dead reckoning with velocity data from DVL and orientation data from IMU.
deadReckonRosNode.py Ros wrapper for EKF base code (deadReckon.py)
ekf.py Script that contains the math behind EKF.
computeError.py Computes the x-error, y-error, z-error, and distance between the estimator and dead reckoning against ground truth in real time.

System Requirements

  • ROS kinetic or melodic:
  • Ubuntu 18.04 LTS:
  • Python 2.7 environment

Relevant Packages

Required: Ubuntu 16.04 with ROS Kinetic (http://wiki.ros.org/kinetic). Also, the following packages are required:

Launch Procedures (need update)

  1. Install all relevant packages stated above