0
Research Papers

# Variational Integrators for Structure-Preserving FilteringOPEN ACCESS

[+] Author and Article Information
Jarvis Schultz

Mem. ASME
Department of Mechanical Engineering,
Northwestern University,
Evanston, IL 60208
e-mail: jschultz@northwestern.edu

Kathrin Flaßkamp

Department of Mechanical Engineering,
Northwestern University,
Evanston, IL 60208
e-mail: kathrin.flasskamp@northwestern.edu

Todd D. Murphey

Mem. ASME
Professor
Department of Mechanical Engineering,
Northwestern University,
Evanston, IL 60208
e-mail: t-murphey@northwestern.edu

Contributed by the Design Engineering Division of ASME for publication in the JOURNAL OF COMPUTATIONAL AND NONLINEAR DYNAMICS. Manuscript received October 27, 2015; final manuscript received September 1, 2016; published online December 2, 2016. Assoc. Editor: Zdravko Terze.

J. Comput. Nonlinear Dynam 12(2), 021005 (Dec 02, 2016) (10 pages) Paper No: CND-15-1347; doi: 10.1115/1.4034728 History: Received October 27, 2015; Revised September 01, 2016

## Abstract

Estimation and filtering are important tasks in most modern control systems. These methods rely on accurate discrete-time approximations of the system dynamics. We present filtering algorithms that are based on discrete mechanics techniques (variational integrators), which are known to preserve system structures (momentum, symplecticity, and constraints, for instance) and have stable long-term energy behavior. These filtering methods show increased performance in simulations and experiments on a real digital control system. The particle filter as well as the extended Kalman filter benefits from the statistics-preserving properties of a variational integrator discretization, especially in low bandwidth applications. Moreover, it is shown how the optimality of the Kalman filter can be preserved through discretization by means of modified discrete-time Riccati equations for the covariance updates. This leads to further improvement in filter accuracy, even in a simple test example.

<>

## Introduction

Estimation and filtering techniques for unknown system states based on noisy sensor data are crucial in many control systems. These methods are implemented in discrete time on digital embedded systems. Thus, they rely on accurate approximations of continuous-time system behavior; in low-bandwidth applications, predictions over longer time-horizons are required. Traditionally, explicit Euler discretizations of the system dynamics or high-order Runge-Kutta methods are chosen. Due to computational restrictions and real-time constraints, the first-order explicit Euler method is often the standard choice. However, low-order variational integration methods provide an alternative that allows efficient and accurate predictions. This paper studies the benefits of structure-preserving integration in two commonly used filtering approaches: (extended) Kalman filters and particle filters.

###### Kalman Filters.

The Kalman filter is likely the most well-known and widely used estimator [1]. When the Kalman filter is applied to a linear system with additive Gaussian noise, it provides the optimal, maximum-likelihood (minimum-variance) state estimator [2]. The Kalman filter has been successfully applied to a wide variety of problems in image processing, wireless communication, aerospace, robotics, and so on. While the Kalman filter is simple to compute, the requirement of linear dynamics limits its applicability to nontrivial systems. The extended Kalman filter (EKF) relaxes the linearity requirement by utilizing local linearizations to approximate posterior distributions as Gaussian. It is only slightly more complex than the Kalman filter, and it is applicable to a larger class of systems. One drawback of the EKF is that its performance is strongly influenced by the severity of any nonlinearities in the system. Nevertheless, the EKF is commonly employed and has demonstrated reasonable performance in a wide range of applications.

###### Particle Filters.

In recent years, particle filters have become a popular technique for estimation [3,4], likely due to their generality. In a particle filter, the uncertainty posterior distribution is represented by a finite collection of parameters referred to as particles. These particles are drawn from a distribution representing the current belief of the system's state, and then each particle is mapped forward in time using a model of the system. As the number of particles approaches infinity, the distribution of the particles approaches the solution to the Fokker-Plank equation associated with the system [5]. A primary feature of the particle filter is its wide applicability. Particle filters work with arbitrary noise model and have no issues with nonlinear system dynamics. A strong disadvantage of particle filter lies in their computational complexity. Its reliability is primarily governed by the number of particles used, and in some problems, the number of particles required to approximate the true posterior distribution may be quite large (104–106).

###### Discrete Mechanics.

Discrete mechanics derives models for mechanical systems via discrete variational principles [6]. These models define numerical integrators (variational integrators (VIs), also called symplectic integrators) which possess beneficial properties: They conserve symplecticity and symmetries (momenta) of the continuous-time system, and they have stable energy behavior even in long-term simulations, which is important in astrodynamics or molecular dynamics simulations [7]. Furthermore, VIs exactly satisfy holonomic constraints common in multibody systems. VIs can be used to design an indirect or a direct optimal control method (cf., e.g., Ref. [8]) that inherits the advantages of forward integration. Additionally, the problem formulation requires fewer optimization variables and leverages structured linearizations for efficient optimization algorithm design (cf. Ref. [9]). Recently, the discrete mechanics concept has been extended to stochastic integrators (see Refs. [10,11]) which almost surely preserve the symplectic structure. In Ref. [12], stochastic variational integrators have been successfully applied for estimation problems on Lie groups using a method based on uncertainty ellipsoids. This paper extends results of the authors' previous works [13,14].

###### Embedded Systems.

One of the primary motivations for this work is to develop and explore filter implementations that preserve geometric structure in order to improve performance while also facilitating implementation on an experimental embedded system. In recent years, much work has been done on leveraging a variety of numerical methods for improving the performance of estimation routines [1517], but these methods typically achieve increased performance through higher-order, computationally expensive methods that may not be appropriate for embedded estimation. For an arbitrary choice of integrator for use in an embedded system, one would hope that the computational cost was low to deal with limited computational resources, and that the formulation could be cast as a one-step map that is easily linearized to fit the form of standard discrete control and estimation routines. One may also hope that the integration method satisfies holonomic constraints as often encountered in mechanical systems. Moreover, the satisfaction of holonomic constraints should not result in loss of the ability to linearize the discrete map across a time-step. The variational integrator (VI) presented in Ref. [9] satisfies all of these requirements while also providing a symplectic integrator with stable long-term energy behavior. The linearizations of that VI are explicit and come with fixed computational cost further aiding in implementation on an embedded system. While the VI from Ref. [9] is not the only integrator used, herein it provides motivation for much of this work. This integrator is a drop-in replacement for linearizable one-step maps, such as the explicit Euler scheme, in standard forms of particle filters and extended Kalman filters. While there is a multitude of other integration schemes available, it is difficult to find another technique that does not compromise at least one of these requirements. For example, many integrators that satisfy holonomic constraints are not easily linearizable. Higher-order integrators often result in multistep methods, and the increased order results in increased computational cost. For several experimental examples presented in this paper, we compare VI-based estimation routines to corresponding routines based on explicit Euler integration. While other choices could be made for this comparison, due to our interest in estimation for embedded systems, we constrain our choice to low-order, one-step maps, and in this context, the explicit Euler integrator is a standard choice.1

