Simple Math functions. More...

Classes
struct	NAV::math::LerpSearchResult
	Lerp Search Result. More...

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

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

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))

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 𝐅𝜏ₛ

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

template<std::integral Out, size_t Bits, std::integral In>
constexpr Out	NAV::math::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	NAV::math::inverseSqrt (const Eigen::MatrixBase< Derived > &matrix)
	Returns the inverse square root of a matrix.

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

LerpSearchResult	NAV::math::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 > > >	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.

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.

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

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

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

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))

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

template<typename T>
T	NAV::math::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 >	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<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<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.

Detailed Description

Simple Math functions.

Author: T. Topp (topp@.nosp@m.ins..nosp@m.uni-s.nosp@m.tutt.nosp@m.gart..nosp@m.de); N. Stahl (HiWi: Elliptical integral)

Date: 2023-07-04

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

◆ 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

◆ 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

◆ 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'

◆ 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

◆ 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

◆ 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)

◆ 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

◆ 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

◆ 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

◆ 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

◆ 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

◆ 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

◆ 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

◆ 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

◆ 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)

◆ 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

◆ 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

Classes

Functions

Detailed Description

Function Documentation

◆ bilinearInterpolation()

◆ calcEllipticalIntegral()

◆ expm()

◆ factorial()

◆ interpretAs()

◆ inverseSqrt()

◆ lerp()

◆ lerpSearch()

◆ LtDLdecomp_choleskyFact()

◆ LtDLdecomp_outerProduct()

◆ normalCDF()

◆ round()

◆ roundSignificantDigits()

◆ sgn()

◆ sign()

◆ skewSymmetricMatrix()

◆ skewSymmetricMatrixSquared()

◆ squaredNormVectorMatrix()