Position probability density function filter to determine real-time measurement errors for map based, vision navigation systems

Abstract

A navigation system for a vehicle comprises onboard sensors including a vision sensor, and an onboard map database of terrain maps. An onboard processer, coupled to the sensors and map database, includes a position PDF filter, which performs a method comprising: receiving image data from the vision sensor corresponding to terrain images captured by the vision sensor of a given area; receiving map data from the map database corresponding to a terrain map of the area; generating a first PDF of image features in the image data; generating a second PDF of map features in the map data; generating a measurement vector PDF by a convolution of the first PDF and second PDF; estimating a position vector PDF using a non-linear filter that receives the measurement vector PDF; and generating statistics from the estimated position vector PDF that include real-time measurement errors of position and angular orientation of the vehicle.