###### Contributions.

To begin, we illustrate the advantageous statistical properties of first- and second-order variational integrators for simulating a stochastic harmonic oscillator. We then present particle filters and extended Kalman filters based on VI discretization and demonstrate their performance in simulated and experimental examples. It is demonstrated in Sec. 5 that both particle filters and extended Kalman filters may be highly sensitive to the choice of integration scheme and that VI-based methods outperform the discrete-time filter obtained from zero-order-hold and explicit Euler discretization—a combination that fits well within the embedded systems requirements discussed in the previous section. A second important contribution of this work is the derivation of structure-preserving covariance updates for a Kalman filter. We prove that the modified update equations additionally capture the Hamiltonian/symplectic structure resulting from the optimality property of the Kalman filter. We then apply a traditional Kalman filter and a Kalman filter with the structure-preserving covariance update equations to a simple example. It is shown that a Kalman filter with the structure-preserving update equations outperforms the filter with standard covariance updates.

In Sec. 2, we give an introduction to variational/symplectic integrators for deterministic and stochastic systems. Section 3 provides an introduction to particle filters and illustrates how symplectic integration can be used in particle filtering in order to reduce the common issue of particle deprivation. In Sec. 4, we recall the definition of discrete-time Kalman filter and show how the one-step maps can be derived from variational integrators. The increased performance of these filters is experimentally demonstrated for a planar crane system in Sec. 5. We extend our approach by deriving structure-preserving covariance updates in Sec. 6. Finally, we discuss the results and provide concluding remarks in Sec. 7.

## Variational Integrators

This section provides a short overview of variational integrators for deterministic/stochastic simulation, optimal control, and estimation. For more details, see Refs. [711].

###### Variational Mechanics.

A mechanical system can be described by its Lagrangian L, a real-valued function that depends on the system's configuration q(t) and velocity $q˙(t)$ at any time t. The Lagrangian is typically the difference of kinetic and potential energy, whereas the system's Hamiltonian H is typically the sum of all energies. In addition, there may be forces $f(q,q˙,u)$ such as friction or a control input force that influence the system's dynamics. The system dynamics are described by the Lagrange-d'Alembert principle, such that they satisfy the variational equation (cf. Ref. [6]) Display Formula

(1)$δ∫0TL(q,q˙) dt+∫0Tf(q,q˙,u)·δq dt=0$

Equivalently, $(q,q˙)$ (for a given control input u on $[0,T]$) satisfy the forced Euler–Lagrange equations Display Formula

(2)$∂L∂q(q,q˙)−ddt(∂L∂q˙(q,q˙))+f(q,q˙,u)=0$

Under certain regularity assumption (cf. Ref. [6] for details), these equations can be equivalently written as Hamilton equations in configuration q and momentum variables $p=(∂L/∂q˙)(q,q˙)$ with Hamiltonian $H(q,p)=p·q˙−L(q,q˙)$ and Hamiltonian forcing fHDisplay Formula

(3)$q˙=∂H∂p, p˙=−∂H∂q+fH(q,p,u)$

In the absence of forces, the flow of a Hamiltonian system (and thus, of an unforced Lagrangian system) is symplectic.2

###### Discrete Mechanics.

In order to simulate the system dynamics, most numerical integration schemes, e.g., Runge-Kutta methods, would be applied to either the Euler–Lagrange equations (2) or the Hamilton equations (3). This differs from variational integrators, which instead discretize the variational equation (1). This process generates an iterative numerical integration scheme, and the approach guarantees that structures of the original continuous system are preserved, e.g., symplecticity and symmetries in terms of system invariances (cf. Ref. [6]). Furthermore, variational integrators have exponentially stable energy behavior over long time horizons (cf. Ref. [7]).

To derive a discrete variational integration scheme, the action map, i.e., the first term in Eq. (1), is approximated over each time step $[kh,(k+1)h]$ by Display Formula

(4)$Ld(qk,qk+1,h)≈∫kh(k+1)hL(q(t),q˙(t))dt$

and the forcing term is discretized in a similar manner with discrete forces $fk−, fk+$. The term $Ld(qk,qk+1,h)$ is referred to as the discrete Lagrangian, and $fk−, fk+$ are the left and right discrete forces, respectively [6]. Taking variations of the discrete configurations qk for $k=1,…,N−1$ leads to the discrete forced Euler–Lagrange equations (cf. Ref. [6,8]) for $k=1,…,N−1$

$D1Ld(qk,qk+1)+D2Ld(qk−1,qk)+fk−+fk−1+=0$

where $DnLd$ is the slot derivative3 of Ld. Discrete momenta are given by the relationships $pk=−D1Ld(qk,qk+1)−fk−$ and $pk+1=D2Ld(qk,qk+1)+fk+$ which implicitly define a symplectic one-step integration map$Ψk+1:(qk,pk)↦(qk+1,pk+1)$. It is shown in Ref. [7] that (in the unforced case) a large class of variational integrators can be written as symplectic partitioned Runge-Kutta methods. In Ref. [8], the definition of symplecticity in VI/symplectic partitioned Runge-Kutta methods is extended to external forces. Thus, we will use both VI and symplectic partitioned Runge-Kutta schemes throughout this paper. In particular, we will use the VI midpoint rule for the planar crane system (Sec. 5) and the symplectic Euler method for Kalman filtering in Sec. 6.

###### Stochastic Symplectic Integrators.

In Ref. [11], the Hamilton-Pontryagin variational principle is extended to stochastic dynamical systems. It is shown that a stochastic mechanical system possesses an almost surely variational structure, and thus is almost surely symplectic and momentum map preserving. These properties carry over to the stochastic variational integrator that Ref. [11] derives from analogous discrete variations. While their first-order integrator is derived from Hamilton-Pontryagin principle in configuration, momentum, and velocity coordinates, the stochastic symplectic Euler method used in this work is based on Hamilton equations in (q, p) coordinates.

In Ref. [10], stochastic integrators for Hamiltonian systems are developed with deterministic dynamics (as in Eq. (3)) and additive noise terms driven by independent Wiener processes. Their methods are built on symplectic partitioned Runge-Kutta schemes as introduced in Ref. [7] for deterministic systems. In our numerical examples, we use one of the first-order methods presented in Ref. [10, Eq. (3.6)] Display Formula

(5)$qk+1=qk+h∂H∂p(pk,qk+1)+σΔkω1pk+1=pk−h∂H∂q(pk,qk+1)+γΔkω2$

for $k=0,1,…,N−1$, where $Δkω1,2$ are discretized independent Wiener processes and $σ,γ∈ℝ$. In the following, we refer to this method as the symplectic Euler scheme. Note that this method simplifies to an explicit symplectic method for separable Hamiltonian systems ($H=T(p)+V(q)$).

