Density Difference Grid Design in a Point-Mass Filter

Abstract

The paper deals with the Bayesian state estimation of nonlinear stochastic dynamic systems. The stress is laid on the point-mass filter, solving the Bayesian recursive relations for the state estimate conditional density computation using the deterministic grid-based numerical integration method. In particular, the grid design is discussed and the novel density difference grid is proposed. The proposed grid design covers such regions of the state-space where the conditional density is significantly spatially varying, by the dense grid. In other regions, a sparse grid is used to keep the computational complexity low. The proposed grid design is thoroughly discussed, analyzed, and illustrated in a numerical study.

1. Introduction

State estimation of nonlinear discrete-time stochastic dynamic systems from noisy or incomplete measurements has been a subject of considerable research interest. The topic plays an important role in signal processing, control, and fault detection of both small and large scale systems in virtually all fields of industry [1] including power generation and transmission, battery live prediction [2,3,4,5,6,7,8], automotive [9,10], navigation [11,12], speech and image processing etc. State estimation methods has also become important in non-technical fields including, for example, economics, biology, or weather forecasting.

In the Bayesian framework, a general solution to the state estimation problem is given by the Bayesian recursive relations (BRRs), which compute the probability density functions (PDFs) of the state conditioned on the measurements. The conditional PDFs fully describe the estimate of the immeasurable state of a nonlinear or non-Gaussian stochastic dynamic system. Besides a set of linear models leading e.g., to the Kalman filter (KF), the BRRs are not analytically tractable and an approximation in their solution must be used. The resulting approximate methods can be divided with respect to the validity of the resulting estimates into global and local filters [13,14].

The local filters provide computationally efficient estimates in the form of the conditional mean and covariance matrix with potentially limited performance due to inherent underlying Gaussian assumption, which is not always realistic (as the first two moments usually do not represent a full description of the immeasurable state). These filters are represented by the extended Kalman filter, the unscented Kalman filter, or the stochastic integration filter [14,15,16,17,18].

As opposed to the local filters, the global filters provide estimates in the form of conditional PDFs without any assumption on the conditional distribution family. The global filters are capable of estimating the state of a strongly nonlinear or non-Gaussian system but usually at the cost of higher computational demands. Among these, the Gaussian sum filter [19], the particle filter [20], and the point-mass filter [21,22,23,24] have attracted a considerable attention.

The point-mass filter (PMF) considered in this paper is based on a numerical solution to the BRRs using deterministic grid-based numerical integration rules, which means the BRRs are solved and the conditional PDF is computed at the grid points only. Although a conceptual design of the PMF is known from the seventies [13], an application in the terrain-aided navigation (TAN) [22] is reported as late as the nineties mainly because of the computational complexity of the filter. Therefore, since the development of the PMF, extensive attention is devoted to the efficient filter design reducing the computational complexity. In the literature, two main directions of reduction can be found;

• Design of the Rao-Blackwellized (or marginalized) PMF [23,25],
• Efficient convolution in the PMF prediction step [21,22,24,26,27,28,29].

The former approach is based on the decomposition of the state vector into two parts; nonlinearly and linearly modeled. As a consequence, just the nonlinearly modeled part of the state is estimated by the computationally expensive PMF and the remaining part of the state vector is estimated by a set of computationally cheap KFs. The Rao–Blackwellized PMF is thus suitable for possibly high-dimensional models with specific conditionally linear structure and Gaussian noises. The latter approach typically does not restrict the class of the allowed state-space models, but it rather focuses on computationally effective implementation of the convolution, as the most demanding step, in the prediction of the PMF.

Recently, a novel method for the efficient convolution, based on the the density-specific grid (DSG) design, was proposed [24]. Compared to other methods (typically assuming one equidistant grid), the DSG assumes two different grids (each with different properties);

• Denser grid supporting symmetrical vicinity of the conditional PDF mean (where a higher volume of the conditional PDF is expected),
• Sparser grid supporting conditional PDF tail region (where a marginal volume of the conditional PDF is expected).

The DSG brings significant improvement to the estimation quality especially for unimodal and rather symmetric conditional PDFs with light tails. If the density is multimodal, heavy-tailed, or heavy-skewed, the benefit of the DSG is marginal.

The goal of the paper is, thus, to design an advanced version of the DSG, further denoted as the density difference grid (DDG). The proposed DDG is based on the differentiation of the conditional PDF at the sparse grid, which allows accurate determination of the regions with a significant spatial change of the PDF. In these regions, the denser grid is created to reasonably well capture the PDF shape. As a consequence, the DDG has an ability to better approximate asymmetric, heavy-tailed, and multi-modal PDFs, i.e., the PDFs with dominant higher-order moments, with a negligible increase of the overall PMF computational complexity.

The rest of the paper is organized as follows. In Section 2, the system description and a brief introduction to the Bayesian state estimation is presented. Then, in Section 3, the points-mass filter is overviewed. This overview is accompanied with a short recapitulation of the state-of-the-art grid designs. Section 4 proposes a novel DDG design and Section 5 shows the DDG performance in a numerical study. Finally, concluding remarks are drawn in Section 6.

2. System Description and Bayesian State Estimation

Let the following discrete-time state-space model of a nonlinear stochastic dynamic system

