#include <EKF3D.hpp>

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< FilterState >	pastStates
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

typedef labust::navigation::KFCore<labust::navigation::LDTravModel> labust::navigation::Estimator3D::KFNav [private]

Definition at line 71 of file EKF3D.hpp.

typedef labust::navigation::KFCore<labust::navigation::RelativeTrackingModel> labust::navigation::Estimator3D::KFNav [private]

Definition at line 72 of file EKF_RTT.hpp.

typedef labust::navigation::KFCore<labust::navigation::EKF_3D_USBLModel> labust::navigation::Estimator3D::KFNav [private]

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

Estimator3D::Estimator3D ( )

Main constructor.

Definition at line 61 of file EKF3D.cpp.

labust::navigation::Estimator3D::Estimator3D ( )

Main constructor.

labust::navigation::Estimator3D::Estimator3D ( )

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.

void labust::navigation::Estimator3D::configureNav	(	KFNav &	nav,
		ros::NodeHandle &	nh
	)		`[private]`

Helper function for navigation configuration.

void labust::navigation::Estimator3D::configureNav	(	KFNav &	nav,
		ros::NodeHandle &	nh
	)		`[private]`

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.

void Estimator3D::onInit ( )

Initialize the estimation filter.

Definition at line 76 of file EKF3D.cpp.

void labust::navigation::Estimator3D::onInit ( )

Initialize the estimation filter.

void labust::navigation::Estimator3D::onInit ( )

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.

void Estimator3D::processMeasurements ( ) [private]

Helper method to process measurements.

Definition at line 294 of file EKF3D.cpp.

void labust::navigation::Estimator3D::processMeasurements ( ) [private]

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

void labust::navigation::Estimator3D::processMeasurements ( ) [private]

Helper method to process measurements.

void Estimator3D::publishState ( ) [private]

Helper method to publish the navigation state.

Definition at line 427 of file EKF3D.cpp.

void labust::navigation::Estimator3D::publishState ( ) [private]

Helper method to publish the navigation state.

void labust::navigation::Estimator3D::publishState ( ) [private]

Helper method to publish the navigation state.

void labust::navigation::Estimator3D::start ( )

Start the estimation loop.

void Estimator3D::start ( )

Start the estimation loop.

Outlier test

Definition at line 496 of file EKF3D.cpp.

void labust::navigation::Estimator3D::start ( )

Start the estimation loop.

Member Data Documentation

bool labust::navigation::Estimator3D::absoluteEKF [private]

Definition at line 208 of file EKF3D.hpp.

double labust::navigation::Estimator3D::alt [private]

Temporary altitude storage.

Temporary altitude, NED currents storage.

Definition at line 178 of file EKF3D.hpp.

ros::Subscriber labust::navigation::Estimator3D::altitude [private]

Definition at line 150 of file EKF3D.hpp.

tf2_ros::TransformBroadcaster labust::navigation::Estimator3D::broadcaster [private]

The transform broadcaster.

Definition at line 174 of file EKF3D.hpp.

ros::Publisher labust::navigation::Estimator3D::buoyancyHat [private]

Definition at line 144 of file EKF3D.hpp.

double labust::navigation::Estimator3D::compassVariance [private]

The compass and gyro variance.

Definition at line 194 of file EKF3D.hpp.

ros::Publisher labust::navigation::Estimator3D::currentsHat [private]

Definition at line 144 of file EKF3D.hpp.

double labust::navigation::Estimator3D::currentTime [private]

Current time in seconds

Definition at line 223 of file EKF_3D_USBL.hpp.

double labust::navigation::Estimator3D::delay_time [private]

Fixed time delay for USBL navigation

Definition at line 228 of file EKF_3D_USBL.hpp.

double labust::navigation::Estimator3D::delayTime [private]

Definition at line 224 of file EKF_3D_USBL.hpp.

float labust::navigation::Estimator3D::deltaXpos [private]

Definition at line 210 of file EKF3D.hpp.

float labust::navigation::Estimator3D::deltaYpos [private]

Definition at line 210 of file EKF3D.hpp.

