Kalman Filter Applied

The Kalman Filter Applied package allow you to simulate a trajectory and estimate the odometry based on recorded measurements. The estimation is generated by using the EKF algorithm with constant velocity. The measurements that are taken into account are the following:

• GPS
• Radar

Installation

Prerequisites: you need to have ROS installed.

mkdir -p catkin_ws/src && cd catkin_ws/src
git clone https://github.com/AleRiccardi/kalman_filter_applied.git
# git clone git@github.com:AleRiccardi/kalman_filter_applied.git

cd ..
catkin init
catkin build
source devel/setup.bash
# source devel/setup.zsh

Usage

To launch the simulator:

roslaunch kfa simulator.launch

To estimate the odometry:

roslaunch kfa kfa.launch

The Kalman Filter

Kalman filtering is an algorithm that provides estimates of some unknown variables given the measurements observed over time. The unknown variables, in our case, are the 3D coordinates of our object in the simulated environment.

The algorithm works in a two-step process: propagation and correction. In the propagation step, the current state is propagated along with their uncertainties. Once the outcome of the next measurement (necessarily corrupted with some amount of error, including random noise) is observed, the estimated state is then corrected.

State vector

The state vector at time t is defined as:

where x, y, z are the 3D coordinates and x, y, z (dot) are the velocities.

Propagation step

The most common dynamic model is a constant velocity (CV) model, which assumes that the velocity is constant during a sampling interval. The linear propagation equation are defined as:

The State-transition matrix is defined as:

where is the time interval between two propagation steps.

The Control-input matrix is defined as:

Correction step

In the Correction step, the current prediction is combined with current observation information to refine the state estimate. The non-linear correction equation are defined as:

GPS measurement

The GPS provides measurements of the position of the object in 3D coordinates:

The observation Jacobian matrix H is the identity matrix:

Radar measurement

The Radar provides measurements of the object in spherical coordinate system:

The observation Jacobian matrix H is defined as:

Contributing

Pull requests are welcome. For major changes, please open an issue to discuss what you would like to change.

Clang

To use clang format you have to run the following command:

./scripts/clang_format.sh

To use clang tidy first you have to generate the compile_commands.json file and then run the script:

catkin config --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON 
catkin build

./scripts/clang_tidy.sh $(catkin locate -b -e kfa)

License

The MIT licence is used.