Академический Документы
Профессиональный Документы
Культура Документы
NAV
NAVLLAB,
AB a Generic Simulation and Post-processing Tool for Navigation
SCIENCE
The ambition of getting one common tool for a - The estimator is a flexible aided inertial naviga-
great variety of navigation tasks was the background tion system, which makes optimal Kalman filtered
for the development of NavLab (Navigation Labo- and smoothed estimates of position, attitude and
ratory).The main emphasis during the development velocity based on the available set of measure-
has been a solid theoretical foundation with a strin- ments.The measurements can be either from the
gent mathematical representation to ensure that simulator or from real sensors of a vehicle.
statistical optimality is maintained throughout the
entire system. NavLab is implemented in Matlab, and This structure makes NavLab useful for a wide range
consists of a simulator and an estimator. of navigation applications, including research and de-
velopment, analysis, real data post-processing and as
- Simulations are carried out by specifying a tra- a decision basis for sensor purchase and mission
jectory for the vehicle, and the available types of planning. NavLab has been used extensively for mass-
sensors. The output is a set of simulated sensor production of accurate navigation results (having
measurements post-processed more than 5,000 hours of real data
Figure 1 - NavLab main structure. Note: The colours used in the figure correspond to the colours of the graphs
generated by the different parts of NavLab (blue is the measurement, red is the smoothed estimate etc).
in four continents). Vehicles navigated by NavLab in- or together, allowing a variety of applications. A list
clude Autonomous Underwater Vehicles (AUVs), Re- of usages is given in Section 4.
mote Operated Vehicles (ROVs), ships and aircraft.
The simulator can simulate artificial measurements
from a chosen scenario. The estimator will, based
1 Introduction
on the available set of measurements from either
For many navigation related activities it is very useful the simulator or from sensors of a real vehicle,
to have one common software tool.The tool should make the best possible estimates of position, atti-
cover applications such as navigation system research tude, velocity and sensor errors. The simulator and
and development, analysis and real data post-pro- estimator are described in more detail in Sections
cessing. With a long tradition of developing naviga- 2 and 3.
SCIENCE
tion systems, The Norwegian Defence Research Es- In addition to the simulator and estimator, NavLab
tablishment (FFI) started development of such a tool includes:
in 1998. The result is NavLab (Navigation Laborato- - A pre-processing tool (Preproc), which is used to
ry), a powerful and versatile tool that serves a vari- handle real measurements (by removing outliers,
ety of navigation purposes. For the long-term success compensating for lever arms and misaligned sen-
of this tool, a strong focus on a solid theoretical foun- sors, converting measurements to the correct
dation and a flexible structure has been crucial. format etc)
- An export tool, which creates files for exporting
1.1 NavLab’s theoretical foundation to other programs (containing the estimated po-
The most significant feature of NavLab is its solid sition, attitude and velocity).
theoretical foundation. NavLab is a result of an in-
novative research process to establish a complete- Figure 2 shows the NavLab program modules. Dif-
ly general theoretical basis for navigation and for ferent modules are used in different cases.Typical
implementation of navigation systems.The develop- examples are:
ment has led to the following contributions: - Simulations: Simulator → Estimator
- A new stringent and unified system for notation - Post-processing of real data: Preproc → Estima-
and mathematical representation tor → Export
- A unified design and implementation of algo-
rithms and aiding techniques for the Kalman filter, The modules interface each other via files of a
where statistical optimality is maintained through- specified format (see [2]), or via memory to save
out the entire system time.
- Elimination of numerical problems by
- Deducing and implementing exact formulas
2 Simulator
(rather than approximations)
- Using only non-singular representations The trajectory simulator can simulate any vehicle
- Controlling accumulation of the computer’s in- trajectory specified by the user. In addition, the user
herent round-off errors. specifies a set of available sensors and their charac-
teristics. Based on the specified trajectory and sen-
Articles reporting the above work will be pub- sor characteristics, the sensor simulators calculate a
lished, but currently the most relevant report avail- set of artificial sensor measurements.
able is [1].
2.1 Trajectory simulator
1.2 A flexible structure The coordinate systems I (Inertial), E (Earth), L
The main structure of NavLab is shown in Figure 1. (Local) and B (Body) are simulated (see [2] for def-
NavLab’s different components can be used alone initions). All relevant positions, orientations, linear
SCIENCE
The most significant error types, such as white-
noise, coloured noise and scale factor error are in-
cluded in the sensor simulators, and any other types
can also be added. The magnitude, time-constants
and other parameters that describe the different
errors are user selectable, and can be given as fixed
values or as functions of time.
locity as a function of time. When developing a tra- gyrocompass optimally as a natural part of its esti- velocity of B relative to
jectory simulator, the actual mathematical quantities mation procedure. Earth, differentiated in the
that are used to describe these changes must be B system
selected carefully, to ensure that it is simple for the The main structure of the estimator is given in Fig-
user to express a trajectory that follows the Earth ure 4. Measurements from the IMU (Inertial Mea- 2
If future measurements are
ellipsoid in both position and attitude. Selecting the surement Unit) are integrated by the navigation available, a better estima-
mathematical quantities1 ω BLBBLB and v• EB
BB
EB actually makes equations (see Section 3.1) to calculate position, at- tor exists, see Section 3.2
this just as simple for the user as it would have titude and velocity. Each time-step where a meas-
been if the surface of the Earth were planar.Thus if urement from any of the aiding sensors is available,
no changes in these quantities are specified, the ve- it will be compared to the corresponding quantity
hicle will travel around the Earth at constant from the navigation equations, and the difference is
depth/height if the initial velocity was horizontal. sent as a measurement to the Kalman filter.
Figure 3 shows an example trajectory from the Note that each of the sensors shown in Figure 4
simulator. This trajectory is simply specified by two are general and can represent different types, e.g.
NavLab has used different types of position meas- - Direction cosine matrix attitude update
urements, including range measurements to a - Numeric drift control
known position (see [5] or [6] for examples of dif- - WGS-84 gravity model and elliptic Earth model
ferent sensor types that have been integrated). - Trapezoid updates to prevent systematic errors
from the forward or backward Euler methods.
The navigation equations and optimal smoothing
are described in Sections 3.1 and 3.2. 3.2 Optimal smoothing
The Kalman filter is the optimal estimator at time t,
Features: when measurements before and including t are
- The estimator accepts arbitrary time-series of used, thus it is well suited for real-time estimation.
measurements from all sensors However, if measurements after t are also available
SCIENCE
- Along with each single sensor measurement, new (which is the case for post-processing, see Section
sensor parameters can be specified, describing 4.1), it is possible to make a better estimator at
that particular measurement, hence describing a time t, by using these additional measurements.The
varying quality best possible algorithm, utilising all measurements
- Zero Velocity Update (ZUPT) and depth/height both before and after t, is called optimal smoothing
measurements are included in the same Kalman (see [4] for details).
filter in an optimal manner - This algorithm is effectively doubling the set of
- The horizontal position measurements are non- relevant measurements for each estimate, since
singular (i.e. with maximum accuracy also near/at the next x seconds of measurements are nor-
the poles) mally just as important as the previous x seconds
- Iterated Extended Kalman filter is used to im- - A symmetrical interval of past and future meas-
prove the performance in cases of significant urements prevents a systematical delay in the es-
nonlinearities. timates, which is unavoidable in real-time estima-
tors
3.1 Navigation equations - Another limitation of an optimal real-time esti-
The navigation equations calculate position, attitude mator (Kalman filter) is its inability to deliver es-
and velocity based on the IMU measurements, as timates that are in accordance with the process
shown in Figure 4. model. At each time-step such estimators make
a prediction (that is in accordance with the
Features: process model), but when a new measurement
- Non-singular for all positions and attitudes arrives, it is weighed against the prediction to give
- Foucault wander azimuth a new updated estimate. Unexpected3 measure-
Figure 4 - Estimator main structure (simplified). The sensors shown can be either simulated or real. (INS: Inertial
Navigation System).
SCIENCE
ture measurements are available and the two algo-
rithms give equal estimates.
4 NavLab Usage
NavLab has been extensively used by numerous dif-
Figure 6 - Trajectory from the HUGIN 1000 AUV. The track
ferent users since 1999, including several internation-
shows the vehicle going northwest. Blue: position
measurement from DGPS+USBL (differential GPS + Ultra al research groups, universities and commercial sur-
Short Base Line acoustic positioning). Green: Kalman filtered vey companies. The flexible structure of NavLab
estimate. Red: Smoothed estimate. makes it useful for a wide range of applications. Some
users are only working with simulated data, whereas be to stay covert, avoid interference with other
others use the estimator alone to post-process real systems or just to save power)
data. Finally, there are many cases where both simu- - Going to areas where certain measurements are
lations and real data processing are of interest. A available or are more accurate (e.g. go close to
summary of current NavLab usage is given below. bottom to get DVL bottom track, go close to a
transponder or go to surface for GPS measure-
Navigation system research and development ments)
(using simulations and real data) - Running manoeuvres to increase the observabil-
- Development, testing and comparison of new ity in the estimator
navigation concepts and algorithms, including - Running in patterns that cancel out error growth
new aiding sensors and aiding techniques
SCIENCE
- Development of real-time navigation systems, When setting up complex mission plans, simulations
where the algorithms are implemented and test- are helpful to ensure effective missions that meet the
ed in NavLab, and then ported to the real-time navigation accuracy requirements for all parts of the
system. A typical development process is: mission (transit phase, mapping phase etc).
- Implement algorithms in NavLab
- Test in simulations (NavLab) Post-processing of real navigation data (using
- Test with real data (NavLab) real data)
- Port algorithms to the real-time navigation sys- Post-processing of real data improves the navigation
tem (C++ or similar program language) accuracy, robustness and integrity compared to a real-
- Test real-time system. time navigation solution. See 4.1 for more details.
The real-time navigation system in the HUGIN ve- Tuning of real-time and post-processing
hicles was developed using NavLab (see [5] for a navigation systems (using real data)
description of the real-time navigation system and Proper Kalman filter tuning is essential for optimal
[7] or [8] for an overview of the HUGIN AUV Pro- estimation accuracy. Tuning is often based on the
gramme). sensor specifications, but the actual sensor per-
formance can differ from these numbers, and in
Analysis of a given navigation system (using such cases the tuning should be based on empiri-
simulations and real data) cal data. Finding the correct tuning based on a
- Analysis of navigation system behaviour under recorded data set is best done by means of the
different manoeuvres/trajectories and sensor error estimates from the smoothing algorithm.
configurations
- Robustness analysis. The performance of the es- Sensor evaluation (using real data)
timator is studied for the cases of: After purchasing a new sensor, an evaluation of the
- Wrong sensor models used in the Kalman filter sensor is usually desired. Large sensor errors might
- Sensor dropouts be detected by inspecting the measurements from
- Sensor errors. this sensor alone, but for a more thorough sensor
evaluation, the measurements should be compared
Teaching navigation theory (using simulations) with other sensors (with uncorrelated errors) or a
By specifying appropriate simulations, everything known reference. Running a relevant mission or lab
from basic principles to complex mechanisms can test and analysing the result in NavLab will usually
be demonstrated and visualised. reveal errors above the specification and often also
the characteristics of such errors.
7
For instance a surface ve- Decision basis for navigation sensor
hicle measuring the AUV selection/purchase (using simulations) Improving sensor calibration (using real data)
position by means of Simulations of the relevant scenarios are carried Even if a sensor is approved in an evaluation, it can
DGPS+USBL. A full set of out to investigate how varying quality of the differ- exhibit systematic errors, typically due to imperfect
measurements is not ent sensors will affect the obtainable navigation calibration or misaligned mounting. Such (determin-
transmitted to the AUV in performance. Parameters for different sensors avail- istic) errors should be removed before sending the
real time, but is available able in the market are usually entered for compar- measurements to the estimator, otherwise the per-
for use in NavLab after the ison.The goal is to achieve a well-balanced and eco- formance will be reduced (in particular for the real-
mission nomical sensor suite. time Kalman filter). To find these systematic errors,
the smoothing algorithm should be used, as it is sig-
Decision basis for mission planning (using nificantly better than the real-time filter at estimating
simulations) such errors.When systematic errors are known, they
Even if the set of sensors is given, the navigation ac- can be compensated for in future missions.
curacy can vary significantly with the mission type.
Important mission parameters include: 4.1 Using NavLab for real data
- Activation/deactivation of sensors or change of post-processing
measurement rate (reasons to deactivate might For vehicles storing their navigation sensor meas-
SCIENCE
still obtaining the required accuracy. For instance, a
submerged vehicle might need to surface to get
position measurements. In Figure 5, we see that
with a position accuracy requirement of 5 metres,
the real-time filter would require position measure-
ments after a period of 2,500 seconds, while with
Figure 7 - A wellhead is mapped repeatedly with
different headings to evaluate the positioning smoothing a position accuracy better than 5 metres
accuracy of the final map. is obtained even with a 2 hours dropout interval.
real-time Kalman filter (see Figure 6 and Figure parameters and steps are
8). In addition, the possibility of rerunning the es- fully controllable, to "One-
timation increases the ability to recover a faulty click" [10] where all processes are automated. In Sci-
data set.To do so, one can modify either the de- entific mode, a general multi-menu based plot func-
graded sensor measurements or the filter tuning tion is used after a simulation or estimation. This
(or both) to get the best possible navigation for function plots a range of figures containing numeri-
the faulty data set cal summaries and many different 2D and 3D plots
- The Integrity of the estimator, i.e. the ability to with a total of more than 500 graphs, for results
detect degraded sensor performance and de- analysis. The plot function is also programmable to
graded total navigation performance, is critical show only a predefined subset of plots for users
for the users of the navigation data.The optimal wanting just a simplified summary of the results.The
smoothing algorithm has a very high capability very simplest output is used in the One-click mode,
of detecting reduced sensor quality. In addition where a green/red light at the end of the estimation
it can often tell which sensor is having problems. indicates if the data was OK or not.
are from the simulator, but since the real world has tioning of the final map is essential, and estimates of
infinite complexity, it is impossible to use a com- the vehicle’s 6 degrees of freedom (position and at-
pletely correct model in the estimator when using titude) are used to position the bathymetric data.
real data. In cases where the model used by the es- Estimation errors in vehicle position will be direct-
timator differs from the model generating the ly translated to errors in the map position, while
measurements, the actual estimation error will be the effect of attitude errors will depend on the
larger than the theoretical limit. The most challeng- geometry between the vehicle and a given patch of
ing part of the estimator development is to keep the seafloor. A crucial test of the entire navigation
its error as close as possible to the theoretical limit system is to verify the position accuracy in the final
in cases of modelling errors (and nonlinearities). To maps. In such tests, all available aiding sensors are
minimize the loss of accuracy, a very careful design used so that the maximum accuracy is evaluated.
and implementation of all parts of the estimator is
vital. In this section it is demonstrated that it is pos- Customers buying HUGIN and NavLab for detailed
sible to achieve a performance close to the optimal seabed mapping have had a strong focus on position
under a range of different non-ideal conditions. accuracy of the maps and have thus run navigation
performance trials as part of the customer accept-
5.1 Verifying performance using the simulator ance tests. These trials determine if the real-life per-
The simulator, having a more complex nonlinear formance of the estimator match the accuracy that
system model than the estimator, is an effective tool was predicted in NavLab simulations before the ve-
for verifying the estimator performance. Any sce- hicle was built. The standard method is to map the
nario can be tested and different modelling errors same object at the seafloor several times, comparing
can be used. After running the estimator, the plot the position estimate of each individual object obser-
function will calculate and plot the true estimation vation. Errors that are uncorrelated between each
error and compare it with the theoretical estima- passing will be visible, as the object will be positioned
tion uncertainty (also Monte Carlo simulations can differently in each observation. Correlated errors are
be run to determine the statistics of the error). typically following the AUV or a ship giving
Thorough and extensive testing of the estimator DGPS+USBL measurements (e.g. timing problems,
since 1999 by different research groups, testing a systematic velocity error and misaligned acoustic po-
variety of scenarios, has proven the estimator to be sitioning transducer). Hence, to also reveal these er-
very robust and to give close to optimal perform- rors, different headings are used for the AUV and ship
ance in all scenarios. for each passing (‘wagon wheel pattern’, as shown in
Figure 7). Figure 7 shows maps from HUGIN 3000 in
5.2 Verifying performance using real data an accuracy test carried out by the HUGIN customer
The ultimate test of the estimator is to use real data C&C Technologies at 1,300 m water depth in the
from a representative mission, where the trajectory Gulf of Mexico in October 2000. 11 different head-
and all sensor errors are (by definition) totally realis- ings were used (5 of the lines were mapped in op-
tic.The challenge with real runs is that it is more dif- posite directions) when mapping the object (a well-
ficult to investigate the estimation errors, since the head), to maximise the visibility of any correlated
true trajectory is unknown. However, some possibili- errors following the AUV or ship.The positions of the
ties do exist, and these are discussed in the following. wellhead observations when using NavLab smooth-
ing are shown in Figure 8, obtaining an accuracy of 1.2
5.2.1 Redundant sensors m and 1.7 m (1σ) north and east (even with a large
A significant sensor measurement can be made un- unmodelled DVL error present, see Section 3.2.1).
available for the navigation system, and later be The theoretical estimation uncertainty in the
used as a reference. For instance, a surface ship smoothed position was about 1.7 m (1σ, north and
might follow a submerged AUV, continually meas- east) during the passings. 60 m from the wellhead, but
uring its position using DGPS+USBL, but not send- within the swath width, another object (natural fea-
ing the measurements to the AUV. The AUV, typi- ture) was also visible in the data. Since the object is
cally using an IMU, a depth sensor, a DVL and in 60 m off the centre of the maps, a somewhat higher
some cases a compass, will have a drift in position uncertainty is expected due to the AUV heading un-
SCIENCE
and east), and in general the tests have proven ex- Plugin: Waypoint editor, FFI (to be published)\
ceptionally good estimator accuracy, even slightly
better than the anticipated theoretical uncertainty [4] Minkler, G and Minkler, J (1993): Theory and
limit. The reason for this is a combination of the Application of Kalman Filtering, Magellan Book
navigation sensors performing somewhat better Company
than their specifications and the estimator produc-
ing close to optimal estimates. [5] Jalving, B, Gade, K, Hagen, O K and Vestgard, K
(2003): A Toolbox of Aiding Techniques for the
HUGIN AUV Integrated Inertial Navigation
6 Conclusions
System. Proceedings from Oceans 2003, Sep-
NavLab is a powerful and versatile tool with usage tember 22-26, San Diego, USA
ranging from research and development by scien-
tists and academics, to mass production of high-ac- [6] Jalving, B, Bovio, E and Gade, K (2003): Inte-
curacy maps by commercial companies (having grated inertial navigation systems for AUVs
post-processed more than 5,000 hours of data for REA applications. Proceedings from
from around the world). MREP 2003, NATO SACLANT Undersea
Research Centre, May 2003, Italy
Even when a real-time navigation system is avail-
able, it is often beneficial to post-process the data [7] Hagen, P E, Størkersen, N and Vestgård, K
with NavLab: (2003): The HUGIN AUVs - multi-role capa-
- The navigation results, i.e. estimates of position, bility for challenging underwater survey oper-
attitude and velocity, will be more accurate and ations. EEZ International, Summer 2003
smooth (no jumps in the data)
- The navigation results will be more reliable (any [8] The HUGIN AUV Programme homepage:
critical sensor errors are detected) www.ffi.no/hugin
- Even in cases of sensor degradation or failure, ac-
curate navigation can often be obtained (no [9] Mathworks homepage: www.mathworks.com
need for a new mission). This is due to the in-
creased robustness of smoothing and the possi- [10] Svartveit, K (2004): NavLab One-Click, FFI (to
bility to rerun the data be published)
- Lower quality navigation sensors might be used,
while still obtaining satisfactory navigational accuracy. [11] Jalving, B, Gade, K, Svartveit, K, Willumsen, A
and Sorhagen, R (2004): DVL Velocity Aiding
The most significant feature of NavLab is its theo- in the HUGIN 1000 Integrated Inertial Navi-
retical foundation, where statistical optimality is main- gation System. Proceedings from ADCPs in
tained throughout the entire system. This has been Action 2004, June 3-4, Nice, France
repeatedly demonstrated through extensive per-
formance verifications, both with simulations and real
Biography of the Author
missions. These tests have proven very high estima-
tor performance, close to the theoretical optimum. Kenneth Gade is MSc from the Norwegian Uni-
versity of Science and Technology (NTNU), De-
partment of Engineering Cybernetics. He works
7 Acknowledgements
as senior scientist at the Norwegian Defence Re-
The author wishes to thank the rest of the Naviga- search Establishment (FFI), where he has been
tion Group at FFI, Bjorn Jalving in particular, for sig- developing integrated navigation systems since
nificant contributions to the development of 1996.The main focus has been GPS independent
NavLab. Also thanks to Bjorn Jalving, Kongsberg (underwater) navigation, where the challenge is
Maritime and HUGIN customers for organising, to find optimal aiding techniques from a variety
carrying out and reporting a range of different tests of sensors to bind or minimize the positional
of the navigation system accuracy. drift.● Kenneth Gade
Copyright © 2005 GITC bv, P.O. Box 112, 8530 AC Lemmer,The Netherlands, www.gitc.nl.