Classes | Public Member Functions | Private Types | Private Member Functions | Private Attributes
labust::navigation::Estimator3D Class Reference

#include <EKF3D.hpp>

List of all members.

Classes

struct  FilterState

Public Member Functions

 Estimator3D ()
 Estimator3D ()
 Estimator3D ()
void onInit ()
void onInit ()
void onInit ()
void start ()
void start ()
void start ()

Private Types

enum  {
  X = 0, Y, Z, K,
  M, N, DoF
}
enum  {
  X = 0, Y, Z, K,
  M, N, DoF
}
enum  {
  X = 0, Y, Z, K,
  M, N, DoF
}
typedef
labust::navigation::KFCore
< labust::navigation::LDTravModel
KFNav
typedef
labust::navigation::KFCore
< labust::navigation::RelativeTrackingModel
KFNav
typedef
labust::navigation::KFCore
< labust::navigation::EKF_3D_USBLModel
KFNav

Private Member Functions

int calculateDelaySteps (double measTime, double arrivalTime)
void configureNav (KFNav &nav, ros::NodeHandle &nh)
void configureNav (KFNav &nav, ros::NodeHandle &nh)
void configureNav (KFNav &nav, ros::NodeHandle &nh)
void deltaPosCallback (const auv_msgs::NED::ConstPtr &msg)
void deltaPosCallback (const auv_msgs::NED::ConstPtr &msg)
void KFmodeCallback (const std_msgs::Bool::ConstPtr &msg)
void KFmodeCallback (const std_msgs::Bool::ConstPtr &msg)
void onAltitude (const std_msgs::Float32::ConstPtr &data)
void onAltitude (const std_msgs::Float32::ConstPtr &data)
void onCurrentsHat (const geometry_msgs::TwistStamped::ConstPtr &data)
void onDepth (const std_msgs::Float32::ConstPtr &data)
void onDepth (const std_msgs::Float32::ConstPtr &data)
void onModelUpdate (const navcon_msgs::ModelParamsUpdate::ConstPtr &update)
void onModelUpdate (const navcon_msgs::ModelParamsUpdate::ConstPtr &update)
void onModelUpdate (const navcon_msgs::ModelParamsUpdate::ConstPtr &update)
void onReset (const std_msgs::Bool::ConstPtr &reset)
void onReset (const std_msgs::Bool::ConstPtr &reset)
void onStateHat (const auv_msgs::NavSts::ConstPtr &data)
void onTargetDepth (const std_msgs::Float32::ConstPtr &data)
void onTargetHeading (const std_msgs::Float32::ConstPtr &data)
void onTargetTau (const auv_msgs::BodyForceReq::ConstPtr &tau)
void onTau (const auv_msgs::BodyForceReq::ConstPtr &tau)
void onTau (const auv_msgs::BodyForceReq::ConstPtr &tau)
void onUSBLfix (const underwater_msgs::USBLFix::ConstPtr &data)
void onUSBLfix (const underwater_msgs::USBLFix::ConstPtr &data)
void onUseGyro (const std_msgs::Bool::ConstPtr &use_gyro)
void onUseGyro (const std_msgs::Bool::ConstPtr &use_gyro)
void processMeasurements ()
void processMeasurements ()
void processMeasurements ()
void publishState ()
void publishState ()
void publishState ()

Private Attributes

bool absoluteEKF
double alt
ros::Subscriber altitude
tf2_ros::TransformBroadcaster broadcaster
ros::Publisher buoyancyHat
double compassVariance
ros::Publisher currentsHat
double currentTime
double delay_time
double delayTime
float deltaXpos
float deltaYpos
ros::Subscriber depth
DvlHandler dvl
int dvl_model
ros::Time dvl_time
 Last DVL measurement received time.
double dvl_timeout
 Timeout value for the DVL.
bool enableBearing
bool enableDelay
bool enableElevation
bool enableRange
GPSHandler gps
double gyroVariance
ImuHandler imu
KFNav::vector inputVec
bool KFmode
bool KFmodePast
double max_dvl
 Maximum dvl speed for sanity checks.
boost::mutex meas_mux
 Mutex for data protection.
KFNav::vector measDelay
KFNav::vector measurements
double min_altitude
 Minimum safe altitude that is accepted as measurement.
