#include <TerrainNav.h>

Inheritance diagram for TerrainNav:

Public Member Functions
	TerrainNav ()

	TerrainNav (char *mapName)

	TerrainNav (char mapName, char vehicleSpecs)

	TerrainNav (char mapName, char vehicleSpecs, const int &filterType)

	TerrainNav (char mapName, char vehicleSpecs, const int &filterType, char *directory)

	TerrainNav (char mapName, char vehicleSpecs, const int &filterType, const int &mapType)

	TerrainNav (char mapName, char vehicleSpecs, const int &filterType, const int &mapType, char *directory)

virtual	~TerrainNav ()

virtual void	estimatePose (poseT *estimate, const int &type)

virtual void	measUpdate (measT *incomingMeas, const int &type)

virtual void	mbUpdate (mbT *incomingMeas)

virtual void	motionUpdate (poseT *incomingNav)

virtual bool	outstandingMeas ()

virtual bool	lastMeasSuccessful ()

virtual void	setInterpMeasAttitude (bool set)

virtual void	setMapInterpMethod (const int &type)

virtual void	setVehicleDriftRate (const double &driftRate)

virtual bool	isConverged ()

virtual int	getFilterType ()

virtual void	useLowGradeFilter ()

virtual void	useHighGradeFilter ()

virtual void	setFilterReinit (const bool allow)

void	reinitFilter (bool lowInfoTransition)

virtual void	setModifiedWeighting (const bool use)

virtual int	getFilterState ()

virtual int	getNumReinits ()

virtual bool	initialized ()

Public Attributes
TNavFilter *	tNavFilter

Protected Member Functions
void	createFilter (const int filterType, const double *windowVar)

void	initVariables ()

void	attemptInitFilter (poseT &initEstimate)

void	initMotion (poseT &initEstimate)

bool	interpolatePoses (const poseT &pose1, const poseT &pose2, poseT &newPose, const double newTime)

void	computeMeasVariance (measT &currMeas)

void	checkVelocityValidity (poseT &currPose)

void	checkRangeValidity (measT &currMeas)

bool	checkFilterHealth ()

Protected Attributes
measT	waitingMeas [MEAS_BUFFER_SIZE]
	static array containing all unincorporated sonar measurements More...

int	numWaitingMeas
	integer indicating the current number of unincorporated sonar measurements More...

bool	lastMeasSuccess

double	lastValidVel [3]

bool	lastVelBotLock

bool	lastMeasValid
	status of last measurement - only used for testing initialization More...

double	lastValidRange [4]

double	lastValidRangeTime [4]

bool	noValidRange [4]

double	lastMeasSuccessTime
	last time associated with successful measurement update More...

double	lastInitAttemptTime
	time of last filter init attempt More...

double	lastBottomLockTime
	time of last bottom lock velocity More...

poseT	estNavOffset
	last valid estimated navigation offset More...

bool	allowFilterReinits
	boolean indicating if filter reinitializations are allowed. More...

bool	useModifiedWeighting
	boolean indicating if modified weighting should be used. More...

int	numReinits
	counter for number of filter reinitializations More...

char *	saveDirectory

char *	vehicleSpecFile

char *	mapFile

int	filterType
	type of filter More...

int	mapType
	type of map More...

TerrainMap *	terrainMap
	terrainMap object containing information about current map being used More...

bool	_initialized
	current state of filter More...

Detailed Description

Class: TerrainNav

This class is used as an interface between a user and the underlying TNavFilter object which uses a terrain navigation algorithm to compute pose estimates based on sonar and inertial sensor measurements. This class's primary purpose is to appropriately handle incoming inertial and sonar measurements, pass them to the underlying TNavFilter object, and return pose estimates when requested by the user.

Intended use:

 Generate terrainNav object and estimate structures once at the start:
       TerrainNav *tercom = new TerrainNav(mapName, vehicleName,
                                          filterType);  
   poseT* tercomEst = new poseT;                        
       poseT* currInertialPose = new poseT; 
       measT* currMeas = new measT; 

 Incorporate inertial sensor information as it comes in:    
      tercom->motionUpdate(currInertialPose); 

 Incorporate sonar measurements as they come in:
       tercom->measUpdate(currMeas, dvl);

 Ask for most recent terrain navigation pose estimate:
       tercom->estimatePose(tercomEst, mmse);

 ***NAVIGATION AND MEASUREMENTS MUST BE ADDED SEQUENTIALLY IN TIME, 
    OTHERWISE SOME MEASUREMENTS MAY BE SKIPPED***

