Trajectory Tracking Using Cumulative Risk–Sensitive Finite Impulse Response Filters

TL;DR

This paper introduces a new filter for trajectory tracking that improves accuracy and robustness in autonomous systems.

Contribution

A novel cumulative risk–sensitive FIR filter is proposed for robust trajectory tracking in uncertain environments.

Findings

The proposed filter significantly reduces average tracking error compared to existing methods.

It demonstrates superior robustness in complex and dynamic environments.

The algorithm is validated through comprehensive vehicle trajectory tracking experiments.

Abstract

Trajectory tracking is a critical component of autonomous driving and robotic motion control. This paper proposes a novel robust finite impulse response (FIR) filter for linear time-invariant systems, aimed at enhancing the accuracy and robustness of trajectory tracking. To address the limitations of infinite impulse response (IIR) filters in complex environments, we integrate a cumulative risk–sensitive criterion with an FIR structure. The proposed filter effectively mitigates model mismatches and temporary modeling uncertainties, making it highly suitable for trajectory tracking in dynamic and uncertain environments. To validate its performance, a comprehensive vehicle trajectory tracking experiment is conducted. The experimental results demonstrate that, compared to the Kalman filter (KF), risk–sensitive filter (RSF), and unbiased FIR (UFIR) filter, the proposed algorithm significantly reduces the average tracking error and exhibits superior robustness in complex scenarios. This work provides a new and effective solution for trajectory tracking applications, with broad potential for practical implementation.

Figures

Full text

1. Introduction

With the continuous advancements in automatic control, sensor technology, and communication systems, trajectory tracking has become increasingly prevalent in intelligent systems, including autonomous driving and robotic motion. Trajectory data are generally collected by global navigation satellite system (GNSS) devices. During the acquisition process, issues such as information latency and missing data may occur [1]. These problems arise due to various factors, including high maneuverability of the vehicle, network failures, and measurement errors [2,3]. State estimation serves as a fundamental approach to addressing the challenges inherent in trajectory tracking [4].

State estimation refers to a method of estimating the state of a system based on its measured outputs. Filtering is a form of state estimation. In this context, filtering does not refer to frequency-domain filtering but rather to time-domain filtering based on state-space equations. It aims to estimate the system’s internal state using noisy measurements while minimizing estimation errors, ensuring accuracy and robustness in dynamic systems. In previous research, numerous state estimation methods have been proposed. Among these, the most renowned is the Kalman filter (KF) [5]. The KF addresses the optimal estimation problem for linear time-invariant systems with known parameters, where both system noise and observation noise are Gaussian. Due to its simplicity and computational efficiency, KF has been widely applied. By employing a straightforward target model, KF can be designed to optimally track maneuvering vehicles, including aircraft, ships, and submarines, within tactical systems [6]. When the system is nonlinear, the performance of the KF significantly deteriorates. To address nonlinear problems, the extended KF (EKF) for nonlinear systems and the unscented KF (UKF) for highly nonlinear systems are derived [7,8]. An EKF using forward-looking infrared sensor data enhances target tracking by leveraging size, shape, motion, and atmospheric jitter, excelling in close-range air-to-air missile tracking despite clutter [9]. Research in [10] uses the EKF to estimate the state of a moving target detected by a single unmanned aerial vehicle and predicts the optimal trajectory using the EKF model, demonstrating that prediction accuracy significantly improves when the model accurately reflects the target dynamics. Some studies adopt hybrid methods combining the KF with other techniques to achieve better and faster tracking results. Research in [11] uses a hybrid method of camshift method as the main tracking technique and KF for prediction and correction.

