Field of the invention

The invention relates to systems for correcting inertially derived navigation information. Specifically, the invention relates to, but is not limited to, the correction of heading information in inertial positioning systems. Additionally, the invention relates to, but is not limited to, the correction of heading information in indoor positioning systems.

Background to the invention Pedestrian positioning is a difficult challenge since users typically spend most of their time indoors. Global navigation Satellite System (GNSS) signals, such as those from the Global Positioning System (GPS), are typically unavailable inside buildings due to the heavy attenuation caused by building materials, or are inaccurate due to multipath. This fundamental problem with GNSS signals means that it is necessary to look to other technologies to either augment or replace GNSS signals when navigating indoors. Furthermore, the construction of typical buildings with a lot of small rooms makes adding infrastructure systems such as UWB and RFID a difficult and potentially expensive task which is unlikely to be practical when navigating over wide areas. Indoor pedestrian navigation using inertial measurement units (IMUs) and a computer processor (collectively termed an Inertial Navigation System or INS) has the advantage that no infrastructure is required and, once initialised, the system is totally self-contained. The sensors (normally three accelerometers and three gyroscopes) have the potential to be small, low power, accurate and inexpensive due to the advances in Micro-electromechanical Sensors (MEMS) technology. However, the performance of low cost MEMS technology is still relatively poor and as a result, their use for positioning applications is relatively limited, unless frequent measurements from external sensors or technologies are available. One recent idea that has advanced the use of MEMS IMUs for pedestrian navigation is the notion of attaching the IMU to a pedestrian's shoe. This results in the substantial advantage that the foot is briefly stationary while it is on the ground. During this period, Zero Velocity Updates (ZVU or ZUPT) can be used to correct the user's velocity. The frequent use of ZUPT measurements consistently bounds many of the errors and as a result, even relatively low cost sensors can provide useful navigation performance.

However, there remain two significant problems with MEMS IMU pedestrian navigation. Firstly, the initial position, velocity and attitude have to be obtained. Typically, position is initialised using GPS, although the ability to do this will depend on whether the user is located in an area where GPS is available, and if it is, whether it is accurate. Heading also needs to be initialised since low cost gyros are unable to measure the rotation of the Earth which is used to initialise heading for more accurate sensors. Instead heading must be obtained from an external sensor such as a magnetometer which is undesirable since magnetic disturbances can cause significant errors.

The second significant problem that remains is heading drift during navigation. Heading drift still remains despite using ZUPT measurements since the heading error is unobservable. Observability is the ability to determine a state from a given sequence of measurements, and in the situation of using ZUPTs to aid a low cost IMU, it is not possible to estimate the heading error using only these measurements. This causes a significant issue since there then becomes a requirement to use heading measurements from external sensors. Typically magnetometers are used, however as previously mentioned, their measurements are often unreliable when navigating in environments such as indoors where there are significant magnetic disturbances. Instead measurements from other systems or methods are used to control heading drift. Statement of the invention

A novel and effective algorithm has been developed for generating heading measurements from basic knowledge of the orientation of the building in which a pedestrian may be walking. The idea is based on the assertion that most buildings are constructed with a rectangular shape. Within this shape, most rooms and corridors are constructed of smaller rectangles which constrain the direction a pedestrian can walk throughout the building to one of four headings.

According to the invention in a first aspect there is provided a system for correcting inertially derived navigation information for use in navigating in a navigation environment, comprising: at least one inertia] sensor; a memory for storing environmental information relating to one or more architectural features of the navigation environment and a processor arranged to: receive inertial data from the at least one inertial sensor, receive environmental information from the memory, implement a stochastic filter to calculate current inertially derived navigation information based on the inertial data, determine a correction signal based on the environmental information and the current inertially derived navigation information, and correct future inertially derived navigation information by inputting the correction signal to the stochastic filter.

The use of a stochastic filter allows the integration of additional sensor data, such as from a GNSS receiver or a magnetometer, into the system. Further, the stochastic filter provides optimal state estimation. By inputting the correction signal into the stochastic filter to correct future inertially derived navigation information errors in the navigation information are mitigated.

The correction signal may comprise a difference between the environmental information and the current inertially derived navigation information. The correction signal may be input to the stochastic filter in dependence on the correction signal being less than a predetermined threshold. The difference between the current inertially derived navigation information and the environmental information provides an estimate of the error in the current inertially derived navigation information. By applying the correction signal to the stochastic filter only when the correction signal is less than a predetermined threshold, the system prevents use of the error signal when the travel of the system is not aligned with the environmental information.

The stochastic filter may be configured to correct the future inertially derived navigation information in dependence on the magnitude of the correction signal. In this way, the stochastic filter is able to rapidly converge on the best estimate of future inertially derived navigation information. If the magnitude of the correction signal is large then the input to the stochastic filter is large and so the correction applied to the future inertially derived navigation information is also large.

A measurement noise applied to the correction signal in the stochastic filter may be sufficient to accommodate short term disturbances of the current inertially derived navigation information. The measurement noise applied to the correction signal in the stochastic filter may be 5 degrees. Short term disturbances may be seen as the movement of the system may not follow precisely the environmental information. Therefore, the use of measurement noise estimation applied to the correction signal in the stochastic filter accounts for these short term disturbances without unduly affecting the correction signal and the corrected future inertially derived navigation information. A value of 5 degrees has been found to be particularly advantageous in accounting for short term disturbances.

