Pos.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 <cstdint>
#include "Navigation/Transformations/CoordinateFrames.hpp"
#include "Navigation/Transformations/Units.hpp"
 
#include "util/Logger/CommonLog.hpp"
#include "NodeData/NodeData.hpp"
#include "util/Container/KeyedMatrix.hpp"
#include "util/Assert.h"
#include <Eigen/src/Core/MatrixBase.h>
 
namespace NAV
{
class Pos : public NodeData
{
  public:
    [[nodiscard]] static std::string type()
    {
        return "Pos";
    }
 
    [[nodiscard]] std::string getType() const override { return type(); }
 
    [[nodiscard]] static std::vector<std::string> parentTypes()
    {
        return { NodeData::type() };
    }
 
    [[nodiscard]] static std::vector<std::string> GetStaticDataDescriptors()
    {
        return {
            "Latitude [deg]",
            "Longitude [deg]",
            "Altitude [m]",
            "North/South [m]",
            "East/West [m]",
            "X-ECEF [m]",
            "Y-ECEF [m]",
            "Z-ECEF [m]",
            "X-ECEF StDev [m]",
            "Y-ECEF StDev [m]",
            "Z-ECEF StDev [m]",
            "XY-ECEF StDev [m]",
            "XZ-ECEF StDev [m]",
            "YZ-ECEF StDev [m]",
            "North StDev [m]",
            "East StDev [m]",
            "Down StDev [m]",
            "NE StDev [m]",
            "ND StDev [m]",
            "ED StDev [m]",
        };
    }
 
    [[nodiscard]] static constexpr size_t GetStaticDescriptorCount() { return 20; }
 
    [[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]
            return rad2deg(latitude());
        case 1: // Longitude [deg]
            return rad2deg(longitude());
        case 2: // Altitude [m]
            return altitude();
        case 3: // North/South [m]
            return CommonLog::calcLocalPosition(lla_position()).northSouth;
        case 4: // East/West [m]
            return CommonLog::calcLocalPosition(lla_position()).eastWest;
        case 5: // X-ECEF [m]
            return e_position().x();
        case 6: // Y-ECEF [m]
            return e_position().y();
        case 7: // Z-ECEF [m]
            return e_position().z();
        case 8: // X-ECEF StDev [m]
            if (auto stDev = e_positionStdev()) { return stDev->get().x(); }
            break;
        case 9: // Y-ECEF StDev [m]
            if (auto stDev = e_positionStdev()) { return stDev->get().y(); }
            break;
        case 10: // Z-ECEF StDev [m]
            if (auto stDev = e_positionStdev()) { return stDev->get().z(); }
            break;
        case 11: // XY-ECEF StDev [m]
            if (e_CovarianceMatrix().has_value()) { return (*e_CovarianceMatrix())(States::PosX, States::PosY); }
            break;
        case 12: // XZ-ECEF StDev [m]
            if (e_CovarianceMatrix().has_value()) { return (*e_CovarianceMatrix())(States::PosX, States::PosZ); }
            break;
        case 13: // YZ-ECEF StDev [m]
            if (e_CovarianceMatrix().has_value()) { return (*e_CovarianceMatrix())(States::PosY, States::PosZ); }
            break;
        case 14: // North StDev [m]
            if (auto stDev = n_positionStdev()) { return stDev->get().x(); }
            break;
        case 15: // East StDev [m]
            if (auto stDev = n_positionStdev()) { return stDev->get().y(); }
            break;
        case 16: // Down StDev [m]
            if (auto stDev = n_positionStdev()) { return stDev->get().z(); }
            break;
        case 17: // NE StDev [m]
            if (n_CovarianceMatrix().has_value()) { return (*n_CovarianceMatrix())(States::PosX, States::PosY); }
            break;
        case 18: // ND StDev [m]
            if (n_CovarianceMatrix().has_value()) { return (*n_CovarianceMatrix())(States::PosX, States::PosZ); }
            break;
        case 19: // ED StDev [m]
            if (n_CovarianceMatrix().has_value()) { return (*n_CovarianceMatrix())(States::PosY, States::PosZ); }
            break;
        default:
            return std::nullopt;
        }
        return std::nullopt;
    }
 
    [[nodiscard]] bool setValueAt(size_t idx, double value) override
    {
        INS_ASSERT(idx < GetStaticDescriptorCount());
        switch (idx)
        {
        case 0: // Latitude [deg]
            _lla_position(0) = deg2rad(value);
            _e_position = trafo::lla2ecef_WGS84(_lla_position);
            break;
        case 1: // Longitude [deg]
            _lla_position(1) = deg2rad(value);
            _e_position = trafo::lla2ecef_WGS84(_lla_position);
            break;
        case 2: // Altitude [m]
            _lla_position(2) = value;
            _e_position = trafo::lla2ecef_WGS84(_lla_position);
            break;
        case 3: // North/South [m]
            _e_position += e_Quat_n() * Eigen::Vector3d(value, 0.0, 0.0);
            _e_position = trafo::lla2ecef_WGS84(_lla_position);
            break;
        case 4: // East/West [m]
            _e_position += e_Quat_n() * Eigen::Vector3d(0.0, value, 0.0);
            _e_position = trafo::lla2ecef_WGS84(_lla_position);
            break;
        case 5: // X-ECEF [m]
            _e_position(0) = value;
            _lla_position = trafo::ecef2lla_WGS84(_e_position);
            break;
        case 6: // Y-ECEF [m]
            _e_position(1) = value;
            _lla_position = trafo::ecef2lla_WGS84(_e_position);
            break;
        case 7: // Z-ECEF [m]
            _e_position(2) = value;
            _lla_position = trafo::ecef2lla_WGS84(_e_position);
            break;
        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]
        default:
            return false;
        }
 
