EKF State Estimation Package

Package Information

Description

The State Estimation module is responsible for the processing of the perception module and localisation sensors’ data and its transformation into useful information. In this case, the data is combined to generate the best estimates possible on the pose and speed of the vehicle and the location of the cones that delimit the track.

Folder Structure

• adapter_ekf_state_est: Adapters to change ros2 interfaces according to simulator or environment
• kalman_filter: Extended Kalman Filter class, used for state estimation
  • kalman_filter/data_association: Data Association class, used to associate observations to landmarks and determine which landmarks are new
  • kalman_filter/motion_model: Motion Model class, used to predict the state based on previous states and measurements
  • kalman_filter/observation_model: Observation Model class, used to correct the state based on observations
• ros_node: Node class

Launch Configurations

• eufs.launch.py: Launch file for the EUFS simulator
• pacsim.launch.py: Launch file for the PacSim simulator

Important Dependencies

• Eigen3

How to Run

Install Dependencies

  ./dependencies_install.sh

Compiling

colcon build --packages-up-to ekf_state_est

Testing

colcon test --packages-select ekf_state_est # use event-handler=console_direct+ for imediate output

To check test results:

colcon test-result --all --verbose

or

source ./install/setup.bash # If in a new terminal
ros2 run ekf_state_est ekf_state_est_test

Running

Use a launch file:

source ./install/setup.bash # If in a new terminal
ros2 launch ekf_state_est eufs.launch.py

or run directly:

source ./install/setup.bash # If in a new terminal
ros2 run ekf_state_est ekf_state_est

Design

Below, some diagrams are presented that can illustrate the structure and behaviour of the program.

Behaviour

The control flow below illustrates briefly the flow of execution and information through the node, you can also checkout documentation pdf to get a better understanding.

The sequence diagrams below illustrate in greater detail the interactions between the components of the system in a normal use case.

1. Information from localisation sensors is received in the adapter (the adapter is a layer for adaptation of the different environments the node runs in).
2. LMNode (the localisation and mapping node) processes the information and requests execution of the prediction step to the Extended Kalman Filter cass (illustrated below).
   1. The Motion Model is used to utilize the Motion data to predict the change in state.
   2. The Motion Model is used to reaturn the G matrix (maps the transformation from the motion to the state)
   3. Get the noise matrix, required for the calculation of the predicted covariance matrix P.
   4. Compute predicted X and P.
3. After the step, the pose and map are updated and published.

File truncated at 100 lines see the full file