The Joys of Smoothing Are B. Willumsen and Øyvind Hegrenæs Kongsberg Maritime Strandpromenaden 5 9 Horten, Norway Abstract-Navigation post-processing or smoothing is the process of optimizing all navigation estimates based on the entire measurement set. For underwater vehicles this gives much smoother and more accurate estimates than what is obtained in real-time. The enhanced quality of the smoothed estimates is shown in this paper by examples from autonomous underwater vehicles (AUVs), remotely operated vehicles (ROVs) and tow-fish, using HUGIN or HAIN navigation systems. Also shown are the many valuable uses of smoothed data, like fault detection, fault identification, system calibration, and parameter identification. The only drawback of smoothed data is the fact that they are not available online, but at a lag. The profit of smoothing with reference to the time smoothed over (lag) is investigated for the test cases. I. INTRODUCTION Inertial navigation is becoming increasingly popular on underwater vehicles. The lower price of inertial platforms, the higher requirements for the vehicles, and the need to go into deeper waters have all made inertial navigation common on many vehicles such as remotely operated vehicles (ROVs), autonomous underwater vehicles (AUVs) and tow-fishes. In maritime and underwater inertial navigation the mathematical process of smoothing is often frowned upon by ignorant users. This is because the word itself is often misinterpreted as the act of manually making noisy data look nice. Many users therefore often refer to this process as navigation post-processing instead. Smoothing represents mathematical equations that provide optimal estimates based on the entire measurement set. The estimates are generally much smoother and do not have sudden jumps, as is often the case with real-time estimates. Since smoothing bases the estimates at a given time on both measurements before and after that given time, the estimates are unavailable in real-time. This is the only drawback of working with smoothed estimates. In all other aspects of navigation they are superior to their real-time counterparts. Kalman filtering has emerged as one of the primary ways of doing data fusion in navigation. The filter itself assures optimality given that the system is linear. Most navigation equations are only close to linear, but the development of the extended Kalman filter has mainly mitigated this problem (see [] or other text books on Kalman filtering). The Rauch-Tung-Striebel algorithm (RTS) is a common approach for calculating the smoothed estimates. This way of doing smoothing may be thought of as forward-backward way of carrying out the estimation. Firstly, the traditional Kalman filter real-time equations going forward, and secondly, a backward sweep on all the forward estimates. The smoothing process is therefore often perceived as doing Kalman filtering backward in time. The forward sweep can be done in both realtime and post-mission. See [] and references therein for details. RTS is the method used throughout this paper. A. Case studies The application of smoothing is illustrated using data from each of the mentioned vehicles: AUV, ROV and tow-fish. The missions represent typical operational examples, and as such case studies. All the vehicles are fitted with an underwater navigation suite consisting of an inertial measurement unit (IMU), a Doppler velocity log (DVL) with bottom-track and a pressure/depth sensor. The tethered vehicles, and often also AUVs, have frequent updates from an ultra short base-line acoustic positioning (USBL) system HiPAP. The USBL measurements are merged with DGPS from the surface ship to provide a global position measurement. The AUV is also always fitted with its own GPS, making position updates at the surface possible. For additional information and overview of INS aiding tools the reader may refer to []. The data of both ROV and tow-fish is recorded with HAIN Subsea []. All processing is done using NavLab [4].. AUV mission An AUV is in general a very stable platform, well suited for carrying sophisticated payload. As for the HUGIN AUVs, they may operate in either supervised mode (monitored from the mother ship) or autonomous mode (independently of the ship). In the latter case, the vehicle autonomously executes the survey as specified in a predetermined mission plan, before returning to a desired pick-up location. The example data used in this paper were collected when running in autonomous mode, with surface GPS as the only position aiding.. ROV mission An ROV may be quite erratic in its behavior, and does in general have motion in all degrees of freedom. The trajectories of the ROV may be quite different based on the application, which may be anything from surveying to construction and between. In the example considered in this paper the ROV went straight forward at a depth exceeding m. This is typical motion in ROV surveying and pipeline inspection.. Tow-fish mission A tow-fish is generally moving faster than an ROV. Because of the speed, the tension of the long cable, and design of the fish, they often have substantial pitching motion. In the example the tow-fish followed a straight line motion at a speed slightly below m/s and at a depth of approximately m.
II. POSITIONING ACCURACY All vehicles will in general benefit from smoothing, though the benefits may vary for different vehicles based on their navigation setup, type of motion and so forth. In regards to positioning aiding, smoothing is particularly effective and tractable when the measurements are sparse. The accuracy of the navigation system estimates is usually specified by their standard deviation, and hence used as a measure of the quality of the estimates. One is often spoiled on the quality of depth estimates as the pressure sensor is always reliable. However it is shown in [5] that smoothing has very good effect on depth estimates in the wave zone. For the tethered vehicles, only the standard deviation of one of the two horizontal directions is shown. In missions where position updates are frequent, the two axes will exhibit a very similar development of the standard deviation. Various examples illustrating the benefits of smoothing and it impact on obtained position accuracy are given below. A. AUV The trajectory in shows the trajectory followed by the AUV. The vehicle surfacing was carried out about - s apart. The effect of doing smoothing in post-processing is evident in Figure. Note that the smoothed and real-time estimates are always identical at the end..8.6 x 4 Position relative to start of navigation (,). HUGIN INS position Start point GPS surface fix Figure Standard deviation of horizontal position for AUV mission B. ROV The quality of the navigation is shown in Figure. One can clearly see the improvement of the smoothing. Standard deviation one axis (m).5.5 North (m).4..8.6.4. End of mission Start of mission - -.5.5 East (m) x 4 Figure AUV mission with GPS surface fix, September. 4 6 8 4 6 8 Figure Standard deviation of horizontal position for ROV mission Investigating the measured positions reveal the smoother results of smoothed estimates Figure 4 and Figure 5. The figures show the deviations of a thought trajectory traveling at constant speed along a straight line from first to final real-time estimate. This odd way of plotting them is used to more clearly show the differences of the two estimates.
6.5 Smoothed 5 Smoothed Across-track position (m).5.5 Standard deviation one axis (m) 4 -.5 5 5 5 5 5 5 5 4 45 5 Figure 4 Across-track position estimates ROV Figure 6 Estimated standard deviation of position for tow-fish 4 Along-track position (m) - - - Cross track position (m) - -4-4 -5-6 -6 5 5 5-8 5 5 5 5 4 45 5 Figure 5 Along-track position of the ROV Figure 7 Across-track positions of tow-fish C. Tow-fish The tow-fish is submerged at a depth more than double that of the ROV. This leads the position measurements being both less frequent and of lower quality. Both these factors contribute to a less accurate position. The one-axis estimated standard deviation of position is shown in Figure 6. The across-track error is shown in Figure 7. Again there is a substantial deviation between the real-time and the smoothed estimates. As always the initialization of the INS must be taken into consideration up to at least s. Following s the performance is almost steady-state. III. PERFORMANCE VERIFICATION The precision of the estimates of the ROV mission is verified by having the ROV going along the exact same path but in opposite direction. This is shown in Figure 8. The results in Figure 8, verifies that the standard deviations shown in Figure are reliable.
Across-track distance (m).5.5.5 -.5 - -.5-5 5 5 5 5 4 45 Along-track distance (m) Figure 8 ROV positions back and forth forth Smoothed forth back Smoothed back gain by smoothing more than seconds. The reason for the difference in appearance of the ROV and the tow-fish, may rest upon both the difference motion and the greater depth of the tow-fish mission. Ratio of standard deviation.4..8.6.4. Horizontal speed Horizontal position Heading Pitch IV. OPTIMAL SMOOTHING TIME The tethered vehicles will always have all the measurements available in real-time, and the operator will also have all estimates available in real-time. This is not possible for AUVs as this requires more than the available data rate. HAIN Subsea provides smoothed estimates in semi-real-time (HAIN PP), guarantied to be available within a specified lag after the valid time. Fully smoothed estimates (only achievable in postprocessing) are always better than the HAIN PP but having this information available almost in real-time might prove quite significant. It doesn t help to know exactly where you were a long time afterwards if it wasn t where you were supposed to be. Given such functionality it is useful to try optimizing the lag time. The lag should be as short as possible in order to get the information early, but as long as possible to ensure quality. In Figure 9 the ratios of the standard deviations are shown. It uses real-time as the denominator, which means that the smaller the ratio, the greater the benefit of smoothing. The biggest lag time possible is the length of the mission of approximately s. At that lag the real-time system is initializing, which results in very high standard deviations before it has settled. Therefore such improvements will only occur on points in time at start-up and not after the system has settled. On speed estimates the effect of smoothing dies out quickly, whereas position and pitch seem to take about 5 s. So based on Figure 9, there is little to gain from using a lag longer than 5 s, with most of the benefit being achieved after 5 s. It is seen that s is not enough time for the heading estimate to settle completely. This is shown in Figure. It should be noted that his test is not enough to determine optimal lag for heading at steady state behavior of the INS. It does however clearly visualize the possible time saving of smoothing as one does not need to wait for heading to settle before starting a mission. Smoothed estimates will provide accurate estimates also in that time frame. The ratio of standard deviations of the tow-fish mission is shown in Figure. The graph indicates that there is little to Standard deviation ( ) Ratio of standard deviation 5 5 5 Lag (s) Figure 9 Ratio of standard deviations (smoothed/real-time) for ROV.9.8.7.6.5.4... 5 5 5.9.8.7.6.5.4... Figure Standard deviation of heading ROV mission Horizontal speed Horizontal position Heading Pitch 4 5 6 Lag (s) Figure Ratio of standard deviations (smoothed/real-time) for tow-fish
V. FAULT DETECTION The performance requirements of underwater navigation systems are increasing. This again puts high demands on each individual sensor, on the mounting and assembly, and finally the usage of them. Detecting a faulty sensor that is producing no or very bad measurements is usually quite straightforward, and the operator is usually able to see this before the navigation system is considered or checked OK. With the high performance requirements today however, the user has to detect if a sensor or set-up is just slightly off its specifications. A frequently applied approach for detecting faults is by examining the estimated sensor errors. If they prove to be outside their specification this is usually an indication that something is wrong. Typical limits of such tests could often be -5 times the specified standard deviation. If estimated sensor errors are outside that limit, one interprets the system as being in error, and further analysis is required. In Figure the standard deviations of the estimated biases on x-axis accelerometer are shown. The values are scaled to the specifications of that accelerometer. This is taken from the ROV mission. The estimates show proportionally how well the error is estimated. This can also be interpreted as an estimate on how well the system is able to estimate the error, where a value of indicates that one is not able to estimate the error. The smoothed estimated errors are more reliable, and are thus better suited for detecting errors..5 In Figure 4 an example of a system not functioning properly is shown. The accelerometer bias in the x-axis is scaled to its specified standard deviation. If one had used -5 times standard deviation as limit in this, the real-time data are only outside for small intervals. The smoothed ones though, are outside almost all of the time. For the record, the reason for this error is the lack of DVL calibration of the system. This is determined by doing a calibration. DVL calibration and in particular the results of this one is explained in Section VII. This also serves as a warning when doing fault identification in an INS. The sensor with a big error may not be the one causing the problem. Proportional error.5.4... -. -. -. -.4 -.5 5 5 5 Figure Estimated x-accelerometer bias scaled to its standard deviation Proportional standard deviation.95.9.85.8.75 Proportional error 7 6 5 4.7 5 5 5 Figure Standard deviation of x-accelerometer bias estimates scaled to specification Examining the estimated errors in Figure, one see that real-time and smoothed differ quite substantially. The figure also indicates that the system is working as expected, since the values are all inside.5 times the specified standard deviation of the bias. Figure show a large discrepancy between the estimates. This indicates that errors will have bigger chances of being concealed in the less accurate real-time estimates. - 4 5 6 Figure 4 Estimated x-accelerometer bias scaled to standard deviation of uncalibrated tow-fish mission VI. FAULT IDENTIFICATION Detecting errors are often quite easier than identifying the cause of them. An example of such was shown in Section V. In Section VII a trial-and-error approach identifies the cause as
lacking DVL calibration. Although not shown here, one could do a more systematic approach by doing smoothing of the navigation data, leaving out the sensors one at a time. This can lead to the navigation indicating error on all the smoothed sets, except for the one that has the faulty sensor left out. VII. DVL CALIBRATION Already touched upon in Section V, the DVL calibration of the system is vital in achieving the desired navigation quality. References [6] and [7] describe a method of integrating the DVL speed measures and gyro direction reading to obtain position estimates. The rotation matrix from DVL to gyro is then obtained by a least squares on these estimates and position measurements by LBL. Having a full scale INS on board the vehicle however makes it possible to do the comparison in the speed instead of the position domain. This is especially the case when one has the smoothed estimates available. The INS uses the depth sensor, acoustic positioning and IMU to calculate the full navigation estimates, and they include the speed of the vehicle at any given moment of the mission. We propose an error model of the DVL given by () DVL INS () t = α ( t t ), () v R v DVL INS d where v () t is the speed measured by the DVL; α is a scale DVL factor error typically caused by speed of sound error; R INS is INS the rotation matrix between DVL and INS; v is the true speed of the vehicle, given in INS frame; t is the time reception of the measurement; t d is the delay from the time the measurement was valid to the time of reception. Taking the absolutes of the velocities of equation () and using the INS estimates yields equation (). () = α ˆ ( ). () v t v t t DVL INS d In () v ˆINS denotes the INS estimated smoothed speed. Based on () the delay t d and scale factor α can be calculated by means of correlation and least squares respectively and successively. Once these are calculated one can solve for R by using the least squares estimation found in [8] on DVL INS (). The INS smoothed velocities are again used instead of the true velocities when doing the calculations. We will use the tow-fish as en example in this case. As explained in Section V, the navigation indicates an error and one therefore tries to do a calibration. Figure 5 shows the INS-estimated velocities, based on all sensors but the DVL. Assuming that the smoothed one is the most correct estimate, we see there is a substantial difference of the two estimates, and one should therefore always use the smoothed for this type of work. The reader should note that the great depth of meters makes for infrequent position updates from the acoustics. In shallower water the more frequent position aid would give a more stable and accurate real-time speed than the one shown in Figure 5. This may lead to the real-time speed being of adequate quality, but the smoothed speed will always be better. The calibration process results in corrections in both delay, scale factor and yaw angle. The other angles are just marginally improved. Examining the estimates before and after applying the corrections on the same set as was used in the calibration is not the best way to test the quality of the calibration. However examining the errors is interesting as they indicate that with the new calibration values there are no longer any indications of the system being in fault. This is shown in Figure 6. Velocity (m/s) Proportional error.5..5..5 -.5 -. -.5 -. 5 5 Figure 5 INS estimated across-track speed of tow-fish in calibration.8.6.4. -. -.4 -.6 -.8 4 5 6 Figure 6 Estimated x-accelerometer bias scaled to standard deviation of calibrated tow-fish mission
VIII. PARAMETER IDENTIFICATION The deviations from the smoothed estimates are shown in Figure 7. It shows the typical high degree of white noise in DGPS-HiPAP, and the lot smoother nature of the real-time estimates Cross track distance (m) - - DGPS-HiPAP - 5 5 5 Figure 7 Deviations from smoothed of ROV mission A pragmatic look at smoothed positions is that the smoothed position rely proportionally more on the IMU measurements than the real-time ones. The real-time ones can be regarded as a filtered version of the position measurements; whereas the smoothed ones are less affected by variation in the position measurements. Therefore plotting the difference of these two should yield a look at the medium term errors. This shows in Figure 7. This can be used in for instance finding parameters that describe the errors in the DGPS-HiPAP. Looking at Figure 7, one could argue that there exist some small oscillations of about.5 meters in the DGPS-HiPAP. The amplitude and time period of these oscillations should be used as parameters in the Kalman Filter. Sometimes this information could be hidden within the high degree of white noise in the DGPS-HiPAP measurements. In such cases the real-time s deviation from the smoothed might prove valuable to inspect. However a thorough mathematical analysis of the DGPS-HiPAP deviation would probably tell the same story. IX. CONCLUSIONS Smoothing of data is very valuable in underwater navigation, yielding significant improvement in both quality and robustness. The equations of smoothing are analytically proven optimal. This paper has taken a practical look at some applications of smoothing. The suggestions here are based on practical experience. A more thorough and analytic examination might give more insight and possibly better methods than the ones suggested in this paper. Besides significant quality improvement of the data one should also not forget the greater robustness, e.g. as shown in [5]. One may argue that the AUV, ROV and the tow-fish data sets are too much alike in terms that their all exhibiting very much straight line motion. However it has been proven in [9] that the straight line is the worst to navigate for inertial navigation systems, and therefore the most interesting to judge performance by. Navigation quality is a direct result of the quality of the sensors. The results here must thus be taken as guiding in the sense that using different sensors might give significantly different results. For various reasons sensor types and qualities are omitted in this paper. ACKNOWLEDGMENT We thank EMGS for allowing the use of and providing the data from the tow-fish mission. REFERENCES [] R.B. Brown and P.Y.C. Hwang, Introduction to Random Signals and Applied Kalman Filtering, rd ed., John Wiley & Sons, 997 [] P. E. Hagen, Ø. Hegrenæs, B. Jalving, Ø. Midtgaard, M. Wiig, and O. K. Hagen, Making AUVs truly autonomous, in Underwater Vehicles. Vienna: I-Tech Education and Publishing, January 9. [] R. Marthiniussen, J.E. Faugstadmo, and H.P. Jakobsen, HAIN: an integrated acoustic positioning and inertial navigation, in OCEANS '4. MTTS/IEEE TECHNO-OCEAN '4, vol., November 4, pp. 6-68. [4] K. Gade, NavLab, a Generic Simulation and Post-processing Tool for Navigation, in European Journal of Navigation, vol., num. 4, November 4, pp. 5-59. [5] A.B. Willumsen, O.K. Hagen, and P.N. Boge, Filtering Depth Measurements in Underwater Vehicles for Improved Seabed Imaging, in OCEANS 7 Europe, June 7, pp. -6. [6] J.C. Kinsey and L.L. Whitcomb, Towards in-situ calibration of gyro and Doppler navigation sensors for precision underwater vehicle navigation, in Robotics and Automation,. Proceedings. ICRA '. IEEE International Conference on, vol. 4,, pp. 46-4. [7] J.C. Kinsey and L.L. Whitcomb, In Situ Alignment Calibration of Attitude and Doppler Sensors for Precision Underwater Vehicle Navigation: Theory and Experiment, in Oceanic Engineering, IEEE Journal of, vol., issue, April 7, pp. 86-99. [8] S. Umeyama, Least-squares estimation of transformation parameters between twopoint patterns, in Pattern Analysis and Machine Intelligence, IEEE Transactions on, vol., issue 4, April 99, pp. 76-8. [9] B. Jalving, K. Gade, O.K. Hagen, K. Vestgard, A toolbox of aiding techniques for the HUGIN AUV integrated inertial navigation system, in OCEANS. Proceedings, vol., September, pp. 46-5.