consume PX4/Matrix repository and preserve history

src/lib/matrix/.clang-tidy

@@ -0,0 +1,9 @@
+Checks:          'clang-diagnostic-*,clang-analyzer-*,*,
+		,-cppcoreguidelines-pro-type-vararg
+		,-cppcoreguidelines-pro-bounds-array-to-pointer-decay
+		,-cppcoreguidelines-pro-bounds-constant-array-index
+		,-cppcoreguidelines-pro-bounds-pointer-arithmetic
+		'
+WarningsAsErrors: '*'
+HeaderFilterRegex: '*.h, *.hpp, *.hh, *.hxx'
+

src/lib/matrix/.github/workflows/test.yml

@@ -0,0 +1,53 @@
+name: Tests
+
+on:
+  push:
+    branches:
+    - 'master'
+  pull_request:
+    branches:
+    - '*'
+
+jobs:
+  build:
+    runs-on: ubuntu-latest
+    strategy:
+      fail-fast: false  # don't cancel if a job from the matrix fails
+      matrix:
+        compiler:
+          - { compiler: GNU,  CC: gcc,   CXX: g++, FORMAT: ON }
+          - { compiler: LLVM, CC: clang, CXX: clang++, FORMAT: OFF }
+        #flavor: [ Release, Coverage ] # TODO
+        flavor: [ Release ]
+        exclude:
+          - compiler: { compiler: LLVM, CC: clang, CXX: clang++ }
+            flavor: Coverage
+
+    steps:
+      - uses: actions/checkout@v2
+        with:
+          submodules: 'recursive'
+      - name: Install Dependencies
+        run: |
+          sudo apt install -y clang cmake g++ gcc lcov
+      - name: Fetch coveralls
+        if: matrix.flavor == 'Coverage'
+        run: |
+          pip install --user cpp-coveralls
+
+      - name: Build & Run
+        run: |
+          cmake -DCMAKE_BUILD_TYPE=${{ matrix.flavor }} -DSUPPORT_STDIOSTREAM=ON -DTESTING=ON -DFORMAT=${{ matrix.compiler.FORMAT }} .
+          make check
+
+          # TODO: enable
+#      - name: Coveralls build
+#        if: matrix.flavor == 'Coverage'
+#        run: |
+#          cpp-coveralls -i matrix
+#      - name: Coveralls
+#        if: matrix.flavor == 'Coverage'
+#        uses: coverallsapp/github-action@master
+#        with:
+#          github-token: ${{ secrets.GITHUB_TOKEN }}
+

src/lib/matrix/.gitignore

@@ -0,0 +1,39 @@
+*.orig
+*.swp
+*~
+astyle/
+build*/
+cmake_install.cmake
+CMakeCache.txt
+CMakeFiles/
+compile_commands.json
+CPackConfig.cmake
+CPackSourceConfig.cmake
+CTestTestfile.cmake
+Makefile
+test/attitude
+test/cmake_install.cmake
+test/CMakeFiles/
+test/copyto
+test/coverage.info
+test/CTestTestfile.cmake
+test/filter
+test/hatvee
+test/helper
+test/integration
+test/inverse
+test/Makefile
+test/matrixAssignment
+test/matrixMult
+test/matrixScalarMult
+test/out/
+test/pseudoinverse
+test/setIdentity
+test/slice
+test/squareMatrix
+test/transpose
+test/vector
+test/vector2
+test/vector3
+test/vectorAssignment
+Testing/

src/lib/matrix/CMakeLists.txt

@@ -0,0 +1,180 @@
+cmake_minimum_required(VERSION 2.8)
+
+set(VERSION_MAJOR "1")
+set(VERSION_MINOR "0")
+set(VERSION_PATCH "2")
+
+project(matrix CXX)
+
+set(CMAKE_CXX_STANDARD 14)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+if (NOT CMAKE_BUILD_TYPE)
+    if(TESTING)
+        set(CMAKE_BUILD_TYPE "Debug" CACHE STRING "Build type" FORCE)
+    else()
+        set(CMAKE_BUILD_TYPE "RelWithDebInfo" CACHE STRING "Build type" FORCE)
+    endif()
+    message(STATUS "set build type to ${CMAKE_BUILD_TYPE}")
+endif()
+
+set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS "Debug;Release;RelWithDebInfo;MinSizeRel;Coverage")
+
+option(SUPPORT_STDIOSTREAM "If enabled provides support for << operator (as used with std::cout)" OFF)
+option(TESTING "Enable testing" OFF)
+option(FORMAT "Enable formatting" OFF)
+option(COV_HTML "Display html for coverage" OFF)
+option(ASAN "Enable address sanitizer" OFF)
+option(UBSAN "Enable undefined behaviour sanitizer" OFF)
+
+set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
+
+include_directories(${CMAKE_SOURCE_DIR})
+
+if(SUPPORT_STDIOSTREAM)
+    add_definitions(-DSUPPORT_STDIOSTREAM)
+endif()
+
+if (("${CMAKE_CXX_COMPILER_ID}" MATCHES "Clang") OR ("${CMAKE_CXX_COMPILER_ID}" MATCHES "AppleClang"))
+	set(CMAKE_CXX_FLAGS_COVERAGE
+		"--coverage -ftest-coverage  -fdiagnostics-absolute-paths -O0 -fprofile-arcs -fno-inline-functions -fno-elide-constructors"
+		CACHE STRING "Flags used by the C++ compiler during coverage builds" FORCE)
+	set(CMAKE_EXE_LINKER_FLAGS_COVERAGE
+		"-ftest-coverage -fdiagnostics-absolute-paths"
+		CACHE STRING "Flags used for linking binaries during coverage builds" FORCE)
+else()
+	set(CMAKE_CXX_FLAGS_COVERAGE
+		# Add when GCC 9 or later is available as part of the default toolchain:  -fprofile-abs-path
+		"--coverage -fprofile-arcs -ftest-coverage -O0 -fno-default-inline -fno-inline -fno-inline-small-functions -fno-elide-constructors"
+		CACHE STRING "Flags used by the C++ compiler during coverage builds" FORCE)
+	set(CMAKE_EXE_LINKER_FLAGS_COVERAGE
+		"--coverage -ftest-coverage -lgcov"
+		CACHE STRING "Flags used for linking binaries during coverage builds" FORCE)
+endif()
+mark_as_advanced(CMAKE_CXX_FLAGS_COVERAGE CMAKE_C_FLAGS_COVERAGE CMAKE_EXE_LINKER_FLAGS_COVERAGE)
+
+add_compile_options(
+    -pedantic
+    -Wall
+    -Warray-bounds
+    -Wcast-align
+    -Wcast-qual
+    -Wconversion
+    -Wctor-dtor-privacy
+    -Wdisabled-optimization
+    -Werror
+    -Wextra
+    -Wfloat-equal
+    -Wformat-security
+    -Wformat=2
+    -Winit-self
+    -Wlogical-op
+    -Wmissing-declarations
+    -Wmissing-include-dirs
+    -Wno-sign-compare
+    -Wno-unused
+    -Wno-unused-parameter
+    -Wnoexcept
+    -Wold-style-cast
+    -Woverloaded-virtual
+    -Wpointer-arith
+    -Wredundant-decls
+    -Wreorder
+    -Wshadow
+    -Wsign-conversion
+    -Wsign-promo
+    -Wstrict-null-sentinel
+    -Wswitch-default
+    -Wundef
+    -Wuninitialized
+    -Wunused-variable
+    )
+
+# clang tolerate unknown gcc options
+if("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang")
+    add_compile_options(
+        -Wno-error=unused-command-line-argument-hard-error-in-future
+        -Wno-unknown-warning-option
+        )
+else()
+    add_compile_options(
+        -Wstrict-overflow=5
+        )
+endif()
+
+# santiziers (ASAN, UBSAN)
+if(ASAN)
+    message(STATUS "address sanitizer enabled")
+
+    # environment variables
+    #  ASAN_OPTIONS=detect_stack_use_after_return=1
+    #  ASAN_OPTIONS=check_initialization_order=1
+
+    add_compile_options(
+        -fsanitize=address
+        -g3
+        -O1
+        -fno-omit-frame-pointer
+        )
+
+    set(CMAKE_EXE_LINKER_FLAGS ${CMAKE_EXE_LINKER_FLAGS} -fsanitize=address)
+
+elseif(UBSAN)
+    message(STATUS "undefined behaviour sanitizer enabled")
+
+    add_compile_options(
+        -fsanitize=undefined
+        -fsanitize=integer
+        -g3
+        -O1
+        -fno-omit-frame-pointer
+        )
+
+    set(CMAKE_EXE_LINKER_FLAGS ${CMAKE_EXE_LINKER_FLAGS} -fsanitize=undefined)
+endif()
+
+
+add_custom_target(check COMMAND ${CMAKE_CTEST_COMMAND} --output-on-failure)
+
+if(TESTING)
+    enable_testing()
+    add_subdirectory(test)
+    add_dependencies(check test_build)
+
+    add_custom_target(clang-tidy COMMAND clang-tidy -p . ${CMAKE_SOURCE_DIR}/test/*.cpp)
+    add_dependencies(clang-tidy test_build)
+endif()
+
+if(FORMAT)
+    set(astyle_exe ${CMAKE_BINARY_DIR}/astyle/src/bin/astyle)
+    add_custom_command(OUTPUT ${astyle_exe}
+        COMMAND wget http://sourceforge.net/projects/astyle/files/astyle/astyle%202.06/astyle_2.06_linux.tar.gz -O /tmp/astyle.tar.gz
+        COMMAND tar -xvf /tmp/astyle.tar.gz
+        COMMAND cd astyle/src && make -f ../build/gcc/Makefile
+        WORKING_DIRECTORY ${CMAKE_BINARY_DIR}
+        )
+
+    add_custom_target(check_format
+        COMMAND scripts/format.sh ${astyle_exe} 0
+        WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
+        DEPENDS ${astyle_exe}
+        VERBATIM
+    )
+
+    add_custom_target(format
+        COMMAND scripts/format.sh ${astyle_exe} 1
+        WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
+        VERBATIM
+        DEPENDS ${astyle_exe}
+    )
+
+    add_dependencies(check check_format)
+endif()
+
+set(CPACK_PACKAGE_VERSION_MAJOR ${VERSION_MAJOR})
+set(CPACK_PACKAGE_VERSION_MINOR ${VERSION_MINOR})
+set(CPACK_PACKAGE_VERSION_PATCH ${VERSION_PATCH})
+set(CPACK_PACKAGE_CONTACT "james.goppert@gmail.com")
+include(CPack)
+
+# vim: set et fenc=utf-8 ft=cmake ff=unix sts=0 sw=4 ts=4 :

src/lib/matrix/LICENSE

@@ -0,0 +1,29 @@
+BSD 3-Clause License
+
+Copyright (c) 2015, PX4 Pro Drone Autopilot
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

src/lib/matrix/README.md

@@ -0,0 +1,146 @@
+# matrix [![Build Status](https://travis-ci.org/PX4/Matrix.svg?branch=master)](https://travis-ci.org/PX4/Matrix) [![Coverage Status](https://coveralls.io/repos/PX4/Matrix/badge.svg?branch=master&service=github)](https://coveralls.io/github/PX4/Matrix?branch=master)
+
+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:
+
+```c++
+    // 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
+```
+<!--  vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : -->

src/lib/matrix/doc/.gitignore

@@ -0,0 +1 @@
+.ipynb_checkpoints/

src/lib/matrix/matrix/AxisAngle.hpp

@@ -0,0 +1,160 @@
+/**
+ * @file AxisAngle.hpp
+ *
+ * @author James Goppert <james.goppert@gmail.com>
+ */
+
+#pragma once
+
+#include "math.hpp"
+
+namespace matrix
+{
+
+template <typename Type>
+class Dcm;
+
+template <typename Type>
+class Euler;
+
+template <typename Type>
+class AxisAngle;
+
+/**
+ * AxisAngle class
+ *
+ * The rotation between two coordinate frames is
+ * described by this class.
+ */
+template<typename Type>
+class AxisAngle : public Vector<Type, 3>
+{
+public:
+    using Matrix31 = Matrix<Type, 3, 1>;
+
+    /**
+     * Constructor from array
+     *
+     * @param data_ array
+     */
+    explicit AxisAngle(const Type data_[3]) :
+        Vector<Type, 3>(data_)
+    {
+    }
+
+    /**
+     * Standard constructor
+     */
+    AxisAngle() = default;
+
+    /**
+     * Constructor from Matrix31
+     *
+     * @param other Matrix31 to copy
+     */
+    AxisAngle(const Matrix31 &other) :
+        Vector<Type, 3>(other)
+    {
+    }
+
+    /**
+     * Constructor from quaternion
+     *
+     * This sets the instance from a quaternion representing coordinate transformation from
+     * frame 2 to frame 1 where the rotation from frame 1 to frame 2 is described
+     * by a 3-2-1 intrinsic Tait-Bryan rotation sequence.
+     *
+     * @param q quaternion
+     */
+    AxisAngle(const Quaternion<Type> &q)
+    {
+        AxisAngle &v = *this;
+        Type mag = q.imag().norm();
+        if (fabs(mag) >= Type(1e-10)) {
+            v = q.imag() * Type(Type(2) * atan2(mag, q(0)) / mag);
+        } else {
+            v = q.imag() * Type(Type(2) * Type(sign(q(0))));
+        }
+    }
+
+    /**
+     * Constructor from dcm
+     *
+     * Instance is initialized from a dcm representing coordinate transformation
+     * from frame 2 to frame 1.
+     *
+     * @param dcm dcm to set quaternion to
+     */
+    AxisAngle(const Dcm<Type> &dcm)
+    {
+        AxisAngle &v = *this;
+        v = AxisAngle<Type>(Quaternion<Type>(dcm));
+    }
+
+    /**
+     * Constructor from euler angles
+     *
+     * This sets the instance to a quaternion representing coordinate transformation from
+     * frame 2 to frame 1 where the rotation from frame 1 to frame 2 is described
+     * by a 3-2-1 intrinsic Tait-Bryan rotation sequence.
+     *
+     * @param euler euler angle instance
+     */
+    AxisAngle(const Euler<Type> &euler)
+    {
+        AxisAngle &v = *this;
+        v = AxisAngle<Type>(Quaternion<Type>(euler));
+    }
+
+    /**
+     * Constructor from 3 axis angle values (unit vector * angle)
+     *
+     * @param x r_x*angle
+     * @param y r_y*angle
+     * @param z r_z*angle
+     */
+    AxisAngle(Type x, Type y, Type z)
+    {
+        AxisAngle &v = *this;
+        v(0) = x;
+        v(1) = y;
+        v(2) = z;
+    }
+
+    /**
+     * Constructor from axis and angle
+     *
+     * @param axis An axis of rotation, normalized if not unit length
+     * @param angle The amount to rotate
+     */
+    AxisAngle(const Matrix31 & axis_, Type angle_)
+    {
+        AxisAngle &v = *this;
+        // make sure axis is a unit vector
+        Vector<Type, 3> a = axis_;
+        a = a.unit();
+        v(0) = a(0)*angle_;
+        v(1) = a(1)*angle_;
+        v(2) = a(2)*angle_;
+    }
+
+
+    Vector<Type, 3> axis() {
+        if (Vector<Type, 3>::norm() > 0) {
+            return Vector<Type, 3>::unit();
+        } else {
+            return Vector3<Type>(1, 0, 0);
+        }
+    }
+
+    Type angle() {
+        return Vector<Type, 3>::norm();
+    }
+};
+
+using AxisAnglef = AxisAngle<float>;
+using AxisAngled = AxisAngle<double>;
+
+} // namespace matrix
+
+/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

src/lib/matrix/matrix/Dcm.hpp

@@ -0,0 +1,187 @@
+/**
+ * @file Dcm.hpp
+ *
+ * A direction cosine matrix class.
+ * All rotations and axis systems follow the right-hand rule.
+ *
+ * This library uses the convention that premultiplying a three dimensional
+ * vector represented in coordinate system 1 will apply a rotation from coordinate system
+ * 1 to coordinate system 2 to the vector.
+ * Likewise, a matrix instance of this class also represents a coordinate transformation
+ * from frame 2 to frame 1.
+ *
+ * @author James Goppert <james.goppert@gmail.com>
+ */
+
+#pragma once
+
+#include "math.hpp"
+
+namespace matrix
+{
+
+template<typename Type>
+class Quaternion;
+
+template<typename Type>
+class Euler;
+
+template<typename Type>
+class AxisAngle;
+
+/**
+ * Direction cosine matrix class
+ *
+ * The rotation between two coordinate frames is
+ * described by this class.
+ */
+template<typename Type>
+class Dcm : public SquareMatrix<Type, 3>
+{
+public:
+    using Vector3 = Matrix<Type, 3, 1>;
+
+    /**
+     * Standard constructor
+     *
+     * Initializes to identity
+     */
+    Dcm() : SquareMatrix<Type, 3>(eye<Type, 3>()) {}
+
+    /**
+     * Constructor from array
+     *
+     * @param _data pointer to array
+     */
+    explicit Dcm(const Type data_[3][3]) : SquareMatrix<Type, 3>(data_)
+    {
+    }
+
+    /**
+     * Constructor from array
+     *
+     * @param _data pointer to array
+     */
+    explicit Dcm(const Type data_[9]) : SquareMatrix<Type, 3>(data_)
+    {
+    }
+
+    /**
+     * Copy constructor
+     *
+     * @param other Matrix33 to set dcm to
+     */
+    Dcm(const Matrix<Type, 3, 3> &other) : SquareMatrix<Type, 3>(other)
+    {
+    }
+
+    /**
+     * Constructor from quaternion
+     *
+     * Instance is initialized from quaternion representing
+     * coordinate transformation from frame 2 to frame 1.
+     *
+     * @param q quaternion to set dcm to
+     */
+    Dcm(const Quaternion<Type> &q)
+    {
+        Dcm &dcm = *this;
+        const Type a = q(0);
+        const Type b = q(1);
+        const Type c = q(2);
+        const Type d = q(3);
+        const Type aa = a * a;
+        const Type ab = a * b;
+        const Type ac = a * c;
+        const Type ad = a * d;
+        const Type bb = b * b;
+        const Type bc = b * c;
+        const Type bd = b * d;
+        const Type cc = c * c;
+        const Type cd = c * d;
+        const Type dd = d * d;
+        dcm(0, 0) = aa + bb - cc - dd;
+        dcm(0, 1) = Type(2) * (bc - ad);
+        dcm(0, 2) = Type(2) * (ac + bd);
+        dcm(1, 0) = Type(2) * (bc + ad);
+        dcm(1, 1) = aa - bb + cc - dd;
+        dcm(1, 2) = Type(2) * (cd - ab);
+        dcm(2, 0) = Type(2) * (bd - ac);
+        dcm(2, 1) = Type(2) * (ab + cd);
+        dcm(2, 2) = aa - bb - cc + dd;
+    }
+
+    /**
+     * Constructor from euler angles
+     *
+     * This sets the transformation matrix from frame 2 to frame 1 where the rotation
+     * from frame 1 to frame 2 is described by a 3-2-1 intrinsic Tait-Bryan rotation sequence.
+     *
+     *
+     * @param euler euler angle instance
+     */
+    Dcm(const Euler<Type> &euler)
+    {
+        Dcm &dcm = *this;
+        Type cosPhi = Type(cos(euler.phi()));
+        Type sinPhi = Type(sin(euler.phi()));
+        Type cosThe = Type(cos(euler.theta()));
+        Type sinThe = Type(sin(euler.theta()));
+        Type cosPsi = Type(cos(euler.psi()));
+        Type sinPsi = Type(sin(euler.psi()));
+
+        dcm(0, 0) = cosThe * cosPsi;
+        dcm(0, 1) = -cosPhi * sinPsi + sinPhi * sinThe * cosPsi;
+        dcm(0, 2) = sinPhi * sinPsi + cosPhi * sinThe * cosPsi;
+
+        dcm(1, 0) = cosThe * sinPsi;
+        dcm(1, 1) = cosPhi * cosPsi + sinPhi * sinThe * sinPsi;
+        dcm(1, 2) = -sinPhi * cosPsi + cosPhi * sinThe * sinPsi;
+
+        dcm(2, 0) = -sinThe;
+        dcm(2, 1) = sinPhi * cosThe;
+        dcm(2, 2) = cosPhi * cosThe;
+    }
+
+
+    /**
+     * Constructor from axis angle
+     *
+     * This sets the transformation matrix from frame 2 to frame 1 where the rotation
+     * from frame 1 to frame 2 is described by a 3-2-1 intrinsic Tait-Bryan rotation sequence.
+     *
+     *
+     * @param euler euler angle instance
+     */
+    Dcm(const AxisAngle<Type> &aa)
+    {
+        Dcm &dcm = *this;
+        dcm = Quaternion<Type>(aa);
+    }
+
+    Vector<Type, 3> vee() const      // inverse to Vector.hat() operation
+    {
+        const Dcm &A(*this);
+        Vector<Type, 3> v;
+        v(0) = -A(1, 2);
+        v(1) =  A(0, 2);
+        v(2) = -A(0, 1);
+        return v;
+    }
+
+    void renormalize()
+    {
+        /* renormalize rows */
+        for (size_t r = 0; r < 3; r++) {
+            matrix::Vector3<Type> rvec(Matrix<Type,1,3>(this->Matrix<Type,3,3>::row(r)).transpose());
+            this->Matrix<Type,3,3>::row(r) = rvec.normalized();
+        }
+    }
+};
+
+using Dcmf = Dcm<float>;
+using Dcmd = Dcm<double>;
+
+} // namespace matrix
+
+/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

src/lib/matrix/matrix/Dual.hpp

@@ -0,0 +1,373 @@
+/**
+ * @file Dual.hpp
+ *
+ * This is a dual number type for calculating automatic derivatives.
+ *
+ * Based roughly on the methods described in:
+ * Automatic Differentiation, C++ Templates and Photogrammetry, by Dan Piponi
+ * and
+ * Ceres Solver's excellent Jet.h
+ *
+ * @author Julian Kent <julian@auterion.com>
+ */
+
+#pragma once
+
+#include "math.hpp"
+
+namespace matrix
+{
+
+template <typename Type, size_t M>
+class Vector;
+
+template <typename Scalar, size_t N>
+struct Dual
+{
+    static constexpr size_t WIDTH = N;
+
+    Scalar value {};
+    Vector<Scalar, N> derivative;
+
+    Dual() = default;
+
+    explicit Dual(Scalar v, size_t inputDimension = 65535)
+    {
+        value = v;
+        if (inputDimension < N) {
+            derivative(inputDimension) = Scalar(1);
+        }
+    }
+
+    explicit Dual(Scalar v, const Vector<Scalar, N>& d) :
+        value(v), derivative(d)
+    {}
+
+    Dual<Scalar, N>& operator=(const Scalar& a)
+    {
+        derivative.setZero();
+        value = a;
+        return *this;
+    }
+
+    Dual<Scalar, N>& operator +=(const Dual<Scalar, N>& a)
+    {
+        return (*this = *this + a);
+    }
+
+    Dual<Scalar, N>& operator *=(const Dual<Scalar, N>& a)
+    {
+        return (*this = *this * a);
+    }
+
+    Dual<Scalar, N>& operator -=(const Dual<Scalar, N>& a)
+    {
+        return (*this = *this - a);
+    }
+
+    Dual<Scalar, N>& operator /=(const Dual<Scalar, N>& a)
+    {
+        return (*this = *this / a);
+    }
+
+    Dual<Scalar, N>& operator +=(Scalar a)
+    {
+        return (*this = *this + a);
+    }
+
+    Dual<Scalar, N>& operator -=(Scalar a)
+    {
+        return (*this = *this - a);
+    }
+
+    Dual<Scalar, N>& operator *=(Scalar a)
+    {
+        return (*this = *this * a);
+    }
+
+    Dual<Scalar, N>& operator /=(Scalar a)
+    {
+        return (*this = *this / a);
+    }
+
+};
+
+// operators
+
+template <typename Scalar, size_t N>
+Dual<Scalar, N> operator+(const Dual<Scalar, N>& a)
+{
+    return a;
+}
+
+template <typename Scalar, size_t N>
+Dual<Scalar, N> operator-(const Dual<Scalar, N>& a)
+{
+    return Dual<Scalar, N>(-a.value, -a.derivative);
+}
+
+template <typename Scalar, size_t N>
+Dual<Scalar, N> operator+(const Dual<Scalar, N>& a, const Dual<Scalar, N>& b)
+{
+    return Dual<Scalar, N>(a.value + b.value, a.derivative + b.derivative);
+}
+
+template <typename Scalar, size_t N>
+Dual<Scalar, N> operator-(const Dual<Scalar, N>& a, const Dual<Scalar, N>& b)
+{
+    return a + (-b);
+}
+
+template <typename Scalar, size_t N>
+Dual<Scalar, N> operator+(const Dual<Scalar, N>& a, Scalar b)
+{
+    return Dual<Scalar, N>(a.value + b, a.derivative);
+}
+
+template <typename Scalar, size_t N>
+Dual<Scalar, N> operator-(const Dual<Scalar, N>& a, Scalar b)
+{
+    return a + (-b);
+}
+
+template <typename Scalar, size_t N>
+Dual<Scalar, N> operator+(Scalar a, const Dual<Scalar, N>& b)
+{
+    return Dual<Scalar, N>(a + b.value, b.derivative);
+}
+
+template <typename Scalar, size_t N>
+Dual<Scalar, N> operator-(Scalar a, const Dual<Scalar, N>& b)
+{
+    return a + (-b);
+}
+
+template <typename Scalar, size_t N>
+Dual<Scalar, N> operator*(const Dual<Scalar, N>& a, const Dual<Scalar, N>& b)
+{
+    return Dual<Scalar, N>(a.value * b.value, a.value * b.derivative + b.value * a.derivative);
+}
+
+template <typename Scalar, size_t N>
+Dual<Scalar, N> operator*(const Dual<Scalar, N>& a, Scalar b)
+{
+    return Dual<Scalar, N>(a.value * b, a.derivative * b);
+}
+
+template <typename Scalar, size_t N>
+Dual<Scalar, N> operator*(Scalar a, const Dual<Scalar, N>& b)
+{
+    return b * a;
+}
+
+template <typename Scalar, size_t N>
+Dual<Scalar, N> operator/(const Dual<Scalar, N>& a, const Dual<Scalar, N>& b)
+{
+    const Scalar inv_b_real = Scalar(1) / b.value;
+    return Dual<Scalar, N>(a.value * inv_b_real, a.derivative * inv_b_real -
+                           a.value * b.derivative * inv_b_real * inv_b_real);
+}
+
+template <typename Scalar, size_t N>
+Dual<Scalar, N> operator/(const Dual<Scalar, N>& a, Scalar b)
+{
+    return a * (Scalar(1) / b);
+}
+
+template <typename Scalar, size_t N>
+Dual<Scalar, N> operator/(Scalar a, const Dual<Scalar, N>& b)
+{
+    const Scalar inv_b_real = Scalar(1) / b.value;
+    return Dual<Scalar, N>(a * inv_b_real, (-inv_b_real * a * inv_b_real) * b.derivative);
+}
+
+// basic math
+
+// sqrt
+template <typename Scalar, size_t N>
+Dual<Scalar, N> sqrt(const Dual<Scalar, N>& a)
+{
+    Scalar real = sqrt(a.value);
+    return Dual<Scalar, N>(real, a.derivative * (Scalar(1) / (Scalar(2) * real)));
+}
+
+// abs
+template <typename Scalar, size_t N>
+Dual<Scalar, N> abs(const Dual<Scalar, N>& a)
+{
+    return a.value >= Scalar(0) ? a : -a;
+}
+
+// ceil
+template <typename Scalar, size_t N>
+Dual<Scalar, N> ceil(const Dual<Scalar, N>& a)
+{
+    return Dual<Scalar, N>(ceil(a.value));
+}
+
+// floor
+template <typename Scalar, size_t N>
+Dual<Scalar, N> floor(const Dual<Scalar, N>& a)
+{
+    return Dual<Scalar, N>(floor(a.value));
+}
+
+// fmod
+template <typename Scalar, size_t N>
+Dual<Scalar, N> fmod(const Dual<Scalar, N>& a, Scalar mod)
+{
+    return Dual<Scalar, N>(a.value - Scalar(size_t(a.value / mod)) * mod, a.derivative);
+}
+
+// max
+template <typename Scalar, size_t N>
+Dual<Scalar, N> max(const Dual<Scalar, N>& a, const Dual<Scalar, N>& b)
+{
+    return a.value >= b.value ? a : b;
+}
+
+// min
+template <typename Scalar, size_t N>
+Dual<Scalar, N> min(const Dual<Scalar, N>& a, const Dual<Scalar, N>& b)
+{
+    return a.value < b.value ? a : b;
+}
+
+// isnan
+template <typename Scalar>
+bool IsNan(Scalar a)
+{
+    return isnan(a);
+}
+
+template <typename Scalar, size_t N>
+bool IsNan(const Dual<Scalar, N>& a)
+{
+    return IsNan(a.value);
+}
+
+// isfinite
+template <typename Scalar>
+bool IsFinite(Scalar a)
+{
+    return isfinite(a);
+}
+
+template <typename Scalar, size_t N>
+bool IsFinite(const Dual<Scalar, N>& a)
+{
+    return IsFinite(a.value);
+}
+
+// isinf
+template <typename Scalar>
+bool IsInf(Scalar a)
+{
+    return isinf(a);
+}
+
+template <typename Scalar, size_t N>
+bool IsInf(const Dual<Scalar, N>& a)
+{
+    return IsInf(a.value);
+}
+
+// trig
+
+// sin
+template <typename Scalar, size_t N>
+Dual<Scalar, N> sin(const Dual<Scalar, N>& a)
+{
+    return Dual<Scalar, N>(sin(a.value), cos(a.value) * a.derivative);
+}
+
+// cos
+template <typename Scalar, size_t N>
+Dual<Scalar, N> cos(const Dual<Scalar, N>& a)
+{
+    return Dual<Scalar, N>(cos(a.value), -sin(a.value) * a.derivative);
+}
+
+// tan
+template <typename Scalar, size_t N>
+Dual<Scalar, N> tan(const Dual<Scalar, N>& a)
+{
+    Scalar real = tan(a.value);
+    return Dual<Scalar, N>(real, (Scalar(1) + real * real) * a.derivative);
+}
+
+// asin
+template <typename Scalar, size_t N>
+Dual<Scalar, N> asin(const Dual<Scalar, N>& a)
+{
+    Scalar asin_d = Scalar(1) / sqrt(Scalar(1) - a.value * a.value);
+    return Dual<Scalar, N>(asin(a.value), asin_d * a.derivative);
+}
+
+// acos
+template <typename Scalar, size_t N>
+Dual<Scalar, N> acos(const Dual<Scalar, N>& a)
+{
+    Scalar acos_d = -Scalar(1) / sqrt(Scalar(1) - a.value * a.value);
+    return Dual<Scalar, N>(acos(a.value), acos_d * a.derivative);
+}
+
+// atan
+template <typename Scalar, size_t N>
+Dual<Scalar, N> atan(const Dual<Scalar, N>& a)
+{
+    Scalar atan_d = Scalar(1) / (Scalar(1) + a.value * a.value);
+    return Dual<Scalar, N>(atan(a.value), atan_d * a.derivative);
+}
+
+// atan2
+template <typename Scalar, size_t N>
+Dual<Scalar, N> atan2(const Dual<Scalar, N>& a, const Dual<Scalar, N>& b)
+{
+    // derivative is equal to that of atan(a/b), so substitute a/b into atan and simplify
+    Scalar atan_d = Scalar(1) / (a.value * a.value + b.value * b.value);
+    return Dual<Scalar, N>(atan2(a.value, b.value), (a.derivative * b.value - a.value * b.derivative) * atan_d);
+}
+
+// retrieve the derivative elements of a vector of Duals into a matrix
+template <typename Scalar, size_t M, size_t N>
+Matrix<Scalar, M, N> collectDerivatives(const Matrix<Dual<Scalar, N>, M, 1>& input)
+{
+    Matrix<Scalar, M, N> jac;
+    for (size_t i = 0; i < M; i++) {
+        jac.row(i) = input(i, 0).derivative;
+    }
+    return jac;
+}
+
+// retrieve the real (non-derivative) elements of a matrix of Duals into an equally sized matrix
+template <typename Scalar, size_t M, size_t N, size_t D>
+Matrix<Scalar, M, N> collectReals(const Matrix<Dual<Scalar, D>, M, N>& input)
+{
+    Matrix<Scalar, M, N> r;
+    for (size_t i = 0; i < M; i++) {
+        for (size_t j = 0; j < N; j++) {
+            r(i,j) = input(i,j).value;
+        }
+    }
+    return r;
+}
+
+#if defined(SUPPORT_STDIOSTREAM)
+template<typename Type, size_t N>
+std::ostream& operator<<(std::ostream& os,
+                         const matrix::Dual<Type, N>& dual)
+{
+    os << "[";
+    os << std::setw(10) << dual.value << ";";
+    for (size_t j = 0; j < N; ++j) {
+        os << "\t";
+        os << std::setw(10) << static_cast<double>(dual.derivative(j));
+    }
+    os << "]";
+    return os;
+}
+#endif // defined(SUPPORT_STDIOSTREAM)
+
+}
+

src/lib/matrix/matrix/Euler.hpp

@@ -0,0 +1,157 @@
+/**
+ * @file Euler.hpp
+ *
+ * All rotations and axis systems follow the right-hand rule
+ *
+ * An instance of this class defines a rotation from coordinate frame 1 to coordinate frame 2.
+ * It follows the convention of a 3-2-1 intrinsic Tait-Bryan rotation sequence.
+ * In order to go from frame 1 to frame 2 we apply the following rotations consecutively.
+ * 1) We rotate about our initial Z axis by an angle of _psi.
+ * 2) We rotate about the newly created Y' axis by an angle of _theta.
+ * 3) We rotate about the newly created X'' axis by an angle of _phi.
+ *
+ * @author James Goppert <james.goppert@gmail.com>
+ */
+
+#pragma once
+
+#include "math.hpp"
+
+namespace matrix
+{
+
+template <typename Type>
+class Dcm;
+
+template <typename Type>
+class Quaternion;
+
+/**
+ * Euler angles class
+ *
+ * This class describes the rotation from frame 1
+ * to frame 2 via 3-2-1 intrinsic Tait-Bryan rotation sequence.
+ */
+template<typename Type>
+class Euler : public Vector<Type, 3>
+{
+public:
+    /**
+     * Standard constructor
+     */
+    Euler() = default;
+
+    /**
+     * Copy constructor
+     *
+     * @param other vector to copy
+     */
+    Euler(const Vector<Type, 3> &other) :
+        Vector<Type, 3>(other)
+    {
+    }
+
+    /**
+     * Constructor from Matrix31
+     *
+     * @param other Matrix31 to copy
+     */
+    Euler(const Matrix<Type, 3, 1> &other) :
+        Vector<Type, 3>(other)
+    {
+    }
+
+    /**
+     * Constructor from euler angles
+     *
+     * Instance is initialized from an 3-2-1 intrinsic Tait-Bryan
+     * rotation sequence representing transformation from frame 1
+     * to frame 2.
+     *
+     * @param phi_ rotation angle about X axis
+     * @param theta_ rotation angle about Y axis
+     * @param psi_ rotation angle about Z axis
+     */
+    Euler(Type phi_, Type theta_, Type psi_) : Vector<Type, 3>()
+    {
+        phi() = phi_;
+        theta() = theta_;
+        psi() = psi_;
+    }
+
+    /**
+     * Constructor from DCM matrix
+     *
+     * Instance is set from Dcm representing transformation from
+     * frame 2 to frame 1.
+     * This instance will hold the angles defining the 3-2-1 intrinsic
+     * Tait-Bryan rotation sequence from frame 1 to frame 2.
+     *
+     * @param dcm Direction cosine matrix
+    */
+    Euler(const Dcm<Type> &dcm)
+    {
+        theta() = asin(-dcm(2, 0));
+
+        if ((fabs(theta() - Type(M_PI / 2))) < Type(1.0e-3)) {
+            phi() = 0;
+            psi() = atan2(dcm(1, 2), dcm(0, 2));
+
+        } else if ((fabs(theta() + Type(M_PI / 2))) < Type(1.0e-3)) {
+            phi() = 0;
+            psi() = atan2(-dcm(1, 2), -dcm(0, 2));
+
+        } else {
+            phi() = atan2(dcm(2, 1), dcm(2, 2));
+            psi() = atan2(dcm(1, 0), dcm(0, 0));
+        }
+    }
+
+    /**
+     * Constructor from quaternion instance.
+     *
+     * Instance is set from a quaternion representing transformation
+     * from frame 2 to frame 1.
+     * This instance will hold the angles defining the 3-2-1 intrinsic
+     * Tait-Bryan rotation sequence from frame 1 to frame 2.
+     *
+     * @param q quaternion
+    */
+    Euler(const Quaternion<Type> &q) : Vector<Type, 3>(Euler(Dcm<Type>(q)))
+    {
+    }
+
+    inline Type phi() const
+    {
+        return (*this)(0);
+    }
+    inline Type theta() const
+    {
+        return (*this)(1);
+    }
+    inline Type psi() const
+    {
+        return (*this)(2);
+    }
+
+    inline Type &phi()
+    {
+        return (*this)(0);
+    }
+    inline Type &theta()
+    {
+        return (*this)(1);
+    }
+    inline Type &psi()
+    {
+        return (*this)(2);
+    }
+
+};
+
+using Eulerf = Euler<float>;
+using Eulerd = Euler<double>;
+
+} // namespace matrix
+
+/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

src/lib/matrix/matrix/LeastSquaresSolver.hpp

@@ -0,0 +1,146 @@
+/**
+ * @file LeastSquaresSolver.hpp
+ *
+ * Least Squares Solver using QR householder decomposition.
+ * It calculates x for Ax = b.
+ * A = Q*R
+ * where R is an upper triangular matrix.
+ *
+ * R*x = Q^T*b
+ * This is efficiently solved for x because of the upper triangular property of R.
+ *
+ * @author Bart Slinger <bartslinger@gmail.com>
+ */
+
+#pragma once
+
+#include "math.hpp"
+
+namespace matrix {
+
+template<typename Type, size_t M, size_t N>
+class LeastSquaresSolver
+{
+public:
+
+    /**
+     * @brief Class calculates QR decomposition which can be used for linear
+     * least squares
+     * @param A Matrix of size MxN
+     *
+     * Initialize the class with a MxN matrix. The constructor starts the
+     * QR decomposition. This class does not check the rank of the matrix.
+     * The user needs to make sure that rank(A) = N and M >= N.
+     */
+    LeastSquaresSolver(const Matrix<Type, M, N> &A)
+    {
+        static_assert(M >= N, "Matrix dimension should be M >= N");
+
+        // Copy contentents of matrix A
+        _A = A;
+
+        for (size_t j = 0; j < N; j++) {
+            Type normx = Type(0);
+            for (size_t i = j; i < M; i++) {
+                normx += _A(i,j) * _A(i,j);
+            }
+            normx = sqrt(normx);
+            Type s = _A(j,j) > 0 ? Type(-1) : Type(1);
+            Type u1 = _A(j,j) - s*normx;
+            // prevent divide by zero
+            // also covers u1. normx is never negative
+            if (normx < Type(1e-8)) {
+                break;
+            }
+            Type w[M] = {};
+            w[0] = Type(1);
+            for (size_t i = j+1; i < M; i++) {
+                w[i-j] = _A(i,j) / u1;
+                _A(i,j) = w[i-j];
+            }
+            _A(j,j) = s*normx;
+            _tau(j) = -s*u1/normx;
+
+            for (size_t k = j+1; k < N; k++) {
+                Type tmp = Type(0);
+                for (size_t i = j; i < M; i++) {
+                    tmp += w[i-j] * _A(i,k);
+                }
+                for (size_t i = j; i < M; i++) {
+                    _A(i,k) -= _tau(j) * w[i-j] * tmp;
+                }
+            }
+
+        }
+    }
+
+    /**
+     * @brief qtb Calculate Q^T * b
+     * @param b
+     * @return Q^T*b
+     *
+     * This function calculates Q^T * b. This is useful for the solver
+     * because R*x = Q^T*b.
+     */
+    Vector<Type, M> qtb(const Vector<Type, M> &b) {
+        Vector<Type, M> qtbv = b;
+
+        for (size_t j = 0; j < N; j++) {
+            Type w[M];
+            w[0] = Type(1);
+            // fill vector w
+            for (size_t i = j+1; i < M; i++) {
+                w[i-j] = _A(i,j);
+            }
+            Type tmp = Type(0);
+            for (size_t i = j; i < M; i++) {
+                tmp += w[i-j] * qtbv(i);
+            }
+
+            for (size_t i = j; i < M; i++) {
+                qtbv(i) -= _tau(j) * w[i-j] * tmp;
+            }
+        }
+        return qtbv;
+    }
+
+    /**
+     * @brief Solve Ax=b for x
+     * @param b
+     * @return Vector x
+     *
+     * Find x in the equation Ax = b.
+     * A is provided in the initializer of the class.
+     */
+    Vector<Type, N> solve(const Vector<Type, M> &b) {
+        Vector<Type, M> qtbv = qtb(b);
+        Vector<Type, N> x;
+
+        // size_t is unsigned and wraps i = 0 - 1 to i > N
+        for (size_t i = N - 1; i < N; i--) {
+            printf("i %d\n", static_cast<int>(i));
+            x(i) = qtbv(i);
+            for (size_t r = i+1; r < N; r++) {
+                x(i) -= _A(i,r) * x(r);
+            }
+            // divide by zero, return vector of zeros
+            if (isEqualF(_A(i,i), Type(0), Type(1e-8))) {
+                for (size_t z = 0; z < N; z++) {
+                    x(z) = Type(0);
+                }
+                break;
+            }
+            x(i) /= _A(i,i);
+        }
+        return x;
+    }
+
+private:
+    Matrix<Type, M, N> _A;
+    Vector<Type, N> _tau;
+
+};
+
+} // namespace matrix
+
+/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

src/lib/matrix/matrix/Matrix.hpp

@@ -0,0 +1,764 @@
+/**
+ * @file Matrix.hpp
+ *
+ * A simple matrix template library.
+ *
+ * @author James Goppert <james.goppert@gmail.com>
+ */
+
+#pragma once
+
+#include <cstdio>
+#include <cstring>
+
+#if defined(SUPPORT_STDIOSTREAM)
+#include <iostream>
+#include <iomanip>
+#endif // defined(SUPPORT_STDIOSTREAM)
+
+#include "math.hpp"
+
+namespace matrix
+{
+
+template <typename Type, size_t M>
+class Vector;
+
+template<typename Type, size_t M, size_t N>
+class Matrix;
+
+template <typename Type, size_t P, size_t Q, size_t M, size_t N>
+class Slice;
+
+template<typename Type, size_t M, size_t N>
+class Matrix
+{
+    Type _data[M][N] {};
+
+public:
+
+    // Constructors
+    Matrix() = default;
+
+    explicit Matrix(const Type data_[M*N])
+    {
+        for (size_t i = 0; i < M; i++) {
+            for (size_t j = 0; j < N; j++) {
+                _data[i][j] = data_[N*i + j];
+            }
+        }
+    }
+
+    explicit Matrix(const Type data_[M][N])
+    {
+        for (size_t i = 0; i < M; i++) {
+            for (size_t j = 0; j < N; j++) {
+                _data[i][j] = data_[i][j];
+            }
+        }
+    }
+
+    Matrix(const Matrix &other)
+    {
+        for (size_t i = 0; i < M; i++) {
+            for (size_t j = 0; j < N; j++) {
+                _data[i][j] = other(i, j);
+            }
+        }
+    }
+
+    template<size_t P, size_t Q>
+    Matrix(const Slice<Type, M, N, P, Q>& in_slice)
+    {
+        Matrix<Type, M, N>& self = *this;
+        for (size_t i = 0; i < M; i++) {
+            for (size_t j = 0; j < N; j++) {
+                self(i, j) = in_slice(i, j);
+            }
+        }
+    }
+
+    /**
+     * Accessors/ Assignment etc.
+     */
+
+
+    inline const Type &operator()(size_t i, size_t j) const
+    {
+        assert(i < M);
+        assert(j < N);
+
+        return _data[i][j];
+    }
+
+    inline Type &operator()(size_t i, size_t j)
+    {
+        assert(i < M);
+        assert(j < N);
+
+        return _data[i][j];
+    }
+
+    Matrix<Type, M, N> & operator=(const Matrix<Type, M, N> &other)
+    {
+        if (this != &other) {
+            Matrix<Type, M, N> &self = *this;
+            for (size_t i = 0; i < M; i++) {
+                for (size_t j = 0; j < N; j++) {
+                    self(i, j) = other(i, j);
+                }
+            }
+        }
+        return (*this);
+    }
+
+    void copyTo(Type dst[M*N]) const
+    {
+        const Matrix<Type, M, N> &self = *this;
+        for (size_t i = 0; i < M; i++) {
+            for (size_t j = 0; j < N; j++) {
+                dst[N*i + j] = self(i, j);
+            }
+        }
+    }
+
+    void copyToColumnMajor(Type dst[M*N]) const
+    {
+        const Matrix<Type, M, N> &self = *this;
+
+        for (size_t i = 0; i < M; i++) {
+            for (size_t j = 0; j < N; j++) {
+                dst[i+(j*M)] = self(i, j);
+            }
+        }
+    }
+
+    /**
+     * Matrix Operations
+     */
+
+    // this might use a lot of programming memory
+    // since it instantiates a class for every
+    // required mult pair, but it provides
+    // compile time size_t checking
+    template<size_t P>
+    Matrix<Type, M, P> operator*(const Matrix<Type, N, P> &other) const
+    {
+        const Matrix<Type, M, N> &self = *this;
+        Matrix<Type, M, P> res{};
+
+        for (size_t i = 0; i < M; i++) {
+            for (size_t k = 0; k < P; k++) {
+                for (size_t j = 0; j < N; j++) {
+                    res(i, k) += self(i, j) * other(j, k);
+                }
+            }
+        }
+
+        return res;
+    }
+
+    Matrix<Type, M, N> emult(const Matrix<Type, M, N> &other) const
+    {
+        Matrix<Type, M, N> res;
+        const Matrix<Type, M, N> &self = *this;
+
+        for (size_t i = 0; i < M; i++) {
+            for (size_t j = 0; j < N; j++) {
+                res(i, j) = self(i, j)*other(i, j);
+            }
+        }
+
+        return res;
+    }
+
+    Matrix<Type, M, N> edivide(const Matrix<Type, M, N> &other) const
+    {
+        Matrix<Type, M, N> res;
+        const Matrix<Type, M, N> &self = *this;
+
+        for (size_t i = 0; i < M; i++) {
+            for (size_t j = 0; j < N; j++) {
+                res(i, j) = self(i, j)/other(i, j);
+            }
+        }
+
+        return res;
+    }
+
+    Matrix<Type, M, N> operator+(const Matrix<Type, M, N> &other) const
+    {
+        Matrix<Type, M, N> res;
+        const Matrix<Type, M, N> &self = *this;
+
+        for (size_t i = 0; i < M; i++) {
+            for (size_t j = 0; j < N; j++) {
+                res(i, j) = self(i, j) + other(i, j);
+            }
+        }
+
+        return res;
+    }
+
+    Matrix<Type, M, N> operator-(const Matrix<Type, M, N> &other) const
+    {
+        Matrix<Type, M, N> res;
+        const Matrix<Type, M, N> &self = *this;
+
+        for (size_t i = 0; i < M; i++) {
+            for (size_t j = 0; j < N; j++) {
+                res(i, j) = self(i, j) - other(i, j);
+            }
+        }
+
+        return res;
+    }
+
+    // unary minus
+    Matrix<Type, M, N> operator-() const
+    {
+        Matrix<Type, M, N> res;
+        const Matrix<Type, M, N> &self = *this;
+
+        for (size_t i = 0; i < M; i++) {
+            for (size_t j = 0; j < N; j++) {
+                res(i, j) = -self(i, j);
+            }
+        }
+
+        return res;
+    }
+
+    void operator+=(const Matrix<Type, M, N> &other)
+    {
+        Matrix<Type, M, N> &self = *this;
+        for (size_t i = 0; i < M; i++) {
+            for (size_t j = 0; j < N; j++) {
+                self(i, j) += other(i, j);
+            }
+        }
+    }
+
+    void operator-=(const Matrix<Type, M, N> &other)
+    {
+        Matrix<Type, M, N> &self = *this;
+        for (size_t i = 0; i < M; i++) {
+            for (size_t j = 0; j < N; j++) {
+                self(i, j) -= other(i, j);
+            }
+        }
+    }
+
+    template<size_t P>
+    void operator*=(const Matrix<Type, N, P> &other)
+    {
+        Matrix<Type, M, N> &self = *this;
+        self = self * other;
+    }
+
+    /**
+     * Scalar Operations
+     */
+
+    Matrix<Type, M, N> operator*(Type scalar) const
+    {
+        Matrix<Type, M, N> res;
+        const Matrix<Type, M, N> &self = *this;
+
+        for (size_t i = 0; i < M; i++) {
+            for (size_t j = 0; j < N; j++) {
+                res(i, j) = self(i, j) * scalar;
+            }
+        }
+
+        return res;
+    }
+
+    inline Matrix<Type, M, N> operator/(Type scalar) const
+    {
+        return (*this)*(1/scalar);
+    }
+
+    Matrix<Type, M, N> operator+(Type scalar) const
+    {
+        Matrix<Type, M, N> res;
+        const Matrix<Type, M, N> &self = *this;
+
+        for (size_t i = 0; i < M; i++) {
+            for (size_t j = 0; j < N; j++) {
+                res(i, j) = self(i, j) + scalar;
+            }
+        }
+
+        return res;
+    }
+
+    inline Matrix<Type, M, N> operator-(Type scalar) const
+    {
+        return (*this) + (-1*scalar);
+    }
+
+    void operator*=(Type scalar)
+    {
+        Matrix<Type, M, N> &self = *this;
+
+        for (size_t i = 0; i < M; i++) {
+            for (size_t j = 0; j < N; j++) {
+                self(i, j) *= scalar;
+            }
+        }
+    }
+
+    void operator/=(Type scalar)
+    {
+        Matrix<Type, M, N> &self = *this;
+        self *= (Type(1) / scalar);
+    }
+
+    inline void operator+=(Type scalar)
+    {
+        Matrix<Type, M, N> &self = *this;
+        for (size_t i = 0; i < M; i++) {
+            for (size_t j = 0; j < N; j++) {
+                self(i, j) += scalar;
+            }
+        }
+    }
+
+    inline void operator-=(Type scalar)
+    {
+        Matrix<Type, M, N> &self = *this;
+        self += (-scalar);
+    }
+
+    bool operator==(const Matrix<Type, M, N> &other) const
+    {
+        return isEqual(*this, other);
+    }
+
+    bool operator!=(const Matrix<Type, M, N> &other) const
+    {
+        const Matrix<Type, M, N> &self = *this;
+        return !(self == other);
+    }
+
+    /**
+     * Misc. Functions
+     */
+
+    void write_string(char * buf, size_t n) const
+    {
+        buf[0] = '\0'; // make an empty string to begin with (we need the '\0' for strlen to work)
+        const Matrix<Type, M, N> &self = *this;
+        for (size_t i = 0; i < M; i++) {
+            for (size_t j = 0; j < N; j++) {
+                snprintf(buf + strlen(buf), n - strlen(buf), "\t%8.8g", double(self(i, j))); // directly append to the string buffer
+            }
+            snprintf(buf + strlen(buf), n - strlen(buf), "\n");
+        }
+    }
+
+    void print() const
+    {
+        // element: tab, point, 8 digits, 4 scientific notation chars; row: newline; string: \0 end
+        static const size_t n = 15*N*M + M + 1;
+        char * buf = new char[n];
+        write_string(buf, n);
+        printf("%s\n", buf);
+        delete[] buf;
+    }
+
+    Matrix<Type, N, M> transpose() const
+    {
+        Matrix<Type, N, M> res;
+        const Matrix<Type, M, N> &self = *this;
+
+        for (size_t i = 0; i < M; i++) {
+            for (size_t j = 0; j < N; j++) {
+                res(j, i) = self(i, j);
+            }
+        }
+
+        return res;
+    }
+
+    // tranpose alias
+    inline Matrix<Type, N, M> T() const
+    {
+        return transpose();
+    }
+
+    template<size_t P, size_t Q>
+    const Slice<Type, P, Q, M, N> slice(size_t x0, size_t y0) const
+    {
+        return Slice<Type, P, Q, M, N>(x0, y0, this);
+    }
+
+    template<size_t P, size_t Q>
+    Slice<Type, P, Q, M, N> slice(size_t x0, size_t y0)
+    {
+        return Slice<Type, P, Q, M, N>(x0, y0, this);
+    }
+
+    const Slice<Type, 1, N, M, N> row(size_t i) const
+    {
+        return slice<1, N>(i,0);
+    }
+
+    Slice<Type, 1, N, M, N> row(size_t i)
+    {
+        return slice<1, N>(i,0);
+    }
+
+    const Slice<Type, M, 1, M, N> col(size_t j) const
+    {
+        return slice<M, 1>(0,j);
+    }
+
+    Slice<Type, M, 1, M, N> col(size_t j)
+    {
+        return slice<M, 1>(0,j);
+    }
+
+    void setRow(size_t i, const Matrix<Type, N, 1> &row_in)
+    {
+        slice<1,N>(i,0) = row_in.transpose();
+    }
+
+    void setRow(size_t i, Type val)
+    {
+        slice<1,N>(i,0) = val;
+    }
+
+    void setCol(size_t j, const Matrix<Type, M, 1> &column)
+    {
+        slice<M,1>(0,j) = column;
+    }
+
+    void setCol(size_t j, Type val)
+    {
+        slice<M,1>(0,j) = val;
+    }
+
+    void setZero()
+    {
+        memset(_data, 0, sizeof(_data));
+    }
+
+    inline void zero()
+    {
+        setZero();
+    }
+
+    void setAll(Type val)
+    {
+        Matrix<Type, M, N> &self = *this;
+
+        for (size_t i = 0; i < M; i++) {
+            for (size_t j = 0; j < N; j++) {
+                self(i, j) = val;
+            }
+        }
+    }
+
+    inline void setOne()
+    {
+        setAll(1);
+    }
+
+    inline void setNaN()
+    {
+        setAll(NAN);
+    }
+
+    void setIdentity()
+    {
+        setZero();
+        Matrix<Type, M, N> &self = *this;
+
+        const size_t min_i = M > N ? N : M;
+        for (size_t i = 0; i < min_i; i++) {
+            self(i, i) = 1;
+        }
+    }
+
+    inline void identity()
+    {
+        setIdentity();
+    }
+
+    inline void swapRows(size_t a, size_t b)
+    {
+        assert(a < M);
+        assert(b < M);
+
+        if (a == b) {
+            return;
+        }
+
+        Matrix<Type, M, N> &self = *this;
+
+        for (size_t j = 0; j < N; j++) {
+            Type tmp = self(a, j);
+            self(a, j) = self(b, j);
+            self(b, j) = tmp;
+        }
+    }
+
+    inline void swapCols(size_t a, size_t b)
+    {
+        assert(a < N);
+        assert(b < N);
+
+        if (a == b) {
+            return;
+        }
+
+        Matrix<Type, M, N> &self = *this;
+
+        for (size_t i = 0; i < M; i++) {
+            Type tmp = self(i, a);
+            self(i, a) = self(i, b);
+            self(i, b) = tmp;
+        }
+    }
+
+    Matrix<Type, M, N> abs() const
+    {
+        Matrix<Type, M, N> r;
+        for (size_t i=0; i<M; i++) {
+            for (size_t j=0; j<N; j++) {
+                r(i,j) = Type(fabs((*this)(i,j)));
+            }
+        }
+        return r;
+    }
+
+    Type max() const
+    {
+        Type max_val = (*this)(0,0);
+        for (size_t i=0; i<M; i++) {
+            for (size_t j=0; j<N; j++) {
+                Type val = (*this)(i,j);
+                if (val > max_val) {
+                    max_val = val;
+                }
+            }
+        }
+        return max_val;
+    }
+
+    Type min() const
+    {
+        Type min_val = (*this)(0,0);
+        for (size_t i=0; i<M; i++) {
+            for (size_t j=0; j<N; j++) {
+                Type val = (*this)(i,j);
+                if (val < min_val) {
+                    min_val = val;
+                }
+            }
+        }
+        return min_val;
+    }
+
+    bool isAllNan() const {
+        const Matrix<float, M, N> &self = *this;
+        bool result = true;
+        for (size_t i = 0; i < M; i++) {
+            for (size_t j = 0; j < N; j++) {
+                result = result && isnan(self(i, j));
+            }
+        }
+        return result;
+    }
+};
+
+template<typename Type, size_t M, size_t N>
+Matrix<Type, M, N> zeros() {
+    Matrix<Type, M, N> m;
+    m.setZero();
+    return m;
+}
+
+template<typename Type, size_t M, size_t N>
+Matrix<Type, M, N> ones() {
+    Matrix<Type, M, N> m;
+    m.setOne();
+    return m;
+}
+
+template<size_t M, size_t N>
+Matrix<float, M, N> nans() {
+    Matrix<float, M, N> m;
+    m.setNaN();
+    return m;
+}
+
+template<typename Type, size_t  M, size_t N>
+Matrix<Type, M, N> operator*(Type scalar, const Matrix<Type, M, N> &other)
+{
+    return other * scalar;
+}
+
+template<typename Type, size_t  M, size_t N>
+bool isEqual(const Matrix<Type, M, N> &x,
+             const Matrix<Type, M, N> &y, const Type eps=Type(1e-4f)) {
+    for (size_t i = 0; i < M; i++) {
+        for (size_t j = 0; j < N; j++) {
+            if (!isEqualF(x(i,j), y(i,j), eps)) {
+                return false;
+            }
+        }
+    }
+    return true;
+}
+
+namespace typeFunction
+{
+template<typename Type>
+Type min(const Type x, const Type y) {
+    bool x_is_nan = isnan(x);
+    bool y_is_nan = isnan(y);
+    // take the non-nan value if there is one
+    if (x_is_nan || y_is_nan) {
+        if (x_is_nan && !y_is_nan) {
+            return y;
+        }
+        // either !x_is_nan && y_is_nan or both are NAN anyways
+        return x;
+    }
+    return (x < y) ? x : y;
+}
+
+template<typename Type>
+Type max(const Type x, const Type y) {
+    bool x_is_nan = isnan(x);
+    bool y_is_nan = isnan(y);
+    // take the non-nan value if there is one
+    if (x_is_nan || y_is_nan) {
+        if (x_is_nan && !y_is_nan) {
+            return y;
+        }
+        // either !x_is_nan && y_is_nan or both are NAN anyways
+        return x;
+    }
+    return (x > y) ? x : y;
+}
+
+template<typename Type>
+Type constrain(const Type x, const Type lower_bound, const Type upper_bound) {
+    if (lower_bound > upper_bound) {
+        return NAN;
+    } else if(isnan(x)) {
+        return NAN;
+    } else {
+        return typeFunction::max(lower_bound, typeFunction::min(upper_bound, x));
+    }
+}
+}
+
+template<typename Type, size_t  M, size_t N>
+Matrix<Type, M, N> min(const Matrix<Type, M, N> &x, const Type scalar_upper_bound) {
+    Matrix<Type,M,N> m;
+    for (size_t i = 0; i < M; i++) {
+        for (size_t j = 0; j < N; j++) {
+            m(i,j) = typeFunction::min(x(i,j),scalar_upper_bound);
+        }
+    }
+    return m;
+}
+
+template<typename Type, size_t  M, size_t N>
+Matrix<Type, M, N> min(const Type scalar_upper_bound, const Matrix<Type, M, N> &x) {
+    return min(x, scalar_upper_bound);
+}
+
+template<typename Type, size_t  M, size_t N>
+Matrix<Type, M, N> min(const Matrix<Type, M, N> &x1, const Matrix<Type, M, N> &x2) {
+    Matrix<Type,M,N> m;
+    for (size_t i = 0; i < M; i++) {
+        for (size_t j = 0; j < N; j++) {
+            m(i,j) = typeFunction::min(x1(i,j),x2(i,j));
+        }
+    }
+    return m;
+}
+
+template<typename Type, size_t  M, size_t N>
+Matrix<Type, M, N> max(const Matrix<Type, M, N> &x, const Type scalar_lower_bound) {
+    Matrix<Type,M,N> m;
+    for (size_t i = 0; i < M; i++) {
+        for (size_t j = 0; j < N; j++) {
+            m(i,j) = typeFunction::max(x(i,j),scalar_lower_bound);
+        }
+    }
+    return m;
+}
+
+template<typename Type, size_t  M, size_t N>
+Matrix<Type, M, N> max(const Type scalar_lower_bound, const Matrix<Type, M, N> &x) {
+    return max(x, scalar_lower_bound);
+}
+
+template<typename Type, size_t  M, size_t N>
+Matrix<Type, M, N> max(const Matrix<Type, M, N> &x1, const Matrix<Type, M, N> &x2) {
+    Matrix<Type,M,N> m;
+    for (size_t i = 0; i < M; i++) {
+        for (size_t j = 0; j < N; j++) {
+            m(i,j) = typeFunction::max(x1(i,j),x2(i,j));
+        }
+    }
+    return m;
+}
+
+template<typename Type, size_t  M, size_t N>
+Matrix<Type, M, N> constrain(const Matrix<Type, M, N> &x,
+                             const Type scalar_lower_bound,
+                             const Type scalar_upper_bound) {
+    Matrix<Type,M,N> m;
+    if (scalar_lower_bound > scalar_upper_bound) {
+        m.setNaN();
+    } else {
+        for (size_t i = 0; i < M; i++) {
+            for (size_t j = 0; j < N; j++) {
+                m(i,j) = typeFunction::constrain(x(i,j), scalar_lower_bound, scalar_upper_bound);
+            }
+        }
+    }
+    return m;
+}
+
+template<typename Type, size_t  M, size_t N>
+Matrix<Type, M, N> constrain(const Matrix<Type, M, N> &x,
+                             const Matrix<Type, M, N> &x_lower_bound,
+                             const Matrix<Type, M, N> &x_upper_bound) {
+    Matrix<Type,M,N> m;
+    for (size_t i = 0; i < M; i++) {
+        for (size_t j = 0; j < N; j++) {
+            m(i,j) = typeFunction::constrain(x(i,j), x_lower_bound(i,j), x_upper_bound(i,j));
+        }
+    }
+    return m;
+}
+
+#if defined(SUPPORT_STDIOSTREAM)
+template<typename Type, size_t  M, size_t N>
+std::ostream& operator<<(std::ostream& os,
+                         const matrix::Matrix<Type, M, N>& matrix)
+{
+    for (size_t i = 0; i < M; ++i) {
+        os << "[";
+        for (size_t j = 0; j < N; ++j) {
+            os << std::setw(10) << matrix(i, j);
+            os << "\t";
+        }
+        os << "]" << std::endl;
+    }
+    return os;
+}
+#endif // defined(SUPPORT_STDIOSTREAM)
+
+} // namespace matrix
+
+/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

src/lib/matrix/matrix/PseudoInverse.hpp

@@ -0,0 +1,119 @@
+/**
+ * @file PseudoInverse.hpp
+ *
+ * Implementation of matrix pseudo inverse
+ *
+ * @author Julien Lecoeur <julien.lecoeur@gmail.com>
+ * @author Julian Kent <julian@auterion.com>
+ */
+
+#pragma once
+
+#include "math.hpp"
+
+namespace matrix
+{
+
+/**
+ * Geninv
+ * Fast pseudoinverse based on full rank cholesky factorisation
+ *
+ * Courrieu, P. (2008). Fast Computation of Moore-Penrose Inverse Matrices, 8(2), 25–29. http://arxiv.org/abs/0804.4809
+ */
+template<typename Type, size_t M, size_t N>
+bool geninv(const Matrix<Type, M, N> & G, Matrix<Type, N, M>& res)
+{
+    size_t rank;
+    if (M <= N) {
+        SquareMatrix<Type, M> A = G * G.transpose();
+        SquareMatrix<Type, M> L = fullRankCholesky(A, rank);
+
+        A = L.transpose() * L;
+        SquareMatrix<Type, M> X;
+        if (!inv(A, X, rank)) {
+            res = Matrix<Type, N, M>();
+            return false; // LCOV_EXCL_LINE -- this can only be hit from numerical issues
+        }
+        // doing an intermediate assignment reduces stack usage
+        A = X * X * L.transpose();
+        res = G.transpose() * (L * A);
+
+    } else {
+        SquareMatrix<Type, N> A = G.transpose() * G;
+        SquareMatrix<Type, N> L = fullRankCholesky(A, rank);
+
+        A = L.transpose() * L;
+        SquareMatrix<Type, N> X;
+        if(!inv(A, X, rank)) {
+            res = Matrix<Type, N, M>();
+            return false; // LCOV_EXCL_LINE -- this can only be hit from numerical issues
+        }
+        // doing an intermediate assignment reduces stack usage
+        A = X * X * L.transpose();
+        res = (L * A) * G.transpose();
+    }
+    return true;
+}
+
+
+template<typename Type>
+Type typeEpsilon();
+
+template<> inline
+float typeEpsilon<float>()
+{
+    return FLT_EPSILON;
+}
+
+/**
+ * Full rank Cholesky factorization of A
+ */
+template<typename Type, size_t N>
+SquareMatrix<Type, N> fullRankCholesky(const SquareMatrix<Type, N> & A,
+                                       size_t& rank)
+{
+    // Loses one ulp accuracy per row of diag, relative to largest magnitude
+    const Type tol = N * typeEpsilon<Type>() * A.diag().max();
+
+    Matrix<Type, N, N> L;
+
+    size_t r = 0;
+    for (size_t k = 0; k < N; k++) {
+
+        if (r == 0) {
+            for (size_t i = k; i < N; i++) {
+                L(i, r) = A(i, k);
+            }
+
+        } else {
+            for (size_t i = k; i < N; i++) {
+                // Compute LL = L[k:n, :r] * L[k, :r].T
+                Type LL = Type();
+                for (size_t j = 0; j < r; j++) {
+                    LL += L(i, j) * L(k, j);
+                }
+                L(i, r) = A(i, k) - LL;
+            }
+        }
+        if (L(k, r) > tol) {
+            L(k, r) = sqrt(L(k, r));
+
+            if (k < N - 1) {
+                for (size_t i = k + 1; i < N; i++) {
+                    L(i, r) = L(i, r) / L(k, r);
+                }
+            }
+
+            r = r + 1;
+        }
+    }
+
+    // Return rank
+    rank = r;
+
+    return L;
+}
+
+} // namespace matrix
+
+/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

src/lib/matrix/matrix/Quaternion.hpp

@@ -0,0 +1,532 @@
+/**
+ * @file Quaternion.hpp
+ *
+ * All rotations and axis systems follow the right-hand rule.
+ * The Hamilton quaternion convention including its product definition is used.
+ *
+ * In order to rotate a vector in frame b (v_b) to frame n by a righthand
+ * rotation defined by the quaternion q_nb (from frame b to n)
+ * one can use the following operation:
+ * v_n = q_nb * [0;v_b] * q_nb^(-1)
+ *
+ * Just like DCM's: v_n = C_nb * v_b (vector rotation)
+ * M_n = C_nb * M_b * C_nb^(-1) (matrix rotation)
+ *
+ * or similarly the reverse operation
+ * v_b = q_nb^(-1) * [0;v_n] * q_nb
+ *
+ * where q_nb^(-1) represents the inverse of the quaternion q_nb^(-1) = q_bn
+ *
+ * The product z of two quaternions z = q2 * q1 represents an intrinsic rotation
+ * in the order of first q1 followed by q2.
+ * The first element of the quaternion
+ * represents the real part, thus, a quaternion representing a zero-rotation
+ * is defined as (1,0,0,0).
+ *
+ * @author James Goppert <james.goppert@gmail.com>
+ */
+
+#pragma once
+
+#include "math.hpp"
+
+namespace matrix
+{
+
+template <typename Type>
+class Dcm;
+
+template <typename Type>
+class Euler;
+
+template <typename Type>
+class AxisAngle;
+
+
+/**
+ * Quaternion class
+ *
+ * The rotation between two coordinate frames is
+ * described by this class.
+ */
+template<typename Type>
+class Quaternion : public Vector<Type, 4>
+{
+public:
+    using Matrix41 = Matrix<Type, 4, 1>;
+    using Matrix31 = Matrix<Type, 3, 1>;
+
+    /**
+     * Constructor from array
+     *
+     * @param data_ array
+     */
+    explicit Quaternion(const Type data_[4]) :
+        Vector<Type, 4>(data_)
+    {
+    }
+
+    /**
+     * Standard constructor
+     */
+    Quaternion()
+    {
+        Quaternion &q = *this;
+        q(0) = 1;
+        q(1) = 0;
+        q(2) = 0;
+        q(3) = 0;
+    }
+
+    /**
+     * Constructor from Matrix41
+     *
+     * @param other Matrix41 to copy
+     */
+    Quaternion(const Matrix41 &other) :
+        Vector<Type, 4>(other)
+    {
+    }
+
+    /**
+     * Constructor from dcm
+     *
+     * Instance is initialized from a dcm representing coordinate transformation
+     * from frame 2 to frame 1.
+     *
+     * @param dcm dcm to set quaternion to
+     */
+    Quaternion(const Dcm<Type> &R)
+    {
+        Quaternion &q = *this;
+        Type t = R.trace();
+        if (t > Type(0)) {
+            t = sqrt(Type(1) + t);
+            q(0) = Type(0.5) * t;
+            t = Type(0.5) / t;
+            q(1) = (R(2,1) - R(1,2)) * t;
+            q(2) = (R(0,2) - R(2,0)) * t;
+            q(3) = (R(1,0) - R(0,1)) * t;
+        } else if (R(0,0) > R(1,1) && R(0,0) > R(2,2)) {
+            t = sqrt(Type(1) + R(0,0) - R(1,1) - R(2,2));
+            q(1) = Type(0.5) * t;
+            t = Type(0.5) / t;
+            q(0) = (R(2,1) - R(1,2)) * t;
+            q(2) = (R(1,0) + R(0,1)) * t;
+            q(3) = (R(0,2) + R(2,0)) * t;
+        } else if (R(1,1) > R(2,2)) {
+            t = sqrt(Type(1) - R(0,0) + R(1,1) - R(2,2));
+            q(2) = Type(0.5) * t;
+            t = Type(0.5) / t;
+            q(0) = (R(0,2) - R(2,0)) * t;
+            q(1) = (R(1,0) + R(0,1)) * t;
+            q(3) = (R(2,1) + R(1,2)) * t;
+        } else {
+            t = sqrt(Type(1) - R(0,0) - R(1,1) + R(2,2));
+            q(3) = Type(0.5) * t;
+            t = Type(0.5) / t;
+            q(0) = (R(1,0) - R(0,1)) * t;
+            q(1) = (R(0,2) + R(2,0)) * t;
+            q(2) = (R(2,1) + R(1,2)) * t;
+        }
+    }
+
+    /**
+     * Constructor from euler angles
+     *
+     * This sets the instance to a quaternion representing coordinate transformation from
+     * frame 2 to frame 1 where the rotation from frame 1 to frame 2 is described
+     * by a 3-2-1 intrinsic Tait-Bryan rotation sequence.
+     *
+     * @param euler euler angle instance
+     */
+    Quaternion(const Euler<Type> &euler)
+    {
+        Quaternion &q = *this;
+        Type cosPhi_2 = Type(cos(euler.phi() / Type(2)));
+        Type cosTheta_2 = Type(cos(euler.theta() / Type(2)));
+        Type cosPsi_2 = Type(cos(euler.psi() / Type(2)));
+        Type sinPhi_2 = Type(sin(euler.phi() / Type(2)));
+        Type sinTheta_2 = Type(sin(euler.theta() / Type(2)));
+        Type sinPsi_2 = Type(sin(euler.psi() / Type(2)));
+        q(0) = cosPhi_2 * cosTheta_2 * cosPsi_2 +
+               sinPhi_2 * sinTheta_2 * sinPsi_2;
+        q(1) = sinPhi_2 * cosTheta_2 * cosPsi_2 -
+               cosPhi_2 * sinTheta_2 * sinPsi_2;
+        q(2) = cosPhi_2 * sinTheta_2 * cosPsi_2 +
+               sinPhi_2 * cosTheta_2 * sinPsi_2;
+        q(3) = cosPhi_2 * cosTheta_2 * sinPsi_2 -
+               sinPhi_2 * sinTheta_2 * cosPsi_2;
+    }
+
+    /**
+     * Quaternion from AxisAngle
+     *
+     * @param aa axis-angle vector
+     */
+    Quaternion(const AxisAngle<Type> &aa)
+    {
+        Quaternion &q = *this;
+        Type angle = aa.norm();
+        Vector<Type, 3> axis = aa.unit();
+        if (angle < Type(1e-10)) {
+            q(0) = Type(1);
+            q(1) = q(2) = q(3) = 0;
+        } else {
+            Type magnitude = sin(angle / Type(2));
+            q(0) = cos(angle / Type(2));
+            q(1) = axis(0) * magnitude;
+            q(2) = axis(1) * magnitude;
+            q(3) = axis(2) * magnitude;
+        }
+    }
+
+    /**
+     * Quaternion from two vectors
+     * Generates shortest rotation from source to destination vector
+     *
+     * @param dst destination vector (no need to normalize)
+     * @param src source vector (no need to normalize)
+     * @param eps epsilon threshold which decides if a value is considered zero
+     */
+    Quaternion(const Vector3<Type> &src, const Vector3<Type> &dst, const Type eps = Type(1e-5))
+    {
+        Quaternion &q = *this;
+        Vector3<Type> cr = src.cross(dst);
+        const float dt = src.dot(dst);
+        if (cr.norm() < eps && dt < 0) {
+            // handle corner cases with 180 degree rotations
+            // if the two vectors are parallel, cross product is zero
+            // if they point opposite, the dot product is negative
+            cr = src.abs();
+            if (cr(0) < cr(1)) {
+                if (cr(0) < cr(2)) {
+                    cr = Vector3<Type>(1, 0, 0);
+                } else {
+                    cr = Vector3<Type>(0, 0, 1);
+                }
+            } else {
+                if (cr(1) < cr(2)) {
+                    cr = Vector3<Type>(0, 1, 0);
+                } else {
+                    cr = Vector3<Type>(0, 0, 1);
+                }
+            }
+            q(0) = Type(0);
+            cr = src.cross(cr);
+        } else {
+            // normal case, do half-way quaternion solution
+            q(0) = dt + sqrt(src.norm_squared() * dst.norm_squared());
+        }
+        q(1) = cr(0);
+        q(2) = cr(1);
+        q(3) = cr(2);
+        q.normalize();
+    }
+
+    /**
+     * Constructor from quaternion values
+     *
+     * Instance is initialized from quaternion values representing coordinate
+     * transformation from frame 2 to frame 1.
+     * A zero-rotation quaternion is represented by (1,0,0,0).
+     *
+     * @param a set quaternion value 0
+     * @param b set quaternion value 1
+     * @param c set quaternion value 2
+     * @param d set quaternion value 3
+     */
+    Quaternion(Type a, Type b, Type c, Type d)
+    {
+        Quaternion &q = *this;
+        q(0) = a;
+        q(1) = b;
+        q(2) = c;
+        q(3) = d;
+    }
+
+    /**
+     * Quaternion multiplication operator
+     *
+     * @param q quaternion to multiply with
+     * @return product
+     */
+    Quaternion operator*(const Quaternion &p) const
+    {
+        const Quaternion &q = *this;
+        return {
+            q(0) * p(0) - q(1) * p(1) - q(2) * p(2) - q(3) * p(3),
+            q(1) * p(0) + q(0) * p(1) - q(3) * p(2) + q(2) * p(3),
+            q(2) * p(0) + q(3) * p(1) + q(0) * p(2) - q(1) * p(3),
+            q(3) * p(0) - q(2) * p(1) + q(1) * p(2) + q(0) * p(3) };
+    }
+
+    /**
+     * Self-multiplication operator
+     *
+     * @param other quaternion to multiply with
+     */
+    void operator*=(const Quaternion &other)
+    {
+        Quaternion &self = *this;
+        self = self * other;
+    }
+
+    /**
+     * Scalar multiplication operator
+     *
+     * @param scalar scalar to multiply with
+     * @return product
+     */
+    Quaternion operator*(Type scalar) const
+    {
+        const Quaternion &q = *this;
+        return scalar * q;
+    }
+
+    /**
+     * Scalar self-multiplication operator
+     *
+     * @param scalar scalar to multiply with
+     */
+    void operator*=(Type scalar)
+    {
+        Quaternion &q = *this;
+        q = q * scalar;
+    }
+
+    /**
+     * Computes the derivative of q_21 when
+     * rotated with angular velocity expressed in frame 1
+     * v_2 = q_21 * v_1 * q_21^-1
+     * d/dt q_21 = 0.5 * q_21 * omega_2
+     *
+     * @param w angular rate in frame 1 (typically body frame)
+     */
+    Matrix41 derivative1(const Matrix31 &w) const
+    {
+        const Quaternion &q = *this;
+        Quaternion<Type> v(0, w(0, 0), w(1, 0), w(2, 0));
+        return q * v  * Type(0.5);
+    }
+
+    /**
+     * Computes the derivative of q_21 when
+     * rotated with angular velocity expressed in frame 2
+     * v_2 = q_21 * v_1 * q_21^-1
+     * d/dt q_21 = 0.5 * omega_1 * q_21
+     *
+     * @param w angular rate in frame 2 (typically reference frame)
+     */
+    Matrix41 derivative2(const Matrix31 &w) const
+    {
+        const Quaternion &q = *this;
+        Quaternion<Type> v(0, w(0, 0), w(1, 0), w(2, 0));
+        return v * q  * Type(0.5);
+    }
+
+    /**
+      * Computes the quaternion exponential of the 3D vector u
+      * as proposed in
+      * [1] Sveier A, Sjøberg AM, Egeland O. "Applied Runge–Kutta–Munthe-Kaas Integration
+      *     for the Quaternion Kinematics".Journal of Guidance, Control, and Dynamics. 2019
+      *
+      * return a quaternion computed as
+      * expq(u)=[cos||u||, sinc||u||*u]
+      * sinc(x)=sin(x)/x in the sin cardinal function
+      *
+      * This can be used to update a quaternion from the body rates
+      * rather than using
+      * qk+1=qk+qk.derivative1(wb)*dt
+      * we can use
+      * qk+1=qk*expq(dt*wb/2)
+      * which is a more robust update.
+      * A re-normalization step might necessary with both methods.
+      *
+      * @param u 3D vector u
+      */
+    static Quaternion expq(const Vector3<Type> &u)
+    {
+        const Type tol = Type(0.2);           // ensures an error < 10^-10
+        const Type c2 = Type(1.0 / 2.0);      // 1 / 2!
+        const Type c3 = Type(1.0 / 6.0);      // 1 / 3!
+        const Type c4 = Type(1.0 / 24.0);     // 1 / 4!
+        const Type c5 = Type(1.0 / 120.0);    // 1 / 5!
+        const Type c6 = Type(1.0 / 720.0);    // 1 / 6!
+        const Type c7 = Type(1.0 / 5040.0);   // 1 / 7!
+
+        Type u_norm = u.norm();
+        Type sinc_u, cos_u;
+
+        if (u_norm < tol) {
+            Type u2 = u_norm * u_norm;
+            Type u4 = u2 * u2;
+            Type u6 = u4 * u2;
+
+            // compute the first 4 terms of the Taylor serie
+            sinc_u = Type(1.0) - u2 * c3 + u4 * c5 - u6 * c7;
+            cos_u = Type(1.0) - u2 * c2 + u4 * c4 - u6 * c6;
+        } else {
+            sinc_u = Type(sin(u_norm) / u_norm);
+            cos_u = Type(cos(u_norm));
+        }
+        Vector<Type, 3> v = sinc_u * u;
+        return Quaternion<Type> (cos_u, v(0), v(1), v(2));
+    }
+
+    /** inverse right Jacobian of the quaternion logarithm u
+      * equation (20) in reference
+      * [1] Sveier A, Sjøberg AM, Egeland O. "Applied Runge–Kutta–Munthe-Kaas Integration
+      *     for the Quaternion Kinematics".Journal of Guidance, Control, and Dynamics. 2019
+      *
+      * This can be used to update a quaternion kinematic cleanly
+      * with higher order integration methods (like RK4) on the quaternion logarithm u.
+      *
+      * @param u 3D vector u
+      */
+    static Dcm<Type> inv_r_jacobian (const Vector3<Type> &u)
+    {
+        const Type tol = Type(1.0e-4);
+        Type u_norm = u.norm();
+        Dcm<Type> u_hat = u.hat();
+
+        if (u_norm < tol) { 	// result smaller than O(||.||^3)
+            return Type(0.5) * (Dcm<Type>() + u_hat + (Type(1.0 / 3.0) + u_norm * u_norm / Type(45.0)) * u_hat * u_hat);
+        } else {
+            return Type(0.5) * (Dcm<Type>() + u_hat + (Type(1.0) - u_norm * Type(cos(u_norm) / sin(u_norm))) / (u_norm * u_norm) * u_hat * u_hat);
+        }
+    }
+
+    /**
+     * Invert quaternion in place
+     */
+    void invert()
+    {
+        *this = this->inversed();
+    }
+
+    /**
+     * Invert quaternion
+     *
+     * @return inverted quaternion
+     */
+    Quaternion inversed() const
+    {
+        const Quaternion &q = *this;
+        Type normSq = q.dot(q);
+        return Quaternion(
+                   q(0)/normSq,
+                   -q(1)/normSq,
+                   -q(2)/normSq,
+                   -q(3)/normSq);
+    }
+
+    /**
+     * Bring quaternion to canonical form
+     */
+    void canonicalize()
+    {
+        *this = this->canonical();
+    }
+
+    /**
+     * Return canonical form of the quaternion
+     *
+     * @return quaternion in canonical from
+     */
+    Quaternion canonical() const
+    {
+        const Quaternion &q = *this;
+
+        for (size_t i = 0; i < 4; i++) {
+            if (fabs(q(i)) > FLT_EPSILON) {
+                return q * Type(matrix::sign(q(i)));
+            }
+        }
+        return q;
+    }
+
+    /**
+     * Rotate quaternion from rotation vector
+     *
+     * @param vec rotation vector
+     */
+    void rotate(const AxisAngle<Type> &vec)
+    {
+        Quaternion res(vec);
+        (*this) = res * (*this);
+    }
+
+    /**
+     * Rotates vector v_1 in frame 1 to vector v_2 in frame 2
+     * using the rotation quaternion q_21
+     * describing the rotation from frame 1 to 2
+     * v_2 = q_21 * v_1 * q_21^-1
+     *
+     * @param vec vector to rotate in frame 1 (typically body frame)
+     * @return rotated vector in frame 2 (typically reference frame)
+     */
+    Vector3<Type> conjugate(const Vector3<Type> &vec) const {
+        const Quaternion& q = *this;
+        Quaternion v(Type(0), vec(0), vec(1), vec(2));
+        Quaternion res = q*v*q.inversed();
+        return Vector3<Type>(res(1), res(2), res(3));
+    }
+
+    /**
+     * Rotates vector v_2 in frame 2 to vector v_1 in frame 1
+     * using the rotation quaternion q_21
+     * describing the rotation from frame 1 to 2
+     * v_1 = q_21^-1 * v_2 * q_21
+     *
+     * @param vec vector to rotate in frame 2 (typically reference frame)
+     * @return rotated vector in frame 1 (typically body frame)
+     */
+    Vector3<Type> conjugate_inversed(const Vector3<Type> &vec) const
+    {
+        const Quaternion& q = *this;
+        Quaternion v(Type(0), vec(0), vec(1), vec(2));
+        Quaternion res = q.inversed()*v*q;
+        return Vector3<Type>(res(1), res(2), res(3));
+    }
+
+    /**
+     * Imaginary components of quaternion
+     */
+    Vector3<Type> imag() const
+    {
+        const Quaternion &q = *this;
+        return Vector3<Type>(q(1), q(2), q(3));
+    }
+
+    /**
+     * Corresponding body z-axis to an attitude quaternion /
+     * last orthogonal unit basis vector
+     *
+     * == last column of the equivalent rotation matrix
+     * but calculated more efficiently than a full conversion
+     */
+    Vector3<Type> dcm_z() const
+    {
+        const Quaternion &q = *this;
+        Vector3<Type> R_z;
+        const Type a = q(0);
+        const Type b = q(1);
+        const Type c = q(2);
+        const Type d = q(3);
+        R_z(0) = 2 * (a * c + b * d);
+        R_z(1) = 2 * (c * d - a * b);
+        R_z(2) = a * a - b * b - c * c + d * d;
+        return R_z;
+    }
+};
+
+using Quatf = Quaternion<float>;
+using Quaternionf = Quaternion<float>;
+
+using Quatd = Quaternion<double>;
+using Quaterniond = Quaternion<double>;
+
+} // namespace matrix
+
+/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

src/lib/matrix/matrix/Scalar.hpp

@@ -0,0 +1,58 @@
+/**
+ * @file Scalar.hpp
+ *
+ * Defines conversion of matrix to scalar.
+ *
+ * @author James Goppert <james.goppert@gmail.com>
+ */
+
+#pragma once
+
+#include "math.hpp"
+
+namespace matrix
+{
+
+template<typename Type>
+class Scalar
+{
+public:
+    Scalar() = delete;
+
+    Scalar(const Matrix<Type, 1, 1> & other) :
+        _value{other(0,0)}
+    {
+    }
+
+    Scalar(Type other) : _value(other)
+    {
+    }
+
+    operator const Type &()
+    {
+        return _value;
+    }
+
+    operator Matrix<Type, 1, 1>() const {
+        Matrix<Type, 1, 1> m;
+        m(0, 0) = _value;
+        return m;
+    }
+
+    operator Vector<Type, 1>() const {
+        Vector<Type, 1> m;
+        m(0) = _value;
+        return m;
+    }
+
+private:
+    const Type _value;
+
+};
+
+using Scalarf = Scalar<float>;
+using Scalard = Scalar<double>;
+
+} // namespace matrix
+
+/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

src/lib/matrix/matrix/Slice.hpp

@@ -0,0 +1,304 @@
+/**
+ * @file Slice.hpp
+ *
+ * A simple matrix template library.
+ *
+ * @author Julian Kent < julian@auterion.com >
+ */
+
+#pragma once
+
+#include "math.hpp"
+
+
+namespace matrix {
+
+template<typename Type, size_t M, size_t N>
+class Matrix;
+
+template<typename Type, size_t M>
+class Vector;
+
+template <typename Type, size_t P, size_t Q, size_t M, size_t N>
+class Slice {
+public:
+    Slice(size_t x0, size_t y0, const Matrix<Type, M, N>* data) :
+        _x0(x0),
+        _y0(y0),
+        _data(const_cast<Matrix<Type, M, N>*>(data)) {
+        static_assert(P <= M, "Slice rows bigger than backing matrix");
+        static_assert(Q <= N, "Slice cols bigger than backing matrix");
+        assert(x0 + P <= M);
+        assert(y0 + Q <= N);
+    }
+
+    const Type &operator()(size_t i, size_t j) const
+    {
+        assert(i < P);
+        assert(j < Q);
+
+        return (*_data)(_x0 + i, _y0 + j);
+    }
+
+    Type &operator()(size_t i, size_t j)
+
+    {
+        assert(i < P);
+        assert(j < Q);
+
+        return (*_data)(_x0 + i, _y0 + j);
+    }
+
+    template<size_t MM, size_t NN>
+    Slice<Type, P, Q, M, N>& operator=(const Slice<Type, P, Q, MM, NN>& other)
+    {
+        Slice<Type, P, Q, M, N>& self = *this;
+        for (size_t i = 0; i < P; i++) {
+            for (size_t j = 0; j < Q; j++) {
+                self(i, j) = other(i, j);
+            }
+        }
+        return self;
+    }
+
+    Slice<Type, P, Q, M, N>& operator=(const Matrix<Type, P, Q>& other)
+    {
+        Slice<Type, P, Q, M, N>& self = *this;
+        for (size_t i = 0; i < P; i++) {
+            for (size_t j = 0; j < Q; j++) {
+                self(i, j) = other(i, j);
+            }
+        }
+        return self;
+    }
+
+    Slice<Type, P, Q, M, N>& operator=(const Type& other)
+    {
+        Slice<Type, P, Q, M, N>& self = *this;
+        for (size_t i = 0; i < P; i++) {
+            for (size_t j = 0; j < Q; j++) {
+                self(i, j) = other;
+            }
+        }
+        return self;
+    }
+
+    // allow assigning vectors to a slice that are in the axis
+    template <size_t DUMMY = 1> // make this a template function since it only exists for some instantiations
+    Slice<Type, 1, Q, M, N>& operator=(const Vector<Type, Q>& other)
+    {
+        Slice<Type, 1, Q, M, N>& self = *this;
+        for (size_t j = 0; j < Q; j++) {
+            self(0, j) = other(j);
+        }
+        return self;
+    }
+
+    template<size_t MM, size_t NN>
+    Slice<Type, P, Q, M, N>& operator+=(const Slice<Type, P, Q, MM, NN>& other)
+    {
+        Slice<Type, P, Q, M, N>& self = *this;
+        for (size_t i = 0; i < P; i++) {
+            for (size_t j = 0; j < Q; j++) {
+                self(i, j) += other(i, j);
+            }
+        }
+        return self;
+    }
+
+    Slice<Type, P, Q, M, N>& operator+=(const Matrix<Type, P, Q>& other)
+    {
+        Slice<Type, P, Q, M, N>& self = *this;
+        for (size_t i = 0; i < P; i++) {
+            for (size_t j = 0; j < Q; j++) {
+                self(i, j) += other(i, j);
+            }
+        }
+        return self;
+    }
+
+    Slice<Type, P, Q, M, N>& operator+=(const Type& other)
+    {
+        Slice<Type, P, Q, M, N>& self = *this;
+        for (size_t i = 0; i < P; i++) {
+            for (size_t j = 0; j < Q; j++) {
+                self(i, j) += other;
+            }
+        }
+        return self;
+    }
+
+    template<size_t MM, size_t NN>
+    Slice<Type, P, Q, M, N>& operator-=(const Slice<Type, P, Q, MM, NN>& other)
+    {
+        Slice<Type, P, Q, M, N>& self = *this;
+        for (size_t i = 0; i < P; i++) {
+            for (size_t j = 0; j < Q; j++) {
+                self(i, j) -= other(i, j);
+            }
+        }
+        return self;
+    }
+
+    Slice<Type, P, Q, M, N>& operator-=(const Matrix<Type, P, Q>& other)
+    {
+        Slice<Type, P, Q, M, N>& self = *this;
+        for (size_t i = 0; i < P; i++) {
+            for (size_t j = 0; j < Q; j++) {
+                self(i, j) -= other(i, j);
+            }
+        }
+        return self;
+    }
+
+    Slice<Type, P, Q, M, N>& operator-=(const Type& other)
+    {
+        Slice<Type, P, Q, M, N>& self = *this;
+        for (size_t i = 0; i < P; i++) {
+            for (size_t j = 0; j < Q; j++) {
+                self(i, j) -= other;
+            }
+        }
+        return self;
+    }
+
+    Slice<Type, P, Q, M, N>& operator*=(const Type& other)
+    {
+        Slice<Type, P, Q, M, N>& self = *this;
+        for (size_t i = 0; i < P; i++) {
+            for (size_t j = 0; j < Q; j++) {
+                self(i, j) *= other;
+            }
+        }
+        return self;
+    }
+
+    Slice<Type, P, Q, M, N>& operator/=(const Type& other)
+    {
+        return operator*=(Type(1) / other);
+    }
+
+    Matrix<Type, P, Q> operator*(const Type& other) const
+    {
+        const Slice<Type, P, Q, M, N>& self = *this;
+        Matrix<Type, P, Q> res;
+        for (size_t i = 0; i < P; i++) {
+            for (size_t j = 0; j < Q; j++) {
+                res(i, j) = self(i, j) * other;
+            }
+        }
+        return res;
+    }
+
+    Matrix<Type, P, Q> operator/(const Type& other) const
+    {
+        const Slice<Type, P, Q, M, N>& self = *this;
+        return self * (Type(1) / other);
+    }
+
+    template<size_t R, size_t S>
+    const Slice<Type, R, S, M, N> slice(size_t x0, size_t y0) const
+    {
+        return Slice<Type, R, S, M, N>(x0 + _x0, y0 + _y0, _data);
+    }
+
+    template<size_t R, size_t S>
+    Slice<Type, R, S, M, N> slice(size_t x0, size_t y0)
+    {
+        return Slice<Type, R, S, M, N>(x0 + _x0, y0 + _y0, _data);
+    }
+
+    void copyTo(Type dst[P*Q]) const
+    {
+        const Slice<Type, P, Q, M, N> &self = *this;
+
+        for (size_t i = 0; i < P; i++) {
+            for (size_t j = 0; j < Q; j++) {
+                dst[i*N+j] = self(i, j);
+            }
+        }
+    }
+
+    void copyToColumnMajor(Type dst[P*Q]) const
+    {
+        const Slice<Type, P, Q, M, N> &self = *this;
+
+        for (size_t i = 0; i < P; i++) {
+            for (size_t j = 0; j < Q; j++) {
+                dst[i+(j*M)] = self(i, j);
+            }
+        }
+    }
+
+    Vector<Type, P<Q?P:Q> diag() const
+    {
+        const Slice<Type, P, Q, M, N>& self = *this;
+        Vector<Type,P<Q?P:Q> res;
+        for (size_t j = 0; j < (P<Q?P:Q); j++) {
+            res(j) = self(j,j);
+        }
+        return res;
+    }
+
+    Type norm_squared() const
+    {
+        const Slice<Type, P, Q, M, N>& self = *this;
+        Type accum(0);
+        for (size_t i = 0; i < P; i++) {
+            for (size_t j = 0; j < Q; j++) {
+                accum += self(i, j)*self(i, j);
+            }
+        }
+        return accum;
+    }
+
+    Type norm() const
+    {
+        return matrix::sqrt(norm_squared());
+    }
+
+    bool longerThan(Type testVal) const
+    {
+        return norm_squared() > testVal*testVal;
+    }
+
+    Type max() const
+    {
+        Type max_val = (*this)(0,0);
+
+        for (size_t i = 0; i < P; i++) {
+            for (size_t j = 0; j < Q; j++) {
+                Type val = (*this)(i,j);
+
+                if (val > max_val) {
+                    max_val = val;
+                }
+            }
+        }
+
+        return max_val;
+    }
+
+    Type min() const
+    {
+        Type min_val = (*this)(0,0);
+
+        for (size_t i = 0; i < P; i++) {
+            for (size_t j = 0; j < Q; j++) {
+                Type val = (*this)(i,j);
+
+                if (val < min_val) {
+                    min_val = val;
+                }
+            }
+        }
+
+        return min_val;
+    }
+
+private:
+    size_t _x0, _y0;
+    Matrix<Type,M,N>* _data;
+};
+
+}

src/lib/matrix/matrix/SparseVector.hpp

@@ -0,0 +1,189 @@
+/**
+ * @file SparseVector.hpp
+ *
+ * SparseVector class.
+ *
+ * @author Kamil Ritz <kritz@ethz.ch>
+ * @author Julian Kent <julian@auterion.com>
+ *
+ */
+
+#pragma once
+
+#include "math.hpp"
+
+namespace matrix {
+template<int N> struct force_constexpr_eval {
+    static const int value = N;
+};
+
+// Vector that only store nonzero elements,
+// which indices are specified as parameter pack
+template<typename Type, size_t M, size_t... Idxs>
+class SparseVector {
+private:
+    static constexpr size_t N = sizeof...(Idxs);
+    static constexpr size_t _indices[N] {Idxs...};
+
+    static constexpr bool duplicateIndices() {
+        for (size_t i = 0; i < N; i++) {
+            for (size_t j = 0; j < i; j++) {
+                if (_indices[i] == _indices[j]) {
+                    return true;
+                }
+            }
+        }
+        return false;
+    }
+    static constexpr size_t findMaxIndex() {
+        size_t maxIndex = 0;
+        for (size_t i = 0; i < N; i++) {
+            if (maxIndex < _indices[i]) {
+                maxIndex = _indices[i];
+            }
+        }
+        return maxIndex;
+    }
+
+    static_assert(!duplicateIndices(), "Duplicate indices");
+    static_assert(N < M, "More entries than elements, use a dense vector");
+    static_assert(N > 0, "A sparse vector needs at least one element");
+    static_assert(findMaxIndex() < M, "Largest entry doesn't fit in sparse vector");
+
+    Type _data[N] {};
+
+    static constexpr int findCompressedIndex(size_t index) {
+        int compressedIndex = -1;
+        for (size_t i = 0; i < N; i++) {
+            if (index == _indices[i]) {
+                compressedIndex = static_cast<int>(i);
+            }
+        }
+        return compressedIndex;
+    }
+
+public:
+    constexpr size_t non_zeros() const {
+        return N;
+    }
+
+    constexpr size_t index(size_t i) const {
+        return SparseVector::_indices[i];
+    }
+
+    SparseVector() = default;
+
+    SparseVector(const matrix::Vector<Type, M>& data) {
+        for (size_t i = 0; i < N; i++) {
+            _data[i] = data(_indices[i]);
+        }
+    }
+
+    explicit SparseVector(const Type data[N]) {
+        memcpy(_data, data, sizeof(_data));
+    }
+
+    template <size_t i>
+    inline Type at() const {
+        static constexpr int compressed_index = force_constexpr_eval<findCompressedIndex(i)>::value;
+        static_assert(compressed_index >= 0, "cannot access unpopulated indices");
+        return _data[compressed_index];
+    }
+
+    template <size_t i>
+    inline Type& at() {
+        static constexpr int compressed_index = force_constexpr_eval<findCompressedIndex(i)>::value;
+        static_assert(compressed_index >= 0, "cannot access unpopulated indices");
+        return _data[compressed_index];
+    }
+
+    inline Type atCompressedIndex(size_t i) const {
+        assert(i < N);
+        return _data[i];
+    }
+
+    inline Type& atCompressedIndex(size_t i) {
+        assert(i < N);
+        return _data[i];
+    }
+
+    void setZero() {
+        for (size_t i = 0; i < N; i++) {
+            _data[i] = Type(0);
+        }
+    }
+
+    Type dot(const matrix::Vector<Type, M>& other) const {
+        Type accum (0);
+        for (size_t i = 0; i < N; i++) {
+            accum += _data[i] * other(_indices[i]);
+        }
+        return accum;
+    }
+
+    matrix::Vector<Type, M> operator+(const matrix::Vector<Type, M>& other) const {
+        matrix::Vector<Type, M> vec = other;
+        for (size_t i = 0; i < N; i++) {
+            vec(_indices[i]) +=  _data[i];
+        }
+        return vec;
+    }
+
+    SparseVector& operator+=(Type t) {
+        for (size_t i = 0; i < N; i++) {
+            _data[i] += t;
+        }
+        return *this;
+    }
+
+    Type norm_squared() const
+    {
+        Type accum(0);
+        for (size_t i = 0; i < N; i++) {
+            accum += _data[i] * _data[i];
+        }
+        return accum;
+    }
+
+    Type norm() const
+    {
+        return matrix::sqrt(norm_squared());
+    }
+
+    bool longerThan(Type testVal) const
+    {
+        return norm_squared() > testVal*testVal;
+    }
+};
+
+template<typename Type, size_t Q, size_t M, size_t ... Idxs>
+matrix::Vector<Type, Q> operator*(const matrix::Matrix<Type, Q, M>& mat, const matrix::SparseVector<Type, M, Idxs...>& vec) {
+    matrix::Vector<Type, Q> res;
+    for (size_t i = 0; i < Q; i++) {
+        const Vector<Type, M> row = mat.row(i);
+        res(i) = vec.dot(row);
+    }
+    return res;
+}
+
+// returns x.T * A * x
+template<typename Type, size_t M, size_t ... Idxs>
+Type quadraticForm(const matrix::SquareMatrix<Type, M>& A, const matrix::SparseVector<Type, M, Idxs...>& x) {
+    Type res = Type(0);
+    for (size_t i = 0; i < x.non_zeros(); i++) {
+        Type tmp = Type(0);
+        for (size_t j = 0; j < x.non_zeros(); j++) {
+            tmp += A(x.index(i), x.index(j)) * x.atCompressedIndex(j);
+        }
+        res += x.atCompressedIndex(i) * tmp;
+    }
+    return res;
+}
+
+template<typename Type,size_t M, size_t... Idxs>
+constexpr size_t SparseVector<Type, M, Idxs...>::_indices[SparseVector<Type, M, Idxs...>::N];
+
+template<size_t M, size_t ... Idxs>
+using SparseVectorf = SparseVector<float, M, Idxs...>;
+
+}

src/lib/matrix/matrix/SquareMatrix.hpp

@@ -0,0 +1,544 @@
+/**
+ * @file SquareMatrix.hpp
+ *
+ * A square matrix
+ *
+ * @author James Goppert <james.goppert@gmail.com>
+ */
+
+#pragma once
+
+#include "math.hpp"
+
+namespace matrix
+{
+
+template <typename Type, size_t M, size_t N>
+class Matrix;
+
+template <typename Type, size_t M>
+class Vector;
+
+template <typename Type, size_t P, size_t Q, size_t M, size_t N>
+class Slice;
+
+template <typename Type, size_t  M>
+class SquareMatrix : public Matrix<Type, M, M>
+{
+public:
+    SquareMatrix() = default;
+
+    explicit SquareMatrix(const Type data_[M][M]) :
+        Matrix<Type, M, M>(data_)
+    {
+    }
+
+    explicit SquareMatrix(const Type data_[M*M]) :
+        Matrix<Type, M, M>(data_)
+    {
+    }
+
+    SquareMatrix(const Matrix<Type, M, M> &other) :
+        Matrix<Type, M, M>(other)
+    {
+    }
+
+    template<size_t P, size_t Q>
+    SquareMatrix(const Slice<Type, M, M, P, Q>& in_slice) : Matrix<Type, M, M>(in_slice)
+    {
+    }
+
+    SquareMatrix<Type, M>& operator=(const Matrix<Type, M, M>& other)
+    {
+        Matrix<Type, M, M>::operator=(other);
+        return *this;
+    }
+
+    template <size_t P, size_t Q>
+    SquareMatrix<Type, M> & operator=(const Slice<Type, M, M, P, Q>& in_slice)
+    {
+        Matrix<Type, M, M>::operator=(in_slice);
+        return *this;
+    }
+
+    template<size_t P, size_t Q>
+    const Slice<Type, P, Q, M, M> slice(size_t x0, size_t y0) const
+    {
+        return Slice<Type, P, Q, M, M>(x0, y0, this);
+    }
+
+    template<size_t P, size_t Q>
+    Slice<Type, P, Q, M, M> slice(size_t x0, size_t y0)
+    {
+        return Slice<Type, P, Q, M, M>(x0, y0, this);
+    }
+
+    // inverse alias
+    inline SquareMatrix<Type, M> I() const
+    {
+        SquareMatrix<Type, M> i;
+        if (inv(*this, i)) {
+            return i;
+        } else {
+            i.setZero();
+            return i;
+        }
+    }
+
+    // inverse alias
+    inline bool I(SquareMatrix<Type, M> &i) const
+    {
+        return inv(*this, i);
+    }
+
+
+    Vector<Type, M> diag() const
+    {
+        Vector<Type, M> res;
+        const SquareMatrix<Type, M> &self = *this;
+
+        for (size_t i = 0; i < M; i++) {
+            res(i) = self(i, i);
+        }
+        return res;
+    }
+
+    // get matrix upper right triangle in a row-major vector format
+    Vector<Type, M * (M + 1) / 2> upper_right_triangle() const
+    {
+        Vector<Type, M * (M + 1) / 2> res;
+        const SquareMatrix<Type, M> &self = *this;
+
+        unsigned idx = 0;
+        for (size_t x = 0; x < M; x++) {
+            for (size_t y = x; y < M; y++) {
+                res(idx) = self(x, y);
+                ++idx;
+            }
+        }
+
+        return res;
+    }
+
+    Type trace() const
+    {
+        Type res = 0;
+        const SquareMatrix<Type, M> &self = *this;
+
+        for (size_t i = 0; i < M; i++) {
+            res += self(i, i);
+        }
+        return res;
+    }
+
+    // zero all offdiagonal elements and keep corresponding diagonal elements
+    template <size_t Width>
+    void uncorrelateCovariance(size_t first)
+    {
+        static_assert(Width <= M, "Width bigger than matrix");
+        assert(first + Width <= M);
+
+        SquareMatrix<Type, M> &self = *this;
+        Vector<Type, Width> diag_elements = self.slice<Width, Width>(first, first).diag();
+        self.uncorrelateCovarianceSetVariance(first, diag_elements);
+    }
+
+    template <size_t Width>
+    void uncorrelateCovarianceSetVariance(size_t first, const Vector<Type, Width> &vec)
+    {
+        static_assert(Width <= M, "Width bigger than matrix");
+        assert(first + Width <= M);
+
+        SquareMatrix<Type, M> &self = *this;
+        // zero rows and columns
+        self.slice<Width, M>(first, 0) = Type(0);
+        self.slice<M, Width>(0, first) = Type(0);
+
+        // set diagonals
+        unsigned vec_idx = 0;
+        for (size_t idx = first; idx < first+Width; idx++) {
+            self(idx,idx) = vec(vec_idx);
+            vec_idx ++;
+        }
+    }
+
+    template <size_t Width>
+    void uncorrelateCovarianceSetVariance(size_t first, Type val)
+    {
+        static_assert(Width <= M, "Width bigger than matrix");
+        assert(first + Width <= M);
+
+        SquareMatrix<Type, M> &self = *this;
+        // zero rows and columns
+        self.slice<Width, M>(first, 0) = Type(0);
+        self.slice<M, Width>(0, first) = Type(0);
+
+        // set diagonals
+        for (size_t idx = first; idx < first+Width; idx++) {
+            self(idx,idx) = val;
+        }
+    }
+
+    // make block diagonal symmetric by taking the average of the two corresponding off diagonal values
+    template <size_t Width>
+    void makeBlockSymmetric(size_t first)
+    {
+        static_assert(Width <= M, "Width bigger than matrix");
+        assert(first + Width <= M);
+
+        SquareMatrix<Type, M> &self = *this;
+        if(Width>1) {
+            for (size_t row_idx = first+1; row_idx < first+Width; row_idx++) {
+                for (size_t col_idx = first; col_idx < row_idx; col_idx++) {
+                    Type tmp = (self(row_idx,col_idx) + self(col_idx,row_idx)) / Type(2);
+                    self(row_idx,col_idx) = tmp;
+                    self(col_idx,row_idx) = tmp;
+                }
+            }
+        }
+    }
+
+    // make rows and columns symmetric by taking the average of the two corresponding off diagonal values
+    template <size_t Width>
+    void makeRowColSymmetric(size_t first)
+    {
+        static_assert(Width <= M, "Width bigger than matrix");
+        assert(first + Width <= M);
+
+        SquareMatrix<Type, M> &self = *this;
+        self.makeBlockSymmetric<Width>(first);
+        for (size_t row_idx = first; row_idx < first+Width; row_idx++) {
+            for (size_t col_idx = 0; col_idx < first; col_idx++) {
+                Type tmp = (self(row_idx,col_idx) + self(col_idx,row_idx)) / Type(2);
+                self(row_idx,col_idx) = tmp;
+                self(col_idx,row_idx) = tmp;
+            }
+            for (size_t col_idx = first+Width; col_idx < M; col_idx++) {
+                Type tmp = (self(row_idx,col_idx) + self(col_idx,row_idx)) / Type(2);
+                self(row_idx,col_idx) = tmp;
+                self(col_idx,row_idx) = tmp;
+            }
+        }
+    }
+
+    // checks if block diagonal is symmetric
+    template <size_t Width>
+    bool isBlockSymmetric(size_t first, const Type eps = Type(1e-8f))
+    {
+        static_assert(Width <= M, "Width bigger than matrix");
+        assert(first + Width <= M);
+
+        SquareMatrix<Type, M> &self = *this;
+        if(Width>1) {
+            for (size_t row_idx = first+1; row_idx < first+Width; row_idx++) {
+                for (size_t col_idx = first; col_idx < row_idx; col_idx++) {
+                    if(!isEqualF(self(row_idx,col_idx), self(col_idx,row_idx), eps)) {
+                        return false;
+                    }
+                }
+            }
+        }
+        return true;
+    }
+
+    // checks if rows and columns are symmetric
+    template <size_t Width>
+    bool isRowColSymmetric(size_t first, const Type eps = Type(1e-8f))
+    {
+        static_assert(Width <= M, "Width bigger than matrix");
+        assert(first + Width <= M);
+
+        SquareMatrix<Type, M> &self = *this;
+        for (size_t row_idx = first; row_idx < first+Width; row_idx++) {
+            for (size_t col_idx = 0; col_idx < first; col_idx++) {
+                if(!isEqualF(self(row_idx,col_idx), self(col_idx,row_idx), eps)) {
+                    return false;
+                }
+            }
+            for (size_t col_idx = first+Width; col_idx < M; col_idx++) {
+                if(!isEqualF(self(row_idx,col_idx), self(col_idx,row_idx), eps)) {
+                    return false;
+                }
+            }
+        }
+        return self.isBlockSymmetric<Width>(first, eps);
+    }
+
+};
+
+using SquareMatrix3f = SquareMatrix<float, 3>;
+using SquareMatrix3d = SquareMatrix<double, 3>;
+
+template<typename Type, size_t M>
+SquareMatrix<Type, M> eye() {
+    SquareMatrix<Type, M> m;
+    m.setIdentity();
+    return m;
+}
+
+template<typename Type, size_t M>
+SquareMatrix<Type, M> diag(Vector<Type, M> d) {
+    SquareMatrix<Type, M> m;
+    for (size_t i=0; i<M; i++) {
+        m(i,i) = d(i);
+    }
+    return m;
+}
+
+template<typename Type, size_t M>
+SquareMatrix<Type, M> expm(const Matrix<Type, M, M> & A, size_t order=5)
+{
+    SquareMatrix<Type, M> res;
+    SquareMatrix<Type, M> A_pow = A;
+    res.setIdentity();
+    size_t i_factorial = 1;
+    for (size_t i=1; i<=order; i++) {
+        i_factorial *= i;
+        res += A_pow / Type(i_factorial);
+        A_pow *= A_pow;
+    }
+
+    return res;
+}
+
+
+/**
+ * inverse based on LU factorization with partial pivotting
+ */
+template<typename Type, size_t M>
+bool inv(const SquareMatrix<Type, M> & A, SquareMatrix<Type, M> & inv, size_t rank = M)
+{
+    SquareMatrix<Type, M> L;
+    L.setIdentity();
+    SquareMatrix<Type, M> U = A;
+    SquareMatrix<Type, M> P;
+    P.setIdentity();
+
+    //printf("A:\n"); A.print();
+
+    // for all diagonal elements
+    for (size_t n = 0; n < rank; n++) {
+
+        // if diagonal is zero, swap with row below
+        if (fabs(U(n, n)) < Type(FLT_EPSILON)) {
+            //printf("trying pivot for row %d\n",n);
+            for (size_t i = n + 1; i < rank; i++) {
+
+                //printf("\ttrying row %d\n",i);
+                if (fabs(U(i, n)) > Type(FLT_EPSILON)) {
+                    //printf("swapped %d\n",i);
+                    U.swapRows(i, n);
+                    P.swapRows(i, n);
+                    L.swapRows(i, n);
+                    L.swapCols(i, n);
+                    break;
+                }
+            }
+        }
+
+#ifdef MATRIX_ASSERT
+        //printf("A:\n"); A.print();
+        //printf("U:\n"); U.print();
+        //printf("P:\n"); P.print();
+        //fflush(stdout);
+        //ASSERT(fabs(U(n, n)) > 1e-8f);
+#endif
+
+        // failsafe, return zero matrix
+        if (fabs(static_cast<float>(U(n, n))) < FLT_EPSILON) {
+            return false;
+        }
+
+        // for all rows below diagonal
+        for (size_t i = (n + 1); i < rank; i++) {
+            L(i, n) = U(i, n) / U(n, n);
+
+            // add i-th row and n-th row
+            // multiplied by: -a(i,n)/a(n,n)
+            for (size_t k = n; k < rank; k++) {
+                U(i, k) -= L(i, n) * U(n, k);
+            }
+        }
+    }
+
+    //printf("L:\n"); L.print();
+    //printf("U:\n"); U.print();
+
+    // solve LY=P*I for Y by forward subst
+    //SquareMatrix<Type, M> Y = P;
+
+    // for all columns of Y
+    for (size_t c = 0; c < rank; c++) {
+        // for all rows of L
+        for (size_t i = 0; i < rank; i++) {
+            // for all columns of L
+            for (size_t j = 0; j < i; j++) {
+                // for all existing y
+                // subtract the component they
+                // contribute to the solution
+                P(i, c) -= L(i, j) * P(j, c);
+            }
+
+            // divide by the factor
+            // on current
+            // term to be solved
+            // Y(i,c) /= L(i,i);
+            // but L(i,i) = 1.0
+        }
+    }
+
+    //printf("Y:\n"); Y.print();
+
+    // solve Ux=y for x by back subst
+    //SquareMatrix<Type, M> X = Y;
+
+    // for all columns of X
+    for (size_t c = 0; c < rank; c++) {
+        // for all rows of U
+        for (size_t k = 0; k < rank; k++) {
+            // have to go in reverse order
+            size_t i = rank - 1 - k;
+
+            // for all columns of U
+            for (size_t j = i + 1; j < rank; j++) {
+                // for all existing x
+                // subtract the component they
+                // contribute to the solution
+                P(i, c) -= U(i, j) * P(j, c);
+            }
+
+            // divide by the factor
+            // on current
+            // term to be solved
+            //
+            // we know that U(i, i) != 0 from above
+            P(i, c) /= U(i, i);
+        }
+    }
+
+    //check sanity of results
+    for (size_t i = 0; i < rank; i++) {
+        for (size_t j = 0; j < rank; j++) {
+            if (!is_finite(P(i,j))) {
+                return false;
+            }
+        }
+    }
+    //printf("X:\n"); X.print();
+    inv = P;
+    return true;
+}
+
+template<typename Type>
+bool inv(const SquareMatrix<Type, 2> & A, SquareMatrix<Type, 2> & inv)
+{
+    Type det = A(0, 0) * A(1, 1) - A(1, 0) * A(0, 1);
+
+    if(fabs(static_cast<float>(det)) < FLT_EPSILON || !is_finite(det)) {
+        return false;
+    }
+
+    inv(0, 0) = A(1, 1);
+    inv(1, 0) = -A(1, 0);
+    inv(0, 1) = -A(0, 1);
+    inv(1, 1) = A(0, 0);
+    inv /= det;
+    return true;
+}
+
+template<typename Type>
+bool inv(const SquareMatrix<Type, 3> & A, SquareMatrix<Type, 3> & inv)
+{
+    Type det = A(0, 0) * (A(1, 1) * A(2, 2) - A(2, 1) * A(1, 2)) -
+               A(0, 1) * (A(1, 0) * A(2, 2) - A(1, 2) * A(2, 0)) +
+               A(0, 2) * (A(1, 0) * A(2, 1) - A(1, 1) * A(2, 0));
+
+    if(fabs(static_cast<float>(det)) < FLT_EPSILON || !is_finite(det)) {
+        return false;
+    }
+
+    inv(0, 0) = A(1, 1) * A(2, 2) - A(2, 1) * A(1, 2);
+    inv(0, 1) = A(0, 2) * A(2, 1) - A(0, 1) * A(2, 2);
+    inv(0, 2) = A(0, 1) * A(1, 2) - A(0, 2) * A(1, 1);
+    inv(1, 0) = A(1, 2) * A(2, 0) - A(1, 0) * A(2, 2);
+    inv(1, 1) = A(0, 0) * A(2, 2) - A(0, 2) * A(2, 0);
+    inv(1, 2) = A(1, 0) * A(0, 2) - A(0, 0) * A(1, 2);
+    inv(2, 0) = A(1, 0) * A(2, 1) - A(2, 0) * A(1, 1);
+    inv(2, 1) = A(2, 0) * A(0, 1) - A(0, 0) * A(2, 1);
+    inv(2, 2) = A(0, 0) * A(1, 1) - A(1, 0) * A(0, 1);
+    inv /= det;
+    return true;
+}
+
+/**
+ * inverse based on LU factorization with partial pivotting
+ */
+template<typename Type, size_t M>
+SquareMatrix<Type, M> inv(const SquareMatrix<Type, M> & A)
+{
+    SquareMatrix<Type, M> i;
+    if (inv(A, i)) {
+        return i;
+    } else {
+        i.setZero();
+        return i;
+    }
+}
+
+/**
+ * cholesky decomposition
+ *
+ * Note: A must be positive definite
+ */
+template<typename Type, size_t M>
+SquareMatrix <Type, M> cholesky(const SquareMatrix<Type, M> & A)
+{
+    SquareMatrix<Type, M> L;
+    for (size_t j = 0; j < M; j++) {
+        for (size_t i = j; i < M; i++) {
+            if (i==j) {
+                float sum = 0;
+                for (size_t k = 0; k < j; k++) {
+                    sum += L(j, k)*L(j, k);
+                }
+                Type res = A(j, j) - sum;
+                if (res <= 0) {
+                    L(j, j) = 0;
+                } else {
+                    L(j, j) = sqrt(res);
+                }
+            } else {
+                float sum = 0;
+                for (size_t k = 0; k < j; k++) {
+                    sum += L(i, k)*L(j, k);
+                }
+                if (L(j, j) <= 0) {
+                    L(i, j) = 0;
+                } else {
+                    L(i, j) = (A(i, j) - sum)/L(j, j);
+                }
+            }
+        }
+    }
+    return L;
+}
+
+/**
+ * cholesky inverse
+ *
+ * TODO: Check if gaussian elimination jumps straight to back-substitution
+ * for L or we need to do it manually. Will impact speed otherwise.
+ */
+template<typename Type, size_t M>
+SquareMatrix <Type, M> choleskyInv(const SquareMatrix<Type, M> & A)
+{
+    SquareMatrix<Type, M> L_inv = inv(cholesky(A));
+    return L_inv.T()*L_inv;
+}
+
+using Matrix3f = SquareMatrix<float, 3>;
+using Matrix3d = SquareMatrix<double, 3>;
+
+} // namespace matrix
+
+/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

src/lib/matrix/matrix/Vector.hpp

@@ -0,0 +1,136 @@
+/**
+ * @file Vector.hpp
+ *
+ * Vector class.
+ *
+ * @author James Goppert <james.goppert@gmail.com>
+ */
+
+#pragma once
+
+#include "math.hpp"
+
+namespace matrix
+{
+
+template <typename Type, size_t M, size_t N>
+class Matrix;
+
+template<typename Type, size_t M>
+class Vector : public Matrix<Type, M, 1>
+{
+public:
+    using MatrixM1 = Matrix<Type, M, 1>;
+
+    Vector() = default;
+
+    Vector(const MatrixM1 & other) :
+        MatrixM1(other)
+    {
+    }
+
+    explicit Vector(const Type data_[M]) :
+        MatrixM1(data_)
+    {
+    }
+
+    template<size_t P, size_t Q>
+    Vector(const Slice<Type, M, 1, P, Q>& slice_in) :
+        Matrix<Type, M, 1>(slice_in)
+    {
+    }
+
+    template<size_t P, size_t Q, size_t DUMMY = 1>
+    Vector(const Slice<Type, 1, M, P, Q>& slice_in)
+    {
+        Vector &self(*this);
+        for (size_t i = 0; i<M; i++) {
+            self(i) = slice_in(0, i);
+        }
+    }
+
+    inline const Type &operator()(size_t i) const
+    {
+        assert(i < M);
+
+        const MatrixM1 &v = *this;
+        return v(i, 0);
+    }
+
+    inline Type &operator()(size_t i)
+    {
+        assert(i < M);
+
+        MatrixM1 &v = *this;
+        return v(i, 0);
+    }
+
+    Type dot(const MatrixM1 & b) const {
+        const Vector &a(*this);
+        Type r(0);
+        for (size_t i = 0; i<M; i++) {
+            r += a(i)*b(i,0);
+        }
+        return r;
+    }
+
+    inline Type operator*(const MatrixM1 & b) const {
+        const Vector &a(*this);
+        return a.dot(b);
+    }
+
+    inline Vector operator*(Type b) const {
+        return Vector(MatrixM1::operator*(b));
+    }
+
+    Type norm() const {
+        const Vector &a(*this);
+        return Type(matrix::sqrt(a.dot(a)));
+    }
+
+    Type norm_squared() const {
+        const Vector &a(*this);
+        return a.dot(a);
+    }
+
+    inline Type length() const {
+        return norm();
+    }
+
+    inline void normalize() {
+        (*this) /= norm();
+    }
+
+    Vector unit() const {
+        return (*this) / norm();
+    }
+
+    Vector unit_or_zero(const Type eps = Type(1e-5)) const {
+        const Type n = norm();
+        if (n > eps) {
+            return (*this) / n;
+        }
+        return Vector();
+    }
+
+    inline Vector normalized() const {
+        return unit();
+    }
+
+    bool longerThan(Type testVal) const {
+        return norm_squared() > testVal*testVal;
+    }
+
+    Vector sqrt() const {
+        const Vector &a(*this);
+        Vector r;
+        for (size_t i = 0; i<M; i++) {
+            r(i) = Type(matrix::sqrt(a(i)));
+        }
+        return r;
+    }
+};
+
+} // namespace matrix
+
+/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

src/lib/matrix/matrix/Vector2.hpp

@@ -0,0 +1,80 @@
+/**
+ * @file Vector2.hpp
+ *
+ * 2D vector class.
+ *
+ * @author James Goppert <james.goppert@gmail.com>
+ */
+
+#pragma once
+
+#include "math.hpp"
+
+namespace matrix
+{
+
+template <typename Type, size_t M>
+class Vector;
+
+template<typename Type>
+class Vector2 : public Vector<Type, 2>
+{
+public:
+
+    using Matrix21 = Matrix<Type, 2, 1>;
+    using Vector3 = Vector<Type, 3>;
+
+    Vector2() = default;
+
+    Vector2(const Matrix21 & other) :
+        Vector<Type, 2>(other)
+    {
+    }
+
+    explicit Vector2(const Type data_[2]) :
+        Vector<Type, 2>(data_)
+    {
+    }
+
+    Vector2(Type x, Type y)
+    {
+        Vector2 &v(*this);
+        v(0) = x;
+        v(1) = y;
+    }
+
+    template<size_t P, size_t Q>
+    Vector2(const Slice<Type, 2, 1, P, Q>& slice_in) : Vector<Type, 2>(slice_in)
+    {
+    }
+
+    template<size_t P, size_t Q>
+    Vector2(const Slice<Type, 1, 2, P, Q>& slice_in) : Vector<Type, 2>(slice_in)
+    {
+    }
+
+    explicit Vector2(const Vector3 & other)
+    {
+        Vector2 &v(*this);
+        v(0) = other(0);
+        v(1) = other(1);
+    }
+
+    Type cross(const Matrix21 & b) const {
+        const Vector2 &a(*this);
+        return a(0)*b(1, 0) - a(1)*b(0, 0);
+    }
+
+    Type operator%(const Matrix21 & b) const {
+        return (*this).cross(b);
+    }
+
+};
+
+
+using Vector2f = Vector2<float>;
+using Vector2d = Vector2<double>;
+
+} // namespace matrix
+
+/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

src/lib/matrix/matrix/Vector3.hpp

@@ -0,0 +1,156 @@
+/**
+ * @file Vector3.hpp
+ *
+ * 3D vector class.
+ *
+ * @author James Goppert <james.goppert@gmail.com>
+ */
+
+#pragma once
+
+#include "math.hpp"
+
+namespace matrix
+{
+
+template <typename Type, size_t M, size_t N>
+class Matrix;
+
+template <typename Type, size_t M>
+class Vector;
+
+template <typename Type>
+class Dcm;
+
+template <typename Type>
+class Vector2;
+
+template<typename Type>
+class Vector3 : public Vector<Type, 3>
+{
+public:
+
+    using Matrix31 = Matrix<Type, 3, 1>;
+
+    Vector3() = default;
+
+    Vector3(const Matrix31 & other) :
+        Vector<Type, 3>(other)
+    {
+    }
+
+    explicit Vector3(const Type data_[3]) :
+        Vector<Type, 3>(data_)
+    {
+    }
+
+    Vector3(Type x, Type y, Type z) {
+        Vector3 &v(*this);
+        v(0) = x;
+        v(1) = y;
+        v(2) = z;
+    }
+
+    template<size_t P, size_t Q>
+    Vector3(const Slice<Type, 3, 1, P, Q>& slice_in) : Vector<Type, 3>(slice_in)
+    {
+    }
+
+    template<size_t P, size_t Q>
+    Vector3(const Slice<Type, 1, 3, P, Q>& slice_in) : Vector<Type, 3>(slice_in)
+    {
+    }
+
+    Vector3 cross(const Matrix31 & b) const {
+        const Vector3 &a(*this);
+        return {a(1)*b(2,0) - a(2)*b(1,0), -a(0)*b(2,0) + a(2)*b(0,0), a(0)*b(1,0) - a(1)*b(0,0)};
+    }
+
+    /**
+     * Override matrix ops so Vector3 type is returned
+     */
+
+    inline Vector3 operator+(Vector3 other) const
+    {
+        return Matrix31::operator+(other);
+    }
+
+    inline Vector3 operator+(Type scalar) const
+    {
+        return Matrix31::operator+(scalar);
+    }
+
+    inline Vector3 operator-(Vector3 other) const
+    {
+        return Matrix31::operator-(other);
+    }
+
+    inline Vector3 operator-(Type scalar) const
+    {
+        return Matrix31::operator-(scalar);
+    }
+
+    inline Vector3 operator-() const
+    {
+        return Matrix31::operator-();
+    }
+
+    inline Vector3 operator*(Type scalar) const
+    {
+        return Matrix31::operator*(scalar);
+    }
+
+    inline Type operator*(Vector3 b) const
+    {
+        return Vector<Type, 3>::operator*(b);
+    }
+
+    inline Vector3 operator%(const Matrix31 & b) const {
+        return (*this).cross(b);
+    }
+
+    /**
+     * Override vector ops so Vector3 type is returned
+     */
+    inline Vector3 unit() const {
+        return Vector3(Vector<Type, 3>::unit());
+    }
+
+    inline Vector3 normalized() const {
+        return unit();
+    }
+
+    const Slice<Type, 2, 1, 3, 1> xy() const
+    {
+        return Slice<Type, 2, 1, 3, 1>(0, 0, this);
+    }
+
+    Slice<Type, 2, 1, 3, 1> xy()
+    {
+        return Slice<Type, 2, 1, 3, 1>(0, 0, this);
+    }
+
+
+    Dcm<Type> hat() const {    // inverse to Dcm.vee() operation
+        const Vector3 &v(*this);
+        Dcm<Type> A;
+        A(0,0) = 0;
+        A(0,1) = -v(2);
+        A(0,2) = v(1);
+        A(1,0) = v(2);
+        A(1,1) = 0;
+        A(1,2) = -v(0);
+        A(2,0) = -v(1);
+        A(2,1) = v(0);
+        A(2,2) = 0;
+        return A;
+    }
+
+};
+
+using Vector3f = Vector3<float>;
+using Vector3d = Vector3<double>;
+
+} // namespace matrix
+
+/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

src/lib/matrix/matrix/filter.hpp

@@ -0,0 +1,26 @@
+#pragma once
+
+#include "math.hpp"
+
+namespace matrix {
+
+template<typename Type, size_t M, size_t N>
+int kalman_correct(
+    const Matrix<Type, M, M> & P,
+    const Matrix<Type, N, M> & C,
+    const Matrix<Type, N, N> & R,
+    const Matrix<Type, N, 1> &r,
+    Matrix<Type, M, 1> & dx,
+    Matrix<Type, M, M> & dP,
+    Type & beta
+)
+{
+    SquareMatrix<Type, N> S_I = SquareMatrix<Type, N>(C*P*C.T() + R).I();
+    Matrix<Type, M, N> K = P*C.T()*S_I;
+    dx = K*r;
+    beta = Scalar<Type>(r.T()*S_I*r);
+    dP = K*C*P*(-1);
+    return 0;
+}
+
+} // namespace matrix

src/lib/matrix/matrix/helper_functions.hpp

@@ -0,0 +1,127 @@
+#pragma once
+
+#include "math.hpp"
+
+#if defined (__PX4_NUTTX) || defined (__PX4_QURT)
+#include <px4_defines.h>
+#endif
+
+namespace matrix
+{
+
+template<typename Type>
+bool is_finite(Type x) {
+#if defined (__PX4_NUTTX)
+    return PX4_ISFINITE(x);
+#elif defined (__PX4_QURT)
+    return __builtin_isfinite(x);
+#else
+    return std::isfinite(x);
+#endif
+}
+
+/**
+ * Compare if two floating point numbers are equal
+ *
+ * NAN is considered equal to NAN and -NAN
+ * INFINITY is considered equal INFINITY but not -INFINITY
+ *
+ * @param x right side of equality check
+ * @param y left side of equality check
+ * @param eps numerical tolerance of the check
+ * @return true if the two values are considered equal, false otherwise
+ */
+template<typename Type>
+bool isEqualF(const Type x, const Type y, const Type eps = Type(1e-4f))
+{
+    return (matrix::fabs(x - y) <= eps)
+           || (isnan(x) && isnan(y))
+           || (isinf(x) && isinf(y) && isnan(x - y));
+}
+
+namespace detail
+{
+
+template<typename Floating>
+Floating wrap_floating(Floating x, Floating low, Floating high) {
+    // already in range
+    if (low <= x && x < high) {
+        return x;
+    }
+
+    const auto range = high - low;
+    const auto inv_range = Floating(1) / range; // should evaluate at compile time, multiplies below at runtime
+    const auto num_wraps = floor((x - low) * inv_range);
+    return x - range * num_wraps;
+}
+
+}  // namespace detail
+
+/**
+ * Wrap single precision floating point value to stay in range [low, high)
+ *
+ * @param x input possibly outside of the range
+ * @param low lower limit of the allowed range
+ * @param high upper limit of the allowed range
+ * @return wrapped value inside the range
+ */
+inline float wrap(float x, float low, float high) {
+    return matrix::detail::wrap_floating(x, low, high);
+}
+
+/**
+ * Wrap double precision floating point value to stay in range [low, high)
+ *
+ * @param x input possibly outside of the range
+ * @param low lower limit of the allowed range
+ * @param high upper limit of the allowed range
+ * @return wrapped value inside the range
+ */
+inline double wrap(double x, double low, double high) {
+    return matrix::detail::wrap_floating(x, low, high);
+}
+
+/**
+ * Wrap integer value to stay in range [low, high)
+ *
+ * @param x input possibly outside of the range
+ * @param low lower limit of the allowed range
+ * @param high upper limit of the allowed range
+ * @return wrapped value inside the range
+ */
+template<typename Integer>
+Integer wrap(Integer x, Integer low, Integer high) {
+    const auto range = high - low;
+
+    if (x < low) {
+        x += range * ((low - x) / range + 1);
+    }
+
+    return low + (x - low) % range;
+}
+
+/**
+ * Wrap value in range [-π, π)
+ */
+template<typename Type>
+Type wrap_pi(Type x)
+{
+    return wrap(x, Type(-M_PI), Type(M_PI));
+}
+
+/**
+ * Wrap value in range [0, 2π)
+ */
+template<typename Type>
+Type wrap_2pi(Type x)
+{
+    return wrap(x, Type(0), Type(M_TWOPI));
+}
+
+template<typename T>
+int sign(T val)
+{
+    return (T(FLT_EPSILON) < val) - (val < T(FLT_EPSILON));
+}
+
+} // namespace matrix

src/lib/matrix/matrix/integration.hpp

@@ -0,0 +1,42 @@
+#pragma once
+
+#include "math.hpp"
+
+namespace matrix {
+
+template<typename Type, size_t M, size_t N>
+int integrate_rk4(
+    Vector<Type, M> (*f)(Type, const Matrix<Type, M, 1> &x, const Matrix<Type, N, 1> & u),
+    const Matrix<Type, M, 1> & y0,
+    const Matrix<Type, N, 1> & u,
+    Type t0,
+    Type tf,
+    Type h0,
+    Matrix<Type, M, 1> & y1
+)
+{
+    // https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta_methods
+    Type t1 = t0;
+    y1 = y0;
+    Type h = h0;
+    Vector<Type, M> k1, k2, k3, k4;
+    if (tf < t0) return -1; // make sure t1 > t0
+    while (t1 < tf) {
+        if (t1 + h0 < tf) {
+            h = h0;
+        } else {
+            h = tf - t1;
+        }
+        k1 = f(t1, y1, u);
+        k2 = f(t1 + h/2, y1 + k1*h/2, u);
+        k3 = f(t1 + h/2, y1 + k2*h/2, u);
+        k4 = f(t1 + h, y1 + k3*h, u);
+        y1 += (k1 + k2*2 + k3*2 + k4)*(h/6);
+        t1 += h;
+    }
+    return 0;
+}
+
+} // namespace matrix
+
+// vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 :

src/lib/matrix/matrix/math.hpp

@@ -0,0 +1,23 @@
+#pragma once
+
+#include <assert.h>
+#include "stdlib_imports.hpp"
+#ifdef __PX4_QURT
+#include "dspal_math.h"
+#endif
+#include "helper_functions.hpp"
+#include "Matrix.hpp"
+#include "SquareMatrix.hpp"
+#include "Slice.hpp"
+#include "Vector.hpp"
+#include "Vector2.hpp"
+#include "Vector3.hpp"
+#include "Euler.hpp"
+#include "Dcm.hpp"
+#include "Scalar.hpp"
+#include "Quaternion.hpp"
+#include "AxisAngle.hpp"
+#include "LeastSquaresSolver.hpp"
+#include "Dual.hpp"
+#include "PseudoInverse.hpp"
+#include "SparseVector.hpp"

src/lib/matrix/matrix/stdlib_imports.hpp

@@ -0,0 +1,139 @@
+/**
+ * @file stdlib_imports.hpp
+ *
+ * This file is needed to shadow the C standard library math functions with ones provided by the C++ standard library.
+ * This way we can guarantee that unwanted functions from the C library will never creep back in unexpectedly.
+ *
+ * @author Pavel Kirienko <pavel.kirienko@zubax.com>
+ */
+
+#pragma once
+
+#include <cmath>
+#include <cstdlib>
+#include <inttypes.h>
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+#ifndef M_TWOPI
+#define M_TWOPI (M_PI * 2.0)
+#endif
+
+namespace matrix {
+
+#if !defined(FLT_EPSILON)
+#define FLT_EPSILON     __FLT_EPSILON__
+#endif
+
+#if defined(__PX4_NUTTX)
+/*
+ * NuttX has no usable C++ math library, so we need to provide the needed definitions here manually.
+ */
+#define MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(name)                                   \
+	inline float       name(float x)       { return ::name##f(x); }          \
+	inline double      name(double x)      { return ::name(x); }             \
+	inline long double name(long double x) { return ::name##l(x); }
+
+#define MATRIX_NUTTX_WRAP_MATH_FUN_BINARY(name)                                                    \
+	inline float       name(float x, float y)             { return ::name##f(x, y); }          \
+	inline double      name(double x, double y)           { return ::name(x, y); }             \
+	inline long double name(long double x, long double y) { return ::name##l(x, y); }
+
+MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(fabs)
+MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(log)
+MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(log10)
+MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(exp)
+MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(sqrt)
+MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(sin)
+MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(cos)
+MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(tan)
+MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(asin)
+MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(acos)
+MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(atan)
+MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(sinh)
+MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(cosh)
+MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(tanh)
+MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(ceil)
+MATRIX_NUTTX_WRAP_MATH_FUN_UNARY(floor)
+
+MATRIX_NUTTX_WRAP_MATH_FUN_BINARY(pow)
+MATRIX_NUTTX_WRAP_MATH_FUN_BINARY(atan2)
+
+#else       // Not NuttX, using the C++ standard library
+
+using std::abs;
+using std::div;
+using std::fabs;
+using std::fmod;
+using std::exp;
+using std::log;
+using std::log10;
+using std::pow;
+using std::sqrt;
+using std::sin;
+using std::cos;
+using std::tan;
+using std::asin;
+using std::acos;
+using std::atan;
+using std::atan2;
+using std::sinh;
+using std::cosh;
+using std::tanh;
+using std::ceil;
+using std::floor;
+using std::frexp;
+using std::ldexp;
+using std::modf;
+
+# if (__cplusplus >= 201103L)
+
+using std::remainder;
+using std::remquo;
+using std::fma;
+using std::fmax;
+using std::fmin;
+using std::fdim;
+using std::nan;
+using std::nanf;
+using std::nanl;
+using std::exp2;
+using std::expm1;
+using std::log2;
+using std::log1p;
+using std::cbrt;
+using std::hypot;
+using std::asinh;
+using std::acosh;
+using std::atanh;
+using std::erf;
+using std::erfc;
+using std::tgamma;
+using std::lgamma;
+using std::trunc;
+using std::round;
+using std::nearbyint;
+using std::rint;
+using std::scalbn;
+using std::ilogb;
+using std::logb;
+using std::nextafter;
+using std::copysign;
+using std::fpclassify;
+using std::isfinite;
+using std::isinf;
+using std::isnan;
+using std::isnormal;
+using std::signbit;
+using std::isgreater;
+using std::isgreaterequal;
+using std::isless;
+using std::islessequal;
+using std::islessgreater;
+using std::isunordered;
+
+# endif
+#endif
+
+}

src/lib/matrix/scripts/format.sh

@@ -0,0 +1,30 @@
+#!/bin/bash
+echo pwd:$PWD
+astyle=$1
+format=$2
+format_wildcards="""
+./matrix/*.*pp
+./test/*.*pp
+"""
+
+#echo astyle: $astyle
+#echo format: $format
+
+if [[ $format -eq 1 ]]
+then
+    echo formatting
+    $astyle ${format_wildcards}
+else
+    echo checking format...
+    $astyle --dry-run ${format_wildcards} | grep Formatted &>/dev/null
+    if [[ $? -eq 0 ]]
+    then
+        echo Error: need to format
+        echo "From cmake build directory run: 'make format'"
+        exit 1
+    fi
+    echo no formatting needed
+    exit 0
+fi
+
+# vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : 

src/lib/matrix/test/CMakeLists.txt

@@ -0,0 +1,67 @@
+include(gtest.cmake)
+
+set(tests
+    setIdentity
+    inverse
+    slice
+    matrixMult
+    vectorAssignment
+    matrixAssignment
+    matrixScalarMult
+    transpose
+    vector
+    vector2
+    vector3
+    attitude
+    filter
+    integration
+    sparseVector
+    squareMatrix
+    helper
+    hatvee
+    copyto
+    least_squares
+    upperRightTriangle
+    dual
+    pseudoInverse
+    )
+
+add_custom_target(test_build)
+foreach(test_name ${tests})
+    add_executable(${test_name}
+        ${test_name}.cpp)
+    add_test(test_${test_name} ${test_name})
+    add_dependencies(test_build ${test_name})
+endforeach()
+target_link_libraries(sparseVector gtest_main)
+
+if (${CMAKE_BUILD_TYPE} STREQUAL "Coverage")
+
+    add_custom_target(coverage_build
+        COMMAND ${CMAKE_CTEST_COMMAND}
+        COMMAND lcov --capture --directory . --output-file coverage.info --rc lcov_branch_coverage=1
+        COMMAND lcov --rc lcov_branch_coverage=1 --summary coverage.info
+        WORKING_DIRECTORY ${CMAKE_BUILD_DIR}
+        DEPENDS test_build
+        )
+
+    add_custom_target(coverage_html
+        COMMAND genhtml coverage.info --output-directory out --branch-coverage
+        COMMAND x-www-browser out/index.html
+        WORKING_DIRECTORY ${CMAKE_BUILD_DIR}
+        DEPENDS coverage_build
+        )
+
+    set(coverage_deps
+        coverage_build)
+
+    if (COV_HTML)
+        list(APPEND coverage_deps coverage_html)
+    endif()
+
+    add_custom_target(coverage
+        DEPENDS ${coverage_deps}
+        )
+endif()
+
+# vim: set et fenc=utf-8 ft=cmake ff=unix sts=0 sw=4 ts=4 :

src/lib/matrix/test/CMakeLists.txt.in

@@ -0,0 +1,18 @@
+cmake_minimum_required(VERSION 2.8.4)
+
+project(googletest-download NONE)
+
+include(ExternalProject)
+ExternalProject_Add(googletest
+	URL https://github.com/google/googletest/archive/e2239ee6043f73722e7aa812a459f54a28552929.zip
+	SOURCE_DIR "${CMAKE_CURRENT_BINARY_DIR}/googletest-src"
+	BINARY_DIR "${CMAKE_CURRENT_BINARY_DIR}/googletest-build"
+	CONFIGURE_COMMAND ""
+	BUILD_COMMAND ""
+	INSTALL_COMMAND ""
+	TEST_COMMAND ""
+	# Wrap download, configure and build steps in a script to log output
+    LOG_DOWNLOAD ON
+    LOG_CONFIGURE ON
+    LOG_BUILD ON
+)

src/lib/matrix/test/attitude.cpp

@@ -0,0 +1,490 @@
+#include "test_macros.hpp"
+#include <matrix/math.hpp>
+#include <iostream>
+
+using namespace matrix;
+
+// manually instantiated all files we intend to test
+// so that coverage works correctly
+// doesn't matter what test this is in
+namespace matrix {
+template class Matrix<float, 3, 3>;
+template class Vector3<float>;
+template class Vector2<float>;
+template class Vector<float, 4>;
+template class Quaternion<float>;
+template class AxisAngle<float>;
+template class Scalar<float>;
+template class SquareMatrix<float, 4>;
+}
+
+int main()
+{
+    // check data
+    Eulerf euler_check(0.1f, 0.2f, 0.3f);
+    Quatf q_check(0.98334744f, 0.0342708f, 0.10602051f, .14357218f);
+    float dcm_data[] =  {
+        0.93629336f, -0.27509585f,  0.21835066f,
+        0.28962948f,  0.95642509f, -0.03695701f,
+        -0.19866933f,  0.0978434f,  0.97517033f
+    };
+    Dcmf dcm_check(dcm_data);
+
+    // euler ctor
+    TEST(isEqual(euler_check, Vector3f(0.1f, 0.2f, 0.3f)));
+
+    // euler default ctor
+    Eulerf e;
+    Eulerf e_zero = zeros<float, 3, 1>();
+    TEST(isEqual(e, e_zero));
+    TEST(isEqual(e, e));
+
+    // euler vector ctor
+    Vector3f v(0.1f, 0.2f, 0.3f);
+    Eulerf euler_copy(v);
+    TEST(isEqual(euler_copy, euler_check));
+
+    // quaternion ctor
+    Quatf q0(1, 2, 3, 4);
+    Quatf q(q0);
+    double eps = 1e-6;
+    TEST(fabs(q(0) - 1) < eps);
+    TEST(fabs(q(1) - 2) < eps);
+    TEST(fabs(q(2) - 3) < eps);
+    TEST(fabs(q(3) - 4) < eps);
+
+    // quaternion ctor: vector to vector
+    // identity test
+    Quatf quat_v(v,v);
+    TEST(isEqual(quat_v.conjugate(v), v));
+    // random test (vector norm can not be preserved with a pure rotation)
+    Vector3f v1(-80.1f, 1.5f, -6.89f);
+    quat_v = Quatf(v1, v);
+    TEST(isEqual(quat_v.conjugate(v1).normalized() * v.norm(), v));
+    // special 180 degree case 1
+    v1 = Vector3f(0.f, 1.f, 1.f);
+    quat_v = Quatf(v1, -v1);
+    TEST(isEqual(quat_v.conjugate(v1), -v1));
+    // special 180 degree case 2
+    v1 = Vector3f(1.f, 2.f, 0.f);
+    quat_v = Quatf(v1, -v1);
+    TEST(isEqual(quat_v.conjugate(v1), -v1));
+    // special 180 degree case 3
+    v1 = Vector3f(0.f, 0.f, 1.f);
+    quat_v = Quatf(v1, -v1);
+    TEST(isEqual(quat_v.conjugate(v1), -v1));
+    // special 180 degree case 4
+    v1 = Vector3f(1.f, 1.f, 1.f);
+    quat_v = Quatf(v1, -v1);
+    TEST(isEqual(quat_v.conjugate(v1), -v1));
+
+    // quat normalization
+    q.normalize();
+    TEST(isEqual(q, Quatf(0.18257419f,  0.36514837f,
+                          0.54772256f,  0.73029674f)));
+    TEST(isEqual(q0.unit(), q));
+    TEST(isEqual(q0.unit(), q0.normalized()));
+
+    // quat default ctor
+    q = Quatf();
+    TEST(isEqual(q, Quatf(1, 0, 0, 0)));
+
+    // quaternion exponential with v=0
+    v = Vector3f();
+    q = Quatf(1.0f, 0.0f, 0.0f, 0.0f);
+    Dcmf M = Dcmf()*0.5f;
+    TEST(isEqual(q, Quatf::expq(v)));
+    TEST(isEqual(M, Quatf::inv_r_jacobian(v)));
+
+    // quaternion exponential with small v
+    v = Vector3f(0.001f,0.002f,-0.003f);
+    q = Quatf(0.999993000008167f, 0.000999997666668f,
+              0.001999995333337f, -0.002999993000005f);
+    {
+        float M_data[] =  {
+            0.499997833331311f, 0.001500333333644f,  0.000999499999533f,
+            -0.001499666666356f, 0.499998333331778f, -0.000501000000933f,
+            -0.001000500000467f, 0.000498999999067f,  0.499999166665889f
+        };
+        M = Dcmf(M_data);
+    }
+    TEST(isEqual(q, Quatf::expq(v)));
+    TEST(isEqual(M, Quatf::inv_r_jacobian(v)));
+
+    // quaternion exponential with v
+    v = Vector3f(1.0f, -2.0f, 3.0f);
+    q = Quatf(-0.825299062075259f, -0.150921327219964f,
+              0.301842654439929f, -0.452763981659893f);
+    {
+        float M_data[] =  {
+            2.574616981530584f, -1.180828156687602f, -1.478757764968596f,
+            1.819171843312398f,  2.095859216561988f,  0.457515529937193f,
+            0.521242235031404f,  1.457515529937193f,  1.297929608280994f
+        };
+        M = Dcmf(M_data);
+    }
+    TEST(isEqual(q, Quatf::expq(v)));
+    TEST(isEqual(M, Quatf::inv_r_jacobian(v)));
+
+    // quaternion kinematic update
+    q = Quatf();
+    float h=0.001f;    // sampling time [s]
+    Vector3f w_B=Vector3f(0.1f,0.2f,0.3f);     // body rate in body frame
+    Quatf qa=q+0.5f*h*q.derivative1(w_B);
+    qa.normalize();
+    Quatf qb=q*Quatf::expq(0.5f*h*w_B);
+    TEST(isEqual(qa, qb));
+
+    // euler to quaternion
+    q = Quatf(euler_check);
+    TEST(isEqual(q, q_check));
+
+    // euler to dcm
+    Dcmf dcm(euler_check);
+    TEST(isEqual(dcm, dcm_check));
+
+    // quaternion to euler
+    Eulerf e1(q_check);
+    TEST(isEqual(e1, euler_check));
+
+    // quaternion to dcm
+    Dcmf dcm1(q_check);
+    TEST(isEqual(dcm1, dcm_check));
+    // quaternion z-axis unit base vector
+    Vector3f q_z = q_check.dcm_z();
+    Vector3f R_z(dcm_check(0, 2), dcm_check(1, 2), dcm_check(2, 2));
+    TEST(isEqual(q_z, R_z));
+
+    // dcm default ctor
+    Dcmf dcm2;
+    SquareMatrix<float, 3> I = eye<float, 3>();
+    TEST(isEqual(dcm2, I));
+
+    // dcm to euler
+    Eulerf e2(dcm_check);
+    TEST(isEqual(e2, euler_check));
+
+    // dcm to quaterion
+    Quatf q2(dcm_check);
+    TEST(isEqual(q2, q_check));
+
+    // dcm renormalize
+    Dcmf A = eye<float, 3>();
+    Dcmf R(euler_check);
+    for (size_t i = 0; i < 1000; i++) {
+        A = R * A;
+    }
+
+    A.renormalize();
+    float err = 0.0f;
+
+    for (size_t r = 0; r < 3; r++) {
+        Vector3f rvec(matrix::Matrix<float,1,3>(A.row(r)).transpose());
+        err += fabs(1.0f - rvec.length());
+    }
+    TEST(err < eps);
+
+    // constants
+    double deg2rad = M_PI / 180.0;
+    double rad2deg = 180.0 / M_PI;
+
+    // euler dcm round trip check
+    for (double roll = -90; roll <= 90; roll += 90) {
+        for (double pitch = -90; pitch <= 90; pitch += 90) {
+            for (double yaw = -179; yaw <= 180; yaw += 90) {
+                // note if theta = pi/2, then roll is set to zero
+                double roll_expected = roll;
+                double yaw_expected = yaw;
+
+                if (fabs(pitch -90) < eps) {
+                    roll_expected = 0;
+                    yaw_expected = yaw - roll;
+
+                } else if (fabs(pitch + 90) < eps) {
+                    roll_expected = 0;
+                    yaw_expected = yaw + roll;
+                }
+
+                if (yaw_expected < -180) {
+                    yaw_expected += 360;
+                }
+
+                if (yaw_expected > 180) {
+                    yaw_expected -= 360;
+                }
+
+                //printf("roll:%d pitch:%d yaw:%d\n", roll, pitch, yaw);
+                Euler<double> euler_expected(
+                    deg2rad * roll_expected,
+                    deg2rad * pitch,
+                    deg2rad * yaw_expected);
+                Euler<double> euler(
+                    deg2rad * roll,
+                    deg2rad * pitch,
+                    deg2rad * yaw);
+                Dcm<double> dcm_from_euler(euler);
+                //dcm_from_euler.print();
+                Euler<double> euler_out(dcm_from_euler);
+                TEST(isEqual(rad2deg * euler_expected, rad2deg * euler_out));
+
+                Eulerf eulerf_expected(
+                    float(deg2rad)*float(roll_expected),
+                    float(deg2rad)*float(pitch),
+                    float(deg2rad)*float(yaw_expected));
+                Eulerf eulerf(float(deg2rad)*float(roll),
+                              float(deg2rad)*float(pitch),
+                              float(deg2rad)*float(yaw));
+                Dcm<float> dcm_from_eulerf;
+                dcm_from_eulerf = eulerf;
+                Euler<float> euler_outf(dcm_from_eulerf);
+                TEST(isEqual(float(rad2deg)*eulerf_expected,
+                             float(rad2deg)*euler_outf));
+            }
+        }
+    }
+
+    // quaterion copy ctors
+    float data_v4[] = {1, 2, 3, 4};
+    Vector<float, 4> v4(data_v4);
+    Quatf q_from_v(v4);
+    TEST(isEqual(q_from_v, v4));
+
+    Matrix<float, 4, 1> m4(data_v4);
+    Quatf q_from_m(m4);
+    TEST(isEqual(q_from_m, m4));
+
+    // quaternion derivative in frame 1
+    Quatf q1(0, 1, 0, 0);
+    Vector<float, 4> q1_dot1 = q1.derivative1(Vector3f(1, 2, 3));
+    float data_q_dot1_check[] = { -0.5f, 0.0f, -1.5f, 1.0f};
+    Vector<float, 4> q1_dot1_check(data_q_dot1_check);
+    TEST(isEqual(q1_dot1, q1_dot1_check));
+
+    // quaternion derivative in frame 2
+    Vector<float, 4> q1_dot2 = q1.derivative2(Vector3f(1, 2, 3));
+    float data_q_dot2_check[] = { -0.5f, 0.0f, 1.5f, -1.0f};
+    Vector<float, 4> q1_dot2_check(data_q_dot2_check);
+    TEST(isEqual(q1_dot2, q1_dot2_check));
+
+    // quaternion product
+    Quatf q_prod_check(
+        0.93394439f, 0.0674002f, 0.20851f, 0.28236266f);
+    TEST(isEqual(q_prod_check, q_check * q_check));
+    q_check *= q_check;
+    TEST(isEqual(q_prod_check, q_check));
+
+    // Quaternion scalar multiplication
+    float scalar = 0.5;
+    Quatf q_scalar_mul(1.0f, 2.0f, 3.0f, 4.0f);
+    Quatf q_scalar_mul_check(1.0f * scalar, 2.0f * scalar,
+                             3.0f * scalar,  4.0f * scalar);
+    Quatf q_scalar_mul_res = scalar * q_scalar_mul;
+    TEST(isEqual(q_scalar_mul_check, q_scalar_mul_res));
+    Quatf q_scalar_mul_res2 = q_scalar_mul * scalar;
+    TEST(isEqual(q_scalar_mul_check, q_scalar_mul_res2));
+    Quatf q_scalar_mul_res3(q_scalar_mul);
+    q_scalar_mul_res3 *= scalar;
+    TEST(isEqual(q_scalar_mul_check, q_scalar_mul_res3));
+
+    // quaternion inverse
+    q = q_check.inversed();
+    TEST(fabs(q_check(0) - q(0)) < eps);
+    TEST(fabs(q_check(1) + q(1)) < eps);
+    TEST(fabs(q_check(2) + q(2)) < eps);
+    TEST(fabs(q_check(3) + q(3)) < eps);
+
+    q = q_check;
+    q.invert();
+    TEST(fabs(q_check(0) - q(0)) < eps);
+    TEST(fabs(q_check(1) + q(1)) < eps);
+    TEST(fabs(q_check(2) + q(2)) < eps);
+    TEST(fabs(q_check(3) + q(3)) < eps);
+
+    // quaternion canonical
+    Quatf q_non_canonical_1(-0.7f,0.4f, 0.3f, -0.3f);
+    Quatf q_canonical_1(0.7f,-0.4f, -0.3f, 0.3f);
+    Quatf q_canonical_ref_1(0.7f,-0.4f, -0.3f, 0.3f);
+    TEST(isEqual(q_non_canonical_1.canonical(),q_canonical_ref_1));
+    TEST(isEqual(q_canonical_1.canonical(),q_canonical_ref_1));
+    q_non_canonical_1.canonicalize();
+    q_canonical_1.canonicalize();
+    TEST(isEqual(q_non_canonical_1,q_canonical_ref_1));
+    TEST(isEqual(q_canonical_1,q_canonical_ref_1));
+
+    Quatf q_non_canonical_2(0.0f, -1.0f, 0.0f, 0.0f);
+    Quatf q_canonical_2(0.0f, 1.0f, 0.0f, 0.0f);
+    Quatf q_canonical_ref_2(0.0f, 1.0f, 0.0f, 0.0f);
+    TEST(isEqual(q_non_canonical_2.canonical(),q_canonical_ref_2));
+    TEST(isEqual(q_canonical_2.canonical(),q_canonical_ref_2));
+    q_non_canonical_2.canonicalize();
+    q_canonical_2.canonicalize();
+    TEST(isEqual(q_non_canonical_2,q_canonical_ref_2));
+    TEST(isEqual(q_canonical_2,q_canonical_ref_2));
+
+    Quatf q_non_canonical_3(0.0f, 0.0f, -1.0f, 0.0f);
+    Quatf q_canonical_3(0.0f, 0.0f, 1.0f, 0.0f);
+    Quatf q_canonical_ref_3(0.0f, 0.0f, 1.0f, 0.0f);
+    TEST(isEqual(q_non_canonical_3.canonical(),q_canonical_ref_3));
+    TEST(isEqual(q_canonical_3.canonical(),q_canonical_ref_3));
+    q_non_canonical_3.canonicalize();
+    q_canonical_3.canonicalize();
+    TEST(isEqual(q_non_canonical_3,q_canonical_ref_3));
+    TEST(isEqual(q_canonical_3,q_canonical_ref_3));
+
+    Quatf q_non_canonical_4(0.0f, 0.0f, 0.0f, -1.0f);
+    Quatf q_canonical_4(0.0f, 0.0f, 0.0f, 1.0f);
+    Quatf q_canonical_ref_4(0.0f, 0.0f, 0.0f, 1.0f);
+    TEST(isEqual(q_non_canonical_4.canonical(),q_canonical_ref_4));
+    TEST(isEqual(q_canonical_4.canonical(),q_canonical_ref_4));
+    q_non_canonical_4.canonicalize();
+    q_canonical_4.canonicalize();
+    TEST(isEqual(q_non_canonical_4,q_canonical_ref_4));
+    TEST(isEqual(q_canonical_4,q_canonical_ref_4));
+
+    Quatf q_non_canonical_5(0.0f, 0.0f, 0.0f, 0.0f);
+    Quatf q_canonical_5(0.0f, 0.0f, 0.0f, 0.0f);
+    Quatf q_canonical_ref_5(0.0f, 0.0f, 0.0f, 0.0f);
+    TEST(isEqual(q_non_canonical_5.canonical(),q_canonical_ref_5));
+    TEST(isEqual(q_canonical_5.canonical(),q_canonical_ref_5));
+    q_non_canonical_5.canonicalize();
+    q_canonical_5.canonicalize();
+    TEST(isEqual(q_non_canonical_5,q_canonical_ref_5));
+    TEST(isEqual(q_canonical_5,q_canonical_ref_5));
+
+    // quaternion setIdentity
+    Quatf q_nonIdentity(-0.7f, 0.4f, 0.5f, -0.3f);
+    q_nonIdentity.setIdentity();
+    TEST(isEqual(q_nonIdentity, Quatf()));
+
+    // non-unit quaternion invese
+    Quatf q_nonunit(0.1f, 0.2f, 0.3f, 0.4f);
+    TEST(isEqual(q_nonunit*q_nonunit.inversed(), Quatf()));
+
+    // rotate quaternion (nonzero rotation)
+    Vector3f rot(1.f, 0.f, 0.f);
+    Quatf q_test;
+    q_test.rotate(rot);
+    Quatf q_true(cos(1.0f / 2), sin(1.0f / 2), 0.0f, 0.0f);
+    TEST(isEqual(q_test, q_true));
+
+    // rotate quaternion (zero rotation)
+    rot(0) = rot(1) = rot(2) = 0.0f;
+    q_test = Quatf();
+    q_test.rotate(rot);
+    q_true = Quatf(cos(0.0f), sin(0.0f), 0.0f, 0.0f);
+    TEST(isEqual(q_test, q_true));
+
+    // rotate quaternion (random non-commutating rotation)
+    q = Quatf(AxisAnglef(5.1f, 3.2f, 8.4f));
+    rot = Vector3f(1.1f, 2.5f, 3.8f);
+    q.rotate(rot);
+    q_true = Quatf(0.3019f, 0.2645f, 0.2268f, 0.8874f);
+    TEST(isEqual(q, q_true));
+
+    // get rotation axis from quaternion (nonzero rotation)
+    q = Quatf(cos(1.0f / 2), 0.0f, sin(1.0f / 2), 0.0f);
+    rot = AxisAnglef(q);
+    TEST(fabs(rot(0)) < eps);
+    TEST(fabs(rot(1) - 1.0f) < eps);
+    TEST(fabs(rot(2)) < eps);
+
+    // get rotation axis from quaternion (zero rotation)
+    q = Quatf(1.0f, 0.0f, 0.0f, 0.0f);
+    rot = AxisAnglef(q);
+    TEST(fabs(rot(0)) < eps);
+    TEST(fabs(rot(1)) < eps);
+    TEST(fabs(rot(2)) < eps);
+
+    // from axis angle (zero rotation)
+    rot(0) = rot(1) = rot(2) = 0.0f;
+    q = Quatf(AxisAnglef(rot));
+    q_true = Quatf(1.0f, 0.0f, 0.0f, 0.0f);
+    TEST(isEqual(q, q_true));
+
+    // from axis angle, with length of vector the rotation
+    float n = float(sqrt(4*M_PI*M_PI/3));
+    q = AxisAnglef(n, n, n);
+    TEST(isEqual(q, Quatf(-1, 0, 0, 0)));
+    q = AxisAnglef(0, 0, 0);
+    TEST(isEqual(q, Quatf(1, 0, 0, 0)));
+
+    // Quaternion initialisation per array
+    float q_array[] = {0.9833f, -0.0343f, -0.1060f, -0.1436f};
+    Quaternion<float>q_from_array(q_array);
+
+    for (size_t i = 0; i < 4; i++) {
+        TEST(fabs(q_from_array(i) - q_array[i]) < eps);
+    }
+
+    // axis angle
+    AxisAnglef aa_true(Vector3f(1.0f, 2.0f, 3.0f));
+    TEST(isEqual(aa_true, Vector3f(1.0f, 2.0f, 3.0f)));
+    AxisAnglef aa_empty;
+    TEST(isEqual(aa_empty, AxisAnglef(0.0f, 0.0f, 0.0f)));
+    float aa_data[] =  {4.0f, 5.0f, 6.0f};
+    AxisAnglef aa_data_init(aa_data);
+    TEST(isEqual(aa_data_init, AxisAnglef(4.0f, 5.0f, 6.0f)));
+
+    AxisAnglef aa_norm_check(Vector3f(0.0f, 0.0f, 0.0f));
+    TEST(isEqual(aa_norm_check.axis(), Vector3f(1, 0, 0)));
+    TEST(isEqualF(aa_norm_check.angle(), 0.0f));
+
+    q = Quatf(-0.29555112749297824f, 0.25532186f,  0.51064372f,  0.76596558f);
+    float r_array[9] = {-0.6949206f, 0.713521f, 0.089292854f, -0.19200698f, -0.30378509f, 0.93319237f, 0.69297814f, 0.63134968f, 0.34810752f};
+    R = Dcmf(r_array);
+    TEST(isEqual(q.imag(), Vector3f(0.25532186f,  0.51064372f,  0.76596558f)));
+
+    // from dcm
+    TEST(isEqual(Quatf(R), q));
+    TEST(isEqual(Quatf(Dcmf(q)), q));
+
+    // to dcm
+    TEST(isEqual(Dcmf(q), R));
+    TEST(isEqual(Dcmf(Quatf(R)), R));
+
+    // conjugate
+    v = Vector3f(1.5f, 2.2f, 3.2f);
+    TEST(isEqual(q.conjugate_inversed(v1), Dcmf(q).T()*v1));
+    TEST(isEqual(q.conjugate(v1), Dcmf(q)*v1));
+
+    AxisAnglef aa_q_init(q);
+    TEST(isEqual(aa_q_init, AxisAnglef(1.0f, 2.0f, 3.0f)));
+
+    AxisAnglef aa_euler_init(Eulerf(0.0f, 0.0f, 0.0f));
+    TEST(isEqual(aa_euler_init, Vector3f(0.0f, 0.0f, 0.0f)));
+
+    Dcmf dcm_aa_check = AxisAnglef(dcm_check);
+    TEST(isEqual(dcm_aa_check, dcm_check));
+
+    AxisAnglef aa_axis_angle_init(Vector3f(1.0f, 2.0f, 3.0f), 3.0f);
+    TEST(isEqual(aa_axis_angle_init, Vector3f(0.80178373f, 1.60356745f, 2.40535118f)));
+    TEST(isEqual(aa_axis_angle_init.axis(), Vector3f(0.26726124f,  0.53452248f,  0.80178373f)));
+    TEST(isEqualF(aa_axis_angle_init.angle(), 3.0f));
+    TEST(isEqual(Quatf((AxisAnglef(Vector3f(0.0f, 0.0f, 1.0f), 0.0f))),
+                 Quatf(1.0f, 0.0f, 0.0f, 0.0f)));
+
+
+    // check consistentcy of quaternion and dcm product
+    Dcmf dcm3(Eulerf(1, 2, 3));
+    Dcmf dcm4(Eulerf(4, 5, 6));
+    Dcmf dcm34 = dcm3 * dcm4;
+    TEST(isEqual(Eulerf(Quatf(dcm3)*Quatf(dcm4)), Eulerf(dcm34)));
+
+    // check corner cases of matrix to quaternion conversion
+    q = Quatf(0,1,0,0); // 180 degree rotation around the x axis
+    R = Dcmf(q);
+    TEST(isEqual(q, Quatf(R)));
+    q = Quatf(0,0,1,0); // 180 degree rotation around the y axis
+    R = Dcmf(q);
+    TEST(isEqual(q, Quatf(R)));
+    q = Quatf(0,0,0,1); // 180 degree rotation around the z axis
+    R = Dcmf(q);
+    TEST(isEqual(q, Quatf(R)));
+
+#if defined(SUPPORT_STDIOSTREAM)
+    std::cout << "q:" << q;
+#endif
+    return 0;
+}
+
+/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

src/lib/matrix/test/copyto.cpp

@@ -0,0 +1,72 @@
+#include "test_macros.hpp"
+#include <matrix/math.hpp>
+
+using namespace matrix;
+
+namespace {
+void doTheCopy(const Matrix<float, 2, 3>& A, float array_A[6])
+{
+    A.copyTo(array_A);
+}
+}
+
+int main()
+{
+    float eps = 1e-6f;
+
+    // Vector3 copyTo
+    const Vector3f v(1, 2, 3);
+    float dst3[3] = {};
+    v.copyTo(dst3);
+    for (size_t i = 0; i < 3; i++) {
+        TEST(fabs(v(i) - dst3[i]) < eps);
+    }
+
+    // Quaternion copyTo
+    Quatf q(1, 2, 3, 4);
+    float dst4[4] = {};
+    q.copyTo(dst4);
+    for (size_t i = 0; i < 4; i++) {
+        TEST(fabs(q(i) - dst4[i]) < eps);
+    }
+
+    // Matrix copyTo
+    Matrix<float, 2, 3> A;
+    A(0,0) = 1;
+    A(0,1) = 2;
+    A(0,2) = 3;
+    A(1,0) = 4;
+    A(1,1) = 5;
+    A(1,2) = 6;
+    float array_A[6] = {};
+    doTheCopy(A, array_A);
+    float array_row[6] = {1, 2, 3, 4, 5, 6};
+    for (size_t i = 0; i < 6; i++) {
+        TEST(fabs(array_A[i] - array_row[i]) < eps);
+    }
+
+    // Matrix copyToColumnMajor
+    A.copyToColumnMajor(array_A);
+    float array_column[6] = {1, 4, 2, 5, 3, 6};
+    for (size_t i = 0; i < 6; i++) {
+        TEST(fabs(array_A[i] - array_column[i]) < eps);
+    }
+
+    // Slice copyTo
+    float dst5[2] = {};
+    v.slice<2,1>(0,0).copyTo(dst5);
+    for (size_t i = 0; i < 2; i++) {
+        TEST(fabs(v(i) - dst5[i]) < eps);
+    }
+
+    float subarray_A[4] = {};
+    A.slice<2,2>(0,0).copyToColumnMajor(subarray_A);
+    float subarray_column[4] = {1,4,2,5};
+    for (size_t i = 0; i < 4; i++) {
+        TEST(fabs(subarray_A[i] - subarray_column[i]) < eps);
+    }
+
+    return 0;
+}
+
+/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

src/lib/matrix/test/dual.cpp

@@ -0,0 +1,310 @@
+#include "test_macros.hpp"
+#include <matrix/math.hpp>
+#include <iostream>
+
+using namespace matrix;
+
+template <typename Scalar, size_t N>
+bool isEqualAll(Dual<Scalar, N> a, Dual<Scalar, N> b)
+{
+    return isEqualF(a.value, b.value) && a.derivative == b.derivative;
+}
+
+template <typename T>
+T testFunction(const Vector<T, 3>& point) {
+    // function is f(x,y,z) = x^2 + 2xy + 3y^2 + z
+    return point(0)*point(0)
+           + 2.f * point(0) * point(1)
+           + 3.f * point(1) * point(1)
+           + point(2);
+}
+
+template <typename Scalar>
+Vector<Scalar, 3> positionError(const Vector<Scalar, 3>& positionState,
+                                const Vector<Scalar, 3>& velocityStateBody,
+                                const Quaternion<Scalar>& bodyOrientation,
+                                const Vector<Scalar, 3>& positionMeasurement,
+                                Scalar dt
+                               )
+{
+    return positionMeasurement - (positionState +  bodyOrientation.conjugate(velocityStateBody) * dt);
+}
+
+int main()
+{
+    const Dual<float, 1> a(3,0);
+    const Dual<float, 1> b(6,0);
+
+    {
+        TEST(isEqualF(a.value, 3.f));
+        TEST(isEqualF(a.derivative(0), 1.f));
+    }
+
+    {
+        // addition
+        Dual<float, 1> c = a + b;
+        TEST(isEqualF(c.value, 9.f));
+        TEST(isEqualF(c.derivative(0), 2.f));
+
+        Dual<float, 1> d = +a;
+        TEST(isEqualAll(d, a));
+        d += b;
+        TEST(isEqualAll(d, c));
+
+        Dual<float, 1> e = a;
+        e += b.value;
+        TEST(isEqualF(e.value, c.value));
+        TEST(isEqual(e.derivative, a.derivative));
+
+        Dual<float, 1> f = b.value + a;
+        TEST(isEqualAll(f, e));
+    }
+
+    {
+        // subtraction
+        Dual<float, 1> c = b - a;
+        TEST(isEqualF(c.value, 3.f));
+        TEST(isEqualF(c.derivative(0), 0.f));
+
+        Dual<float, 1> d = b;
+        TEST(isEqualAll(d, b));
+        d -= a;
+        TEST(isEqualAll(d, c));
+
+        Dual<float, 1> e = b;
+        e -= a.value;
+        TEST(isEqualF(e.value, c.value));
+        TEST(isEqual(e.derivative, b.derivative));
+
+        Dual<float, 1> f = a.value - b;
+        TEST(isEqualAll(f, -e));
+    }
+
+    {
+        // multiplication
+        Dual<float, 1> c = a*b;
+        TEST(isEqualF(c.value, 18.f));
+        TEST(isEqualF(c.derivative(0), 9.f));
+
+        Dual<float, 1> d = a;
+        TEST(isEqualAll(d, a));
+        d *= b;
+        TEST(isEqualAll(d, c));
+
+        Dual<float, 1> e = a;
+        e *= b.value;
+        TEST(isEqualF(e.value, c.value));
+        TEST(isEqual(e.derivative, a.derivative * b.value));
+
+        Dual<float, 1> f = b.value * a;
+        TEST(isEqualAll(f, e));
+    }
+
+    {
+        // division
+        Dual<float, 1> c = b/a;
+        TEST(isEqualF(c.value, 2.f));
+        TEST(isEqualF(c.derivative(0), -1.f/3.f));
+
+        Dual<float, 1> d = b;
+        TEST(isEqualAll(d, b));
+        d /= a;
+        TEST(isEqualAll(d, c));
+
+        Dual<float, 1> e = b;
+        e /= a.value;
+        TEST(isEqualF(e.value, c.value));
+        TEST(isEqual(e.derivative, b.derivative / a.value));
+
+        Dual<float, 1> f = a.value / b;
+        TEST(isEqualAll(f, 1.f/e));
+    }
+
+    {
+        Dual<float, 1> blank;
+        TEST(isEqualF(blank.value, 0.f));
+        TEST(isEqualF(blank.derivative(0), 0.f));
+    }
+
+    {
+        // sqrt
+        TEST(isEqualF(sqrt(a).value, sqrt(a.value)));
+        TEST(isEqualF(sqrt(a).derivative(0), 1.f/sqrt(12.f)));
+    }
+
+    {
+        // abs
+        TEST(isEqualAll(a, abs(-a)));
+        TEST(!isEqualAll(-a, abs(a)));
+        TEST(isEqualAll(-a, -abs(a)));
+    }
+
+    {
+        // ceil
+        Dual<float, 1> c(1.5,0);
+        TEST(isEqualF(ceil(c).value, ceil(c.value)));
+        TEST(isEqualF(ceil(c).derivative(0), 0.f));
+    }
+
+    {
+        // floor
+        Dual<float, 1> c(1.5,0);
+        TEST(isEqualF(floor(c).value, floor(c.value)));
+        TEST(isEqualF(floor(c).derivative(0), 0.f));
+    }
+
+    {
+        // fmod
+        TEST(isEqualF(fmod(a, 0.8f).value, fmod(a.value, 0.8f)));
+        TEST(isEqual(fmod(a, 0.8f).derivative, a.derivative));
+    }
+
+    {
+        // max/min
+        TEST(isEqualAll(b, max(a, b)));
+        TEST(isEqualAll(a, min(a, b)));
+    }
+
+    {
+        // isnan
+        TEST(!IsNan(a));
+        Dual<float, 1> c(sqrt(-1.f),0);
+        TEST(IsNan(c));
+    }
+
+    {
+        // isfinite/isinf
+        TEST(IsFinite(a));
+        TEST(!IsInf(a));
+        Dual<float, 1> c(sqrt(-1.f),0);
+        TEST(!IsFinite(c));
+        TEST(!IsInf(c));
+        Dual<float, 1> d(INFINITY,0);
+        TEST(!IsFinite(d));
+        TEST(IsInf(d));
+    }
+
+    {
+        // sin/cos/tan
+        TEST(isEqualF(sin(a).value, sin(a.value)));
+        TEST(isEqualF(sin(a).derivative(0), cos(a.value))); // sin'(x) = cos(x)
+
+        TEST(isEqualF(cos(a).value, cos(a.value)));
+        TEST(isEqualF(cos(a).derivative(0), -sin(a.value))); // cos'(x) = -sin(x)
+
+        TEST(isEqualF(tan(a).value, tan(a.value)));
+        TEST(isEqualF(tan(a).derivative(0), 1.f + tan(a.value)*tan(a.value))); // tan'(x) = 1 + tan^2(x)
+    }
+
+    {
+        // asin/acos/atan
+        Dual<float, 1> c(0.3f, 0);
+        TEST(isEqualF(asin(c).value, asin(c.value)));
+        TEST(isEqualF(asin(c).derivative(0), 1.f/sqrt(1.f - 0.3f*0.3f))); // asin'(x) = 1/sqrt(1-x^2)
+
+        TEST(isEqualF(acos(c).value, acos(c.value)));
+        TEST(isEqualF(acos(c).derivative(0), -1.f/sqrt(1.f - 0.3f*0.3f))); // acos'(x) = -1/sqrt(1-x^2)
+
+        TEST(isEqualF(atan(c).value, atan(c.value)));
+        TEST(isEqualF(atan(c).derivative(0), 1.f/(1.f + 0.3f*0.3f))); // tan'(x) = 1 + x^2
+    }
+
+    {
+        // atan2
+        TEST(isEqualF(atan2(a, b).value, atan2(a.value, b.value)));
+        TEST(isEqualAll(atan2(a, Dual<float,1>(b.value)), atan(a/b.value))); // atan2'(y, x) = atan'(y/x)
+    }
+
+    {
+        // partial derivatives
+        // function is f(x,y,z) = x^2 + 2xy + 3y^2 + z, we need with respect to d/dx and d/dy at the point (0.5, -0.8, 2)
+
+        using D = Dual<float, 2>;
+
+        // set our starting point, requesting partial derivatives of x and y in column 0 and 1
+        Vector3<D> dualPoint(D(0.5f, 0), D(-0.8f, 1), D(2.f));
+
+        Dual<float, 2> dualResult = testFunction(dualPoint);
+
+        // compare to a numerical derivative:
+        Vector<float, 3> floatPoint = collectReals(dualPoint);
+        float floatResult = testFunction(floatPoint);
+
+        float h = 0.0001f;
+        Vector<float, 3> floatPoint_plusDX = floatPoint;
+        floatPoint_plusDX(0) += h;
+        float floatResult_plusDX = testFunction(floatPoint_plusDX);
+
+        Vector<float, 3> floatPoint_plusDY = floatPoint;
+        floatPoint_plusDY(1) += h;
+        float floatResult_plusDY = testFunction(floatPoint_plusDY);
+
+        Vector2f numerical_derivative((floatResult_plusDX - floatResult)/h,
+                                      (floatResult_plusDY - floatResult)/h);
+
+        TEST(isEqualF(dualResult.value, floatResult, 0.0f));
+        TEST(isEqual(dualResult.derivative, numerical_derivative, 1e-2f));
+
+    }
+
+    {
+        // jacobian
+        // get residual of x/y/z with partial derivatives of rotation
+
+        Vector3f direct_error;
+        Matrix<float, 3, 4> numerical_jacobian;
+        {
+            Vector3f positionState(5,6,7);
+            Vector3f velocityState(-1,0,1);
+            Quaternionf velocityOrientation(0.2f,-0.1f,0,1);
+            Vector3f positionMeasurement(4.5f, 6.2f, 7.9f);
+            float dt = 0.1f;
+
+            direct_error = positionError(positionState,
+                                         velocityState,
+                                         velocityOrientation,
+                                         positionMeasurement,
+                                         dt);
+            float h = 0.001f;
+            for (size_t i = 0; i < 4; i++)
+            {
+                Quaternion<float> h4 = velocityOrientation;
+                h4(i) += h;
+                numerical_jacobian.col(i) = (positionError(positionState,
+                                             velocityState,
+                                             h4,
+                                             positionMeasurement,
+                                             dt)
+                                             - direct_error)/h;
+            }
+        }
+        Vector3f auto_error;
+        Matrix<float, 3, 4> auto_jacobian;
+        {
+            using D4 = Dual<float, 4>;
+            using Vector3d4 = Vector3<D4>;
+            Vector3d4 positionState(D4(5), D4(6), D4(7));
+            Vector3d4 velocityState(D4(-1), D4(0), D4(1));
+
+            // request partial derivatives of velocity orientation
+            // by setting these variables' derivatives in corresponding columns [0...3]
+            Quaternion<D4> velocityOrientation(D4(0.2f, 0),D4(-0.1f, 1),D4(0, 2),D4(1, 3));
+
+            Vector3d4 positionMeasurement(D4(4.5f), D4(6.2f), D4(7.9f));
+            D4 dt(0.1f);
+
+
+            Vector3d4 error = positionError(positionState,
+                                            velocityState,
+                                            velocityOrientation,
+                                            positionMeasurement,
+                                            dt);
+            auto_error = collectReals(error);
+            auto_jacobian = collectDerivatives(error);
+        }
+        TEST(isEqual(direct_error, auto_error, 0.0f));
+        TEST(isEqual(numerical_jacobian, auto_jacobian, 1e-3f));
+
+    }
+    return 0;
+}

src/lib/matrix/test/filter.cpp

@@ -0,0 +1,29 @@
+#include "test_macros.hpp"
+#include <matrix/filter.hpp>
+
+using namespace matrix;
+
+int main()
+{
+    const size_t n_x = 6;
+    const size_t n_y = 5;
+    SquareMatrix<float, n_x> P = eye<float, n_x>();
+    SquareMatrix<float, n_y> R = eye<float, n_y>();
+    Matrix<float, n_y, n_x> C;
+    C.setIdentity();
+    float data[] = {1,2,3,4,5};
+    Vector<float, n_y> r(data);
+
+    Vector<float, n_x> dx;
+    SquareMatrix<float, n_x> dP;
+    float beta = 0;
+    kalman_correct<float, 6, 5>(P, C, R, r, dx, dP, beta);
+
+    float data_check[] = {0.5,1,1.5,2,2.5,0};
+    Vector<float, n_x> dx_check(data_check);
+    TEST(isEqual(dx, dx_check));
+
+    return 0;
+}
+
+/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

src/lib/matrix/test/gtest.cmake

@@ -0,0 +1,12 @@
+set_directory_properties(PROPERTIES COMPILE_OPTIONS "")
+
+# Download and unpack googletest at configure time
+configure_file(${CMAKE_CURRENT_LIST_DIR}/CMakeLists.txt.in googletest-download/CMakeLists.txt)
+execute_process(COMMAND ${CMAKE_COMMAND} -G "${CMAKE_GENERATOR}" . RESULT_VARIABLE result1 WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/googletest-download)
+execute_process(COMMAND ${CMAKE_COMMAND} --build . RESULT_VARIABLE result2 WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/googletest-download)
+if(result1 OR result2)
+	message(FATAL_ERROR "Preparing googletest failed: ${result1} ${result2}")
+endif()
+
+# Add googletest, defines gtest and gtest_main targets
+add_subdirectory(${CMAKE_CURRENT_BINARY_DIR}/googletest-src ${CMAKE_CURRENT_BINARY_DIR}/googletest-build EXCLUDE_FROM_ALL)

src/lib/matrix/test/hatvee.cpp

@@ -0,0 +1,19 @@
+#include "test_macros.hpp"
+#include <matrix/math.hpp>
+
+using namespace matrix;
+
+int main()
+{
+    Euler<float> euler(0.1f, 0.2f, 0.3f);
+    Dcm<float> R(euler);
+    Dcm<float> skew = R - R.T();
+    Vector3<float> w = skew.vee();
+    Vector3<float> w_check(0.1348f, 0.4170f, 0.5647f);
+
+    TEST(isEqual(w, w_check));
+    TEST(isEqual(skew, w.hat()));
+    return 0;
+}
+
+/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

src/lib/matrix/test/helper.cpp

@@ -0,0 +1,78 @@
+#include "test_macros.hpp"
+#include <matrix/math.hpp>
+
+using namespace matrix;
+
+int main()
+{
+    // general wraps
+    TEST(fabs(wrap(4., 0., 10.) - 4.) < FLT_EPSILON);
+    TEST(fabs(wrap(4., 0., 1.)) < FLT_EPSILON);
+    TEST(fabs(wrap(-4., 0., 10.) - 6.) < FLT_EPSILON);
+    TEST(fabs(wrap(-18., 0., 10.) - 2.) < FLT_EPSILON);
+    TEST(fabs(wrap(-1.5, 3., 5.) - 4.5) < FLT_EPSILON);
+    TEST(fabs(wrap(15.5, 3., 5.) - 3.5) < FLT_EPSILON);
+    TEST(fabs(wrap(-1., 30., 40.) - 39.) < FLT_EPSILON);
+    TEST(fabs(wrap(-8000., -555., 1.) - (-216.)) < FLT_EPSILON);
+    TEST(fabs(wrap(0., 0., 360.)) < FLT_EPSILON);
+    TEST(fabs(wrap(0. - FLT_EPSILON, 0., 360.) - (360. - FLT_EPSILON)) < FLT_EPSILON);
+    TEST(fabs(wrap(0. + FLT_EPSILON, 0., 360.) - FLT_EPSILON) < FLT_EPSILON);
+    TEST(fabs(wrap(360., 0., 360.)) < FLT_EPSILON);
+    TEST(fabs(wrap(360. - FLT_EPSILON, 0., 360.) - (360. - FLT_EPSILON)) < FLT_EPSILON);
+    TEST(fabs(wrap(360. + FLT_EPSILON, 0., 360.) - FLT_EPSILON) < FLT_EPSILON);
+
+    // integer wraps
+    TEST(wrap(-10, 0, 10) == 0);
+    TEST(wrap(-4, 0, 10) == 6);
+    TEST(wrap(0, 0, 10) == 0)
+    TEST(wrap(4, 0, 10) == 4);
+    TEST(wrap(10, 0, 10) == 0);
+
+    // wrap pi
+    TEST(fabs(wrap_pi(0.)) < FLT_EPSILON);
+    TEST(fabs(wrap_pi(4.) - (4. - M_TWOPI)) < FLT_EPSILON);
+    TEST(fabs(wrap_pi(-4.) - (-4. + M_TWOPI)) < FLT_EPSILON);
+    TEST(fabs(wrap_pi(3.) - (3.)) < FLT_EPSILON);
+    TEST(fabs(wrap_pi(100.) - (100. - 32. * M_PI)) < FLT_EPSILON);
+    TEST(fabs(wrap_pi(-100.) - (-100. + 32. * M_PI)) < FLT_EPSILON);
+    TEST(fabs(wrap_pi(-101.) - (-101. + 32. * M_PI)) < FLT_EPSILON);
+    TEST(!is_finite(wrap_pi(NAN)));
+
+    // wrap 2pi
+    TEST(fabs(wrap_2pi(0.)) < FLT_EPSILON);
+    TEST(fabs(wrap_2pi(-4.) - (-4. + 2. * M_PI)) < FLT_EPSILON);
+    TEST(fabs(wrap_2pi(3.) - (3.)) < FLT_EPSILON);
+    TEST(fabs(wrap_2pi(200.) - (200. - 31. * M_TWOPI)) < FLT_EPSILON);
+    TEST(fabs(wrap_2pi(-201.) - (-201. + 32. * M_TWOPI)) < FLT_EPSILON);
+    TEST(!is_finite(wrap_2pi(NAN)));
+
+    // Equality checks
+    TEST(isEqualF(1., 1.));
+    TEST(!isEqualF(1., 2.));
+    TEST(!isEqualF(NAN, 1.f));
+    TEST(!isEqualF(1.f, NAN));
+    TEST(!isEqualF(INFINITY, 1.f));
+    TEST(!isEqualF(1.f, INFINITY));
+    TEST(isEqualF(NAN, NAN));
+    TEST(isEqualF(NAN, -NAN));
+    TEST(isEqualF(-NAN, NAN));
+    TEST(isEqualF(INFINITY, INFINITY));
+    TEST(!isEqualF(INFINITY, -INFINITY));
+    TEST(!isEqualF(-INFINITY, INFINITY));
+    TEST(isEqualF(-INFINITY, -INFINITY));
+
+    Vector3f a(1, 2, 3);
+    Vector3f b(4, 5, 6);
+    TEST(!isEqual(a, b));
+    TEST(isEqual(a, a));
+
+    Vector3f c(1, 2, 3);
+    Vector3f d(1, 2, NAN);
+    TEST(!isEqual(c, d));
+    TEST(isEqual(c, c));
+    TEST(isEqual(d, d));
+
+    return 0;
+}
+
+/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

src/lib/matrix/test/integration.cpp

@@ -0,0 +1,26 @@
+#include "test_macros.hpp"
+#include <matrix/integration.hpp>
+
+using namespace matrix;
+
+Vector<float, 6> f(float t, const Matrix<float, 6, 1> &  /*y*/, const Matrix<float, 3, 1> &  /*u*/);
+
+Vector<float, 6> f(float t, const Matrix<float, 6, 1> &  /*y*/, const Matrix<float, 3, 1> &  /*u*/) {
+    float v = -sin(t);
+    return v*ones<float, 6, 1>();
+}
+
+int main()
+{
+    Vector<float, 6> y = ones<float, 6, 1>();
+    Vector<float, 3> u = ones<float, 3, 1>();
+    float t0 = 0;
+    float tf = 2;
+    float h = 0.001f;
+    integrate_rk4(f, y, u, t0, tf, h, y);
+    float v = 1 + cos(tf) - cos(t0);
+    TEST(isEqual(y, (ones<float, 6, 1>()*v)));
+    return 0;
+}
+
+/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

src/lib/matrix/test/inverse.cpp

@@ -0,0 +1,164 @@
+#include "test_macros.hpp"
+#include <matrix/math.hpp>
+
+using namespace matrix;
+
+static const size_t n_large = 50;
+
+int main()
+{
+    float data[9] = {0, 2, 3,
+                     4, 5, 6,
+                     7, 8, 10
+                    };
+    float data_check[9] = {
+        -0.4f, -0.8f,  0.6f,
+        -0.4f,  4.2f, -2.4f,
+        0.6f, -2.8f,  1.6f
+    };
+
+    SquareMatrix<float, 3> A(data);
+    SquareMatrix<float, 3> A_I = inv(A);
+    SquareMatrix<float, 3> A_I_check(data_check);
+    TEST((A_I - A_I_check).abs().max() < 1e-6f);
+
+    float data_2x2[4] = {12, 2,
+                         -7, 5
+                        };
+    float data_2x2_check[4] = {
+        0.0675675675f, -0.02702702f,
+        0.0945945945f, 0.162162162f
+    };
+
+    SquareMatrix<float, 2> A2x2(data_2x2);
+    SquareMatrix<float, 2> A2x2_I = inv(A2x2);
+    SquareMatrix<float, 2> A2x2_I_check(data_2x2_check);
+    TEST(isEqual(A2x2_I, A2x2_I_check));
+
+    SquareMatrix<float, 2> A2x2_sing = ones<float, 2, 2>();
+    SquareMatrix<float, 2> A2x2_sing_I;
+    TEST(inv(A2x2_sing, A2x2_sing_I) == false);
+
+    SquareMatrix<float, 3> A3x3_sing = ones<float, 3, 3>();
+    SquareMatrix<float, 3> A3x3_sing_I;
+    TEST(inv(A3x3_sing, A3x3_sing_I) == false)
+
+    // stess test
+    SquareMatrix<float, n_large> A_large;
+    A_large.setIdentity();
+    SquareMatrix<float, n_large> A_large_I;
+    A_large_I.setZero();
+
+    for (size_t i = 0; i < n_large; i++) {
+        A_large_I = inv(A_large);
+        TEST(isEqual(A_large, A_large_I));
+    }
+
+    SquareMatrix<float, 3> zero_test = zeros<float, 3, 3>();
+    TEST(isEqual(inv(zero_test), zeros<float, 3, 3>()));
+
+    // test pivotting
+    float data2[81] = {
+        -2,   1,   1,  -1,  -5,   1,   2,  -1,   0,
+        -3,   2,  -1,   0,   2,   2,  -1,  -5,   3,
+        0,   0,   0,   1,   4,  -3,   3,   0,  -2,
+        2,   2,  -1,  -2,  -1,   0,   3,   0,   1,
+        -1,   2,  -1,  -1,  -3,   3,   0,  -2,   3,
+        0,   1,   1,  -3,   3,  -2,   0,  -4,   0,
+        1,   0,   0,   0,   0,   0,  -2,   4,  -3,
+        1,  -1,   0,  -1,  -1,   1,  -1,  -3,   4,
+        0,   3,  -1,  -2,   2,   1,  -2,   0,  -1
+    };
+
+    float data2_check[81] = {
+        6, -4,   3, -3, -9, -8, -10,   8,  14,
+        -2, -7,  -5, -3, -2, -2, -16,  -5,   8,
+        -2,  0, -23,  7, -24, -5, -28, -14,   9,
+        3, -7,   2, -5,  -4, -6, -13,   4,  13,
+        -1,  4,  -8,  5,  -8,  0,  -3,  -5,  -2,
+        6,  7,  -7,  7, -21, -7,  -5,   3,   6,
+        1,  4,  -4,  4,  -7, -1,   0,  -1,  -1,
+        -7,  3, -11,  5,   1,  6,  -1, -13, -10,
+        -8,  0, -11,  3,   3,  6,  -5, -14,  -8
+    };
+    SquareMatrix<float, 9> A2(data2);
+    SquareMatrix<float, 9> A2_I = inv(A2);
+    SquareMatrix<float, 9> A2_I_check(data2_check);
+    TEST((A2_I - A2_I_check).abs().max() < 1e-3f);
+
+    float data3[9] = {
+        0, 1, 2,
+        3, 4, 5,
+        6, 7, 9
+    };
+    float data3_check[9] = {
+        -0.3333333f, -1.6666666f, 1,
+        -1, 4, -2,
+        1, -2, 1
+    };
+    SquareMatrix<float, 3> A3(data3);
+    SquareMatrix<float, 3> A3_I = inv(A3);
+    SquareMatrix<float, 3> A3_I_check(data3_check);
+    TEST(isEqual(inv(A3), A3_I_check));
+    TEST(isEqual(A3_I, A3_I_check));
+    TEST(A3.I(A3_I));
+    TEST(isEqual(A3_I, A3_I_check));
+
+    // cover singular matrices
+    A3(0, 0) = 0;
+    A3(0, 1) = 0;
+    A3(0, 2) = 0;
+    A3_I = inv(A3);
+    SquareMatrix<float, 3>  Z3 = zeros<float, 3, 3>();
+    TEST(!A3.I(A3_I));
+    TEST(!Z3.I(A3_I));
+    TEST(isEqual(A3_I, Z3));
+    TEST(isEqual(A3.I(), Z3));
+
+    for(size_t i = 0; i < 9; i++) {
+        A2(0, i) = 0;
+    }
+    A2_I = inv(A2);
+    SquareMatrix<float, 9> Z9 = zeros<float, 9, 9>();
+    TEST(!A2.I(A2_I));
+    TEST(!Z9.I(A2_I));
+    TEST(isEqual(A2_I, Z9));
+    TEST(isEqual(A2.I(), Z9));
+
+    // cover NaN
+    A3(0, 0) = NAN;
+    A3(0, 1) = 0;
+    A3(0, 2) = 0;
+    A3_I = inv(A3);
+    TEST(isEqual(A3_I, Z3));
+    TEST(isEqual(A3.I(), Z3));
+
+    A2(0, 0) = NAN;
+    A2_I = inv(A2);
+    TEST(isEqual(A2_I, Z9));
+    TEST(isEqual(A2.I(), Z9));
+
+    float data4[9] = {
+        1.33471626f,  0.74946721f, -0.0531679f,
+        0.74946721f,  1.07519593f,  0.08036323f,
+        -0.0531679f,  0.08036323f,  1.01618474f
+    };
+    SquareMatrix<float, 3> A4(data4);
+
+    float data4_cholesky[9] = {
+        1.15529921f,  0.f,  0.f,
+        0.6487213f,  0.80892311f,  0.f,
+        -0.04602089f,  0.13625271f,  0.99774847f
+    };
+    SquareMatrix<float, 3> A4_cholesky_check(data4_cholesky);
+    SquareMatrix<float, 3> A4_cholesky = cholesky(A4);
+    TEST(isEqual(A4_cholesky_check, A4_cholesky));
+
+    SquareMatrix<float, 3> I3;
+    I3.setIdentity();
+    TEST(isEqual(choleskyInv(A4)*A4, I3));
+    TEST(isEqual(cholesky(Z3), Z3));
+    return 0;
+}
+
+/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

src/lib/matrix/test/least_squares.cpp

@@ -0,0 +1,100 @@
+#include "test_macros.hpp"
+#include <matrix/math.hpp>
+
+using namespace matrix;
+
+int test_4x3(void);
+template<typename Type> int test_4x4(void);
+int test_4x4_type_double(void);
+int test_div_zero(void);
+
+int main()
+{
+    int ret;
+
+    ret = test_4x4<float>();
+    if (ret != 0) return ret;
+
+    ret = test_4x4<double>();
+    if (ret != 0) return ret;
+
+    ret = test_4x3();
+    if (ret != 0) return ret;
+
+    ret = test_div_zero();
+    if (ret != 0) return ret;
+
+    return 0;
+}
+
+int test_4x3() {
+    // Start with an (m x n) A matrix
+    float data[12] = {20.f, -10.f, -13.f,
+                      17.f,  16.f, -18.f,
+                      0.7f,  -0.8f,   0.9f,
+                      -1.f,  -1.1f,  -1.2f
+                     };
+    Matrix<float, 4, 3> A(data);
+
+    float b_data[4] = {2.0f, 3.0f, 4.0f, 5.0f};
+    Vector<float, 4> b(b_data);
+
+    float x_check_data[3] = {-0.69168233f,
+                             -0.26227593f,
+                             -1.03767522f
+                            };
+    Vector<float, 3> x_check(x_check_data);
+
+    LeastSquaresSolver<float, 4, 3> qrd = LeastSquaresSolver<float, 4, 3>(A);
+
+    Vector<float, 3> x = qrd.solve(b);
+    TEST(isEqual(x, x_check));
+    return 0;
+}
+
+template<typename Type>
+int test_4x4() {
+    // Start with an (m x n) A matrix
+    const Type data[16] = { 20.f, -10.f, -13.f,  21.f,
+                            17.f,  16.f, -18.f, -14.f,
+                            0.7f,  -0.8f,   0.9f,  -0.5f,
+                            -1.f,  -1.1f,  -1.2f,  -1.3f
+                          };
+    Matrix<Type, 4, 4> A(data);
+
+    Type b_data[4] = {2.0f, 3.0f, 4.0f, 5.0f};
+    Vector<Type, 4> b(b_data);
+
+    Type x_check_data[4] = { 0.97893433f,
+                             -2.80798701f,
+                             -0.03175765f,
+                             -2.19387649f
+                           };
+    Vector<Type, 4> x_check(x_check_data);
+
+    LeastSquaresSolver<Type, 4, 4> qrd = LeastSquaresSolver<Type, 4, 4>(A);
+
+    Vector<Type, 4> x = qrd.solve(b);
+    TEST(isEqual(x, x_check));
+    return 0;
+}
+
+int test_div_zero() {
+    float data[4] = {0.0f, 0.0f, 0.0f, 0.0f};
+    Matrix<float, 2, 2> A(data);
+
+    float b_data[2] = {1.0f, 1.0f};
+    Vector<float, 2> b(b_data);
+
+    // Implement such that x returns zeros if it reaches div by zero
+    float x_check_data[2] = {0.0f, 0.0f};
+    Vector<float, 2> x_check(x_check_data);
+
+    LeastSquaresSolver<float, 2, 2> qrd = LeastSquaresSolver<float, 2, 2>(A);
+
+    Vector<float, 2> x = qrd.solve(b);
+    TEST(isEqual(x, x_check));
+    return 0;
+}
+
+/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

src/lib/matrix/test/matrixAssignment.cpp

@@ -0,0 +1,260 @@
+#include "test_macros.hpp"
+#include <matrix/math.hpp>
+
+using namespace matrix;
+
+template class matrix::Matrix<float, 3, 2>;
+
+int main()
+{
+    Matrix3f m;
+    m.setZero();
+    m.zero();
+    m(0, 0) = 1;
+    m(0, 1) = 2;
+    m(0, 2) = 3;
+    m(1, 0) = 4;
+    m(1, 1) = 5;
+    m(1, 2) = 6;
+    m(2, 0) = 7;
+    m(2, 1) = 8;
+    m(2, 2) = 9;
+
+    float data[9] = {1, 2, 3, 4, 5, 6, 7, 8, 9};
+    Matrix3f m2(data);
+
+    for(size_t i=0; i<3; i++) {
+        for (size_t j = 0; j < 3; j++) {
+            TEST(fabs(data[i*3 + j] - m2(i,j)) < FLT_EPSILON);
+        }
+    }
+
+    Matrix3f m_nan;
+    m_nan.setNaN();
+    for(size_t i=0; i<3; i++) {
+        for (size_t j = 0; j < 3; j++) {
+            TEST(isnan(m_nan(i,j)));
+        }
+    }
+    TEST(m_nan.isAllNan());
+
+    float data2d[3][3] = {
+        {1, 2, 3},
+        {4, 5, 6},
+        {7, 8, 9}
+    };
+    m2 = Matrix3f(data2d);
+    for(size_t i=0; i<3; i++) {
+        for (size_t j = 0; j < 3; j++) {
+            TEST(fabs(data[i*3 + j] - m2(i,j)) < FLT_EPSILON);
+        }
+    }
+    TEST(!m2.isAllNan());
+
+    float data_times_2[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18};
+    Matrix3f m3(data_times_2);
+
+    TEST(isEqual(m, m2));
+    TEST(!(isEqual(m, m3)));
+
+    m2 *= 2;
+    TEST(isEqual(m2, m3));
+
+    m2 /= 2;
+    m2 -= 1;
+    float data_minus_1[9] = {0, 1, 2, 3, 4, 5, 6, 7, 8};
+    TEST(isEqual(Matrix3f(data_minus_1), m2));
+
+    m2 += 1;
+    TEST(isEqual(Matrix3f(data), m2));
+
+    m3 -= m2;
+
+    TEST(isEqual(m3, m2));
+
+    // set rows and columns to value
+    Matrix3f m2e(data2d);
+
+    float data2e_check1[3][3] = {
+        {1, 11, 3},
+        {4, 11, 6},
+        {7, 11, 9}
+    };
+    Matrix3f m2e_check1(data2e_check1);
+
+    float data2e_check2[3][3] = {
+        {1, 11, 3},
+        {4, 11, 6},
+        {0, 0, 0}
+    };
+    Matrix3f m2e_check2(data2e_check2);
+
+    m2e.setCol(1, 11);
+    TEST(isEqual(m2e, m2e_check1));
+    m2e.setRow(2, 0);
+    TEST(isEqual(m2e, m2e_check2));
+
+    float data_row_02_swap[9] = {
+        7, 8, 9,
+        4, 5, 6,
+        1, 2, 3,
+    };
+
+    float data_col_02_swap[9] = {
+        3, 2, 1,
+        6, 5, 4,
+        9, 8, 7
+    };
+
+    Matrix3f m4(data);
+
+    TEST(isEqual(-m4, m4*(-1)));
+
+    // col swap
+    m4.swapCols(0, 2);
+    TEST(isEqual(m4, Matrix3f(data_col_02_swap)));
+    m4.swapCols(0, 2);
+
+    // row swap
+    m4.swapRows(0, 2);
+    TEST(isEqual(m4, Matrix3f(data_row_02_swap)));
+    m4.swapRows(0, 2);
+
+    // swapping with same row should do nothing
+    m4.swapRows(0, 0);
+    m4.swapRows(1, 1);
+    m4.swapRows(2, 2);
+    TEST(isEqual(m4, Matrix3f(data)));
+
+    // swapping with same col should do nothing
+    m4.swapCols(0, 0);
+    m4.swapCols(1, 1);
+    m4.swapCols(2, 2);
+    TEST(isEqual(m4, Matrix3f(data)));
+
+    TEST(fabs(m4.min() - 1) < FLT_EPSILON);
+    TEST(fabs((-m4).min() + 9) < FLT_EPSILON);
+
+    Scalar<float> s = 1;
+    const Vector<float, 1> & s_vect = s;
+    TEST(fabs(s - 1) < FLT_EPSILON);
+    TEST(fabs(s_vect(0) - 1.0f) < FLT_EPSILON);
+
+    Matrix<float, 1, 1> m5 = s;
+    TEST(fabs(m5(0,0) - s) < FLT_EPSILON);
+
+    Matrix<float, 2, 2> m6;
+    m6.setRow(0, Vector2f(1, 2));
+    float m7_array[] = {1,2,0,0};
+    Matrix<float, 2, 2> m7(m7_array);
+    TEST(isEqual(m6, m7));
+    m6.setCol(0, Vector2f(3, 4));
+    float m8_array[] = {3,2,4,0};
+    Matrix<float, 2, 2> m8(m8_array);
+    TEST(isEqual(m6, m8));
+
+    m7.setNaN();
+    TEST(m7 != m8);
+
+    // min, max, constrain matrix values with scalar
+    float data_m9[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18};
+    float lower_bound = 7;
+    float upper_bound = 11;
+    float data_m9_lower_bounded[9] = {7, 7, 7, 8, 10, 12, 14, 16, 18};
+    float data_m9_upper_bounded[9] = {2, 4, 6, 8, 10, 11, 11, 11, 11};
+    float data_m9_lower_constrained[9] = {7, 7, 7, 8, 10, 11, 11, 11, 11};
+    Matrix3f m9(data_m9);
+    Matrix3f m9_lower_bounded(data_m9_lower_bounded);
+    Matrix3f m9_upper_bounded(data_m9_upper_bounded);
+    Matrix3f m9_lower_upper_constrained(data_m9_lower_constrained);
+    TEST(isEqual(max(m9, lower_bound), m9_lower_bounded));
+    TEST(isEqual(max(lower_bound, m9), m9_lower_bounded));
+    TEST(isEqual(min(m9, upper_bound), m9_upper_bounded));
+    TEST(isEqual(min(upper_bound, m9), m9_upper_bounded));
+    TEST(isEqual(constrain(m9, lower_bound, upper_bound), m9_lower_upper_constrained));
+    TEST(isEqual(constrain(m9, 8.0f, 7.0f), m_nan));
+
+    // min, max, constrain matrix values with matrix of same size
+    float data_m10[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18};
+    float data_m10_lower_bound[9] = {5, 7, 4, 8, 19, 10, 20, 16, 18};
+    float data_m10_lower_bounded_ref[9] = {5, 7, 6, 8, 19, 12, 20, 16, 18};
+    float data_m10_upper_bound[9] = {6, 4, 8, 18, 20, 11, 30, 16, 18};
+    float data_m10_upper_bounded_ref[9] = {2, 4, 6, 8, 10, 11, 14, 16, 18};
+    float data_m10_constrained_ref[9] = {5, NAN, 6, 8, 19, 11, 20, 16, 18};
+    Matrix3f m10(data_m10);
+    Matrix3f m10_lower_bound(data_m10_lower_bound);
+    Matrix3f m10_lower_bounded_ref(data_m10_lower_bounded_ref);
+    Matrix3f m10_upper_bound(data_m10_upper_bound);
+    Matrix3f m10_upper_bounded_ref(data_m10_upper_bounded_ref);
+    Matrix3f m10_constrained_ref(data_m10_constrained_ref);
+    TEST(isEqual(max(m10, m10_lower_bound), m10_lower_bounded_ref));
+    TEST(isEqual(max(m10_lower_bound, m10), m10_lower_bounded_ref));
+    TEST(isEqual(min(m10, m10_upper_bound), m10_upper_bounded_ref));
+    TEST(isEqual(min(m10_upper_bound, m9), m10_upper_bounded_ref));
+    TEST(isEqual(constrain(m10, m10_lower_bound, m10_upper_bound), m10_constrained_ref));
+
+    // min, max, constrain with NAN
+    TEST(isEqualF(matrix::typeFunction::min(5.f, NAN), 5.f));
+    TEST(isEqualF(matrix::typeFunction::min(NAN, 5.f), 5.f));
+    TEST(isEqualF(matrix::typeFunction::min(NAN, NAN), NAN));
+    TEST(isEqualF(matrix::typeFunction::max(5.f, NAN), 5.f));
+    TEST(isEqualF(matrix::typeFunction::max(NAN, 5.f), 5.f));
+    TEST(isEqualF(matrix::typeFunction::max(NAN, NAN), NAN));
+    TEST(isEqualF(matrix::typeFunction::constrain(NAN, 5.f, 6.f), NAN));
+    TEST(isEqualF(matrix::typeFunction::constrain(1.f, 5.f, 4.f), NAN));
+    TEST(isEqualF(matrix::typeFunction::constrain(6.f, NAN, 5.f), 5.f));
+    TEST(isEqualF(matrix::typeFunction::constrain(1.f, 5.f, NAN), 5.f));
+    Vector2f v1{NAN, 5.0f};
+    Vector2f v1_min = min(v1, 1.f);
+    Matrix3f m11 = min(m10_constrained_ref,NAN);
+    TEST(isEqualF(fmin(NAN, 1.f), float(v1_min(0))));
+    TEST(isEqual(m11, m10_constrained_ref));
+
+    // check write_string()
+    float comma[6] = {
+        1.f, 12345.123f,
+        12345.1228f, .1234567891011f,
+        12345678910.123456789f, 1234567891011.123456789101112f
+    };
+    Matrix<float, 3, 2> Comma(comma);
+    const size_t len = 15*2*3 + 2 + 1;
+    char buffer[len];
+    Comma.print(); // for debugging in case of failure
+    Comma.write_string(buffer, len);
+    printf("%s\n", buffer); // for debugging in case of failure
+    char output[] = "\t       1\t12345.123\n\t12345.123\t0.12345679\n\t1.2345679e+10\t1.234568e+12\n";
+    printf("%s\n", output); // for debugging in case of failure
+    for (size_t i = 0; i < len; i++) {
+        if(buffer[i] != output[i]) { // for debugging in case of failure
+            printf("%d: \"%c\" != \"%c\"", int(i), buffer[i], output[i]); // LCOV_EXCL_LINE only print on failure
+        }
+        TEST(buffer[i] == output[i]);
+        if (buffer[i] == '\0') {
+            break;
+        }
+    }
+
+    // check print()
+    // Redirect stdout
+    TEST(freopen("testoutput.txt", "w", stdout) != NULL);
+    // write
+    Comma.print();
+    fclose(stdout);
+    // read
+    FILE *fp = fopen("testoutput.txt", "r");
+    TEST(fp != nullptr);
+    TEST(!fseek(fp, 0, SEEK_SET));
+    for (size_t i = 0; i < len; i++) {
+        char c = static_cast<char>(fgetc(fp));
+        if (c == '\n') {
+            break;
+        }
+        printf("%d %d %d\n", static_cast<int>(i), output[i], c);
+        TEST(c == output[i]);
+    }
+    TEST(!fclose(fp));
+
+    return 0;
+}
+
+/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

src/lib/matrix/test/matrixMult.cpp

@@ -0,0 +1,71 @@
+#include "test_macros.hpp"
+#include <matrix/math.hpp>
+
+using namespace matrix;
+
+int main()
+{
+    float data[9] = {1, 0, 0, 0, 1, 0, 1, 0, 1};
+    Matrix3f A(data);
+
+    float data_check[9] = {1, 0, 0, 0, 1, 0, -1, 0, 1};
+    Matrix3f A_I(data_check);
+    Matrix3f I;
+    I.setIdentity();
+    Matrix3f R = A * A_I;
+    TEST(isEqual(R, I));
+
+    Matrix3f R2 = A;
+    R2 *= A_I;
+    TEST(isEqual(R2, I));
+
+    TEST(R2==I);
+    TEST(A!=A_I);
+    Matrix3f A2 = eye<float, 3>()*2;
+    Matrix3f B = A2.emult(A2);
+    Matrix3f B_check = eye<float, 3>()*4;
+    Matrix3f C_check = eye<float, 3>()*2;
+    TEST(isEqual(B, B_check));
+    Matrix3f C = B_check.edivide(C_check);
+
+    float off_diagonal_nan[9] = {2, NAN, NAN, NAN, 2, NAN, NAN, NAN, 2};
+    // off diagonal are NANs because division by 0
+    TEST(C == Matrix3f(off_diagonal_nan));
+
+    // Test non-square matrix
+    float data_43[12] = {1,3,2,
+                         2,2,1,
+                         5,2,1,
+                         2,3,4
+                        };
+    float data_32[6] = {2,3,
+                        1,7,
+                        5,4
+                       };
+
+    Matrix<float, 4, 3> m43(data_43);
+    Matrix<float, 3, 2> m32(data_32);
+
+    Matrix<float, 4, 2> m42 = m43 * m32;
+
+    float data_42[8] = {15,32,
+                        11,24,
+                        17,33,
+                        27,43
+                       };
+    Matrix<float, 4, 2> m42_check(data_42);
+    TEST(isEqual(m42, m42_check))
+
+    float data_42_plus2[8] = {17,34,
+                              13,26,
+                              19,35,
+                              29,45
+                             };
+    Matrix<float, 4, 2> m42_plus2_check(data_42_plus2);
+    Matrix<float, 4, 2> m42_plus2 = m42 - (-2);
+    TEST(isEqual(m42_plus2, m42_plus2_check));
+
+    return 0;
+}
+
+/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

src/lib/matrix/test/matrixScalarMult.cpp

@@ -0,0 +1,18 @@
+#include "test_macros.hpp"
+
+#include <matrix/math.hpp>
+
+using namespace matrix;
+
+int main()
+{
+    float data[9] = {1, 2, 3, 4, 5, 6, 7, 8, 9};
+    Matrix3f A(data);
+    A = A * 2;
+    float data_check[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18};
+    Matrix3f A_check(data_check);
+    TEST(isEqual(A, A_check));
+    return 0;
+}
+
+/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

src/lib/matrix/test/pseudoInverse.cpp

@@ -0,0 +1,153 @@
+#include "test_macros.hpp"
+#include <matrix/PseudoInverse.hpp>
+
+using namespace matrix;
+
+static const size_t n_large = 20;
+
+int main()
+{
+    // 3x4 Matrix test
+    float data0[12] = {
+        0.f, 1.f,  2.f,  3.f,
+        4.f, 5.f,  6.f,  7.f,
+        8.f, 9.f, 10.f, 11.f
+    };
+
+    float data0_check[12] = {
+        -0.3375f, -0.1f,  0.1375f,
+        -0.13333333f, -0.03333333f,  0.06666667f,
+        0.07083333f,  0.03333333f, -0.00416667f,
+        0.275f,  0.1f, -0.075f
+    };
+
+    Matrix<float, 3, 4> A0(data0);
+    Matrix<float, 4, 3> A0_I;
+    bool ret = geninv(A0, A0_I);
+    TEST(ret);
+    Matrix<float, 4, 3> A0_I_check(data0_check);
+
+    TEST((A0_I - A0_I_check).abs().max() < 1e-5);
+
+    // 4x3 Matrix test
+    float data1[12] = {
+        0.f, 4.f, 8.f,
+        1.f, 5.f, 9.f,
+        2.f, 6.f, 10.f,
+        3.f, 7.f, 11.f
+    };
+
+    float data1_check[12] = {
+        -0.3375f, -0.13333333f,  0.07083333f,  0.275f,
+        -0.1f, -0.03333333f,  0.03333333f,  0.1f,
+        0.1375f,  0.06666667f, -0.00416667f, -0.075f
+    };
+
+    Matrix<float, 4, 3> A1(data1);
+    Matrix<float, 3, 4> A1_I;
+    ret = geninv(A1, A1_I);
+    TEST(ret);
+    Matrix<float, 3, 4> A1_I_check(data1_check);
+
+    TEST((A1_I - A1_I_check).abs().max() < 1e-5);
+
+    // Stess test
+    Matrix<float, n_large, n_large - 1> A_large;
+    A_large.setIdentity();
+    Matrix<float, n_large - 1, n_large> A_large_I;
+
+    for (size_t i = 0; i < n_large; i++) {
+        ret = geninv(A_large, A_large_I);
+        TEST(ret);
+        TEST(isEqual(A_large, A_large_I.T()));
+    }
+
+    // Square matrix test
+    float data2[9] = {0, 2, 3,
+                      4, 5, 6,
+                      7, 8, 10
+                     };
+    float data2_check[9] = {
+        -0.4f, -0.8f,  0.6f,
+        -0.4f,  4.2f, -2.4f,
+        0.6f, -2.8f,  1.6f
+    };
+
+    SquareMatrix<float, 3> A2(data2);
+    SquareMatrix<float, 3> A2_I;
+    ret = geninv(A2, A2_I);
+    TEST(ret);
+    SquareMatrix<float, 3> A2_I_check(data2_check);
+    TEST((A2_I - A2_I_check).abs().max() < 1e-3);
+
+    // Null matrix test
+    Matrix<float, 6, 16> A3;
+    Matrix<float, 16, 6> A3_I;
+    ret = geninv(A3, A3_I);
+    TEST(ret);
+    Matrix<float, 16, 6> A3_I_check;
+    TEST((A3_I - A3_I_check).abs().max() < 1e-5);
+
+    // Mock-up effectiveness matrix
+    const float B_quad_w[6][16] = {
+        {-0.5717536f,  0.43756646f,  0.5717536f, -0.43756646f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
+        { 0.35355328f, -0.35355328f,  0.35355328f, -0.35355328f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
+        { 0.28323701f,  0.28323701f, -0.28323701f, -0.28323701f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
+        { 0.f,  0.f,  0.f,  0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
+        { 0.f,  0.f,  0.f,  0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
+        {-0.25f, -0.25f, -0.25f, -0.25f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}
+    };
+    Matrix<float, 6, 16> B = Matrix<float, 6, 16>(B_quad_w);
+    const float A_quad_w[16][6] = {
+        { -0.495383f,  0.707107f,  0.765306f,  0.0f, 0.0f, -1.000000f },
+        {  0.495383f, -0.707107f,  1.000000f,  0.0f, 0.0f, -1.000000f },
+        {  0.495383f,  0.707107f, -0.765306f,  0.0f, 0.0f, -1.000000f },
+        { -0.495383f, -0.707107f, -1.000000f,  0.0f, 0.0f, -1.000000f },
+        { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
+        { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
+        { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
+        { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
+        { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
+        { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
+        { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
+        { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
+        { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
+        { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
+        { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
+        { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}
+    };
+    Matrix<float, 16, 6> A_check = Matrix<float, 16, 6>(A_quad_w);
+    Matrix<float, 16, 6> A;
+    ret = geninv(B, A);
+    TEST(ret);
+    TEST((A - A_check).abs().max() < 1e-5);
+
+    // Real-world test case
+    const float real_alloc[5][6] = {
+        { 0.794079,  0.794079,  0.794079,  0.794079,  0.0000,  0.0000},
+        { 0.607814,  0.607814,  0.607814,  0.607814,  1.0000,  1.0000},
+        {-0.672516,  0.915642, -0.915642,  0.672516,  0.0000,  0.0000},
+        { 0.159704,  0.159704,  0.159704,  0.159704, -0.2500, -0.2500},
+        { 0.607814, -0.607814,  0.607814, -0.607814,  1.0000,  1.0000}
+    };
+    Matrix<float, 5, 6> real ( real_alloc);
+    Matrix<float, 6, 5> real_pinv;
+    ret = geninv(real, real_pinv);
+    TEST(ret);
+
+    // from SVD-based inverse
+    const float real_pinv_expected_alloc[6][5] = {
+        { 2.096205,  -2.722267,   2.056547,   1.503279,   3.098087},
+        { 1.612621,  -1.992694,   2.056547,   1.131090,   2.275467},
+        {-1.062688,   2.043479,  -2.056547,  -0.927950,  -2.275467},
+        {-1.546273,   2.773052,  -2.056547,  -1.300139,  -3.098087},
+        {-0.293930,   0.443445,   0.000000,  -0.226222,   0.000000},
+        {-0.293930,   0.443445,   0.000000,  -0.226222,   0.000000}
+    };
+    Matrix<float, 6, 5> real_pinv_expected(real_pinv_expected_alloc);
+    TEST((real_pinv - real_pinv_expected).abs().max() < 1e-4);
+
+    return 0;
+}
+
+/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

src/lib/matrix/test/setIdentity.cpp

@@ -0,0 +1,39 @@
+#include "test_macros.hpp"
+#include <matrix/math.hpp>
+
+using namespace matrix;
+
+int main()
+{
+    Matrix3f A;
+    A.setIdentity();
+
+    for (size_t i = 0; i < 3; i++) {
+        for (size_t j = 0; j < 3; j++) {
+            if (i == j) {
+                TEST(fabs(A(i, j) -  1) < FLT_EPSILON);
+
+            } else {
+                TEST(fabs(A(i, j) -  0) < FLT_EPSILON);
+            }
+        }
+    }
+
+    Matrix3f B;
+    B.identity();
+
+    for (size_t i = 0; i < 3; i++) {
+        for (size_t j = 0; j < 3; j++) {
+            if (i == j) {
+                TEST(fabs(B(i, j) -  1) < FLT_EPSILON);
+
+            } else {
+                TEST(fabs(B(i, j) -  0) < FLT_EPSILON);
+            }
+        }
+    }
+
+    return 0;
+}
+
+/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

src/lib/matrix/test/slice.cpp

@@ -0,0 +1,235 @@
+#include "test_macros.hpp"
+#include <matrix/math.hpp>
+
+using namespace matrix;
+
+template class matrix::Slice<float, 2, 3, 4, 5>; // so that we get full coverage results
+
+
+int main()
+{
+    float data[9] = {0, 2, 3,
+                     4, 5, 6,
+                     7, 8, 10
+                    };
+    SquareMatrix<float, 3> A(data);
+
+    // Test row slicing
+    Matrix<float, 2, 3> B_rowslice(A.slice<2, 3>(1, 0));
+    float data_check_rowslice[6] = {
+        4, 5, 6,
+        7, 8, 10
+    };
+    Matrix<float, 2, 3> B_check_rowslice(data_check_rowslice);
+    TEST(isEqual(B_rowslice, B_check_rowslice));
+
+    // Test column slicing
+    Matrix<float, 3, 2> B_colslice(A.slice<3, 2>(0, 1));
+    float data_check_colslice[6] = {
+        2, 3,
+        5, 6,
+        8, 10
+    };
+    Matrix<float, 3, 2> B_check_colslice(data_check_colslice);
+    TEST(isEqual(B_colslice, B_check_colslice));
+
+    // Test slicing both
+    Matrix<float, 2, 2> B_bothslice(A.slice<2, 2>(1, 1));
+    float data_check_bothslice[4] = {
+        5, 6,
+        8, 10
+    };
+    Matrix<float, 2, 2> B_check_bothslice(data_check_bothslice);
+    TEST(isEqual(B_bothslice, B_check_bothslice));
+
+    //Test block writing
+    float data_2[4] = {
+        11, 12,
+        13, 14
+    };
+
+    Matrix<float, 2, 2> C(data_2);
+    A.slice<2, 2>(1, 1) = C;
+
+    float data_2_check[9] = {
+        0, 2, 3,
+        4, 11, 12,
+        7, 13, 14
+    };
+    Matrix<float, 3, 3> D(data_2_check);
+    TEST(isEqual(A, D));
+
+    //Test writing to slices
+    Matrix<float, 3, 1> E;
+    E(0,0) = -1;
+    E(1,0) = 1;
+    E(2,0) = 3;
+
+    Matrix<float, 2, 1> F;
+    F(0,0) = 9;
+    F(1,0) = 11;
+
+    E.slice<2,1>(0,0) = F;
+
+    float data_3_check[3] = {9, 11, 3};
+    Matrix<float, 3, 1> G (data_3_check);
+    TEST(isEqual(E, G));
+    TEST(isEqual(E, Matrix<float,3,1>(E.slice<3,1>(0,0))));
+
+    Matrix<float, 2, 1> H = E.slice<2,1>(0,0);
+    TEST(isEqual(H,F));
+
+    float data_4_check[5] = {3, 11, 9, 0, 0};
+    {   // assigning row slices to each other
+        const Matrix<float, 3, 1> J (data_3_check);
+        Matrix<float, 5, 1> K;
+        K.row(2) = J.row(0);
+        K.row(1) = J.row(1);
+        K.row(0) = J.row(2);
+
+        Matrix<float, 5, 1> K_check(data_4_check);
+        TEST(isEqual(K, K_check));
+    }
+    {   // assigning col slices to each other
+        const Matrix<float, 1, 3> J (data_3_check);
+        Matrix<float, 1, 5> K;
+        K.col(2) = J.col(0);
+        K.col(1) = J.col(1);
+        K.col(0) = J.col(2);
+
+        Matrix<float, 1, 5> K_check(data_4_check);
+        TEST(isEqual(K, K_check));
+    }
+
+    // check that slice of a slice works for reading
+    const Matrix<float, 3, 3> cm33(data);
+    Matrix<float, 2, 1> topRight = cm33.slice<2,3>(0,0).slice<2,1>(0,2);
+    float top_right_check[2] = {3,6};
+    TEST(isEqual(topRight, Matrix<float, 2, 1>(top_right_check)));
+
+    // check that slice of a slice works for writing
+    Matrix<float, 3, 3> m33(data);
+    m33.slice<2,3>(0,0).slice<2,1>(0,2) = Matrix<float, 2, 1>();
+    const float data_check[9] = {0, 2, 0,
+                                 4, 5, 0,
+                                 7, 8, 10
+                                };
+    TEST(isEqual(m33, Matrix<float, 3, 3>(data_check)));
+
+    // longerThan
+    Vector3f v5;
+    v5(0) = 3;
+    v5(1) = 4;
+    v5(2) = 9;
+    TEST(v5.xy().longerThan(4.99f));
+    TEST(!v5.xy().longerThan(5.f));
+    TEST(isEqualF(5.f, v5.xy().norm()));
+
+    // min/max
+    TEST(m33.row(1).max() == 5);
+    TEST(m33.col(0).min() == 0);
+    TEST((m33.slice<2,2>(1,1).max()) == 10);
+
+    // assign scalar value to slice
+    Matrix<float, 3, 1> L;
+    L(0,0) = -1;
+    L(1,0) = 1;
+    L(2,0) = 3;
+
+    L.slice<2,1>(0,0) = 0.0f;
+
+    float data_5_check[3] = {0, 0, 3};
+    Matrix<float, 3, 1> M (data_5_check);
+    TEST(isEqual(L, M));
+
+    // return diagonal elements
+    float data_6[9] = {0, 2, 3,
+                       4, 5, 6,
+                       7, 8, 10
+                      };
+    SquareMatrix<float, 3> N(data_6);
+
+    Vector3f v6 = N.slice<3,3>(0,0).diag();
+    Vector3f v6_check = {0, 5, 10};
+    TEST(isEqual(v6,v6_check));
+    Vector2f v7 = N.slice<2,3>(1,0).diag();
+    Vector2f v7_check = {4, 8};
+    TEST(isEqual(v7,v7_check));
+    Vector2f v8 = N.slice<3,2>(0,1).diag();
+    Vector2f v8_check = {2, 6};
+    TEST(isEqual(v8,v8_check));
+    Vector2f v9(N.slice<1,2>(1,1));
+    Vector2f v9_check = {5, 6};
+    TEST(isEqual(v9,v9_check));
+    Vector3f v10(N.slice<1,3>(1,0));
+    Vector3f v10_check = {4, 5, 6};
+    TEST(isEqual(v10,v10_check));
+
+    // Different assignment operators
+    SquareMatrix3f O(data);
+    float operand_data [4] = {2, 1, -3, -1};
+    const SquareMatrix<float, 2> operand(operand_data);
+
+    O.slice<2,2>(1,0) += operand;
+    float O_check_data_1 [9] = {0, 2, 3,  6, 6, 6,  4, 7, 10};
+    TEST(isEqual(O, SquareMatrix3f(O_check_data_1)));
+
+    O = SquareMatrix3f(data);
+    O.slice<2,1>(1,1) += operand.slice<2,1>(0,0);
+    float O_check_data_2 [9] = {0, 2, 3,  4, 7, 6,  7, 5, 10};
+    TEST(isEqual(O, SquareMatrix3f(O_check_data_2)));
+
+    O = SquareMatrix3f(data);
+    O.slice<3,3>(0,0) += -1;
+    float O_check_data_3 [9] = {-1, 1, 2,  3, 4, 5,  6, 7, 9};
+    TEST(isEqual(O, SquareMatrix3f(O_check_data_3)));
+
+    O = SquareMatrix3f(data);
+    O.col(1) += Vector3f{1, -2, 3};
+    float O_check_data_4 [9] = {0, 3, 3,  4, 3, 6,  7, 11, 10};
+    TEST(isEqual(O, SquareMatrix3f(O_check_data_4)));
+
+    O = SquareMatrix3f(data);
+    O.slice<2,2>(1,0) -= operand;
+    float O_check_data_5 [9] = {0, 2, 3,  2, 4, 6,  10, 9, 10};
+    TEST(isEqual(O, SquareMatrix3f(O_check_data_5)));
+
+    O = SquareMatrix3f(data);
+    O.slice<2,1>(1,1) -= operand.slice<2,1>(0,0);
+    float O_check_data_6 [9] = {0, 2, 3,  4, 3, 6,  7, 11, 10};
+    TEST(isEqual(O, SquareMatrix3f(O_check_data_6)));
+
+    O = SquareMatrix3f(data);
+    O.slice<3,3>(0,0) -= -1;
+    float O_check_data_7 [9] = {1, 3, 4,  5, 6, 7,  8, 9, 11};
+    TEST(isEqual(O, SquareMatrix3f(O_check_data_7)));
+
+    O = SquareMatrix3f(data);
+    O.col(1) -= Vector3f{1, -2, 3};
+    float O_check_data_8 [9] = {0, 1, 3,  4, 7, 6,  7, 5, 10};
+    TEST(isEqual(O, SquareMatrix3f(O_check_data_8)));
+
+    O = SquareMatrix3f(data);
+    O.slice<2,1>(1,1) *= 5.f;
+    float O_check_data_9 [9] = {0, 2, 3,  4, 25, 6,  7, 40, 10};
+    TEST(isEqual(O, SquareMatrix3f(O_check_data_9)));
+
+    O = SquareMatrix3f(data);
+    O.slice<2,1>(1,1) /= 2.f;
+    float O_check_data_10 [9] = {0, 2, 3,  4, 2.5, 6,  7, 4, 10};
+    TEST(isEqual(O, SquareMatrix3f(O_check_data_10)));
+
+    // Different operations
+    O = SquareMatrix3f(data);
+    SquareMatrix<float, 2> res_11(O.slice<2,2>(1,1) * 2.f);
+    float O_check_data_11 [4] = {10, 12, 16, 20};
+    TEST(isEqual(res_11, SquareMatrix<float, 2>(O_check_data_11)));
+
+    O = SquareMatrix3f(data);
+    SquareMatrix<float, 2> res_12(O.slice<2,2>(1,1) / 2.f);
+    float O_check_data_12 [4] = {2.5, 3, 4, 5};
+    TEST(isEqual(res_12, SquareMatrix<float, 2>(O_check_data_12)));
+    return 0;
+}
+
+/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

src/lib/matrix/test/sparseVector.cpp

@@ -0,0 +1,119 @@
+#include <matrix/math.hpp>
+#include <gtest/gtest.h>
+
+using namespace matrix;
+
+TEST(sparseVectorTest, defaultConstruction) {
+    SparseVectorf<24, 4, 6> a;
+    EXPECT_EQ(a.non_zeros(), 2);
+    EXPECT_EQ(a.index(0), 4);
+    EXPECT_EQ(a.index(1), 6);
+    a.at<4>() = 1.f;
+    a.at<6>() = 2.f;
+}
+
+TEST(sparseVectorTest, initializationWithData) {
+    const float data[3] = {1.f, 2.f, 3.f};
+    SparseVectorf<24, 4, 6, 22> a(data);
+    EXPECT_EQ(a.non_zeros(), 3);
+    EXPECT_EQ(a.index(0), 4);
+    EXPECT_EQ(a.index(1), 6);
+    EXPECT_EQ(a.index(2), 22);
+    EXPECT_FLOAT_EQ(a.at<4>(), data[0]);
+    EXPECT_FLOAT_EQ(a.at<6>(), data[1]);
+    EXPECT_FLOAT_EQ(a.at<22>(), data[2]);
+}
+
+TEST(sparseVectorTest, initialisationFromVector) {
+    const Vector3f vec(1.f, 2.f, 3.f);
+    const SparseVectorf<3, 0, 2> a(vec);
+    EXPECT_FLOAT_EQ(a.at<0>(), vec(0));
+    EXPECT_FLOAT_EQ(a.at<2>(), vec(2));
+}
+
+TEST(sparseVectorTest, accessDataWithCompressedIndices) {
+    const Vector3f vec(1.f, 2.f, 3.f);
+    SparseVectorf<3, 0, 2> a(vec);
+    for (size_t i = 0; i < a.non_zeros(); i++) {
+        a.atCompressedIndex(i) = static_cast<float>(i);
+    }
+    EXPECT_FLOAT_EQ(a.at<0>(), a.atCompressedIndex(0));
+    EXPECT_FLOAT_EQ(a.at<2>(), a.atCompressedIndex(1));
+}
+
+TEST(sparseVectorTest, setZero) {
+    const float data[3] = {1.f, 2.f, 3.f};
+    SparseVectorf<24, 4, 6, 22> a(data);
+    a.setZero();
+    EXPECT_FLOAT_EQ(a.at<4>(), 0.f);
+    EXPECT_FLOAT_EQ(a.at<6>(), 0.f);
+    EXPECT_FLOAT_EQ(a.at<22>(), 0.f);
+}
+
+TEST(sparseVectorTest, additionWithDenseVector) {
+    Vector<float, 4> dense_vec;
+    dense_vec.setAll(1.f);
+    const float data[3] = {1.f, 2.f, 3.f};
+    const SparseVectorf<4, 1, 2, 3> sparse_vec(data);
+    const Vector<float, 4> res = sparse_vec + dense_vec;
+    EXPECT_FLOAT_EQ(res(0), 1.f);
+    EXPECT_FLOAT_EQ(res(1), 2.f);
+    EXPECT_FLOAT_EQ(res(2), 3.f);
+    EXPECT_FLOAT_EQ(res(3), 4.f);
+}
+
+TEST(sparseVectorTest, addScalar) {
+    const float data[3] = {1.f, 2.f, 3.f};
+    SparseVectorf<4, 1, 2, 3> sparse_vec(data);
+    sparse_vec += 2.f;
+    EXPECT_FLOAT_EQ(sparse_vec.at<1>(), 3.f);
+    EXPECT_FLOAT_EQ(sparse_vec.at<2>(), 4.f);
+    EXPECT_FLOAT_EQ(sparse_vec.at<3>(), 5.f);
+}
+
+TEST(sparseVectorTest, dotProductWithDenseVector) {
+    Vector<float, 4> dense_vec;
+    dense_vec.setAll(3.f);
+    const float data[3] = {1.f, 2.f, 3.f};
+    const SparseVectorf<4, 1, 2, 3> sparse_vec(data);
+    float res = sparse_vec.dot(dense_vec);
+    EXPECT_FLOAT_EQ(res, 18.f);
+}
+
+TEST(sparseVectorTest, multiplicationWithDenseMatrix) {
+    Matrix<float, 2, 3> dense_matrix;
+    dense_matrix.setAll(2.f);
+    dense_matrix(1, 1) = 3.f;
+    const Vector3f dense_vec(0.f, 1.f, 5.f);
+    const SparseVectorf<3, 1, 2> sparse_vec(dense_vec);
+    const Vector<float, 2> res_sparse = dense_matrix * sparse_vec;
+    const Vector<float, 2> res_dense = dense_matrix * dense_vec;
+    EXPECT_TRUE(isEqual(res_dense, res_sparse));
+}
+
+TEST(sparseVectorTest, quadraticForm) {
+    float matrix_data[9] = {1, 2, 3,
+                            2, 4, 5,
+                            3, 5, 6
+                           };
+    const SquareMatrix<float, 3> dense_matrix(matrix_data);
+    const Vector3f dense_vec(0.f, 1.f, 5.f);
+    const SparseVectorf<3, 1, 2> sparse_vec(dense_vec);
+    EXPECT_FLOAT_EQ(quadraticForm(dense_matrix, sparse_vec), 204.f);
+}
+
+TEST(sparseVectorTest, norms) {
+    const float data[2] = {3.f, 4.f};
+    const SparseVectorf<4, 1, 3> sparse_vec(data);
+    EXPECT_FLOAT_EQ(sparse_vec.norm_squared(), 25.f);
+    EXPECT_FLOAT_EQ(sparse_vec.norm(), 5.f);
+    EXPECT_TRUE(sparse_vec.longerThan(4.5f));
+    EXPECT_FALSE(sparse_vec.longerThan(5.5f));
+}
+
+
+int main(int argc, char **argv) {
+    testing::InitGoogleTest(&argc, argv);
+    std::cout << "Run SparseVector tests" << std::endl;
+    return RUN_ALL_TESTS();
+}

src/lib/matrix/test/squareMatrix.cpp

@@ -0,0 +1,148 @@
+#include "test_macros.hpp"
+
+#include <matrix/math.hpp>
+
+using namespace matrix;
+
+int main()
+{
+    float data[9] = {1, 2, 3,
+                     4, 5, 6,
+                     7, 8, 10
+                    };
+    SquareMatrix<float, 3> A(data);
+    Vector3<float> diag_check(1, 5, 10);
+
+    TEST(isEqual(A.diag(), diag_check));
+    TEST(A.trace() - 16 < FLT_EPSILON);
+
+    float data_check[9] = {
+        1.01158503f,  0.02190432f,  0.03238144f,
+        0.04349195f,  1.05428524f,  0.06539627f,
+        0.07576783f,  0.08708946f,  1.10894048f
+    };
+
+    float dt = 0.01f;
+    SquareMatrix<float, 3> eA = expm(SquareMatrix<float, 3>(A*dt), 5);
+    SquareMatrix<float, 3> eA_check(data_check);
+    TEST((eA - eA_check).abs().max() < 1e-3f);
+
+    SquareMatrix<float, 2> A_bottomright = A.slice<2,2>(1,1);
+    SquareMatrix<float, 2> A_bottomright2;
+    A_bottomright2 = A.slice<2,2>(1,1);
+
+    float data_bottomright[4] = {5, 6,
+                                 8, 10
+                                };
+    SquareMatrix<float, 2> bottomright_check(data_bottomright);
+    TEST(isEqual(A_bottomright, bottomright_check));
+    TEST(isEqual(A_bottomright2, bottomright_check));
+
+    // test diagonal functions
+    float data_4x4[16] = {1, 2, 3, 4,
+                          5, 6, 7, 8,
+                          9, 10, 11, 12,
+                          13, 14,15, 16
+                         };
+
+    SquareMatrix<float, 4> B(data_4x4);
+    B.uncorrelateCovariance<1>(1);
+    float data_B_check[16] = {1, 0, 3, 4,
+                              0, 6, 0, 0,
+                              9, 0, 11, 12,
+                              13, 0,15, 16
+                             };
+    SquareMatrix<float, 4> B_check(data_B_check);
+    TEST(isEqual(B, B_check))
+
+    SquareMatrix<float, 4> C(data_4x4);
+    C.uncorrelateCovariance<2>(1);
+    float data_C_check[16] = {1, 0, 0, 4,
+                              0, 6, 0, 0,
+                              0, 0, 11, 0,
+                              13, 0,0, 16
+                             };
+    SquareMatrix<float, 4> C_check(data_C_check);
+    TEST(isEqual(C, C_check))
+
+    SquareMatrix<float, 4> D(data_4x4);
+    D.uncorrelateCovarianceSetVariance<2>(0, Vector2f{20,21});
+    float data_D_check[16] = {20, 0, 0, 0,
+                              0, 21, 0, 0,
+                              0, 0, 11, 12,
+                              0, 0,15, 16
+                             };
+    SquareMatrix<float, 4> D_check(data_D_check);
+    TEST(isEqual(D, D_check))
+
+    SquareMatrix<float, 4> E(data_4x4);
+    E.uncorrelateCovarianceSetVariance<3>(1, 33);
+    float data_E_check[16] = {1, 0, 0, 0,
+                              0, 33, 0, 0,
+                              0, 0, 33, 0,
+                              0, 0,0, 33
+                             };
+    SquareMatrix<float, 4> E_check(data_E_check);
+    TEST(isEqual(E, E_check))
+
+    // test symmetric functions
+    SquareMatrix<float, 4> F(data_4x4);
+    F.makeBlockSymmetric<2>(1);
+    float data_F_check[16] = {1, 2, 3, 4,
+                              5, 6, 8.5, 8,
+                              9, 8.5, 11, 12,
+                              13, 14,15, 16
+                             };
+    SquareMatrix<float, 4> F_check(data_F_check);
+    TEST(isEqual(F, F_check))
+    TEST(F.isBlockSymmetric<2>(1));
+    TEST(!F.isRowColSymmetric<2>(1));
+
+    SquareMatrix<float, 4> G(data_4x4);
+    G.makeRowColSymmetric<2>(1);
+    float data_G_check[16] = {1, 3.5, 6, 4,
+                              3.5, 6, 8.5, 11,
+                              6, 8.5, 11, 13.5,
+                              13, 11,13.5, 16
+                             };
+    SquareMatrix<float, 4> G_check(data_G_check);
+    TEST(isEqual(G, G_check));
+    TEST(G.isBlockSymmetric<2>(1));
+    TEST(G.isRowColSymmetric<2>(1));
+
+    SquareMatrix<float, 4> H(data_4x4);
+    H.makeBlockSymmetric<1>(1);
+    float data_H_check[16] = {1, 2, 3, 4,
+                              5, 6, 7, 8,
+                              9, 10, 11, 12,
+                              13, 14,15, 16
+                             };
+    SquareMatrix<float, 4> H_check(data_H_check);
+    TEST(isEqual(H, H_check))
+    TEST(H.isBlockSymmetric<1>(1));
+    TEST(!H.isRowColSymmetric<1>(1));
+
+    SquareMatrix<float, 4> J(data_4x4);
+    J.makeRowColSymmetric<1>(1);
+    float data_J_check[16] = {1, 3.5, 3, 4,
+                              3.5, 6, 8.5, 11,
+                              9, 8.5, 11, 12,
+                              13, 11,15, 16
+                             };
+    SquareMatrix<float, 4> J_check(data_J_check);
+    TEST(isEqual(J, J_check));
+    TEST(J.isBlockSymmetric<1>(1));
+    TEST(J.isRowColSymmetric<1>(1));
+    TEST(!J.isBlockSymmetric<3>(1));
+
+    float data_K[16] = {1, 2, 3, 4,
+                        2, 3, 4, 11,
+                        3, 4, 11, 12,
+                        4, 11,15, 16
+                       };
+    SquareMatrix<float, 4> K(data_K);
+    TEST(!K.isRowColSymmetric<1>(2));
+    return 0;
+}
+
+/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

src/lib/matrix/test/test_data.py

@@ -0,0 +1,149 @@
+from __future__ import print_function
+from pylab import *
+from pprint import pprint
+import scipy.linalg
+
+# test cases, derived from doc/nasa_rotation_def.pdf
+
+#pylint: disable=all
+
+def euler_to_quat(phi, theta, psi):
+    "Quaternion from (body 3(psi)-2(theta)-1(phi) euler angles"
+    s1 = sin(psi/2)
+    c1 = cos(psi/2)
+    s2 = sin(theta/2)
+    c2 = cos(theta/2)
+    s3 = sin(phi/2)
+    c3 = cos(phi/2)
+    return array([
+        s1*s2*s3 + c1*c2*c3,
+        -s1*s2*c3 + s3*c1*c2,
+        s1*s3*c2 + s2*c1*c3,
+        s1*c2*c3 - s2*s3*c1
+    ])
+
+def euler_to_dcm(phi, theta, psi):
+    s1 = sin(psi)
+    c1 = cos(psi)
+    s2 = sin(theta)
+    c2 = cos(theta)
+    s3 = sin(phi)
+    c3 = cos(phi)
+    return array([
+        [c1*c2, c1*s2*s3 - s1*c3, c1*s2*c3 + s1*s3],
+        [s1*c2, s1*s2*s3 + c1*c3, s1*s2*c3 - c1*s3],
+        [-s2, c2*s3, c2*c3],
+    ])
+
+def quat_prod(q, r):
+    "Quaternion product"
+    return array([
+        r[0]*q[0] - r[1]*q[1] - r[2]*q[2] - r[3]*q[3],
+        r[0]*q[1] + r[1]*q[0] - r[2]*q[3] + r[3]*q[2],
+        r[0]*q[2] + r[1]*q[3] + r[2]*q[0] - r[3]*q[1],
+        r[0]*q[3] - r[1]*q[2] + r[2]*q[1] + r[3]*q[0]
+    ])
+
+def dcm_to_euler(dcm):
+    return array([
+        arctan(dcm[2,1]/ dcm[2,2]),
+        arctan(-dcm[2,0]/ sqrt(1 - dcm[2,0]**2)),
+        arctan(dcm[1,0]/ dcm[0,0]),
+    ])
+
+def dcm_from_quat(q):
+    q1 = q[0]
+    q2 = q[1]
+    q3 = q[2]
+    q4 = q[3]
+    return array([
+        [q1*q1 + q2*q2 - q3*q3 - q4*q4, 2*(q2*q3 - q1*q4), 2*(q2*q4 + q1*q3)],
+        [2*(q2*q3 + q1*q4), q1*q1 - q2*q2 + q3*q3 - q4*q4, 2*(q3*q4 - q1*q2)],
+        [2*(q2*q4 - q1*q3), 2*(q3*q4 + q1*q2), q1*q1 - q2*q2 - q3*q3 + q4*q4]
+    ])
+
+def quat_to_euler(q):
+    "Quaternion to (body 3(psi)-2(theta)-1(phi) euler angles"
+    return dcm_to_euler(dcm_from_quat(q))
+
+def quat_to_dcm(q):
+    return euler_to_dcm(quat_to_euler(q))
+
+phi = 0.1
+theta = 0.2
+psi = 0.3
+print('euler', phi, theta, psi)
+
+q = euler_to_quat(phi, theta, psi)
+assert(abs(norm(q) - 1) < FLT_EPSILON)
+assert(abs(norm(q) - 1) < FLT_EPSILON)
+assert(norm(array(quat_to_euler(q)) - array([phi, theta, psi])) < FLT_EPSILON)
+print('\nq:')
+pprint(q)
+
+dcm = euler_to_dcm(phi, theta, psi)
+assert(norm(dcm[:,0]) == 1)
+assert(norm(dcm[:,1]) == 1)
+assert(norm(dcm[:,2]) == 1)
+assert(abs(dcm[:,0].dot(dcm[:,1])) < FLT_EPSILON)
+assert(abs(dcm[:,0].dot(dcm[:,2])) < FLT_EPSILON)
+print('\ndcm:')
+pprint(dcm)
+
+print('\nq*q', quat_prod(q, q))
+
+q2 = quat_prod(q, q)
+pprint(q2)
+print(norm(q2))
+
+print('\nq3:')
+q3 = array([1,2,3,4])
+pprint(q3)
+print('\nq3_norm:')
+q3_norm =q3 / norm(q3)
+pprint(q3_norm)
+
+print('\ninverse')
+A = array([[0,2,3], [4,5,6], [7,8,10]])
+pprint(A)
+pprint(inv(A))
+
+print('\nmatrix exponential')
+A = 0.01*array([[1.0,2.0,3.0], [4.0,5.0,6.0], [7.0,8.0,10.0]])
+eA_check = scipy.linalg.expm(A)
+
+pprint(eA_check)
+
+eA_approx = eye(3)
+k = 1.0
+A_pow = A
+for i in range(1,3):
+    k *= i
+    # print(i, k, '\n', A_pow/k, '\n')
+    eA_approx +=  A_pow/k
+    A_pow = A_pow.dot(A)
+print(eA_approx)
+
+print('\nqr decomposition 4x4')
+A = array([[20.0, -10.0, -13.0, 21.0], [ 17.0, 16.0, -18.0, -14], [0.7, -0.8, 0.9, -0.5], [-1.0, -1.1, -1.2, -1.3]])
+b = array([[2.], [3.], [4.], [5.]])
+x = scipy.linalg.lstsq(A,b)[0]
+print('A:')
+pprint(A)
+print('b:')
+pprint(b)
+print('x:')
+pprint(scipy.linalg.lstsq(A,b)[0])
+
+print('\nqr decomposition 4x3')
+A = array([[20.0, -10.0, -13.0], [ 17.0, 16.0, -18.0], [0.7, -0.8, 0.9], [-1.0, -1.1, -1.2]])
+b = array([[2.], [3.], [4.], [5.]])
+x = scipy.linalg.lstsq(A,b)[0]
+print('A:')
+pprint(A)
+print('b:')
+pprint(b)
+print('x:')
+pprint(x)
+
+# vim: set et ft=python fenc=utf-8 ff=unix sts=4 sw=4 ts=8 : 

src/lib/matrix/test/test_macros.hpp

@@ -0,0 +1,46 @@
+/**
+ * @file test_marcos.hpp
+ *
+ * Helps with cmake testing.
+ *
+ * @author James Goppert <james.goppert@gmail.com>
+ *         Pavel Kirienko <pavel.kirienko@zubax.com>
+ */
+#pragma once
+
+#include <cstdio>
+#include <cmath>    // cmath has to be introduced BEFORE we poison the C library identifiers
+
+#define TEST(X) if(!(X)) { fprintf(stderr, "test failed on %s:%d\n", __FILE__, __LINE__); return -1;}
+
+/**
+ * This construct is needed to catch any unintended use of the C standard library.
+ * Feel free to extend the list of poisoned identifiers with as many C functions as possible.
+ * The current list was constructed by means of automated parsing of http://en.cppreference.com/w/c/numeric/math
+ */
+#ifdef __GNUC__
+
+// float functions
+# pragma GCC poison fabsf fmodf
+# pragma GCC poison remainderf remquof fmaf fmaxf fminf fdimf fnanf expf
+# pragma GCC poison exp2f expm1f logf log10f log2f log1pf powf sqrtf cbrtf
+# pragma GCC poison hypotf sinf cosf tanf asinf acosf atanf atan2f sinhf
+# pragma GCC poison coshf tanhf asinhf acoshf atanhf erff erfcf tgammaf
+# pragma GCC poison lgammaf ceilf floorf truncf roundf nearbyintf rintf
+# pragma GCC poison frexpf ldexpf modff scalbnf ilogbf logbf nextafterf
+# pragma GCC poison copysignf
+
+// the list of double functions is missing because otherwise most functions from std:: would be also poisoned,
+// which we don't want
+
+// long double functions
+# pragma GCC poison fabsl fabsl fabsl fmodl
+# pragma GCC poison remainderl remquol fmal fmaxl fminl fdiml fnanl expl
+# pragma GCC poison exp2l expm1l logl log10l log2l log1pl powl sqrtl cbrtl
+# pragma GCC poison hypotl sinl cosl tanl asinl acosl atanl atan2l sinhl
+# pragma GCC poison coshl tanhl asinhl acoshl atanhl erfl erfcl tgammal
+# pragma GCC poison lgammal ceill floorl truncl roundl nearbyintl rintl
+# pragma GCC poison frexpl ldexpl modfl scalbnl ilogbl logbl nextafterl
+# pragma GCC poison copysignl
+
+#endif

src/lib/matrix/test/transpose.cpp

@@ -0,0 +1,19 @@
+#include "test_macros.hpp"
+
+#include <matrix/math.hpp>
+
+using namespace matrix;
+
+int main()
+{
+    float data[6] = {1, 2, 3, 4, 5, 6};
+    Matrix<float, 2, 3> A(data);
+    Matrix<float, 3, 2> A_T = A.transpose();
+    float data_check[6] = {1, 4, 2, 5, 3, 6};
+    Matrix<float, 3, 2> A_T_check(data_check);
+    TEST(isEqual(A_T, A_T_check));
+
+    return 0;
+}
+
+/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

src/lib/matrix/test/upperRightTriangle.cpp

@@ -0,0 +1,23 @@
+#include "test_macros.hpp"
+#include <matrix/math.hpp>
+
+using namespace matrix;
+
+int main()
+{
+    float data[9] = {1, 2, 3,
+                     4, 5, 6,
+                     7, 8, 10
+                    };
+    float urt[6] = {1, 2, 3, 5, 6, 10};
+
+    SquareMatrix<float, 3> A(data);
+
+    for(size_t i=0; i<6; i++) {
+        TEST(fabs(urt[i] - A.upper_right_triangle()(i)) < FLT_EPSILON);
+    }
+
+    return 0;
+}
+
+/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

src/lib/matrix/test/vector.cpp

@@ -0,0 +1,48 @@
+#include "test_macros.hpp"
+
+#include <matrix/math.hpp>
+
+using namespace matrix;
+
+int main()
+{
+    // test data
+    float data1[] = {1,2,3,4,5};
+    float data2[] = {6,7,8,9,10};
+    Vector<float, 5> v1(data1);
+    Vector<float, 5> v2(data2);
+
+    // copy constructor
+    Vector<float, 5> v3(v2);
+    TEST(isEqual(v2, v3));
+
+    // norm, dot product
+    TEST(isEqualF(v1.norm(), 7.416198487095663f));
+    TEST(isEqualF(v1.norm_squared(), v1.norm() * v1.norm()));
+    TEST(isEqualF(v1.norm(), v1.length()));
+    TEST(isEqualF(v1.dot(v2), 130.0f));
+    TEST(isEqualF(v1.dot(v2), v1 * v2));
+
+    // unit, unit_zero, normalize
+    TEST(isEqualF(v2.unit().norm(), 1.f));
+    TEST(isEqualF(v2.unit_or_zero().norm(), 1.f));
+    TEST(isEqualF(Vector<float, 5>().unit_or_zero().norm(), 0.f));
+    v2.normalize();
+    TEST(isEqualF(v2.norm(), 1.f));
+
+    // sqrt
+    float data1_sq[] = {1,4,9,16,25};
+    Vector<float, 5> v4(data1_sq);
+    TEST(isEqual(v1, v4.sqrt()));
+
+    // longerThan
+    Vector<float, 2> v5;
+    v5(0) = 3;
+    v5(1) = 4;
+    TEST(v5.longerThan(4.99f));
+    TEST(!v5.longerThan(5.f));
+
+    return 0;
+}
+
+/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

src/lib/matrix/test/vector2.cpp

@@ -0,0 +1,40 @@
+
+
+#include <matrix/math.hpp>
+
+#include "test_macros.hpp"
+
+using namespace matrix;
+
+int main()
+{
+    Vector2f a(1, 0);
+    Vector2f b(0, 1);
+    TEST(fabs(a % b - 1.0f) < FLT_EPSILON);
+
+    Vector2f c;
+    TEST(fabs(c(0) - 0) < FLT_EPSILON);
+    TEST(fabs(c(1) - 0) < FLT_EPSILON);
+
+    Matrix<float, 2, 1> d(a);
+    TEST(fabs(d(0,0) - 1) < FLT_EPSILON);
+    TEST(fabs(d(1,0) - 0) < FLT_EPSILON);
+
+    Vector2f e(d);
+    TEST(fabs(e(0) - 1) < FLT_EPSILON);
+    TEST(fabs(e(1) - 0) < FLT_EPSILON);
+
+    float data[] = {4,5};
+    Vector2f f(data);
+    TEST(fabs(f(0) - 4) < FLT_EPSILON);
+    TEST(fabs(f(1) - 5) < FLT_EPSILON);
+
+    Vector3f g(1.23f, 423.4f, 3221.f);
+    Vector2f h(g);
+    TEST(fabs(h(0) - 1.23f) < FLT_EPSILON);
+    TEST(fabs(h(1) - 423.4f) < FLT_EPSILON);
+
+    return 0;
+}
+
+/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

src/lib/matrix/test/vector3.cpp

@@ -0,0 +1,61 @@
+#include "test_macros.hpp"
+
+#include <matrix/math.hpp>
+
+using namespace matrix;
+
+int main()
+{
+    Vector3f a(1, 0, 0);
+    Vector3f b(0, 1, 0);
+    Vector3f c = a.cross(b);
+    TEST(isEqual(c, Vector3f(0,0,1)));
+    c = a % b;
+    TEST(isEqual(c, Vector3f(0,0,1)));
+    Matrix<float, 3, 1> d(c);
+    Vector3f e(d);
+    TEST(isEqual(e, d));
+    float data[] = {4, 5, 6};
+    Vector3f f(data);
+    TEST(isEqual(f, Vector3f(4, 5, 6)));
+
+    TEST(isEqual(a + b, Vector3f(1, 1, 0)));
+    TEST(isEqual(a - b, Vector3f(1, -1, 0)));
+    TEST(isEqualF(a * b, 0.0f));
+    TEST(isEqual(-a, Vector3f(-1, 0, 0)));
+    TEST(isEqual(a.unit(), a));
+    TEST(isEqual(a.unit(), a.normalized()));
+    TEST(isEqual(a*2.0, Vector3f(2, 0, 0)));
+
+    Vector2f g2(1,3);
+    Vector3f g3(7, 11, 17);
+    g3.xy() = g2;
+    TEST(isEqual(g3, Vector3f(1, 3, 17)));
+
+    const Vector3f g4(g3);
+    Vector2f g5 = g4.xy();
+    TEST(isEqual(g5,g2));
+    TEST(isEqual(g2,Vector2f(g4.xy())));
+
+    Vector3f h;
+    TEST(isEqual(h,Vector3f(0,0,0)));
+
+    Vector<float, 4> j;
+    j(0) = 1;
+    j(1) = 2;
+    j(2) = 3;
+    j(3) = 4;
+
+    Vector3f k = j.slice<3,1>(0,0);
+    Vector3f k_test(1,2,3);
+    TEST(isEqual(k,k_test));
+
+    Vector3f m1(1, 2, 3);
+    Vector3f m2(3.1f, 4.1f, 5.1f);
+    TEST(isEqual(m2, m1 + 2.1f));
+    TEST(isEqual(m2 - 2.1f, m1));
+
+    return 0;
+}
+
+/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */

src/lib/matrix/test/vectorAssignment.cpp

@@ -0,0 +1,34 @@
+#include <matrix/math.hpp>
+
+#include "test_macros.hpp"
+
+using namespace matrix;
+
+int main()
+{
+    Vector3f v;
+    v(0) = 1;
+    v(1) = 2;
+    v(2) = 3;
+
+    static const float eps = 1e-7f;
+
+    TEST(fabs(v(0) - 1) < eps);
+    TEST(fabs(v(1) - 2) < eps);
+    TEST(fabs(v(2) - 3) < eps);
+
+    Vector3f v2(4, 5, 6);
+
+    TEST(fabs(v2(0) - 4) < eps);
+    TEST(fabs(v2(1) - 5) < eps);
+    TEST(fabs(v2(2) - 6) < eps);
+
+    SquareMatrix<float, 3> m = diag(Vector3f(1,2,3));
+    TEST(fabs(m(0, 0) - 1) < eps);
+    TEST(fabs(m(1, 1) - 2) < eps);
+    TEST(fabs(m(2, 2) - 3) < eps);
+
+    return 0;
+}
+
+/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */