Data Structures
struct	LerpSearchResult
	Lerp Search Result. More...

Functions
auto	bilinearInterpolation (const auto &tx, const auto &ty, const auto &c00, const auto &c10, const auto &c01, const auto &c11)
	Bilinear interpolation.

double	calcEllipticalIntegral (double phi, double m)
	Calculates the incomplete elliptical integral of the second kind.

template<std::floating_point T>
T	csc (const T &x)
	Calculates the cosecant of a value (csc(x) = sec(pi/2 - x) = 1 / sin(x))

template<typename Derived>
Derived::PlainObject	expm (const Eigen::MatrixBase< Derived > &X, size_t order)
	Calculates the state transition matrix 𝚽 limited to specified order in 𝐅𝜏ₛ

uint64_t	factorial (uint64_t n)
	Calculates the factorial of an unsigned integer.

template<std::integral Out, size_t Bits, std::integral In>
constexpr Out	interpretAs (In in)
	Interprets the input integer with certain amount of Bits as Output type. Takes care of sign extension.

template<typename Derived>
Derived::PlainObject	inverseSqrt (const Eigen::MatrixBase< Derived > &matrix)
	Returns the inverse square root of a matrix.

template<typename Derived>
Derived::PlainObject	lerp (const Eigen::MatrixBase< Derived > &a, const Eigen::MatrixBase< Derived > &b, auto t)
	Linear interpolation between vectors.

LerpSearchResult	lerpSearch (const auto &data, const auto &value)
	Searches the value in the data container.

template<typename Derived>
std::optional< std::pair< Eigen::Matrix< typename Derived::Scalar, Derived::RowsAtCompileTime, Derived::ColsAtCompileTime >, Eigen::Vector< typename Derived::Scalar, Derived::RowsAtCompileTime > > >	LtDLdecomp_choleskyFact (const Eigen::MatrixBase< Derived > &Q)
	Find (L^T D L)-decomposition of Q-matrix via a backward Cholesky factorization in a bordering method formulation.

template<typename Derived>
std::optional< std::pair< Eigen::Matrix< typename Derived::Scalar, Derived::RowsAtCompileTime, Derived::ColsAtCompileTime >, Eigen::Vector< typename Derived::Scalar, Derived::RowsAtCompileTime > > >	LtDLdecomp_outerProduct (const Eigen::MatrixBase< Derived > &Qmatrix)
	Find (L^T D L)-decomposition of Q-matrix via outer product method.

double	normalCDF (double value)
	Calculates the cumulative distribution function (CDF) of the standard normal distribution.

template<std::floating_point T>
constexpr T	round (const T &value, size_t digits)
	Round the number to the specified amount of digits.

template<std::floating_point T>
constexpr T	roundSignificantDigits (T value, size_t digits)
	Round the number to the specified amount of significant digits.

template<std::floating_point T>
T	sec (const T &x)
	Calculates the secant of a value (sec(x) = csc(pi/2 - x) = 1 / cos(x))

template<typename T>
int	sgn (const T &val)
	Returns the sign of the given value.

template<typename T>
T	sign (const T &x, const T &y)
	Change the sign of x according to the value of y.

template<typename Derived>
Eigen::Matrix< typename Derived::Scalar, 3, 3 >	skewSymmetricMatrix (const Eigen::MatrixBase< Derived > &a)
	Calculates the skew symmetric matrix of the given vector. This is needed to perform the cross product with a scalar product operation.

template<typename Derived>
Eigen::Matrix< typename Derived::Scalar, 3, 3 >	skewSymmetricMatrixSquared (const Eigen::MatrixBase< Derived > &a)
	Calculates the square of a skew symmetric matrix of the given vector.

template<typename DerivedA, typename DerivedQ>
DerivedA::Scalar	squaredNormVectorMatrix (const Eigen::MatrixBase< DerivedA > &a, const Eigen::MatrixBase< DerivedQ > &Q)
	Calculates the squared norm of the vector and matrix.

Function Documentation

◆ bilinearInterpolation()

auto NAV::math::bilinearInterpolation	(	const auto &	tx,
		const auto &	ty,
		const auto &	c00,
		const auto &	c10,
		const auto &	c01,
		const auto &	c11 )

Bilinear interpolation.

Parameters

[in]	tx	Distance in x component to interpolate [0, 1]
[in]	ty	Distance in y component to interpolate [0, 1]
[in]	c00	Value for tx = ty = 0
[in]	c10	Value for tx = 1 and ty = 0
[in]	c01	Value for tx = 0 and ty = 1
[in]	c11	Value for tx = ty = 1

c01 ---— c11

c00 ---— c10

Note: See https://www.scratchapixel.com/lessons/mathematics-physics-for-computer-graphics/interpolation/bilinear-filtering.html

Definition at line 394 of file Math.hpp.

◆ calcEllipticalIntegral()

double NAV::math::calcEllipticalIntegral	(	double	phi,
		double	m )

Calculates the incomplete elliptical integral of the second kind.

