Patent Description:
At a network junction of a transport network, multiple tracks intersect with one another. Therefore, the network junction presents multiple route options for the vehicle to take. It is important to know the particular route taken by the vehicle through the network junction, in order to allow for seamless operation of the vehicle through the network junction. This information is currently provided via track-side infrastructure.

Track-side infrastructure includes, for example, balises or beacons, which are installed at fixed locations along the tracks. The track-side infrastructure is able to detect the passing of a vehicle and is generally available along each route option provided by the network junction. The track-side infrastructure may transmit signals indicating that the vehicle has passed a particular location to either the network signalling system or the vehicle. Accordingly, by using the track-side infrastructure, the network signalling system or the vehicle itself is able to determine the particular route option taken by the vehicle through the network junction. The track-side infrastructure is expensive to install and maintain. In addition, the use of track-side infrastructure is dependent on the communication link with the vehicle or the network signalling system. Therefore, the route detection method using the track-side infrastructure is not autonomous.

While it is common for a vehicle to carry a Global Navigation Satellite System (GNSS) sensor, the GNSS sensor suffers from a lack of signal availability and continuity in many common operational environments, e.g., tunnels. Therefore, it is generally not sufficient to rely upon the GNSS sensor alone to position a vehicle.

Thus, there is a need to provide a system and a method for reliably determining a route a vehicle takes through a junction of a transport network without utilising the track-side infrastructure.

<CIT> proposes a method of determining the location of a vehicle on a map, the map comprising a plurality of points associated with features, comprising: capturing, using a video camera an image of a scene from the vehicle, identifying points in the image corresponding to features in the scene, and comparing the points in the captured image to the map to determine the position of the vehicle. The method further comprises the step of capturing at least one further image of the scene, identifying points in the at least one further image and comparing the points identified in the image and the or each further images. The method may also comprise modeling the motion of the vehicle using a predictive filter, such as a Kalman filter, or an Extended Kalman filter. <CIT> discloses determining a current vehicle location using recursive Bayesian filters for each of a plurality of different paths of travel.

According to a first aspect of the present invention, there is provided a computer-implemented method as set out in claim <NUM>.

The transport network may be a guided transport network (e.g., railway, tramway). The guided transport network has tracks to which movement of the vehicle (e.g., a train, a tram) is strictly confined in normal operations.

The transport network may also be a road or motorway network having tracks (e.g., roads or routes) for a vehicle (e.g., car, lorry) to follow, or a waterway network having tracks (e.g. a canal) constraining a route taken by a vehicle (e.g., a boat).

In general, the transport network imposes track constraints to the position and the movement of the vehicle. For example, the vehicle is expected to move along the curvature of the tracks/routes provided by the transport network, unless something goes seriously wrong. The track geometry data, as its name suggests, is data indicative of the track geometry of the transport network, and thus defines the track constraints imposed by the transport network to the vehicle.

The Bayesian estimation filter algorithm is commonly used to estimate a state of a system based upon sensor measurements which indirectly measure the state. The Bayesian estimation filter algorithm may be used interchangeably with "sensor fusion algorithm" or "optimal estimation algorithm".

Advantageously, a plurality of Bayesian estimation filter algorithms each associated with a respective one of the plurality of route options presented by the junction are used to resolve the particular route option taken by the vehicle. As such, there is a reduced or removed need for the provision of track-side infrastructure to disambiguate the route option taken by the vehicle through a junction, and the route can be determined when track-side infrastructure is unavailable in the locality of a network junction. This permits for seamless travel of the vehicle through the junction, and allows for a significant cost saving to the network operator in terms of procurement, installation and maintenance of track-side infrastructure.

The data indicative of probabilities of the vehicle taking the associated route options comprise innovation values calculated by the plurality of Bayesian estimation filter algorithms.

Innovation is a parameter calculated in the update step of a Bayesian estimation filter algorithm. In general, the value of the innovation represents a difference between a measurement fed to the Bayesian estimation filter algorithm and a predicted measurement.

The plurality of Bayesian estimation filter algorithms are configured to calculate the innovation values based upon the pseudo-measurements of the associated route options and the respective estimated positions of the vehicle.

It will be understood that the innovation value is a difference between the pseudo-measurement and a product of an estimated state of the vehicle and a measurement model. The estimated state of the vehicle is generated by the Bayesian estimation filter algorithm and includes the estimated position of the vehicle. The measurement model is arranged to map a state space in which the estimated state lies to a measurement space in which the pseudo-measurement lies.

Selecting one of the plurality of route options which presents the highest probability may comprise selecting one of the plurality of route options which achieves the lowest innovation value.

Selecting one of the plurality of route options which presents the highest probability may comprise: comparing the innovation values calculated by the plurality of Bayesian estimation filter algorithms against a predetermined threshold value, and selecting one of the plurality of route options which presents an innovation value below the predetermined threshold value.

The plurality of Bayesian estimation filter algorithms may be configured such that the estimated positions of the vehicle lie along the associated route options indicated by the track geometry data.

That is, the plurality of Bayesian estimation filter algorithms may be constrained by the track geometry data, and the estimated positions of the vehicle may be constrained to be on the tracks of the transport network as defined by the track geometry data. This may be achieved in various ways. In an example, the final estimated position (unconstrained) output by the algorithm may be orthogonally projected onto the tracks defined by the track geometry data so as to obtain a constrained solution. In an alternative example, the track geometry data imposes implicit one-dimensional motion constraints on the time evolution of the estimated state of the algorithms.

The plurality of Bayesian estimation filter algorithms may be executed in parallel to one another.

The vehicle may carry at least one sensor, the at least one sensor being arranged to output a signal indicative of a motion of the vehicle.

The at least one sensor may comprise an inertial measurement unit, IMU, and a sensor other than an IMU.

The sensor other than an IMU may be selected from a group consisting of at least one of a GNSS sensor, a balise reader, a radar, a speed sensor, an odometer, a video camera and a lidar detector. The IMU may be a low-cost MEMS IMU.

Determining that the vehicle is approaching a junction of the transport network may comprise executing a first Bayesian estimation filter algorithm to estimate a position of the vehicle based upon the track geometry data; and determining that the estimated position of the vehicle is approaching a junction of the transport network.

The first Bayesian estimation filter algorithm may be executed to:.

The first Bayesian estimation filter algorithm may comprise a strapdown inertial navigation algorithm. The method may further comprise executing the strapdown inertial navigation algorithm in the prediction step based upon the track geometry data and the first sensor data. The strapdown inertial navigation algorithm may be configured to calculate a speed of the IMU along a track defined by the track geometry data based at least upon the first sensor data, to integrate the speed over time and to generate the predicted position of the IMU based at least upon the track geometry data and the integration of the speed.

Constraining the strapdown inertial navigation algorithm by the track geometry data in this way reduces the problem of estimating an unconstrained three-dimensional position of the vehicle to the problem of estimating a one-dimensional position of the vehicle along the track of the transport network, because the vehicle has only one degree of freedom along the track.

Advantageously, by having the strapdown inertial navigation algorithm constrained by the track geometry data, the predicted position of the vehicle is constrained to be on the track of the transport network as defined by the track geometry data. Further, the strapdown inertial navigation algorithm models the propagation of kinematic state estimation errors into the one-dimensional position solution space and allows the instantaneous curvature and tangent information of the track as defined in the track geometry data to be taken into account.

Generating a plurality of Bayesian estimation filter algorithms may comprise generating a plurality of copies of the first Bayesian estimation filter algorithm.

The computer-implemented method may further comprise executing at least one copy of the first Bayesian estimation filter algorithm in an update step to update the predicted position of the vehicle based at least upon the second sensor data and the pseudo-measurement of the associated route option.

That is, the pseudo-measurement derived from the track geometry data may be used together with the second sensor data for updating the predicted position of the vehicle.

The computer-implemented method may further comprise deleting copies of the first Bayesian estimation filter algorithm which are associated with the unselected route options.

In this way, only the copy associated with the selected route option remains after the vehicle passes the junction. The remaining copy of the Bayesian estimation filter algorithm serves (as the first Bayesian estimation filter algorithm) to continue estimating the position of the vehicle for determining whether the vehicle is approaching a next junction.

The plurality of Bayesian estimation filter algorithms may comprise an unscented Kalman filter.

