BsplineKF.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 <vector>
 
namespace NAV::BsplineKF
{
[[nodiscard]] Eigen::MatrixXd initialErrorCovarianceMatrix_P0(const std::vector<Eigen::VectorXd>& 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 rotateCoeffStates(Eigen::MatrixXd& x);
 
void rotateErrorCovariances(Eigen::MatrixXd& P,
                            uint8_t& numStates,
                            const double& sigmaScalingFactorAngRate = 3.0,
                            const double& sigmaScalingFactorAccel = 3.0);
 
[[nodiscard]] Eigen::MatrixXd designMatrix_H(const double& ti,
                                             const double& splineSpacing,
                                             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 PinDataBsplineKF& pinDataBsplineKF,
                                            const uint8_t& numStates,
                                            const double& dtInit,
                                            std::vector<Eigen::VectorXd>& processNoiseVariances,
                                            KalmanFilter& kalmanFilter,
                                            const bool& imuCharacteristicsIdentical,
                                            const bool& imuBiasesIdentical);
 
} // namespace NAV::BsplineKF