$${\mathbf{x}}_{k+1}={\mathbf{f}}_k({\mathbf{x}}_k,{\mathbf{u}}_k)+{\mathbf{w}}_k,k=0,1,2,\dots ,T,$$

(1)

$${\mathbf{z}}_k={\mathbf{h}}_k\left({\mathbf{x}}_k\right)+{\mathbf{v}}_k,k=0,1,2,\dots ,T,$$

(2)

be considered, where the vectors ${\mathbf{x}}_k\in {\mathbb{R}}^{n_x}$, ${\mathbf{u}}_k\in {\mathbb{R}}^{n_u}$, and ${\mathbf{z}}_k\in {\mathbb{R}}^{n_z}$ represent the unknown state of the system and the known input and measurement at time instant k, respectively. The state and measurement functions ${\mathbf{f}}_k:{\mathbb{R}}^{n_x\times n_u}\to {\mathbb{R}}^{n_x}$ and ${\mathbf{h}}_k:{\mathbb{R}}^{n_x}\to {\mathbb{R}}^{n_z}$ are known general transformations. Particular realizations of the state and measurement noises ${\mathbf{w}}_k$ and ${\mathbf{v}}_k$ are unknown, but their PDFs, i.e., the state noise PDF $p\left({\mathbf{w}}_k\right)$ and the measurement noise PDF $p\left({\mathbf{v}}_k\right)$, are supposed to be known and independent of the known initial state PDF $p\left({\mathbf{x}}_0\right)$.

The goal of the state estimation, particularly of the filtering, is to find an estimate of the state ${\mathbf{x}}_k$ conditioned on all measurements ${\mathbf{z}}^k=[{\mathbf{z}}_0,{\mathbf{z}}_1,\dots ,{\mathbf{z}}_k]$ up to the time instant k. In the Bayesian framework, the state estimate in the form of the conditional PDF $p\left({\mathbf{x}}_k\right|{\mathbf{z}}^k),\forall k$, is sought. Note that, considering the model (1), (2), the state transition PDF and the BRRs (3)–(5) should be conditioned also on available sequence of the input ${\mathbf{u}}_k,\forall k$. However, for the sake of notational simplicity, the input signal is assumed to be implicitly part of the condition and it is not explicitly stated, i.e., $p\left({\mathbf{x}}_{k+1}\right|{\mathbf{x}}_k)=p\left({\mathbf{x}}_{k+1}\right|{\mathbf{x}}_k;{\mathbf{u}}_k)$, $p\left({\mathbf{x}}_k\right|{\mathbf{z}}^k)=p\left({\mathbf{x}}_k\right|{\mathbf{z}}^k;{\mathbf{u}}^{k-1})$, and  $p\left({\mathbf{x}}_{k+1}\right|{\mathbf{z}}^k)=p\left({\mathbf{x}}_{k+1}\right|{\mathbf{z}}^k;{\mathbf{u}}^k)$.

The Bayesian state estimator design stems from the solution of the Bayesian recursive relations (BRRs) for the recursive computation of the conditional PDFs [15]

$$p\left({\mathbf{x}}_k\right|{\mathbf{z}}^k)=\frac{p({\mathbf{x}}_k,{\mathbf{z}}_k|{\mathbf{z}}^{k-1})}{p\left({\mathbf{z}}_k\right|{\mathbf{z}}^{k-1})}=\frac{p\left({\mathbf{x}}_k\right|{\mathbf{z}}^{k-1})p\left({\mathbf{z}}_k\right|{\mathbf{x}}_k)}{p\left({\mathbf{z}}_k\right|{\mathbf{z}}^{k-1})},$$

(3)

$$p\left({\mathbf{x}}_k\right|{\mathbf{z}}^{k-1})=\int p\left({\mathbf{x}}_k\right|{\mathbf{x}}_{k-1})p\left({\mathbf{x}}_{k-1}\right|{\mathbf{z}}^{k-1})d{\mathbf{x}}_{k-1},$$

(4)

where $p\left({\mathbf{x}}_k\right|{\mathbf{z}}^k)$ is the filtering PDF computed by the Bayes’ rule (3) and $p\left({\mathbf{x}}_k\right|{\mathbf{z}}^{k-1})$ is the one-step predictive PDF computed by the Chapman–Kolmogorov Equation (4). The PDFs $p\left({\mathbf{x}}_k\right|{\mathbf{x}}_{k-1})$ and $p\left({\mathbf{z}}_k\right|{\mathbf{x}}_k)$ are the state transition PDF obtained from system dynamics (1) and the measurement PDF obtained from measurement Equation (2), respectively. The PDF

$$p\left({\mathbf{z}}_k\right|{\mathbf{z}}^{k-1})=\int p\left({\mathbf{x}}_k\right|{\mathbf{z}}^{k-1})p\left({\mathbf{z}}_k\right|{\mathbf{x}}_k)d{\mathbf{x}}_k$$

(5)

is the one-step predictive PDF of the measurement. The recursion (3), (4) starts from the initial PDF, i.e., $p\left({\mathbf{x}}_0\right|{\mathbf{z}}^{-1})=p\left({\mathbf{x}}_0\right)$.

3. Point-Mass Filter Summary and Grid Designs

The PMF is based on the numerical solution to BRRs using the deterministic integration rules. As a consequence, a significant part of the (continuous) state-space is approximated by the grid of isolated points in which the conditional PDFs are computed. The PDF evaluated at the grid points is denoted as the point-mass PDF or, alternatively, as the point-mass density (PMD).

