Continuous Time Particle Filtering

1 Introduction

The work described here is directly motivated by problems encountered in creating a state estimation system for the K9 experimental Mars rover at NASA Ames Research Center [Willeke and Dearden, 2004]. As with many robotic systems, K-9 contains numerous sensors that report on its performance. These sensors produce telemetry at different rates, and frequently at rates that vary over time. As a result, using a fixed time-step state estimator, for example a Kalman filter, is extremely difficult. In this paper we describe a continuoustime state estimation algorithm based on particle filtering that can handle telemetry arriving at different rates and changes in system behavior between observations. The system can also update its model of system behavior to improve its performance and to detect gradual degradation of the system.

State estimation is typically done either using discrete diagnosis algorithms such as Livingstone [Williams and Nayak, 1996], or more recently, using hybrid system representations. The discrete approach is well suited for domains where there are occasional transitions between relatively stable states, and where sensor readings tend to be very reliable. Unfortunately,

Richard Dearden

University of Birmingham Birmingham B15 2TT, UK R.W.Dearden@cs.bham.ac.uk

this is not the case for K-9 and discrete approaches have largely proven unhelpful [Washington et al., 2000]. For this reason, we use a hybrid system model for state representation and particle filtering for state estimation.

Probabilistic hybrid automata (PHAs) [Hofbaur and Williams, 2002] are commonly used for representing hybrid systems in diagnosis and state estimation (other representations have similar expressive power). In the PHA model, a system consists of a set of discrete modes and a set of continuous variables. For each mode, a set of differential equations describes the continuous behavior in that mode. Transitions between modes are stochastic; a transition matrix represents the probability of being in state s ′ at time t + 1 given that the system is in state s at time t. Transition probabilities may depend on the values of the continuous variables.

The problem with applying the PHA model (and other similar representations) to systems such as K-9 is the assumption that a transition matrix applied at fixed time intervals is sufficient to describe the evolution of the system. This implies that only one discrete transition can occur per interval, and also that it doesn't matter when in the interval the transition occurs. These assumptions lead to two problems. First, they make the system model unwieldy as they force a single transition matrix to represent the effects of multiple discrete transitions. Second, they reduce efficiency by forcing model steps to be small enough for the assumptions to be reasonable. The continuous time model avoids these assumptions, allowing state updates to be performed only when a new observation arrives or when a significant event occurs.

The approach we take is to build a continuous-time model of the system (see Section 2). As before, there is a differential equation model governing the continuous system behavior for every discrete system mode, but now the discrete state transitions are represented as a continuous-time Markov jump process. That is, we keep a set of transition probabilities for each discrete mode, as well as the distribution over how long the system will remain in the mode before transitioning out.

As we said above, we use particle filtering to track the system state (see Section 4). For efficiency, and to make it possible to track large systems, we use Rao-Blackwellized particle filters [Doucet et al., 2000a], in which the discrete mode is sampled, just as in a standard particle filter, but the continuous state is tracked using an unscented Kalman-Bucy filter.

This work addresses the key challenge that many mode changes on the rover, as with most systems, are not observable directly. Commanded changes such as moving from stopped to driving are observable, but the occurrence of faults, and other mode transitions triggered by the environment or the continuous state, do not produce telemetry directly, and can only be inferred from their effects on the continuous system behavior. The algorithm we have developed to address these difficulties, the continuous-time particle filter, is described in Section 5.

One major advantage of the continuous-time approach is that it allows resource-bounded computation. Although the cost of a single update may be higher than that of a discrete update (because of the Kalman-Bucy filter equations), computation need only be performed when new observations arrive. Indeed, if computation is limited, observations can even be ignored until sufficient computation is available to update the estimate. For discrete approaches, an update must be performed for each time step, even if no telemetry has arrived.

In Section 6, we discuss how the parameters of the continuous-time Markov jump process can be learned by observing the system. Section 7 applies the approach to the simulated rover model presented in Section 3.

2 Continuous-time hybrid-state processes

