RtklibPosObs.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/PosVel.hpp"
#include "util/Eigen.hpp"
#include "Navigation/Transformations/Units.hpp"
 
namespace NAV
{
class RtklibPosObs : public PosVel
{
  public:
#ifdef TESTING
    RtklibPosObs() = default;
 
    RtklibPosObs(const InsTime& insTime,
                 std::optional<Eigen::Vector3d> e_position,
                 std::optional<Eigen::Vector3d> lla_position,
                 std::optional<Eigen::Vector3d> e_velocity,
                 std::optional<Eigen::Vector3d> n_velocity,
                 uint8_t Q,
                 uint8_t ns,
                 std::optional<Eigen::Vector3d> sdXYZ,
                 std::optional<Eigen::Vector3d> sdNED,
                 std::optional<double> sdxy,
                 std::optional<double> sdyz,
                 std::optional<double> sdzx,
                 std::optional<double> sdne,
                 std::optional<double> sded,
                 std::optional<double> sddn,
                 double age,
                 double ratio,
                 std::optional<Eigen::Vector3d> sdvNED,
                 std::optional<double> sdvne,
                 std::optional<double> sdved,
                 std::optional<double> sdvdn,
                 std::optional<Eigen::Vector3d> sdvXYZ,
                 std::optional<double> sdvxy,
                 std::optional<double> sdvyz,
                 std::optional<double> sdvzx)
        : Q(Q), ns(ns), age(age), ratio(ratio)
    {
        this->insTime = insTime;
 
        if (lla_position) { lla_position->head<2>() = deg2rad(lla_position->head<2>()); }
 
        if (e_position && sdXYZ && sdxy && sdzx && sdyz)
        {
            Eigen::Matrix3d cov;
            cov << std::pow(sdXYZ->x(), 2), *sdxy, -(*sdzx),
                -(*sdxy), std::pow(sdXYZ->y(), 2), *sdyz,
                *sdzx, -(*sdyz), std::pow(sdXYZ->z(), 2);
            this->setPositionAndStdDev_e(*e_position, cov);
            this->setPosCovarianceMatrix_e(cov);
        }
        else if (lla_position && sdNED && sdne && sddn && sded)
        {
            Eigen::Matrix3d cov;
            cov << std::pow(sdNED->x(), 2), *sdne, -(*sddn),
                -(*sdne), std::pow(sdNED->y(), 2), *sded,
                *sddn, -(*sded), std::pow(sdNED->z(), 2);
            this->setPositionAndStdDev_lla(*lla_position, cov);
            this->setPosCovarianceMatrix_n(cov);
        }
        else if (e_position) { this->setPosition_e(*e_position); }
        else if (lla_position) { this->setPosition_lla(*lla_position); }
 
        if (e_velocity && sdvXYZ && sdvxy && sdvzx && sdvyz)
        {
            Eigen::Matrix3d cov;
            cov << std::pow(sdvXYZ->x(), 2), *sdvxy, -(*sdvzx),
                -(*sdvxy), std::pow(sdvXYZ->y(), 2), *sdvyz,
                *sdvzx, -(*sdvyz), std::pow(sdvXYZ->z(), 2);
            this->setVelocityAndStdDev_e(*e_velocity, cov);
        }
        else if (n_velocity && sdvNED && sdvne && sdvdn && sdved)
        {
            Eigen::Matrix3d cov;
            cov << std::pow(sdvNED->x(), 2), *sdvne, -(*sdvdn),
                -(*sdvne), std::pow(sdvNED->y(), 2), *sdved,
                *sdvdn, -(*sdved), std::pow(sdvNED->z(), 2);
            this->setVelocityAndStdDev_n(*n_velocity, cov);
        }
        else if (e_velocity) { this->setVelocity_e(*e_velocity); }
        else if (n_velocity) { this->setVelocity_n(*n_velocity); }
    }
#endif
 
    [[nodiscard]] static std::string type()
    {
        return "RtklibPosObs";
    }
 
    [[nodiscard]] static std::vector<std::string> parentTypes()
    {
        auto parent = PosVel::parentTypes();
        parent.push_back(PosVel::type());
        return parent;
    }
 
    [[nodiscard]] static std::vector<std::string> GetStaticDataDescriptors()
    {
        auto desc = PosVel::GetStaticDataDescriptors();
        desc.reserve(GetStaticDescriptorCount());
        desc.emplace_back("Q [-]");
        desc.emplace_back("ns [-]");
        desc.emplace_back("age [s]");
        desc.emplace_back("ratio [-]");
 
        return desc;
    }
 
    [[nodiscard]] static constexpr size_t GetStaticDescriptorCount() { return 43; }
 
    [[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]
        case 20: // Velocity norm [m/s]
        case 21: // X velocity ECEF [m/s]
        case 22: // Y velocity ECEF [m/s]
        case 23: // Z velocity ECEF [m/s]
        case 24: // North velocity [m/s]
        case 25: // East velocity [m/s]
        case 26: // Down velocity [m/s]
        case 27: // X velocity ECEF StDev [m/s]
        case 28: // Y velocity ECEF StDev [m/s]
        case 29: // Z velocity ECEF StDev [m/s]
        case 30: // XY velocity StDev [m]
        case 31: // XZ velocity StDev [m]
        case 32: // YZ velocity StDev [m]
        case 33: // North velocity StDev [m/s]
        case 34: // East velocity StDev [m/s]
        case 35: // Down velocity StDev [m/s]
        case 36: // NE velocity StDev [m]
        case 37: // ND velocity StDev [m]
        case 38: // ED velocity StDev [m]
            return PosVel::getValueAt(idx);
        case 39: // Q [-]
            return Q;
        case 40: // ns [-]
            return ns;
        case 41: // age [s]
            return age;
        case 42: // ratio [-]
            return ratio;
        default:
            return std::nullopt;
        }
        return std::nullopt;
    }
 
    uint8_t Q = 0;
    uint8_t ns = 0;
    double age = std::nan("");
    double ratio = std::nan("");
};
 
} // namespace NAV