Function navtk::navutils::discretize_first_order

Function Documentation

std::pair<Matrix, Matrix> navtk::navutils::discretize_first_order(const Matrix &f, const Matrix &q, double dt)

Converts from continuous-time dynamic system representation to discrete-time representation using first-order algorithm.

If the continuous time system is

\( \dot{\textbf{x}} = \textbf{Fx} + \textbf{w} \)

\( E[\textbf{w}(t-\tau)\textbf{w}^T(t-\tau)] = \textbf{Q}\delta(\tau) \)

where \( E[] \) is the expectation operator and \( \delta(\tau) \) is the Dirac delta function.

This can be converted to an equivalent discrete-time system

\( \textbf{x}_{t_{k+1}} = \pmb{\Phi}\textbf{x}_{t_k} + \textbf{w}_d \)

where

\( E[\textbf{w}_{d_j}\textbf{w}^T_{d_k}] = \textbf{Q}_d \delta_{kj} \)

and \( \delta_{kj} \) is the Kronecker delta function.

When \(\textbf{F}\) is continuous over the time interval between \(t_{k}\) and \(t_{k+1}\), then \(\pmb{\Phi}\) can be calculated as

\( \begin{equation} \pmb{\Phi}=e^{\textbf{F}\Delta t} = \textbf{I} + \sum\limits_{n=1}^\infty{1 \over {n!}}\textbf{F}^n\Delta t^n\end{equation} \)

\( \Delta t = t_{k+1} - t_k \)

This function calculates \(\pmb{\Phi}\) using the first-order calculation

\(\pmb{\Phi}_1 \approx \textbf{I} + \textbf{F}\Delta t\)

and

\(\textbf{Q}_{d_1} \approx \textbf{Q}\Delta t\)