3.1. Point-Mass PDF Approximation

Approximation of a conditional PDF $p\left({\mathbf{x}}_k\right|{\mathbf{z}}^m)$, where $m=k$ for the filtering PDF (3) and $m=k-1$ for the predictive PDF (4), by a piece-wise constant PMD $\widehat{p}\left({\mathbf{x}}_k\right|{\mathbf{z}}^m;{\xi }_k)$ defined at the set of the discrete grid points ${\xi }_k={\left\{{\xi }_k^{\left(i\right)}\right\}}_{i=1}^N,{\xi }_k^{\left(i\right)}\in {\mathbb{R}}^{n_x}$, is as follows

$$p\left({\mathbf{x}}_k\right|{\mathbf{z}}^m)\approx \widehat{p}\left({\mathbf{x}}_k\right|{\mathbf{z}}^m;{\xi }_k)\triangleq {\displaystyle \sum _{i=1}^N}{P}_{k|m}\left({\xi }_k^{\left(i\right)}\right)S\{{\mathbf{x}}_k;{\xi }_k^{\left(i\right)},{\Delta }_k\},$$

(6)

with

• $P_{k|m}\left({\xi }_k^{\left(i\right)}\right)=c_k{\tilde{P}}_{k|m}\left({\xi }_k^{\left(i\right)}\right)$, where ${\tilde{P}}_{k|m}\left({\xi }_k^{\left(i\right)}\right)=p\left({\xi }_k^{\left(i\right)}\right|{\mathbf{z}}^m)$ is the value of the conditional PDF $p\left({\mathbf{x}}_k\right|{\mathbf{z}}^m)$ evaluated at i-th grid point ${\xi }_k^{\left(i\right)}$, $c_k={\delta }_k{\sum }_{i=1}^N{\tilde{P}}_{k|m}\left({\xi }_k^{\left(i\right)}\right)$ is a normalization constant, and ${\delta }_k$ is a volume of the i-th point neighbourhood defined below,
• ${\Delta }_k={[{\Delta }_k\left(1\right),{\Delta }_k\left(2\right),\dots ,{\Delta }_k\left(n_x\right)]}^{\mathrm{T}}$ defines a (hyper-)rectangular neighbourhood of a grid point ${\xi }_k^{\left(i\right)}$, where the PDF $p\left({\mathbf{x}}_k\right|{\mathbf{z}}^m)$ is assumed to be constant and has value $P_{k|m}\left({\xi }_k^{\left(i\right)}\right)$, and
• $S\{{\mathbf{x}}_k;{\xi }_k^{\left(i\right)},{\Delta }_k\}$ is the selection function defined as

  $$S\{{\mathbf{x}}_k;{\xi }_k^{\left(i\right)},{\Delta }_k\}=1,$$

  (7)

  if ${\mathbf{x}}_k\left(j\right)\in [{\xi }_k^{\left(i\right)}\left(j\right)-\frac{{\Delta }_k\left(j\right)}{2},{\xi }_k^{\left(i\right)}\left(j\right)+\frac{{\Delta }_k\left(j\right)}{2}]$ for $j=1,2,\dots ,n_x$, and

  $$S\{{\mathbf{x}}_k;{\xi }_k^{\left(i\right)},{\Delta }_k\}=0,$$

  (8)

  otherwise, so that

  $$\begin{array}{cc}\hfill \int S\{{\mathbf{x}}_k;{\xi }_k^{\left(i\right)}\}d{\mathbf{x}}_k& ={\Delta }_k\left(1\right)\times {\Delta }_k\left(2\right)\times \dots \times {\Delta }_k\left(n_x\right)\hfill \\ \hfill & ={\delta }_k.\hfill \end{array}$$

  (9)

Note that in (6)–(9) the notation $\mathbf{x}\left(j\right)$ meaning the j-th element of the vector $\mathbf{x}$ was used. Illustration of the point-mass PDF approximation (6) with omitted time indices is shown in Figure 1. The point-mass density (6) can be, thus, interpreted as a sum of weighted uniform distributions. Another interpretations, such as the a sum of the Dirac delta functions, can be found in [22,26].

3.2. Point-Mass Filter Algorithm

The basic algorithm of the point-mass filter (PMF) can be summarised by the following Algorithm 1 [26,30]:

Algorithm 1: Point-Mass Filter

