InertialIntegrator.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 <functional>
#include <optional>
#include <memory>
 
#include "NodeData/IMU/ImuPos.hpp"
#include "NodeData/State/PosVelAtt.hpp"
 
#include "Navigation/Gravity/Gravity.hpp"
#include "Navigation/Math/NumericalIntegration.hpp"
#include "Navigation/Time/InsTime.hpp"
#include "Navigation/INS/Mechanization.hpp"
 
#include "util/Container/ScrollingBuffer.hpp"
#include "util/Eigen.hpp"
#include "util/Json.hpp"
 
namespace NAV
{
 
class InertialIntegrator
{
  public:
    enum class IntegrationAlgorithm
    {
        SingleStepRungeKutta1, 
        SingleStepRungeKutta2, 
        SingleStepHeun2,       
        SingleStepRungeKutta3, 
        SingleStepHeun3,       
        SingleStepRungeKutta4, 
        MultiStepRK3,          
        MultiStepRK4,          
        COUNT,                 
    };
 
    struct Measurement
    {
        double dt = 0.0;                
        Eigen::Vector3d p_acceleration; 
        Eigen::Vector3d p_angularRate;  
 
        Eigen::Vector3d p_biasAcceleration = Eigen::Vector3d::Zero(); 
        Eigen::Vector3d p_biasAngularRate = Eigen::Vector3d::Zero();  
    };
 
    void reset();
 
    [[nodiscard]] bool hasInitialPosition() const;
 
    void setInitialState(const PosVelAtt& state);
 
    void setState(const PosVelAtt& state);
 
    void setTotalSensorBiases(const Eigen::Vector3d& p_biasAcceleration, const Eigen::Vector3d& p_biasAngularRate);
 
    void applySensorBiasesIncrements(const Eigen::Vector3d& p_deltaBiasAcceleration, const Eigen::Vector3d& p_deltaBiasAngularRate);
 
    void applyStateErrors_n(const Eigen::Vector3d& lla_positionError, const Eigen::Vector3d& n_velocityError, const Eigen::Vector3d& n_attitudeError_b);
 
    void applyStateErrors_e(const Eigen::Vector3d& e_positionError, const Eigen::Vector3d& e_velocityError, const Eigen::Vector3d& e_attitudeError_b);
 
    [[nodiscard]] const ScrollingBuffer<Measurement>& getMeasurements() const;
 
    [[nodiscard]] std::optional<std::reference_wrapper<const PosVelAtt>> getLatestState() const;
 
    [[nodiscard]] const Eigen::Vector3d& p_getLastAccelerationBias() const;
 
    [[nodiscard]] const Eigen::Vector3d& p_getLastAngularRateBias() const;
 
    enum class IntegrationFrame : int
    {
        ECEF, 
        NED,  
    };
 
    [[nodiscard]] IntegrationFrame getIntegrationFrame() const;
 
    [[nodiscard]] std::optional<Eigen::Vector3d> p_calcCurrentAcceleration() const;
 
    [[nodiscard]] std::optional<Eigen::Vector3d> p_calcCurrentAngularRate() const;
 
    std::shared_ptr<PosVelAtt> calcInertialSolution(const InsTime& obsTime, const Eigen::Vector3d& p_acceleration, const Eigen::Vector3d& p_angularRate, const ImuPos& imuPos);
 
    std::shared_ptr<PosVelAtt> calcInertialSolutionDelta(const InsTime& obsTime, const double& dt, Eigen::Vector3d p_deltaVelocity, Eigen::Vector3d p_deltaTheta,
                                                         Eigen::Vector3d p_acceleration, Eigen::Vector3d p_angularRate, const ImuPos& imuPos);
 
  private:
    std::shared_ptr<PosVelAtt> calcInertialSolutionFromMeasurementBuffer(const ImuPos& imuPos);
 
    void setBufferSizes();
 
    ScrollingBuffer<Measurement> _measurements = ScrollingBuffer<Measurement>(2);
    ScrollingBuffer<PosVelAtt> _states = ScrollingBuffer<PosVelAtt>(1);
 
    Eigen::Vector3d p_lastBiasAcceleration = Eigen::Vector3d::Zero(); 
    Eigen::Vector3d p_lastBiasAngularRate = Eigen::Vector3d::Zero();  
 
    // #########################################################################################################################################
 
    IntegrationFrame _integrationFrame = IntegrationFrame::NED;
 
    IntegrationAlgorithm _integrationAlgorithm = IntegrationAlgorithm::SingleStepRungeKutta3;
 
    // #########################################################################################################################################
 
    PosVelAttDerivativeConstants<double> _posVelAttDerivativeConstants;
 
    friend bool InertialIntegratorGui(const char* label, InertialIntegrator& integrator, float width);
    friend void to_json(json& j, const InertialIntegrator& data);
    friend void from_json(const json& j, InertialIntegrator& data);
};
 
bool InertialIntegratorGui(const char* label, InertialIntegrator& integrator, float width = 250.0F);
 
void to_json(json& j, const InertialIntegrator& data);
void from_json(const json& j, InertialIntegrator& data);
 
const char* to_string(InertialIntegrator::IntegrationAlgorithm algorithm);
 
} // namespace NAV