Three-Dimensional Extended Target Tracking and Shape Learning Based on Double Fourier Series and Expectation Maximization

Abstract

This paper investigates the problem of tracking targets with unknown but fixed 3D star-convex shapes using point cloud measurements. While existing methods typically model shape parameters as random variables evolving according to predefined prior models, this evolution process is often unknown in practice. We propose a particular approach within the Expectation Conditional Maximization (ECM) framework that circumvents this limitation by treating shape-defining quantities as parameters estimated directly via optimization. The objective is the joint estimation of target kinematics, extent, and orientation in 3D space. Specifically, the 3D shape is modeled using a radial function estimated via double Fourier series (DFS) expansion, and orientation is represented using the compact, singularity-free axis-angle method. The ECM algorithm facilitates this joint estimation: an Unscented Kalman Smoother infers kinematics in the E-step, while the M-step estimates DFS shape parameters and rotation angles by minimizing regularized cost functions, promoting robustness and smoothness. The effectiveness of the proposed algorithm is substantiated through two experimental evaluations.

1. Introduction

The accurate perception and tracking of targets in three-dimensional (3D) environments are a fundamental requirement for many modern intelligent systems, including autonomous vehicles, robotic platforms, and advanced surveillance systems [1,2,3,4]. While traditional point target tracking algorithms assume that one target only gives rise to one measurement, they are insufficient when dealing with objects possessing significant spatial extent relative to sensor resolution, known as extended target [5,6]. The effective tracking of these extended targets demands algorithms that can estimate not only their kinematic state (position, velocity) but also their physical characteristics, including shape and orientation, within a 3D space.

A substantial body of work considers the extended target tracking (ETT) problem, especially in 2D space [5,6]. Corresponding methods process 2D measurements based on diverse extent models. One common approach employs random matrix to model the target’s shape as an ellipse [7,8]. These extent models often assume that the extent state follows a specific probability distribution, such as a Gaussian distribution. However, this underlying distribution is frequently unknow in practice. Addressing this limitation, paper [9] presents an alternative method where extent characteristics, including semi-axis lengths, are treated as parameters for direct estimation. For complex star-convex targets, more sophisticated shape modeling methods, such as B-splines [10] and Gaussian processes (GPs) [11,12], have been developed. Notably, the random hyper-surface model (RHM) is a typical approach for representing arbitrary star-convex shapes within the frequency domain [13,14]. This model defines the target’s spatial extent using a Fourier series expansion, with its coefficients jointly estimated alongside the target’s kinematics. RHM offers a parameterization particularly well-suited for star-convex extents, effectively balancing representational accuracy with computational complexity. Numerous adaptations of such Fourier-based approaches have subsequently been explored across diverse application settings [15,16,17,18,19,20,21].

When 3D surface measurements from devices like multilayer LiDAR sensor or depth cameras are available, it becomes possible to model the target’s 3D shape. A few approaches to 3D extended target tracking involved measurement models using basic geometric shapes [22] or restricted shape estimation [23,24,25]. The methods are suitable for when the detailed shape information is not critical for the application. For instance, where the target’s shape is unknown beforehand, random matrix models can represent the target using forms like ellipsoid, or rectangular cuboids. However, the representation power of RM models remains limited to these relatively simple geometric shapes. To address more complex geometries, Gaussian Processes have been developed for 3D space, allowing the modeling of arbitrary shaped star-convex targets [26,27]. While GPs effectively capture complex 3D shapes, their representation (often via a radius function) typically results in a high-dimensional state vector. This high dimensionality poses challenges for filtering algorithms and increases the computational burden. In contrast, the double Fourier series (DFS) shape model proposed in [28] utilizes a shape vector of significantly lower dimensionality compared to GP models, while also enabling the estimation of both shape and pose. It’s worth noting that a common characteristic across many of these 3D shape measurement models is the modeling of the shape-characterizing state as a random variable, typically assumed to follow a Gaussian distribution.

Differing from traditional 3D methods that rely on prior shape evolution models, our approach models the parameters defining the extent state directly, treating them as quantities with clear physical meaning. Within the Expectation Conditional Maximization (ECM) framework, we propose an extended target tracking method for the joint estimation of kinematics, extent, and orientation in 3D space. The main contributions are listed as follows.

(1)

The unknown but fixed 3D shape is represented via a radial function in spherical coordinates, with a Double Fourier Series (DFS) expansion employed for its modeling. This approach converts the continuous radial function into a parametric representation characterized by a finite set of coefficients.

(2)

The axis-angle representation constitutes a compact and singularity-free approach for parameterizing 3D orientation. It describes a rotation by specifying a unit vector, which defines the axis of rotation, and a scalar angle, which denotes the magnitude of rotation about this axis. Furthermore, this method exhibits both geometric intuitiveness and notable computational efficiency.

(3)

Joint estimation of the target’s kinematic state and shape parameters is achieved via the Expectation Conditional Maximization (ECM) framework. The E-step infers the kinematic state using an unscented Kalman smoother filter, while the M-step estimates shape and rotation parameters by minimizing separate cost functions with added regularization for robustness and smoothness.

The remainder of this paper is organized as follows. Section 2 formulates the problem and presents the system, including the motion model, shape representation, and measurement model. Section 3 derives an ECM-based algorithm for 3D ETT problem. Section 4 presents simulation results to validate the proposed approach across two distinct scenarios. Finally, Section 5 summarizes the main findings and concludes the article.

Notation: Throughout this paper, the superscripts “−1” and “$\top$” represent the inverse and transpose of a matrix, respectively; $\mathcal{N}(\mu ,\sum )$ represents the Gaussian distribution with mean $\mu$ and covariance $\sum$; $E[\cdot ]$ and $E[\cdot |\cdot ]$ is mathematical expectation and conditional expectation. The notation $\otimes$ refers to the Kronecker product. $I$ denote the identity matrix with proper dimensions; $|\cdot |$ and $Tr[\cdot ]$ denote the determinant and trace of matrices, respectively; $\left|\right|\cdot |{|}_2$ denotes the Euclidean norm.

2. Problem Formulation

This section first introduces a model for the spatial extent of 3D target, which employs a radial function parameterized via a Double Fourier Series (DFS). The target’s orientation is then described using the axis–angle representation, a method characterized by its compactness and singularity-free nature. Following this, two kinematic state evolution models specifically designed for 3D extended target tracking are presented. Finally, the paper details the measurement model that integrates this DFS-based shape representation with the selected axis–angle orientation.

