Method of Kalman filtering for estimating the position and velocity of a tracked object

In a tracking system having a sensor mounted on a vehicle for sensing the presence of an object, a method of Kalman filtering for estimating the position and velocity of the object, comprising the steps of:

• a. providing the tracking system with initial estimates of velocity error state information;
• b. initializing a Kalman filter with a priori position and velocity error state information;
• c. receiving a first measurement from the sensor, the first sensor measurement including relative cross-range position information about the object;
• d. constructing the Kalman filter's covariance matrix to reflect the a priori position error state information and the relative cross-range position information from the first sensor measurement;
• and
• e. correcting the initial position estimate in accordance with the first sensor measurement;
• f. deriving a measurement frame of reference for the vehicle and object wherein the measurement frame is defined as having one axis pointing towards the estimated relative position of the object;
• g. receiving sensor measurements to develop at least a measured line-of-sight vector to the object;
• h. transforming the sensor measurements into the measurement frame;
• and
• i. updating the Kalman filter with the transformed sensor measurements for estimating the position and velocity of the tracked object.

In a tracking system having a sensor mounted on a vehicle for sensing the presence of an object, a method of Kalman filtering for estimating the position and velocity of the object, comprising the steps of:

• a. providing the tracking system with initial estimates of position and velocity of the object;
• b. initializing a Kalman filter with a priori position and velocity error state information;
• c. receiving a first measurement from the sensor to establish the measured line-of--sight vector to the object the first sensor measurement including relative cross-range position information about the object;
• d. constructing a frame of reference having one axis along the measured line-of-sight vector;
• e. constructing the Kalman filter's covariance matrix relative to the constructed frame of reference to reflect the a priori position error state information and the relative cross-range position information from the first sensor measurement;
• f. analytically rotating the covariance matrix into the Kalman filter's inertial computational frame;
• g. correcting the initial position estimate in accordance with the first sensor measurement;
• h. deriving a measurement frame of reference for the vehicle and object wherein the measurement frame is defined as having one axis pointing towards the estimated relative position of the object;
• i. receiving sensor measurements to develop at least a measured line of sight vector to the object;
• j. transforming the sensor measurements into the measurement frame;
• and
• k. updating the Kalman filter with the transformed sensor measurements for estimating the position and velocity of the tracked object.

In a tracking system having a sensor mounted on a vehicle for sensing the presence of an object, a method of Kalman filtering for estimating the position and velocity of the object, comprising the steps of:

• a. referencing the Lakman filter's state vector and covariance matrix in an inertial computational frame of reference;
• b. providing the tracking system with initial estimates of position and velocity of the object;
• c. initializing the Kalman filter with at least position and velocity error states;
• d. deriving a measurement frame of reference for the vehicle and object wherein the measurement frame is defined as having one axis pointing towards the estimated relative position of the object;
• e. receiving sensor measurements to develop at least a measured line-of-sight vector to the object;
• f. transforming the sensor measurements into the measurement frame;
• g. updating the Kalman filter with the tansformed sensor measurements and computing the Kalman filter's measurement magic using a transformation matrix to relate the measurement frame to the inertial computational frame;
• h. providing trajectory models of the vehicle and object;
• i. correcting the trajectory models based on the estimated position and velocity error states;
• j. resetting the position and velocity error states to zero;
• and
• propagating the trajectory models and the filter's covariance matrix to the next time step.