PosVel.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 "NodeData/State/Pos.hpp"
#include <Eigen/src/Core/Matrix.h>
 
namespace NAV
{
class PosVel : public Pos
{
  public:
    [[nodiscard]] static std::string type()
    {
        return "PosVel";
    }
 
    [[nodiscard]] static std::vector<std::string> parentTypes()
    {
        auto parent = Pos::parentTypes();
        parent.push_back(Pos::type());
        return parent;
    }
 
    [[nodiscard]] static std::vector<std::string> GetStaticDataDescriptors()
    {
        auto desc = Pos::GetStaticDataDescriptors();
        desc.reserve(GetStaticDescriptorCount());
        desc.emplace_back("Velocity norm [m/s]");
        desc.emplace_back("X velocity ECEF [m/s]");
        desc.emplace_back("Y velocity ECEF [m/s]");
        desc.emplace_back("Z velocity ECEF [m/s]");
        desc.emplace_back("North velocity [m/s]");
        desc.emplace_back("East velocity [m/s]");
        desc.emplace_back("Down velocity [m/s]");
        desc.emplace_back("X velocity ECEF StDev [m/s]");
        desc.emplace_back("Y velocity ECEF StDev [m/s]");
        desc.emplace_back("Z velocity ECEF StDev [m/s]");
        desc.emplace_back("XY velocity StDev [m]");
        desc.emplace_back("XZ velocity StDev [m]");
        desc.emplace_back("YZ velocity StDev [m]");
        desc.emplace_back("North velocity StDev [m/s]");
        desc.emplace_back("East velocity StDev [m/s]");
        desc.emplace_back("Down velocity StDev [m/s]");
        desc.emplace_back("NE velocity StDev [m]");
        desc.emplace_back("ND velocity StDev [m]");
        desc.emplace_back("ED velocity StDev [m]");
        return desc;
    }
 
    [[nodiscard]] static constexpr size_t GetStaticDescriptorCount() { return 39; }
 
    [[nodiscard]] std::vector<std::string> staticDataDescriptors() const override { return GetStaticDataDescriptors(); }
 
    [[nodiscard]] size_t staticDescriptorCount() const override { return GetStaticDescriptorCount(); }
 
    [[nodiscard]] std::optional<double> getValueAt(size_t idx) const override
    {
        INS_ASSERT(idx < GetStaticDescriptorCount());
        switch (idx)
        {
        case 0:  // Latitude [deg]
        case 1:  // Longitude [deg]
        case 2:  // Altitude [m]
        case 3:  // North/South [m]
        case 4:  // East/West [m]
        case 5:  // X-ECEF [m]
        case 6:  // Y-ECEF [m]
        case 7:  // Z-ECEF [m]
        case 8:  // X-ECEF StDev [m]
        case 9:  // Y-ECEF StDev [m]
        case 10: // Z-ECEF StDev [m]
        case 11: // XY-ECEF StDev [m]
        case 12: // XZ-ECEF StDev [m]
        case 13: // YZ-ECEF StDev [m]
        case 14: // North StDev [m]
        case 15: // East StDev [m]
        case 16: // Down StDev [m]
        case 17: // NE StDev [m]
        case 18: // ND StDev [m]
        case 19: // ED StDev [m]
            return Pos::getValueAt(idx);
        case 20: // Velocity norm [m/s]
            return e_velocity().norm();
        case 21: // X velocity ECEF [m/s]
            return e_velocity().x();
        case 22: // Y velocity ECEF [m/s]
            return e_velocity().y();
        case 23: // Z velocity ECEF [m/s]
            return e_velocity().z();
        case 24: // North velocity [m/s]
            return n_velocity().x();
        case 25: // East velocity [m/s]
            return n_velocity().y();
        case 26: // Down velocity [m/s]
            return n_velocity().z();
        case 27: // X velocity ECEF StDev [m/s]
            if (auto stDev = e_velocityStdev()) { return stDev->get().x(); }
            break;
        case 28: // Y velocity ECEF StDev [m/s]
            if (auto stDev = e_velocityStdev()) { return stDev->get().z(); }
            break;
        case 29: // Z velocity ECEF StDev [m/s]
            if (auto stDev = e_velocityStdev()) { return stDev->get().z(); }
            break;
        case 30: // XY velocity StDev [m]
            if (e_CovarianceMatrix().has_value() && (*e_CovarianceMatrix()).get().hasAnyCols(States::Vel)) { return (*e_CovarianceMatrix())(States::VelX, States::VelY); }
            break;
        case 31: // XZ velocity StDev [m]
            if (e_CovarianceMatrix().has_value() && (*e_CovarianceMatrix()).get().hasAnyCols(States::Vel)) { return (*e_CovarianceMatrix())(States::VelX, States::VelZ); }
            break;
        case 32: // YZ velocity StDev [m]
            if (e_CovarianceMatrix().has_value() && (*e_CovarianceMatrix()).get().hasAnyCols(States::Vel)) { return (*e_CovarianceMatrix())(States::VelY, States::VelZ); }
            break;
        case 33: // North velocity StDev [m/s]
            if (auto stDev = n_velocityStdev()) { return stDev->get().x(); }
            break;
        case 34: // East velocity StDev [m/s]
            if (auto stDev = n_velocityStdev()) { return stDev->get().y(); }
            break;
        case 35: // Down velocity StDev [m/s]
            if (auto stDev = n_velocityStdev()) { return stDev->get().z(); }
            break;
        case 36: // NE velocity StDev [m]
            if (n_CovarianceMatrix().has_value() && (*n_CovarianceMatrix()).get().hasAnyCols(States::Vel)) { return (*n_CovarianceMatrix())(States::VelX, States::VelY); }
            break;
        case 37: // ND velocity StDev [m]
            if (n_CovarianceMatrix().has_value() && (*n_CovarianceMatrix()).get().hasAnyCols(States::Vel)) { return (*n_CovarianceMatrix())(States::VelX, States::VelZ); }
            break;
        case 38: // ED velocity StDev [m]
            if (n_CovarianceMatrix().has_value() && (*n_CovarianceMatrix()).get().hasAnyCols(States::Vel)) { return (*n_CovarianceMatrix())(States::VelY, States::VelZ); }
            break;
        default:
            return std::nullopt;
        }
        return std::nullopt;
    }
 