The processor may be further arranged to extract the environmental information from overhead imagery and store it in the memory. The overhead imagery may comprise map or photogrammetry imagery. Extracting the environmental information from overhead imagery obviates the need to obtain environmental information through an initialisation process and/or through observations made using the system itself. Map and photogrammetry imagery are freely available through applications such as Google Earth. By using such freely available survey data, the need for detailed information about the navigation environment is removed. For example, when the navigation environment is the inside of a building, there is no requirement for a detailed knowledge of the layout inside the building as the necessary environmental information is extracted from the freely available data. The at least one inertial sensor may comprise a gyroscope, the current inertially derived navigation information may comprise heading information, and the environmental information may comprise a principal bearing aligned with at least one direction defined by reference to architectural features of the navigation environment. The use of heading information and a principal bearing as described above allows the system to estimate the error in the heading information using the correction signal. This is because the principal bearing defines directions of travel in which the system may be considered to be travelling. Any deviation from these directions of travel can be considered to be due to error.

The environmental information may comprise four bearings, the four bearings may comprise the principal bearing and three further bearings derived from the principle bearing, and the four bearings may be separated by ninety degrees. The four bearings separated by ninety degrees make use of the fact that motion within a building is often aligned to one of those four directions due to limitations imposed on movement direction by internal walls etc.

The correction signal may comprise a difference between the heading information and one of the four bearings. By differencing the heading information and one of the four bearings, the direction of travel of the system may be resolved to the correct quadrant of the navigation reference frame.

The principal bearing may be expressed in a geodetic reference frame. This allows the principal bearing to be used to correct heading information expressed in a geodetic navigation frame. The use of a bearing in a geodetic reference frame also allows for easier integration of GNSSS data to the system as GNSS data is commonly expressed in a geodetic reference frame.

The processor may be further arranged to receive GPS navigation information from a GPS receiver and input the GPS navigation information into the stochastic filter. This feature allows GPS data to be used to strengthen the corrected inertially derived navigation information when GPS data of a sufficiently good quality is available to be integrated into the system The stochastic filter may be implemented as a Kalman filter. A Kalman filter is one type of stochastic filter. The skilled person will understand that other types of stochastic filter (or recursive Bayesian estimator) may be suitable for use with the invention. The term Kalman filter is used for the remainder of the application for clarity.

An indoor positioning system may comprise the system described above.

According to the invention in a second aspect there is provided a device for correcting inertially derived navigation information for use in navigating in a navigation environment, comprising: at least one inertial sensor; a memory for storing environmental information relating to one or more architectural features of the navigation environment and a processor arranged to: receive inertial data from the at least one inertial sensor, receive environmental information from the memory, implement a Kalman filter to calculate current inertially derived navigation information based on the inertial data, determine a correction signal based on the environmental information and the current inertially derived navigation information, and correct future inertially derived navigation information by inputting the correction signal to the Kalman filter.

According to the invention in a third aspect there is provided a method for correcting inertially derived navigation information for use in navigating in a navigation environment, comprising: receiving inertial data from at least one inertial sensor; receiving environmental information relating to one or more architectural features of the navigation environment; implementing a Kalman filter to calculate current inertially derived navigation information based on the inertial data; determining a correction signal based on the environmental information and the current inertially derived navigation information; and correcting future inertially derived navigation information by inputting the correction signal to the Kalman filter.

Specific description of the invention

Exemplary embodiments of the invention will now be described with reference to the accompanying drawings in which: Figure 1 is a plan view of an exemplary navigation environment;

Figure 2 is a plan view of an exemplary internal layout of a building;

Figure 3 is a block schematic diagram of a system according to an embodiment of the invention; Figure 4 is a block schematic diagram of a method of extracting bearing information from overhead imagery;

Figure 5 is a block schematic diagram of a method of correcting inertially derived navigation information according to an embodiment of the invention;

Figure 6 is an exemplary configuration of a system for use in field trials of the present invention;

Figure 7a is the trajectory of walking on a football pitch boundary line as recorded by a real time kinematic (RTK) system and an embodiment of the invention during a field trial;

Figure 7b is a position innovation solution calculated using an embodiment of the invention against an RTK solution;

Figure 8 is a heading innovation of a solution calculated using an embodiment of the invention against an RTK solution;

Figure 9a is an aerial photogrammetry image showing a position solution calculated using an embodiment of the present invention and a High Sensitivity GPS position;

Figure 9b is an aerial photogrammetry image showing a position solution calculated using an embodiment of the present invention and an IMU+ ZUPT solution

With reference to Figure 1 , a building 10, a fence 12 and a road 14 are shown. These elements of figure 1 represent exemplary architectural features that may be present in a navigation environment. The architectural features may be proximate an inertial sensor. That is to say, a positioning system incorporating an inertial sensor may be in use in a navigation environment including architectural features such as those shown in Figure 1.

The architectural features provide environmental information that can be used to correct inertially derived navigation information. As used herein, the term "environmental information" encompasses any geospatial information relating to the navigation environment. Environmental information may, for example, be information pertaining to the geospatial alignment of a feature. The geospatial alignment information may comprise horizontal geospatial alignment information and/or vertical geospatial alignment information. Horizontal geospatial alignment information may, for example, relate to the heading of each of the walls of a building. In particular, horizontal geospatial alignment information may comprise four orthogonal bearings aligned with the external walls of a building. Vertical geospatial alignment information may, for example, relate to the gradient of floors within a building. In particular, vertical geospatial alignment information may comprise a height plane aligned with a floor of a building.

