Tracking of Sinking Underwater Node using Inertial Navigation

(1)

Västerås, Sweden

Thesis for the Degree of Master of Science in Engineering - Robotics

30.0 credits

TRACKING OF SINKING

UNDERWATER NODE USING INERTIAL

NAVIGATION

Kristoffer Lindve

kle16002@student.mdh.se

Examiner: Nikola Petrovic

Mälardalen University, Västerås, Sweden

Supervisor(s): Ivan Tomasic

Mälardalen University, Västerås, Sweden

Company Supervisor(s): Isaac Skog

Swedish Defence Research Agency (FOI), Kista, Sweden

(2)

Abstract

Tracking objects underwater is prone to error since no equivalent system to GPS exists for underwater applications. Accurate positioning is vital for conducting surveillance. However, it is hard for a system to keep an accurate prediction while moving without a GPS source. Noise in sensor measurements causes drift which quickly leads to significant errors unless these errors can be successfully identified and removed. This study researches how to track the position of a portable platform that, at least in theory, should have the capability to be dropped into the ocean and track its own position. The initial position is known, but as soon as the node is dropped into the water, then this position can be seen as old. When dropped, the node starts moving with the current until coming to a complete stop somewhere on the ocean floor. A prediction of the position is given by combining multiple sensors in an extended Kalman filter aided by zero velocity updates. Once the node becomes stationary, smoothing is applied with prior information to improve the initial sensor bias estimates. This is done over several iterations allowing the initial biases to converge. The results indicate that this method could improve the prediction of the position estimate on the ocean floor.

(3)

Acknowledgements

I want to express my appreciation for a couple of people who helped during this project. First and foremost, I want to thank my supervisor at FOI, Isaac Skog. He provided guidance and expertise throughout the progress of my thesis. He was also essential in the planning process and in defining the scope of the thesis.

My gratitude also goes to Frej-Eric Emmoth at FOI, who assembled the hardware. He played a crucial part in getting the hardware working and in conducting the experiments. I also want to thank Stefan Wångerud at FOI for helping with many practical aspects of this project and all the others at FOI who contributed somewhere along the process.

Last but not least, I am thankful to Ivan Tomasic, my supervisor at MDH, for advising and helping with the structure and language in this report.

(4)

3. Method 13 3.1 Hardware . . . 13 3.1.1 Sensors . . . 13 3.1.2 System . . . 13 3.2 Software . . . 15 3.3 Proposed Solution . . . 16 3.3.1 Sensor Fusion . . . 16 3.3.2 Smoothing . . . 16 3.4 Experiment Procedure . . . 17 3.4.1 Data Collection . . . 18 3.4.2 Magnetometer Calibration . . . 20 3.4.3 Experiment . . . 20 4. Implementation 22 4.1 Simulated Data . . . 22 4.2 Inertial Navigation . . . 23 4.2.1 Sensor model . . . 24 4.2.2 State Vector . . . 24 4.2.3 Orientation Representation . . . 24 4.3 EKF Implementation . . . 25 4.4 Motion Model . . . 26 4.4.1 Jacobian . . . 27 4.5 Measurement Models . . . 27 4.5.1 GPS . . . 28 4.5.2 Depth Sensor . . . 28 4.5.3 Magnetometer . . . 28

4.5.4 Zero Velocity Updates . . . 28

4.5.5 Observation Matrix . . . 29

5. Results 31 5.1 Simulation Results . . . 31

(5)

6. Discussion 35 6.1 Simulation . . . 35 6.2 Experiment . . . 35 6.3 Solution Summery . . . 36 7. Conclusions 37 7.1 Future Work . . . 37 References 38 Appendix A 40

(6)

1. Introduction

Underwater localisation has been a well-researched topic in recent decades. With oceans covering over 70 per cent of the earth’s surface [1], there is a need for accurate tracking while discovering and exploring the oceans. Estimating the position above sea level is a well-researched topic, with many viable and practical solutions for all types of environments. This, in turn, has resulted in widespread adoption of tracking systems in modern technology to the point where most cars have a built-in navigation system and almost all phones have a tracking system. In 2019, an estimated 6.4 billion GPS systems were in use, and this number is only projected to increase with time [2].

Research into underwater robotics started during the 1950s [3], well before modern technology had emerged and localisation was a complicated problem. Since then, many new methods have been developed, however, it is still difficult to achieve an accurate localisation of submerged vehicles.

Advances in recent years has been possible as a result of the evolution of microchip technologies. Improvements in inertial measurement sensors technology have contributed to increasing accuracy and reduction in cost, especially for systems where the ability to use external position sensors is limited. Underwater localisation almost always includes inertial navigation systems (INS), where internal motion sensors (such as accelerometer and gyroscope) are used to measure the movement of the platform. Inertia measurements from an inertial measurement unit (IMU) are commonly used with Kalman filtering, which combines the data to predict position and movement. However, this prediction is often only accurate for a short period of time. Errors from the inertial sensors accumulates which makes estimations unreliable after a short period, thus improvements in the filtering process are widely researched.

Many new technologies have also risen to prominence in recent years for underwater localisation. Most research focuses on autonomous underwater vehicle (AUVs) where many popular methods use communication between anchors and nodes to improve the estimated position [4] [5]. Doppler velocity loggers (DVL) have also risen in prominence in IMU-DVL solutions due to their ability to provide velocity estimates that are not affected by drift [6] [7]. Some more passive methods, such as using a geomagnetic map with geomagnetism measured by the AUVs, have also proven to limit the errors, but this is only viable when traversing longer distances [8]. Others have tried to address the problem of drift in sensors by using sensor arrays, thus allowing estimation and compensation for individual sensor errors, which have proven to improve the quality of the readings significantly [9].

1.1 Project Background

This study was done in collaboration with the Swedish Defence Research Agency (FOI) and revolves around an underwater data collection platform, which is part of a larger research project. The research question is whether the method presented in this thesis can improve the estimated location of the platform. The platform is supposed to act as an individual node in a distributed network of nodes. The platform is equipped with different sensors but lacks motors and other ways to manipulate motion, except for an inflatable pillow. This inflatable pillow, connected to a tank of compressed air, allows the node to resurface on command. The node is a rigid platform with no moving parts that is dropped from a boat at the water surface and allowed to sink and then rest on the ocean floor.

The platform is shaped like a tetrahedron and built around three pillars, providing the overall form and structural integrity of the platform. In figure1, an image of one of these nodes can be seen. The platform, equipped with various sensors, can collect information about the underwater environment. However, the sensors are not necessarily suitable for tracking its movement, so additional hardware constructed for this purpose are used as a part of this study.

(7)

Figure 1: The platform which is being tracked.

The research topic for this thesis comprises exploring methods for helping the nodes to track their position. The chosen solution will only use passives sensors, since one of the purposes of the platform is surveillance. An active sensor requires a signal to be sent out to gather information, which would also reveal the position and presence of the node to anyone listening. As the node is at least indirectly used for military research, this is unwanted attention. On the other hand, a passive sensor only listens to existing signals and is, thereby, not as easily detectable. This project will, however, solely focus on the localisation aspect, and all other applications for the platform will be disregarded.

1.2 Problem Formulation

