bflib

Bayesian Filters Library

This library allows the fast implementation of linear and non-linear system predictors based on Bayesian Filters

Examples

Aircraft takeoff (Linear Kalman Filter): linear_aircraft.cpp	Aircraft takeoff (Extended Kalman Filter): non_linear_aircraft.cpp
Pendulum (Extended Kalman Filter): pendulum.cpp	Sine wave prediction (Extended Kalman Filter): sine.cpp
Robot localization (Extended Kalman Filter): robot_localization_ekf.cpp	Robot localization (Particles Filter): robot_localization_pf.cpp

Features

• Linear Kalman Filter
• Extended Kalman Filter
• Particles Filter ( Monte Carlo )
• Simulate process and sensors
• Time features
• Access to state uncertainty
• Built-in data association
• Easy integration ( Header-Only )
• Optimized for speed
• Eigen as the only dependecy for the library (Samples may use LibPython and OpenCV for data visualization )

To-do

• Unscented Kalman Filter
• Better documentation

Example code

This example shows how to predict an aircraft altitude and speed during a takeoff using an approximated linear system. This process can be described by the following set of equations:

These equations can be rewritten as a system of state-space linear equations:

#include <bflib/KF.hpp>
#include <iostream>

using namespace std;

typedef KF<float, 2, 1, 1> Aircraft;

// The process model
void process(Aircraft::StateMatrix &A, Aircraft::InputMatrix &B, Aircraft::OutputMatrix &C, double dt)
{
    // Fills the A, B and C matrixes of the process
    A << 1, dt,
         0, 1;
    
    B << (dt*dt)/2.0,
         dt; 

    C << 1, 0;
}

int main(int argc, char *argv[])
{
    // The system standard deviation
    float sigma_x_s = 0.01; // std for position
    float sigma_x_v = 0.02; // std for speed
    float sigma_y_s = 5.0; // std for position sensor

    // Creates a linear kalman filter with float data type, 2 states, 1 input and 1 output
    Aircraft kf;

    // Sets the process
    kf.setProcess(process);

    // Creates a new process covariance matrix Q
    Aircraft::ModelCovariance Q;
    // Fills the Q matrix
    Q << sigma_x_s*sigma_x_s, sigma_x_s*sigma_x_v,
         sigma_x_v*sigma_x_s, sigma_x_v*sigma_x_v;  
    // Sets the new Q to the KF
    kf.setQ(Q);

    // Creates a new sensor covariance matrix R
    Aircraft::SensorCovariance R;
    // Fills the R matrix
    R << sigma_y_s*sigma_y_s;
    // Sets the new R to the KF
    kf.setR(R);

    // Creates two states vectors, one for the simulation and one for the kalman output
    Aircraft::State x, xK;

    // Creates an input vector and fills it
    Aircraft::Input u;
    u << 0.1;

    // Creates an output vector
    Aircraft::Output y;

    // Defines the simulation max time and the sample time
    float T = 30;
    float dt = 0.1;
    // Creates a variable to hold the time 
    float t = 0;
    while (t < T)
    {
        // Simulate the system in order to obtain the sensor reading (y).
        // It is not needed on a real system
        kf.simulate(x, y, u, dt);
        // Run the kalman filter
        kf.run(xK, y, u, dt);

        // Prints the predicted state
        cout << xK << endl;

        // Increments the simulation time
        t += dt;
    }

    return 0;
}