ros::Subscriber modelUpdate
KFNav nav
KFNav::vector newMeas
labust::tools::OutlierRejection OR
KFNav::ModelParams params [DoF]
std::deque< FilterStatepastStates
KFNav::matrix Pstart
ros::Publisher pubRange
ros::Publisher pubRangeFiltered
ros::Publisher pubStateHat
ros::Publisher pubStateMeas
ros::Publisher pubwk
bool quadMeasAvailable
ros::Subscriber resetTopic
KFNav::matrix Rstart
ros::Publisher stateHat
ros::Publisher stateMeas
ros::Subscriber sub
ros::Subscriber subCurrentsHat
ros::Subscriber subKFmode
ros::Subscriber subStateHat
ros::Subscriber subTargetDepth
ros::Subscriber subTargetHeading
ros::Subscriber subTargetTau
ros::Subscriber subUSBL
ros::Subscriber subUSBLfix
ros::Subscriber tauAch
KFNav::vector tauIn
ros::Publisher turns_pub
 Turn count publisher.
double u
ros::Publisher unsafe_dvl
labust::math::unwrap unwrap
ros::Subscriber useGyro
bool useYawRate
double v
double xc
double yc

Detailed Description

The 3D state estimator for ROVs and AUVs. Extended with altitude and pitch estimates.

Todo:
Extract the lat/lon part into the llnode.

The 3D state estimator for ROVs and AUVs. Extended with altitude and pitch estimates.

Todo:
Extract the lat/lon part into the llnode.

The 3D state estimator for ROVs and AUVs. Extended with altitude and pitch estimates.

Todo:
Extract the lat/lon part into the llnode.

Definition at line 68 of file EKF3D.hpp.


Member Typedef Documentation

Definition at line 71 of file EKF3D.hpp.

Definition at line 72 of file EKF_RTT.hpp.

Definition at line 79 of file EKF_3D_USBL.hpp.


Member Enumeration Documentation

anonymous enum [private]
Enumerator:
X 
Y 
Z 
K 
M 
N 
DoF 

Definition at line 70 of file EKF3D.hpp.

anonymous enum [private]
Enumerator:
X 
Y 
Z 
K 
M 
N 
DoF 

Definition at line 78 of file EKF_3D_USBL.hpp.

anonymous enum [private]
Enumerator:
X 
Y 
Z 
K 
M 
N 
DoF 

Definition at line 71 of file EKF_RTT.hpp.


Constructor & Destructor Documentation

Main constructor.

Definition at line 61 of file EKF3D.cpp.

Main constructor.

Main constructor.


Member Function Documentation

int Estimator3D::calculateDelaySteps ( double  measTime,
double  arrivalTime 
) [private]

Calculate measurement delay in time steps

Definition at line 590 of file EKF_3D_USBL.cpp.

void Estimator3D::configureNav ( KFNav nav,
ros::NodeHandle nh 
) [private]

Helper function for navigation configuration.

Definition at line 152 of file EKF3D.cpp.

Helper function for navigation configuration.

Helper function for navigation configuration.

void Estimator3D::deltaPosCallback ( const auv_msgs::NED::ConstPtr &  msg) [private]

Callbacks for relative/absolute mode switching

Definition at line 251 of file EKF3D.cpp.

void labust::navigation::Estimator3D::deltaPosCallback ( const auv_msgs::NED::ConstPtr &  msg) [private]

Callbacks for relative/absolute mode switching

void Estimator3D::KFmodeCallback ( const std_msgs::Bool::ConstPtr &  msg) [private]

Definition at line 259 of file EKF3D.cpp.

void labust::navigation::Estimator3D::KFmodeCallback ( const std_msgs::Bool::ConstPtr &  msg) [private]
void Estimator3D::onAltitude ( const std_msgs::Float32::ConstPtr &  data) [private]

Handle the altitude measurement.

Definition at line 230 of file EKF3D.cpp.

void labust::navigation::Estimator3D::onAltitude ( const std_msgs::Float32::ConstPtr &  data) [private]

Handle the altitude measurement.

void Estimator3D::onCurrentsHat ( const geometry_msgs::TwistStamped::ConstPtr &  data) [private]

Handle target input forces and torques.

Definition at line 159 of file EKF_RTT.cpp.

