CPC G01S 19/393 (2019.08) [G01C 21/1652 (2020.08); G01S 19/38 (2013.01); G01S 19/428 (2013.01); G01S 19/43 (2013.01); G01S 19/485 (2020.05); G01S 19/51 (2013.01); G01S 19/54 (2013.01); G05D 1/0291 (2013.01); G06F 7/405 (2013.01); G06F 7/42 (2013.01)] | 20 Claims |
1. 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 theft 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.
|