As used herein, the term "inertially derived navigation information" encompasses any output obtained from an inertial sensor and any navigation information calculated using an output from an inertial sensor. For example, navigation information may be raw rate gyro or accelerometer data output by an inertial sensor. Navigation information may also be acceleration, velocity, position, angular velocity and/or angular orientation calculated using inertial sensor outputs. In particular, the navigation information may be heading information. The acceleration, velocity, position, angular velocity and/or angular orientation may be calculated using inertial sensor outputs and additional information obtained from, for example, a GPS receiver and/or any other navigation aid.

Each of the building 10, the fence 12 and the road 14 provide a bearing along which a user of a positioning system is likely to travel if they are in the vicinity of the particular feature. A user walking alongside the fence 12 is likely to follow the bearing defined by the fence 12. A user walking along the road 14 is likely to follow the bearing defined by the road 14. Similarly, a user walking alongside the building 10 is likely to follow a bearing defined by the building 10. Therefore, by determining the bearing 18 of, for example, an external wall 16a of the building 10, information about the likely direction of motion of a user navigating in the vicinity of the building 10 may be determined. A user inside the building 10 is also likely to follow bearings identifiable from the bearings of the external walls 16a, 16b, 16c and 16d when moving around within the building 10. This can be seen with reference to Figure 2, which shows the layout of the inside of the building 10. The internal walls 20 are all aligned with one of the external walls 16a-16d to define the corridors and rooms that make up the interior of the building 10.

The direction of motion of a user moving around within the building is often constrained to directions aligned with the internal walls 20 and the external walls 16a-16d of the building 10. The layout shown in Figure 2 is exemplary and does not relate to a particular building; however the principles of the direction of travel of users within buildings holds true for many buildings.

The term "cardinal heading" is used to describe four possible headings that the user may be likely to walk in most of the time when inside or proximate a building. Many buildings are constructed in the way shown in Figure 2. Manhattan, New York is a good example of a large number of buildings all aligned in a single direction most of the buildings in this area will have rooms and corridors aligned with bearings of 29.4 degrees, 119.4 degrees, 209.4 degrees or 299.4 degrees.

The four cardinal headings may be aligned with these bearings. The cardinal headings may be represented by a principal bearing since the other bearings may be derived from the principal bearing by offsetting each additional bearing by 90 degrees.

Environmental information, such as the principal bearing, may be extracted from overhead imagery. The term "overhead imagery" encompasses any plan view of the navigation environment. Specifically, overhead imagery may be a plan view of the outside of an architectural feature, such as a building. In particular, overhead imagery encompasses map or photogrammetry information. A method for deriving the environmental information is to use the distance and angle measurement tool in the Google Earth application. This method may be undertaken manually or automatically. Other methods for the derivation of environmental information from overhead imagery are described below. Embodiments of the invention comprise an algorithm for using cardinal heading information derived from maps to restrict heading drift that occurs when using a low cost foot mounted IMU for navigation. Bearing information can be derived quickly and in an automated manner using free maps or aerial images without the need for detailed internal maps of a building. In some situations, large areas can be covered using a single bearing measurement, such as parts of Manhattan, whereas other areas will require more detailed resolution, such as European cities. The use of bearing information relating to architectural features of the navigation environment obviates the need to record such data using the system itself. The use of bearing measurements is able to significantly reduce the drift of IMU- only navigation without the need for measurements from GPS, compasses or other sensors.

Position accuracy can be maintained below 5 metres for significantly long periods of up to 40 minutes. This kind of accuracy has previously only been achievable using high accuracy inertial sensors, but even these devices still need ZUPT or other sensor measurements to control position drift. The accuracy obtained is comparable with high performance ring laser gyros with drift less than 1 degree per hour. Using such gyros, it is known in the prior art to achieve a position error calculated to be just 5 metres in over 40 minutes of walking inside a building. The invention would still be advantageous when used in systems having high accuracy inertial sensors of the type described above if, for example, such systems run for long periods, for example greater than 1 hour.

The algorithm according to the invention is simple to implement, low in computational resource, works in real-time, and can be easily scaled to large areas even if the map information is derived manually. Furthermore, it is demonstrated that the algorithm is robust to short periods where the pedestrian will walk in directions not consistent with the building, which represent short term disturbances in the heading of the system. The system of the invention is termed Cardinal Heading Aided for Inertial Navigation (CHAIN).

Results from real data trials at a public hospital show the possibility for various future applications that could help people to navigate in non-GNSS environment. Such possible applications include patient's movement monitoring by doctors in hospital, or navigating visitors around the hospital. The system could be used for guidance for blind people or people with visual difficulties. Another possible application would be for leisure activities such as shopping, tourism and virtual gaming. Indeed they are many more possible future applications that can be realized as a result of our proposed algorithm. The algorithm is based on sensors that are already available in mobile devices, which indicates that the overall cost of the system could be relatively inexpensive. With the current achievable accuracy shown in this paper, it is expected that the proposed approach can become a stepping stone for others to utilize it for their own developed application.

Exemplary system

Referring to Figure 3, a system 30 for correcting inertially derived navigation information for use in navigating in a navigation environment is shown in a block schematic.