Additionally, we approximate the deterministic mechanics by a second-order variational midpoint integrator, which, applied to stochastic mechanical systems, leads to Display Formula

(6)$qk+1=qk+h∂H∂p(pk+pk+12,qk+qk+12)+σΔkω1pk+1=pk−h∂H∂q(pk+pk+12,qk+qk+12)+γΔkω2$

Alternatively, the midpoint integrator can be derived variationally from $Ld(qk,qk+1,h)=h·L((qk+1+qk)/2,(qk+1−qk)/h)$ which we will use in the examples presented in Sec. 5. The stochastic processes are then included as shown in Ref. [11].

The following example of a stochastic harmonic oscillator is adapted from Ref. [10], and we use it to illustrate the crucial role of the integration scheme in stochastic simulations.

Example 2.1. (Stochastic Harmonic Oscillator). The stochastic differential equations for the harmonic oscillator with configuration $q∈ℝ$, momentum $p∈ℝ$, Hamiltonian $H(q,p)=(1/2)(q2+p2)$, and Wiener processes $w1,2$ are given by Display Formula

(7)$(dqdp)=( 01−10)(qp)dt+(σ dw1(t)γ dw2(t))$

We use the analytic solution presented in Ref. [10] to investigate the flow from a set of initial conditions (circle about (0, 0) in Fig. 1) and to compare the approximation quality of the different integrators. The first integration method is the standard Euler–Maruyama integrator (see, e.g., Ref. [5]). As can be seen in Fig. 1, its flow artificially expands the radius of the circle (blue dots), and the mean location of the circle drifts significantly from the true solution. This is unlike the behavior of the two symplectic methods (second-order midpoint in red; points mostly hidden by first-order symplectic Euler results depicted in green), which both follow the exact flow very closely and retain the circle area. In particular, we cannot observe a clear superiority of the second-order method over the symplectic Euler scheme in this example. Both symplectic integrators have statistics-preserving4 properties that are crucially important for estimator performance, as we illustrate in the remainder of this paper.

## Particle Filters

This section will provide a brief introduction to particle filters and it will provide a numerical example illustrating how a symplectic integrator can be leveraged to alleviate a common issue with particle filters.

###### Basic Particle Filtering.

The key concept behind a particle filter is that uncertainty distributions of the system's state will be represented by a finite collections of particles. No analytical expression for the probability density function associated with these distributions is required; rather, we assume that the particles are drawn from this distribution. In this way, the collection of particles itself represents an approximation of the distribution. This is opposed to other filters, e.g., the Kalman filter, where the uncertainty distribution is parameterized and the filter predicts the evolution of the parameters. We will now sketch the steps required for a particle filter following the terminology and syntax of Ref. [3].

###### Initialization.

A particle filter is initialized by generating a set of M particles $X0$ where each particle $x0m$ with $1≤m≤M$ represents a possible initial state of the system. This initial particle collection may be drawn from a known distribution or even assumed to all have the same value.

###### Proposal Distribution.

In each step of the particle filter, each particle from the previous time-step is mapped forward to the current time using a noisy system model and knowledge of the controls used during the previous time-step. This new collection of particles is referred to as a proposal distribution. This step is also the step where the integration techniques of this work are implemented.

###### Importance Weights.

In order to incorporate measurements, an importance weight for each particle is calculated. The importance weight uses a noisy measurement model to evaluate the probability of a measurement zt given a particle $xtm$, i.e., the weight of particle m at time t is $wtm=p(zt | xtm)$.

###### Resampling.

In most implementations of the particle filter, the next step is to incorporate the measurement zt into the system's uncertainty distribution through a resampling process. In this process, the particles representing a sampling of the posterior distribution for the current time-step are generated by randomly drawing with replacement a new set of M particles from the proposal distribution. This random drawing is biased by the importance weights calculated in the previous step. Particles from the proposal distribution with higher measurement probability have a higher probability of being drawn for inclusion in the posterior distribution. In this way, particles with low probability are culled from the collection, and the total particle distribution is drawn to the measurement. This process allows the statistic of the posterior distribution to include knowledge of the measurements and it helps to ensure that the process of generating the proposal distribution is not wasting time computational effort on integrating particles that have very low posterior probability.

###### Iterate.

To continue the particle filter, the posterior particle set from time-step t is used as the initial set for the Proposal Distribution step of time-step t + 1.

###### Particle Filters and Choice of Integrator.

A particle filter inherits the statistical properties of the integration scheme that is used for propagating the collection of particles. We demonstrate this by revisiting the stochastic harmonic oscillator example. In the applications presented in Sec. 5, we show an example of how a common issue with particle filters (particle deprivation) can be significantly reduced by using variational integrators.

In particle filtering algorithms, a collection of particles is mapped forward a single time-step using a noisy process model in order to predict a proposal distribution. Then, a measurement of the system is taken, and a new set of particles is produced by resampling the proposal distribution to produce an approximation of the posterior distribution. Particles with higher probability given the measurement value (and its corresponding probability density function) have a higher probability of being selected for the posterior distribution. This resampling process incorporates the measurement in the filter state by removing particles with low probability.

Typically, if measurements are collected regularly from a reliable sensor, the incorporation of the measurements will help to regulate errors caused by inaccurate integration of the particle states. However, there are many situations where one may encounter unreliable measurements, for example, in high-dimensional data association problems, systems with unreliable communication networks, or tracking systems suffering from occlusions. In these situations, the particle filter must predict the state of the system over long time horizons without measurements. As a result, symplectic integrators may significantly increase the performance of the particle filter. An illustration of this can be seen in the following example.

Example 3.1. (Stochastic Harmonic Oscillator). In Fig. 2, the statistical properties of the harmonic oscillator of Eq. (7) are simulated by integrating a collection of 1000 particles for 60 s with a time-step of 0.0625 s using a midpoint VI and an explicit Euler integrator (RK1). The plot shows the eigenvalues of the covariance of the particle distribution as a function of time. It can be seen that the RK1 scheme adds significantly more noise to the system than the variational integrator. This is true even though the two collections of particles were driven by the same set of sample paths of the Wiener process. Even in this simple system, with no measurements to regulate the integration errors, the nonsymplectic RK1 integrator significantly overpredicts the uncertainty in the system state.

###### Particle Deprivation.

A common issue encountered in particle filters is that of particle deprivation [3]. In a filter suffering from particle deprivation, there are too few particles in the vicinity of the true state of the system. When this occurs, the resampling process may eventually drive the number of unique particles down to a single particle. While there are known heuristics for preventing this situation [3], we point out that an integrator adding artificial diffusion to the collection of particles will increase the filter's probability of suffering from particle deprivation when measurements are incorporated. Results illustrating this point are in Sec. 5.

## Kalman Filter

As a starting point, we consider the following formulation of a Kalman filter; originally developed in Ref. [1]. For a textbook reference, we refer to Ref. [20].

Proposition 4.1. (Discrete-time Kalman filter, [1]). Let a model of the system and the measurement be given by

