Computerized method for generating low-bias estimates of position of a vehicle from sensor data

Abstract

A computerized method generates unbiased estimates of parameters such as position, velocity, and acceleration of a vehicle. The method assigns initial vehicle states. Unbiased vehicle state estimates are obtained using a model incorporating a unique parameterization of the vehicle motion and state dynamics, and sensor registration bias states. Sensor measurements of the vehicle are used to iteratively update the vehicle states and covariances. Previous vehicle state and covariance, are propagated to produce current state and covariance estimates. If needed, a new vehicle model is adopted. Sensor registration biases are estimated to generate corrected measurement information. A measurement matrix and a measurement residual is determined. The Kalman gain matrix is determined, and used with measurement residual for state update. A new current state is generated. State covariance update is determined the same way, and used to generate a new current state covariance. These steps are repeated, producing unbiased vehicle parameter estimates.