    /* -------------------------------------------------------------------------------------------------------- */
    /*                                                 Velocity                                                 */
    /* -------------------------------------------------------------------------------------------------------- */
 
    struct States
    {
        States() = delete;
 
        enum StateKeys
        {
            PosX,         
            PosY,         
            PosZ,         
            VelX,         
            VelY,         
            VelZ,         
            States_COUNT, 
        };
        inline static const std::vector<StateKeys> Pos = { PosX, PosY, PosZ };
        inline static const std::vector<StateKeys> Vel = { States::VelX, States::VelY, States::VelZ };
        inline static const std::vector<StateKeys> PosVel = { States::PosX, States::PosY, States::PosZ,
                                                              States::VelX, States::VelY, States::VelZ };
    };
 
    [[nodiscard]] const Eigen::Vector3d& e_velocity() const { return _e_velocity; }
 
    [[nodiscard]] const Eigen::Vector3d& n_velocity() const { return _n_velocity; }
 
    [[nodiscard]] std::optional<std::reference_wrapper<const Eigen::Vector3d>> e_velocityStdev() const { return _e_velocityStdev; }
 
    [[nodiscard]] std::optional<std::reference_wrapper<const Eigen::Vector3d>> n_velocityStdev() const { return _n_velocityStdev; }
 
    [[nodiscard]] std::optional<std::reference_wrapper<const KeyedMatrixXd<States::StateKeys, States::StateKeys>>> e_CovarianceMatrix() const { return _e_covarianceMatrix; }
 
    [[nodiscard]] std::optional<std::reference_wrapper<const KeyedMatrixXd<States::StateKeys, States::StateKeys>>> n_CovarianceMatrix() const { return _n_covarianceMatrix; }
 
    // ###########################################################################################################
    //                                                  Setter
    // ###########################################################################################################
 
    void setVelocity_e(const Eigen::Vector3d& e_velocity)
    {
        _e_velocity = e_velocity;
        _n_velocity = n_Quat_e() * e_velocity;
    }
 
    void setVelocity_n(const Eigen::Vector3d& n_velocity)
    {
        _e_velocity = e_Quat_n() * n_velocity;
        _n_velocity = n_velocity;
    }
 
    void setVelocityAndStdDev_e(const Eigen::Vector3d& e_velocity, const Eigen::Matrix3d& e_velocityCovarianceMatrix)
    {
        setVelocity_e(e_velocity);
        _e_velocityStdev = e_velocityCovarianceMatrix.diagonal().cwiseSqrt();
        _n_velocityStdev = (n_Quat_e() * e_velocityCovarianceMatrix * e_Quat_n()).diagonal().cwiseSqrt();
    }
 
    void setVelocityAndStdDev_n(const Eigen::Vector3d& n_velocity, const Eigen::Matrix3d& n_velocityCovarianceMatrix)
    {
        setVelocity_n(n_velocity);
        _n_velocityStdev = n_velocityCovarianceMatrix.diagonal().cwiseSqrt();
        _e_velocityStdev = (e_Quat_n() * n_velocityCovarianceMatrix * n_Quat_e()).diagonal().cwiseSqrt();
    }
 
    template<typename Derived>
    void setPosVelCovarianceMatrix_e(const Eigen::MatrixBase<Derived>& e_covarianceMatrix)
    {
        INS_ASSERT_USER_ERROR(e_covarianceMatrix.rows() == 6, "This function needs a 6x6 matrix as input");
        INS_ASSERT_USER_ERROR(e_covarianceMatrix.cols() == 6, "This function needs a 6x6 matrix as input");
 
        _e_covarianceMatrix = KeyedMatrixXd<States::StateKeys,
                                            States::StateKeys>(e_covarianceMatrix,
                                                               States::PosVel);
        _n_covarianceMatrix = _e_covarianceMatrix;
 
        Eigen::Quaterniond n_Quat_e = trafo::n_Quat_e(latitude(), longitude());
        Eigen::Quaterniond e_Quat_n = trafo::e_Quat_n(latitude(), longitude());
 
        (*_n_covarianceMatrix)(all, all).setZero();
        (*_n_covarianceMatrix)(States::Pos, States::Pos) = n_Quat_e * (*_e_covarianceMatrix)(States::Pos, States::Pos) * e_Quat_n;
        (*_n_covarianceMatrix)(States::Vel, States::Vel) = n_Quat_e * (*_e_covarianceMatrix)(States::Vel, States::Vel) * e_Quat_n;
    }
 
    template<typename Derived>
    void setPosVelCovarianceMatrix_n(const Eigen::MatrixBase<Derived>& n_covarianceMatrix)
    {
        INS_ASSERT_USER_ERROR(n_covarianceMatrix.rows() == 6, "This function needs a 6x6 matrix as input");
        INS_ASSERT_USER_ERROR(n_covarianceMatrix.cols() == 6, "This function needs a 6x6 matrix as input");
 
        _n_covarianceMatrix = KeyedMatrixXd<States::StateKeys,
                                            States::StateKeys>(n_covarianceMatrix,
                                                               States::PosVel);
        _e_covarianceMatrix = _n_covarianceMatrix;
 
        Eigen::Quaterniond n_Quat_e = trafo::n_Quat_e(latitude(), longitude());
        Eigen::Quaterniond e_Quat_n = trafo::e_Quat_n(latitude(), longitude());
 
        (*_e_covarianceMatrix)(all, all).setZero();
        (*_e_covarianceMatrix)(States::Pos, States::Pos) = e_Quat_n * (*_n_covarianceMatrix)(States::Pos, States::Pos) * n_Quat_e;
        (*_e_covarianceMatrix)(States::Vel, States::Vel) = e_Quat_n * (*_n_covarianceMatrix)(States::Vel, States::Vel) * n_Quat_e;
    }
 
    /* -------------------------------------------------------------------------------------------------------- */
    /*                                             Member variables                                             */
    /* -------------------------------------------------------------------------------------------------------- */
 
  private:
    Eigen::Vector3d _e_velocity{ std::nan(""), std::nan(""), std::nan("") };
    Eigen::Vector3d _n_velocity{ std::nan(""), std::nan(""), std::nan("") };
 
    std::optional<Eigen::Vector3d> _e_velocityStdev;
    std::optional<Eigen::Vector3d> _n_velocityStdev;
 
    std::optional<KeyedMatrixXd<States::StateKeys, States::StateKeys>> _e_covarianceMatrix;
 
    std::optional<KeyedMatrixXd<States::StateKeys, States::StateKeys>> _n_covarianceMatrix;
};
 
} // namespace NAV