This thesis aims to design and test a model that can improve localisation for this specific case. The purpose is to get an accurate position for the platform when the node is stationary on the bottom of the ocean. This position, on the ocean floor, is what is of interest since this is where the node will collecting data of the environment for other purposes.

However, estimating the position of a platform under water is not straightforward. Tracking here lacks much of the information and tools that a land vehicle could utilise. Nevertheless, with information about the platform’s environment and characteristics regarding its movements, it should be feasible to limit the overall errors regarding the positioning. The drift that is a result from integrating the values from inertial sensors is the most significant source of error. By exploring ways to minimise faulty data due to the drift, improvements to the localisation system can be made.

The research questions focus on the target hardware and the target environment and the results are partly evaluated in a simulated environment, based on a dynamic model with some hardware tests performed at a later stage to verify the algorithm’s performance. The research questions are:

(8)

• RQ1: Which methods are common for localisation, Which sensors can be used, and how do they contribute to the knowledge about the state?

• RQ2: How can sensor fusion algorithms be used to limit the drift and reduce the overall error?

• RQ3: How do we measure the reliability and accuracy for underwater localisation? Can the selected sensors be considered a feasible solution based on the results from the simulation of the developed model?

1.3 Limitations

Due to the nature of thesis work, a time constraint is always present, limiting what can be achieved within a project. Some simplifications and assumptions are therefore made. These limitations are:

1. Data is analysed and processed off-line

2. Task such as construction of the hardware is performed by other parties present at FOI. 3. The true position is not known for the experiment. Since tests are performed in an

uncon-trolled environment, some assumptions needed to be made regarding the performance and outcome.

(9)

2. Background & Related Works

In this section, some basic theory behind concepts that are relevant to this work are covered, such as navigation systems, sensors, data fusion algorithms, simulation techniques and simulation dynamics.

2.1 Sensor Fusion and Localisation Techniques

Localisation, in this context, is determining the position of an object in relation to a global ref-erence. The estimate of the position can generally not be regarded as exact since there are many sources of errors. Several techniques have been developed to combine data, which allow for more accurate positioning and attitude tracking (orientation). These actively perform tracking of the so-called "state" of the object, which can entail, for example, the position, velocity, orientation. There are several techniques for dealing with common issues faced in localisation algorithms. 2.1.1 Inertial Navigation Systems

Inertial navigation systems are systems used for the localisation of moving objects and estimate their state. Inertial navigation’s central premise is that motion can be tracked by integrating the angular and linear forces. Without the presence of sensor errors, then a system like this would suffice in predicting the state of an object. This is, however, not achievable since sensors are never perfect. The predictions can therefore only be trusted to a certain degree. By integrating linear and angular forces that a vehicle is subjected to (given by the accelerometer and gyroscope), velocity estimates and angular estimates can be achieved, and by integrating the velocity, a position is given. How long the prediction is accurate depends mainly on the sensors’ precision, accuracy, and resolution since all sensors produce random errors. These errors – although small or seemingly insignificant – may accumulate over time. Part of the weakness with using integration is that the errors are also integrated twice in the case of the position. This leads to an error that grows significantly over time in all states. Tracking orientation is needed to transform the measured acceleration (measured relative to the orientation of the sensor) to an acceleration relative to the global reference frame. Thus, the integration of errors leads to drift, which is inherent to INS. Therefore, these systems are usually combined with filters (so-called sensor fusion) to adjust for drift, thereby improving the estimate of the state. There are many kinds of sensors, and they all provide a variety of information about the system’s state. Generally, one source is not enough to provide sufficient information about the system to determine its state. Combining multiple sensors is usually beneficial as each sensor has its weaknesses and strengths, thus allowing for sensors to complement each other. Estimating the movement based on knowledge about the initial position and direction but with no access to global positioning data is referred to as dead reckoning. In this state, the error will increase with time since there is no method to adjust for and fix errors. [10] 2.1.2 Sensor Fusion

Combining data from different sensors is vital to accurately determine a vehicles’ complete state. A model describing how each measurement relates to the state is required in order to be able to combine different types of data. This model can either be generalised, i.e. the same model is used for many implementations, or be more specifically modelled to match a specific environment or implementation. This is done by adding vehicle-specific constraints or situation-specific constraints. Different methods exist for combining data from each sensor. The methods typically have different strengths and drawbacks. Depending on the implementation, they can all add an edge to the outcome in their own way. Some popular sensor fusion methods are:

Complementary Filter: The complementary filter is used to estimate the attitude by combining data from the accelerometer, gyroscope and magnetometer. This filter is simple and the least complex of the filters here mentioned. It combines the sensors in order to exploit each sensor’s specific strength efficiently. The complementary filter is highly efficient and suitable for platforms with limited computing power.

(10)

Kalman Filter: The Kalman filter is more complex compared to the complementary filter. It uses a motion model which estimates movement and the magnitude of the uncertainty in the state estimate, adjusting the state when new measurements arrive. The Kalman filter is often the preferred method in navigation solutions.

Particle Filter: In many non-linear systems, a particle filter can be preferred to a Kalman filter. Regular Kalman filters typically deal with a linear function, although if extended they can deal with non-linear systems as well. The main difference is that a particle filter does not necessarily assume a Gaussian distribution, making it more flexible. However, this also comes with greater computation complexity. In a paper presented by Habbachi et al. [11], one type of particle filter named Iterative Mean Density Truncation algorithm (IMeDeT) was used in tracking. The method achieved better results compared to an EKF on simulated data when determining the attitude. 2.1.3 Kalman Filters

Kalman filtering is one of the most popular methods for performing sensor fusion in navigation systems. Regular Kalman filter performs sensor fusion on linear models. While tracking a pre-defined set of variables referred to as the state vector or the state of an object, a Kalman filter also continuously estimates the impact of noise on the measurements through a feedback loop. The feedback loop is controlled by a variable called the Kalman gain, which tells the system how much it can trust the predicted state versus the measured state. The filter typically uses Gaussian distributions to determine the most likely state each time the system is updated with new values. One of the primary sources for the following section about Kalman filters can be found in Grewal’s et al. book on inertial navigation in [10].

Extended Kalman filter An extended Kalman filter (EKF) is an extension of a standard Kalman filter (which only provided support for linear models). An EKF supports nonlinear state estimation models by linearizing a non linear function through the first order Taylor series. The equations for EKF can be seen in table1, the variables seen in this table will be explained later on. The main steps – very similar to a standard Kalman filter – include the update step, prediction steps and estimation of the impact of noise on all states.

The Extended Kalman filter uses the Jacobian matrix to approximate a linear function from a non-linear function. Therefore, the Extended Kalman filter can still use the Gaussian distribution to approximate the state. The functions are, with some exceptions, almost identical compared to a regular Kalman filter.

Adaptive Kalman filter Since certain types of noise are time-varying, i.e, they change with time and can therefore not be described by constants. An extension of the extended Kalman provides this ability by modelling the noise variables having them as a part of the state vector, which has shown to be beneficial, see [12]. Adaptive Kalman filter is a version where typically biases are modelled and added as a part of the tracked state. This allows the biases to be actively estimated from the measurements.