Initialization: Set $k=0$, construct the initial grid of points ${\left\{{\xi }_0^{\left(i\right)}\right\}}_{i=0}^N$, and define the initial point-mass PDF $$\widehat{p}\left({\mathbf{x}}_0\right|{\mathbf{z}}^{-1};{\xi }_0)={\displaystyle \sum _{i=1}^N}{P}_{0|-1}\left({\xi }_0^{\left(i\right)}\right)S\{{\mathbf{x}}_0;{\xi }_0^{\left(i\right)},{\Delta }_0\},$$ (10) approximating the initial PDF $p\left({\mathbf{x}}_0\right|{\mathbf{z}}^{-1})$; Measurement update: Compute the filtering point-mass PDF $$\widehat{p}\left({\mathbf{x}}_k\right|{\mathbf{z}}^k;{\xi }_k)={\displaystyle \sum _{i=1}^N}{P}_{k|k}\left({\xi }_k^{\left(i\right)}\right)S\{{\mathbf{x}}_k;{\xi }_k^{\left(i\right)},{\Delta }_k\},$$ (11) where $$P_{k|k}\left({\xi }_k^{\left(i\right)}\right)=\frac{p\left({\mathbf{z}}_k\right|{\mathbf{x}}_k={\xi }_k^{\left(i\right)})P_{k|k-1}\left({\xi }_k^{\left(i\right)}\right)}{{\sum }_{i=1}^{N}p\left({\mathbf{z}}_k\right|{\mathbf{x}}_k={\xi }_k^{\left(i\right)})P_{k|k-1}\left({\xi }_k^{\left(i\right)}\right){\delta }_k}.$$ (12) is the value of the filtering PDF at i-th grid point; Grid construction: Based on the filtering estimate (11) and the state Equation (1), construct the new grid of points ${\left\{{\xi }_{k+1}^{\left(j\right)}\right\}}_{j=1}^N$; Time update: Compute the predictive point-mass PDF at the new grid of points according to $$\widehat{p}\left({\mathbf{x}}_{k+1}\right|{\mathbf{z}}^k;{\xi }_{k+1})={\displaystyle \sum _{j=1}^N}{P}_{k+1|k}\left({\xi }_{k+1}^{\left(j\right)}\right)S\{{\mathbf{x}}_{k+1};{\xi }_{k+1}^{\left(j\right)},{\Delta }_{k+1}\},$$ (13) where the value of the predictive PDF at j-th grid point is $$P_{k+1|k}\left({\xi }_{k+1}^{\left(j\right)}\right)={\displaystyle \sum _{i=1}^N}p\left({\xi }_{k+1}^{\left(j\right)}\right|{\mathbf{x}}_k={\xi }_k^{\left(i\right)})P_{k|k}\left({\xi }_k^{\left(i\right)}\right){\delta }_k.$$ (14) Set $k=k+1$ and go to the step 2

For the sake of clarity, a detailed derivation of the PMF can be found in Appendix A. Note that the PMF filter provides estimates in the form of the PMD $\widehat{p}\left({\mathbf{x}}_k\right|{\mathbf{z}}^m;{\xi }_k)$ approximating the conditional PDF $p\left({\mathbf{x}}_k\right|{\mathbf{z}}^m)$. Assuming small grid point vicinity ${\Delta }_k$, the conditional moments, e.g., the mean and covariance matrix, can be easily computed according to [23]

$${\widehat{\mathbf{x}}}_{\mathrm{PMD},k|m}={\displaystyle \sum _{i=1}^N}{\delta }_k{P}_{k|m}\left({\xi }_k^{\left(i\right)}\right){\xi }_k^{\left(i\right)}\approx {\widehat{\mathbf{x}}}_{k|m}=\mathrm{E}\left[{\mathbf{x}}_k\right|{\mathbf{z}}^m],$$

(15)

$${\mathbf{P}}_{\mathrm{PMD},k|m}={\displaystyle \sum _{i=1}^N}{\delta }_k{P}_{k|m}\left({\xi }_k^{\left(i\right)}\right)({\xi }_k^{\left(i\right)}-{\widehat{\mathbf{x}}}_{k|m}){({\xi }_k^{\left(i\right)}-{\widehat{\mathbf{x}}}_{k|m})}^T\approx {\mathbf{P}}_{k|m}=\mathrm{cov}\left[{\mathbf{x}}_k\right|{\mathbf{z}}^m],$$

(16)

when required. The moments are, however, generally not needed for the PMF run.

3.3. Grid Design

A crucial part of the PMF design that is significantly affecting the filter estimation performance is a specification of the grid points before time update step (13) and, by extension, in the initialization (10).

When designing a grid, the number of grid points should be decided first. The more grid points N are considered, the better the accuracy of the density approximation can be achieved but at the cost of higher computational demands. Because of the convolution (13), (14), the computational complexity grows exponentially with N. Thus, the number of grid points is solely driven by the available computational capacity and required precision of the estimate (an analysis of ideal grid resolution can be found in [31]).

The grid should be designed to cover a significant region of the conditional PDFs support sufficiently well. Therefore, it is necessary to specify two grid characteristics

• Region of the PDF support to be covered by the grid points,
• Locations of grid points inside the region.

The former characteristic specification is common for all single-grid design techniques. The region $\mathcal{R}$ for the predictive PDF $p\left({\mathbf{x}}_{k+1}\right|{\mathbf{z}}^k)$ is (hyper-)rectangular and centred around the mean ${\widehat{\mathbf{x}}}_{k+1|k}=\mathrm{E}\left[{\mathbf{x}}_{k+1}\right|{\mathbf{z}}^k]$ and is selected so that

$${\int }_{\mathcal{R}}p\left({\mathbf{x}}_{k+1}\right|{\mathbf{z}}^k)>P_{\mathrm{thr}},$$

(17)

where $P_{\mathrm{thr}}$ is a user-defined threshold. Unfortunately, except for the initial condition, neither the predictive moments nor the predictive PDF is known at “Grid construction” step of Algorithm 1 (the PDF is to be computed in the subsequent step “Time update”). A reasonable approach is to compute the first two (approximate) predictive moments of the state

