AttitudeModel.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 <array>
#include <cmath>
#include <cstdint>
#include <imgui.h>
#include <Eigen/Core>
 
#include "Navigation/GNSS/SystemModel/Units.hpp"
#include "Navigation/Transformations/CoordinateFrames.hpp"
#include "Units.hpp"
#include "util/Container/KeyedMatrix.hpp"
#include "internal/gui/widgets/InputWithUnit.hpp"
#include "MotionModel.hpp"
 
namespace NAV
{

template<typename StateKeyType>
class AttitudeModel
{
  public:
    void initialize()
    {
        for (int i = 0; i < _gui_covarianceAngularVelocity.rows(); i++)
        {
            _covarianceAngularVelocity(i) = convertUnit(_gui_covarianceAngularVelocity(i), _gui_covarianceAngularVelocityUnit);
        }
    }

    template<typename Scalar, int Size, typename Derived>
    void updatePhiAndQ(KeyedMatrix<Scalar, StateKeyType, StateKeyType, Size, Size>& Phi,
                       KeyedMatrix<Scalar, StateKeyType, StateKeyType, Size, Size>& Q,
                       double dt,
                       const double& latitude,
                       const double& longitude,
                       const Eigen::QuaternionBase<Derived>& n_quat_b)
    {
        Phi.template block<4>(Att, Att).setIdentity();
        Q.template block<4>(Att, Att) = calcProcessNoiseMatrix(dt, latitude, longitude, n_quat_b)(all, all);
    }

    template<typename Derived>
    [[nodiscard]] std::pair<KeyedMatrix4d<StateKeyType>, KeyedMatrix4d<StateKeyType>>
        calcPhiAndQ(double dt, const double& latitude, const double& longitude, const Eigen::QuaternionBase<Derived>& n_quat_b)
    {
        KeyedMatrix4d<StateKeyType> Phi(Eigen::Matrix4d::Zero(), Att, Att);
        KeyedMatrix4d<StateKeyType> Q(Eigen::Matrix4d::Zero(), Att, Att);
        updatePhiAndQ(Phi, Q, dt, latitude, longitude, n_quat_b);
 
        return { Phi, Q };
    }

    bool ShowGui(float itemWidth, float unitWidth, const char* id)
    {
        bool changed = false;
 
        if (gui::widgets::InputDouble3LWithUnit(fmt::format("{} of the angular velocity due to user motion (Roll, Pitch, Yaw)##{}",
                                                            _gui_covarianceAngularVelocityUnit == Units::CovarianceAngularVelocityUnits::deg_sqrts
                                                                    || _gui_covarianceAngularVelocityUnit == Units::CovarianceAngularVelocityUnits::degsqrts_min
                                                                    || _gui_covarianceAngularVelocityUnit == Units::CovarianceAngularVelocityUnits::rad_sqrts
                                                                ? "StdDev"
                                                                : "Variance",
                                                            id)
                                                    .c_str(),
                                                itemWidth, unitWidth, _gui_covarianceAngularVelocity.data(), 0.0, std::numeric_limits<double>::max(),
                                                _gui_covarianceAngularVelocityUnit, MakeComboItems<Units::CovarianceAngularVelocityUnits>().c_str(),
                                                "%.1e", ImGuiInputTextFlags_CharsScientific))
        {
            LOG_DEBUG("{}: _gui_covarianceAngularVelocity changed to {}", id, _gui_covarianceAngularVelocity.transpose());
            LOG_DEBUG("{}: _gui_covarianceAngularVelocityUnit changed to {}", id, to_string(_gui_covarianceAngularVelocityUnit));
            changed = true;
        }
 
        return changed;
    }
 
  private:
    const std::array<StateKeyType, 4>& Att = Keys::Att<StateKeyType>;

    template<typename Derived>
    [[nodiscard]] KeyedMatrix4d<StateKeyType>
        calcProcessNoiseMatrix(double dt, double latitude, double longitude, const Eigen::QuaternionBase<Derived>& n_quat_b) const
    {
        // Scaling matrix in n-frame
        Eigen::Matrix3d S_n = _covarianceAngularVelocity.asDiagonal();
 
        Eigen::Matrix4d n_covQuat_b = trafo::covRPY2quat(S_n, n_quat_b);
        Eigen::Matrix4d e_J_n = trafo::covQuat2QuatJacobian(trafo::e_Quat_n(latitude, longitude));
 
        // Scaling matrix in e-frame
        Eigen::Matrix4d S_e = e_J_n * n_covQuat_b * e_J_n.transpose();
 
        return { dt * S_e, Att, Att };
    }

    Units::CovarianceAngularVelocityUnits _gui_covarianceAngularVelocityUnit = Units::CovarianceAngularVelocityUnits::deg_sqrts;
    Eigen::Vector3d _gui_covarianceAngularVelocity = { 100.0, 100.0, 100.0 } /* [ deg / √(s) ] */;

    Eigen::Vector3d _covarianceAngularVelocity;

    friend void to_json(json& j, const AttitudeModel& data)
    {
        j = {
            { "covarianceAngularVelocityUnit", data._gui_covarianceAngularVelocityUnit },
            { "covarianceAngularVelocity", data._gui_covarianceAngularVelocity },
        };
    }

    friend void from_json(const json& j, AttitudeModel& data)
    {
        if (j.contains("covarianceAngularVelocityUnit")) { j.at("covarianceAngularVelocityUnit").get_to(data._gui_covarianceAngularVelocityUnit); }
        if (j.contains("covarianceAngularVelocity")) { j.at("covarianceAngularVelocity").get_to(data._gui_covarianceAngularVelocity); }
    }
};
 
} // namespace NAV