2.1. DFS-Based Shape Model for 3D Target

Advanced sensors, such as depth cameras and LiDAR, can generate detailed three-dimensional (3D) point cloud data, extracting precise shape estimates from such data is crucial for object classification and predicting future behavior. To effectively learn shapes from these point clouds, it is essential to formulate a suitable object extent description method that possesses high representational power for diverse 3D shapes while also being sufficiently compact to support efficient online tracking algorithms.

This section details the modeling of the target shape in spherical coordinates via a radial function $f(\theta ,\phi )$. Such functions relate angular coordinate pairs $(\theta ,\phi )$ to a radial distance in each orientation and are adept at representing star-convex geometries. In our methodology, this radial function is modeled using a Double Fourier Series (DFS) expansion, the specific form of which is given by:

$$\begin{array}{cc}\hfill f(\zeta ,\theta ,\phi )& ={\zeta }_0+{\displaystyle \sum _{n=1}^N{\displaystyle \sum _{m=1}^M[{\zeta }_{nm}^{cc}}}\cos (n\theta )\cos (m\varphi )+{\zeta }_{nm}^{ss}\sin (n\theta )\sin (m\varphi )\hfill \\ & +{\zeta }_{nm}^{sc}\sin (n\theta )\cos (m\varphi )+{\zeta }_{nm}^{cs}\cos (n\theta )\sin (m\varphi )]\hfill \\ \hfill =& S(\theta ,\phi )\cdot \zeta \hfill \end{array}$$

(1)

where

$$\zeta =[{\zeta }_0,{\zeta }_{11}^{cc},{\zeta }_{11}^{ss},{\zeta }_{11}^{sc},{\zeta }_{11}^{cs},{\zeta }_{12}^{cc},{\zeta }_{12}^{ss},{\zeta }_{12}^{sc},{\zeta }_{12}^{cs},\dots ,{\zeta }_{NM}^{cc},{\zeta }_{NM}^{ss},{\zeta }_{NM}^{sc},{\zeta }_{NM}^{cs}]$$

$$\begin{array}{ll}\hfill S(\theta ,\phi )=[1,& \cos \theta \cos \varphi ,\sin \theta \sin \varphi ,\sin \theta \cos \varphi ,\hfill \\ & \cos \theta \sin \varphi ,\cos \theta \cos 2\varphi ,\sin \theta \sin 2\varphi ,\sin \theta \cos 2\varphi ,\cos \theta \sin 2\varphi \dots ,\\ & \cos N\theta \cos M\varphi ,\sin N\theta \sin M\varphi ,\sin N\theta \cos M\varphi ,\cos N\theta \sin M\varphi ]\end{array}$$

Here, ${\zeta }_0$ is the base radial parameter, ${\zeta }_{nm}^{cc},{\zeta }_{nm}^{ss},{\zeta }_{nm}^{sc},{\zeta }_{nm}^{cs}$ are the Fourier coefficients, and $N,M$ are the number of angle $\theta$ and $\varphi$. $\theta \in [-\pi ,\pi ]$ is azimuth and $\varphi \in [-\pi /2,\pi /2]$ is elevation. The shape parameter $\zeta$ collects ${\zeta }_0$ and all $4NM$ coefficients, resulting in a dimension of $d_{\zeta }=1+4NM$.

2.2. Orientation Representation (Axis-Angle)

To represent the target’s three-dimensional (3D) orientation, this work utilizes the axis-angle parameterization, often expressed compactly as an orientation vector $\omega =\gamma a$. In this formulation, $a$ is a unit vector denoting the axis of orientation, and $\gamma$ is the scalar angle defining the magnitude of rotation orientation according to the right-hand rule.

This choice of representation is underpinned by several compelling advantages pertinent to tracking applications. Firstly, the axis–angle representation (particularly in its three-parameter rotation vector form $\omega$) provides a geometrically intuitive and unique mapping for any 3D rotation (up to 2π periodicity in angle), without requiring the satisfaction of complex inter-parameter constraints for its validity—unlike, for instance, the nine-parameter rotation matrix which must adhere to strict orthogonality and determinant conditions. Secondly, the practical application and conversion of this representation are significantly facilitated by Rodrigues’ rotation formula. This formula not only provides a straightforward and computationally efficient method for determining the effect of the rotation on a vector (i.e., rotating a vector) but also, more fundamentally, offers an explicit algorithm to compute the exponential map from the Lie algebra so (3) (the space of 3 × 3 skew-symmetric matrices, representing infinitesimal rotations) to the Special Orthogonal group SO (3) (the Lie group of finite 3D rotations) [29]. The corresponding rotation matrix $R(\omega )$ is computed using Rodrigues’ rotation formula:

$$R(\omega )=I_3+\sin (\gamma ){[b]}_{\times }+(1-\cos (\gamma )){[b]}_{\times }^2$$

(2)

where

$${[b]}_{\times }=\left(\begin{array}{ccc}0& -b_3& b_2\\ b_3& 0& -b_1\\ -b_2& b_1& 0\end{array}\right)$$

(3)

${[b]}_{\times }$ is the skew-symmetric cross-product matrix of $b$. The axis-angle coordinates $\omega =\gamma a$ are a natural and compact rotation representation in terms of its geometric building blocks.

2.3. Kinematic State Model

A.

line kinematic model

The kinematic state $x_t={\left[c_t^{\top } v_t^{\top }\right]}^{\top }$ at time $t$ includes the center position $c_t=\left[c_{x,t} c_{y,t} c_{z,t}\right]$ and velocity components $v_t=[v_{x,t} v_{y,t} v_{z,t}]$. The dynamic model of extended target is modeled by

$$x_{t+1}=F{x}_t+w_t$$

(4)

where $F$ is the state transition matrix, the process noise $w_t~\mathcal{N}(0,C)$.

For constant velocity model (CV) [27]:

$$F=\left[\begin{array}{cc}1& T\\ 0& 1\end{array}\right]\otimes I_3\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}C=\left[\begin{array}{cc}{\displaystyle \frac{T^3}{3}}& {\displaystyle \frac{T^2}{2}}\\ {\displaystyle \frac{T^2}{2}}& T\end{array}\right] \otimes ({\sigma }_c^2{I}_3)$$

(5)

where $T$ is sampling time, ${\sigma }_c$ is the process noise variance for the centroid, $\otimes$ is the Kronecker product, and $I_3$ denotes an identity matrix.

B.

nonlinear kinematic model

While the target moves with a constant acceleration (CA) between time step, the acceleration state vector $a_t=[a_{x,t}{ \mathrm{a}}_{y,t}{ \mathrm{a}}_{z,t}]$ is augmented the kinematic state. Combining these into the 9-dimensional state vector $x_t={[c_t^{\top },v_t^{\top },a_t^{\top }]}^{\top }$, the state evolution model is given as follows [30],

$$x_{t+1}=F^{CA}{x}_t+w_t^{CA}$$

(6)

where $F^{CA}$ is the state transition matrix, the process noise $w_t^{CA}~\mathcal{N}(0,Q^{CA})$.

$$F^{CA}=\left[\begin{array}{ccc}{I}_3& T{I}_3& {\displaystyle \frac{1}{2}}{T}^2{I}_3\\ 0_3& I_3& T{I}_3 \\ 0_3& 0_3& I_3\end{array}\right] Q^{CA}={\sigma }_a^2\left[\begin{array}{ccc}{\displaystyle \frac{T^5}{20}}{I}_3& {\displaystyle \frac{T^4}{8}}{I}_3& {\displaystyle \frac{T^3}{6}}{I}_3\\ {\displaystyle \frac{T^4}{8}}{I}_3& {\displaystyle \frac{T^3}{3}}{I}_3& {\displaystyle \frac{T^2}{20}}{I}_3 \\ {\displaystyle \frac{T^3}{6}}{I}_3& {\displaystyle \frac{T^2}{2}}{I}_3& T{I}_3\end{array}\right]$$

(7)

The scalar ${\sigma }_a^2$ represents the intensity of the underlying noise process and is a crucial tuning parameter for the CA model.

2.4. Measurement Model

Sensor data at time $t$ consists of a set of $N_t$ noisy 3D measurements ${\left\{z_{j,t}\right\}}_{j=1}^{N_t}$ originating from the target’s surface. Each measurement $z_{j,t}$ is related to the state and parameters via the non-line measurement function plus noise. A single measurement can be expressed as

$$z_{j,t}=c_t+f_{local}+e_t$$

(8)

where $c_t$ is the center of the target, $f_{local}$ is the radial function representation which describes the target’s shape in local coordinate. However, the measurement $z_{j,t}$ is obtained in global coordinates. Consequently, a global-to-local transformation is essential. This transformation yields the following local representation,

$$f_{local}=R_{global}^{local}{f}_{global}=R(\omega )\left[\begin{array}{l}f(\zeta ,{\theta }_{j,t},{\varphi }_{j,t})\cos ({\varphi }_{j,t})\cos ({\theta }_{j,t})\\ f(\zeta ,{\theta }_{j,t},{\varphi }_{j,t})\cos ({\varphi }_{j,t})\sin ({\theta }_{j,t})\\ f(\zeta ,{\theta }_{j,t},{\varphi }_{j,t})\sin ({\varphi }_{j,t})\end{array}\right]$$

(9)

$f(\zeta ,{\theta }_{j,t},{\varphi }_{j,t})$ represents the radial function form in global coordinate, which modeled using DFS parameterized by the coefficient $\zeta$, and $e_t$ is zero-mean Gaussian measurement noise with covariance $R_t$. $({\theta }_{j,t},{\varphi }_{j,t})$ is the angle pair of the measurement source on the target surface that originates $z_{j,t}$.

$R(\omega )$ is the rotation matrix that converts the global coordinates to local coordinates for the corresponding orientation parameter $\omega$. This parameters $\omega$ utilize the axis-angle representation, expressed as $\omega =\gamma a$, where $a=[a_x,a_y,a_z]$ is a unit vector ($\left|\right|a|{|}_2=1$) indicating the axis of rotation orientation, and $\gamma$ is the angle of rotation about the axis $a$ (typically in radial, following the right-hand rule). This parameter $\omega$ compactly stores both axis and angle information. Specifically, its magnitude $\left|\right|\omega |{|}_2=\gamma$ give the rotation angle, while its direction $\omega /\left|\right|\omega |{|}_2=a$ give the axis for rotation orientation (for $\gamma \ne 0$). This axis-angle representation uses only 3 parameters (the components of $\omega$) and advantageously avoids the gimbal lock singularities associated with Euler angles.

The parameter $\zeta$ is the set of coefficients for the DFS that defines the entire 3D shape of the target. The core assumption of the model is that the target is a rigid body. Therefore, this single, unified shape parameter $\zeta$ is used to calculate the surface radius for any point $({\theta }_t,{\varphi }_t)$ on the object’s surface. When process the $j$th measurement, and we use its specific projected angles $({\theta }_{j,t},{\varphi }_{j,t})$ as inputs to the universal shape function defined by $\zeta$ to get the predicted radius at that exact spot.

Let $Z^G=z_{j,t}-c_t$ and $({\theta }_{j,t},{\varphi }_{j,t})$ be expressed as,

$${\theta }_{j,t}=\arccos ({\displaystyle \frac{x_t^G}{\left|\right|z_{j,t}-c_t|{|}_2}})$$

(10)

$${\varphi }_{j,t}=\mathrm{atan}2(y_t^G,x_t^G)$$

(11)

where $Z_t^G\triangleq (x_t^G,y_t^G,z_t^G)$.

As the measurement $z_{j,t}$ cannot be isolated as an explicit function of the state vector and noise, Equation (8) defines an implicit measurement model. It can be rewritten as:

$$\begin{array}{ll}0=& \underset{h(x_t,z_{j,t},\zeta ,\omega )}{\underbrace{-z_{j,t}+c_t+R(\omega )\left[\begin{array}{l}f(\zeta ,{\theta }_{j,t},{\varphi }_{j,t})\cos ({\varphi }_{j,t})\cos ({\theta }_{j,t})\\ f(\zeta ,{\theta }_{j,t},{\varphi }_{j,t})\cos ({\varphi }_{j,t})\sin ({\theta }_{j,t})\\ f(\zeta ,{\theta }_{j,t},{\varphi }_{j,t})\sin ({\varphi }_{j,t})\end{array}\right]}}+e_t\\ & =h(x_t,z_{j,t},\zeta ,\omega )+e_t\end{array}$$

