A Rao-Blackwellized Particle Filter for Superelliptical Extended Target Tracking

Abstract

In this work, we propose a new method to track extended targets of different shapes such as ellipses, rectangles and rhombi. We provide an analytical framework to express these shapes as superelliptical contours and propose a Bayesian filtering scheme that can handle measurements from the contour of the object. The method utilizes the Rao-Blackwellized particle filtering algorithm with novel sensor-object geometry constraints. The success of the algorithm is demonstrated using both simulations and real-data experiments, and the algorithm has been demonstrated to be of high performance in various challenging scenarios.

I Introduction

With the ever-growing number of applications involving closer target-to-sensor proximity and high-resolution sensors, targets can occupy multiple sensor resolution cells, generating multiple measurements. This motivates the extended target tracking (ETT) problem where both the target extent (shape) and the kinematic states are to be estimated [1] from these multiple measurements. ETT algorithms can be used in crowd surveillance, maritime vessel tracking, and tracking both vehicles and pedestrians [1, 2]. The target extent can be modeled as elliptical [3, 4, 5], rectangular [6] or star-convex [7, 8].

The random matrix models (RMM) [9, 3, 10, 11] are commonly used in ETT algorithms. The underlying assumption is that measurements are distributed across the body of the target. However, this assumption limits their direct use in scenarios with contour measurements [12], such as those from Lidar sensors. For these cases, algorithms such as the Gaussian process model (GP) [7, 13] and random hypersurface model (RHS) [14, 8] can be used to model complex contours as well as complex surfaces. One approach in [15] is to use multiple ellipses to represent the target extent when the measurements are generated from the target contour.

Another approach for ETT with Lidar measurements is to use geometric object descriptions where the sensor-object geometry can be integrated naturally. In [16], a Gaussian mixture (GM) model probability hypothesis density filter is used to model elliptical and rectangular contours as GMs, where the sensor-object geometry is used to solve the measurement association problem. The work by [17] uses a particle filter approach for ETT of a known shape where the expected measurements are obtained using ray casting. The virtual measurement model (VMM) introduced in [18] tracks an elliptical contour using virtual measurements generated via ray tracing. However, the VMM’s virtual measurement generation is computationally expensive, so a later work [19] trained a Gaussian process regression model to predict the VMM’s adaptation law, eliminating the need for virtual measurement generation during runtime.

In this paper, we propose using contours known as superellipses or Lamé curves [20] to represent the target extent, which are a family of curves containing rhombi, ellipses and more rectangular shapes. We choose the Rao-Blackwellized particle filter (RBPF) [21] as our inference method to explore the potential of using superellipses in extended target tracking. Furthermore, we incorporate the sensor-object geometry into our inference framework analytically, overcoming the challenges of tracking with partially observed contour measurements frequently encountered in Lidar sensor applications.

II Problem Formulation

II-A Superellipses, Extent State and Measurement Model

(a) $q=1$

(b) $q=1.5$

(c) $q=2$

(d) $q=5$

Figure 1: Solution sets to (4) for different values of $q$. In all cases, the extent parameters are selected as $d^{(1)}=5$, $d^{(2)}=3$, $\varphi=\pi/4$ and $c^{(1)}=c^{(2)}=6$. As $q$ tends to infinity, the contour becomes a rectangle.

An ellipse in $\mathbb{R}^{2}$ can be expressed as the set of points $\mathbf{y}=[y^{(1)}\ y^{(2)}]^{T}$ that satisfy the equation

$$(\mathbf{y}-\mathbf{c})^{T}\mathbf{W}^{-1}(\mathbf{y}-\mathbf{c})=1,$$ (1)

where $\mathbf{c}=[c^{(1)}\ c^{(2)}]^{T}\in\mathbb{R}^{2}$ is called the centroid of the ellipse and $\mathbf{W}$ is a symmetric positive-definite matrix. Due to its positive-definiteness, $\mathbf{W}$ can be expressed as $\mathbf{W}=\mathbf{R}_{\varphi}\mathbf{D}^{2}\mathbf{R}_{\varphi}^{T}$, where $\mathbf{D}=\text{diag}\left([d^{(1)}\ d^{(2)}]\right)$ is a diagonal matrix of positive half-lengths along major and minor axes, respectively, and $\mathbf{R}_{\varphi}$ is the rotation matrix by $\varphi$ radians in the counterclockwise direction. With these parameters, (1) can be rewritten as

$$\left(\mathbf{D}^{-1}\mathbf{R}_{\varphi}^{T}(\mathbf{y}-\mathbf{c})\right)^{T}\left(\mathbf{D}^{-1}\mathbf{R}_{\varphi}^{T}(\mathbf{y}-\mathbf{c})\right)=1,$$ (2)

which implies

$$\|\mathbf{D}^{-1}\mathbf{R}_{\varphi}^{T}(\mathbf{y}-\mathbf{c})\|_{2}^{2}=1,$$ (3)

where $\|\cdot\|_{2}$ denotes the $\ell_{2}$ or the Euclidean norm. One generalization of (3) can be obtained by not restricting it to the $\ell_{2}$ norm, which yields

$$\|\mathbf{D}^{-1}\mathbf{R}_{\varphi}^{T}(\mathbf{y}-\mathbf{c})\|_{q}^{q}=1,$$ (4)

where the $\ell_{q}$ norm of a vector $\mathbf{y}\in\mathbb{R}^{2}$ is defined as

