Discrete-Time Estimation of Nonlinear Continuous-Time Stochastic Systems

Abstract

In this paper we consider the problem of state estimation of a dynamic system whose evolution is described by a nonlinear continuous-time stochastic model. We also assume that the system is observed by a sensor in discrete-time moments. To perform state estimation using uncertain discrete-time data, the system model needs to be discretized. We compare two methods of discretization. The first method uses the classical forward Euler method. The second method is based on the continuous-time simulation of the deterministic part of the nonlinear system between consecutive times of measurement. For state estimation we apply an unscented Kalman Filter, which—as opposed to the well known Extended Kalman Filter—does not require calculation of the Jacobi matrix of the nonlinear transformation associated with this method.

1 Introduction

The main tool for state estimation of dynamic systems is Kalman filtering [4, 8, 9]. If the system observed is described by continuous-time equations there are two main approaches to state estimation. In the first method one derives discrete-time model describing the process and uses a standard method for estimating it [4, 10], namely the Kalman Filter for a linear approach, or one of its variants for a nonlinear approach: for example the Extended Kalman Filter [2, 4], the Unscented Kalman Filter [7, 14], or the Particle Filter [1, 3, 6, 12]. The second method consists in directly utilizing a continuous-time estimator: the Kalman-Bucy Filter [4] in a linear approach or various nonlinear filters [3, 13].

Both approaches have their merits. Implementation of discrete-time filters is straightforward, yet the discrete-time model is only an approximation dependent on the sampling period. Thus another layer of design uncertainty is introduced. On the other hand, implementation of continuous-time filters is more complex (moreover, the measurement equations are also given in continuous time).

For most physical systems the respective continuous-time (CT) description gives best approximation of the actual phenomena which govern the process behavior. On the other hand, it is the discrete-time (DT) measurement process which is typically associated with the principle of operation of sensors. There is therefore a need for a so-called hybrid CT/DT method of state estimation.

In this article we will investigate some hybrid method in which a predictive part of the estimation algorithm is performed by continuous-time simulation of a deterministic part of a stochastic differential equation that describes an analyzed system. In the stochastic differential equations modeling the process, the stochastic part has the zero mean (in the probabilistic sense), thus we assume that it does not contribute to the evolution of the prediction¹. We will compare this method with a classical one using the standard forward Euler discretization. Though the simulation-based method is computationally more expensive than the Euler method, it gives accurate results independent of the sampling time.

The paper is organized as follows. In Sect. 2 a nonlinear continuous-time stochastic system model and a discrete-time measurement equation are presented. Two methods for discrete-time discretization are described in Sect. 3. The Unscented Kalman filter which is a basis for state estimation is recalled in Sect. 4. A simulation example is presented in Sect. 5. Section 6 contains conclusions.

2 System Model

We consider a dynamic system described by the following nonlinear stochastic differential equation:

$$\begin{aligned} d\varvec{x}(t) = \varvec{a}(\varvec{x}(t))dt + \varvec{b}(\varvec{x}(t))d\varvec{w}(t), \quad t \in \mathbb {R}^{+} = [0, \infty ) \end{aligned}$$

(1)

where an independent variable t is interpreted as time, $d\varvec{x} \in \mathbb {R}^{n}$ is an infinitesimal increment of the system state $\varvec{x} \in \mathbb {R}^{n}$, $d\varvec{w} \in \mathbb {R}^{r}$ is an infinitesimal increment of r-dimensional Wiener process $\varvec{w} \in \mathbb {R}^{r}$ describing the uncertainty, $\varvec{a}: \mathbb {R}^{n} \rightarrow \mathbb {R}^{n}$ is a nonlinear vector function describing the dynamics of the system, and $\varvec{b}: \mathbb {R}^{n} \rightarrow L(\mathbb {R}^{r}, \mathbb {R}^{n})$ is a nonlinear map describing the influence of the noise on the system², where $L(\mathbb {R}^{r}, \mathbb {R}^{n})$ is the space of $n \times r$ matrices.

3 Model Discretization

