IRWKF.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 "../PinData.hpp"
#include "Navigation/Math/KalmanFilter.hpp"
#include "NodeData/IMU/ImuObs.hpp"
 
#include <vector>
 
// TODO: Refactor: 'NAV::ImuFusion::IRWKF'
namespace NAV::IRWKF
{
[[nodiscard]] Eigen::MatrixXd initialStateTransitionMatrix_Phi(const double& dt, const uint8_t& numStates);
 
[[nodiscard]] Eigen::MatrixXd initialErrorCovarianceMatrix_P0(const std::vector<Eigen::Vector3d>& initCovariances, const uint8_t& numStates, const bool& imuCharacteristicsIdentical = false);
 
void processNoiseMatrix_Q(Eigen::MatrixXd& Q, const double& dt, const std::vector<Eigen::VectorXd>& processNoiseVariances, const uint8_t& numStates, const bool& imuCharacteristicsIdentical = false);
 
void stateTransitionMatrix_Phi(Eigen::MatrixXd& Phi, const double& dt);
 
[[nodiscard]] Eigen::MatrixXd designMatrix_H(const Eigen::Matrix3d& DCM_accel,
                                             const Eigen::Matrix3d& DCM_gyro,
                                             const size_t& pinIndex,
                                             const uint8_t& numMeasurements,
                                             const uint8_t& numStates,
                                             const uint8_t& numStatesEst,
                                             const uint8_t& numStatesPerPin);
 
KalmanFilter initializeKalmanFilterManually(const size_t& numInputPins,
                                            const std::vector<PinData>& pinData,
                                            const PinDataIRWKF& pinDataIRWKF,
                                            const uint8_t& numStates,
                                            const double& dtInit,
                                            std::vector<Eigen::VectorXd>& processNoiseVariances,
                                            KalmanFilter& kalmanFilter,
                                            const bool& imuCharacteristicsIdentical,
                                            const bool& imuBiasesIdentical);
 
KalmanFilter initializeKalmanFilterAuto(const size_t& nInputPins,
                                        const std::vector<PinData>& pinData,
                                        const PinDataIRWKF& pinDataIRWKF,
                                        const std::vector<size_t>& cumulatedPinIds,
                                        const std::vector<std::shared_ptr<const NAV::ImuObs>>& cumulatedImuObs,
                                        const bool& initJerkAngAcc,
                                        const double& dtInit,
                                        const uint8_t& numStates,
                                        const uint8_t& numMeasurements,
                                        std::vector<Eigen::VectorXd>& processNoiseVariances,
                                        KalmanFilter& kalmanFilter);
 
Eigen::Vector3d mean(const std::vector<std::vector<double>>& sensorType, const size_t& containerPos);
 
Eigen::Vector3d variance(const std::vector<std::vector<double>>& sensorType, const size_t& containerPos);
} // namespace NAV::IRWKF