$$\left\|\mathbf{y}\right\|_{q}=\left\|\begin{bmatrix}y^{(1)}\\ y^{(2)}\end{bmatrix}\right\|_{q}=\left(|y^{(1)}|^{q}+|y^{(2)}|^{q}\right)^{\nicefrac{{1}}{{q}}}.$$ (5)

for $q\geq 1$. Note that (4) has a solution for all $q>0$, although they do not define a norm for $1>q>0$. Figure 1 shows the solution sets to (4) for different values of $q$. These mathematical objects are called Lamé ovals or superellipses when $q>2$ [20], and have found use in various domains of science, engineering and architecture [22, 23, 24, 25].

Consider the variables $\tilde{\mathbf{y}}=\left[\tilde{y}^{(1)}\ \tilde{y}^{(2)}\right]^{T}$ and $\bm{\lambda}=\left[\lambda^{(1)}\ \lambda^{(2)}\right]^{T}$, which are defined as

$$\tilde{\mathbf{y}}:=\mathbf{R}_{\varphi}^{T}(\mathbf{y}-\mathbf{c})$$ (6)

and

$$\lambda^{(j)}=|d^{(j)}|^{-q}\text{ for $j=1,2$},$$ (7)

respectively. With these definitions, we can write (4) as

$$\bm{\lambda}^{T}|\tilde{\mathbf{y}}|^{q}=\lambda^{(1)}|\tilde{y}^{(1)}|^{q}+\lambda^{(2)}|\tilde{y}^{(2)}|^{q}=1,$$ (8)

where the absolute value and the exponentiation on $\tilde{\mathbf{y}}$ are interpreted element-wise. Note that $\lambda^{(j)}$’s are strictly positive due to (7).

We can use equation (8) to define a non-linear and implicit equation to describe measurements taken from the contour of a superelliptical object. When a measurement $\mathbf{y}\in\mathbb{R}^{2}$ lies exactly on the object contour, we have

$$0=h(\mathbf{x}^{e},\mathbf{y})=\bm{\lambda}^{T}\left|\mathbf{R}_{\varphi}^{T}(\mathbf{y}-\mathbf{c})\right|^{q}-1$$ (9)

satisfied, where

$$\mathbf{x}^{e}:=\begin{bmatrix}\varphi&c^{(1)}&c^{(2)}&\lambda^{(1)}&\lambda^{(2)}\end{bmatrix}^{T}.$$ (10)

Changing the value of $q$ changes the contour shape, as illustrated in Figure 1.

II-B Analytical Sensor-Object Geometry Relations

One problem with extent estimation with contour measurements occurs when the sensor only takes measurements from one side of the target, possibly causing the estimated extent along the unobserved side to deteriorate. In this section, we derive some analytical conditions for determining which axes of the target are visible. These conditions can be incorporated into the filtering framework to update only the observed extension state estimate.

Figure 2: Eight regions in which the sensor could be located with respect to the target. The circle denotes the centroid, the black line denotes the heading axis of the target with $q=3$.

(a) ${[0\ 3\ \nicefrac{{\pi}}{{6}}\ 2\sqrt{3}\ 2]}^{T}$

(b) ${[2\ (3\text{-}2\sqrt{3})\ \nicefrac{{\pi}}{{6}}\ \sqrt{3}\ 6]}^{T}$

(c) ${[\text{-}3\ (3\text{-}\sqrt{3})\ \nicefrac{{\pi}}{{6}}\ 2\sqrt{3}\ 2]}^{T}$

(d) ${[\text{-}2.5\ 49\ 0\ 10\ 45]}^{T}$

Figure 3: Various superellipses that can fit the point cloud in green, originally generated from the one in 3a and corrupted with $\mathcal{N}\!\left(\mathbf{0}_{2\times 1},0.01\mathbf{I}_{2}\right)$ noise. The extent parameters $\mathbf{x}^{e}={[c^{(1)}\ c^{(2)}\ \varphi\ d^{(1)}\ d^{(2)}]}^{T}$ are given in the captions, with $q=6$ is fixed for all superellipses.

In relation to the target, the sensor can be located in eight different regions as described by tangent lines to the target contour when $q\geq 2$, located on its four sides. The boundaries of these eight regions are described by the equations

$$\left|\mathbf{e}_{1}^{T}\mathbf{R}_{\varphi}^{T}(\mathbf{y}-\mathbf{c})\right| =d^{(1)}\ \text{ and }\$$ (11a)

$$\left|\mathbf{e}_{2}^{T}\mathbf{R}_{\varphi}^{T}(\mathbf{y}-\mathbf{c})\right| =d^{(2)},$$ (11b)

where $\mathbf{e}_{j}\in\mathbb{R}^{2}$ is the unit vector along the $j$-th dimension. Each equation in (11) result in two pairs of lines parallel to each other and perpendicular to the other two, as shown in Figure 2 with dotted lines. When the sensor is in regions R2, R4, R6 and R8, both axes of the target are visible, and hence extensions along both axes can be inferred successfully. However, only the extension $d^{(1)}$ along the major axis can be inferred when the sensor is in regions R1 and R5, and that along the minor axis (i.e. $d^{(2)}$) can be inferred when it is in regions R3 and R7. If the sensor location is denoted with $\mathbf{s}=[s^{(1)}\ s^{(2)}]^{T}$, then its location relative to the target can be computed according to $\tilde{\mathbf{s}}:=\mathbf{R}_{\varphi}^{T}(\mathbf{s}-\mathbf{c})$. Then, the condition the sensor satisfies in a region where the extension along the dimension $j$ is visible becomes