To be able to perform state estimation using Kalman methods, we need to obtain a discrete-time version of the model (1). However, due to the nonlinearity of (1), only approximate methods are applicable. As has been mentioned we will apply two different methods: the forward Euler method and the simulation method.

The well-known forward Euler method results in the following discrete-time version of (1):

$$\begin{aligned} \varvec{x}_{k} = \varvec{x}_{k-1} + T\varvec{a}(\varvec{x_{k-1}}) + \varvec{b}(\varvec{x_{k-1}}) \varDelta \varvec{w}_{k-1}, \quad k \in \mathbb {N} = \left\{ 1,2,3,\ldots \right\} \end{aligned}$$

(2)

where T is the sampling time (in seconds), $\varvec{x}_{k} \in \mathbb {R}^{n}$ is the state of the discrete-time model (2) at time kT, and $\varDelta \varvec{w}_{k} \in \mathbb {R}^{r}$ is a vector random variable with a multivariate normal distribution $\mathcal {N}(\varvec{0}, \varvec{Q})$, with the zero mean and the covariance matrix

$$\begin{aligned} \varvec{Q} = T \varvec{I}_{r} \end{aligned}$$

(3)

where $\varvec{I}_{r}$ is the $r \times r$ identity matrix.

Based on (2), since $\varDelta \varvec{w}$ is a zero mean noise, the model-based prediction equation needed for estimation has the following (homogeneous) form:

$$\begin{aligned} \varvec{x}_{k} = \varvec{x}_{k-1} + T\varvec{a}(\varvec{x_{k-1}}), \quad k \in \mathbb {N} \end{aligned}$$

(4)

The second discretization method is based on simulation of the deterministic part of model (1). In this method the following (homogeneous) deterministic ordinary differential equation is simulated between consecutive sampling instants:

$$\begin{aligned} \frac{d\varvec{x}(t)}{dt} = \varvec{a}(\varvec{x}(t)), \quad \text {for } t \in \left[ (k-1)T, kT \right] , \quad k \in \mathbb {N} \end{aligned}$$

(5)

A numerical solution to equation (5) can be computed, for example, using one of the standard Runge-Kutta methods, to obtain a prediction of the state $x_{k}$ for the time instant kT based on the state $x_{k-1}$ computed at the time instant $(k-1)T$.

Besides the fact that the simulation method gives more accurate results of the prediction, it has two drawbacks, as compared to the forward Euler discretization. First, it is computationally more expensive. Second, the simulation method ignores the random part of the model (1), and thus the noise covariance matrix of the discrete noise is not computed. A simple, yet not faultless solution to this problem is to assume the same noise component $\varvec{b}(\varvec{x_{k-1}}) \varDelta \varvec{w}_{k-1}$ as for the Euler method (2). Other, more suitable solutions of this problem will be investigated in the future work.

Finally, we supplement both the discrete-time models with a standard discrete-time measurement equation

$$\begin{aligned} \varvec{y}_{k} = \varvec{c}(\varvec{x}_{k}) + \varvec{\zeta }_{k} \end{aligned}$$

(6)

where $\varvec{y}_{k} \in \mathbb {R}^{p}$ is the measurement vector, $\varvec{c}: \mathbb {R}^{n} \rightarrow \mathbb {R}^{p}$ is (in general) a nonlinear vector function describing the measurement principle, and $\varvec{\zeta }_{k} \in \mathbb {R}^{p}$ is a vector random variable (measurement noise) with a multivariate normal distribution $\mathcal {N}(\varvec{0}, \varvec{R})$, with the zero mean and a known covariance matrix $\varvec{R}$.

4 State Estimation

To perform state estimation we use the discrete-time model (2)–(6) and an Unscented Kalman Filter described below.

4.1 Unscented Kalman Filter

Calculation of the Jacobi matrix for the nonlinear functions $\varvec{a}$, $\varvec{b}$ and $\varvec{c}$ can be a difficult task (for the discrete-time model based on the Euler method) or even not feasible (for the simulation method, since the explicit form of the discrete-time model is not available). To overcome this problem we utilize an approach which does not require the computation of Jacobi matrix—the Unscented Kalman Filter (UKF).