However, the KF and its extended variants belong to the category of infinite impulse response (IIR) estimation, which inherently suffers from several limitations. These include limited robustness against model mismatches, increased sensitivity to temporary modeling uncertainties, and strong dependence on initial conditions and noise statistics [5,12,13,14]. These drawbacks highlight the need for alternative estimators, such as finite impulse response (FIR) estimators, which rely on a finite set of recent measurements for state estimation. FIR estimators offer improved robustness against temporary modeling uncertainties and ensure BIBO stability, albeit at the cost of higher computational complexity—a challenge mitigated by advancements in computational technology. The robustness of the KF and unbiased FIR (UFIR) filter has been evaluated under various conditions in [15], demonstrating that the UFIR filter outperforms the KF in scenarios involving inaccuracies in noise statistics, model mismatches, and temporary uncertainties. Research in [16] presents the extended least square UFIR filter, a nonlinear state estimator that provides consistent performance under uncertain noise modeling, outperforming the traditional EKF and particle filter, and it is suitable for tracking applications using the constant velocity model. In [17], experimental results further confirm that the UFIR filter for vehicle tracking over wireless sensor networks with delayed and missing data surpasses the performance of the KF and $[eqn]$ filter. In [18], the research proposes a variational Bayesian-based scheme for monitoring sensor noise covariance in nonlinear state space, achieving better estimation accuracy, with its effectiveness verified through localization and rotary flexible joint experiments. In [19], FIR filters outperform the KF in object tracking under high noise conditions. An iterative FIR filter for nonlinear systems is proposed, which improves target tracking by using unscented transformation and variational Bayesian for better accuracy and robustness with a reduced computational load [20]. The research in [21] presents a novel state estimation method that combines the UFIR filter and KF using transfer learning to handle parameter uncertainties and noise statistics, improving robustness and performance in target tracking.

In summary, robust filters have demonstrated significant potential in addressing trajectory tracking challenges. In [22], an FIR filter integrating the instantaneous risk–sensitive criterion with the FIR estimator is introduced. This filter, termed the instantaneous risk–sensitive FIR (IRSFIR) filter, exhibits superior robustness compared to conventional FIR filters. Building upon the risk–sensitive yet distinct criterion from the IRSFIR filter, we introduce a cumulative risk–sensitive cost function that accounts for the total estimation error accumulated from the initial time to the present within the estimation horizon. The main contributions of this paper are summarized as follows:

1. This paper introduces a new robust FIR filter based on a cumulative risk–sensitive cost function, which accounts for the total estimation error accumulated from the initial time to the present moment within the estimation horizon by reformulating the complex exponential cost function into a tractable max–min optimization problem.
2. To validate the proposed approach, a trajectory tracking experiment is conducted to assess the performance of four filters. The experimental results demonstrate that the proposed cumulative risk–sensitive FIR (CRSFIR) filter outperforms the KF, risk–sensitive filter (RSF), and UFIR filter in terms of trajectory tracking accuracy.

The remainder of this paper is structured as follows: Section 2 derives the extended state-space model and formulates the problem. Section 3 provides a detailed derivation of the CRSFIR filter. Section 4 presents the experimental results and analysis. Finally, Section 5 concludes this paper.

2. Extend State–Space Model and Problem Formulation

Consider a linear discrete time–invariant system, which is represented as

$$[eqn]$$ $$[eqn]$$

where k is the discrete time index, $[eqn]$ is the state vector, $[eqn]$ is the input vector, $[eqn]$ is the measurement vector, and $[eqn]$ , $[eqn]$ , $[eqn]$ , and $[eqn]$ are the state transition matrix, input matrix, noise matrix, and observation matrix, respectively. The system noise $[eqn]$ and the measurement noise $[eqn]$ are zero–mean white Gaussian and mutually uncorrelated. The covariances of $[eqn]$ and $[eqn]$ are denoted by Q and R, respectively.

To derive the CRSFIR filter, the state–space equations (Equations (1) and (2)) are extended over a horizon $[eqn]$ consisting of N points, where $[eqn]$ . The extended models are represented as

$$[eqn]$$ $$[eqn]$$

where the extended vectors are specified as

$$[eqn]$$ $$[eqn]$$ $$[eqn]$$ $$[eqn]$$ $$[eqn]$$

where the symbol ⊤ represents the transpose operation. The extended matrices are represented by

$$[eqn]$$ $$[eqn]$$ $$[eqn]$$ $$[eqn]$$ $$[eqn]$$