ros::Subscriber labust::navigation::Estimator3D::depth [private]

Definition at line 150 of file EKF3D.hpp.

DvlHandler labust::navigation::Estimator3D::dvl [private]

The DVL handler.

Definition at line 162 of file EKF3D.hpp.

int labust::navigation::Estimator3D::dvl_model [private]

The DVL model selector.

Definition at line 190 of file EKF3D.hpp.

ros::Time labust::navigation::Estimator3D::dvl_time [private]

Last DVL measurement received time.

Definition at line 168 of file EKF3D.hpp.

double labust::navigation::Estimator3D::dvl_timeout [private]

Timeout value for the DVL.

Definition at line 170 of file EKF3D.hpp.

bool labust::navigation::Estimator3D::enableBearing [private]

Definition at line 232 of file EKF_3D_USBL.hpp.

bool labust::navigation::Estimator3D::enableDelay [private]

USBL measurements enable flags

Definition at line 232 of file EKF_3D_USBL.hpp.

bool labust::navigation::Estimator3D::enableElevation [private]

Definition at line 232 of file EKF_3D_USBL.hpp.

bool labust::navigation::Estimator3D::enableRange [private]

Definition at line 232 of file EKF_3D_USBL.hpp.

GPSHandler labust::navigation::Estimator3D::gps [private]

The GPS handler.

Definition at line 154 of file EKF3D.hpp.

double labust::navigation::Estimator3D::gyroVariance [private]

Definition at line 194 of file EKF3D.hpp.

ImuHandler labust::navigation::Estimator3D::imu [private]

The Imu handler.

Definition at line 158 of file EKF3D.hpp.

KFNav::vector labust::navigation::Estimator3D::inputVec [private]

The input vector.

Definition at line 143 of file EKF_RTT.hpp.

bool labust::navigation::Estimator3D::KFmode [private]

Definition at line 208 of file EKF3D.hpp.

bool labust::navigation::Estimator3D::KFmodePast [private]

Definition at line 208 of file EKF3D.hpp.

double labust::navigation::Estimator3D::max_dvl [private]

Maximum dvl speed for sanity checks.

Definition at line 164 of file EKF3D.hpp.

boost::mutex labust::navigation::Estimator3D::meas_mux [private]

Mutex for data protection.

Definition at line 196 of file EKF3D.hpp.

KFNav::vector labust::navigation::Estimator3D::measDelay [private]

Definition at line 170 of file EKF_3D_USBL.hpp.

KFNav::vector labust::navigation::Estimator3D::measurements [private]

The measurements vector and arrived flag vector.

Definition at line 136 of file EKF3D.hpp.

double labust::navigation::Estimator3D::min_altitude [private]

Minimum safe altitude that is accepted as measurement.

Definition at line 166 of file EKF3D.hpp.

ros::Subscriber labust::navigation::Estimator3D::modelUpdate [private]

Definition at line 150 of file EKF3D.hpp.

KFNav labust::navigation::Estimator3D::nav [private]

The navigation filter.

Definition at line 128 of file EKF3D.hpp.

KFNav::vector labust::navigation::Estimator3D::newMeas [private]

Definition at line 136 of file EKF3D.hpp.

labust::tools::OutlierRejection labust::navigation::Estimator3D::OR [private]

Definition at line 252 of file EKF_3D_USBL.hpp.

KFNav::ModelParams labust::navigation::Estimator3D::params [private]

Model parameters

Definition at line 182 of file EKF3D.hpp.

std::deque<FilterState> labust::navigation::Estimator3D::pastStates [private]

Definition at line 250 of file EKF_3D_USBL.hpp.

KFNav::matrix labust::navigation::Estimator3D::Pstart [private]

Definition at line 212 of file EKF3D.hpp.

ros::Publisher labust::navigation::Estimator3D::pubRange [private]

Definition at line 178 of file EKF_3D_USBL.hpp.

ros::Publisher labust::navigation::Estimator3D::pubRangeFiltered [private]

Definition at line 178 of file EKF_3D_USBL.hpp.

