| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // This file is part of INSTINCT, the INS Toolkit for Integrated | ||
| 2 | // Navigation Concepts and Training by the Institute of Navigation of | ||
| 3 | // the University of Stuttgart, Germany. | ||
| 4 | // | ||
| 5 | // This Source Code Form is subject to the terms of the Mozilla Public | ||
| 6 | // License, v. 2.0. If a copy of the MPL was not distributed with this | ||
| 7 | // file, You can obtain one at https://mozilla.org/MPL/2.0/. | ||
| 8 | |||
| 9 | #include "BsplineKF.hpp" | ||
| 10 | |||
| 11 | #include "QuadraticBsplines.hpp" | ||
| 12 | |||
| 13 | #include "Navigation/Transformations/Units.hpp" | ||
| 14 | #include "util/Logger.hpp" | ||
| 15 | |||
| 16 | 3 | Eigen::MatrixXd NAV::BsplineKF::initialErrorCovarianceMatrix_P0(const std::vector<Eigen::VectorXd>& initCovariances, const uint8_t& numStates, const bool& imuCharacteristicsIdentical) | |
| 17 | { | ||
| 18 |
2/4✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
|
3 | Eigen::MatrixXd P = Eigen::MatrixXd::Zero(numStates, numStates); |
| 19 | |||
| 20 |
4/8✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
|
3 | P.block<9, 9>(0, 0).diagonal() = initCovariances.at(0); // Angular rate B-spline factors |
| 21 |
4/8✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
|
3 | P.block<9, 9>(9, 9).diagonal() = initCovariances.at(1); // Acceleration B-spline factors |
| 22 | |||
| 23 | // Biases | ||
| 24 | 3 | size_t j = 2; | |
| 25 |
2/2✓ Branch 0 taken 12 times.
✓ Branch 1 taken 3 times.
|
15 | for (uint32_t i = 18; i < numStates; i += 6) |
| 26 | { | ||
| 27 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 12 times.
|
12 | if (!imuCharacteristicsIdentical) |
| 28 | { | ||
| 29 | ✗ | j = 2 + (i - 18) / 3; // access bias variances for each sensor from the third element onwards | |
| 30 | } | ||
| 31 |
4/8✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 12 times.
✗ Branch 11 not taken.
|
12 | P.block<3, 3>(i, i).diagonal() = initCovariances.at(j); |
| 32 |
4/8✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 12 times.
✗ Branch 11 not taken.
|
12 | P.block<3, 3>(i + 3, i + 3).diagonal() = initCovariances.at(j + 1); |
| 33 | } | ||
| 34 | |||
| 35 | 3 | return P; | |
| 36 | ✗ | } | |
| 37 | |||
| 38 | 7485 | void NAV::BsplineKF::processNoiseMatrix_Q(Eigen::MatrixXd& Q, const double& dt, const std::vector<Eigen::VectorXd>& processNoiseVariances, const uint8_t& numStates, const bool& imuCharacteristicsIdentical) | |
| 39 | { | ||
| 40 |
5/10✓ Branch 1 taken 7485 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7485 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7485 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7485 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 7485 times.
✗ Branch 14 not taken.
|
7485 | Q.block<9, 9>(0, 0).diagonal() = processNoiseVariances.at(0) * dt; // Random walk of the angular rate B-spline factors |
| 41 |
5/10✓ Branch 1 taken 7485 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7485 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7485 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7485 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 7485 times.
✗ Branch 14 not taken.
|
7485 | Q.block<9, 9>(9, 9).diagonal() = processNoiseVariances.at(1) * dt; // Random walk of the acceleration B-spline factors |
| 42 | |||
| 43 | // Random walk of the bias states | ||
| 44 | 7485 | size_t j = 2; | |
| 45 |
2/2✓ Branch 0 taken 29940 times.
✓ Branch 1 taken 7485 times.
|
37425 | for (uint32_t i = 18; i < numStates; i += 6) // FIXME: i = 'numStatesEst' better than '12'? (magic numbers) |
| 46 | { | ||
| 47 |
2/2✓ Branch 0 taken 29928 times.
✓ Branch 1 taken 12 times.
|
29940 | if (!imuCharacteristicsIdentical) |
| 48 | { | ||
| 49 | 29928 | j = 2 + (i - 18) / 3; // access 2 bias variances for each sensor from the third element onwards | |
| 50 | } | ||
| 51 |
5/10✓ Branch 1 taken 29940 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 29940 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 29940 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 29940 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 29940 times.
✗ Branch 14 not taken.
|
29940 | Q.block<3, 3>(i, i).diagonal() = processNoiseVariances.at(j) * dt; // variance for the process noise of the angular rate |
| 52 |
5/10✓ Branch 1 taken 29940 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 29940 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 29940 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 29940 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 29940 times.
✗ Branch 14 not taken.
|
29940 | Q.block<3, 3>(i + 3, i + 3).diagonal() = processNoiseVariances.at(j + 1) * dt; // variance for the process noise of the acceleration |
| 53 | } | ||
| 54 | 7485 | } | |
| 55 | |||
| 56 | 61 | void NAV::BsplineKF::rotateCoeffStates(Eigen::MatrixXd& x) | |
| 57 | { | ||
| 58 |
3/6✓ Branch 1 taken 61 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 61 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 61 times.
✗ Branch 8 not taken.
|
61 | x.block<6, 1>(0, 0) = x.block<6, 1>(3, 0); // Angular rate coeffs (c_{omega,k+1} = c_{omega,k}) |
| 59 |
3/6✓ Branch 1 taken 61 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 61 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 61 times.
✗ Branch 8 not taken.
|
61 | x.block<6, 1>(9, 0) = x.block<6, 1>(9, 0); // Acceleration coeffs (c_{f,k+1} = c_{f,k}) |
| 60 | 61 | } | |
| 61 | |||
| 62 | 61 | void NAV::BsplineKF::rotateErrorCovariances(Eigen::MatrixXd& P, uint8_t& numStates, const double& sigmaScalingFactorAngRate, const double& sigmaScalingFactorAccel) | |
| 63 | { | ||
| 64 | 61 | int nCoeffsPerStackedBspline = 9; | |
| 65 | 61 | auto nCoeffsRemainingAngRate = static_cast<int>(numStates) - nCoeffsPerStackedBspline; | |
| 66 | 61 | auto nCoeffsRemainingAccel = static_cast<int>(numStates) - 2 * nCoeffsPerStackedBspline; | |
| 67 | |||
| 68 | // Shift all error covariances corresponding to angular rate B-spline coeffs | ||
| 69 |
3/6✓ Branch 1 taken 61 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 61 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 61 times.
✗ Branch 8 not taken.
|
61 | P.block<6, 6>(0, 0) = P.block<6, 6>(3, 3); // Variances of the angular rate |
| 70 |
3/6✓ Branch 1 taken 61 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 61 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 61 times.
✗ Branch 8 not taken.
|
61 | P.block(0, 9, 6, nCoeffsRemainingAngRate) = P.block(3, 9, 6, nCoeffsRemainingAngRate); // Upper right corner |
| 71 |
3/6✓ Branch 1 taken 61 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 61 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 61 times.
✗ Branch 8 not taken.
|
61 | P.block(9, 0, nCoeffsRemainingAngRate, 6) = P.block(9, 3, nCoeffsRemainingAngRate, 6); // Lower left corner |
| 72 | |||
| 73 | // Set angular rate error covariances for the new B-spline coeff | ||
| 74 |
3/6✓ Branch 1 taken 61 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 61 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 61 times.
✗ Branch 8 not taken.
|
61 | P.block(0, 6, numStates, 3) = Eigen::MatrixXd::Zero(numStates, 3); // Column |
| 75 |
3/6✓ Branch 1 taken 61 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 61 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 61 times.
✗ Branch 8 not taken.
|
61 | P.block(6, 0, 3, numStates) = Eigen::MatrixXd::Zero(3, numStates); // Row |
| 76 |
6/12✓ Branch 1 taken 61 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 61 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 61 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 61 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 61 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 61 times.
✗ Branch 18 not taken.
|
61 | P.block<3, 3>(6, 6).diagonal() = std::pow(sigmaScalingFactorAngRate, 2) * P.block<3, 3>(3, 3).diagonal(); // Variance (i.e. squared) |
| 77 | |||
| 78 | // Shift all error covariances corresponding to acceleration B-spline coeffs | ||
| 79 |
3/6✓ Branch 1 taken 61 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 61 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 61 times.
✗ Branch 8 not taken.
|
61 | P.block<6, 6>(9, 9) = P.block<6, 6>(12, 12); // Variances of the acceleration |
| 80 |
3/6✓ Branch 1 taken 61 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 61 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 61 times.
✗ Branch 8 not taken.
|
61 | P.block<9, 6>(0, 9) = P.block<9, 6>(0, 12); // Top middle part |
| 81 |
3/6✓ Branch 1 taken 61 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 61 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 61 times.
✗ Branch 8 not taken.
|
61 | P.block(9, 18, 6, nCoeffsRemainingAccel) = P.block(12, 18, 6, nCoeffsRemainingAccel); // Middle right part |
| 82 |
3/6✓ Branch 1 taken 61 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 61 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 61 times.
✗ Branch 8 not taken.
|
61 | P.block<6, 9>(9, 0) = P.block<6, 9>(12, 0); // Middle left part |
| 83 |
3/6✓ Branch 1 taken 61 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 61 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 61 times.
✗ Branch 8 not taken.
|
61 | P.block(18, 9, nCoeffsRemainingAccel, 6) = P.block(18, 12, nCoeffsRemainingAccel, 6); // Lower middle part |
| 84 | |||
| 85 | // Set acceleration error covariances for the new B-spline coeff | ||
| 86 |
3/6✓ Branch 1 taken 61 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 61 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 61 times.
✗ Branch 8 not taken.
|
61 | P.block(0, 15, numStates, 3) = Eigen::MatrixXd::Zero(numStates, 3); // Column |
| 87 |
3/6✓ Branch 1 taken 61 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 61 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 61 times.
✗ Branch 8 not taken.
|
61 | P.block(15, 0, 3, numStates) = Eigen::MatrixXd::Zero(3, numStates); // Row |
| 88 |
6/12✓ Branch 1 taken 61 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 61 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 61 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 61 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 61 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 61 times.
✗ Branch 18 not taken.
|
61 | P.block<3, 3>(15, 15).diagonal() = std::pow(sigmaScalingFactorAccel, 2) * P.block<3, 3>(12, 12).diagonal(); // Variance (i.e. squared) |
| 89 | 61 | } | |
| 90 | |||
| 91 | 7482 | Eigen::MatrixXd NAV::BsplineKF::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) | |
| 92 | { | ||
| 93 |
2/4✓ Branch 1 taken 7482 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7482 times.
✗ Branch 5 not taken.
|
7482 | Eigen::MatrixXd H = Eigen::MatrixXd::Zero(numMeasurements, numStates); |
| 94 | |||
| 95 |
1/2✓ Branch 1 taken 7482 times.
✗ Branch 2 not taken.
|
7482 | auto qBspline = NAV::BsplineKF::quadraticBsplines(ti, splineSpacing); |
| 96 | |||
| 97 | // Set quadratic B-splines in design matrix H | ||
| 98 |
2/2✓ Branch 0 taken 22446 times.
✓ Branch 1 taken 7482 times.
|
29928 | for (size_t knotIdx = 0; knotIdx < 3; knotIdx++) |
| 99 | { | ||
| 100 | 22446 | auto stateIdx = static_cast<Eigen::Index>(3 * knotIdx); | |
| 101 |
6/12✓ Branch 1 taken 22446 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 22446 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 22446 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 22446 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 22446 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 22446 times.
✗ Branch 17 not taken.
|
22446 | H.block<3, 3>(0, stateIdx).diagonal() = qBspline.at(knotIdx) * Eigen::Vector3d::Ones(); |
| 102 | LOG_DATA("stateIdx = {}, knotIdx = {}, H =\n{}", stateIdx, knotIdx, H); | ||
| 103 | } | ||
| 104 | |||
| 105 | // Set the three stacked quadratic B-spline elements for the acceleration (analogous to the angular rate block) | ||
| 106 |
5/10✓ Branch 1 taken 7482 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7482 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7482 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7482 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 7482 times.
✗ Branch 14 not taken.
|
7482 | H.block<3, 9>(3, 9) = DCM_accel.transpose() * H.block<3, 9>(0, 0); |
| 107 | // Rotate the angular rate blocks | ||
| 108 |
5/10✓ Branch 1 taken 7482 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7482 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7482 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7482 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 7482 times.
✗ Branch 14 not taken.
|
7482 | H.block<3, 9>(0, 0) = DCM_gyro.transpose() * H.block<3, 9>(0, 0); |
| 109 | LOG_DATA("H =\n{}", H); | ||
| 110 | |||
| 111 | // Map the biases to the states as in the IRWKF | ||
| 112 |
2/2✓ Branch 0 taken 5981 times.
✓ Branch 1 taken 1501 times.
|
7482 | if (pinIndex > 0) |
| 113 | { | ||
| 114 | 5981 | const auto stateIndex = static_cast<uint8_t>(numStatesEst + numStatesPerPin * (pinIndex - 1)); | |
| 115 | LOG_DATA("stateIndex = {}", stateIndex); | ||
| 116 | |||
| 117 |
3/6✓ Branch 1 taken 5981 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5981 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5981 times.
✗ Branch 8 not taken.
|
5981 | H.block<6, 6>(0, stateIndex) = Eigen::MatrixXd::Identity(6, 6); |
| 118 | } | ||
| 119 | |||
| 120 | 14964 | return H; | |
| 121 | ✗ | } | |
| 122 | |||
| 123 | 3 | NAV::KalmanFilter NAV::BsplineKF::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) | |
| 124 | { | ||
| 125 | // ------------------------------------------ State vector x --------------------------------------------- | ||
| 126 | 3 | std::vector<Eigen::VectorXd> initStates; | |
| 127 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | initStates.resize(numStates); |
| 128 | |||
| 129 | // Initial B-spline coefficients of the angular rate in [rad/s] | ||
| 130 |
1/2✓ Branch 0 taken 3 times.
✗ Branch 1 not taken.
|
3 | if (pinDataBsplineKF.initCoeffsAngularRateUnit == PinData::AngRateUnit::deg_s) |
| 131 | { | ||
| 132 |
2/4✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
|
3 | initStates[0] = deg2rad(pinDataBsplineKF.initCoeffsAngRate); |
| 133 | } | ||
| 134 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 3 times.
|
3 | if (pinDataBsplineKF.initCoeffsAngularRateUnit == PinData::AngRateUnit::rad_s) |
| 135 | { | ||
| 136 | ✗ | initStates[0] = pinDataBsplineKF.initCoeffsAngRate; | |
| 137 | } | ||
| 138 | |||
| 139 | // Initial B-spline coefficients of the acceleration in [m/s] | ||
| 140 |
1/2✓ Branch 0 taken 3 times.
✗ Branch 1 not taken.
|
3 | if (pinDataBsplineKF.initCoeffsAccelUnit == PinData::AccelerationUnit::m_s2) |
| 141 | { | ||
| 142 |
1/2✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
|
3 | initStates[1] = pinDataBsplineKF.initCoeffsAccel; |
| 143 | } | ||
| 144 | |||
| 145 | 3 | size_t pinDataBiasesIdenticalIdx = 1; | |
| 146 |
2/2✓ Branch 0 taken 12 times.
✓ Branch 1 taken 3 times.
|
15 | for (size_t pinIndex = 1; pinIndex < numInputPins; ++pinIndex) |
| 147 | { | ||
| 148 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 12 times.
|
12 | if (!imuBiasesIdentical) |
| 149 | { | ||
| 150 | ✗ | pinDataBiasesIdenticalIdx = pinIndex; | |
| 151 | } | ||
| 152 | |||
| 153 | // Initial bias of the angular rate in [rad/s] | ||
| 154 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 12 times.
|
12 | if (pinData[pinDataBiasesIdenticalIdx].initAngularRateBiasUnit == PinData::AngRateUnit::rad_s) |
| 155 | { | ||
| 156 | ✗ | initStates[2 * pinIndex] = pinData[pinDataBiasesIdenticalIdx].initAngularRateBias.array(); | |
| 157 | } | ||
| 158 |
1/2✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
|
12 | else if (pinData[pinDataBiasesIdenticalIdx].initAngularRateBiasUnit == PinData::AngRateUnit::deg_s) |
| 159 | { | ||
| 160 |
3/6✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 12 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 12 times.
✗ Branch 10 not taken.
|
12 | initStates[2 * pinIndex] = deg2rad(pinData[pinDataBiasesIdenticalIdx].initAngularRateBias).array(); |
| 161 | } | ||
| 162 | |||
| 163 | // Initial bias of the acceleration in [m/s²] | ||
| 164 |
1/2✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
|
12 | if (pinData[pinDataBiasesIdenticalIdx].initAccelerationBiasUnit == PinData::AccelerationUnit::m_s2) |
| 165 | { | ||
| 166 |
2/4✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 12 times.
✗ Branch 7 not taken.
|
12 | initStates[1 + 2 * pinIndex] = pinData[pinDataBiasesIdenticalIdx].initAccelerationBias.array(); |
| 167 | } | ||
| 168 | } | ||
| 169 | |||
| 170 |
2/4✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
|
3 | kalmanFilter.x.block<9, 1>(0, 0) = initStates[0]; |
| 171 |
2/4✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
|
3 | kalmanFilter.x.block<9, 1>(9, 0) = initStates[1]; |
| 172 | LOG_DATA("Initial B-spline coefficients in the state vector x:\n{}", kalmanFilter.x.block<18, 1>(0, 0)); | ||
| 173 | |||
| 174 | 3 | uint32_t containerIndex = 2; | |
| 175 |
2/2✓ Branch 0 taken 12 times.
✓ Branch 1 taken 3 times.
|
15 | for (uint32_t pinIndex = 0; pinIndex < numInputPins - 1UL; ++pinIndex) |
| 176 | { | ||
| 177 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 12 times.
|
12 | if (!imuBiasesIdentical) |
| 178 | { | ||
| 179 | ✗ | containerIndex = 2 + 2 * pinIndex; | |
| 180 | } | ||
| 181 |
2/4✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 12 times.
✗ Branch 6 not taken.
|
12 | kalmanFilter.x.block<3, 1>(18 + 6 * pinIndex, 0) = initStates[containerIndex]; |
| 182 |
2/4✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 12 times.
✗ Branch 6 not taken.
|
12 | kalmanFilter.x.block<3, 1>(21 + 6 * pinIndex, 0) = initStates[1 + containerIndex]; |
| 183 | } | ||
| 184 | LOG_DATA("Initial bias values:\n{}", kalmanFilter.x.block<18, 1>(18, 0)); | ||
| 185 | |||
| 186 | // -------------------------------------- State transition matrix ---------------------------------------- | ||
| 187 |
2/4✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
|
3 | kalmanFilter.Phi = Eigen::MatrixXd::Identity(numStates, numStates); |
| 188 | |||
| 189 | // ------------------------------------- Error covariance matrix P --------------------------------------- | ||
| 190 | |||
| 191 | 3 | std::vector<Eigen::VectorXd> initErrorCovariances; | |
| 192 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | initErrorCovariances.resize(2 + 2 * (numInputPins - 1)); |
| 193 | |||
| 194 | // Initial error covariance of the B-spline KF angular rate coefficients in [rad²/s²] | ||
| 195 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 3 times.
|
3 | if (pinDataBsplineKF.initCovarianceCoeffsAngRateUnit == PinData::AngRateVarianceUnit::rad2_s2) |
| 196 | { | ||
| 197 | ✗ | initErrorCovariances[0] = pinDataBsplineKF.initCovarianceCoeffsAngRate; | |
| 198 | } | ||
| 199 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 3 times.
|
3 | else if (pinDataBsplineKF.initCovarianceCoeffsAngRateUnit == PinData::AngRateVarianceUnit::deg2_s2) |
| 200 | { | ||
| 201 | ✗ | initErrorCovariances[0] = deg2rad(pinDataBsplineKF.initCovarianceCoeffsAngRate); | |
| 202 | } | ||
| 203 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 3 times.
|
3 | else if (pinDataBsplineKF.initCovarianceCoeffsAngRateUnit == PinData::AngRateVarianceUnit::rad_s) |
| 204 | { | ||
| 205 | ✗ | initErrorCovariances[0] = pinDataBsplineKF.initCovarianceCoeffsAngRate.array().pow(2); | |
| 206 | } | ||
| 207 |
1/2✓ Branch 0 taken 3 times.
✗ Branch 1 not taken.
|
3 | else if (pinDataBsplineKF.initCovarianceCoeffsAngRateUnit == PinData::AngRateVarianceUnit::deg_s) |
| 208 | { | ||
| 209 |
4/8✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
|
3 | initErrorCovariances[0] = deg2rad(pinDataBsplineKF.initCovarianceCoeffsAngRate).array().pow(2); |
| 210 | } | ||
| 211 | |||
| 212 | // Initial error covariance of the B-spline KF acceleration coefficients in [(m^2)/(s^4)] | ||
| 213 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 3 times.
|
3 | if (pinDataBsplineKF.initCovarianceCoeffsAccelUnit == PinData::AccelerationVarianceUnit::m2_s4) |
| 214 | { | ||
| 215 | ✗ | initErrorCovariances[1] = pinDataBsplineKF.initCovarianceCoeffsAccel; | |
| 216 | } | ||
| 217 |
1/2✓ Branch 0 taken 3 times.
✗ Branch 1 not taken.
|
3 | else if (pinDataBsplineKF.initCovarianceCoeffsAccelUnit == PinData::AccelerationVarianceUnit::m_s2) |
| 218 | { | ||
| 219 |
3/6✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
|
3 | initErrorCovariances[1] = pinDataBsplineKF.initCovarianceCoeffsAccel.array().pow(2); |
| 220 | } | ||
| 221 | |||
| 222 | 3 | size_t pinDataIdx = 1; | |
| 223 |
2/2✓ Branch 0 taken 12 times.
✓ Branch 1 taken 3 times.
|
15 | for (size_t pinIndex = 1; pinIndex < numInputPins; ++pinIndex) |
| 224 | { | ||
| 225 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 12 times.
|
12 | if (!imuCharacteristicsIdentical) |
| 226 | { | ||
| 227 | ✗ | pinDataIdx = pinIndex; | |
| 228 | } | ||
| 229 | |||
| 230 | // Initial Covariance of the bias of the angular rate in [rad²/s²] | ||
| 231 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 12 times.
|
12 | if (pinData[pinDataIdx].initCovarianceBiasAngRateUnit == PinData::AngRateVarianceUnit::rad2_s2) |
| 232 | { | ||
| 233 | ✗ | initErrorCovariances[2 * pinIndex] = pinData[pinDataIdx].initCovarianceBiasAngRate; | |
| 234 | } | ||
| 235 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 12 times.
|
12 | else if (pinData[pinDataIdx].initCovarianceBiasAngRateUnit == PinData::AngRateVarianceUnit::deg2_s2) |
| 236 | { | ||
| 237 | ✗ | initErrorCovariances[2 * pinIndex] = deg2rad(pinData[pinDataIdx].initCovarianceBiasAngRate); | |
| 238 | } | ||
| 239 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 12 times.
|
12 | else if (pinData[pinDataIdx].initCovarianceBiasAngRateUnit == PinData::AngRateVarianceUnit::rad_s) |
| 240 | { | ||
| 241 | ✗ | initErrorCovariances[2 * pinIndex] = pinData[pinDataIdx].initCovarianceBiasAngRate.array().pow(2); | |
| 242 | } | ||
| 243 |
1/2✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
|
12 | else if (pinData[pinDataIdx].initCovarianceBiasAngRateUnit == PinData::AngRateVarianceUnit::deg_s) |
| 244 | { | ||
| 245 |
4/8✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 12 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 12 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 12 times.
✗ Branch 13 not taken.
|
12 | initErrorCovariances[2 * pinIndex] = deg2rad(pinData[pinDataIdx].initCovarianceBiasAngRate).array().pow(2); |
| 246 | } | ||
| 247 | |||
| 248 | // Initial Covariance of the bias of the acceleration in [(m^2)/(s^4)] | ||
| 249 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 12 times.
|
12 | if (pinData[pinDataIdx].initCovarianceBiasAccUnit == PinData::AccelerationVarianceUnit::m2_s4) |
| 250 | { | ||
| 251 | ✗ | initErrorCovariances[1 + 2 * pinIndex] = pinData[pinDataIdx].initCovarianceBiasAcc; | |
| 252 | } | ||
| 253 |
1/2✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
|
12 | else if (pinData[pinDataIdx].initCovarianceBiasAccUnit == PinData::AccelerationVarianceUnit::m_s2) |
| 254 | { | ||
| 255 |
3/6✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 12 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 12 times.
✗ Branch 10 not taken.
|
12 | initErrorCovariances[1 + 2 * pinIndex] = pinData[pinDataIdx].initCovarianceBiasAcc.array().pow(2); |
| 256 | } | ||
| 257 | } | ||
| 258 | |||
| 259 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | kalmanFilter.P = NAV::BsplineKF::initialErrorCovarianceMatrix_P0(initErrorCovariances, numStates, imuCharacteristicsIdentical); |
| 260 | |||
| 261 | // -------------------------------------- Process noise matrix Q ----------------------------------------- | ||
| 262 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | processNoiseVariances.resize(2 * numInputPins); |
| 263 | |||
| 264 | // 𝜎_AngRateFactors: Standard deviation of the noise on the B-spline coefficients angular rate states [rad/s] | ||
| 265 |
1/5✗ Branch 0 not taken.
✗ Branch 1 not taken.
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
|
3 | switch (pinDataBsplineKF.varCoeffsAngRateUnit) |
| 266 | { | ||
| 267 | ✗ | case PinData::AngRateVarianceUnit::rad2_s2: | |
| 268 | ✗ | processNoiseVariances[0] = pinDataBsplineKF.varCoeffsAngRateNoise; | |
| 269 | ✗ | break; | |
| 270 | ✗ | case PinData::AngRateVarianceUnit::deg2_s2: | |
| 271 | ✗ | processNoiseVariances[0] = deg2rad(pinDataBsplineKF.varCoeffsAngRateNoise); | |
| 272 | ✗ | break; | |
| 273 | 3 | case PinData::AngRateVarianceUnit::deg_s: | |
| 274 |
4/8✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
|
3 | processNoiseVariances[0] = deg2rad(pinDataBsplineKF.varCoeffsAngRateNoise).array().pow(2); |
| 275 | 3 | break; | |
| 276 | ✗ | case PinData::AngRateVarianceUnit::rad_s: | |
| 277 | ✗ | processNoiseVariances[0] = pinDataBsplineKF.varCoeffsAngRateNoise.array().pow(2); | |
| 278 | ✗ | break; | |
| 279 | } | ||
| 280 | |||
| 281 | // 𝜎_AccelFactors: Standard deviation of the noise on the B-spline coefficients acceleration states [m/s] | ||
| 282 |
1/3✗ Branch 0 not taken.
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | switch (pinDataBsplineKF.varCoeffsAccelUnit) |
| 283 | { | ||
| 284 | ✗ | case PinData::AccelerationVarianceUnit::m2_s4: | |
| 285 | ✗ | processNoiseVariances[1] = pinDataBsplineKF.varCoeffsAccelNoise; | |
| 286 | ✗ | break; | |
| 287 | 3 | case PinData::AccelerationVarianceUnit::m_s2: | |
| 288 |
3/6✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
|
3 | processNoiseVariances[1] = pinDataBsplineKF.varCoeffsAccelNoise.array().pow(2); |
| 289 | 3 | break; | |
| 290 | } | ||
| 291 | |||
| 292 |
2/2✓ Branch 0 taken 12 times.
✓ Branch 1 taken 3 times.
|
15 | for (size_t pinIndex = 1; pinIndex < numInputPins; ++pinIndex) |
| 293 | { | ||
| 294 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 12 times.
|
12 | if (!imuCharacteristicsIdentical) |
| 295 | { | ||
| 296 | ✗ | pinDataIdx = pinIndex; | |
| 297 | } | ||
| 298 | |||
| 299 | // 𝜎_biasAngRate Standard deviation of the bias on the angular rate state [rad/s²] | ||
| 300 |
1/5✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 12 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
12 | switch (pinData[pinDataIdx].varBiasAngRateNoiseUnit) |
| 301 | { | ||
| 302 | ✗ | case PinData::AngRateVarianceUnit::rad2_s2: | |
| 303 | ✗ | processNoiseVariances[2 * pinIndex] = pinData[pinDataIdx].varBiasAngRateNoise; | |
| 304 | ✗ | break; | |
| 305 | ✗ | case PinData::AngRateVarianceUnit::deg2_s2: | |
| 306 | ✗ | processNoiseVariances[2 * pinIndex] = deg2rad(pinData[pinDataIdx].varBiasAngRateNoise); | |
| 307 | ✗ | break; | |
| 308 | 12 | case PinData::AngRateVarianceUnit::deg_s: | |
| 309 |
4/8✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 12 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 12 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 12 times.
✗ Branch 13 not taken.
|
12 | processNoiseVariances[2 * pinIndex] = deg2rad(pinData[pinDataIdx].varBiasAngRateNoise).array().pow(2); |
| 310 | 12 | break; | |
| 311 | ✗ | case PinData::AngRateVarianceUnit::rad_s: | |
| 312 | ✗ | processNoiseVariances[2 * pinIndex] = pinData[pinDataIdx].varBiasAngRateNoise.array().pow(2); | |
| 313 | ✗ | break; | |
| 314 | } | ||
| 315 | |||
| 316 | // 𝜎_biasAcceleration Standard deviation of the noise on the acceleration state [m/s³] | ||
| 317 |
1/3✗ Branch 1 not taken.
✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
|
12 | switch (pinData[pinDataIdx].varBiasAccelerationNoiseUnit) |
| 318 | { | ||
| 319 | ✗ | case PinData::AccelerationVarianceUnit::m2_s4: | |
| 320 | ✗ | processNoiseVariances[1 + 2 * pinIndex] = pinData[pinDataIdx].varBiasAccelerationNoise; | |
| 321 | ✗ | break; | |
| 322 | 12 | case PinData::AccelerationVarianceUnit::m_s2: | |
| 323 |
3/6✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 12 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 12 times.
✗ Branch 10 not taken.
|
12 | processNoiseVariances[1 + 2 * pinIndex] = pinData[pinDataIdx].varBiasAccelerationNoise.array().pow(2); |
| 324 | 12 | break; | |
| 325 | } | ||
| 326 | } | ||
| 327 | |||
| 328 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | NAV::BsplineKF::processNoiseMatrix_Q(kalmanFilter.Q, dtInit, processNoiseVariances, numStates, imuCharacteristicsIdentical); |
| 329 | |||
| 330 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
6 | return kalmanFilter; |
| 331 | 3 | } | |
| 332 |