Inertial navigation for divers

The Study Group captured data using an Android device strapped to a skateboard to simulate the type of movements a diver might make underwater. Different modes of movement were evident from filtered versions of the sensor outputs, so indicating that a pedometry based solution ought to be feasible. The Study Group then formulated and investigated the feasibility of a generic dead-reckoning system. Although sensors provide more data than is strictly necessary, significant errors arise from imperfect calibration and from noise for which the Study Group derived estimates of the resulting drift in position over time. The accuracy of practical numerical integration schemes in the context of rotating frames was investigated, and a Kalman filter was used to reduce error in the orientation data by combining accelerometer and gyroscopic data.


Executive Summary
A SCUBA diver would like to know his/her position underwater, relative to the dive start position.GPS is not an option as the signals do not travel underwater.Transponder/pinger systems have already been developed, but these are expensive and require configuration by the boat operator and the diver.
A simple standalone affordable dead-reckoning system is needed, but it is inherently difficult to determine the diver's movements.The system needs to be very cheap, so that price is not a barrier to its deployment.The system needs to be small.The Study Group investigated the possibility of such a system making use of cheaply available accelerometer, gyroscope and magnetic field sensors, similar to those included in many modern smart phones.
The Study Group captured data using an Android device strapped to a skateboard to simulate the type of movements a diver might make underwater.Different modes of movement were evident from filtered versions of the sensor outputs, so indicating that a pedometry based solution ought to be feasible.The Study Group then formulated and investigated the feasibility of a generic dead-reckoning system.Although sensors provide more data than is strictly necessary, significant errors arise from imperfect calibration and from noise for which the Study Group derived estimates of the resulting drift in position over time.The accuracy of practical numerical integration schemes in the context of rotating frames was investigated, and a Kalman filter was used to reduce error in the orientation data by combining accelerometer and gyroscopic data.(1.1.1)A SCUBA diver would like to know his/her position underwater, relative to the dive start position.This requires a gyro system optimised for underwater use, utilising low cost hardware sensor inputs.GPS is not an option as the signals do not travel underwater.Transponder/pinger systems have already been developed, but these are expensive and require configuration by the boat operator and the diver.
(1.1.2)A simple standalone affordable dead-reckoning system is needed, but it is inherently tricky to determine the diver's movements.The system needs to be very cheap, so that price is not a barrier to its deployment.The system needs to be small. (1.1. 3) The study group sought to address the following questions: how should the position be calculated and what accuracy can be obtained?This led to considerations of which characteristics of motion are important.
(1.1.4)A further consideration was to think about the likely sources of errors and deviations from idealised behaviour.Some technical information on these device outputs is provided by Anscamobile, who market the Corona SDK for mobile platform development [2].Some basic information on MEMS type accelerometers, including the likely source of error is covered by a Texas Instruments briefing note [3].
(1.1.5)As a practical focus for the study group, a modest land-based solution was implemented in an android app.As accelerometers and compasses are standard in so many phone/tablet models, this was deemed to be a suitable test platform for practical ideas.
2 Understanding the available data 2.1 Feasibility of using data from an Android device (2.1.1)To establish the feasibility of the proposed methods, an Android device was pushed along the floor on a skateboard and recorded data for the acceleration a and angular velocity ω.This data was extracted from the device and was used as input data to solve equations (1) to (3) numerically using MATLAB.Results are shown in figures 1 and 2, and provide an encouraging demonstration of the feasibility of the methods. (2.1. 2) The device was placed flat on the skateboard, with the z axis aligned approximately downwards.The movement along x and y axes is shown, with initial condition (0,0).The units are normalised, and we consider  only qualitative features of the calculation.In Figure 2, x and y are approximately aligned to the corridor width and length respectively, while in Figure 1, the device is placed approximately at a 45 degree angle to the corridor length.Both figures show roughly straight lines as expected, with Figure 2 demonstrating more deviation after a longer period of time.

Movement scenarios and data captured
(2.2.1)In order to generate realistic data for analysis, an iPhone app was used to log data streams from the iPhone's inertial sensors, namely the accelerometers, gyroscopes and magnetometer.A number of different motion scenarios were used, with the device attached to a skateboard; the device hand-held while walking; and the device at rest on the window sill of the building (in order to estimate background noise in the sensors).
(2.2.2) With the iPhone attached to the skateboard, two sub-scenarios were used: the skateboard moved with an obvious push and allowed to decelerate noticeably between pushing actions and the skateboard moved at a near constant speed.
(2.2.3)In total, three data sets were captured for the skateboard with each of a pushing motion, the walking experiment, and the iPhone at rest.Two data sets were captured for the skateboard at a (near) constant speed.