$xk+1=Akxk+Gkwk,x0∼N(x0,P0), wk∼N(0,Qk)zk=Ckxk+vk, vk∼N(0,Rk)$

with Rk > 0 and${wk}k=0N, {vk}k=0N$ uncorrelated Gaussian white noise processes with covariance Qk and Rk respectively. The initial state x0 is assumed to be drawn from a Gaussian centered around a nominal initial state x0 with covariance the same as the filter's initial covariance P0. A measurement at time-step k is denoted zk. Then, the optimal estimation of the state,$x̂k+1$, given measurements up to step k, is given by the following update equations:

$Prediction: Pk+1−=AkPkAkT+GkQkGkT x̂k+1−=Akx̂kMeasurement update: Kk+1=Pk+1−Ck+1T(Ck+1Pk+1−Ck+1T+Rk+1)−1 Pk+1=(1−Kk+1Ck+1)Pk+1− x̂k+1=x̂k+1−+Kk+1(zk+1−Ck+1x̂k+1−)$

In Kalman's original work [1], the state transition matrix of the continuous-time system is used for Ak in Proposition 4.1. In general, this transition matrix cannot be computed analytically, but it can be approximated by an integration scheme applied to the continuous differential equation.

To give an example, when applying the explicit Euler scheme to the dynamical system $x˙=Ax$, one obtains the discrete update matrix $Ak=(1+hA)$, whereas the symplectic Euler scheme leads to the one-step map

$Ak=((1−hA11)−1h(1−hA11)−1A12hA21(1−hA11)−1h2A21(1−hA11)−1A12+1+hA22)$

###### Linearizations for Extended Kalman Filters.

The midpoint variational integrator has a unique characteristic referred to as a structured linearization [21,22]. Many estimation algorithms, such as the EKF, rely on local linearizations to adapt linear system tools to nonlinear systems. Typically, in the continuous time setting, to obtain a local linearization at a point, one would use a Taylor expansion of $x˙=f(x,u)$ about that point. This linearization is an infinitesimal linearization, but the discrete-time linearization is what is required. In other words, one needs to linearize the discrete map $xk+1=fk(xk,uk)$. In a standard implementation of an EKF [3], the discrete-time Kalman filter from Proposition 4.1 is adapted to a nonlinear system using the following linearizations: Display Formula

(8)$Ak=∂fk∂x|x̂k,uk and Ck=∂hk∂x|x̂k+1−$

where the measurement model is assumed to be of the form $zk=h(xk)+vk$.

As the presented VI is a one step method, it admits a simple linearization and even though the method is an implicit method, one can calculate an explicit linearization of the map [22]. Furthermore, since the VI is exactly sampling a nearby mechanical system, the VI linearization is an exact linearization of the nearby mechanical system. It is thus called a structured linearization [21,22]. This restriction on the behavior of the linearization greatly improves its local accuracy. In Sec. 5, we show an example of how the improved accuracy of this linearization leads to better EKF estimator performance.

In principle, a one-step map can be derived and linearized for implicitly defined RK schemes in the same manner, but higher order RK methods lead to complex Taylor series expansions due to the intermediate evaluation points. For that reason, implementations are often restricted to first-order methods. While this paper does not explicitly consider results related to estimation for constrained systems, we note that an additional interesting feature of the midpoint VI integrator is that it exactly satisfies holonomic constraints at each time-step while still admitting the explicit, structured linearization.

## Application to Planar Crane System

This section presents experimental and simulated examples demonstrating how the choice of discretization scheme may dramatically affect the performance of standard particle filters and extended Kalman filters. Further, we demonstrate improved performance of both filters when leveraging the structure-preserving properties of variational integrators.

In Secs. 5.1 and 5.2, we apply particle filters and EKFs to the planar crane system as shown in Fig. 3. This system and corresponding modeling strategy have been discussed by the authors in Refs. [21] and [23]. In those works, a modeling strategy was presented where the position of the winch system xr and the length of the string r were treated as kinematic inputs, i.e., inputs where the system has sufficient control authority to perfectly track any desired trajectory [24]. With this modeling strategy, the Lagrangian for the system is only a function of the dynamic configuration variables (x, y) and is given by

Display Formula

(9)$L(q,q˙)=12m(x˙2+y˙2)−mgy$

The corresponding midpoint discrete Lagrangian (Eq. (4)) for the planar crane system is then Display Formula

(10)$Ld(qk,qk+1)=m2Δt((xk+1−xk)2+(yk+1−yk)2)−mg2Δt(yk+1+yk)$

As a result of the kinematic input assumption, all damping in the crane portion of the system is assumed to be negligible. Moreover, through experimentation, we found that the damping in the swing dynamics of the payload was low enough to also be considered negligible. Thus, the deterministic models of the dynamics of this system have no damping.

For all results in this section, the noise models are assumed to be additive Gaussian noise with zero mean. The covariances parameterizing these distributions are all assumed to be diagonal, and the experimental system only measures relevant configuration variables, i.e., there are no measurements of velocity or discrete generalized momentum.

###### Numerical Results.

Figure 4 shows simulated effects of the choice of integration scheme on performance of the EKF algorithm at a range of time-steps. Each point on the plot was generated by running 1000 trials of the filter on a nominal feasible trajectory for the system described in Fig. 3. In the “prediction” step of the EKF, actual samples from the nominal trajectory were used, i.e., the predictions without additive noise corresponded exactly with the nominal trajectory. Measurements were simulated by adding Gaussian noise to the nominal trajectory. For each trial, the L2 error between the filtered signal and the reference signal was determined at each time-step. These errors were then averaged to produce an “error norm” for each trial. The error norms were then averaged, and their standard deviations calculated to produce the points and error bars in Fig. 4. For the upper, solid curve, the local linearization was performed by evaluating the infinitesimal derivatives of the continuous dynamics about the current best estimate, and then using an explicit Euler approximation to convert this into a discrete linearization. For the bottom curve, the structured linearization of the midpoint variational integrator discussed in Sec. 4.1 was used. It is easily seen that as the filter frequency decreases, the performance of the discrete-mechanics-based variational integrator representation significantly outperforms the explicit Euler approximation.

###### Experimental Results.

In this section, we use experimental data from the robotic planar crane system described in Refs. [21] and [23]. This system utilizes digital encoders to close control loops around xr and r. Thus, the kinematic-input modeling strategy, mentioned in Sec. 5, is used again. Since these inputs are assumed to be perfectly controlled, the dynamic configuration variables (x, y) are the only state variables estimated.

Figure 5 shows the parametric evolution of estimates of the dynamic configuration variables using particle filters and EKFs at two different frequencies using both a midpoint VI and an RK1 integrator to represent the system. The strength of the VI integrator can be seen by noting that the particle filter and the EKF estimates of the system's uncertainty are nearly identical even at frequencies as low as 6 Hz. The two filter estimates are not only in agreement with each other, but they are also in agreement between the frequencies. We emphasize that, at 6 Hz, the time-step used for integration, measuring, linearization, and estimation is $≈$0.167 s. With this large time-step, the RK1 particle filter is useless.