in which $[eqn]$ . The symbol $[eqn]$ can be represented as $[eqn]$ by replacing E with B, and the symbol I is the identity matrix. The noise term $[eqn]$ in (4) can be shown to be zero–mean with the covariance $[eqn]$ , which is represented by

$$[eqn]$$

where $[eqn]$ and $[eqn]$ . The current state $[eqn]$ can be represented in terms of the state $[eqn]$ , the input vectors $[eqn]$ , and the noise vectors $[eqn]$ as

$$[eqn]$$

where $[eqn]$ and $[eqn]$ are the last row vectors of $[eqn]$ and $[eqn]$ , respectively.

In [22], the instantaneous risk–sensitive criterion is defined as

$$[eqn]$$

where $[eqn]$ represents the risk–sensitive parameter, the symbol $[eqn]$ denotes the expectation operator, and $[eqn]$ corresponds to the state estimate of the IRSFIR filter, derived from measurements over the horizon $[eqn]$ . Building upon (17), we introduce a cumulative formulation of the risk–sensitive criterion. In contrast to the instantaneous risk–sensitive criterion, the cumulative risk–sensitive criterion is defined as follows:

$$[eqn]$$ $$[eqn]$$

where $[eqn]$ . Here, the index s denotes the starting point of error accumulation, the horizon length of $[eqn]$ is $[eqn]$ , $[eqn]$ represents the state estimate of the CRSFIR filter at time k, and $[eqn]$ corresponds to the state estimate of the IRSFIR filter at time l, computed using measurements collected over the interval $[eqn]$ . The notation $[eqn]$ indicates the same term as its preceding counterpart.

The primary distinction between the instantaneous and cumulative criteria lies in the structure of the exponential term. Specifically, the instantaneous criterion incorporates only the error at the current time step within its exponential term. In contrast, the cumulative criterion considers the aggregation of errors over the interval $[eqn]$ . This cumulative approach depends on prior estimates derived from the interval $[eqn]$ , which are computed using the IRSFIR filter. As demonstrated in (18), measurements within $[eqn]$ are directly employed, while those within $[eqn]$ are indirectly integrated through IRSFIR filter estimates. Consequently, the final estimate $[eqn]$ effectively utilizes all measurements across the range $[eqn]$ .

The problem can be formulated as follows: Given the models in (1) and (2), develop a new FIR filter by minimizing the criterion defined in (18).

3. Cumulative Risk–Sensitive FIR Filter

In (18), the constant term $[eqn]$ does not influence the extremum–seeking process and can thus be omitted. Given that the exponential function is monotonically increasing, its omission does not alter the extremum–seeking behavior. Consequently, the cost function (18) can be simplified as

$$[eqn]$$

Equation (19) is mathematically equivalent to (18) when optimizing for the extremum. In (18), the accumulation of errors initiates at time s. Consequently, in the extended state–space equations, the subscript m should be substituted with s, and the interval length adjusted from N to $[eqn]$ . Here, E denotes the expectation operator, and (19) can be reformulated as an integral operation. The expectation of the exponential quadratic cost function (19) can be evaluated using the joint probability density functions of $[eqn]$ , $[eqn]$ , and $[eqn]$ . Accordingly, the joint probability density function $[eqn]$ is represented as

$$[eqn]$$

where $[eqn]$ represents the constant term and $[eqn]$ is given by

$$[eqn]$$

where $[eqn]$ and $[eqn]$ are the mean and covariance of $[eqn]$ , and they can be computed by the least mean square criterion [23]. Specifically, $[eqn]$ and $[eqn]$ can be represented as

$$[eqn]$$ $$[eqn]$$

The simplified cost function can be computed as

$$[eqn]$$

where $[eqn]$ . To simplify (24), a useful mathematical result can be applied [24]. Specifically, the integral

$$[eqn]$$

is finite if and only if $[eqn]$ attains a minimum over the domain $[eqn]$ . Under this condition, the value of the integral is proportional to

$$[eqn]$$

Meanwhile, based on (4), $[eqn]$ can be expressed in terms of $[eqn]$ and $[eqn]$ . Consequently, the criterion (24) can be further reduced to