Making sense of the data
(2.3.1)The strongest signal in the captured accelerometer data was that due to gravity.This signal may play an important role in determining an absolute reference frame, but for the study group we considered it a nuisance signal.
A bandpass filter with a characteristic shown in Figure 3 was applied to the three accelerometer axis signals.The characteristics of this filter were that the passband be between 0.5Hz and 4Hz.This was chosen on the basis that anything below 0.5Hz was likely to be dominated by the gravitational signal and that anything above 4Hz might well be noise.
(2.3.2)An example application of this filter to one of the skateboard pushing datasets resulted in the axial accelerations in Figure 4. Compass heading information from the magnetometer is included in this figure also (although this signal has not been filtered).As can be seen, each of the accelerometer readings is centred around zero; this is to be expected as the filter has suppressed the constant signals in the accelerometer readings.
One can also see impulses in the acceleration signals, as might be expected by the nature of the scenario (the skateboard being pushed and allowed to slow down noticeably between pushes).The acceleration peaks in both the positive and negative directions indicate maximum acceleration and deceleration.(2.3.3)Next we considered the magnitude of the accelerations, i.e.
The results of this are in Figure 5 for all 11 datasets captured, although their number is obscured by noise in the data.Colour coding is used to distinguish static (green), walking (red), skateboard being pushed (blue) and skateboard at a near-constant speed (black).
(2.3.4)In order to see these magnitudes more clearly, a low pass filter with a cutoff of around 0.5Hz was applied to the magnitude outputs.The response of this filter is shown in Figure 6.The resulting smoothed magnitudes are shown in Figure 7. Figure 4: Accelerometer readings in the x, y and z directions, filtered with the bandpass filtered described in Figure 3.Note the different scale on the z-acceleration component.Heading information from the magnetometer is also included, showing how directional information changed as the orientation of the device changed throughout the data capture experiment.

Counter-intuitive filtering artefacts
(2.4.1)The filtering adds some artefacts that may make the acceleration plots of Figures 5 and 7 seem incorrect.In particular, the acceleration does not hit the baseline value (i.e. the 'at rest' acceleration magnitude corresponding to a = 0) periodically, as might be expected for the push scenarios1 .This is due to the removal of high-frequency terms in the filtering.Refer back to Figure 4 to see that the accelerations do indeed oscillate between positive and negative values.Colour coding is used to distinguish static (green), walking (red), skateboard being pushed (blue) and skateboard at a near-constant speed (black).
(2.4.2) Both the lowpass and bandpass filters are 100 point FIR filters, designed using R's fir1 command.The sampling rate of the iPhone accelerometers is around 15Hz.Therefore it can be readily seen that the extended transient spikes in the first 10-15 seconds are caused by these filter settle times.
2.5 Towards a V1.0 dead-reckoning system In particular, there is a clear oscillation in the blue signals corresponding to the pushing action on the skateboard (blue); the near-constant scenario on the skateboard (black) is not quite as constant as walking (red), which might be expected; there is a consistent baseline in the static signal (green).
(2.5.2) These observations suggest that different modes of movement can be distinguished in the accelerometer readings using fairly simple techniques.
As a minimum, it should be possible to estimate diver speeds using accelerometer readings and to combine this with magnetometer readings to develop a dead-reckoning system based on pedometery principles.Colour coding is used to distinguish static (green), walking (red), skateboard being pushed (blue) and skateboard at a near-constant speed (black).Time is in seconds.
3 Coordinate frames and equations of motion 3.1 Reference frames, measurements and objectives The problem has two frames of reference: the fixed (global) reference frame for known earth-based quantities such as the magnetic field vector and the gravitational acceleration vector; and the diver's reference frame for quantities measured by a device attached to the diver. (3.1. 2) The two frames of reference are depicted in Figure 8.A device orientation matrix relates the coordinates of a point expressed in the diver's reference frame to its coordinates in the global reference frame.
(3.1.3)In the diver's local coordinate frame, the device can measure the following quantities: a(t) -acceleration vector from accelerometers; ω(t) -angular velocity vector from gyroscopes; b(t) -magnetic field vector from sensor; and h(t) -depth from pressure transducer.
(3.1.4)The constant magnetic field of the earth B and gravity −g Ẑ are fixed in the global coordinate system.
(3.1.5)The objective is to estimate the following quantities: X(t) -diver position in global coordinate system; U (t) -diver velocity in global coordinate system; and M (t) -device orientation matrix (orthogonal rotation).

