ProcessNoise.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 <Eigen/Core>
 
#include "Navigation/GNSS/SystemModel/MotionModel.hpp"
#include "Navigation/INS/Keys.hpp"
#include "Navigation/Transformations/CoordinateFrames.hpp"
#include "util/Container/KeyedMatrix.hpp"
 
namespace NAV
{

Eigen::Matrix3d G_RandomWalk(const Eigen::Vector3d& sigma2);

Eigen::Matrix3d G_GaussMarkov1(const Eigen::Vector3d& sigma2, const Eigen::Vector3d& beta);

[[nodiscard]] Eigen::Vector3d psd2BiasGaussMarkov(const Eigen::Vector3d& sigma2_bd, const Eigen::Vector3d& tau_bd);

[[nodiscard]] Eigen::Matrix3d Q_psi_psi(const Eigen::Vector3d& S_rg, const Eigen::Vector3d& S_bgd, const double& tau_s);

[[nodiscard]] Eigen::Matrix3d ien_Q_dv_psi(const Eigen::Vector3d& S_rg, const Eigen::Vector3d& S_bgd, const Eigen::Matrix3d& ien_F_21, const double& tau_s);

[[nodiscard]] Eigen::Matrix3d ien_Q_dv_dv(const Eigen::Vector3d& S_ra, const Eigen::Vector3d& S_bad, const Eigen::Vector3d& S_rg, const Eigen::Vector3d& S_bgd, const Eigen::Matrix3d& ien_F_21, const double& tau_s);

[[nodiscard]] Eigen::Matrix3d ien_Q_dv_domega(const Eigen::Vector3d& S_bgd, const Eigen::Matrix3d& ien_F_21, const Eigen::Matrix3d& ien_Dcm_b, const double& tau_s);

[[nodiscard]] Eigen::Matrix3d n_Q_dr_psi(const Eigen::Vector3d& S_rg, const Eigen::Vector3d& S_bgd, const Eigen::Matrix3d& n_F_21, const Eigen::Matrix3d& T_rn_p, const double& tau_s);

[[nodiscard]] Eigen::Matrix3d ie_Q_dr_psi(const Eigen::Vector3d& S_rg, const Eigen::Vector3d& S_bgd, const Eigen::Matrix3d& ie_F_21, const double& tau_s);

[[nodiscard]] Eigen::Matrix3d n_Q_dr_dv(const Eigen::Vector3d& S_ra, const Eigen::Vector3d& S_bad, const Eigen::Vector3d& S_rg, const Eigen::Vector3d& S_bgd, const Eigen::Matrix3d& n_F_21, const Eigen::Matrix3d& T_rn_p, const double& tau_s);

[[nodiscard]] Eigen::Matrix3d ie_Q_dr_dv(const Eigen::Vector3d& S_ra, const Eigen::Vector3d& S_bad, const Eigen::Vector3d& S_rg, const Eigen::Vector3d& S_bgd, const Eigen::Matrix3d& ie_F_21, const double& tau_s);

[[nodiscard]] Eigen::Matrix3d n_Q_dr_dr(const Eigen::Vector3d& S_ra, const Eigen::Vector3d& S_bad, const Eigen::Vector3d& S_rg, const Eigen::Vector3d& S_bgd, const Eigen::Matrix3d& n_F_21, const Eigen::Matrix3d& T_rn_p, const double& tau_s);

[[nodiscard]] Eigen::Matrix3d ie_Q_dr_dr(const Eigen::Vector3d& S_ra, const Eigen::Vector3d& S_bad, const Eigen::Vector3d& S_rg, const Eigen::Vector3d& S_bgd, const Eigen::Matrix3d& ie_F_21, const double& tau_s);

[[nodiscard]] Eigen::Matrix3d n_Q_dr_df(const Eigen::Vector3d& S_bad, const Eigen::Matrix3d& T_rn_p, const Eigen::Matrix3d& n_Dcm_b, const double& tau_s);

[[nodiscard]] Eigen::Matrix3d ie_Q_dr_df(const Eigen::Vector3d& S_bad, const Eigen::Matrix3d& ie_Dcm_b, const double& tau_s);

[[nodiscard]] Eigen::Matrix3d n_Q_dr_domega(const Eigen::Vector3d& S_bgd, const Eigen::Matrix3d& n_F_21, const Eigen::Matrix3d& T_rn_p, const Eigen::Matrix3d& n_Dcm_b, const double& tau_s);

[[nodiscard]] Eigen::Matrix3d ie_Q_dr_domega(const Eigen::Vector3d& S_bgd, const Eigen::Matrix3d& ie_F_21, const Eigen::Matrix3d& ie_Dcm_b, const double& tau_s);

[[nodiscard]] Eigen::Matrix3d Q_df_dv(const Eigen::Vector3d& S_bad, const Eigen::Matrix3d& b_Dcm_ien, const double& tau_s);

[[nodiscard]] Eigen::Matrix3d Q_df_df(const Eigen::Vector3d& S_bad, const double& tau_s);

[[nodiscard]] Eigen::Matrix3d Q_domega_psi(const Eigen::Vector3d& S_bgd, const Eigen::Matrix3d& b_Dcm_ien, const double& tau_s);

[[nodiscard]] Eigen::Matrix3d Q_domega_domega(const Eigen::Vector3d& S_bgd, const double& tau_s);

[[nodiscard]] Eigen::Matrix2d Q_gnss(const double& S_cPhi, const double& S_cf, const double& tau_s);

template<typename KeyType, typename T>
[[nodiscard]] KeyedMatrixX<T, KeyType, KeyType> e_noiseInput_G(const Eigen::Quaterniond& b_quat_p,
                                                               const Eigen::Quaternion<T>& e_quat_b,
                                                               bool accelBiases, bool gyroBiases,
                                                               bool accelScaleFactor, bool gyroScaleFactor,
                                                               bool accelMisalignment, bool gyroMisalignment)
 
{
    std::vector<KeyType> rowKeys;
    rowKeys.reserve(3 + 3 + 4
                    + 3 * (static_cast<size_t>(accelBiases) + static_cast<size_t>(gyroBiases))
                    + 3 * (static_cast<size_t>(accelScaleFactor) + static_cast<size_t>(gyroScaleFactor))
                    + 4 * (static_cast<size_t>(accelMisalignment) + static_cast<size_t>(gyroMisalignment)));
    std::vector<KeyType> colKeys;
    colKeys.reserve(3 + 3 + 3
                    + 3 * (static_cast<size_t>(accelBiases) + static_cast<size_t>(gyroBiases))
                    + 3 * (static_cast<size_t>(accelScaleFactor) + static_cast<size_t>(gyroScaleFactor)));
 
    constexpr std::array<KeyType, 3> AngleErrorKeys = { Keys::AttQ1, Keys::AttQ2, Keys::AttQ3 }; // 3 quaternion keys are used for the angle error
 
    rowKeys.insert(rowKeys.end(), Keys::PosVelAtt<KeyType>.begin(), Keys::PosVelAtt<KeyType>.end());
    colKeys.insert(colKeys.end(), Keys::PosVel<KeyType>.begin(), Keys::PosVel<KeyType>.end());
    colKeys.insert(colKeys.end(), AngleErrorKeys.begin(), AngleErrorKeys.end());
 
    if (accelBiases)
    {
        rowKeys.insert(rowKeys.end(), Keys::AccelBias<KeyType>.begin(), Keys::AccelBias<KeyType>.end());
        colKeys.insert(colKeys.end(), Keys::AccelBias<KeyType>.begin(), Keys::AccelBias<KeyType>.end());
    }
    if (gyroBiases)
    {
        rowKeys.insert(rowKeys.end(), Keys::GyroBias<KeyType>.begin(), Keys::GyroBias<KeyType>.end());
        colKeys.insert(colKeys.end(), Keys::GyroBias<KeyType>.begin(), Keys::GyroBias<KeyType>.end());
    }
    if (accelScaleFactor)
    {
        rowKeys.insert(rowKeys.end(), Keys::AccelScaleFactor<KeyType>.begin(), Keys::AccelScaleFactor<KeyType>.end());
        colKeys.insert(colKeys.end(), Keys::AccelScaleFactor<KeyType>.begin(), Keys::AccelScaleFactor<KeyType>.end());
    }
    if (gyroScaleFactor)
    {
        rowKeys.insert(rowKeys.end(), Keys::GyroScaleFactor<KeyType>.begin(), Keys::GyroScaleFactor<KeyType>.end());
        colKeys.insert(colKeys.end(), Keys::GyroScaleFactor<KeyType>.begin(), Keys::GyroScaleFactor<KeyType>.end());
    }
    if (accelMisalignment) { rowKeys.insert(rowKeys.end(), Keys::AccelMisalignment<KeyType>.begin(), Keys::AccelMisalignment<KeyType>.end()); }
    if (gyroMisalignment) { rowKeys.insert(rowKeys.end(), Keys::GyroMisalignment<KeyType>.begin(), Keys::GyroMisalignment<KeyType>.end()); }
 
    KeyedMatrixX<T, KeyType, KeyType> G(Eigen::MatrixX<T>::Zero(static_cast<int>(rowKeys.size()), static_cast<int>(colKeys.size())), rowKeys, colKeys);
 
    Eigen::Quaternion<T> e_quat_p = e_quat_b * b_quat_p.cast<T>();
    G(Keys::Vel<KeyType>, Keys::Vel<KeyType>) = e_quat_p.toRotationMatrix();
    G(Keys::Att<KeyType>, AngleErrorKeys) = trafo::covRPY2quatJacobian(e_quat_p) * e_quat_p.toRotationMatrix();
    if (accelBiases) { G(Keys::AccelBias<KeyType>, Keys::AccelBias<KeyType>).setIdentity(); }
    if (gyroBiases) { G(Keys::GyroBias<KeyType>, Keys::GyroBias<KeyType>).setIdentity(); }
    if (accelScaleFactor) { G(Keys::AccelScaleFactor<KeyType>, Keys::AccelScaleFactor<KeyType>).setIdentity(); }
    if (gyroScaleFactor) { G(Keys::GyroScaleFactor<KeyType>, Keys::GyroScaleFactor<KeyType>).setIdentity(); }
 
    return G;
}

template<typename KeyType, typename T>
[[nodiscard]] KeyedMatrixX<T, KeyType, KeyType> e_noiseScale_W(const Eigen::Vector3d& sigma2_ra, const Eigen::Vector3d& sigma2_rg,
                                                               const Eigen::Vector3d& sigma2_bad, const Eigen::Vector3d& sigma2_bgd,
                                                               const Eigen::Vector3d& sigma2_sad, const Eigen::Vector3d& sigma2_sgd,
                                                               bool accelBiases, bool gyroBiases,
                                                               bool accelScaleFactor, bool gyroScaleFactor)
 
{
    std::vector<KeyType> keys;
    keys.reserve(3 + 3 + 3
                 + 3 * (static_cast<size_t>(accelBiases) + static_cast<size_t>(gyroBiases))
                 + 3 * (static_cast<size_t>(accelScaleFactor) + static_cast<size_t>(gyroScaleFactor)));
 
    constexpr std::array<KeyType, 3> AngleErrorKeys = { Keys::AttQ1, Keys::AttQ2, Keys::AttQ3 }; // 3 quaternion keys are used for the angle error
    keys.insert(keys.end(), Keys::PosVel<KeyType>.begin(), Keys::PosVel<KeyType>.end());
    keys.insert(keys.end(), AngleErrorKeys.begin(), AngleErrorKeys.end());
 
    if (accelBiases) { keys.insert(keys.end(), Keys::AccelBias<KeyType>.begin(), Keys::AccelBias<KeyType>.end()); }
    if (gyroBiases) { keys.insert(keys.end(), Keys::GyroBias<KeyType>.begin(), Keys::GyroBias<KeyType>.end()); }
    if (accelScaleFactor) { keys.insert(keys.end(), Keys::AccelScaleFactor<KeyType>.begin(), Keys::AccelScaleFactor<KeyType>.end()); }
    if (gyroScaleFactor) { keys.insert(keys.end(), Keys::GyroScaleFactor<KeyType>.begin(), Keys::GyroScaleFactor<KeyType>.end()); }
 
    KeyedMatrixX<T, KeyType, KeyType> W(Eigen::MatrixX<T>::Zero(static_cast<int>(keys.size()), static_cast<int>(keys.size())), keys, keys);
 
    W(Keys::Vel<KeyType>, Keys::Vel<KeyType>).diagonal() = sigma2_ra.cast<T>();
    W(AngleErrorKeys, AngleErrorKeys).diagonal() = sigma2_rg.cast<T>();
    if (accelBiases) { W(Keys::AccelBias<KeyType>, Keys::AccelBias<KeyType>).diagonal() = sigma2_bad.cast<T>(); }
    if (gyroBiases) { W(Keys::GyroBias<KeyType>, Keys::GyroBias<KeyType>).diagonal() = sigma2_bgd.cast<T>(); }
    if (accelScaleFactor) { W(Keys::AccelScaleFactor<KeyType>, Keys::AccelScaleFactor<KeyType>).diagonal() = sigma2_sad.cast<T>(); }
    if (gyroScaleFactor) { W(Keys::GyroScaleFactor<KeyType>, Keys::GyroScaleFactor<KeyType>).diagonal() = sigma2_sgd.cast<T>(); }
 
    return W;
}
 
} // namespace NAV