The UKF is based on an unscented transform that is used to determine the mean value and the covariance matrix of a random variable subjected to a nonlinear transformation. In this method, the multivariate normal probability density function of a random variable (before the nonlinear transformation) is represented by a specific, small set of the so-called $\sigma$-points. Next, each of these points is transformed using the nonlinear function (the Euler discrete-time model (4)) or each is simulated using the differential equation (5). From the transformed points, the mean value of the random variable and its covariance matrix can next be easily computed.

The UKF has some similar features to particle filters (PF). There are, however, two significant differences described below.

• 1. The number of the UKF $\sigma$-points is not large, and they are chosen ‘optimally’ so as to best preserve the shape of the multivariate normal distribution, whereas in the particle filter the number of particles is large and they are sampled from some initial distribution. Thus the UKF is less computationally expensive.

• 2. In the UKF, the initial (pre nonlinear transformation) and the final (post nonlinear transformation) random variables are assumed to have multivariate normal distribution, thus the $\sigma$-points (post nonlinear transformation) are ‘fitted back’ into the multivariate normal distribution, whereas in the PF the particles determine the resulting probability distribution function (it can thus be arbitrary, not normal, multimodal, etc.). Therefore, using UKF it is not possible to model other probability distributions than multivariate normal distribution. It is a significant simplification, as a nonlinear transformation of a normal random variable is generally not normal.

One step of the UKF algorithm for discrete-time models presented in Sect. 2 is described below.

First, based on the results from the previous step, i.e. the state $\varvec{x}_{k-1|k-1}$ and the corresponding covariance matrix $\varvec{P}_{k-1|k-1}$, a new set of $\sigma$-points is computed. If the state is an n-dimensional multivariate normal random variable, then this set contains $(2n+1)$ $\sigma$-points $\varvec{x}_{k-1|k-1}^{i}$, $i = 0, 1, 2,\ldots , 2n$ with the corresponding weights $W^{i}$, calculated as [7]

$$\begin{aligned} \varvec{x}_{k-1|k-1}^{0}&=\ \hat{\varvec{x}}_{k-1|k-1}, \qquad W^{0} \in (-1, 1) \nonumber \\ \varvec{x}_{k-1|k-1}^{j}&=\ \hat{\varvec{x}}_{k-1|k-1} + \left( \sqrt{ \frac{n}{1-W^{0}} \varvec{P}_{k-1|k-1}} \right) ^{j}, \qquad W^{j} = \frac{1-W^{0}}{2n} \\ \varvec{x}_{k-1|k-1}^{j+n}&=\ \hat{\varvec{x}}_{k-1|k-1} - \left( \sqrt{ \frac{n}{1-W^{0}} \varvec{P}_{k-1|k-1}} \right) ^{j}, \qquad W^{j+n} = \frac{1-W^{0}}{2n} \nonumber \\&\qquad \qquad \qquad j=1,2,\ldots ,n\nonumber \end{aligned}$$

(7)

where a new index j is introduced to emphasize the symmetry of the points, the vector

$$ \left( \sqrt{ \frac{n}{1-W^{0}} \varvec{P}_{k-1|k-1}} \right) ^{j} $$

is the j-th row (and the j-th column) of the square root (in the matrix sense) of the matrix

$$ \frac{n}{1-W^{0}} \varvec{P}_{k-1|k-1} $$

The value of the weight $W^{0} \in (-1, 1)$ controls the position of $\sigma$-points. For $W^{0} > 0$ the $\sigma$-points are further from the central $\sigma$-point $x^{0}$, whereas for $W^{0}<0$ they are closer to the central $\sigma$-point $x^{0}$. For a more detailed discussion on the choice of weights refer to [7] and [14]. Note that, naturally, the weights satisfy the relationship $\sum _{i=0}^{2n} W^{i} = 1$.