$$[eqn]$$

Based on (24) and (25), the minimization of the criterion (19) can ultimately be simplified to

$$[eqn]$$

where $[eqn]$ can be represented as

$$[eqn]$$

By substituting (22) and (23) into (27), $[eqn]$ can be represented in a compact form as

$$[eqn]$$

where $[eqn]$ and $[eqn]$ are represented, respectively, by

$$[eqn]$$ $$[eqn]$$

Specific expressions for each block of the matrix $[eqn]$ can be found in Appendix A.

According to (29), we define a, b, and c as

$$[eqn]$$

Then, the blocks of $[eqn]$ can be decomposed into the following submatrices:

$$[eqn]$$

where the symbol ⋆ represents the symmetric counterpart of the corresponding block within the matrix structure. According to (31) and (32), Equation (28) can be rewritten in a simpler form as

$$[eqn]$$

where $[eqn]$ , $[eqn]$ , and $[eqn]$ are the transpose matrices of $[eqn]$ , $[eqn]$ , and $[eqn]$ , respectively. To solve the optimization problem (26), the following useful result can be applied [24]. When the inequality conditions

$$[eqn]$$

are satisfied, the optimal values of a and b, which minimize $[eqn]$ with respect to a and maximize $[eqn]$ with respect to b, exist and are represented by

$$[eqn]$$

Then, the solution of (26) is given as

$$[eqn]$$

where $[eqn]$ is a coefficient matrix designed to extract the $[eqn]$ component from Equation (34). Equation (35) indicates that the computation of $[eqn]$ requires the prior calculation of $[eqn]$ . The complete CRSFIR filter algorithm is outlined in Algorithm 1. Algorithm 1 CRSFIR filter algorithm.

As outlined in Algorithm 1, the CRSFIR filter operates through two primary phases. Initially, the estimates produced by the IRSFIR filter prior to time k are aggregated. Subsequently, these estimates are incorporated into the solving formula (35) to derive the CRSFIR filter estimate at time k. Although the inclusion of IRSFIR filter computations elevates the overall computational complexity of the algorithm, the CRSFIR filter substantially enhances estimation performance by considering accumulated estimation errors over a defined time interval.

4. GNSS–Based Tracking of a Moving Vehicle

To evaluate the performance of the CRSFIR filter in trajectory tracking, we conducted an experiment focused on tracking the trajectory of a moving vehicle. The experiment comprises two distinct trajectory segments. The first segment features fewer turns and primarily consists of straight–line motion, with minimal surrounding buildings. In contrast, the second segment involves more frequent turns and a higher density of surrounding buildings, creating a more complex environment. A GNSS processing unit is responsible for parsing the transmitted satellite signals to obtain precise location data. The sampling interval $[eqn]$ during the experiment is set to 1 s. The raw data collected by the GNSS are initially obtained from the World Geodetic System 1984 (WGS84) coordinate system. To facilitate the evaluation of the filtering algorithm’s performance, WGS84 coordinates are converted to the Earth–centered, Earth–fixed (ECEF) coordinate system. The ECEF is a Cartesian system with three axes, $[eqn]$ , $[eqn]$ , and $[eqn]$ , measured in meters. The experiment uses a simplified trajectory tracking model, with the coefficient matrices being represented as

$$[eqn]$$

