METHOD AND DEVICE FOR ASSISTING WITH THE NAVIGATION OF A FLEET OF VEHICLES USING AN INVARIANT KALMAN FILTER

Abstract

The invention relates to a method for assisting with the navigation of a fleet of vehicles comprising a main vehicle (1) and a secondary vehicle (2) that is mobile in relation to the main vehicle (1), said method involving the steps of: •receiving relative movement data (Y1, Y2), acquired by at least one sensor (2, 12), between the main vehicle (1) and the secondary vehicle (2); •estimating (100, 200) a navigation status of the fleet of vehicles by an invariant Kalman filter using the received data (Y1, Y2) as observations, the navigation status comprising •first variables representing a first rigid transformation linking a location mark associated with the main vehicle (1) to a reference point, and •second variables representing a second rigid transformation linking a location mark associated with the main vehicle (1) to a location mark associated with the secondary vehicle (2), the invariant Kalman filter using, as an internal composition law, a law comprising a term-by term composition of the first rigid transformation and the second rigid transformation.

Claims

1-14. (canceled)

15. A method for assisting the navigation of a fleet of vehicles comprising a main vehicle and a secondary vehicle movable relative to the main vehicle, the method comprising: receiving data acquired by at least one sensor, the received data comprising relative kinematic data between the main vehicle and the secondary vehicle, estimating a navigation state of the fleet of vehicles by an invariant Kalman filter using the received data as observations, wherein the navigation state comprises: first variables representative of a first rigid transformation linking a frame attached to the main vehicle to a reference frame, and second variables representative of a second rigid transformation linking a frame attached to the main vehicle to a frame attached to the secondary vehicle, wherein the invariant Kalman filter uses as binary operation an operation comprising a term-by-term composition of the first rigid transformation and of the second rigid transformation.

16. The method of claim 15, wherein the navigation state of the fleet further comprises at least one error state of the at least one sensor, and wherein the binary operation is additive for the at least one error state of to the at least one sensor.

17. The method of claim 15, comprising receiving proprioceptive movement data from the main vehicle and from the secondary vehicle, and wherein the estimating comprises phases of propagating a navigation state of the fleet previously estimated by the invariant Kalman filter using the proprioceptive movement data from the main vehicle and the secondary vehicle to obtain a propagated state, updating the propagated state using the observations to obtain a new navigation state of the fleet.

18. The method of claim 15, wherein the proprioceptive movement data are acquired by respective proprioceptive sensors of the main vehicle and of the secondary vehicle, and the navigation state of the fleet further comprises at least one error state of at least one of the proprioceptive sensors for which the binary operation is additive.

19. The method of claim 15, wherein: the first variables comprise a rotation matrix representing an attitude of the main vehicle and a position vector of the main vehicle, and the second variables comprise a rotation matrix representing the attitude of the main vehicle relative to the secondary vehicle and a position vector of the main vehicle relative to the secondary vehicle.

20. The method of claim 15, wherein: the navigation state of the fleet comprises a velocity vector v.sub.p of the main vehicle in the reference frame, and a velocity vector equal to R.sub.S.sup.T (v.sub.p−v.sub.S), wherein v.sub.S is a velocity of the secondary vehicle in the reference frame, and R.sub.S is a rotation matrix representing the attitude of the secondary vehicle relative to the reference frame, the binary operation applies a transformation to one of the position vectors and applies the same transformation to one of the velocity vectors.

21. The method of claim 15, wherein estimating the navigation state of the fleet of vehicles is carried out by the main vehicle, and the received data comprise position data of the secondary vehicle in a frame attached to the main vehicle acquired by at least one first sensor of the main vehicle.

22. The method of claim 21, wherein the at least one first sensor of the main vehicle comprises a lidar or a camera.

23. The method of claim 15, wherein estimating the navigation state of the fleet of vehicles is carried out by the main vehicle, and the received data comprise position data of the main vehicle in a frame attached to the secondary vehicle acquired by at least one second sensor of the secondary vehicle.

24. The method of claim 23, wherein the at least one second sensor of the secondary vehicle comprises a lidar or a camera.