Note: This class can handle various combinations of sonar and inertial sensor data rates. If the sonar and inertial sensor measurements are out of sync, the inertial pose measurements are interpolated to provide an estimate of the vehicle pose associated with each sonar measurement. If the sonar data rate is faster than the inertial sensor data rate, sonar measurements are collected and stored until the next inertial sensor measurement comes in, at which point all collected measurements are added into the navigation filter in sequence.

Constructor & Destructor Documentation

TerrainNav::TerrainNav ( )

inline

TerrainNav::TerrainNav ( char * mapName )

Initializes a new TerrainNav with terrain map "mapName.txt". The mapping AUV specs and the Point Mass Filter algorithm are used as defaults.

TerrainNav::TerrainNav	(	char *	mapName,
		char *	vehicleSpecs
	)

Sets private variables mapFile = "mapName.txt", vehicleSpecFile = "vehicleName.txt", and saveDirectory = NULL, default mapType = 1 (DEM). Makes a call to createFilter(1) to initialize a new TerrainNav object with default type = 1 (Point Mass Filter).

TerrainNav::TerrainNav	(	char *	mapName,
		char *	vehicleSpecs,
		const int &	filterType
	)

Sets private variables mapFile = "mapName.txt", vehicleSpecFile = "vehicleName.txt", and saveDirectory = NULL, default mapType = 1 (DEM). Makes a call to createFilter(filterType) to initialize a new TerrainNav object with specified type.

TerrainNav::TerrainNav	(	char *	mapName,
		char *	vehicleSpecs,
		const int &	filterType,
		char *	directory
	)

Sets private variables mapFile = "mapName.txt", vehicleSpecFile = "vehicleName.txt", and saveDirectory = "directory", default mapType = 1 (DEM). Makes a call to createFilter(filterType) to initialize a new TerrainNav object with specified type.

TerrainNav::TerrainNav	(	char *	mapName,
		char *	vehicleSpecs,
		const int &	filterType,
		const int &	mapType
	)

Sets private variables mapFile = "mapName.txt", vehicleSpecFile = "vehicleName.txt", and mapType = mapType (1 is DEM, 2 is octree). Makes a call to createFilter(filterType) to initialize a new TerrainNav object with specified type.

TerrainNav::TerrainNav	(	char *	mapName,
		char *	vehicleSpecs,
		const int &	filterType,
		const int &	mapType,
		char *	directory
	)

Sets private variables mapFile = "mapName.txt", vehicleSpecFile = "vehicleName.txt", and saveDirectory = "directory", mapType = mapType (1 is DEM, 2 is octree). Makes a call to createFilter(filterType) to initialize a new TerrainNav object with specified type.

TerrainNav::~TerrainNav ( )

virtual

Frees all storage associated with the TerrainNav object.

Member Function Documentation

void TerrainNav::attemptInitFilter ( poseT & initEstimate )

protected

Try to initialize the terrain navigation filter, TNavFilter, based on the first inertial sensor measurement contained in the initEstimate poseT structure. Initialization is only allowed if the following conditions are met:

Vehicle has bottom lock
Vehicle has valid velocity measurements
Vehicle is within a valid region of the terrain map
Vehicle has valid sonar measurements
Vehicle is below the surface

bool TerrainNav::checkFilterHealth ( )

protected

This function checks the health of the filter and calls the reinitFilter() function if necessary. This checks the health by performing the following checks: a. Check if the length of time since the last successful measurement exceeds a set maximum. (uses lastMeasSuccessTime) b. Check if the x/y uncertainty of the filter exceeds a set maximum. (uses currVar)

void TerrainNav::checkRangeValidity ( measT & currMeas )

protected

Check validity of Measured ranges. If invalid, changes beamStatus flags in currMeas to false. Validity check is based on the following:

NaN range values
range falls within MIN_RANGE < r < MAX_RANGE boundaries
dr/dt of an individual beam exceeds MAX_DRDT
all but one range measurement are equal

void TerrainNav::checkVelocityValidity ( poseT & currPose )

protected

