Adaptive Target Tracking Using Retrospective Cost Input Estimation

TL;DR

This work addresses robust causal differentiation for target tracking under unknown and time-varying noise by extending retrospective cost input estimation (RCIE) with an online adaptive Kalman filter that updates the process-noise covariance $\widetilde{V}_k$ to minimize the innovations discrepancy. The approach achieves performance close to the best fixed $\widetilde{V}_k$ across single and double differentiation while removing the need for manual tuning, demonstrated through RCIE-based differentiation and a CarSim collision-avoidance scenario. The key contribution is an online, tuning-free framework that jointly estimates input and state for reliable velocity and acceleration estimates in autonomous systems, with practical impact on real-time target tracking under changing sensor conditions. The results indicate substantial robustness gains in dynamic environments, paving the way for adaptive noise-covariance strategies and extensions to bias handling and acceleration estimation in autonomous sensing pipelines.

Abstract

Target tracking of surrounding vehicles is essential for collision avoidance in autonomous vehicles. Our approach to target tracking is based on causal numerical differentiation on relative position data to estimate relative velocity and acceleration. Causal numerical differentiation is useful for a wide range of estimation and control problems with application to robotics and autonomous systems. The present paper extends prior work on causal numerical differentiation based on retrospective cost input estimation (RCIE). Since the variance of the input-estimation error and its correlation with the state-estimation error (the sum of the variance and the correlation is denoted as $\widetilde{V}$) used in the Kalman filter update are unknown, the present paper considers an adaptive discrete-time Kalman filter, where $\widetilde{V}_k$ is updated at each time step $k$ to minimize the difference between the sample variance of the innovations and the variance of the innovations given by the Kalman filter. The performance of this approach is shown to reach the performance of numerical differentiation based on RCIE with the best possible fixed value of $\widetilde{V}_k$. The proposed method thus eliminates the need to determine the best possible fixed value for $\widetilde{V}_k$. Finally, RCIE with an adaptive Kalman filter is applied to target tracking of a vehicle using simulated data from CarSim.

Figures (14)

• Figure 1: Performance metrics $\widetilde{S}_{k_{\rm f}}$ and $\rho_{k_{\rm f}}$ versus the logarithm of $\widetilde{V}$ for single differentiation, where $k_{\rm f} = 10^4$ steps. The metrics are computed for $100$ values of $\widetilde{V}$ in the range $[10^{-6},10^2]$. However, for clarity, the plots show values of $\widetilde{V}$ from $10^{-4}$ to $0.1$. (a) plots $\rho_{k_{\rm f}}$ versus $\log_{10} \widetilde{V}$. $\rho_{k_{\rm f}}$ is minimized at $\widetilde{V}= \widetilde{V}_{\rm sdmr} = 0.0077$. (b) plots $\widetilde{S}_{k_{\rm f}}$ versus $\log_{10} \widetilde{V}$. $\widetilde{S}_{k}$ is minimized at $\widetilde{V}= \widetilde{V}_{\rm sdms} = 0.0110$.
• Figure 2: Performance metrics versus the logarithm of $\widetilde{V}$ for double differentiation, where $k_{\rm f} = 10^4$ steps. The metrics are computed for $100$ values of $\widetilde{V}$ in the range $[10^{-6},10^{-2}]$. However, for clarity, the plots show values of $\widetilde{V}$ from $10^{-6}$ to $10^{-2.8}$. (a) plots $\rho_{k_{\rm f}}$ versus $\log_{10} \widetilde{V}$. $\rho_{k_{\rm f}}$ is minimized at $\widetilde{V}= \widetilde{V}_{\rm sdmr} = 1.5199 \times 10^{-4}$. (b) plots $\widetilde{S}_{k_{\rm f}}$ versus $\log_{10} \widetilde{V}$. $\widetilde{S}_{k}$ is minimized at $\widetilde{V}= \widetilde{V}_{\rm ddms} = 7.9248 \times 10^{-5}$.
• Figure 3: Block diagram of RCIE with adaptive Kalman filter. The adaptive Kalman filter consists of the forecast subsystem, the data-assimilation subsystem, and adaption of $\widetilde{V}_k$. The innovation $z$ and the output $\hat{d}$ of the adaptive input-estimation subsystem are the inputs to the adaptive Kalman filter.
• Figure 4: Example \ref{['adapt_vtilde_sd']}: Adaptation of $\widetilde{V}_k$ for single differentiation. The first derivative $y^{(1)}_k$ and its estimate $\widehat{y}^{(1)}_k$ versus $k$. (a) $\widehat{y}^{(1)}$ follows $y^{(1)}$ after about 15 steps. (b) $\widehat{y}^{(1)}$ and $y^{(1)}$ at steady state.
• Figure 5: Example \ref{['adapt_vtilde_sd']}: Adaptation of $\widetilde{V}_k$ for single differentiation. (a) plots $\widetilde{S}_k$ versus $k$. (b) plots $\widetilde{V}_{{\rm opt},k}$ versus $k$.

Theorems & Definitions (3)

• Example VI.1
• Example VI.2
• Example VI.3