$$\left|\mathbf{e}_{2}^{T}\mathbf{R}_{\varphi}^{T}(\mathbf{s}-\mathbf{c})\right| >d^{(2)}\qquad\text{for $j=1$, and}$$ (12a)

$$\left|\mathbf{e}_{1}^{T}\mathbf{R}_{\varphi}^{T}(\mathbf{s}-\mathbf{c})\right| >d^{(1)}\qquad\text{for $j=2$}.$$ (12b)

These boundaries can be extended by a margin to account for the limited angular sensor resolution as

$$\left|\mathbf{e}_{2}^{T}\mathbf{R}_{\varphi}^{T}(\mathbf{s}-\mathbf{c})\right| >d^{(2)}+\varepsilon^{(2)}\qquad\text{for $j=1$, and}$$ (13a)

$$\left|\mathbf{e}_{1}^{T}\mathbf{R}_{\varphi}^{T}(\mathbf{s}-\mathbf{c})\right| >d^{(1)}+\varepsilon^{(1)}\qquad\text{for $j=2$},$$ (13b)

which push the boundaries in (11) outwards from the target by $\varepsilon^{(j)}>0$ along the relevant dimensions. For future use, we define the binary variables

$$b^{(j)} :=\mathds{1}\left(\left|\mathbf{e}_{j^{\prime}}^{T}\mathbf{R}_{\varphi}^{T}(\mathbf{s}-\mathbf{c})\right|>d^{(j^{\prime})}+\varepsilon^{(j^{\prime})}\right)$$ (14)

to describe the visibility of the object extension along the $j$-th axis, where $\mathds{1}(\cdot)$ is the indicator function obtaining the value 1 if the condition in the parentheses is met and 0 otherwise, and $j^{\prime}=2$ when $j=1$ and vice versa.

Finally, we note that these conditions are valid for sufficiently rectangular targets, namely those that can be modeled with a high $q$ value in the superellipse equation (4). In the case of lower $q$, especially when $0<q\leq 1$, the regions as well as observable properties of the extent contour change.

II-C Implicit Measurement with Scale Constraints

Using the implicit equation (9) to find a suitable superellipse fitting a set of measurements $\mathcal{Y}=\{\mathbf{y}^{m}\}_{m=1}^{M}$ requires minimizing the total deviation

$$\sum_{m=1}^{M}\left(h(\mathbf{x}^{e},\mathbf{y}^{m})\right)^{2}=\sum_{m=1}^{M}\left(\bm{\lambda}^{T}\left|\mathbf{R}_{\varphi}^{T}(\mathbf{y}^{m}-\mathbf{c})\right|^{q}-1\right)^{2}.$$ (15)

Four alternative superellipses that could minimize (15) are shown in Figure 3, where measurements are taken from two sides of the contour in Figure 3a. Aside from the original superellipse, the other three are also viable options in the sense of (15), as they also have the measurements approximately along their contours in relation to their extent parameters. Notice that if we had measurements from all four sides of the vehicle, the bad fits in Figures 3b and 3c would not occur, while the one in Figure 3d can still occur if the algorithm is initialized poorly.

To prevent the algorithm from converging towards estimates such as in Figures 3b and 3c, we suggest enforcing additional scale constraints based on the transformed measurements $\tilde{\mathbf{y}}^{m}:=\mathbf{R}_{\varphi}^{T}(\mathbf{y}^{m}-\mathbf{c})$. Assuming no occlusions and sufficiently high sensor resolution, the contour measurements are generated along the full length of each visible side. This means that the transformed measurements $\tilde{\mathbf{y}}^{m}$ will lie exactly in the interval $[-d^{(j)},d^{(j)}]$ in their $j$-th dimension. The limits of the interval are attained regardless of the value of $q$. That is, for each dimension $j=1,2$ we have the constraints

$$\min_{m}\left\{\mathbf{e}_{j}^{T}\mathbf{R}_{\varphi}^{T}(\mathbf{y}^{m}-\mathbf{c})\right\} =-d^{(j)}\ \text{ and }\$$ (16a)

$$\max_{m}\left\{\mathbf{e}_{j}^{T}\mathbf{R}_{\varphi}^{T}(\mathbf{y}^{m}-\mathbf{c})\right\} =d^{(j)}.$$ (16b)

Lastly, it is important to note that each scaling constraint should only be applied if the relevant extension is visible by the sensor according to (13).

III ETT with Superelliptical Extents

In this section, we introduce our state-space model and our inference method based on the Rao-Blackwellized particle filter (RBPF). In the first subsection, we derive the filtering equations for a superelliptical extent model with a known $q$ value and extend the formulation to the unknown case in the following subsection.

Let the state-space model with discrete time index $k$ have the unknown state vector

$$\mathbf{x}_{k}:=\begin{bmatrix}\varphi_{k}&c_{k}^{(1)}&c_{k}^{(2)}&\dot{c}_{k}^{(1)}&\dot{c}_{k}^{(2)}&\lambda_{k}^{(1)}&\lambda_{k}^{(2)}\end{bmatrix}^{T},$$ (17)

where $\dot{c}^{(1)}_{k}$ and $\dot{c}^{(2)}_{k}$ are the time derivatives of the centroid. The state dynamics and the measurement equation of the state-space model are defined as

$$\mathbf{x}_{k+1} =\mathbf{F}\mathbf{x}_{k}+\mathbf{G}\mathbf{w}_{k},$$ (18a)

$$0 =h(\mathbf{x}_{k},\mathbf{y}_{k}^{m})+e_{k}^{m}$$ (18b)