$${\widehat{\mathbf{x}}}_{\mathrm{A},k+1|k}=\mathrm{E}\left[{\mathbf{x}}_{k+1}\right|{\mathbf{z}}^k],$$

(18)

$${\mathbf{P}}_{\mathrm{A},k+1|k}=\mathrm{cov}\left[{\mathbf{x}}_{k+1}\right|{\mathbf{z}}^k],$$

(19)

and to construct an approximate Gaussian predictive PDF

$$p_{\mathrm{A}}\left({\mathbf{x}}_{k+1}\right|{\mathbf{z}}^k)=\mathcal{N}\{{\mathbf{x}}_{k+1};{\widehat{\mathbf{x}}}_{\mathrm{A},k+1|k},{\mathbf{P}}_{\mathrm{A},k+1|k}\},$$

(20)

where the notation $\mathcal{N}\{\mathbf{x};\widehat{\mathbf{x}},\mathbf{P}\}$ stands for the Gaussian PDF of the random variable $\mathbf{x}$ with the mean $\widehat{\mathbf{x}}$ and the covariance matrix $\mathbf{P}$ (for simplicity, the random variable has same notation as its realization). Then, the sought region $\mathcal{R}$ can be found analogously to (17), where the approximate predictive PDF $p_{\mathrm{A}}\left({\mathbf{x}}_{k+1}\right|{\mathbf{z}}^k)$ (20) is used instead of the exact one $p\left({\mathbf{x}}_{k+1}\right|{\mathbf{z}}^k)$. The predictive moments (18) and (19) are computed on the basis of known filtering moments (given by (15), (16), and (11)) and state equation (1). If the dynamics is nonlinear, approximate predictive moments can be computed e.g., by the unscented transformation or various numerical integration rules [16,32]. Note that, the approximate PDF (20) is used for the region $\mathcal{R}$ determination only.

The latter characteristic is specific for each grid design technique.

3.3.1. Equidistant Grid Design

The equidistant grid design [22] is a standard grid design technique, which covers the region $\mathcal{R}$ with equidistantly located points, i.e., the neighbourhood ${\Delta }_k$ is the same for all grid points.

3.3.2. Density Specific Grid Design

The DSG design [24], is based on an unequally spaced placement of the fixed number of points over the PDF support $\mathcal{R}$ to be approximated. The main idea is to approximate the PDF, where the volume of the PDF is significant or significantly varying by a denser grid (as in this area, a slope of the PDF is expected to be significant) and the tail of the PDF by a sparser grid (as the tail is flat). Thus, the DSG design determines, besides the region $\mathcal{R}$, also the hyper-rectangular subregion ${\mathcal{R}}_{\mathrm{in}}$, where the grid is denser. The determination of the subregion again depends on the computed approximate predictive moments (18), (19).

As can be seen in [24], the DSG design significantly outperforms the standard equidistant grid design if the underlying conditional PDF is unimodal, rather symmetric, and light-tailed. If any of the mentioned properties are violated, the DSG benefit is marginal.

3.4. Goal of the Paper

The goal of the paper is to propose a novel unequally spaced grid design, called the density difference grid design (further abbreviated as DDG), which provides improved PMF estimation performance independently of the conditional PDF properties.

4. Density Difference Grid Design

The DDG design is based on the idea that such part of the support, where the conditional PDF is significantly spatially varying, should be covered by the dense grid (to sufficiently well capture PDF behavior). On the other hand, the remaining part of the support, i.e., the parts where the conditional PDF is almost flat, can be reasonably well approximated by the sparse grid. To realize this basic idea of the DDG, the following three steps should be performed:

• Design of the sparse (outer) grid and evaluation of the predictive PMD at these grid points,
• Differentiation of the “sparse” predictive PMD,
• Design of the dense (inner) grids in the vicinity of such “sparse” grid points, which are associated with a large value of the difference.

Compared to the DSG design discussed in [24], the DDG design utilizes full information about the shape of the predictive PMD (and not only the moments) and also the denser (inner) grid need not be hyper-rectangular, which further improves the fit of the PDF and its grid support. The steps are particularized below.

4.1. Sparse Grid Design

The sparse grid design takes advantage of the PMF time-update, that the PDF value of a single point ${\xi }_{k+1}^{\left(j\right)}$ can be counted independently of any other grid points. The sparse grid design is given by Algorithm 2:

Algorithm 2: DDG Sparse Grid