Among all of the Bayesian estimation filter algorithms which handle non-linear process models, the unscented Kalman filter is advantageous in that it is better at handling nonlinearities and it does not require the evaluation of explicit, or numerically approximated, Jacobians for the process models.

The plurality of Bayesian estimation filter algorithms may comprise a Lie-group unscented Kalman filter. That is, the Bayesian estimation filter algorithm may operate in a state space and/or a measurement space which is represented by Lie groups (in particular, matrix Lie groups). It is advantageous to represent the state and the measurement spaces using Lie groups, because Lie groups can easily represent a complex state which comprises multiple sub-states using a product matrix Lie group without losing the typological structure of the state space.

Obtaining the track geometry data may comprise: accessing a map database, wherein the map database comprises sample points positioned along tracks within the transport network; retrieving from the map database sample points in the vicinity of the vehicle; and applying an interpolation function through the retrieved sample points to obtain a track constraint function, wherein the track geometry data comprises the track constraint function.

Advantageously, by applying an interpolation function through the retrieved sample points from the map database, the track constraint function (which is included within the track geometry data) comprises lines/curves which represent centrelines of the tracks within the transport network. Thus, the track constraint function is able to represent the potential route(s) of the vehicle, the potential direction of the vehicle, and kinematic constraints to be imposed within the Bayesian estimation filter algorithm to improve the estimation of errors in the kinematic state (e.g., the position) of the vehicle when no source of absolute position information is available. It would be appreciated that the density of the sample points (which may be used interchangeably with "support points") affects the accuracy of the track constraint function.

The track constraint function may comprise a twice differentiable curve with a continuous second-order derivative at least one of the retrieved sample points.

Advantageously, the twice differentiable curve with a continuous second-order derivative allows the curvature and tangent information of the tracks (in particular, the track centrelines) within the transport network to be modelled more accurately than would be possible with a conventional piecewise linear interpolation using the same number of sample points. Accordingly, the track constraint function provides a powerful constraint on the to-be-estimated kinematic state of the vehicle without discontinuities, and improves the estimation of errors in the output of the Bayesian estimation filter algorithm.

The interpolation function may comprise a cubic spline function.

Advantageously, the cubic spline interpolation implicitly provides a twice differentiable curve with a continuous second-order derivative. Furthermore, amongst all of the twice differentiable functions, the cubic spline interpolation function yields the smallest norm of strain energy and allows the track constraint function obtained thereby to have a curve progression with minimal oscillations between the sample points.

According to a second aspect of the present invention, there is provided a computer program as set out in claim <NUM>.

According to a third aspect of the present invention, there is provided a computer readable medium as set out in claim <NUM>.

According to a fourth aspect of the present invention, there is provided an apparatus as set out in claim <NUM>.

The apparatus may be attached to the vehicle.

Where appropriate any of the optional features described above in relation to one of the aspects described herein may be applied to another one of the aspects described herein.

Embodiments are now described, by way of example only, with reference to the accompanying drawings, in which:.

In the figures, like parts are denoted by like reference numerals.

It will be appreciated that the drawings are for illustration purposes only and are not drawn to scale.

A model of a transport network is schematically shown in <FIG>. The transport network may be a guided transport network (e.g., railway, tramway) which has tracks to which movement of the vehicle (e.g., a train, a tram) is strictly confined in normal operations. Alternatively, the transport network may be a road or motorway network, which has tracks (e.g., roads or routes) for a vehicle (e.g., a car, a lorry) to follow. Alternatively, the transport network may be a waterway having tracks (e.g. a canal) constraining a route taken by a vehicle (e.g., a boat). In the following exemplary embodiments, it is generally assumed that the transport network is a railway, though it is to be understood that the techniques described herein are not limited to such.

The transport network is represented by vertices <NUM>-<NUM> and edges <NUM>-<NUM>. The vertices <NUM>-<NUM> are located along the tracks of the transport network. In an example, the positions of the vertices are known a priori. The vertices may be independent of the locations of fixed track-side infrastructure (e.g., balises) installed along the rail tracks. The vertices may also be referred to as support points in the following description. The edges <NUM>-<NUM> correspond to the tracks between the vertices <NUM>-<NUM>. A sequence of the vertices connected via the edges <NUM>-<NUM> forms a path for a vehicle. The accuracy of a path is determined by the density of the vertices.

In the presently described example, the model shown in <FIG> is stored digitally in a map database <NUM>. The map database <NUM> may also be referred to as a support point database. <FIG> provides an example of the configuration of the map database <NUM>.

In the example of <FIG>, the map database <NUM> includes four types of data entities, i.e., Edge Connections <NUM>, Edges <NUM>, Edge Vertices <NUM> and Vertices <NUM>. The connections in <FIG> are all one-to-many, with the single dash end representing the one end. Each of the data entities <NUM>-<NUM> has an "ID" attribute. The Edge Connections entity <NUM> further comprises a "From ID" attribute, a "From end" attribute and a "To ID" attribute. The Edges <NUM> entity further comprises a "Name" attribute. The Edge Vertices entity further comprises an "Edge ID" attribute, a "Vertex ID" attribute, a "Vertex order" attribute and an "Arc length" attribute. The Vertices entity <NUM> further comprises a "Location" attribute (storing location data such as, the Earth-Centred Earth-Fixed (ECEF) location of the respective vertex) and a "Name" attribute. The vertices <NUM>-<NUM> of <FIG> are represented by instances of the Vertices entity <NUM>. An instance of the Edges entity <NUM> may include all of the vertices between junctions. For example, three instances of the Edges entity <NUM> may be provided for the transport network shown in <FIG>, with the first instance including the vertices <NUM>, <NUM>, <NUM>, <NUM>, the second instance including the vertices <NUM>, <NUM>, <NUM> and with the third instance including the vertices <NUM>, <NUM>, <NUM>. Generally, it will be understood that references below to Edge Connections <NUM>, Edges <NUM>, Edge Vertices <NUM> and Vertices <NUM> refer to instances of the corresponding entities.

Edge Vertices <NUM> associate the Edges <NUM> with the Vertices <NUM> which are located on or at the ends of each respective one of the Edges <NUM>. In an example, the "Arc length" attribute of the Edge Vertices entity <NUM> indicates the arc length from the start (or the end) of an instance of the Edges <NUM> to a vertex included within that instance. In an alternative example, the parameter indicates the arc length from the start of an entire path (of which the instance of the Edges <NUM> is a part) to the vertex. The Arc length attribute provides the arc length of the tracks through the Vertices <NUM>.

The Edge Connections <NUM> define the connections between the Edges <NUM>. For example, an instance of Edge Connections entity <NUM> may define a "connected to" relationship between the first instance of the Edges entity <NUM> which includes the vertices <NUM>, <NUM>, <NUM>, <NUM>, and the second instance of the Edges entity <NUM> which includes the vertices <NUM>, <NUM>, <NUM> and the third instance of the Edges entity <NUM> which includes the vertices <NUM>, <NUM>, <NUM>.

The map database <NUM> may be stored locally on a vehicle <NUM> (a representation of which is shown in the model of <FIG>) which travels within the transport network, such that the vehicle can access the map database without requiring wireless communication channels. Alternatively, the map database <NUM> may be stored at a server (not shown) remote from the vehicle <NUM>. It will be appreciated that the map database <NUM> may take any suitable configuration, and is not limited to the example provided by <FIG>. In an example, the map database <NUM> is implemented using an SQL compliant database. It will be appreciated, however, that the map database <NUM> may be implemented in any way and need not be relational. For example, the database <NUM> may comprise flat files.

<FIG> schematically illustrates the vehicle <NUM>. The vehicle <NUM> carries a controller <NUM> and a sensor group. The sensor group includes sensors for monitoring the motion of the vehicle <NUM>. The sensor group includes at least one sensor <NUM> which provides information indicating reference positions of the vehicle and at least one sensor <NUM> which provides information indicating absolute positions of the vehicle.

Examples of the sensor <NUM> include a GNSS sensor, a balise reader, a video camera and a lidar detector. The sensor <NUM> relies upon data transmission between the vehicle <NUM> and external sources (e.g., satellite, or balises installed along the tracks of the transport network or a database containing information for scene matching) to provide absolution positional information for the vehicle <NUM>.

