Kalman Tracking

Jan 22, 2015

In an earlier blog we demonstrated a Maximum Likelihood (MLE) method to model the accuracy of an electromagnetic sensor network against a non-maneuvering mobile emitter. In this model, we apply a Kalman filter to reduce the error of the Maximum Likelihood (MLE) method and test it against a maneuvering target.

The difference equation for the movement of the platform is [Bar-Shalom]

xk+1 = Axk + Buk + vk

with the measurement (z)

zk = Hxk + wk

We assume that the platform does not maneuver between its waypoints or "positions of intended movement" (PIM) that are shown as red circles in the graphic above. The sensor sampling rate is on the order of minutes so course changes do not need to be modeled, which allows us to set u and v to zero in the ship motion model for x.

In the mesurement model for z, we set wk equal to the standard deviation of the kth sensor measurement. This value is updated on each sensor detection and the value for zk is used in the filter to produce the estimated position for .

We assume that the ship motion is piece-wise linear between maneuvers at each PIM. These maneuvers can be speed changes, course changes or a combination of the two. As long as the maneuvers are not significant, the Kalman filter can continue to provide good position and speed estimates.

We set a threshold for the change in the measurement residual (ν)

νk+1 = zk+1 - Hk+1k+1|k

when the filter is stable and the prediction covariance is low. If ν exceeds this threshold, the Prediction covariance is reset to its initial value to allow the filter to stabilize to the new course and speed.

In the animation, above, the left screen shows an overview the ship's path as it makes way to each PIM. The sensor netwok, with its three sensors (red diamonds) are shown, and when they are in line-of-site range of the ship's emitter, their line-of-bearing is shown with its variance. When two or more sensors detect the emitter, a fix can be calculated and its error ellipse is displayed. The black line is the ship's path and the blue dots are the Kalman filter's estimated positions (). The right screen shows a perspective view of the ship, the sensor error ellipse (red) and the Kalman position estimate with its error in blue.

As the ship moves from it's initial position, the Kalman filter, given the sensor network error values, finally stabilizes after about 30 samples (one sample per minute). Once stable its position estimate is well within the sensor network's error ellipse as is shown on the right. At the second PIM, the ship changes course from 023°T to 090°T. The filter detects the significant maneuver, resets and restablizes on the ship's new course and speed.

When the ship moves outside of S2's range, the error ellipse elongates and drives the Kalman's filter position estimate to the North. Once the sensor network can no longer track the ship, the Kalman estimate is used to predict the ship's position. Even with the northward drift, after 8 nautical miles (at 10 knots) the position estimate is only .6 nm from the ship's actual position.


Bar-Shalom, Yaakov, Estimation with Applications To Tracking and Navigation, John Wiley & Sons, Ltd., New York, NY, 2001.

< Previous Next >