INSTINCT Code Coverage Report

Directory: src/
File: Navigation/INS/SensorCombiner/IRWKF/IRWKF.cpp
Date: 2025-02-07 16:54:41
Exec Total Coverage
Lines: 136 291 46.7%
Functions: 8 9 88.9%
Branches: 219 736 29.8%
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 "IRWKF.hpp"
10
11 #include "Navigation/Transformations/Units.hpp"
12 #include "util/Logger.hpp"
13
14 1 Eigen::MatrixXd NAV::IRWKF::initialStateTransitionMatrix_Phi(const double& dt, const uint8_t& numStates)
15 {
16 1 const auto nStates = static_cast<Eigen::Index>(numStates);
17
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 Eigen::MatrixXd Phi = Eigen::MatrixXd::Identity(nStates, nStates);
18
19
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 Phi.block<3, 3>(0, 3).diagonal().setConstant(dt); // dependency of angular rate on angular acceleration
20
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 Phi.block<3, 3>(6, 9).diagonal().setConstant(dt); // dependency of acceleration on jerk
21
22 1 return Phi;
23 ✗ }
24
25 1 Eigen::MatrixXd NAV::IRWKF::initialErrorCovarianceMatrix_P0(const std::vector<Eigen::Vector3d>& initCovariances, const uint8_t& numStates, const bool& imuCharacteristicsIdentical)
26 {
27
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 Eigen::MatrixXd P = Eigen::MatrixXd::Zero(numStates, numStates);
28
29
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 1 P.block<3, 3>(0, 0).diagonal() = initCovariances.at(0); // initial covariance of the angular rate
30
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 1 P.block<3, 3>(3, 3).diagonal() = initCovariances.at(1); // initial covariance of the angular acceleration
31
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 1 P.block<3, 3>(6, 6).diagonal() = initCovariances.at(2); // initial covariance of the acceleration
32
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 1 P.block<3, 3>(9, 9).diagonal() = initCovariances.at(3); // initial covariance of the jerk
33
34 1 size_t j = 4;
35
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 1 times.
 4 for (uint32_t i = 12; i < numStates; i += 6)
36 {
37
1/2
✓ Branch 0 taken 3 times.
✗ Branch 1 not taken.
 3 if (!imuCharacteristicsIdentical)
38 {
39 3 j = 4 + (i - 12) / 3; // access bias variances for each sensor from the fifth element onwards
40 }
41
42
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<3, 3>(i, i).diagonal() = initCovariances.at(j);
43
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<3, 3>(i + 3, i + 3).diagonal() = initCovariances.at(j + 1);
44 }
45
46 1 return P;
47 ✗ }
48
49 9904 void NAV::IRWKF::processNoiseMatrix_Q(Eigen::MatrixXd& Q, const double& dt, const std::vector<Eigen::VectorXd>& processNoiseVariances, const uint8_t& numStates, const bool& imuCharacteristicsIdentical)
50 {
51 // Integrated Random Walk of the angular rate
52
6/12
✓ Branch 2 taken 9904 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9904 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9904 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 9904 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 9904 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 9904 times.
✗ Branch 18 not taken.
 9904 Q.block<3, 3>(0, 0).diagonal() = processNoiseVariances.at(0) / 3. * std::pow(dt, 3);
53
6/12
✓ Branch 2 taken 9904 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9904 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9904 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 9904 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 9904 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 9904 times.
✗ Branch 18 not taken.
 9904 Q.block<3, 3>(0, 3).diagonal() = processNoiseVariances.at(0) / 2. * std::pow(dt, 2);
54
6/12
✓ Branch 2 taken 9904 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9904 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9904 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 9904 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 9904 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 9904 times.
✗ Branch 18 not taken.
 9904 Q.block<3, 3>(3, 0).diagonal() = processNoiseVariances.at(0) / 2. * std::pow(dt, 2);
55
5/10
✓ Branch 1 taken 9904 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9904 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9904 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 9904 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 9904 times.
✗ Branch 14 not taken.
 9904 Q.block<3, 3>(3, 3).diagonal() = processNoiseVariances.at(0) * dt;
56
57 // Integrated Random Walk of the acceleration
58
6/12
✓ Branch 2 taken 9904 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9904 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9904 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 9904 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 9904 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 9904 times.
✗ Branch 18 not taken.
 9904 Q.block<3, 3>(6, 6).diagonal() = processNoiseVariances.at(1) / 3. * std::pow(dt, 3);
59
6/12
✓ Branch 2 taken 9904 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9904 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9904 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 9904 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 9904 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 9904 times.
✗ Branch 18 not taken.
 9904 Q.block<3, 3>(6, 9).diagonal() = processNoiseVariances.at(1) / 2. * std::pow(dt, 2);
60
6/12
✓ Branch 2 taken 9904 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9904 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9904 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 9904 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 9904 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 9904 times.
✗ Branch 18 not taken.
 9904 Q.block<3, 3>(9, 6).diagonal() = processNoiseVariances.at(1) / 2. * std::pow(dt, 2);
61
5/10
✓ Branch 1 taken 9904 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9904 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9904 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 9904 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 9904 times.
✗ Branch 14 not taken.
 9904 Q.block<3, 3>(9, 9).diagonal() = processNoiseVariances.at(1) * dt;
62
63 // Random Walk of the bias states
64 9904 size_t j = 4;
65
2/2
✓ Branch 0 taken 29712 times.
✓ Branch 1 taken 9904 times.
 39616 for (uint32_t i = 12; i < numStates; i += 6) // FIXME: i = 'numStatesEst' better than '12'? (magic numbers)
66 {
67
1/2
✓ Branch 0 taken 29712 times.
✗ Branch 1 not taken.
 29712 if (!imuCharacteristicsIdentical)
68 {
69 29712 j = 2 + (i - 12) / 3; // access 2 bias variances for each sensor from the third element onwards
70 }
71
5/10
✓ Branch 1 taken 29712 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 29712 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 29712 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 29712 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 29712 times.
✗ Branch 14 not taken.
 29712 Q.block<3, 3>(i, i).diagonal() = processNoiseVariances.at(j) * dt; // variance for the process noise of the angular rate
72
5/10
✓ Branch 1 taken 29712 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 29712 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 29712 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 29712 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 29712 times.
✗ Branch 14 not taken.
 29712 Q.block<3, 3>(i + 3, i + 3).diagonal() = processNoiseVariances.at(j + 1) * dt; // variance for the process noise of the acceleration
73 }
74 9904 }
75
76 9903 void NAV::IRWKF::stateTransitionMatrix_Phi(Eigen::MatrixXd& Phi, const double& dt)
77 {
78
3/6
✓ Branch 1 taken 9903 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9903 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9903 times.
✗ Branch 8 not taken.
 9903 Phi.block<3, 3>(0, 3).diagonal().setConstant(dt); // dependency of angular rate on angular acceleration
79
3/6
✓ Branch 1 taken 9903 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9903 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9903 times.
✗ Branch 8 not taken.
 9903 Phi.block<3, 3>(6, 9).diagonal().setConstant(dt); // dependency of acceleration on jerk
80 9903 }
81
82 10004 Eigen::MatrixXd NAV::IRWKF::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)
83 {
84
2/4
✓ Branch 1 taken 10004 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10004 times.
✗ Branch 5 not taken.
 10004 Eigen::MatrixXd H = Eigen::MatrixXd::Zero(numMeasurements, numStates);
85
86 // Mounting angles of sensor with latest measurement
87
3/6
✓ Branch 1 taken 10004 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10004 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10004 times.
✗ Branch 8 not taken.
 10004 H.block<3, 3>(0, 0) = DCM_accel.transpose(); // Inverse rotation for angular rate
88
3/6
✓ Branch 1 taken 10004 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10004 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10004 times.
✗ Branch 8 not taken.
 10004 H.block<3, 3>(3, 6) = DCM_gyro.transpose(); // Inverse rotation for acceleration
89
90 // Mapping of bias states on sensor with the latest measurement
91
2/2
✓ Branch 0 taken 9003 times.
✓ Branch 1 taken 1001 times.
 10004 if (pinIndex > 0)
92 {
93 9003 const auto stateIndex = static_cast<uint8_t>(numStatesEst + numStatesPerPin * (pinIndex - 1));
94 LOG_DATA("stateIndex = {}", stateIndex);
95
96
3/6
✓ Branch 1 taken 9003 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9003 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9003 times.
✗ Branch 8 not taken.
 9003 H.block<6, 6>(0, stateIndex) = Eigen::MatrixXd::Identity(6, 6);
97 }
98
99 10004 return H;
100 ✗ }
101
102 ✗ NAV::KalmanFilter NAV::IRWKF::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)
103 {
104 // ------------------------------------------ State vector x ---------------------------------------------
105
106 ✗ std::vector<Eigen::Vector3d> initStates;
107 ✗ initStates.resize(6 + 2 * numInputPins); // FIXME: Shouldn't this be 'numStates'?
108
109 // Initial angular rate in [rad/s]
110 ✗ if (pinDataIRWKF.initAngularRateUnit == PinData::AngRateUnit::deg_s)
111 {
112 ✗ initStates[0] = deg2rad(pinDataIRWKF.initAngularRate);
113 }
114 ✗ if (pinDataIRWKF.initAngularRateUnit == PinData::AngRateUnit::rad_s)
115 {
116 ✗ initStates[0] = pinDataIRWKF.initAngularRate;
117 }
118
119 // Initial acceleration in [m/s²]
120 ✗ if (pinDataIRWKF.initAccelerationUnit == PinData::AccelerationUnit::m_s2)
121 {
122 ✗ initStates[1] = pinDataIRWKF.initAcceleration;
123 }
124
125 // Initial angular acceleration in [rad/s²]
126 ✗ if (pinDataIRWKF.initAngularAccUnit == PinDataIRWKF::AngularAccUnit::deg_s2)
127 {
128 ✗ initStates[2] = deg2rad(pinDataIRWKF.initAngularAcc);
129 }
130 ✗ if (pinDataIRWKF.initAngularAccUnit == PinDataIRWKF::AngularAccUnit::rad_s2)
131 {
132 ✗ initStates[2] = pinDataIRWKF.initAngularAcc;
133 }
134
135 // Initial jerk in [m/s³]
136 ✗ if (pinDataIRWKF.initJerkUnit == PinDataIRWKF::JerkUnit::m_s3)
137 {
138 ✗ initStates[3] = pinDataIRWKF.initJerk;
139 }
140
141 ✗ size_t pinDataBiasesIdenticalIdx = 1;
142 ✗ for (size_t pinIndex = 1; pinIndex < numInputPins; ++pinIndex)
143 {
144 ✗ if (!imuBiasesIdentical)
145 {
146 ✗ pinDataBiasesIdenticalIdx = pinIndex;
147 }
148
149 // Initial bias of the angular rate in [rad/s]
150 ✗ if (pinData[pinDataBiasesIdenticalIdx].initAngularRateBiasUnit == PinData::AngRateUnit::rad_s)
151 {
152 ✗ initStates[2 + 2 * pinIndex] = pinData[pinDataBiasesIdenticalIdx].initAngularRateBias.array();
153 }
154 ✗ else if (pinData[pinDataBiasesIdenticalIdx].initAngularRateBiasUnit == PinData::AngRateUnit::deg_s)
155 {
156 ✗ initStates[2 + 2 * pinIndex] = deg2rad(pinData[pinDataBiasesIdenticalIdx].initAngularRateBias).array();
157 }
158
159 // Initial bias of the acceleration in [m/s²]
160 ✗ if (pinData[pinDataBiasesIdenticalIdx].initAccelerationBiasUnit == PinData::AccelerationUnit::m_s2)
161 {
162 ✗ initStates[3 + 2 * pinIndex] = pinData[pinDataBiasesIdenticalIdx].initAccelerationBias.array();
163 }
164 }
165
166 ✗ kalmanFilter.x.block<3, 1>(0, 0) = initStates[0];
167 ✗ kalmanFilter.x.block<3, 1>(3, 0) = initStates[1];
168 ✗ kalmanFilter.x.block<3, 1>(6, 0) = initStates[2];
169 ✗ kalmanFilter.x.block<3, 1>(9, 0) = initStates[3];
170
171 ✗ uint32_t containerIndex = 4;
172 ✗ for (uint32_t pinIndex = 0; pinIndex < numInputPins - 1UL; ++pinIndex)
173 {
174 ✗ if (!imuBiasesIdentical)
175 {
176 ✗ containerIndex = 4 + 2 * pinIndex;
177 }
178 ✗ kalmanFilter.x.block<3, 1>(12 + 6 * pinIndex, 0) = initStates[containerIndex];
179 ✗ kalmanFilter.x.block<3, 1>(15 + 6 * pinIndex, 0) = initStates[1 + containerIndex];
180 }
181
182 // -------------------------------------- State transition matrix ----------------------------------------
183 ✗ kalmanFilter.Phi = NAV::IRWKF::initialStateTransitionMatrix_Phi(dtInit, numStates);
184
185 // ------------------------------------- Error covariance matrix P ---------------------------------------
186
187 ✗ std::vector<Eigen::Vector3d> initErrorCovariances;
188 ✗ initErrorCovariances.resize(6 + 2 * numInputPins);
189
190 // Initial Covariance of the angular rate in [rad²/s²]
191 ✗ if (pinData[0].initCovarianceAngularRateUnit == PinData::AngRateVarianceUnit::rad2_s2)
192 {
193 ✗ initErrorCovariances[0] = pinData[0].initCovarianceAngularRate;
194 }
195 ✗ else if (pinData[0].initCovarianceAngularRateUnit == PinData::AngRateVarianceUnit::deg2_s2)
196 {
197 ✗ initErrorCovariances[0] = deg2rad(pinData[0].initCovarianceAngularRate);
198 }
199 ✗ else if (pinData[0].initCovarianceAngularRateUnit == PinData::AngRateVarianceUnit::rad_s)
200 {
201 ✗ initErrorCovariances[0] = pinData[0].initCovarianceAngularRate.array().pow(2);
202 }
203 ✗ else if (pinData[0].initCovarianceAngularRateUnit == PinData::AngRateVarianceUnit::deg_s)
204 {
205 ✗ initErrorCovariances[0] = deg2rad(pinData[0].initCovarianceAngularRate).array().pow(2);
206 }
207
208 // Initial Covariance of the angular acceleration in [(rad^2)/(s^4)]
209 ✗ if (pinDataIRWKF.initCovarianceAngularAccUnit == PinDataIRWKF::AngularAccVarianceUnit::rad2_s4)
210 {
211 ✗ initErrorCovariances[1] = pinDataIRWKF.initCovarianceAngularAcc;
212 }
213 ✗ else if (pinDataIRWKF.initCovarianceAngularAccUnit == PinDataIRWKF::AngularAccVarianceUnit::deg2_s4)
214 {
215 ✗ initErrorCovariances[1] = deg2rad(pinDataIRWKF.initCovarianceAngularAcc);
216 }
217 ✗ else if (pinDataIRWKF.initCovarianceAngularAccUnit == PinDataIRWKF::AngularAccVarianceUnit::rad_s2)
218 {
219 ✗ initErrorCovariances[1] = pinDataIRWKF.initCovarianceAngularAcc.array().pow(2);
220 }
221 ✗ else if (pinDataIRWKF.initCovarianceAngularAccUnit == PinDataIRWKF::AngularAccVarianceUnit::deg_s2)
222 {
223 ✗ initErrorCovariances[1] = deg2rad(pinDataIRWKF.initCovarianceAngularAcc).array().pow(2);
224 }
225
226 // Initial Covariance of the acceleration in [(m^2)/(s^4)]
227 ✗ if (pinData[0].initCovarianceAccelerationUnit == PinData::AccelerationVarianceUnit::m2_s4)
228 {
229 ✗ initErrorCovariances[2] = pinData[0].initCovarianceAcceleration;
230 }
231 ✗ else if (pinData[0].initCovarianceAccelerationUnit == PinData::AccelerationVarianceUnit::m_s2)
232 {
233 ✗ initErrorCovariances[2] = pinData[0].initCovarianceAcceleration.array().pow(2);
234 }
235
236 // Initial Covariance of the jerk in [(m^2)/(s^6)]
237 ✗ if (pinDataIRWKF.initCovarianceJerkUnit == PinDataIRWKF::JerkVarianceUnit::m2_s6)
238 {
239 ✗ initErrorCovariances[3] = pinDataIRWKF.initCovarianceJerk;
240 }
241 ✗ else if (pinDataIRWKF.initCovarianceJerkUnit == PinDataIRWKF::JerkVarianceUnit::m_s3)
242 {
243 ✗ initErrorCovariances[3] = pinDataIRWKF.initCovarianceJerk.array().pow(2);
244 }
245
246 ✗ size_t pinDataIdx = 1;
247 ✗ for (size_t pinIndex = 1; pinIndex < numInputPins; ++pinIndex)
248 {
249 ✗ if (!imuCharacteristicsIdentical)
250 {
251 ✗ pinDataIdx = pinIndex;
252 }
253
254 // Initial Covariance of the bias of the angular rate in [rad²/s²]
255 ✗ if (pinData[pinDataIdx].initCovarianceBiasAngRateUnit == PinData::AngRateVarianceUnit::rad2_s2)
256 {
257 ✗ initErrorCovariances[2 + 2 * pinIndex] = pinData[pinDataIdx].initCovarianceBiasAngRate;
258 }
259 ✗ else if (pinData[pinDataIdx].initCovarianceBiasAngRateUnit == PinData::AngRateVarianceUnit::deg2_s2)
260 {
261 ✗ initErrorCovariances[2 + 2 * pinIndex] = deg2rad(pinData[pinDataIdx].initCovarianceBiasAngRate);
262 }
263 ✗ else if (pinData[pinDataIdx].initCovarianceBiasAngRateUnit == PinData::AngRateVarianceUnit::rad_s)
264 {
265 ✗ initErrorCovariances[2 + 2 * pinIndex] = pinData[pinDataIdx].initCovarianceBiasAngRate.array().pow(2);
266 }
267 ✗ else if (pinData[pinDataIdx].initCovarianceBiasAngRateUnit == PinData::AngRateVarianceUnit::deg_s)
268 {
269 ✗ initErrorCovariances[2 + 2 * pinIndex] = deg2rad(pinData[pinDataIdx].initCovarianceBiasAngRate).array().pow(2);
270 }
271
272 // Initial Covariance of the bias of the acceleration in [(m^2)/(s^4)]
273 ✗ if (pinData[pinDataIdx].initCovarianceBiasAccUnit == PinData::AccelerationVarianceUnit::m2_s4)
274 {
275 ✗ initErrorCovariances[3 + 2 * pinIndex] = pinData[pinDataIdx].initCovarianceBiasAcc;
276 }
277 ✗ else if (pinData[pinDataIdx].initCovarianceBiasAccUnit == PinData::AccelerationVarianceUnit::m_s2)
278 {
279 ✗ initErrorCovariances[3 + 2 * pinIndex] = pinData[pinDataIdx].initCovarianceBiasAcc.array().pow(2);
280 }
281 }
282
283 ✗ kalmanFilter.P = NAV::IRWKF::initialErrorCovarianceMatrix_P0(initErrorCovariances, numStates, imuCharacteristicsIdentical);
284
285 // -------------------------------------- Process noise matrix Q -----------------------------------------
286 ✗ processNoiseVariances.resize(2 * numInputPins);
287
288 // 𝜎_AngAcc Standard deviation of the noise on the angular acceleration state [rad/s²]
289 ✗ switch (pinDataIRWKF.varAngularAccNoiseUnit)
290 {
291 ✗ case PinDataIRWKF::AngularAccVarianceUnit::rad2_s4:
292 ✗ processNoiseVariances[0] = pinDataIRWKF.varAngularAccNoise;
293 ✗ break;
294 ✗ case PinDataIRWKF::AngularAccVarianceUnit::deg2_s4:
295 ✗ processNoiseVariances[0] = deg2rad(pinDataIRWKF.varAngularAccNoise);
296 ✗ break;
297 ✗ case PinDataIRWKF::AngularAccVarianceUnit::deg_s2:
298 ✗ processNoiseVariances[0] = deg2rad(pinDataIRWKF.varAngularAccNoise).array().pow(2);
299 ✗ break;
300 ✗ case PinDataIRWKF::AngularAccVarianceUnit::rad_s2:
301 ✗ processNoiseVariances[0] = pinDataIRWKF.varAngularAccNoise.array().pow(2);
302 ✗ break;
303 }
304
305 // 𝜎_jerk Standard deviation of the noise on the jerk state [m/s³]
306 ✗ switch (pinDataIRWKF.varJerkNoiseUnit)
307 {
308 ✗ case PinDataIRWKF::JerkVarianceUnit::m2_s6:
309 ✗ processNoiseVariances[1] = pinDataIRWKF.varJerkNoise;
310 ✗ break;
311 ✗ case PinDataIRWKF::JerkVarianceUnit::m_s3:
312 ✗ processNoiseVariances[1] = pinDataIRWKF.varJerkNoise.array().pow(2);
313 ✗ break;
314 }
315
316 ✗ for (size_t pinIndex = 1; pinIndex < numInputPins; ++pinIndex)
317 {
318 ✗ if (!imuCharacteristicsIdentical)
319 {
320 ✗ pinDataIdx = pinIndex;
321 }
322
323 // 𝜎_biasAngRate Standard deviation of the bias on the angular rate state [rad/s²]
324 ✗ switch (pinData[pinDataIdx].varBiasAngRateNoiseUnit)
325 {
326 ✗ case PinData::AngRateVarianceUnit::rad2_s2:
327 ✗ processNoiseVariances[2 * pinIndex] = pinData[pinDataIdx].varBiasAngRateNoise;
328 ✗ break;
329 ✗ case PinData::AngRateVarianceUnit::deg2_s2:
330 ✗ processNoiseVariances[2 * pinIndex] = deg2rad(pinData[pinDataIdx].varBiasAngRateNoise);
331 ✗ break;
332 ✗ case PinData::AngRateVarianceUnit::deg_s:
333 ✗ processNoiseVariances[2 * pinIndex] = deg2rad(pinData[pinDataIdx].varBiasAngRateNoise).array().pow(2);
334 ✗ break;
335 ✗ case PinData::AngRateVarianceUnit::rad_s:
336 ✗ processNoiseVariances[2 * pinIndex] = pinData[pinDataIdx].varBiasAngRateNoise.array().pow(2);
337 ✗ break;
338 }
339
340 // 𝜎_biasAcceleration Standard deviation of the noise on the acceleration state [m/s³]
341 ✗ switch (pinData[pinIndex].varBiasAccelerationNoiseUnit)
342 {
343 ✗ case PinData::AccelerationVarianceUnit::m2_s4:
344 ✗ processNoiseVariances[1 + 2 * pinIndex] = pinData[pinDataIdx].varBiasAccelerationNoise;
345 ✗ break;
346 ✗ case PinData::AccelerationVarianceUnit::m_s2:
347 ✗ processNoiseVariances[1 + 2 * pinIndex] = pinData[pinDataIdx].varBiasAccelerationNoise.array().pow(2);
348 ✗ break;
349 }
350 }
351
352 ✗ NAV::IRWKF::processNoiseMatrix_Q(kalmanFilter.Q, dtInit, processNoiseVariances, numStates, imuCharacteristicsIdentical);
353
354 ✗ return kalmanFilter;
355 ✗ }
356
357 1 NAV::KalmanFilter NAV::IRWKF::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)
358 {
359 // ---------------------------- Measurements (for initial state vector x0) -------------------------------
360
361 1 std::vector<std::vector<std::shared_ptr<const NAV::ImuObs>>> sensorMeasurements; // pinIndex / msgIndex(imuObs)
362
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 sensorMeasurements.resize(nInputPins);
363
364 1 std::vector<Eigen::Vector3d> initStates;
365
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 initStates.resize(6 + 2 * nInputPins); // state vector x
366
367 // Split cumulated imuObs into vectors for each sensor
368
2/2
✓ Branch 1 taken 100 times.
✓ Branch 2 taken 1 times.
 101 for (size_t msgIndex = 0; msgIndex < cumulatedImuObs.size(); msgIndex++)
369 {
370
1/2
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
 100 sensorMeasurements[cumulatedPinIds[msgIndex]].push_back(cumulatedImuObs[msgIndex]); // 'push_back' instead of 'resize()' and 'operator[]' since number of msgs of a certain pin are not known in advance
371 }
372
373 1 std::vector<std::vector<std::vector<double>>> sensorComponents; // pinIndex / axisIndex / msgIndex(double)
374
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 sensorComponents.resize(nInputPins);
375
376
2/2
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 1 times.
 5 for (size_t pinIndex = 0; pinIndex < nInputPins; pinIndex++) // loop thru connected sensors
377 {
378
1/2
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
 4 sensorComponents[pinIndex].resize(numMeasurements);
379
2/2
✓ Branch 0 taken 24 times.
✓ Branch 1 taken 4 times.
 28 for (size_t axisIndex = 0; axisIndex < numMeasurements; axisIndex++) // loop thru the 6 measurements: AccX, GyroX, AccY, GyroY, AccZ, GyroZ
380 {
381
2/2
✓ Branch 2 taken 600 times.
✓ Branch 3 taken 24 times.
 624 for (size_t msgIndex = 0; msgIndex < sensorMeasurements[pinIndex].size(); msgIndex++) // loop thru the msg of each measurement axis
382 {
383
2/2
✓ Branch 0 taken 300 times.
✓ Branch 1 taken 300 times.
 600 if (axisIndex < 3) // Accelerations X/Y/Z
384 {
385
2/4
✓ Branch 6 taken 300 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 300 times.
✗ Branch 10 not taken.
 300 sensorComponents[pinIndex][axisIndex].push_back(sensorMeasurements[pinIndex][msgIndex]->p_acceleration(static_cast<uint32_t>(axisIndex)));
386 }
387 else // Gyro X/Y/Z
388 {
389
2/4
✓ Branch 6 taken 300 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 300 times.
✗ Branch 10 not taken.
 300 sensorComponents[pinIndex][axisIndex].push_back(sensorMeasurements[pinIndex][msgIndex]->p_angularRate(static_cast<uint32_t>(axisIndex - 3)));
390 }
391 }
392 }
393 }
394
395 // --------------------------- Averaging single measurements of each sensor ------------------------------
396 // Accelerations X/Y/Z (pos. 6,7,8 in state vector) - init value is mean of reference sensor, i.e. pinIndex = 0
397
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 initStates[2] = mean(sensorComponents[0], 0);
398 // Jerk X/Y/Z
399
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 initStates[3] = Eigen::Vector3d::Zero();
400 // Angular Rate X/Y/Z (pos. 0,1,2 in state vector) - init value is mean of reference sensor, i.e. pinIndex = 0
401
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 initStates[0] = mean(sensorComponents[0], 3);
402 // Angular Acceleration X/Y/Z
403
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 initStates[1] = Eigen::Vector3d::Zero();
404
405 // Bias-inits
406
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 1 times.
 4 for (size_t pinIndex = 0; pinIndex < nInputPins - 1; pinIndex++) // nInputPins - 1 since there are only relative biases
407 {
408 3 auto stateIndex = 4 + 2 * pinIndex; // 4 states are Acceleration, Jerk, Angular Rate, Angular Acceleration (see above), plus 2 bias states per sensor
409
3/6
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
 3 initStates[stateIndex + 1] = mean(sensorComponents[pinIndex + 1], 0) - initStates[2]; // Acceleration biases
410
3/6
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
 3 initStates[stateIndex] = mean(sensorComponents[pinIndex + 1], 3) - initStates[0]; // Angular rate biases
411 }
412
413 // ------------------------------------ State transition matrix Phi --------------------------------------
414
415
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 kalmanFilter.Phi = IRWKF::initialStateTransitionMatrix_Phi(dtInit, numStates);
416
417 // ----------------------- Variance of each sensor (initial error covariance P0) -------------------------
418
419 1 std::vector<Eigen::Vector3d> initErrorCovariances;
420
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 initErrorCovariances.resize(6 + 2 * nInputPins); // error covariance matrix P
421
422 // Acceleration variances X/Y/Z (pos. 6,7,8 on diagonal of P matrix) - init value is variance of reference sensor, i.e. pinIndex = 0
423
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 initErrorCovariances[2] = variance(sensorComponents[0], 0);
424 // Angular Rate variances X/Y/Z (pos. 0,1,2 on diagonal of P matrix) - init value is variance of reference sensor, i.e. pinIndex = 0
425
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 initErrorCovariances[0] = variance(sensorComponents[0], 3);
426
427
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 1 if (initJerkAngAcc)
428 {
429 // Jerk variances X/Y/Z
430
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 initErrorCovariances[3] = initErrorCovariances[2];
431 // Angular Acceleration variances X/Y/Z
432
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 initErrorCovariances[1] = initErrorCovariances[0];
433 }
434 else
435 {
436 // Jerk variances X/Y/Z
437 ✗ initErrorCovariances[3] = Eigen::Vector3d::Zero();
438 // Angular Acceleration variances X/Y/Z
439 ✗ initErrorCovariances[1] = Eigen::Vector3d::Zero();
440 }
441
442 // P-matrix bias inits
443
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 1 times.
 4 for (size_t pinIndex = 0; pinIndex < nInputPins - 1; pinIndex++) // nInputPins - 1 since there are only relative biases
444 {
445 3 auto stateIndex = 4 + 2 * pinIndex; // 4 states are Acceleration, Jerk, Angular Rate, Angular Acceleration (see above), plus 2 bias states per sensor
446
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 initErrorCovariances[stateIndex + 1] = variance(sensorComponents[pinIndex + 1], 0); // Acceleration biases
447
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 initErrorCovariances[stateIndex] = variance(sensorComponents[pinIndex + 1], 3); // Angular rate biases
448
449 // Choose the bigger one of the two variances, i.e. of sensor #'pinIndex' and the reference sensor #0 (since bias is the difference of these two sensors)
450
2/2
✓ Branch 0 taken 9 times.
✓ Branch 1 taken 3 times.
 12 for (int axisIndex = 0; axisIndex < 3; axisIndex++)
451 {
452 // Acceleration variance
453
2/4
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 9 times.
✗ Branch 7 not taken.
 9 initErrorCovariances[stateIndex + 1](axisIndex) = std::max(initErrorCovariances[stateIndex + 1](axisIndex),
454
1/2
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
 9 initErrorCovariances[2](axisIndex));
455 // Angular rate variance
456
2/4
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 9 times.
✗ Branch 7 not taken.
 9 initErrorCovariances[stateIndex](axisIndex) = std::max(initErrorCovariances[stateIndex](axisIndex),
457
1/2
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
 9 initErrorCovariances[0](axisIndex));
458 }
459 }
460
461
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 kalmanFilter.x.block<3, 1>(0, 0) = initStates[0];
462
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 kalmanFilter.x.block<3, 1>(3, 0) = initStates[1];
463
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 kalmanFilter.x.block<3, 1>(6, 0) = initStates[2];
464
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 kalmanFilter.x.block<3, 1>(9, 0) = initStates[3];
465
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 1 times.
 4 for (uint32_t pinIndex = 0; pinIndex < nInputPins - 1UL; ++pinIndex)
466 {
467 3 auto containerIndex = 4 + 2 * pinIndex;
468
2/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
 3 kalmanFilter.x.block<3, 1>(12 + 6 * pinIndex, 0) = initStates[containerIndex];
469
2/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
 3 kalmanFilter.x.block<3, 1>(15 + 6 * pinIndex, 0) = initStates[1 + containerIndex];
470 }
471
472
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 kalmanFilter.P = IRWKF::initialErrorCovarianceMatrix_P0(initErrorCovariances, numStates);
473
474 // ------------------------------------------------------- Process noise matrix Q ----------------------------------------------------------
475
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 processNoiseVariances.resize(2 * nInputPins);
476
477 // 𝜎_AngAcc Standard deviation of the noise on the angular acceleration state [rad/s²]
478
1/5
✗ Branch 0 not taken.
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
 1 switch (pinDataIRWKF.varAngularAccNoiseUnit)
479 {
480 ✗ case PinDataIRWKF::AngularAccVarianceUnit::rad2_s4:
481 ✗ processNoiseVariances[0] = pinDataIRWKF.varAngularAccNoise;
482 ✗ break;
483 ✗ case PinDataIRWKF::AngularAccVarianceUnit::deg2_s4:
484 ✗ processNoiseVariances[0] = deg2rad(pinDataIRWKF.varAngularAccNoise);
485 ✗ break;
486 1 case PinDataIRWKF::AngularAccVarianceUnit::deg_s2:
487
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 1 processNoiseVariances[0] = deg2rad(pinDataIRWKF.varAngularAccNoise).array().pow(2);
488 1 break;
489 ✗ case PinDataIRWKF::AngularAccVarianceUnit::rad_s2:
490 ✗ processNoiseVariances[0] = pinDataIRWKF.varAngularAccNoise.array().pow(2);
491 ✗ break;
492 }
493
494 // 𝜎_jerk Standard deviation of the noise on the jerk state [m/s³]
495
1/3
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 switch (pinDataIRWKF.varJerkNoiseUnit)
496 {
497 ✗ case PinDataIRWKF::JerkVarianceUnit::m2_s6:
498 ✗ processNoiseVariances[1] = pinDataIRWKF.varJerkNoise;
499 ✗ break;
500 1 case PinDataIRWKF::JerkVarianceUnit::m_s3:
501
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 1 processNoiseVariances[1] = pinDataIRWKF.varJerkNoise.array().pow(2);
502 1 break;
503 }
504
505
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 1 times.
 4 for (size_t pinIndex = 0; pinIndex < nInputPins - 1; ++pinIndex)
506 {
507 // 𝜎_biasAngRate Standard deviation of the bias on the angular rate state [rad/s²]
508
1/5
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 3 switch (pinData[1 + pinIndex].varBiasAngRateNoiseUnit)
509 {
510 ✗ case PinData::AngRateVarianceUnit::rad2_s2:
511 ✗ processNoiseVariances[2 + 2 * pinIndex] = pinData[1 + pinIndex].varBiasAngRateNoise;
512 ✗ break;
513 ✗ case PinData::AngRateVarianceUnit::deg2_s2:
514 ✗ processNoiseVariances[2 + 2 * pinIndex] = deg2rad(pinData[1 + pinIndex].varBiasAngRateNoise);
515 ✗ break;
516 ✗ case PinData::AngRateVarianceUnit::deg_s:
517 ✗ processNoiseVariances[2 + 2 * pinIndex] = deg2rad(pinData[1 + pinIndex].varBiasAngRateNoise).array().pow(2);
518 ✗ break;
519 3 case PinData::AngRateVarianceUnit::rad_s:
520
3/6
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 3 times.
✗ Branch 10 not taken.
 3 processNoiseVariances[2 + 2 * pinIndex] = pinData[1 + pinIndex].varBiasAngRateNoise.array().pow(2);
521 3 break;
522 }
523
524 // 𝜎_biasAcceleration Standard deviation of the noise on the acceleration state [m/s³]
525
1/3
✗ Branch 1 not taken.
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 switch (pinData[pinIndex].varBiasAccelerationNoiseUnit)
526 {
527 ✗ case PinData::AccelerationVarianceUnit::m2_s4:
528 ✗ processNoiseVariances[3 + 2 * pinIndex] = pinData[1 + pinIndex].varBiasAccelerationNoise;
529 ✗ break;
530 3 case PinData::AccelerationVarianceUnit::m_s2:
531
3/6
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 3 times.
✗ Branch 10 not taken.
 3 processNoiseVariances[3 + 2 * pinIndex] = pinData[1 + pinIndex].varBiasAccelerationNoise.array().pow(2);
532 3 break;
533 }
534 }
535
536
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 IRWKF::processNoiseMatrix_Q(kalmanFilter.Q, dtInit, processNoiseVariances, numStates);
537
538
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 return kalmanFilter;
539 1 }
540
541 16 Eigen::Vector3d NAV::IRWKF::mean(const std::vector<std::vector<double>>& sensorType, const size_t& containerPos)
542 {
543
2/4
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
 16 Eigen::Vector3d meanVector = Eigen::Vector3d::Zero();
544
545
2/2
✓ Branch 0 taken 48 times.
✓ Branch 1 taken 16 times.
 64 for (size_t axisIndex = 0; axisIndex < 3; axisIndex++)
546 {
547 96 meanVector(static_cast<int>(axisIndex)) = std::accumulate(sensorType[axisIndex + containerPos].begin(), // NOLINT(boost-use-ranges,modernize-use-ranges) // There is no ranges::accumulate
548 48 sensorType[axisIndex + containerPos].end(), 0.)
549 48 / static_cast<double>(sensorType[axisIndex + containerPos].size());
550 }
551
552 16 return meanVector;
553 }
554
555 8 Eigen::Vector3d NAV::IRWKF::variance(const std::vector<std::vector<double>>& sensorType, const size_t& containerPos)
556 {
557
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 8 Eigen::Vector3d varianceVector = Eigen::Vector3d::Zero();
558
559
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 auto means = mean(sensorType, containerPos); // mean values for each axis
560
561
2/2
✓ Branch 0 taken 24 times.
✓ Branch 1 taken 8 times.
 32 for (size_t axisIndex = 0; axisIndex < 3; axisIndex++)
562 {
563
1/2
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
 24 auto N = sensorType.at(axisIndex + containerPos).size(); // Number of msgs along the specific axis
564
565
1/2
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
 24 std::vector<double> absolSquared(N, 0.); // Inner part of the variance calculation (squared absolute values)
566
567
2/2
✓ Branch 0 taken 600 times.
✓ Branch 1 taken 24 times.
 624 for (size_t msgIndex = 0; msgIndex < N; msgIndex++)
568 {
569
1/2
✓ Branch 3 taken 600 times.
✗ Branch 4 not taken.
 600 absolSquared[msgIndex] = std::pow(std::abs(sensorType[axisIndex + containerPos][msgIndex] - means(static_cast<int>(axisIndex))), 2);
570 }
571
572
1/2
✓ Branch 4 taken 24 times.
✗ Branch 5 not taken.
 24 varianceVector(static_cast<int>(axisIndex)) = (1. / (static_cast<double>(N) - 1.)) * std::accumulate(absolSquared.begin(), absolSquared.end(), 0.); // NOLINT(boost-use-ranges,modernize-use-ranges) // There is no ranges::accumulate
573 24 }
574
575 16 return varianceVector;
576 }
577