WO2011066605A1

WO2011066605A1 - Cooperative localisation

Info

Publication number: WO2011066605A1
Application number: PCT/AU2010/001607
Authority: WO
Inventors: Hugh Durrant-Whyte; Tim Bailey; Mitch Bryson
Original assignee: The University Of Sydney
Priority date: 2009-12-04
Filing date: 2010-12-01
Publication date: 2011-06-09
Also published as: AU2009243519A1

Abstract

A method and apparatus for performing cooperative localisation, comprising updating internal parameter values of a first entity (2) (e.g. a manned or unmanned vehicle) using updated external parameter values of a second entity (4) (e.g. a manned or unmanned vehicle) by using: (i) correlations between external parameters (40) of the second entity (4) and external parameters (20) of the first entity (2); and (ii) correlations between the external parameters (20) of the first entity (2) and internal parameters (22) of the first entity (2); wherein: internal parameters (22, 42) (e.g. acceleration) are those parameters of a given entity whose values are only determined by the given entity; and external parameters (20,40) (e.g. position, velocity) are those parameters of a given entity whose values are determined by at least one entity other than the given entity.

Description

COOPERATIVE LOCALISATION

FIELD OF THE INVENTION

The present invention relates to methods of, and apparatus for, cooperative localisation, also known as cooperative navigation, of two or more entities.

BACKGROUND

Cooperative localisation, or cooperative navigation, is known. Typically, cooperative localisation for an entity or platform (for example, a manned or unmanned vehicle) involves determining the values of certain parameters that are capable of being observed externally from the entity (for example, position and attitude) based on measurements of the entity taken by and shared between other, external entities.

Performing cooperative localisation for groups or teams of platforms is known. For example, a multi-entity system in which a team of entities track each others' positions to provide localisation information for the system as a whole, is known.

SUMMARY OF THE INVENTION

In a first aspect the present invention provides a method of performing cooperative localisation; the method comprising updating internal parameter values of a first entity using updated external parameter values of a second entity by using: (i) correlations between external parameters of the second entity and external parameters of the first entity; and (ii) correlations between the external parameters of the first entity and internal parameters of the first entity; wherein: internal parameters are those parameters of a given entity whose values are only determined by the given entity; and external parameters are those parameters of a given entity whose values are

determined by at least one entity other than the given entity.

The method may further comprise updating external

parameters values of the first entity by using the correlations between external parameters of the second entity and external parameters of the first entity.

Updating the internal parameter values of a first entity may further use: (iii) correlations between the external parameters of the second entity and internal parameters of the second entity .

The method may further comprise updating internal

parameter values of the second entity using correlations between the external parameters of the second entity and internal parameters of the second entity.

The correlations between the external parameters of the second entity and the external parameters of the first entity may arise from determination by the first entity of the external parameters of the second entity and determination by the second entity of the external parameters of the first entity.

The method may be performed remote from the first entity and the second entity.

The method may be performed by the first entity and/or the second entity.

Using correlations may comprise determining and using an information matrix.

Any updating of parameter values may be carried out substantially simultaneously.

The method may further comprise updating parameter values of a plurality of further entities using correlations between parameters of the second entity and parameters of the plurality of further entities .

One or more of the entities may be a vehicle.

One or more of the vehicles may be unmanned.

In a further aspect the present invention provides apparatus for implementing a method according to any of the above aspects.

In a further aspect the present invention provides a computer program or plurality of computer programs arranged such that when executed by a computer system it/they cause the computer system to operate in accordance with a method according to any of the above aspects.

In a further aspect the present invention provides a machine readable storage medium storing a computer program or at least one of the plurality of computer programs according to the above aspect. BRIEF DESCRIPTION OF THE DRAWINGS

Figure 1 is a schematic illustration of a localisation system;

Figure 2 is a schematic illustration of a first vehicle;

Figure 3 is a schematic illustration of a second vehicle;

Figure 4 is a schematic illustration of the localisation system showing certain correlations between different parameter sets; and

Figure 5 is a process flow chart showing certain steps of an example cooperative localisation methodology.

DETAILED DESCRIPTION

Figure 1 is a schematic illustration of a localisation system 1. Figure 1 schematically shows the relative positions of certain entities of the localisation system 1 that are useful in describing a first embodiment of the present invention.