Define a threshold $p_{\mathrm{thr}}$ below which the probability at a grid point is considered to be negligible and can be neglected. The threshold setting can be related to the computation precision of a given platform; State Similarly as in the DSG design [24], compute the predictive moments ${\widehat{\mathbf{x}}}_{\mathrm{A},k+1|k}$ (18) and ${\mathbf{P}}_{\mathrm{A},k+1|k}$ (19) and set the number of standard deviations (STDs) $n_{\sigma }$ of the marginal distributions of (20), which are used for specification of an a priori (hyper-)rectangular region ${\mathcal{R}}_{\mathrm{A}}$ to be covered by the grid points; At the boundaries of the region ${\mathcal{R}}_{\mathrm{A}}$ select a small set of grid points and evaluate the predictive probability at these points. If the probability of the point is (significantly) below the threshold $p_{\mathrm{thr}}$, then the particular bound can be shifted towards the mean. On the other hand, if the probability of the point is (significantly) over the threshold $p_{\mathrm{thr}}$, then the particular bound can be shifted in an opposite direction from the mean. The shifted points are, then, re-evaluated; When all boundary points are properly set, create the smallest possible hyper-rectangle defining $\mathcal{R}$ including all the boundary points. As a consequence, the resulting region $\mathcal{R}$ need not be symmetrical with respect to the mean; Define a cardinality of the sparse grid $N_{\mathrm{sparse}}$ and dense grid $N_{\mathrm{dense}}=N-N_{\mathrm{sparse}}$. Fill the region $\mathcal{R}$ by equidistantly placed $N_{\mathrm{sparse}}$ hyper-rectangular grid points ${\left\{{\xi }_{\mathrm{sparse},k+1}^{\left(j\right)}\right\}}_{j=1}^{N_{\mathrm{sparse}}}$ (each sparse grid point is associated with a neighbourhood ${\Delta }_{\mathrm{sparse},k+1}$). A reasonable choice $N_{\mathrm{sparse}}=\frac{1}{3}N$;

This algorithm is illustrated in Figure 2 for the two dimensional state-space.

4.2. Differentiation of Sparse Grid PMD

To find the state-space regions, where the conditional predictive PDF $p\left({\mathbf{x}}_{k+1}\right|{\mathbf{z}}^k)$ is expected to significantly spatially vary (i.e., the regions to be covered by the dense grid), the PMD evaluated at the sparse grid points is differenced according to Algorithm 3:

Algorithm 3: Sparse Grid PMD Differentiation

Compute the PMD for $N_{\mathrm{sparse}}$ sparse grid points according to the convolution (14); For each sparse grid point ${\xi }_{\mathrm{sparse},k+1}^{\left(j\right)}$ compute the divided difference vector ${\mathbf{d}}_{k+1}\in {\mathbb{R}}^{2^{n_x}}$ [14,33] according to $${\mathbf{d}}_{k+1}^{\left(j\right)}=[\frac{P_{k+1|k}\left({\xi }_{\mathrm{sparse},k+1}^{\left(j\right)}\right)-P_{k+1|k}\left({\xi }_{\mathrm{sparse},k+1}^{\left({ı}_1\right)}\right)}{{\parallel {\xi }_{\mathrm{sparse},k+1}^{\left(j\right)}-{\xi }_{\mathrm{sparse},k+1}^{\left({ı}_1\right)}\parallel }_2},\dots ,\frac{P_{k+1|k}\left({\xi }_{\mathrm{sparse},k+1}^{\left(j\right)}\right)-P_{k+1|k}\left({\xi }_{\mathrm{sparse},k+1}^{\left({ı}_{2^{n_x}}\right)}\right)}{{\parallel {\xi }_{\mathrm{sparse},k+1}^{\left(j\right)}-{\xi }_{\mathrm{sparse},k+1}^{\left({ı}_{2^{n_x}}\right)}\parallel }_2}],$$ (21) where ${\parallel ·\parallel }_2$ denotes the Euclidean norm and the index vector $ı=[{ı}_1,{ı}_2,\dots ,{ı}_{2^{n_x}}]$ is selected so that the closest neighbouring points of the j-th point ${\xi }_{\mathrm{sparse},k+1}^{\left(j\right)}$ are chosen. It means two neighbouring points are selected for $n_x=1$, four neighbouring points for $n_x=2$, eight neighbouring points for $n_x=3$, etc.; Compute the grid points rating as a norm of the vector ${\mathbf{d}}_{k+1}\in {\mathbb{R}}^{2^{n_x}}$ (21) according to $$d_{k+1}^{\left(j\right)}={\parallel {\mathbf{d}}_{k+1}^{\left(j\right)}\parallel }_2,\forall j,$$ (22) which characterizes the overall spatial variability of the PDF in the vicinity of the j-th sparse grid point ${\xi }_{\mathrm{sparse},k+1}^{\left(j\right)}$.

This algorithm is illustrated in Figure 3 for the two dimensional state-space.

4.3. Design of Dense Grid

Having defined sparse grid points (Algorithm 2) and computed their difference rating (Algorithm 3), which in fact precisely denotes the state-space regions with significant change or variability of the conditional PDF, it is possible to construct the dense grid according to Algorithm 4:

Algorithm 4: Dense Grid Design

Sort the sparse grid points ${\left\{{\xi }_{\mathrm{sparse},k+1}^{\left(j\right)}\right\}}_{j=1}^{N_{\mathrm{sparse}}}$ in a descending order according to their rating $d_{k+1}^{\left(j\right)}$ (22); Set the splitting ratio r (similarly as in the DSG design in [24]), which determines number of dense grid points $r^{n_x}$ used for covering the vicinity of the j-th sparse grid ${\xi }_{\mathrm{sparse},k+1}^{\left(j\right)}$. Reasonable choice is $r=\{2,3\}$, but note that, the ratio need not be constant and it can depend on the rating $d_{k+1}^{\left(j\right)}$ (22); the higher rating, the higher ratio; Recursively split the vicinity of the ordered grid points until the number of the dense grid points $N_{\mathrm{dense}}$ is reached;

This algorithm is illustrated in Figure 4 for the two dimensional state-space.

4.4. Summary of DDG Design and Properties

