GitHub - PX4/PX4-Matrix: Lightweight, dependency free Matrix library (BSD)

ARCHIVED - PX4-Matrix is now maintained within PX4-Autopilot. https://github.com/PX4/PX4-Autopilot/tree/4a3d64f1d76856d22323d1061ac6e560efda0a05/src/lib/matrix

A simple and efficient template based matrix library.

License

(BSD) The Matrix library is licensed under a permissive 3-clause BSD license. Contributions must be made under the same license.

Features

Compile time size checking.
Euler angle (321), DCM, Quaternion conversion through constructors.
High testing coverage.

Limitations

No dynamically sized matrices.

Library Overview

matrix/math.hpp : Provides matrix math routines.
- Matrix (MxN)
- Square Matrix (MxM, has inverse)
- Vector (Mx1)
- Scalar (1x1)
- Quaternion
- Dcm
- Euler (Body 321)
- Axis Angle
matrix/filter.hpp : Provides filtering routines.
- kalman_correct
matrix/integrate.hpp : Provides integration routines.
- integrate_rk4 (Runge-Kutta 4th order)

Testing

The tests can be executed as following:

mkdir build
cd build
cmake  -DTESTING=ON ..
make check

Formatting

The format can be checked as following:

mkdir build
cd build
cmake  -DFORMAT=ON ..
make check_format

Example

See the test directory for detailed examples. Some simple examples are included below:

    // define an euler angle (Body 3(yaw)-2(pitch)-1(roll) rotation)
    float roll = 0.1f;
    float pitch = 0.2f;
    float yaw = 0.3f;
    Eulerf euler(roll, pitch, yaw);

    // convert to quaternion from euler
    Quatf q_nb(euler);

    // convert to DCM from quaternion
    Dcmf dcm(q_nb);

    // you can assign a rotation instance that already exist to another rotation instance, e.g.
    dcm = euler;

    // you can also directly create a DCM instance from euler angles like this
    dcm = Eulerf(roll, pitch, yaw);

    // create an axis angle representation from euler angles
    AxisAngle<float> axis_angle = euler;

    // use axis angle to initialize a DCM
    Dcmf dcm2(AxisAngle(1, 2, 3));
    
    // use axis angle with axis/angle separated to init DCM
    Dcmf dcm3(AxisAngle(Vector3f(1, 0, 0), 0.2));

    // do some kalman filtering
    const size_t n_x = 5;
    const size_t n_y = 3;

    // define matrix sizes
    SquareMatrix<float, n_x> P;
    Vector<float, n_x> x;
    Vector<float, n_y> y;
    Matrix<float, n_y, n_x> C;
    SquareMatrix<float, n_y> R;
    SquareMatrix<float, n_y> S;
    Matrix<float, n_x, n_y> K;

    // define measurement matrix
    C = zero<float, n_y, n_x>(); // or C.setZero()
    C(0,0) = 1;
    C(1,1) = 2;
    C(2,2) = 3;

    // set x to zero
    x = zero<float, n_x, 1>(); // or x.setZero()

    // set P to identity * 0.01
    P = eye<float, n_x>()*0.01;

    // set R to identity * 0.1
    R = eye<float, n_y>()*0.1;

    // measurement
    y(0) = 1;
    y(1) = 2;
    y(2) = 3;

    // innovation
    r = y - C*x;

    // innovations variance
    S = C*P*C.T() + R;

    // Kalman gain matrix
    K = P*C.T()*S.I();
    // S.I() is the inverse, defined for SquareMatrix

    // correction
    x += K*r;

    // slicing
    float data[9] = {0, 2, 3,
                     4, 5, 6,
                     7, 8, 10
                    };
    SquareMatrix<float, 3> A(data);

    // Slice a 3,3 matrix starting at row 1, col 0,
    // with size 2 x 3, warning, no size checking
    Matrix<float, 2, 3> B(A.slice<2, 3>(1, 0));

    // this results in:
    // 4, 5, 6
    // 7, 8, 10