Figure 1 further shows the directions of certain signals passed between the entities, and certain determinations (of parameter values) made by the entities using appropriate sensing

equipment, useful in describing the first embodiment. The signals and determinations are represented in Figure 1 by a solid arrow.

The entities shown in Figure 1 and used in this embodiment are: a first vehicle 2, a second vehicle 4, and a satellite system 6.

In this embodiment, the satellite system 6 is part of a Global Positioning System.

The first and second vehicles 2, 4, are described in more detail later below with reference to Figure 2.

The signals and determinations shown in Figure 1 are the following: a signal corresponding to a measurement of the relative distance between the first vehicle 2 and the second vehicle 4, hereinafter referred to as the "relative measurement 8", and Global Positioning System (GPS) signals sent by the satellite system 6 to the second vehicle 4, hereinafter referred to as the "GPS signals 10".

In this embodiment, the relative measurement 8 is an example of a determination of values of certain parameters of the second vehicle 4 made by the first vehicle 2, and a

determination of values of certain parameters of the first vehicle 2 made by the second vehicle 4. More generally, as used herein, terminology "determination" of a value of a parameter encompasses any determination process or combination of such processes, for example measurement, inference, and estimation. The measurement of the second vehicle 4 made by the first vehicle 2 is made using appropriate sensing equipment mounted on the first vehicle 2. In this embodiment, the first vehicle 2 measures the position and velocity of the second vehicle 4 relative to the first vehicle 2. The measurement of the first vehicle 2 made by the second vehicle 4 is made using appropriate sensing equipment mounted on the second vehicle 2. In this embodiment, the second vehicle 4 measures the position and velocity of the first vehicle 2 relative to the second vehicle 4.

The first vehicle 2 sends a measured value of the second vehicle's relative position and velocity to the second vehicle 4. Also, the first vehicle 2 receives a value of its position and velocity relative to the second vehicle 4, as measured by the second vehicle 4. Likewise, the second vehicle 4 sends a measured value of the first vehicle's relative position and velocity to the first vehicle 2. Also, the second vehicle 4 receives a value of its position and velocity relative to the first vehicle 2, as measured by the first vehicle 2.

In this embodiment, the GPS signals 10 allow the second vehicle 4 to perform a measurement of the position of the second vehicle 4. This measurement of the position of the second vehicle is typically an accurate measure of the global position (i.e. location on the face of the earth) of the second vehicle 4.

Figure 2 is a schematic illustration showing the

localisation functionality of the first vehicle 2. The first vehicle 2 is shown in Figure 2 as having a first set of

parameters that, in this embodiment, are measured internally (hereinafter referred to as "first internal parameters 20") , and a second set of parameters that, in this embodiment, are measured externally (hereinafter referred to as the "first external parameters 22") .

The first internal parameters 20 are parameters

corresponding to the first vehicle 2 whose values are only determined by the first vehicle 2 itself, i.e. parameters whose values are not determined by an external entity, for example the second vehicle 4. For example, the first internal parameters 20 may include parameters corresponding to an accelerometer, a gyroscope, slippage, tyre shape, camera calibration parameters or an inertial measurement unit (IMU) onboard the first vehicle 2. As before, the terminology "determined" encompasses any determination process or combination of processes, for example measurement, inference, and estimation.

The first external parameters 22 are parameters

corresponding to the first vehicle 2 whose values are determined by an external entity. In this embodiment, the first external parameters 22 include the position and the velocity of the first vehicle 2 relative to the second vehicle 4. The values of these parameters are determined by an external entity, i.e. measured by the second vehicle 4, as part of the relative measurement 8, as described above, to determine relative values of the first external parameters 22.

In this embodiment, the first internal parameters 20 and the first external parameters 22 are interrelated, e.g. the first external parameters 22 are functions of the first internal parameters 20. Thus, the first internal parameters 20 and the first external parameters 22 are correlated. This correlation is represented in Figure 2 by a dotted line, and will hereinafter be referred to as the "first internal correlation 24".

The first vehicle 2 calculates estimates of the values of the first external parameters 22 from determined values of the first internal parameters 20. In this embodiment, the determined values of the first internal parameters 20 contain a number of errors or biases. These errors are propagated from the