(12)

The zero vector appearing in Equation (12) can be interpreted as representing a pseudo-measurement. This pseudo-measurement is effectively a nonlinear function of the state and the original measurement, assumed to be corrupted by additive Gaussian noise.

3. ECM Tracking Algorithm Based on DFS

This section details the DFS-ECM framework for joint motion state, shape, and orientation estimation. Its core, the Expectation Conditional Maximization (ECM) algorithm, uses a batch method involving iterative Kalman smoothing over a sliding data window. This incorporates information from previous and subsequent scans. For a batch window from time 1 to $T_b$, the ECM framework defines these variables:

$$X=(x_1,\dots ,x_t\dots ,x_{T_b})$$

$$Z=(Z_1,\dots ,Z_t\dots ,Z_{T_b})$$

$$\Lambda =({\Lambda }_1,\dots ,{\Lambda }_t\dots ,{\Lambda }_{T_b})$$

where $X$ denotes the latent variables within the sliding window; $Z$ represents the set of measurements within the sliding window, including $Z_t=(z_{1,t},\dots z_{j,t},\dots ,z_{N_t,t})$; $\Lambda$ denotes the complete sequence of unknown parameters, where each ${\Lambda }_t=(\omega ,\zeta )$ of the sliding window at time step $t$. ECM is an iterative optimization method whose objective is to maximize the log-likelihood function of the incomplete data (latent variable) $X$ with respect to the parameter $\Lambda$.

The main terms used in this algorithm can be found in

Table 1. Summary of the main terms used in the derivation of the algorithm.

$P_t$	The error covariance of target state at time $t$
$P_{t|t-1}$	The predict error covariance of target state at time $t$
$P_{j,zz,t|t-1}$	The $j$th innovation covariance at time $t$
$P_{j,xz,t|t-1}$	The $j$th cross-covariance at time $t$
$P_{j,t}$	The $j$th error covariance at time $t$
$P_{j,t|T_b}$	The $j$th smoothed covariance at time $t$
$P_{t|T_b}$	The smoothed error covariance at time $t$
$p(x_t|x_{t-1})$	Transition Probability density of state $x_t$
$p(z_{j,t}|x_t,{\Lambda }^{(i)})$	The likelihood function
$p(X^{(i+1)},Z|{\Lambda }^{(i)})$	The likelihood function of complete data
$p(x_0)$	The prior Probability density of state
$P_s(\zeta )$	The smoothness penalty term of the cost function
$i$	Iteration times of ECM
$j$	Index of measurement number

.

We define the complete-data log-likelihood function $L({\Lambda }^{(i+1)})$ and its corresponding conditional expectation function $Q({\Lambda }^{(i+1)};{\Lambda }^{(i)})$ of the $(i+1)$th iteration as follows:

$$L({\Lambda }^{(i+1)})=\ln p(X^{(i+1)},Z|{\Lambda }^{(i+1)})$$

(13)

$$Q({\Lambda }^{(i+1)};{\Lambda }^{(i)})=E[L({\Lambda }^{(i+1)})|Z,{\Lambda }^{(i)}]$$

(14)

As the $Q$ function involves multiple unknown parameters, a direct application of the traditional maximization step is not feasible. Instead, we utilize multiple conditional maximization steps to iteratively estimate the parameters.

3.1. E-Step

The primary purpose of the E-step is to estimate the state $x_t$ and its corresponding error covariance matrix $P_t$, a process that involves an Unscented Kalman forward filter (UKF) [31,32] and backward Rauch–Tung–Striebel Smoother smoother [33,34]. The forward propagation is performed as,

Let $n$ be the dimension of the kinematic state $x_t$, we generate $2n+1$ sigma points ${\mathcal{X}}_{k,t-1}$ based on the previous state estimate $x_{t-1}$ and error covariance $P_{t-1}$ (state initialized as $x_0={\mu }_0$, $P_0={\sum }_0$):