The DDG design is given by the three steps realized by Algorithms 2–4. An example of the final DDG for the PMD shown in Figure 3 can be found in Figure 5.

Compared to the recently proposed DSG, the DDG dense (inner) grid need not be rectangular and, thus, it more realistically covers the support of the conditional PDF with the significant volume without any requirement on computation and utilization of a set of higher-order moments. The DDG design also reduces the user interaction and minimizes the number of the user-defined parameters (compared to the standard grid design, the DDG design requires specification of three additional parameters only; the number of sparse grid points $N_{\mathrm{sparse}}$ and the threshold $p_{\mathrm{thr}}$ in Algorithm 2 and splitting ration r in Algorithm 4). On the other, the DDG design is slightly more computationally intensive as illustrated in the following section.

5. Numerical Illustration

The influence of the grid design is illustrated in two examples; static point-mass approximation and dynamic state estimation using a PMF with three different grid designs:

• Standard equidistant (EQ) grid design ([22], Section 3.3.1),
• DSG design ([24], Section 3.3.2),
• Proposed DDG design (Section 4).

5.1. PDF Point-Mass Approximation Quality

In the first example, let a true two-dimensional, i.e., $n_x=2$, Gaussian mixture PDF with four components

$$p\left(\mathbf{x}\right)={\displaystyle \sum _{g=1}^4}{\omega }_g\mathcal{N}\{\mathbf{x};{\widehat{\mathbf{x}}}_g,{\mathbf{P}}_g\}$$

(23)

be considered, where the particular weights, means, and covariance matrices are given as follows

$$\left[\begin{array}{cccc}{\omega }_1& {\omega }_2& {\omega }_3& {\omega }_4\end{array}\right]=\left[\begin{array}{cccc}0.1& 0.3& 0.4& 0.2\end{array}\right],$$

(24)

$$\left[\begin{array}{cccc}{\widehat{\mathbf{x}}}_1& {\widehat{\mathbf{x}}}_2& {\widehat{\mathbf{x}}}_3& {\widehat{\mathbf{x}}}_4\end{array}\right]=\left[\begin{array}{cccc}1& 2& 4& 5\\ 4& 1& 3.5& -3.5\end{array}\right],$$

(25)

$${\mathbf{P}}_1={\mathbf{P}}_2={\mathbf{P}}_3={\mathbf{P}}_4=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right].$$

(26)

The PDF is illustrated in Figure 6. The PDF (23) is approximated by the PMD $\widehat{p}(\mathbf{x};\xi )$ (6) with the number of grid points $N=\{50,200,500\}$ for all three considered grid designs with $\mathcal{R}$ covering three STDs. The DSG and DDG designs used such user-defined parameters, which provided the best performance, namely the PMD with the DSG used ${\mathcal{R}}_{in}$ covering two STDs and the PMD with the DDG used parameters $r=2$ and $N_{\mathrm{sparse}}=0.4N$.

The quality of the PMD is evaluated using the criteria

• Integral sum error: ISE = $1-\int \widehat{p}(\mathbf{x};\xi )d\mathbf{x}$,
• Mean error: ME = ${\sum }_{i=1}^{n_{\mathbf{x}}}|\mathbf{x}\left(i\right)-{\widehat{\mathbf{x}}}_{\mathrm{PMD}}\left(i\right)|$.

The results can be found in

Table 1. Approximation errors of the point-mass density (PMD) with three different grid designs.

	N	50	200	500
EQ	ISE	0.0468	0.0062	$5.5585\times {10}^{-8}$
	ME	1.5848	$0.0606$	$2.4479\times {10}^{-5}$
DSG	ISE	0.0733	$6.3488\times {10}^{-5}$	$3.8642\times {10}^{-5}$
	ME	0.7567	$6.1405\times {10}^{-5}$	$3.1102\times {10}^{-4}$
DDG	ISE	$1.1102\times {10}^{-15}$	$2.2204\times {10}^{-16}$	$2.2204\times {10}^{-16}$
	ME	0.2439	$2.5314\times {10}^{-5}$	$6.7043\times {10}^{-6}$

. It can be seen that the proposed DDG provides the best approximation quality among the considered grids with the same number of points. A degraded performance of the DSG-based design for the approximation of multimodal distributions can also be noted.

For the sake of completeness, a layout of the tested grids is shown in Figure 7.

5.2. PMF in Terrain-Aided Navigation with Spherical Coordinates

The second example considers the terrain-aided navigation (TAN) scenario. The TAN system is a navigation system suitable for GNSS-denied environments, where the vehicle horizontal position is determined by the correlation of the measured vertical terrain profile and the pre-recorded terrain map [23]. The models used in the TAN are highly nonlinear, thus, they are often used as benchmark examples for illustration of the state estimation techniques performance. In this section, the considered the TAN scenario is described by the state-space model [34]

$${\mathbf{x}}_{k+1}=\left[\begin{array}{c}{\varphi }_{k+1}\\ {\lambda }_{k+1}\end{array}\right]=\left[\begin{array}{c}{\varphi }_k+(\phi \left({\lambda }_{k+1}\right)-\phi \left({\lambda }_k\right))\tan \left(K\right)\\ {\lambda }_k+\frac{\Delta T{\parallel \mathbf{V}\parallel }_2}{R\left({\lambda }_k\right)}\cos \left(K\right)\end{array}\right]+{\mathbf{w}}_k,$$