Unscented Kalman filter An improved model of the Extended Kalman filter, as presented by Julier et al [13], promises better overall performance when compared with an EKF, especially on highly non-linear models. Unlike EKF, the so-called Unscented Kalman Filter (UKF) does not use a Gaussian distribution. This filter approximates a transformation function with multiple points around the expected state, the so-called sigma points. The Kalman gain is calculated using these Sigma points and by measuring the correlation with the measured values. UKF is generally considered a better tracking option for highly non-linear systems, but a correctly implemented UKF should always at least deliver the same performance as an EKF.

2.1.4 Kalman Smoothing

Kalman filters are built to work in real-time systems. The filter takes past measurements into account when estimating the current state. Smoothing filters can be used for systems that do not

(11)

Extended Kalman Filter Equation Prediction Step ˆ x ← ˆx + f (ˆx, u, t) P ← F P FT _{+ Q(t)} F = δf_δx|x=ˆx(t) Update Step: K = P HT_{(HP H}T _{+ R)}−1 H = δf_δx|x=ˆx ˆ x ← ˆx + K(z − h(ˆx)) P ← P − KHP

Table 1: Extended Kalman filter equations as defined in [10] by Grewal et al.

require real-time-data, as well as for post-processing applications. Smoothing filters make use of all data to produce a more confident state estimation. One common smoother is called the Rauch Tung Striebel smoother (RTSS), also referred to as a Kalman smoother, for more information see [14]. RTSS first uses forward recursion, which is performed by the Kalman filter. Then backwards recursion can be described as a backward Kalman filter, and the purpose is to use all future and past measurements to update and reduce the estimation error of each time-step.

The equations for the backwards recursion step defined in RTSS are: C = PkFxT(xk)[Pk+1− ] −1 ₍₁₎ xs_k = xk+ C(xsk+1− x − k+1) (2) P_ks= Pk+ C(Pk+1s − P − k+1)C T ₍₃₎

where Fx(xk) is the jacobian around the state, P is the process covariance matrix (P− for the

predicted step, P is after the updated step, and Ps _{is the process covariance for the smoothed}

path). x−

k is the predicted state vector, xk is the updated state vector, and xs is the smoothed

state. All these variables are recorded in the Kalman filter except for C, which is the RTSS version of the Kalman gain.

By reconstructing the path, the certainty of the state is increased throughout the entire path. This is very useful since it also gives a better estimate of the initial position. This attribute was shown to be very useful by S.C Reina et al. in [15] in a method presented as Global Iterative Extend Kalman Filter (GIEKF). This method used smoothing to better approximate the path and thereby providing a new initial state estimate based on all future data. This new, improved initial state was then passed back as an input to a new iteration of the forward Kalman filtering. By using this online feedback loop over small segments of the path each time, the original path was greatly improved. It also helped linearize the system and improving the estimate of the total odometry.

2.1.5 Zero Velocity Update

The noise that is integrated over time causes drift, leading to a severely erroneous estimation of the state where the error is growing with time. This is problematic for many different reasons. One method of bounding the error during specific periods is referred to as zero velocity potential update (ZUPT), also referred to as just zero velocity update. The central concept of ZUPT is about detecting when the sensor is stationary. Which can be seen when looking at the measurements from the accelerometer and gyroscope over a certain time period. This information is used to stop the accumulation of noise while the object is considered to be stationary.

There are 3 common ZUPT models [16] used:

• Acceleration Moving Variance Detector: Measures the variance between samples (k refers to individual samples) in the accelerometer data inside the window (Ωn) by comparing

each value (ya

k) to the mean over the entire window ¯yna. This is divided by the number of

samples (N) and the variance in the accelerometer (σ2 a).

(12)

Tv= 1 N σ2 a X k∈Ωn ||yak− ¯yna|| 2 ₍₄₎

• Acceleration Magnitude Detector: Checks if the accelerometer data (ya

k) magnitude is

close to the expected gravity magnitude (g). Tm= 1 N σ2 a X k∈Ωn (||ya_k|| − g)2 ₍₅₎

• Angular Rate Energy Detector: Checks the energy in the measured angular rate. This is done by calculating the norm of each measurement (yw

k) inside a window and then divide

by the number of samples (N) and the variance in the gyroscope (σ2 w). Tw= 1 N σ2 w X k∈Ωn ||ykw|| 2 ₍₆₎

These will provide an averaged value over a set of adjacent measurements, referred to as a window, which relates to a probability functions for the two states (moving and stationary).

In a paper by I. skog et al [17], standard methods for zero-velocity detection were evaluated and a solution was presented which is referred to as the Shoe detector (stance hypothesis op-timal detection). The compared algorithms comprised Acceleration-Moving Variance Detector, Acceleration-Magnitude Detector and Angular Rate Energy Detector. The Angular Rate Energy Detector performed best out of the traditional methods but was slightly superseded by the SHOE detector, which is a combination of all three methods. The outcome for the Angular Rate Energy Detector was attributed to the gyroscope holding a stationary signal better than the accelerometer. The Shoe detector, also called a General Likelihood Ratio Test (GLRT), is a combination of these three methods for a more generalised model. The GLRT model is defined as:

Ts= 1 N X k∈Ωn ( 1 σ2 a ||ya k− g ¯ ya n || ¯ya n|| ||2₊ 1 σ2 w ||yw k|| 2₎ ₍₇₎

The value which the algorithm gives (T ) is compared to a threshold (λt). This threshold is

set and decides if the sensor is stationary (H(0)) or moving H(1). This decides the state where the threshold can usually be a constant, although there are applications where adaptive threshold implementations are required [18].

H(0) : if T < λt

H(1) : if T >= λt (8)

2.2 Sensors and Measurements

When tracking an object, the most crucial part is to have the correct hardware setup. There are plenty of sensors that contribute with different types of information. Combining this information is the task of the fusion algorithms. But having the best possible sensor combination and sensors of high quality is vital for the performance of the fusion algorithms.

Sensors typically come in all different price intervals. Cheaper sensors tend to be less accurate, while more expensive sensors can get measurements of higher quality. In navigation implement-ations, Inertial Measurement Unit and Global Positioning System (GPS) are commonly used. However, some other sensors can be helpful depending on the implementation, for example, speed sensor in cars. This section briefly covers a relevant sensor for underwater applications and their attributes.

(13)

2.2.1 Inertial Measurement Unit

An IMU is usually a set of sensors combined on one chip that measures the bodies’ movement. An IMU is usually built from accelerometers, gyroscopes and, sometimes, magnetometers. There are many different types of accelerometers/gyroscopes/magnetometers, but one of the most common types by far is a Micro-electromechanical (MEMS) sensor. MEMS is what this project uses and will therefore be the focus of the study.

Accelerometer: Accelerometers are used to measure linear acceleration. Generally, triaxial accelerometers are used, which essentially are three sensors staked orthogonally to each other. This allows them to measure acceleration in all three dimensions. The most common type of accelerometers is a MEMS solution, but there are also a wide variety of different types where the choice depends on the budget and implementation. In MEMS sensors, a linear acceleration measurement is typical achieved by measuring the displacement of a small mass attached to a spring, alternatively by measuring the differing frequency of a vibrating mass. This mass is affected when the body that the accelerometer is placed on moves. The movement of the mass that is measured is proportional to the acceleration of the body.

Gyroscope: A gyroscope measures the angular velocities. These are usually used to calculate changes in the orientation of an object. The typical setup is a three-axis gyroscope with three orthogonal sensors measuring the angular velocity around all three axis. A common type of gyro is electromechanical gyros, which can be manufactured both as expensive, highly accurate sensors, and cheap low-accurate sensors. This type of sensor measures the Coriolis acceleration, which proportional to the angular velocity. [19]

Magnetometer/Compass A magnetometer provides information about the orientation of a sensor in relation to the earth’s magnetic field. The earth acts as a giant magnet with magnetic north and magnetic south. A magnetometer senses this magnetic field, and with this information, an orientation can be derived. This sensor allows for calibration of the vehicle’s orientation, and it is not affected by drift since no integration is used. The sensor is very sensitive to disturbances and needs to be calibrated often. Sudden movements and electrical fields may affect the accuracy of a magnetometer by large margins. Therefore, it is essential that the sensors are not placed close to sources in the form of magnetic fields that can cause disturbances. The earth’s magnetic poles have an offset from the earth’s true north/west/east. This offset differs depending on the location. What is important to know is that it differs from the global coordinate system which GPS uses, and therefore this difference must be compensated before being fused with the other data. This is commonly done with a simple lookup table of recorded magnetic fields, depending on location. The offset between the magnetic north to true north is measured in a declination and inclination angle.

2.2.2 Noise Characteristics

The noise can be classified into two groups, deterministic and non-deterministic.

Deterministicnoise can be measured and removed through calibration, this type of noise does not change in relation to time but is dependant on the input. Non-deterministic noise (alternatively called stochastic noise) changes over time or is the type that classifies as white noise (usually with a Gaussian distribution). This type of noise has a random component and can therefore not be removed by calibration and normally requires a method to characterise noise. There is also an adaptive method that estimates certain noise variables which are random but have a time component.

IMU Noise: The main types of noise that can be found in an IMU (Accelerometer and Gyro-scope) are:

1. Bias

(14)

3. Random Walk

The bias is a constant offset that is additive. This term contains both a constant part, which is easily measured and removed with an average and a time-varying part. This time-varying bias varies during run-time and between runs, which means that it needs to be estimated and adjusted. There are many ways of adjusting for this time-varying term, and the Adaptive Kalman filter is one prominent example.

The Scale factors contain linear and non-linear irregularities in the scaling of the measure-ments. Scaling the data correctly ensures that both low and high values will receive the correct measurements.

The linear scaling factor,which is partly constant, can be adjusted with the calibration of the sensor. This calibration is typically done by rotating the sensor into many different known positions. The measurements can then be used to create a transformation matrix between the measured value and the desired value (as shown in [20], [21]), which is a common method for removing noise. Misalignment errors are also a common error source, and the result of axes not being entirely orthogonal to each other, or not be aligned correctly with the sensor body. The calibration will correct this also.

The non-linear scaling factors are harder to adjust for since a routine calibration might not suffice. Due to their non-linearity, they cannot either be entirely predicted. This problem can gen-erally be mitigated with higher quality sensors. One error source is the temperature which affects the scaling. The temperature can be measured and the calibration can be adjusted accordingly, which is the common mitigation technique for IMUs, and also the technique used for the IMU in this thesis.

Random Walk Errors, also referred to as white noise, has the attribute of integrating to zero. These errors are impossible to remove entirely due to the nature of them being completely random. However, this noise can be estimated through techniques and the estimation can be taken into consideration when the data is used to make a prediction.

A commonly used model for modelling noise in a sensor, which is similarly defined in [22], is: measured= (I + M + SF )true+ b(t) + RW (9)

Where I is an identity matrix, M is the misalignment error, SF is the scale factors, b(t) is the bias which contains a constant time-varying bias part, and RW is the additive white noise. true

is the true value, and measured is the measurement value from the sensor.

A model like this aims to define as much of the signal as possible into derivable terms. The part of the signal that cannot be attributed to a predicable term will be included in the random walk term, which is non-deterministic.

Magnetometer: A perfect magnetometer would produce a sphere with the centre in the origin of the coordinates of data points when rotated around all three axes. This is, however, not generally the case. The mean of a magnetometer is usually not zero. Partly, this is because there are magnetic material everywhere that cause magnetic fields around the device. These fields have an additive effect which gives the measurements an offset. This can be seen as a constant bias. This offset is referred to as hard-iron distortion. There is also other materials that do not necessarily produce a magnetic field but instead distort them. This will affect how the sensor pick up the earth’s magnetic field, thus not producing a perfect sphere. This imperfection is referred to as soft iron distortion.

Adjusting for this is done with a calibration. This calibration gives a matrix for compensating the soft iron distortion (S) and an offset in X,Y,Z for compensating for the hard iron effect (H).

Madj = (Mmeas− H) ∗ S (10)

In10, it shows how the compensation is performed where Mmeas is the measurement values and

(15)

2.2.3 GPS

Global positioning system uses a triangulation method between multiple satellites, that transmits their respective positions with a timestamp when the message is sent. This allows a receiver on the ground to triangulate the position. [23]

GPS is a common tool that is integrated and fused with INS because it provides a global position that allows for drift adjustment. However, GPS is usually not an adequate system since it has a low sampling frequency and can suffer from long periods of outages due to environmental factors.

A global position via GPS is generally represented in the geodetic coordinate system. This refers to using longitude, Latitude and Altitude as a way of describing the position relative to the earth. This system uses parallel lines stretching around the earth from east to west, intersected by lines stretching between the geographic poles. These lines provide the longitude and Latitude angle while the Altitude represent the distance from the earth’s centre. This system is globally consistent but not necessarily intuitive when understanding distances. Another more practical approach for tracking is the Earth-Centered, Earth-Fixed (ECEF) coordinate system. This refers to using the classical X,Y,Z coordinates to describe the position, where changes can be measured in meters. This system is by definition a "flat earth" system which is only accurate in smaller areas but practical since it supports Euclidean geometry. [24]

The conversion from geodetic to ECEF coordinates as defined by B. Hofmann-Wellenhof et al in [25] can be seen below:

X = (N (Φ) + h)cos(Φ)cos(λ) (11) Y = (N (Φ) + h)cos(Φ)sin(λ) (12) Z = (b 2 a2N (Φ) + h)sin(Φ) (13) N (Φ) = a 2 pa2_cos2_{(Φ) + b}2_sin2_(Φ) = a p1 − e2_sin2_(Φ) (14) e = 1 − b 2 a2 (15)

Where X,Y and Z are the ECEF coordinates, Φ, λ and h is latitude, longitude and height and a,b are semi-major and semi-minor axis of the earth.

For practical reasons measuring position relative to a local coordinate system is often preferred. Local tangent plane coordinates converts the global coordinates to a local frame based on a ref-erence position, where the starting position is origo. This is a popular method used in tracking applications. The frames are usually defined as either ENU ("East North Up") or NED ("North East Down"), which refer to how the frame of reference is rotated in relation to the global system, see figure2 .

2.2.4 Pressure sensor

For underwater purposes, pressure sensors have been used and tested to determine depth [26] for AUVs. Since pressure increases the deeper the platform sinks, this is a way how the position in the ’down/up’ direction can be measured. Other implementations of pressure sensors also exist. One interesting example is a differential pressure sensor used to measure underwater velocity in [27].

2.3 Environment and Simulations

To verify the results, simulations are typically used because it is a practical method. Simulations normally model and simplify physical aspects and qualities of the target environment in order to provide realistic data.

(16)

Figure 2: A Local ENU frame is shown in relation to ECEF (X,Y,Z) and geodetic (φ,λ) coordinates. Image is courtesy of Mike1024 1_.

2.3.1 Hydro/Fluid Dynamics

When working with underwater vehicles, it is important to consider that the dynamics that affect land vehicles are slightly different compared to underwater vehicles. Some changes are the result of replacing the air with water. This is, however, more important when trying to manoeuvre with the help of motors. Two of the main forces are listed below:

• Buoyancy force is the force that keeps less dense objects at the surface while higher density object sink. The argument is that if an object is going to sink, it needs to be more dense than the substance it is in. If it is less dense, then it will lead to an equilibrium where it floats with a part above and a part below the surface, like the buoy.

Fb= gρV (16)

where Fb us the buoyancy force, g is the gravitational force, ρ is the liquids density and V is

the volume of the object.

• Drag force is the force required to move the water particles around and in front of the object. This is the same as the wind resistance above land, but it affects an object in a greater magnitude underwater since water is a denser substance.

Fd= 0.5CDACρ(∆V )2 (17)

Where Fd is the drag force, CD is the drag coefficient (this can change slightly with

temper-ature), AC is the cross-sectional area opposite to the force direction, ρ is the liquid’s density

and ∆V is the object’s velocity relative to the velocity of the water. The drag force is also partially dependent on the viscosity of the liquid.

When an object reaches an equilibrium – meaning the sum of all forces is zero – a constant velocity is reached. This is referred to as an object’s terminal velocity. As long as the forces affecting an object are constant and do not change, the object will reach this equilibrium.

(17)

2.3.2 Monte Carlo Simulations

When multiple random parameters are involved in a process, it can be difficult to predict or assess the range of plausible outcomes of a system. This can be true with INS systems where a 6 DOF IMU is used since each accelerometer and gyro will have multiple possible error sources. A common method to evaluate these types of systems is Monte Carlo simulations. These systems are typically hard to evaluate mathematically since many different sources contribute to the error. Monte Carlo Simulations consist of multiple runs with a specific set of random parameters changing each run, meaning each run will be unique. Even if each run creates a unique solution after performing a certain number of runs, a pattern of how the system behaves can be observed. This can be used for risk assessment and for detecting the reasonable error bounds of a complex system.[28]

(18)

3. Method

The development process of the solution contains a research part presented in section 2.. This part contains past work, state-of-the-art solutions and algorithms. The purpose of this section is to build knowledge about the solutions and existing problems. It will also explain why certain algorithms have been picked.

The focus of this thesis is the implementation and testing of algorithms. The results will also focus on this aspect of the project. Primarily, the results will look into the error of the position, error here is calculated as the deviation between the actual and the estimated position (using Pythagoras theorem). If the results were extended to also compare error in all state (such as position, speed and orientation), it would not necessary add more knowledge since they are coupled. The results are primarily based on simulated data. Verifying results outside a simulated environment is hard, especially under water. There are ways to do this, but these methods extend past the scope of this thesis.

Some hardware is used as a part of this thesis. However, the construction of the physical hardware is not a part of the thesis. Instead, this task has been performed by people at FOI with knowledge in this area. The hardware includes, for example, the construction of a waterproof container, power supply and PCB design. These parts will be mentioned later. The system running on the hardware is out of necessity developed as a part of this project. However, this is not the focus of the thesis, unlike the implementation of the sensor fusion algorithm, and will therefore only be covered briefly.

3.1 Hardware

For this project, a hardware module housing all the electronics was constructed. All the equipment for tracking the node is placed in this watertight module and fastened on one side of one of the node’s legs. An illustration of this is shown in figure3. The module contains different sensors and a small computer for controlling the system as well as logging the data, with accessories such as a battery. The parts for this module can be seen in figure4.

3.1.1 Sensors

Two different sensing units are used, an INS system (containing GPS, Accelerometer, Gyroscope and Magnetometer) and a pressure measuring system (containing a pressure sensor and a temper-ature sensor).

• The INS system is a complete general-purpose tracking unit capable of filtering and tracking as long as it has a GPS signal. The system used is the MicroStrain 3DM-CX5-GNSS/INS system2_{, this sensor is factory calibrated, which removes the necessity for calibration.}

• Pressure Sensor is the MS5837-30BA pressure sensor 3 _{which has a built-in temperature}

sensor. This sensor is placed in contact with the water thereby providing the temperature and pressure readings of the outside fluid.

3.1.2 System

To sample and save the measurements from each sensor, a Linux based system running on a Raspberry Pi Zero W4 _{is used. This controller was chosen to try to minimize the system’s power}

usage while also reducing the development time needed. The benefits of running a Linux system is that the OS will take care of tasks such as scheduling and maintaining the file system.

The systems also run Robot Operating system (ROS)5 _{on top of the Linux core, with the}

purpose to record data from the INS and the pressure sensor. ROS is used as it allows access to the data within the system. Which is important since it allows the operator to check that the platform has an initial GPS position before allowing it to go under water. It also gives the operator

2_{https://www.microstrain.com/sites/default/files/3dm-cx5-45_datasheet_8400-0118_rev_g.pdf} 3_{https://bluerobotics.com/store/sensors-sonars-cameras/sensors/bar30-sensor-r1/}

4_{https://www.raspberrypi.org/products/raspberry-pi-zero-w/} 5

(19)

Figure 3: Hardware module in white, where the sensors are placed, attached to a one of the nodes legs.

Figure 4: The plastic container (with battery and electronics) which was constructed for this thesis that is attached to one of the node’s legs.

(20)

Figure 5: An overview of the system design for the sampling platform is shown. The input to the system is shown on the left-hand side. Inside the system, the white rectangles indicate different programs, and the coloured boxes are the different types of data.

some control over the system, and since the developer of the INS also offer support for ROS6_this

is a practical solution.

Peer-to-peer Wifi access was set up to give system access to the operator without having to open the watertight module every time, which would be time-consuming and, in specific environments, risky. The plastic shell allows for signals to easily pass through. At the prototype stage, these features are essential to ensure the system is running when performing experiments. There are energy costs associated with using WIFI, although it was determined that the benefits would outweigh the extra cost.

A software implemented emergency shutdown was also added. This would be activated if the battery ran low while a test was being performed. The system would, in that case, be notified via a digital pin. This activates a safe shutdown protocol to give time for the system to save and shutdown correctly. This is important in order to mitigate the risk of losing the already recorded data. A block diagram showing the system design can be seen in figure5.

3.2 Software

The simulations, implementation and processing of the data for the project is done in the Mat-lab/Simulink environment. The system samples each sensor and saves the data to a log file with timestamps for each sample. The INS sensors is running its own system inside the sensor and therefore provides its own timestamps. Since the system receives samples from two different sys-tems, it also keeps track of information to synchronise the two. The INS systems uses the GPS time system (given in TOW/Week), while the pressure/depth sensor uses the system time. This data can later be extracted and post-processed in Matlab, which is used to determine the position and analyse the results.

(21)

3.3 Proposed Solution

Tracking objects for more extended periods with GPS outage has shown to be complicated. Im-provements such as ZUPT have proven effective, but only for short periods. The method which showed the most promise during the research was GIEKF. This method was discussed in (2.1.4), and uses smoothing to improve the initial estimate, and this new estimate then passed back through the forward Kalman filter again. However, it can be questioned how much this method relies on sound knowledge of the final and initial states.

One of the issues in the thesis is that knowledge about the state is inadequate while the node is submerged. The initial state can be known since the node will be above the surface in this state, with access to GPS. This procedure is covered in more detail in the next section (3.4). Nevertheless, as soon as the node is underwater, the error will generally only increase with time. Zero velocity update will allow drift to be stopped when the node reaches the bottom.

3.3.1 Sensor Fusion

To achieve the requested outcome, the proposition is to use an extended Kalman filter for tracking. Kalman filters are widely used in this area, and even if other methods, such as particle filters, exist, these are not commonly used. There is no justification for spending time evaluating the performance of alternatives, since this would require a separate project. An alternative could have been an unscented Kalman, as there are promising research that suggests that this choice would perform better. This could be another research topic, but first when a working system is in place. Gyroscope and accelerometer bias can be a significant sources of error. Because of their time-varying nature, they cannot be entirely adjusted for by a calibration (see2.2.2). Since there will be no GPS that can adjust for drift, a bias error will accumulate every time step. Therefore, an adaptive filter is used to estimate these. Bias is generally not particularly large and changes slowly over time. If not done correctly, this is a significant error because it is integrated into all states every time step.

The EKF combines the data from each sensor while keeping track of the uncertainty. The benefits of using a Kalman is that every part of the system can be added under one filter. This is practical when dealing with different sensors and algorithms, such as zero velocity detection. The implementation for the Kalman filter used in this thesis is presented in depth later in section4.3. A simple flowchart showing the structure can be seen in figure 6. As shown in the figure, the prediction is given by the inertial navigation equations which uses the accelerometer and gyroscope data (from the IMU). This prediction is updated using the measurement values from the magnetometer, pressure sensor, ZUPT and GPS in the Kalman filter, which provides a more accurate prediction of the actual state. This is done by taking into account the expected noise in the prediction versus the measurements.

3.3.2 Smoothing

Backward Kalman filters, or smoothers, have been effective in recreating paths. The implementa-tion was presented in secimplementa-tion2.1.4. Many methods such as GIEKF have used smoothing effectively to improve odometry estimations, although it should be noted that these methods typically have much more information available. Shorter periods are generally subjected to these methods, and not long periods with GPS outages, at least none that have been cited in thesis. Extended periods with little data about the environment can cause issues.

In this case, both the position on the ocean floor and the path are unknown. However, this can be simulated, which allows for a known path. Smoothing methods are also a good way of recreating a path when all data is available.

The idea is to use smoother to improve the certainty of the initial estimate, which is similar to the GIEKF method. The goal is to get a better estimate of the position on the ocean floor. Therefore, measurements up until the node is stationary on the ocean floor can be used. Using forward and backward filtering on the data allows for the improvement of the initial state. The new initial state can be fed back into the Kalman filter to provide an improved path with a better estimate of the final position. For this thesis, only the bias state is improved with this method, as

(22)

Figure 6: The Kalman filter design represented in a simple flow chart.

there were some stability issues when updating the entire state. Another research project could be to improve the entire initial state.

The method, which will be refereed to as the iterative EKF, can be divided into the following steps:

1. Predict path with the EKF up until the node is stationary. 2. Perform Kalman smoothing to get an improved initial state. 3. Updated initial bias based on smoothed path.

4. Repeat process X times.

Another possibility is to recreate the path with smoothing once the node is above water and the GPS signals start coming. The benefit of this is that this path will have a higher rate of certainty which can be used for comparison with the predicted position on the ocean floor. However, this was not possible due to the amount of time it took before the node received a GPS fix while waiting to be picked up.

3.4 Experiment Procedure

This section describes the procedure for conducting the experiment and how the data is collected. The purpose is to collect data in a relevant way and in a repeatable manner. The scope is to get an accurate estimate of the position while the node is positioned on the ocean floor. An image illustrating the optimal steps for the path can be seen in figure7.

The node is tracked by using inertial navigation. Tracking is aided by zero velocity updates and depth measurements as soon as the node disappears under the water surface. The periods where the node is moving and when there is no access to any global positioning source, is where the error accumulates. The path can be divided into five different stages:

1. Initial State (at the surface) 2. Sinking State

(23)

Figure 7: An illustration of the procedure when testing the node. The node is dropped from a boat, sinks to the bottom, stays there for a while before rising back to the surface.

3. Stationary State (on ocean floor) 4. Rising State

5. Final state (at the surface)

What is of interest is the nodes position on the ocean floor in the stationary state. Drift will occur mainly in state 2 and state 4. Stage 1 and state 5 will have access to GPS, which allows for a better state estimate. In state 3, the node will be stationary and a zero velocity update will stop the drift from accumulating.

3.4.1 Data Collection

While the system is running, data is being logged from each sensor. Data from the INS sensors’ onboard filter is also logged. This onboard filter provides good estimates of the state while it has a GPS fix. However, without a GPS fix, the position quickly deteriorates, which is why it is not a viable solution for this purpose. It is, however, used to get the initial filter state while the node is on the boat. The rates for the data collected can be seen in table2. The sampling rate has been set at a rate that is considered sufficient for this project.

The measured water density at the testing location can be seen in figure8. The density data is vital for the pressure measurements, which rely on this data to decide the depth. However, the density does not change much, as can be seen. Because of this, an average is used, which is a good enough estimate for this thesis.

Once the data is recorded, it is segmented into different sets, where each set is one submerging of the node. The starting position of each segment is set as the origin coordinates. The rest of the data relates to this position through a local tangent plane for the NED format. Each segment is cut with a margin, allowing the filter to gain sufficient knowledge about its initial state while still having access to GPS.

(24)

Senors Rate IMU: Acc 100 Hz Gyro 100 Hz Mag 100 Hz GPS: GNSS 1 Hz Depth: Pressure 10 Hz

Table 2: Sampling rate of each sensor and each type of data.

Figure 8: The waters density is shown in relation to the depth. This data is measured the same day as the experiment. The fluctuations at around 16 m depth is likely due to salt layers in the Baltic sea with effect the density.

(25)

Figure 9: Data set used to calibrate the magnetometer before tests were performed. 3.4.2 Magnetometer Calibration

Before starting the recording, the magnetometer was calibrated. This procedure was done after the recording device has been fastened to one of the nodes. The node was moved to a location with no visible iron or electronics that could interfere. The node was rotated a couple of times from different angles. From this data set, the hard iron and soft iron effects are calculated using the Matlab’s "magcal7_{" function. In figure}₉_{the recorded data set can be seen.}

The inclination and declination are provided by the INS sensor, which uses the GPS position to get these values from a table. Similarly, other values such as gravity can also be provided from tables with the initial GPS position.

For calibrated data, the magnetometer intensity, which is the norm for the magnetic vector, should be constant. In the figure 10 the intensity before and after the calibration can be seen from one recording. The intensity changes between two different levels in the calibrated data. The reason is that the node is on the boat, which provides a higher intensity level. Meanwhile, the lower intensity level is when the node is in the water. The boat adds a magnetic field which distorts the measurements. As soon as the node drops from the boat, the intensity falls to a "base level" around the expected strength.

3.4.3 Experiment

The experiments were performed during two days with access to the hardware. The first day focused more on testing the hardware to ensure that everything worked.On the second day, the real tests were performed. The hardware was fastened to one of the nodes. A picture of this can be seen in figure 11. The node was put on a boat, transported to a location where the depth was around 40 meters and dropped into the ocean. In figure12 images showing the node being dropped/picked can be seen. After some time, the node was commanded to come back up. The first thing to appear is the inflatable pillow, shown in figure13. The node was then pulled back into the boat. At this stage, all the systems on the node were checked. This to ensure that everything is running and that the node has a GPS fix.

(26)

Figure 10: The intensity of the magnetometer data before and after the calibration using the calibration set. The difference in intensity correlated with when the node was on/off the boat.

Figure 11: The container fastened to one of the nodes with the pressure sensor facing upward.

(27)

Figure 13: The red inflatable pillow indicates were the node is floating just below the water surface.

4. Implementation

The following sections covers the theoretical part of the implementation. The mathematical equa-tions are presented for each step.

4.1 Simulated Data

For development and testing, simulated data is used to evaluate results. Real world data is not available for the larger part of the project and when available it is in a limited capacity. Therefore simulated data is vital in order to simultaneously develop the hardware and software. The model used for the simulation is of a rigid body with the shape of a cone.

The main forces that are considered for producing the linear and angular acceleration were mentioned in section2.3.1. They are also listed below for a short description.

1. Drag force ( ~Fd): This force comes from the node’s velocity relative to the water’s velocity.

The drag forces will have an x,y and z component. The simulated model assumes that the x and y direction data comes from a current with a time-varying velocity while the z components come from the data when the platform is sinking.

2. Buoyancy Force ( ~Fb): This force exclusively works in the up/down direction. This force also

changes when the platform is brought to the surface since it depends on the volume. The node uses an inflatable pillow, increasing its volume, thus increasing the buoyancy force and allowing the node to float.

3. Gravitational force ( ~Fg): This force is the earth’s gravity and can be assumed to be constant.

The sum of all linear forces acting on the platform are defined as: ~ Fd = sign(∆V ). ∗Fcx Fcy Fdz ~ Fb =0 0 ghρ ~ Fg=0 0 gm X ~_F_sinking ₌_F_cx1 _F_cy1 _F_b1_{+ F}_dz1_{− F}_g X ~_F_rising ₌_F_cx2 _F_cy2 _F_b2_{− F}_dz2_{− F}_g

where ~Fdis the drag force divided into x,y,z direction where x,y (Fcx,Fcy) is from the current and z

(28)

Figure 14: Position and attitude produced in the simulated environment with an each of the states represented.

Fcx2, Fcy2, Fdz2). Lastly sign(∆V ) is the sign, i.e. a +1 or −1, depending on if the difference in

velocity between the current velocity and the bodies velocity, is positive or negative. The buoyancy Fb and gravitation force Fg are only vertical forces.

The torque equation gives the angular velocity. This by assuming that the forces are acting with an offset from the centre of the node. From this, the torque (T) can be calculated with:

T = rF orcecenter× ~F (18)

(19) Where rF orcecenteris the offset from the centre where the force is acting. This translates to angular

velocity with the follow equation:

T = Iα (20)

where I is the inertia and α is the angular acceleration.

With these equations, a trajectory is created, and noise is added to the measurements. Figure

14 illustrates how a simulation trajectory is represented in the position and attitude is shown. Noise is added with a simulated IMU sensor, specifically Matlab’s "imuSensor"8_{. This simulated}

IMU takes the state and adds noise according to a set of parameters that have been modelled after the INS sensors datasheet9_{. The rest of the sensors, such as the GPS and the pressure sensor, are}

modelled with Gaussian noise.

4.2 Inertial Navigation

The inertial navigation equations are used to predict the movement of the node. The data from the IMU is used as an input for these equations. The sample rate of the IMU sets the discrete-time step for the Kalman filter, where other sensor data is fused into the prediction.

The IMU gives acceleration and angular velocity. From these values, the prediction of the state can be updated by integration of the acceleration and combining the acceleration with the previously known velocity (Vk−1), the velocity is given (Vk).

Vk = Vk−1+ akt (21)

By integrating once more the position (Pk) is given.

Pk = Pk−1+ Vk−1+ ak

t2

2 (22)

8_{https://se.mathworks.com/help/nav/ug/introduction-to-simulating-imu-measurements.html} 9_{https://www.microstrain.com/sites/default/files/3dm-gx5-45_datasheet_8400-0091_rev_o.pdf}

(29)

The body frame is the frame in which sensor data is measured. The acceleration needs to be rotated into a global frame, referred to as the navigation frame (the local tangent NED frame). This is done with a rotation matrix (RB2N), which rotates from the body frame to the navigation

frame.

aNk = RB2N(θk)aBk (23)

In the state, the orientation, θk, can be expressed as Euler angles. This is convenient and

understandable. However, for updating the orientation, quaternions are used since they are better equipped to deal with non-linearities. The state is converted to the quaternion representation then converted back to Euler representation after it has been updated.

To update the orientation with the angular velocity the following equation is used: qk+1= Ω(~ωk)qk = [ ~ωk]X ω~k − ~ωkT 0 qk (24) ~

ωis the angular velocity which is the gyroscope readings with the bias removed, and [ ~ωk]X is the

skew-symmetric matrix of the angular velocity vector. qk is the quaternion representation which

is described later on in (4.2.3). 4.2.1 Sensor model

For tracking the state of a node, a model of how the measurements relate to the movement is required. The motion can be defined by (21-24), but how the measurements are related to the input is defined by a sensor model.

The model for the accelerometer and gyroscope can be defined by (9) from (2.2). The IMU sensor is factory calibrated and temperature compensated to a fine margin. An assumption is there-fore made that the miss-alignment and the scale factors are negligible. Following this assumption, a very simple model can be used:

true= meas+ b(t) (25)

where only the bias is an important noise factor. Bias is slowly time-varying, which could be an important factor for applications like this, where global positioning is unavailable.

This means that the acceleration and angular velocity can be defined as:

aB = ameas+ ba(t) (26)

ωB = ωmeas+ bg(t) (27)

where aB, ωBis the acceleration and the angular velocity, ameas, ωmeasis the measured values for

each and finally ba(t), bg(t)is the bias for each.

4.2.2 State Vector

The states that are tracked are the position and velocity given by (21-22). These account for the linear movement. The orientation is also tracked, which is given by (24), for convenience, Euler angles are used. Lastly, the biases for the sensors are tracked, which allows for adjustment of the measurements in (26-27), theses biases will be indirectly measured and adjusted for by the Kalman filter.

This give the state vector (Xk) which can be described as:

Xk =Pk Vk Ek bak bgk

(28) 4.2.3 Orientation Representation

The body’s orientation needs to have a representation in relation to a global coordinate frame. This global coordinate frame (referred to as the navigation frame) defines the body’s movement

(30)

and rotates from a fixed reference. There are well-known issues with using the Euler angle repres-entation for this, such as a gimbal lock. The non-linear represrepres-entation in the form of quaternions is a more robust method. However, non-linear representations are more complex to implement into Kalman filters. One traditional way to solve this is by using Euler angles to represent the orientation in the Kalman filter but then convert to the quaternions representation every time a change is made. This solution leverages the best of both systems and avoids the problems with both systems.

The quaternions representation used in the thesis is defined by one scalar value q4, and a vector

part ~e This definition can be found in a paper by A. Sabatini in [29]. ~ e =q1 q2 q3 T (29) q =~eT q4 T (30) From the quaternion, a rotation matrix between the navigation frame and the body frame (or sensor frame) is defined by

CNB= 1 p||~ω||2_{+ q}2 4   q2 1− q22− q32+ q42 2(q1q2+ q3q4) 2(q1q3− q2q4) 2(q1q2− q3q4) −q21+ q22− q32+ q42 2(q2q3+ q1q4) 2(q1q3+ q2q4) 2(q2q3− q1q4) −q21− q22+ q32+ q24  qk (31)

4.3 EKF Implementation

One of the many benefits of Kalman filtering is that it keeps track of how the error propagates through each state and through time. While updating the position with measurements, the un-certainty is also increased. The implementation for an extended Kalman filter in discrete time, is summarised in algorithm1, this section will go through each step more thoroughly.

The state is predicted using the equation

X_k− = f (Xk−1, Uk, dt) (32)

where Uk is the measurements from the IMU, Xk−1 is the previous prediction, dt is the time step

and f is a function that that updates the prediction based on the inertia navigation equations. This is explained in depth in the next section (in4.4). In X−

k the negative sign indicates that it is

the predicted state. The filter later updates the predicted state with measurements in the update step.

At each time step, new measurements are added, and the state is updated. When the state is updated, the process covariance matrix (P ) or alternatively called the process uncertainty matrix, also needs to be updated. This is done with

P_k−= F (Xk, Uk)Pk−1F (Xk, Uk)T + GQGT (33)

which propagates the error from the measurements to get a new estimate of the uncertainty for each state. The Jacobian matrix (F ) is used to estimate how the error propagates through each state, which is a linearized model of the motion model, see section4.4.1. In essence, it tells each state how a certain amount of error is added in one state affect all other states. P is the process covariance matrix, F represents the Jacobian matrix, Q is the process noise, and G converts the process noise into the correct format so it can be added to the process covariance matrix.

Whenever there are new measurements with new information about the state, such as Magne-tometer or GPS values, these are used to update the current prediction. This allows for reduced uncertainty since the noise in the measurements is assumed to not be correlated with the noise in the motion model. This step updates the current prediction with measurements and the process covariance matrix. For this purpose, the Kalman gain (K) is used, which is calculated by:

K = P_k−H(X_k−)(H(X_k−)P_k−H(X_k−)T + R)−1 (34) The Kalman gain uses the uncertainty to decide how much the measurement can be trusted vs how much of the predicted position can be trusted.

(31)

The Kalman gain is calculated using the process covariance matrix (P), the linearized meas-urement model (H), explained further in section4.5.5, and the measurement noise (R). The meas-urement model is explained and derived in section4.5which goes into more depth.

The state is updated using the Kalman gain and the measurement model in

Xk = Xk+ K(˜zk− h(Xk−)) (35)

where ’˜z’ is the measured state and ’h(X−

k )’ is the predicted measurement based on the predicted

state.

Since orientation does not use a linear system, updating it with simple addition could cause some issues. The orientation is therefore updated using the following equations:

θnew= rot2eul((I − [θadj]X)RB2N(θ)) (36)

which takes care of the issues with nonlinearities for small angle adjustments. θadj is the values

which was given by the update equation (’K(˜z− ˆz)’) for the angles and ’θ’ is the current prediction. The last step updates the process covariance matrix with the new information about the state. Since new information about the environment has been added, the uncertainty, therefore, decreases. This is done with the following equation:

Pk = (I − KH(X_k−)P_k− (37)

followed by

Pk← (Pk+ PkT)/2 (38)

which ensures that the matrix is symmetrical. Algorithm 1:Extended Kalman Filter

1 initialisation of variables; 2 for 2=k:n do 3 Prediction Step: 4 X−_k = f (X_k−1, U_k, dt) 5 P−_k = F (Xk, Uk)Pk−1F (Xk, Uk)T + GQGT 6 Update Step: 7 K = PkH(Xk−)(H(X − k )PkH(Xk−) T _{+ R)}−1 8 Xk = Xk+ K(˜zk− h(Xk−)) 9 Pk = (I − KH(X_k−)P_k− 10 Pk ← (Pk+ PkT)/2 11 end

4.4 Motion Model

The linear acceleration of the object relative to the global reference is what is of interest. For this reason, the measurements from the accelerometer (which are measured in the body frame) are transformed to the navigation frame, as described previously in section4.2. When this is done, the gravity vector needs to be removed. This gives us the following model:

aN = RB2Nameas+ RB2Nba− g (39)

where aN is the acceleration in the navigation frame, RB2N is the rotation matrix that transforms

the body to the navigation frame and g is the gravity vector.

Combining this equation with the linear INS integration will give us the dynamic model for linear movement. The state is updated with

xk+1= Axk+ B(RB2Nameask+ RB2Nbak− g) (40) where the A updates the state based on previous knowledge, and B takes in the new input which is the measured acceleration after the bias and gravity influences have been removed.

(32)

A =         1 0 0 T 0 0 0 1 0 0 T 0 0 0 1 0 0 T 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1         (41) B =         0.5T2 ₀ ₀ 0 0.5T2 ₀ 0 0 0.5T2 T 0 0 0 T 0 0 0 T         (42)

Updating the orientation is done with the angular velocity from the gyroscope. The angular velocity is expressed relative to the body frame in:

ωBk= ωmeask+ bgk (43)

The orientation is then updated with (24). 4.4.1 Jacobian

A core principle of the extended Kalman filter is to linearize around the mean. This linearization is performed by using the Jacobian matrix, which is the first order of the Taylor series expansion. What the Jacobian describes is how changes in the state affect the dynamic model and the overall uncertainty. This knowledge is used to update the process covariance matrix, which needs to be updated every time the state is updated. The process covariance matrix expresses how confident the system is of the current state. Each update from the dynamic model increases this uncertainty since more noise is integrated into the state.

It can be expressed using the gradient as the partial derivative of the state, as defined in equation28. With this method, the Jacobian matrix can be described by:

F =         δP δP δP δV δP δE δP δBa δP Bg δV δP δV δV δV δE δV δBa δV Bg δE δP δE δV δE δE δE δBa δE Bg δBa δP δBa δV δBa δE δBa δBa δBa δBg δBg δP δBg δV δBg δE δBg δBa δBg δBg         (44)

By adding the equation described in this chapter and the taking the partial derivative the following is achieved: F =       I ∆t 0 0 0 0 I [RB2N(ameas+ ba)]X∆t RB2N∆t 0 0 0 I 0 −RB2N∆t 0 0 0 I 0 0 0 0 0 I       (45) The Jacobian matrix is used to estimate how the error propagates to all coupled states, which leads to increased uncertainty.

4.5 Measurement Models

Measurements are used to reduce the uncertainty which accumulates from the input of the dynamic model. These provide additional information about the state which can be used. This additional information increases the certainty of the state, allowing for corrections to be made across all coupled states. The sensors primarily used for this purpose are GPS, pressure sensor (for depth) and magnetometer. However, even algorithms providing information about the state will aid the certainty, such as the ZUPT algorithm.