The sensor <NUM> includes an inertial measurement unit (IMU) <NUM>, and may include other sensors (e.g., a radar, a speed sensor, or an odometer). The IMU <NUM> may have a six degree-of-freedom (DOF) configuration and have one gyroscope and one accelerometer for each of three orthogonal axes. The accelerometer is for detecting the specific force, and the gyroscope is for detecting the angular rate. It would be appreciated that the IMU <NUM> may take a simpler configuration. The odometer may include a wheel-odometer, a visual-odometer, or a radar-odometer. In general, the odometer estimates the speed of the vehicle <NUM>. The IMU <NUM> may be a low-cost Micro-Electro-Mechanical Systems (MEMS) IMU.

The sensor <NUM> allows for dead reckoning navigation and does not rely upon signal transmission between the vehicle <NUM> and external sources. However, the sensor <NUM> provides information that can be used to compute a relative position of the vehicle <NUM> given a set of initial conditions. Further, the sensor <NUM> is subject to cumulative error and may undergo regular calibration procedures in order to maintain quality.

<FIG> schematically illustrates an exemplary structure of the controller <NUM>. The controller <NUM> comprises a CPU 9a which is configured to read and execute instructions stored in a RAM memory 9b which may be a volatile memory. The RAM 9b stores instructions for execution by the CPU 9a and data used by those instructions. The controller <NUM> further comprises non-volatile storage 9c, such as, for example, a hard disk drive, although it will be appreciated that any other form of non-volatile storage may be used. Computer readable instructions for execution by the CPU 9a may be stored in the non-volatile storage 9c. Further, the non-volatile storage 9c may store a copy of the map database <NUM>.

The controller <NUM> further comprises an I/O interface 9d to which peripheral devices used in connection with the controller <NUM> may be connected. The peripheral devices may include keyboard, data storage devices, etc. A communications interface 9i may also be provided. The communications interface 9i may provide for short range connections to other devices (e.g. the sensors <NUM>, <NUM>). The short range connections may be via Bluetooth, near-field communication (NFC), etc. The communications interface 9i may also provide for connection to networks such as the Internet or satellites, for longer range communication. The longer range communication may be used to retrieve the map database <NUM> if the map database <NUM> is stored in a server remote from the vehicle <NUM>. The longer range communication may also be used by a GNSS sensor to generate absolute positional information. The CPU 9a, RAM 9b, non-volatile storage 9c, I/O interface 9d and communications interface 9i are connected together by a bus 9j.

It will be appreciated that the arrangement of components illustrated in <FIG> is merely exemplary, and that the controller <NUM> may comprise different, additional or fewer components than those illustrated in <FIG>.

The computer readable instructions stored in the non-volatile storage 9c provide functional components as shown in <FIG>. The computer readable instructions when executed by the CPU 9a, causes the controller <NUM> to carry out the processing steps shown in <FIG> and <FIG> to determine a route taken by the vehicle <NUM> through a junction.

In particular, as illustrated in <FIG>, the computer readable instructions provide a data server <NUM>, a track-constraint manager <NUM>, and a navigation filter <NUM> which may also be referred to as a Bayesian estimation filter. In the particular example of <FIG>, the navigation filter <NUM> is implemented as an unscented Kalman filter. The navigation filter <NUM> includes a state predictor <NUM> and a state updater <NUM>.

The data server <NUM> is responsible for collecting and distributing the sensor data received from the sensors <NUM>, <NUM> to the navigation filter <NUM>. Two data streams are output by the data server <NUM>, i.e., (i) the inertial measurements collected from the IMU <NUM> and used as a control input to drive the state predictor <NUM>, and; (ii) all non-IMU sensor measurements used to drive the state updater <NUM>.

The track-constraint manager <NUM> manages the interface between the navigation filter <NUM> and the map database <NUM> and provides an up-to-date track constraint function xE(s) to a constrained strapdown inertial navigation system (INS) <NUM> (shown in <FIG>) of the state predictor <NUM>.

The track constraint function xE(s) comprises three-dimensional curves that provide an approximation of the centrelines of the tracks within a part of the transport network which is in the vicinity of the vehicle <NUM>. The track constraint function xE(s) may be considered as an example of the "track geometry data" since it indicates the geometry of the tracks of the transport network. Given that the vehicle <NUM> is expected to follow the tracks, the track constraint function xE(s) thus represents the constraint applied by the tracks of the transport network to the kinematic state of the vehicle. The parameter s is the arc-length of the centreline of the track from a chosen reference point. The superscript E is used to indicate that the track constraint function is defined with respect to the E-frame, which is a rotating frame of reference, fixed in the body of the Earth, and represented by a right-handed Cartesian set of axes with origin located at the centre of the Earth (i.e., the ECEF coordinate system).

The track constraint function xE(s) is constructed on-the-fly by the track constraint manager <NUM> from a set of support points (i.e., instances of the Vertices entity <NUM>) stored in the map database <NUM>. The track constraint manager <NUM> uses a latest estimated state x̃ output by the state predictor <NUM> (which includes the estimated position of the vehicle <NUM> as described below) to query the database <NUM>, and identifies all support points in a local neighbourhood around the estimated position of the vehicle <NUM> and all possible trajectories that the vehicle <NUM> could then follow. The identified support points characterise the local track geometry and topology so that track constraints can be constructed for all possible routes passing through the estimated position of the vehicle <NUM>. The track constraint manager <NUM> may construct multiple track constraint functions if there are multiple routes within the neighbourhood of the estimated position of the vehicle <NUM>.

In an example as shown in <FIG>, when the track constraint manager <NUM> determines that the vehicle <NUM> is between the vertices <NUM> and <NUM>, the track constraint manager <NUM> identifies all support points within a range of three vertices at either side of the current estimate of the position of the vehicle <NUM> from the map database <NUM>. The identified support points are vertices <NUM> to <NUM> shown in <FIG>. In practice, the track constraint manager <NUM> may identify, for example, twenty support points on either side of the vehicle <NUM> in the vicinity of the current estimate of the position of the vehicle <NUM>. It has been found that twenty support points will generally provide sufficient information to understand the track geometry in the neighbourhood of the vehicle <NUM>. The support points are identified dynamically according to the current estimate of the position of the vehicle <NUM>.

The possible trajectories that the vehicle <NUM> could follow can be easily determined based upon the Edge Vertices <NUM> and the Edge Connections <NUM> associated with the identified support points. For example, when the track constraint manager <NUM> determines that the vehicle <NUM> is between the vertices <NUM> and <NUM> and travelling towards the vertex <NUM>, the track constraint manager <NUM> can determine that there are two possible paths (i.e., via edge <NUM> or edge <NUM>) for the vehicle <NUM> to take after it passes the vertex <NUM>. Each trajectory is defined by a subset of the identified support points, accounting for any implicit constraints in the transport network. The movement direction of the vehicle <NUM> may be directly measured by the sensors <NUM>, <NUM>, or alternatively may be indicated in the latest estimated state x̃ output by the state predictor <NUM>.

The track constraint manager <NUM> constructs the track constraint function xE(s) as a polynomial function using the identified support points and the possible trajectories. The track constraint function xE(s) is constructed by interpolating the identified support points. In an example, the track constraint function xE(s) has continuous second-order derivatives through each of the identified support points.

The use of polynomial function with continuous second-order derivatives to interpolate the identified support points is advantageous in that it allows the centrelines of the tracks to be modelled more accurately than would be possible with a conventional piecewise linear interpolation using the same number of support points. Railway tracks, for example, (and most of the motorways) are constructed to enable safe and comfortable motion with low lateral jerk. Therefore, the variation in track curvature is smooth. As a consequence, a twice differentiable curve with a continuous second-order derivative is a suitable mathematical model for the track. Indeed, unlike the more conventional piecewise linear interpolation schemes, such a model provides a powerful constraint on the to-be-estimated kinematic state (e.g., position, direction of motion and the rate-of-change of direction of motion) of the vehicle without discontinuities.