with measurements $$\mathcal{Y}_{k}=\{\mathbf{y}^{m}_{k}\}_{m=1}^{M_{k}}$$, where

$$h(\mathbf{x}_{k},\mathbf{y}_{k}^{m})=\bm{\lambda}^{T}_{k}\left|\mathbf{R}_{\varphi_{k}}^{T}(\mathbf{y}^{m}_{k}-\mathbf{c}_{k})\right|^{q}-1.$$ (18c)

$\mathbf{F}$ is the state transition matrix, $\mathbf{w}_{k}\sim\mathcal{N}(\mathbf{0},\mathbf{Q})$ is the process noise and $e_{k}^{m}\sim\mathcal{N}(0,r_{k}^{h})$ is the additive pseudomeasurement noise accounting for the mismatch between the predicted contour and the obtained measurements, describing how much discrepancy from the superellipse equation as given in (9) is tolerated.

Consider the filtering problem where we are interested in computing the posterior density $p(\mathbf{x}_{k}|\mathcal{Y}_{1:k})$. Following the RBPF framework, we partition our state variable into the linear state $\mathbf{x}_{k}^{l}$ and nonlinear state $\mathbf{x}_{k}^{n}$, and decompose their joint posterior as

$$p(\mathbf{x}_{k}^{l},\mathbf{x}_{0:k}^{n}|\mathcal{Y}_{1:k})=p(\mathbf{x}_{k}^{l}|\mathbf{x}_{0:k}^{n},\mathcal{Y}_{1:k})p(\mathbf{x}_{0:k}^{n}|\mathcal{Y}_{1:k}).$$ (19a)

The second factor, i.e. the posterior density of nonlinear states, is approximated by an empirical density

$$p(\mathbf{x}_{0:k}^{n}|\mathcal{Y}_{1:k})\approx\sum_{i=1}^{N}w_{k,(i)}\delta(\mathbf{x}_{0:k}^{n}-\mathbf{x}_{0:k,(i)}^{n}),$$ (19b)

with sample trajectories $$\mathbf{x}_{0:k,(i)}^{n} w_{k,(i)}$$ and weights . This approximation allows the conditional density of the linear state given the nonlinear state to be expressed analytically. Under Gaussianity assumptions, the conditional posterior density of the linear state becomes

$$p(\mathbf{x}_{k}^{l}|\mathbf{x}_{0:k,(i)},\mathcal{Y}_{1:k})=\mathcal{N}\!\left(\mathbf{x}_{k}^{l}\,;\,\bm{\mu}_{k|k,(i)},\,\bm{\Sigma}_{k|k,(i)}\right).$$ (19c)

where the parameters $\bm{\mu}_{k|k,(i)}$ and $\bm{\Sigma}_{k|k,(i)}$ are the conditional posterior mean and covariance, respectively, which can be computed using a Kalman filter [26] run conditionally on the nonlinear state.

III-A Superelliptical Extent Model with Known $q$

Let $\mathbf{x}_{k}^{n}:=[\varphi_{k}\ c_{k}^{(1)}\ c_{k}^{(2)}\ \dot{c}_{k}^{(1)}\ \dot{c}_{k}^{(2)}]^{T}$ denote our nonlinear state, while $q$ is assumed to be known. Conditioned on $\mathbf{x}_{k}^{n}$, the measurement equation in (18b) is linear in the remaining states $\mathbf{x}_{k}^{l}:=\bm{\lambda}_{k}$. Therefore, we partition [27] the state dynamics equation (18a) as

$$\mathbf{x}_{k+1}^{n} =\mathbf{F}^{n}\mathbf{x}_{k}^{n}+\mathbf{G}^{n}\mathbf{w}_{k}^{n},$$ (20a)

$$\mathbf{x}_{k+1}^{l} =\mathbf{x}_{k}^{l}+\mathbf{w}_{k}^{l},$$ (20b)

with

$$\mathbf{F}^{n} =\text{blkdiag}\left(1,\begin{bmatrix}1&T\\ 0&1\end{bmatrix}\otimes\mathbf{I}_{2}\right)$$ (21a)

$$\mathbf{G}^{n} =\text{blkdiag}\left(1,\begin{bmatrix}\nicefrac{{T^{2}}}{{2}}\\ T\end{bmatrix}\otimes\mathbf{I}_{2}\right)$$ (21b)

$$\mathbf{w}_{k}^{n} \sim\mathcal{N}\!\left(\mathbf{0}_{3\times 1},\mathbf{Q}^{n}\right)\text{ with } \mathbf{Q}^{n}=\text{blkdiag}\left(\sigma_{\varphi}^{2},\sigma_{a}^{2}\mathbf{I}_{2}\right)$$ (21c)

$$\mathbf{w}_{k}^{l} \sim\mathcal{N}\!\left(\mathbf{0}_{2\times 1},\mathbf{Q}^{l}\right)\text{ with } \mathbf{Q}^{l}=\sigma^{2}_{\lambda}\mathbf{I}_{2}$$ (21d)

where $\mathbf{w}_{k}^{n}$ is the vector of angle and acceleration noises of the constant velocity model, $\mathbf{w}_{k}^{l}$ is the extension process noise, $\mathbf{I}_{n}$ denotes the $n\times n$ identity matrix and $\otimes$ denotes the Kronecker product.