An IMU 32 is connected via link 33 to a processor 34. A GPS receiver 36 is also connected to the processor 34 by link 38. Additional sensors 40 may also be connected to the processor 34 by link 42. The additional sensors may, for example comprise a magnetometer, compass or other navigation aid. A memory 44 is also connected to the processor 34 by link 46.

The IMU 32 comprises a number of inertial sensors. Specifically, the IMU comprises 3 accelerometers aligned along 3 orthogonal axes, and 3 rate gyros aligned along the same three orthogonal axes. The IMU 32 provides raw IMU data to the processor 34 along link 33. The raw IMU data comprises raw accelerations from the accelerometers and raw angular velocities from the rate gyros.

The GPS receiver 36 and the additional sensor 40 may provide supplemental information to aid the processor in determining the position of the system 30. The skilled person will appreciate that the GPS receiver 36 and the additional sensor 40 are not essential features of the system 30 of the invention. However, in some embodiments of the invention, the GPS receiver and/or additional sensors may be integrated into the system to provide GPS observables to a Kalman filter when available as described below. The memory 44 is arranged to store environmental information. In some embodiments of the invention the environmental information may be in the form of maps and/or photogrammetry data. Such data is freely available from resources such as Google Earth. In other embodiments the environmental information may be in the form of bearing information relating to one or more walls of one or more buildings. The environmental information is provided to the processor 34 via link 46.

The processor is arranged to receive the inertial data 33, the GPS data 38 and/or other sensor information 42 and the environmental information 46. The processor is further arranged to compute current inertially derived navigation information 48 based on the inertial data using a Kalman filter. The processor is further arranged to determine a correction signal based on the environmental information and the current inertially derived navigation information. The correction signal is fed back within the processor 34 as an input to the Kalman filter at subsequent epochs to correct future navigation information. This is explained in greater detail below. It will be understood by the person skilled in the art that different elements of the system 30 may be located in various locations. For example, the primary sensors, e.g. the IMU 32, the GPS unit 36 and the other navigation aids 40 are located in the positioning system so as to be able to calculate navigation information relevant to the user. However, the memory 44 and the processor functions for determining environmental information may be located elsewhere. For example, the memory 44 and the processor functions for determining environmental information may be located at a remote server. The processor 34 is then arranged to provide a client/server relationship with the remote server in order to receive environmental information transmitted by the means for determining environmental information. The transmission of environmental information may be achieved by any known methods, e.g. radio link, Bluetooth (RTM) link etc.

Deriving cardinal headings from map data Street level maps, along with other types of maps such as world maps, topographic maps and geological maps, are very useful for street level navigation. This is because they provide useful street level information to users that includes features such as building outlines and roads, and uses either a line map (2D representation), or aerial imagery (3D-like representation). An extra piece of information commonly found maps is that the map is orientated such that North is always pointing straight up, East to the right, West to the left and South is pointing to the bottom of the map. This important map information may be utilised, together with classical edge detection algorithm, to show the concept of deriving building bearings from minimal map information. There are numerous methods for edge detection but the Canny method has been selected as it is a simple method used in the field of image processing. Other methods of edge detection may be used within the scope of the invention as claimed.

The Canny method works by looking for the minimum and maximum value in the first derivative of image pixel values. Points that sit within this threshold will be detected as edge points.

After implementation of the edge detection algorithm, a Hough Transform is used to detect straight line features from the building image on the map. Straight line detection may be implemented to add accuracy to the detection of building edges. This is because edge detection shows where edges are, but may not conclusively show what the edges are geometrically. That is to say it may not be determinable from edge detection whether the edge is a straight line or an arc.

The Hough Transform dictates that if certain points satisfy a line equation, then it will be considered as a straight line. In some embodiments of the invention, the longest detected straight line is selected for reliability purposes as a short straight feature may not present the true building orientation.

The start and end point of the selected line are then stored in terms of pixel values and then the equation below is used:

Where ψ is a derived building heading, dx/dy is the difference between the start and end y-pixel value, dy/dx is the difference between the start and end x-pixel value and Q is the quadrant area.

Equation (1) is used because it is known that overhead imagery is orientated such that it can be divided into four standard quadrant angles, where quadrant 1 represents 0 degrees to 90 degrees, quadrant 2 represents 90 degrees to 180 degrees, quadrant 3 represents 180 degrees to 270 degrees and quadrant 4 represents 270 degrees to 360 degrees.

The flowchart shown in Figure 4 shows a block schematic of the heading derivation process. Referring to Figure 4, a digital overhead image is read 50. A Canny method is used 52 on the digital image to detect edges within the imagery. A Hough Transform is applied 54 to the imagery after edge detection in order to detect straight lines. Straight lines are then extracted 56 and a building heading derived from the extracted straight lines 58.

After the building heading is acquired, an offset of 90° is incrementally added to determine the remaining three headings to make up environmental information relating to building headings. The algorithm used in embodiments of the invention is based mainly on traditional inertial navigation equations, with errors controlled through the use of measurements applied using a Kalman filter. The following sections give an overview of this approach, followed by a detailed description of the new CHAIN algorithm.

Low cost IMUs typically consist of micro-electromechanical (MEMS) gyros and accelerometers which are 'strapped' down to the body of interest. The body of interest may, for example, be a foot of a pedestrian user of the system. Hence the accelerations and angular rates from the IMU are measured in the body frame rather than a geodetic frame.