Check validity of DVL/INS velocity. If invalid, changes dvlValid flag in currPose to false. Validity check is based on the following:

velocity is below specified MAX
vehicle x-direction velocity is greater than zero (vehicle not stationary)
estimated acceleration exceeds specified MAX_ACCEL

void TerrainNav::computeMeasVariance ( measT & currMeas )

protected

Fills in the covariance array of the "currMeas" measT struct with a % of the measured range for each beam. If the covariance array is already filled or the current measurement has no corresponding sensor type, this function returns without making any changes. Otherwise, the percentRangeError property of the corresponding sensor is used to evaluate the variance as follows: sigma = percentRangeError*range;

void TerrainNav::createFilter	(	const int	filterType,
		const double *	windowVar
	)

protected

Sets octreeMap in tNavFilter to oMap.

Initializes the terrain navigation filter object based on the specified filter type: 1: 3D Point Mass Filter (default - TODO:Make Other Filter Types work?) 2: 8D Particle Filter

3D Extended Kalman Filter
8D Sigma Point (Unscented) Kalman Filter

windowVar is used to size the initalization window for the new filter.

If the filter object already exists, this function will delete the current filter object and create a new one with stored saveDirectory, vehicleSpecFile and mapFile.

void TerrainNav::estimatePose	(	poseT *	estimate,
		const int &	type
	)

virtual

This is the primary function in the TerrainNav class. The function takes in a pointer to a poseT which it fills with the terrain correlation pose estimate based on previous measurements. The time stamp of the pose estimate indicates the last update time of the navigation filter. Type indicates the estimator type: 1: Maximum Likelihood Estimate 2: Minimum Mean Square Error Estimate.

Reimplemented in TerrainNavClient.

virtual int TerrainNav::getFilterState ( )

inlinevirtual

Returns the integer filterState describing the current filter state.

Reimplemented in TerrainNavClient.

virtual int TerrainNav::getFilterType ( )

inlinevirtual

Returns the integer filterType describing the current TNavFilter type.

virtual int TerrainNav::getNumReinits ( )

inlinevirtual

Returns the integer numReinits describing the number of reinitializations.

Reimplemented in TerrainNavClient.

virtual bool TerrainNav::initialized ( )

inlinevirtual

void TerrainNav::initMotion ( poseT & initEstimate )

protected

Initializes the terrain navigation filter, TNavFilter, based on the first inertial sensor measurement contained in the initEstimate poseT structure.

void TerrainNav::initVariables ( )

protected

Initializes private variables associated with a terrainNav object

bool TerrainNav::interpolatePoses	(	const poseT &	pose1,
		const poseT &	pose2,
		poseT &	newPose,
		const double	newTime
	)

protected

Linearly interpolates pose information from pose1 and pose2 to a new pose at time newTime. The interpolated pose is recorded in newPose.

virtual bool TerrainNav::isConverged ( )

inlinevirtual

Returns a boolean indicating if the terrain navigation filter has converged to an estimate.

Reimplemented in TerrainNavClient.

virtual bool TerrainNav::lastMeasSuccessful ( )

inlinevirtual

Returns a boolean indicating if the last sonar measurement was successfully incorporated into the filter or not.

Reimplemented in TerrainNavClient.

void TerrainNav::mbUpdate ( mbT * incomingMeas )

virtual

Passes the current multibeam range information along to the TNavFilter object. The function takes in a pointer to a mbT containing the current multibeam measurement ranges.

Reimplemented in TerrainNavClient.

void TerrainNav::measUpdate	(	measT *	incomingMeas,
		const int &	type
	)

virtual

Passes the current sonar measurement information along to the TNavFilter object. The function takes in a pointer to a measT containing the current sonar measurement information. It also takes in an integer type variable which relates the sonar measurement type as follows: 1: DVL 2: Multibeam 3: Single Beam 4: Homer Relative Measurement

Reimplemented in TerrainNavClient.

void TerrainNav::motionUpdate ( poseT * incomingNav )

virtual

Passes the current inertial measurement information along to the TNavFilter object. The function takes in a pointer to a poseT containing the current inertial measurement information.

Reimplemented in TerrainNavClient.

virtual bool TerrainNav::outstandingMeas ( )

inlinevirtual

Returns a boolean indicating if there are any measurements not yet incorporated into the PDF and waiting for more recent inertial measurement data.