In a particular example, the track constraint function xE(s) is a cubic spline, and the parameter s is the arc-length along the spline from a chosen reference point. A cubic spline is a spline interpolation constructed of piecewise third-order polynomials which pass through a set of control points (e.g., the identified support points). By construction, the cubic spline implicitly provides a twice differentiable curve with a continuous second-order derivative. Furthermore, the cubic spline yields the smallest norm of strain energy amongst all of the twice differentiable functions with a continuous second-order derivative that interpolate a set of supporting points and satisfy the same end-point conditions. The result obtained by the cubic spline interpolation is a curve progression with minimal oscillations between the supporting points.

The track constraint manager <NUM> may calculate the coefficients of a cubic spline parameterised by arc length, based upon the positions of the identified support points, the boundary conditions (either natural or fixed) of the cubic spline and the continuity requirements of first-order and second-order derivatives. To facilitate the calculation of the cubic spline by the track constraint manager <NUM>, the support points stored in the map database <NUM> may be processed, for example, by a server (not shown), during the construction of the map database <NUM> based upon survey data in accordance with the processing steps shown in <FIG> and described below. In an example, the survey data includes discrete points sampled along the tracks of the transport network and includes information indicating the positions of the discrete points. The server which performs the steps of <FIG> may be remote from the vehicle <NUM> and be responsible for constructing the map database <NUM> based upon the survey data.

At step S11, the server calculates the coefficients of a conventional cubic spline parameterised by chord length based upon the survey data. In particular, the calculation is based upon the positions of at least some of the discrete points included within the survey data, the boundary conditions (either natural or fixed) of the cubic spline and the continuity requirements of first-order and second-order derivatives. At step S12, the server computes a new set of support points which are equally spaced along the arc-length of the cubic spline calculated at step S11. At step S13, the server calculates the coefficients of a cubic spline parameterised by arc-length, using the new set of support points calculated at step S12, the boundary conditions and the continuity requirements of first-order and second-order derivatives. The server may iterate through steps S12 and S13 in order to yield better results (e.g., to ensure that the new set of support points calculated at step S12 are substantially equally spaced along the re-parameterised cubic spline calculated at step S13). The new set of support points calculated at step S12 may be stored in the map database <NUM> as support points.

In general, the track constraint function xE(s) represents an a priori knowledge of the track geometry of the transport network, and is useful for improving the navigation accuracy of the navigation filter <NUM>. In particular, the track constraint function xE(s) improves the observability of errors in both the vehicle's navigation solution and the errors in the sensor measurements (e.g., the biases in the inertial measurements of the IMU <NUM>) output by the sensors <NUM>, <NUM> which are used to calculate the navigation solution. Further, when used in the navigation filter <NUM> as described below, the track constraint function xE(s) reduces the problem of estimating an unconstrained three-dimensional motion of the vehicle <NUM> to the problem of estimating a one-dimensional constrained motion of the vehicle <NUM> along the track of the transport network, because the vehicle <NUM> has only one degree of freedom along the track.

By constructing the track constraint function xE(s) on-the-fly from a small subset of support points in the vicinity of the current estimate of the position of the vehicle <NUM>, the track constraint function xE(s) focuses upon the local constraints imposed on the vehicle by a small part of the transport network (instead of the entire transport network) in the neighbourhood of the vehicle, and the track constraint function xE(s) can be computed with great flexibility and efficiency. The local constraints provide sufficient information for estimating the kinematic state of the vehicle <NUM>. Indeed, it has been realised that the geometry of the tracks far away from the vehicle <NUM> is less useful (if at all) for determining the kinematic state of the vehicle <NUM>.

For ease of description, the dashed lines <NUM> and <NUM> shown in <FIG> represent the curved track centrelines defined by the track constraint function xE(s) which is constructed by cubic spline interpolation through the vertices <NUM>, <NUM> and <NUM>. It is assumed that the tracks between the vertices <NUM> to <NUM> and <NUM> and <NUM> are straight, and therefore the edges <NUM> to <NUM> represents the track centrelines defined by the track constraint function xE(s).

The navigation filter <NUM> utilises a Bayesian filter algorithm and includes prediction and update steps for estimating the state x̃ of the vehicle <NUM>. The estimated state x̃ includes the kinematic state of the vehicle <NUM> (e.g., the position, velocity and/or attitude of the vehicle <NUM>). The estimated state x̃ further includes states (e.g., the biases in the inertial measurements of the IMU <NUM>) which are required to represent the kinematic state of the vehicle <NUM> and which may be determined in order to minimise the accumulation of errors in the estimation process.

In the prediction step <NUM>, the state predictor <NUM> of the navigation filter <NUM> calculates the mean of the estimated state x̃ (also called an a priori estimate) at step <NUM> and its associated error covariance P̃ at step <NUM>, based upon the track constraint function xE(s), the IMU measurement data <MAT> and sB, and the estimate state x̂ and its associated error covariance P̂ generated during the previous update step <NUM> by the state updater <NUM> (or the estimated state x̃ and its associated error covariance P̃ generated during the previous prediction step <NUM> if there is no available non-IMU measurement as described below).

The IMU measurement data may be referred to as "first sensor data". The non-IMU measurement data may be referred to as "second sensor data".

The prediction step <NUM> is data driven by the inertial measurements collected from the IMU <NUM>. The prediction step <NUM> is used to propagate the estimated state forward in time between updates of the non-IMU sensor measurement.

When a prediction step <NUM> completes, the data server <NUM> is queried at step <NUM> to determine if there is any pending non-IMU sensor measurement. If there is no available non-IMU measurement, then the prediction step <NUM> is repeated with each incoming IMU measurement data until a time when non-IMU measurement data becomes available. Further, when a prediction step <NUM> completes, the newly estimated state x̃ and its associated error covariance P̃ are passed to the track constraint manager <NUM> in order to provide up-to-date track constraint function xE(s).

When a non-IMU sensor measurement y is available from the data server <NUM>, the update step <NUM> is triggered. In the update step <NUM>, the state updater <NUM> of the navigation filter <NUM> calculates the mean of the estimated state x̂ (also called an a posteriori estimate) and its associated error covariance P̂ at step <NUM>, based upon the a priori estimate x̃ and its associated error covariance P̃ and the non-IMU sensor measurement y. The estimated state x̂ and its associated error covariance P̂ are subsequently supplied to the prediction step <NUM> of the next epoch.

After the completion of a prediction or an update step, the mean of the estimated state x̃ or x̂ is available for the controller <NUM> to generate an output indicative of a position of the vehicle <NUM> within the transport network.

In this way, the navigation filter <NUM> is able to provide a relatively accurate estimate of the state of the vehicle <NUM> by fusing the IMU sensor measurement and the non-IMU sensor measurement with the track constraint function xE(s). Due to the nature of the Bayesian estimation filter (which filters noise instead of data), the noise contained in the estimated state of the vehicle <NUM> is of a lesser extent than either the process noise contained in the state process model (e.g., constrained strapdown INS <NUM>) or the measurement noises contained in the IMU sensor measurement and the non-IMU sensor measurement.

In the particular example provided by <FIG>, the navigation filter <NUM> is implemented as an unscented Kalman filter. The unscented Kalman filter is suitable for handling non-linear process models and does not require the evaluation of explicit, or numerically approximated, Jacobians for the process models (which are however required by the extended Kalman filter).

In the unscented Kalman filter, an unscented transform is used to estimate the mean and error covariance of the state as it evolves in time according to a non-linear process model. The fundamental assumption is that the distribution of the estimated state can be approximated well by a Gaussian distribution at all times, in particular, after the state x̂ is propagated through the state process model (represented by the constrained strapdown INS <NUM> in the example of <FIG>), and after the state x̃ is adjusted to incorporate the non-IMU sensor measurement y output by the data server <NUM>.

The unscented transform is performed utilising a carefully constructed set of sample points (referred to as "sigma points") from the mean of the estimated state (e.g., x̃, x̂) and its associated error covariance (e.g., P̃, P̂). The number of the sigma points is 2N+<NUM>, with N being the dimension of the state. The 2N+<NUM> sigma points includes one point equal to the mean of the estimated state, and 2N points arranged symmetrically about the mean. The calculation of the sigma points for an unscented Kalman filter is described in references [<NUM>]-[<NUM>] and is well known in the field of art. The sigma points are transformed through the relevant system models and the resulting transformed points are used to recover estimates of the mean and the error covariance of the transformed state.

The prediction step <NUM> and the update step <NUM> of the navigation filter <NUM> are described in more detail below with reference to the state predictor <NUM> and the state updater <NUM>.