Generally in an INS, after determination of initial position, velocity and attitude during system initialisation, gyro angular rates are integrated once to get the attitude of the system. Using the calculated orientation, acceleration outputs from the three accelerometers are transformed into the desired frame of reference. The navigation frame is chosen as the reference frame because user position in a geodetic coordinate system is required.

Subsequently after correcting for local gravity, the acceleration outputs are then integrated to get the velocity, and integrated again to get the position. Normal strapdown navigation equations are used to resolve and update the position and attitude of the IMU. Velocity in the navigation frame is computed by numerically integrating the following differential equation:

= Ctf ^b - (2o>l + «¾£,) * v ⁿ + _S ⁿ (2) where ^v ™ is the velocity in the local (North, East, Down) navigation frame; ^c is the rotation matrix that transforms measurements from the IMU in the body frame, ^b , to the navigation frame, ⁿ ; f ^b is the accelerometer measurement in the body frame; ^ω fe is the rotation rate of the Earth in the navigation frame; ^ω η is the transport rate of the navigation frame and 9 ⁿ is the gravity vector in the navigation frame. The middle terms may be ignored when using low cost IMUs since less accurate inertial sensors are incapable of measuring Earth rotation and also navigation is done with a small velocity. The attitude of the IMU is maintained using the following differential equation:

Where b is the skew-symmetric matrix of body frame rotations corrected for Earth rotation and transport rate.

The position and attitude of the system can then be regularly updated by numerical integration of the IMU output. However, due to low cost specification, low cost IMUs contain errors such as biases and noise. After only short periods of time, due to numerical mechanisation, these errors get accumulated and cause significant position and attitude errors. A Kalman filter may be used to estimate these errors and will be explained further below.

Kalman filters provide optimal state estimation. In some embodiments of the invention, the state vector that used is shown below: χ = (δρ dv ⁿ δω 6g ^b δα*) ^τ (4) Where <¾ ^? is the vector of latitude, longitude and height errors; ^δν>η is the vector of navigation frame velocity errors; ^5ω is the vector of attitude errors (roll, pitch and yaw); Sg ^b \$ the vector of gyro bias errors and <^ ^α¾ is the vector of accelerometer bias errors.

The Kalman filter is used in feedback form. This allows errors that are calculated from the Kalman filter to be used to correct the inertial sensor measurements and navigation parameters. Any observable state may be modelled using the Kalman filter. This allows the production of an estimate of the error in a given part of the system provided that that error is observable by the Kalman filter. The estimated error can then be fed back into the input of the Kalman filter and used to correct the output from the filter at subsequent epochs. Kalman filters provide for expandable systems as new inputs may be integrated into the system easily by adding more inputs to the Kalman filter. Only the forward Kalman filter is considered herein since our focus is real-time use. However, the skilled person will envisage other embodiments using Kalman filter smoothing. Such algorithms give much better results in terms of accuracy.

Observability may result from the ability to estimate certain parameters in a Kalman filter. This can be illustrated by considering a static IMU. For example, if an incorrect roll or pitch measurement is computed, this will result in the gravity vector being incorrectly subtracted from the accelerometer measurements. This results in a residual acceleration error which will result in a velocity error after numerical integration. Therefore, by using ZUPTs in the Kalman filter, the error can be controlled. However, if the heading of the IMU is changed, this does not affect the velocity and therefore ZUPT measurements are unable to restrict that error. The relationship between velocity errors and attitude errors in Local Level Frame is shown below:

Where / is the force in the body frame (including gravity) and ^δ Ψ , δθ , ^δ Ψ are the roll, pitch and yaw errors respectively. During ZUPT, the horizontal forces in the local level frame are essentially zero and specific force fo in downward direction is approximately close to the negative gravity constant. Therefore from this equation, it can be seen that the velocity errors in North and East directions are only related to errors in roll and pitch attitudes via a specific force /D in downwards direction. This means that during ZUPT period, the dynamic change in horizontal velocity is proportional to the change in roll and pitch errors. By improving the velocity estimation through ZUPT means that roll and pitch errors are improved as well but not the heading errors and this has actually motivated the idea in this paper.

The knowledge of derived building heading as shown above is utilized to provide a measurement update to the Kalman filter. The measurement matrix which uses heading update together with ZUPT is shown below: H = [0 0 0 0 0 0 0 - sin(0) 1 0 0 0 0 0 θ]

The measurement is z _fc = ώ + n _k ,

Where

Ψ DerivedHeading Ψ INSOnfy

Where n _k is the measurement noise with covariance matrix R _f c = E (r n^ )

This measurement update is done during ZUPT epoch. The difference between the derived heading and the measured heading from INS is used as an update to alman filter.

As described above, cardinal heading measurements are obtained by extracting the principal heading of individual buildings on a map or aerial photo. Assuming that a user typically walks along these four directions as they navigate the building, the heading information can be used to update the INS heading. The algorithm relies on the assumption that the remaining position error after applying ZUPTs is mainly a result of heading error. In fact, this is a reasonable assumption since ZUPTs are able to control most of the significant error sources apart from heading error (and z-axis gyro bias drift if the IMU is mounted with z mainly pointing down). Based on this assumption, the direction of a step that a pedestrian has walked from the INS may be computed using the following equation:

Where & ^E and ^ώΝ are the changes in East and North-axis positions over one step. This heading measurement is based only on the change in position caused by a single step, and therefore Ψκ ^β ν not only consists of the true heading plus drift, it also consists of other small errors from inertial navigation.