25. The method of claim 15, wherein the received data comprise relative kinematic data between the main vehicle and a third object separate from the main vehicle and from the secondary vehicle, the relative kinematic data between the main vehicle and the third object also being used as observations by the invariant Kalman filter.

26. The method of claim 25, wherein the third object is a daymark.

27. The method of claim 25, wherein the relative kinematic data between the main vehicle and a third object comprise data acquired by a GPS/GNSS receiver of the main vehicle.

28. The method of claim 15, comprising: carrying out the estimating step based on position data of the secondary object in the frame attached to the main vehicle so as to generate a first estimation of the navigation state of the fleet, carrying out the estimating step based on position data of the main vehicle in a frame attached to the secondary object, so as to generate a second estimation of the navigation state of the fleet, merging the first and second estimations of the navigation state of the fleet so as to produce a third estimation of the navigation state of the fleet, the merging comprising for example the calculation of an average of the first and second estimations of the navigation state of the fleet.

29. The method of claim 28, wherein the merging comprising computing an average of the first estimation of the navigation state of the fleet and of the second estimation of the navigation state of the fleet.

30. The method according to claim 15, wherein the received data used as observations form a vector Y.sub.1=l(X.sub.1,y.sub.0), wherein l(.,.) is a left group action for the binary operation, X.sub.1 is the navigation state of the fleet, and y.sub.0 is a vector of the same dimension as Y.sub.1, and wherein the invariant Kalman filter uses an innovation Z=l({circumflex over (X)}.sub.1.sup.−1,Y.sub.1).

31. The method according to claim 15, wherein the received data used as observations form a vector Y.sub.2=l({circumflex over (X)}.sub.2.sup.1,y.sub.0)) wherein l(., .) is a left group action for the binary operation, X.sub.2 is the navigation state of the fleet, and y.sub.0 is a vector of the same dimension as Y.sub.2, and wherein the invariant Kalman filter uses an innovation Z=l({circumflex over (X)}.sub.2,Y.sub.2).

32. A device for assisting the navigation of a fleet of vehicles comprising a main vehicle and a secondary vehicle movable relative to the main vehicle, the device comprising: a receiving interface to receive data acquired by at least one sensor, the data comprising relative kinematic data between the main vehicle and the secondary vehicle, a processing unit configured to implement an invariant Kalman filter so as to estimate a navigation state of the fleet of vehicles, wherein the invariant Kalman filter is configured to use the received data as observations, wherein the navigation state comprises: first variables representative of a first rigid transformation linking a frame attached to the main vehicle to a reference frame, and second variables representative of a second rigid transformation linking a frame attached to the main vehicle to a frame attached to the secondary vehicle, and wherein the invariant Kalman filter is configured to use as binary operation a term-by-term composition of the first rigid transformation and of the second rigid transformation.

Description

DESCRIPTION OF THE FIGURES

[0059] Other characteristics, aims and advantages of the invention will emerge from the following description, which is purely illustrative and not restrictive, and which should be read in relation to the appended drawings in which:

[0060] FIG. 1 illustrates different objects movable relative to each other, in particular a fleet of vehicles.

[0061] FIG. 2 schematically illustrates the internal components of a main vehicle and of a secondary vehicle of a fleet of vehicles, according to one embodiment of the invention.

[0062] FIG. 3 is a flowchart of steps of a navigation assistance method, according to one embodiment of the invention.

[0063] In all the figures, similar elements bear identical reference numerals.

DETAILED DESCRIPTION OF THE INVENTION

[0064] 1/ Fleet of Vehicles

[0065] Referring to FIG. 1, a fleet of vehicles comprises a main vehicle 1 and at least one secondary vehicle 2 movable relative to the main vehicle 1. Each vehicle 1, 2 can be of any type: land vehicle, ship, aircraft, etc.

[0066] Objects external to the fleet of vehicles are also represented in FIG. 1: daymarks (lampposts and Eiffel tower), as well as the Earth itself. As will be seen below, these objects are reference points for the navigation of the fleet.