Next, the $\sigma$-points $\left\{ \varvec{x}^{j}_{k-1|k-1} \right\}$ are transformed to the new state according to (4) or using (5). In such a way a new set of the transformed points $\left\{ \varvec{x}_{k|k-1}^{i} \right\}$ is obtained (the weights of the $\sigma$-points remain unchanged).

The predicted state $\hat{\varvec{x}}_{k|k-1}$ is computed as the weighted sum of the transformed $\sigma$-points

$$\begin{aligned} \hat{\varvec{x}}_{k|k-1} = \sum _{i=0}^{2n} W^{i} \varvec{x}_{k|k-1}^{i} \end{aligned}$$

(8)

To compute a predicted measurement, the set of the transformed $\sigma$-points is transformed again, this time using the nonlinear function adequate for observations (6). This results in the ‘measurement’ points

$$\begin{aligned} \varvec{z}_{k|k-1}^{i} = \varvec{c}\left( \varvec{x}_{k|k-1}^{i} \right) , \quad i=0,\ldots , 2n \end{aligned}$$

(9)

The predicted measurement is calculated similarly as the predicted state:

$$\begin{aligned} \hat{\varvec{z}}_{k|k-1} = \sum _{i=0}^{2n} W^{i} \varvec{z}_{k|k-1}^{i} \end{aligned}$$

(10)

Using the above results we calculate the covariance matrix of the predicted state

$$\begin{aligned} \varvec{P}_{k|k-1} = \varvec{Q} + \sum _{i=0}^{2n} W^{i} \left( \varvec{x}_{k|k-1}^{i} - \hat{\varvec{x}}_{k|k-1} \right) \left( \varvec{x}_{k|k-1}^{i} - \hat{\varvec{x}}_{k|k-1} \right) ^{\intercal } \end{aligned}$$

(11)

where $\varvec{Q}$ is the covariance matrix (3), and the covariance matrix of the predicted measurement

$$\begin{aligned} \varvec{S}_{k} = \varvec{R} + \sum _{i=0}^{2n} W^{i} \left( \varvec{z}_{k|k-1}^{i} - \hat{\varvec{z}}_{k|k-1} \right) \left( \varvec{z}_{k|k-1}^{i} - \hat{\varvec{z}}_{k|k-1} \right) ^{\intercal } \end{aligned}$$

(12)

where $\varvec{R}$ is the covariance matrix of the measurement noise $\varvec{\zeta }_{k}$ in (6).

The gain of the UKF is:

$$\begin{aligned} \varvec{K}_{k} = \Biggl [ \sum _{i=0}^{2n} W^{i} \left( \varvec{x}_{k|k-1}^{i} - \hat{\varvec{x}}_{k|k-1} \right) \left( \varvec{z}_{k|k-1}^{i} - \hat{\varvec{z}}_{k|k-1} \right) ^{\intercal } \Biggr ] \varvec{S}_{k}^{-1} \end{aligned}$$

(13)

The UKF innovation is

$$\begin{aligned} \varvec{v}_{k} = \varvec{z}_{k} - \hat{\varvec{z}}_{k|k-1} \end{aligned}$$

(14)

where $\varvec{z}_{k}$ is the ‘true’ measurement collected by a sensor.

The updated state estimate is calculated according to the following equation:

$$\begin{aligned} \hat{\varvec{x}}_{k|k} = \hat{\varvec{x}}_{k|k-1} + \varvec{K}_{k} \varvec{v}_{k} \end{aligned}$$

(15)

with the corresponding covariance matrix

$$\begin{aligned} \varvec{P}_{k|k} = \varvec{P}_{k|k-1} - \varvec{K}_{k} \varvec{S}_{k} \varvec{K}_{k}^{\intercal } \end{aligned}$$

(16)

5 Simulation Example

In this section we present an exemplary process model and a simulation example.

5.1 Process Model

We consider a nonlinear continuous-time stochastic model of a mobile robot shown in Fig. 1. The robot has two wheels of radius r, which are connected by axle of an length l. Both wheels can rotate at different speeds, thereby changing the orientation of the robot. We assume that the wheels do not slip and that the robot moves only in the direction of the body orientation. The angle between the body orientation and the x-axis is denoted as $\theta$. The model is based on [5, 11, 15], however we add two additional state variables ($\omega _{1}$ and $\omega _{2}$) describing the angular velocities of the wheels.