If a user is walking in one of the four cardinal directions, an estimate of the heading error can be derived by forming the observation: δφ = ψ carnal - φ _3ίβρ

(7)

Where Ψεατ&ίηαΐ is the cardinal heading derived from a map and resolved in the correct quadrant. The angle is resolved in the correct quadrant by comparing step with four possible cardinal building headings. If the difference δψ is less than a predetermined threshold, the measurement is applied in the Kalman filter. Conversely, if the difference is exceeded, no update is applied. This accommodates periods where the user is not walking in a direction consistent with the building such as around corners.

In some embodiments of the invention a further condition on the application of the difference measurement to the Kalman filter may be that the user is walking in a straight line. Specifically, the current heading may be compared with the last heading. If the two don't agree within a predetermined threshold number of degrees (e.g. 10 degrees), then it is assumed that the user is not walking in a straight line. Therefore, if a difference between the current heading and the previous heading is less than a predetermined threshold the difference δψ is applied to the Kalman filter.

For the avoidance of doubt, the difference between the current and the previous headings may be used to determine whether the difference between the inertially derived heading and a cardinal heading, δψ, is applied to the Kalman filter. Generally, pedestrians do not walk exactly in straight lines and therefore an appropriate measurement noise should be used in the Kalman filter to accommodate this. For example, a user may travel in a direction that, over the long term, is in the direction of one of the cardinal headings, but, during the short term, experiences disturbances away from the precise cardinal heading. These disturbances may be related to objects obstructing the path of a user. The disturbances may be bound by the width of corridors or rooms within a building. A measurement noise may be selected to accommodate the maximum disturbances in measured heading that may be experienced within a navigation environment. It should also be noted here that the heading error measurement does not relate directly to the physical attachment of the IMU which can be mounted in any orientation on the user's foot. This is significant because it does not matter if the user is walking sideways or even backwards for the algorithm to work. Referring to Figure 5, a flow diagram showing a method of correcting inertially derived navigation information is shown. The system may be initialized 60 with starting position and attitude data. The initial position may be provided by another system, such as a GNSS, or may be provided by starting the system at a known point, e.g. a point surveyed previously. Initial roll and pitch information may be provided by holding the system stationary for a given time period, say 10 seconds, to allow the Kalman filter to resolve the roll and pitch using the gravity vector. The initial heading of the system may be provided manually or using a supplementary system such as a GNSS.

In some embodiments of the invention, the system is integrated with a GPS receiver as shown in Figure 3. In such embodiments the GPS data received by the processor may be used to provide position updates for the Kalman filter algorithm computing inertially derived navigation information. In many applications of the invention a building will be entered from an environment in which GPS signals are present and/or have sufficient integrity to provide a position update. A GPS receiver may therefore be used to initialise the system.

The alignment of a building, i.e. the principal bearing, in which the system is to navigate is determined 62. This is undertaken using the edge and straight line detection algorithm described above on overhead imagery data. From the principal bearing four cardinal headings are derived 64. The four cardinal headings are derived by adding increments of 90 degrees to the principal heading.

The skilled person will appreciate that more than four cardinal headings may be derived. For example, 8 cardinal headings may be defined by adding increments of 45 degrees to the principal bearing determined using the edge and straight line recognition algorithm. The invention is not restricted to using 4 bearings, although this is a feature of certain embodiments. Buildings such as the Nottingham Geospatial Building, University of Nottingham has a main rectangular building, but also an atrium at an angle to the rest of the building. In some embodiments of the an additional angle may be used to account for such angles.

The principal bearing is aligned in a navigation frame of reference. This is because the principal heading is extracted from overhead imagery data, which is aligned to a navigation reference frame. The principal bearing may, for example be referenced in a locally level navigation frame, or in a geodetic navigation reference frame. In this way, an absolute bearing referenced to north is defined as the principal bearing and used in the system of the invention. Definition of the principal bearing in a navigation reference frame allows the computed inertially derived heading information to be compared to the environmental information in a navigation reference frame. This allows the easy integration of other sensor data, such as GNSS data, into the system.

GNSS data is observed in the WGS 84 geodetic reference frame. Therefore, in order to integrate data from such systems into the system of the invention, the system should also provide navigation information in a navigation reference frame. That is, the system should calculate inertially derived navigation information having absolute values for position and orientation.

By extracting the principal bearing from overhead imagery, the need to record a principal direction or bearing using the system itself is obviated. Using the system to record the principal bearing is flawed in that the errors in the system are incorporated into the estimation of the principal bearing. In addition, the principal bearing is not referenced to a navigation reference frame using such systems.

The processor 34 is arranged to calculate the inertially derived heading at step 66. The inertially derived heading is calculated using a Kalman filter. The Kalman filter takes as its inputs the raw data from the IMU 32 and produces inertially derived navigation information. The Kalman filter may also be used to observe the errors in the raw data from the IMU 32, which may be fed back as inputs to the Kalman filter to correct navigation information computed by the Kalman filter in the future, at subsequent epochs. The Kalman filter allows for the use of other sensor data, such as GNSS data, to be input into the system. As mentioned above, GNSS data is expressed in the WGS 84 geodetic navigation frame. The system should also operate in a navigation reference frame in order to facilitate the easy integration of GNSS data into the system. Therefore the use of a Kalman filter in conjunction with an algorithm to derive the principal bearing from overhead imagery data provides the advantage that the system for use in correcting inertially derived navigation data using cardinal headings can easily be expanded to include other sensors that operate in navigation frames of reference. Further, the invention can use any additional measurements from any sensor that provides useful navigation information. These can be used optimally through the filter. The additional measurements include, for example, GPS measurements, position from Wi-Fi, RFID, Bluetooth, ranges from systems such as pseudolites, time difference of arrival measurements from, for example, LORAN, other map information, for example knowing where doors are can constrain position; or measurements from computer vision, such as motion of sensor, rotation, height, position etc. Any of this information can be used in the Kalman filter which will all work to improving the estimation of the inertial errors.