[0067] In the following, different frames are considered: a main frame attached to the main vehicle 1, a secondary frame attached to the secondary vehicle 2, and a reference frame. The reference frame is for example a celestial frame attached to stars or to the Earth.

[0068] Referring to FIG. 2, the main vehicle 1 comprises a data receiving interface.

[0069] The receiving interface comprises at least one first sensor 3 onboard the main vehicle 1. The at least one first sensor 3 is configured to acquire kinematic data from the secondary vehicle 2 in the main frame. The at least one first sensor 3 comprises for example a lidar, a camera or an odometer.

[0070] In the present disclosure, it is considered that the expression “kinematic data” covers in particular positions, velocities or accelerations.

[0071] The receiving interface also comprises a communication interface 4 adapted to communicate with the secondary vehicle 2, in particular receive from the latter kinematic data from the main vehicle 1 in the frame attached to the secondary vehicle 2. The communication interface 4 is of the wireless radio type, and comprises for example an antenna.

[0072] The main vehicle 1 furthermore comprises at least one proprioceptive sensor 6.

[0073] The proprioceptive sensor 6 comprises for example an inertial unit. The inertial unit comprises a plurality of inertial sensors such as gyrometers and accelerometers. As a variant, the proprioceptive sensor 6 comprises at least one odometer.

[0074] The main vehicle 1 further comprises a receiver 8 configured to acquire relative kinematic data between the main vehicle 1 and a third object separate from the main vehicle 1 and from the secondary vehicle 2. The receiver is for example a GPS/GNSS receiver, in which case the third object is the Earth or one of the stars to which the celestial frame is attached. As a variant or in addition, this receiver comprises a lidar, a camera, in which case the third object can be a daymark located in the vicinity of the main vehicle 1. In another variant, the receiver comprises an odometer which has a relative velocity of the carrier relative to the Earth.

[0075] The main vehicle 1 furthermore comprises a data processing unit 10. The processing unit 10 is arranged to process data received by the receiving interface (therefore received by the first sensor 3 or received by the communication interface 4), by the inertial unit 6 or by the receiver 8.

[0076] The data processing unit 10 typically comprises at least one processor configured to implement a navigation assistance method which will be described below, by means of an invariant-type Kalman filter. The invariant Kalman filter is typically in the form of a computer program executable by the processor of the data processing unit. The general operation of an invariant Kalman filter is known per se. However, it will be seen below that the binary operation used to configure the invariant Kalman filter implemented by processing unit 10 is chosen in a particular manner, so as to adapt to the context of the assistance in the navigation of the fleet of vehicles comprising the vehicles 1 and 2.

[0077] Preferably, the processing unit 10 comprises at least two processors, so as to implement two Kalman filters in parallel. It will be seen below that these two Kalman filters do not use exactly the same input data.

[0078] Furthermore, the secondary vehicle 2 comprises at least one second sensor 12 and a communication interface 14 for transmitting data acquired by the at least one second sensor 12 to the communication interface 4 of the main vehicle 1.

[0079] The second sensor 12 is configured to acquire movement data from the main vehicle 1 in the frame attached to the secondary vehicle 2. The at least one second sensor 12 comprises for example a lidar, a camera or an odometer.

[0080] The secondary vehicle 2 comprises means for providing proprioceptive movement data from the secondary vehicle.

[0081] These providing means comprise for example at least one proprioceptive sensor 16. The proprioceptive sensor is for example one or more of the types of sensors envisaged for the proprioceptive sensor 6.

[0082] As a variant, these providing means comprise a memory storing an a priori model of evolution of the secondary vehicle 2. This memory can also be integrated into the secondary vehicle 2 as well as into the main vehicle 1.

[0083] 2/ Configuration of the Invariant Kalman Filter

[0084] The invariant Kalman filter implemented by the processing unit 10 is configured to estimate a navigation state of the fleet comprising the main vehicle 1 and the secondary vehicle 2.

[0085] The navigation state comprises first variables representative of a first rigid transformation linking the main frame (attached to the main vehicle 1) to the reference frame, and second variables representative of a second rigid transformation linking the secondary frame (attached to the secondary vehicle 2) to the main frame.