Fig. 1

Mobile robot on the Cartesian plane

The position x-y of each wheel ($i=1,2$) of the robot evolves according to the equations

$$\begin{aligned} \begin{aligned} dx_{i}&= r \cos {\theta }d\phi _{i} \\ dy_{i}&= r \sin {\theta }d\phi _{i} \end{aligned} \end{aligned}$$

(17)

where ($\phi _{i}$, $i=1,2$) are the angles by which the wheels rotate about their axes.

Thus the equations describing the motion of the robot are

$$\begin{aligned} \begin{aligned} dx&= \frac{dx_{1} + dx_{2}}{2} \\ dy&= \frac{dy_{1} + dy_{2}}{2} \\ ld\theta&= rd\phi _{1} - rd\phi _{2} \end{aligned} \end{aligned}$$

(18)

where x and y yield the position of the middle point of the axle, l is the length of the axle and $\theta$ is the angle of orientation of the robot.

By substituting (17) into (18) one obtains

$$\begin{aligned} \begin{aligned} dx&= \frac{r \cos {\theta }d\phi _{1} + r \cos {\theta }d\phi _{1}}{2} = \frac{r \cos {\theta }(d\phi _{1} + d\phi _{1})}{2}\\ dy&= \frac{r \sin {\theta }d\phi _{1} + r \sin {\theta }d\phi _{1}}{2} = \frac{r \sin {\theta }(d\phi _{1} + d\phi _{1})}{2}\\ d\theta&= \frac{rd\phi _{1} - rd\phi _{2}}{l} \end{aligned} \end{aligned}$$

(19)

The angles ($\phi _{i}$, $i=1,2$) by which the wheels rotate about their axes are described by the following differential equations

$$\begin{aligned} \begin{aligned} d\phi _{1}&= \omega _{1}(t)dt \\ d\phi _{2}&= \omega _{2}(t)dt \\ \end{aligned} \end{aligned}$$

(20)

where $\omega _{1}$ and $\omega _{2}$ are the angular velocities of the corresponding wheels.

We assume that the angular velocities of the wheels can be described by the following stochastic differential equations:

$$\begin{aligned} \begin{aligned} d\omega _{1}&= \kappa _{1} (\mu _{1} - \omega _{1}) + \sqrt{D} dw_{1} \\ d\omega _{2}&= \kappa _{2} (\mu _{2} - \omega _{2}) + \sqrt{D} dw_{2} \\ \end{aligned} \end{aligned}$$

(21)

where $\kappa _{i}$ ($i=1,2$) describes the rate of the ith velocity mean reversion, $\mu _{i}$ is the long-term mean of the ith velocity, $dw_{1}$ and $dw_{2}$ are infinitesimal increments of two independent one-dimensional Wiener processes, respectively, and $\sqrt{D}$ is a (modeling) noise scaling factor. Using the above equations, the trajectory of the mobile robot can be described by the following set of stochastic differential equations:

$$\begin{aligned} \begin{bmatrix} dx \\ dy \\ d\theta \\ d\omega _{1} \\ d\omega _{2} \\ \end{bmatrix} = \begin{bmatrix} r \frac{\omega _{1}(t)+\omega _{2}(t)}{2} \cos {\theta (t)} \\ r \frac{\omega _{1}(t)+\omega _{2}(t)}{2} \sin {\theta (t)} \\ \frac{r}{l} (\omega _{1}(t) - \omega _{2}(t))\\ \kappa _{1} (\mu _{1} - \omega _{1})\\ \kappa _{2} (\mu _{2} - \omega _{2})\\ \end{bmatrix}dt + \sqrt{D} \begin{bmatrix} 0&0 \\ 0&0 \\ 0&0 \\ 1&0 \\ 0&1 \\ \end{bmatrix} \begin{bmatrix} d w_{1} \\ d w_{2} \\ \end{bmatrix} \end{aligned}$$