The state predictor <NUM> generates at step <NUM> a set of sigma points which are denoted by <MAT>, based upon the current mean of the estimated state x̂ and its associated error covariance P̂ generated during the update step <NUM> of the previous epoch.

The state predictor <NUM> further includes a constrained strapdown INS <NUM> (shown in <FIG>) which serves as a state process model and performs step <NUM> to propagate the estimated state forward in time. The IMU measurement data <MAT> and sB are used as a control input to the constrained strapdown INS <NUM> during step <NUM>. The constrained strapdown INS <NUM> integrates the IMU measurement data <MAT> and sB to determine the position, velocity and attitude of the IMU <NUM> relative to an Earth-Centred Earth-Fixed (ECEF) Cartesian coordinate frame-of-reference. Because the IMU <NUM> is strapped down to the vehicle <NUM>, the position, velocity and attitude of the IMU <NUM> are equivalent to or closely related to the position, velocity and attitude of the vehicle <NUM>, which are a part of the estimated state (e.g., x̃, x̂) of the navigation filter <NUM>.

The measurement data sB refers to the specific force resolved in a B-frame, which is a rotating frame-of-reference that is fixed with respect to the vehicle <NUM> that carries the IMU <NUM>. The B-frame is represented by a right-handed Cartesian set of co-ordinate axes with origin located at the centre-of-mass of the IMU <NUM> which is itself strapped down rigidly to the vehicle <NUM>. The B-frame includes an x<NUM> axis which is parallel to the centreline of the vehicle <NUM>, and is therefore fixed relative to the vehicle <NUM>. The measurement data <MAT> refers to the angular rate of the B frame relative to an I-frame resolved in the B frame. The I-frame is a Newtonian inertial frame of reference that is represented by a right-handed Cartesian set of axes with origin located at the centre of the Earth and coincident with the E-frame at some chosen reference epoch (e.g., t = t<NUM>).

The term "constrained" in the name of the INS <NUM> means that, unlike conventional strapdown inertial navigation algorithms, the integration carried out by the INS <NUM> which determines the position of the IMU <NUM> based upon velocity data is modified through incorporation of the track constraint function xE(s) so that the final position produced is constrained to be on the curve of the track constraint function xE(s). This ensures that the state predictor <NUM> can never produce a position solution that lies off the track.

An example of the constrained strapdown INS <NUM> is illustrated in <FIG>. The INS <NUM> outputs data <NUM>, <NUM> and <NUM>. Data <NUM>, <MAT>, is the attitude (i.e., orientation) of the B-frame relative to the I-frame, which is equal to the attitude of the vehicle <NUM> relative to the I-frame. Data <NUM>, <MAT>, is the velocity of the IMU <NUM> relative to the E-frame. Data <NUM>, <MAT>, is the position of the IMU <NUM> relative to the E-frame.

The constrained strapdown INS <NUM> has a T-frame integrator <NUM>, which integrates the IMU measurement data <MAT> and sB to calculate data <NUM> and a thrust inertial velocity <MAT> of the IMU <NUM>. The T-frame integrator <NUM> has an initial condition <NUM>, which includes, for example, an initial value of a thrust inertial velocity of the IMU <NUM> and an initial value of an attitude rotation matrix resolving the I-frame coordinates into coordinates of a thrust velocity frame (i.e., the T-frame). The T-frame is an orthogonal right-handed co-ordinate frame-of-reference with axes parallel to those of the B frame. The translation of the origin of the T frame relative to the I-frame, i.e. the centre of the Earth, is defined to be the thrust velocity <MAT>.

The constrained strapdown INS <NUM> further comprises a G-frame integrator <NUM> which calculates a gravitational inertial velocity <MAT> of the IMU <NUM>, by integrating data <NUM>, <MAT>, which is the Earth's angular rate and gE which is the Earth's gravitation. <MAT> is a known parameter for the INS <NUM>. gE is determined at block <NUM> using standard Earth models based upon the value of the data <NUM> previously calculated by an R-frame integrator <NUM>.

The G-frame integrator <NUM> has an initial condition <NUM> which includes, for example, an initial value of the gravitational inertial velocity <MAT> and an initial value of an attitude rotation matrix resolving coordinates of a gravitational velocity frame (i.e., the G-frame) into the I-frame coordinates. The G-frame is an orthogonal right-handed co-ordinate frame of reference with axes parallel to those of the E-frame. The translation of the origin of the G-frame relative to the I-frame is defined to be exactly the gravitational velocity <MAT>.

The T-frame integrator <NUM> and G-frame integrator <NUM> are not affected by the track constraint function xE(s), and may be implemented using well-known algorithms.

The thrust inertial velocity <MAT> and the gravitational inertial velocity <MAT> of the IMU <NUM> are then combined at an adder <NUM> to generate <MAT>, which is the true inertial velocity of the IMU <NUM>. The true velocity of the IMU <NUM> <MAT>, data <NUM> and data <NUM> are then supplied to an R-frame integrator <NUM> for calculating data <NUM> and data <NUM>.

The R-frame integrator <NUM> integrates the speed of the R-frame along the track, i.e., the first-order derivative Ṡ of the arc-length s along the tracks from a chosen reference point, and subsequently uses the integration of Ṡ with the track constraint function xE(s) to derive the velocity and the position of the IMU <NUM>. The arc-length s is a function of time t.

Ṡ can be calculated according to Equation (<NUM>) as follows: <MAT> In Equation (<NUM>), <MAT>.

RR (t) is an attitude rotation matrix resolving the coordinates of an R-frame into E-frame coordinates. The R-frame, is attached to a fixed point-of-reference on a bogie of the vehicle <NUM> and therefore moves along the associated shifted version of the track centreline. The R-frame has an x<NUM> axis which is parallel to the centreline of the track and points forward. The track constraint function xE(s) therefore gives the E-frame position co-ordinates of the R-frame and the tangent vector of the track constraint function xE(s) gives the R-frame's x<NUM> axis direction resolved in E-frame coordinates. <MAT> is an attitude rotation matrix resolving B-frame coordinates into E-frame coordinates. <MAT> and <MAT> can be calculated using the integration products of the T-frame integrator <NUM> and the G-frame integrator <NUM>.

In Equation (<NUM>), <MAT> is the latest value of data <NUM> calculated in the previous cycle by the R-frame integrator <NUM>. Further, <MAT> represents the position of the B-frame relative to the R-frame resolved in the B-frame. <MAT> is modelled as a fixed lever-arm vector. In practice, the lever-arm vector may not be constant due to small rotations of the B-frame relative to the R-frame when, for example, the vehicle <NUM> travels around a bend and the IMU <NUM> is not mounted directly above the R-frame. Further, <MAT> of Equation (<NUM>) can be calculated as follows: <MAT> <MAT> is the IMU measurement data output by the IMU <NUM>. <MAT> is data <NUM>.

Subsequently, the R-frame integrator <NUM> integrates Ṡ over time under an initial condition <NUM> to update the value of the arc-length s. The initial condition <NUM> includes an initial value of the arc-length s and/or the chosen reference point.

The track constraint function xE(s), represented by block <NUM>, is supplied to the R-frame integrator <NUM>. The E-frame position co-ordinates of the R-frame, i.e., <MAT>, is equivalent to the value of the track constraint function xE(s) at the integrated value of Ṡ. That is, <MAT> <MAT> can be further used to update the value of data <NUM> and data <NUM> according to Equations (<NUM>) and (<NUM>): <MAT> <MAT> <MAT> is an attitude rotation matrix resolving the I-frame coordinates into the E-frame coordinates, and is determined by the G-frame integrator <NUM> since the axes of the G-frame are parallel to those of the E-frame.

Thus, the R-frame integrator <NUM> integrates the along-track speed of the IMU <NUM> to obtain distance travelled by the IMU <NUM> along the curve of the track constraint function xE(s) relative to a chosen reference point. The constrained strapdown INS <NUM> therefore encapsulates the time evolution function for the primary navigation sub-states of interest, namely: (i) the B-frame attitude relative to I-frame, i.e., data <NUM>; (ii) the B-frame velocity relative to the I-frame, i.e., <MAT>; and (iii) the distance along-track of the R-frame, i.e., s(t).

