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 <Eigen/src/Core/MatrixBase.h>
 
#include "NodeData/NodeData.hpp"
#include "Navigation/GNSS/SystemModel/MotionModel.hpp"
#include "Navigation/Transformations/CoordinateFrames.hpp"
#include "Navigation/Transformations/Units.hpp"
 
#include "util/Logger/CommonLog.hpp"
#include "util/Container/KeyedMatrix.hpp"
#include "util/Assert.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->x(); }
            break;
        case 9: // Y-ECEF StDev [m]
            if (auto stDev = e_positionStdev()) { return stDev->y(); }
            break;
        case 10: // Z-ECEF StDev [m]
            if (auto stDev = e_positionStdev()) { return stDev->z(); }
            break;
        case 11: // XY-ECEF StDev [m]
            if (_e_covarianceMatrix) { return std::sqrt(std::abs((*_e_covarianceMatrix)(Keys::PosX, Keys::PosY))); }
            break;
        case 12: // XZ-ECEF StDev [m]
            if (_e_covarianceMatrix) { return std::sqrt(std::abs((*_e_covarianceMatrix)(Keys::PosX, Keys::PosZ))); }
            break;
        case 13: // YZ-ECEF StDev [m]
            if (_e_covarianceMatrix) { return std::sqrt(std::abs((*_e_covarianceMatrix)(Keys::PosY, Keys::PosZ))); }
            break;
        case 14: // North StDev [m]
            if (auto stDev = n_positionStdev()) { return stDev->x(); }
            break;
        case 15: // East StDev [m]
            if (auto stDev = n_positionStdev()) { return stDev->y(); }
            break;
        case 16: // Down StDev [m]
            if (auto stDev = n_positionStdev()) { return stDev->z(); }
            break;
        case 17: // NE StDev [m]
            if (_n_covarianceMatrix) { return std::sqrt(std::abs((*_n_covarianceMatrix)(Keys::PosX, Keys::PosY))); }
            break;
        case 18: // ND StDev [m]
            if (_n_covarianceMatrix) { return std::sqrt(std::abs((*_n_covarianceMatrix)(Keys::PosX, Keys::PosZ))); }
            break;
        case 19: // ED StDev [m]
            if (_n_covarianceMatrix) { return std::sqrt(std::abs((*_n_covarianceMatrix)(Keys::PosY, Keys::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                                                 */
    /* -------------------------------------------------------------------------------------------------------- */

    [[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<Eigen::Vector3d> e_positionStdev() const
    {
        if (_e_covarianceMatrix)
        {
            return (*_e_covarianceMatrix)(Keys::Pos<Keys::MotionModelKey>, Keys::Pos<Keys::MotionModelKey>).diagonal().cwiseSqrt();
        }
        return std::nullopt;
    }

    [[nodiscard]] std::optional<Eigen::Vector3d> n_positionStdev() const
    {
        if (_n_covarianceMatrix)
        {
            return (*_n_covarianceMatrix)(Keys::Pos<Keys::MotionModelKey>, Keys::Pos<Keys::MotionModelKey>).diagonal().cwiseSqrt();
        }
        return std::nullopt;
    }

    [[nodiscard]] std::optional<std::reference_wrapper<const KeyedMatrixXd<Keys::MotionModelKey, Keys::MotionModelKey>>> e_CovarianceMatrix() const { return _e_covarianceMatrix; }

    [[nodiscard]] std::optional<std::reference_wrapper<const KeyedMatrixXd<Keys::MotionModelKey, Keys::MotionModelKey>>> 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 DerivedP, typename Derived>
    void setPositionAndCov_e(const Eigen::MatrixBase<DerivedP>& e_position, const Eigen::MatrixBase<Derived>& e_covarianceMatrix)
    {
        setPosition_e(e_position);
        setPosCovarianceMatrix_e(e_covarianceMatrix);
    }

    template<typename DerivedP, typename Derived>
    void setPositionAndCov_n(const Eigen::MatrixBase<DerivedP>& lla_position, const Eigen::MatrixBase<Derived>& n_covarianceMatrix)
    {
        setPosition_lla(lla_position);
        setPosCovarianceMatrix_n(n_covarianceMatrix);
    }

    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<Keys::MotionModelKey, Keys::MotionModelKey>(
            e_covarianceMatrix, Keys::Pos<Keys::MotionModelKey>);
 
        Eigen::Matrix3d J = n_Quat_e().toRotationMatrix();
 
        _n_covarianceMatrix = KeyedMatrixXd<Keys::MotionModelKey, Keys::MotionModelKey>(
            J * e_covarianceMatrix * J.transpose(), Keys::Pos<Keys::MotionModelKey>);
    }

    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<Keys::MotionModelKey, Keys::MotionModelKey>(
            n_covarianceMatrix, Keys::Pos<Keys::MotionModelKey>);
 
        Eigen::Matrix3d J = e_Quat_n().toRotationMatrix();
 
        _e_covarianceMatrix = KeyedMatrixXd<Keys::MotionModelKey, Keys::MotionModelKey>(
            J * n_covarianceMatrix * J.transpose(), Keys::Pos<Keys::MotionModelKey>);
    }
 
    /* -------------------------------------------------------------------------------------------------------- */
    /*                                             Member variables                                             */
    /* -------------------------------------------------------------------------------------------------------- */
 
  protected:
    std::optional<KeyedMatrixXd<Keys::MotionModelKey, Keys::MotionModelKey>> _e_covarianceMatrix;

    std::optional<KeyedMatrixXd<Keys::MotionModelKey, Keys::MotionModelKey>> _n_covarianceMatrix;
 
  private:
    Eigen::Vector3d _e_position{ std::nan(""), std::nan(""), std::nan("") };
    Eigen::Vector3d _lla_position{ std::nan(""), std::nan(""), std::nan("") };
};
 
} // namespace NAV