[0086] The first rigid transformation allows for example switching from the frame linked to the main vehicle 1 to the reference frame, and the second one allows switching from the frame linked to the main vehicle 1 to the frame linked to the secondary vehicle 2.

[0087] In a well-known manner, a rigid transformation (also known as affine isometry), is a transformation that preserves the distances between pairs of points of a solid. Thus, each of the first and second rigid transformations can be characterized by the composition of a rotation and a translation.

[0088] In the following, an embodiment will be detailed in which the navigation state, denoted X, comprises the following elements:

X=(R.sub.p,x.sub.p,R.sub.sp,x.sub.sp)

where R.sub.p, x.sub.p, R.sub.sp, x.sub.sp are defined as follows: [0089] R.sub.p and x.sub.p are respectively a rotation matrix and a vector of dimension 3, representing the attitude and the position of the main vehicle: a vector u written in the frame of the main vehicle 1 becomes the vector R.sub.pu+x.sub.p in the fixed frame. [0090] R.sub.sp and x.sub.sp are respectively a rotation matrix and a dimension vector 3, representing the attitude and the relative position of the main vehicle relative to the secondary vehicle: a vector u written in the frame of the main vehicle 1 becomes the vector R.sub.spu+x.sub.sp in the frame of the secondary vehicle.

[0091] In this particular embodiment, the first variables are R.sub.p, x.sub.p and the second variables are R.sub.sp, x.sub.sp.

[0092] The expression “the object X′ is of the same nature as the state vector”, used below, will mean that X′ is a succession of matrices and vectors similar to X.

[0093] The number 3×(r+v)=12 will also be called “dimension of state X” where r is the number of rotation matrices appearing in X and v the number of vectors appearing in X. In other embodiments, this number can be different.

[0094] Furthermore, the invariant Kalman filter is further configured to use as observations relative kinematic data between the main vehicle 1 and the secondary vehicle 2 received by the receiving interface, coming from the first sensor 3 of the main vehicle 1.

[0095] The observation here will be the relative position of the secondary vehicle expressed in the frame of the main vehicle:

Y=−R.sub.sp.sup.T x.sub.sp

[0096] The invariant Kalman filter is configured to use as binary operation an operation comprising a term-by-term composition of the first rigid transformation and of the second rigid transformation.

[0097] This binary operation, denoted *, applies the following transformation to two objects (R.sub.p, x.sub.p, R.sub.sp, x.sub.sp) and (R.sub.p′, x.sub.p′, R.sub.sp′, x.sub.sp′) in one embodiment:

(R.sub.p,x.sub.p,R.sub.sp,x.sub.sp)*(R.sub.p′,x.sub.p′,R.sub.sp′,x.sub.sp′)=(R.sub.pR.sub.p′,x.sub.p′+R.sub.px.sub.p′,R.sub.spR.sub.sp′,x.sub.sp+R.sub.spx.sub.sp)

[0098] 3/ Method for Assisting the Navigation of the Fleet

[0099] Referring to FIG. 3, a method 100 for assisting the navigation of the fleet according to a first embodiment, and implementing an invariant Kalman filter configured as indicated in section 2/, comprises the following steps.

[0100] It is assumed that an estimation {circumflex over (X)}.sub.1 of the navigation state of the fleet has been estimated by the invariant Kalman filter.

[0101] In an acquisition step 102, the first sensor 3 acquires a first group of movement data Y.sub.1 from objects external the main vehicle 1 in the main frame. These data can comprise: [0102] position data of the secondary vehicle 2 in the main frame (the corresponding external object is then the secondary vehicle 2) [0103] position data of at least one daymark in the main frame (the corresponding external object is then this daymark). The daymark is at a known position in the reference frame.

[0104] These data Y.sub.1 are transmitted to the processing unit 10.

[0105] In a step 104, the processing unit 10 calculates the difference between the observed measurements Y.sub.1 and the expected measurements (this difference, denoted Z.sub.1, is called innovation in the literature dealing with Kalman filters). The expected measurements are deduced from the state X.sub.1 previously estimated by the invariant Kalman filter.