Figure 6 shows the time evolution of the eigenvalues of the covariances from each of the estimators in Fig. 5. This figure further demonstrates that the VI covariance predictions are stable even at large time-steps. Additionally, it demonstrates that, in this particular system, the structured linearization allows the EKF to perform nearly identically to the particle filter. In this case, the increased accuracy of the structured linearization avoids the increased computational expense of the particle filter while achieving similar performance.

In Sec. 3.3, the issue of particle deprivation in particle filters was discussed. It was explained that the VI reduces the likelihood of this issue by removing artificial noise injected by a traditional integrator; this is especially true at low frequencies. Figure 7 shows the number of unique particles as a function of time for the 6 Hz particle filters using the VI and RK1 integrator. In several cases, the particle distribution from the RK1 integrator is artificially spread to the point that resampling produces only two unique particles (out of 1000) which implies that uncertainty estimates from the filter are essentially useless.

## Kalman Filter With Structure-Preserving Covariance Updates

In Sec. 4, we discussed the difference between symplectic and nonsymplectic discrete system matrices Ak in the Kalman filter and showed benefits of using symplectic discretization schemes for filtering applications in Sec. 5. However, the Kalman filter provides further structure due to its optimality property. In this section, we derive a modification to the previously used Kalman filter that preserves this optimality structure through discretization and thereby further improves the filter performance.

To this aim, we start with the continuous-time Kalman filter, as introduced in Ref. [25] (see Proposition 6.1 below). The continuous-time Kalman filtering problem is dual to a linear quadratic regulator (LQR) problem: While a linear quadratic regulator controller minimizes a quadratic cost functional, the Kalman filter determines state estimates that minimize the expected squared error to the process' true state (cf. Ref. [25]). In particular, both problems exhibit an additional Hamiltonian structure (independent of the mechanical Hamiltonian structure). This can be seen by examining the state-adjoint system that belongs to the respective problem's optimal solutions.

Our approach aims at additionally preserving this property through numerical discretization. Therefore, we set up a symplectic discretization of the state-adjoint system for the Kalman filtering problem. It is not necessary to explicitly solve for the adjoint equations, though. Instead, we directly derive modified discrete-time Riccati equations which are independent of the states and adjoints (as previously presented in Ref. [14]). Thus, our Kalman filter algorithm only consists of a single covariance update step and an all-in-one state update afterward (see Algorithm 6.1). The performance of this algorithm is studied in Example 6.1 and compared to the discrete-time Kalman filter methods with standard covariance updates.

Proposition 6.1. (From Ref. [25]). Let the model of the system and the measurement be given by

$x˙=Ax+Gw, z=Cx+v$

where$x(t0)∼N(x0,P0), w∼N(0,Q), v∼N(0,R)$ with white noise processes${w(t)}, {v(t)}$ being uncorrelated, also with x0. Then, the optimal filter is given by

$x̂˙=Ax̂+K(z−Cx̂), x̂(0)=x0, with K=PCTR−1P˙=AP+PAT−PCTR−1CP+GQGT, P(t0)=P0$

In Ref. [25], the duality relation to the LQR problem is used to state a Hamiltonian for the filtering problem

$H(x,w)=−12||GTx||Q2−wTATx+12||Cw||R−12$

such that the state-adjoint equations are given by Display Formula

(11)$x˙=∂H∂w=−ATx+CTR−1Cww˙=−∂H∂x=GQGTx+Aw$

where w is the adjoint vector. The covariance matrix P(t) with $P(t0)=P0≥0$ (P0 symmetric) is determined by Display Formula

(12)$P(t)=(Θ21(t,t0)+Θ22(t,t0)P0)(Θ11(t,t0)+Θ12(t,t0)P0)−1$

with Display Formula

(13)$Θ(t,t0)=(Θ11(t,t0)Θ12(t,t0)Θ21(t,t0)Θ22(t,t0))$

being the transition matrix of the continuous-time state-adjoint equations. As shown in Ref. [25, Sec. 10], the continuous state-adjoint system in Eq. (11) has a unique matrix solution pair $(X(t),W(t))$ that satisfies initial conditions $X(t0)=1,W(t0)=P0$. The solution pair satisfies the identity Display Formula

(14)$W(t)=P(t)X(t), ∀t≥t0$

from which the covariance equation (12) can be derived.

Lemma 6.1. The state-adjoint system (11) is Hamiltonian and the transition matrix$Θ(t,t0)$ is symplectic.

Proof. The system matrix of the combined state adjoint equations is $S:=(−ATCTR−1CGQGTA)$ and, since $CTR−1C$ and GQGT are symmetric, it satisfies $(JS)T=JS$ with $J=(01−10)$. So we have a (linear) Hamiltonian system, for which the flow is defined by the transition matrix. Thus, it follows that $Θ(t,t0)$ is a symplectic matrix for all $t≥t0$.◻

In our discrete-time approximation of the Riccati equation for the covariance matrix, we derive discrete-time covariance updates that exactly preserve the following two properties: the symplectic structure of the state-adjoint differential equations and their linear relationship defined by P(t). As in the continuous-time Kalman filter, the update equation for the covariance matrix Pk does not depend on the state or on the adjoint. For known system and error covariance matrices at all time nodes, ${Pk}k=1N$ can be computed beforehand.

Theorem 6.1. Let a system and measurement model be given as in Proposition 6.1 for a 2 n-dimensional mechanical system with state$(q,p)T$. Let$td={tk}k=0N$ denote a set of discrete time points, with$tk=kh$ for step size h, and assume P0 symmetric is given. We denote by$Ψk$ the one-step map of the symplectic Euler5 discretization of the four-dimensional state-adjoint system (11): Display Formula

(15)$(vk+1yk+1λk+1μk+1)=(vkykλkμk)+h(−ATCTR−1CGQGTA)(vk+1ykλkμk+1)$

with coordinates$xk=(vk,yk)T$ and$wk=(λk,μk)T$. Note that$vk, yk, λk, μk∈ℝn$. Then,$Ψk$ is symplectic by construction. Further, if the discrete covariance updates for$k=0,1,…,N−1$ are computed byDisplay Formula

(16)$Pk+1=(Ψk21+Ψk22Pk)·(Ψk11+Ψk12Pk)−1$

with the linear one-step map$Ψk$ being identified with its representing matrix and$Ψk=(Ψk11Ψk12Ψk21Ψk22)$, then we have that$wk=Pkxk$ for all$k=0,1,…,N$.

Proof. Since $Θ(t,0)$ is a transition matrix, it satisfies the property

$Θ((k+1)h,0)=Θ((k+1)h,kh)·Θ(kh,0)$