Parameters

[in]	phi	Interval bound the integration uses from 0 to phi
[in]	m	Function parameter that is integrated 1-m*sin(t)^2

Returns: Incomplete elliptical integral of the second kind

Note: See http://www2.iap.fr/users/pichon/doc/html_xref/elliptic-es.html

Definition at line 53 of file Math.cpp.

◆ csc()

template<std::floating_point T>

T NAV::math::csc ( const T & x )

Calculates the cosecant of a value (csc(x) = sec(pi/2 - x) = 1 / sin(x))

Definition at line 130 of file Math.hpp.

◆ expm()

template<typename Derived>

Derived::PlainObject NAV::math::expm	(	const Eigen::MatrixBase< Derived > &	X,
		size_t	order )

Calculates the state transition matrix 𝚽 limited to specified order in 𝐅𝜏ₛ

Parameters

[in]	X	Matrix
[in]	order	The order of the Taylor polynom to calculate

Note: See [17] Groves, ch. 3.2.3, eq. 3.34, p. 98

Definition at line 149 of file Math.hpp.

◆ factorial()

uint64_t NAV::math::factorial ( uint64_t n )

Calculates the factorial of an unsigned integer.

Parameters

[in] n Unsigned integer

Returns: The factorial of 'n'

Definition at line 14 of file Math.cpp.

◆ interpretAs()

template<std::integral Out, size_t Bits, std::integral In>

Out NAV::math::interpretAs ( In in )

constexpr

Interprets the input integer with certain amount of Bits as Output type. Takes care of sign extension.

Template Parameters

Out	Output type
Bits	Size of the input data
In	Input data type (needs to be bigger than the amount of Bits)

Parameters

[in] in Number as two's complement, with the sign bit (+ or -) occupying the MSB

Returns: Output type

Definition at line 74 of file Math.hpp.

◆ inverseSqrt()

template<typename Derived>

Derived::PlainObject NAV::math::inverseSqrt ( const Eigen::MatrixBase< Derived > & matrix )

nodiscard

Returns the inverse square root of a matrix.

Parameters

matrix Matrix to use

Definition at line 314 of file Math.hpp.

◆ lerp()

template<typename Derived>

Derived::PlainObject NAV::math::lerp	(	const Eigen::MatrixBase< Derived > &	a,
		const Eigen::MatrixBase< Derived > &	b,
		auto	t )

Linear interpolation between vectors.

Parameters

a	Left value
b	Right value
t	Multiplier. [0, 1] for interpolation

Returns: a + t * (b - a)

Definition at line 351 of file Math.hpp.

◆ lerpSearch()

LerpSearchResult NAV::math::lerpSearch	(	const auto &	data,
		const auto &	value )

Searches the value in the data container.

Parameters

[in]	data	Data container
[in]	value	Value to search

Definition at line 367 of file Math.hpp.

◆ LtDLdecomp_choleskyFact()

template<typename Derived>

std::optional< std::pair< Eigen::Matrix< typename Derived::Scalar, Derived::RowsAtCompileTime, Derived::ColsAtCompileTime >, Eigen::Vector< typename Derived::Scalar, Derived::RowsAtCompileTime > > > NAV::math::LtDLdecomp_choleskyFact ( const Eigen::MatrixBase< Derived > & Q )

Find (L^T D L)-decomposition of Q-matrix via a backward Cholesky factorization in a bordering method formulation.

Parameters

[in] Q Symmetric positive definite matrix to be factored

Returns: L - Factor matrix (strict lower triangular); D - Vector with entries of the diagonal matrix

Note: See [12] de Jonge 1996, Algorithm FMFAC6

Definition at line 230 of file Math.hpp.

◆ LtDLdecomp_outerProduct()

template<typename Derived>

std::optional< std::pair< Eigen::Matrix< typename Derived::Scalar, Derived::RowsAtCompileTime, Derived::ColsAtCompileTime >, Eigen::Vector< typename Derived::Scalar, Derived::RowsAtCompileTime > > > NAV::math::LtDLdecomp_outerProduct ( const Eigen::MatrixBase< Derived > & Qmatrix )

Find (L^T D L)-decomposition of Q-matrix via outer product method.

Parameters

[in] Qmatrix Symmetric positive definite matrix to be factored

Returns: L - Factor matrix (strict lower triangular); D - Vector with entries of the diagonal matrix

Note: See [12] de Jonge 1996, Algorithm FMFAC5

Attention: Consider using NAV::math::LtDLdecomp_choleskyFact() because it is faster by up to a factor 10

Definition at line 186 of file Math.hpp.

◆ normalCDF()

double NAV::math::normalCDF ( double value )

Calculates the cumulative distribution function (CDF) of the standard normal distribution.

$\begin{equation} \label{eq:eq-normalDistCDF} \Phi(x) = \int\displaylimits_{-\infty}^x \frac{1}{\sqrt{2\pi}} \exp{\left(-\frac{1}{2} v^2\right)} \text{d}v \end{equation}$