Since the integration carried out by the R-frame integrator <NUM> incorporates the track constraint function xE(s), the position (i.e., data <NUM>) of the IMU <NUM> determined by the INS <NUM> is constrained to be on the curve of the track constraint function xE(s). The position (i.e., data <NUM>) of the IMU <NUM> is a part of the estimated state (e.g., x̃, x̂) of the navigation filter <NUM>.

The T-frame integrator <NUM> and the G-frame integrator <NUM> are executed in parallel. Each of the T-frame integrator <NUM> and the G-frame integrator <NUM> provides one updated data sample (including, <MAT> (which is equal to <MAT>) and <MAT> provided by the T-frame integrator <NUM>, and <MAT> (which is equal to <MAT>) and <MAT> provided by the G-frame integrator <NUM>) after receiving a number of incoming IMU data samples. This is referred to as a minor-loop update. The R-frame integrator <NUM> receives a number of data samples from the T-frame integrator <NUM> and the G-frame integrator <NUM> in order to complete an update cycle, which is referred to as major-loop update. Therefore, the R-frame integrator <NUM> is updated at a slower rate than the T-frame integrator <NUM> and the G-frame integrator <NUM>.

As shown in <FIG>, each of the sigma-points <MAT> generated at step <NUM> is passed through the constrained strapdown INS <NUM>, which generates sigma points <MAT>. The values of the sigma-points <MAT> provide at least a part of the initial conditions <NUM>, <NUM> and <NUM>.

The resulting sigma points <MAT> are subsequently used to calculate the mean of a new estimated state and its covariance, x̃,P̃ at steps <NUM>, <NUM>. This is based upon an assumption that the estimated state can be approximately by a Gaussian distribution.

The mean of a new estimated state and its covariance, x̃ ,P̃ (also referred to as an a priori estimate) are provided to the track constraint manager <NUM> to query the map database <NUM>, such that the track constraint manager <NUM> dynamically provides an up-to-date track constraint function xE(s) in the vicinity of the estimated position of the vehicle <NUM> to the constrained strapdown INS <NUM>.

If it is determined at step <NUM>, after querying the data server <NUM>, that there are no available non-IMU measurements, the new filter state mean and covariance, x̃,P̃ are processed at step <NUM> to generate a new set of sigma points <MAT> to initiate the state of the constrained strapdown INS <NUM>.

The state updater <NUM> is triggered to start an update step <NUM> based upon the a priori estimate x̃ and its error covariance P̃, if non-IMU sensor measurement data is available from the data server <NUM>.

The non-IMU sensor measurement data may be of different measurement types. For example, the non-IMU sensor measurement data may be provided by a wheel-, visual- or radar-odometer or a radar, which is a part of the sensor <NUM> providing reference positional information. Further or alternatively, the non-IMU sensor measurement data may be provided by a balise reader or a GNSS sensor, which is a part of the sensor <NUM> providing absolute positional information.

For each measurement type, the state updater <NUM> comprises a measurement model that characterises both the quality of the measurement and the functional relationship between the measurement and the state in the absence of measurement errors.

Thus, the state updater <NUM> creates at step <NUM> an appropriate measurement model for each measurement y from a particular non-IMU sensor. The created measurement model comprises: (i) R, the error covariance characterising the measurement noise process, assumed that the measurement noise has zero-mean and satisfies Gaussian distribution; and (ii) a function h(x) which maps the state space into the measurement space. By using the function h(x) and a state x̃, a predicted measurement ỹ can be calculated as ỹ = h(x̃). The function h(x) may also be referred to as an observation model.

The measurement model may be created suitably, based upon the measurement type and the elements of the state.

Having created the function h(x), the unscented transform is again used within the update step <NUM> to compute the Kalman gain that is used to incorporate new information into the estimated state.

In particular, the state updater <NUM> constructs at step <NUM> a set of 2N+<NUM> sigma points from the a priori estimate x̃ and its error covariance P̃. The set of sigma points are denoted as <MAT>.

The state updater <NUM> subsequently propagates each of the sigma points <MAT> through the function h(x) to obtain a set of predicted measurement sigma points <MAT> at step <NUM>. The measurement sigma points are calculated according to Equation (<NUM>): <MAT>.

The state updater <NUM> further calculates the Kalman gain, and the a posteriori estimate (i.e., the updated state estimate) at step <NUM>, based upon the sigma points <MAT>, <MAT>, the non-IMU measurement data y, and the measurement noise R.

In particular, the state updater <NUM> calculates at step <NUM> an error covariance <MAT> of the predicted measurements using <MAT> and the measurement noise R. The state updater <NUM> further calculates at step <NUM> a cross covariance <MAT> of the estimated state and the predicted measurement using <MAT> and <MAT>.

The state updater <NUM> then calculates at step <NUM> the Kalman gain K based upon the error covariance <MAT> and the cross covariance <MAT> according to Equation (<NUM>): <MAT>.

The difference between the measurement y and the mean (denoted by ỹ) of the sigma points <MAT> is called "innovation". The innovation encapsulates the new information introduced into the update step <NUM>. The innovation is used in conjunction with the Kalman gain and the a priori estimate x̃ and its error covariance P̃ to compute the a posteriori estimate x̂ and its error covariance P̂. In particular, the a posteriori estimate x̂ and its error covariance P̂ are calculated according to Equations (<NUM>) and (<NUM>). <MAT> <MAT>.

The track constraint function xE(s) may be further exploited during the update step. How this is done is dependent upon the application and whether the constraint can be treated as absolute or approximate. For example, if the vehicle <NUM> is a train and vibration of the vehicle <NUM> is ignored, the orientation of the train travelling along a railway track can be determined from the track constraint function xE(s) which represents the centreline of the track and the position of the train. It would be appreciated that only pitch and yaw of the train can be determined from the track constraint function xE(s), but not roll. Roll, however, can be constrained to a certain degree if the cant of the train tracks is also incorporated into the map database <NUM>. It would be understood that the cant of the tracks may only provide a part of the total roll of the train because the train tends to roll on its suspension when travelling around bends. Hence, a very accurate pseudo-measurement of the attitude (in particular, the pitch and yaw components of the attitude) of the train can be derived based upon the track constraint function xE(s) and the estimated position of the train indicated in the a priori estimated state x̃. The roll component of the attitude may also be estimated by imposing a dynamical constraint - e.g., a train stays on the track and therefore the only acceleration acting is centripetal acceleration.

The pseudo-measurement of the attitude of the train is essentially the direction of the x<NUM> axis of the B-frame from the track constraint function xE(s) given the arc-length parameter s. The pseudo-measurement may be constructed from the tangent and curvature values of the track constraint function xE(s) at an estimated position (which is a part of the estimated state x̃) of the train.

The term "pseudo" means that the measurement is not a real measurement obtained from any of the sensors <NUM>, <NUM>, but is used as a measurement together with the non-IMU sensor measurement in the update step. In particular, the pseudo-measurement of the attitude of the train may be supplied to the state updater <NUM> as a measurement y. The state updater <NUM> computes the measurement model h(x) for the pseudo-measurement at step <NUM>, and the pseudo-measurement is used to compute the Kalman gain K and the a posteriori estimate x̂ and its error covariance P̂.

It would be appreciated that if the vehicle <NUM> is a car and the transport network is a road network, a similar pseudo-measurement of the attitude of the car can also be applied to the state updater <NUM>, but with less accuracy because cars have greater freedom and higher manoeuvrability within the road network.

For implementation purpose, an update of the pseudo-measurement of the attitude may be applied intermittently. For example, an update of the pseudo-measurement of the attitude may be applied together with every visual odometer measurement update.

The utilisation of the track constraint function xE(s) within both of the prediction and the update steps <NUM>, <NUM> of the navigation filer <NUM> as described above constitutes optimal use of the track constraint information. By imposing the track constraint function xE(s) in the constrained strapdown INS <NUM>, the evolution of the estimate of the position of the vehicle <NUM> is always consistent with one-dimensional motion along the track centreline represented by the track constraint function xE(s), and the state prediction (e.g., x̃) and the propagation of the state estimate errors (e.g., P̃) generated by the state predictor <NUM> are kept consistent with the track constraint function xE(s). By further imposing the track constraint function xE(s) as a pseudo-measurement in the update step, the state updater <NUM> only adjusts the estimate of the position of the vehicle <NUM> along the track centreline represented by the track constraint function xE(s). The track constraint function xE(s) improves both the mean of the estimated state (e.g., x̃, x̂) and the associated error covariance (e.g., P̃, P).