The symplectic Euler discretization of Eq. (11) approximates the solution on the time grid $td={tk}k=0N, xk≈x(hk), wk≈w(hk)$. Thus, we have $Ψk$ as an approximation of $Θ((k+1)h,kh)=Θ(tk+1,tk$):

$(xk+1wk+1)=Ψk(xkwk)≈Θ((k+1)h,kh)(xkwk)$

and, by construction, $Ψk$ is symplectic (the symplectic Euler scheme indeed leads to a symplectic one-step map, see Ref. [7] for a proof).

The discrete state-adjoint relations, $(xk+1,wk+1)T=Ψk(xk,wk)T$, and the discrete covariance updates as defined in Eq. (16) are consistent in the way that a linear relationship between state and adjoints via the covariance matrices is ensured, i.e., $wk=Pkxk$ for all $k=0,1,…,N$ (compare to Eq. (14) in the continuous-time setting):

$Pk+1xk+1=wk+1$
$⇔ Pk+1(Ψk11xk+Ψk12wk)=Ψk21xk+Ψk22wk$
$⇔ Pk+1(Ψk11+Ψk12Pk)xk=(Ψk21+Ψk22Pk)xk$
$⇔ Pk+1=(Ψk21+Ψk22Pk)(Ψk11+Ψk12Pk)−1◻$

Note that as long as we have a time-invariant estimation problem, $Ψk$ is a constant matrix that can be obtained from solving the set of Eq. (15) for $(vk+1,yk+1,λk+1,μk+1)$ and generating the matrix entries just once. If the system and measurement model matrices are time-varying though, this step would have to be performed in every iteration.

For estimation problems of mechanical systems, it is desirable to also preserve the mechanical structure (i.e., Hamiltonian matrix, A, or Hamiltonian plus forcing), as demonstrated in Sec. 4. Therefore, we combine the structure-preserving covariance update of Theorem 6.1 with discrete symplectic state updates. This leads to the following algorithm that resembles the continuous-time filter (see Proposition 6.1) in that it does not split into prediction and measurement update steps as the discrete Kalman filter does (cf. Proposition 4.1).

Algorithm 6.1. For the implementation of the structure-preserving Kalman filter with symplectic state and covariance updates using symplectic Euler integration, we modify the continuous-time Kalman filter algorithm of Ref. [20] to 1. take measurement6$zk+1$,2. update covariance matrix by3. $Pk+1=(Ψk21+Ψk22Pk)(Ψk11+Ψk12Pk)−1,$4. compute Kalman gain as $Kk+1=Pk+1CTR−1$,5. update state by $(qk+1pk+1)=(qkpk)+h[(A−Kk+1C)(qk+1pk)+Kk+1zk+1]$

Step 4 is again an implicit, but linear update, so it can be analytically inverted beforehand and then directly evaluated during filtering. While Algorithm 6.1 may not initially appear to follow the same procedure of the standard discrete Kalman filter (Proposition 4.1) in that there is no prediction or update step, we point out that one could rewrite the standard discrete Kalman filter to give a single covariance update equation analogous to the update rule in Step 2 of Algorithm 6.1. In general, the covariance update from Algorithm 6.1 and from Proposition 4.1 will not result in the same covariance evolution.

The previous results specifically use a first-order symplectic scheme for two reasons: We want to give practitioners a good idea how to implement the filter and we want to have a fair comparison to the standard approach that is based on nonsymplectic, first-order, explicit Euler discretizations (see Example 6.1). However, if higher-order symplectic discretization schemes, e.g., a second-order midpoint rule, are used, only the calculation of the one-step map based on a modified Eq. (15) and the state update in Step 4 of the algorithm need to be adjusted.

Example 6.1. (Stochastic Harmonic Oscillator). We revisit the example of a stochastic harmonic oscillator as introduced in Example 2.1. The structure-preserving Kalman filter method as presented in Algorithm 6.1 is compared to the discrete-time variants introduced in Sec. 4: the standard covariance updates with either an explicit Euler discretization for the discrete filter state update or a symplectic discretization by midpoint rule. Figure 8 shows the resulting filter behavior in state space (top row), as well as the covariance matrix eigenvalues (bottom left) and the mechanical energy of the oscillator (bottom right). All integrators use a step-size of h = 0.03125 and identical measurements generated from one sample path. Unlike the examples presented in Sec. 4, the symplectic state updates alone do not improve the filter; likely due to the simplistic nature of the harmonic oscillator. However, the novel variant of a structure-preserving filter has smaller covariances, as can be seen from the ellipses in the top plot and the covariance eigenvalues. In particular, the mechanical energy of the filtered state, which would ideally—in the absence of noise and without numerical errors—be a constant, is much more accurately preserved than in the other two cases.

## Conclusion

This paper presents three structure-preserving filtering methods for mechanical control systems: (1) Particle filter with symplectic state predictions, (2) Extended Kalman filter with symplectic state predictions and structured linearizations of variational integrators, and (3) Kalman filter with symplectic state update and additional symplectic covariance update. The filtering techniques have been tested in simulated examples and in a real-time experiment with a planar crane system. Symplectic discrete-time predictions of the states increased the performance of both the particle and the extended Kalman filter compared to the standard choice of explicit Euler updates. Replacing the covariance updates in the linear Kalman filter can improve its performance even further. We derive a modified discrete-time Riccati equation for the covariance matrices that preserves the Hamiltonian/symplectic structure of the system of optimality conditions that belongs to the continuous-time Kalman filter. This novel filter variant can be extended to nonlinear systems by linearizing the discretized state-adjoint system of the optimality conditions. The one-step map of the symplectic state-adjoint scheme, which is used for the covariance Riccati equations, will then become time dependent. Thus, one would expect an increased computational effort. However, this will be balanced by the observed higher accuracy of the method which allows a coarser time-grid than the nonsymplectic first-order methods. Further theoretical studies of the symplectic filtering approach can be performed by backwards error analysis [7] applied to the state-adjoint system. We also note that recent work [18] has shown that it may be possible to construct arbitrarily high-order variational integrators that are still linearizable one-step maps. These integrators are developed through backward error analysis [7] by constructing surrogate Lagrangians. High-order VI-based filtering techniques are another area for future investigation. In many real-world applications, the filtering methods presented herein would be combined with optimal control techniques. First results of a combination with the presented filters are given in Ref. [13]; a detailed study is left for future work.

## Acknowledgements

This work was supported by Army Research Office grant W911NF-14-1-0461, and by the National Science Foundation under Grant CMMI 1334609. Any opinions, findings, and conclusions or recommendations expressed in this material are those of the author(s) and do not necessarily reflect the views of the National Science Foundation.

## References