Reimplemented in TerrainNavClient.

void TerrainNav::reinitFilter ( bool lowInfoTransition )

This function reinitializes the filter. It will copy relevant information from the current filter object, delete that object, then re-create the filter.

23-Aug-2016 RGH : Exposing for use with TerrainAid and TerrainNavClient

virtual void TerrainNav::setFilterReinit ( const bool allow )

inlinevirtual

Overwrite boolean allowFilterReinits with input argument, allow.

Reimplemented in TerrainNavClient.

virtual void TerrainNav::setInterpMeasAttitude ( bool set )

inlinevirtual

Sets a boolean indicating if the sonar measurement attitude information should be determined from interpolated inertial poses. Default = 0;

Reimplemented in TerrainNavClient.

virtual void TerrainNav::setMapInterpMethod ( const int & type )

inlinevirtual

Specifies the interpolation method to use for determining inter-grid map depth values. 0: nearest-neighbor (no interpolation) 1: bilinear 2: bicubic 3: spline Default = 0;

Reimplemented in TerrainNavClient.

virtual void TerrainNav::setModifiedWeighting ( const bool use )

inlinevirtual

Overwrite boolean useModifiedWeighting with input argument, use.

Reimplemented in TerrainNavClient.

virtual void TerrainNav::setVehicleDriftRate ( const double & driftRate )

inlinevirtual

Sets the vehicle inertial drift rate parameter. The default value is determined by the vehicle specification sheet. The driftRate parameter is given in units of % drift in meters/sec.

Reimplemented in TerrainNavClient.

virtual void TerrainNav::useHighGradeFilter ( )

inlinevirtual

Force filter settings for low grade system: 3DOF system with ALLOW_ATTITUDE_SEARCH=0, DEAD_RECKON=0, SEARCH_GYRO=0

Reimplemented in TerrainNavClient.

virtual void TerrainNav::useLowGradeFilter ( )

inlinevirtual

Force filter settings for low grade system: 7DOF system with ALLOW_ATTITUDE_SEARCH=1, DEAD_RECKON=1, SEARCH_GYRO=1

Reimplemented in TerrainNavClient.

Member Data Documentation

bool TerrainNav::_initialized

protected

current state of filter

pcl pointcloud object containing current map when using octree boolean indiating true if initialized successfully (i.e., the mapfile was found)

bool TerrainNav::allowFilterReinits

protected

boolean indicating if filter reinitializations are allowed.

poseT TerrainNav::estNavOffset

protected

last valid estimated navigation offset

int TerrainNav::filterType

protected

type of filter

double TerrainNav::lastBottomLockTime

protected

time of last bottom lock velocity

double TerrainNav::lastInitAttemptTime

protected

time of last filter init attempt

bool TerrainNav::lastMeasSuccess

protected

boolean indicating if the previous sonar measurement was successfully incoporated into the navigation filter

double TerrainNav::lastMeasSuccessTime

protected

last time associated with successful measurement update

bool TerrainNav::lastMeasValid

protected

status of last measurement - only used for testing initialization

double TerrainNav::lastValidRange[4]

protected

double TerrainNav::lastValidRangeTime[4]

protected

double TerrainNav::lastValidVel[3]

protected

array containing the last good velocity measurements and bottom-lock status / Velocity is given in the body frame

bool TerrainNav::lastVelBotLock

protected

char* TerrainNav::mapFile

protected

int TerrainNav::mapType

protected

type of map

bool TerrainNav::noValidRange[4]

protected

int TerrainNav::numReinits

protected

counter for number of filter reinitializations

int TerrainNav::numWaitingMeas

protected

integer indicating the current number of unincorporated sonar measurements

char* TerrainNav::saveDirectory

protected

TerrainMap* TerrainNav::terrainMap

protected

terrainMap object containing information about current map being used

TNavFilter* TerrainNav::tNavFilter

tNavFilter object used to recursively incorporate vehicle measurements according to a terrain navigation algorithm

bool TerrainNav::useModifiedWeighting

protected

boolean indicating if modified weighting should be used.

char* TerrainNav::vehicleSpecFile

protected

measT TerrainNav::waitingMeas[MEAS_BUFFER_SIZE]

protected

static array containing all unincorporated sonar measurements

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

Public Member Functions

Public Attributes

Protected Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation