matrix

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