TERRAIN REFERENCED NAVIGATION SYSTEM

A ground vehicle navigation system comprising: an inertial navigation system arranged to output an orientation estimate of the ground vehicle and a first position estimate of the ground vehicle; a terrain map comprising terrain data; a terrain gradient based navigation unit arranged to output a second position estimate of the ground vehicle based on a comparison between the orientation estimate and terrain gradient data extracted from the terrain map; and an iterative algorithm unit arranged to determine a system error state in each iteration; wherein in each iteration the iterative algorithm unit is arranged to: receive the first position estimate and the second position estimate; and update the system error state for the next iteration based on the system error state, the first position estimate and the second position estimate. The iterative algorithm unit may then apply the updated system error state to the INS measurements in order to calculate a more accurate, current estimate of the position of the ground vehicle. In most circumstances, ground vehicles have all wheels/tracks/etc. in contact with the ground. This being the case, the orientation of a ground vehicle at any given time is indicative of the gradient of the ground beneath the vehicle at that time (which is not the case for aircraft). This provides a possible navigation solution: the orientation of a vehicle, as measured by an IMU, can be compared instantaneously, or over time with known terrain gradient data in order to provide an estimate of the vehicle's position.