Kalman, R. E. , 1960, “ A New Approach to Linear Filtering and Prediction Problems,” J. Basic Eng., 82(1), pp. 35–45.
Stengel, R. F. , 1986, Optimal Control and Estimation, Dover Publications, Mineola, NY.
Thrun, S. , Burgard, W. , and Fox, D. , 2005, Probabilistic Robotics, (Intelligent Robotics and Autonomous Agents), The MIT Press, Cambridge, MA.
Thrun, S. , 2002, “ Particle Filters in Robotics,” 17th Annual Conf. on Uncertainty in AI (UAI), pp. 511–518.
Chirikjian, G. S. , 2009, Stochastic Models, Information Theory, and Lie Groups: Classical Results and Geometric Methods (Applied and Numerical Harmonic Analysis), Vol. 1, Birkhäuser/ Springer, Berlin.
Marsden, J. E. , and West, M. , 2001, “ Discrete Mechanics and Variational Integrators,” Acta Numerica, 10, pp. 357–514.
Hairer, E. , Lubich, C. , and Wanner, G. , 2006, Geometric Numerical Integration: Structure-Preserving Algorithms for Ordinary Differential Equations, 2nd ed., Vol. 31, Springer Series in Computational Mathematics, Springer, Berlin.
Ober-Blöbaum, S. , Junge, O. , and Marsden, J. E. , 2011, “ Discrete Mechanics and Optimal Control: An Analysis,” Control, Optim. Calculus Var., 17(2), pp. 322–352.
Johnson, E. , Schultz, J. , and Murphey, T. , 2014, “ Structured Linearization of Discrete Mechanical Systems for Analysis and Optimal Control,” IEEE Trans. Autom. Sci. Eng., 12(1), pp. 140–152.
Milstein, G. N. , Repin, Y. M. , and Tretyakov, M. V. , 2002, “ Symplectic Integration of Hamiltonian Systems With Additive Noise,” SIAM J. Numer. Anal., 39(6), pp. 2066–2088.
Bou-Rabee, N. , and Owhadi, H. , 2009, “ Stochastic Variational Integrators,” IMA J. Numer. Anal., 29(2), pp. 421–443.
Sanyal, A. K. , Lee, T. , Leok, M. , and McClamroch, N. H. , 2008, “ Global Optimal Attitude Estimation Using Uncertainty Ellipsoids,” Syst. Control Lett., 57(3), pp. 236–245.
Schultz, J. , and Murphey, T. , 2014, “ Extending Filter Performance Through Structured Integration,” American Controls Conf. (ACC), pp. 430–436.
Flaßkamp, K. , and Murphey, T. , 2015, “ Variational Integrators in Linear Optimal Filtering,” American Controls Conference (ACC), pp. 5140–5145.
Mazzoni, T. , 2007, “ Computational Aspects of Continuous–Discrete Extended Kalman-Filtering,” Comput. Stat., 23(4), pp. 519–539.
Patel, H. G. , and Sharma, S. N. , 2014, “ Third-Order Continuous-Discrete Filtering for a Nonlinear Dynamical System,” ASME J. Comput. Nonlinear Dyn., 9(3), p. 034502.
Emran, B. J. , Al-Omari, M. , Abdel-Hafez, M. F. , and Jaradat, M. A. , 2015, “ Hybrid Low-Cost Approach for Quadrotor Attitude Estimation,” ASME J. Comput. Nonlinear Dyn., 10(3), p. 031010.
De La Torre, G. , and Murphey, T. , 2016, “ On the Benefits of Surrogate Lagrangians in Optimal Control and Planning Algorithms,” IEEE International Conference on Decision and Control (CDC), (to be published).
Lew, A. , Marsden, J. E. , Ortiz, M. , and West, M. , 2004, “ Variational Time Integrators,” Int. J. Numer. Methods Eng., 60(1), pp. 153–212.
Lewis, F. L. , Xie, L. , and Popa, D. , 2008, Optimal and Robust Estimation, 2nd ed., CRC Press, Boca Raton, FL.
Schultz, J. , and Murphey, T. D. , 2013, “ Embedded Control Synthesis Using One-Step Methods in Discrete Mechanics,” American Controls Conference (ACC), pp. 5393–5298.
Murphey, T. D. , and Johnson, E. R. , 2011, “ Control Aesthetics in Software Architecture for Robotic Marionettes,” American Controls Conference (ACC), pp. 3825–3830.
Schultz, J. , and Murphey, T. , 2012, “ Trajectory Generation for Underactuated Control of a Suspended Mass,” IEEE International Conference on Robotics and Automation (ICRA), pp. 123–129.
Johnson, E. , and Murphey, T. D. , 2007, “ Dynamic Modeling and Motion Planning for Marionettes: Rigid Bodies Articulated by Massless Strings,” IEEE International Conference on Robotics and Automation (ICRA), pp. 330–335.
Kalman, R. E. , and Bucy, E. S. , 1961, “ New Results in Linear Filtering and Prediction Theory,” ASME J. Basic Eng., 83(1), pp. 95–108.
View article in PDF format.

## References

Kalman, R. E. , 1960, “ A New Approach to Linear Filtering and Prediction Problems,” J. Basic Eng., 82(1), pp. 35–45.
Stengel, R. F. , 1986, Optimal Control and Estimation, Dover Publications, Mineola, NY.
Thrun, S. , Burgard, W. , and Fox, D. , 2005, Probabilistic Robotics, (Intelligent Robotics and Autonomous Agents), The MIT Press, Cambridge, MA.
Thrun, S. , 2002, “ Particle Filters in Robotics,” 17th Annual Conf. on Uncertainty in AI (UAI), pp. 511–518.
Chirikjian, G. S. , 2009, Stochastic Models, Information Theory, and Lie Groups: Classical Results and Geometric Methods (Applied and Numerical Harmonic Analysis), Vol. 1, Birkhäuser/ Springer, Berlin.
Marsden, J. E. , and West, M. , 2001, “ Discrete Mechanics and Variational Integrators,” Acta Numerica, 10, pp. 357–514.
Hairer, E. , Lubich, C. , and Wanner, G. , 2006, Geometric Numerical Integration: Structure-Preserving Algorithms for Ordinary Differential Equations, 2nd ed., Vol. 31, Springer Series in Computational Mathematics, Springer, Berlin.
Ober-Blöbaum, S. , Junge, O. , and Marsden, J. E. , 2011, “ Discrete Mechanics and Optimal Control: An Analysis,” Control, Optim. Calculus Var., 17(2), pp. 322–352.
Johnson, E. , Schultz, J. , and Murphey, T. , 2014, “ Structured Linearization of Discrete Mechanical Systems for Analysis and Optimal Control,” IEEE Trans. Autom. Sci. Eng., 12(1), pp. 140–152.
Milstein, G. N. , Repin, Y. M. , and Tretyakov, M. V. , 2002, “ Symplectic Integration of Hamiltonian Systems With Additive Noise,” SIAM J. Numer. Anal., 39(6), pp. 2066–2088.
Bou-Rabee, N. , and Owhadi, H. , 2009, “ Stochastic Variational Integrators,” IMA J. Numer. Anal., 29(2), pp. 421–443.
Sanyal, A. K. , Lee, T. , Leok, M. , and McClamroch, N. H. , 2008, “ Global Optimal Attitude Estimation Using Uncertainty Ellipsoids,” Syst. Control Lett., 57(3), pp. 236–245.
Schultz, J. , and Murphey, T. , 2014, “ Extending Filter Performance Through Structured Integration,” American Controls Conf. (ACC), pp. 430–436.
Flaßkamp, K. , and Murphey, T. , 2015, “ Variational Integrators in Linear Optimal Filtering,” American Controls Conference (ACC), pp. 5140–5145.
Mazzoni, T. , 2007, “ Computational Aspects of Continuous–Discrete Extended Kalman-Filtering,” Comput. Stat., 23(4), pp. 519–539.
Patel, H. G. , and Sharma, S. N. , 2014, “ Third-Order Continuous-Discrete Filtering for a Nonlinear Dynamical System,” ASME J. Comput. Nonlinear Dyn., 9(3), p. 034502.
Emran, B. J. , Al-Omari, M. , Abdel-Hafez, M. F. , and Jaradat, M. A. , 2015, “ Hybrid Low-Cost Approach for Quadrotor Attitude Estimation,” ASME J. Comput. Nonlinear Dyn., 10(3), p. 031010.
De La Torre, G. , and Murphey, T. , 2016, “ On the Benefits of Surrogate Lagrangians in Optimal Control and Planning Algorithms,” IEEE International Conference on Decision and Control (CDC), (to be published).
Lew, A. , Marsden, J. E. , Ortiz, M. , and West, M. , 2004, “ Variational Time Integrators,” Int. J. Numer. Methods Eng., 60(1), pp. 153–212.
Lewis, F. L. , Xie, L. , and Popa, D. , 2008, Optimal and Robust Estimation, 2nd ed., CRC Press, Boca Raton, FL.
Schultz, J. , and Murphey, T. D. , 2013, “ Embedded Control Synthesis Using One-Step Methods in Discrete Mechanics,” American Controls Conference (ACC), pp. 5393–5298.
Murphey, T. D. , and Johnson, E. R. , 2011, “ Control Aesthetics in Software Architecture for Robotic Marionettes,” American Controls Conference (ACC), pp. 3825–3830.
Schultz, J. , and Murphey, T. , 2012, “ Trajectory Generation for Underactuated Control of a Suspended Mass,” IEEE International Conference on Robotics and Automation (ICRA), pp. 123–129.
Johnson, E. , and Murphey, T. D. , 2007, “ Dynamic Modeling and Motion Planning for Marionettes: Rigid Bodies Articulated by Massless Strings,” IEEE International Conference on Robotics and Automation (ICRA), pp. 330–335.
Kalman, R. E. , and Bucy, E. S. , 1961, “ New Results in Linear Filtering and Prediction Theory,” ASME J. Basic Eng., 83(1), pp. 95–108.

## Figures

Fig. 1

Approximations of the stochastic harmonic oscillator flow for a set of initial conditions. All integrators use the same step-size h = 0.03125 and an identical sample path of the Wiener process with σ=γ=1.0. The results from the Euler-Maruyama integrator are depicted in blue with circular markers, the VI midpoint in red with diamond markers, and the symplectic Euler in green with square markers. The analytic solution from Ref. [10] is shown in black with no markers for comparison. It is difficult to distinguish the VI midpoint, the symplectic Euler, and the analytic solution as they lie on top of each other (see online figure for color).

Fig. 2

Image of the particle filter covariance propagation for the harmonic oscillator without resampling. Note that the system's covariance matrix has two eigenvalues, thus there are two eigenvalues plotted for each integrator.

Fig. 3

Schematic of planar crane system including relevant geometric parameters

Fig. 4

Plot illustrating variations in EKF filter performance using two different discrete representations of the continuous dynamics. Both curves are simulated from 1000 trials, with Gaussian noise added to produce “measurements”.

Fig. 5

This set of figures illustrates the results of utilizing a particle filter (PF) and an EKF to estimate the dynamic configuration variables of the system shown in Fig. 3. For each filter, both VI and RK1 integrators are used. In the left plots, measurements, controls, and integrations happen at 30 Hz, while in the right plots they all occur at 6 Hz. To generate the data for a given frequency, an experimental system was sent a set of commands at the desired frequency; the same commands were used to step the integrators for predictions. A Microsoft Kinect® configured to provide data at the target frequency was used to measure both of the dynamic configuration variables (x, y). For the 30 Hz data, only some of the measurements/estimator updates are shown to avoid overcrowding the figure. The particle filters used 1000 particles and the “low variance sampler” algorithm from page 110 of [3] is used to resample. The ellipses shown represent the local covariance estimates for each filter. The eigenvalues and eigenvectors of the covariance are used to define the size and orientation of the ellipses. Note that the VI-based filters outperform the RK1-based filters at both frequencies, and that the VI filter performance is similar for both frequencies. Additionally, note that the VI covariance estimates are in excellent agreement between the filters at both frequencies; this is not true for the RK1 filters.

Fig. 6

This figure shows the time evolution of the eigenvalues of the covariance prediction for particle filters (PF) and EKFs applied to the planar crane problem of Fig. 3 with both a 30 Hz (left plots) and a 6 Hz (right plots) measurement, estimator and integrator update rate. These eigenvalues are the same eigenvalues used to define the major and minor axes of the uncertainty ellipses plotted in Fig. 5. Thus, there are two eigenvalues for each filter and integrator. Note that the VI-based PF and EKF estimates are in excellent agreement with each other even when compared across frequencies. The two 6 Hz RK1 filters predict very different covariance propagation, and they both disagree with the corresponding 30 Hz filters.

Fig. 7

Illustration of the VI improving particle deprivation. The red circles represent the 6 Hz VI particle filter, and the noisy, dashed blue line is the 6 Hz RK1 particle filter.

Fig. 8

Comparison of the Kalman filter variants for the harmonic oscillator. Analytic deterministic solution is given in black, standard discrete covariance updates with explicit Euler state updates in blue, VI midpoint state updates with standard discrete covariance updates in green, and symplectic Euler state update with symplectic covariance update in red. The top left figure shows the filtered states and some covariance ellipses in the phase plane. The top right figure is a zoomed-in view of the data in the top left figure. The symplectic covariance updates increase the filter performance. The bottom left figure shows the time evolution of the eigenvalues of the covariance matrices (as in Fig. 2 there are two eigenvalues per filter). The symplectic method leads to eigenvalues that are an order of magnitude smaller. The bottom right figure shows the mechanical energy of the oscillator's filtered states for each of the filter variants; the symplectic covariance update yields a better approximation that the two other filters with standard covariance updates.

## Errata

Some tools below are only available to our subscribers or users with an online account.

### Related Content

Customize your page view by dragging and repositioning the boxes below.

Related Journal Articles
Related Proceedings Articles
Related eBook Content
Topic Collections