In the RBPF framework, the linear state measurement update (KF-MU) and time update (KF-TU) steps are performed analytically conditioned on particles representing the nonlinear states. For the nonlinear states, the weight update (PF-WU) and sampling (PF-S) steps are performed. We introduce the details of these steps in the following. A pseudocode for this filter is given in Algorithm 1.

PF-WU

The weight update of the particles can be expressed as a multiplication of the previous weights with corresponding incremental weights, denoted as $\alpha_{k,(i)}$. Using (18b), the incremental pseudomeasurement update weights are

$$\alpha^{h}_{k,(i)}=\prod_{m=1}^{M_{k}}\mathcal{N}\!\left(\bm{\mu}^{T}_{k|k-1,(i)}\left|\mathbf{R}_{\varphi_{k,(i)}}^{T}(\mathbf{y}^{m}_{k}-\mathbf{c}_{k,(i)})\right|^{q}\,;\,1,\,r_{k}^{h}\right),$$ (22)

where $r_{k}^{h}$ is the pseudomeasurement covariance parameter.

If the extension along the $j$-th axis is deemed to be visible according to (13), its corresponding scale constraints (16) can be applied as a soft constraint using Gaussian factors. For each dimension $j=1,2$ the corresponding scale constraint in (16) is expressed as

$$\alpha^{s,(j)}_{k,(i)} =\mathcal{N}\!\left(\min_{m}\left\{\mathbf{e}_{j}^{T}\mathbf{R}_{\varphi_{k,(i)}}^{T}(\mathbf{y}^{m}_{k}-\mathbf{c}_{k,(i)})\right\}\,;\,-\hat{d}^{(j)}_{k,(i)},\,r_{k}^{s}\right)$$ (23a)

$$\times\mathcal{N}\!\left(\max_{m}\left\{\mathbf{e}_{j}^{T}\mathbf{R}_{\varphi_{k,(i)}}^{T}(\mathbf{y}^{m}_{k}-\mathbf{c}_{k,(i)})\right\}\,;\,\hat{d}^{(j)}_{k,(i)},\,r_{k}^{s}\right),$$ (23b)

where $$r_{k}^{s} \hat{d}^{(j)}_{k,(i)} \mu_{k|k-1,(i)}^{(j)}=\hat{\lambda}_{k|k-1,(i)}^{(j)} d^{(j)} \lambda^{(j)} b^{(j)}_{k,(i)}$$ is the variance parameter for the scaling constraint, and is calculated using from the relation between and according to (7). The corresponding binary variables for the sensor-object geometry criteria along each dimension are calculated according to (14). The combined scale constraint for both axes including the sensor-object geometry criteria can be expressed as

$$\alpha^{s}_{k,(i)}=\left(\alpha^{s,(1)}_{k,(i)}\right)^{b^{(1)}_{k,(i)}}\left(\alpha^{s,(2)}_{k,(i)}\right)^{b^{(2)}_{k,(i)}},$$ (23c)

which results in the weight update

$$\tilde{w}_{k,(i)}=w_{k-1,(i)}\times\alpha^{h}_{k,(i)}\times\alpha^{s}_{k,(i)}\times\frac{p\left(\mathbf{x}^{n}_{k,(i)}|\mathbf{x}^{n}_{0:k-1,(i)},\mathcal{Y}_{1:k-1}\right)}{q\left(\mathbf{x}^{n}_{k,(i)}|\mathbf{x}^{n}_{0:k-1,(i)},\mathcal{Y}_{1:k}\right)}.$$ (24)

where $q\left(\mathbf{x}^{n}_{k,(i)}|\mathbf{x}^{n}_{0:k-1,(i)},\mathcal{Y}_{1:k}\right)$ is the proposal density from which the particles are sampled. Finally, to ensure the positivity of the extent variables, the weights of the particles with negative $\lambda_{k,(i)}^{(j)}$ are set to zero.

KF-MU

Conditional on the nonlinear states, the measurement equation in (18b) can be compactly written as

$$\mathbf{1}_{M_{k}\times 1}=\mathbf{H}_{k}(\mathbf{x}_{k,(i)}^{n},\mathcal{Y}_{k})\mathbf{x}_{k}^{l}+\mathbf{e}_{k},$$ (25a)

where $$\mathbf{1}_{M_{k}\times 1} M_{k}\times 1 \mathbf{e}_{k}\sim\mathcal{N}(\mathbf{0}_{M_{k}\times 1},r_{k}^{h}\mathbf{I}_{M_{k}})$$ denotes the matrix of ones, and

$$\mathbf{H}_{k}(\mathbf{x}_{k,(i)}^{n},\mathcal{Y}_{k})=\begin{bmatrix}\left(\left|\mathbf{R}_{\varphi_{k,(i)}}^{T}(\mathbf{y}^{1}_{k}-\mathbf{c}_{k,(i)})\right|^{q}\right)^{T}\\ \vdots\\ \left(\left|\mathbf{R}_{\varphi_{k,(i)}}^{T}(\mathbf{y}^{M_{k}}_{k}-\mathbf{c}_{k,(i)})\right|^{q}\right)^{T}\end{bmatrix},$$ (25b)

where $m=1,...,M_{k}$. Then, omitting the dependence of $\mathbf{H}_{k}(x_{k,(i)}^{n},\mathcal{Y}_{k})$ on $\mathbf{x}_{k,(i)}^{n}$ and $\mathcal{Y}_{k}$ for notational brevity, the Kalman filter measurement update equations can be written as

$$\bm{\mu}_{k|k,(i)} =\bm{\mu}_{k|k-1,(i)}+\mathbf{K}_{k,(i)}(\mathbf{1}_{M_{k}\times 1}-\mathbf{H}_{k}\bm{\mu}_{k|k-1,(i)}),$$ (26a)