[0106] In a correction step 106, the data processing unit 10 multiplies the innovation Z.sub.1 by a matrix K.sub.1 called “gain” matrix, which expresses Z.sub.1 in a linear correction dx.sub.1=K.sub.1Z.sub.1 to be applied to the state of the system.

[0107] The choice of the gains is a classic question common to most estimation methods (see below).

[0108] In a retraction step 108, the processing unit 10 transforms the linear correction dx.sub.1 into a non-linear correction C.sub.1 of the same nature as {circumflex over (X)}.sub.1 (the state {circumflex over (X)}.sub.1 is not a vector because it contains rotations). The transformation used is any function taking as argument a vector of the dimension of the state X (12 in this embodiment) and returning an object of the same nature as X, but a particularly efficient choice is the term-by-term exponential of the Lie group of the pairs of rigid transformations.

[0109] A non-linear update step 110 is then implemented by the processing unit 10. In this step 110, the processing unit 10 combines the estimation X.sub.1 of the state of the system with the non-linear correction C.sub.1 to build a corrected estimation:

{circumflex over (X)}.sub.1.sup.+=C.sub.1*X.sub.1

Where the symbol * is the binary operation defined above. The gain matrix K.sub.1 is chosen so as to stabilize the non-linear estimation error e defined by:

e={circumflex over (X)}.sub.1*X.sup.−1

Where the symbol. .sup.−1 is the usual inversion associated with the operation *

(R.sub.p,x.sub.p,R.sub.sp,x.sub.sp).sup.−1=(R.sub.p.sup.T,−R.sub.p.sup.Tx.sub.p,R.sub.sp.sup.T,−R.sub.sp.sup.Ts.sub.sp)

The error can also be explicitly written in the following manner:

e=({circumflex over (R)}.sub.pR.sub.p.sup.T,{circumflex over (x)}.sub.p−{circumflex over (R)}.sub.pR.sub.p.sup.Tx.sub.p,{circumflex over (R)}.sub.pR.sub.p.sup.T,x.sub.p−{circumflex over (R)}.sub.pR.sub.p.sup.Tx.sub.p)

[0110] This error is used to build the linearized system according to the usual procedure of the invariant filtering, from which the matrix K.sub.1 is deduced, for example by integrating a Riccati equation.

[0111] In a propagation step 112, known per se to those skilled in the art, the processing unit 10 generates a propagated navigation state, from the state X.sub.1.sup.+. To do so, the processing unit 10 applies the evolution model which can be, for example, an odometry, an a priori model or a conventional integration of inertial measurements acquired by the proprioceptive sensors 6, 16 included in the vehicles 1, 2.

[0112] The steps described above form an iteration of the invariant Kalman filter.

[0113] Thanks to the invariant Kalman filter, a property that would also be obtained in a linear case, is obtained: the evolution of the estimation error is autonomous (it depends neither on X nor on {circumflex over (X)}.sub.1).

[0114] The processing unit 10 repeats these same steps 102, 104, 106, 108, 110, 112 in new iterations of the invariant Kalman filter. The state estimated during the propagation step 112 of a given iteration is used as input data for the innovation calculation 104 and non-linear update 110 steps of a next iteration.

[0115] Ultimately, thanks to the method 100, the main vehicle 1 can obtain assistance not only on its own navigation, but also on the navigation of the secondary vehicle 2, based on the different data measured by the first sensor 3 and the proprioceptive sensors 6, 16.

[0116] A method 200 for assisting the navigation of the fleet according to a second embodiment, and implementing an invariant Kalman filter also configured as indicated above, is also shown in the right part of FIG. 3; this method 200 comprises the following steps.

[0117] In an acquisition step 202, movement data Y.sub.2 from the main vehicle 1 in at least one frame attached to an object external to the main vehicle are acquired. The data Y.sub.2 can comprise: [0118] kinematic data from the main vehicle 1 in the secondary frame acquired by the second sensor 12, for example position data of the main vehicle 1 in the secondary frame (in which case the corresponding external object is the secondary vehicle) [0119] data acquired by the receiver 8 (the corresponding object can then be considered to be the Earth, since these data allow geolocating the main vehicle relative to the Earth).

