WiFiPositioning.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 "util/Eigen.hpp"
#include "util/CallbackTimer.hpp"
#include <vector>
 
#include "internal/Node/Node.hpp"
#include "NodeData/WiFi/WiFiObs.hpp"
#include "NodeData/State/Pos.hpp"
#include "NodeData/State/PosVel.hpp"
 
#include "Navigation/Math/KeyedKalmanFilter.hpp"
#include "Navigation/Math/KalmanFilter.hpp"
#include "Navigation/Math/LeastSquares.hpp"
 
namespace NAV
{
class WiFiPositioning : public Node
{
  public:
    WiFiPositioning();
    ~WiFiPositioning() override;
    WiFiPositioning(const WiFiPositioning&) = delete;
    WiFiPositioning(WiFiPositioning&&) = delete;
    WiFiPositioning& operator=(const WiFiPositioning&) = delete;
    WiFiPositioning& operator=(WiFiPositioning&&) = delete;

    [[nodiscard]] static std::string typeStatic();

    [[nodiscard]] std::string type() const override;

    [[nodiscard]] static std::string category();

    void guiConfig() override;

    [[nodiscard]] json save() const override;

    void restore(const json& j) override;
 
  private:
    constexpr static size_t INPUT_PORT_INDEX_WIFI_OBS = 0; 
    constexpr static size_t OUTPUT_PORT_INDEX_WIFISOL = 0; 

    uint8_t _numStates = 6;

    uint8_t _numMeasurements = 1;

    KalmanFilter _kalmanFilter{ _numStates, _numMeasurements }; // TODO: Change to KeyedKalmanFilter
 
    // --------------------------------------------------------------- Gui -----------------------------------------------------------------

    bool initialize() override;

    void deinitialize() override;

    size_t _nWifiInputPins = 1;

    void updateNumberOfInputPins();
 
    // ------------------------------------------------------------ Algorithm --------------------------------------------------------------

    enum class Frame : uint8_t
    {
        ECEF, 
        LLA,  
    };

    Frame _frame = Frame::ECEF;

    enum class SolutionMode : uint8_t
    {
        LSQ, 
        KF,  
    };

    SolutionMode _solutionMode = SolutionMode::LSQ;

    bool _estimateBias = false;

    bool _weightedSolution = false;

    bool _useInitialValues = false;

    struct State
    {
        Eigen::Vector3d e_position = Eigen::Vector3d::Zero();
        Eigen::Vector3d e_velocity = Eigen::Vector3d::Zero();
        double bias = 0;
    };

    State _state;

    State _initialState;

    std::vector<std::string> _deviceMacAddresses{ 1 };

    std::vector<Eigen::Vector3d> _devicePositions{ 1 };

    std::vector<double> _deviceBias{ 1, 0.0 };

    std::vector<double> _deviceScale{ 1, 0.0 };

    size_t _numOfDevices = 0;

    struct Device
    {
        Eigen::Vector3d position{ Eigen::Vector3d::Zero() }; 
        InsTime time;                                        
        double distance{ 0.0 };                              
        double distanceStd{ 0.0 };                           
    };

    std::vector<Device> _devices{ 1 };

    InsTime _lastPredictTime;

    void recvWiFiObs(InputPin::NodeDataQueue& queue, size_t pinIdx);

    LeastSquaresResult<Eigen::VectorXd, Eigen::MatrixXd> lsqSolution();

    void kfSolution();
 
    // ###########################################################################################################

    enum class MeasurementNoiseUnit : uint8_t
    {
        meter2, 
        meter,  
    };

    MeasurementNoiseUnit _measurementNoiseUnit = MeasurementNoiseUnit::meter;

    double _measurementNoise = 10;
 
    // ###########################################################################################################

    enum class ProcessNoiseUnit : uint8_t
    {
        meter2, 
        meter,  
    };

    ProcessNoiseUnit _processNoiseUnit = ProcessNoiseUnit::meter;

    double _processNoise = 10;
 
    // ###########################################################################################################

    enum class InitCovariancePositionUnit : uint8_t
    {
        meter2, 
        meter,  
    };

    InitCovariancePositionUnit _initCovariancePositionUnit = InitCovariancePositionUnit::meter;

    Eigen::Vector3d _initCovariancePosition{ 100, 100, 100 };
 
    // ###########################################################################################################

    enum class InitCovarianceVelocityUnit : uint8_t
    {
        m2_s2, 
        m_s,   
    };

    InitCovarianceVelocityUnit _initCovarianceVelocityUnit = InitCovarianceVelocityUnit::m_s;

    Eigen::Vector3d _initCovarianceVelocity{ 10, 10, 10 };
 
    // ###########################################################################################################

    enum class InitCovarianceBiasUnit : uint8_t
    {
        meter2, 
        meter,  
    };

    InitCovarianceBiasUnit _initCovarianceBiasUnit = InitCovarianceBiasUnit::meter;

    double _initCovarianceBias = 10;
};
} // namespace NAV