| 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 <cstdint> | ||
| 13 | #include <imgui.h> | ||
| 14 | #include <imgui_internal.h> | ||
| 15 | #include <limits> | ||
| 16 | #include <set> | ||
| 17 | #include <type_traits> | ||
| 18 | |||
| 19 | #include "NodeRegistry.hpp" | ||
| 20 | #include "Navigation/GNSS/Core/SatelliteIdentifier.hpp" | ||
| 21 | #include "Navigation/INS/Units.hpp" | ||
| 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 |
57/114✓ 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 172 taken 120 times.
✗ Branch 173 not taken.
✓ Branch 177 taken 120 times.
✗ Branch 178 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 | CreateInputPin("True", Pin::Type::Flow, supportedDataIdentifier, &ErrorModel::receiveObs); |
| 62 | |||
| 63 |
1/2✓ Branch 3 taken 120 times.
✗ Branch 4 not taken.
|
120 | CreateOutputPin("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 | ✗ | ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver); | |
| 325 | ✗ | if (ImGui::TreeNode(fmt::format("Manual Cycle-slips Tree##{}", size_t(id)).c_str())) | |
| 326 | { | ||
| 327 | ✗ | ImGui::BeginGroup(); | |
| 328 | { | ||
| 329 | ✗ | ImGui::TextUnformatted("New:"); | |
| 330 | |||
| 331 | ✗ | float groupWidth = 62.0F * 2 + ImGui::GetStyle().ItemSpacing.x; | |
| 332 | ✗ | ImGui::BeginHorizontal(fmt::format("Horizontal SatSigId {}", size_t(id)).c_str()); | |
| 333 | { | ||
| 334 | ✗ | ImGui::SetNextItemWidth(62.0F); | |
| 335 | ✗ | if (ShowCodeSelector(fmt::format("##Manual cycle-slip code {}", size_t(id)).c_str(), _manualCycleSlipSignal.code, Freq_All, true)) | |
| 336 | { | ||
| 337 | ✗ | flow::ApplyChanges(); | |
| 338 | } | ||
| 339 | ✗ | ImGui::SetNextItemWidth(62.0F); | |
| 340 | ✗ | SatId satId = _manualCycleSlipSignal.toSatId(); | |
| 341 | ✗ | if (ShowSatelliteSelector(fmt::format("##Manual cycle-slip sat{}", size_t(id)).c_str(), satId, satId.satSys, true)) | |
| 342 | { | ||
| 343 | ✗ | _manualCycleSlipSignal.satNum = satId.satNum; | |
| 344 | ✗ | flow::ApplyChanges(); | |
| 345 | } | ||
| 346 | ✗ | ImGui::EndHorizontal(); | |
| 347 | } | ||
| 348 | |||
| 349 | ✗ | ImGui::SetNextItemWidth(groupWidth); | |
| 350 | ✗ | if (ImGui::InputInt(fmt::format("##Ambiguity value {}", size_t(id)).c_str(), &_manualCycleSlipAmbiguity)) { flow::ApplyChanges(); } | |
| 351 | ✗ | if (ImGui::IsItemHovered()) { ImGui::SetTooltip("Ambiguity value"); } | |
| 352 | |||
| 353 | ✗ | ImGui::BeginHorizontal(fmt::format("Horizontal LLI/Button {}", size_t(id)).c_str(), ImVec2(groupWidth, 0.0F)); | |
| 354 | { | ||
| 355 | ✗ | if (ImGui::Checkbox(fmt::format("LLI##Ambiguity LLI {}", size_t(id)).c_str(), &_manualCycleSlipSetLLI)) { flow::ApplyChanges(); } | |
| 356 | ✗ | ImGui::Spring(); | |
| 357 | ✗ | if (ImGui::Button(fmt::format("Add##manual cycle-slip {}", size_t(id)).c_str())) | |
| 358 | { | ||
| 359 | ✗ | _manualCycleSlips[{ _manualCycleSlipTime, _manualCycleSlipSignal }] = { _manualCycleSlipAmbiguity, _manualCycleSlipSetLLI }; | |
| 360 | ✗ | flow::ApplyChanges(); | |
| 361 | } | ||
| 362 | ✗ | ImGui::EndHorizontal(); | |
| 363 | } | ||
| 364 | } | ||
| 365 | ✗ | ImGui::EndGroup(); | |
| 366 | |||
| 367 | ✗ | ImGui::SameLine(); | |
| 368 | ✗ | if (gui::widgets::TimeEdit(fmt::format("Manual Cycle-slips Time##{}", size_t(id)).c_str(), | |
| 369 | ✗ | _manualCycleSlipTime, _manualCycleSlipTimeEditFormat, 130.0F, 2)) | |
| 370 | { | ||
| 371 | ✗ | flow::ApplyChanges(); | |
| 372 | } | ||
| 373 | |||
| 374 | ✗ | if (!_manualCycleSlips.empty() | |
| 375 | ✗ | && ImGui::BeginTable(fmt::format("Manual cycle-slips Table##{}", size_t(id)).c_str(), 5, | |
| 376 | ImGuiTableFlags_Borders | ImGuiTableFlags_NoHostExtendX | ImGuiTableFlags_SizingFixedFit | ImGuiTableFlags_ScrollY, | ||
| 377 | ✗ | ImVec2(0.0F, std::min(25.0F + static_cast<float>(_manualCycleSlips.size()) * 28.0F, 300.0F) | |
| 378 | ✗ | * gui::NodeEditorApplication::monoFontRatio()))) | |
| 379 | { | ||
| 380 | ✗ | ImGui::TableSetupColumn("Time"); | |
| 381 | ✗ | ImGui::TableSetupColumn("Signal"); | |
| 382 | ✗ | ImGui::TableSetupColumn("Value"); | |
| 383 | ✗ | ImGui::TableSetupColumn("LLI"); | |
| 384 | ✗ | ImGui::TableSetupColumn("Delete"); | |
| 385 | |||
| 386 | ✗ | ImGui::TableSetupScrollFreeze(1, 1); | |
| 387 | ✗ | ImGui::TableHeadersRow(); | |
| 388 | |||
| 389 | ✗ | std::optional<std::pair<InsTime, SatSigId>> _toRemove; | |
| 390 | ✗ | for (auto& cycleSlip : _manualCycleSlips) | |
| 391 | { | ||
| 392 | ✗ | const auto& [insTime, satSigId] = cycleSlip.first; | |
| 393 | ✗ | auto& [ambValue, setLLI] = cycleSlip.second; | |
| 394 | ✗ | ImGui::TableNextRow(); | |
| 395 | |||
| 396 | ✗ | ImGui::TableNextColumn(); | |
| 397 | ✗ | ImGui::TextUnformatted(fmt::format("{}", insTime.toYMDHMS(GPST)).c_str()); | |
| 398 | |||
| 399 | ✗ | ImGui::TableNextColumn(); | |
| 400 | { | ||
| 401 | ✗ | ImGui::SetNextItemWidth(62.0F); | |
| 402 | ✗ | auto newSatSigId = satSigId; | |
| 403 | ✗ | if (ShowCodeSelector(fmt::format("##Manual cycle-slip code {} {} {}", size_t(id), insTime, satSigId).c_str(), newSatSigId.code, Freq_All, true)) | |
| 404 | { | ||
| 405 | ✗ | flow::ApplyChanges(); | |
| 406 | } | ||
| 407 | ✗ | ImGui::SameLine(); | |
| 408 | ✗ | ImGui::SetNextItemWidth(62.0F); | |
| 409 | ✗ | SatId satId = newSatSigId.toSatId(); | |
| 410 | ✗ | if (ShowSatelliteSelector(fmt::format("##Manual cycle-slip sat{} {} {}", size_t(id), insTime, satSigId).c_str(), satId, satId.satSys, true)) | |
| 411 | { | ||
| 412 | ✗ | newSatSigId.satNum = satId.satNum; | |
| 413 | ✗ | flow::ApplyChanges(); | |
| 414 | } | ||
| 415 | ✗ | if (newSatSigId != satSigId) | |
| 416 | { | ||
| 417 | ✗ | _toRemove = cycleSlip.first; | |
| 418 | ✗ | _manualCycleSlips[{ insTime, newSatSigId }] = { ambValue, setLLI }; | |
| 419 | } | ||
| 420 | } | ||
| 421 | |||
| 422 | ✗ | ImGui::TableNextColumn(); | |
| 423 | ✗ | ImGui::SetNextItemWidth(120.0F); | |
| 424 | ✗ | if (ImGui::InputInt(fmt::format("##Ambiguity value {} {} {}", size_t(id), insTime, satSigId).c_str(), &ambValue)) | |
| 425 | { | ||
| 426 | ✗ | flow::ApplyChanges(); | |
| 427 | } | ||
| 428 | |||
| 429 | ✗ | ImGui::TableNextColumn(); | |
| 430 | ✗ | if (ImGui::Checkbox(fmt::format("##Ambiguity LLI {} {} {}", size_t(id), insTime, satSigId).c_str(), &setLLI)) | |
| 431 | { | ||
| 432 | ✗ | flow::ApplyChanges(); | |
| 433 | } | ||
| 434 | |||
| 435 | ✗ | ImGui::TableNextColumn(); | |
| 436 | ✗ | if (ImGui::Button(fmt::format("X##Manual cycle remove {} {} {}", size_t(id), insTime, satSigId).c_str())) | |
| 437 | { | ||
| 438 | ✗ | _toRemove = cycleSlip.first; | |
| 439 | } | ||
| 440 | } | ||
| 441 | ✗ | if (_toRemove) | |
| 442 | { | ||
| 443 | ✗ | _manualCycleSlips.erase(*_toRemove); | |
| 444 | ✗ | flow::ApplyChanges(); | |
| 445 | } | ||
| 446 | ✗ | ImGui::EndTable(); | |
| 447 | } | ||
| 448 | |||
| 449 | ✗ | ImGui::TreePop(); | |
| 450 | } | ||
| 451 | } | ||
| 452 | ✗ | ImGui::Unindent(); | |
| 453 | |||
| 454 | ✗ | ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver); | |
| 455 | ✗ | if (ImGui::TreeNode(fmt::format("Simulated Ambiguities and Cycle-slips##{}", size_t(id)).c_str())) | |
| 456 | { | ||
| 457 | ✗ | auto nAmb = static_cast<int>(_ambiguities.size()); | |
| 458 | ✗ | if (nAmb > 0) | |
| 459 | { | ||
| 460 | ✗ | std::set<InsTime> ambiguityTimes; | |
| 461 | ✗ | for (int i = 0; i < nAmb; i++) | |
| 462 | { | ||
| 463 | ✗ | const auto& ambVec = std::next(_ambiguities.begin(), i)->second; | |
| 464 | ✗ | for (const auto& amb : ambVec) | |
| 465 | { | ||
| 466 | ✗ | ambiguityTimes.insert(amb.first); | |
| 467 | } | ||
| 468 | } | ||
| 469 | ✗ | if (ambiguityTimes.size() < 64 - 1) | |
| 470 | { | ||
| 471 | ✗ | ImGui::PushFont(Application::MonoFont()); | |
| 472 | |||
| 473 | ✗ | if (ImGui::BeginTable(fmt::format("Ambiguities##{}", size_t(id)).c_str(), static_cast<int>(ambiguityTimes.size() + 1), | |
| 474 | ImGuiTableFlags_Borders | ImGuiTableFlags_NoHostExtendX | ImGuiTableFlags_SizingFixedFit | ImGuiTableFlags_ScrollX | ImGuiTableFlags_ScrollY, | ||
| 475 | ✗ | ImVec2(0.0F, 300.0F * gui::NodeEditorApplication::monoFontRatio()))) | |
| 476 | { | ||
| 477 | ✗ | ImGui::TableSetupColumn(""); | |
| 478 | ✗ | for (const auto& time : ambiguityTimes) | |
| 479 | { | ||
| 480 | ✗ | auto t = time.toYMDHMS(GPST); | |
| 481 | ✗ | t.sec = std::round(t.sec * 1e2) / 1e2; | |
| 482 | ✗ | ImGui::TableSetupColumn(str::replaceAll_copy(fmt::format("{}", t), " ", "\n").c_str()); | |
| 483 | } | ||
| 484 | ✗ | ImGui::TableSetupScrollFreeze(1, 1); | |
| 485 | ✗ | ImGui::TableHeadersRow(); | |
| 486 | |||
| 487 | ✗ | for (int i = 0; i < nAmb; i++) | |
| 488 | { | ||
| 489 | ✗ | ImGui::TableNextRow(); | |
| 490 | ✗ | ImGui::TableNextColumn(); | |
| 491 | ✗ | const auto& ambiguities = std::next(_ambiguities.begin(), i); | |
| 492 | ✗ | if (ambiguities == _ambiguities.end()) { break; } | |
| 493 | ✗ | ImGui::TextUnformatted(fmt::format("{}", ambiguities->first).c_str()); | |
| 494 | ✗ | ImGui::TableSetBgColor(ImGuiTableBgTarget_CellBg, ImGui::GetColorU32(ImGui::GetStyle().Colors[ImGuiCol_TableHeaderBg])); | |
| 495 | |||
| 496 | ✗ | for (const auto& time : ambiguityTimes) | |
| 497 | { | ||
| 498 | ✗ | ImGui::TableNextColumn(); | |
| 499 | ✗ | auto iter = std::ranges::find_if(ambiguities->second, [&time](const auto& timeAndAmb) { | |
| 500 | ✗ | return timeAndAmb.first == time; | |
| 501 | }); | ||
| 502 | ✗ | if (iter != ambiguities->second.end()) | |
| 503 | { | ||
| 504 | ✗ | ImVec4 color; | |
| 505 | ✗ | auto cycleIter = std::ranges::find_if(_cycleSlips, [&](const CycleSlipInfo& cycleSlip) { | |
| 506 | ✗ | return cycleSlip.time == time && cycleSlip.satSigId == ambiguities->first; | |
| 507 | }); | ||
| 508 | ✗ | if (cycleIter != _cycleSlips.end()) { color = ImColor(240, 128, 128); } | |
| 509 | ✗ | else if (ambiguities->second.back().first == time) { color = ImGui::GetStyle().Colors[ImGuiCol_Text]; } | |
| 510 | ✗ | else { color = ImGui::GetStyle().Colors[ImGuiCol_TextDisabled]; } | |
| 511 | ✗ | ImGui::TextColored(color, "%s%s", | |
| 512 | ✗ | fmt::format("{}", iter->second).c_str(), | |
| 513 | ✗ | cycleIter != _cycleSlips.end() && cycleIter->LLI ? " (LLI)" : ""); | |
| 514 | ✗ | if (ImGui::IsItemHovered() && cycleIter != _cycleSlips.end()) { ImGui::SetTooltip("Cycle-slip"); } | |
| 515 | } | ||
| 516 | ✗ | else if (time < ambiguities->second.front().first) | |
| 517 | { | ||
| 518 | ✗ | ImGui::TableSetBgColor(ImGuiTableBgTarget_CellBg, ImColor(70, 70, 70)); | |
| 519 | } | ||
| 520 | } | ||
| 521 | } | ||
| 522 | |||
| 523 | ✗ | ImGui::EndTable(); | |
| 524 | } | ||
| 525 | ✗ | ImGui::PopFont(); | |
| 526 | } | ||
| 527 | else | ||
| 528 | { | ||
| 529 | ✗ | ImGui::TextColored(ImColor(255, 0, 0), "More than 64 timestamps for ambiguities, which cannot be displayed in a table"); | |
| 530 | } | ||
| 531 | ✗ | } | |
| 532 | |||
| 533 | ✗ | ImGui::TreePop(); | |
| 534 | } | ||
| 535 | } | ||
| 536 | ✗ | } | |
| 537 | |||
| 538 | ✗ | json NAV::ErrorModel::save() const | |
| 539 | { | ||
| 540 | LOG_TRACE("{}: called", nameId()); | ||
| 541 | |||
| 542 | ✗ | json j; | |
| 543 | |||
| 544 | ✗ | j["imuAccelerometerBiasUnit"] = _imuAccelerometerBiasUnit; | |
| 545 | ✗ | j["imuAccelerometerBias_p"] = _imuAccelerometerBias_p; | |
| 546 | ✗ | j["imuGyroscopeBiasUnit"] = _imuGyroscopeBiasUnit; | |
| 547 | ✗ | j["imuGyroscopeBias_p"] = _imuGyroscopeBias_p; | |
| 548 | ✗ | j["imuAccelerometerNoiseUnit"] = _imuAccelerometerNoiseUnit; | |
| 549 | ✗ | j["imuAccelerometerNoise"] = _imuAccelerometerNoise; | |
| 550 | ✗ | j["imuAccelerometerRng"] = _imuAccelerometerRng; | |
| 551 | ✗ | j["imuGyroscopeNoiseUnit"] = _imuGyroscopeNoiseUnit; | |
| 552 | ✗ | j["imuGyroscopeNoise"] = _imuGyroscopeNoise; | |
| 553 | ✗ | j["imuGyroscopeRng"] = _imuGyroscopeRng; | |
| 554 | ✗ | j["imuAccelerometerRWUnit"] = _imuAccelerometerRWUnit; | |
| 555 | ✗ | j["imuAccelerometerRW"] = _imuAccelerometerRW; | |
| 556 | ✗ | j["imuAccelerometerRWRng"] = _imuAccelerometerRWRng; | |
| 557 | |||
| 558 | ✗ | j["imuGyroscopeRWUnit"] = _imuGyroscopeRWUnit; | |
| 559 | ✗ | j["imuGyroscopeRW"] = _imuGyroscopeRW; | |
| 560 | ✗ | j["imuGyroscopeRWRng"] = _imuGyroscopeRWRng; | |
| 561 | ✗ | j["imuAccelerometerIRWUnit"] = _imuAccelerometerIRWUnit; | |
| 562 | ✗ | j["imuAccelerometerIRW"] = _imuAccelerometerIRW; | |
| 563 | ✗ | j["imuAccelerometerIRWRng"] = _imuAccelerometerIRWRng; | |
| 564 | ✗ | j["imuGyroscopeIRWUnit"] = _imuGyroscopeIRWUnit; | |
| 565 | ✗ | j["imuGyroscopeIRW"] = _imuGyroscopeIRW; | |
| 566 | ✗ | j["imuGyroscopeIRWRng"] = _imuGyroscopeIRWRng; | |
| 567 | |||
| 568 | // ######################################################################################################################################### | ||
| 569 | ✗ | j["positionBiasUnit"] = _positionBiasUnit; | |
| 570 | ✗ | j["positionBias"] = _positionBias; | |
| 571 | ✗ | j["velocityBiasUnit"] = _velocityBiasUnit; | |
| 572 | ✗ | j["velocityBias"] = _velocityBias; | |
| 573 | ✗ | j["attitudeBiasUnit"] = _attitudeBiasUnit; | |
| 574 | ✗ | j["attitudeBias"] = _attitudeBias; | |
| 575 | ✗ | j["positionNoiseUnit"] = _positionNoiseUnit; | |
| 576 | ✗ | j["positionNoise"] = _positionNoise; | |
| 577 | ✗ | j["positionRng"] = _positionRng; | |
| 578 | ✗ | j["velocityNoiseUnit"] = _velocityNoiseUnit; | |
| 579 | ✗ | j["velocityNoise"] = _velocityNoise; | |
| 580 | ✗ | j["velocityRng"] = _velocityRng; | |
| 581 | ✗ | j["attitudeNoiseUnit"] = _attitudeNoiseUnit; | |
| 582 | ✗ | j["attitudeNoise"] = _attitudeNoise; | |
| 583 | ✗ | j["attitudeRng"] = _attitudeRng; | |
| 584 | // ######################################################################################################################################### | ||
| 585 | ✗ | j["pseudorangeNoiseUnit"] = _gui_pseudorangeNoiseUnit; | |
| 586 | ✗ | j["pseudorangeNoise"] = _gui_pseudorangeNoise; | |
| 587 | ✗ | j["pseudorangeRng"] = _pseudorangeRng; | |
| 588 | ✗ | j["carrierPhaseNoiseUnit"] = _gui_carrierPhaseNoiseUnit; | |
| 589 | ✗ | j["carrierPhaseNoise"] = _gui_carrierPhaseNoise; | |
| 590 | ✗ | j["carrierPhaseRng"] = _carrierPhaseRng; | |
| 591 | ✗ | j["dopplerNoiseUnit"] = _gui_dopplerNoiseUnit; | |
| 592 | ✗ | j["dopplerNoise"] = _gui_dopplerNoise; | |
| 593 | ✗ | j["dopplerRng"] = _dopplerRng; | |
| 594 | ✗ | j["ambiguityLimits"] = _gui_ambiguityLimits; | |
| 595 | ✗ | j["ambiguityRng"] = _ambiguityRng; | |
| 596 | ✗ | j["manualAmbiguities"] = _manualCycleSlips; | |
| 597 | ✗ | j["manualCycleSlipTime"] = _manualCycleSlipTime; | |
| 598 | ✗ | j["manualCycleSlipTimeEditFormat"] = _manualCycleSlipTimeEditFormat; | |
| 599 | ✗ | j["manualCycleSlipSignal"] = _manualCycleSlipSignal; | |
| 600 | ✗ | j["manualCycleSlipAmbiguity"] = _manualCycleSlipAmbiguity; | |
| 601 | ✗ | j["manualCycleSlipSetLLI"] = _manualCycleSlipSetLLI; | |
| 602 | ✗ | j["cycleSlipFrequencyUnit"] = _gui_cycleSlipFrequencyUnit; | |
| 603 | ✗ | j["cycleSlipFrequency"] = _gui_cycleSlipFrequency; | |
| 604 | ✗ | j["cycleSlipRange"] = _gui_cycleSlipRange; | |
| 605 | ✗ | j["cycleSlipDetectionProbabilityUnit"] = _gui_cycleSlipDetectionProbabilityUnit; | |
| 606 | ✗ | j["cycleSlipDetectionProbability"] = _gui_cycleSlipDetectionProbability; | |
| 607 | ✗ | j["cycleSlipRng"] = _cycleSlipRng; | |
| 608 | |||
| 609 | ✗ | j["filterFreq"] = Frequency_(_filterFreq); | |
| 610 | ✗ | j["filterCode"] = _filterCode; | |
| 611 | |||
| 612 | ✗ | return j; | |
| 613 | ✗ | } | |
| 614 | |||
| 615 | 6 | void NAV::ErrorModel::restore(json const& j) | |
| 616 | { | ||
| 617 | LOG_TRACE("{}: called", nameId()); | ||
| 618 | |||
| 619 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("imuAccelerometerBiasUnit")) { j.at("imuAccelerometerBiasUnit").get_to(_imuAccelerometerBiasUnit); } |
| 620 |
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); } |
| 621 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("imuGyroscopeBiasUnit")) { j.at("imuGyroscopeBiasUnit").get_to(_imuGyroscopeBiasUnit); } |
| 622 |
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); } |
| 623 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("imuAccelerometerNoiseUnit")) { j.at("imuAccelerometerNoiseUnit").get_to(_imuAccelerometerNoiseUnit); } |
| 624 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("imuAccelerometerNoise")) { j.at("imuAccelerometerNoise").get_to(_imuAccelerometerNoise); } |
| 625 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("imuAccelerometerRng")) { j.at("imuAccelerometerRng").get_to(_imuAccelerometerRng); } |
| 626 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("imuGyroscopeNoiseUnit")) { j.at("imuGyroscopeNoiseUnit").get_to(_imuGyroscopeNoiseUnit); } |
| 627 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("imuGyroscopeNoise")) { j.at("imuGyroscopeNoise").get_to(_imuGyroscopeNoise); } |
| 628 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("imuGyroscopeRng")) { j.at("imuGyroscopeRng").get_to(_imuGyroscopeRng); } |
| 629 | |||
| 630 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("imuAccelerometerRWUnit")) { j.at("imuAccelerometerRWUnit").get_to(_imuAccelerometerRWUnit); } |
| 631 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("imuAccelerometerRW")) { j.at("imuAccelerometerRW").get_to(_imuAccelerometerRW); } |
| 632 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("imuAccelerometerRWRng")) { j.at("imuAccelerometerRWRng").get_to(_imuAccelerometerRWRng); } |
| 633 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("imuGyroscopeRWUnit")) { j.at("imuGyroscopeRWUnit").get_to(_imuGyroscopeRWUnit); } |
| 634 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("imuGyroscopeRW")) { j.at("imuGyroscopeRW").get_to(_imuGyroscopeRW); } |
| 635 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("imuGyroscopeRWRng")) { j.at("imuGyroscopeRWRng").get_to(_imuGyroscopeRWRng); } |
| 636 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("imuAccelerometerIRWUnit")) { j.at("imuAccelerometerIRWUnit").get_to(_imuAccelerometerIRWUnit); } |
| 637 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("imuAccelerometerIRW")) { j.at("imuAccelerometerIRW").get_to(_imuAccelerometerIRW); } |
| 638 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("imuAccelerometerIRWRng")) { j.at("imuAccelerometerIRWRng").get_to(_imuAccelerometerIRWRng); } |
| 639 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("imuGyroscopeIRWUnit")) { j.at("imuGyroscopeIRWUnit").get_to(_imuGyroscopeIRWUnit); } |
| 640 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("imuGyroscopeIRW")) { j.at("imuGyroscopeIRW").get_to(_imuGyroscopeIRW); } |
| 641 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("imuGyroscopeIRWRng")) { j.at("imuGyroscopeIRWRng").get_to(_imuGyroscopeIRWRng); } |
| 642 | // ######################################################################################################################################### | ||
| 643 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("positionBiasUnit")) { j.at("positionBiasUnit").get_to(_positionBiasUnit); } |
| 644 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("positionBias")) { j.at("positionBias").get_to(_positionBias); } |
| 645 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("velocityBiasUnit")) { j.at("velocityBiasUnit").get_to(_velocityBiasUnit); } |
| 646 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("velocityBias")) { j.at("velocityBias").get_to(_velocityBias); } |
| 647 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("attitudeBiasUnit")) { j.at("attitudeBiasUnit").get_to(_attitudeBiasUnit); } |
| 648 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("attitudeBias")) { j.at("attitudeBias").get_to(_attitudeBias); } |
| 649 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("positionNoiseUnit")) { j.at("positionNoiseUnit").get_to(_positionNoiseUnit); } |
| 650 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("positionNoise")) { j.at("positionNoise").get_to(_positionNoise); } |
| 651 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("positionRng")) { j.at("positionRng").get_to(_positionRng); } |
| 652 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("velocityNoiseUnit")) { j.at("velocityNoiseUnit").get_to(_velocityNoiseUnit); } |
| 653 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("velocityNoise")) { j.at("velocityNoise").get_to(_velocityNoise); } |
| 654 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("velocityRng")) { j.at("velocityRng").get_to(_velocityRng); } |
| 655 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("attitudeNoiseUnit")) { j.at("attitudeNoiseUnit").get_to(_attitudeNoiseUnit); } |
| 656 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("attitudeNoise")) { j.at("attitudeNoise").get_to(_attitudeNoise); } |
| 657 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("attitudeRng")) { j.at("attitudeRng").get_to(_attitudeRng); } |
| 658 | // ######################################################################################################################################### | ||
| 659 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("pseudorangeNoiseUnit")) { j.at("pseudorangeNoiseUnit").get_to(_gui_pseudorangeNoiseUnit); } |
| 660 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("pseudorangeNoise")) { j.at("pseudorangeNoise").get_to(_gui_pseudorangeNoise); } |
| 661 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("pseudorangeRng")) { j.at("pseudorangeRng").get_to(_pseudorangeRng); } |
| 662 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("carrierPhaseNoiseUnit")) { j.at("carrierPhaseNoiseUnit").get_to(_gui_carrierPhaseNoiseUnit); } |
| 663 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("carrierPhaseNoise")) { j.at("carrierPhaseNoise").get_to(_gui_carrierPhaseNoise); } |
| 664 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("carrierPhaseRng")) { j.at("carrierPhaseRng").get_to(_carrierPhaseRng); } |
| 665 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("dopplerNoiseUnit")) { j.at("dopplerNoiseUnit").get_to(_gui_dopplerNoiseUnit); } |
| 666 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("dopplerNoise")) { j.at("dopplerNoise").get_to(_gui_dopplerNoise); } |
| 667 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("dopplerRng")) { j.at("dopplerRng").get_to(_dopplerRng); } |
| 668 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("ambiguityLimits")) { j.at("ambiguityLimits").get_to(_gui_ambiguityLimits); } |
| 669 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("ambiguityRng")) { j.at("ambiguityRng").get_to(_ambiguityRng); } |
| 670 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 6 times.
|
6 | if (j.contains("manualAmbiguities")) { j.at("manualAmbiguities").get_to(_manualCycleSlips); } |
| 671 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 6 times.
|
6 | if (j.contains("manualCycleSlipTime")) { j.at("manualCycleSlipTime").get_to(_manualCycleSlipTime); } |
| 672 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 6 times.
|
6 | if (j.contains("manualCycleSlipTimeEditFormat")) { j.at("manualCycleSlipTimeEditFormat").get_to(_manualCycleSlipTimeEditFormat); } |
| 673 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 6 times.
|
6 | if (j.contains("manualCycleSlipSignal")) { j.at("manualCycleSlipSignal").get_to(_manualCycleSlipSignal); } |
| 674 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 6 times.
|
6 | if (j.contains("manualCycleSlipAmbiguity")) { j.at("manualCycleSlipAmbiguity").get_to(_manualCycleSlipAmbiguity); } |
| 675 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 6 times.
|
6 | if (j.contains("manualCycleSlipSetLLI")) { j.at("manualCycleSlipSetLLI").get_to(_manualCycleSlipSetLLI); } |
| 676 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("cycleSlipFrequencyUnit")) { j.at("cycleSlipFrequencyUnit").get_to(_gui_cycleSlipFrequencyUnit); } |
| 677 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("cycleSlipFrequency")) { j.at("cycleSlipFrequency").get_to(_gui_cycleSlipFrequency); } |
| 678 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("cycleSlipRange")) { j.at("cycleSlipRange").get_to(_gui_cycleSlipRange); } |
| 679 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("cycleSlipDetectionProbabilityUnit")) { j.at("cycleSlipDetectionProbabilityUnit").get_to(_gui_cycleSlipDetectionProbabilityUnit); } |
| 680 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("cycleSlipDetectionProbability")) { j.at("cycleSlipDetectionProbability").get_to(_gui_cycleSlipDetectionProbability); } |
| 681 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("cycleSlipRng")) { j.at("cycleSlipRng").get_to(_cycleSlipRng); } |
| 682 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("filterFreq")) |
| 683 | { | ||
| 684 | 6 | uint64_t value = 0; | |
| 685 |
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); |
| 686 | 6 | _filterFreq = Frequency_(value); | |
| 687 | } | ||
| 688 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | if (j.contains("filterCode")) { j.at("filterCode").get_to(_filterCode); } |
| 689 | 6 | } | |
| 690 | |||
| 691 | 12 | bool NAV::ErrorModel::resetNode() | |
| 692 | { | ||
| 693 | LOG_TRACE("{}: called", nameId()); | ||
| 694 | |||
| 695 | 12 | _lastObservationTime.reset(); | |
| 696 | 12 | _dt = 0.0; | |
| 697 | |||
| 698 | // ImuObs | ||
| 699 | 12 | _imuAccelerometerRng.resetSeed(size_t(id)); | |
| 700 | 12 | _imuGyroscopeRng.resetSeed(size_t(id)); | |
| 701 | 12 | _imuAccelerometerRWRng.resetSeed(size_t(id)); | |
| 702 | 12 | _imuGyroscopeRWRng.resetSeed(size_t(id)); | |
| 703 | 12 | _imuAccelerometerIRWRng.resetSeed(size_t(id)); | |
| 704 | 12 | _imuGyroscopeIRWRng.resetSeed(size_t(id)); | |
| 705 | 12 | _randomWalkAccelerometer.setZero(); | |
| 706 | 12 | _randomWalkGyroscope.setZero(); | |
| 707 | 12 | _integratedRandomWalkAccelerometer.setZero(); | |
| 708 | 12 | _integratedRandomWalkGyroscope.setZero(); | |
| 709 | 12 | _integratedRandomWalkAccelerometer_velocity.setZero(); | |
| 710 | 12 | _integratedRandomWalkGyroscope_velocity.setZero(); | |
| 711 | |||
| 712 | // PosVelAtt | ||
| 713 | 12 | _positionRng.resetSeed(size_t(id)); | |
| 714 | 12 | _velocityRng.resetSeed(size_t(id)); | |
| 715 | 12 | _attitudeRng.resetSeed(size_t(id)); | |
| 716 | |||
| 717 | // GnssObs | ||
| 718 | 12 | _pseudorangeRng.resetSeed(size_t(id)); | |
| 719 | 12 | _carrierPhaseRng.resetSeed(size_t(id)); | |
| 720 | 12 | _dopplerRng.resetSeed(size_t(id)); | |
| 721 | 12 | _ambiguityRng.resetSeed(size_t(id)); | |
| 722 | 12 | _ambiguities.clear(); | |
| 723 | 12 | _cycleSlips.clear(); | |
| 724 | 12 | _cycleSlipRng.resetSeed(size_t(id)); | |
| 725 | 12 | _cycleSlipWindowStartTime.reset(); | |
| 726 | |||
| 727 | 12 | return true; | |
| 728 | } | ||
| 729 | |||
| 730 | 14 | void NAV::ErrorModel::afterCreateLink(OutputPin& startPin, InputPin& endPin) | |
| 731 | { | ||
| 732 | LOG_TRACE("{}: called for {} ==> {}", nameId(), size_t(startPin.id), size_t(endPin.id)); | ||
| 733 | |||
| 734 |
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) |
| 735 | { | ||
| 736 | 8 | return; // Link on Output Port | |
| 737 | } | ||
| 738 | |||
| 739 | // Store previous output pin identifier | ||
| 740 |
1/2✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
|
6 | auto previousOutputPinDataIdentifier = outputPins.front().dataIdentifier; |
| 741 | // Overwrite output pin identifier with input pin identifier | ||
| 742 |
1/2✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
|
6 | outputPins.front().dataIdentifier = startPin.dataIdentifier; |
| 743 | |||
| 744 |
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 |
| 745 | { | ||
| 746 | // Check if connected links on output port are still valid | ||
| 747 |
2/2✓ Branch 6 taken 1 times.
✓ Branch 7 taken 6 times.
|
7 | for (auto& link : outputPins.front().links) |
| 748 | { | ||
| 749 |
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()) |
| 750 | { | ||
| 751 |
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)) |
| 752 | { | ||
| 753 | // If the link is not valid anymore, delete it | ||
| 754 | ✗ | outputPins.front().deleteLink(*endPin); | |
| 755 | } | ||
| 756 | } | ||
| 757 | } | ||
| 758 | |||
| 759 | // Refresh all links connected to the output pin if the type changed | ||
| 760 |
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) |
| 761 | { | ||
| 762 |
2/2✓ Branch 6 taken 1 times.
✓ Branch 7 taken 6 times.
|
7 | for (auto& link : outputPins.front().links) |
| 763 | { | ||
| 764 |
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()) |
| 765 | { | ||
| 766 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | outputPins.front().recreateLink(*connectedPin); |
| 767 | } | ||
| 768 | } | ||
| 769 | } | ||
| 770 | } | ||
| 771 | 6 | } | |
| 772 | |||
| 773 | 14 | void NAV::ErrorModel::afterDeleteLink(OutputPin& startPin, InputPin& endPin) | |
| 774 | { | ||
| 775 | LOG_TRACE("{}: called for {} ==> {}", nameId(), size_t(startPin.id), size_t(endPin.id)); | ||
| 776 | |||
| 777 | 14 | if ((endPin.parentNode->id != id // Link on Output port is removed | |
| 778 |
2/2✓ Branch 2 taken 4 times.
✓ Branch 3 taken 4 times.
|
8 | && !inputPins.front().isPinLinked()) // and the Input port is not linked |
| 779 |
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 |
| 780 |
2/2✓ Branch 2 taken 2 times.
✓ Branch 3 taken 4 times.
|
6 | && !outputPins.front().isPinLinked())) // and the Output port is not linked |
| 781 | { | ||
| 782 | 6 | outputPins.front().dataIdentifier = supportedDataIdentifier; | |
| 783 | } | ||
| 784 | 14 | } | |
| 785 | |||
| 786 | 58446 | void NAV::ErrorModel::receiveObs(NAV::InputPin::NodeDataQueue& queue, size_t /* pinIdx */) | |
| 787 | { | ||
| 788 |
1/2✓ Branch 1 taken 58439 times.
✗ Branch 2 not taken.
|
58446 | auto obs = queue.extract_front(); |
| 789 |
3/4✓ Branch 1 taken 58432 times.
✓ Branch 2 taken 10 times.
✓ Branch 5 taken 58440 times.
✗ Branch 6 not taken.
|
58439 | if (!_lastObservationTime.empty()) { _dt = static_cast<double>((obs->insTime - _lastObservationTime).count()); } |
| 790 | |||
| 791 | // Accelerometer Bias in platform frame coordinates [m/s^2] | ||
| 792 |
1/2✓ Branch 1 taken 58439 times.
✗ Branch 2 not taken.
|
58442 | Eigen::Vector3d accelerometerBias_p = convertUnit(_imuAccelerometerBias_p, _imuAccelerometerBiasUnit); |
| 793 | LOG_DATA("{}: accelerometerBias_p = {} [m/s^2]", nameId(), accelerometerBias_p.transpose()); | ||
| 794 | |||
| 795 | // Gyroscope Bias in platform frame coordinates [rad/s] | ||
| 796 |
1/2✓ Branch 1 taken 58435 times.
✗ Branch 2 not taken.
|
58439 | Eigen::Vector3d gyroscopeBias_p = convertUnit(_imuGyroscopeBias_p, _imuGyroscopeBiasUnit); |
| 797 | LOG_DATA("{}: gyroscopeBias_p = {} [rad/s]", nameId(), gyroscopeBias_p.transpose()); | ||
| 798 | |||
| 799 | // ######################################################################################################################################### | ||
| 800 | |||
| 801 | // Accelerometer Noise standard deviation in platform frame coordinates [m/s^2/sqrt(s)] | ||
| 802 |
1/2✓ Branch 1 taken 58437 times.
✗ Branch 2 not taken.
|
58435 | Eigen::Vector3d accelerometerNoiseStd = convertUnit(_imuAccelerometerNoise, _imuAccelerometerNoiseUnit); |
| 803 | LOG_DATA("{}: accelerometerNoiseStd = {} [m/s^2/sqrt(s)]", nameId(), accelerometerNoiseStd.transpose()); | ||
| 804 | |||
| 805 | // Gyroscope Noise standard deviation in platform frame coordinates [rad/s/sqrt(s)] | ||
| 806 |
1/2✓ Branch 1 taken 58445 times.
✗ Branch 2 not taken.
|
58437 | Eigen::Vector3d gyroscopeNoiseStd = convertUnit(_imuGyroscopeNoise, _imuGyroscopeNoiseUnit); |
| 807 | LOG_DATA("{}: gyroscopeNoiseStd = {} [rad/s/sqrt(s)]", nameId(), gyroscopeNoiseStd.transpose()); | ||
| 808 | |||
| 809 | // Select the correct data type and make a copy of the node data to modify | ||
| 810 |
6/10✓ Branch 1 taken 58436 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 58442 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 58442 times.
✓ Branch 9 taken 58438 times.
✓ Branch 11 taken 34216 times.
✓ Branch 12 taken 24222 times.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
|
175325 | if (NAV::NodeRegistry::NodeDataTypeAnyIsChildOf(outputPins.front().dataIdentifier, { ImuObsSimulated::type() })) |
| 811 | { | ||
| 812 |
1/2✓ Branch 2 taken 34225 times.
✗ Branch 3 not taken.
|
34222 | invokeCallbacks(OUTPUT_PORT_INDEX_FLOW, |
| 813 |
2/4✓ Branch 3 taken 34218 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 34222 times.
✗ Branch 8 not taken.
|
68439 | receiveImuObsWDelta(std::make_shared<ImuObsSimulated>(*std::static_pointer_cast<const ImuObsSimulated>(obs)), |
| 814 | accelerometerBias_p, | ||
| 815 | gyroscopeBias_p, | ||
| 816 | accelerometerNoiseStd, | ||
| 817 | gyroscopeNoiseStd)); | ||
| 818 | } | ||
| 819 |
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() })) |
| 820 | { | ||
| 821 | ✗ | invokeCallbacks(OUTPUT_PORT_INDEX_FLOW, | |
| 822 | ✗ | receiveImuObsWDelta(std::make_shared<ImuObsWDelta>(*std::static_pointer_cast<const ImuObsWDelta>(obs)), | |
| 823 | accelerometerBias_p, | ||
| 824 | gyroscopeBias_p, | ||
| 825 | accelerometerNoiseStd, | ||
| 826 | gyroscopeNoiseStd)); | ||
| 827 | } | ||
| 828 |
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() })) |
| 829 | { | ||
| 830 | ✗ | invokeCallbacks(OUTPUT_PORT_INDEX_FLOW, | |
| 831 | ✗ | receiveImuObs(std::make_shared<ImuObs>(*std::static_pointer_cast<const ImuObs>(obs)), | |
| 832 | accelerometerBias_p, | ||
| 833 | gyroscopeBias_p, | ||
| 834 | accelerometerNoiseStd, | ||
| 835 | gyroscopeNoiseStd)); | ||
| 836 | } | ||
| 837 |
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() })) |
| 838 | { | ||
| 839 |
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)))); |
| 840 | } | ||
| 841 | ✗ | else if (NAV::NodeRegistry::NodeDataTypeAnyIsChildOf(outputPins.front().dataIdentifier, { PosVel::type() })) | |
| 842 | { | ||
| 843 | ✗ | invokeCallbacks(OUTPUT_PORT_INDEX_FLOW, receivePosVel(std::make_shared<PosVel>(*std::static_pointer_cast<const PosVel>(obs)))); | |
| 844 | } | ||
| 845 | ✗ | else if (NAV::NodeRegistry::NodeDataTypeAnyIsChildOf(outputPins.front().dataIdentifier, { Pos::type() })) | |
| 846 | { | ||
| 847 | ✗ | invokeCallbacks(OUTPUT_PORT_INDEX_FLOW, receivePos(std::make_shared<Pos>(*std::static_pointer_cast<const Pos>(obs)))); | |
| 848 | } | ||
| 849 | ✗ | else if (NAV::NodeRegistry::NodeDataTypeAnyIsChildOf(outputPins.front().dataIdentifier, { GnssObs::type() })) | |
| 850 | { | ||
| 851 | ✗ | invokeCallbacks(OUTPUT_PORT_INDEX_FLOW, receiveGnssObs(std::make_shared<GnssObs>(*std::static_pointer_cast<const GnssObs>(obs)))); | |
| 852 | } | ||
| 853 | |||
| 854 | 58444 | _lastObservationTime = obs->insTime; | |
| 855 | 189547 | } | |
| 856 | |||
| 857 | 34213 | std::shared_ptr<NAV::ImuObs> NAV::ErrorModel::receiveImuObs(const std::shared_ptr<ImuObs>& imuObs, | |
| 858 | const Eigen::Vector3d& accelerometerBias_p, | ||
| 859 | const Eigen::Vector3d& gyroscopeBias_p, | ||
| 860 | const Eigen::Vector3d& accelerometerNoiseStd, | ||
| 861 | const Eigen::Vector3d& gyroscopeNoiseStd) | ||
| 862 | { | ||
| 863 |
2/2✓ Branch 1 taken 34201 times.
✓ Branch 2 taken 6 times.
|
34213 | if (!_lastObservationTime.empty()) |
| 864 | { | ||
| 865 | { | ||
| 866 | // Accelerometer RW standard deviation in platform frame coordinates [m/s^2/sqrt(s)] | ||
| 867 |
1/2✓ Branch 1 taken 34214 times.
✗ Branch 2 not taken.
|
34201 | Eigen::Vector3d accelerometerRWStd = convertUnit(_imuAccelerometerRW, _imuAccelerometerRWUnit); |
| 868 | LOG_DATA("{}: accelerometerRWStd = {} [m/s^2/sqrt(s)]", nameId(), accelerometerRWStd.transpose()); | ||
| 869 | |||
| 870 | ✗ | _randomWalkAccelerometer += Eigen::Vector3d{ | |
| 871 |
1/2✓ Branch 1 taken 34218 times.
✗ Branch 2 not taken.
|
34213 | _imuAccelerometerRWRng.getRand_normalDist(0.0, accelerometerRWStd(0)), |
| 872 |
2/4✓ Branch 1 taken 34215 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34218 times.
✗ Branch 5 not taken.
|
34218 | _imuAccelerometerRWRng.getRand_normalDist(0.0, accelerometerRWStd(1)), |
| 873 |
3/6✓ Branch 1 taken 34219 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34220 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 34219 times.
✗ Branch 8 not taken.
|
34218 | _imuAccelerometerRWRng.getRand_normalDist(0.0, accelerometerRWStd(2)) |
| 874 |
3/6✓ Branch 1 taken 34213 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34214 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 34202 times.
✗ Branch 8 not taken.
|
68433 | } * sqrt(_dt); |
| 875 | } | ||
| 876 | { | ||
| 877 | // Accelerometer IRW standard deviation in platform frame coordinates [m/s^3/sqrt(s)] | ||
| 878 |
1/2✓ Branch 1 taken 34215 times.
✗ Branch 2 not taken.
|
34202 | Eigen::Vector3d accelerometerIRWStd = convertUnit(_imuAccelerometerIRW, _imuAccelerometerIRWUnit); |
| 879 | LOG_DATA("{}: accelerometerIRWStd = {} [m/s^3/sqrt(s)]", nameId(), accelerometerIRWStd.transpose()); | ||
| 880 | |||
| 881 | // compute velocity RW first | ||
| 882 | ✗ | _integratedRandomWalkAccelerometer_velocity += Eigen::Vector3d{ | |
| 883 |
1/2✓ Branch 1 taken 34218 times.
✗ Branch 2 not taken.
|
34207 | _imuAccelerometerIRWRng.getRand_normalDist(0.0, accelerometerIRWStd(0)), |
| 884 |
2/4✓ Branch 1 taken 34219 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34218 times.
✗ Branch 5 not taken.
|
34218 | _imuAccelerometerIRWRng.getRand_normalDist(0.0, accelerometerIRWStd(1)), |
| 885 |
3/6✓ Branch 1 taken 34215 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34221 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 34221 times.
✗ Branch 8 not taken.
|
34218 | _imuAccelerometerIRWRng.getRand_normalDist(0.0, accelerometerIRWStd(2)) |
| 886 |
3/6✓ Branch 1 taken 34207 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34218 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 34213 times.
✗ Branch 8 not taken.
|
68436 | } * sqrt(_dt); |
| 887 | |||
| 888 | // then compute IRW | ||
| 889 |
2/4✓ Branch 1 taken 34216 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34214 times.
✗ Branch 5 not taken.
|
34213 | _integratedRandomWalkAccelerometer += _integratedRandomWalkAccelerometer_velocity * _dt; |
| 890 | } | ||
| 891 | { | ||
| 892 | // Gyro RW standard deviation in platform frame coordinates [rad/s/sqrt(s)] | ||
| 893 |
1/2✓ Branch 1 taken 34217 times.
✗ Branch 2 not taken.
|
34214 | Eigen::Vector3d gyroscopeRWStd = convertUnit(_imuGyroscopeRW, _imuGyroscopeRWUnit); |
| 894 | LOG_DATA("{}: gyroscopeRWStd = {} [rad/s/sqrt(s)]", nameId(), gyroscopeRWStd.transpose()); | ||
| 895 | |||
| 896 | ✗ | _randomWalkGyroscope += Eigen::Vector3d{ | |
| 897 |
1/2✓ Branch 1 taken 34221 times.
✗ Branch 2 not taken.
|
34216 | _imuGyroscopeRWRng.getRand_normalDist(0.0, gyroscopeRWStd(0)), |
| 898 |
2/4✓ Branch 1 taken 34217 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34217 times.
✗ Branch 5 not taken.
|
34221 | _imuGyroscopeRWRng.getRand_normalDist(0.0, gyroscopeRWStd(1)), |
| 899 |
3/6✓ Branch 1 taken 34217 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34220 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 34220 times.
✗ Branch 8 not taken.
|
34217 | _imuGyroscopeRWRng.getRand_normalDist(0.0, gyroscopeRWStd(2)) |
| 900 |
3/6✓ Branch 1 taken 34216 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34220 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 34219 times.
✗ Branch 8 not taken.
|
68437 | } * sqrt(_dt); |
| 901 | } | ||
| 902 | { | ||
| 903 | // Gyro RW standard deviation in platform frame coordinates [rad/s^2/sqrt(s)] | ||
| 904 |
1/2✓ Branch 1 taken 34219 times.
✗ Branch 2 not taken.
|
34219 | Eigen::Vector3d gyroscopeIRWStd = convertUnit(_imuGyroscopeIRW, _imuGyroscopeIRWUnit); |
| 905 | LOG_DATA("{}: gyroscopeIRWStd = {} [rad/s^2/sqrt(s)]", nameId(), gyroscopeIRWStd.transpose()); | ||
| 906 | |||
| 907 | // compute velocity RW first | ||
| 908 | ✗ | _integratedRandomWalkGyroscope_velocity += Eigen::Vector3d{ | |
| 909 |
1/2✓ Branch 1 taken 34220 times.
✗ Branch 2 not taken.
|
34219 | _imuGyroscopeIRWRng.getRand_normalDist(0.0, gyroscopeIRWStd(0)), |
| 910 |
2/4✓ Branch 1 taken 34219 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34221 times.
✗ Branch 5 not taken.
|
34220 | _imuGyroscopeIRWRng.getRand_normalDist(0.0, gyroscopeIRWStd(1)), |
| 911 |
3/6✓ Branch 1 taken 34219 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34221 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 34220 times.
✗ Branch 8 not taken.
|
34221 | _imuGyroscopeIRWRng.getRand_normalDist(0.0, gyroscopeIRWStd(2)) |
| 912 |
3/6✓ Branch 1 taken 34219 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34218 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 34217 times.
✗ Branch 8 not taken.
|
68439 | } * sqrt(_dt); |
| 913 | |||
| 914 | // then compute IRW | ||
| 915 |
2/4✓ Branch 1 taken 34218 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34218 times.
✗ Branch 5 not taken.
|
34217 | _integratedRandomWalkGyroscope += _integratedRandomWalkGyroscope_velocity * _dt; |
| 916 | } | ||
| 917 | } | ||
| 918 | |||
| 919 | // ######################################################################################################################################### | ||
| 920 | |||
| 921 | 34222 | imuObs->p_acceleration += accelerometerBias_p | |
| 922 | ✗ | + _randomWalkAccelerometer | |
| 923 |
3/6✓ Branch 1 taken 34223 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34222 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 34222 times.
✗ Branch 8 not taken.
|
68447 | + _integratedRandomWalkAccelerometer; |
| 924 | |||
| 925 | 34219 | imuObs->p_angularRate += gyroscopeBias_p | |
| 926 | ✗ | + _randomWalkGyroscope | |
| 927 |
3/6✓ Branch 1 taken 34221 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34219 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 34219 times.
✗ Branch 8 not taken.
|
68439 | + _integratedRandomWalkGyroscope; |
| 928 |
2/2✓ Branch 0 taken 34216 times.
✓ Branch 1 taken 3 times.
|
34219 | if (_dt > 1e-9) |
| 929 | { | ||
| 930 |
1/2✓ Branch 1 taken 34219 times.
✗ Branch 2 not taken.
|
68430 | imuObs->p_acceleration += Eigen::Vector3d{ _imuAccelerometerRng.getRand_normalDist(0.0, accelerometerNoiseStd(0)), |
| 931 |
2/4✓ Branch 1 taken 34218 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34221 times.
✗ Branch 5 not taken.
|
34219 | _imuAccelerometerRng.getRand_normalDist(0.0, accelerometerNoiseStd(1)), |
| 932 |
3/6✓ Branch 1 taken 34221 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34220 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 34218 times.
✗ Branch 8 not taken.
|
34221 | _imuAccelerometerRng.getRand_normalDist(0.0, accelerometerNoiseStd(2)) } |
| 933 |
3/6✓ Branch 1 taken 34213 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34217 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 34203 times.
✗ Branch 8 not taken.
|
102652 | / std::sqrt(_dt); // Scale with input frequency |
| 934 |
1/2✓ Branch 1 taken 34220 times.
✗ Branch 2 not taken.
|
68421 | imuObs->p_angularRate += Eigen::Vector3d{ _imuGyroscopeRng.getRand_normalDist(0.0, gyroscopeNoiseStd(0)), |
| 935 |
2/4✓ Branch 1 taken 34220 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34219 times.
✗ Branch 5 not taken.
|
34220 | _imuGyroscopeRng.getRand_normalDist(0.0, gyroscopeNoiseStd(1)), |
| 936 |
3/6✓ Branch 1 taken 34219 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34220 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 34220 times.
✗ Branch 8 not taken.
|
34219 | _imuGyroscopeRng.getRand_normalDist(0.0, gyroscopeNoiseStd(2)) } |
| 937 |
3/6✓ Branch 1 taken 34207 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34214 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 34214 times.
✗ Branch 8 not taken.
|
102640 | / std::sqrt(_dt); // Scale with input frequency |
| 938 | } | ||
| 939 | |||
| 940 | 34217 | return imuObs; | |
| 941 | } | ||
| 942 | |||
| 943 | 34224 | std::shared_ptr<NAV::ImuObsWDelta> NAV::ErrorModel::receiveImuObsWDelta(const std::shared_ptr<ImuObsWDelta>& imuObsWDelta, | |
| 944 | const Eigen::Vector3d& accelerometerBias_p, | ||
| 945 | const Eigen::Vector3d& gyroscopeBias_p, | ||
| 946 | const Eigen::Vector3d& accelerometerNoiseStd, | ||
| 947 | const Eigen::Vector3d& gyroscopeNoiseStd) | ||
| 948 | { | ||
| 949 |
1/2✓ Branch 2 taken 34224 times.
✗ Branch 3 not taken.
|
34224 | receiveImuObs(imuObsWDelta, accelerometerBias_p, gyroscopeBias_p, accelerometerNoiseStd, gyroscopeNoiseStd); |
| 950 | |||
| 951 |
2/2✓ Branch 0 taken 34214 times.
✓ Branch 1 taken 5 times.
|
34219 | double imuObsWDeltaAverageWindow = _dt != 0.0 ? _dt / imuObsWDelta->dtime : 1.0; |
| 952 | |||
| 953 |
4/8✓ Branch 1 taken 34217 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34221 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 34214 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 34215 times.
✗ Branch 13 not taken.
|
34212 | imuObsWDelta->dvel += imuObsWDelta->dtime * (accelerometerBias_p + _randomWalkAccelerometer + _integratedRandomWalkAccelerometer); |
| 954 |
4/8✓ Branch 1 taken 34215 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34216 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 34215 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 34219 times.
✗ Branch 13 not taken.
|
34215 | imuObsWDelta->dtheta += imuObsWDelta->dtime * (gyroscopeBias_p + _randomWalkGyroscope + _integratedRandomWalkGyroscope); |
| 955 |
2/2✓ Branch 0 taken 34215 times.
✓ Branch 1 taken 4 times.
|
34219 | if (_dt > 1e-9) |
| 956 | { | ||
| 957 |
1/2✓ Branch 1 taken 34220 times.
✗ Branch 2 not taken.
|
68423 | imuObsWDelta->dvel += Eigen::Vector3d{ _imuAccelerometerRng.getRand_normalDist(0.0, accelerometerNoiseStd(0) / std::sqrt(imuObsWDeltaAverageWindow)), |
| 958 |
2/4✓ Branch 1 taken 34217 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34220 times.
✗ Branch 5 not taken.
|
34220 | _imuAccelerometerRng.getRand_normalDist(0.0, accelerometerNoiseStd(1) / std::sqrt(imuObsWDeltaAverageWindow)), |
| 959 |
3/6✓ Branch 1 taken 34220 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34218 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 34217 times.
✗ Branch 8 not taken.
|
34220 | _imuAccelerometerRng.getRand_normalDist(0.0, accelerometerNoiseStd(2) / std::sqrt(imuObsWDeltaAverageWindow)) } |
| 960 |
4/8✓ Branch 2 taken 34209 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 34216 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 34214 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 34202 times.
✗ Branch 12 not taken.
|
102643 | * imuObsWDelta->dtime / std::sqrt(_dt); // Scale with input frequency |
| 961 |
1/2✓ Branch 1 taken 34217 times.
✗ Branch 2 not taken.
|
68426 | imuObsWDelta->dtheta += Eigen::Vector3d{ _imuGyroscopeRng.getRand_normalDist(0.0, gyroscopeNoiseStd(0) / std::sqrt(imuObsWDeltaAverageWindow)), |
| 962 |
2/4✓ Branch 1 taken 34214 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34219 times.
✗ Branch 5 not taken.
|
34217 | _imuGyroscopeRng.getRand_normalDist(0.0, gyroscopeNoiseStd(1) / std::sqrt(imuObsWDeltaAverageWindow)), |
| 963 |
3/6✓ Branch 1 taken 34219 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34221 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 34218 times.
✗ Branch 8 not taken.
|
34219 | _imuGyroscopeRng.getRand_normalDist(0.0, gyroscopeNoiseStd(2) / std::sqrt(imuObsWDeltaAverageWindow)) } |
| 964 |
4/8✓ Branch 2 taken 34208 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 34219 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 34218 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 34214 times.
✗ Branch 12 not taken.
|
102638 | * imuObsWDelta->dtime / std::sqrt(_dt); // Scale with input frequency |
| 965 | } | ||
| 966 | |||
| 967 | 34218 | return imuObsWDelta; | |
| 968 | } | ||
| 969 | |||
| 970 | ✗ | std::shared_ptr<NAV::Pos> NAV::ErrorModel::receivePos(const std::shared_ptr<Pos>& pos) | |
| 971 | { | ||
| 972 | // Position Bias in latLonAlt in [rad, rad, m] | ||
| 973 | ✗ | Eigen::Vector3d lla_positionBias = Eigen::Vector3d::Zero(); | |
| 974 | ✗ | switch (_positionBiasUnit) | |
| 975 | { | ||
| 976 | ✗ | case PositionBiasUnits::meter: | |
| 977 | { | ||
| 978 | ✗ | Eigen::Vector3d e_positionBias = trafo::e_Quat_n(pos->latitude(), pos->longitude()) * _positionBias; | |
| 979 | ✗ | if (!e_positionBias.isZero()) | |
| 980 | { | ||
| 981 | ✗ | lla_positionBias = trafo::ecef2lla_WGS84(pos->e_position() + e_positionBias) - pos->lla_position(); | |
| 982 | } | ||
| 983 | ✗ | break; | |
| 984 | } | ||
| 985 | } | ||
| 986 | LOG_DATA("{}: lla_positionBias = {} [rad, rad, m]", nameId(), lla_positionBias.transpose()); | ||
| 987 | |||
| 988 | // Velocity bias in local-navigation coordinates in [m/s] | ||
| 989 | ✗ | Eigen::Vector3d n_velocityBias = Eigen::Vector3d::Zero(); | |
| 990 | ✗ | switch (_velocityBiasUnit) | |
| 991 | { | ||
| 992 | ✗ | case VelocityBiasUnits::m_s: | |
| 993 | ✗ | n_velocityBias = _velocityBias; | |
| 994 | ✗ | break; | |
| 995 | } | ||
| 996 | LOG_DATA("{}: n_velocityBias = {} [m/s]", nameId(), n_velocityBias.transpose()); | ||
| 997 | |||
| 998 | // Roll, pitch, yaw bias in [rad] | ||
| 999 | ✗ | Eigen::Vector3d attitudeBias = Eigen::Vector3d::Zero(); | |
| 1000 | ✗ | switch (_attitudeBiasUnit) | |
| 1001 | { | ||
| 1002 | ✗ | case AttitudeBiasUnits::rad: | |
| 1003 | ✗ | attitudeBias = _attitudeBias; | |
| 1004 | ✗ | break; | |
| 1005 | ✗ | case AttitudeBiasUnits::deg: | |
| 1006 | ✗ | attitudeBias = deg2rad(_attitudeBias); | |
| 1007 | ✗ | break; | |
| 1008 | } | ||
| 1009 | LOG_DATA("{}: attitudeBias = {} [rad]", nameId(), attitudeBias.transpose()); | ||
| 1010 | |||
| 1011 | // ######################################################################################################################################### | ||
| 1012 | |||
| 1013 | // Position Noise standard deviation in latitude, longitude and altitude [rad, rad, m] | ||
| 1014 | ✗ | Eigen::Vector3d lla_positionNoiseStd = Eigen::Vector3d::Zero(); | |
| 1015 | ✗ | Eigen::Vector3d NED_pos_variance = Eigen::Vector3d::Zero(); | |
| 1016 | ✗ | switch (_positionNoiseUnit) | |
| 1017 | { | ||
| 1018 | ✗ | case PositionNoiseUnits::meter: | |
| 1019 | { | ||
| 1020 | ✗ | Eigen::Vector3d e_positionNoiseStd = trafo::e_Quat_n(pos->latitude(), pos->longitude()) * _positionNoise; | |
| 1021 | ✗ | if (!e_positionNoiseStd.isZero()) | |
| 1022 | { | ||
| 1023 | ✗ | lla_positionNoiseStd = trafo::ecef2lla_WGS84(pos->e_position() + e_positionNoiseStd) - pos->lla_position(); | |
| 1024 | } | ||
| 1025 | ✗ | NED_pos_variance = _positionNoise.cwiseAbs2(); | |
| 1026 | ✗ | break; | |
| 1027 | } | ||
| 1028 | ✗ | case PositionNoiseUnits::meter2: | |
| 1029 | { | ||
| 1030 | ✗ | Eigen::Vector3d e_positionNoiseStd = trafo::e_Quat_n(pos->latitude(), pos->longitude()) * _positionNoise.cwiseSqrt(); | |
| 1031 | ✗ | if (!e_positionNoiseStd.isZero()) | |
| 1032 | { | ||
| 1033 | ✗ | lla_positionNoiseStd = trafo::ecef2lla_WGS84(pos->e_position() + e_positionNoiseStd) - pos->lla_position(); | |
| 1034 | } | ||
| 1035 | ✗ | NED_pos_variance = _positionNoise; | |
| 1036 | ✗ | break; | |
| 1037 | } | ||
| 1038 | } | ||
| 1039 | LOG_DATA("{}: lla_positionNoiseStd = {} [rad, rad, m]", nameId(), lla_positionNoiseStd.transpose()); | ||
| 1040 | |||
| 1041 | // ######################################################################################################################################### | ||
| 1042 | |||
| 1043 | ✗ | pos->setPositionAndCov_n(pos->lla_position() | |
| 1044 | ✗ | + lla_positionBias | |
| 1045 | ✗ | + Eigen::Vector3d{ _positionRng.getRand_normalDist(0.0, lla_positionNoiseStd(0)), | |
| 1046 | ✗ | _positionRng.getRand_normalDist(0.0, lla_positionNoiseStd(1)), | |
| 1047 | ✗ | _positionRng.getRand_normalDist(0.0, lla_positionNoiseStd(2)) }, | |
| 1048 | ✗ | NED_pos_variance.asDiagonal().toDenseMatrix()); | |
| 1049 | |||
| 1050 | ✗ | return pos; | |
| 1051 | } | ||
| 1052 | |||
| 1053 | ✗ | std::shared_ptr<NAV::PosVel> NAV::ErrorModel::receivePosVel(const std::shared_ptr<PosVel>& posVel) | |
| 1054 | { | ||
| 1055 | // Position Bias in latLonAlt in [rad, rad, m] | ||
| 1056 | ✗ | Eigen::Vector3d lla_positionBias = Eigen::Vector3d::Zero(); | |
| 1057 | ✗ | switch (_positionBiasUnit) | |
| 1058 | { | ||
| 1059 | ✗ | case PositionBiasUnits::meter: | |
| 1060 | { | ||
| 1061 | ✗ | Eigen::Vector3d e_positionBias = trafo::e_Quat_n(posVel->latitude(), posVel->longitude()) * _positionBias; | |
| 1062 | ✗ | if (!e_positionBias.isZero()) | |
| 1063 | { | ||
| 1064 | ✗ | lla_positionBias = trafo::ecef2lla_WGS84(posVel->e_position() + e_positionBias) - posVel->lla_position(); | |
| 1065 | } | ||
| 1066 | ✗ | break; | |
| 1067 | } | ||
| 1068 | } | ||
| 1069 | LOG_DATA("{}: lla_positionBias = {} [rad, rad, m]", nameId(), lla_positionBias.transpose()); | ||
| 1070 | |||
| 1071 | // Velocity bias in local-navigation coordinates in [m/s] | ||
| 1072 | ✗ | Eigen::Vector3d n_velocityBias = Eigen::Vector3d::Zero(); | |
| 1073 | ✗ | switch (_velocityBiasUnit) | |
| 1074 | { | ||
| 1075 | ✗ | case VelocityBiasUnits::m_s: | |
| 1076 | ✗ | n_velocityBias = _velocityBias; | |
| 1077 | ✗ | break; | |
| 1078 | } | ||
| 1079 | LOG_DATA("{}: n_velocityBias = {} [m/s]", nameId(), n_velocityBias.transpose()); | ||
| 1080 | |||
| 1081 | // ######################################################################################################################################### | ||
| 1082 | |||
| 1083 | // Position Noise standard deviation in latitude, longitude and altitude [rad, rad, m] | ||
| 1084 | ✗ | Eigen::Vector3d lla_positionNoiseStd = Eigen::Vector3d::Zero(); | |
| 1085 | ✗ | Eigen::Vector3d NED_pos_variance = Eigen::Vector3d::Zero(); | |
| 1086 | ✗ | Eigen::Vector3d NED_velocity_variance = Eigen::Vector3d::Zero(); | |
| 1087 | ✗ | switch (_positionNoiseUnit) | |
| 1088 | { | ||
| 1089 | ✗ | case PositionNoiseUnits::meter: | |
| 1090 | { | ||
| 1091 | ✗ | Eigen::Vector3d e_positionNoiseStd = trafo::e_Quat_n(posVel->latitude(), posVel->longitude()) * _positionNoise; | |
| 1092 | ✗ | if (!e_positionNoiseStd.isZero()) | |
| 1093 | { | ||
| 1094 | ✗ | lla_positionNoiseStd = trafo::ecef2lla_WGS84(posVel->e_position() + e_positionNoiseStd) - posVel->lla_position(); | |
| 1095 | } | ||
| 1096 | ✗ | NED_pos_variance = _positionNoise.cwiseAbs2(); | |
| 1097 | ✗ | break; | |
| 1098 | } | ||
| 1099 | ✗ | case PositionNoiseUnits::meter2: | |
| 1100 | { | ||
| 1101 | ✗ | Eigen::Vector3d e_positionNoiseStd = trafo::e_Quat_n(posVel->latitude(), posVel->longitude()) * _positionNoise.cwiseSqrt(); | |
| 1102 | ✗ | if (!e_positionNoiseStd.isZero()) | |
| 1103 | { | ||
| 1104 | ✗ | lla_positionNoiseStd = trafo::ecef2lla_WGS84(posVel->e_position() + e_positionNoiseStd) - posVel->lla_position(); | |
| 1105 | } | ||
| 1106 | ✗ | NED_pos_variance = _positionNoise; | |
| 1107 | ✗ | break; | |
| 1108 | } | ||
| 1109 | } | ||
| 1110 | LOG_DATA("{}: lla_positionNoiseStd = {} [rad, rad, m]", nameId(), lla_positionNoiseStd.transpose()); | ||
| 1111 | |||
| 1112 | // Velocity Noise standard deviation in local-navigation coordinates in [m/s] | ||
| 1113 | ✗ | Eigen::Vector3d n_velocityNoiseStd = Eigen::Vector3d::Zero(); | |
| 1114 | ✗ | switch (_velocityNoiseUnit) | |
| 1115 | { | ||
| 1116 | ✗ | case VelocityNoiseUnits::m_s: | |
| 1117 | ✗ | n_velocityNoiseStd = _velocityNoise; | |
| 1118 | ✗ | NED_velocity_variance = _velocityNoise.cwiseAbs2(); | |
| 1119 | ✗ | break; | |
| 1120 | ✗ | case VelocityNoiseUnits::m2_s2: | |
| 1121 | ✗ | n_velocityNoiseStd = _velocityNoise.cwiseSqrt(); | |
| 1122 | ✗ | NED_velocity_variance = _velocityNoise; | |
| 1123 | ✗ | break; | |
| 1124 | } | ||
| 1125 | LOG_DATA("{}: n_velocityNoiseStd = {} [m/s]", nameId(), n_velocityNoiseStd.transpose()); | ||
| 1126 | |||
| 1127 | // ######################################################################################################################################### | ||
| 1128 | |||
| 1129 | ✗ | Eigen::Matrix<double, 6, 6> cov_n = Eigen::Matrix<double, 6, 6>::Zero(); | |
| 1130 | ✗ | cov_n.block<3, 3>(0, 0).diagonal() = NED_pos_variance; | |
| 1131 | ✗ | cov_n.block<3, 3>(3, 3).diagonal() = NED_velocity_variance; | |
| 1132 | |||
| 1133 | ✗ | posVel->setPosVelAndCov_n(posVel->lla_position() | |
| 1134 | ✗ | + lla_positionBias | |
| 1135 | ✗ | + Eigen::Vector3d{ _positionRng.getRand_normalDist(0.0, lla_positionNoiseStd(0)), | |
| 1136 | ✗ | _positionRng.getRand_normalDist(0.0, lla_positionNoiseStd(1)), | |
| 1137 | ✗ | _positionRng.getRand_normalDist(0.0, lla_positionNoiseStd(2)) }, | |
| 1138 | ✗ | posVel->n_velocity() | |
| 1139 | ✗ | + n_velocityBias | |
| 1140 | ✗ | + Eigen::Vector3d{ _velocityRng.getRand_normalDist(0.0, n_velocityNoiseStd(0)), | |
| 1141 | ✗ | _velocityRng.getRand_normalDist(0.0, n_velocityNoiseStd(1)), | |
| 1142 | ✗ | _velocityRng.getRand_normalDist(0.0, n_velocityNoiseStd(2)) }, | |
| 1143 | cov_n); | ||
| 1144 | |||
| 1145 | ✗ | return posVel; | |
| 1146 | } | ||
| 1147 | |||
| 1148 | 24222 | std::shared_ptr<NAV::PosVelAtt> NAV::ErrorModel::receivePosVelAtt(const std::shared_ptr<PosVelAtt>& posVelAtt) | |
| 1149 | { | ||
| 1150 | // Position Bias in latLonAlt in [rad, rad, m] | ||
| 1151 |
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(); |
| 1152 |
1/2✓ Branch 0 taken 24222 times.
✗ Branch 1 not taken.
|
24222 | switch (_positionBiasUnit) |
| 1153 | { | ||
| 1154 | 24222 | case PositionBiasUnits::meter: | |
| 1155 | { | ||
| 1156 |
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; |
| 1157 |
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()) |
| 1158 | { | ||
| 1159 | ✗ | lla_positionBias = trafo::ecef2lla_WGS84(posVelAtt->e_position() + e_positionBias) - posVelAtt->lla_position(); | |
| 1160 | } | ||
| 1161 | 24222 | break; | |
| 1162 | } | ||
| 1163 | } | ||
| 1164 | LOG_DATA("{}: lla_positionBias = {} [rad, rad, m]", nameId(), lla_positionBias.transpose()); | ||
| 1165 | |||
| 1166 | // Velocity bias in local-navigation coordinates in [m/s] | ||
| 1167 |
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(); |
| 1168 |
1/2✓ Branch 0 taken 24222 times.
✗ Branch 1 not taken.
|
24222 | switch (_velocityBiasUnit) |
| 1169 | { | ||
| 1170 | 24222 | case VelocityBiasUnits::m_s: | |
| 1171 |
1/2✓ Branch 1 taken 24222 times.
✗ Branch 2 not taken.
|
24222 | n_velocityBias = _velocityBias; |
| 1172 | 24222 | break; | |
| 1173 | } | ||
| 1174 | LOG_DATA("{}: n_velocityBias = {} [m/s]", nameId(), n_velocityBias.transpose()); | ||
| 1175 | |||
| 1176 | // Roll, pitch, yaw bias in [rad] | ||
| 1177 |
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(); |
| 1178 |
1/3✗ Branch 0 not taken.
✓ Branch 1 taken 24222 times.
✗ Branch 2 not taken.
|
24222 | switch (_attitudeBiasUnit) |
| 1179 | { | ||
| 1180 | ✗ | case AttitudeBiasUnits::rad: | |
| 1181 | ✗ | attitudeBias = _attitudeBias; | |
| 1182 | ✗ | break; | |
| 1183 | 24222 | case AttitudeBiasUnits::deg: | |
| 1184 |
1/2✓ Branch 1 taken 24222 times.
✗ Branch 2 not taken.
|
24222 | attitudeBias = deg2rad(_attitudeBias); |
| 1185 | 24222 | break; | |
| 1186 | } | ||
| 1187 | LOG_DATA("{}: attitudeBias = {} [rad]", nameId(), attitudeBias.transpose()); | ||
| 1188 | |||
| 1189 | // ######################################################################################################################################### | ||
| 1190 | |||
| 1191 | // Position Noise standard deviation in latitude, longitude and altitude [rad, rad, m] | ||
| 1192 |
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(); |
| 1193 |
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(); |
| 1194 |
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(); |
| 1195 |
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(); |
| 1196 |
1/3✓ Branch 0 taken 24222 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
|
24222 | switch (_positionNoiseUnit) |
| 1197 | { | ||
| 1198 | 24222 | case PositionNoiseUnits::meter: | |
| 1199 | { | ||
| 1200 |
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; |
| 1201 |
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()) |
| 1202 | { | ||
| 1203 |
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(); |
| 1204 | } | ||
| 1205 |
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(); |
| 1206 | 24222 | break; | |
| 1207 | } | ||
| 1208 | ✗ | case PositionNoiseUnits::meter2: | |
| 1209 | { | ||
| 1210 | ✗ | Eigen::Vector3d e_positionNoiseStd = trafo::e_Quat_n(posVelAtt->latitude(), posVelAtt->longitude()) * _positionNoise.cwiseSqrt(); | |
| 1211 | ✗ | if (!e_positionNoiseStd.isZero()) | |
| 1212 | { | ||
| 1213 | ✗ | lla_positionNoiseStd = trafo::ecef2lla_WGS84(posVelAtt->e_position() + e_positionNoiseStd) - posVelAtt->lla_position(); | |
| 1214 | } | ||
| 1215 | ✗ | NED_pos_variance = _positionNoise; | |
| 1216 | ✗ | break; | |
| 1217 | } | ||
| 1218 | } | ||
| 1219 | LOG_DATA("{}: lla_positionNoiseStd = {} [rad, rad, m]", nameId(), lla_positionNoiseStd.transpose()); | ||
| 1220 | |||
| 1221 | // Velocity Noise standard deviation in local-navigation coordinates in [m/s] | ||
| 1222 |
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(); |
| 1223 |
1/3✓ Branch 0 taken 24222 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
|
24222 | switch (_velocityNoiseUnit) |
| 1224 | { | ||
| 1225 | 24222 | case VelocityNoiseUnits::m_s: | |
| 1226 |
1/2✓ Branch 1 taken 24222 times.
✗ Branch 2 not taken.
|
24222 | n_velocityNoiseStd = _velocityNoise; |
| 1227 |
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(); |
| 1228 | 24222 | break; | |
| 1229 | ✗ | case VelocityNoiseUnits::m2_s2: | |
| 1230 | ✗ | n_velocityNoiseStd = _velocityNoise.cwiseSqrt(); | |
| 1231 | ✗ | NED_velocity_variance = _velocityNoise; | |
| 1232 | ✗ | break; | |
| 1233 | } | ||
| 1234 | LOG_DATA("{}: n_velocityNoiseStd = {} [m/s]", nameId(), n_velocityNoiseStd.transpose()); | ||
| 1235 | |||
| 1236 | // Attitude Noise standard deviation in [rad] | ||
| 1237 |
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(); |
| 1238 |
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) |
| 1239 | { | ||
| 1240 | ✗ | case AttitudeNoiseUnits::rad: | |
| 1241 | ✗ | attitudeNoiseStd = _attitudeNoise; | |
| 1242 | ✗ | RPY_attitude_variance = _attitudeNoise.cwiseAbs2(); | |
| 1243 | ✗ | break; | |
| 1244 | 24222 | case AttitudeNoiseUnits::deg: | |
| 1245 |
1/2✓ Branch 1 taken 24222 times.
✗ Branch 2 not taken.
|
24222 | attitudeNoiseStd = deg2rad(_attitudeNoise); |
| 1246 |
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(); |
| 1247 | 24222 | break; | |
| 1248 | ✗ | case AttitudeNoiseUnits::rad2: | |
| 1249 | ✗ | attitudeNoiseStd = _attitudeNoise.cwiseSqrt(); | |
| 1250 | ✗ | RPY_attitude_variance = _attitudeNoise; | |
| 1251 | ✗ | break; | |
| 1252 | ✗ | case AttitudeNoiseUnits::deg2: | |
| 1253 | ✗ | attitudeNoiseStd = deg2rad(_attitudeNoise.cwiseSqrt()); | |
| 1254 | ✗ | RPY_attitude_variance = deg2rad(deg2rad(_attitudeNoise)); | |
| 1255 | ✗ | break; | |
| 1256 | } | ||
| 1257 | LOG_DATA("{}: attitudeNoiseStd = {} [rad]", nameId(), attitudeNoiseStd.transpose()); | ||
| 1258 | |||
| 1259 | // ######################################################################################################################################### | ||
| 1260 | |||
| 1261 |
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(); |
| 1262 |
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; |
| 1263 |
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; |
| 1264 |
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; |
| 1265 | |||
| 1266 |
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(); |
| 1267 |
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(); |
| 1268 |
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()); |
| 1269 | |||
| 1270 |
1/2✓ Branch 4 taken 24222 times.
✗ Branch 5 not taken.
|
72666 | posVelAtt->setPosVelAttAndCov_n(posVelAtt->lla_position() |
| 1271 |
1/2✓ Branch 1 taken 24222 times.
✗ Branch 2 not taken.
|
24222 | + lla_positionBias |
| 1272 |
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)), |
| 1273 |
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)), |
| 1274 |
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)) }, |
| 1275 | 24222 | posVelAtt->n_velocity() | |
| 1276 |
1/2✓ Branch 1 taken 24222 times.
✗ Branch 2 not taken.
|
24222 | + n_velocityBias |
| 1277 |
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)), |
| 1278 |
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)), |
| 1279 |
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)) }, |
| 1280 |
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() |
| 1281 |
1/2✓ Branch 1 taken 24222 times.
✗ Branch 2 not taken.
|
24222 | + attitudeBias |
| 1282 |
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)), |
| 1283 |
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)), |
| 1284 |
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)) }), |
| 1285 |
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()); |
| 1286 | |||
| 1287 | 48444 | return posVelAtt; | |
| 1288 | } | ||
| 1289 | |||
| 1290 | ✗ | std::shared_ptr<NAV::GnssObs> NAV::ErrorModel::receiveGnssObs(const std::shared_ptr<GnssObs>& gnssObs) | |
| 1291 | { | ||
| 1292 | LOG_DATA("{}: [{}] Simulating error on GnssObs", nameId(), gnssObs->insTime.toYMDHMS(GPST)); | ||
| 1293 | ✗ | double pseudorangeNoise{}; // [m] | |
| 1294 | ✗ | switch (_gui_pseudorangeNoiseUnit) | |
| 1295 | { | ||
| 1296 | ✗ | case PseudorangeNoiseUnits::meter: | |
| 1297 | ✗ | pseudorangeNoise = _gui_pseudorangeNoise; | |
| 1298 | ✗ | break; | |
| 1299 | } | ||
| 1300 | ✗ | double carrierPhaseNoise{}; // [m] | |
| 1301 | ✗ | switch (_gui_carrierPhaseNoiseUnit) | |
| 1302 | { | ||
| 1303 | ✗ | case CarrierPhaseNoiseUnits::meter: | |
| 1304 | ✗ | carrierPhaseNoise = _gui_carrierPhaseNoise; | |
| 1305 | ✗ | break; | |
| 1306 | } | ||
| 1307 | ✗ | double dopplerNoise{}; // [m/s] | |
| 1308 | ✗ | switch (_gui_dopplerNoiseUnit) | |
| 1309 | { | ||
| 1310 | ✗ | case DopplerNoiseUnits::m_s: | |
| 1311 | ✗ | dopplerNoise = _gui_dopplerNoise; | |
| 1312 | ✗ | break; | |
| 1313 | } | ||
| 1314 | |||
| 1315 | ✗ | double dtCycleSlipSeconds{}; // [s] | |
| 1316 | ✗ | switch (_gui_cycleSlipFrequencyUnit) | |
| 1317 | { | ||
| 1318 | ✗ | case CycleSlipFrequencyUnits::per_day: | |
| 1319 | ✗ | dtCycleSlipSeconds = InsTimeUtil::SECONDS_PER_DAY / _gui_cycleSlipFrequency; | |
| 1320 | ✗ | break; | |
| 1321 | ✗ | case CycleSlipFrequencyUnits::per_hour: | |
| 1322 | ✗ | dtCycleSlipSeconds = InsTimeUtil::SECONDS_PER_HOUR / _gui_cycleSlipFrequency; | |
| 1323 | ✗ | break; | |
| 1324 | ✗ | case CycleSlipFrequencyUnits::per_minute: | |
| 1325 | ✗ | dtCycleSlipSeconds = InsTimeUtil::SECONDS_PER_MINUTE / _gui_cycleSlipFrequency; | |
| 1326 | ✗ | break; | |
| 1327 | } | ||
| 1328 | ✗ | auto dtCycleSlip = std::chrono::nanoseconds(static_cast<int64_t>(dtCycleSlipSeconds * 1e9)); | |
| 1329 | |||
| 1330 | ✗ | double cycleSlipDetectionProbability{}; // [0, 1] | |
| 1331 | ✗ | switch (_gui_cycleSlipDetectionProbabilityUnit) | |
| 1332 | { | ||
| 1333 | ✗ | case CycleSlipDetectionProbabilityUnits::percent: | |
| 1334 | ✗ | cycleSlipDetectionProbability = _gui_cycleSlipDetectionProbability / 100.0; | |
| 1335 | ✗ | break; | |
| 1336 | } | ||
| 1337 | |||
| 1338 | ✗ | if (_cycleSlipWindowStartTime.empty() | |
| 1339 | ✗ | || gnssObs->insTime >= _cycleSlipWindowStartTime + dtCycleSlip) | |
| 1340 | { | ||
| 1341 | ✗ | _cycleSlipWindowStartTime = gnssObs->insTime; | |
| 1342 | LOG_DATA("{}: [{}] Starting new cycle-slip window", nameId(), _cycleSlipWindowStartTime.toYMDHMS(GPST)); | ||
| 1343 | } | ||
| 1344 | |||
| 1345 | ✗ | size_t nObs = 0; | |
| 1346 | ✗ | for (auto& obs : gnssObs->data) | |
| 1347 | { | ||
| 1348 | ✗ | if (obs.satSigId.freq() & _filterFreq | |
| 1349 | ✗ | && obs.satSigId.code & _filterCode) | |
| 1350 | { | ||
| 1351 | ✗ | nObs++; | |
| 1352 | } | ||
| 1353 | } | ||
| 1354 | |||
| 1355 | ✗ | for (auto& obs : gnssObs->data) | |
| 1356 | { | ||
| 1357 | ✗ | if (obs.pseudorange) { obs.pseudorange.value().value += _pseudorangeRng.getRand_normalDist(0.0, pseudorangeNoise); } | |
| 1358 | ✗ | if (obs.doppler) { obs.doppler.value() += rangeRate2doppler(_dopplerRng.getRand_normalDist(0.0, dopplerNoise), obs.satSigId.freq(), 0); } // TODO: Add frequency number here for GLONASS | |
| 1359 | |||
| 1360 | ✗ | if (obs.carrierPhase) | |
| 1361 | { | ||
| 1362 | // ------------------------------------------- Noise --------------------------------------------- | ||
| 1363 | ✗ | auto lambda = InsConst::C / obs.satSigId.freq().getFrequency(0); // wave-length [m] // TODO: Add frequency number here for GLONASS | |
| 1364 | ✗ | obs.carrierPhase.value().value += _carrierPhaseRng.getRand_normalDist(0.0, carrierPhaseNoise) / lambda; | |
| 1365 | |||
| 1366 | // ---------------------------------------- Cycle-slip ------------------------------------------- | ||
| 1367 | |||
| 1368 | ✗ | bool manualCycleSlip = false; | |
| 1369 | ✗ | for (const auto& cycleSlip : _manualCycleSlips) | |
| 1370 | { | ||
| 1371 | ✗ | const auto& [insTime, satSigId] = cycleSlip.first; | |
| 1372 | ✗ | if (std::abs((insTime - gnssObs->insTime).count()) < 1e-4 && obs.satSigId == satSigId) | |
| 1373 | { | ||
| 1374 | ✗ | manualCycleSlip = true; | |
| 1375 | |||
| 1376 | ✗ | const auto& [ambValue, setLLI] = cycleSlip.second; | |
| 1377 | ✗ | _ambiguities[obs.satSigId].emplace_back(gnssObs->insTime, ambValue); | |
| 1378 | ✗ | obs.carrierPhase.value().LLI = static_cast<uint8_t>(setLLI); | |
| 1379 | ✗ | _cycleSlips.push_back(CycleSlipInfo{ .time = gnssObs->insTime, .satSigId = obs.satSigId, .LLI = obs.carrierPhase.value().LLI != 0 }); | |
| 1380 | |||
| 1381 | ✗ | LOG_DEBUG("{}: [{}] Applied manual cycle-slip for satellite [{}] with LLI {}", nameId(), gnssObs->insTime.toYMDHMS(GPST), | |
| 1382 | obs.satSigId, obs.carrierPhase.value().LLI); | ||
| 1383 | } | ||
| 1384 | } | ||
| 1385 | |||
| 1386 | ✗ | if (!manualCycleSlip // If already manual cycle-slip applied do not do another | |
| 1387 | ✗ | && obs.satSigId.freq() & _filterFreq // GUI selected frequencies | |
| 1388 | ✗ | && obs.satSigId.code & _filterCode // GUI selected codes | |
| 1389 | ✗ | && _gui_cycleSlipFrequency != 0.0 // 0 Frequency means disabled | |
| 1390 | ✗ | && !_lastObservationTime.empty() // Do not apply a cycle slip on the first message | |
| 1391 | ✗ | && (_cycleSlips.empty() || _cycleSlips.back().time < _cycleSlipWindowStartTime)) // In the current window, there was no cycle-slip yet | |
| 1392 | { | ||
| 1393 | ✗ | double probabilityCycleSlip = _dt / (dtCycleSlipSeconds * static_cast<double>(nObs)) * 2.0; // Double chance, because often does not happen otherwise | |
| 1394 | ✗ | if (_cycleSlipRng.getRand_uniformRealDist(0.0, 1.0) <= probabilityCycleSlip | |
| 1395 | ✗ | || (gnssObs->insTime >= _cycleSlipWindowStartTime + dtCycleSlip - std::chrono::nanoseconds(static_cast<int64_t>((_dt + 0.001) * 1e9)))) // Last message this window | |
| 1396 | { | ||
| 1397 | ✗ | int newAmbiguity = 0; | |
| 1398 | ✗ | int oldAmbiguity = !_ambiguities[obs.satSigId].empty() | |
| 1399 | ✗ | ? _ambiguities[obs.satSigId].back().second | |
| 1400 | ✗ | : static_cast<int>(_ambiguityRng.getRand_uniformIntDist(_gui_ambiguityLimits[0], _gui_ambiguityLimits[1])); | |
| 1401 | ✗ | auto deltaAmbiguity = static_cast<int>(_cycleSlipRng.getRand_uniformIntDist(1, _gui_cycleSlipRange)); | |
| 1402 | ✗ | auto signAmbiguity = _cycleSlipRng.getRand_uniformIntDist(0, 1) == 0.0 ? 1 : -1; | |
| 1403 | |||
| 1404 | ✗ | if (_gui_ambiguityLimits[0] == _gui_ambiguityLimits[1]) // Ambiguities disabled | |
| 1405 | { | ||
| 1406 | ✗ | newAmbiguity = oldAmbiguity + signAmbiguity * deltaAmbiguity; | |
| 1407 | } | ||
| 1408 | ✗ | else if (oldAmbiguity == _gui_ambiguityLimits[0]) | |
| 1409 | { | ||
| 1410 | ✗ | newAmbiguity = std::min(oldAmbiguity + deltaAmbiguity, _gui_ambiguityLimits[1]); | |
| 1411 | } | ||
| 1412 | ✗ | else if (oldAmbiguity == _gui_ambiguityLimits[1]) | |
| 1413 | { | ||
| 1414 | ✗ | newAmbiguity = std::max(oldAmbiguity - deltaAmbiguity, _gui_ambiguityLimits[0]); | |
| 1415 | } | ||
| 1416 | else | ||
| 1417 | { | ||
| 1418 | ✗ | newAmbiguity = std::clamp(oldAmbiguity + signAmbiguity * deltaAmbiguity, _gui_ambiguityLimits[0], _gui_ambiguityLimits[1]); | |
| 1419 | } | ||
| 1420 | |||
| 1421 | ✗ | _ambiguities[obs.satSigId].emplace_back(gnssObs->insTime, newAmbiguity); | |
| 1422 | |||
| 1423 | ✗ | if (_cycleSlipRng.getRand_uniformRealDist(0.0, 1.0) <= cycleSlipDetectionProbability) | |
| 1424 | { | ||
| 1425 | ✗ | obs.carrierPhase.value().LLI = 1; | |
| 1426 | } | ||
| 1427 | ✗ | _cycleSlips.push_back(CycleSlipInfo{ .time = gnssObs->insTime, .satSigId = obs.satSigId, .LLI = obs.carrierPhase.value().LLI != 0 }); | |
| 1428 | ✗ | LOG_DEBUG("{}: [{}] Simulating cycle-slip for satellite [{}] with LLI {}", nameId(), gnssObs->insTime.toYMDHMS(GPST), | |
| 1429 | obs.satSigId, obs.carrierPhase.value().LLI); | ||
| 1430 | } | ||
| 1431 | } | ||
| 1432 | |||
| 1433 | // ----------------------------------------- Ambiguity ------------------------------------------- | ||
| 1434 | ✗ | if (!_ambiguities.contains(obs.satSigId) && _gui_ambiguityLimits[0] != _gui_ambiguityLimits[1]) | |
| 1435 | { | ||
| 1436 | ✗ | _ambiguities[obs.satSigId].emplace_back(gnssObs->insTime, _ambiguityRng.getRand_uniformIntDist(_gui_ambiguityLimits[0], _gui_ambiguityLimits[1])); | |
| 1437 | } | ||
| 1438 | ✗ | if (_ambiguities.contains(obs.satSigId)) | |
| 1439 | { | ||
| 1440 | ✗ | obs.carrierPhase.value().value += _ambiguities.at(obs.satSigId).back().second; | |
| 1441 | } | ||
| 1442 | } | ||
| 1443 | } | ||
| 1444 | |||
| 1445 | ✗ | return gnssObs; | |
| 1446 | } | ||
| 1447 |