[0120] The data Y.sub.2 are transmitted to the main vehicle 1, where appropriate via the communication interfaces 14 and 4 when they come from the secondary vehicle 2. The data Y.sub.2 are transmitted to the processing unit 10.

[0121] In a step 204 similar to step 104, the processing unit 10 calculates the difference (innovation Z.sub.2) between the observed measurements Y.sub.2 and expected measurements. The expected measurements are deduced from a state previously estimated by the invariant Kalman filter, denoted X.sub.2.

[0122] In a correction step 206, the processing unit 10 multiplies the innovation Z.sub.2 by a gain matrix K.sub.2 which expresses Z.sub.2 in a linear correction dx.sub.2=K.sub.2Z.sub.2 to be applied to the state of the system.

[0123] This correction step 206 is similar to step 106, with the difference that the gain matrix K.sub.2 is chosen so as to stabilize a second non-linear error variable e defined by:

e=X.sup.−1*{circumflex over (X)}.sub.2

[0124] In a retraction step 208 identical to step 108, the processing unit 10 transforms the linear correction dx.sub.2 into a non-linear correction C.sub.2 of the same nature as X.sub.2 (the state X.sub.2 is not a vector because it contains rotations).

[0125] A non-linear update step 210 similar to step 110 is then implemented by the processing unit 10. In this step 210, the processing unit 10 combines the estimation X.sub.2 of the state of the system with the non-linear correction C.sub.2 to build a corrected estimation in the following manner:

{circumflex over (X)}.sub.2.sup.+={circumflex over (X)}.sub.2*C.sub.2

[0126] In a propagation step 212 identical to step 112, the processing unit 10 generates a propagated state from the state {circumflex over (X)}.sub.2.sup.+.

[0127] The steps described above form an iteration of the invariant Kalman filter.

[0128] The processing unit 10 repeats these same steps 202, 204, 206, 208, 210, 212 in new iterations of the invariant Kalman filter. The state estimated during the propagation step 212 of a given iteration is used as input data for the innovation calculation 204 and non-linear update 210 steps of a next iteration.

[0129] As in the method 100 according to the first embodiment, the dependence of the evolution of the error relative to the state of the system is reduced.

[0130] Either of the methods 100, 200 described above can be implemented by the main vehicle 1.

[0131] The fundamental difference between the method 100 according to the first embodiment and the method 200 according to the second embodiment lies in the relative kinematic data between the main vehicle 1 and the secondary vehicle 2 used as an observation by the invariant Kalman filter: in the case of the method 100, these data are expressed in the frame attached to the main vehicle 1, while in the case of the method 200, these data are expressed in an external frame.

[0132] The processing unit 10 of the main vehicle 1 advantageously implements a method according to a third embodiment, combining a implementation in parallel of the two preceding methods 100 and 200.

[0133] The first method 100 leading to obtaining the data {circumflex over (X)}.sub.1.sup.+ is for example implemented by a first processor of the processing unit 10, while the second method 200 leading to obtaining the data {circumflex over (X)}.sub.2.sup.+ is implemented by a second processor of the processing unit. In other words, two invariant Kalman filters are implemented in parallel by these two processors.

[0134] In a merging step 302, the processing unit 10 merges the data {circumflex over (X)}.sub.1.sup.+ and {circumflex over (X)}.sub.2.sup.+ in order to obtain an optimized estimation of the navigation state of the fleet, denoted {circumflex over (X)}.sub.opt.sup.+. For example, {circumflex over (X)}.sub.opt.sup.+ is the average of {circumflex over (X)}.sub.2.sup.+ and {circumflex over (X)}.sub.2.sup.+. As the states {circumflex over (X)}.sub.1.sup.+ and {circumflex over (X)}.sub.2.sup.+ are not vectors, their classic average is replaced with any average definition adapted to manifolds. Those skilled in the art may find generalized average definitions for manifolds in the document Markley, F. L., Cheng, Y., Crassidis, J. L., & Oshman, Y. (2007). Averaging quaternions. Journal of Guidance, Control, and Dynamics, 30(4), 1193-1197.