Formal mathematical relationships
(3.2.1)In an idealised world, this problem may be solved by integrating the rate equations for the device orientation matrix M , and for the diver velocity U and position X in the global reference frame, using the accelerometer readings a and gyro readings ω as input vectors: (3.2.2) Two additional equations relate data (magnetic field and depth) in the diver's frame to the corresponding data in the global frame: (3.2.3)A reasonable plan then is to numerically integrate (1-3) and to use (4)(5) to correct as much of the noise and drift errors as possible.A scheme for using the magnetometer information to correct errors in the rotation matrix is outlined in Appendix C.
4 Theoretical treatment of errors 4.1 Noise and the integration of the circular motion (4.1.1)Noise in the measured data can lead to the build up of error during the integration of the equations of motion.To gain insight into this effect, a simple case of circular motion, expressed in the notation of the global and diver reference frames was defined as follows. (4.1. 2) The initial conditions of motion were U(0) = (0, 1, 0), ( 7) and random noise was added to the constant angular velocity and acceleration that define circular motion: Here, n(t) and m(t) are vectors whose entries are random numbers drawn from a standard normal distribution, N (0, 1).

Estimating the drift in position
(4.2.1)An analytical estimate for the drift in position arising from random noise in the acceleration data can be obtained for linear motion as follows.The analysis is a summary (in one dimension of linear motion) of the more detailed analysis given in Appendix A.  (4.2.3)We integrate ẍ = a using a simple Euler scheme to find cumulative errors in x at time t = nδt: So for large n, the standard deviation of position error is approximately which grows with time.For = 10 −3 this gives σ ≈ 45 m after 30 minutes, and σ ≈ 125 m after 1 hour, so illustrating the significance of noise.
(4.2.4)The Study Group did not complete an analysis of the effects of angular velocity measurement noise upon the estimation of orientation.The analysis would proceed along similar lines to that above and in Appendix A.
4.3 Numerical integration schemes and their effects (4.3.1)In a real time implementation, MATLAB integration routines (e.g.ode45) cannot be used, rather low-order and fast integration schemes must be explicitly coded and executed in real-time software.An understanding of how best to implement such schemes and of their accuracy and execution speed is required.The Study Group examined the performance of first and second order in time schemes, and the management of error accumulation in the context of circular motion. (4.3. 2) The Study Group proposed a similar approach to that proposed by Woodman (2007) in [4], seeking to compute C(t) = C(0) exp( t 0 Ω(τ )dτ ) where C = M T and Ω is the skew symmetric form of the angular rate vector ω.(4.3.3)Given a numerical time step δt, this matrix can be approximated, as proposed in [4], by using σ = δt|ω| and with the angular velocity values corresponding to the interval (t, t + δt).
(4.3.4)Then, we can compute an approximation of the velocity and trajectory by using the backward Euler scheme (first order accurate in time), as in [4] or use the following second-order accurate time discretization scheme: and then, (4.3.5)This is a well-known scheme that is used in many applied numerical works (see, for instance, [5]).Results for a circular trajectory are shown in Figure 10.This scheme has also been found to work for an ellipse.34) in [4]) is incorrect because 3D rotations do not commute.For example, if Ω = Ω 1 for an interval ∆ 1 t, followed by Ω = Ω 2 for an interval ∆t 2 , then that has a different rotation matrix from that obtained had Ω = Ω 2 for ∆t 2 been followed by Ω = Ω 1 for ∆t 1 .However, the exponential formula would give the same result both ways because integral t 0 Ω(τ )dτ would be the same either way.Even the equation C(t + δt) = C(t) exp( t+δt t Ω(τ )dτ ) (i.e equation (35) in [4]) would seem to have errors of the order of δt 2 because of non-commutativity, so reducing equation (41) of [4] to being no better than the forward-Euler formula C(t + δt) = C(t)(1 + Ω(t)δt).
(4.3.7)The Study Group reviewed the quaternionic representation of 3-D rotations as means of accommodating non-commutativity and of correcting the rotation matrix to ensure its orthogonality during the course of numerical integration.The quaternionic representation is briefly described in Appendix B. The Study Group did not have time to implement the quaternionic scheme, but it should be implemented and tested in subsequent work.
5 Improving the estimates of the motion: correlation and filtering 5.1 Kalman filtering of compass and gyroscope data (5.1.1)Determining an accurate orientation of the diver with respect to the global coordinate system is just as important as determining accurately the magnitudes of the displacements.If the displacements of the diver are in the wrong direction, this will negate the utility of the navigation device.
(5.1.2)There are different and independent measurements available that are able to provide information about the orientation of the diver, including: the gyroscope, which provides angular velocity ω; the accelerometer, which provides orientation relative to the direction of gravity g; and the compass, which provides orientation relative to the direction of the earth's magnetic field b.
(5.1.3)All these device measurements contain some amount of error.We propose to use a Kalman filter to combine all three measurements and get one optimal estimate of the three angles φ, θ and ψ that determine the rotation matrix R.
(5.1.4)The measured gravity vector can be obtained by low pass filtering the acceleration readings to filter out the non-gravity components.The same idea can be applied to calculate the rotation matrix for the measured magnetic force vector and the known magnetic field vector.Once the rotation matrix is known then the calculation of the three angles φ, θ and ψ is trivial.
(5.1.5)The Kalman filter matrices are derived in a similar way to [6].The currently implemented Kalman filter is a discrete and linear Kalman filter.This works well for the 1D case, however if we wanted to implement the full 3D system we would prefer an extended Kalman filter to better handle the nonlinearity in the equations.For a good introduction to the Kalman filter see [7].The only difference between our method and the method proposed by [6] is (11).Here we used a finite difference to estimate the time derivative.
x(t + 1) = 1 ∆t 0 1 (5.1.6)Some interesting plots showing how the Kalman filter improves the estimates of the diver orientation by combining compass and angular velocity measurements can be found in Figures 11 and 12.   6.1.1)In this Study Group, we captured data using an Android device strapped to a skateboard to simulate the type of movements a diver might make underwater.Different modes of movement (non-movement, constant speed, impulsive movement) are evident from filtered versions of the sensor outputs, so indicating that a pedometry based solution ought to be feasible.
(6.1.2)We also investigated the feasibility of a more generic dead-reckoning system.Though sensors provide more data than is strictly necessary, noise and imperfect calibration lead to significant errors.Owing to the nature of the problem, these errors accumulate over time leading to significant drift errors over the time scales of interest.Using estimates of the white noise produced by the accelerometers we derived estimates of the drift in position over time.The accuracy of practical numerical integration schemes was also investigated.By combining accelerometer and gyroscopic data we showed how a Kalman filter can be used to reduce error in the orientation data.