        return true;
    }
 
    /* -------------------------------------------------------------------------------------------------------- */
    /*                                           Rotation Quaternions                                           */
    /* -------------------------------------------------------------------------------------------------------- */
 
    [[nodiscard]] Eigen::Quaterniond e_Quat_n() const
    {
        return trafo::e_Quat_n(latitude(), longitude());
    }
 
    [[nodiscard]] Eigen::Quaterniond n_Quat_e() const
    {
        return e_Quat_n().conjugate();
    }
 
    /* -------------------------------------------------------------------------------------------------------- */
    /*                                                 Position                                                 */
    /* -------------------------------------------------------------------------------------------------------- */
 
    struct States
    {
        States() = delete;
 
        enum StateKeys : uint8_t
        {
            PosX,         
            PosY,         
            PosZ,         
            States_COUNT, 
        };
        inline static const std::vector<StateKeys> Pos = { PosX, PosY, PosZ };
    };
 
    [[nodiscard]] const Eigen::Vector3d& lla_position() const { return _lla_position; }
 
    [[nodiscard]] const double& latitude() const { return lla_position()(0); }
 
    [[nodiscard]] const double& longitude() const { return lla_position()(1); }
 
    [[nodiscard]] const double& altitude() const { return lla_position()(2); }
 
    [[nodiscard]] const Eigen::Vector3d& e_position() const { return _e_position; }
 
    [[nodiscard]] std::optional<std::reference_wrapper<const Eigen::Vector3d>> e_positionStdev() const { return _e_positionStdev; }
 
    [[nodiscard]] std::optional<std::reference_wrapper<const Eigen::Vector3d>> n_positionStdev() const { return _n_positionStdev; }
 
    [[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
    // ###########################################################################################################
 
    template<typename Derived>
    void setPosition_e(const Eigen::MatrixBase<Derived>& e_position)
    {
        _e_position = e_position;
        _lla_position = trafo::ecef2lla_WGS84(e_position);
    }
 
    template<typename Derived>
    void setPosition_lla(const Eigen::MatrixBase<Derived>& lla_position)
    {
        _e_position = trafo::lla2ecef_WGS84(lla_position);
        _lla_position = lla_position;
    }
 
    template<typename Derived, typename Derived2>
    void setPositionAndStdDev_e(const Eigen::MatrixBase<Derived>& e_position, const Eigen::MatrixBase<Derived2>& e_positionCovarianceMatrix)
    {
        setPosition_e(e_position);
        _e_positionStdev = e_positionCovarianceMatrix.diagonal().cwiseSqrt();
        _n_positionStdev = (n_Quat_e() * e_positionCovarianceMatrix * e_Quat_n()).diagonal().cwiseSqrt();
    }
 
    template<typename Derived, typename Derived2>
    void setPositionAndStdDev_lla(const Eigen::MatrixBase<Derived>& lla_position, const Eigen::MatrixBase<Derived2>& n_positionCovarianceMatrix)
    {
        setPosition_lla(lla_position);
        _n_positionStdev = n_positionCovarianceMatrix.diagonal().cwiseSqrt();
        _e_positionStdev = (e_Quat_n() * n_positionCovarianceMatrix * n_Quat_e()).diagonal().cwiseSqrt();
    }
 
    template<typename Derived>
    void setPosCovarianceMatrix_e(const Eigen::MatrixBase<Derived>& e_covarianceMatrix)
    {
        INS_ASSERT_USER_ERROR(e_covarianceMatrix.rows() == 3, "This function needs a 3x3 matrix as input");
        INS_ASSERT_USER_ERROR(e_covarianceMatrix.cols() == 3, "This function needs a 3x3 matrix as input");
 
        _e_covarianceMatrix = KeyedMatrixXd<States::StateKeys,
                                            States::StateKeys>(e_covarianceMatrix,
                                                               States::Pos);
        _n_covarianceMatrix = KeyedMatrixXd<States::StateKeys,
                                            States::StateKeys>(n_Quat_e()
                                                                   * (*_e_covarianceMatrix)(States::Pos, States::Pos)
                                                                   * e_Quat_n(),
                                                               States::Pos);
    }
 
    template<typename Derived>
    void setPosCovarianceMatrix_n(const Eigen::MatrixBase<Derived>& n_covarianceMatrix)
    {
        INS_ASSERT_USER_ERROR(n_covarianceMatrix.rows() == 3, "This function needs a 3x3 matrix as input");
        INS_ASSERT_USER_ERROR(n_covarianceMatrix.cols() == 3, "This function needs a 3x3 matrix as input");
 
        _n_covarianceMatrix = KeyedMatrixXd<States::StateKeys,
                                            States::StateKeys>(n_covarianceMatrix,
                                                               States::Pos);
        _e_covarianceMatrix = KeyedMatrixXd<States::StateKeys,
                                            States::StateKeys>(e_Quat_n()
                                                                   * (*_n_covarianceMatrix)(States::Pos, States::Pos)
                                                                   * n_Quat_e(),
                                                               States::Pos);
    }
 
    /* -------------------------------------------------------------------------------------------------------- */
    /*                                             Member variables                                             */
    /* -------------------------------------------------------------------------------------------------------- */
 
  private:
    Eigen::Vector3d _e_position{ std::nan(""), std::nan(""), std::nan("") };
    Eigen::Vector3d _lla_position{ std::nan(""), std::nan(""), std::nan("") };
 
    std::optional<Eigen::Vector3d> _e_positionStdev;
    std::optional<Eigen::Vector3d> _n_positionStdev;
 
    std::optional<KeyedMatrixXd<States::StateKeys, States::StateKeys>> _e_covarianceMatrix;
 
    std::optional<KeyedMatrixXd<States::StateKeys, States::StateKeys>> _n_covarianceMatrix;
};
 
} // namespace NAV