Getting started - brhannan/track Wiki

Getting started

Creating a new filter

Filter constructors accept an optional string input specifies a motion profile. Allowed values are 1d_cons_vel, 2d_const_vel, 3d_const_vel, 1d_const_accel, 2d_const_accel, 3d_const_accel. When this input argument is provided, all filter properties are automatically initialized so that the filter is ready for use without additional user input.

For example,

track::KalmanFilter<double> kf("2d_const_vel", 0.1); // sample time = 0.1 sec

will create a KalmanFilter object kf that can be used to track a target that is expected to travel on a 2D constant-velocity trajectory. kf has a state transition matrix F = [ 1 0.1; 0 1] (semicolons indicate the end of a row) and a measurement matrix H = [ 1, 0, 0, 0; 0, 1, 0, 0 ]. Its process noise matrix is a 4-by-4 identity matrix and its measurement noise matrix is a 2-by-2 identity matrix. All parameters have data type double.

All track filters allow time-varying matrices (for example, the state transition matrix may change with time) and support flexible sample rate.

You can omit the motion profile input and instead provide your own matrices. For example:

track::KalmanFilter<double> kf();
// Create matrices to use in kf.
std::vector<double> f_vals = {1, 0.1, 0, 1}; // Row-unwrapped vector of matrix values.
track::Matrix<double> F(f_fals,2,2); // Tell track::Matrix to create a 2-by-2 matrix.
std::vector<double> h_vals = { 1, 0, 0, 0, 0, 1, 0, 0 };
track::Matrix<double> H(h_fals,2,4);
// Set kf properties.
kf.state_transition_matrix = F;
kf.measurement_matrix = H;
// Optionally set process_noise, measurement_noise, control_matrix values here...

Using the filter

The init() method must be called before stepping the filter. init() initializes and validates filter properties.

kf.init()

Call the update() method to update the filter. A measurement vector (a single-column track::Matrix) must be provided as an input to update(). Perform the predict step by calling predict().

// Provide a length-2 measurement vector. Assume the measurement matrix has 2 rows.
std::vector<double> meas_vec = {x_meas, y_meas};
Matrix<double> y(meas_vec,2,1);
kf.update(y);
kf.predict();

Working with matrices

The track::Matrix class provides a container for storing matrices and implements common matrix operations, such as addition, multiplication, and inversion.

There are a number of ways to create a new matrix. You can use a std::vector of values to place into a matrix along with the desired matrix dimensions.

std::vector<double> v {1, 0, 0, 1};
// Reshape v into a 2-by-2 identity matrix. v is wrapped row-wise.
track::Matrix<double> mat(v,2,2);

The template argument double specifies the data type of the matrix. The syntax track::Matrix<> ... indicates that the default type, double will be used. Providing the string "identity" to the Matrix constructor will create an identity matrix.

// Create a 2-by-2 identity matrix.
track::Matrix<double> mat("identity",2,2);

Similarly, you can provide "zeros" or "ones" to create a matrix of all-zeros or all-ones.

track::Matrix<double> mat0("zeros",2,3);
track::Matrix<double> mat1("ones",4,5);

Two track::Matrix objects M1 and M2 can be multiplied with M1*M2. Addition and subtraction are carried out with M1+M2 and M1-M2. A matrix M can be multiplied by a scalar v with v*M. M/v will divide each element of M by the scalar value v. Use the inverse() method to invert a matrix: track::Matrix<> Minv = M.inverse() (<> defaults to <double>).

Dependencies

Track requires Boost.