Comparision of Probabilistic Navigation methods for a Swimming Robot
|
|
- Toby Maxwell
- 5 years ago
- Views:
Transcription
1 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
2
3 Contents 1 Introduction 5 2 Probabilistic Navigation Algorithms Bayes filter Kalman Filter Particle Filter Implementation and Numerical Simulations Implementation of EKF and particle filter for the swimming robot Numerical simulations Performance Metrics and Tuning of Parameters Performance Metrics Tuning of Parameters Experimental Tests Experimental Setup Implementing EKF algorithm in closed loop Results Conclusion 32 List of Figures 33 3
4
5 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
6 (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
7 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
8 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
9 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 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 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
10 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
11 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 Σ 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
12 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 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
13 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
14 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 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
15 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 ) 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
16 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 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
17 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 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
18 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) 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 = Sensor and actuator noise (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
19 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 σ 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 σ xy σ ψ σ v σ v σ ψ. (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 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 Hence, there is a slight modification in the definition of R for particle filter. However, Q is defined as in (3.14). R = σ xy σ 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
20 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] 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] 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
21 Real Measured EKF y [m] 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 y [m] x [m] Figure 3.3: Low motion error covariance, high measurement error covariance 130 Real Measured Kalman Filter y [m] x [m] Figure 3.4: High motion error covariance, low measurement error covariance 21
22 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 y [m] x [m] Figure 3.5: Particle filter simulation on trajectory Real Measured PF y [m] x [m] Figure 3.6: Particle filter simulation on trajectory 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
23 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] x [m] Figure 3.7: Comparison of trajectories estimated by EKF and Particle filter on trajectory PF 2 EKF Real y [m] x [m] Figure 3.8: Comparison of trajectories estimated by EKF and Particle filter on trajectory 2 23
24 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 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 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
25 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 = 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. = 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
26 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 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
27 Position error [m] Position error [m] GPS error variance [m 2 ] Compass error variance [deg 2 ] (a) Position error against σ GPS (b) Position error against σ c Shape error, [m] Shape error, [m] GPS error variance [m 2 ] Compass error variance [deg 2 ] (c) Shape error against σ GPS (d) Shape error against σ c Direction estimation error, [rad] Direction estimation error, [rad] GPS error variance [m 2 ] (e) Direction error against σ GPS Compass error variance [deg 2 ] (f) Direction error against σ c Figure 4.1: Impact of GPS and compass measurement error variance on performance 27
28 Position error [m] Position error [m] Position prediction error variance [m 2 ] Heading prediction error variance [deg 2 ] (a) Position error against σ xy (b) Position error against σ ψ Shape error, [m] Shape error, [m] Position prediction error variance [m 2 ] Heading prediction error variance [deg 2 ] (c) Shape error against σ xy (d) Shape error against σ ψ Direction estimation error, [rad] Direction estimation error, [rad] Position prediction error variance [m 2 ] (e) Direction error against σ xy Heading prediction error variance [deg 2 ] (f) Direction error against σ ψ Figure 4.2: Impact of Position and heading prediction error variance on performance 28
29 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
30 y covariance matrix of the state distribution. The dimension of the motion error covariance matrix is also reduced to 3 3. R = σ xy σ xy σ ψ. (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 x(t) [m] Measured Estimated y(t) [m] x 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
31 y Measured Measured, no noise Estimated x(t) [m] Measured Measured, no noise Estimated y(t) [m] x 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
32 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
1 Kalman Filter Introduction
1 Kalman Filter Introduction You should first read Chapter 1 of Stochastic models, estimation, and control: Volume 1 by Peter S. Maybec (available here). 1.1 Explanation of Equations (1-3) and (1-4) Equation
More information2D Image Processing (Extended) Kalman and particle filter
2D Image Processing (Extended) Kalman and particle filter Prof. Didier Stricker Dr. Gabriele Bleser Kaiserlautern University http://ags.cs.uni-kl.de/ DFKI Deutsches Forschungszentrum für Künstliche Intelligenz
More informationRobot Localisation. Henrik I. Christensen. January 12, 2007
Robot Henrik I. Robotics and Intelligent Machines @ GT College of Computing Georgia Institute of Technology Atlanta, GA hic@cc.gatech.edu January 12, 2007 The Robot Structure Outline 1 2 3 4 Sum of 5 6
More informationA Comparison of Particle Filters for Personal Positioning
VI Hotine-Marussi Symposium of Theoretical and Computational Geodesy May 9-June 6. A Comparison of Particle Filters for Personal Positioning D. Petrovich and R. Piché Institute of Mathematics Tampere University
More information2D Image Processing. Bayes filter implementation: Kalman filter
2D Image Processing Bayes filter implementation: Kalman filter Prof. Didier Stricker Kaiserlautern University http://ags.cs.uni-kl.de/ DFKI Deutsches Forschungszentrum für Künstliche Intelligenz http://av.dfki.de
More informationECE276A: Sensing & Estimation in Robotics Lecture 10: Gaussian Mixture and Particle Filtering
ECE276A: Sensing & Estimation in Robotics Lecture 10: Gaussian Mixture and Particle Filtering Lecturer: Nikolay Atanasov: natanasov@ucsd.edu Teaching Assistants: Siwei Guo: s9guo@eng.ucsd.edu Anwesan Pal:
More informationL11. EKF SLAM: PART I. NA568 Mobile Robotics: Methods & Algorithms
L11. EKF SLAM: PART I NA568 Mobile Robotics: Methods & Algorithms Today s Topic EKF Feature-Based SLAM State Representation Process / Observation Models Landmark Initialization Robot-Landmark Correlation
More informationModeling and state estimation Examples State estimation Probabilities Bayes filter Particle filter. Modeling. CSC752 Autonomous Robotic Systems
Modeling CSC752 Autonomous Robotic Systems Ubbo Visser Department of Computer Science University of Miami February 21, 2017 Outline 1 Modeling and state estimation 2 Examples 3 State estimation 4 Probabilities
More informationAUTOMOTIVE ENVIRONMENT SENSORS
AUTOMOTIVE ENVIRONMENT SENSORS Lecture 5. Localization BME KÖZLEKEDÉSMÉRNÖKI ÉS JÁRMŰMÉRNÖKI KAR 32708-2/2017/INTFIN SZÁMÚ EMMI ÁLTAL TÁMOGATOTT TANANYAG Related concepts Concepts related to vehicles moving
More information2D Image Processing. Bayes filter implementation: Kalman filter
2D Image Processing Bayes filter implementation: Kalman filter Prof. Didier Stricker Dr. Gabriele Bleser Kaiserlautern University http://ags.cs.uni-kl.de/ DFKI Deutsches Forschungszentrum für Künstliche
More informationLecture 6: Bayesian Inference in SDE Models
Lecture 6: Bayesian Inference in SDE Models Bayesian Filtering and Smoothing Point of View Simo Särkkä Aalto University Simo Särkkä (Aalto) Lecture 6: Bayesian Inference in SDEs 1 / 45 Contents 1 SDEs
More informationKalman filtering and friends: Inference in time series models. Herke van Hoof slides mostly by Michael Rubinstein
Kalman filtering and friends: Inference in time series models Herke van Hoof slides mostly by Michael Rubinstein Problem overview Goal Estimate most probable state at time k using measurement up to time
More informationSLAM Techniques and Algorithms. Jack Collier. Canada. Recherche et développement pour la défense Canada. Defence Research and Development Canada
SLAM Techniques and Algorithms Jack Collier Defence Research and Development Canada Recherche et développement pour la défense Canada Canada Goals What will we learn Gain an appreciation for what SLAM
More informationRobotics 2 Target Tracking. Kai Arras, Cyrill Stachniss, Maren Bennewitz, Wolfram Burgard
Robotics 2 Target Tracking Kai Arras, Cyrill Stachniss, Maren Bennewitz, Wolfram Burgard Slides by Kai Arras, Gian Diego Tipaldi, v.1.1, Jan 2012 Chapter Contents Target Tracking Overview Applications
More informationA Study of Covariances within Basic and Extended Kalman Filters
A Study of Covariances within Basic and Extended Kalman Filters David Wheeler Kyle Ingersoll December 2, 2013 Abstract This paper explores the role of covariance in the context of Kalman filters. The underlying
More informationAutonomous Mobile Robot Design
Autonomous Mobile Robot Design Topic: Extended Kalman Filter Dr. Kostas Alexis (CSE) These slides relied on the lectures from C. Stachniss, J. Sturm and the book Probabilistic Robotics from Thurn et al.
More informationRobotics. Lecture 4: Probabilistic Robotics. See course website for up to date information.
Robotics Lecture 4: Probabilistic Robotics See course website http://www.doc.ic.ac.uk/~ajd/robotics/ for up to date information. Andrew Davison Department of Computing Imperial College London Review: Sensors
More informationLecture 7: Optimal Smoothing
Department of Biomedical Engineering and Computational Science Aalto University March 17, 2011 Contents 1 What is Optimal Smoothing? 2 Bayesian Optimal Smoothing Equations 3 Rauch-Tung-Striebel Smoother
More informationBayes Filter Reminder. Kalman Filter Localization. Properties of Gaussians. Gaussians. Prediction. Correction. σ 2. Univariate. 1 2πσ e.
Kalman Filter Localization Bayes Filter Reminder Prediction Correction Gaussians p(x) ~ N(µ,σ 2 ) : Properties of Gaussians Univariate p(x) = 1 1 2πσ e 2 (x µ) 2 σ 2 µ Univariate -σ σ Multivariate µ Multivariate
More information17 : Markov Chain Monte Carlo
10-708: Probabilistic Graphical Models, Spring 2015 17 : Markov Chain Monte Carlo Lecturer: Eric P. Xing Scribes: Heran Lin, Bin Deng, Yun Huang 1 Review of Monte Carlo Methods 1.1 Overview Monte Carlo
More informationROBOTICS 01PEEQW. Basilio Bona DAUIN Politecnico di Torino
ROBOTICS 01PEEQW Basilio Bona DAUIN Politecnico di Torino Probabilistic Fundamentals in Robotics Gaussian Filters Course Outline Basic mathematical framework Probabilistic models of mobile robots Mobile
More informationL06. LINEAR KALMAN FILTERS. NA568 Mobile Robotics: Methods & Algorithms
L06. LINEAR KALMAN FILTERS NA568 Mobile Robotics: Methods & Algorithms 2 PS2 is out! Landmark-based Localization: EKF, UKF, PF Today s Lecture Minimum Mean Square Error (MMSE) Linear Kalman Filter Gaussian
More informationMobile Robot Localization
Mobile Robot Localization 1 The Problem of Robot Localization Given a map of the environment, how can a robot determine its pose (planar coordinates + orientation)? Two sources of uncertainty: - observations
More informationThe Unscented Particle Filter
The Unscented Particle Filter Rudolph van der Merwe (OGI) Nando de Freitas (UC Bereley) Arnaud Doucet (Cambridge University) Eric Wan (OGI) Outline Optimal Estimation & Filtering Optimal Recursive Bayesian
More informationProbabilistic Fundamentals in Robotics. DAUIN Politecnico di Torino July 2010
Probabilistic Fundamentals in Robotics Gaussian Filters Basilio Bona DAUIN Politecnico di Torino July 2010 Course Outline Basic mathematical framework Probabilistic models of mobile robots Mobile robot
More informationLecture 2: From Linear Regression to Kalman Filter and Beyond
Lecture 2: From Linear Regression to Kalman Filter and Beyond January 18, 2017 Contents 1 Batch and Recursive Estimation 2 Towards Bayesian Filtering 3 Kalman Filter and Bayesian Filtering and Smoothing
More informationRobotics. Mobile Robotics. Marc Toussaint U Stuttgart
Robotics Mobile Robotics State estimation, Bayes filter, odometry, particle filter, Kalman filter, SLAM, joint Bayes filter, EKF SLAM, particle SLAM, graph-based SLAM Marc Toussaint U Stuttgart DARPA Grand
More informationMobile Robot Localization
Mobile Robot Localization 1 The Problem of Robot Localization Given a map of the environment, how can a robot determine its pose (planar coordinates + orientation)? Two sources of uncertainty: - observations
More informationFrom Bayes to Extended Kalman Filter
From Bayes to Extended Kalman Filter Michal Reinštein Czech Technical University in Prague Faculty of Electrical Engineering, Department of Cybernetics Center for Machine Perception http://cmp.felk.cvut.cz/
More informationParticle Filters. Pieter Abbeel UC Berkeley EECS. Many slides adapted from Thrun, Burgard and Fox, Probabilistic Robotics
Particle Filters Pieter Abbeel UC Berkeley EECS Many slides adapted from Thrun, Burgard and Fox, Probabilistic Robotics Motivation For continuous spaces: often no analytical formulas for Bayes filter updates
More informationNonlinear and/or Non-normal Filtering. Jesús Fernández-Villaverde University of Pennsylvania
Nonlinear and/or Non-normal Filtering Jesús Fernández-Villaverde University of Pennsylvania 1 Motivation Nonlinear and/or non-gaussian filtering, smoothing, and forecasting (NLGF) problems are pervasive
More informationA Reservoir Sampling Algorithm with Adaptive Estimation of Conditional Expectation
A Reservoir Sampling Algorithm with Adaptive Estimation of Conditional Expectation Vu Malbasa and Slobodan Vucetic Abstract Resource-constrained data mining introduces many constraints when learning from
More informationLocal Positioning with Parallelepiped Moving Grid
Local Positioning with Parallelepiped Moving Grid, WPNC06 16.3.2006, niilo.sirola@tut.fi p. 1/?? TA M P E R E U N I V E R S I T Y O F T E C H N O L O G Y M a t h e m a t i c s Local Positioning with Parallelepiped
More informationBayesian Methods in Positioning Applications
Bayesian Methods in Positioning Applications Vedran Dizdarević v.dizdarevic@tugraz.at Graz University of Technology, Austria 24. May 2006 Bayesian Methods in Positioning Applications p.1/21 Outline Problem
More information10-701/15-781, Machine Learning: Homework 4
10-701/15-781, Machine Learning: Homewor 4 Aarti Singh Carnegie Mellon University ˆ The assignment is due at 10:30 am beginning of class on Mon, Nov 15, 2010. ˆ Separate you answers into five parts, one
More informationTSRT14: Sensor Fusion Lecture 9
TSRT14: Sensor Fusion Lecture 9 Simultaneous localization and mapping (SLAM) Gustaf Hendeby gustaf.hendeby@liu.se TSRT14 Lecture 9 Gustaf Hendeby Spring 2018 1 / 28 Le 9: simultaneous localization and
More informationAutonomous Navigation for Flying Robots
Computer Vision Group Prof. Daniel Cremers Autonomous Navigation for Flying Robots Lecture 6.2: Kalman Filter Jürgen Sturm Technische Universität München Motivation Bayes filter is a useful tool for state
More informationAn introduction to Bayesian statistics and model calibration and a host of related topics
An introduction to Bayesian statistics and model calibration and a host of related topics Derek Bingham Statistics and Actuarial Science Simon Fraser University Cast of thousands have participated in the
More information9 Multi-Model State Estimation
Technion Israel Institute of Technology, Department of Electrical Engineering Estimation and Identification in Dynamical Systems (048825) Lecture Notes, Fall 2009, Prof. N. Shimkin 9 Multi-Model State
More informationChapter 4 State Estimation
Chapter 4 State Estimation Navigation of an unmanned vehicle, always depends on a good estimation of the vehicle states. Especially if no external sensors or marers are available, more or less complex
More informationLagrangian Data Assimilation and Manifold Detection for a Point-Vortex Model. David Darmon, AMSC Kayo Ide, AOSC, IPST, CSCAMM, ESSIC
Lagrangian Data Assimilation and Manifold Detection for a Point-Vortex Model David Darmon, AMSC Kayo Ide, AOSC, IPST, CSCAMM, ESSIC Background Data Assimilation Iterative process Forecast Analysis Background
More informationPATTERN RECOGNITION AND MACHINE LEARNING CHAPTER 13: SEQUENTIAL DATA
PATTERN RECOGNITION AND MACHINE LEARNING CHAPTER 13: SEQUENTIAL DATA Contents in latter part Linear Dynamical Systems What is different from HMM? Kalman filter Its strength and limitation Particle Filter
More informationParticle Filters; Simultaneous Localization and Mapping (Intelligent Autonomous Robotics) Subramanian Ramamoorthy School of Informatics
Particle Filters; Simultaneous Localization and Mapping (Intelligent Autonomous Robotics) Subramanian Ramamoorthy School of Informatics Recap: State Estimation using Kalman Filter Project state and error
More informationMulti-Robotic Systems
CHAPTER 9 Multi-Robotic Systems The topic of multi-robotic systems is quite popular now. It is believed that such systems can have the following benefits: Improved performance ( winning by numbers ) Distributed
More informationDimension Reduction. David M. Blei. April 23, 2012
Dimension Reduction David M. Blei April 23, 2012 1 Basic idea Goal: Compute a reduced representation of data from p -dimensional to q-dimensional, where q < p. x 1,...,x p z 1,...,z q (1) We want to do
More informationSensor Fusion: Particle Filter
Sensor Fusion: Particle Filter By: Gordana Stojceska stojcesk@in.tum.de Outline Motivation Applications Fundamentals Tracking People Advantages and disadvantages Summary June 05 JASS '05, St.Petersburg,
More informationIntroduction to Probabilistic Graphical Models: Exercises
Introduction to Probabilistic Graphical Models: Exercises Cédric Archambeau Xerox Research Centre Europe cedric.archambeau@xrce.xerox.com Pascal Bootcamp Marseille, France, July 2010 Exercise 1: basics
More informationArtificial Intelligence
Artificial Intelligence Roman Barták Department of Theoretical Computer Science and Mathematical Logic Summary of last lecture We know how to do probabilistic reasoning over time transition model P(X t
More informationLecture 2: From Linear Regression to Kalman Filter and Beyond
Lecture 2: From Linear Regression to Kalman Filter and Beyond Department of Biomedical Engineering and Computational Science Aalto University January 26, 2012 Contents 1 Batch and Recursive Estimation
More informationMachine Learning Techniques for Computer Vision
Machine Learning Techniques for Computer Vision Part 2: Unsupervised Learning Microsoft Research Cambridge x 3 1 0.5 0.2 0 0.5 0.3 0 0.5 1 ECCV 2004, Prague x 2 x 1 Overview of Part 2 Mixture models EM
More informationEKF and SLAM. McGill COMP 765 Sept 18 th, 2017
EKF and SLAM McGill COMP 765 Sept 18 th, 2017 Outline News and information Instructions for paper presentations Continue on Kalman filter: EKF and extension to mapping Example of a real mapping system:
More informationLego NXT: Navigation and localization using infrared distance sensors and Extended Kalman Filter. Miguel Pinto, A. Paulo Moreira, Aníbal Matos
Lego NXT: Navigation and localization using infrared distance sensors and Extended Kalman Filter Miguel Pinto, A. Paulo Moreira, Aníbal Matos 1 Resume LegoFeup Localization Real and simulated scenarios
More informationHuman Pose Tracking I: Basics. David Fleet University of Toronto
Human Pose Tracking I: Basics David Fleet University of Toronto CIFAR Summer School, 2009 Looking at People Challenges: Complex pose / motion People have many degrees of freedom, comprising an articulated
More informationRao-Blackwellized Particle Filter for Multiple Target Tracking
Rao-Blackwellized Particle Filter for Multiple Target Tracking Simo Särkkä, Aki Vehtari, Jouko Lampinen Helsinki University of Technology, Finland Abstract In this article we propose a new Rao-Blackwellized
More informationSTA 4273H: Statistical Machine Learning
STA 4273H: Statistical Machine Learning Russ Salakhutdinov Department of Computer Science! Department of Statistical Sciences! rsalakhu@cs.toronto.edu! h0p://www.cs.utoronto.ca/~rsalakhu/ Lecture 7 Approximate
More informationGaussian Mixtures Proposal Density in Particle Filter for Track-Before-Detect
12th International Conference on Information Fusion Seattle, WA, USA, July 6-9, 29 Gaussian Mixtures Proposal Density in Particle Filter for Trac-Before-Detect Ondřej Straa, Miroslav Šimandl and Jindřich
More informationLinear Dynamical Systems
Linear Dynamical Systems Sargur N. srihari@cedar.buffalo.edu Machine Learning Course: http://www.cedar.buffalo.edu/~srihari/cse574/index.html Two Models Described by Same Graph Latent variables Observations
More informationSensor Tasking and Control
Sensor Tasking and Control Sensing Networking Leonidas Guibas Stanford University Computation CS428 Sensor systems are about sensing, after all... System State Continuous and Discrete Variables The quantities
More informationOptimization-Based Control
Optimization-Based Control Richard M. Murray Control and Dynamical Systems California Institute of Technology DRAFT v1.7a, 19 February 2008 c California Institute of Technology All rights reserved. This
More informationIntelligent Systems and Control Prof. Laxmidhar Behera Indian Institute of Technology, Kanpur
Intelligent Systems and Control Prof. Laxmidhar Behera Indian Institute of Technology, Kanpur Module - 2 Lecture - 4 Introduction to Fuzzy Logic Control In this lecture today, we will be discussing fuzzy
More informationMulti-Sensor Fusion for Localization of a Mobile Robot in Outdoor Environments
Multi-Sensor Fusion for Localization of a Mobile Robot in Outdoor Environments Thomas Emter, Arda Saltoğlu and Janko Petereit Introduction AMROS Mobile platform equipped with multiple sensors for navigation
More informationBayesian Regression Linear and Logistic Regression
When we want more than point estimates Bayesian Regression Linear and Logistic Regression Nicole Beckage Ordinary Least Squares Regression and Lasso Regression return only point estimates But what if we
More informationExtended Object and Group Tracking with Elliptic Random Hypersurface Models
Extended Object and Group Tracing with Elliptic Random Hypersurface Models Marcus Baum Benjamin Noac and Uwe D. Hanebec Intelligent Sensor-Actuator-Systems Laboratory ISAS Institute for Anthropomatics
More informationChris Bishop s PRML Ch. 8: Graphical Models
Chris Bishop s PRML Ch. 8: Graphical Models January 24, 2008 Introduction Visualize the structure of a probabilistic model Design and motivate new models Insights into the model s properties, in particular
More informationData assimilation in high dimensions
Data assimilation in high dimensions David Kelly Courant Institute New York University New York NY www.dtbkelly.com February 12, 2015 Graduate seminar, CIMS David Kelly (CIMS) Data assimilation February
More informationGaussian Processes for Sequential Prediction
Gaussian Processes for Sequential Prediction Michael A. Osborne Machine Learning Research Group Department of Engineering Science University of Oxford Gaussian processes are useful for sequential data,
More informationIntroduction to Mobile Robotics Bayes Filter Particle Filter and Monte Carlo Localization
Introduction to Mobile Robotics Bayes Filter Particle Filter and Monte Carlo Localization Wolfram Burgard, Cyrill Stachniss, Maren Bennewitz, Kai Arras 1 Motivation Recall: Discrete filter Discretize the
More informationLecture 8: Bayesian Estimation of Parameters in State Space Models
in State Space Models March 30, 2016 Contents 1 Bayesian estimation of parameters in state space models 2 Computational methods for parameter estimation 3 Practical parameter estimation in state space
More informationSIMULTANEOUS STATE AND PARAMETER ESTIMATION USING KALMAN FILTERS
ECE5550: Applied Kalman Filtering 9 1 SIMULTANEOUS STATE AND PARAMETER ESTIMATION USING KALMAN FILTERS 9.1: Parameters versus states Until now, we have assumed that the state-space model of the system
More informationEstimating the Shape of Targets with a PHD Filter
Estimating the Shape of Targets with a PHD Filter Christian Lundquist, Karl Granström, Umut Orguner Department of Electrical Engineering Linöping University 583 33 Linöping, Sweden Email: {lundquist, arl,
More informationIntroduction to Machine Learning
Introduction to Machine Learning Brown University CSCI 1950-F, Spring 2012 Prof. Erik Sudderth Lecture 25: Markov Chain Monte Carlo (MCMC) Course Review and Advanced Topics Many figures courtesy Kevin
More informationLecture 6: Multiple Model Filtering, Particle Filtering and Other Approximations
Lecture 6: Multiple Model Filtering, Particle Filtering and Other Approximations Department of Biomedical Engineering and Computational Science Aalto University April 28, 2010 Contents 1 Multiple Model
More informationParallel Particle Filter in Julia
Parallel Particle Filter in Julia Gustavo Goretkin December 12, 2011 1 / 27 First a disclaimer The project in a sentence. workings 2 / 27 First a disclaimer First a disclaimer The project in a sentence.
More informationE190Q Lecture 10 Autonomous Robot Navigation
E190Q Lecture 10 Autonomous Robot Navigation Instructor: Chris Clark Semester: Spring 2015 1 Figures courtesy of Siegwart & Nourbakhsh Kilobots 2 https://www.youtube.com/watch?v=2ialuwgafd0 Control Structures
More informationGaussian Process Approximations of Stochastic Differential Equations
Gaussian Process Approximations of Stochastic Differential Equations Cédric Archambeau Dan Cawford Manfred Opper John Shawe-Taylor May, 2006 1 Introduction Some of the most complex models routinely run
More informationCS491/691: Introduction to Aerial Robotics
CS491/691: Introduction to Aerial Robotics Topic: State Estimation Dr. Kostas Alexis (CSE) World state (or system state) Belief state: Our belief/estimate of the world state World state: Real state of
More informationProbabilistic Robotics
University of Rome La Sapienza Master in Artificial Intelligence and Robotics Probabilistic Robotics Prof. Giorgio Grisetti Course web site: http://www.dis.uniroma1.it/~grisetti/teaching/probabilistic_ro
More informationStochastic Structural Dynamics Prof. Dr. C. S. Manohar Department of Civil Engineering Indian Institute of Science, Bangalore
Stochastic Structural Dynamics Prof. Dr. C. S. Manohar Department of Civil Engineering Indian Institute of Science, Bangalore Lecture No. # 33 Probabilistic methods in earthquake engineering-2 So, we have
More informationAdaptive Particle Filter for Nonparametric Estimation with Measurement Uncertainty in Wireless Sensor Networks
Article Adaptive Particle Filter for Nonparametric Estimation with Measurement Uncertainty in Wireless Sensor Networks Xiaofan Li 1,2,, Yubin Zhao 3, *, Sha Zhang 1,2, and Xiaopeng Fan 3 1 The State Monitoring
More informationFor final project discussion every afternoon Mark and I will be available
Worshop report 1. Daniels report is on website 2. Don t expect to write it based on listening to one project (we had 6 only 2 was sufficient quality) 3. I suggest writing it on one presentation. 4. Include
More informationPart 1: Expectation Propagation
Chalmers Machine Learning Summer School Approximate message passing and biomedicine Part 1: Expectation Propagation Tom Heskes Machine Learning Group, Institute for Computing and Information Sciences Radboud
More informationTSRT14: Sensor Fusion Lecture 8
TSRT14: Sensor Fusion Lecture 8 Particle filter theory Marginalized particle filter Gustaf Hendeby gustaf.hendeby@liu.se TSRT14 Lecture 8 Gustaf Hendeby Spring 2018 1 / 25 Le 8: particle filter theory,
More informationWhat is Motion? As Visual Input: Change in the spatial distribution of light on the sensors.
What is Motion? As Visual Input: Change in the spatial distribution of light on the sensors. Minimally, di(x,y,t)/dt 0 As Perception: Inference about causes of intensity change, e.g. I(x,y,t) v OBJ (x,y,z,t)
More informationIsobath following using an altimeter as a unique exteroceptive sensor
Isobath following using an altimeter as a unique exteroceptive sensor Luc Jaulin Lab-STICC, ENSTA Bretagne, Brest, France lucjaulin@gmail.com Abstract. We consider an underwater robot equipped with an
More informationSLAM for Ship Hull Inspection using Exactly Sparse Extended Information Filters
SLAM for Ship Hull Inspection using Exactly Sparse Extended Information Filters Matthew Walter 1,2, Franz Hover 1, & John Leonard 1,2 Massachusetts Institute of Technology 1 Department of Mechanical Engineering
More informationAnalytically-Guided-Sampling Particle Filter Applied to Range-only Target Tracking
Analytically-Guided-Sampling Particle Filter Applied to Range-only Target Tracing Guoquan P. Huang and Stergios I. Roumeliotis Abstract Particle filtering (PF) is a popular nonlinear estimation technique
More informationSequential Monte Carlo Methods for Bayesian Computation
Sequential Monte Carlo Methods for Bayesian Computation A. Doucet Kyoto Sept. 2012 A. Doucet (MLSS Sept. 2012) Sept. 2012 1 / 136 Motivating Example 1: Generic Bayesian Model Let X be a vector parameter
More informationDynamic System Identification using HDMR-Bayesian Technique
Dynamic System Identification using HDMR-Bayesian Technique *Shereena O A 1) and Dr. B N Rao 2) 1), 2) Department of Civil Engineering, IIT Madras, Chennai 600036, Tamil Nadu, India 1) ce14d020@smail.iitm.ac.in
More informationLecture: Gaussian Process Regression. STAT 6474 Instructor: Hongxiao Zhu
Lecture: Gaussian Process Regression STAT 6474 Instructor: Hongxiao Zhu Motivation Reference: Marc Deisenroth s tutorial on Robot Learning. 2 Fast Learning for Autonomous Robots with Gaussian Processes
More informationNavigation. Global Pathing. The Idea. Diagram I: Overall Navigation
Navigation Diagram I: Overall Navigation Global Pathing The Idea (focus on 2D coordinates Takes advantage of prior info: navigation space/ dimensions target destination location Risks: Map Resolution too
More informationImage Alignment and Mosaicing Feature Tracking and the Kalman Filter
Image Alignment and Mosaicing Feature Tracking and the Kalman Filter Image Alignment Applications Local alignment: Tracking Stereo Global alignment: Camera jitter elimination Image enhancement Panoramic
More informationin a Rao-Blackwellised Unscented Kalman Filter
A Rao-Blacwellised Unscented Kalman Filter Mar Briers QinetiQ Ltd. Malvern Technology Centre Malvern, UK. m.briers@signal.qinetiq.com Simon R. Masell QinetiQ Ltd. Malvern Technology Centre Malvern, UK.
More informationA Comparison of the EKF, SPKF, and the Bayes Filter for Landmark-Based Localization
A Comparison of the EKF, SPKF, and the Bayes Filter for Landmark-Based Localization and Timothy D. Barfoot CRV 2 Outline Background Objective Experimental Setup Results Discussion Conclusion 2 Outline
More informationSequential Monte Carlo methods for filtering of unobservable components of multidimensional diffusion Markov processes
Sequential Monte Carlo methods for filtering of unobservable components of multidimensional diffusion Markov processes Ellida M. Khazen * 13395 Coppermine Rd. Apartment 410 Herndon VA 20171 USA Abstract
More informationEKF, UKF. Pieter Abbeel UC Berkeley EECS. Many slides adapted from Thrun, Burgard and Fox, Probabilistic Robotics
EKF, UKF Pieter Abbeel UC Berkeley EECS Many slides adapted from Thrun, Burgard and Fox, Probabilistic Robotics Kalman Filter Kalman Filter = special case of a Bayes filter with dynamics model and sensory
More informationVEHICLE WHEEL-GROUND CONTACT ANGLE ESTIMATION: WITH APPLICATION TO MOBILE ROBOT TRACTION CONTROL
1/10 IAGNEMMA AND DUBOWSKY VEHICLE WHEEL-GROUND CONTACT ANGLE ESTIMATION: WITH APPLICATION TO MOBILE ROBOT TRACTION CONTROL K. IAGNEMMA S. DUBOWSKY Massachusetts Institute of Technology, Cambridge, MA
More informationEKF, UKF. Pieter Abbeel UC Berkeley EECS. Many slides adapted from Thrun, Burgard and Fox, Probabilistic Robotics
EKF, UKF Pieter Abbeel UC Berkeley EECS Many slides adapted from Thrun, Burgard and Fox, Probabilistic Robotics Kalman Filter Kalman Filter = special case of a Bayes filter with dynamics model and sensory
More informationCombine Monte Carlo with Exhaustive Search: Effective Variational Inference and Policy Gradient Reinforcement Learning
Combine Monte Carlo with Exhaustive Search: Effective Variational Inference and Policy Gradient Reinforcement Learning Michalis K. Titsias Department of Informatics Athens University of Economics and Business
More information5.1 2D example 59 Figure 5.1: Parabolic velocity field in a straight two-dimensional pipe. Figure 5.2: Concentration on the input boundary of the pipe. The vertical axis corresponds to r 2 -coordinate,
More informationNew Fast Kalman filter method
New Fast Kalman filter method Hojat Ghorbanidehno, Hee Sun Lee 1. Introduction Data assimilation methods combine dynamical models of a system with typically noisy observations to obtain estimates of the
More information