(27)

$$z_k=h_k\left({\mathbf{x}}_k\right)+v_k,$$

(28)

where $k=0,1,2,\dots ,10$, ${\varphi }_k$ is the sought vehicle latitude in degrees, ${\lambda }_k$ is the longitude in degrees, $\phi \left({\lambda }_k\right)=\log \left(\frac{1+\sin \left({\lambda }_k\right)}{\cos \left({\lambda }_k\right)}\right)$, $\Delta T=1$ is the sampling period in minutes, $K=0.4363$ is the constant heading relative to the north in radians (measurement provided e.g., by the compass), $\mathbf{V}={[V_{\mathrm{north}},V_{\mathrm{east}}]}^T$ is the known constant velocity vector (measurement provided e.g., by an inertial navigation system) in meters/sec, and

$$R\left({\lambda }_k\right)=\sqrt{\frac{{\left(r_1^2\cos \left({\lambda }_k\right)\right)}^2+{\left(r_2^2\sin \left({\lambda }_k\right)\right)}^2}{{\left(r_1\cos \left({\lambda }_k\right)\right)}^2+{\left(r_2\sin \left({\lambda }_k\right)\right)}^2}}$$

(29)

is Earth radius at latitude ${\lambda }_k$. Constants $r_1=6,378,137$ and $r_2=6,356,800$ are radii of the Earth at the equator and pole, respectively, in meters. The measurement function $h_k(·)$ is the the map in the Geographic coordinate system. For the simulation, the maps provided by the Shuttle Radar Topography Mission (SRTM) were used (https://www2.jpl.nasa.gov/srtm/). The noises ${\mathbf{w}}_k$ and $v_k$ are described as

$$p\left({\mathbf{w}}_k\right)=\mathcal{N}\{{\mathbf{w}}_k;[\begin{array}{c}0\\ 0\end{array}],[\begin{array}{cc}1.8·{10}^{-5}& 0\\ 0& 1.8·{10}^{-5}\end{array}\left]\right\},$$

(30)

$$p\left(v_k\right)=\mathcal{N}\{v_k;0,3\}.$$

(31)

The PMF was configured with three grid designs with $N=100$, all with $\mathcal{R}$ covering $\pm 4$ STDs in each axis (i.e., in latitude and longitude). Namely, the following implementations were compared

• PMF with equidistant (EQ) grid design,
• PMF with DSG grid design, where the inner denser grid covers ${\mathcal{R}}_{in}$ with $\pm 1$ STD in each axis,
• PMF with the proposed DDG design with the user-defined parameters $p_{\mathrm{thr}}={10}^{-4}$, $N_{\mathrm{sparse}}=50$ and $r=3$. Thus, six sparse points with the highest rating $d_{k+1}^{\left(j\right)}$ (22) were split into $r^2\times 6=54$ dense grid points (each sparse point were split into nine dense points). In total, this implementation used $N=44+54=98$ grid points.

Performance of the filters were compared using the following criteria

• Root-mean-square deviation [$deg$]: RMSE = ${\sum }_{i=1}^{n_{\mathbf{x}}}{({\mathbf{x}}_k\left(i\right)-{\widehat{\mathbf{x}}}_{\mathrm{PMD},k|k}\left(i\right))}^2$,
• Average standard deviation [$deg$]: ASTD = $\frac{1}{n_{\mathbf{x}}}\left(\sqrt{\mathrm{diag}\left({\mathbf{P}}_{\mathrm{PMD},k|k}\right)}\right)$,
• time [$sec$],

in $M=100$ Monte–Carlo simulations, where the notation $\left(\mathbf{A}\right)$ is the trace of the matrix $\mathbf{A}$ and $\mathrm{diag}\left(\mathbf{A}\right)$ stands for the diagonalization of the matrix $\mathbf{A}$, i.e., it zeroes all non-diagonal elements. The results averaged over the time are given in

Table 2. Estimation results.

	EQ	DSG	DDG
RMSE $\left[deg\right]$	0.015675	0.015521	0.013853
ASTD $\left[deg\right]$	0.012741	0.012466	0.012999
Time $\left[sec\right]$	0.26061	0.26131	0.26656

. It can be seen that the DDG method is better than the equidistant and DSG grid methods, while keeping similar computational complexity. The PMF estimate with the DDG is approximately about 11% more accurate. Note that transformation of the angular longitude and latitude error into the Cartesian coordinate system results in the improvement of the several meters (only by better selection of the grid points and with almost the same computational complexity).

6. Concluding Remarks

The paper focused on the point-mass filter for the nonlinear state estimation with the stress on the grid design as a key filter design step. In particular, a new density difference grid design was proposed. The proposed design was based on evaluation of the predictive PDF in a set of the spare grid points. Subsequently, such point-mass density was differentiated and the state-space regions with significantly varying density were located. These regions are, finally, covered by the dense grid and the detailed point-mass density was completed. The density difference grid properties were analyzed, discussed, and illustrated. It was shown that the point-mass density and the point-mass filter with the proposed grid provides consistently better performance independently of the conditional probability density function shape (e.g., multimodal, heavy-tailed) than with other grid design strategies, while the computational complexity is preserved. The performance was illustrated in two examples, including terrain-aided navigation inspired scenario.