internally determined values of the first internal parameters 20 to the estimated values of the first external parameters 22. The first internal correlation 24 is expressed as the expected value (E) of the difference between the errors in the determined values of the first internal parameters 20 and the errors in the estimated values of the first external parameters 22, i.e. where: C_lintis a measure of the correlation between the first internal and external parameters 20, 22;

l_intis a measure of the errors in the first internal parameters 20; and

ε_1βχί is a measure of the errors in the first external parameters 22.

Figure 3 is a schematic illustration showing the

localisation functionality of the second vehicle 4. The second vehicle 4 is shown in Figure 3 as having a first set of

parameters whose values, in this embodiment, are determined internally (hereinafter referred to as "second internal

parameters 40") , and a second set of parameters whose values, in this embodiment, are determined externally (hereinafter referred to as the "second external parameters 42") .

The second internal parameters 40 are parameters

corresponding to the second vehicle 4 whose values are only determined by the second vehicle 4 itself, i.e. parameters whose values are not determined by an external entity, for example the first vehicle 2. For example, the second internal parameters 40 may include parameters corresponding to an accelerometer, a gyroscope, slippage, tyre shape, camera calibration parameters or an inertial measurement unit (IMU) onboard the second vehicle 4. As before, the terminology "determined" encompasses any determination process or combination of processes, for example measurement, inference, and estimation.

The second external parameters 42 are parameters

corresponding to the second vehicle 4 whose values are

determined by an external entity. In this embodiment, the second external parameters 42 include the position and the velocity of the second vehicle 4 relative to the first vehicle 2. The values of these parameters are determined by an external entity, i.e. measured by the first vehicle 2, as part of the relative measurement 8 as described above, to determine relative values of the second external parameters 42.

The second internal parameters 40 and the second external parameters 42 are interrelated, e.g. the second external parameters 42 are functions of the second internal parameters 40. Thus, the second internal parameters 40 and the second external parameters 42 are correlated. This correlation is represented in Figure 3 by a dotted line, and will hereinafter be referred to as the "second internal correlation 44".

The second vehicle 4 calculates estimates of the values of the second external parameters 42 from determined values of the second internal parameters 40. In this embodiment, the

determined values of the second internal parameters 40 contain a number of errors or biases. These errors are propagated from the internally determined values of the second internal parameters 40 to the estimated values of the second external parameters 42.

The second internal correlation 44 is expressed as the expected value of the difference between the errors in the determined values of the second internal parameters 40 and the errors in the estimated values of the second external parameters 42, i.e.

where: C_2intis a measure of the correlation between the second internal and external parameters 40, 42;

^_j^is a measure of the errors in the second internal parameters 40; and

£_{2 e}xi i^{s a} measure of the errors in the second external

parameters 42.

Figure 4 is a schematic illustration of the localisation system 1 showing certain correlations between different

parameter sets. Figure 4 indicates the first internal

correlation 24 between the first internal parameters 20 and the first external parameters 22, and the second internal

correlation 44 between the second internal parameters 40 and the second external parameters 42. Figure 4 further shows a first external correlation 30 and a second external correlation 32, as described in more detail later below.

The values of the position and velocity of the first vehicle 2, as determined by the second vehicle 4, are relative to the position and velocity of the second vehicle 4. In other words, the values of the first external parameters 22 determined by the second vehicle 4 are relative to the values of the second external parameters 42. Likewise, the values of the position and velocity of the second vehicle 4, as determined by the first vehicle 2, are relative to the position and velocity of the first vehicle 2, i.e. the values of the second external

parameters 42 determined by the first vehicle 2 are relative to the values of the first external parameters 22. Thus, the first external parameters 20 and the second external parameters 42 are correlated. The correlation between the first and second external parameters 22, 42 is hereinafter referred to as the first external correlation 30, and is indicated in Figure 4 by a dotted line.

The first external correlation 30 is expressed as the expected value of the difference between the errors in the estimated values of the first external parameters 22 and the errors in the estimated values of the second external parameters

42, i.e.

l,ext 2,ext ^

where C_lextis a measure of the correlation between the first and second external parameters 22, 42.

At certain points in time, the satellite system 6 provides the second vehicle 4 with the GPS signals 10, as described above with reference to Figure 1. Using the GPS signals 10 the second vehicle 4 determines the position of the second vehicle 4.

Thus, the second external parameters 42, which include the position of the second vehicle 4, are correlated with the second vehicle's satellite measurement state. The correlation between the second external parameters 42 and the second vehicle' s satellite measurement state is hereinafter referred to as the second external correlation 32, and is indicated in Figure 4 by a dotted line.

The second external correlation 32 is expressed as the expected value of the difference between the errors in the estimated values of the second external parameters 42 and the errors in the values of the second external parameters 42 derived from the GPS signals 10 of the satellite system 6, i.e.

Where : C 2,ext is a measure of the correlation between the estimated and determined values of the second external

parameters 22; and

£_2sal is a measure of errors in the values of the second external parameters 42 as determined from the GPS signals 10.

In this embodiment, the above defined first and second internal correlations 24, 44, and the first and second external correlations 30, 32 have constant values, i.e. C_l-_m , C₂-_m , C_{l ext} , and C_2extare constants.

Using the above defined correlations, the first vehicle 2 is able to correct errors in and biases in determinations of its internal parameters, i.e. errors and biases in the first internal parameters 20, as described below with reference to Figure 5.

Apparatus, including a central processor, for implementing the above arrangement, and performing the method steps to be described later below, may be provided by configuring or adapting any suitable apparatus, for example one or more computers or other processing apparatus or processors, and/or providing additional modules. The apparatus may comprise a computer, a network of computers, or one or more processors, for implementing instructions and using data, including instructions and data in the form of a computer program or plurality of computer programs stored in or on a machine readable storage medium such as computer memory, a computer disk, ROM, PROM etc., or any combination of these or other storage media.

Figure 5 is a process flow chart showing certain steps of an example cooperative localisation methodology used in the first embodiment of the present invention.

At step s2, the second vehicle 4 determines values of the second internal parameters 40. The determined values of the second internal parameters 40 contain errors or biases, as described above with reference to Figure 3.

At step s4, the second vehicle 4 estimates values of the second external parameters 42 using determined values of the second internal parameters 40. The errors present in the determined values of the second internal parameters 40 are propagated through to the estimated values of the second external parameters 42. At step s6, the second vehicle 4 receives the GPS signals 10 from the satellite system 6. The second vehicle 4 determines the position of the second vehicle 4 using the GPS signals 10.

At step s8, the second vehicle 4 corrects the estimated values of the second external parameters 42 using the positional information determined from the GPS signals 10 at step s6.

At step slO, the relative measurement 8 is made as described above.

The first vehicle 2 sends a determined value of the second vehicle's relative position and velocity to the second vehicle 4. Also, the first vehicle 2 receives a value of its position and velocity relative to the second vehicle 4, as determined by the second vehicle 4.

Likewise, the second vehicle 4 sends a determined value of the first vehicle's relative position and velocity to the first vehicle 2. The value of the first vehicle's relative position and velocity are dependent on the corrected estimates of the second vehicle' s position and velocity (that were corrected, as described above with reference to step s8) . Also, the second vehicle 4 receives a value of its position and velocity relative to the first vehicle 2, as determined by the first vehicle 2.

Thus, at step slO the first vehicle 2 has access to the following information: estimated values of the first external parameters 22 (i.e. estimates of its position and velocity); determined values of the second external parameters 42 relative to the estimated first external parameters 22 (i.e. relative measurements of the second vehicle' s position and velocity taken by the first vehicle 2); and determined values of the first external parameters 22 relative to the corrected estimates of the second external parameters 42 (i.e. relative measurements of the first vehicle's position and velocity taken by the second vehicle 4 ) .

At step sl2, the first vehicle 2 corrects the estimated values of the first external parameters 22 using the information gained from the relative measurement 8 taken at step slO described above. The first vehicle corrects the estimated values of its position and velocity (the first external

parameters 22) using the determined values of the second external parameters 42 relative to the estimated first external parameters 22 and the determined values of the first external parameters 22 relative to the corrected estimates of the second external parameters 42.

The first external correlation 30 is expressed as

C_{l ext} = Ε(ε_{ι ext} -ε_{2 ext}) . At step s8, the second vehicle 4 corrects errors in the estimated second external parameters 42. In other words, in this embodiment £₂ext reduced. Therefore, since

C_{l ext} is a constant, the value of £_lext is also reduced, i.e. the first vehicle 2 correspondingly corrects errors in its

estimations of the first external parameters 22.

Thus, at step sl2 the first vehicle 2 has access to the following information: corrected estimated values of the first external parameters 22 (i.e. corrected estimates of its position and velocity) ; and determined values of the first internal parameters 20.

At step sl4, the first vehicle 2 corrects the values of the first internal parameters 20 using the corrected estimated values of the first external parameters 22, and determined values of the first internal parameters 20.

The first internal correlation 24 is expressed as

C_lm = E(_lm - _{{ exl}) . At step sl2, the first vehicle 2 corrects errors in the estimated first external parameters 22. In other words, in this embodiment £_lexl is reduced. Therefore, since C_Vmt is a constant, the value of s_Vm is also reduced, i.e. the first vehicle 2 correspondingly corrects errors in the values of the first internal parameters 20.

Thus, a methodology that enables the internal state, or set of internal parameters, of one vehicle to be corrected and/or updated from the corrected and/or updated state of another vehicle is provided. In other words, parameters relating to one entity, whose values are only determined by that entity, are updated using updated information about a different entity .

The provided methodology exploits statistical correlations developed between states on different platforms. The

localisation solution for each individual vehicle is not independent and the localisation of all vehicles needs to be treated as a whole. Cooperative localisation is preferably undertaken simultaneously for all vehicles. While this tends to impose potentially significant additional computation and communication, substantial improvements in team localisation performance and capability tend to be achieved.

For a homogeneous team of vehicles (for example, if the first vehicle 2 and the second vehicle 4 are of the same type) , better estimates of external landmarks can be achieved by integrating measurements made by different platforms at many locations which can, in turn, improve the individual platforms navigation accuracy.

For a heterogeneous group of platforms (for example, if the first vehicle 2 and the second vehicle 4 are of different types), a platform carrying low-precision navigation sensors can make use of high-precision navigation sensors hosted on other platforms to improve its navigation performance. In particular, for example, platforms equipped with low-grade inertial sensors can re-align by exploiting information from other platforms with high-grade inertial sensing. Equally, a platform which cannot accomplish a navigation task by itself, due to limits in sensing or environments, can be navigation-enabled by cooperation with other platforms. In particular, absolute localisation

information, such as that determined using a GPS satellite system, of one or more platforms can be shared with other platforms which otherwise have no access to this localisation information .

Many algorithms are possible for implementing the above described methodology invention. Three possible algorithms are:

1. All relative measurements and estimations are sent to a central fusion site. A single Kalman filter state estimator is used to fuse range, relative range and absolute observations to estimate a global state vector consisting of all vehicle states.

2. All relative measurements and estimations are sent to a central fusion site. A single information filter is used to fuse range, relative range and absolute observations to estimate a global state vector consisting of all platform states. This approach is preferred over the Kalman-filter approach as it exploits the natural sparsity of the cooperative localisation problem as described below.

3. An information filter is implemented at each platform in a decentralised manner. Each local information filter fuses local range, relative range and absolute observations to provide a local update for the platform location. Parts of this

information are then communicated to adjacent platforms and used to update their respective location estimates. This approach is also preferred for small networks. Efficient approximations to this algorithm may be possible for large networks of platforms. Certain elements of an information-filter based algorithm are described below.

In this embodiment, the estimated first external

parameters 22 comprises an estimated position and velocity of the first vehicle 2 at a particular time t. This is implemented as a time-dependent state vector _j(t) :

where: _¾( is the estimated position of the first vehicle 2 determined by the first vehicle 2 from the determined values of the first internal parameters 20; and j(t) is the estimated velocity of the first vehicle 2 determined by the first vehicle 2 from the determined values of the first internal parameters 20.

Likewise, the estimated values of the second external parameters 42 comprises an estimated position and velocity of the second vehicle 4 at a particular time t. This is implemented as a time-dependent state vector x₂ (t) :

Where: _¾( is the estimated position of the second vehicle

4 determined by the second vehicle 4 from the determined values of the second internal parameters 40; and

x₂(t) is the estimated velocity of the second vehicle 4 determined by the second vehicle 4 from the determined values of the second internal parameters 40.

Thus, a state vector x(t) comprising sub-state vectors of the first and second vehicles at time t is formulated as:

The state vector x(t)has a probability density function (x)

The state vector x(t)has an estimated covariance matrix

P(x) :

P(x) = cov(x - x)

where x is a "state mean". The state mean is an estimated state vector comprising sub-state vectors of the vehicles 2, 4, at time t given any previous measurements and observations made at a time up to and including time t-1.

In this example algorithm, an information matrix Y(x)is calculated. This is defined as:

Y = P ¹

Also, an information vector y(x) is calculated. This is defined as:

y = P 'x

The probability density function /^"(x) is parameterised in terms of the information matrix and the information vector to give the information form (or canonical form) , denoted as

N_j{ ,y,Y) , of the probability density function. (x) = N,(x,y,Y) oc exp^- ^-x^rYx + x^ry

where (x)is the probability of X.

This is equivalent to the following:

where (x₁ ,x₂)is the joint probability of the state vectors of the vehicles 2 , 4.

A marginal probability distribution function for the first vehicle 2, and a marginal probability distribution function for the second vehicle 4 are calculated. The marginal probabilities are calculated using a process of marginalisation .

The following information is useful for understanding the process of marginalisation.

Marginalisation eliminates one of the two variables through integration. Thus, the marginal probability

distribution function for the first vehicle 2 is calculated as follows :

P(x_l ) = | (^{χ χ} ₂ ^χ ₂

Likewise, the marginal probability distribution function for the second vehicle 4 is calculated as follows:

^(Χ₂) = | ^(^Χ1 » ^Χ2)^^Χ1

Thus, estimates of the state vectors x x₂for the first and second vehicles 2, 4 are provided. Moreover, appropriate information from the localisation system 1 through which the vehicles 2, 4 are linked is combined. This tends to provide more accurate estimates of the state vectors x^,x_B for the vehicles 2, 4, than those estimates that are based on

measurements taken by the respective entities alone.

An advantage of using the information form of the

probability density function P(x) is that it is simple to update the information matrix and information vector with any new information that is received by the vehicles 2, 4, or with any measurements made by the vehicles 2, 4. Thus, for example, it tends to be simple and easy to update the internal state of the first vehicle 2 using new positional information received from the satellite system 6 by the second vehicle 4.

An advantage of this formulation is that there is no requirement to invert an information matrix. Thus, computation tends to be simple.

A further advantage is that the information matrix tends to be sparse. Also, the information matrix tends to be

separable in to vector/matrix additive parts. Thus, augmenting an information matrix or information vector with future vehicle information, e.g. a new position of a vehicle, tends to be computationally simple and efficient. Furthermore, updating the information matrix or information vector with information relating to two or more entities, for example a relative measurement taken between two vehicles, tends to be

computationally simple and efficient.

A further advantage of using the information form of the probability density function P(x) is that only the components of the information matrix and information vector directly related to an update measurement are required to be updated. This is regardless of how the state vectors of the entities are

correlated. This means that information fusion processes tend to be computationally trivial. The update is local to the entities directly related to the measurement, and this update need not be propagated to other entities at the time of the measurement. Thus, computation and communication tend to be minimised .

In the above embodiments, cooperative localisation is performed using two vehicles, i.e. the first vehicle and the second vehicle. However, in other embodiments, any number of the same and/or different vehicles may be incorporated. For example, a localisation system may comprise any number of aircraft and any number of land based vehicles. Moreover, in other embodiments any number of vehicles may be replaced by any number of different appropriate entities, for example these entities may be a human, a robot, or a building etc. Moreover, any vehicle may be manned or unmanned.

In the above embodiments, the relative measurement is provided by a direct measurement of a vehicle by another vehicle, i.e. the first vehicle making a relative measurement of the second vehicle and vice versa. However, in other

embodiments a relative measurement may be provided by a

different means, or a combination of the same and different means. For example, in other embodiments the relative

measurement may be provided by a measurement by more than one entity of a common landmark, e.g. the first and second vehicles could each make a relative measurement of a common landmark and transmit their respective results to one another.

In the above embodiments, the second vehicle is adapted to receive GPS signals. However, in other embodiments any entity may be adapted to receive GPS or any other appropriate signals. More generally, in embodiments where absolute positioning is employed, absolute positioning systems other than GPS may be employed, for example a system based on lasers may be employed.

In the above embodiments, the first and second external parameter set comprise the position and velocity of the first and second vehicle respectively. However, in other embodiments, each external parameter set may comprise any appropriate parameter corresponding to the respective entity that is observed by an external, i.e. different, entity. For example, an external parameter set may include parameters such as orientation, temperature etc. Likewise, the internal parameter sets may comprise any appropriate parameters that are only determined internally by the respective entity.

Any given parameter may be an external parameter for a particular entity or group of entities. However, the same parameter may be an internal parameter for a different entity or group of entities in the same embodiment. Furthermore, the same parameter may be an internal parameter for the same entity or group of entities in a different localisation system, for example a different embodiment.

In the above embodiments a cooperative localisation algorithm involves the use of an information filter, i.e. the algorithm uses the information form of the probability density function. However, in other embodiments a different appropriate cooperative localisation algorithm is used, such as a

cooperative localisation algorithm that utilises the covariance form of the probability density function.

In the above embodiments, the cooperative localisation algorithm is performed in a decentralized manner by the entities in the localisation. However, in other embodiments the

algorithm is performed in a centralized manner, for example by a central processor at a central site. In other embodiments, the algorithm may be performed such that some parts of the algorithm are implemented in a decentralized manner, and other parts are implemented in a centralized manner.

In the claims which follow and in the preceding

description of the invention, except where the context requires otherwise due to express language or necessary implication, the word "comprise" or variations such as "comprises" or

"comprising" is used in an inclusive sense, i.e. to specify the presence of the stated features but not to preclude the presence or addition of further features in various embodiments of the invention .

It is to be understood that, if any prior art publication is referred to herein, such reference does not constitute an admission that the publication forms a part of the common general knowledge in the art, in Australia or any other country.

Claims

THE CLAIMS DEFINING THE INVENTION ARE AS FOLLOWS:

1. A method of performing c ;ooperative localisation; the method comprising updating internal parameter values of a first entity using updated external parameter values of a second entity by using:

(i) correlations between external parameters of the second entity and external parameters of the first entity; and

(ii) correlations between the external parameters of the first entity and internal parameters of the first entity; wherein: internal parameters are those parameters of a given entity whose values are only determined by the given entity; and external parameters are those parameters of a given entity whose values are determined by at 1east one entity other than the given entity.

2. A method of performing co operative localisation according to claim 1, the method further comprising updating external parameters values of the first entity by using the correlations between external parameters of the second entity and external parameters of the first entity

3. A method of performing cooperative localisation according to claim 1 or claim 2, wherein updating the internal parameter values of a first entity further uses:

(iii) correlations between the external parameters of the second entity and internal parameters of the second entity.

4 A method of performing cooperative localisation according to any of claims 1 to 3, the method further comprising updating internal parameter values of the second entity using

correlations between the external parameters of the second entity and internal parameters of the second entity.

5. A method of performing cooperative localisation according to any of claims 1 to 4 , wherein the correlations between the external parameters of the second entity and the external parameters of the first entity arise from determination by the first entity of the external parameters of the second entity and determination by the second entity of the external parameters of the first entity.

6. A method of performing cooperative localisation according to any of claims 1 to 5, wherein the method is performed remote from the first entity and the second entity.

7. A method of performing cooperative localisation according to any of claims 1 to 5, wherein the method is performed by the first entity and/or the second entity.

8. A method of performing cooperative localisation according to any of claims 1 to 7, wherein using correlations comprises determining and using an information matrix.

9. A method of performing cooperative localisation according to any of claims 1 to 8 , wherein any updating of parameter values is carried out substantially simultaneously.

10. A method of performing cooperative localisation according to any of claims 1 to 9, the method further comprising updating parameter values of a plurality of further entities using correlations between parameters of the second entity and parameters of the plurality of further entities.

11. A method of performing cooperative localisation according to any of claims 1 to 10, wherein one or more of the entities is a vehicle.

12. A method of performing cooperative localisation according to claim 11, wherein one or more of the vehicles is unmanned.

13. Apparatus for implementing the method of any of claims 1 to 12.

14. A computer program or plurality of computer programs arranged such that when executed by a computer system it/they cause the computer system to operate in accordance with the method of any of claims 1 to 12.

15. A machine readable storage medium storing a computer program or at least one of the plurality of computer programs according to claim 14.