ros::Publisher labust::navigation::Estimator3D::pubStateHat [private]

Definition at line 155 of file EKF_RTT.hpp.

ros::Publisher labust::navigation::Estimator3D::pubStateMeas [private]

Estimated and measured state publisher.

Definition at line 155 of file EKF_RTT.hpp.

ros::Publisher labust::navigation::Estimator3D::pubwk [private]

Definition at line 178 of file EKF_3D_USBL.hpp.

bool labust::navigation::Estimator3D::quadMeasAvailable [private]

Variables for relative/absolute mode switching

Definition at line 208 of file EKF3D.hpp.

ros::Subscriber labust::navigation::Estimator3D::resetTopic [private]

Definition at line 150 of file EKF3D.hpp.

KFNav::matrix labust::navigation::Estimator3D::Rstart [private]

Definition at line 212 of file EKF3D.hpp.

ros::Publisher labust::navigation::Estimator3D::stateHat [private]

Definition at line 144 of file EKF3D.hpp.

ros::Publisher labust::navigation::Estimator3D::stateMeas [private]

Estimated and measured state publisher.

Definition at line 144 of file EKF3D.hpp.

ros::Subscriber labust::navigation::Estimator3D::sub [private]

Definition at line 150 of file EKF3D.hpp.

ros::Subscriber labust::navigation::Estimator3D::subCurrentsHat [private]

Definition at line 159 of file EKF_RTT.hpp.

ros::Subscriber labust::navigation::Estimator3D::subKFmode [private]

Definition at line 150 of file EKF3D.hpp.

ros::Subscriber labust::navigation::Estimator3D::subStateHat [private]

Sensors and input subscribers.

Definition at line 159 of file EKF_RTT.hpp.

ros::Subscriber labust::navigation::Estimator3D::subTargetDepth [private]

Definition at line 159 of file EKF_RTT.hpp.

ros::Subscriber labust::navigation::Estimator3D::subTargetHeading [private]

Definition at line 159 of file EKF_RTT.hpp.

ros::Subscriber labust::navigation::Estimator3D::subTargetTau [private]

Definition at line 159 of file EKF_RTT.hpp.

ros::Subscriber labust::navigation::Estimator3D::subUSBL [private]

Definition at line 182 of file EKF_3D_USBL.hpp.

ros::Subscriber labust::navigation::Estimator3D::subUSBLfix [private]

Definition at line 159 of file EKF_RTT.hpp.

ros::Subscriber labust::navigation::Estimator3D::tauAch [private]

Sensors and input subscribers.

Definition at line 150 of file EKF3D.hpp.

KFNav::vector labust::navigation::Estimator3D::tauIn [private]

The input vector.

Definition at line 132 of file EKF3D.hpp.

ros::Publisher labust::navigation::Estimator3D::turns_pub [private]

Turn count publisher.

Definition at line 146 of file EKF3D.hpp.

double labust::navigation::Estimator3D::u [private]

Definition at line 167 of file EKF_RTT.hpp.

ros::Publisher labust::navigation::Estimator3D::unsafe_dvl [private]

Definition at line 144 of file EKF3D.hpp.

labust::math::unwrap labust::navigation::Estimator3D::unwrap [private]

Heading unwrapper.

Definition at line 140 of file EKF3D.hpp.

ros::Subscriber labust::navigation::Estimator3D::useGyro [private]

Definition at line 150 of file EKF3D.hpp.

bool labust::navigation::Estimator3D::useYawRate [private]

The flag to indicate existing yaw-rate measurements.

Definition at line 186 of file EKF3D.hpp.

double labust::navigation::Estimator3D::v [private]

Definition at line 167 of file EKF_RTT.hpp.

double labust::navigation::Estimator3D::xc [private]

Definition at line 167 of file EKF_RTT.hpp.

double labust::navigation::Estimator3D::yc [private]

Definition at line 167 of file EKF_RTT.hpp.

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

Classes

Public Member Functions

Private Types

Private Member Functions

Private Attributes

Detailed Description

Member Typedef Documentation

Member Enumeration Documentation

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation