TRN Quick Start Guide : libtrnav use and configuration
At it's core, TRN is relatively simple to use. The quick start guide takes you through the basic steps:
- Building libtrnav
- Creating a TRN instance
- Setting TRN configuration options
- Understanding TRN Parameters
- Providing TRN input : using PoseT and MeasT data structures
- Interpreting TRN output (position estimates)
An example application, trn-player, is included.
Contents
- TRN Quick Start Guide : libtrnav use and configuration
Building libtrnav
Dependencies
- libnetcdf
- mframe (MBARI internal repo, git@bitbucket.org:mbari/mframe.git)
- libpthread
- libm
- libcurl (for some versions of NetCDF)
- libhdf5 (for some versions of NetCDF)
- librt (some linux builds)
libtrnav may be built with either cmake or gnu make. cmake is the preferred build system.
In the following sections the following convention is used:
- $TRN_BUILD : directory containing libtrnav (and mframe) repo(s)
- $LIBTRNAV : path to the libtrnav repo
- $MFRAME : path to the mframe repo
- $SANDBOX : path to directory containing libtrnav installation and libtrnav-data
These environment variables are used only to create a consistent frame of reference in the documentation and examples; they are not required for TRN or referenced by any of the software.
Build Sandboxed Installation for trn-player Example
libtrnav includes a script, opt/examples/trn-player/pkg/trnplayer-sandbox.sh that generates a sandboxed build for the trn-player example:
$LIBTRNAV/opt/examples/trn-player/pkg/trnplayer-sandbox.sh -v
The script is also available in the Appendices.
creates directory trnplayer-<id> (TRN_BUILD) containing the sandbox directory (SANDBOX). If the -k option is used, the libtrnav and mframe directories are preserved; otherwise, they are deleted from trnplayer-<id>.
Alternatively, mframe and libtrnav may be built manually (installed to trnplayer/sandbox) as follows:
# create TRN_BUILD directory, go there
mkdir trnplayer
cd trnplayer
export TRN_BUILD=$PWD
# build mframe
cd $TRN_BUILD
git clone git@bitbucket.org:mbari/mframe.git
cd mframe
mkdir build
cd build
cmake -DCMAKE_INSTALL_PREFIX=$PWD/pkg ..
cmake --build .
cmake --install .
# build libtrnav, install to TRN_BUILD/sandbox
cd $TRN_BUILD
git clone git@bitbucket.org:mbari/libtrnav.git
cd libtrnav
mkdir build
cmake -DCMAKE_INSTALL_PREFIX=$TRN_BUILD/sandbox ..
cmake --build .
cmake --install .
# set SANDBOX environment variable
cd $TRN_BUILD/sandbox
export SANDBOX=$PWD
# install libtrnav-data (optional, to run trn-player examples)
cd $SANDBOX
git clone git@bitbucket.org:mbari/libtrnav-data.git
General Build and Installation
It is recommended to build libtrnav using cmake. The general build instruction are similar to the sandboxed example, except that libtrnav is installed to the default location (/usr/local) or another directory:
# build libtrnav
cd $TRN_BUILD
git clone git@bitbucket.org:mbari/libtrnav.git
cd libtrnav
mkdir build
cmake ..
cmake --build .
cmake --install .
Use option -DCMAKE_INSTALL_PREFIX=<path> when configuring libtrnav to install to another directory (e.g. as in the sandboxed build).
All cmake build output goes to the build directory.
If mframe is not installed to one of the default locations:
- /usr/local
- TRN_BUILD/mframe/build/pkg
the MFRAME_PKG option must be specified when building libtrnav :
cmake -DMFRAME_PKG_PATH=<path> [options...] ..
cmake build options
cmake options include
buildOptROV=ON|OFF Enable build of opt/rov libraries and applications
buildTrnNoRand=ON|OFF Disable TRN random seed generation
buildTrnLog=ON|OFF Disable binary TRN log
buildTrnLogEst=ON|OFF Disable binary TRN estimate log
buildTrnCEPCorr=ON|OFF Enable CEP correction (deprecated, don't use)
enable during cmake configuring using
cmake -D<option>=<value>...
The install path for cmake may be configured using
cmake -DCMAKE_INSTALL_PREFIX=<path>...
or during install using
cmake --install . --prefix=\<path\>
In both cases, the install path should be absolute.
Build with gnu make (deprecated)
The core libraries are built using
make
sudo make install
components that require libmframe are built with the trnc target.
make trnc
for a full list of targets (including application-specific), use
make help
The install path for gnu make is generally set using the environment variable PREFIX. The environment variable DESTDIR may also be used for greater flexibility:
${DESTDIR}${PREFIX}/...
Note that DESTDIR should include a trailing slash, PREFIX should not.
gnu make output goes to the libtranav/bin and libtrnav/build. The build directory contains intermediate files, bin contains libraries and executables.
tar.gz distribution
The top level Makefile includes a dist target that builds a tar.gz distribution, libtrnav-\<version>.tar.gz
The opt/sentry directory includes a script for packaging libtrnav and libmframe, along with scripting to configure and install them using either cmake or gnu make. This is used to distribute libtrnav for the WHOI Sentry vehicle. This is a convenient way to distribute the code and build stand-alone installations in a test directory.
Details are provided in opt/sentry/README-trndev-build.md.
Build Doxygen doc
The libtrnav doc directory contains a Doxygen configuration file; while the code does not consistently include doxygen markup, Doxygen generates a fairly sensible package that is convenient to navigate. It may be built using the gnu make doc target:
make doc
Output will be written to doc/doxygen.out
libtrnav Contents
See the Software Guide for a list of libtrnav applications and libraries.
Creating a TRN instance
The C++ API has a number of constructors with different configuration information, for example:
// minimal constructors
TerrainNav();
// commonly used
TerrainNav(char *mapName, char *vehicleSpecs,
const int &filterType, const int &mapType);
TerrainNav(char *mapName, char *vehicleSpecs,
const int &filterType, const int &mapType, char *directory);
TerrainNav(char* mapName, char* vehicleSpecs, char* particles,
const int& filterType, const int& mapType, char* directory);
The C API constructor takes a trn_cfg_t structure. The structure members are used to invoke a C++ constructor.
wtnav_t *wtnav_new(trn_cfg_t *cfg);
The C++ TRN constructor arguments are described in the following section.
In the quick start section, we'll reference the C++ API; the basic steps are similar for the C API, in which the trn_cfg_t structure replaces the constructor arguments.
It's worth noting that TRN may be run as a TCP/IP server using the trn-server application. trn-server enables applications to perform TRN functions via TCP using TrnClient (or it's base class TerrainNavClient). trn-server configuration is essentially the same as for a TRN instance constructed in an application.
Configuring TRN
Consider the parameters passed to the TRN C++ constructor, summarized in the following table and explained below.
| Parameter | Description | ||||
|---|---|---|---|---|---|
| mapName | map file name; format should match type specified by mapType argument (path is specified using TRN_MAPFILES environment variable) |
||||
| vehicleSpecs | Vehicle specs file name; contents defined below (path is specified using TRN_CONFIGDIR environment variable) |
||||
| filterType | An integer (enumeration) indicating a TRN filter variant. While several experiment filter types exist, filter type 2 (TRN_FT_PARTICLE, a particle filter) is used almost exclusively. | ||||
| mapType | An integer (enumeration) indicating a TRN map format:
| ||||
| directory | directory in which to save TRN logs and store copies of config files | ||||
| particles | particle file name (logs particle file information; optional (a lot of data) |
Applications typically use a configuration file and environment variables to obtain values used to construct TerrainNav and/or initialize utility classes TNavConfig or TrnAttr that are used to manage configuration values. TrnAttr is used to parse and use key/value pairs primarily used for terrainAid.cfg.
Fig: TRN configuration
The map files, configuration files, and environment variables are described in greater detail in the following sections.
Application Configuration Files
There are three files strictly required to configure TRN:
| File | Description |
|---|---|
| Map file | Contains TRN map data in one of two supported formats |
| Vehicle specification file | Enumerates bathymetry sensors used by TRN and describes mounting geometry |
| Sensor specification file(s) | Describes sensor beam geometry and other TRN parameters referenced by the vehicle specification file. |
The format and content of these is described below.
Though each application is different, most use a configuration file and environment variables to specify TRN configuration parameters, though it is not strictly required.
terrainAid.cfg
Some applications use a file called terrainAid.cfg to load configuration values; it is convenient, but not required. The trn-player example app also includes a configuration file parser that supports terrainAid.cfg syntax.
terrainAid.cfg is a text file with key/value entries using the format:
// comment...
key = value;
Commonly used parameters include:
| Parameter | Description |
|---|---|
| mapFileName | map file name (path specified by TRN_MAPFILES) |
| map_type | Numeric map type value (1: DEM/GRD 2: BO) |
| vehicleCfgName | vehicle specification file |
| resonCfgName | Reson multibeam sonar sensor specification |
| dvlCfgName | DVL sensor specification file |
| filterType | type of TRN filter (usually 2: particle filter) |
| terrainNavServer | TRN server IP address applies only to trn-server (TRN via TCP/IP) |
| terrainNavServer | TRN server IP port applies only to trn-server (TRN via TCP/IP) |
| allowFilterReinits | enable TRN filter reinitialization if true |
These values are used primarily by the replay application (libtrnav/opt/dorado):
| Parameter | Description |
|---|---|
| samplePeriod | for DVL, used to skip lines with timestamps inside previous sample |
| useMbTrnData | Use the MbTrn data format (MbTrn.log) if true, otherwise use TerrainAid.log |
| lrauvDvlFilename | use lrauv log data |
| useIDTData | use Imagenex DeltaT data |
| maxNorthingCov | convergence criteria |
| maxEastingCov | convergence criteriay |
| maxNorthingError | convergence criteriay |
| maxEastingError | convergence criteria |
Other parameters that are experimental or used for debug may generally be excluded or commented out; these include
| Parameter | Description |
|---|---|
| particlesName | name of particles file (logs particle state information). IO/storage intensive; generally for debugging |
| useModifiedWeighting | select one of several alternative particle weighting schemes. |
| forceLowGradeFilter | deprecated? |
In practice, applications like replay populate a (singleton) TNavConfig instance or TrnAttr using the contents of a configuration file. The TrnAttr struct contains a set of elements associated with terrainAid.cfg, a config file used by Dorado and some other applications. The TrnClient uses TrnAttr, and replay has code for parsing terrainAid.cfg into TRN_attr. TrnAttr should be used for new applications that use terrainAid.cfg keys.
In contrast, the TrnPlayer example in the Quick Start Guide does not require a configuration file. It uses the TrnPlayerCtx to parse options passed on the command line. The context class contains configuration values and instance variables. This pattern helps to support multiple instances concurrently. TrnPlayer does support an extended terrainAid.cfg syntax, and TrnPlayerCtx includes parsing code.
The ROV TRN implementation (libtrnav/opt/rov) uses a similar context base approach; it's trnxpp_ctx context class supports both command line and configuration files.
Environment Variables
Several environment variables may be used to set the paths file arguments, though it is not required by TRN per se. This is done to enable use cases in which TRN configuration parameters are passed remotely, e.g. to a TRN server instance, whose host environment may differ from the callers.
Core Environment variables are summarized in the following table; additional variables are described in later sections.
| Environment | Description |
|---|---|
| TRN_LOGFILES | Output directory for TRN logs and config file copies |
| TRN_DATAFILES | Input directory for TRN config files (e.g. vehicle specs, sensor specs) |
| TRN_MAPFILES | Path to TRN map files |
Map Files
As indicated above, TRN supports two map file formats: digital elevation maps (.GRD) or binary octree files. Binary octrees are typically used, and though a single monolithic map file may be used, they are often sub-divided into smaller tiles that reduce memory use. Tools exist in the libtrnav repository to generate tiled GRD and BO maps.
When tiled maps are used, they must be stored in the same directory, along with a tiles.csv file defining the tile boundaries.
The name of the map directory or single map file is passed to the TRN constructor, and the TRN_MAPFILES environment variable set accordingly.
Vehicle and Sensor Spec Files
There are two types of configuration files associated with libtrnav: the vehicle specification file and one or more sensor specification files. These are required by TRN.
The vehicle spec file specify some TRN parameters, and enumerates the sensors that will be used by TRN, and describes mounting orientation. Each of the sensors is defined in a separate sensor spec file, and defines additional sensor-specific TRN parameters, e.g. beam geometry. These are described in detail below, with examples.
With these files, TRN supports processing bathymetry (beams) from several types of commonly used sensors - e.g. DVLs and multibeam sonars; the parameters they describe are primarily about describing the sensor geometry and orientation with respect to the platform. TRN is able to apply the appropriate geometry calculations to bathymetry inputs for the given navigation and attitude.
There are use cases that don't fit typical fixed sensors, for example, in a system in which a bathymetry sensor is on a rotating platform, the mounting angles change dynamically. In such cases, the application may set the relevant configuration file parameters to zero, and geometry calculations for TRN inputs will need to be computed by the application.
Vehicle Specification File
The vehicle specification file is a text file containing a list of sensor config files, sensor mounting geometries, and a process noise parameter. It's format is rigid; it uses a naming convention <NAME>_spec.cfg, and its contents are arranged as follows:
# comments...
NAME:<config file name (w/o _spec.cfg)>
NUMSENSORS:<number of sensor entries (integer)>
DRIFTRATE:<process noise figure (floating point)>
SENSORNAME:<sensor spec file name (w/o _spec.cfg)>
DR:<phi>,<theta>,<psi>
DT:<x>,<y>,<z>
...
Example: Vehicle Spec File
# Vehicle specification sheet
#----------------------------
NAME:mappingAUV
NUMSENSORS:3
DRIFTRATE:1.5
SENSORNAME:rdiDVL
DR:0,0,0
DT:0,0,0
SENSORNAME:resonSeabat8101
DR:0,0,0
DT:0,0,0
SENSORNAME:imagenexdeltat
DR:0,0,0
DT:0,0,0
The parameters are described as follows:
| Name | Description/th> |
|---|---|
| NAME | Configuration name (matches file name, w/o _spec.cfg suffix) |
| NUMSENSORS | Number of sensor entries to follow |
| DRIFTRATE | Somewhat of a misnomer, represents TRN process noise, typically 1.5/2 depends on terrain, instruments, etc. |
| SENSORNAME | name of sensor spec file (w/o _spec.cfg suffix) |
| DR | sensor orientation, NED Euler angles in vehicle frame (degrees) |
| DT | sensor mounting location, distance from vehicle cente (meters) |
Mounting geometry parameters DR and DT describe the location and orientation of the sensor with respect to the origin in the vehicle frame of references; typically this is X/FWD, Y/STBD, Z/DOWN, i.e. NED.
DT is the XYZ distance from the vehicle center, in meters. DR are NED Euler angles (phi, theta, psi) with respect to FWD, STBD, DOWN.
Sensor Specification File
The sensor specification files, referenced by the vehicle spec file, are text files containing a bathymetry sensor configuration information. A number of different sensors are supported. It's format is rigid; it uses a naming convention <NAME>_spec.cfg, and their contents are arranged as follows:
# comments...
NAME:<sensor name (matches file, w/o _spec.cfg suffix)>
TYPE: <sensor type (integer)>
NUMBEAMS: <number of beams>
PERCENTRANGEERROR: <range error, %>
BEAMWIDTH: <beam width (deprecated)>
# optional fields, type-dependent
PITCH ANGLES:<comma separated list of pitch angles>
DELTA_PITCH:<pitch angle increment>
INIT_YAW ANGLE:<comma separated list of yaw angles>
DELTA_YAW: <yaw angle increment>
INIT_PITCH ANGLES|starting pitch angle(s) in sensor frame, decimal degrees|
| Name | Description | ||||||||||||
|---|---|---|---|---|---|---|---|---|---|---|---|---|---|
| NAME | Configuration name (matches file name, w/o _spec.cfg suffix) | ||||||||||||
| TYPE | Sensor type enumeration value
| ||||||||||||
| NUMBEAMS | number of beams | ||||||||||||
| PERCENTRANGEERROR | estimated range error (% of range) | ||||||||||||
| BEAMWIDTH | width of beams, m (deprecated) | ||||||||||||
| PITCH ANGLES | sensor beam geometry: pitch angles in sensor frame, decimal degrees used for non-multibeam types |
||||||||||||
| DELTA_PITCH | sensor beam geometry: pitch angle increment, decimal degrees used for non-multibeam types |
||||||||||||
| YAW ANGLES | sensor beam geometry: yaw angles in sensor frame, decimal degrees used for non-multibeam type |
||||||||||||
| DELTA_YAW | sensor beam geometry:yaw angle increment, decimal degrees used for non-multibeam types |
The sensor beam geometry parameters assume that bathymetry sensors have beams that arranged in an angular or radial array at uniform angular intervals, which is true of all currently used TRN sensors.
Multibeam sensors are assumed to extend radially from the sensor origin, i.e. subtending an arc with a specified swath angle, typically 90-120 degrees.
Non-multibeam sensors (e.g. DVLs) are assumed to be arranged in a radial array around the sensor origin, with azimuth YAW ANGLE (relative to E) separated by DELTA_YAW. The beams are pitched downward at PITCH ANGLE, optionally incrementing by DELTA_PITCH. DVLs typically have 4 beams, separated by 90 degrees, oriented at an outward angle with respect to vertical.
For both types of sensors,
Example: multibeam sensor spec file
# Sensor specification sheet
#----------------------------
NAME: resonSeabat8101
TYPE: 2
NUMBEAMS: 20
PERCENTRANGEERROR: 4
BEAMWIDTH: 1.5
PITCH ANGLES: 0
DELTA_PITCH: 0
INIT_YAW ANGLE: 0
DELTA_YAW: 0
Example: DVL sensor spec file
# Sensor specification sheet
#----------------------------
NAME: rdiDVL
TYPE: 1
NUMBEAMS: 4
PERCENTRANGEERROR: 4
BEAMWIDTH: 3.9
PITCH ANGLES:30, 30, 30, 30
YAW ANGLES: -45, 135, 45, -135
#----------------------------------------------------------------
# Sensor beam configuration diagram:
#
# Beam numbering convention, looking down on Dvl from above.
# x and y are the vehicle axes. x points to port, y to starboard.
#
# x
# ^
# |
# 1 | 3
# |------> y
# 4 2
#
# Each beam is spaced 45 degrees from the horizontal/vertical.
# The inclination angle is 30 degrees.
Relevant files in the repository include:
terrain-nav/TerrainNav.h,cpp
terrain-nav/structDefs.h
trnw/trn_msg.h
Other TRN Configuration: run-time and compilation options
To recap, we've looked at TRN inputs and parameters required to create a TRN instance:
- configuration file (e.g. terrainAid.cfg)
- map file
- vehicle spec file
- Number of sensors
- Navigation drift rate
- Bathymetry sensor specification list and mounting geometry
- sensor spec file(s)
- sensor name and type
- estimated range error
- beam geometry
Run time TRN Configuration
The following table summarizes TRN parameters that may be set at run-time:
| Parameter | File | Description |
|---|---|---|
| setVehicleDriftRate(double &driftRate) | terrain-nav/TerrainNav.h |
Sets the vehicle inertial drift rate parameter. The driftRate parameter is given in units of % drift in meters/sec. Default : value in vehicle specification sheet. |
| setFilterReinit(bool allow) | terrain-nav/TerrainNav.h | Enable TRN filter reinitialization. Default: false |
| void setEstNavOffset(double offset_x, double offset_y, double offset_z) | terrain-nav/TerrainNav.h | This function sets the x, y, z values of the estNavOffset structure that holds the prior offset estimate. This function is used prior to a particle filter reinit callvin order to force the reinit to be centered on a particular offset value rather than the actual last offset estimate. This capability was created for use by the MB-System program mbtrnpp so that reinit could, if necessary, be forced to be centered on a zero offset or a prior very good offset estimate if that is known. Resetting the prior offset estimate addresses the occasional tendency of TRN to converge on similar topoography far from the actual location - when this occurs convergence will soon be lost, but the reinit should be centered on a more likely position if that is known external to TRN. |
| setInitStdDevXYZ(double sdev_x, double sdev_y, double sdev_z) | terrain-nav/TerrainNav.h | Values potentially used in reinitFilter and estimatePose Replaces _initVars member variable, use NULL to reset to defaults Using the method except immediately before filter reinitialization (i.e. on a running filter) may give undefined output. |
Compile time options
In addition, there are a number of TRN parameters that may be set when compiling libtrnav, described in the table below. This list is not comprehensive; the code contains a number of parameters introduced over time to try out experimental features for research; these are generally disabled and should remain so. Note that these are rarely modified; change only if you know what you are doing.
| Parameter | File | Description |
|---|---|---|
| TRN_MAX_BEAMS | terrain-nav/structDefs.h | Maximum number of bathymetry beams Default: 120 |
| TRN_MSG_SIZE | terrain-nav/structDefs.h | Maximum TRN server (commsT) message length (bytes) Default: 8000 |
| N_COVAR | terrain-nav/structDefs.h | covariance matrix size for TRN estimates Default: 45 |
| MAX_PARTICLES | particleFilterDefs.h | maximum number of particles; note this has a potentially large impact on computational load and TRN performance Default: 10000 |
| MAX_NIS_VALUE | terrain-nav/TerrainNav.h | Maximum allowable NIS Windowed Average (used by TerrainNav) Default: 1.4 |
| NIS_WINDOW_LENGTH | terrain-nav/genFilterDefs.h | Length of NIS window (used by filter) Default: 29 |
| MAX_RANGE | terrain-nav/genFilterDefs.h | Maximum allowable sonar range Default: 220 |
| MAP_NOISE_MULTIPLIER | terrain-nav/genFilterDefs.h | Multiplier on map noise in particle filter should be 1.0 usually |
| MOTION_NOISE_MULTIPLIER | terrain-nav/genFilterDefs.h | Multiplier on motion noise stdev in particle filter should be 2.0 usually |
| [XYZ]_STDDEV_INIT | terrain-nav/genFilterDefs.h | FILTER INITIALIZATION PARAMETERS
[XYZ]_STDDEV_INIT values establish a map search area; values have units of meters, and are application-specific.
If the distribution is uniform, these comprise a box (see initDistribType, TNavFilter.h).
If set incorrectly, TRN may fail to initialize correctly, resulting in errors, e.g. TerrainNav::Filter not initialized - vehicle is currently within a non-valid region of the reference map TerrainNav::Cannot compute pose estimate; motion has not been initialized. Typical values include: [XY]_STDDEV_INIT 60.0 Portuguese Ledge/Dorado 600.0 Axial/Sentry [Z]_STDDEV_INIT 5.0 Portuguese Ledge/Dorado, Axial/Sentry 10.0 ? |
Providing TRN Input: PoseT, MeasT
Once a TRN instance is configured, it is provided with bathymetry and navigation inputs that it uses to update the particle filter state. Updates are periodically passed to TrN using in an appropriate data structure (poseT, measT) via TRN's input methods: motionUpdate and measUpdate.
Fig: TRN cycle
motionUpdate is used to update vehicle navigation and attitude information. It requiers a poseT structure, and must be called first; calls made to measUpdate before motionUpdate has successfully completed will fail.
measUpdate is used to provide bathymetry at a specified location and attitude to TRN, and requires a measT structure and sensor type.
estimatePose accepts a poseT reference and an estimate type. It returns the current TRN position estimate of the specified type along uncertainties (array of covariances).
The C++ API calls are as follows:
void TerrainNav::motionUpdate(poseT* incomingNav)
void TerrainNav::measUpdate(measT* incomingMeas, const int& type)
void TerrainNav::estimatePose(poseT* estimate, const int &type)
PoseT and MeasT are defined in terrain-nav/structDefs.h.
The C API uses wmeast_t and wposet_t, which are thin C wrappers around PoseT and MeasT objects. wposet_t and wmeast_t are defined in trnw/trnw.h
poseT
The PoseT structure is used for both input and output to TRN.
For input (motionUpdate), most applications populate only a subset of these fields for input:
| Parameter | Description |
|---|---|
| time | Time in posix epoch seconds (seconds since 1970-01-01T00:00:00) |
| x,y,z | bathymetry sensor origin, world frame (e.g. UTM) (meters) |
| vx,vy,vz | velocity in x,y,z direction (m/s) |
| phi,theta,psi | vehicle pitch, roll, heading, world frame (radians) |
| dvlValid | true if DVL input is valid |
| gpsValid | true if GPS input is valid |
| bottomLock | true if DVL has bottom lock |
vx is strictly required for TRN input; vy, vz are optional. The complete structure is shown below.
To fully initialize, TRN requires that a motion update be received with vx (along-track velocity) not equal to zero, which indicates forward motion. Until that happens, TRN emits an error message indicating that TRN has not been initialized when measurement updates are received. Once motion has been established, motion and measurement updates may occur in any order.
vx is used for most configurations to indicate motion, and its value is not critical, but must be != 0 in order for TRN to initialize.
In general, TRN does not require velocity to operate, but in some contexts, velocity may be integrated to determine position, in which case it is needed.
For output (estimatePose), a PoseT instance is passed into estimatePose, which populates these fields:
| Parameter | Description | ||||||||||
|---|---|---|---|---|---|---|---|---|---|---|---|
| time | Time in posix epoch seconds (seconds since 1970-01-01T00:00:00) | ||||||||||
| x,y,z | position estimate, world frame (e.g. UTM) (meters) | ||||||||||
| covariance | position estimate uncertainty array:
|
The remaining fields are somewhat application-specific, and may be used in some TRN configurations. The complete poseT structure appears below.
struct poseT {
double x, y, z; // North, East, Down position (m)
double vx, vy, vz, ve; // Vehicle velocity wrto terrain (m/s) [1]
double vw_x, vw_y, vw_z; // Vehicle velocity wrto water (m/s) [1]
double vn_x, vn_y, vn_z; // Vehicle velocity wrto an inertial frame (m/s) [1]
double wx, wy, wz; // Vehicle angular velocity wrto an inertial frame (rad/s) [1]
double ax, ay, az; // Vehicle aceleration wrto an inertial frame (m/s^2) [1]
double phi, theta, psi; // 3-2-1 Euler angles relating Body frame to
// an inertial NED frame (rad).
double psi_berg, psi_dot_berg; // TRN states
double time; // Time (seconds since 01-01-1970T00:00:00)
bool dvlValid; // DVL measurement valid flag
bool gpsValid; // GPS measurement valid flag
bool bottomLock; // DVL bottom lock valid flag
double covariance[N_COVAR]; // Covariances: XYZ, phi, theta, psi, wy, wz
// (passively stable in roll) (see above units)
// [1] coordinates in Body Frame
poseT();
poseT& operator=(const poseT& rhs);
poseT& operator-=(const poseT& rhs);
poseT& operator+=(const poseT& rhs);
int serialize(char *buf, int buflen);
int unserialize(char *buf, int buflen);
};
measT
The measT structure is used for measurement update (bathymetry) input. In libtrnav, measurements are often referred to as beams; this stems from the fact that TRN bathymetry sensors are historically acoustic instruments, i.e. sonars and DVLs. They could, however, include LIDAR or other sensors. Here, measurement and beams are used interchangeably.
struct measT {
double time; // Measurement time (s)
int dataType; // Sensor type [1]
double phi, theta, psi; // 3-2-1 Euler angles relating Body frame to
// an inertial NED frame (rad).
double x, y, z; // North, East, Down position (m)
double* covariance; // covariance array [2]
double* ranges; // beam ranges [2]
double* crossTrack; // beam component perpendiclar to velocity [2]
double* alongTrack; // beam component parallel to velocity [2]
double* altitudes; // beam component downward [2]
double* alphas; // alpha values [2]
bool* measStatus; // beam measurement valid flag [2]
unsigned int ping_number; // measurement number
int numMeas; // number of beams
int* beamNums; // beam numbers (for sensors w/ variable beams) [2]
// [1] sensor type enumerations
// 1: DVL
// 2: Multibeam
// 3: Single Beam
// 4: Homer Relative Measurement
// 5: Imagenex multibeam
// 6: Side-looking DVL
// [2] array with dimension numMeas
measT();
measT(unsigned int nummeas, int datatype);
~measT();
void clean();
measT& operator=(const measT& rhs);
int serialize(char *buf, int buflen);
int unserialize(char *buf, int buflen);
};
On input, measT is passed into measUpdate, with these fields populated:
| Parameter | Description | ||||||||||||
|---|---|---|---|---|---|---|---|---|---|---|---|---|---|
| time | Time in posix epoch seconds (seconds since 1970-01-01T00:00:00) | ||||||||||||
| time | Time in posix epoch seconds (seconds since 1970-01-01T00:00:00) | ||||||||||||
| x,y,z | bathymetry sensor origin, world frame (e.g. UTM) (meters) | ||||||||||||
| phi,theta,psi | vehicle pitch, roll, heading, world frame (radians) | ||||||||||||
| dataType | sensor type
|
||||||||||||
| ranges | Beam range (length) array (meters) | ||||||||||||
| numMeas | number of beams (size of arrays ranges, crossTrack, etc.) | ||||||||||||
| crossTrack | beam range component perpindicular to direction or travel (m) | ||||||||||||
| alongTrack | beam range component in direction or travel (m) | ||||||||||||
| altitudes | beam range component down (m) | ||||||||||||
| measStatus | beam valid flag array (true if valid) | ||||||||||||
| ping_number_ | Bathymetry measurement sequence number (assumed to increase monotonically, > 0) | ||||||||||||
| beamNums | Beam numbers of ranges and components |
To recap, the TRN computation cycle is shown again below, in which TRN is provided with periodic motion and measurement updates (in that order), followed by a call to estimatePose to request a position estimate with uncertainties.
Fig: TRN cycle
The call to estimatePose requires a poseT (to contain the return value), and a measurement type:
void estimatePose(poseT* estimate, const int &type);
The measurement type argument is described below; in general, the MMSE value is used.
| Type | Description |
|---|---|
| 1: MLE | Maximum Likelihood Estimator, which maximizes the probability of observed data |
| 2: MMSE | minimum mean square error, which minimizes the expected squared difference between the estimated value and the true value |
In other words, MLE is more a peak (best single match), which is potentially an incorrect local maximum. MSE is the best/closest, across the range of estimates.
The covariance array returned has N_COVAR elements, the most commonly used of which are:
| Index | Description |
|---|---|
| 0 | x/N covariance |
| 2 | y/E covariance |
| 5 | z/D covariance |
| 1 | XY covariance |
Input streams may be decimated (sub-sampled) as needed. Sensor data streams are typically at different rates. Bathymetry inputs should correspond to the location and attitude associated with them. If data are buffered, be aware that updates should not use data that is too old, or for which there is significant temporal skew between motion and measurement updates.
Example TRN application: trn-player
A reference TRN application called trn-player is included in the libtrnav repository (libtrnav/opt/examples/trn-player).
trn-player provides a minimal demonstration of TRN:
- Configures a TRN instance
- Reads bathymetry, navigation, and attitude data from a log file
- Passes poseT and measT data structures to motionUpdate and measUpdate
- Calls estimatePose and sends TRN output to the console in CSV format (convenient for plotting), and to log files.
The contents are as follows:
| File | Description |
|---|---|
|
opt/examples/trn-player/TrnPlayer.hpp,h opt/examples/trn-player/TrnPlayer.cpp,h opt/examples/trn-player/TrnPlayerCtx.hpp,h opt/examples/trn-player/trn-player.cpp,h terrain-nav/TerrainNav.cpp,h |
trn-player source code |
| opt/examples/trn-player/pkg/trnplayer-sandbox.sh | generates sandboxed build for trn-player example |
Data and configuration files may be obtained from the libtrnav-data repository: https://bitbucket.org/mbari/libtrnav-data
| File | Description |
|---|---|
|
libtrnav-data/trn-player/data/csv libtrnav-data/trn-player/data/mbtrn libtrnav-data/trn-player/data/tnav |
TRN configuration files and data using different log formats. |
| libtrnav-data/trn-player/maps/PortTiles | Binary octree map |
| libtrnav-data/trn-player/trnplayer.cfg | trn-player configuration file |
The trn-player example builds with libtrnav; see build instructions above and/or use the trnplayer-sandbox.sh script to build the sandboxed version used below.
In this example, the following conventions are used:
- $TRN_BUILD : directory containing libtrnav (and mframe) repo(s)
- $LIBTRNAV : path to the libtrnav repo
- $MFRAME : path to the mframe repo
- $SANDBOX : path to directory containing libtrnav installation and libtrnav-data
These environment variables are used only to create a consistent frame of reference in the documentation and examples; they are not required for TRN or referenced by any of the software.
Running trn-player
These instructions show how to use trn-player using sample data that is available in the libtrnav-data repo. This example shows how to run mbtrn, one of several sets in libtrnav-data.
Clone the sample data repo into the $SANDBOX directory, if you haven't already done so:
cd $SANDBOX
git clone git@bitbucket.org:mbari/libtrnav-data.git
The $SANDBOX directory should contain:
bin
include
lib
share
libtrnav-data
Run TRN player using the example configuration file included with libtrnav-data:
cd $SANDBOX
# run trn-player using default settings and verbose output
./bin/trn-player --config=libtrnav-data/trn-player/trnplayer.cfg -v
# for help and other options use
./bin/trn-player -h
The default used above runs a data set from an MbTrn.log; the sample data also includes data from TerrainNav.log, TerrainAid.log, and CSV files (DVL, IDT, and multibeam formats).
To run other other data sets from the example config file, edit the config file, commenting out all but the selected configuration, then run again as above.
To process your own data, use the example configuration file and config file reference ($LIBTRNAV/sandbox/share/README-config.md) to create a new configuration file that references your data, maps, and configuration files, then run as above.
The trn-player default location for maps and data are ./maps and ./data; configuration files are assumed to be located in the data directory. the -c option specifies the config directory.
Alternatively, these methods may be used to reference trn-player inputs:
# Option 1: use data and map symlinks
Make symbolic links to the sample data maps and data:
ln -s libtrnav-data/trn-player/data/mbtrn data
ln -s libtrnav-data/trn-player/maps maps
# Option 2: use the trn-player options to set
# config (-c), data (-d), and map (-m) directories:
-c "libtrnav-data/trn-player/data/mbtrn"
-d "libtrnav-data/trn-player/data/mbtrn"
-m "libtrnav-data/trn-player/maps"
# Option 3: copy/rename the map, data directories to your directory
cp -R libtrnav-data/trn-player/maps maps
cp -R libtrnav-data/trn-player/data/mbtrn data
Example: process tnav example data without configuration files
The example data set in libtrnav-data/trn-player/tnav uses the TerrainNav.log and has a different vehicle specs file; if configuration file is not used, options must be passed on the command line to configure trn-player.
Using command line options only:
cd $SANDBOX
./bin/trn-player \
-d "libtrnav-data/trn-player/data/tnav" \
-c "libtrnav-data/trn-player/data/tnav" \
-m "libtrnav-data/trn-player/maps" \
-V "RDIequippedAUV_specs.cfg" \
-s 1 -l 1 -F -v
# -V : vehicle specs file name
# -s : sensor type DVL
# -l : log type TerrainNav.log
# -F : force beam status flags valid
# -v : verbose output
Or, assuming that symlinks are being used to point to maps and data, the tnav example data may be run by replacing the data symlink to point to the tnav directory and passing the appropriate options on the trn-player command line:
# from the build-trn-player sandbox
cd $SANDBOX
# change the data symlink to point to the tnav data set
rm data
ln -s libtrnav-data/trn-player/data/tnav data
# run with options to set the sensor type, log type
# and vehicle specs name. The -F option forces the
# beam status flags to valid.
./bin/trn-player -v -s 1 -l 1 -V "RDIequippedAUV_specs.cfg" -F
Though not strictly required, configuration files can simplify use in command line or scripted contexts.
The trn-player app supports configuration files consisting of key = value pairs, where keys are trn-player long options. In addition, legacy terrainAid.cfg syntax and options are also valid. terrainAid.cfg syntax requires lines to be terminated with semicolons; they are optional for trn-player. There is limited support for using environment variables in config files. A syntax summary is provided libtrnav share directory.
terrainAid.cfg options may be specified as long options to trn-player, e.g. --allowFilterReinits=true.
The following table summarizes trn-player options:
| trn-player | terrainAid.cfg | Argument | Description |
|---|---|---|---|
| -c, --cdir | - | directory string | config directory |
| -C, --config | - | path string | config file path |
| -d, --ddir | - | directory string | data directory |
| --dperiod | samplePeriod | long int msec | decimation period (msec) >0 : Decimates records to match specified input period as nearly as possible <=0 : Disabled (use all records) |
| -D, --debug | - | integer ≥ 0 | debug output |
| -e, --eofile | - | file name (w/o path) | estimate output file name |
| -f, --ftype | filter_type |
0: TRN_FT_NONE 1: TRN_FT_POINTMASS 2: TRN_FT_PARTICLE (default) 3: TRN_FT_BANK values other then 2 are experimental |
filter type |
| -F, --fstat | - | - | force beam status true (TerrainNav.log input format) |
| -G, --oflags | - | flags string (one or more) p: pretty m: measurement CSV e: estimate CSV q: quiet S: output MMSE L: output MLE B: output both MLE, MMSE |
output control flags |
| -h, --help | - | - | print help message |
| --interp | map_interp | 0: nearest-neighbor (none) 1: bilinear 2: bicubic 3: spline | map interpolation method (DEM maps only) |
| -i, --iformat | - | Types and default names 0: MbTrn.log 1: TerrainNav.log 2: TerrainAid.log 3: dvl.csv [1] 4: idt.csv [1] 5: mb.csv [1] [1] implies moformat = iformat |
input file format |
| -I, --ifile | - | file name (w/o path) | input file name (override lookup based on input format) |
| -m, --mdir | - | directory string | map directory |
| -M, --mfile | mapFileName | file name (w/o path) | map file name |
| -N, --nfile | file name (w/o path) | navigation log file name Only used w/ TerrainAid.log Note: terrainAid.cfg boolean option useDVLSide sets to dvlSide.log |
|
| -o, --odir | - | directory string | TRN output log directory |
| -O, --mofile | - | file name (w/o path) | measurement output file name |
| --pfomode | - | integer enum: 0: HISTOGRAM (distribution summary) 1: PARTICLES (all particles; large data volume) 2: NONE writes to odir/filterDistrib.txt |
log full particle states or a histogram Note: generates a large volume of data |
| -P, --pfile | particlesName | file name (w/o path) | particles file name |
| -r, --reinit | allowFilterReinits | boolean 0/1, N/Y, false/true | Allow TRN particle filter reinits |
| -s, --stype | - | integer enum 0 : UNDEFINED 1 : TRN_SENSOR_DVL DVL 2 : TRN_SENSOR_MB Multibeam/Generic 3 : TRN_SENSOR_PENCIL Single Beam 4 : TRN_SENSOR_HOMER Homer Relative Measurement 5 : TRN_SENSOR_DELTAT Imagenex DeltaT |
Bathymetry data format passed to measurement update May differ from sensor of origin Corresponds to terrainAid.cfg sensor_type |
| -v, --verbose | - | boolean 0/1, N/Y, false/true | Enable verbose output |
| -V, --vfile | vehicleCfgName | file name (w/o path) | vehicle spec file name |
| -w, --mweight | - | integer enum 0 : No weighting modifications 1 : Shandor's original alpha modification 2 : Crossbeam with Shandor's weighting 3 : Subcloud with Shandor's original 4 : Subcloud with modified NIS always on |
set modified weighting scheme |
| -x |
map_type |
0 : UNDEFINED 1 : TRN_MAP_DEM Digital Elevation Map (.GRD) 2 : TRN_MAP_BO Binary Octree Map (.BO) |
map file format |
|
-Z , --moformat |
- | integer enum 3: DVL CSV] 4: IDT CSV 5: MB/Generic CSV |
measurement output CSV file format (input file format enum) Use -Z, --moformat to override default (== iformat) i.e. to convert DVL, IDT to MB/Generic format No default name Use -Z, --mofile to enable measurement output, set file name |
| - | forceLowGradeFilter | boolean 0/1, N/Y, false/true | use low grade particle filter (rarely use/deprecated) |
| - |
dvlCfgName resonCfgName lrauvDvlFilename terrainNavServer terrainNavPort maxNorthingCov maxNorthingError maxEastingCov maxEastingError RollOffset useIDTData useMbTrnData useMbTrnServer |
terrainAidCfg parameters not used in TrnPlayer |
CSV Formats and Output Flags Details
trn-player output is sent to the console (stdout). Debug and error information is directed to stderr.
CSV file output is supported for measurements or estimates. Setting the measurement output file name (using -O, --mofile) and/or the estimated output file name (-E, --eofile) enables file output. If the input file uses a CSV format, the output format defaults to the same format; the output format may be overridden using -Z, --moformat, though not all conversions are supported. Any input format may be written to the multibeam/generic format. The format details are described below.
CSV file output may also be captured to a file using redirection, e.g.
trn-player [options...] -G e > path/to/file.csv
CSV estimate output format
When a TRN estimate output file is named (-E, --eofile), TRN estimates are written
to the specifed file in the output directory.
The CSV field format is as follows:
field indices
[ 1] mse time (epoch sec, double)
[ 2] mse N (m)
[ 3] mse E (m)
[ 4] mse D (m)
[ 5] mse vx (m/s)
[ 6] mse vy (m/s)
[ 7] mse vz (m/s)
[ 8] mse roll (phi) (rad)
[ 9] mse pitch (theta) (rad)
[ 10] mse rheading (psi) (rad)
[ 11] nav (pt) time (epoch sec, double)
[ 12] nav N (m)
[ 13] nav E (m)
[ 14] nav D (m)
[ 15] nav roll (phi) (rad)
[ 16] nav pitch (theta) (rad)
[ 17] nav rheading (psi) (rad)
[ 18] offset N (mse - pt) (m)
[ 19] offset E (mse - pt) (m)
[ 20] offset D (mse - pt) (m)
[ 21] covariance N (m)
[ 22] covariance E (m)
[ 23] covariance D (m)
[ 24] covariance magnitude (m)
CSV File Formats
Three CSV output formats are supported for input and output: DVL, IDT, and MB (multibeam/generic).
DVL CSV fields
field indices
[ 1] = time decimal epoch seconds
[ 2] = auvN northing (m)
[ 3] = auvE easting (m)
[ 4] = depth depth (m)
[ 5] = yaw heading/psi (rad)
[ 6] = pitch pitch/theta (rad)
[ 7] = roll roll/phi (rad)
[ 8] = 0 flag(0)
[ 9] = 0 flag(0)
[ 10] = 0 flag(0)
[ 11] = vx velocity x (m/s)
[ 12] = vy velocity y (m/s)
[ 13] = vz velocity z (m/s)
[ 14] = dvlvalid dvl valid (1: valid)
[ 15] = bottomlock has bottomlock (1: valid)
[ 16] = numbeams number of beams (4)
[ 17] = beam_number[i] beam number
[ 18] = beamStatus[i] beam valid (1:valid)
[ 19] = range[i] beam range (m)
...
(fixed number of beams (4), 3 fields per beam)
```
_IDT CSV Fields_
field indices [ 1] = time decimal epoch seconds [ 2] = auvN northing (m) [ 3] = auvE easting (m) [ 4] = depth depth (m) [ 5] = yaw heading/psi (rad) [ 6] = pitch pitch/theta (rad) [ 7] = roll roll/phi (rad) [ 8] = 0; flag(0) [ 9] = 0; flag(0) [ 10] = 0; flag(0) [ 11] = vx velocity x (m/s) [ 12] = vy velocity y (m/s) [ 13] = vz velocity z (m/s) [ 14] = dvlvalid dvl valid (1: valid) [ 15] = bottomlock has bottomlock (1: valid) [ 16] = numbeams number of beams (120) [ 17] = beam_number[i] beam number [ 18] = beamStatus[i] beam valid (1:valid) [ 19] = range[i] beam range (m) ... (fixed number of beams (120), 3 fields per beam) ```
MB CSV Fields
field indices
[ 1] = time decimal epoch seconds
[ 2] = auvN northing (m)
[ 3] = auvE easting (m)
[ 4] = depth depth (m)
[ 5] = yaw heading/psi (rad)
[ 6] = pitch pitch/theta (rad)
[ 7] = roll roll/phi (rad)
[ 8] = 0 flag(0)
[ 9] = 0 flag(0)
[ 10] = 0 flag(0)
[ 11] = vx velocity x (m/s)
[ 12] = vy velocity y (m/s)
[ 13] = vz velocity z (m/s)
[ 14] = dvlvalid dvl valid (1: valid)
[ 15] = bottomlock has bottomlock (1: valid)
[ 16] = numbeams number of beams
[ 17] = beam_number[i] beam number
[ 18] = beamStatus[i] beam valid (1:valid)
[ 19] = range[i] beam range (m)
[ 20] = alongTrack[i] along track (m)
[ 21] = crossTrack[i] across track (m)
[ 22] = altitudes[i] down (m)
...
(variable number of beams, 6 fields per beam)
```
#### oflags Description
The oflags parameter is a character string used to control
console output content and format. If multiple are specified, they may be interleaved on the console.
Defaults are 'pS' (pretty output with MMSE)
_p: pretty_
Formatted console output:
MMSE [t,x,y,z,s] : 63898130419.000, 4061708.231, 594373.492, 67.741, 1 OFS [x,y,z] : +39.921, +40.644, +25.505 COV [x,y,z,m] : 1.646, 1.675, 1.622, 2.855
_m: measurement CSV_
Measurement (TRN inputs) in CSV format specified by -Z, --moformat.
If unspecified, the input format is used for CSV types.
Example: --moformat=1 (DVL)
63898130270.000,4061675.360,594219.527,16.263,0.167,-0.192,0.396,0,0,0,0.100,0.000,0.000,1,1,4,0,1,115.917969,1,1,70.320312,2,1,77.679688,3,1,96.435547
_e: estimate CSV_
TRN Estimate in CSV format
Format fields: mse time (epoch sec, double) mse N,E,D (m) mse vx, vy, vz (m/s) mse roll, pitch, heading (phi, theta, psi) (rad) nav (pt) time (epoch sec, double) nav N,E,D (m) nav roll, pitch, heading (phi, theta, psi) (rad) offset (mse - pt) x, y, z (m) covariance x,y,z,magnitude (m)
_S: output MMSE in pretty output_
_L: output MLE in pretty output_
_B: output both MLE, MMSE in pretty output_
An undefined value (e.g. '?') may be used to suppress console output.
__About trn-player output__
Each time trn-player is run, TRN creates a directory, trn-player-TRN[.N] where N is an integer that is incremented for each session. Logs produced by the TRN session are stored there.
By default, trn-player outputs a TRN summary each time estimatePose is called:
MMSE [t,x,y,z] : 63898130388.000, 4061606.865, 594335.044, 44.745 OFS [x,y,z] : -58.695, +30.902, +6.336 COV [x,y,z,m] : 2.176, 2.542, 0.710, 3.420
TrnPlayerCtx has a number of flags that control output content and format, set with the -O option. For example, there are a couple of CSV formats that make plotting easier.
Use the -h option for a description of all trn-player options.
trn-player output is directed to stderr; TerrainNav also directs some output to stderr, e.g. when map tiles change.
#### trn-player Code Highlights
Configuring and running TRN is relatively simple, as shown in the code below.
In trn-player, a TrnPlayerCtx instance is used to hold configuration values (command line options are parsed to override defaults) and some shared variables. Apart from vehicle and sensor spec files, no environment variables or special configuration files are required to use TRN.
The TrnPlayer::configure() method captures the steps needed to configure a TRN instance:
```cpp
// Parse input from command line or config file
// configure TRN instance
// Replaces both ctx and trn
int TrnPlayer::configure(int argc, char **argv)
{
if(ctx != NULL)
delete ctx;
ctx = new TrnPlayerCtx();
// load command line options
int test = TrnPlayerCtx::parse(argc, argv, ctx);
if(ctx->verbose) {
show();
}
if(ctx->is_help_set) {
// show help and exit
ctx->show_help(basename(argv[0]));
return -1;
}
if(test != 0)
return test;
if(trn != NULL)
delete trn;
// configure a TRN instance
try {
if(ctx->is_particles_set == 0) {
trn = new TerrainNav(ctx->mpath, ctx->vpath, ctx->filter_type, ctx->map_type, ctx->odir);
} else {
trn = new TerrainNav(ctx->mpath, ctx->vpath, ctx->ppath, ctx->filter_type, ctx->map_type, ctx->odir);
}
} catch(...) {
fprintf(stderr, "%s ERR TerrainNav failed; check input directories and file names; use -v\n", __func__);
return -1;
}
// copy config file to output directory
copy_config();
trn->setFilterReinit(ctx->reinit_en);
trn->setModifiedWeighting(ctx->mod_weight);
trn->setMapInterpMethod(ctx->map_interp);
if(ctx->force_lgf)
trn->useLowGradeFilter();
else {
trn->useHighGradeFilter();
}
if(ctx->pf_omode != PFO_NONE)
trn->tNavFilter->setDistribToSave(ctx->pf_omode);
getSensorGeometry();
return 0;
}
The TrnPlayer::run() method demonstrates the TRN update and output cycle:
// main application logic:
// - configure a TRN instance
// - iterate over log records
// - fill in poseT, measT from log records
// - update TRN state using motionUpdate, measUpdate
// - get TRN estimate using estimatePose
// - output estimate
int TrnPlayer::run(int argc, char **argv)
{
// configure if needed
if(trn == NULL) {
int test = configure(argc, argv);
// return on error/help request
if(test != 0)
return test;
}
if(trn == NULL || ctx == NULL) {
// configuration error
fprintf(stderr, "%s - ERR trn (%p) or ctx (%p) NULL; call configure or pass options\n", __func__, trn, ctx);
return -1;
}
// open IO files
init_io();
// poseT, measT structs for TRN IO
poseT pt, mse, mle;
measT mt;
poseT *pt_set[3] = {&pt, &mse, &mle};
measT *mt_set[1] = {&mt};
// init measT (may be changed by record reader)
mt.dataType = ctx->sensor_type;
mt.numMeas = 4;
mt.ranges = (double *) malloc(mt.numMeas * sizeof(double));
mt.crossTrack = (double *) malloc(mt.numMeas * sizeof(double));
mt.alongTrack = (double *) malloc(mt.numMeas * sizeof(double));
mt.altitudes = (double *) malloc(mt.numMeas * sizeof(double));
mt.alphas = (double *) malloc(mt.numMeas * sizeof(double));
mt.covariance = (double *) malloc(mt.numMeas * sizeof(double));
mt.beamNums = (int *) malloc(mt.numMeas * sizeof(int));
mt.measStatus = (bool *) malloc(mt.numMeas * sizeof(bool));
// iterate over data records
// until EOF or error limit exceeded
do {
// ----- Read bathymetry and navigation data -----
// populate poseT, measT
status = getNextRecord(&pt, &mt);
rec_n++;
if(status == 0) {
// read valid
val_n++;
// udpate range stats
if(mt.ping_number < ctx->ping_range[0])
ctx->ping_range[0] = mt.ping_number;
if(mt.ping_number > ctx->ping_range[1])
ctx->ping_range[1] = mt.ping_number;
if(mt.time < ctx->time_range[0])
ctx->time_range[0] = mt.time;
if(mt.time > ctx->time_range[1])
ctx->time_range[1] = mt.time;
// ----- Update TRN -----
// call motionUpdate, measUpdate
// ensure motionUpdate valid before measUpdate
trn->motionUpdate(&pt);
trn->measUpdate(&mt, ctx->sensor_type);
// ----- TRN estimate -----
trn->estimatePose(&mse, TRN_EST_MMSE);
trn->estimatePose(&mle, TRN_EST_MLE);
// ----- do something with TRN -----
if( (ctx->last_meas = trn->lastMeasSuccessful())) {
// estimate valid
est_n++;
if (NULL != ctx->part_out) {
// write particle states
trn->tNavFilter->saveCurrDistrib(*ctx->part_out);
}
}
// write output
write_output(pt, mt, mse, mle);
reset_pt(pt_set, 3);
reset_mt(mt_set, 1);
} else {
// record read invalid
if(status < 0)
err_n++;
if(status == RI_DEC)
dec_n++;
}
} while(err_n <= MAX_ERRS && status != 1);
if(ctx->verbose) {
show_summary();
}
// release memory resources
delete trn;
trn = NULL;
mt.clean();
return (err_n >= MAX_ERRS ? -1 : 0);
}
Core TrnPlayerCtx data members
// Filter Type
// 0 : TRN_FT_NONE
// 1 : TRN_FT_POINTMASS
// 2 : TRN_FT_PARTICLE (always use 2)
// 3 : TRN_FT_BANK
int filter_type;
// Map Type
// 0 : UNDEFINED
// 1 : TRN_MAP_DEM Digital Elevation Map (.GRD)
// 2 : TRN_MAP_BO Binary Octree Map (.BO)
int map_type;
// Sensor Type
// 0 : UNDEFINED
// 1 : TRN_SENSOR_DVL DVL
// 2 : TRN_SENSOR_MB Multibeam
// 3 : TRN_SENSOR_PENCIL Single Beam
// 4 : TRN_SENSOR_HOMER Homer Relative Measurement
// 5 : TRN_SENSOR_DELTAT Imagenex multibeamconst int sensor_type=2;
int sensor_type;
// Use modified weigting
// 0 : TRN_WT_NONE No modification
// 1 : TRN_WT_NORM Shandor's original alpha modification
// 2 : TRN_WT_XBEAM Crossbeam with original
// 3 : TRN_WT_SUBCL Subcloud with original
// 4 : TRN_FORCE_SUBCL Force Subcloud every measurement
// 5 : TRN_WT_INVAL invalid
int mod_weight;
// allow filter reinitialization
bool reinit_en;
// Map interpolation method
// 0 : nearest-neighbor (no interpolation, default)
// 1 : bilinear
// 2 : bicubic
// 3 : spline
int map_interp;
// force low grade filter if true (use high grade otherwise)
bool force_lgf;
// Input Log
// 0 : MbTrn.log
// 1 : TerrainNav.log
// 2 : TerrainAid.log
// 3 : trn.csv
int log_type;
// CSV format
// 0 : CSV_DVL
// 1 : CSV_IDT
// 2 : CSV_MB
int csv_itype;
int csv_otype;
// Hack to force beam status valid
// for TerrainNav.log on LRAUV w/ RDI
bool force_status;
// mounting geometry parameters
// for input sensor_type, defined in vfile (vehicle spec)
// dr: euler angles phi,theta,psi (pitch,roll,yaw) (deg)
// dt: translation (x,y,z) (m)
double geo_dr[3];
double geo_dt[3];
// Output control flags (bitfield)
// -- Format Flags --
// OUT_PRETTY 0x1
// OUT_EST_CSV 0x2
// OUT_MEAS_CSV 0x4
// -- Content Flags --
// OUT_MMSE 0x100 (OUT_PRETTY)
// OUT_MLE 0x200 (OUT_PRETTY)
unsigned int oflags;
// Bathymetry log reader
DataLogReader *trn_log;
// Navigation log reader
DataLogReader *nav_log;
...
Note that only a few are needed to configure a TRN instance:
// configure a TRN instance
TerrainNav *trn = new TerrainNav(ctx->map, ctx->vspec, ctx->filter_type, ctx->map_type, ctx->odir);
Interpreting TRN Output: Estimates and Convergence
The poseT structure returned by estimatePose contains both an position estimate (position on the map, in world frame) and a covariance array that provides estimated uncertainties, as described above.
When uncertainty is low, it is said that TRN is converged, i.e. that there is some degree of confidence that it's position estimate is correct. There are common conditions under which uncertainties are very low and the location estimate is incorrect.
What conditions cause poor or invalid convergence?
TRN has an critical requirement that each update consists of independent observations. When overlapping TRN inputs are provided, the measurements potentially become correlated, which violates the requirement and potentially leads to invalid output.
As a guideline, TRN inputs should be spaced by at least ~3 m. For example, if a platform is moving at 1 m/s and using a sonar pinging at 3 Hz, there are 3 samples/m; every 9th sample would be ~3m apart. TRN implementation choices need to take into account vehicle speed, sensor sampling rates, and the information content of the terrain. Terrain that is flat or contains few distinguishing features contains less information that TRN can use to identify a unique location on the map.
Configuration, especially sensor and mounting geometry, ara another common source of issues with TRN. Ensure that offset parameters and rotations (DT, DR) are correct, and that the beam components have the correct sign and are on the correct axes.
Consistent failures in measUpdates are sometimes seen as errors in the TRN syslog indication that motion has not been initiated. This usually means that a valid motionUpdate has not occurred. There may be a number of reasons for this, and can mean that vx value is provided to motionUpdate is always zero.
How is it possible to tell whether TRN is converged, and the estimate is valid?
In typical applications, we are interested in determining a navigation offset (difference between estimated location and the location reported by navigation sensors), rather than an absolute location.
When TRN is converged, the offset is expected to be reasonable small (a few 10s of meters at most) and relatively stable (i.e., not highly variable in magnitude or sign). The covariances are also expected to relatively be small and stable, but not vanishingly small or constant.
What can be done if TRH is reporting false positives?
Instantaneous values are not generally reliable indicators of convergence. Instead, a common strategy is to track offset and covariance magnitude over a window, and only accept it as valid when convergence criteria have been within an acceptable range for a number of measurements, for example covariance magnitude 0.1 < x < 4.0 for 200 measurements.
Common issues and troubleshooting strategies
| Issue | Diagnostics |
|---|---|
| Not converging (in information-dense terrain) |
Check mounting geometry (TD,TR) and signs Check beam component magnitude, sign, directions Assess convergence criteria Ensure that TRN update rates are not too frequent or infrequent. Ensure that measurements are independent. |
| Converges quickly with suspiciously small covariances and/or high stability |
Reinitialize TRN Assess convergence criteria Ensure that TRN update rates are not too frequent or infrequent. |
| Offset magnitude sign opposite expected value |
Check mounting geometry (TD,TR) and signs Check beam component magnitude, sign, directions |
| TRN segfaults | Check trn-log Ensure that number of TRN_MAX_BEAMS and TRN_MSG_SIZE are adequate. |
Log files
TRN produces several log files that are especially useful for resolving issues.
TRN generates log files that include a message logs for debugging, and data logs containing input and output.
| Log File | Format | Contents |
|---|---|---|
| trn.log.nnnn | text | TRN message log (segmented, 0000-9999) |
| TrnIO.log | binary | TRN data log (multiple record types) |
| TerrainNav.log | binary | TRN data log (single record type, legacy) |
| TNavPFLog.log | binary | TRN particle state log (a lot of data) |
The TRN message log (trn-log) is a text file that provides a useful first line of diagnostic information. Though loosely structured, it provides a chronological output stream, and is easily read and searched.
The TrnIO log may contain multiple record types, identified with IDs:
MOTN_IN = motion update input
MEAS_IN = measurement update input
MSE_OUT = MMSE output
MLE_OUT = MLE output
TrnIO record types are dynamically sized, and contain minimal record headers and sync patterns, making them efficient in terms of storage and straightforward to search and parse, even if corrupted. The trnLog-player utility may be used to replay the TrnIO log, and produces a content summary along with output compatible with tlp-plot (also in libtrnav). The tlp-plot utility generates PDF plots of TRN output and performance.
The TerrainNav log uses a single record type to record (MLE, MMSE) outputs and inputs, which is less efficient; it writes all fields, including arrays (which are statically sized to TRN_MAX_BEAMS), whether they are used or not. Also, record type is ambiguous, and do not contain any sync patterns.
Logs are written to the directory passed to TerrainNav constructor, along with configuraton files used to configure the TRN instance.