void Estimator3D::onDepth ( const std_msgs::Float32::ConstPtr &  data) [private]

Handle the depth measurement.

Definition at line 222 of file EKF3D.cpp.

void labust::navigation::Estimator3D::onDepth ( const std_msgs::Float32::ConstPtr &  data) [private]

Handle the depth measurement.

Initialize the estimation filter.

Definition at line 76 of file EKF3D.cpp.

Initialize the estimation filter.

Initialize the estimation filter.

void labust::navigation::Estimator3D::onModelUpdate ( const navcon_msgs::ModelParamsUpdate::ConstPtr &  update) [private]

On model updates.

void Estimator3D::onModelUpdate ( const navcon_msgs::ModelParamsUpdate::ConstPtr &  update) [private]

On model updates.

Definition at line 193 of file EKF3D.cpp.

void labust::navigation::Estimator3D::onModelUpdate ( const navcon_msgs::ModelParamsUpdate::ConstPtr &  update) [private]

On model updates.

void Estimator3D::onReset ( const std_msgs::Bool::ConstPtr &  reset) [private]

Handle the state reset.

Definition at line 130 of file EKF3D.cpp.

void labust::navigation::Estimator3D::onReset ( const std_msgs::Bool::ConstPtr &  reset) [private]

Handle the state reset.

void Estimator3D::onStateHat ( const auv_msgs::NavSts::ConstPtr &  data) [private]

Handle target input forces and torques.

Definition at line 151 of file EKF_RTT.cpp.

void Estimator3D::onTargetDepth ( const std_msgs::Float32::ConstPtr &  data) [private]

Handle the depth measurement.

Definition at line 172 of file EKF_RTT.cpp.

void Estimator3D::onTargetHeading ( const std_msgs::Float32::ConstPtr &  data) [private]

Handle the depth measurement.

Definition at line 178 of file EKF_RTT.cpp.

void Estimator3D::onTargetTau ( const auv_msgs::BodyForceReq::ConstPtr &  tau) [private]

Handle target input forces and torques.

Definition at line 165 of file EKF_RTT.cpp.

void Estimator3D::onTau ( const auv_msgs::BodyForceReq::ConstPtr &  tau) [private]

Handle input forces and torques.

Definition at line 212 of file EKF3D.cpp.

void labust::navigation::Estimator3D::onTau ( const auv_msgs::BodyForceReq::ConstPtr &  tau) [private]

Handle input forces and torques.

void labust::navigation::Estimator3D::onUSBLfix ( const underwater_msgs::USBLFix::ConstPtr &  data) [private]

Handle the USBL measurement.

void Estimator3D::onUSBLfix ( const underwater_msgs::USBLFix::ConstPtr &  data) [private]

Handle the USBL measurement.

Definition at line 275 of file EKF_3D_USBL.cpp.

void Estimator3D::onUseGyro ( const std_msgs::Bool::ConstPtr &  use_gyro) [private]

Handle the gyro/compass switch.

Definition at line 138 of file EKF3D.cpp.

void labust::navigation::Estimator3D::onUseGyro ( const std_msgs::Bool::ConstPtr &  use_gyro) [private]

Handle the gyro/compass switch.

Helper method to process measurements.

Definition at line 294 of file EKF3D.cpp.

Handle the range measurement. Handle the bearing measurement. Helper method to process measurements.

Helper method to process measurements.

void Estimator3D::publishState ( ) [private]

Helper method to publish the navigation state.

Definition at line 427 of file EKF3D.cpp.

Helper method to publish the navigation state.

Helper method to publish the navigation state.

Start the estimation loop.

Start the estimation loop.

Outlier test

Definition at line 496 of file EKF3D.cpp.

Start the estimation loop.


Member Data Documentation

Definition at line 208 of file EKF3D.hpp.

Temporary altitude storage.

Temporary altitude, NED currents storage.

Definition at line 178 of file EKF3D.hpp.

Definition at line 150 of file EKF3D.hpp.

The transform broadcaster.

Definition at line 174 of file EKF3D.hpp.

Definition at line 144 of file EKF3D.hpp.

The compass and gyro variance.

Definition at line 194 of file EKF3D.hpp.

Definition at line 144 of file EKF3D.hpp.

Current time in seconds

Definition at line 223 of file EKF_3D_USBL.hpp.

Fixed time delay for USBL navigation

Definition at line 228 of file EKF_3D_USBL.hpp.

Definition at line 224 of file EKF_3D_USBL.hpp.

Definition at line 210 of file EKF3D.hpp.

Definition at line 210 of file EKF3D.hpp.

Definition at line 150 of file EKF3D.hpp.

The DVL handler.

Definition at line 162 of file EKF3D.hpp.

The DVL model selector.

Definition at line 190 of file EKF3D.hpp.

Last DVL measurement received time.

Definition at line 168 of file EKF3D.hpp.

Timeout value for the DVL.

Definition at line 170 of file EKF3D.hpp.

Definition at line 232 of file EKF_3D_USBL.hpp.

USBL measurements enable flags

Definition at line 232 of file EKF_3D_USBL.hpp.

Definition at line 232 of file EKF_3D_USBL.hpp.

Definition at line 232 of file EKF_3D_USBL.hpp.

The GPS handler.

Definition at line 154 of file EKF3D.hpp.

Definition at line 194 of file EKF3D.hpp.

The Imu handler.

Definition at line 158 of file EKF3D.hpp.

The input vector.

Definition at line 143 of file EKF_RTT.hpp.

Definition at line 208 of file EKF3D.hpp.

Definition at line 208 of file EKF3D.hpp.

Maximum dvl speed for sanity checks.

Definition at line 164 of file EKF3D.hpp.

Mutex for data protection.

Definition at line 196 of file EKF3D.hpp.

Definition at line 170 of file EKF_3D_USBL.hpp.

The measurements vector and arrived flag vector.

Definition at line 136 of file EKF3D.hpp.

Minimum safe altitude that is accepted as measurement.

Definition at line 166 of file EKF3D.hpp.

Definition at line 150 of file EKF3D.hpp.

The navigation filter.

Definition at line 128 of file EKF3D.hpp.

Definition at line 136 of file EKF3D.hpp.

Definition at line 252 of file EKF_3D_USBL.hpp.

Model parameters

Definition at line 182 of file EKF3D.hpp.

Definition at line 250 of file EKF_3D_USBL.hpp.

Definition at line 212 of file EKF3D.hpp.

Definition at line 178 of file EKF_3D_USBL.hpp.

Definition at line 178 of file EKF_3D_USBL.hpp.

Definition at line 155 of file EKF_RTT.hpp.

Estimated and measured state publisher.

Definition at line 155 of file EKF_RTT.hpp.

Definition at line 178 of file EKF_3D_USBL.hpp.

Variables for relative/absolute mode switching

Definition at line 208 of file EKF3D.hpp.

Definition at line 150 of file EKF3D.hpp.

Definition at line 212 of file EKF3D.hpp.

Definition at line 144 of file EKF3D.hpp.

Estimated and measured state publisher.

Definition at line 144 of file EKF3D.hpp.

Definition at line 150 of file EKF3D.hpp.

Definition at line 159 of file EKF_RTT.hpp.

Definition at line 150 of file EKF3D.hpp.

Sensors and input subscribers.

Definition at line 159 of file EKF_RTT.hpp.

Definition at line 159 of file EKF_RTT.hpp.

Definition at line 159 of file EKF_RTT.hpp.

Definition at line 159 of file EKF_RTT.hpp.

Definition at line 182 of file EKF_3D_USBL.hpp.

Definition at line 159 of file EKF_RTT.hpp.

Sensors and input subscribers.

Definition at line 150 of file EKF3D.hpp.

The input vector.

Definition at line 132 of file EKF3D.hpp.

Turn count publisher.

Definition at line 146 of file EKF3D.hpp.

Definition at line 167 of file EKF_RTT.hpp.

Definition at line 144 of file EKF3D.hpp.

Heading unwrapper.

Definition at line 140 of file EKF3D.hpp.

Definition at line 150 of file EKF3D.hpp.

The flag to indicate existing yaw-rate measurements.

Definition at line 186 of file EKF3D.hpp.

Definition at line 167 of file EKF_RTT.hpp.

Definition at line 167 of file EKF_RTT.hpp.

Definition at line 167 of file EKF_RTT.hpp.


The documentation for this class was generated from the following files:


labust_navigation
Author(s): Gyula Nagy
autogenerated on Fri Aug 28 2015 11:23:33