(22)

where the state of the robot is its position (x, y), the angle of the body orientation $\theta$ and the angular velocities of the wheels ($\omega _{1},\omega _{2}$).

We assume that the position (x, y) and the angle $\theta$ are measured. Therefore the map $\varvec{c}(\varvec{x}_{k})$ in (6) is linear and is independent of the state $x_{k}$, and can be consequently described by the following matrix:

$$\begin{aligned} \varvec{c}(\varvec{x}_{k}) = \varvec{C} = \begin{bmatrix} 1&0&0&0&0 \\ 0&1&0&0&0 \\ 0&0&1&0&0 \\ \end{bmatrix} \end{aligned}$$

(23)

The covariance matrix of the measurement noise $\zeta$ is

$$\begin{aligned} \varvec{R} = \begin{bmatrix} 1&0&0 \\ 0&1&0 \\ 0&0&0.05 \end{bmatrix} \end{aligned}$$

(24)

The initial state is

$$\begin{aligned} \begin{aligned} x_{0}&= \begin{bmatrix} 0.0,&0.0,&\pi /4,&1.0,&1.0 \end{bmatrix}^{\intercal } \\ \end{aligned} \end{aligned}$$

(25)

and the other parameters of the model are as follows:

$$\begin{aligned} \begin{aligned} D&= 0.04 \\ r&= 1, \quad&l&= 4\\ \kappa _{1}&= 0.01, \quad&\kappa _{2}&= 0.01 \\ \mu _{1}&= 2.0, \quad&\mu _{2}&= 2.4 \\ \end{aligned} \end{aligned}$$

(26)

The process was simulated for $0 \le t \le 50$, and 500 trajectories were generated. The estimation results represent the average of the results obtained in all the simulation runs.

An exemplary trajectory of the robot positions (x, y) on the plane with the corresponding measurements is presented in Fig. 2.

Fig. 2

Exemplary trajectory (x, y) of the robot with the corresponding measurements

5.2 Estimation Results

The outcomes³ of the estimation of position x, position y, angle $\theta$ and the angular velocity $\omega _{1}$, for both methods are presented in Figs. 3, 4, 5 and 6, respectively. The results for the forward Euler method are marked with dashed lines and the results for the simulation-based method are denoted by solid lines.

Fig. 3

Position x estimation errors obtained from: the forward Euler method (dashed line) and the simulation-based method (solid line)

Fig. 4

Position y estimation errors obtained from: the forward Euler method (dashed line) and the simulation-based method (solid line)

Fig. 5

Angle $\theta$ estimation errors obtained from: the forward Euler method (dashed line) and the simulation-based method (solid line)

Fig. 6

Angular velocity $\omega _{1}$ estimation errors obtained from: the forward Euler method (dashed line) and the simulation-based method (solid line)

The observed correlation between the errors of both methods results from the fact that the two estimators were tested for the same set of 500 trajectories.

Based on the presented example we can conclude that the estimation errors for the simulation-based approach are smaller than the errors gained with the forward Euler method. Clearly, the resulting difference in performance depends on the process under estimation and the sampling period T. Nevertheless, the estimates for the angular velocity are almost the same for both methods. This is because the velocities were modeled as the Wiener processes, for which better prediction has no effect.

6 Conclusions

To draw a general conclusion about the performance quality of both methods, an analytic method for determining bounds on the errors is needed. For the Euler method, the one-step local error is of the order $O(T^2)$. Thus, for example, a two times smaller sampling period leads to a four times smaller prediction error, approximately. Whereas in the simulation-based method the local error can be made arbitrary small, depending on the chosen integration step, which can be much smaller than T. With proper optimization using adaptive methods of integration of ordinary differential equations (eg. ODE23, ODE45), the increase in the computational cost of the simulation-based method need not to be high. Moreover, the simulation-based method yields an additional degree of freedom. By choosing a method of integration and its parameters one can trade-off between better estimation performance and lower computational costs.