At step 68 it is decided whether the inertially derived heading information is within 10 degrees of one of the cardinal headings. If the inertially derived heading is not within 10 degrees one of the cardinal headings then no additional correction is applied to future heading calculations 70. If the inertially derived heading is within 10 degrees one of the cardinal headings then the cardinal heading data is used to correct future inertially derived heading information. The cardinal heading data is used by differencing the relevant cardinal heading from the inertially derived heading information. The difference then forms a correction signal that can be input into the Kalman filter to correct navigation information calculated by the Kalman filter at subsequent epochs.

The system uses the magnitude of the correction signal to correct future inertially derived navigation information. That is, the magnitude of the correction signal provides information to the Kalman filter on how large the error in the current inertially derived heading is. The Kalman filter is therefore able to apply a correction that will quickly resolve the error.

Field trials

Three field trials were undertaken to test the proposed approach. The first trial involved normal walking around typical football pitch with a real time kinematic positioning system to act as a position reference to evaluate the accuracy of the foot mounted IMU. For the second and third trials, normal walking and irregular walking were undertaken respectively in a typical indoor environment at Queen Medical Centre (QMC) hospital, Nottingham within a built up area of about 65 000 m ² . There was no ground reference used in QMC trial due to the difficulty of providing such a reference system inside buildings, hence the result is discussed using Google Earth overhead imagery as a coarse approximation. Total walking distance is approximated as typical walking velocity of .5 m/s multiplied by the total time of the trial. For all trials, a MicroStrain 3DM-GX3-25 IMU was used which has typical technical specifications of a low cost IMU grade with a dimension of 44mm x 25 mm x 11 mm, weighing only 11.5 g. The accelerometer bias stability is quoted as ±0.01 g, and for the 300 deg/s model, the gyro biases are specified as ±0.2deg/s. The particular IMU used has a limit of 1200 deg/s for angular rotation and 18g for acceleration, which is sufficient for the walking trial. This is because the typical rotation of a pedestrian's foot is typically between 600 deg/s to 900 deg/s while walking. The IMU measurements were synchronized with GPS time using the IESSG Precise Time Data Logger (PTDL) which is able to accurately timestamp the serial data from an IMU and record it using an SD card. The GPS time stamp is recorded for the purpose of synchronising the IMU with GPS so that a comparison can be made between the CHAIN and GPS solutions. Theoretically, if autonomous navigation is sought only in indoor environment without any comparison (assuming initial position is known), the GPS time stamp may not be needed and can be replaced by the IMU's internal clock. Figure 6 shows example of the system setup used for field trials. The Backpack contains the PTDL, a 12V battery and u-Blox ANTARIS 4 High Sensitivity GPS receiver and a Leica GPS 1200 Real Time Kinematic system (RTK) while the IMU is shown to be mounted on foot. The initial position for the IMU was estimated from the GPS position (which in practice may assume that navigation would start in a well received GPS signal area). The initial roll and pitch of the IMU was calculated during a short stationary period (1 second) by differencing the accelerometer measurements with the local gravity vector. The heading was initialised manually, but it a one-off magnetometer reading could be sufficient to initialise the algorithm. Normal strapdown navigation equations were used to resolve and update the position and attitude of the IMU.

Once initialised, the system computes its position relative to the initial position. Measurements were then post processed using the University of Nottingham's POINT (Position and Orientation Integration) software (although the algorithm could still be used in real-time). The software was designed specifically with the purpose of allowing easy integration with measurements from external sensors.

The following sections describe trials that have been conducted to test the new algorithm. Football Pitch Trial with RTK Reference

Since there was no reference system available that could work in indoor environment to verify the accuracy using proposed algorithm, an outdoor test was necessary to emulate the indoor environment. The outdoor test was conducted with a 40-minute normal walk on a football pitch at the University of Nottingham. The football pitch is approximately 95m x 55m in dimension with a typical white boundary line. The user walked ten circuits around the boundary line of the pitch so that the user was walking approximately in straight lines apart from at the corners, emulating a walk around corridors in a building. The RTK system was also used in this trial as a ground reference with an accuracy of approximately 2 cm (figure was given by the RTK system) throughout the whole trial. Figure 7a shows the comparison of the two trajectories. Figure 7b shows the position difference of the CHAIN solution compared to the RT solution. It should be noted that the difference also includes the non-constant offset of the IMU moving on the user's foot relative to the GPS antenna on the user's back which is shown by the smaller higher frequency oscillations. The maximum horizontal difference is less than 5 metres with a standard deviation of 1.7m and 1.2m for North and East respectively. For the North and East errors, the oscillations that occur are a result of a full round of walking (there are 10 peaks which are equivalent to 10 rounds of walking). This appears to be a result of the IMU solution resulting in slightly shorter distance measurements than the RTK truth. However, the height error is still prominent with maximum height error of about 11 metres after 40 minutes of walking.