which can be expressed with the error function

$\begin{equation} \label{eq:eq-normalDistCDF-erf} \Phi(x) = \frac{1}{2} \left[ 1 + \text{erf}{\left(\frac{x}{\sqrt{2}}\right)} \right] \end{equation}$

Using the property

$\begin{equation} \label{eq:eq-erf-minus} \text{erf}{\left( -x \right)} = -\text{erf}{\left( x \right)} \end{equation}$

and the complementary error function

$\begin{equation} \label{eq:eq-erfc} \text{erfc}{\left( x \right)} = 1 - \text{erf}{\left( x \right)} \end{equation}$

we can simplify equation $\eqref{eq:eq-normalDistCDF-erf}$ to

$\begin{equation} \label{eq:eq-normalDistCDF-erfc} \begin{aligned} \Phi(x) &= \frac{1}{2} \left[ 1 - \text{erf}{\left(-\frac{x}{\sqrt{2}}\right)} \right] \\ &= \frac{1}{2} \text{erfc}{\left(-\frac{x}{\sqrt{2}}\right)} \end{aligned} \end{equation}$

Parameters

value Value to calculate the CDF for

Definition at line 48 of file Math.cpp.

◆ round()

template<std::floating_point T>

T NAV::math::round	(	const T &	value,
		size_t	digits )

constexpr

Round the number to the specified amount of digits.

Parameters

[in]	value	Value to round
[in]	digits	Amount of digits

Returns: The rounded value

Definition at line 41 of file Math.hpp.

◆ roundSignificantDigits()

template<std::floating_point T>

T NAV::math::roundSignificantDigits	(	T	value,
		size_t	digits )

constexpr

Round the number to the specified amount of significant digits.

Parameters

[in]	value	Value to round
[in]	digits	Amount of digits

Returns: The rounded value

Definition at line 52 of file Math.hpp.

◆ sec()

template<std::floating_point T>

T NAV::math::sec ( const T & x )

Calculates the secant of a value (sec(x) = csc(pi/2 - x) = 1 / cos(x))

Definition at line 123 of file Math.hpp.

◆ sgn()

template<typename T>

int NAV::math::sgn ( const T & val )

Returns the sign of the given value.

Parameters

[in] val Value to get the sign from

Returns: Sign of the given value

Definition at line 139 of file Math.hpp.

◆ sign()

template<typename T>

T NAV::math::sign	(	const T &	x,
		const T &	y )

Change the sign of x according to the value of y.

Parameters

[in]	x	input value
[in]	y	input value

Returns: -x or +x

Definition at line 335 of file Math.hpp.

◆ skewSymmetricMatrix()

template<typename Derived>

Eigen::Matrix< typename Derived::Scalar, 3, 3 > NAV::math::skewSymmetricMatrix ( const Eigen::MatrixBase< Derived > & a )

Calculates the skew symmetric matrix of the given vector. This is needed to perform the cross product with a scalar product operation.

Template Parameters

Derived Derived Eigen Type

Parameters

[in] a The vector

Returns: Skew symmetric matrix

Note: See Groves (2013) equation (2.50)

Definition at line 90 of file Math.hpp.

◆ skewSymmetricMatrixSquared()

template<typename Derived>

Eigen::Matrix< typename Derived::Scalar, 3, 3 > NAV::math::skewSymmetricMatrixSquared ( const Eigen::MatrixBase< Derived > & a )

Calculates the square of a skew symmetric matrix of the given vector.

Template Parameters

Derived Derived Eigen Type

Parameters

[in] a The vector

Returns: Square of skew symmetric matrix

Definition at line 108 of file Math.hpp.

◆ squaredNormVectorMatrix()

template<typename DerivedA, typename DerivedQ>

DerivedA::Scalar NAV::math::squaredNormVectorMatrix	(	const Eigen::MatrixBase< DerivedA > &	a,
		const Eigen::MatrixBase< DerivedQ > &	Q )

Calculates the squared norm of the vector and matrix.

$\begin{equation} \label{eq:eq-squaredNorm} ||\mathbf{\dots}||^2_{\mathbf{Q}} = (\dots)^T \mathbf{Q}^{-1} (\dots) \end{equation}$

Parameters

a	Vector
Q	Covariance matrix of the vector

Returns: Squared norm

Definition at line 273 of file Math.hpp.

Data Structures

Functions

Function Documentation

◆ bilinearInterpolation()

◆ calcEllipticalIntegral()

◆ csc()

◆ expm()

◆ factorial()

◆ interpretAs()

◆ inverseSqrt()

◆ lerp()

◆ lerpSearch()

◆ LtDLdecomp_choleskyFact()

◆ LtDLdecomp_outerProduct()

◆ normalCDF()

◆ round()

◆ roundSignificantDigits()

◆ sec()

◆ sgn()

◆ sign()

◆ skewSymmetricMatrix()

◆ skewSymmetricMatrixSquared()

◆ squaredNormVectorMatrix()