A Survey of Code Optimization Methods for Kalman Filter Extensions <s> Background <s> This book presents a unified treatment of linear and nonlinear filtering theory for engineers, with sufficient emphasis on applications to enable the reader to use the theory. The need for this book is twofold. First, although linear estimation theory is relatively well known, it is largely scattered in the journal literature and has not been collected in a single source. Second, available literature on the continuous nonlinear theory is quite esoteric and controversial, and thus inaccessible to engineers uninitiated in measure theory and stochastic differential equations. Furthermore, it is not clear from the available literature whether the nonlinear theory can be applied to practical engineering problems. In attempting to fill the stated needs, the author has retained as much mathematical rigor as he felt was consistent with the prime objective" to explain the theory to engineers. Thus, the author has avoided measure theory in this book by using mean square convergence, on the premise that everyone knows how to average. As a result, the author only requires of the reader background in advanced calculus, theory of ordinary differential equations, and matrix analysis. <s> BIB001 </s> A Survey of Code Optimization Methods for Kalman Filter Extensions <s> Background <s> Based on an interpolation formula, accurate state estimators for nonlinear systems can be derived. The estimators do not require derivative information which makes them simple to implement. <s> BIB002 </s> A Survey of Code Optimization Methods for Kalman Filter Extensions <s> Background <s> We develop and analyze real-time and accurate filters for nonlinear filtering problems based on the Gaussian distributions. We present the systematic formulation of Gaussian filters and develop efficient and accurate numerical integration of the optimal filter. We also discuss the mixed Gaussian filters in which the conditional probability density is approximated by the sum of Gaussian distributions. A new update rule of weights for Gaussian sum filters is proposed. Our numerical tests demonstrate that new filters significantly improve the extended Kalman filter with no additional cost, and the new Gaussian sum filter has a nearly optimal performance. <s> BIB003 </s> A Survey of Code Optimization Methods for Kalman Filter Extensions <s> Background <s> This paper points out the flaws in using the extended Kalman filter (EKE) and introduces an improvement, the unscented Kalman filter (UKF), proposed by Julier and Uhlman (1997). A central and vital operation performed in the Kalman filter is the propagation of a Gaussian random variable (GRV) through the system dynamics. In the EKF the state distribution is approximated by a GRV, which is then propagated analytically through the first-order linearization of the nonlinear system. This can introduce large errors in the true posterior mean and covariance of the transformed GRV, which may lead to sub-optimal performance and sometimes divergence of the filter. The UKF addresses this problem by using a deterministic sampling approach. The state distribution is again approximated by a GRV, but is now represented using a minimal set of carefully chosen sample points. These sample points completely capture the true mean and covariance of the GRV, and when propagated through the true nonlinear system, captures the posterior mean and covariance accurately to the 3rd order (Taylor series expansion) for any nonlinearity. The EKF in contrast, only achieves first-order accuracy. Remarkably, the computational complexity of the UKF is the same order as that of the EKF. Julier and Uhlman demonstrated the substantial performance gains of the UKF in the context of state-estimation for nonlinear control. Machine learning problems were not considered. We extend the use of the UKF to a broader class of nonlinear estimation problems, including nonlinear system identification, training of neural networks, and dual estimation problems. In this paper, the algorithms are further developed and illustrated with a number of additional examples. <s> BIB004 </s> A Survey of Code Optimization Methods for Kalman Filter Extensions <s> Background <s> In this paper, a Gaussian filter for nonlinear Bayesian estimation is introduced that is based on a deterministic sample selection scheme. For an effective sample selection, a parametric density function representation of the sample points is employed, which allows approximating the cumulative distribution function of the prior Gaussian density. The computationally demanding parts of the optimization problem formulated for approximation are carried out off-line for obtaining an efficient filter, whose estimation quality can be altered by adjusting the number of used sample points. The improved performance of the proposed Gaussian filter compared to the well-known unscented Kalman filter is demonstrated by means of two examples. <s> BIB005 </s> A Survey of Code Optimization Methods for Kalman Filter Extensions <s> Background <s> In this paper, we present a new nonlinear filter for high-dimensional state estimation, which we have named the cubature Kalman filter (CKF). The heart of the CKF is a spherical-radial cubature rule, which makes it possible to numerically compute multivariate moment integrals encountered in the nonlinear Bayesian filter. Specifically, we derive a third-degree spherical-radial cubature rule that provides a set of cubature points scaling linearly with the state-vector dimension. The CKF may therefore provide a systematic solution for high-dimensional nonlinear filtering problems. The paper also includes the derivation of a square-root version of the CKF for improved numerical stability. The CKF is tested experimentally in two nonlinear state estimation problems. In the first problem, the proposed cubature rule is used to compute the second-order statistics of a nonlinearly transformed Gaussian random variable. The second problem addresses the use of the CKF for tracking a maneuvering aircraft. The results of both experiments demonstrate the improved performance of the CKF over conventional nonlinear filters. <s> BIB006 </s> A Survey of Code Optimization Methods for Kalman Filter Extensions <s> Background <s> In this paper, we present a regularization method in the nonstationary inverse problem for diffuse optical tomography (DOT). The regularization is based on a choosing time evolution process such that in a stationary state it has a covariance function which corresponds to a process with similar smoothness properties as the first-order smoothness Tikhonov regularization. The proposed method is computationally more lightweight than the method where the regularization is augmented as a measurement. The method was tested in the case of the inverse problem of DOT. A solid phantom with optical properties similar to tissue was made, incorporating two moving parts that simulate two different physiological processes: a localized change in absorption and a surrounding rotating two-part shell which simulates slow oscillations in the tissue background physiology. A sequence of measurements of the phantom was made and the reconstruction of the image sequence was computed using this method. It allows the recovery of the full time series of images from relatively slow measurements with one source active at a time. In practice, this allows instruments with a larger dynamic range to be applied to the imaging of functional phenomena using DOT. <s> BIB007 </s> A Survey of Code Optimization Methods for Kalman Filter Extensions <s> Background <s> Particle filters (PFs) are powerful sampling-based inference/learning algorithms for dynamic Bayesian networks (DBNs). They allow us to treat, in a principled way, any type of probability distribution, nonlinearity and non-stationarity. They have appeared in several fields under such names as "condensation", "sequential Monte Carlo" and "survival of the fittest". In this paper, we show how we can exploit the structure of the DBN to increase the efficiency of particle filtering, using a technique known as Rao-Blackwellisation. Essentially, this samples some of the variables, and marginalizes out the rest exactly, using the Kalman filter, HMM filter, junction tree algorithm, or any other finite dimensional optimal filter. We show that Rao-Blackwellised particle filters (RBPFs) lead to more accurate estimates than standard PFs. We demonstrate RBPFs on two problems, namely non-stationary online regression with radial basis function networks and robot localization and map building. We also discuss other potential application areas and provide references to some finite dimensional optimal filters. <s> BIB008
In discrete-time Bayesian filtering, the state of a dynamical system is estimated based on noisy measurements. The state model describes how the n-dimensional state x is propagated in time. A state model can be expressed as where f (·) is the state transition function, x t−1 is the state of the previous time step, and ε x t is the state transition noise. The state's distribution is updated using measurements of the form where y t is the realized measurement, h(·) is the measurement function, and ε y is the measurement noise. To shorten notations we use augmented state z = [ x ε ], where applicable. In the augmented state ε is either state transition or measurement noise. The state and measurement noises are assumed independent and white. We also assume that all the interesting variables are in the state. If the state or measurement noises are not white, they can be modeled using more variables in the state. Also we omit the time index t when its absence should not cause confusion. In general, a KFE can be divided into two parts: 1. Prediction: where µ − t is the prior mean computed using the state transition function and posterior of the previous time step and P − t is the prior covariance. 2. Update: where y − t is the predicted mean, z , S t is the innovation covariance, K t is the Kalman gain, µ t is the updated state, and P t is the updated covariance. The mean and covariance matrices associated to measurement and state transition functions can be written using expectations: In (7), P x − t h(z − t ) refers to the rows that correspond to the state variables of the expectations have analytic form: and the algorithm is the KF. There are extensions that approximate the expectations (10)-(12) using analytic differentiation (or integration) of the functions. For example, the Extended Kalman Filter (EKF) uses the first order Taylor expansion and the Second Order Extended Kalman Filter (EKF2) does the linearization based on the second order Taylor expansion BIB001 . There are also algorithms that use numerical differentiation to approximate (10)- BIB007 . In Divided Difference (DD) filters BIB002 the computation is based on numerical differentiation of the functions to obtain a linear model. DD filters use 2n + 1 function evaluations. The Second Order Polynomial Kalman Filter (SOKF2) BIB003 uses n 2 2 + 3 2 n + 1 points to fit a second order polynomial to the function and then computes the analytic covariance matrices for the polynomial. One very commonly used and large group of KFEs is algorithms that can be written in form: where χ i are so-called sigma-points that are chosen according to the prior distribution and w s i and w c i are associated weights. Examples of this kind of filters are Unscented Kalman Filter (UKF) BIB004 , different Cubature Kalman Filters (CKFs) BIB006 , and Gauss-Hermite Quadrature Kalman filter (QKF) BIB003 . The selection and number of sigma-points and weights depend on the algorithm used. The UKF is used usually with 2n + 1 sigma-points. The Gaussian Filter (GF) uses kn + 1 sigma-points, where k > 1 is an integer parameter BIB005 . CKFs are developed for different orders and they use O(n m ) sigma-points, where m is the order of the cubature rule. The number of sigma-points in QKFs increases exponentially, as the number of sigma-points is α n , where α > 1 BIB003 . There are also algorithms that allow an arbitrary number of points, for example Smart Sampling Kalman Filter (S 2 KF)  , which uses at least n + 1 sigma-points. Algorithms that use (17)- (18) do not compute J explicitly. In some optimizations J is required and can be computed using statistical linearization   This matrix defines the relationship between P zz and P g(z)z . Because this computation requires solving a set of linear equations it should be avoided when the dimension of z is large. The computational requirements of the algorithms can be reduced in several ways. One way is to compute the expected values numerically only for state variables that are transformed by a nonlinear function and compute the expectations for linear parts analytically. The improving of implementation efficiency using the linear KF update for the linear state variables is more familiar in particle filtering. Specifically Rao-Blackwellized Particle Filter (RBPF) solves the estimates of conditionally linear variables using a KF and rest of the variables using particles BIB008 . The dimension of the output of the nonlinear part can also be reduced to reduce the computational burden. In (18) the m × m covariance matrix is updated for every sigma point. Thus, halving the dimension m of g(·) reduces the operations applied to the covariance matrix to one fourth. This kind of update optimizations are considered in Section 4. When some state variables are unobservable i.e. they do not contribute to the output of the measurement function and are static i.e. their mean and covariance does not change in the state propagation, they do not need to be updated at every time step. Computing their estimate only when they are observed or their mean or covariance changes can also be used to reduce the computational burden. These optimizations are presented in Section 5. It is also possible to compute the Kalman gain (7) faster by exploiting the structure of the innovation covariance (6) as shown in Section 6.