The controller <NUM> further carries out processing steps for determining a route which the vehicle <NUM> takes through a junction of the transport network. The processing steps are shown in <FIG> and described below.

At step S1, the controller <NUM> (in particular, the track-constraint manager <NUM> of <FIG>) obtains the track geometry data (e.g., the track constraint function xE(s) and/or the network topology stored in the map database <NUM>) which indicates the track geometry of at least a part of the transport network in the vicinity of the vehicle <NUM>. The track constraint function xE(s) is obtained on-the-fly dynamically based upon the current position of the vehicle <NUM>. The current position of the vehicle <NUM> may be an initial position of the vehicle <NUM>, with the initial position being determined by the sensor <NUM> which provides information indicating an absolute position of the vehicle <NUM>, Alternatively, the current position of the vehicle <NUM> may be the latest estimated position of the vehicle <NUM> indicated within the estimated state x̃ output by the state predictor <NUM>.

At step S2, the controller <NUM> determines whether the vehicle <NUM> is approaching a network junction based upon the track constraint function xE(s) and an estimated position of the vehicle <NUM>. The estimated positon is generated by the navigation filter <NUM> of <FIG> as described above.

In the example of <FIG>, the vehicle <NUM> is moving downwards from the vertex <NUM> to the vertex <NUM>, and vertex <NUM> is a network junction because it provides two possible route options. Based upon the track constraint function xE(s) and the movement direction of the vehicle <NUM>, the controller <NUM> determines that the vehicle <NUM> is approaching a network junction represented by the vertex <NUM>. The movement direction of the vehicle <NUM> may be directly measured by the sensors <NUM>, <NUM>, or alternatively may be indicated in the estimated state x̃ output by the state predictor <NUM>.

If it is determined at step S2 that the vehicle <NUM> is not approaching a network junction, steps S1 and S2 would be repeated until it is determined that the vehicle <NUM> is approaching a network junction.

If it is determined at step S2 that the vehicle <NUM> is approaching a network junction, the processing moves to step S3 where the controller <NUM> determines the route options from the network junction. The route options may be determined based upon the network topology stored in the map database <NUM>, in particular, the instances of the Edge Connections <NUM>. In the example of <FIG>, the controller <NUM> queries the map database <NUM> and determines that the network junction (i.e., the vertex <NUM>) provides a first route option towards the vertex <NUM> and a second route option towards the vertex <NUM>. Subsequently, the controller <NUM> may obtain the track constraint function xE(s) which indicates the track geometry of each of the route options separately.

At step S4, the controller <NUM> generates a plurality of Bayesian estimation filter algorithms each associated with a possible route option determined at step S3. Each of the plurality of Bayesian estimation filter algorithms provides a navigation solution (including an estimated position of the vehicle <NUM>) which is constrained by the track geometry data indicative of the associated route option.

In an example, the plurality of Bayesian estimation filter algorithms may be copies of the navigation filter <NUM>. That is, the controller <NUM> generates a first copy of the navigation filter <NUM> which estimates the position of the vehicle <NUM> based upon the track constraint function xE(s) indicating the first route option towards the vertex <NUM> (as if the second route option does not exist). The controller further generates a second copy of the navigation filter <NUM> which estimates the position of the vehicle <NUM> based upon the track constraint function xE(s) indicating the second route option towards the vertex <NUM> (as if the first route option does not exist).

<FIG> schematically illustrates the result provided by the first copy of the navigation filter <NUM>. The first copy estimates that the position of the vehicle <NUM> is at position <NUM> with an error range of <NUM>.

<FIG> schematically illustrates the result provided by the second copy of the navigation filter <NUM>. The second copy estimates that the position of the vehicle <NUM> is at position <NUM> with an error range of <NUM>.

Because the navigation filter <NUM> would not produce an estimated position of the vehicle <NUM> which is off the track defined by the track constrained function, the estimated positions <NUM>, <NUM> lie on the route options associated with the copies of the navigation filter <NUM>.

Based upon the estimated position of the vehicle <NUM>, each copy of the navigation filter <NUM> further determines the supposed moving direction of the vehicle <NUM> (i.e., the pseudo-measurement of the attitude of the vehicle as described above). The pseudo-measurement is calculated as a tangent of the the track constraint function xE(s) at the estimated position of the vehicle <NUM>, and may also be referred to as an "attitude pseudo-measurement".

In the example of <FIG>, the first copy of the navigation filter <NUM> determines that the pseudo-measurement of the attitude of the vehicle <NUM> is along arrow <NUM>. In the example of <FIG>, the second copy of the navigation filter <NUM> determines that the pseudo-measurement of the attitude of the vehicle <NUM> is along arrow <NUM>.

The pseudo-measurements are fed into each copy of the navigation filter <NUM> as a part of the measurement y. Therefore, the innovation value output by each copy of the navigation filter is affected by the value of the respective pseudo-measurement.

As described above, the innovation represents a difference between the estimated state and the measurement. The difference is small when the estimated state resembles the true state which is observed by the measurement. Conversely, the difference is large when the estimated state differs from the true state. It would be understood that if, for example, the vehicle <NUM> indeed takes the first route option towards the vertex <NUM>, then the pseudo-measurement <NUM> would approximate the real attitude of the vehicle <NUM>. Thus the innovation of the first copy of the navigation filter <NUM> would have a small value. If, for example, the vehicle <NUM> takes the second route option towards the vertex <NUM>, then the pseudo-measurement <NUM> would significantly differ from the real attitude of the vehicle <NUM>. Thus, the innovation of the first copy of the navigation filter <NUM> would have a greater value.

Therefore, the innovation values output by the plurality of Bayesian estimation filter algorithms indicate probabilities of the vehicle <NUM> taking the respective route option provided by the network junction.

The plurality of Bayesian estimation filter algorithms may be executed in parallel. In one example, the plurality of Bayesian estimation filter algorithms are implemented as separate threads running in parallel. In an alternative example, the plurality of Bayesian estimation filter algorithms are implemented on separate processors. In any event, each one of the plurality of Bayesian estimation filter algorithms is synchronised with the measurements of the sensors <NUM>, <NUM>.

At step S5, the controller <NUM> monitors the output (in particular, the innovation values) of the plurality of Bayesian estimation filter algorithms when the vehicle <NUM> moves through the junction. That is, each of the plurality of Bayesian estimation filter algorithms recursively predicts the position of the vehicle <NUM> based upon the IMU measurements and the track constraint function indicative of the associated route option, determines the attitude pseudo-measurement of the vehicle <NUM> at the predicted position, and calculates the innovation value during the update phase based upon the predicted position, the attitude pseudo-measurement and the non-IMU sensor measurements.

In practice, because it is unknown how long it would take the vehicle <NUM> to move through the junction, the controller <NUM> may monitor the output of the plurality of Bayesian estimation filter algorithms for a fixed time period. The fixed time period is set to be long enough for a vehicle to cross a junction. Alternatively, the the controller <NUM> may monitor the output of the plurality of Bayesian estimation filter algorithms until the output clearly diverge from one another.

<FIG> show the plots of the innovation values calculated by the first copy and the second copy of the navigation filter <NUM>. The Y axis of each plot indicates the magnitude of the innovation values. The X axis of each plot represents time. Curve <NUM> is output by the first copy of the navigation filter <NUM> which is assigned to the first route option towards the vertex <NUM>. Curve <NUM> is output by the second copy of the navigation filter <NUM> which is assigned to the second route option towards the vertex <NUM>. In <FIG>, the vehicle <NUM> takes the second route option. In <FIG>, the vehicle <NUM> takes the first route option.

From these plots, it is clear that the innovation value for the correct route is significantly lower than that of the incorrect route.

At step S6, the controller <NUM> determines the route option taken by the vehicle <NUM> by selecting one of the plurality of route options which presents the highest probability based upon the output of the plurality of Bayesian estimation filter algorithms.

In an example, the controller <NUM> selects the route option which provides the lowest average innovation value over a time period during which the vehicle <NUM> passes through the network junction.

