| 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 "ErrorModel.hpp" | ||
| 10 | |||
| 11 | #include <algorithm> | ||
| 12 | #include <imgui.h> | ||
| 13 | #include <imgui_internal.h> | ||
| 14 | #include <limits> | ||
| 15 | #include <set> | ||
| 16 | #include <type_traits> | ||
| 17 | |||
| 18 | #include "NodeRegistry.hpp" | ||
| 19 | #include "Navigation/INS/Units.hpp" | ||
| 20 | #include "internal/NodeManager.hpp" | ||
| 21 | namespace nm = NAV::NodeManager; | ||
| 22 | #include "internal/FlowManager.hpp" | ||
| 23 | |||
| 24 | #include "NodeData/IMU/ImuObsSimulated.hpp" | ||
| 25 | #include "NodeData/IMU/ImuObsWDelta.hpp" | ||
| 26 | |||
| 27 | #include "Navigation/Transformations/CoordinateFrames.hpp" | ||
| 28 | #include "Navigation/Transformations/Units.hpp" | ||
| 29 | #include "Navigation/GNSS/Functions.hpp" | ||
| 30 | |||
| 31 | #include "internal/gui/widgets/HelpMarker.hpp" | ||
| 32 | #include "internal/gui/widgets/imgui_ex.hpp" | ||
| 33 | #include "internal/gui/widgets/InputWithUnit.hpp" | ||
| 34 | #include "internal/gui/NodeEditorApplication.hpp" | ||
| 35 | |||
| 36 | #include "util/Eigen.hpp" | ||
| 37 | #include "util/StringUtil.hpp" | ||
| 38 | |||
| 39 | // ---------------------------------------------------------- Private variabels ------------------------------------------------------------ | ||
| 40 | |||
| 41 | namespace NAV | ||
| 42 | { | ||
| 43 | /// List of supported data identifiers | ||
| 44 | const std::vector<std::string> supportedDataIdentifier{ | ||
| 45 | ImuObs::type(), ImuObsWDelta::type(), | ||
| 46 | Pos::type(), PosVel::type(), PosVelAtt::type(), | ||
| 47 | GnssObs::type() | ||
| 48 | }; | ||
| 49 | |||
| 50 | } // namespace NAV | ||
| 51 | |||
| 52 | // ---------------------------------------------------------- Member functions ------------------------------------------------------------- | ||
| 53 | |||
| 54 | 120 | NAV::ErrorModel::ErrorModel() | |
| 55 |
56/112✓ Branch 1 taken 120 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 120 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 120 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 120 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 120 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 120 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 120 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 120 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 120 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 120 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 120 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 120 times.
✗ Branch 37 not taken.
✓ Branch 39 taken 120 times.
✗ Branch 40 not taken.
✓ Branch 42 taken 120 times.
✗ Branch 43 not taken.
✓ Branch 45 taken 120 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 120 times.
✗ Branch 49 not taken.
✓ Branch 51 taken 120 times.
✗ Branch 52 not taken.
✓ Branch 54 taken 120 times.
✗ Branch 55 not taken.
✓ Branch 57 taken 120 times.
✗ Branch 58 not taken.
✓ Branch 60 taken 120 times.
✗ Branch 61 not taken.
✓ Branch 63 taken 120 times.
✗ Branch 64 not taken.
✓ Branch 66 taken 120 times.
✗ Branch 67 not taken.
✓ Branch 69 taken 120 times.
✗ Branch 70 not taken.
✓ Branch 72 taken 120 times.
✗ Branch 73 not taken.
✓ Branch 75 taken 120 times.
✗ Branch 76 not taken.
✓ Branch 78 taken 120 times.
✗ Branch 79 not taken.
✓ Branch 81 taken 120 times.
✗ Branch 82 not taken.
✓ Branch 84 taken 120 times.
✗ Branch 85 not taken.
✓ Branch 87 taken 120 times.
✗ Branch 88 not taken.
✓ Branch 90 taken 120 times.
✗ Branch 91 not taken.
✓ Branch 93 taken 120 times.
✗ Branch 94 not taken.
✓ Branch 96 taken 120 times.
✗ Branch 97 not taken.
✓ Branch 99 taken 120 times.
✗ Branch 100 not taken.
✓ Branch 102 taken 120 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 120 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 120 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 120 times.
✗ Branch 112 not taken.
✓ Branch 114 taken 120 times.
✗ Branch 115 not taken.
✓ Branch 117 taken 120 times.
✗ Branch 118 not taken.
✓ Branch 120 taken 120 times.
✗ Branch 121 not taken.
✓ Branch 123 taken 120 times.
✗ Branch 124 not taken.
✓ Branch 126 taken 120 times.
✗ Branch 127 not taken.
✓ Branch 129 taken 120 times.
✗ Branch 130 not taken.
✓ Branch 132 taken 120 times.
✗ Branch 133 not taken.
✓ Branch 135 taken 120 times.
✗ Branch 136 not taken.
✓ Branch 138 taken 120 times.
✗ Branch 139 not taken.
✓ Branch 141 taken 120 times.
✗ Branch 142 not taken.
✓ Branch 144 taken 120 times.
✗ Branch 145 not taken.
✓ Branch 147 taken 120 times.
✗ Branch 148 not taken.
✓ Branch 150 taken 120 times.
✗ Branch 151 not taken.
✓ Branch 153 taken 120 times.
✗ Branch 154 not taken.
✓ Branch 156 taken 120 times.
✗ Branch 157 not taken.
✓ Branch 159 taken 120 times.
✗ Branch 160 not taken.
✓ Branch 162 taken 120 times.
✗ Branch 163 not taken.
✓ Branch 165 taken 120 times.
✗ Branch 166 not taken.
✓ Branch 170 taken 120 times.
✗ Branch 171 not taken.
|
120 | : Node(typeStatic()) |
| 56 | { | ||
| 57 | LOG_TRACE("{}: called", name); | ||
| 58 | 120 | _hasConfig = true; | |
| 59 | 120 | _guiConfigDefaultWindowSize = { 812, 530 }; | |
| 60 | |||
| 61 |
1/2✓ Branch 2 taken 120 times.
✗ Branch 3 not taken.
|
120 | nm::CreateInputPin(this, "True", Pin::Type::Flow, supportedDataIdentifier, &ErrorModel::receiveObs); |
| 62 | |||
| 63 |
1/2✓ Branch 3 taken 120 times.
✗ Branch 4 not taken.
|
120 | nm::CreateOutputPin(this, "Biased", Pin::Type::Flow, supportedDataIdentifier); |
| 64 | |||
| 65 |
1/2✓ Branch 4 taken 120 times.
✗ Branch 5 not taken.
|
120 | std::mt19937_64 gen(static_cast<uint64_t>(std::chrono::system_clock::now().time_since_epoch().count())); |
| 66 |
1/2✓ Branch 2 taken 120 times.
✗ Branch 3 not taken.
|
120 | std::uniform_int_distribution<uint64_t> dist(0, std::numeric_limits<uint64_t>::max() / 2); |
| 67 | |||
| 68 |
1/2✓ Branch 1 taken 120 times.
✗ Branch 2 not taken.
|
120 | _imuAccelerometerRng.seed = dist(gen); |
| 69 |
1/2✓ Branch 1 taken 120 times.
✗ Branch 2 not taken.
|
120 | _imuGyroscopeRng.seed = dist(gen); |
| 70 | |||
| 71 |
1/2✓ Branch 1 taken 120 times.
✗ Branch 2 not taken.
|
120 | _imuAccelerometerRWRng.seed = dist(gen); |
| 72 |
1/2✓ Branch 1 taken 120 times.
✗ Branch 2 not taken.
|
120 | _imuGyroscopeRWRng.seed = dist(gen); |
| 73 |
1/2✓ Branch 1 taken 120 times.
✗ Branch 2 not taken.
|
120 | _imuAccelerometerIRWRng.seed = dist(gen); |
| 74 |
1/2✓ Branch 1 taken 120 times.
✗ Branch 2 not taken.
|
120 | _imuGyroscopeIRWRng.seed = dist(gen); |
| 75 | |||
| 76 |
1/2✓ Branch 1 taken 120 times.
✗ Branch 2 not taken.
|
120 | _positionRng.seed = dist(gen); |
| 77 |
1/2✓ Branch 1 taken 120 times.
✗ Branch 2 not taken.
|
120 | _velocityRng.seed = dist(gen); |
| 78 |
1/2✓ Branch 1 taken 120 times.
✗ Branch 2 not taken.
|
120 | _attitudeRng.seed = dist(gen); |
| 79 | |||
| 80 |
1/2✓ Branch 1 taken 120 times.
✗ Branch 2 not taken.
|
120 | _pseudorangeRng.seed = dist(gen); |
| 81 |
1/2✓ Branch 1 taken 120 times.
✗ Branch 2 not taken.
|
120 | _carrierPhaseRng.seed = dist(gen); |
| 82 |
1/2✓ Branch 1 taken 120 times.
✗ Branch 2 not taken.
|
120 | _dopplerRng.seed = dist(gen); |
| 83 |
1/2✓ Branch 1 taken 120 times.
✗ Branch 2 not taken.
|
120 | _ambiguityRng.seed = dist(gen); |
| 84 |
1/2✓ Branch 1 taken 120 times.
✗ Branch 2 not taken.
|
120 | _cycleSlipRng.seed = dist(gen); |
| 85 | 120 | } | |
| 86 | |||
| 87 | 252 | NAV::ErrorModel::~ErrorModel() | |
| 88 | { | ||
| 89 | LOG_TRACE("{}: called", nameId()); | ||
| 90 | 252 | } | |
| 91 | |||
| 92 | 234 | std::string NAV::ErrorModel::typeStatic() | |
| 93 | { | ||
| 94 |
1/2✓ Branch 1 taken 234 times.
✗ Branch 2 not taken.
|
468 | return "ErrorModel"; |
| 95 | } | ||
| 96 | |||
| 97 | ✗ | std::string NAV::ErrorModel::type() const | |
| 98 | { | ||
| 99 | ✗ | return typeStatic(); | |
| 100 | } | ||
| 101 | |||
| 102 | 114 | std::string NAV::ErrorModel::category() | |
| 103 | { | ||
| 104 |
1/2✓ Branch 1 taken 114 times.
✗ Branch 2 not taken.
|
228 | return "Data Processor"; |
| 105 | } | ||
| 106 | |||
| 107 | ✗ | void NAV::ErrorModel::guiConfig() | |
| 108 | { | ||
| 109 | ✗ | if (outputPins.front().dataIdentifier.size() != 1 | |
| 110 | ✗ | || !inputPins.front().isPinLinked()) | |
| 111 | { | ||
| 112 | ✗ | ImGui::TextUnformatted("Please connect the input pin to show the options"); | |
| 113 | ✗ | return; | |
| 114 | } | ||
| 115 | |||
| 116 | ✗ | float itemWidth = 470 * gui::NodeEditorApplication::windowFontRatio(); | |
| 117 | ✗ | float unitWidth = 180 * gui::NodeEditorApplication::windowFontRatio(); | |
| 118 | |||
| 119 | ✗ | auto inputDoubleWithUnit = [&](const char* title, double& data, auto& unit, const char* combo_items_separated_by_zeros, const char* format) { | |
| 120 | ✗ | if (auto response = gui::widgets::InputDoubleWithUnit(fmt::format("{}##{}", title, size_t(id)).c_str(), itemWidth, unitWidth, | |
| 121 | &data, unit, combo_items_separated_by_zeros, 0.0, 0.0, | ||
| 122 | format, ImGuiInputTextFlags_CharsScientific)) | ||
| 123 | { | ||
| 124 | ✗ | if (response == gui::widgets::InputWithUnitChange_Input) { LOG_DEBUG("{}: {} changed to {}", nameId(), title, data); } | |
| 125 | ✗ | if (response == gui::widgets::InputWithUnitChange_Unit) { LOG_DEBUG("{}: {} unit changed to {}", nameId(), title, fmt::underlying(unit)); } | |
| 126 | ✗ | flow::ApplyChanges(); | |
| 127 | } | ||
| 128 | ✗ | }; | |
| 129 | |||
| 130 | ✗ | auto inputVector3WithUnit = [&](const char* title, Eigen::Vector3d& data, auto& unit, const char* combo_items_separated_by_zeros, const char* format) { | |
| 131 | ✗ | if (auto response = gui::widgets::InputDouble3WithUnit(fmt::format("{}##{}", title, size_t(id)).c_str(), itemWidth, unitWidth, | |
| 132 | data.data(), unit, combo_items_separated_by_zeros, | ||
| 133 | format, ImGuiInputTextFlags_CharsScientific)) | ||
| 134 | { | ||
| 135 | ✗ | if (response == gui::widgets::InputWithUnitChange_Input) { LOG_DEBUG("{}: {} changed to {}", nameId(), title, data.transpose()); } | |
| 136 | ✗ | if (response == gui::widgets::InputWithUnitChange_Unit) { LOG_DEBUG("{}: {} unit changed to {}", nameId(), title, fmt::underlying(unit)); } | |
| 137 | ✗ | flow::ApplyChanges(); | |
| 138 | } | ||
| 139 | ✗ | }; | |
| 140 | |||
| 141 | ✗ | auto rngInput = [&](const char* title, RandomNumberGenerator& rng) { | |
| 142 | ✗ | float currentCursorX = ImGui::GetCursorPosX(); | |
| 143 | ✗ | if (ImGui::Checkbox(fmt::format("##rng.useSeed {} {}", title, size_t(id)).c_str(), &rng.useSeed)) | |
| 144 | { | ||
| 145 | ✗ | LOG_DEBUG("{}: {} rng.useSeed changed to {}", nameId(), title, rng.useSeed); | |
| 146 | ✗ | flow::ApplyChanges(); | |
| 147 | } | ||
| 148 | ✗ | if (ImGui::IsItemHovered()) { ImGui::SetTooltip("Use seed?"); } | |
| 149 | ✗ | ImGui::SameLine(); | |
| 150 | ✗ | if (!rng.useSeed) | |
| 151 | { | ||
| 152 | ✗ | ImGui::BeginDisabled(); | |
| 153 | } | ||
| 154 | ✗ | ImGui::SetNextItemWidth(itemWidth - (ImGui::GetCursorPosX() - currentCursorX)); | |
| 155 | ✗ | if (ImGui::SliderULong(fmt::format("{} Seed##{}", title, size_t(id)).c_str(), &rng.seed, 0, std::numeric_limits<uint64_t>::max() / 2, "%lu")) | |
| 156 | { | ||
| 157 | ✗ | LOG_DEBUG("{}: {} rng.seed changed to {}", nameId(), title, rng.seed); | |
| 158 | ✗ | flow::ApplyChanges(); | |
| 159 | } | ||
| 160 | ✗ | if (!rng.useSeed) | |
| 161 | { | ||
| 162 | ✗ | ImGui::EndDisabled(); | |
| 163 | } | ||
| 164 | ✗ | }; | |
| 165 | |||
| 166 | ✗ | auto noiseGuiInput = [&]<typename T>(const char* title, T& data, auto& unit, const char* combo_items_separated_by_zeros, const char* format, RandomNumberGenerator& rng) { | |
| 167 | ✗ | if constexpr (std::is_same_v<T, double>) { inputDoubleWithUnit(title, data, unit, combo_items_separated_by_zeros, format); } | |
| 168 | ✗ | else if constexpr (std::is_same_v<T, Eigen::Vector3d>) { inputVector3WithUnit(title, data, unit, combo_items_separated_by_zeros, format); } | |
| 169 | |||
| 170 | ✗ | rngInput(title, rng); | |
| 171 | ✗ | }; | |
| 172 | |||
| 173 | ✗ | if (NAV::NodeRegistry::NodeDataTypeAnyIsChildOf(outputPins.front().dataIdentifier, { ImuObs::type() }) | |
| 174 | ✗ | || NAV::NodeRegistry::NodeDataTypeAnyIsChildOf(outputPins.front().dataIdentifier, { Pos::type() })) | |
| 175 | { | ||
| 176 | ✗ | ImGui::TextUnformatted("Offsets:"); | |
| 177 | ✗ | ImGui::Indent(); | |
| 178 | { | ||
| 179 | ✗ | if (NAV::NodeRegistry::NodeDataTypeAnyIsChildOf(outputPins.front().dataIdentifier, { ImuObs::type() })) | |
| 180 | { | ||
| 181 | ✗ | inputVector3WithUnit("Accelerometer Bias (platform)", _imuAccelerometerBias_p, _imuAccelerometerBiasUnit, MakeComboItems<Units::ImuAccelerometerUnits>().c_str(), "%.2g"); | |
| 182 | ✗ | inputVector3WithUnit("Gyroscope Bias (platform)", _imuGyroscopeBias_p, _imuGyroscopeBiasUnit, MakeComboItems<Units::ImuGyroscopeUnits>().c_str(), "%.2g"); | |
| 183 | } | ||
| 184 | ✗ | else if (NAV::NodeRegistry::NodeDataTypeAnyIsChildOf(outputPins.front().dataIdentifier, { Pos::type() })) | |
| 185 | { | ||
| 186 | ✗ | inputVector3WithUnit(fmt::format("Position Bias ({})", _positionBiasUnit == PositionBiasUnits::meter ? "NED" : "LatLonAlt").c_str(), | |
| 187 | ✗ | _positionBias, _positionBiasUnit, "m, m, m\0\0", "%.2g"); | |
| 188 | ✗ | if (NAV::NodeRegistry::NodeDataTypeAnyIsChildOf(outputPins.front().dataIdentifier, { PosVel::type() })) | |
| 189 | { | ||
| 190 | ✗ | inputVector3WithUnit("Velocity Bias (NED)", _velocityBias, _velocityBiasUnit, "m/s\0\0", "%.2g"); | |
| 191 | ✗ | if (NAV::NodeRegistry::NodeDataTypeAnyIsChildOf(outputPins.front().dataIdentifier, { PosVelAtt::type() })) | |
| 192 | { | ||
| 193 | ✗ | inputVector3WithUnit("RollPitchYaw Bias", _attitudeBias, _attitudeBiasUnit, "rad\0deg\0\0", "%.2g"); | |
| 194 | } | ||
| 195 | } | ||
| 196 | } | ||
| 197 | } | ||
| 198 | ✗ | ImGui::Unindent(); | |
| 199 | } | ||
| 200 | |||
| 201 | ✗ | if (NAV::NodeRegistry::NodeDataTypeAnyIsChildOf(outputPins.front().dataIdentifier, { ImuObs::type() }) | |
| 202 | ✗ | || NAV::NodeRegistry::NodeDataTypeAnyIsChildOf(outputPins.front().dataIdentifier, { Pos::type() }) | |
| 203 | ✗ | || NAV::NodeRegistry::NodeDataTypeAnyIsChildOf(outputPins.front().dataIdentifier, { GnssObs::type() })) | |
| 204 | { | ||
| 205 | ✗ | ImGui::TextUnformatted("Measurement noise:"); | |
| 206 | ✗ | ImGui::Indent(); | |
| 207 | { | ||
| 208 | ✗ | if (NAV::NodeRegistry::NodeDataTypeAnyIsChildOf(outputPins.front().dataIdentifier, { ImuObs::type() })) | |
| 209 | { | ||
| 210 | ✗ | noiseGuiInput("Accelerometer Noise (Std. dev)", _imuAccelerometerNoise, _imuAccelerometerNoiseUnit, MakeComboItems<Units::ImuAccelerometerNoiseUnits>().c_str(), "%.2g", _imuAccelerometerRng); | |
| 211 | ✗ | noiseGuiInput("Gyroscope Noise (Std. dev)", _imuGyroscopeNoise, _imuGyroscopeNoiseUnit, MakeComboItems<Units::ImuGyroscopeNoiseUnits>().c_str(), "%.2g", _imuGyroscopeRng); | |
| 212 | } | ||
| 213 | ✗ | else if (NAV::NodeRegistry::NodeDataTypeAnyIsChildOf(outputPins.front().dataIdentifier, { Pos::type() })) | |
| 214 | { | ||
| 215 | ✗ | noiseGuiInput(fmt::format("Position Noise ({})", _positionNoiseUnit == PositionNoiseUnits::meter | |
| 216 | ✗ | ? "Standard deviation" | |
| 217 | : "Variance") | ||
| 218 | .c_str(), | ||
| 219 | ✗ | _positionNoise, _positionNoiseUnit, "m, m, m\0m^2, m^2, m^2\0\0", | |
| 220 | ✗ | "%.2g", _positionRng); | |
| 221 | ✗ | if (NAV::NodeRegistry::NodeDataTypeAnyIsChildOf(outputPins.front().dataIdentifier, { PosVel::type() })) | |
| 222 | { | ||
| 223 | ✗ | noiseGuiInput(fmt::format("Velocity Noise ({})", _velocityNoiseUnit == VelocityNoiseUnits::m_s ? "Standard deviation" | |
| 224 | : "Variance") | ||
| 225 | .c_str(), | ||
| 226 | ✗ | _velocityNoise, _velocityNoiseUnit, "m/s\0m^2/s^2\0\0", "%.2g", _velocityRng); | |
| 227 | ✗ | if (NAV::NodeRegistry::NodeDataTypeAnyIsChildOf(outputPins.front().dataIdentifier, { PosVelAtt::type() })) | |
| 228 | { | ||
| 229 | ✗ | noiseGuiInput(fmt::format("Attitude Noise ({})", _attitudeNoiseUnit == AttitudeNoiseUnits::rad || _attitudeNoiseUnit == AttitudeNoiseUnits::deg | |
| 230 | ✗ | ? "Standard deviation" | |
| 231 | : "Variance") | ||
| 232 | .c_str(), | ||
| 233 | ✗ | _attitudeNoise, _attitudeNoiseUnit, "rad\0deg\0rad^2\0deg^2\0\0", "%.2g", _attitudeRng); | |
| 234 | } | ||
| 235 | } | ||
| 236 | } | ||
| 237 | ✗ | else if (NAV::NodeRegistry::NodeDataTypeAnyIsChildOf(outputPins.front().dataIdentifier, { GnssObs::type() })) | |
| 238 | { | ||
| 239 | ✗ | noiseGuiInput("Pseudorange Noise", _gui_pseudorangeNoise, _gui_pseudorangeNoiseUnit, "m\0\0", "%.3g", _pseudorangeRng); | |
| 240 | ✗ | noiseGuiInput("Carrier-phase Noise", _gui_carrierPhaseNoise, _gui_carrierPhaseNoiseUnit, "m\0\0", "%.3g", _carrierPhaseRng); | |
| 241 | ✗ | noiseGuiInput("Doppler/Range-rate Noise", _gui_dopplerNoise, _gui_dopplerNoiseUnit, "m/s\0\0", "%.3g", _dopplerRng); | |
| 242 | } | ||
| 243 | } | ||
| 244 | ✗ | ImGui::Unindent(); | |
| 245 | } | ||
| 246 | ✗ | if (NAV::NodeRegistry::NodeDataTypeAnyIsChildOf(outputPins.front().dataIdentifier, { ImuObs::type() })) | |
| 247 | { | ||
| 248 | ✗ | ImGui::TextUnformatted("Random walk noise:"); | |
| 249 | ✗ | ImGui::Indent(); | |
| 250 | ✗ | noiseGuiInput("Accelerometer RW (Std. dev)", _imuAccelerometerRW, _imuAccelerometerRWUnit, MakeComboItems<Units::ImuAccelerometerNoiseUnits>().c_str(), "%.2g", _imuAccelerometerRWRng); | |
| 251 | ✗ | noiseGuiInput("Gyroscope RW (Std. dev)", _imuGyroscopeRW, _imuGyroscopeRWUnit, MakeComboItems<Units::ImuGyroscopeNoiseUnits>().c_str(), "%.2g", _imuGyroscopeRWRng); | |
| 252 | ✗ | ImGui::Unindent(); | |
| 253 | ✗ | ImGui::TextUnformatted("Integrated Random walk noise:"); | |
| 254 | ✗ | ImGui::Indent(); | |
| 255 | ✗ | noiseGuiInput("Accelerometer IRW (Std. dev)", _imuAccelerometerIRW, _imuAccelerometerIRWUnit, MakeComboItems<Units::ImuAccelerometerIRWUnits>().c_str(), "%.2g", _imuAccelerometerIRWRng); | |
| 256 | ✗ | noiseGuiInput("Gyroscope IRW (Std. dev)", _imuGyroscopeIRW, _imuGyroscopeIRWUnit, MakeComboItems<Units::ImuGyroscopeIRWUnits>().c_str(), "%.2g", _imuGyroscopeIRWRng); | |
| 257 | ✗ | ImGui::Unindent(); | |
| 258 | } | ||
| 259 | |||
| 260 | ✗ | if (NAV::NodeRegistry::NodeDataTypeAnyIsChildOf(outputPins.front().dataIdentifier, { GnssObs::type() })) | |
| 261 | { | ||
| 262 | ✗ | ImGui::TextUnformatted("Ambiguities:"); | |
| 263 | ✗ | ImGui::Indent(); | |
| 264 | { | ||
| 265 | ✗ | ImGui::SetNextItemWidth((itemWidth - ImGui::GetStyle().ItemInnerSpacing.x) / 2.0F); | |
| 266 | ✗ | if (ImGui::InputIntL(fmt::format("##Ambiguity Bounds lower {}", size_t(id)).c_str(), _gui_ambiguityLimits.data(), | |
| 267 | ✗ | std::numeric_limits<int>::lowest(), _gui_ambiguityLimits[1], 0, 0)) | |
| 268 | { | ||
| 269 | ✗ | LOG_DEBUG("{}: Ambiguity lower bound changed to {}", nameId(), _gui_ambiguityLimits[0]); | |
| 270 | ✗ | flow::ApplyChanges(); | |
| 271 | } | ||
| 272 | ✗ | ImGui::SameLine(); | |
| 273 | ✗ | ImGui::SetCursorPosX(ImGui::GetCursorPosX() - ImGui::GetStyle().ItemSpacing.x + ImGui::GetStyle().ItemInnerSpacing.x); | |
| 274 | ✗ | ImGui::SetNextItemWidth((itemWidth - ImGui::GetStyle().ItemInnerSpacing.x) / 2.0F); | |
| 275 | ✗ | if (ImGui::InputIntL(fmt::format("Ambiguity Bounds##Ambiguity Bounds upper{}", size_t(id)).c_str(), &_gui_ambiguityLimits[1], | |
| 276 | ✗ | _gui_ambiguityLimits[0], std::numeric_limits<int>::max(), 0, 0)) | |
| 277 | { | ||
| 278 | ✗ | LOG_DEBUG("{}: Ambiguity upper bound changed to {}", nameId(), _gui_ambiguityLimits[1]); | |
| 279 | ✗ | flow::ApplyChanges(); | |
| 280 | } | ||
| 281 | ✗ | rngInput("Ambiguity", _ambiguityRng); | |
| 282 | } | ||
| 283 | ✗ | ImGui::Unindent(); | |
| 284 | |||
| 285 | ✗ | ImGui::TextUnformatted("Cycle-slip:"); | |
| 286 | ✗ | ImGui::Indent(); | |
| 287 | { | ||
| 288 | ✗ | noiseGuiInput("Frequency", _gui_cycleSlipFrequency, _gui_cycleSlipFrequencyUnit, "/ day\0/ hour\0/ minute\0\0", "%.2g", _cycleSlipRng); | |
| 289 | |||
| 290 | ✗ | if (auto response = gui::widgets::SliderDoubleWithUnit(fmt::format("Detection probability (LLI)##{}", size_t(id)).c_str(), itemWidth, unitWidth, | |
| 291 | &_gui_cycleSlipDetectionProbability, 0.0, 100.0, | ||
| 292 | ✗ | _gui_cycleSlipDetectionProbabilityUnit, "%\0\0", "%.2f")) | |
| 293 | { | ||
| 294 | ✗ | if (response == gui::widgets::InputWithUnitChange_Input) { LOG_DEBUG("{}: Detection probability (LLI) changed to {}", nameId(), _gui_cycleSlipDetectionProbability); } | |
| 295 | ✗ | if (response == gui::widgets::InputWithUnitChange_Unit) { LOG_DEBUG("{}: Detection probability (LLI) unit changed to {}", nameId(), fmt::underlying(_gui_cycleSlipDetectionProbabilityUnit)); } | |
| 296 | ✗ | flow::ApplyChanges(); | |
| 297 | } | ||
| 298 | ✗ | ImGui::SameLine(); | |
| 299 | ✗ | gui::widgets::HelpMarker("The chance that the Lock-of-Loss (LLI) indicator is set, when a cycle-slip occurs"); | |
| 300 | |||
| 301 | ✗ | ImGui::SetNextItemWidth(itemWidth); | |
| 302 | ✗ | if (ImGui::DragInt(fmt::format("Ambiguity Range to slip to##{}", size_t(id)).c_str(), &_gui_cycleSlipRange, | |
| 303 | 1.0F, 1, std::numeric_limits<int>::max(), "+/- %d")) | ||
| 304 | { | ||
| 305 | ✗ | LOG_DEBUG("{}: Cycle-slip ambiguity range changed to {}", nameId(), _gui_cycleSlipRange); | |
| 306 | ✗ | flow::ApplyChanges(); | |
| 307 | } | ||
| 308 | ✗ | ImGui::SameLine(); | |
| 309 | ✗ | gui::widgets::HelpMarker("If the ambiguity range is set, the cycle-slips are bound by it too"); | |
| 310 | |||
| 311 | ✗ | ImGui::SetNextItemWidth(itemWidth); | |
| 312 | ✗ | if (ShowFrequencySelector(fmt::format("Satellite Frequencies##{}", size_t(id)).c_str(), _filterFreq)) | |
| 313 | { | ||
| 314 | ✗ | flow::ApplyChanges(); | |
| 315 | } | ||
| 316 | ✗ | ImGui::SameLine(); | |
| 317 | ✗ | gui::widgets::HelpMarker("Frequency and code selection only affects the cycle-slip simulation"); | |
| 318 | |||
| 319 | ✗ | ImGui::SetNextItemWidth(itemWidth); | |
| 320 | ✗ | if (ShowCodeSelector(fmt::format("Signal Codes##{}", size_t(id)).c_str(), _filterCode, _filterFreq)) | |
| 321 | { | ||
| 322 | ✗ | flow::ApplyChanges(); | |
| 323 | } | ||
| 324 | } | ||
| 325 | ✗ | ImGui::Unindent(); | |
| 326 | |||
| 327 | ✗ | ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver); | |
| 328 | ✗ | if (ImGui::TreeNode(fmt::format("Simulated Ambiguities and Cycle-slips##{}", size_t(id)).c_str())) | |
| 329 | { | ||
| 330 | ✗ | auto nAmb = static_cast<int>(_ambiguities.size()); | |
| 331 | ✗ | if (nAmb > 0) | |
| 332 | { | ||
| 333 | ✗ | std::set<InsTime> ambiguityTimes; | |
| 334 | ✗ | for (int i = 0; i < nAmb; i++) | |
| 335 | { | ||
| 336 | ✗ | const auto& ambVec = std::next(_ambiguities.begin(), i)->second; | |
| 337 | ✗ | for (const auto& amb : ambVec) | |
| 338 | { | ||
| 339 | ✗ | ambiguityTimes.insert(amb.first); | |
| 340 | } | ||
| 341 | } | ||
| 342 | ✗ | if (ambiguityTimes.size() < 64 - 1) | |
| 343 | { | ||
| 344 | ✗ | ImGui::PushFont(Application::MonoFont()); | |
| 345 | |||
| 346 | ✗ | if (ImGui::BeginTable(fmt::format("Ambiguities##{}", size_t(id)).c_str(), static_cast<int>(ambiguityTimes.size() + 1), | |
| 347 | ImGuiTableFlags_Borders | ImGuiTableFlags_NoHostExtendX | ImGuiTableFlags_SizingFixedFit | ImGuiTableFlags_ScrollX | ImGuiTableFlags_ScrollY, | ||
| 348 | ✗ | ImVec2(0.0F, 300.0F * gui::NodeEditorApplication::monoFontRatio()))) | |
| 349 | { | ||
| 350 | ✗ | ImGui::TableSetupColumn(""); | |
| 351 | ✗ | for (const auto& time : ambiguityTimes) | |
| 352 | { | ||
| 353 | ✗ | auto t = time.toYMDHMS(GPST); | |
| 354 | ✗ | t.sec = std::round(t.sec * 1e2) / 1e2; | |
| 355 | ✗ | ImGui::TableSetupColumn(str::replaceAll_copy(fmt::format("{}", t), " ", "\n").c_str()); | |
| 356 | } | ||
| 357 | ✗ | ImGui::TableSetupScrollFreeze(1, 1); | |
| 358 | ✗ | ImGui::TableHeadersRow(); | |
| 359 | |||
| 360 | ✗ | for (int i = 0; i < nAmb; i++) | |
| 361 | { | ||
| 362 | ✗ | ImGui::TableNextRow(); | |
| 363 | ✗ | ImGui::TableNextColumn(); | |
| 364 | ✗ | const auto& ambiguities = std::next(_ambiguities.begin(), i); | |
| 365 | ✗ | if (ambiguities == _ambiguities.end()) { break; } | |
| 366 | ✗ | ImGui::TextUnformatted(fmt::format("{}", ambiguities->first).c_str()); | |
| 367 | ✗ | ImGui::TableSetBgColor(ImGuiTableBgTarget_CellBg, ImGui::GetColorU32(ImGui::GetStyle().Colors[ImGuiCol_TableHeaderBg])); | |
| 368 | |||
| 369 | ✗ | for (const auto& time : ambiguityTimes) | |
| 370 | { | ||
| 371 | ✗ | ImGui::TableNextColumn(); | |
| 372 | ✗ | auto iter = std::ranges::find_if(ambiguities->second, [&time](const auto& timeAndAmb) { | |
| 373 | ✗ | return timeAndAmb.first == time; | |
| 374 | }); | ||
| 375 | ✗ | if (iter != ambiguities->second.end()) | |
| 376 | { | ||
| 377 | ✗ | ImVec4 color; | |
| 378 | ✗ | auto cycleIter = std::ranges::find_if(_cycleSlips, [&](const CycleSlipInfo& cycleSlip) { | |
| 379 | ✗ | return cycleSlip.time == time && cycleSlip.satSigId == ambiguities->first; | |
| 380 | }); | ||
| 381 | ✗ | if (cycleIter != _cycleSlips.end()) { color = ImColor(240, 128, 128); } | |
| 382 | ✗ | else if (ambiguities->second.back().first == time) { color = ImGui::GetStyle().Colors[ImGuiCol_Text]; } | |
| 383 | ✗ | else { color = ImGui::GetStyle().Colors[ImGuiCol_TextDisabled]; } | |
| 384 | ✗ | ImGui::TextColored(color, "%s%s", | |
| 385 | ✗ | fmt::format("{}", iter->second).c_str(), | |
| 386 | ✗ | cycleIter != _cycleSlips.end() && cycleIter->LLI ? " (LLI)" : ""); | |
| 387 | ✗ | if (ImGui::IsItemHovered() && cycleIter != _cycleSlips.end()) { ImGui::SetTooltip("Cycle-slip"); } | |
| 388 | } | ||
| 389 | ✗ | else if (time < ambiguities->second.front().first) | |
| 390 | { | ||
| 391 | ✗ | ImGui::TableSetBgColor(ImGuiTableBgTarget_CellBg, ImColor(70, 70, 70)); | |
| 392 | } | ||
| 393 | } | ||
| 394 | } | ||
| 395 | |||
| 396 | ✗ | ImGui::EndTable(); | |
| 397 | } | ||
| 398 | ✗ | ImGui::PopFont(); | |
| 399 | } | ||
| 400 | else | ||
| 401 | { | ||
| 402 | ✗ | ImGui::TextColored(ImColor(255, 0, 0), "More than 64 timestamps for ambiguities, which cannot be displayed in a table"); | |
| 403 | } | ||
| 404 | ✗ | } | |
| 405 | |||
| 406 | ✗ | ImGui::TreePop(); | |
| 407 | } | ||
| 408 | } | ||
| 409 | ✗ | } | |
| 410 | |||
| 411 | ✗ | json NAV::ErrorModel::save() const | |
| 412 | { | ||
| 413 | LOG_TRACE("{}: called", nameId()); | ||
| 414 | |||
| 415 | ✗ | json j; | |
| 416 | |||
| 417 | ✗ | j["imuAccelerometerBiasUnit"] = _imuAccelerometerBiasUnit; | |
| 418 | ✗ | j["imuAccelerometerBias_p"] = _imuAccelerometerBias_p; | |
| 419 | ✗ | j["imuGyroscopeBiasUnit"] = _imuGyroscopeBiasUnit; | |
| 420 | ✗ | j["imuGyroscopeBias_p"] = _imuGyroscopeBias_p; | |
| 421 | ✗ | j["imuAccelerometerNoiseUnit"] = _imuAccelerometerNoiseUnit; | |
| 422 | ✗ | j["imuAccelerometerNoise"] = _imuAccelerometerNoise; | |
| 423 | ✗ | j["imuAccelerometerRng"] = _imuAccelerometerRng; | |
| 424 | ✗ | j["imuGyroscopeNoiseUnit"] = _imuGyroscopeNoiseUnit; | |
| 425 | ✗ | j["imuGyroscopeNoise"] = _imuGyroscopeNoise; | |
| 426 | ✗ | j["imuGyroscopeRng"] = _imuGyroscopeRng; | |
| 427 | ✗ | j["imuAccelerometerRWUnit"] = _imuAccelerometerRWUnit; | |
| 428 | ✗ | j["imuAccelerometerRW"] = _imuAccelerometerRW; | |
| 429 | ✗ | j["imuAccelerometerRWRng"] = _imuAccelerometerRWRng; | |
| 430 | |||
| 431 | ✗ | j["imuGyroscopeRWUnit"] = _imuGyroscopeRWUnit; | |
| 432 | ✗ | j["imuGyroscopeRW"] = _imuGyroscopeRW; | |
| 433 | ✗ | j["imuGyroscopeRWRng"] = _imuGyroscopeRWRng; | |
| 434 | ✗ | j["imuAccelerometerIRWUnit"] = _imuAccelerometerIRWUnit; | |
| 435 | ✗ | j["imuAccelerometerIRW"] = _imuAccelerometerIRW; | |
| 436 | ✗ | j["imuAccelerometerIRWRng"] = _imuAccelerometerIRWRng; | |
| 437 | ✗ | j["imuGyroscopeIRWUnit"] = _imuGyroscopeIRWUnit; | |
| 438 | ✗ | j["imuGyroscopeIRW"] = _imuGyroscopeIRW; | |
| 439 | ✗ | j["imuGyroscopeIRWRng"] = _imuGyroscopeIRWRng; | |
| 440 | |||
| 441 | // ######################################################################################################################################### | ||
| 442 | ✗ | j["positionBiasUnit"] = _positionBiasUnit; | |
| 443 | ✗ | j["positionBias"] = _positionBias; | |
| 444 | ✗ | j["velocityBiasUnit"] = _velocityBiasUnit; | |
| 445 | ✗ | j["velocityBias"] = _velocityBias; | |
| 446 | ✗ | j["attitudeBiasUnit"] = _attitudeBiasUnit; | |
| 447 | ✗ | j["attitudeBias"] = _attitudeBias; | |
| 448 | ✗ | j["positionNoiseUnit"] = _positionNoiseUnit; | |
| 449 | ✗ | j["positionNoise"] = _positionNoise; | |
| 450 | ✗ | j["positionRng"] = _positionRng; | |
| 451 | ✗ | j["velocityNoiseUnit"] = _velocityNoiseUnit; | |
| 452 | ✗ | j["velocityNoise"] = _velocityNoise; | |
| 453 | ✗ | j["velocityRng"] = _velocityRng; | |
| 454 | ✗ | j["attitudeNoiseUnit"] = _attitudeNoiseUnit; | |
| 455 | ✗ | j["attitudeNoise"] = _attitudeNoise; | |
| 456 | ✗ | j["attitudeRng"] = _attitudeRng; | |
| 457 | // ######################################################################################################################################### | ||
| 458 | ✗ | j["pseudorangeNoiseUnit"] = _gui_pseudorangeNoiseUnit; | |
| 459 | ✗ | j["pseudorangeNoise"] = _gui_pseudorangeNoise; | |
| 460 | ✗ | j["pseudorangeRng"] = _pseudorangeRng; | |
| 461 | ✗ | j["carrierPhaseNoiseUnit"] = _gui_carrierPhaseNoiseUnit; | |
| 462 | ✗ | j["carrierPhaseNoise"] = _gui_carrierPhaseNoise; | |
| 463 | ✗ | j["carrierPhaseRng"] = _carrierPhaseRng; | |
| 464 | ✗ | j["dopplerNoiseUnit"] = _gui_dopplerNoiseUnit; | |
| 465 | ✗ | j["dopplerNoise"] = _gui_dopplerNoise; | |
| 466 | ✗ | j["dopplerRng"] = _dopplerRng; | |
| 467 | ✗ | j["ambiguityLimits"] = _gui_ambiguityLimits; | |
| 468 | ✗ | j["ambiguityRng"] = _ambiguityRng; | |
| 469 | ✗ | j["cycleSlipFrequencyUnit"] = _gui_cycleSlipFrequencyUnit; | |
| 470 | ✗ | j["cycleSlipFrequency"] = _gui_cycleSlipFrequency; | |
| 471 | ✗ | j["cycleSlipRange"] = _gui_cycleSlipRange; | |
| 472 | ✗ | j["cycleSlipDetectionProbabilityUnit"] = _gui_cycleSlipDetectionProbabilityUnit; | |
| 473 | ✗ | j["cycleSlipDetectionProbability"] = _gui_cycleSlipDetectionProbability; | |
| 474 | ✗ | j["cycleSlipRng"] = _cycleSlipRng; | |
| 475 | |||
| 476 | ✗ | j["filterFreq"] = Frequency_(_filterFreq); | |
| 477 | ✗ | j["filterCode"] = _filterCode; | |
| 478 | |||
| 479 | ✗ | return j; | |
| 480 | ✗ | } | |
| 481 | |||
| 482 | 6 | void NAV::ErrorModel::restore(json const& j) | |
| 483 | { | ||
| 484 | LOG_TRACE("{}: called", nameId()); | ||
| 485 | |||
| 486 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("imuAccelerometerBiasUnit")) { j.at("imuAccelerometerBiasUnit").get_to(_imuAccelerometerBiasUnit); } |
| 487 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("imuAccelerometerBias_p")) { j.at("imuAccelerometerBias_p").get_to(_imuAccelerometerBias_p); } |
| 488 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("imuGyroscopeBiasUnit")) { j.at("imuGyroscopeBiasUnit").get_to(_imuGyroscopeBiasUnit); } |
| 489 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("imuGyroscopeBias_p")) { j.at("imuGyroscopeBias_p").get_to(_imuGyroscopeBias_p); } |
| 490 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("imuAccelerometerNoiseUnit")) { j.at("imuAccelerometerNoiseUnit").get_to(_imuAccelerometerNoiseUnit); } |
| 491 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("imuAccelerometerNoise")) { j.at("imuAccelerometerNoise").get_to(_imuAccelerometerNoise); } |
| 492 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("imuAccelerometerRng")) { j.at("imuAccelerometerRng").get_to(_imuAccelerometerRng); } |
| 493 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("imuGyroscopeNoiseUnit")) { j.at("imuGyroscopeNoiseUnit").get_to(_imuGyroscopeNoiseUnit); } |
| 494 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("imuGyroscopeNoise")) { j.at("imuGyroscopeNoise").get_to(_imuGyroscopeNoise); } |
| 495 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("imuGyroscopeRng")) { j.at("imuGyroscopeRng").get_to(_imuGyroscopeRng); } |
| 496 | |||
| 497 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("imuAccelerometerRWUnit")) { j.at("imuAccelerometerRWUnit").get_to(_imuAccelerometerRWUnit); } |
| 498 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("imuAccelerometerRW")) { j.at("imuAccelerometerRW").get_to(_imuAccelerometerRW); } |
| 499 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("imuAccelerometerRWRng")) { j.at("imuAccelerometerRWRng").get_to(_imuAccelerometerRWRng); } |
| 500 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("imuGyroscopeRWUnit")) { j.at("imuGyroscopeRWUnit").get_to(_imuGyroscopeRWUnit); } |
| 501 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("imuGyroscopeRW")) { j.at("imuGyroscopeRW").get_to(_imuGyroscopeRW); } |
| 502 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("imuGyroscopeRWRng")) { j.at("imuGyroscopeRWRng").get_to(_imuGyroscopeRWRng); } |
| 503 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("imuAccelerometerIRWUnit")) { j.at("imuAccelerometerIRWUnit").get_to(_imuAccelerometerIRWUnit); } |
| 504 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("imuAccelerometerIRW")) { j.at("imuAccelerometerIRW").get_to(_imuAccelerometerIRW); } |
| 505 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("imuAccelerometerIRWRng")) { j.at("imuAccelerometerIRWRng").get_to(_imuAccelerometerIRWRng); } |
| 506 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("imuGyroscopeIRWUnit")) { j.at("imuGyroscopeIRWUnit").get_to(_imuGyroscopeIRWUnit); } |
| 507 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("imuGyroscopeIRW")) { j.at("imuGyroscopeIRW").get_to(_imuGyroscopeIRW); } |
| 508 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("imuGyroscopeIRWRng")) { j.at("imuGyroscopeIRWRng").get_to(_imuGyroscopeIRWRng); } |
| 509 | // ######################################################################################################################################### | ||
| 510 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("positionBiasUnit")) { j.at("positionBiasUnit").get_to(_positionBiasUnit); } |
| 511 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("positionBias")) { j.at("positionBias").get_to(_positionBias); } |
| 512 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("velocityBiasUnit")) { j.at("velocityBiasUnit").get_to(_velocityBiasUnit); } |
| 513 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("velocityBias")) { j.at("velocityBias").get_to(_velocityBias); } |
| 514 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("attitudeBiasUnit")) { j.at("attitudeBiasUnit").get_to(_attitudeBiasUnit); } |
| 515 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("attitudeBias")) { j.at("attitudeBias").get_to(_attitudeBias); } |
| 516 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("positionNoiseUnit")) { j.at("positionNoiseUnit").get_to(_positionNoiseUnit); } |
| 517 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("positionNoise")) { j.at("positionNoise").get_to(_positionNoise); } |
| 518 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("positionRng")) { j.at("positionRng").get_to(_positionRng); } |
| 519 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("velocityNoiseUnit")) { j.at("velocityNoiseUnit").get_to(_velocityNoiseUnit); } |
| 520 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("velocityNoise")) { j.at("velocityNoise").get_to(_velocityNoise); } |
| 521 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("velocityRng")) { j.at("velocityRng").get_to(_velocityRng); } |
| 522 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("attitudeNoiseUnit")) { j.at("attitudeNoiseUnit").get_to(_attitudeNoiseUnit); } |
| 523 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("attitudeNoise")) { j.at("attitudeNoise").get_to(_attitudeNoise); } |
| 524 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("attitudeRng")) { j.at("attitudeRng").get_to(_attitudeRng); } |
| 525 | // ######################################################################################################################################### | ||
| 526 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("pseudorangeNoiseUnit")) { j.at("pseudorangeNoiseUnit").get_to(_gui_pseudorangeNoiseUnit); } |
| 527 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("pseudorangeNoise")) { j.at("pseudorangeNoise").get_to(_gui_pseudorangeNoise); } |
| 528 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("pseudorangeRng")) { j.at("pseudorangeRng").get_to(_pseudorangeRng); } |
| 529 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("carrierPhaseNoiseUnit")) { j.at("carrierPhaseNoiseUnit").get_to(_gui_carrierPhaseNoiseUnit); } |
| 530 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("carrierPhaseNoise")) { j.at("carrierPhaseNoise").get_to(_gui_carrierPhaseNoise); } |
| 531 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("carrierPhaseRng")) { j.at("carrierPhaseRng").get_to(_carrierPhaseRng); } |
| 532 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("dopplerNoiseUnit")) { j.at("dopplerNoiseUnit").get_to(_gui_dopplerNoiseUnit); } |
| 533 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("dopplerNoise")) { j.at("dopplerNoise").get_to(_gui_dopplerNoise); } |
| 534 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("dopplerRng")) { j.at("dopplerRng").get_to(_dopplerRng); } |
| 535 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("ambiguityLimits")) { j.at("ambiguityLimits").get_to(_gui_ambiguityLimits); } |
| 536 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("ambiguityRng")) { j.at("ambiguityRng").get_to(_ambiguityRng); } |
| 537 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("cycleSlipFrequencyUnit")) { j.at("cycleSlipFrequencyUnit").get_to(_gui_cycleSlipFrequencyUnit); } |
| 538 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("cycleSlipFrequency")) { j.at("cycleSlipFrequency").get_to(_gui_cycleSlipFrequency); } |
| 539 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("cycleSlipRange")) { j.at("cycleSlipRange").get_to(_gui_cycleSlipRange); } |
| 540 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("cycleSlipDetectionProbabilityUnit")) { j.at("cycleSlipDetectionProbabilityUnit").get_to(_gui_cycleSlipDetectionProbabilityUnit); } |
| 541 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("cycleSlipDetectionProbability")) { j.at("cycleSlipDetectionProbability").get_to(_gui_cycleSlipDetectionProbability); } |
| 542 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("cycleSlipRng")) { j.at("cycleSlipRng").get_to(_cycleSlipRng); } |
| 543 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("filterFreq")) |
| 544 | { | ||
| 545 | 6 | uint64_t value = 0; | |
| 546 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
6 | j.at("filterFreq").get_to(value); |
| 547 | 6 | _filterFreq = Frequency_(value); | |
| 548 | } | ||
| 549 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("filterCode")) { j.at("filterCode").get_to(_filterCode); } |
| 550 | 6 | } | |
| 551 | |||
| 552 | 12 | bool NAV::ErrorModel::resetNode() | |
| 553 | { | ||
| 554 | LOG_TRACE("{}: called", nameId()); | ||
| 555 | |||
| 556 | 12 | _lastObservationTime.reset(); | |
| 557 | 12 | _dt = 0.0; | |
| 558 | |||
| 559 | // ImuObs | ||
| 560 | 12 | _imuAccelerometerRng.resetSeed(size_t(id)); | |
| 561 | 12 | _imuGyroscopeRng.resetSeed(size_t(id)); | |
| 562 | 12 | _imuAccelerometerRWRng.resetSeed(size_t(id)); | |
| 563 | 12 | _imuGyroscopeRWRng.resetSeed(size_t(id)); | |
| 564 | 12 | _imuAccelerometerIRWRng.resetSeed(size_t(id)); | |
| 565 | 12 | _imuGyroscopeIRWRng.resetSeed(size_t(id)); | |
| 566 | 12 | _randomWalkAccelerometer.setZero(); | |
| 567 | 12 | _randomWalkGyroscope.setZero(); | |
| 568 | 12 | _integratedRandomWalkAccelerometer.setZero(); | |
| 569 | 12 | _integratedRandomWalkGyroscope.setZero(); | |
| 570 | 12 | _integratedRandomWalkAccelerometer_velocity.setZero(); | |
| 571 | 12 | _integratedRandomWalkGyroscope_velocity.setZero(); | |
| 572 | |||
| 573 | // PosVelAtt | ||
| 574 | 12 | _positionRng.resetSeed(size_t(id)); | |
| 575 | 12 | _velocityRng.resetSeed(size_t(id)); | |
| 576 | 12 | _attitudeRng.resetSeed(size_t(id)); | |
| 577 | |||
| 578 | // GnssObs | ||
| 579 | 12 | _pseudorangeRng.resetSeed(size_t(id)); | |
| 580 | 12 | _carrierPhaseRng.resetSeed(size_t(id)); | |
| 581 | 12 | _dopplerRng.resetSeed(size_t(id)); | |
| 582 | 12 | _ambiguityRng.resetSeed(size_t(id)); | |
| 583 | 12 | _ambiguities.clear(); | |
| 584 | 12 | _cycleSlips.clear(); | |
| 585 | 12 | _cycleSlipRng.resetSeed(size_t(id)); | |
| 586 | 12 | _cycleSlipWindowStartTime.reset(); | |
| 587 | |||
| 588 | 12 | return true; | |
| 589 | } | ||
| 590 | |||
| 591 | 14 | void NAV::ErrorModel::afterCreateLink(OutputPin& startPin, InputPin& endPin) | |
| 592 | { | ||
| 593 | LOG_TRACE("{}: called for {} ==> {}", nameId(), size_t(startPin.id), size_t(endPin.id)); | ||
| 594 | |||
| 595 |
3/4✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 8 times.
✓ Branch 4 taken 6 times.
|
14 | if (endPin.parentNode->id != id) |
| 596 | { | ||
| 597 | 8 | return; // Link on Output Port | |
| 598 | } | ||
| 599 | |||
| 600 | // Store previous output pin identifier | ||
| 601 |
1/2✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
|
6 | auto previousOutputPinDataIdentifier = outputPins.front().dataIdentifier; |
| 602 | // Overwrite output pin identifier with input pin identifier | ||
| 603 |
1/2✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
|
6 | outputPins.front().dataIdentifier = startPin.dataIdentifier; |
| 604 | |||
| 605 |
2/4✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
6 | if (previousOutputPinDataIdentifier != outputPins.front().dataIdentifier) // If the identifier changed |
| 606 | { | ||
| 607 | // Check if connected links on output port are still valid | ||
| 608 |
2/2✓ Branch 6 taken 1 times.
✓ Branch 7 taken 6 times.
|
7 | for (auto& link : outputPins.front().links) |
| 609 | { | ||
| 610 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
|
1 | if (auto* endPin = link.getConnectedPin()) |
| 611 | { | ||
| 612 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 1 times.
|
1 | if (!outputPins.front().canCreateLink(*endPin)) |
| 613 | { | ||
| 614 | // If the link is not valid anymore, delete it | ||
| 615 | ✗ | outputPins.front().deleteLink(*endPin); | |
| 616 | } | ||
| 617 | } | ||
| 618 | } | ||
| 619 | |||
| 620 | // Refresh all links connected to the output pin if the type changed | ||
| 621 |
2/4✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
6 | if (outputPins.front().dataIdentifier != previousOutputPinDataIdentifier) |
| 622 | { | ||
| 623 |
2/2✓ Branch 6 taken 1 times.
✓ Branch 7 taken 6 times.
|
7 | for (auto& link : outputPins.front().links) |
| 624 | { | ||
| 625 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
|
1 | if (auto* connectedPin = link.getConnectedPin()) |
| 626 | { | ||
| 627 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | outputPins.front().recreateLink(*connectedPin); |
| 628 | } | ||
| 629 | } | ||
| 630 | } | ||
| 631 | } | ||
| 632 | 6 | } | |
| 633 | |||
| 634 | 14 | void NAV::ErrorModel::afterDeleteLink(OutputPin& startPin, InputPin& endPin) | |
| 635 | { | ||
| 636 | LOG_TRACE("{}: called for {} ==> {}", nameId(), size_t(startPin.id), size_t(endPin.id)); | ||
| 637 | |||
| 638 | 14 | if ((endPin.parentNode->id != id // Link on Output port is removed | |
| 639 |
2/2✓ Branch 2 taken 4 times.
✓ Branch 3 taken 4 times.
|
8 | && !inputPins.front().isPinLinked()) // and the Input port is not linked |
| 640 |
6/6✓ Branch 0 taken 8 times.
✓ Branch 1 taken 6 times.
✓ Branch 3 taken 6 times.
✓ Branch 4 taken 4 times.
✓ Branch 5 taken 6 times.
✓ Branch 6 taken 8 times.
|
28 | || (startPin.parentNode->id != id // Link on Input port is removed |
| 641 |
2/2✓ Branch 2 taken 2 times.
✓ Branch 3 taken 4 times.
|
6 | && !outputPins.front().isPinLinked())) // and the Output port is not linked |
| 642 | { | ||
| 643 | 6 | outputPins.front().dataIdentifier = supportedDataIdentifier; | |
| 644 | } | ||
| 645 | 14 | } | |
| 646 | |||
| 647 | 58447 | void NAV::ErrorModel::receiveObs(NAV::InputPin::NodeDataQueue& queue, size_t /* pinIdx */) | |
| 648 | { | ||
| 649 |
1/2✓ Branch 1 taken 58442 times.
✗ Branch 2 not taken.
|
58447 | auto obs = queue.extract_front(); |
| 650 |
3/4✓ Branch 1 taken 58435 times.
✓ Branch 2 taken 6 times.
✓ Branch 5 taken 58434 times.
✗ Branch 6 not taken.
|
58442 | if (!_lastObservationTime.empty()) { _dt = static_cast<double>((obs->insTime - _lastObservationTime).count()); } |
| 651 | |||
| 652 | // Accelerometer Bias in platform frame coordinates [m/s^2] | ||
| 653 |
1/2✓ Branch 1 taken 58439 times.
✗ Branch 2 not taken.
|
58441 | Eigen::Vector3d accelerometerBias_p = convertUnit(_imuAccelerometerBias_p, _imuAccelerometerBiasUnit); |
| 654 | LOG_DATA("{}: accelerometerBias_p = {} [m/s^2]", nameId(), accelerometerBias_p.transpose()); | ||
| 655 | |||
| 656 | // Gyroscope Bias in platform frame coordinates [rad/s] | ||
| 657 |
1/2✓ Branch 1 taken 58439 times.
✗ Branch 2 not taken.
|
58439 | Eigen::Vector3d gyroscopeBias_p = convertUnit(_imuGyroscopeBias_p, _imuGyroscopeBiasUnit); |
| 658 | LOG_DATA("{}: gyroscopeBias_p = {} [rad/s]", nameId(), gyroscopeBias_p.transpose()); | ||
| 659 | |||
| 660 | // ######################################################################################################################################### | ||
| 661 | |||
| 662 | // Accelerometer Noise standard deviation in platform frame coordinates [m/s^2/sqrt(s)] | ||
| 663 |
1/2✓ Branch 1 taken 58441 times.
✗ Branch 2 not taken.
|
58439 | Eigen::Vector3d accelerometerNoiseStd = convertUnit(_imuAccelerometerNoise, _imuAccelerometerNoiseUnit); |
| 664 | LOG_DATA("{}: accelerometerNoiseStd = {} [m/s^2/sqrt(s)]", nameId(), accelerometerNoiseStd.transpose()); | ||
| 665 | |||
| 666 | // Gyroscope Noise standard deviation in platform frame coordinates [rad/s/sqrt(s)] | ||
| 667 |
1/2✓ Branch 1 taken 58439 times.
✗ Branch 2 not taken.
|
58441 | Eigen::Vector3d gyroscopeNoiseStd = convertUnit(_imuGyroscopeNoise, _imuGyroscopeNoiseUnit); |
| 668 | LOG_DATA("{}: gyroscopeNoiseStd = {} [rad/s/sqrt(s)]", nameId(), gyroscopeNoiseStd.transpose()); | ||
| 669 | |||
| 670 | // Select the correct data type and make a copy of the node data to modify | ||
| 671 |
6/10✓ Branch 1 taken 58445 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 58446 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 58439 times.
✓ Branch 9 taken 58438 times.
✓ Branch 11 taken 34215 times.
✓ Branch 12 taken 24223 times.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
|
175324 | if (NAV::NodeRegistry::NodeDataTypeAnyIsChildOf(outputPins.front().dataIdentifier, { ImuObsSimulated::type() })) |
| 672 | { | ||
| 673 |
1/2✓ Branch 2 taken 34225 times.
✗ Branch 3 not taken.
|
34224 | invokeCallbacks(OUTPUT_PORT_INDEX_FLOW, |
| 674 |
2/4✓ Branch 3 taken 34224 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 34224 times.
✗ Branch 8 not taken.
|
68441 | receiveImuObsWDelta(std::make_shared<ImuObsSimulated>(*std::static_pointer_cast<const ImuObsSimulated>(obs)), |
| 675 | accelerometerBias_p, | ||
| 676 | gyroscopeBias_p, | ||
| 677 | accelerometerNoiseStd, | ||
| 678 | gyroscopeNoiseStd)); | ||
| 679 | } | ||
| 680 |
5/10✓ Branch 1 taken 24222 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 24222 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 24222 times.
✓ Branch 9 taken 24222 times.
✗ Branch 11 not taken.
✓ Branch 12 taken 24222 times.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
|
72666 | else if (NAV::NodeRegistry::NodeDataTypeAnyIsChildOf(outputPins.front().dataIdentifier, { ImuObsWDelta::type() })) |
| 681 | { | ||
| 682 | ✗ | invokeCallbacks(OUTPUT_PORT_INDEX_FLOW, | |
| 683 | ✗ | receiveImuObsWDelta(std::make_shared<ImuObsWDelta>(*std::static_pointer_cast<const ImuObsWDelta>(obs)), | |
| 684 | accelerometerBias_p, | ||
| 685 | gyroscopeBias_p, | ||
| 686 | accelerometerNoiseStd, | ||
| 687 | gyroscopeNoiseStd)); | ||
| 688 | } | ||
| 689 |
5/10✓ Branch 1 taken 24222 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 24222 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 24222 times.
✓ Branch 9 taken 24222 times.
✗ Branch 11 not taken.
✓ Branch 12 taken 24222 times.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
|
72666 | else if (NAV::NodeRegistry::NodeDataTypeAnyIsChildOf(outputPins.front().dataIdentifier, { ImuObs::type() })) |
| 690 | { | ||
| 691 | ✗ | invokeCallbacks(OUTPUT_PORT_INDEX_FLOW, | |
| 692 | ✗ | receiveImuObs(std::make_shared<ImuObs>(*std::static_pointer_cast<const ImuObs>(obs)), | |
| 693 | accelerometerBias_p, | ||
| 694 | gyroscopeBias_p, | ||
| 695 | accelerometerNoiseStd, | ||
| 696 | gyroscopeNoiseStd)); | ||
| 697 | } | ||
| 698 |
5/10✓ Branch 1 taken 24222 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 24222 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 24222 times.
✓ Branch 9 taken 24222 times.
✓ Branch 11 taken 24222 times.
✗ Branch 12 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
|
72666 | else if (NAV::NodeRegistry::NodeDataTypeAnyIsChildOf(outputPins.front().dataIdentifier, { PosVelAtt::type() })) |
| 699 | { | ||
| 700 |
3/6✓ Branch 3 taken 24222 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 24222 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 24222 times.
✗ Branch 11 not taken.
|
24222 | invokeCallbacks(OUTPUT_PORT_INDEX_FLOW, receivePosVelAtt(std::make_shared<PosVelAtt>(*std::static_pointer_cast<const PosVelAtt>(obs)))); |
| 701 | } | ||
| 702 | ✗ | else if (NAV::NodeRegistry::NodeDataTypeAnyIsChildOf(outputPins.front().dataIdentifier, { PosVel::type() })) | |
| 703 | { | ||
| 704 | ✗ | invokeCallbacks(OUTPUT_PORT_INDEX_FLOW, receivePosVel(std::make_shared<PosVel>(*std::static_pointer_cast<const PosVel>(obs)))); | |
| 705 | } | ||
| 706 | ✗ | else if (NAV::NodeRegistry::NodeDataTypeAnyIsChildOf(outputPins.front().dataIdentifier, { Pos::type() })) | |
| 707 | { | ||
| 708 | ✗ | invokeCallbacks(OUTPUT_PORT_INDEX_FLOW, receivePos(std::make_shared<Pos>(*std::static_pointer_cast<const Pos>(obs)))); | |
| 709 | } | ||
| 710 | ✗ | else if (NAV::NodeRegistry::NodeDataTypeAnyIsChildOf(outputPins.front().dataIdentifier, { GnssObs::type() })) | |
| 711 | { | ||
| 712 | ✗ | invokeCallbacks(OUTPUT_PORT_INDEX_FLOW, receiveGnssObs(std::make_shared<GnssObs>(*std::static_pointer_cast<const GnssObs>(obs)))); | |
| 713 | } | ||
| 714 | |||
| 715 | 58445 | _lastObservationTime = obs->insTime; | |
| 716 | 189550 | } | |
| 717 | |||
| 718 | 34219 | std::shared_ptr<NAV::ImuObs> NAV::ErrorModel::receiveImuObs(const std::shared_ptr<ImuObs>& imuObs, | |
| 719 | const Eigen::Vector3d& accelerometerBias_p, | ||
| 720 | const Eigen::Vector3d& gyroscopeBias_p, | ||
| 721 | const Eigen::Vector3d& accelerometerNoiseStd, | ||
| 722 | const Eigen::Vector3d& gyroscopeNoiseStd) | ||
| 723 | { | ||
| 724 |
2/2✓ Branch 1 taken 34214 times.
✓ Branch 2 taken 5 times.
|
34219 | if (!_lastObservationTime.empty()) |
| 725 | { | ||
| 726 | { | ||
| 727 | // Accelerometer RW standard deviation in platform frame coordinates [m/s^2/sqrt(s)] | ||
| 728 |
1/2✓ Branch 1 taken 34211 times.
✗ Branch 2 not taken.
|
34214 | Eigen::Vector3d accelerometerRWStd = convertUnit(_imuAccelerometerRW, _imuAccelerometerRWUnit); |
| 729 | LOG_DATA("{}: accelerometerRWStd = {} [m/s^2/sqrt(s)]", nameId(), accelerometerRWStd.transpose()); | ||
| 730 | |||
| 731 | ✗ | _randomWalkAccelerometer += Eigen::Vector3d{ | |
| 732 |
1/2✓ Branch 1 taken 34217 times.
✗ Branch 2 not taken.
|
34217 | _imuAccelerometerRWRng.getRand_normalDist(0.0, accelerometerRWStd(0)), |
| 733 |
2/4✓ Branch 1 taken 34217 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34221 times.
✗ Branch 5 not taken.
|
34217 | _imuAccelerometerRWRng.getRand_normalDist(0.0, accelerometerRWStd(1)), |
| 734 |
3/6✓ Branch 1 taken 34219 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34221 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 34221 times.
✗ Branch 8 not taken.
|
34221 | _imuAccelerometerRWRng.getRand_normalDist(0.0, accelerometerRWStd(2)) |
| 735 |
3/6✓ Branch 1 taken 34217 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34218 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 34213 times.
✗ Branch 8 not taken.
|
68432 | } * sqrt(_dt); |
| 736 | } | ||
| 737 | { | ||
| 738 | // Accelerometer IRW standard deviation in platform frame coordinates [m/s^3/sqrt(s)] | ||
| 739 |
1/2✓ Branch 1 taken 34209 times.
✗ Branch 2 not taken.
|
34213 | Eigen::Vector3d accelerometerIRWStd = convertUnit(_imuAccelerometerIRW, _imuAccelerometerIRWUnit); |
| 740 | LOG_DATA("{}: accelerometerIRWStd = {} [m/s^3/sqrt(s)]", nameId(), accelerometerIRWStd.transpose()); | ||
| 741 | |||
| 742 | // compute velocity RW first | ||
| 743 | ✗ | _integratedRandomWalkAccelerometer_velocity += Eigen::Vector3d{ | |
| 744 |
1/2✓ Branch 1 taken 34219 times.
✗ Branch 2 not taken.
|
34219 | _imuAccelerometerIRWRng.getRand_normalDist(0.0, accelerometerIRWStd(0)), |
| 745 |
2/4✓ Branch 1 taken 34217 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34221 times.
✗ Branch 5 not taken.
|
34219 | _imuAccelerometerIRWRng.getRand_normalDist(0.0, accelerometerIRWStd(1)), |
| 746 |
3/6✓ Branch 1 taken 34221 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34221 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 34221 times.
✗ Branch 8 not taken.
|
34221 | _imuAccelerometerIRWRng.getRand_normalDist(0.0, accelerometerIRWStd(2)) |
| 747 |
3/6✓ Branch 1 taken 34219 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34221 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 34219 times.
✗ Branch 8 not taken.
|
68430 | } * sqrt(_dt); |
| 748 | |||
| 749 | // then compute IRW | ||
| 750 |
2/4✓ Branch 1 taken 34218 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34220 times.
✗ Branch 5 not taken.
|
34219 | _integratedRandomWalkAccelerometer += _integratedRandomWalkAccelerometer_velocity * _dt; |
| 751 | } | ||
| 752 | { | ||
| 753 | // Gyro RW standard deviation in platform frame coordinates [rad/s/sqrt(s)] | ||
| 754 |
1/2✓ Branch 1 taken 34219 times.
✗ Branch 2 not taken.
|
34220 | Eigen::Vector3d gyroscopeRWStd = convertUnit(_imuGyroscopeRW, _imuGyroscopeRWUnit); |
| 755 | LOG_DATA("{}: gyroscopeRWStd = {} [rad/s/sqrt(s)]", nameId(), gyroscopeRWStd.transpose()); | ||
| 756 | |||
| 757 | ✗ | _randomWalkGyroscope += Eigen::Vector3d{ | |
| 758 |
1/2✓ Branch 1 taken 34221 times.
✗ Branch 2 not taken.
|
34217 | _imuGyroscopeRWRng.getRand_normalDist(0.0, gyroscopeRWStd(0)), |
| 759 |
2/4✓ Branch 1 taken 34219 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34220 times.
✗ Branch 5 not taken.
|
34221 | _imuGyroscopeRWRng.getRand_normalDist(0.0, gyroscopeRWStd(1)), |
| 760 |
3/6✓ Branch 1 taken 34221 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34221 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 34220 times.
✗ Branch 8 not taken.
|
34220 | _imuGyroscopeRWRng.getRand_normalDist(0.0, gyroscopeRWStd(2)) |
| 761 |
3/6✓ Branch 1 taken 34217 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34220 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 34218 times.
✗ Branch 8 not taken.
|
68439 | } * sqrt(_dt); |
| 762 | } | ||
| 763 | { | ||
| 764 | // Gyro RW standard deviation in platform frame coordinates [rad/s^2/sqrt(s)] | ||
| 765 |
1/2✓ Branch 1 taken 34218 times.
✗ Branch 2 not taken.
|
34218 | Eigen::Vector3d gyroscopeIRWStd = convertUnit(_imuGyroscopeIRW, _imuGyroscopeIRWUnit); |
| 766 | LOG_DATA("{}: gyroscopeIRWStd = {} [rad/s^2/sqrt(s)]", nameId(), gyroscopeIRWStd.transpose()); | ||
| 767 | |||
| 768 | // compute velocity RW first | ||
| 769 | ✗ | _integratedRandomWalkGyroscope_velocity += Eigen::Vector3d{ | |
| 770 |
1/2✓ Branch 1 taken 34221 times.
✗ Branch 2 not taken.
|
34220 | _imuGyroscopeIRWRng.getRand_normalDist(0.0, gyroscopeIRWStd(0)), |
| 771 |
2/4✓ Branch 1 taken 34220 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34220 times.
✗ Branch 5 not taken.
|
34221 | _imuGyroscopeIRWRng.getRand_normalDist(0.0, gyroscopeIRWStd(1)), |
| 772 |
3/6✓ Branch 1 taken 34218 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34221 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 34220 times.
✗ Branch 8 not taken.
|
34220 | _imuGyroscopeIRWRng.getRand_normalDist(0.0, gyroscopeIRWStd(2)) |
| 773 |
3/6✓ Branch 1 taken 34220 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34220 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 34220 times.
✗ Branch 8 not taken.
|
68438 | } * sqrt(_dt); |
| 774 | |||
| 775 | // then compute IRW | ||
| 776 |
2/4✓ Branch 1 taken 34221 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34218 times.
✗ Branch 5 not taken.
|
34220 | _integratedRandomWalkGyroscope += _integratedRandomWalkGyroscope_velocity * _dt; |
| 777 | } | ||
| 778 | } | ||
| 779 | |||
| 780 | // ######################################################################################################################################### | ||
| 781 | |||
| 782 | 34222 | imuObs->p_acceleration += accelerometerBias_p | |
| 783 | ✗ | + _randomWalkAccelerometer | |
| 784 |
3/6✓ Branch 1 taken 34223 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34222 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 34224 times.
✗ Branch 8 not taken.
|
68446 | + _integratedRandomWalkAccelerometer; |
| 785 | |||
| 786 | 34224 | imuObs->p_angularRate += gyroscopeBias_p | |
| 787 | ✗ | + _randomWalkGyroscope | |
| 788 |
3/6✓ Branch 1 taken 34221 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34224 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 34224 times.
✗ Branch 8 not taken.
|
68448 | + _integratedRandomWalkGyroscope; |
| 789 |
2/2✓ Branch 0 taken 34219 times.
✓ Branch 1 taken 5 times.
|
34224 | if (_dt > 1e-9) |
| 790 | { | ||
| 791 |
1/2✓ Branch 1 taken 34221 times.
✗ Branch 2 not taken.
|
68437 | imuObs->p_acceleration += Eigen::Vector3d{ _imuAccelerometerRng.getRand_normalDist(0.0, accelerometerNoiseStd(0)), |
| 792 |
2/4✓ Branch 1 taken 34220 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34221 times.
✗ Branch 5 not taken.
|
34221 | _imuAccelerometerRng.getRand_normalDist(0.0, accelerometerNoiseStd(1)), |
| 793 |
3/6✓ Branch 1 taken 34220 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34221 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 34221 times.
✗ Branch 8 not taken.
|
34221 | _imuAccelerometerRng.getRand_normalDist(0.0, accelerometerNoiseStd(2)) } |
| 794 |
3/6✓ Branch 1 taken 34217 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34220 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 34218 times.
✗ Branch 8 not taken.
|
102660 | / std::sqrt(_dt); // Scale with input frequency |
| 795 |
1/2✓ Branch 1 taken 34220 times.
✗ Branch 2 not taken.
|
68435 | imuObs->p_angularRate += Eigen::Vector3d{ _imuGyroscopeRng.getRand_normalDist(0.0, gyroscopeNoiseStd(0)), |
| 796 |
2/4✓ Branch 1 taken 34219 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34221 times.
✗ Branch 5 not taken.
|
34220 | _imuGyroscopeRng.getRand_normalDist(0.0, gyroscopeNoiseStd(1)), |
| 797 |
3/6✓ Branch 1 taken 34220 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34220 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 34220 times.
✗ Branch 8 not taken.
|
34221 | _imuGyroscopeRng.getRand_normalDist(0.0, gyroscopeNoiseStd(2)) } |
| 798 |
3/6✓ Branch 1 taken 34216 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34219 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 34217 times.
✗ Branch 8 not taken.
|
102659 | / std::sqrt(_dt); // Scale with input frequency |
| 799 | } | ||
| 800 | |||
| 801 | 34222 | return imuObs; | |
| 802 | } | ||
| 803 | |||
| 804 | 34226 | std::shared_ptr<NAV::ImuObsWDelta> NAV::ErrorModel::receiveImuObsWDelta(const std::shared_ptr<ImuObsWDelta>& imuObsWDelta, | |
| 805 | const Eigen::Vector3d& accelerometerBias_p, | ||
| 806 | const Eigen::Vector3d& gyroscopeBias_p, | ||
| 807 | const Eigen::Vector3d& accelerometerNoiseStd, | ||
| 808 | const Eigen::Vector3d& gyroscopeNoiseStd) | ||
| 809 | { | ||
| 810 |
1/2✓ Branch 2 taken 34225 times.
✗ Branch 3 not taken.
|
34226 | receiveImuObs(imuObsWDelta, accelerometerBias_p, gyroscopeBias_p, accelerometerNoiseStd, gyroscopeNoiseStd); |
| 811 | |||
| 812 |
2/2✓ Branch 0 taken 34219 times.
✓ Branch 1 taken 5 times.
|
34224 | double imuObsWDeltaAverageWindow = _dt != 0.0 ? _dt / imuObsWDelta->dtime : 1.0; |
| 813 | |||
| 814 |
4/8✓ Branch 1 taken 34224 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34225 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 34223 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 34221 times.
✗ Branch 13 not taken.
|
34224 | imuObsWDelta->dvel += imuObsWDelta->dtime * (accelerometerBias_p + _randomWalkAccelerometer + _integratedRandomWalkAccelerometer); |
| 815 |
4/8✓ Branch 1 taken 34223 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34225 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 34222 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 34224 times.
✗ Branch 13 not taken.
|
34221 | imuObsWDelta->dtheta += imuObsWDelta->dtime * (gyroscopeBias_p + _randomWalkGyroscope + _integratedRandomWalkGyroscope); |
| 816 |
2/2✓ Branch 0 taken 34218 times.
✓ Branch 1 taken 6 times.
|
34224 | if (_dt > 1e-9) |
| 817 | { | ||
| 818 |
1/2✓ Branch 1 taken 34221 times.
✗ Branch 2 not taken.
|
68434 | imuObsWDelta->dvel += Eigen::Vector3d{ _imuAccelerometerRng.getRand_normalDist(0.0, accelerometerNoiseStd(0) / std::sqrt(imuObsWDeltaAverageWindow)), |
| 819 |
2/4✓ Branch 1 taken 34217 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34220 times.
✗ Branch 5 not taken.
|
34221 | _imuAccelerometerRng.getRand_normalDist(0.0, accelerometerNoiseStd(1) / std::sqrt(imuObsWDeltaAverageWindow)), |
| 820 |
3/6✓ Branch 1 taken 34219 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34220 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 34221 times.
✗ Branch 8 not taken.
|
34220 | _imuAccelerometerRng.getRand_normalDist(0.0, accelerometerNoiseStd(2) / std::sqrt(imuObsWDeltaAverageWindow)) } |
| 821 |
4/8✓ Branch 2 taken 34215 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 34217 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 34219 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 34216 times.
✗ Branch 12 not taken.
|
102659 | * imuObsWDelta->dtime / std::sqrt(_dt); // Scale with input frequency |
| 822 |
1/2✓ Branch 1 taken 34220 times.
✗ Branch 2 not taken.
|
68431 | imuObsWDelta->dtheta += Eigen::Vector3d{ _imuGyroscopeRng.getRand_normalDist(0.0, gyroscopeNoiseStd(0) / std::sqrt(imuObsWDeltaAverageWindow)), |
| 823 |
2/4✓ Branch 1 taken 34219 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34220 times.
✗ Branch 5 not taken.
|
34220 | _imuGyroscopeRng.getRand_normalDist(0.0, gyroscopeNoiseStd(1) / std::sqrt(imuObsWDeltaAverageWindow)), |
| 824 |
3/6✓ Branch 1 taken 34217 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34221 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 34218 times.
✗ Branch 8 not taken.
|
34220 | _imuGyroscopeRng.getRand_normalDist(0.0, gyroscopeNoiseStd(2) / std::sqrt(imuObsWDeltaAverageWindow)) } |
| 825 |
4/8✓ Branch 2 taken 34216 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 34218 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 34215 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 34217 times.
✗ Branch 12 not taken.
|
102650 | * imuObsWDelta->dtime / std::sqrt(_dt); // Scale with input frequency |
| 826 | } | ||
| 827 | |||
| 828 | 34223 | return imuObsWDelta; | |
| 829 | } | ||
| 830 | |||
| 831 | ✗ | std::shared_ptr<NAV::Pos> NAV::ErrorModel::receivePos(const std::shared_ptr<Pos>& pos) | |
| 832 | { | ||
| 833 | // Position Bias in latLonAlt in [rad, rad, m] | ||
| 834 | ✗ | Eigen::Vector3d lla_positionBias = Eigen::Vector3d::Zero(); | |
| 835 | ✗ | switch (_positionBiasUnit) | |
| 836 | { | ||
| 837 | ✗ | case PositionBiasUnits::meter: | |
| 838 | { | ||
| 839 | ✗ | Eigen::Vector3d e_positionBias = trafo::e_Quat_n(pos->latitude(), pos->longitude()) * _positionBias; | |
| 840 | ✗ | if (!e_positionBias.isZero()) | |
| 841 | { | ||
| 842 | ✗ | lla_positionBias = trafo::ecef2lla_WGS84(pos->e_position() + e_positionBias) - pos->lla_position(); | |
| 843 | } | ||
| 844 | ✗ | break; | |
| 845 | } | ||
| 846 | } | ||
| 847 | LOG_DATA("{}: lla_positionBias = {} [rad, rad, m]", nameId(), lla_positionBias.transpose()); | ||
| 848 | |||
| 849 | // Velocity bias in local-navigation coordinates in [m/s] | ||
| 850 | ✗ | Eigen::Vector3d n_velocityBias = Eigen::Vector3d::Zero(); | |
| 851 | ✗ | switch (_velocityBiasUnit) | |
| 852 | { | ||
| 853 | ✗ | case VelocityBiasUnits::m_s: | |
| 854 | ✗ | n_velocityBias = _velocityBias; | |
| 855 | ✗ | break; | |
| 856 | } | ||
| 857 | LOG_DATA("{}: n_velocityBias = {} [m/s]", nameId(), n_velocityBias.transpose()); | ||
| 858 | |||
| 859 | // Roll, pitch, yaw bias in [rad] | ||
| 860 | ✗ | Eigen::Vector3d attitudeBias = Eigen::Vector3d::Zero(); | |
| 861 | ✗ | switch (_attitudeBiasUnit) | |
| 862 | { | ||
| 863 | ✗ | case AttitudeBiasUnits::rad: | |
| 864 | ✗ | attitudeBias = _attitudeBias; | |
| 865 | ✗ | break; | |
| 866 | ✗ | case AttitudeBiasUnits::deg: | |
| 867 | ✗ | attitudeBias = deg2rad(_attitudeBias); | |
| 868 | ✗ | break; | |
| 869 | } | ||
| 870 | LOG_DATA("{}: attitudeBias = {} [rad]", nameId(), attitudeBias.transpose()); | ||
| 871 | |||
| 872 | // ######################################################################################################################################### | ||
| 873 | |||
| 874 | // Position Noise standard deviation in latitude, longitude and altitude [rad, rad, m] | ||
| 875 | ✗ | Eigen::Vector3d lla_positionNoiseStd = Eigen::Vector3d::Zero(); | |
| 876 | ✗ | Eigen::Vector3d NED_pos_variance = Eigen::Vector3d::Zero(); | |
| 877 | ✗ | switch (_positionNoiseUnit) | |
| 878 | { | ||
| 879 | ✗ | case PositionNoiseUnits::meter: | |
| 880 | { | ||
| 881 | ✗ | Eigen::Vector3d e_positionNoiseStd = trafo::e_Quat_n(pos->latitude(), pos->longitude()) * _positionNoise; | |
| 882 | ✗ | if (!e_positionNoiseStd.isZero()) | |
| 883 | { | ||
| 884 | ✗ | lla_positionNoiseStd = trafo::ecef2lla_WGS84(pos->e_position() + e_positionNoiseStd) - pos->lla_position(); | |
| 885 | } | ||
| 886 | ✗ | NED_pos_variance = _positionNoise.cwiseAbs2(); | |
| 887 | ✗ | break; | |
| 888 | } | ||
| 889 | ✗ | case PositionNoiseUnits::meter2: | |
| 890 | { | ||
| 891 | ✗ | Eigen::Vector3d e_positionNoiseStd = trafo::e_Quat_n(pos->latitude(), pos->longitude()) * _positionNoise.cwiseSqrt(); | |
| 892 | ✗ | if (!e_positionNoiseStd.isZero()) | |
| 893 | { | ||
| 894 | ✗ | lla_positionNoiseStd = trafo::ecef2lla_WGS84(pos->e_position() + e_positionNoiseStd) - pos->lla_position(); | |
| 895 | } | ||
| 896 | ✗ | NED_pos_variance = _positionNoise; | |
| 897 | ✗ | break; | |
| 898 | } | ||
| 899 | } | ||
| 900 | LOG_DATA("{}: lla_positionNoiseStd = {} [rad, rad, m]", nameId(), lla_positionNoiseStd.transpose()); | ||
| 901 | |||
| 902 | // ######################################################################################################################################### | ||
| 903 | |||
| 904 | ✗ | pos->setPositionAndCov_n(pos->lla_position() | |
| 905 | ✗ | + lla_positionBias | |
| 906 | ✗ | + Eigen::Vector3d{ _positionRng.getRand_normalDist(0.0, lla_positionNoiseStd(0)), | |
| 907 | ✗ | _positionRng.getRand_normalDist(0.0, lla_positionNoiseStd(1)), | |
| 908 | ✗ | _positionRng.getRand_normalDist(0.0, lla_positionNoiseStd(2)) }, | |
| 909 | ✗ | NED_pos_variance.asDiagonal().toDenseMatrix()); | |
| 910 | |||
| 911 | ✗ | return pos; | |
| 912 | } | ||
| 913 | |||
| 914 | ✗ | std::shared_ptr<NAV::PosVel> NAV::ErrorModel::receivePosVel(const std::shared_ptr<PosVel>& posVel) | |
| 915 | { | ||
| 916 | // Position Bias in latLonAlt in [rad, rad, m] | ||
| 917 | ✗ | Eigen::Vector3d lla_positionBias = Eigen::Vector3d::Zero(); | |
| 918 | ✗ | switch (_positionBiasUnit) | |
| 919 | { | ||
| 920 | ✗ | case PositionBiasUnits::meter: | |
| 921 | { | ||
| 922 | ✗ | Eigen::Vector3d e_positionBias = trafo::e_Quat_n(posVel->latitude(), posVel->longitude()) * _positionBias; | |
| 923 | ✗ | if (!e_positionBias.isZero()) | |
| 924 | { | ||
| 925 | ✗ | lla_positionBias = trafo::ecef2lla_WGS84(posVel->e_position() + e_positionBias) - posVel->lla_position(); | |
| 926 | } | ||
| 927 | ✗ | break; | |
| 928 | } | ||
| 929 | } | ||
| 930 | LOG_DATA("{}: lla_positionBias = {} [rad, rad, m]", nameId(), lla_positionBias.transpose()); | ||
| 931 | |||
| 932 | // Velocity bias in local-navigation coordinates in [m/s] | ||
| 933 | ✗ | Eigen::Vector3d n_velocityBias = Eigen::Vector3d::Zero(); | |
| 934 | ✗ | switch (_velocityBiasUnit) | |
| 935 | { | ||
| 936 | ✗ | case VelocityBiasUnits::m_s: | |
| 937 | ✗ | n_velocityBias = _velocityBias; | |
| 938 | ✗ | break; | |
| 939 | } | ||
| 940 | LOG_DATA("{}: n_velocityBias = {} [m/s]", nameId(), n_velocityBias.transpose()); | ||
| 941 | |||
| 942 | // ######################################################################################################################################### | ||
| 943 | |||
| 944 | // Position Noise standard deviation in latitude, longitude and altitude [rad, rad, m] | ||
| 945 | ✗ | Eigen::Vector3d lla_positionNoiseStd = Eigen::Vector3d::Zero(); | |
| 946 | ✗ | Eigen::Vector3d NED_pos_variance = Eigen::Vector3d::Zero(); | |
| 947 | ✗ | Eigen::Vector3d NED_velocity_variance = Eigen::Vector3d::Zero(); | |
| 948 | ✗ | switch (_positionNoiseUnit) | |
| 949 | { | ||
| 950 | ✗ | case PositionNoiseUnits::meter: | |
| 951 | { | ||
| 952 | ✗ | Eigen::Vector3d e_positionNoiseStd = trafo::e_Quat_n(posVel->latitude(), posVel->longitude()) * _positionNoise; | |
| 953 | ✗ | if (!e_positionNoiseStd.isZero()) | |
| 954 | { | ||
| 955 | ✗ | lla_positionNoiseStd = trafo::ecef2lla_WGS84(posVel->e_position() + e_positionNoiseStd) - posVel->lla_position(); | |
| 956 | } | ||
| 957 | ✗ | NED_pos_variance = _positionNoise.cwiseAbs2(); | |
| 958 | ✗ | break; | |
| 959 | } | ||
| 960 | ✗ | case PositionNoiseUnits::meter2: | |
| 961 | { | ||
| 962 | ✗ | Eigen::Vector3d e_positionNoiseStd = trafo::e_Quat_n(posVel->latitude(), posVel->longitude()) * _positionNoise.cwiseSqrt(); | |
| 963 | ✗ | if (!e_positionNoiseStd.isZero()) | |
| 964 | { | ||
| 965 | ✗ | lla_positionNoiseStd = trafo::ecef2lla_WGS84(posVel->e_position() + e_positionNoiseStd) - posVel->lla_position(); | |
| 966 | } | ||
| 967 | ✗ | NED_pos_variance = _positionNoise; | |
| 968 | ✗ | break; | |
| 969 | } | ||
| 970 | } | ||
| 971 | LOG_DATA("{}: lla_positionNoiseStd = {} [rad, rad, m]", nameId(), lla_positionNoiseStd.transpose()); | ||
| 972 | |||
| 973 | // Velocity Noise standard deviation in local-navigation coordinates in [m/s] | ||
| 974 | ✗ | Eigen::Vector3d n_velocityNoiseStd = Eigen::Vector3d::Zero(); | |
| 975 | ✗ | switch (_velocityNoiseUnit) | |
| 976 | { | ||
| 977 | ✗ | case VelocityNoiseUnits::m_s: | |
| 978 | ✗ | n_velocityNoiseStd = _velocityNoise; | |
| 979 | ✗ | NED_velocity_variance = _velocityNoise.cwiseAbs2(); | |
| 980 | ✗ | break; | |
| 981 | ✗ | case VelocityNoiseUnits::m2_s2: | |
| 982 | ✗ | n_velocityNoiseStd = _velocityNoise.cwiseSqrt(); | |
| 983 | ✗ | NED_velocity_variance = _velocityNoise; | |
| 984 | ✗ | break; | |
| 985 | } | ||
| 986 | LOG_DATA("{}: n_velocityNoiseStd = {} [m/s]", nameId(), n_velocityNoiseStd.transpose()); | ||
| 987 | |||
| 988 | // ######################################################################################################################################### | ||
| 989 | |||
| 990 | ✗ | Eigen::Matrix<double, 6, 6> cov_n = Eigen::Matrix<double, 6, 6>::Zero(); | |
| 991 | ✗ | cov_n.block<3, 3>(0, 0).diagonal() = NED_pos_variance; | |
| 992 | ✗ | cov_n.block<3, 3>(3, 3).diagonal() = NED_velocity_variance; | |
| 993 | |||
| 994 | ✗ | posVel->setPosVelAndCov_n(posVel->lla_position() | |
| 995 | ✗ | + lla_positionBias | |
| 996 | ✗ | + Eigen::Vector3d{ _positionRng.getRand_normalDist(0.0, lla_positionNoiseStd(0)), | |
| 997 | ✗ | _positionRng.getRand_normalDist(0.0, lla_positionNoiseStd(1)), | |
| 998 | ✗ | _positionRng.getRand_normalDist(0.0, lla_positionNoiseStd(2)) }, | |
| 999 | ✗ | posVel->n_velocity() | |
| 1000 | ✗ | + n_velocityBias | |
| 1001 | ✗ | + Eigen::Vector3d{ _velocityRng.getRand_normalDist(0.0, n_velocityNoiseStd(0)), | |
| 1002 | ✗ | _velocityRng.getRand_normalDist(0.0, n_velocityNoiseStd(1)), | |
| 1003 | ✗ | _velocityRng.getRand_normalDist(0.0, n_velocityNoiseStd(2)) }, | |
| 1004 | cov_n); | ||
| 1005 | |||
| 1006 | ✗ | return posVel; | |
| 1007 | } | ||
| 1008 | |||
| 1009 | 24222 | std::shared_ptr<NAV::PosVelAtt> NAV::ErrorModel::receivePosVelAtt(const std::shared_ptr<PosVelAtt>& posVelAtt) | |
| 1010 | { | ||
| 1011 | // Position Bias in latLonAlt in [rad, rad, m] | ||
| 1012 |
2/4✓ Branch 1 taken 24222 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24222 times.
✗ Branch 5 not taken.
|
24222 | Eigen::Vector3d lla_positionBias = Eigen::Vector3d::Zero(); |
| 1013 |
1/2✓ Branch 0 taken 24222 times.
✗ Branch 1 not taken.
|
24222 | switch (_positionBiasUnit) |
| 1014 | { | ||
| 1015 | 24222 | case PositionBiasUnits::meter: | |
| 1016 | { | ||
| 1017 |
4/8✓ Branch 2 taken 24222 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 24222 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 24222 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 24222 times.
✗ Branch 13 not taken.
|
24222 | Eigen::Vector3d e_positionBias = trafo::e_Quat_n(posVelAtt->latitude(), posVelAtt->longitude()) * _positionBias; |
| 1018 |
2/4✓ Branch 2 taken 24222 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 24222 times.
|
24222 | if (!e_positionBias.isZero()) |
| 1019 | { | ||
| 1020 | ✗ | lla_positionBias = trafo::ecef2lla_WGS84(posVelAtt->e_position() + e_positionBias) - posVelAtt->lla_position(); | |
| 1021 | } | ||
| 1022 | 24222 | break; | |
| 1023 | } | ||
| 1024 | } | ||
| 1025 | LOG_DATA("{}: lla_positionBias = {} [rad, rad, m]", nameId(), lla_positionBias.transpose()); | ||
| 1026 | |||
| 1027 | // Velocity bias in local-navigation coordinates in [m/s] | ||
| 1028 |
2/4✓ Branch 1 taken 24222 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24222 times.
✗ Branch 5 not taken.
|
24222 | Eigen::Vector3d n_velocityBias = Eigen::Vector3d::Zero(); |
| 1029 |
1/2✓ Branch 0 taken 24222 times.
✗ Branch 1 not taken.
|
24222 | switch (_velocityBiasUnit) |
| 1030 | { | ||
| 1031 | 24222 | case VelocityBiasUnits::m_s: | |
| 1032 |
1/2✓ Branch 1 taken 24222 times.
✗ Branch 2 not taken.
|
24222 | n_velocityBias = _velocityBias; |
| 1033 | 24222 | break; | |
| 1034 | } | ||
| 1035 | LOG_DATA("{}: n_velocityBias = {} [m/s]", nameId(), n_velocityBias.transpose()); | ||
| 1036 | |||
| 1037 | // Roll, pitch, yaw bias in [rad] | ||
| 1038 |
2/4✓ Branch 1 taken 24222 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24222 times.
✗ Branch 5 not taken.
|
24222 | Eigen::Vector3d attitudeBias = Eigen::Vector3d::Zero(); |
| 1039 |
1/3✗ Branch 0 not taken.
✓ Branch 1 taken 24222 times.
✗ Branch 2 not taken.
|
24222 | switch (_attitudeBiasUnit) |
| 1040 | { | ||
| 1041 | ✗ | case AttitudeBiasUnits::rad: | |
| 1042 | ✗ | attitudeBias = _attitudeBias; | |
| 1043 | ✗ | break; | |
| 1044 | 24222 | case AttitudeBiasUnits::deg: | |
| 1045 |
1/2✓ Branch 1 taken 24222 times.
✗ Branch 2 not taken.
|
24222 | attitudeBias = deg2rad(_attitudeBias); |
| 1046 | 24222 | break; | |
| 1047 | } | ||
| 1048 | LOG_DATA("{}: attitudeBias = {} [rad]", nameId(), attitudeBias.transpose()); | ||
| 1049 | |||
| 1050 | // ######################################################################################################################################### | ||
| 1051 | |||
| 1052 | // Position Noise standard deviation in latitude, longitude and altitude [rad, rad, m] | ||
| 1053 |
2/4✓ Branch 1 taken 24222 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24222 times.
✗ Branch 5 not taken.
|
24222 | Eigen::Vector3d lla_positionNoiseStd = Eigen::Vector3d::Zero(); |
| 1054 |
2/4✓ Branch 1 taken 24222 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24222 times.
✗ Branch 5 not taken.
|
24222 | Eigen::Vector3d NED_pos_variance = Eigen::Vector3d::Zero(); |
| 1055 |
2/4✓ Branch 1 taken 24222 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24222 times.
✗ Branch 5 not taken.
|
24222 | Eigen::Vector3d NED_velocity_variance = Eigen::Vector3d::Zero(); |
| 1056 |
2/4✓ Branch 1 taken 24222 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24222 times.
✗ Branch 5 not taken.
|
24222 | Eigen::Vector3d RPY_attitude_variance = Eigen::Vector3d::Zero(); |
| 1057 |
1/3✓ Branch 0 taken 24222 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
|
24222 | switch (_positionNoiseUnit) |
| 1058 | { | ||
| 1059 | 24222 | case PositionNoiseUnits::meter: | |
| 1060 | { | ||
| 1061 |
4/8✓ Branch 2 taken 24222 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 24222 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 24222 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 24222 times.
✗ Branch 13 not taken.
|
24222 | Eigen::Vector3d e_positionNoiseStd = trafo::e_Quat_n(posVelAtt->latitude(), posVelAtt->longitude()) * _positionNoise; |
| 1062 |
2/4✓ Branch 2 taken 24222 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 24222 times.
✗ Branch 5 not taken.
|
24222 | if (!e_positionNoiseStd.isZero()) |
| 1063 | { | ||
| 1064 |
4/8✓ Branch 5 taken 24222 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 24222 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 24222 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 24222 times.
✗ Branch 15 not taken.
|
24222 | lla_positionNoiseStd = trafo::ecef2lla_WGS84(posVelAtt->e_position() + e_positionNoiseStd) - posVelAtt->lla_position(); |
| 1065 | } | ||
| 1066 |
2/4✓ Branch 1 taken 24222 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24222 times.
✗ Branch 5 not taken.
|
24222 | NED_pos_variance = _positionNoise.cwiseAbs2(); |
| 1067 | 24222 | break; | |
| 1068 | } | ||
| 1069 | ✗ | case PositionNoiseUnits::meter2: | |
| 1070 | { | ||
| 1071 | ✗ | Eigen::Vector3d e_positionNoiseStd = trafo::e_Quat_n(posVelAtt->latitude(), posVelAtt->longitude()) * _positionNoise.cwiseSqrt(); | |
| 1072 | ✗ | if (!e_positionNoiseStd.isZero()) | |
| 1073 | { | ||
| 1074 | ✗ | lla_positionNoiseStd = trafo::ecef2lla_WGS84(posVelAtt->e_position() + e_positionNoiseStd) - posVelAtt->lla_position(); | |
| 1075 | } | ||
| 1076 | ✗ | NED_pos_variance = _positionNoise; | |
| 1077 | ✗ | break; | |
| 1078 | } | ||
| 1079 | } | ||
| 1080 | LOG_DATA("{}: lla_positionNoiseStd = {} [rad, rad, m]", nameId(), lla_positionNoiseStd.transpose()); | ||
| 1081 | |||
| 1082 | // Velocity Noise standard deviation in local-navigation coordinates in [m/s] | ||
| 1083 |
2/4✓ Branch 1 taken 24222 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24222 times.
✗ Branch 5 not taken.
|
24222 | Eigen::Vector3d n_velocityNoiseStd = Eigen::Vector3d::Zero(); |
| 1084 |
1/3✓ Branch 0 taken 24222 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
|
24222 | switch (_velocityNoiseUnit) |
| 1085 | { | ||
| 1086 | 24222 | case VelocityNoiseUnits::m_s: | |
| 1087 |
1/2✓ Branch 1 taken 24222 times.
✗ Branch 2 not taken.
|
24222 | n_velocityNoiseStd = _velocityNoise; |
| 1088 |
2/4✓ Branch 1 taken 24222 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24222 times.
✗ Branch 5 not taken.
|
24222 | NED_velocity_variance = _velocityNoise.cwiseAbs2(); |
| 1089 | 24222 | break; | |
| 1090 | ✗ | case VelocityNoiseUnits::m2_s2: | |
| 1091 | ✗ | n_velocityNoiseStd = _velocityNoise.cwiseSqrt(); | |
| 1092 | ✗ | NED_velocity_variance = _velocityNoise; | |
| 1093 | ✗ | break; | |
| 1094 | } | ||
| 1095 | LOG_DATA("{}: n_velocityNoiseStd = {} [m/s]", nameId(), n_velocityNoiseStd.transpose()); | ||
| 1096 | |||
| 1097 | // Attitude Noise standard deviation in [rad] | ||
| 1098 |
2/4✓ Branch 1 taken 24222 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24222 times.
✗ Branch 5 not taken.
|
24222 | Eigen::Vector3d attitudeNoiseStd = Eigen::Vector3d::Zero(); |
| 1099 |
1/5✗ Branch 0 not taken.
✓ Branch 1 taken 24222 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
|
24222 | switch (_attitudeNoiseUnit) |
| 1100 | { | ||
| 1101 | ✗ | case AttitudeNoiseUnits::rad: | |
| 1102 | ✗ | attitudeNoiseStd = _attitudeNoise; | |
| 1103 | ✗ | RPY_attitude_variance = _attitudeNoise.cwiseAbs2(); | |
| 1104 | ✗ | break; | |
| 1105 | 24222 | case AttitudeNoiseUnits::deg: | |
| 1106 |
1/2✓ Branch 1 taken 24222 times.
✗ Branch 2 not taken.
|
24222 | attitudeNoiseStd = deg2rad(_attitudeNoise); |
| 1107 |
3/6✓ Branch 1 taken 24222 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24222 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 24222 times.
✗ Branch 8 not taken.
|
24222 | RPY_attitude_variance = deg2rad(_attitudeNoise).cwiseAbs2(); |
| 1108 | 24222 | break; | |
| 1109 | ✗ | case AttitudeNoiseUnits::rad2: | |
| 1110 | ✗ | attitudeNoiseStd = _attitudeNoise.cwiseSqrt(); | |
| 1111 | ✗ | RPY_attitude_variance = _attitudeNoise; | |
| 1112 | ✗ | break; | |
| 1113 | ✗ | case AttitudeNoiseUnits::deg2: | |
| 1114 | ✗ | attitudeNoiseStd = deg2rad(_attitudeNoise.cwiseSqrt()); | |
| 1115 | ✗ | RPY_attitude_variance = deg2rad(deg2rad(_attitudeNoise)); | |
| 1116 | ✗ | break; | |
| 1117 | } | ||
| 1118 | LOG_DATA("{}: attitudeNoiseStd = {} [rad]", nameId(), attitudeNoiseStd.transpose()); | ||
| 1119 | |||
| 1120 | // ######################################################################################################################################### | ||
| 1121 | |||
| 1122 |
2/4✓ Branch 1 taken 24222 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24222 times.
✗ Branch 5 not taken.
|
24222 | Eigen::Matrix<double, 9, 9> cov_n = Eigen::Matrix<double, 9, 9>::Zero(); |
| 1123 |
3/6✓ Branch 1 taken 24222 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24222 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 24222 times.
✗ Branch 8 not taken.
|
24222 | cov_n.block<3, 3>(0, 0).diagonal() = NED_pos_variance; |
| 1124 |
3/6✓ Branch 1 taken 24222 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24222 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 24222 times.
✗ Branch 8 not taken.
|
24222 | cov_n.block<3, 3>(3, 3).diagonal() = NED_velocity_variance; |
| 1125 |
3/6✓ Branch 1 taken 24222 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24222 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 24222 times.
✗ Branch 8 not taken.
|
24222 | cov_n.block<3, 3>(6, 6).diagonal() = RPY_attitude_variance; |
| 1126 | |||
| 1127 |
2/4✓ Branch 1 taken 24222 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24222 times.
✗ Branch 5 not taken.
|
24222 | Eigen::Matrix<double, 10, 9> J = Eigen::Matrix<double, 10, 9>::Zero(); |
| 1128 |
2/4✓ Branch 1 taken 24222 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24222 times.
✗ Branch 5 not taken.
|
24222 | J.topLeftCorner<6, 6>().setIdentity(); |
| 1129 |
3/6✓ Branch 3 taken 24222 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 24222 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 24222 times.
✗ Branch 10 not taken.
|
24222 | J.bottomRightCorner<4, 3>() = trafo::covRPY2quatJacobian(posVelAtt->n_Quat_b()); |
| 1130 | |||
| 1131 |
1/2✓ Branch 4 taken 24222 times.
✗ Branch 5 not taken.
|
72666 | posVelAtt->setPosVelAttAndCov_n(posVelAtt->lla_position() |
| 1132 |
1/2✓ Branch 1 taken 24222 times.
✗ Branch 2 not taken.
|
24222 | + lla_positionBias |
| 1133 |
3/6✓ Branch 1 taken 24222 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24222 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 24222 times.
✗ Branch 8 not taken.
|
48444 | + Eigen::Vector3d{ _positionRng.getRand_normalDist(0.0, lla_positionNoiseStd(0)), |
| 1134 |
2/4✓ Branch 1 taken 24222 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24222 times.
✗ Branch 5 not taken.
|
24222 | _positionRng.getRand_normalDist(0.0, lla_positionNoiseStd(1)), |
| 1135 |
3/6✓ Branch 1 taken 24222 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24222 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 24222 times.
✗ Branch 8 not taken.
|
24222 | _positionRng.getRand_normalDist(0.0, lla_positionNoiseStd(2)) }, |
| 1136 | 24222 | posVelAtt->n_velocity() | |
| 1137 |
1/2✓ Branch 1 taken 24222 times.
✗ Branch 2 not taken.
|
24222 | + n_velocityBias |
| 1138 |
3/6✓ Branch 1 taken 24222 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24222 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 24222 times.
✗ Branch 8 not taken.
|
48444 | + Eigen::Vector3d{ _velocityRng.getRand_normalDist(0.0, n_velocityNoiseStd(0)), |
| 1139 |
2/4✓ Branch 1 taken 24222 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24222 times.
✗ Branch 5 not taken.
|
24222 | _velocityRng.getRand_normalDist(0.0, n_velocityNoiseStd(1)), |
| 1140 |
3/6✓ Branch 1 taken 24222 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24222 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 24222 times.
✗ Branch 8 not taken.
|
24222 | _velocityRng.getRand_normalDist(0.0, n_velocityNoiseStd(2)) }, |
| 1141 |
2/4✓ Branch 2 taken 24222 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 24222 times.
✗ Branch 6 not taken.
|
48444 | trafo::n_Quat_b(posVelAtt->rollPitchYaw() |
| 1142 |
1/2✓ Branch 1 taken 24222 times.
✗ Branch 2 not taken.
|
24222 | + attitudeBias |
| 1143 |
3/6✓ Branch 1 taken 24222 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24222 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 24222 times.
✗ Branch 8 not taken.
|
48444 | + Eigen::Vector3d{ _attitudeRng.getRand_normalDist(0.0, attitudeNoiseStd(0)), |
| 1144 |
2/4✓ Branch 1 taken 24222 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24222 times.
✗ Branch 5 not taken.
|
24222 | _attitudeRng.getRand_normalDist(0.0, attitudeNoiseStd(1)), |
| 1145 |
3/6✓ Branch 1 taken 24222 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24222 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 24222 times.
✗ Branch 8 not taken.
|
24222 | _attitudeRng.getRand_normalDist(0.0, attitudeNoiseStd(2)) }), |
| 1146 |
3/6✓ Branch 1 taken 24222 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24222 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 24222 times.
✗ Branch 8 not taken.
|
24222 | J * cov_n * J.transpose()); |
| 1147 | |||
| 1148 | 48444 | return posVelAtt; | |
| 1149 | } | ||
| 1150 | |||
| 1151 | ✗ | std::shared_ptr<NAV::GnssObs> NAV::ErrorModel::receiveGnssObs(const std::shared_ptr<GnssObs>& gnssObs) | |
| 1152 | { | ||
| 1153 | LOG_DATA("{}: [{}] Simulating error on GnssObs", nameId(), gnssObs->insTime.toYMDHMS(GPST)); | ||
| 1154 | ✗ | double pseudorangeNoise{}; // [m] | |
| 1155 | ✗ | switch (_gui_pseudorangeNoiseUnit) | |
| 1156 | { | ||
| 1157 | ✗ | case PseudorangeNoiseUnits::meter: | |
| 1158 | ✗ | pseudorangeNoise = _gui_pseudorangeNoise; | |
| 1159 | ✗ | break; | |
| 1160 | } | ||
| 1161 | ✗ | double carrierPhaseNoise{}; // [m] | |
| 1162 | ✗ | switch (_gui_carrierPhaseNoiseUnit) | |
| 1163 | { | ||
| 1164 | ✗ | case CarrierPhaseNoiseUnits::meter: | |
| 1165 | ✗ | carrierPhaseNoise = _gui_carrierPhaseNoise; | |
| 1166 | ✗ | break; | |
| 1167 | } | ||
| 1168 | ✗ | double dopplerNoise{}; // [m/s] | |
| 1169 | ✗ | switch (_gui_dopplerNoiseUnit) | |
| 1170 | { | ||
| 1171 | ✗ | case DopplerNoiseUnits::m_s: | |
| 1172 | ✗ | dopplerNoise = _gui_dopplerNoise; | |
| 1173 | ✗ | break; | |
| 1174 | } | ||
| 1175 | |||
| 1176 | ✗ | double dtCycleSlipSeconds{}; // [s] | |
| 1177 | ✗ | switch (_gui_cycleSlipFrequencyUnit) | |
| 1178 | { | ||
| 1179 | ✗ | case CycleSlipFrequencyUnits::per_day: | |
| 1180 | ✗ | dtCycleSlipSeconds = InsTimeUtil::SECONDS_PER_DAY / _gui_cycleSlipFrequency; | |
| 1181 | ✗ | break; | |
| 1182 | ✗ | case CycleSlipFrequencyUnits::per_hour: | |
| 1183 | ✗ | dtCycleSlipSeconds = InsTimeUtil::SECONDS_PER_HOUR / _gui_cycleSlipFrequency; | |
| 1184 | ✗ | break; | |
| 1185 | ✗ | case CycleSlipFrequencyUnits::per_minute: | |
| 1186 | ✗ | dtCycleSlipSeconds = InsTimeUtil::SECONDS_PER_MINUTE / _gui_cycleSlipFrequency; | |
| 1187 | ✗ | break; | |
| 1188 | } | ||
| 1189 | ✗ | auto dtCycleSlip = std::chrono::nanoseconds(static_cast<int64_t>(dtCycleSlipSeconds * 1e9)); | |
| 1190 | |||
| 1191 | ✗ | double cycleSlipDetectionProbability{}; // [0, 1] | |
| 1192 | ✗ | switch (_gui_cycleSlipDetectionProbabilityUnit) | |
| 1193 | { | ||
| 1194 | ✗ | case CycleSlipDetectionProbabilityUnits::percent: | |
| 1195 | ✗ | cycleSlipDetectionProbability = _gui_cycleSlipDetectionProbability / 100.0; | |
| 1196 | ✗ | break; | |
| 1197 | } | ||
| 1198 | |||
| 1199 | ✗ | if (_cycleSlipWindowStartTime.empty() | |
| 1200 | ✗ | || gnssObs->insTime >= _cycleSlipWindowStartTime + dtCycleSlip) | |
| 1201 | { | ||
| 1202 | ✗ | _cycleSlipWindowStartTime = gnssObs->insTime; | |
| 1203 | LOG_DATA("{}: [{}] Starting new cycle-slip window", nameId(), _cycleSlipWindowStartTime.toYMDHMS(GPST)); | ||
| 1204 | } | ||
| 1205 | |||
| 1206 | ✗ | size_t nObs = 0; | |
| 1207 | ✗ | for (auto& obs : gnssObs->data) | |
| 1208 | { | ||
| 1209 | ✗ | if (obs.satSigId.freq() & _filterFreq | |
| 1210 | ✗ | && obs.satSigId.code & _filterCode) | |
| 1211 | { | ||
| 1212 | ✗ | nObs++; | |
| 1213 | } | ||
| 1214 | } | ||
| 1215 | |||
| 1216 | ✗ | for (auto& obs : gnssObs->data) | |
| 1217 | { | ||
| 1218 | ✗ | if (obs.pseudorange) { obs.pseudorange.value().value += _pseudorangeRng.getRand_normalDist(0.0, pseudorangeNoise); } | |
| 1219 | ✗ | if (obs.doppler) { obs.doppler.value() += rangeRate2doppler(_dopplerRng.getRand_normalDist(0.0, dopplerNoise), obs.satSigId.freq(), 0); } // TODO: Add frequency number here for GLONASS | |
| 1220 | |||
| 1221 | ✗ | if (obs.carrierPhase) | |
| 1222 | { | ||
| 1223 | // ------------------------------------------- Noise --------------------------------------------- | ||
| 1224 | ✗ | auto lambda = InsConst::C / obs.satSigId.freq().getFrequency(0); // wave-length [m] // TODO: Add frequency number here for GLONASS | |
| 1225 | ✗ | obs.carrierPhase.value().value += _carrierPhaseRng.getRand_normalDist(0.0, carrierPhaseNoise) / lambda; | |
| 1226 | |||
| 1227 | // ---------------------------------------- Cycle-slip ------------------------------------------- | ||
| 1228 | |||
| 1229 | ✗ | if (obs.satSigId.freq() & _filterFreq // GUI selected frequencies | |
| 1230 | ✗ | && obs.satSigId.code & _filterCode // GUI selected codes | |
| 1231 | ✗ | && _gui_cycleSlipFrequency != 0.0 // 0 Frequency means disabled | |
| 1232 | ✗ | && !_lastObservationTime.empty() // Do not apply a cycle slip on the first message | |
| 1233 | ✗ | && (_cycleSlips.empty() || _cycleSlips.back().time < _cycleSlipWindowStartTime)) // In the current window, there was no cycle-slip yet | |
| 1234 | { | ||
| 1235 | ✗ | double probabilityCycleSlip = _dt / (dtCycleSlipSeconds * static_cast<double>(nObs)) * 2.0; // Double chance, because often does not happen otherwise | |
| 1236 | ✗ | if (_cycleSlipRng.getRand_uniformRealDist(0.0, 1.0) <= probabilityCycleSlip | |
| 1237 | ✗ | || (gnssObs->insTime >= _cycleSlipWindowStartTime + dtCycleSlip - std::chrono::nanoseconds(static_cast<int64_t>((_dt + 0.001) * 1e9)))) // Last message this window | |
| 1238 | { | ||
| 1239 | ✗ | int newAmbiguity = 0; | |
| 1240 | ✗ | int oldAmbiguity = !_ambiguities[obs.satSigId].empty() | |
| 1241 | ✗ | ? _ambiguities[obs.satSigId].back().second | |
| 1242 | ✗ | : static_cast<int>(_ambiguityRng.getRand_uniformIntDist(_gui_ambiguityLimits[0], _gui_ambiguityLimits[1])); | |
| 1243 | ✗ | auto deltaAmbiguity = static_cast<int>(_cycleSlipRng.getRand_uniformIntDist(1, _gui_cycleSlipRange)); | |
| 1244 | ✗ | auto signAmbiguity = _cycleSlipRng.getRand_uniformIntDist(0, 1) == 0.0 ? 1 : -1; | |
| 1245 | |||
| 1246 | ✗ | if (_gui_ambiguityLimits[0] == _gui_ambiguityLimits[1]) // Ambiguities disabled | |
| 1247 | { | ||
| 1248 | ✗ | newAmbiguity = oldAmbiguity + signAmbiguity * deltaAmbiguity; | |
| 1249 | } | ||
| 1250 | ✗ | else if (oldAmbiguity == _gui_ambiguityLimits[0]) | |
| 1251 | { | ||
| 1252 | ✗ | newAmbiguity = std::min(oldAmbiguity + deltaAmbiguity, _gui_ambiguityLimits[1]); | |
| 1253 | } | ||
| 1254 | ✗ | else if (oldAmbiguity == _gui_ambiguityLimits[1]) | |
| 1255 | { | ||
| 1256 | ✗ | newAmbiguity = std::max(oldAmbiguity - deltaAmbiguity, _gui_ambiguityLimits[0]); | |
| 1257 | } | ||
| 1258 | else | ||
| 1259 | { | ||
| 1260 | ✗ | newAmbiguity = std::clamp(oldAmbiguity + signAmbiguity * deltaAmbiguity, _gui_ambiguityLimits[0], _gui_ambiguityLimits[1]); | |
| 1261 | } | ||
| 1262 | |||
| 1263 | ✗ | _ambiguities[obs.satSigId].emplace_back(gnssObs->insTime, newAmbiguity); | |
| 1264 | |||
| 1265 | ✗ | if (_cycleSlipRng.getRand_uniformRealDist(0.0, 1.0) <= cycleSlipDetectionProbability) | |
| 1266 | { | ||
| 1267 | ✗ | obs.carrierPhase.value().LLI = 1; | |
| 1268 | } | ||
| 1269 | ✗ | _cycleSlips.push_back(CycleSlipInfo{ .time = gnssObs->insTime, .satSigId = obs.satSigId, .LLI = obs.carrierPhase.value().LLI != 0 }); | |
| 1270 | ✗ | LOG_DEBUG("{}: [{}] Simulating cycle-slip for satellite [{}] with LLI {}", nameId(), gnssObs->insTime.toYMDHMS(GPST), | |
| 1271 | obs.satSigId, obs.carrierPhase.value().LLI); | ||
| 1272 | } | ||
| 1273 | } | ||
| 1274 | |||
| 1275 | // ----------------------------------------- Ambiguity ------------------------------------------- | ||
| 1276 | ✗ | if (!_ambiguities.contains(obs.satSigId) && _gui_ambiguityLimits[0] != _gui_ambiguityLimits[1]) | |
| 1277 | { | ||
| 1278 | ✗ | _ambiguities[obs.satSigId].emplace_back(gnssObs->insTime, _ambiguityRng.getRand_uniformIntDist(_gui_ambiguityLimits[0], _gui_ambiguityLimits[1])); | |
| 1279 | } | ||
| 1280 | ✗ | if (_ambiguities.contains(obs.satSigId)) | |
| 1281 | { | ||
| 1282 | ✗ | obs.carrierPhase.value().value += _ambiguities.at(obs.satSigId).back().second; | |
| 1283 | } | ||
| 1284 | } | ||
| 1285 | } | ||
| 1286 | |||
| 1287 | ✗ | return gnssObs; | |
| 1288 | } | ||
| 1289 |