An iterated-extended Kalman filter algorithm for tracking surface and sub-surface targets

Coastal System Station has developed, implemented and tested an algorithm for tracking surface and sub-surface targets. The algorithm accepts various combinations of range, elevation, bearing, speed and Doppler measurements. At a minimum, the tracker requires bearing and elevation measurements; additional measurements will improve filter performance but are not required. The tracker consists of an iterated-extended Kalman filter with measurement/track matching logic. Special attention has been given to deriving good initial estimates of target position, initial estimates of the error covariance matrix, and correcting filter divergence with a line search. The improved initial error covariance estimate reduces filter transients and improves filter accuracy. Filter stability problems uncovered during testing were also corrected by adding measurement de-weighting to spurious elevation and bearing measurements. Problems associated with missing range measurements have been solved by implementing a multi-depth mode Kalman filter which allows the Kalman filter to determine a unique x, y, z position solution for target track given only elevation and bearing angle measurements. The multi-depth mode works by creating a family of filters for each target; each filter in the family restricts the target´s tracked depth to within prescribed limits. The measurement/track matching logic computes the normalized residual error inner product test statistic. This test statistic has a chi-squared distribution and is used to statistically compare new measurements to all existing target tracks. The tracking algorithm has been tested with several at-sea data sets consisting of elevation and bearing measurements only. In most cases, the algorithm successfully localizes the target positions to within the prescribed angular (elevation and bearing) errors.