and $[eqn]$ . The state matrix is represented as $[eqn]$ . The contents of the state matrix represent the positions and velocities in the $[eqn]$ , $[eqn]$ , and $[eqn]$ directions, respectively. The system noise covariance is given by $[eqn]$ , and the measurement noise covariance is given by $[eqn]$ . To validate the performance of the CRSFIR filter, which is designed for linear systems, it is compared with other linear filtering methods. The KF and UFIR filter are selected as representative examples of IIR and FIR approaches, respectively. Additionally, the RSF, known for its robustness, is included in the comparison to further evaluate the effectiveness of the CRSFIR filter under varying conditions. The risk–sensitive parameter $[eqn]$ . The horizon length $[eqn]$ and $[eqn]$ . The choice of the horizon length is not fixed and can also take other values. However, since the horizon length directly affects whether the filter constraints can be satisfied, extensive simulation and testing are required to determine a suitable value. Figure 1 shows the true trajectory of the first path segment in the experiment. The experiment starts at the location marked by the red square and ends at the location marked by the red circle. In Figure 2a, the vehicle trajectories generated by the four filters are shown. It is clearly observed that the tracking performance of all four filters is satisfactory. From a global perspective, there is no significant superiority or inferiority among the four filters. By zooming in on two turning points in the figure, it becomes clear that the CRSFIR filter demonstrates superior tracking performance. In Figure 2b, the figure shows the estimation error curves generated by the CRSFIR filter, KF, RSF, and UFIR filter along the $[eqn]$ , $[eqn]$ , and $[eqn]$ axes. Table 1 lists the root mean square error (RMSE) of the four filters in the process. For the first path segment, the RMSE along the $[eqn]$ -axis shows minimal differences among the four filters. The CRSFIR filter outperforms both the KF and UFIR filter, ranking second only to the RSF. In the $[eqn]$ and $[eqn]$ axes, the CRSFIR filter exhibits markedly better performance than the other three filters. Overall, the CRSFIR filter exhibits the best performance in terms of trajectory tracking accuracy. The computation times for the four filters are 3.3620 s (CRSFIR), 0.0034 s (KF), 0.0110 s (RSF), and 0.0125 s (UFIR), respectively. As expected, the computation time of the CRSFIR filter is the longest.

Figure 3 shows the true trajectory of the second path segment in the experiment. As depicted in Figure 3, the second path segment features more turns and is surrounded by more buildings compared to the first segment, making it more challenging for trajectory tracking. Such an environment is typical in real–world scenarios, making it well suited for evaluating the robustness of the CRSFIR filter in trajectory tracking. In Figure 4a, the vehicle trajectories generated by the four filters are displayed, with magnified views at four turning points. It is evident that the CRSFIR filter estimate aligns more closely with the true trajectory compared to the other filters. Figure 4b presents the estimation errors of the four filters. The error curves of the four filters exhibit minor differences, but the CRSFIR filter clearly converges faster in certain regions, highlighting the advantage of FIR filters over IIR filters. Table 2 lists the RMSE values of the four filters in the second path segment. As shown in Table 2, the RMSE of the CRSFIR filter is significantly lower than that of the KF, RSF, and UFIR filter in all three directions. This indicates that the CRSFIR filter exhibits superior robustness in trajectory tracking under complex environments. The computation times of the four filters in the second path segment are 9.1711 s (CRSFIR), 0.0047 s (KF), 0.0174 s (RSF), and 0.0193 s (UFIR), respectively. This result is similar to that in the first path segment, since CRSFIR initially calculates $[eqn]$ , which consumes a significant amount of time.

Based on the two trajectory tracking examples mentioned above, it can be concluded that the CRSFIR filter outperforms the KF, RSF, and UFIR filter in terms of tracking accuracy, while also demonstrating superior robustness.

5. Conclusions

This paper proposes a novel robust FIR filter based on a cumulative risk–sensitive criterion, distinguishing it from conventional filters that rely on instantaneous criteria. The cumulative criterion considers all estimation errors from the initial time to the current time within the estimation horizon, providing a more comprehensive evaluation of performance. By integrating the risk–sensitive criterion into the FIR estimation framework, the proposed filter achieves enhanced robustness compared to traditional IIR and FIR filters. To validate its performance in trajectory tracking, a vehicle trajectory tracking experiment is conducted under realistic environmental conditions. Experimental results demonstrate that the CRSFIR filter outperforms the KF, RSF, and UFIR filter in terms of tracking accuracy. However, its significantly longer computation time limits its applicability to offline estimation rather than real–time applications. There are several aspects of the filter algorithm that can be further improved, including enhancing computational efficiency, developing adaptive algorithms, conducting a more comprehensive convergence analysis, and extending its application to nonlinear systems.