Comparision of Probabilistic Navigation methods for a Swimming Robot

Comparision of Probabilistic Navigation methods for a Swimming Robot Anwar Ahmad Quraishi Semester Project, Autumn 2013 Supervisor: Yannic Morel BioRobotics Laboratory Headed by Prof. Aue Jan Ijspeert January 10, 2014

Contents 1 Introduction 5 2 Probabilistic Navigation Algorithms 8 2.1 Bayes filter............................................ 8 2.2 Kalman Filter........................................... 11 2.3 Particle Filter........................................... 13 3 Implementation and Numerical Simulations 17 3.1 Implementation of EKF and particle filter for the swimming robot............. 17 3.2 Numerical simulations...................................... 20 4 Performance Metrics and Tuning of Parameters 24 4.1 Performance Metrics....................................... 24 4.2 Tuning of Parameters...................................... 25 5 Experimental Tests 29 5.1 Experimental Setup....................................... 29 5.2 Implementing EKF algorithm in closed loop.......................... 29 5.3 Results............................................... 30 6 Conclusion 32 List of Figures 33 3

Chapter 1 Introduction The objective of the ongoing project EnviRobot, of which the Biorobotics Laboratory is an active participant, is to develop a swimming robot capable of tracing sources of pollutants in water bodies. The robot is to be equipped with a suite of biological sensors, allowing it to measure the concentration of a range of different pollutants in its vicinity. The data collected by the robot along its trajectory, together with the information from the navigation system, which includes a Global Positioning System receiver and an electronic compass, can be used to assess the chemical concentration trends in water. Guiding the robot in such a manner that it moves in the direction of greater increase in concentration, one may hope to uncover areas of interest, such as zones of accumulation and possible pollution sources. To estimate such concentration trends, it is necessary to consider the position at which each concentration measurement was performed. More specifically, the robot attempts to construct a local map of the function going from position to concentration. In doing so, if the position information corresponding to each measure is erroneous, the reconstructed map will be accordingly deformed, leading to inaccurate concentration trend estimates (see figure 1.1), and ineffective guidance commands for the robot. Note however that the position information need not necessarily be exact in an absolute frame of reference. Specifically, the estimation process operates on a sliding window of n samples. These last n samples are used to reconstruct the trend in the robot s surroundings. So long that the relative positioning between all sample positions is faithful to reality (meaning, that the shape of the trajectory over the estimation window is preserved), a large constant offset in the position estimates will have little to no impact on quality of estimation. If the shape of the estimated trajectory is close that of the actual trajectory, we are able to avoid large deformations of the estimated local concentration profile, and the robot will be able to pic up the correct trend in its local position. Unfortunately, the measurements from the navigation instruments are typically colored by noise and they need to be processed before being exploitable by the concentration estimation algorithm. This is the tas that is addressed in this project. Specifically, we implement and assess performance of, both through numerical simulations but also through experimental tests, a number of probabilistic navigation methods. The objective lies in estimating a probability density function of the robot s position by exploiting data provided by navigation instruments, and complementing that data using a simple model of the swimming robot s dynamics. We begin by deriving a Bayes filter ([1]) for the case considered. This filter provides a recursive relationship allowing to compute the probability distribution of the current position (conditioned on measurement and control input history) using a measurement model (describing how the robot s state and measures are related), a motion model (dynamic model of the swimming robot), and the probability 5

(a) Real and Measured trajectory (b) Real and Estimated trajectory Figure 1.1: Concentration measures superimposed over Measured and Estimated trajectory Figure 1.2: The AmphiBot 3 robot, developed at the Biorobotics Laboratory, EPFL. Image source: BioRobotics Laboratory website distribution of position at the previous time step. Note that Bayes filter only provides a general formalism for solving probabilistic problems. To quantitatively address the navigation problem, we then translate the obtained Bayes filter using two different implementation methods. Namely, we construct an Extended Kalman Filter (EKF, [1]), which relies on linearization of the model of the system s dynamics to enhance sensor measures. While the EKF is a very well established tool, it is important to note that, on a fundamental level, it relies on an approximation of the vehicle dynamics. This approximation is in many cases justified in practice. However, on a theoretical level, the legitimacy of EKF has in some instances been questioned. To circumvent such issues, we also develop a Particle Filter (PF, [1]), which also allows to use a system model in improving state estimates, but with no requirements of linearization. Instead, the technique uses a Monte Carlo approach, by which a cloud of particles, whose movements are governed by the system model, is used to assess the robot s position. The robotic platform considered in this project is the AmphiBot 3 swimming robot (a new version of the one introduced in [2], see Fig 1.2), developed within the EPFL BioRobotics Laboratory. This eel-lie robot is composed of a chain of identical modules, connected by actuated rotary joints, and terminated with a rigid caudal appendage. Movements of the joints is coordinated (typically, using a Central Pattern Generator, CPG [2]) is such a manner that a traveling wave is propagated along the robot s body, in the rostro-caudal direction. Amplitude of joints oscillation along the body is modulated in such a manner as to mimic the swimming gait of an eel or a lamprey, with amplitudes increasing in the rostro-caudal direction (low near the head, larger near the tail). AmphiBot provides an interesting platform to test the navigation algorithms presented hereafter. This report is structured as follows. In Chapter 2, we present a detailed derivation and discussion of 6

the Bayes filter, the Extended Kalman Filter and the Particle Filter, designed to address our navigation problem. Efficacy of these algorithms is first illustrated using numerical simulations, whose results are presented in Chapter 3. Then, in an effort to enhance performance of the algorithms, we develop a number of performance metrics, allowing to assess, among other things, to what extent navigation is facilitating the concentration trend estimation procedure. Using these metrics, we are able to tune algorithm parameters and improve navigation accuracy, as discussed in Chapter 4. Finally, the EKF is tested experimentally on AmphiBot 3 ([2]), using the setup available in the BioRobotics Laboratory. Experimental results are provided in Chapter 5. Chapter 6 concludes this report. 7

Chapter 2 Probabilistic Navigation Algorithms In order to estimate the trajectory of the robot, two probabilistic navigation algorithms, namely an extended Kalman filter and a particle filter are used in this project. These algorithms are specific implementations of a general method called Bayes filter. This chapter first gives an introduction to Bayes filter in section 2.1 and then explains the EKF and particle filter algorithms in the latter sections. 2.1 Bayes filter The Bayes filter (as shown in [1]) is a general algorithm for calculating probability distribution over the state of the robot in its environment. It recursively calculates the probability distribution over the current state conditioned on measurement and control input history using the probability distribution at the previous time step, a measurement model that describes the relationship between state and sensor measurement, and a motion model that describes the dynamics of the robot. If at time step, the state of the robot is denoted by s, the sensor measurements by z and the control input by u, and the probability distribution to be computed is p(s Z, U ), (2.1) [ [ where Z z 1... z ], the history of measurements and U u 1... u ], the history of control inputs. To compute this distribution, a motion model and a measurement model are needed. The motion model describes the dynamics of the robot. It can be represented by a probability distribution over the current state, conditioned on previous state and control input as p(s s 1, u ). (2.2) The measurement model describes how sensor measurements are generated from the robot s current state. It can be represented by a probability distribution over sensor measurements conditioned on robot state as p(z s ). (2.3) s, u and z can be vectors, whose dimensions would depend on how the system is modeled. For example, the [ state s could be a vector containing the robot position in two dimensions and its heading, ]. then s x y ψ 8

Figure 2.1: A Dynamic Bayesian Networ that depicts the evolution of states and measurements In summary, (2.2) is called the motion model and (2.3) is called measurement model. Because of the inaccuracy in the sensors and actuators, the measurements and motion of the robot are not deterministic. As a result, the evolution of both state and measurements are governed by probabilistic laws and not by deterministic functions. These probabilistic relations can be depicted by a graph as shown in figure 2.1. Such a graphical representation is nown as a Dynamic Bayesian Networ (DBN). Thus, DBNs are in some sense a generalization of the Kalman filter and Particle filter methods. 2.1.1 Bayes filter algorithm The Bayes filter is a recursive algorithm. Each iteration of the algorithm can be divided into tho steps, a prediction step and a correction step. In the prediction step, a predicted probability distribution is computed using the motion model. In the correction step, the sensor measures are used to compute the probability distribution of the current state using the predicted probability distribution. One iteration of the algorithm is shown here. Taing as input probability distribution at the previous time step, p(s 1 Z 1, U 1 ), the control input, u, measurement z, the current probability distribution, p(s Z, U ) is calculated as p(s Z 1, U ) = p(s s 1, u )p(s 1 Z 1, U 1 )ds 1, (2.4) p(s Z, U ) = ηp(z s )p(s z 1, u ), (2.5) where η is a normalization factor. The algorithm leaves open as to what the motion and measurement probabilities are. They are defined differently in different implementations of the algorithm. Two implementations, the Extended Kalman filter and Particle filter are discussed in the latter sections. 2.1.2 Derivation of the Bayes filter In the derivation of Bayes filter, it is assumed that the Marov assumption holds. The Marov assumptions states that future state depends exclusively on the current state and future control input, and is independent of the previous state history. It is illustrated by figure 2.1, where for example, s +1 depends on x and on u +1, but not on x 1 directly. 9

Using Bayes conditional probability theorem, we expand the posterior as follows, p(s Z, U ) = p(z, U s ) p(s ). (2.6) p(z, U ) We then expand the first term in (2.6) using the definition of conditional probabilities to obtain p(s Z, U ) = p(z s, Z 1, U ) p(u, Z 1 s ) p(s ). (2.7) p(z, U ) We then recombine the two last terms of the denominator. In particular, using Bayes inference theorem, we have hence p(s Z 1, U ) = p(u, Z 1 s ) p(s ), (2.8) p(z 1, U ) p(u, Z 1 s )p(s ) = p(s Z 1, U ) p(z 1, U ). (2.9) substituting (2.9) into (2.7), we obtain p(s Z, U ) = p(z s, Z 1, U ) p(s Z 1, U ) p(z 1, U ), (2.10) p(z, U ) which, using Bayes theorem to combine the last term of the numerator with the denominator, yields p(s Z, U ) = p(z s, Z 1, U ) p(s Z 1, U ). (2.11) p(z Z 1, U ) The denominator in (2.11) being independent of s, we obtain p(s Z, U ) p(z s, Z 1, U ) p(s Z 1, U ). (2.12) Then, note that the sensor output at time instant t is exclusively function of the state s. Thus, p(s Z, U ) p(z s ) p(s Z 1, U ). (2.13) Using the theorem of total probabilities, we expand the second term on the right-hand-side of (2.13) as follows, p(s Z, U ) p(z s ) p(s s 1, Z 1, U )p(s 1 Z 1, U )ds 1. (2.14) Finally, using the Marov assumption, we obtain p(s Z, U ) p(z s ) p(s s 1, u ) p(s 1 Z 1, U 1 )ds 1, (2.15) which corresponds to a factored form of the Bayes filter, and can trivially be decomposed into it s prediction-correction form, shown in (2.4),(2.5). 10

2.2 Kalman Filter The Kalman filter is a popular technique used for navigation and position tracing of mobile robots. This algorithm is a specific implementation of a general Bayes filter. It represents the probability distribution of the state of the robot by a Gaussian distribution. The benefit is that a Gaussian distribution can be described by just two parameters, namely the mean and covariance. This maes such a representation efficient in terms of memory and computation. This section provides a description of the Kalman filter and of the EKF algorithms (see [1] for a detailed description and mathematical derivation). The Kalman filter is a two-step recursive algorithm, consisting of the prediction step and the correction step. At each time step, it taes as input the probability distribution of the state at the previous time step, the current control input and the current measurements to compute the current probability distribution of state. For the first time step, the initial conditions need to be specified, which shall be discussed in the chapter on Implementation (chapter 3). In the Kalman filter method, the motion model is assumed to be a linear function. Hence, the predicted state at time step, s is a linear function of state at the previous time step, x 1 and the most recent control input u, with added Gaussian noise as s = A x 1 + B u + ε, (2.16) where the state, s is a vector of dimension n and control input, u is a vector of dimension m. A is a matrix of dimension n n and B is a matrix of dimension n m. The random variable ε is a Gaussian random vector of dimension n that models the motion noise. It has zero mean and a covariance R. Hence, the motion probability, p(s u, s 1 ) is a Gaussian distribution with mean A s 1 + B u and covariance R. p(s u, s 1 ) N (A s 1 + B u, R ). (2.17) Similarly, The measurement model is also assumed to be linear. Hence, measurement at time step, z is expressed as a linear function of the state s with added Gaussian noise as z = C s + δ, (2.18) where, z is a vector of dimension p. C is a matrix of dimension p n. The random variable δ is a Gaussian random vector of dimension p that models measurement noise. It has zero mean and covariance Q. Hence, the measurement probability p(z s ) is a Gaussian distribution with mean C s and covariance Q. p(z s ) N (C x, Q ). (2.19) The probability distribution of the initial state must be a Gaussian, with mean µ and covariance Σ. 2.2.1 The Kalman filter algorithm The Kalman filter algorithm computes the current probability distribution, p(s Z, U ) using the old distribution p(s 1 Z 1, U 1 ), new control input, u and new measurement, z. It has two steps, prediction step and the correction step. 11

In the prediction step, the predicted distribution is calculated as µ = A µ 1 + B u, (2.20) Σ = A Σ 1 A T + R. (2.21) The predicted distribution is a multivariate Gaussian distribution with mean µ and covariance Σ. In the correction step, the sensor measurement vector, z is used to compute the current distribution as K = Σ C T (C Σ C T + Q ) 1, (2.22) µ = µ + K (z C µ ), (2.23) Σ = (I K C ) Σ. (2.24) The probability distribution of the current state is a multivariate Gaussian distribution with mean µ and covariance Σ. This will be the input to the filter for the next iteration. 2.2.2 Extended Kalman filter The Kalman filter assumes that the state transition and the measurement models are linear functions. However, the dynamics of most mobile robot systems, including ours, are described by non-linear functions. s = g(s 1, u ) + ε, (2.25) z = h(s ) + δ, (2.26) where g and h are nonlinear functions. ε and δ are Gaussian random vectors as defined before. Here, the assumption of linearity does not hold. The reason for the linearity assumption is the following. The Kalman filter represents distributions with a Gaussian distribution. The linear transformation of a Gaussian function is again a Gaussian. Hence, the new distribution, p(s Z, U ), computed from the old distribution, p(s 1 Z 1, U 1 ) will be a Gaussian distribution if the linearity assumption holds. However, a linear approximation of the non linear function, g(s 1, u ) could be constructed with the help of a first order Taylor expansion around µ 1 as follows. g(s 1, u ) = g(µ 1, u ) + g (µ 1, u )(s 1 µ 1 ) (2.27) = g(µ 1, u ) + G (s 1 µ 1 ), (2.28) where g (µ 1, u ) represents the partial derivative with respect to the state, g(s, u )/ s. Similarly, for the measurement function, h(x ) = h( µ ) + H (s µ ). (2.29) Here, G and H are Jacobians of the functions g and h computed at time step. 12

Now, the state transition and measurement probabilities are the following. p(s u, s 1 ) N (g(s 1, u ), R ), (2.30) p(z s ) N (h(s ), Q ). (2.31) The Extended Kalman filter uses these linear approximations to compute the probability distribution over robot state. The prediction step computes the predicted distribution. µ = g(µ 1, u ), (2.32) Σ = G Σ 1 G T + R. (2.33) The correction step uses the sensor measurement to compute the updated (current) probability distribution. K = Σ H T (H Σ H T + Q ) 1, (2.34) µ = µ + K (z h( µ )), (2.35) Σ = (I K H ) Σ. (2.36) The probability distribution of the current state is a multivariate Gaussian distribution with mean µ and covariance Σ. This will be the input to the filter for the next iteration. 2.3 Particle Filter The particle filter is an implementation of the Bayes filter that represents probability distributions with a non-parametric distribution. For particle filter, the probability distribution of the robot s state is represented by a set of particles, each of which reflects one possible state of the robot. Seen another way, particles are random samples drawn from the probability distribution of the robot state. Hence, the lielier an area of the state space is, the greater the density of particles, as shown in figure 2.2. In other words, the density of particles is higher near the most liely state. The advantage is that it is possible to represent any arbitrary probability distribution with particles, not just linear Gaussian as in the case of Kalman filter. This section explains the particle filter algorithm (see [1] for a detailed description and mathematical derivation). Figure 2.2: Left: Example of an arena with position of the robot shown by the blue circle. Right: Possible distribution of particles in the arena. Notice that the density is higher at the position of the robot. 13

The particle filter is also a recursive, two step algorithm consisting of a prediction and an update step. The algorithm is initialized with a certain distribution of particles depending on the initial conditions. In the prediction step, the predicted positions of all the particles are calculated by propagating each of the particles state at the previous step through the system dynamics, using the motion model. In the correction step, the sensor measurements are used to resample the obtained, predicted distribution. Specifically, particles whose states are unliely (in light of sensor measures) are discarded, remaining (liely) particles are duplicated to replace lost ones." Motion model Suppose we have a set of M particles, χ. The set χ represents the probability distribution of the robot state at time step. [ χ s [1]... s [M] ], (2.37) where s [m] denotes the state of the m th particle. The predicted state of each particle is a function of the previous state of that particle and the recent control input, with an added noise that models the noise in actuators of the robot. s [m] Here, g, the motion model, can be any function, linear or nonlinear. = g(s [m] 1, u ) + ε. (2.38) ε can be any random vector (including a Gaussian random variable) that appropriately models the motion noise. It has the same dimension as s [m]. Thus, the motion probability of mth particle can be written as p(s[m] s[m] 1, u ). Hence, the predicted state of each particle, s [m] can be seen as a sample drawn from this probability distribution. s [m] p(s [m] s[m] 1, u ). (2.39) Measurement model The measurement at time step, z, is expressed as a function of the state at at that time step plus an added noise that models the noise in the sensors. z = h(s [m] ) + δ. (2.40) Here h, the measurement model, can be any function and δ can be any random vector that appropriately models the noise in the sensors. It has the same dimension as z. Hence, the measurement probability can be written as p(z s [m] ). It is worth observing that for a given sensor measurement z, the probability p(z s [m] ) is higher if the state of mth particle, s[m] is in agreement with the sensor measurement z. 2.3.1 Particle filter algorithm The Particle filter algorithm is a recursive algorithm. At each time step, it computes the current probability distribution of the robot state, p(s Z, U ), which is represented by a set of particles, χ, using the old distribution, p(s 1 Z 1, U 1 ) represented by the particle set, χ 1, current control input, u, and current measurement, z. It has two steps, a prediction step and a correction step. 14

In the prediction step, the motion model is used to predict the new state of each particle. This is done by drawing a random sample from the probability distribution p(s [m] s[m] 1, u ) for each particle m. The predicted probability distribution of the robot state is then represented by the set of particles with the predicted state. s [m] p(s [m] s[m] 1, u ), (2.41) [ ] χ = s [1]... s [M]. (2.42) In the correction step, the measurement model and current sensor measurement is used to compute the current probability distribution of the robot state. First, each particle in the set χ is assigned a weight w [m]. w [m] = p(z s [m] ). (2.43) The particles whose states are compatible with the newly available sensor measurements (in the light of the measurement model, (2.40)) receive a higher weight than those particles whose states are not compatible with the new sensor measurements. Next, the particles are resampled according to their weights to generate the new set of particles representing the current probability distribution. To resample, a particle is first drawn form the set χ with probability proportional to its weight, w [m]. A copy of this particle is added to the new particle set. The drawn particle is then placed bac in the set χ. This process is repeated M times, where M is the total number of particles. Repeat M times: Draw particle s [p] from χ with probability w [p] Add a copy of s [p] to the new particle set χ Replace s [p] bac into χ The set of particles χ represents the probability distribution of the current robot state, p(s Z, U ). 2.3.2 A variation of particle filter algorithm In the particle filter algorithm explained in the previous section, in the prediction step, a predicted position is calculated for each particle. In the correction step, each particle is assigned a weight according to the measurement model, and the particle set is resampled. An alternative approach, which can be used if sensor and actuator errors are modeled by Gaussian distributions, is to predict the new particle positions and correct their positions using the measurement model in one step. The weights for resampling are then calculated accordingly. This approach is adapted from the FastSLAM algorithm ([3]). Let R be the covariance of the motion error, ε (see equation (2.38)) and Q be the covariance of the measurement error, δ (see equation (2.40)). The combined prediction-correction step is then s [m] = g(s [m] 1, u ) + (R 1 [ χ = s [1]... s [M] + Q 1 )Q 1 (z h(s [m] ), (2.44) ], (2.45) 15

where the term (R 1 + Q 1 )Q 1 (z h(s [m] ) incorporates the sensor measurement into the state of particle m. The weights are calculated as w [m] 1 = 2π(R + Q ) exp( 1 2 (z h(s [m] )T (R + Q )(z h(s [m] )). (2.46) Note that there is an implicit restriction that the error covariance matrices, R and Q should have the same dimension. We have used this approach in this project. 2.3.3 Estimated state The final step in the particle filter algorithm is computing the estimated state, which is the expected value of the probability distribution of state. Particle filter represents this distribution by a set of particles. Hence, the final state estimate is computed by taing the weighted mean of the states of particles that have a weight higher than a certain threshold. For example, if the weighted mean is to be calculated for particles that have a weight of at least w t, ŝ = where w [i] = M i=1 [i] w [i] s, (2.47) M w [i] i=1 w [i], if w [i] w t (2.48) 0, if w [i] < w t 16

Chapter 3 Implementation and Numerical Simulations 3.1 Implementation of EKF and particle filter for the swimming robot In the previous sections, the EKF and particle filter algorithms were explained. The most important aspects of of implementation are: defining the state variable, defining the motion and measurement models, characterizing the sensor and actuator noise, and initializing the filter algorithm. The motion model and measurement model are defined using the physics and dynamics of the robot. Sensor and actuator noise can be modeled with the help of experiments. Sensor noise characteristics are also usually specified by their datasheets. Both EKF and particle filter are recursive algorithms. To initialize them, initial conditions need to be provided. These initial conditions are problem specific and depend on how much information about the initial state of the robot is available. 3.1.1 Robot state, control input and measurements The state of the robot is defined to comprise of the position, heading, speed and derivatives of speed and heading. Because the robot remains near the surface, we can approximate its position as being constrained to a 2-dimensional plane. [ s x y ψ v v ψ ] T. (3.1) The average curvature of the robot, α, which is calculated from the joint angles, is available at each time step for computing the change in heading of the robot in the motion model. This forms the control input. u = α. (3.2) To streamline derivations, we consider a case in which the robot exhibits a single, constant steady state speed, v c (which one may thin of as cruise speed). Accordingly, the speed of the robot is modeled to increase from zero initially to this steady state speed (using the equivalent of a second order low-pass 17

filter). The measurement consists of position in 2 dimensions measured with the help of GPS and heading measured with the help of an electronic compass, that is, [ ] T z x y ψ. (3.3) 3.1.2 Motion and measurement model A simple dynamic model for the robot is assumed. x and y are calculated by integrating velocity. ψ is calculated from the average curvature of the robot α. Note that the following model corresponds to a discretization of a simple continuous time model, in which Euler integration is used. x = x 1 + v 1 cos(ψ )dt, (3.4) ȳ = y 1 + v 1 sin(ψ )dt, (3.5) ψ = ψ 1 + ψ 1 dt, (3.6) v = v 1 + v 1 dt, (3.7) v = v 1 + v 1 dt, (3.8) ψ = ψ 1 + ψ 1 dt, (3.9) where ψ v are calculated as v = 1 v 2 (v v c ), (3.10) ψ = 3 ψ + 4 α, (3.11) where 1, 2, 3 and 4 are constants and α is the average curvature of the robot which directly affects joint angles. The equation (3.10) is such that the robot s speed gradually increases from 0 to v c. Equation (3.11) relates the average curvature of the robot, α, to the angular acceleration of the robot. The measurements are position (x, y) from GPS and heading ψ from compass, which are part of the state variable. Hence, the measurement function happens to be a linear function of the state in our case. z = C s, (3.12) C = 3.1.3 Sensor and actuator noise 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0. (3.13) The sensor and actuator noise are modeled with a Gaussian distribution for both EKF and Particle filter algorithms. They are assumed to be zero mean with some variance. The values of these variances are important parameters of the algorithm. 18

A GPS measure typically has an error of the order of a few meters, and an electronic compass, an error of the order of a degree. The GPS is modeled with error covariance of σ GPS and compass with covariance of σ c. It is assumed that the errors in measurement of each quantity is independent of the other, which is a reasonable assumption because GPS and compass are independent devices. The measurement consists [ ] T of 3 variables, z x y ψ and the measurement error covariance, Q is defined as Q = σ GPS 0 0 0 σ GPS 0. (3.14) 0 0 σ c [ ] T The state vector, s x y ψ v v ψ consists of 6 variables. It is again assumed that the errors in each of the variables is independent of all others. The motion error covariance, R is then defined as R = σ xy 0 0 0 0 0 0 σ xy 0 0 0 0 0 0 σ ψ 0 0 0 0 0 0 σ v 0 0 0 0 0 0 σ v 0 0 0 0 0 0 σ ψ. (3.15) However, some approximations are made here. Since speed is constant once it reaches v c, and ψ are directly calculated from α, σ v, σ v, σ ψ are assumed to be zero. The numerical values for the measurement error covariance should ideally be chosen according to the typical errors in the sensor measurements, either determined experimentally or as mentioned in their specifications/datasheets. Similarly, the numerical values motion error covariance should be chosen according to the typical errors in the actuators of the robot. In any case, since these parameters directly affect the performance of navigation algorithms, they need to be further tuned to achieve improvements in practical scenarios. In this project, multiple simulations are run and the values of these parameters that give the best performance are determined as discussed in chapter 4. Note that the linearity assumption in EKF algorithm requires that the noise be modeled by a Gaussian distribution, however there is no such restriction in the particle filter. 3.1.4 Error covariance matrices for Particle filter Note that in the Particle filter algorithm, the error covariance matrices need to have the same dimension, as explained in section 2.3.2. Hence, there is a slight modification in the definition of R for particle filter. However, Q is defined as in (3.14). R = σ xy 0 0 0 σ xy 0, (3.16) 0 0 σ ψ which has the same dimension as Q. This does not change any of the motion or measurement model equations, because σ v, σ v, σ ψ are assumed to be zero. 19

3.1.5 Initial conditions The algorithm is initialized with the first measurement from the GPS and compass. The algorithms were tested with numerical simulations on several trajectories. The results of both algorithms are presented. 3.2 Numerical simulations Numerical simulations were performed on trajectories generated by a high fidelity hydrodynamic numerical model, where the robot dynamics were modeled using the recursive algorithms explained in [4] and the hydrodynamic forces and interactions between the robot and surrounding fluid were represented using the model introduced in [5]. 3.2.1 EKF simulation results The EKF algorithm was tested with numerical simulation on several trajectories. The results on two different trajectories is shown in figures 3.1 and 3.2. The estimated trajectory tends to follow GPS measures, but is made consistent with the dynamics of the robot by virtue of the model used in filtering. 150 Real Measured EKF 140 y [m] 130 120 110 100 200 210 220 230 240 250 260 x [m] Figure 3.1: EKF simulation on trajectory 1 In EKF, the new probability distribution over state is computed using the measurement model and the motion model. This distribution depends on the error covariances of motion and measurement. Thus, the motion and measurement error covariance, in essence, control the influence of motion model and the sensor measurements on the estimated trajectory. Estimated trajectories obtained by setting the motion error covariance and measurement error covariance to extreme values are shown in figures 3.3 and 3.4. High measurement error covariance and low motion error covariance implies the motion model has higher influence than the measurement model on the estimated trajectory. In other words, the algorithm trusts the dynamics of the robot more and almost ignores the measurements, resulting in the estimated trajectory drifting away from the real trajectory (figure 3.3). In contrast, high motion error covariance and low measurement error covariance implies that the algorithm trusts the measurement more and ignores the dynamics of the robot, resulting in the estimated trajectory closely following the measured trajectory (figure 3.4). In practice, it is preferable to have a balance between trusting the sensor 20

155 150 Real Measured EKF 145 140 y [m] 135 130 125 120 115 240 250 260 270 280 x [m] Figure 3.2: EKF simulation on trajectory 2 measurements and the model of the dynamics of the robot, so that the estimated trajectory remains close to the real trajectory but is also consistent with the dynamics of the robot. 130 Real Measured Kalman Filter 125 120 y [m] 115 110 105 100 200 205 210 215 220 225 230 235 x [m] Figure 3.3: Low motion error covariance, high measurement error covariance 130 Real Measured Kalman Filter 125 120 y [m] 115 110 105 100 195 200 205 210 215 220 225 230 235 x [m] Figure 3.4: High motion error covariance, low measurement error covariance 21

3.2.2 PF simulation results The results of numerical simulation of the particle filter algorithm are shown in figures 3.5 and 3.6. The trajectory estimated by PF matches more closely with the real trajectory, while still following GPS measures. In other words, it is easier to achieve an agreeable balance between trust model and measures. 150 Real Measured PF 2 140 y [m] 130 120 110 100 200 210 220 230 240 250 260 x [m] Figure 3.5: Particle filter simulation on trajectory 1 155 150 Real Measured PF 2 145 140 y [m] 135 130 125 120 115 240 250 260 270 280 x [m] Figure 3.6: Particle filter simulation on trajectory 2 3.2.3 Comparison between EKF and particle filter A comparison between trajectories estimated by EKF and Particle filter is shown in figures 3.7 and 3.8. The performance of the particle filter is marginally better than that of the EKF, as can be seen in table 3.1. 22

Trajectory 1 Trajectory 2 Mean estimation error of PF 3.14 ± 1.35m 2.22 ± 1.00m Mean estimation error of EKF 3.29 ± 1.70m 3.07 ± 1.50m Mean measurement error 3.70 ± 1.22m 3.15 ± 1.40m Table 3.1: Quantitative comparison of performance of EKF and Particle filter 150 PF 2 EKF Real 140 y [m] 130 120 110 100 200 210 220 230 240 250 260 x [m] Figure 3.7: Comparison of trajectories estimated by EKF and Particle filter on trajectory 1 155 150 PF 2 EKF Real 145 140 y [m] 135 130 125 120 115 240 250 260 270 280 x [m] Figure 3.8: Comparison of trajectories estimated by EKF and Particle filter on trajectory 2 23

Chapter 4 Performance Metrics and Tuning of Parameters 4.1 Performance Metrics It was shown in the previous chapter that the adjustable parameters (motion and measurement error covariance) have a direct impact on the behavior of the navigation algorithm and the estimated trajectory (figures 3.4, 3.3). The goal here is to minimize the error between the estimated and the real trajectory (which is nown in the simulation scenario) and preserve the shape of the original trajectory. To quantitatively measure the performance of the algorithm and asses the impact on estimation of concentration trends, several metrics are developed. They are estimation error, shape error, concentration error and direction estimation error. 4.1.1 Estimation error Estimation error represents the difference between the actual and estimated position of the robot over its trajectory. It is calculated as e e = 1 n (ˆx x ) 2 + (ŷ y ) 2, (4.1) n =1 where (ˆx, ŷ ) is the estimated position and (x, y ) is the real position at time step and n is the total number of sample points in the trajectory. 4.1.2 Shape error Shape error is a measure of the deformation of the shape of the estimated trajectory as compared to the real trajectory, without considering the possible offsets. It is computed by averaging the deformation over a sliding window of w sample points, because the concentration estimation algorithm also operates on a sliding window. Deformation over one window is computed by translating the estimated trajectory in such a way that the first point on the estimated trajectory in the window, (ˆx, ŷ ) coincides with the corresponding point on the real trajectory, (x, y ), and then computing the estimation error over the remaining points in the 24

window. The translation of the estimated trajectory is done to disregard the possible constant offsets in the real and estimated trajectories. Deformation in one window is calculated as E = 1 +w (ˆx l x l + x ) 2 + (ŷ l y l + y ) 2. (4.2) w The shape error is computed by averaging the error over all the windows. l= e s = 1 n w E. (4.3) n w =1 4.1.3 Concentration error This concentration is computed using a regression technique, the detailed description of which lies beyond the scope of this project report. Using the concentration measures and the corresponding estimated position at which these measures were obtained, a local model of concentration dispersion is estimated.the intent with e c is to directly assess the impact of the navigation algorithm on the estimation technique. e c = 1 n (ĉ c ) 2. (4.4) n where ĉ is the estimated concentration and c is the real concentration at time step. =1 4.1.4 Direction estimation error The objective of the concentration estimation technique mentioned in the previous subsection is to estimate the direction of greatest increase in concentration at the robot s location, noted ψ s. Direction estimation error measures the impact of the navigation algorithm on the estimation of ψ s. e ψ = 1 n ( n ˆψ s ψ s ) 2. (4.5) =1 where ˆψ s is the estimated direction of greatest concentration gradient and ψ s is the real direction of greatest concentration gradient at time step. 4.2 Tuning of Parameters The noise in sensors and actuators is modeled by Gaussian distributions, characterized by their mean and variance. The errors are assumed to be zero mean and having some finite variance. As explained in section 3.1.3, these are important tunable parameters that affect the performance of the estimation algorithms. The effect of following parameters is considered. σ GPS σ c σ xy σ ψ GPS measurement error variance Compass measurement error variance Position prediction error variance Heading prediction error variance 25

The simulations are run several times by gradually varying these parameters. For each run of the simulation, the performance is measured using all the performance metrics defined above. This approach is similar to grid search. 4.2.1 Impact of measurement and prediction error variance As can be seen in figure 4.1, higher σ GPS results in lower position and shape error. The direction error has a minima for an intermediate value of σ GPS. The Compass is fairly precise and less erroneous compared to the GPS. Increase in σ c leads to increase in position error and shape error. The direction error has a maxima for an intermediate value for σ c. The direction of highest gradient in concentration is not estimated by the navigation algorithm. Hence, it is influenced by several factors not related to position estimation. However, it confirms the hypothesis that direction error is higher if the estimated trajectory is deformed (i.e., if shape error is higher), because it largely follows the trend in shape error. The prediction error variance is varied by setting the measurement error variance to the best values obtained in the previous step. It is seen in figure 4.2 that Position error varies little with changes in σ xy and σ ψ. However, shape error and direction error increases on increasing σ xy, and decreases on increasing σ ψ. The conclusion is that the compass measurement is reliable, and hence, it can be modeled with low measurement error, typically setting σ c to a value in accordance with the sensor specifications (of the order of a degree) and σ ψ to a comparatively higher value. The trajectory measured by GPS is very deformed (see section 3.2 for plots of trajectories). Hence, the shape error is higher if GPS measurement has higher influence on the estimated trajectory, which happens when σ GPS is low and σ xy is high. However, if σ GPS is set too high and σ xy is set too low, the estimated trajectory largely follows the dynamics of the system (which is closer to pure inertial navigation), and will drift away from the real trajectory. Hence, in our case, σ GPS is set to a value higher than typical error in GPS (which is of the order of a few meters) and σ xy is set to a comparatively low value. There was no significant effect of the parameters on concentration error. 26

2.55 2.8 2.5 2.7 2.45 2.6 Position error [m] 2.4 2.35 2.3 2.25 2.2 Position error [m] 2.5 2.4 2.3 2.2 2.15 2.1 2.1 2.05 0 50 100 150 GPS error variance [m 2 ] 2 0 5 10 15 20 25 30 35 40 45 Compass error variance [deg 2 ] (a) Position error against σ GPS (b) Position error against σ c 0.35 0.35 0.3 0.3 Shape error, [m] 0.25 0.2 Shape error, [m] 0.25 0.2 0.15 0.15 0.1 0 50 100 150 GPS error variance [m 2 ] 0.1 0 5 10 15 20 25 30 35 40 45 Compass error variance [deg 2 ] (c) Shape error against σ GPS (d) Shape error against σ c 0.83 1 Direction estimation error, [rad] 0.82 0.81 0.8 0.79 0.78 0.77 0.76 0.75 0.74 Direction estimation error, [rad] 0.95 0.9 0.85 0.8 0.75 0.73 0 50 100 150 GPS error variance [m 2 ] (e) Direction error against σ GPS 0.7 0 5 10 15 20 25 30 35 40 45 Compass error variance [deg 2 ] (f) Direction error against σ c Figure 4.1: Impact of GPS and compass measurement error variance on performance 27

2.6 2.025 2.5 2.02 Position error [m] 2.4 2.3 2.2 2.1 Position error [m] 2.015 2.01 2.005 2 2 1.995 1.9 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 Position prediction error variance [m 2 ] 1.99 0 5 10 15 20 25 30 35 40 Heading prediction error variance [deg 2 ] (a) Position error against σ xy (b) Position error against σ ψ 0.11 0.0118 0.1 0.0116 0.09 0.0114 Shape error, [m] 0.08 0.07 0.06 0.05 0.04 Shape error, [m] 0.0112 0.011 0.0108 0.0106 0.03 0.0104 0.02 0.0102 0.01 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 Position prediction error variance [m 2 ] 0.01 0 5 10 15 20 25 30 35 40 Heading prediction error variance [deg 2 ] (c) Shape error against σ xy (d) Shape error against σ ψ 0.84 0.78 Direction estimation error, [rad] 0.82 0.8 0.78 0.76 0.74 Direction estimation error, [rad] 0.77 0.76 0.75 0.74 0.73 0.72 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 Position prediction error variance [m 2 ] (e) Direction error against σ xy 0.72 0 5 10 15 20 25 30 35 40 Heading prediction error variance [deg 2 ] (f) Direction error against σ ψ Figure 4.2: Impact of Position and heading prediction error variance on performance 28

Chapter 5 Experimental Tests Once the parameters of the navigation algorithms are adjusted to improve their performance, the EKF algorithm is tested experimentally on the real robot. The tests are performed in an indoor environment at the BioRobotics laboratory. Some aspects of the real environment in which the robot is intended to be used are reproduced. The goal is to test the navigation algorithms developed in this project on the real robot hardware. The Particle filter algorithm was not tested experimentally because it is a computationally intensive algorithm, which may not run in real time on the robot hardware. Additionally, time constraints did not allow us to experiment with it. 5.1 Experimental Setup The experiments are performed in a rectangular pool of water of size 6m 1.5m at the laboratory. The pool is equipped with overhead cameras that trac the position of the robot and communicate this position to the robot via a wireless communication lin. A random error is added to this position measurement, to qualitatively emulate the type of error seen in practice. This additional error term was computed using a bounded random wal. The robot is programmed to swim around the pool and the position estimation in performed in real time. The EKF algorithm is running off-board, on a computer that is connected to the robot via a wireless communication lin. This computer receives the measured position and the average curvature of the robot at each time step over the wireless lin. The measured and estimated positions are recorded. 5.2 Implementing EKF algorithm in closed loop The EKF algorithm is used to estimate the position of the robot in real time, as the robot moves. Given the limited computational capacity and time to compute the estimated position, some approximations were made to simplify the calculations in the EKF. First, the state is defined to contain only three variables, (x, y) position and heading, ψ, as opposed to six variables as defined in equation (3.1). [ s x y ψ ] T. (5.1) The variables v, v, ψ are necessary, they are still computed in the loop, but are not a part of the EKF equations. The benefit is that the Jacobian of the motion model is reduced to a 3 3 matrix, so is the 29

y covariance matrix of the state distribution. The dimension of the motion error covariance matrix is also reduced to 3 3. R = σ xy 0 0 0 σ xy 0 0 0 σ ψ. (5.2) This reduces the complexity of the several matrix multiplications and one matrix inversion done in each time step. Additionally, since the matrix dimension is small, a closed form calculation of matrix inverse implemented, further speeding up the computation. The rest of the algorithm remains the same. 5.3 Results The tests are first performed using position measurement without noise, as shown in figure 5.1 and then an error term is added to the position measures to replicate the behavior of GPS as shown in 5.2. Note that, as seen in figure 5.2, the additional error term tends to behave worse than an actual GPS. As a result, the case considered here is actually more challenging than what could be expected in practice. This was done to test the robustness of the navigation algorithm. It can be seen that the EKF is able to filter out significant amount of noise and provide a reasonable estimate of the trajectory of the robot. 3 Measured Estimated 6 5 4 2 x(t) [m] 3 2 1 Measured Estimated 1 0 0 5 10 15 20 25 30 35 40 45 1.4 0 1.2 1 y(t) [m] 1 0.8 0.6 0.4 0.2 2 0 1 2 3 4 5 6 x 0 0 5 10 15 20 25 30 35 40 45 Time [s] (a) Real and estimated trajectory (b) x and y Position as a function of time Figure 5.1: Measured and estimated trajectory with non-noisy position measures 30

y 0 1 2 3 4 5 6 3.5 3 Measured Measured, no noise Estimated 6 5 4 2.5 2 1.5 x(t) [m] 3 2 1 Measured Measured, no noise Estimated 1 0 0 5 10 15 20 25 30 35 40 45 50 0.5 2 0 1.5 0.5 1 y(t) [m] 1 0.5 1.5 0 x 0.5 0 5 10 15 20 25 30 35 40 45 50 Time [s] (a) Real and estimated trajectory (b) x and y Position as a function of time Figure 5.2: Measured and estimated trajectory with noisy position measures 31

Chapter 6 Conclusion In this project, two Probabilistic Navigation techniques, namely the extended Kalman filter and the particle filter were implemented and tested for a swimming robot. The fine purpose of the robot, within the context of the EnviRobot project, is to trac sources of pollutants in water by building a local concentration profile and moving in the direction of greater increase in concentration. To asses the local concentration trends, it is important to have a good estimate of the trajectory of the robot, as explained in chapter 1 (see figure 1.1). The Navigation Techniques implemented in this project compute a probability distribution of the estimated position of the robot. The two techniques were compared by performing numerical simulations on several trajectories. The particle filter was found to perform marginally better than the EKF. A quantitative comparison on two trajectories is shown in table 3.1 in chapter 3. A number of performance metrics are developed to quantitatively asses the performance of EKF and Particle filter. The parameters of the algorithms that influence the estimated trajectories were tuned by running several simulations, as presented in chapter 4. The EKF represents probability distribution over state of the robot by a Gaussian. Such a probability distribution is parametric and can be completely described by two parameters, mean µ and covariance Σ, maing the algorithm efficient in terms of memory and computational complexity. However, the disadvantage is that EKF approximates dynamics of the system by linearizing the equations using a Taylor expansion. Further, it can represent the probability distributions only with a Gaussian function, which is not always a good approximation of an arbitrary probability distribution. The Particle Filter overcomes the shortcomings of the EKF in that it does not need a linear approximation of nonlinear system dynamics and it can represent any arbitrary probability distribution. On the other hand, it is computationally very expensive. The system dynamics equations have to be solved for each particle at every time step. Additional computation needs to be performed to infer the expected state of the robot from the probability distribution represented by particles. Mobile robotic platforms are usually limited in terms of computational power, maing Particle filter unsuitable for them. In the case of this project, only EKF was tested experimentally. The particle filter was not tested because of computational constraints and limited time for experimentation with it. Experimental tests were performed with the robot swimming in a pool of water at the BioRobotics Laboratory and position estimation running in real time. An "indoor GPS" system that emulates the actual GPS was used for position measurement. The results of the tests were encouraging as EKF was able to provide a reasonable estimate of the trajectory of the robot in spite of the large amount of noise in the position measures. 32