VanLoan.hpp

Go to the documentation of this file.

// This file is part of INSTINCT, the INS Toolkit for Integrated
// Navigation Concepts and Training by the Institute of Navigation of
// the University of Stuttgart, Germany.
//
// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at https://mozilla.org/MPL/2.0/.
 
 
#pragma once
 
#include <utility>
#include <Eigen/Core>
#include <unsupported/Eigen/MatrixFunctions>
 
namespace NAV
{
 
template<typename DerivedF, typename DerivedG, typename DerivedW>
[[nodiscard]] std::pair<typename DerivedF::PlainObject, typename DerivedF::PlainObject>
    calcPhiAndQWithVanLoanMethod(const Eigen::MatrixBase<DerivedF>& F,
                                 const Eigen::MatrixBase<DerivedG>& G,
                                 const Eigen::MatrixBase<DerivedW>& W,
                                 typename DerivedF::Scalar dt)
{
    //     ┌            ┐
    //     │ -F  ┊ GWG^T│
    // A = │------------│ * dT
    //     │  0  ┊  F^T │
    //     └            ┘
    // W = Power Spectral Density of u (See Brown & Hwang (2012) chapter 3.9, p. 126 - footnote)
    // W = Identity, as noise scale factor is included within G matrix
    Eigen::Matrix<typename DerivedF::Scalar, Eigen::Dynamic, Eigen::Dynamic>
        A = Eigen::Matrix<typename DerivedF::Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(2 * F.rows(), 2 * F.cols());
    A.topLeftCorner(F.rows(), F.cols()) = -F;
    A.topRightCorner(F.rows(), F.cols()) = G * W * G.transpose();
    A.bottomRightCorner(F.rows(), F.cols()) = F.transpose();
    A *= dt;
 
    // Exponential Matrix of A (https://eigen.tuxfamily.org/dox/unsupported/group__MatrixFunctions__Module.html#matrixbase_exp)
    Eigen::Matrix<typename DerivedF::Scalar, Eigen::Dynamic, Eigen::Dynamic> B = A.exp();
 
    //               ┌                ┐
    //               │ ... ┊ Phi^-1 Q │
    // B = expm(A) = │----------------│
    //               │  0  ┊   Phi^T  │
    //               └                ┘
    typename DerivedF::PlainObject Phi = B.bottomRightCorner(F.rows(), F.cols()).transpose();
    typename DerivedF::PlainObject Q = Phi * B.topRightCorner(F.rows(), F.cols());
 
    return { Phi, Q };
}
 
} // namespace NAV