[0135] Where appropriate, this average is weighted by covariance matrices associated with the data {circumflex over (X)}.sub.1.sup.+ and {circumflex over (X)}.sub.2.sup.+ expressing the uncertainty of these estimations. These covariance matrices are also produced by the two invariant Kalman filters of the methods 100 and 200.

[0136] The invention is not limited to the embodiments described above.

[0137] It is possible to include in the navigation state of the fleet velocities or the vectors representing the characteristic point positions q.sub.i (or daymarks). This navigation state may be limited to comprising position and rotation data.

[0138] Furthermore, the considered fleet of vehicles can comprise several secondary vehicles, and the navigation state can be extended so as to comprise elements specific to each of the secondary vehicles of the fleet.

[0139] In addition, it is not mandatory to use inertial data acquired by an inertial unit during the implementation of either of the methods described above. However, when such inertial data are used, the state of the system should include states representative of the velocity of each vehicle equipped with an inertial unit. It will be: [0140] the velocity v.sub.p of the main vehicle in the reference frame, [0141] the velocity deviation v.sub.sp, relative to the fixed frame, between the two vehicles, projected in the frame attached to the secondary vehicle. To put it another way, v.sub.sp is defined by

v.sub.sp=R.sub.s.sup.T(v.sub.p−v.sub.s)

where v.sub.p is the velocity of the main carrier in the fixed frame, vs the velocity of the secondary carrier in the fixed frame and R.sub.s=R.sub.pR.sub.sp.sup.T the rotation matrix allowing the switching of the coordinates of a point in the secondary frame to its coordinates in the fixed frame. Only one of these two states can also be added to the system.

[0142] It should be noted that the navigation state could, in another embodiment, be formed by the natural variables (R.sub.p,x.sub.p,R.sub.s,x.sub.s), where the rotation matrix R.sub.S and the vector x.sub.s∈ custom-character .sup.3 are such that a point with coordinates u∈.sup.3 in the frame attached to the secondary vehicle will have the coordinates R.sub.su+x.sub.s in the fixed frame. In this case, the binary operation to be used is more complex:

[00001] $(R_{p}, x_{p}, R_{s}, x_{s}) * (R_{p}^{'}, x_{p}^{'}, R_{s}^{'}, x_{s}^{'}) = (\begin{matrix} R_{p} R_{p}^{'} \\ x_{p} + R_{p} x_{p}^{'} \\ R_{p} R_{s}^{'} R_{p}^{T} R_{s} \\ R_{p} R_{s}^{'} R_{p}^{T} (x_{s} - x_{p}) + R_{p} x_{s}^{'} + x_{p} \end{matrix})$

METHOD AND DEVICE FOR ASSISTING WITH THE NAVIGATION OF A FLEET OF VEHICLES USING AN INVARIANT KALMAN FILTER

Assignee

Inventors

Cpc classification

Classification Explorer

G06F7/405

PHYSICS

Classification Explorer

G01S19/393

PHYSICS

Classification Explorer

G01S19/485

PHYSICS

Classification Explorer

G01S19/51

PHYSICS

Classification Explorer

G05D1/0291

PHYSICS

Classification Explorer

G01S19/38

PHYSICS

Classification Explorer

G06F7/42

PHYSICS

Classification Explorer

G01C21/1652

PHYSICS

Classification Explorer

G01S19/43

PHYSICS

Classification Explorer

G01S19/428

PHYSICS

Classification Explorer

G01S19/54

PHYSICS

International classification

Classification Explorer

G01S19/51

PHYSICS

Classification Explorer

G01C21/16

PHYSICS

Classification Explorer

G01S19/39

PHYSICS

Classification Explorer

G01S19/43

PHYSICS

Classification Explorer

G01S19/48

PHYSICS

Classification Explorer

G01S19/54

PHYSICS

Abstract

Claims

Description