A continuous-time hybrid-state process consists of interacting discrete-state and continuous-state random variables.

The discrete-state variables Zt evolve according to dynamics described by continuous-time Markov jump processes. A Markov jump process is a random variable Zt that is parametrized by time t ∈ [0,∞). Zt starts in an initial state z and remains in this state for a random amount of time before it makes a transition to a different state. The time that Zt stays in a particular state is exponentially distributed, due to the Markovian property of the process.

Mathematically, a Markov jump process Zt is characterized by its intensity matrix

where

in which qij is the transition rate that defines the probability per time unit that the system makes a transition from state i to state j and qi is the total transition rate out of state i. The amount of time that the process stays in state i is distributed according to the exponential distribution fi(t) = qi exp(-qit). When the process leaves state i, it enters the next state j with probability Pij = qij ∑j qij if i ̸= j and 0 otherwise. In this paper, we assume that the process is stationary, i.e. the intensity matrix is the same at all times.

In recent work, [Nodelman et al., 2002] introduced continuous-time Bayesian networks (CTBNs) as a way to factorize Markov jump processes over discrete-valued variables. Our work extends the inference to hybrid-state processes.

The instantiation of Zt corresponds to a unique set of equations that govern the dynamics of the continuous variables Xt. Let τ1, τ2, . . . denote the times at which Zt changes value. Let zi denote the value of Zt in the interval [τi, τi+1]. During the time span of [τi, τi+1] the variables Xt follow nonlinear dynamics given by

and the observations Yt are given by

where Wt ∼ N(0, Φt) and Vt ∼ N(0, Ψt) are the respective process and measurement noise, andUt is the control input.

In this work, we assume that some variables in Zt may be observed, but only at random discrete points in time. On the other hand, Yt are observed more frequently, but as noisy measurements of the true continuous state Xt.

3 A continuous-time rover model

As a testbed, we use a continuous-time rover model. The rover employs a rocker-bogey suspension system for its six wheels. On each side, the front two wheels are attached to the bogey, which pivots around its center. The center of the bogey is attached to the rocker, which is attached to the main rover chassis and the rear wheels. Motion from the bogey can cause the rocker to pivot around the differential axle, a mechanism that centers the rover chassis between the left and right sets of wheels. From the rocker angles and the bogey angles, we can infer the rover orientation and thus the wheel heights relative to the rover chassis.

We are interested in reasoning about the rover wheel dynamics under the effects of weather, terrain, ground conditions and faulty wheel behavior. The model consists of 20 state variables and 4 transient variables. There are two groups of state variables: 5 that model background conditions and 15 that directly model the rover. The background variables model environmental factors such as weather, terrain, ground rockiness and ground stickiness, while the rover variables model the solar energy level available to the rover, the rover speed, the wheel heights, the wheel stuck conditions, and the rover roll angle. The CTBN structure is shown in Figure 1.

The model comprises 11 binary and ternary discrete variables and 9 continuous variables. The only observable discrete variables are Sunny, Rainy, T errain and Rockiness. The observation of these variables at sparse time points corresponds to situations when the rover takes measurements of its surroundings, to plan navigation or to manage power consumption. The wheel stuck states are never observed and are inferred from observations of the continuous wheel variables. The wheel stuck condition is more frequently induced when the rover travels at high speed and when the ground is rocky.

The rover dynamics are adapted from the kinematic analysis in [Hacot, 1998]. The front wheels receive an input proportional to some height perturbation induced by the terrain. The change in the middle wheel height is proportional to the difference between the front and middle wheel height, taken relative to the rover body which experiences roll (torsional rotation) on uneven terrain. To illustrate the system's nonlinearity, the equation for the middle wheel height is:

where Θt is the roll angle, θB t is the bogey angle, and l1, l2 and β are constant parameters of the physical rover model. The change in the back wheel is defined in a similar manner. For each wheel, the proportionality constant κt is dependent on the speed and whether the wheel is stuck. The speed is affected by the availability of solar energy and by ground surface characteristics such as stickiness and rockiness.

Noisy measurements of the continuous variables that model the bogey and rocker angles are observed more frequently than the discrete observations. The bogey angle is determined from the front and middle wheel heights. The rocker angle depends on the middle and back wheel heights as well as the bogey angle.

4 Preliminaries

Let the hybrid system state be represented as St = {Zt, Xt}. We denote y1:t as the history of observations from time 1 to time t. The aim is to track the probability p(St|y1:t) that any given state may be the true system state, conditioned upon the sequence of past observations y1:t. This probability distribution, also referred to as the belief state, can be recursively estimated given the previous belief state p(St-1|y1:t-1) and the current observation yt:

where p(St|y1:t-1) = ∫ p(St|St-1)p(St-1|y1:t-1)dSt-1. However, the computation of this integral leads to intractability in all but the smallest conditional linear Gaussian models. As a result, one must resort to approximate inference methods to compute the updated belief state.

4.1 Particle filtering

Particle filtering (PF) [Doucet et al., 2000b] approximates (4) by a discrete sum of particles or samples of possible states drawn from that distribution:

where δ(·) denotes the Dirac delta function. Since it is difficult to sample from p(St|y1:t) directly, importance sampling is used, in which particles are drawn from a more tractable proposal distribution, then each particle is weighted to account for this bias. Since variance increases as the state space increases, the number of particles required to achieve decent accuracy increases as well, thus making PF an expensive solution for tracking complex, high-dimensional systems.

Rao-Blackwellization [Doucet et al., 2000a] is a technique that improves PF by analytically marginalizing out some of the variables. This method is especially applicable to our domain since the structure of the rover model can be factorized:

Hence, Rao-Blackwellized Particle Filtering (RBPF) can be used to sample only from the (lower-dimensional) discrete distribution and the continuous distribution can subsequently be computed using the Kalman filter.

4.2 The unscented Kalman-Bucy filter

The Kalman filter [Grewal and Andrews, 2001] is an efficient, recursive method that finds the least-square estimate of the state of a discrete-time, linear stochastic process with Gaussian noise. The Kalman-Bucy filter is the continuous-time counterpart to the discrete-time Kalman filter. It uses a differential equation to generate an estimate ˆxt of the continuous state, and generates from that an estimate ˆyt of the observation. It also generates Pt, the covariance of the state estimate error xt - ˆxt, and Rt, the covariance of yt - ˆyt. The update of Pt also uses a differential equation. Given these estimates, the probability of the observation yt is given by a normal distribution with mean ˆ yt and covariance Rt.

The Kalman-Bucy filter assumes a linear transition model and observation model. To handle nonlinear models, we adopt an approach similar to the discrete-time unscented Kalman filter (UKF) [Wan and van der Merwe, 2000] and extend the Kalman-Bucy filter by applying the unscented transformation [Julier and Uhlman, 2002] to the state estimation. Thus, the unscented Kalman-Bucy filter (UKB) is a continuous-time filter that allows for nonlinear models. UKB is used in RBPF as an approximation to the Kalman-Bucy filter. This approximation introduces bias and does not provide the theoretical guarantee that variance is reduced in RBPF.

5 The continuous-time particle filter

In the continuous-time particle filter (CTPF), we perform an update each time a discrete observation is received. If no discrete-state process is observed, updates can be triggered by an asynchronous request for update. Technically, this is treated the same way as observing the discrete-state process with a vacuous observation. If the new observation is received at time tn and the previous observation was at time tn-1, an update means generating particles of the state at tn, using the particles generated at tn-1. We assume that observations of the discrete-state processes are sparse and intermittent. We make no assumption that observations come at regular intervals or that the observations coincide with any discrete transitions. Let ζn be the observation at time tn. We may observe ζn as the entirety of Ztn , or ζn may be an assignment of values to a subset of the variables in Ztn , or ζn may be vacuous, assigning values to none of the variables. In any case, we will say that a value z agrees with the observation ζn if it assigns the same value to all variables assigned by ζn.

At a high level, the CTPF works as follows. When updating from tn-1 to tn, we sample a trajectory from the Markov jump process for the time interval [tn-1, tn]. This results in a set of time points τ0 = tn-1, τ1, . . . , τk = tn at which Z changes value. Let zi denote the value of Z in interval [τi, τi+1]. We then perform Rao-Blackwellization to obtain updates for the continuous variables. We go through the sequence of intervals, starting at [τ0, τ1] and ending at [τk-1, τk], propagating our estimate of the mean and variance of the continuous variables X using UKB. In the time period [τi, τi+1], the continuous dynamics are governed by the value zi. As we propagate the continuous variables, we also compute the probability of the trajectory, which is used as the particle weight. We repeat this process to obtain a set of weighted particles at time tn, and resample from these to get a new set of unweighted particles.

The behavior of the Markov jump process governing Z is characterized by the intensity matrix QZ. The process of generating a trajectory of the Markov jump process from time tn-1 to tn is as follows. For each particle at time tn-1, let z0 be the value of Z in that particle. Set τ0 = tn-1 and i = 0. We use the intensity matrix to sample a time from the exponential distribution fzi (t) = qzi exp(-qzi t) to obtain the length of time γ until Z next changes value. If τ i+γ < tn, we sample zi+1 from the intensity matrix, and continue to loop, setting τi+1 = τi +γ. Otherwise, we know that Z = zi at tn. If this value agrees with the evidence ζn, the particle is kept, otherwise it is rejected.

Once we have the jump process, we proceed to propagate the continuous variables X through the time points τ0, . . . , τk. Since there may be many observations of the continuous variables in the interval [τi, τi+1], when we propagate them from τi to τi+1, we need to consider all the observations between those times. Let δ0 be τi, δℓ be τi+1 and δ1, . . . , δℓ-1 be the time points at which continuous observations were received. We loop through the intervals [δj, δj+1] in turn. For each interval, we propagate the mean and variance of the continuous variables forward using UKB. We also obtain the probability of the observation at time δj+1, given by the predictive density p(yδj+1 |yδ1:δj ) = N (yδj+1 ; ˆyδj+1 , Rδj+1 ) where ˆyδj+1 and Rδj+1 are the estimate and covariance of yδj+1 under UKB. We then multiply the probability of all the observations together over all time points δj in [τi, τi+1] and over all τi to obtain the probability of the sequence of observations. This becomes the weight of the particle.

We repeat this procedure for each of the M particles from time tn-1. Each particle consists of a candidate Markov jump process for each discrete variable in Zt, and the mean and covariance for the continuous variables in Xt. The Markov jump process candidates that are far from the ground truth will lead to poor tracking of the continuous variables due to incorrect parametrization of the continuous-state dynamics. Particles associated with the ill-fit Markov jump processes will have low weights and will be resampled out.

The pseudocode for CTPF is shown in Figure 2.

• 1: let M be the number of particles 2: let N be the number of discrete observations 3: for n = 1 to N do 4: Dynamics propagation 5: for m = 1 to M do 6: let ζn be the nth observation 7: let tn be the observed time of ζn 8: Simulate a Markov jump process 9: let i = 0, τ0 = tn-1 10: let z0 be the value of Z in the mth particle at time tn-1 11: while τi < tn do 12: Sample γ from the PDF f(t) = qzi exp(-qzi t) 13: if τi + γ < tn then 14: let τi+1 = τi + γ 15: Sample zi+1 using the intensity matrix QZ 16: else 17: let τi+1 = tn; let z(m) = zi 18: i ← i + 1 19: if z(m) disagrees with ζn, reject the particle 20: let k = i 21: Propagate continuous variables 22: let ˆxtn-1 and Ptn-1 be the mean and covariance of the continuous variables in the mth particle at time tn-1 23: let w(m) = 1 24: for i = 0 to k - 1 do 25: let δ1, . . . , δℓ-1 be the times of continuous observations in [τi, τi+1] 26: let δ0 = τi, δℓ = τi+1 27: for j = 0 to ℓ - 1 do 28: Compute ˆxδj+1 , Pδj+1 , ˆyδj+1 and Rδj+1 using UKB beginning with ˆxδj and Pδj , under the dynamics determined by zi 29: w(m) ← w(m) × N (yδj+1 ; ˆyδj+1 , Rδj+1 ) 30: let ˆx(m) and P (m) be ˆxtn and Ptn 31: Resampling 32: Normalize w(m) ← w(m) ∑ w(m) 33: Select M new particles where the probability of (z(m) , ˆx(m) , P (m) ) is w(m)

Figure 2: Continuous-time particle filter

6 Learning

In real-world applications, we may not know the exact parametrization of Zt. Instead, we may have a prior distribution over the intensity matrices QZ and need to fine tune our estimate of QZ as observations about Zt are made.

In this case, we model QZ as a random variable and we attribute a probability distribution φ(QZ) over the space of admissible intensity matrices, QZ . Since the observations are exponentially distributed, we model φ(QZ) as a Gamma distribution since Gamma and exponential are conjugates.

Assume that the elements of QZ are distributed according to qij ∼ Γ ( 1 βi , αij ). The prior distribution QZ is given by:

Upon the arrival of a discrete-state observation ζn, we simulate a Markov jump process J over the time interval [tn-1, tn]. From J, we can compute the likelihood of QZ:

where Nij is the number of transitions from state i to j in the process J and Ri is the time spent in state i during process J. From this, we update the QZ-distribution

Upon the arrival of the next discrete-state observation, we sample qij ∼ Γ ( 1 Ri+βi ,Nij +αij ) to get an instantiation for QZ that reflects the updated distribution.

The learning procedure is integrated into algorithm 2 as follows: we sample QZ ∼ φ(QZ) right before line (8) and perform the QZ-distribution update, as prescribed in Equations (7)-(9), after line (20) of the pseudocode.

This procedure may seem like magic. After all, we are using statistics generated from simulating from QZ itself to obtain an updated estimate of QZ. The reason it works is that not all QZs from the previous set of particles have the same chance of being accepted into the next set of particles. First of all, if the trajectory disagrees with the discrete observation ζn, it will be rejected. Second, the trajectories will be weighted by the probability of the continuous observations, and these weighted particles will be resampled. A trajectory, and thus a new intensity matrix, that agrees well with the observations, is more likely to be kept. In this way, the set of intensity matrices that are kept will be influenced by the observations, both discrete and continuous.

Other learning approaches have been explored in [Nodelman et al., 2003] for parameter learning of CTBNs, and in [Bladt and Sørensen, 2003] for statistical inference of discretely observed Markov jump processes. However, these approaches are offline learning approaches where the sequence of discrete-time observations are assumed known a priori to the learning process. Our approach integrates online learning into the particle filtering framework, where we also take advantage of the continuous observations in eliminating poor candidates for QZ. This advantage allows for relatively quick convergence to the neighborhood of the true QZ value.

7 Experimental results

We evaluate the performance of the CTPF algorithm on a simulated small model and the rover model. The small model consists of one discrete ternary variable, two continuous state variables, and two continuous observation variables. Depending on the state of the discrete-valued process, the continuous behavior can be either linear, polynomial or sinusoidal. The low dimensionality of the small model makes it feasible to compare against the discrete-time particle filter and to evaluate the performance of the learning procedure.

Figure 3 shows an experiment comparing CTPF to the discrete-time particle filter (DTPF). The results shown are averaged over 50 runs. The first graph shows the performance of CTPF with 10 particles and 55 updates, comparing the estimate of one of the continuous state variables produced by CTPF with the actual value of that variable. While CTPF updates the discrete variables at infrequent intervals, DTPF performs an update at equally-spaced intervals fixed by a preset time granularity. Both algorithms perform continuous inference, via UKB or UKF, at many intermediate time points, corresponding to when the continuous variables are observed.

The plotted points are the estimates at these intermediate points. From the graph, we see that CTPF is consistently able to track the variable through the different mode changes. The second graph (DTPF1) shows DTPF's performance using the same number of particles and the same number of updates as CTPF. We see that DTPF1 has worse tracking throughout, and does particularly poorly from time 75 to time 100. CTPF outperforms DTPF because it is able to perform updates precisely when they are needed.

DTPF is considerably faster than CTPF because it does not require solving differential equations. This was exacerbated by a slow Matlab implementation of the Kalman-Bucy filter, compared to a well-optimized implementation of the Kalman filter. We expect this difference to lessen with a more efficient implementation of CTPF. Nonetheless, we ran experiments that allowed DTPF the same running time as CTPF.

We first allowed DTPF to use more particles than CTPF, while maintaining the same number of updates. The results of DTPF, with 58 particles and 55 updates, are shown as DTPF2, in the third graph of Figure 3. When the number of updates is not sufficient for DTPF to track the variables, increasing the number of particles does not particularly help. We then allowed DTPF to update more frequently than CTPF, but use the same number of particles. The results, with 10 particles and 584 updates, are shown in the fourth graph and are labeled DTPF3. The DTPF3 results show a significant improvement in the performance of DTPF, although DTPF still does not perform as well as CTPF when the continuous variables are oscillating rapidly. Furthermore, we found the variance of the estimates in DTPF3 to be higher than that produced by CTPF.

Figure 4 shows the results of learning the Q matrix on the small model. The results were quite promising. The first graph compares the estimation error achieved by CTPF with learning the Q matrix and CTPF with using the true Q matrix. Both sets of CTPF experiments used 10 particles. We see that the error of CTPF with learning comes close to that of CTPF without learning after 1000 time steps. The second graph shows the distance of the learned Q matrix from the true Q matrix over time, computed as the sum of the elementwise differences between the two matrices. The graph shows the learned Q matrix converging towards the true Q matrix.

Lastly, we tested the CTPF algorithm on our main application, the continuous-time rover model. Despite the nonlinearity of the model, CTPF is able to track the wheel heights relatively well with 10 particles. The results of the CTPF inference, averaged over 20 runs, are presented in Figure 5 (one graph per rover wheel). The tracking error is mainly due to incorrect estimation of the wheel stuck variable. Whenever the CTPF estimate drifts from the ground truth, it is able to quickly recover from the error and continue to closely track the wheel heights. The empirical results confirm CTPF's applicability for monitoring complex continuous-time systems.

Figure 4: Learning results

Figure 5: CTPF results on the rover

8 Conclusion and future work

Since most real-world systems evolve in continuous time, CTPF is a natural way to address the time granularity problem that is often associated with systems that have components that evolve at different rates. By performing state estimation in continuous time, CTPF is also better suited for resourcebounded computation since inference is no longer performed at every fixed time step (as in the discrete-time particle filter). Our results show that CTPF can effectively track the state of complex systems such as the rover. Moreover, when comparison is possible with discrete-time approaches, CTPF tracks the state more accurately. We believe that the approach is an important step in making recent advances in particle filtering applicable to a much larger set of continuous-time problems.

An important aspect in hybrid system modelling is the issue of autonomous transitions, in which the continuous state triggers a discrete transition. We model this in our approach by making the intensity matrices dependent on the continuous variables. This leads to difficulties in that while the system is in a state, the distribution over the time the system remains there may vary. At present we approximate this by only computing the intensity matrix at updates or when the discrete state changes. This is consistent with our aim of resource-bounded computation. We plan to investigate other alternatives such as resampling from an updated intensity matrix when the current distribution deviates drastically from the assumed distribution used for sampling.

Other areas of future work include applying this approach with factored state spaces [Ng et al., 2002] to reduce the complexity of the Kalman-Bucy updates, and implementating this approach onboard an actual robot.