$$\bm{\Sigma}_{k|k,(i)} =\bm{\Sigma}_{k|k-1,(i)}-\mathbf{K}_{k,(i)}\mathbf{S}_{k,(i)}\mathbf{K}_{k,(i)}^{T},$$ (26b)

where

$$\mathbf{S}_{k,(i)} =\mathbf{H}_{k}\bm{\Sigma}_{k|k-1,(i)}\mathbf{H}_{k}^{T}+r_{k}^{h}\mathbf{I}_{M_{k}},$$ (26c)

$$\mathbf{D}_{k,(i)} =\text{diag}\left(b^{(1)}_{k,(i)},b^{(2)}_{k,(i)}\right),$$ (26d)

$$\mathbf{K}_{k,(i)} =\mathbf{D}_{k,(i)}\bm{\Sigma}_{k|k-1,(i)}\mathbf{H}_{k}^{T}\mathbf{S}_{k,(i)}^{-1}.$$ (26e)

Notice that (26e) would be the standard Kalman gain, except for the $\mathbf{D}_{k,(i)}$ term with two diagonal entries given in (14). This matrix, which we refer to as the Kalman gain mask, multiplies the rows of the original Kalman gain matrix by zero if the corresponding extent is not visible. This allows the mean and variance of that extent variable to remain unchanged after the update equations in (26a) and (26b). While such an update results in a drift from a fully Bayesian approach, it enables analytical incorporation of the sensor-object geometry constraints into our filtering framework easily.

PF-S

To perform the particle filter time update step, we draw new particles from the proposal density

$$\mathbf{x}_{k+1,(i)}^{n}\sim q\left(\mathbf{x}^{n}_{k+1,(i)}|\mathbf{x}^{n}_{0:k,(i)},\mathcal{Y}_{1:k}\right).$$ (27)

If the proposal density is selected as the transition density $p\left(\mathbf{x}^{n}_{k+1,(i)}|\mathbf{x}^{n}_{0:k,(i)},\mathcal{Y}_{1:k}\right)$, the samples will be drawn from the following Gaussian proposal

$$q(\cdot)=\mathcal{N}\!\left(\mathbf{x}_{k+1}^{n}\,;\,\mathbf{F}^{n}\mathbf{x}_{k,(i)}^{n},\,\mathbf{G}^{n}\mathbf{Q}^{n}(\mathbf{G}^{n})^{T}\right).$$ (28)

KF-TU

Finally, the Kalman filter time update is performed in accordance with the dynamics in (20b), as

$$\bm{\mu}_{k+1|k,(i)} =\bm{\mu}_{k|k,(i)},$$ (29a)

$$\bm{\Sigma}_{k+1|k,(i)} =\bm{\Sigma}_{k|k,(i)}+\mathbf{Q}^{l}.$$ (29b)

1:Initialize particles by sampling $\mathbf{x}_{1,(i)}^{n}$ from the prior with identical weights $w_{1,(i)}=1/N$, conditional means $\bm{\mu}_{1|0,(i)}$ and covariances $\bm{\Sigma}_{1|0,(i)}$ for all $i=1,...,N$.

2:for $k=1:K$ do

3:     for $i=1:N$ do $\triangleright$ PF-WU

4:         Compute $\tilde{w}_{k,(i)}$ using (22) - (24)

5:     end for

6:     Normalize weights $w_{k,(i)}=\tilde{w}_{k,(i)}/\sum_{i^{\prime}=1}^{N}\tilde{w}_{k,(i^{\prime})}$

7:     Resample particles with replacement

8:     for $i=1:N$ do $\triangleright$ KF-MU, PF-S & KF-TU

9:         Compute $\bm{\mu}_{k|k,(i)}$ and $\bm{\Sigma}_{k|k,(i)}$ using (25) - (26)

10:         Propagate $\mathbf{x}_{k,(i)}^{n}$ to $\mathbf{x}_{k+1,(i)}^{n}$ using (27)

11:         Compute $\bm{\mu}_{k+1|k,(i)}$ and $\bm{\Sigma}_{k+1|k,(i)}$ using (29)

12:     end for

13:end for

Algorithm 1 A Rao-Blackwellized Particle Filter for ETT with Superelliptical Extents

III-B Superelliptical Extent Model with Unknown $q$

The algorithm can be easily modified to estimate the unknown shape of an object by defining the superellipse exponent $q$ as an unknown variable. This allows the algorithm to adapt to the target shape with a suitable superellipse. In this case, the nonlinear state vector can be augmented with $q_{k}$, i.e. $\mathbf{x}_{k}^{n}:=[q_{k}\ \varphi_{k}\ c_{k}^{(1)}\ c_{k}^{(2)}\ \dot{c}_{k}^{(1)}\ \dot{c}_{k}^{(2)}]^{T}$, while the linear state $\mathbf{x}_{k}^{l}:=\bm{\lambda}_{k}$ is kept the same. Then, the nonlinear state dynamics parameters in (21) are modified to be

$$\mathbf{F}^{n} =\text{blkdiag}\left(\mathbf{I}_{2},\begin{bmatrix}1&T\\ 0&1\end{bmatrix}\otimes\mathbf{I}_{2}\right),$$ (30a)

$$\mathbf{G}^{n} =\text{blkdiag}\left(\mathbf{I}_{2},\begin{bmatrix}\nicefrac{{T^{2}}}{{2}}\\ T\end{bmatrix}\otimes\mathbf{I}_{2}\right),$$ (30b)