$${\mathcal{X}}_{k,t-1}=\left\{\begin{array}{ll}{x}_{t-1}& k=0\\ x_t+{(\sqrt{(n+\lambda )P_{t-1}})}_k& k=1,\dots ,n\\ x_t-{(\sqrt{(n+\lambda )P_{t-1}})}_k& k=n+1,\dots ,2n\end{array}\right.$$

(15)

The weights for mean $W_k^{(m)}$ and covariance $W_k^{(c)}$ are:

$$W_k^{(m)}=\left\{\begin{array}{l}{\displaystyle \frac{\lambda }{n+\lambda }}\hspace{1em}\hspace{1em}k=0\\ {\displaystyle \frac{1}{2(n+\lambda )}}\hspace{1em}\hspace{1em}k\ne 0\end{array}\right.$$

(16)

$$W_k^{(c)}=\left\{\begin{array}{l}{\displaystyle \frac{\lambda }{n+\lambda }}+ (1-{\alpha }^2+\beta ) \hspace{1em}\hspace{1em}k=0\\ {\displaystyle \frac{1}{2(n+\lambda )}}\hspace{1em}\hspace{1em}k\ne 0\end{array}\right.$$

(17)

where $\lambda ={\alpha }^2(n+\kappa )-n$ is a scaling parameter, $\alpha$ controls the distance of the Sigma points from mean, $\kappa$ is a secondary parameter $\beta$ is used to incorporate covariance information.

Prediction step of UKF:

(1) Propagate Sigma Points: Pass each state sigma point through the process model $F$,

$${\mathcal{X}}_{k,t|t-1}=F{\mathcal{X}}_{k,t-1}$$

(18)

(2) Compute the predicted state ${\widehat{x}}_{t|t-1}$ and predict error covariance $P_{t|t-1}$:

$${\widehat{x}}_{t|t-1}={\displaystyle \sum _{k=0}^{2n}{W}_k^{(m)}{\mathcal{X}}_{k,t|t-1}}$$

(19)

$$P_{t|t-1}={\displaystyle \sum _{k=0}^{2n}{W}_k^{(c)}(}{\mathcal{X}}_{k,t|t-1}-{\widehat{x}}_{t|t-1}){({\mathcal{X}}_{k,t|t-1}-{\widehat{x}}_{t|t-1})}^{\top }+C$$

(20)

Update step of UKF: the update proceeds sequentially for each measurement. Initialize the updated state and error covariance for this time step with predicted values,

$${\widehat{x}}_{0,t}={\widehat{x}}_{t|t-1}$$

(21)

$$P_{0,t}=P_{t|t-1}$$

(22)

the measurements ${\left\{z_{j,t}\right\}}_{j=1}^{N_t}$ are processed using the following steps,

(1) Predict the $j$th measurement ${\mathcal{Z}}_{j,k,t|t-1}$ for each sigma point ${\mathcal{X}}_{k,t|t-1}$ using the parameter $\zeta ,\omega$,

$${\mathcal{Z}}_{j,k,t|t-1}=h({\mathcal{X}}_{k,t|t-1},\zeta ,\omega )$$

(23)

(2) Compute the $j$th weighted average measurement ${\widehat{z}}_{j,t|t-1}$ of the predicted sigma point,

$${\widehat{z}}_{j,t|t-1}={\displaystyle \sum _{k=0}^{2n}{W}_k^{(m)}}{\mathcal{Z}}_{k,t|t-1}$$

(24)

(3) Estimate the $j$th innovation covariance $P_{j,zz,t|t-1}$ and the cross-covariance $P_{j,xz,t|t-1}$,

$$P_{j,zz,t|t-1}={\displaystyle \sum _{k=0}^{2n}{W}_k^{(c)}({\mathcal{Z}}_{k,t|t-1}-{\widehat{z}}_{j,t|t-1})}{({\mathcal{Z}}_{k,t|t-1}-{\widehat{z}}_{j,t|t-1})}^{\top }+R_t$$

(25)

$$P_{j,xz,t|t-1}={\displaystyle \sum _{k=0}^{2n}{W}_k^{(c)}({\mathcal{X}}_{k,t|t-1}-{\widehat{x}}_{t|t-1})}{({\mathcal{Z}}_{k,t|t-1}-{\widehat{z}}_{j,t|t-1})}^{\top }$$

(26)

(4) Calculate the $j$th Kalman Gain $K_{j,t}$,

$$K_{j,t}=P_{j,xz,t|t-1}{P}_{j,zz,t|t-1}^{-1}$$

(27)

(5) Update the $j$th state ${\widehat{x}}_{j,t}$ and its error covariance $P_{j,t}$,

$${\widehat{x}}_{j,t}={\widehat{x}}_{j-1,t}+K_{j,t}(0-{\widehat{z}}_{j,t|t-1})$$

(28)

$$P_{j,t}=P_{j-1,t}-K_{j,t}{P}_{j,zz,t|t-1}{K}_{j,t}^{\top }$$

(29)

When processing all $N_t$ measurements, the UKF state ${\widehat{x}}_t$ estimation and its error covariance $P_t$ are:

$${\widehat{x}}_t={\widehat{x}}_{N_t,t}$$

(30)

$$P_t=P_{N_t,t}$$

(31)

After sequentially processing the data from time $t=1$ to $t=T_b$ with an Unscented Kalman Filter (UKF), we obtain the series of filter states ${\left\{{\widehat{x}}_t\right\}}_{t=1}^{T_b}$ and error covariances ${\left\{P_t\right\}}_{t=1}^{T_b}$. Subsequently, the Rauch–Tung–Striebel smoothing algorithm is applied to compute the smoothed state ${\widehat{x}}_{t|T_b}$ and its error covariance $P_{t|T_b}$ as follows,

The backward pass begins at the final time step $T_b$. (1) Initialize the smoothed state ${\widehat{x}}_{0,t|T_b}$ and error covariance $P_{0,t|T_b}$ for this time step with UKF estimation,

$${\widehat{x}}_{0,t|T_b}={\widehat{x}}_{T_b}$$

(32)

$$P_{0,t|T_b}=P_{T_b}$$

(33)

The algorithm then iterates backward in time, from $t=T_b-1$ down to $t=1$. For each time step $t$, the following computations with the measurements ${\left\{z_{j,t}\right\}}_{j=1}^{N_t}$ are performed:

(2) Compute the $j$th Smoothed Gain $G_{j,t}$,

$$G_{j,t}=P_{j,xz,t+1|t}{P}_{t+1|t}^{-1}$$

(34)

where $P_{j,xz,t+1|t}$ is the $j$th cross covariance and $P_{t+1|t}$ is the predict error covariance of forward UKF, both obtained from the forward UKF pass.

(3) Compute the $j$th Smoothed state ${\widehat{x}}_{j,t|T_b}$,

$${\widehat{x}}_{j,t|T_b}={\widehat{x}}_{j-1,t|T_b}+G_t({\widehat{x}}_{t+1|T_b}-{\widehat{x}}_{t+1|t})$$

(35)

(4) Compute the $j$th Smoothed error Covariance $P_{j,t|T_b}$,

$$P_{j,t|T_b}=P_{j-1,t|T_b}+G_{j,t}(P_{t+1|T_b}-P_{t+1|t})G_{j,t}^{\top }$$

(36)

When processing all $N_t$ measurements, the smoothed state ${\widehat{x}}_{t|T_b}$ estimation and its error covariance $P_{t|T_b}$ are,

$${\widehat{x}}_{t|T_b}={\widehat{x}}_{N_t,t|T_b}$$

(37)

$$P_{t|T_b}=P_{N_t,t|T_b}$$

(38)

Considering the Markov property of state propagation and the independence of measurement sources, the log-likelihood function $L({\Lambda }^{(i+1)})$ is written as,

$$L({\Lambda }^{(i+1)})=\ln p(x_0)+{\displaystyle \sum _{t=1}^{T_b}\ln p(x_{t+1}|x_t)}+{\displaystyle \sum _{t=1}^{T_b}{\displaystyle \sum _{j=1}^{N_t}\ln p(z_{j,t}|x_t,{\Lambda }^{(i+1)})}}$$

(39)

So,

$$\begin{array}{cc}\hfill Q({\Lambda }^{(i+1)};{\Lambda }^{(i)}))& =E_{Z,{\Lambda }^{(i)}}[\ln p(x_0)]+{\displaystyle \sum _{t=1}^{T_b}{E}_{Z,{\Lambda }^{(i)}}}\ln p(x_t|x_{t-1})+{\displaystyle \sum _{t=1}^{T_b}{\displaystyle \sum _{j=1}^{N_t}{E}_{Z,{\Lambda }^{(i)}}\ln p(z_{j,t}|x_t,{\Lambda }^{(i+1)})}}\hfill \\ & ={\displaystyle \sum _{t=1}^{T_b}{\displaystyle \sum _{j=1}^{N_t}{E}_{Z,{\Lambda }^{(i)}}\ln p(z_{j,t}|x_t,{\Lambda }^{(i+1)})+Const}}\hfill \end{array}$$

(40)

Under the assumption of Gaussian measurement noise, the measurement likelihood becomes,

$$p(z_{j,t}|x_t,\omega ,\zeta )=N(z_{j,t}-h(x_t,z_{j,t},\omega ,\zeta );0,R_t)$$

(41)

Insert Equation (41) into (40), $Q$ is,

$$\begin{array}{l}Q({\Lambda }^{(i+1)};{\Lambda }^{(i)})=={\displaystyle \sum _{t=1}^{T_b}{\displaystyle \sum _{j=1}^{N_t}{E}_{Z,{\Lambda }^{(i)}}\ln p(z_{j,t}|x_t,{\Lambda }_t^{(i+1)})+Const}}\\ =-{\displaystyle \frac{1}{2}}{\displaystyle \sum _{j=1}^{N_t}\{{\displaystyle \sum _{t=1}^{T_b}\ln \left|R_{j,t}\right|}+\mathrm{Tr}{\displaystyle \sum _{t=1}^{T_b}(R_{j,t})E_{Z,{\Lambda }^{(i)}}[(z_{j,t}-h(x_t,z_{j,t},\omega ,\zeta )){(•)}^{\top }]}}\}+Const\end{array}$$

(42)

where $(•)$ denotes the term same as the former one.

3.2. M-Step

After obtaining $Q({\Lambda }^{(i+1)};{\Lambda }^{(i)})$ in the E-step of the algorithm, the task of the M-step is to find ${\Lambda }^{(i+1)}$, thereby updating the target’s shape and axis-angle parameters from the previous iteration. Since $Q({\Lambda }^{(i+1)};{\Lambda }^{(i)})$ is a non-linear function of the parameters, its analytical solution is generally difficult to obtain. Here, parameters are estimated by minimizing regularized cost function which equivalent to maxing $Q({\Lambda }^{(i+1)};{\Lambda }^{(i)})$ estimation.

(1)

Shape parameter optimization

The shape parameter $\zeta$ are estimated by minimizing the cost function $J(\zeta )$ over a sliding window of $T_b$ time steps ($t=1$ to $T_b$).

$$J_{original}(\zeta )=({\displaystyle \frac{1}{{\displaystyle \sum _{t=1}^{T_b}{N}_t}}}{\displaystyle \sum _{t=1}^{T_b}{\displaystyle \sum _{j=1}^{N_t}\left|\right|z_{j,t}-h({\widehat{x}}_t,z_{j,t},\zeta ,\widehat{\omega })|{|}_{R_t^{-1}}^2)+{\eta }_{\zeta }\left|\right|{\zeta }_{2:end}|{|}_2^2}}+P_s(\zeta )$$

(43)

where, ${\widehat{x}}_t$, $\widehat{\omega }$ are the estimates of state and orientation at time $t$ within the window.$\left|\right|e|{|}_{R^{-1}}^2=e^{\top }{R}^{-1}e$ denotes the squared Mahalanobis distance. The first term is the average squared Mahalanobis distance over all valid measurements in the window. ${\eta }_{\zeta }$ is the L2 regularization parameter applied to the coefficients (excluding ${\eta }_0$) to prevent overfitting and improving conditioning. $P_s(\zeta )$ is the smoothness penalty term.

Remark 1.

Using the original cost function $J_{original}(\zeta )$ can lead to artificial bulges or distortions near the poles ($\varphi =\pm \pi /2$). This occurs because the spherical coordinate system used by the DFS is degenerate at the poles, and measurement near these regions can exert disproportionate influence during optimization, leading to overfitting localized at the poles.

To mitigate these artifacts, we introduce a latitude-dependent weight factor $w_{pole}(\varphi )$ that down-weights measurements near the poles. A common choice is

$$w_{pole}(\varphi )={\cos }^h(|\varphi |)$$

(44)

where $h\ge 1$ (e.g., $h=1$ or $h=2$) is a tuning parameter controlling the strength of the suppression.

The modified cost function $J(\zeta )$ is,

$$J(\zeta )=({\displaystyle \frac{1}{{\displaystyle \sum _{t=1}^{T_b}{\displaystyle \sum _{j=1}^{N_t}{w}_{pole}({\widehat{\varphi }}_{j,t})}}}}{\displaystyle \sum _{t=1}^{T_b}{\displaystyle \sum _{j=1}^{N_t}{w}_{pole}({\widehat{\varphi }}_{j,t})\left|\right|z_{j,t}-h({\widehat{x}}_t,z_{j,t},\zeta ,\widehat{\omega })|{|}_{R_t^{-1}}^2)+{\eta }_{\zeta }\left|\right|\zeta |{|}_2^2}}+P_s(\zeta )$$

(45)

Remark 2.

By multiplying the squared Mahalanobis distance of each measurement by $w_{pole}(\varphi )$, we reduces the contribution of measurements where $\left|\varphi \right|$ is close to $\pi /2$. This is mathematically equivalent to assuming a higher effective measurement noise variance ($R_{j,t}^{*}=R_t/w_{pole}({\widehat{\varphi }}_{j,t})$) near the poles. It prevents the optimizer from overly fitting to potentially unreliable projections in these geometrically sensitive regions. Allowing the regularization terms to enforce a smoother, more plausible shape. The tuning parameter $h$ control how aggressively measurements near the poles are down-weighted.

Improved Smoothness Penalty: A physically meaningful smoothness penalty for the DFS on a sphere penalizes higher spatial frequencies more heavily. The indices $n$ and $m$ in the DFS correspond to the frequencies in the azimuth and elevation directions, respectively. Higher values of $n$ and $m$ represent finer details or oscillations on the surface. A suitable penalty terms $P_s(\zeta )$ is given by:

$$P_s(\zeta )={\eta }_s{\displaystyle \sum _{n=1}^N{\displaystyle \sum _{m=1}^M(n^{\nu }+m^{\nu })}}({({\zeta }_{n,m}^{cc})}^2+{({\zeta }_{n,m}^{ss})}^2+{({\zeta }_{n,m}^{sc})}^2+{({\zeta }_{n,m}^{cs})}^2)$$

(46)

where ${\eta }_s$ is the smoothness regularization strength parameter. The term ($n^{\nu }+m^{\nu }$) acts as a weighting factor that increases with the frequency indices $n$ and $m$. A common is $\nu =2$, making the penalty proportional to ($n^2+m^2$). The sum of squares involves all four coefficient types for the given ($n$, $m$) pair.

(2)

Orientation parameter optimization

Minimize $J(\omega )$ at step $t$, regularizing towards the previous estimate $\widehat{\omega }$:

$$J(\omega )={\displaystyle \frac{1}{N}}{\displaystyle \sum _{j=1}^{N_t}\left|\right|z_{j,t}-h({\widehat{x}}_t,z_{j,t},\widehat{\zeta },\omega )|{|}_{R_t^{-1}}^2+{\eta }_{\omega }}||\omega -\widehat{\omega }{||}_2^2$$

(47)

where ${\widehat{x}}_t,\widehat{\zeta }$ are the current estimates of state and shape. $\widehat{\omega }$ is the estimated orientation parameter from the previous time step. ${\eta }_{\omega }$ is the regularization parameter controlling the penalty for deviating from the previous orientation estimation, promoting temporal smoothness.

(3)

Optimization Constraints

Constraints can be applied during optimization:

 A.

Shape parameter constraints:

${\zeta }_0>\epsilon$ (small positive value)

 B.

Rotation parameter constraints:

angle limit: $\left|\right|\omega |{|}_2\le \pi$.

Iteration of the EM algorithm ceases upon convergence or reaching a limit. Convergence is typically determined when the change in the likelihood function value between successive iterations is sufficiently small. Alternatively, termination occurs if the iteration count reaches a predefined upper bound.

The overall algorithm flow is summarized in Algorithm 1.

Algorithm 1. DFS-ECM

Initial parameters: Measurement set batch $Z$, State batch $\mathrm{X}$, orientation and extent parameters $\Lambda$, Maximum Iterations $\mathrm{I}$

begin   Setup $i=0$ 1       while not converged and $i<\mathrm{I}$ do 2              expectation: 3                  for $t=1:T_b$ do 4                       for $j=1:N_t$ do 5                           Calculate ${\widehat{x}}_{j,t}$ and $P_{j,t}$ according to (28–29) 6                        end for 7                    end for 8                    for $t=T_b:-1:1$ do 9                        Calculate smoothed ${\hat{x}}_{t|T_b}$ and $P_{t|T_b}$ according to (37–38) 10                    end for 11               maximization: 12                   Calculate $\zeta$, $\omega$ according to (45,47) 13               $i++$; 14         end while end begin

4. Simulation Results

Our proposed method, referred to as DFS-ECM in this section, are evaluated in two distinct scenarios. The simulations are run on an Intel Core™ i7-7700 processor at 3.6 GHz with 32 GB of RAM using MATLAB 2021a. Root Mean Square Error (RMSE) and the Intersection-Over-Union (IOU) are used to assess overall performance.

4.1. Evaluation Index

Root-Mean-Square Error (RMSE) is defined below:

$$RMS{E}_c=\sqrt{{\displaystyle \frac{1}{M}}{\displaystyle \sum _{i=1}^M{(C_i-c_i)}^2}}$$

(48)

where $RMS{E}_c$ represents the RMSE of the parameter $c$, $C_i$ represents the true and $c_i$ represents the estimated value.

The shape estimation performance is evaluated by Intersection-Over-Union (IOU). IOU is defined as follows:

$$IOU(v_{true},\widehat{v})={\displaystyle \frac{volume(v_{true}\cap \widehat{v})}{volume(v_{true}\cup \widehat{v})}}$$

(49)

where volumes $v_{true}$ is the true target shape, and $\widehat{v}$ represents the estimate. $IOU(v_{true},\widehat{v})\in [0,1]$, $1$ corresponds to a perfect match, while $0$ indicates no intersection between the true and the estimated extent.

4.2. Scenario I

In this scenario, we test the DFS-ECM algorithm with ellipsoid and cubic target. Every target moves in a linear trajectory with the constant velocity (CV) model, and with a sensor located at the origin. The trajectory of target motion is presented in Figure 1. The initial kinematic state is set to [0 m, 0 m, 0 m, 10 m/s, 10 m/s, 0 m/s]. At each instant, 100 points measurements are originated from random sources which are sample from a uniform distribution defined over the target surface. The sampling time of all algorithms is set to $1 s$. The orientation is represented by the axis angle parameter $\omega =(0,0,0)$.

The process noise standard deviations are set to ${\sigma }_c=0.1$, For the sigma points, a scaling parameter $\alpha =1$, $\kappa =0$, $\beta =2$. For the extent parameter, ${\zeta }_0=1.0$ and ${\zeta }_{2,\dots ,d_{\zeta }}=0$, the covariance matrix is set to $diag(0.1,\dots ,0.1)\in {ℝ}^{d_{\zeta }\times d_{\zeta }}$. The degree of double Fourier series is chosen as $M=N=2$.

Figure 2 illustrates the typical results and tracking trajectories in one Monte Carlo run. Figure 2a,b depict the tracking scene for the ellipsoid, cube, respectively. For enhanced clarity regarding the reconstruction outcomes, Figure 3 illustrates the reconstruction results for 3D shapes obtained without incorporating the weight. A comparison between Figure 2 and Figure 3 reveals that optimizing shape reconstruction with the weight yields superior accuracy.

We evaluated centroid estimation for linearly moving ellipsoidal and cubic targets using 100 Monte Carlo trials (randomizing noise and sample points). Figure 4 shows the centroid RMSE for sliding window lengths $T_b=5$, $10$, and $15$. For the ellipsoid (Figure 4a), $T_b=5$ yields the minimum RMSE; performance worsens for $T_b=10$ and $T_b=15$, suggesting potential overfitting. For the cube (Figure 4b), $T_b=15$ is optimal. Generally, errors decrease over the simulation (indicating stabilization), and the relative performance of different window sizes remains consistent.

Figure 5 illustrates the RMSE values of the velocity estimates with the sliding window length $T_b=5$, $10$, and $15$. This indicates that while there’s a large initial uncertainty in velocity, the estimation becomes highly accurate over time, and the choice of window size (within this range) has minimal impact on the long-term velocity estimation accuracy.

Figure 6 presents the Intersection over Union (IOU) values for the shape estimates, obtained using sliding window lengths of $T_b=5$, $10$, and $15$. For both the ellipsoidal and cubic targets, the analysis indicates that the highest IOU score (representing the best shape estimate accuracy) is achieved with the window length $T_b=15$.

As detailed in

Table 2. Orientation parameters estimation.

Target	Angle	T = 5 (rad)	T = 10 (rad)	T = 15 (rad)
Ellipsoid	${\omega }_x$	0.020	0.030	0.030
	${\omega }_y$	0.080	0.050	0.060
	${\omega }_z$	0.050	0.050	0.050
Cube	${\omega }_x$	0.043	0.042	0.043
	${\omega }_y$	0.004	0.004	0.003
	${\omega }_z$	0.015	0.012	0.016

, the errors associated with the estimated orientataion parameter components remain consistently below 0.1 for both ellipsoidal and cubic target types, indicating a high degree of estimation accuracy.

For comparison, we use a standard random matrix-based extended target tracker [35], henceforce denoted as RM.

Table 3. Performance comparation of two diffenernt algorithm.

Target	Algorithm	Centroid RMSE (m)	Velocity RMSE (m/s)	IOU
Ellipsoid	RM	0.27	0.10	0.90
	DFS-ECM	0.26	0.07	0.96
Cube	RM	0.27	0.10	0.70
	DFS-ECM	0.20	0.07	0.95

summarizes the comparative performance of the RM and DFS-ECM approaches. The results consistently show that the DFS-ECM method outperforms the RM model in terms of centroid estimation accuracy, velocity estimation accuracy, and shape estimation quality (as measured by IOU) across all tested scenarios.

As presented in

Table 4. Performance comparation of different muber of measurements.

Target	Number of Measurement	Centroid RMSE (m)	IOU
Ellipsoid	10	0.29	0.91
	20	0.28	0.92
	40	0.27	0.96
Cube	10	0.30	0.90
	20	0.28	0.92
	40	0.288	0.95

, increasing the number of measurements positively impacts overall tracking performance. Specifically, a higher measurement count leads to a lower Centroid RMSE, indicating improved positioning accuracy. Concurrently, the shape estimation quality, measured by IOU, steadily improves for both the ellipsoid and the cube, confirming that more data yields more accurate reconstructions.

4.3. Scenario II

The performance of the DFS-ECM algorithm is further evaluated using a nonlinear scenario. In this experiment, a cubic target moves along a curved trajectory in three-dimensional space, as depicted in Figure 7. The target’s motion is governed by the constant acceleration (CA) model, which is defined in Equation (6). This simulation utilizes the same DFS model hyperparameters and EM parameters as those employed in Scenario Ⅰ. Within the CA model, the scalar parameter ${\sigma }_a^2$ is set to 0.001. At each instant, 100 measurement points are sampled uniformly from the target’s surface. The orientation is represented by the axis angle parameter $\omega =(0.1,-0.2,0.05)$.

To better illustrate the reconstruction quality, Figure 8 presents magnified views of typical reconstruction examples for the cubic target. These examples demonstrate satisfactory reconstruction performance.

Figure 9 presents the centroid RMSE over time for the cubic target, comparing window lengths $T_b=5$, $10$, and $15$. Optimal performance (lowest RMSE) is achieved with the shortest window,$T_b=5$. Conversely, the larger window lengths ($T_b=10$ and $T_b=15$) yield higher errors, potentially indicating underfitting in this nonlinear scenario. However, all three RMSE curves exhibit convergence towards stable values over time.

The velocity RMSE for the cubic target is presented in Figure 10. Notably, while the larger window sizes ($T_b=10$ and $T_b=15$) show similar performance with lower peak errors and better long-term stability (lower RMSE), the smallest window ($T_b=5$) suffers from a higher peak error (occurring later) and maintains a higher error level beyond approximately 18 s.

Figure 11 plots the volume Intersection over Union (IOU), representing shape estimation accuracy, over time for three different window sizes. Towards the end of the observation period (after 40 s), the performance stabilizes for all window sizes. They converge to similar and relatively high IOU values, approximately in the range of 0.86–0.88. While these results are strong, the accuracy is slightly below that achieved for the cube in the linear scenario. We attribute this minor performance difference to the inherent challenges of the nonlinear model. The nonlinearity likely introduces small errors into the centroid position estimate, which subsequently impacts the overall accuracy of the shape reconstruction.

The performance of orientation parameter estimation is detailed in Figure 12. The error dynamics for individual rotation components within a single Monte Carlo run are illustrated in Figure 12a–c. The overall accuracy, depicted by the average Root Mean Square Error (RMSE) across all Monte Carlo runs, is shown in Figure 12d. The low error values demonstrate the algorithm’s capability for accurate angle estimation.

Figure 13 illustrates the computational cost, specifically the average processing time per step required by the algorithm, as a function of different window lengths ($T_b=5$, $10$, and $15$). A clear positive correlation is evident: longer window lengths necessitate incNreased computation time per step.

5. Conclusions

This paper proposed a method for joint 3D extended object tracking with unknown but fixed shape in the framework of Expectation Conditional Maximization (ECM). By introducing a shape representation based on a Double Fourier Series (DFS) radial function and utilizing the axis-angle representation for orientation, this method enables the direct estimation of target shape and orientation parameters, overcoming the reliance on prior shape evolution models. Leveraging the ECM algorithm, kinematic state inference is performed using an Unscented Kalman Filter (UKF) in the E-step, while shape and orientation parameters are estimated via minimization of regularized cost functions in the M-step. This ultimately achieves the joint, robust, and smooth estimation of kinematics, shape, and orientation. The methods provide a full expression for the target’s shape, this detailed information can be used in 3D applications such as navigation, identification and classification. Future work will focus on adaptive tuning and joint estimation schemes to further advance the capability of 3D target tracking.