After a free inertial navigation of approximately 40 minutes in duration and 3000 metres in distance, the absolute position error is about 3.5 metres, or about 0.12% of the total distance travelled. This is a significant improvement in the use of low cost MEMS positioning which demonstrates the effectiveness of the CHAIN algorithm. In fact, such performance is difficult to achieve even with high quality inertial sensors, unless they can be foot mounted. For example, even navigation grade inertial sensors have a typical drift of approximately 2km per hour, so unless regular measurement updates (such as ZUPT) can be applied, the performance is not comparable to the low cost CHAIN solution.

To demonstrate the application of the heading updates, Figure 8 shows the Kalman filter innovation for the heading measurements. The innovation is the difference between the INS derived step heading, and the building heading. The difference comprises of the INS heading error, other small INS drift, and also the variation of the user's step in relation to the heading of the building. The standard deviation of heading error is shown to be only 2.1 degrees whilst the maximum heading error is 9.7 degrees (which corresponds to the 10 degree acceptance threshold). The maximum values probably occur as the user walks around corners and the walking in straight lines assumption is not correct. Q C Hospital Trial with Normal Walking

A second trial using the low cost IMU was undertaken at Queens Medical Centre Hospital, Nottingham. This building is selected because it represents a typical building with many straight features. The normal walking trial was done for about 40 minutes with an approximate distance of 3 km. The u-Blox GPS receiver was also used for comparison purposes to indicate the performance of a high sensitivity receiver in this building.

Figures 9a and 9b show the trajectories taken during the trial. Referring to Figure 9a, the solid line shows the output of CHAIN system, the dot markers shows HSGPS output. Referring to Figure 9b, the dashed line shows the IMU + ZUPT solution and the solid line shows the output of CHAIN system.

Although the GPS receiver can track more than 4 satellites in some parts of the building, no useful comparisons can be made between GPS solution with the proposed CHAIN solution because of the high multipath measurements (see dots in Figure 9a).

However coarse analysis using aerial imagery of Google Earth as shown in the figure is sufficient to indicate that the integrated position solution is usually better than 5 m, and typically < 2 m most of the time with respect to the image. Furthermore, as expected, the heading is always consistent with the building. As mentioned before, there was no ground reference except the freely available aerial imagery of QMC building; hence only rough approximation of the trajectory analysis for Figure 9b can be done using Google Earth. Nonetheless it does provide a useful insight into the effectiveness of this approach against a standard ZUPT. It is obvious that CHAIN solution overcomes a standard IMU + ZUPT solution based on the difference between the two trajectories as shown in Figure 9b. In this figure, it is clear that the majority of the position drift occurs as a result of heading drift. The difference between the start and end position for the CHAIN system is about 2.30m, approximately only about 0.08% position error from a total walking distance of 3000m which again is a significant improvement in performance. For the IMU + ZUPT only approach, the difference is about 220 m, approximately about 7% of the total distance and with a significantly corrupted heading solution.

QMC Hospital Trial with Irregular Walking

A third trial for a period of 15 minutes was undertaken to simulate the irregular walking behaviour with the same start and end location. This is to examine the performance of the algorithm when the walking in straight lines assumption within the building does not necessarily hold true. Four different types of walking pattern were analysed and alphabetically identified as A, B, C and D below.

[A] Entering QMC through the main entrance to a convenience shop, walking into two aisles before coming out from the shop. Walking into another shop, did one round before coming out through the main entrance to the starting position.

[B] After cornering, a 'zigzag' walk was undertaken.

[C] Backwards walk.

[D] Walked to the spiral stairs, down to the lower floor until the end and made a small loop around a pillar. Then walked up to another stairs for three levels and then walked down again to the spiral stairs. Walked up the spiral stairs to the start of walk.

With these irregular walking patterns, the start and end position error still gives an error of about 1.25 m, again approximately only 0.1 % of the total walking distance of ~1248 m. In this trial, the system also showed robustness for a short period when the heading measurement is not being updated.

The present invention comprises an algorithm for using simple heading information derived from maps to restrict the heading drift that occurs when using a low cost foot mounted IMU for navigation. The algorithm uses the simple notion, that users are typically constrained to walk in one of four cardinal headings due to the way corridors and rooms are typically constructed. The heading of the building can be obtained by taking measurements from maps or aerial images which is simple and fast to extract for even relatively large areas. This information may be automatically derived using, for example, computer vision algorithms incorporating edge and straight line detection. The algorithm has the significant advantage that it is not necessary to derive detailed indoor maps of buildings for it to be effective, as indoor maps are typically not available.

The effectiveness of the algorithm has been demonstrated through analysis of real world data by using the foot mounted IMU approach. Data gathered during experimental tests were used to show its effectiveness as a self contained system without depending on other sensors for measurement updates. It has been shown that the estimated accuracy in position is below 5 m in 40 minutes walk and about 0.1% of the total distance by using only forward Kalman filter. This approach provides an important leap in autonomous inertial pedestrian positioning by addressing the main problem of heading drift. Importantly, the algorithm is simple to implement, uses little additional processing requirements and has been shown to be robust even in situations where the user walks in different directions and non-straight lines.

The invention has been described with reference to specific embodiments of the invention. Other embodiments of the invention will be conceivable by the skilled person without departing from the scope of the claims.