$$\mathbf{w}_{k}^{n} \sim\mathcal{N}\!\left(\mathbf{0}_{4\times 1},\mathbf{Q}^{n}\right)\text{ with }\mathbf{Q}^{n}=\text{blkdiag}\left(\sigma_{q}^{2},\sigma_{\varphi}^{2},\sigma_{a}^{2}\mathbf{I}_{2}\right),$$ (30c)

with the $q_{k}$ variable assumed to be slowly varying together with the strict positivity constraint.

IV Experimental Results

The performance of the algorithm is evaluated in four different simulation scenarios and one scenario with real data. In the scenarios where $q$ is known, the nonlinear states $\mathbf{x}_{1,(i)}^{n}$ are sampled from the Gaussian $\mathcal{N}\!\left(\bm{\mu}^{n}_{0},\bm{\Sigma}^{n}_{0}\right)$ with

$$\bm{\mu}^{n}_{0} =\begin{bmatrix}0&\bar{m}^{(1)}&\bar{m}^{(2)}&\bar{v}^{(1)}&\bar{v}^{(2)}\end{bmatrix}^{T},$$ (31a)

$$\bm{\Sigma}^{n}_{0} =\text{blkdiag}\left(\left(\nicefrac{{\pi}}{{4}}\right)^{2},\mathbf{I}_{2},4\mathbf{I}_{2}\right),$$ (31b)

where $$\bar{m}^{(j)} \bar{v}^{(j)} q q=5$$ are the center coordinates of the bounding box containing the first set of measurements and is the velocity estimate obtained via the means of the first two sets of measurements. We follow this two-step initialization approach for the velocity component to improve the particle efficiency. In the known shape scenarios, the superellipse exponent is selected as . Each particle’s conditional mean and variance are set to

$$\bm{\mu}_{1|0,(i)} =\begin{bmatrix}2^{-q}&1^{-q}\end{bmatrix}^{T}\ \text{ and }\ \bm{\Sigma}_{1|0,(i)}=100\mathbf{I}_{2}$$ (31c)

for $i=1,...,N$ where $N=1000$ is the number of particles. Notice that this selection of $\bm{\mu}_{1|0,(i)}$ initializes the extent half-lengths as $2\ m$ and $1\ m$.

Throughout all simulations, the process noise standard deviations are selected as $\sigma_{\varphi}=\nicefrac{{\pi}}{{36}}$, $\sigma_{a}=2$ and $\sigma_{\lambda}=10^{-4}$, while the proposal density is chosen as in (28). The extension observation conditions in (13) are used with $\varepsilon^{(1)}=\varepsilon^{(2)}=0.5$ $m$. The implicit measurement covariance in (22) and scale constraint covariance in (23) are selected as $r_{k}^{h}=r_{k}^{s}=(0.3)^{2}$.

IV-A Simulation Results

We test the performance of our algorithm in the following four simulated scenarios.

Linear,

where the target follows a linear trajectory along its main axis without drifting, starting from (-40, -10) and ending at (35, -10) with a linear velocity of $3$ $\nicefrac{{m}}{{s}}$,

Curved,

where the target follows a similar trajectory starting from (-40, -10), except for a slight turn towards the positive direction (Figure 4a),

Drifting,

where the trajectory is similar to that of the Curved scenario, except the target also starts drifting while turning (Figure 4b), and

U-turn,

where the target follows a semicircle with a radius of $10$ $m$ centered around the sensor between brief periods of linear motion, with a linear velocity of $2$ $\nicefrac{{m}}{{s}}$ (Figure 4c).

(a) Curved scenario, plotted every 25 frames.

(b) Drifting scenario, plotted every 25 frames.

(c) U-turn scenario, plotted every 35 frames.

Figure 4: Sample outputs of the algorithm with known $q=5$. The solid black lines indicate the posterior (contour, centroid and velocity vector) while the blue ones indicate the true extent (contour, centroid and velocity vector). The triangle indicates the sensor location.

Roughly ordered in an increasingly challenging order, each of these scenarios serves to test the performance of the algorithm in different respects. The first two are relatively simple scenarios to demonstrate elementary performance. The latter two illustrate performance in cases where the second extension of the target is not visible for a significant amount of time, testing the effectiveness of gain masking in (26d).

Throughout the simulations, the sampling time is chosen as $T=0.1\ s$. The underlying true shape is a superellipse with $q=5$ and half-lengths $d^{(1)}=2.5\ m$ and $d^{(2)}=1.5\ m$. The measurement noise is simulated by adding zero-mean Gaussian noises to the range and bearing of the noise-free Lidar measurements, with the standard deviations set to $\sigma_{r}=0.01m$ and $\sigma_{\theta}=0.005^{\circ}$, respectively. The Lidar resolution is chosen to be $0.2^{\circ}$ with a $360^{\circ}$ field of view and it is assumed to be positioned at the origin.

Table I: Performance metrics and computation times in different simulation scenarios with known shape. All results are averages over 100 Monte Carlo runs.

Scenario		Linear	Curved	Drifting	U-turn
RMSE	$\mathbf{c}\ (m)$	0.3302	0.3694	0.3249	0.3728
	$\mathbf{v}\ (\nicefrac{{m}}{{s}})$	0.2744	0.3217	0.2978	0.3145
	$d^{(1)}\ (m)$	0.2139	0.2247	0.2032	0.1383
	$d^{(2)}\ (m)$	0.1340	0.2193	0.1757	0.3055
