Fleet Pose Estimation with Invariant Kalman Filtering
Find Innovative SolutionsGenerate Solutions
Solution Overview
Problem
Existing navigation systems for fleets of vehicles face challenges in accurately estimating the state of multiple moving vehicles, particularly when surrounded by other potentially moving vehicles, as they rely on assumptions about fixed reference points that do not hold in dynamic environments.
Innovation Solution
A method using an invariant Kalman filter that estimates the navigation state of a fleet by incorporating relative kinematic data between main and secondary vehicles, with an internal composition law that allows for term-to-term composition of rigid transformations, enabling the use of surrounding vehicles as mobile reference points and simplifying calculations.
Engineering Contradictions & Design Principles
Engineering Contradiction Analysis
1Reliability
If an ordinary Kalman filter or extended Kalman filter is used for fleet navigation, then the estimation can be performed, but the error evolution depends on the system state leading to divergence in nonlinear cases
Solution Approach 1:
The patent transforms the navigation state parameters into a Lie group structure (SE(3) for rigid transformations), where the error evolution becomes autonomous and independent of the system state. This parameter transformation allows the use of invariant Kalman filter that maintains constant error statistics regardless of the fleet's configuration or motion state, resolving the divergence problem of extended Kalman filters.
2Device complexity
If invariant Kalman filter is used with proper group operation, then the error evolution becomes autonomous and simple, but finding the correct operation for specific systems is difficult and requires specialized knowledge
Solution Approach 1:
The patent applies a universal Lie group framework (SE(3)) that can handle multiple types of rigid transformations (rotations, translations, and their compositions) in a unified manner. This universal approach eliminates the need to derive specialized operations for each specific navigation scenario, making the invariant Kalman filter applicable to various fleet configurations without increasing implementation difficulty.
3Device complexity
If fixed reference points are assumed for navigation estimation, then calculations are simplified, but the assumption does not hold in dynamic environments with moving vehicles
Solution Approach 1:
The patent models the fleet of vehicles as a dynamic system where each vehicle's frame can move relative to others through rigid transformations in SE(3). Instead of assuming fixed reference points, the system dynamically tracks the relative poses between vehicles, allowing the navigation estimation to adapt to changing fleet configurations while maintaining mathematical tractability through the Lie group structure.
Data Source
Figure 1~2
Figure 3
AI summary
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.