Figure 1 :
Figure 1: Skateboard moving approximately in a straight line.

Figure 3 :
Figure3: The pass band response, stopband response and phase response of the bandpass filter used to suppress gravity and higher frequency noise.Frequency is in Hz.

Figure 5 :
Figure5: Accelerometer magnitudes for each of the 11 data sets captured (overlayed and partially obscured in noise), following prefiltering of axial components using the bandpass filter of Figure3.Colour coding is used to distinguish static (green), walking (red), skateboard being pushed (blue) and skateboard at a near-constant speed (black).

( 2 . 5 . 1 )
Each of the transportation modes of Figure7can be distinguished by eye.

Figure 6 :
Figure6: Lowpass filter with a cut-off of 0.5Hz applied to the acceleration magnitudes, chosen to display the coarse features of the motion more clearly.

Figure 7 :
Figure7: Smoothed accelerometer magnitudes for each of the eleven data sets captured (see text for details).Colour coding is used to distinguish static (green), walking (red), skateboard being pushed (blue) and skateboard at a near-constant speed (black).Time is in seconds.

Figure 8 :
Figure 8: The fixed and diver reference frames and their notation.

( 4 . 1 . 3 )
The above equations were numerically integrated in MATLAB, for four different regimes of added noise; the results of this investigation are shown in Figure9.They show how random noise introduces systematic drift into the integrated solutions of the equations of motion. .

Figure 9 :
Figure 9: Trajectories showing the drift caused by the effect of random noise upon the numerical integration of the circular motion equations (9) and (10).

Figure 10 :
Figure10: Top: Results for method described in Woodman (2007)[4] (order 1).Bottom: Results for second order method described above.In both cases we consider time steps 0.1 and 0.01.

Figure 11 :
Figure 11: First example showing how the Kalman filter improves the estimates of the diver orientation by combining compass and angular velocity measurements.