In another example, after each attitude pseudo-measurement is obtained, the controller <NUM> compares the innovations output by the plurality of Bayesian estimation filter algorithms against thresholds. For the algorithms which provide innovations above the thresholds, the controller <NUM> determines that the vehicle <NUM> is unlikely to take the route options which those algorithms are associated with. Those unlikely route options would be withdrawn from consideration and those Bayesian estimation filter algorithms associated with those unlikely route options would be culled. The remaining route option after the network junction has been traversed has the highest probability for the vehicle <NUM> to take. If the vehicle <NUM> has travelled a predetermined distance after passing the junction, and there are still more than one route options which do not produce innovations above the thresholds, then the route option which provides the lowest average innovation value is selected.

In the plot shown in <FIG>, it is clear that curve <NUM> output by the second copy of the navigation filter <NUM> achieves lower innovation values. Therefore, the controller <NUM> would determine that the vehicle has taken the second route option towards the vertex <NUM> after passing the junction.

In the plot shown in <FIG>, it is clear that curve <NUM> output by the first copy of the navigation filter <NUM> achieves lower innovation values. Therefore, the controller <NUM> would determine that the vehicle has taken the first route option towards the vertex <NUM> after passing the junction. The results are consistent with the actual route taken by the vehicle <NUM> as shown in the upper parts of <FIG>.

After the route taken by the vehicle <NUM> is determined, the controller <NUM> keeps the Bayesian estimation filter algorithm associated with the determined route for further processing (e.g., for repeating the processing steps of <FIG>). The Bayesian estimation filter algorithms associated with the incorrect route options are terminated or deleted to save computational resources of the controller <NUM>.

The processing steps shown in <FIG> do not rely upon the absolute positions of the vehicle <NUM> to disambiguate the route taken by the vehicle <NUM> through a network junction. Rather, the above described processing steps calculate the probabilities of the vehicle taking each possible route option, by generating a plurality of Bayesian estimation filter algorithms each associated with a respective one of the plurality of route options and comparing the innovation values generated by the algorithms. The innovation values represent the probabilities of the vehicle taking the respective route option. The innovation values for the correct route option which is actually taken by the vehicle <NUM> are significantly lower than that of other route options. The innovation values are calculated using a pseudo-measurement of an attitude of the vehicle <NUM> formed from knowledge of the track geometry (e.g., the track constraint function) at the estimated position of the vehicle <NUM>. In this way, by comparing the innovation values generated by the algorithms, the most likely route option taken by the vehicle <NUM> through the network junction is resolved.

The processing steps shown in <FIG> do not rely upon the provision of track-side infrastructure to disambiguate the route option taken by the vehicle <NUM> through a junction, and therefore can determine the route when track-side infrastructure is unavailable in the locality of a network junction. This permits for seamless travel of the vehicle <NUM> through the junction. Further, the processing steps shown in <FIG> allows for a significant cost saving to the network operator in terms of procurement, installation and maintenance of track-side infrastructure.

It would be appreciated that the plurality of Bayesian estimation filter algorithms generated at step S4 may take a different form other than the navigation filter <NUM> shown in <FIG>.

In particular, the plurality of Bayesian estimation filter algorithms generated at step S4 may be extended Kalman filter, unscented Kalman filter, or any other suitable type of Bayesian estimation filter algorithms, as long as algorithms are able to fuse the sensor data output by the sensors <NUM>, <NUM> and to estimate a state (including the position and attitude) of the vehicle <NUM> based upon the sensor data. <FIG> illustrates the general working principle of a Bayesian estimation filter algorithm suitable for use by the techniques described herein. The Bayesian estimation filter algorithm includes a system model M <NUM>, an observation model h <NUM> and a Kalman gain K <NUM>. The Bayesian estimation filter algorithm uses an observation or measurement y (e.g., sensor data output by at least one of the sensors <NUM>, <NUM>) to estimate a state x̃ (including the position and attitude) of the vehicle <NUM>. The observation model h <NUM> maps the state space into the observed space (i.e., measurement space). The difference between the observation y and the estimated state x̃ after being mapped into the observed space using the model h is calculated at an adder <NUM> to generate an innovation value I. The innovation I is amplified by the Kalman gain K <NUM> and the amplified result is fed back into the system model M <NUM> for recursive processing.

The track constraints defined in track geometry data (e.g., the track constraint function) may be imposed suitably to the Bayesian estimation filter algorithm, such that the estimated positions of the vehicle provided by the Bayesian estimation filter algorithm lies along a track defined by the track constraint function.

Constraining the Bayesian estimation filter algorithm with track constraints may be achieved in various ways. In an example, the final estimated position (unconstrained) output by the algorithm may be orthogonally projected onto the tracks defined by the track geometry data so as to obtain a constrained solution. The strapdown INS <NUM> described above and shown in <FIG> provides an alternative example.

In an example, the state space and the measurement space of the Bayesian estimation filter algorithm of <FIG> (and navigation filter <NUM> of <FIG>) may be Euclidean vector space, in particular N-dimensional Euclidean vector-space. That is, the state (e.g., x̃, x̂) and its error covariance (e.g., P̃, P̂) as well as the measurement y are represented as Euclidean vectors.

In an alternative example, the state (e.g., x̃, x̂) and its error covariance (e.g., P̃, P̂^)as well as the measurement y are represented as elements of Lie groups (in particular, matrix Lie groups). The working principle of the Bayesian estimation filter algorithm of <FIG> (and navigation filter <NUM> of <FIG>) is not affected by the use of Lie groups. The details of Lie groups, Lie algebras, and the implementation of a sensor fusion algorithm (e.g., the navigation filter <NUM>) using the machinery of Lie groups are known from references [<NUM>] to [<NUM>].

It is advantageous to represent the state and the measurement using matrix Lie groups, because matrix Lie groups can easily represent a complex state which comprises multiple sub-states using a product matrix Lie group. The product matrix Lie group encapsulates the typological structure of the state space. In contrast, the topological structure of the state space may be lost in the process of translating a state model into an N-dimensional Euclidean vector-space.

The processing steps shown in <FIG>, the Bayesian estimation filter algorithm of <FIG>, and navigation filter <NUM> of <FIG> may be implemented in C++ language or any other suitable programming language.

In the example provided by <FIG>, the navigation filter <NUM> is implemented as an unscented Kalman filter. It would be appreciated that the navigation filter <NUM> may be implemented using a different type of the Bayesian estimation filter, such as, extended Kalman filter, etc..

Claim 1:
A computer-implemented method of determining a position of a vehicle (<NUM>) within a transport network and determining a route option taken by the vehicle (<NUM>), comprising:
obtaining (S1) track geometry data indicating track geometry of at least a part of the transport network;
determining (S2), based upon the track geometry data and sensor data from at least one sensor (<NUM>, <NUM>) arranged to output a signal indicative of a motion of the vehicle, that the vehicle is approaching a junction;
determining (S3), based upon the track geometry data, a plurality of route options from the junction;
generating (S4) a plurality of Bayesian estimation filter algorithms each associated with a respective one of the plurality of route options and configured to estimate a position of the vehicle (<NUM>) based upon the track geometry data indicative of the associated route option, the plurality of Bayesian estimation filter algorithms being configured to estimate a position of the vehicle (<NUM>) based upon the output signal of the at least one sensor, wherein the plurality of Bayesian estimation filter algorithms are configured to output data indicative of probabilities of the vehicle (<NUM>) taking the associated route options;
monitoring (S5) the output of the plurality of Bayesian estimation filter algorithms as the vehicle (<NUM>) passes through the junction; and
determining (S6) the route option taken by the vehicle by selecting one of the plurality of route options which presents the highest probability based upon the output of the plurality of Bayesian estimation filter algorithms,
characterized in that
the output data comprise innovation values calculated by the plurality of Bayesian estimation filter algorithms based upon pseudo-measurements of the associated route options, the pseudo-measurements of the associated route options being determined based upon the track geometry data indicative of the associated route options,
wherein the pseudo-measurements comprise a pseudo-measurement of an attitude of the vehicle (<NUM>) along a track defined by the track geometry data indicative of one of the route options and wherein the pseudo-measurement of the attitude of the vehicle (<NUM>) is calculated based upon a tangent of the track defined by the track geometry data at the estimated position of the vehicle (<NUM>).