IOU		0.8297	0.8202	0.6846	0.7609
Time $(s)$		0.2023	0.2280	0.2314	0.3363

We use root mean square error (RMSE) and Intersection-Over-Union (IOU) [28] as our performance metrics over $M_{c}=100$ Monte Carlo runs. RMSEs of the state estimates are

$$\text{RMSE}_{x}=\sqrt{\frac{1}{KM_{c}}\sum_{k=1}^{K}\sum_{i=1}^{M_{c}}(x_{k}-\hat{x}_{k}^{i})^{2}},$$ (32)

where $x_{k}$ denotes the true state and $\hat{x}_{k}^{i}$ is its estimate, calculated according to

$$\hat{\mathbf{x}}_{k}^{n}=\sum_{i=1}^{N}w_{k,(i)}\mathbf{x}_{k,(i)}^{n},\ \text{ and }\ \hat{\mathbf{x}}_{k}^{l}=\sum_{i=1}^{N}w_{k,(i)}\bm{\mu}_{k|k,(i)}.$$ (33)

Velocity and centroid errors are calculated using their respective Euclidean norms. To account for the performance in extent estimation, we calculate IOU according to

$$\text{IOU}=\frac{1}{KM_{c}}\sum_{k=1}^{K}\sum_{i=1}^{M_{c}}\frac{\text{area}\left(\hat{\mathbf{X}}^{e}_{k,i}\cap\mathbf{X}^{e}_{k,\text{true}}\right)}{\text{area}\left(\hat{\mathbf{X}}^{e}_{k,i}\cup\mathbf{X}^{e}_{k,\text{true}}\right)}$$ (34)

where $\hat{\mathbf{X}}^{e}_{k,i}$ denotes the posterior superellipse region described by the posterior extent state $\hat{\mathbf{x}}^{e}_{k,i}$ in Monte Carlo run $i$, and $\hat{\mathbf{X}}^{e}_{k,\text{true}}$ is the true extent of the target.

IV-A1 Known Shape Scenarios

Sample outputs of the algorithm with a known shape, i.e. a known $q$ value, are given in Figure 4 while the performance metrics and computation times are given in Table I. Note that the RMSEs are comparable in the U-turn and Drifting scenarios, even when one side of the target is invisible throughout most of the simulation. The half-length state corresponding to $d^{(2)}$, unobservable during these scenarios, also exhibits sufficiently small RMSE values in these scenarios, indicating that the corresponding state estimate has not deteriorated. These show the effectiveness of the Kalman gain mask $\mathbf{D}_{k,(i)}$, preventing erroneous updates to the extension estimates and preserving the centroid estimate.

IV-A2 Unknown Shape Scenario

To illustrate the performance of the algorithm for targets with unknown shapes, i.e. unknown $q$, we run the algorithm with the augmented state in Section III-B in three elementary scenarios. In these scenarios, targets follow a linear trajectory with constant speed over $K=100$ time steps. The state $q_{k}$ is initialized by sampling from the prior $q_{1,(i)}\sim\mathcal{N}\!\left(2,(0.2)^{2}\right)$ with its process noise standard deviation set to $\sigma_{q}=0.1$, while $N=1500$ particles are used to compensate for the increase in the nonlinear state dimension. All other parameters are left identical. The results are presented in Figure 5. It is seen that the algorithm can find a superellipse that accommodates the obtained measurements. The estimated centroids also appear to reflect a suitable trajectory for the target.

IV-B Real Data Application

Finally, we illustrate the performance of our algorithm on real data. For this purpose, we use the data presented in [15], where a Lidar sensor with $T=0.1\ s$ sampling time and angular resolution $0.2^{\circ}$ is used to collect contour measurements from a moving vehicle whose rear and right sides are visible. On the same dataset, we present the output of both the known and unknown shape approaches. For the known shape algorithm, the superelliptical exponent $q=5$ is assumed to better represent the target as an automobile, while the unknown shape algorithm is initialized again with $\mathcal{N}\!\left(2,(0.2)^{2}\right)$ for the prior of the state $q_{k}$. The outputs of the algorithms are shown in Figure 6. Both algorithms successfully track the vehicle and estimate its velocity, and the algorithm with the adaptable target shape finds a suitable superellipse although initialized with an elliptical prior.

Figure 5: Three different simulation results with the augmented target state. The vehicle is moving from left to right. Solid lines indicate the estimated contour and the velocity vector, the dotted line indicates the estimated centroid trajectory and the final and true $q$ values are given in the figure.

(a) Known shape with $q=5$.

(b) Unknown shape initialized with an ellipse.

Figure 6: Example output of the algorithm with real data from [15], plotted every 12 frames. The vehicle is moving from right to left. The solid black lines indicate the posterior (contour, centroid and velocity vector) while the dotted line indicates the centroid trajectory.

V Conclusion

In this paper, we have proposed a new ETT algorithm for contour measurements which represents the target shape as a family of curves known as superellipses. Through the variation of a single exponent parameter, the algorithm unifies a variety of shapes such as diamonds, ellipses and rectangles with varying degrees of rounded corners in a single, implicit measurement equation. We have used this model in a Rao-Blackwellized particle filter, with scaling constraints to prevent particles from reaching local minima of the implicit measurement model proposed and analytical sensor-object geometry relations by modifying the Kalman filter equations. Using both simulations and real-world data, we have shown that the algorithm has high tracking performance, as well as being able to estimate the unknown shape of the target.