0.4.1
Loading...
Searching...
No Matches
ImuSimulator.cpp
Go to the documentation of this file.
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 "ImuSimulator.hpp"
10
11#include <cmath>
12#include <ctime>
13#include <optional>
14
15#include "Navigation/Time/InsTime.hpp"
16#include "util/Logger.hpp"
17#include "Navigation/Ellipsoid/Ellipsoid.hpp"
18#include "Navigation/INS/Functions.hpp"
19#include "Navigation/INS/LocalNavFrame/Mechanization.hpp"
20#include "Navigation/Gravity/Gravity.hpp"
21#include "Navigation/Math/Math.hpp"
22#include "Navigation/Transformations/Units.hpp"
23
24#include "internal/NodeManager.hpp"
25#include <Eigen/src/Geometry/AngleAxis.h>
26#include <Eigen/src/Geometry/Quaternion.h>
27namespace nm = NAV::NodeManager;
28#include "internal/FlowManager.hpp"
29#include "internal/gui/widgets/imgui_ex.hpp"
30#include "internal/gui/widgets/HelpMarker.hpp"
31#include "internal/gui/NodeEditorApplication.hpp"
32
33#include "NodeData/IMU/ImuObsSimulated.hpp"
34#include "NodeData/State/PosVelAtt.hpp"
35
47
48NAV::ImuSimulator::~ImuSimulator()
49{
50 LOG_TRACE("{}: called", nameId());
51}
52
53std::string NAV::ImuSimulator::typeStatic()
54{
55 return "ImuSimulator";
56}
57
58std::string NAV::ImuSimulator::type() const
59{
60 return typeStatic();
61}
62
63std::string NAV::ImuSimulator::category()
64{
65 return "Data Simulator";
66}
67
68void NAV::ImuSimulator::guiConfig()
69{
70 float columnWidth = 140.0F * gui::NodeEditorApplication::windowFontRatio();
71
72 if (_trajectoryType != TrajectoryType::Csv && ImGui::TreeNode("Start Time"))
73 {
74 auto startTimeSource = static_cast<int>(_startTimeSource);
75 if (ImGui::RadioButton(fmt::format("Current Computer Time##{}", size_t(id)).c_str(), &startTimeSource, static_cast<int>(StartTimeSource::CurrentComputerTime)))
76 {
77 _startTimeSource = static_cast<decltype(_startTimeSource)>(startTimeSource);
78 LOG_DEBUG("{}: startTimeSource changed to {}", nameId(), fmt::underlying(_startTimeSource));
79 flow::ApplyChanges();
80 }
81 if (_startTimeSource == StartTimeSource::CurrentComputerTime)
82 {
83 ImGui::Indent();
84
85 std::time_t t = std::time(nullptr);
86 std::tm* now = std::localtime(&t); // NOLINT(concurrency-mt-unsafe)
87
88 ImGui::Text("%d-%02d-%02d %02d:%02d:%02d", now->tm_year + 1900, now->tm_mon + 1, now->tm_mday, now->tm_hour, now->tm_min, now->tm_sec);
89
90 ImGui::Unindent();
91 }
92
93 if (ImGui::RadioButton(fmt::format("Custom Time##{}", size_t(id)).c_str(), &startTimeSource, static_cast<int>(StartTimeSource::CustomTime)))
94 {
95 _startTimeSource = static_cast<decltype(_startTimeSource)>(startTimeSource);
96 LOG_DEBUG("{}: startTimeSource changed to {}", nameId(), fmt::underlying(_startTimeSource));
97 flow::ApplyChanges();
98 }
99 if (_startTimeSource == StartTimeSource::CustomTime)
100 {
101 ImGui::Indent();
102 if (gui::widgets::TimeEdit(fmt::format("{}", size_t(id)).c_str(), _startTime, _startTimeEditFormat))
103 {
104 LOG_DEBUG("{}: startTime changed to {}", nameId(), _startTime);
105 flow::ApplyChanges();
106 }
107 ImGui::Unindent();
108 }
109 ImGui::TreePop();
110 }
111
112 ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver);
113 if (ImGui::TreeNode("Output data-rate"))
114 {
115 ImGui::SetNextItemWidth(columnWidth);
116 if (ImGui::InputDoubleL(fmt::format("IMU internal rate##{}", size_t(id)).c_str(), &_imuInternalFrequency, 1e-3, 1e4, 0.0, 0.0, "%.3f Hz"))
117 {
118 LOG_DEBUG("{}: imuInternalFrequency changed to {}", nameId(), _imuInternalFrequency);
119 flow::ApplyChanges();
120 }
121 ImGui::SameLine();
122 gui::widgets::HelpMarker("Used to calculate the dTime, dTheta, dVel fields");
123 ImGui::SetNextItemWidth(columnWidth);
124 if (ImGui::InputDoubleL(fmt::format("IMU output rate##{}", size_t(id)).c_str(), &_imuFrequency, 1e-3, _imuInternalFrequency, 0.0, 0.0, "%.3f Hz"))
125 {
126 LOG_DEBUG("{}: imuFrequency changed to {}", nameId(), _imuFrequency);
127 flow::ApplyChanges();
128 }
129 ImGui::SetNextItemWidth(columnWidth);
130 if (ImGui::InputDouble(fmt::format("Trajectory output rate##{}", size_t(id)).c_str(), &_gnssFrequency, 0.0, 0.0, "%.3f Hz"))
131 {
132 LOG_DEBUG("{}: gnssFrequency changed to {}", nameId(), _gnssFrequency);
133 flow::ApplyChanges();
134 }
135
136 ImGui::TreePop();
137 }
138
139 ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver);
140 if (ImGui::TreeNode("Scenario"))
141 {
142 ImGui::SetNextItemWidth(columnWidth);
143 if (ImGui::BeginCombo(fmt::format("Trajectory##{}", size_t(id)).c_str(), to_string(_trajectoryType)))
144 {
145 for (size_t i = 0; i < static_cast<size_t>(TrajectoryType::COUNT); i++)
146 {
147 const bool is_selected = (static_cast<size_t>(_trajectoryType) == i);
148 if (ImGui::Selectable(to_string(static_cast<TrajectoryType>(i)), is_selected))
149 {
150 _trajectoryType = static_cast<TrajectoryType>(i);
151 LOG_DEBUG("{}: trajectoryType changed to {}", nameId(), fmt::underlying(_trajectoryType));
152
153 if (_trajectoryType == TrajectoryType::Csv && inputPins.empty())
154 {
155 nm::CreateInputPin(this, CsvData::type().c_str(), Pin::Type::Object, { CsvData::type() });
156 }
157 else if (_trajectoryType != TrajectoryType::Csv && !inputPins.empty())
158 {
159 nm::DeleteInputPin(inputPins.front());
160 }
161
162 flow::ApplyChanges();
163 doDeinitialize();
164 }
165
166 if (is_selected) // Set the initial focus when opening the combo (scrolling + keyboard navigation focus)
167 {
168 ImGui::SetItemDefaultFocus();
169 }
170 }
171 ImGui::EndCombo();
172 }
173 if (_trajectoryType == TrajectoryType::Csv)
174 {
175 auto TextColoredIfExists = [this](int index, const char* text, const char* type) {
176 ImGui::TableSetColumnIndex(index);
177 auto displayTable = [&]() {
178 if (auto csvData = getInputValue<CsvData>(INPUT_PORT_INDEX_CSV);
179 csvData && std::ranges::find(csvData->v->description, text) != csvData->v->description.end())
180 {
181 ImGui::TextUnformatted(text);
182 ImGui::TableNextColumn();
183 ImGui::TextUnformatted(type);
184 }
185 else
186 {
187 ImGui::TextDisabled("%s", text);
188 ImGui::TableNextColumn();
189 ImGui::TextDisabled("%s", type);
190 }
191 };
192 displayTable();
193 };
194
195 if (ImGui::TreeNode(fmt::format("Format description##{}", size_t(id)).c_str()))
196 {
197 ImGui::TextUnformatted("Time information:");
198 ImGui::SameLine();
199 gui::widgets::HelpMarker("You can either provide the time in GPS time or in UTC time.");
200 if (ImGui::BeginTable(fmt::format("##Time ({})", size_t(id)).c_str(), 5,
201 ImGuiTableFlags_Borders | ImGuiTableFlags_RowBg | ImGuiTableFlags_NoHostExtendX))
202 {
203 ImGui::TableSetupColumn("Field", ImGuiTableColumnFlags_WidthFixed);
204 ImGui::TableSetupColumn("Format", ImGuiTableColumnFlags_WidthFixed);
205 ImGui::TableSetupColumn("", ImGuiTableColumnFlags_WidthFixed);
206 ImGui::TableSetupColumn("Field", ImGuiTableColumnFlags_WidthFixed);
207 ImGui::TableSetupColumn("Format", ImGuiTableColumnFlags_WidthFixed);
208 ImGui::TableHeadersRow();
209
210 ImGui::TableNextRow();
211 TextColoredIfExists(0, "GpsCycle", "uint");
212 TextColoredIfExists(3, "YearUTC", "uint");
213 ImGui::TableNextRow();
214 TextColoredIfExists(0, "GpsWeek", "uint");
215 TextColoredIfExists(3, "MonthUTC", "uint");
216 ImGui::TableNextRow();
217 TextColoredIfExists(0, "GpsToW [s]", "uint");
218 TextColoredIfExists(3, "DayUTC", "uint");
219 ImGui::TableNextRow();
220 TextColoredIfExists(3, "HourUTC", "uint");
221 ImGui::TableNextRow();
222 TextColoredIfExists(3, "MinUTC", "uint");
223 ImGui::TableNextRow();
224 TextColoredIfExists(3, "SecUTC", "double");
225
226 ImGui::EndTable();
227 }
228
229 ImGui::TextUnformatted("Position information:");
230 ImGui::SameLine();
231 gui::widgets::HelpMarker("You can either provide the position in ECEF coordinates\nor in latitude, longitude and altitude.");
232 if (ImGui::BeginTable(fmt::format("##Position ({})", size_t(id)).c_str(), 5,
233 ImGuiTableFlags_Borders | ImGuiTableFlags_RowBg | ImGuiTableFlags_NoHostExtendX))
234 {
235 ImGui::TableSetupColumn("Field", ImGuiTableColumnFlags_WidthFixed);
236 ImGui::TableSetupColumn("Format", ImGuiTableColumnFlags_WidthFixed);
237 ImGui::TableSetupColumn("", ImGuiTableColumnFlags_WidthFixed);
238 ImGui::TableSetupColumn("Field", ImGuiTableColumnFlags_WidthFixed);
239 ImGui::TableSetupColumn("Format", ImGuiTableColumnFlags_WidthFixed);
240 ImGui::TableHeadersRow();
241
242 ImGui::TableNextRow();
243 TextColoredIfExists(0, "Pos ECEF X [m]", "double");
244 TextColoredIfExists(3, "Latitude [deg]", "double");
245 ImGui::TableNextRow();
246 TextColoredIfExists(0, "Pos ECEF Y [m]", "double");
247 TextColoredIfExists(3, "Longitude [deg]", "double");
248 ImGui::TableNextRow();
249 TextColoredIfExists(0, "Pos ECEF Z [m]", "double");
250 TextColoredIfExists(3, "Altitude [m]", "double");
251
252 ImGui::EndTable();
253 }
254
255 ImGui::TextUnformatted("Attitude information:");
256 ImGui::SameLine();
257 gui::widgets::HelpMarker("You can either provide the attitude as an angle or quaternion.");
258 if (ImGui::BeginTable(fmt::format("##Attitude ({})", size_t(id)).c_str(), 5,
259 ImGuiTableFlags_Borders | ImGuiTableFlags_RowBg | ImGuiTableFlags_NoHostExtendX))
260 {
261 ImGui::TableSetupColumn("Field", ImGuiTableColumnFlags_WidthFixed);
262 ImGui::TableSetupColumn("Format", ImGuiTableColumnFlags_WidthFixed);
263 ImGui::TableSetupColumn("", ImGuiTableColumnFlags_WidthFixed);
264 ImGui::TableSetupColumn("Field", ImGuiTableColumnFlags_WidthFixed);
265 ImGui::TableSetupColumn("Format", ImGuiTableColumnFlags_WidthFixed);
266 ImGui::TableHeadersRow();
267
268 ImGui::TableNextRow();
269 TextColoredIfExists(0, "Roll [deg]", "double");
270 TextColoredIfExists(3, "n_Quat_b w", "double");
271 ImGui::TableNextRow();
272 TextColoredIfExists(0, "Pitch [deg]", "double");
273 TextColoredIfExists(3, "n_Quat_b x", "double");
274 ImGui::TableNextRow();
275 TextColoredIfExists(0, "Yaw [deg]", "double");
276 TextColoredIfExists(3, "n_Quat_b y", "double");
277 ImGui::TableNextRow();
278 TextColoredIfExists(3, "n_Quat_b z", "double");
279
280 ImGui::EndTable();
281 }
282
283 ImGui::TreePop();
284 }
285 }
286 else
287 {
288 const auto* txt = _trajectoryType == TrajectoryType::Fixed
289 ? "Position"
290 : (_trajectoryType == TrajectoryType::Linear
291 ? "Start position"
292 : (_trajectoryType == TrajectoryType::Circular
293 ? "Center position"
294 : (_trajectoryType == TrajectoryType::RoseFigure
295 ? "Center/Tangential position"
296 : "")));
297
298 if (gui::widgets::PositionInput(fmt::format("{}##PosInput {}", txt, size_t(id)).c_str(), _startPosition, gui::widgets::PositionInputLayout::TWO_ROWS, columnWidth))
299 {
300 flow::ApplyChanges();
301 doDeinitialize();
302 }
303 }
304
305 if (_trajectoryType == TrajectoryType::Fixed)
306 {
307 ImGui::SetNextItemWidth(columnWidth);
308 double roll = rad2deg(_fixedTrajectoryStartOrientation.x());
309 if (ImGui::InputDoubleL(fmt::format("##Roll{}", size_t(id)).c_str(), &roll, -180, 180, 0.0, 0.0, "%.3f°"))
310 {
311 _fixedTrajectoryStartOrientation.x() = deg2rad(roll);
312 LOG_DEBUG("{}: roll changed to {}", nameId(), roll);
313 flow::ApplyChanges();
314 doDeinitialize();
315 }
316 ImGui::SameLine();
317 ImGui::SetNextItemWidth(columnWidth);
318 double pitch = rad2deg(_fixedTrajectoryStartOrientation.y());
319 if (ImGui::InputDoubleL(fmt::format("##Pitch{}", size_t(id)).c_str(), &pitch, -90, 90, 0.0, 0.0, "%.3f°"))
320 {
321 _fixedTrajectoryStartOrientation.y() = deg2rad(pitch);
322 LOG_DEBUG("{}: pitch changed to {}", nameId(), pitch);
323 flow::ApplyChanges();
324 doDeinitialize();
325 }
326 ImGui::SameLine();
327 ImGui::SetNextItemWidth(columnWidth);
328 double yaw = rad2deg(_fixedTrajectoryStartOrientation.z());
329 if (ImGui::InputDoubleL(fmt::format("##Yaw{}", size_t(id)).c_str(), &yaw, -180, 180, 0.0, 0.0, "%.3f°"))
330 {
331 _fixedTrajectoryStartOrientation.z() = deg2rad(yaw);
332 LOG_DEBUG("{}: yaw changed to {}", nameId(), yaw);
333 flow::ApplyChanges();
334 doDeinitialize();
335 }
336 ImGui::SameLine();
337 ImGui::SetCursorPosX(ImGui::GetCursorPosX() - ImGui::GetStyle().ItemSpacing.x + ImGui::GetStyle().ItemInnerSpacing.x);
338 ImGui::TextUnformatted("Orientation (Roll, Pitch, Yaw)");
339 }
340 else if (_trajectoryType == TrajectoryType::Linear)
341 {
342 ImGui::SetNextItemWidth(columnWidth);
343 if (ImGui::InputDouble(fmt::format("##North velocity{}", size_t(id)).c_str(), &_n_linearTrajectoryStartVelocity.x(), 0.0, 0.0, "%.3f m/s"))
344 {
345 LOG_DEBUG("{}: n_linearTrajectoryStartVelocity changed to {}", nameId(), _n_linearTrajectoryStartVelocity.transpose());
346 flow::ApplyChanges();
347 doDeinitialize();
348 }
349 ImGui::SameLine();
350 ImGui::SetNextItemWidth(columnWidth);
351 if (ImGui::InputDouble(fmt::format("##East velocity{}", size_t(id)).c_str(), &_n_linearTrajectoryStartVelocity.y(), 0.0, 0.0, "%.3f m/s"))
352 {
353 LOG_DEBUG("{}: n_linearTrajectoryStartVelocity changed to {}", nameId(), _n_linearTrajectoryStartVelocity.transpose());
354 flow::ApplyChanges();
355 doDeinitialize();
356 }
357 ImGui::SameLine();
358 ImGui::SetNextItemWidth(columnWidth);
359 if (ImGui::InputDouble(fmt::format("##Down velocity{}", size_t(id)).c_str(), &_n_linearTrajectoryStartVelocity.z(), 0.0, 0.0, "%.3f m/s"))
360 {
361 LOG_DEBUG("{}: n_linearTrajectoryStartVelocity changed to {}", nameId(), _n_linearTrajectoryStartVelocity.transpose());
362 flow::ApplyChanges();
363 doDeinitialize();
364 }
365 ImGui::SameLine();
366 ImGui::SetCursorPosX(ImGui::GetCursorPosX() - ImGui::GetStyle().ItemSpacing.x + ImGui::GetStyle().ItemInnerSpacing.x);
367 ImGui::TextUnformatted("Velocity (North, East, Down)");
368 }
369 else if (_trajectoryType == TrajectoryType::Circular || _trajectoryType == TrajectoryType::RoseFigure)
370 {
371 if (ImGui::BeginTable(fmt::format("CircularOrRoseTrajectory##{}", size_t(id)).c_str(), 2, ImGuiTableFlags_SizingFixedFit | ImGuiTableFlags_NoHostExtendX))
372 {
373 ImGui::TableNextColumn();
374 ImGui::SetNextItemWidth(columnWidth);
375 if (ImGui::InputDoubleL(fmt::format("{}##{}", _trajectoryType == TrajectoryType::Circular ? "Radius" : "Radius/Amplitude", size_t(id)).c_str(), &_trajectoryRadius, 1e-3, std::numeric_limits<double>::max(), 0.0, 0.0, "%.3f m"))
376 {
377 LOG_DEBUG("{}: circularTrajectoryRadius changed to {}", nameId(), _trajectoryRadius);
378 flow::ApplyChanges();
379 doDeinitialize();
380 }
381
382 ImGui::TableNextColumn();
383 ImGui::SetNextItemWidth(columnWidth);
384 if (ImGui::InputDouble(fmt::format("Horizontal speed##{}", size_t(id)).c_str(), &_trajectoryHorizontalSpeed, 0.0, 0.0, "%.3f m/s"))
385 {
386 LOG_DEBUG("{}: circularTrajectoryHorizontalSpeed changed to {}", nameId(), _trajectoryHorizontalSpeed);
387 flow::ApplyChanges();
388 doDeinitialize();
389 }
390 // ####################################################################################################
391 ImGui::TableNextColumn();
392 ImGui::SetNextItemWidth(columnWidth);
393 double originAngle = rad2deg(_trajectoryRotationAngle);
394 if (ImGui::DragDouble(fmt::format("{}##{}", _trajectoryType == TrajectoryType::Circular ? "Origin angle" : "Rotation angle", size_t(id)).c_str(), &originAngle, 15.0, -360.0, 360.0, "%.3f°"))
395 {
396 _trajectoryRotationAngle = deg2rad(originAngle);
397 LOG_DEBUG("{}: originAngle changed to {}", nameId(), originAngle);
398 flow::ApplyChanges();
399 doDeinitialize();
400 }
401
402 ImGui::TableNextColumn();
403 ImGui::SetNextItemWidth(columnWidth);
404 if (ImGui::InputDouble(fmt::format("Vertical speed (Up)##{}", size_t(id)).c_str(), &_trajectoryVerticalSpeed, 0.0, 0.0, "%.3f m/s"))
405 {
406 LOG_DEBUG("{}: circularTrajectoryVerticalSpeed changed to {}", nameId(), _trajectoryVerticalSpeed);
407 flow::ApplyChanges();
408 doDeinitialize();
409 }
410 // ####################################################################################################
411 ImGui::TableNextColumn();
412 auto tableStartX = ImGui::GetCursorPosX();
413 ImGui::SetNextItemWidth(200 * gui::NodeEditorApplication::windowFontRatio());
414 if (ImGui::BeginCombo(fmt::format("Motion##{}", size_t(id)).c_str(), to_string(_trajectoryDirection)))
415 {
416 for (size_t i = 0; i < static_cast<size_t>(Direction::COUNT); i++)
417 {
418 const bool is_selected = (static_cast<size_t>(_trajectoryDirection) == i);
419 if (ImGui::Selectable(to_string(static_cast<Direction>(i)), is_selected))
420 {
421 _trajectoryDirection = static_cast<Direction>(i);
422 LOG_DEBUG("{}: circularTrajectoryDirection changed to {}", nameId(), fmt::underlying(_trajectoryDirection));
423 flow::ApplyChanges();
424 doDeinitialize();
425 }
426
427 if (is_selected) // Set the initial focus when opening the combo (scrolling + keyboard navigation focus)
428 {
429 ImGui::SetItemDefaultFocus();
430 }
431 }
432 ImGui::EndCombo();
433 }
434 ImGui::SetCursorPosX(tableStartX + columnWidth * 2.0F + ImGui::GetStyle().ItemSpacing.x * 1.0F);
435
436 ImGui::TableNextColumn();
437 // ####################################################################################################
438 if (_trajectoryType == TrajectoryType::Circular)
439 {
440 ImGui::TableNextColumn();
441 ImGui::SetNextItemWidth(columnWidth);
442 if (ImGui::DragInt(fmt::format("Osc Frequency##{}", size_t(id)).c_str(), &_circularHarmonicFrequency, 1.0F, 0, 100, "%d [cycles/rev]"))
443 {
444 LOG_DEBUG("{}: circularHarmonicFrequency changed to {}", nameId(), _circularHarmonicFrequency);
445 flow::ApplyChanges();
446 doDeinitialize();
447 }
448 ImGui::SameLine();
449 gui::widgets::HelpMarker("This modulates a harmonic oscillation on the circular path.\n"
450 "The frequency is in units [cycles per revolution].");
451
452 ImGui::TableNextColumn();
453 ImGui::SetNextItemWidth(columnWidth);
454 if (ImGui::DragDouble(fmt::format("Osc Amplitude Factor##{}", size_t(id)).c_str(), &_circularHarmonicAmplitudeFactor, 0.01F, 0.0, 10.0, "%.3f * r"))
455 {
456 LOG_DEBUG("{}: circularHarmonicAmplitudeFactor changed to {}", nameId(), _circularHarmonicAmplitudeFactor);
457 flow::ApplyChanges();
458 doDeinitialize();
459 }
460 ImGui::SameLine();
461 gui::widgets::HelpMarker("This modulates a harmonic oscillation on the circular path.\n"
462 "This factor determines the amplitude of the oscillation\n"
463 "with respect to the radius of the circle.");
464 }
465 else if (_trajectoryType == TrajectoryType::RoseFigure)
466 {
467 ImGui::TableNextColumn();
468 ImGui::SetNextItemWidth(columnWidth);
469 if (ImGui::InputIntL(fmt::format("n (angular freq.)##{}", size_t(id)).c_str(), &_rosePetNum, 1.0, std::numeric_limits<int>::max(), 1, 1))
470 {
471 LOG_DEBUG("{}: Rose figure numerator changed to {}", nameId(), _rosePetNum);
472 flow::ApplyChanges();
473 doDeinitialize();
474 }
475
476 ImGui::TableNextColumn();
477 ImGui::SetNextItemWidth(columnWidth);
478 if (ImGui::InputIntL(fmt::format("d (angular freq.)##{}", size_t(id)).c_str(), &_rosePetDenom, 1.0, std::numeric_limits<int>::max(), 1, 1))
479 {
480 LOG_DEBUG("{}: Rose figure denominator changed to {}", nameId(), _rosePetDenom);
481 flow::ApplyChanges();
482 doDeinitialize();
483 }
484 ImGui::SameLine();
485 if (gui::widgets::BeginHelpMarker())
486 {
487 ImGui::TextUnformatted("Angular frequency k=n/d, n,d = natural numbers. In case of d=1,\n"
488 "for even k number of petals is 2*k, for odd k, number of petals is k.\n"
489 "Adjusts the integration limits of the incomplete elliptical integral\n"
490 "of the second kind. If k is rational (d >1), the correct limit needs \n"
491 "to be choosen depending on the symmetry (petals) of the figure.");
492 float width = 500.0F;
493 ImGui::Image(gui::NodeEditorApplication::m_RoseFigure, ImVec2(width, width * 909.0F / 706.0F));
494 ImGui::TextUnformatted("Graphic by Jason Davies, CC BY-SA 3.0,\nhttps://commons.wikimedia.org/w/index.php?curid=30515231");
495 gui::widgets::EndHelpMarker();
496 }
497 }
498 // ####################################################################################################
499
500 ImGui::EndTable();
501 }
502 }
503 ImGui::TreePop();
504 }
505
506 ImGui::SetNextItemOpen(true, ImGuiCond_FirstUseEver);
507 if (_trajectoryType != TrajectoryType::Csv && ImGui::TreeNode("Simulation Stop Condition"))
508 {
509 auto simulationStopCondition = static_cast<int>(_simulationStopCondition);
510 if (_trajectoryType != TrajectoryType::Fixed)
511 {
512 if (ImGui::RadioButton(fmt::format("##simulationStopConditionDuration{}", size_t(id)).c_str(), &simulationStopCondition, static_cast<int>(StopCondition::Duration)))
513 {
514 _simulationStopCondition = static_cast<decltype(_simulationStopCondition)>(simulationStopCondition);
515 LOG_DEBUG("{}: simulationStopCondition changed to {}", nameId(), fmt::underlying(_simulationStopCondition));
516 flow::ApplyChanges();
517 doDeinitialize();
518 }
519 ImGui::SameLine();
520 }
521
522 ImGui::SetNextItemWidth(columnWidth);
523 if (ImGui::InputDoubleL(fmt::format("Duration##{}", size_t(id)).c_str(), &_simulationDuration, 0.0, std::numeric_limits<double>::max(), 0.0, 0.0, "%.3f s"))
524 {
525 LOG_DEBUG("{}: simulationDuration changed to {}", nameId(), _simulationDuration);
526 flow::ApplyChanges();
527 doDeinitialize();
528 }
529 if (_trajectoryType != TrajectoryType::Fixed)
530 {
531 if (ImGui::RadioButton(fmt::format("##simulationStopConditionDistanceOrCirclesOrRoses{}", size_t(id)).c_str(), &simulationStopCondition, static_cast<int>(StopCondition::DistanceOrCirclesOrRoses)))
532 {
533 _simulationStopCondition = static_cast<decltype(_simulationStopCondition)>(simulationStopCondition);
534 LOG_DEBUG("{}: simulationStopCondition changed to {}", nameId(), fmt::underlying(_simulationStopCondition));
535 flow::ApplyChanges();
536 doDeinitialize();
537 }
538 ImGui::SameLine();
539 }
540
541 if (_trajectoryType == TrajectoryType::Linear)
542 {
543 ImGui::SetNextItemWidth(columnWidth);
544 if (ImGui::InputDoubleL(fmt::format("Distance to start##{}", size_t(id)).c_str(), &_linearTrajectoryDistanceForStop, 0.0, std::numeric_limits<double>::max(), 0.0, 0.0, "%.3f m"))
545 {
546 LOG_DEBUG("{}: linearTrajectoryDistanceForStop changed to {}", nameId(), _linearTrajectoryDistanceForStop);
547 flow::ApplyChanges();
548 doDeinitialize();
549 }
550 }
551 else if (_trajectoryType == TrajectoryType::Circular)
552 {
553 ImGui::SetNextItemWidth(columnWidth);
554 if (ImGui::InputDoubleL(fmt::format("Number of Circles##{}", size_t(id)).c_str(), &_circularTrajectoryCircleCountForStop, 0.0, std::numeric_limits<double>::max(), 1.0, 1.0, "%.3f"))
555 {
556 LOG_DEBUG("{}: circularTrajectoryCircleCountForStop changed to {}", nameId(), _circularTrajectoryCircleCountForStop);
557 flow::ApplyChanges();
558 doDeinitialize();
559 }
560 }
561 else if (_trajectoryType == TrajectoryType::RoseFigure)
562 {
563 ImGui::SetNextItemWidth(columnWidth);
564 if (ImGui::InputDoubleL(fmt::format("Number of rose figures##{}", size_t(id)).c_str(), &_roseTrajectoryCountForStop, 0.0, std::numeric_limits<double>::max(), 1.0, 1.0, "%.3f"))
565 {
566 LOG_DEBUG("{}: RoseTrajectoryCountForStop changed to {}", nameId(), _roseTrajectoryCountForStop);
567 flow::ApplyChanges();
568 doDeinitialize();
569 }
570 }
571 ImGui::TreePop();
572 }
573
574 ImGui::SetNextItemOpen(false, ImGuiCond_FirstUseEver);
575 if (ImGui::TreeNode("Simulation models"))
576 {
577 if (_trajectoryType != TrajectoryType::Fixed && _trajectoryType != TrajectoryType::Csv && _trajectoryType != TrajectoryType::RoseFigure)
578 {
579 ImGui::TextUnformatted(fmt::format("Spline (current knots {})", _splines.x.size()).c_str());
580 {
581 ImGui::Indent();
582 ImGui::SetNextItemWidth(columnWidth - ImGui::GetStyle().IndentSpacing);
583 if (ImGui::InputDoubleL(fmt::format("Sample Interval##{}", size_t(id)).c_str(), &_splines.sampleInterval, 0.0, std::numeric_limits<double>::max(), 0.0, 0.0, "%.3e s"))
584 {
585 LOG_DEBUG("{}: spline sample interval changed to {}", nameId(), _splines.sampleInterval);
586 flow::ApplyChanges();
587 doDeinitialize();
588 }
589 ImGui::Unindent();
590 }
591 }
592 else if (_trajectoryType == TrajectoryType::RoseFigure)
593 {
594 ImGui::TextUnformatted(fmt::format("Spline (current knots {})", _splines.x.size()).c_str());
595 {
596 ImGui::Indent();
597 ImGui::SetNextItemWidth(columnWidth - ImGui::GetStyle().IndentSpacing);
598 if (ImGui::InputDoubleL(fmt::format("Sample Distance##{}", size_t(id)).c_str(), &_roseStepLengthMax, 0.0, std::numeric_limits<double>::max(), 0.0, 0.0, "%.3e m"))
599 {
600 LOG_DEBUG("{}: Spline sample distance (rose figure) changed to {}", nameId(), _roseStepLengthMax);
601 flow::ApplyChanges();
602 doDeinitialize();
603 }
604 ImGui::Unindent();
605 }
606 }
607 ImGui::TextUnformatted("Measured acceleration");
608 {
609 ImGui::Indent();
610 ImGui::SetNextItemWidth(columnWidth - ImGui::GetStyle().IndentSpacing);
611
612 if (ComboGravitationModel(fmt::format("Gravitation Model##{}", size_t(id)).c_str(), _gravitationModel))
613 {
614 LOG_DEBUG("{}: Gravitation Model changed to {}", nameId(), NAV::to_string(_gravitationModel));
615 flow::ApplyChanges();
616 }
617 if (ImGui::Checkbox(fmt::format("Coriolis acceleration##{}", size_t(id)).c_str(), &_coriolisAccelerationEnabled))
618 {
619 LOG_DEBUG("{}: coriolisAccelerationEnabled changed to {}", nameId(), _coriolisAccelerationEnabled);
620 flow::ApplyChanges();
621 }
622 if (ImGui::Checkbox(fmt::format("Centrifugal acceleration##{}", size_t(id)).c_str(), &_centrifgalAccelerationEnabled))
623 {
624 LOG_DEBUG("{}: centrifgalAccelerationEnabled changed to {}", nameId(), _centrifgalAccelerationEnabled);
625 flow::ApplyChanges();
626 }
627 ImGui::Unindent();
628 }
629 ImGui::TextUnformatted("Measured angular rates");
630 {
631 ImGui::Indent();
632 if (ImGui::Checkbox(fmt::format("Earth rotation rate##{}", size_t(id)).c_str(), &_angularRateEarthRotationEnabled))
633 {
634 LOG_DEBUG("{}: angularRateEarthRotationEnabled changed to {}", nameId(), _angularRateEarthRotationEnabled);
635 flow::ApplyChanges();
636 }
637 if (ImGui::Checkbox(fmt::format("Transport rate##{}", size_t(id)).c_str(), &_angularRateTransportRateEnabled))
638 {
639 LOG_DEBUG("{}: angularRateTransportRateEnabled changed to {}", nameId(), _angularRateTransportRateEnabled);
640 flow::ApplyChanges();
641 }
642 ImGui::Unindent();
643 }
644
645 ImGui::TreePop();
646 }
647
648 Imu::guiConfig();
649}
650
651json NAV::ImuSimulator::save() const
652{
653 LOG_TRACE("{}: called", nameId());
654
655 json j;
656
657 j["startTimeSource"] = _startTimeSource;
658 j["startTimeEditFormat"] = _startTimeEditFormat;
659 j["startTime"] = _startTime;
660 // ###########################################################################################################
661 j["imuInternalFrequency"] = _imuInternalFrequency;
662 j["imuFrequency"] = _imuFrequency;
663 j["gnssFrequency"] = _gnssFrequency;
664 // ###########################################################################################################
665 j["trajectoryType"] = _trajectoryType;
666 j["startPosition"] = _startPosition;
667 j["fixedTrajectoryStartOrientation"] = _fixedTrajectoryStartOrientation;
668 j["n_linearTrajectoryStartVelocity"] = _n_linearTrajectoryStartVelocity;
669 j["circularHarmonicFrequency"] = _circularHarmonicFrequency;
670 j["circularHarmonicAmplitudeFactor"] = _circularHarmonicAmplitudeFactor;
671 j["trajectoryHorizontalSpeed"] = _trajectoryHorizontalSpeed;
672 j["trajectoryVerticalSpeed"] = _trajectoryVerticalSpeed;
673 j["trajectoryRadius"] = _trajectoryRadius;
674 j["trajectoryRotationAngle"] = _trajectoryRotationAngle;
675 j["trajectoryDirection"] = _trajectoryDirection;
676 j["rosePetNum"] = _rosePetNum;
677 j["rosePetDenom"] = _rosePetDenom;
678 // ###########################################################################################################
679 j["simulationStopCondition"] = _simulationStopCondition;
680 j["simulationDuration"] = _simulationDuration;
681 j["linearTrajectoryDistanceForStop"] = _linearTrajectoryDistanceForStop;
682 j["circularTrajectoryCircleCountForStop"] = _circularTrajectoryCircleCountForStop;
683 j["roseTrajectoryCountForStop"] = _roseTrajectoryCountForStop;
684 // ###########################################################################################################
685 j["splineSampleInterval"] = _splines.sampleInterval;
686 j["roseStepLengthMax"] = _roseStepLengthMax;
687 j["gravitationModel"] = _gravitationModel;
688 j["coriolisAccelerationEnabled"] = _coriolisAccelerationEnabled;
689 j["centrifgalAccelerationEnabled"] = _centrifgalAccelerationEnabled;
690 j["angularRateEarthRotationEnabled"] = _angularRateEarthRotationEnabled;
691 j["angularRateTransportRateEnabled"] = _angularRateTransportRateEnabled;
692 // ###########################################################################################################
693 j["Imu"] = Imu::save();
694
695 return j;
696}
697
698void NAV::ImuSimulator::restore(json const& j)
699{
700 LOG_TRACE("{}: called", nameId());
701
702 if (j.contains("startTimeSource"))
703 {
704 j.at("startTimeSource").get_to(_startTimeSource);
705 }
706 if (j.contains("startTimeEditFormat"))
707 {
708 j.at("startTimeEditFormat").get_to(_startTimeEditFormat);
709 }
710 if (j.contains("startTime"))
711 {
712 j.at("startTime").get_to(_startTime);
713 }
714 // ###########################################################################################################
715 if (j.contains("imuInternalFrequency"))
716 {
717 j.at("imuInternalFrequency").get_to(_imuInternalFrequency);
718 }
719 if (j.contains("imuFrequency"))
720 {
721 j.at("imuFrequency").get_to(_imuFrequency);
722 }
723 if (j.contains("gnssFrequency"))
724 {
725 j.at("gnssFrequency").get_to(_gnssFrequency);
726 }
727 // ###########################################################################################################
728 if (j.contains("trajectoryType"))
729 {
730 j.at("trajectoryType").get_to(_trajectoryType);
731
732 if (_trajectoryType == TrajectoryType::Csv && inputPins.empty())
733 {
734 nm::CreateInputPin(this, CsvData::type().c_str(), Pin::Type::Object, { CsvData::type() });
735 }
736 else if (_trajectoryType != TrajectoryType::Csv && !inputPins.empty())
737 {
738 nm::DeleteInputPin(inputPins.front());
739 }
740 }
741 if (j.contains("startPosition"))
742 {
743 j.at("startPosition").get_to(_startPosition);
744 }
745 if (j.contains("fixedTrajectoryStartOrientation"))
746 {
747 j.at("fixedTrajectoryStartOrientation").get_to(_fixedTrajectoryStartOrientation);
748 }
749 if (j.contains("n_linearTrajectoryStartVelocity"))
750 {
751 j.at("n_linearTrajectoryStartVelocity").get_to(_n_linearTrajectoryStartVelocity);
752 }
753 if (j.contains("circularHarmonicFrequency"))
754 {
755 j.at("circularHarmonicFrequency").get_to(_circularHarmonicFrequency);
756 }
757 if (j.contains("circularHarmonicAmplitudeFactor"))
758 {
759 j.at("circularHarmonicAmplitudeFactor").get_to(_circularHarmonicAmplitudeFactor);
760 }
761 if (j.contains("trajectoryHorizontalSpeed"))
762 {
763 j.at("trajectoryHorizontalSpeed").get_to(_trajectoryHorizontalSpeed);
764 }
765 if (j.contains("trajectoryVerticalSpeed"))
766 {
767 j.at("trajectoryVerticalSpeed").get_to(_trajectoryVerticalSpeed);
768 }
769 if (j.contains("trajectoryRadius"))
770 {
771 j.at("trajectoryRadius").get_to(_trajectoryRadius);
772 }
773 if (j.contains("trajectoryRotationAngle"))
774 {
775 j.at("trajectoryRotationAngle").get_to(_trajectoryRotationAngle);
776 }
777 if (j.contains("trajectoryDirection"))
778 {
779 j.at("trajectoryDirection").get_to(_trajectoryDirection);
780 }
781 if (j.contains("rosePetNum"))
782 {
783 j.at("rosePetNum").get_to(_rosePetNum);
784 }
785 if (j.contains("rosePetDenom"))
786 {
787 j.at("rosePetDenom").get_to(_rosePetDenom);
788 }
789 // ###########################################################################################################
790 if (j.contains("simulationStopCondition"))
791 {
792 j.at("simulationStopCondition").get_to(_simulationStopCondition);
793 }
794 if (j.contains("simulationDuration"))
795 {
796 j.at("simulationDuration").get_to(_simulationDuration);
797 }
798 if (j.contains("linearTrajectoryDistanceForStop"))
799 {
800 j.at("linearTrajectoryDistanceForStop").get_to(_linearTrajectoryDistanceForStop);
801 }
802 if (j.contains("circularTrajectoryCircleCountForStop"))
803 {
804 j.at("circularTrajectoryCircleCountForStop").get_to(_circularTrajectoryCircleCountForStop);
805 }
806 if (j.contains("roseTrajectoryCountForStop"))
807 {
808 j.at("roseTrajectoryCountForStop").get_to(_roseTrajectoryCountForStop);
809 }
810 // ###########################################################################################################
811 if (j.contains("splineSampleInterval"))
812 {
813 j.at("splineSampleInterval").get_to(_splines.sampleInterval);
814 }
815 if (j.contains("roseStepLengthMax"))
816 {
817 j.at("roseStepLengthMax").get_to(_roseStepLengthMax);
818 }
819 if (j.contains("gravitationModel"))
820 {
821 j.at("gravitationModel").get_to(_gravitationModel);
822 }
823 if (j.contains("coriolisAccelerationEnabled"))
824 {
825 j.at("coriolisAccelerationEnabled").get_to(_coriolisAccelerationEnabled);
826 }
827 if (j.contains("centrifgalAccelerationEnabled"))
828 {
829 j.at("centrifgalAccelerationEnabled").get_to(_centrifgalAccelerationEnabled);
830 }
831 if (j.contains("angularRateEarthRotationEnabled"))
832 {
833 j.at("angularRateEarthRotationEnabled").get_to(_angularRateEarthRotationEnabled);
834 }
835 if (j.contains("angularRateTransportRateEnabled"))
836 {
837 j.at("angularRateTransportRateEnabled").get_to(_angularRateTransportRateEnabled);
838 }
839 // ###########################################################################################################
840 if (j.contains("Imu"))
841 {
842 Imu::restore(j.at("Imu"));
843 }
844}
845
846std::optional<NAV::InsTime> NAV::ImuSimulator::getTimeFromCsvLine(const CsvData::CsvLine& line, const std::vector<std::string>& description) const // NOLINT(readability-convert-member-functions-to-static)
847{
848 auto gpsCycleIter = std::ranges::find(description, "GpsCycle");
849 auto gpsWeekIter = std::ranges::find(description, "GpsWeek");
850 auto gpsTowIter = std::ranges::find(description, "GpsToW [s]");
851 if (gpsCycleIter != description.end() && gpsWeekIter != description.end() && gpsTowIter != description.end())
852 {
853 const auto* gpsCycle = std::get_if<double>(&line.at(static_cast<size_t>(gpsCycleIter - description.begin())));
854 const auto* gpsWeek = std::get_if<double>(&line.at(static_cast<size_t>(gpsWeekIter - description.begin())));
855 const auto* gpsTow = std::get_if<double>(&line.at(static_cast<size_t>(gpsTowIter - description.begin())));
856 if (gpsCycle && gpsWeek && gpsTow)
857 {
858 return InsTime{ static_cast<int32_t>(*gpsCycle), static_cast<int32_t>(*gpsWeek), *gpsTow };
859 }
860 }
861 else
862 {
863 auto yearUTCIter = std::ranges::find(description, "YearUTC");
864 auto monthUTCIter = std::ranges::find(description, "MonthUTC");
865 auto dayUTCIter = std::ranges::find(description, "DayUTC");
866 auto hourUTCIter = std::ranges::find(description, "HourUTC");
867 auto minUTCIter = std::ranges::find(description, "MinUTC");
868 auto secUTCIter = std::ranges::find(description, "SecUTC");
869 if (yearUTCIter != description.end() && monthUTCIter != description.end() && dayUTCIter != description.end()
870 && hourUTCIter != description.end() && minUTCIter != description.end() && secUTCIter != description.end())
871 {
872 const auto* yearUTC = std::get_if<double>(&line.at(static_cast<size_t>(yearUTCIter - description.begin())));
873 const auto* monthUTC = std::get_if<double>(&line.at(static_cast<size_t>(monthUTCIter - description.begin())));
874 const auto* dayUTC = std::get_if<double>(&line.at(static_cast<size_t>(dayUTCIter - description.begin())));
875 const auto* hourUTC = std::get_if<double>(&line.at(static_cast<size_t>(hourUTCIter - description.begin())));
876 const auto* minUTC = std::get_if<double>(&line.at(static_cast<size_t>(minUTCIter - description.begin())));
877 const auto* secUTC = std::get_if<double>(&line.at(static_cast<size_t>(secUTCIter - description.begin())));
878 if (yearUTC && monthUTC && dayUTC && hourUTC && minUTC && secUTC)
879 {
880 return InsTime{
881 static_cast<uint16_t>(*yearUTC), static_cast<uint16_t>(*monthUTC), static_cast<uint16_t>(*dayUTC),
882 static_cast<uint16_t>(*hourUTC), static_cast<uint16_t>(*minUTC), *secUTC, UTC
883 };
884 }
885 }
886 }
887
888 LOG_ERROR("{}: Could not find the necessary columns in the CSV file to determine the time.", nameId());
889 return std::nullopt;
890}
891
892std::optional<Eigen::Vector3d> NAV::ImuSimulator::e_getPositionFromCsvLine(const CsvData::CsvLine& line, const std::vector<std::string>& description) const // NOLINT(readability-convert-member-functions-to-static)
893{
894 auto posXIter = std::ranges::find(description, "Pos ECEF X [m]");
895 auto posYIter = std::ranges::find(description, "Pos ECEF Y [m]");
896 auto posZIter = std::ranges::find(description, "Pos ECEF Z [m]");
897 if (posXIter != description.end() && posYIter != description.end() && posZIter != description.end())
898 {
899 const auto* posX = std::get_if<double>(&line.at(static_cast<size_t>(posXIter - description.begin())));
900 const auto* posY = std::get_if<double>(&line.at(static_cast<size_t>(posYIter - description.begin())));
901 const auto* posZ = std::get_if<double>(&line.at(static_cast<size_t>(posZIter - description.begin())));
902 if (posX && posY && posZ && !std::isnan(*posX) && !std::isnan(*posY) && !std::isnan(*posZ))
903 {
904 return Eigen::Vector3d{ *posX, *posY, *posZ };
905 }
906 }
907 else
908 {
909 auto latIter = std::ranges::find(description, "Latitude [deg]");
910 auto lonIter = std::ranges::find(description, "Longitude [deg]");
911 auto altIter = std::ranges::find(description, "Altitude [m]");
912 if (latIter != description.end() && lonIter != description.end() && altIter != description.end())
913 {
914 const auto* lat = std::get_if<double>(&line.at(static_cast<size_t>(latIter - description.begin())));
915 const auto* lon = std::get_if<double>(&line.at(static_cast<size_t>(lonIter - description.begin())));
916 const auto* alt = std::get_if<double>(&line.at(static_cast<size_t>(altIter - description.begin())));
917 if (lat && lon && alt && !std::isnan(*lat) && !std::isnan(*lon) && !std::isnan(*alt))
918 {
919 return trafo::lla2ecef_WGS84(Eigen::Vector3d(deg2rad(*lat), deg2rad(*lon), *alt));
920 }
921 }
922 }
923
924 LOG_ERROR("{}: Could not find the necessary columns in the CSV file to determine the position.", nameId());
925 return std::nullopt;
926}
927
928std::optional<Eigen::Quaterniond> NAV::ImuSimulator::n_getAttitudeQuaternionFromCsvLine_b(const CsvData::CsvLine& line, const std::vector<std::string>& description) const
929{
930 auto rollIter = std::ranges::find(description, "Roll [deg]");
931 auto pitchIter = std::ranges::find(description, "Pitch [deg]");
932 auto yawIter = std::ranges::find(description, "Yaw [deg]");
933 if (rollIter != description.end() && pitchIter != description.end() && yawIter != description.end())
934 {
935 const auto* roll = std::get_if<double>(&line.at(static_cast<size_t>(rollIter - description.begin())));
936 const auto* pitch = std::get_if<double>(&line.at(static_cast<size_t>(pitchIter - description.begin())));
937 const auto* yaw = std::get_if<double>(&line.at(static_cast<size_t>(yawIter - description.begin())));
938 if (roll && pitch && yaw && !std::isnan(*roll) && !std::isnan(*pitch) && !std::isnan(*yaw))
939 {
940 return trafo::n_Quat_b(deg2rad(*roll), deg2rad(*pitch), deg2rad(*yaw));
941 }
942 }
943 else
944 {
945 auto quatWIter = std::ranges::find(description, "n_Quat_b w");
946 auto quatXIter = std::ranges::find(description, "n_Quat_b x");
947 auto quatYIter = std::ranges::find(description, "n_Quat_b y");
948 auto quatZIter = std::ranges::find(description, "n_Quat_b z");
949 if (quatWIter != description.end() && quatXIter != description.end() && quatYIter != description.end() && quatZIter != description.end())
950 {
951 const auto* w = std::get_if<double>(&line.at(static_cast<size_t>(quatWIter - description.begin())));
952 const auto* x = std::get_if<double>(&line.at(static_cast<size_t>(quatXIter - description.begin())));
953 const auto* y = std::get_if<double>(&line.at(static_cast<size_t>(quatYIter - description.begin())));
954 const auto* z = std::get_if<double>(&line.at(static_cast<size_t>(quatZIter - description.begin())));
955 if (w && x && y && z && !std::isnan(*w) && !std::isnan(*x) && !std::isnan(*y) && !std::isnan(*z))
956 {
957 return Eigen::Quaterniond{ *w, *x, *y, *z };
958 }
959 }
960 }
961
962 LOG_ERROR("{}: Could not find the necessary columns in the CSV file to determine the attitude.", nameId());
963 return std::nullopt;
964}
965
966bool NAV::ImuSimulator::initializeSplines()
967{
968 std::vector<long double> splineTime;
969
970 auto unwrapAngle = [](auto angle, auto prevAngle, auto rangeMax) {
971 auto x = angle - prevAngle;
972 x = std::fmod(x + rangeMax, 2 * rangeMax);
973 if (x < 0)
974 {
975 x += 2 * rangeMax;
976 }
977 x -= rangeMax;
978
979 return prevAngle + x;
980 };
981
982 if (_trajectoryType == TrajectoryType::Fixed)
983 {
984 splineTime.push_back(-30.0); // 10 seconds in the past; simply to define the derivative at zero seconds
985 splineTime.push_back(-20.0); // 10 seconds in the past; simply to define the derivative at zero seconds
986 splineTime.push_back(-10.0); // 10 seconds in the past; simply to define the derivative at zero seconds
987 splineTime.push_back(0.0);
988 splineTime.push_back(_simulationDuration);
989 splineTime.push_back(_simulationDuration + 10.0); // 10 seconds past simulation end; simply to define the derivative at end node
990 splineTime.push_back(_simulationDuration + 20.0); // 10 seconds past simulation end; simply to define the derivative at end node
991 splineTime.push_back(_simulationDuration + 30.0); // 10 seconds past simulation end; simply to define the derivative at end node
992
993 Eigen::Vector3d e_startPosition = _startPosition.e_position.cast<double>();
994 std::vector<long double> X(splineTime.size(), e_startPosition[0]);
995 std::vector<long double> Y(splineTime.size(), e_startPosition[1]);
996 std::vector<long double> Z(splineTime.size(), e_startPosition[2]);
997 std::vector<long double> Roll(splineTime.size(), _fixedTrajectoryStartOrientation.x());
998 std::vector<long double> Pitch(splineTime.size(), _fixedTrajectoryStartOrientation.y());
999 std::vector<long double> Yaw(splineTime.size(), _fixedTrajectoryStartOrientation.z());
1000
1001 _splines.x.setPoints(splineTime, X);
1002 _splines.y.setPoints(splineTime, Y);
1003 _splines.z.setPoints(splineTime, Z);
1004
1005 _splines.roll.setPoints(splineTime, Roll);
1006 _splines.pitch.setPoints(splineTime, Pitch);
1007 _splines.yaw.setPoints(splineTime, Yaw);
1008 }
1009 else if (_trajectoryType == TrajectoryType::Linear)
1010 {
1011 double roll = 0.0;
1012 double pitch = _n_linearTrajectoryStartVelocity.head<2>().norm() > 1e-8 ? calcPitchFromVelocity(_n_linearTrajectoryStartVelocity) : 0;
1013 double yaw = _n_linearTrajectoryStartVelocity.head<2>().norm() > 1e-8 ? calcYawFromVelocity(_n_linearTrajectoryStartVelocity) : 0;
1014 const auto& e_startPosition = _startPosition.e_position;
1015
1016 size_t nOverhead = static_cast<size_t>(std::round(1.0 / _splines.sampleInterval)) + 1;
1017
1018 splineTime = std::vector<long double>(nOverhead, 0.0);
1019 std::vector<long double> splineX(nOverhead, e_startPosition[0]);
1020 std::vector<long double> splineY(nOverhead, e_startPosition[1]);
1021 std::vector<long double> splineZ(nOverhead, e_startPosition[2]);
1022 std::vector<long double> splineRoll(nOverhead, roll);
1023 std::vector<long double> splinePitch(nOverhead, pitch);
1024 std::vector<long double> splineYaw(nOverhead, yaw);
1025
1026 // @brief Calculates the derivative of the curvilinear position and velocity
1027 // @param[in] y [𝜙, λ, h, v_N, v_E, v_D]^T Latitude, longitude, altitude, velocity NED in [rad, rad, m, m/s, m/s, m/s]
1028 // @param[in] n_acceleration Acceleration in local-navigation frame coordinates [m/s^s]
1029 // @return The curvilinear position and velocity derivatives ∂/∂t [𝜙, λ, h, v_N, v_E, v_D]^T
1030 auto f = [](const Eigen::Vector<double, 6>& y, const Eigen::Vector3d& n_acceleration) {
1031 Eigen::Vector<double, 6> y_dot;
1032 y_dot << lla_calcTimeDerivativeForPosition(y.tail<3>(), // Velocity with respect to the Earth in local-navigation frame coordinates [m/s]
1033 y(0), // 𝜙 Latitude in [rad]
1034 y(2), // h Altitude in [m]
1035 calcEarthRadius_N(y(0)), // North/South (meridian) earth radius [m]
1036 calcEarthRadius_E(y(0))), // East/West (prime vertical) earth radius [m]
1037 n_acceleration;
1038
1039 return y_dot;
1040 };
1041
1042 Eigen::Vector3d lla_lastPosition = _startPosition.latLonAlt();
1043 for (size_t i = 2; i <= nOverhead; i++) // Calculate one second backwards
1044 {
1045 Eigen::Vector<double, 6> y; // [𝜙, λ, h, v_N, v_E, v_D]^T
1046 y << lla_lastPosition,
1047 _n_linearTrajectoryStartVelocity;
1048
1049 y += -_splines.sampleInterval * f(y, Eigen::Vector3d::Zero());
1050 lla_lastPosition = y.head<3>();
1051
1052 Eigen::Vector3d e_position = trafo::lla2ecef_WGS84(lla_lastPosition);
1053
1054 splineTime.at(nOverhead - i) = -_splines.sampleInterval * static_cast<double>(i - 1);
1055 splineX.at(nOverhead - i) = e_position(0);
1056 splineY.at(nOverhead - i) = e_position(1);
1057 splineZ.at(nOverhead - i) = e_position(2);
1058 }
1059
1060 lla_lastPosition = _startPosition.latLonAlt();
1061 for (size_t i = 1;; i++)
1062 {
1063 Eigen::Vector<double, 6> y; // [𝜙, λ, h, v_N, v_E, v_D]^T
1064 y << lla_lastPosition,
1065 _n_linearTrajectoryStartVelocity;
1066
1067 y += _splines.sampleInterval * f(y, Eigen::Vector3d::Zero());
1068 lla_lastPosition = y.head<3>();
1069
1070 auto e_position = trafo::lla2ecef_WGS84(lla_lastPosition);
1071
1072 auto simTime = _splines.sampleInterval * static_cast<double>(i);
1073 splineTime.push_back(simTime);
1074 splineX.push_back(e_position(0));
1075 splineY.push_back(e_position(1));
1076 splineZ.push_back(e_position(2));
1077 splineRoll.push_back(roll);
1078 splinePitch.push_back(pitch);
1079 splineYaw.push_back(yaw);
1080
1081 if (_simulationStopCondition == StopCondition::Duration
1082 && simTime > _simulationDuration + 1.0)
1083 {
1084 break;
1085 }
1086 if (_simulationStopCondition == StopCondition::DistanceOrCirclesOrRoses)
1087 {
1088 auto horizontalDistance = calcGeographicalDistance(_startPosition.latitude(), _startPosition.longitude(),
1089 lla_lastPosition(0), lla_lastPosition(1));
1090 auto distance = std::sqrt(std::pow(horizontalDistance, 2) + std::pow(_startPosition.altitude() - lla_lastPosition(2), 2))
1091 - _n_linearTrajectoryStartVelocity.norm() * 1.0;
1092 if (distance > _linearTrajectoryDistanceForStop)
1093 {
1094 break;
1095 }
1096 }
1097 }
1098
1099 _splines.x.setPoints(splineTime, splineX);
1100 _splines.y.setPoints(splineTime, splineY);
1101 _splines.z.setPoints(splineTime, splineZ);
1102
1103 _splines.roll.setPoints(splineTime, splineRoll);
1104 _splines.pitch.setPoints(splineTime, splinePitch);
1105 _splines.yaw.setPoints(splineTime, splineYaw);
1106 }
1107 else if (_trajectoryType == TrajectoryType::Circular)
1108 {
1109 double simDuration{};
1110 if (_simulationStopCondition == StopCondition::Duration)
1111 {
1112 simDuration = _simulationDuration;
1113 }
1114 else if (_simulationStopCondition == StopCondition::DistanceOrCirclesOrRoses)
1115 {
1116 double omega = _trajectoryHorizontalSpeed / _trajectoryRadius;
1117 simDuration = _circularTrajectoryCircleCountForStop * 2 * M_PI / omega;
1118 }
1119
1120 constexpr double OFFSET = 1.0; // [s]
1121
1122 for (size_t i = 0; i <= static_cast<size_t>(std::round((simDuration + 2.0 * OFFSET) / _splines.sampleInterval)); i++)
1123 {
1124 splineTime.push_back(_splines.sampleInterval * static_cast<double>(i) - OFFSET);
1125 }
1126 LOG_DATA("{}: Sim Time [{:.3f}, {:.3f}] with dt = {:.3f} (simDuration = {:.3f})", nameId(),
1127 static_cast<double>(splineTime.front()), static_cast<double>(splineTime.back()), static_cast<double>(splineTime.at(1) - splineTime.at(0)), simDuration);
1128
1129 std::vector<long double> splineX(splineTime.size());
1130 std::vector<long double> splineY(splineTime.size());
1131 std::vector<long double> splineZ(splineTime.size());
1132
1133 Eigen::Vector3d e_origin = _startPosition.e_position;
1134 Eigen::Vector3d lla_origin = _startPosition.latLonAlt();
1135
1136 Eigen::Quaterniond e_quatCenter_n = trafo::e_Quat_n(lla_origin(0), lla_origin(1));
1137
1138 for (uint64_t i = 0; i < splineTime.size(); i++)
1139 {
1140 auto phi = _trajectoryHorizontalSpeed * static_cast<double>(splineTime[i]) / _trajectoryRadius; // Angle of the current point on the circle
1141 phi *= _trajectoryDirection == Direction::CW ? -1 : 1;
1142 phi += _trajectoryRotationAngle;
1143 // LOG_DATA("{}: [t={:.3f}] phi = {:.3f}°", nameId(), static_cast<double>(splineTime.at(i)), rad2deg(phi));
1144
1145 Eigen::Vector3d n_relativePosition{ _trajectoryRadius * std::sin(phi) * (1 + _circularHarmonicAmplitudeFactor * std::sin(phi * static_cast<double>(_circularHarmonicFrequency))), // [m]
1146 _trajectoryRadius * std::cos(phi) * (1 + _circularHarmonicAmplitudeFactor * std::sin(phi * static_cast<double>(_circularHarmonicFrequency))), // [m]
1147 -_trajectoryVerticalSpeed * static_cast<double>(splineTime[i]) }; // [m]
1148
1149 Eigen::Vector3d e_relativePosition = e_quatCenter_n * n_relativePosition;
1150
1151 Eigen::Vector3d e_position = e_origin + e_relativePosition;
1152
1153 splineX[i] = e_position[0];
1154 splineY[i] = e_position[1];
1155 splineZ[i] = e_position[2];
1156 }
1157
1158 _splines.x.setPoints(splineTime, splineX);
1159 _splines.y.setPoints(splineTime, splineY);
1160 _splines.z.setPoints(splineTime, splineZ);
1161 }
1162 else if (_trajectoryType == TrajectoryType::RoseFigure)
1163 {
1164 // Formulas and notation for the rose figure from https://en.wikipedia.org/wiki/Rose_(mathematics)
1165
1166 // Adjusting the integration bounds needs a factor * PI
1167 // | d \ n | 1 | 2 | 3 | 4 | 5 | 6 | 7 |
1168 // | ------ | --- | --- | --- | --- | --- | --- | --- |
1169 // | 1 | 1 | 2 | 1 | 2 | 1 | 2 | 1 |
1170 // | 2 | 4 | 1 | 4 | 2 | 4 | 1 | 4 |
1171 // | 3 | 3 | 6 | 1 | 6 | 3 | 2 | 3 |
1172 // | 4 | 8 | 4 | 8 | 1 | 8 | 4 | 8 |
1173 // | 5 | 5 | 10 | 5 | 10 | 1 | 10 | 5 |
1174 // | 6 | 12 | 3 | 4 | 6 | 12 | 1 | 12 |
1175 // | 7 | 7 | 14 | 7 | 14 | 7 | 14 | 1 |
1176 // | 8 | 16 | 8 | 16 | 4 | 16 | 8 | 16 |
1177 // | 9 | 9 | 18 | 3 | 18 | 9 | 6 | 9 |
1178
1179 int n = _rosePetNum;
1180 int d = _rosePetDenom;
1181
1182 for (int i = 2; i <= n; i++) // reduction of fraction ( 4/2 ==> 2/1 )
1183 {
1184 if (n % i == 0 && d % i == 0)
1185 {
1186 n /= i;
1187 d /= i;
1188 i--;
1189 }
1190 }
1191
1192 auto isOdd = [](auto a) { return static_cast<int>(a) % 2 != 0; };
1193 auto isEven = [](auto a) { return static_cast<int>(a) % 2 == 0; };
1194
1195 double integrationFactor = 0.0;
1196 if (isOdd(d))
1197 {
1198 if (isOdd(n)) { integrationFactor = static_cast<double>(d); }
1199 else { integrationFactor = 2.0 * static_cast<double>(d); }
1200 }
1201 else // if (isEven(d))
1202 {
1203 if (isEven(n)) { integrationFactor = static_cast<double>(d); }
1204 else { integrationFactor = 2.0 * static_cast<double>(d); }
1205 }
1206
1207 auto nVirtPoints = static_cast<size_t>(1.0 / (_roseStepLengthMax / 50.0));
1208 splineTime.resize(nVirtPoints); // Preallocate points to make the spline start at the right point
1209 std::vector<long double> splineX(splineTime.size());
1210 std::vector<long double> splineY(splineTime.size());
1211 std::vector<long double> splineZ(splineTime.size());
1212
1213 Eigen::Vector3d e_origin = trafo::lla2ecef_WGS84(_startPosition.latLonAlt());
1214
1215 Eigen::Quaterniond e_quatCenter_n = trafo::e_Quat_n(_startPosition.latLonAlt()(0), _startPosition.latLonAlt()(1));
1216
1217 double lengthOld = -_roseStepLengthMax / 2.0; // n-1 length
1218 double dPhi = 0.001; // Angle step size
1219 double maxPhi = std::numeric_limits<double>::infinity(); // Interval for integration depending on selected stop criteria
1220 if (_simulationStopCondition == StopCondition::DistanceOrCirclesOrRoses)
1221 {
1222 maxPhi = _roseTrajectoryCountForStop * integrationFactor * M_PI;
1223 }
1224
1225 // k = n/d
1226 double roseK = static_cast<double>(_rosePetNum) / static_cast<double>(_rosePetDenom);
1227
1228 _roseSimDuration = 0.0;
1229
1230 // We cannot input negative values or zero
1231 for (double phi = dPhi; phi <= maxPhi + static_cast<double>(nVirtPoints) * dPhi; phi += dPhi) // NOLINT(clang-analyzer-security.FloatLoopCounter, cert-flp30-c)
1232 {
1233 double length = _trajectoryRadius / roseK * math::calcEllipticalIntegral(roseK * phi, 1.0 - std::pow(roseK, 2.0));
1234 double dL = length - lengthOld;
1235
1236 if (dL > _roseStepLengthMax)
1237 {
1238 phi -= dPhi;
1239 dPhi /= 2.0;
1240 continue;
1241 }
1242 if (dL < _roseStepLengthMax / 3.0) // Avoid also too small steps
1243 {
1244 phi -= dPhi;
1245 dPhi *= 1.5;
1246 continue;
1247 }
1248 lengthOld = length;
1249
1250 double time = length / _trajectoryHorizontalSpeed;
1251 splineTime.push_back(time);
1252 Eigen::Vector3d n_relativePosition{ _trajectoryRadius * std::cos(roseK * phi) * std::sin(phi * (_trajectoryDirection == Direction::CW ? -1.0 : 1.0) + _trajectoryRotationAngle), // [m]
1253 _trajectoryRadius * std::cos(roseK * phi) * std::cos(phi * (_trajectoryDirection == Direction::CW ? -1.0 : 1.0) + _trajectoryRotationAngle), // [m]
1254 -_trajectoryVerticalSpeed * time }; // [m]
1255
1256 Eigen::Vector3d e_relativePosition = e_quatCenter_n * n_relativePosition;
1257 Eigen::Vector3d e_position = e_origin + e_relativePosition;
1258
1259 // LOG_DATA("{}: t={: 8.3f}s | l={:8.6}m | phi={:6.3f}", nameId(), time, length, rad2deg(phi));
1260
1261 splineX.push_back(e_position[0]);
1262 splineY.push_back(e_position[1]);
1263 splineZ.push_back(e_position[2]);
1264
1265 if (_simulationStopCondition == StopCondition::DistanceOrCirclesOrRoses && std::abs(maxPhi - phi) < 1.0 * dPhi)
1266 {
1267 LOG_TRACE("{}: Rose figure simulation duration: {:8.6}s | l={:8.6}m", nameId(), time, length);
1268 _roseSimDuration = time;
1269 }
1270 else if (_simulationStopCondition == StopCondition::Duration && _roseSimDuration == 0.0 && time > _simulationDuration)
1271 {
1272 _roseSimDuration = _simulationDuration;
1273 maxPhi = phi;
1274 }
1275 }
1276
1277 maxPhi = integrationFactor * M_PI + 1e-15; // For exactly 2*pi the elliptical integral is failing. Bug in the function.
1278 // LOG_DATA("maxPhi {:6.2f}", rad2deg(maxPhi));
1279 double endLength = _trajectoryRadius / roseK * math::calcEllipticalIntegral(roseK * maxPhi, 1.0 - std::pow(roseK, 2.0));
1280 // LOG_DATA("endLength {:8.6}m", endLength);
1281 for (size_t i = 0; i < nVirtPoints; i++)
1282 {
1283 double phi = maxPhi - static_cast<double>(i) * dPhi;
1284 double length = _trajectoryRadius / roseK * math::calcEllipticalIntegral(roseK * phi, 1.0 - std::pow(roseK, 2.0));
1285 double time = (length - endLength) / _trajectoryHorizontalSpeed;
1286 splineTime[nVirtPoints - i - 1] = time;
1287
1288 Eigen::Vector3d n_relativePosition{ _trajectoryRadius * std::cos(roseK * phi) * std::sin(phi * (_trajectoryDirection == Direction::CW ? -1.0 : 1.0) + _trajectoryRotationAngle), // [m]
1289 _trajectoryRadius * std::cos(roseK * phi) * std::cos(phi * (_trajectoryDirection == Direction::CW ? -1.0 : 1.0) + _trajectoryRotationAngle), // [m]
1290 -_trajectoryVerticalSpeed * time }; // [m]
1291
1292 Eigen::Vector3d e_relativePosition = e_quatCenter_n * n_relativePosition;
1293 Eigen::Vector3d e_position = e_origin + e_relativePosition;
1294
1295 // LOG_DATA("{}: t={: 8.3f}s | l={:8.6}m | phi={:6.3f}", nameId(), time, length, rad2deg(phi));
1296
1297 splineX[nVirtPoints - i - 1] = e_position[0];
1298 splineY[nVirtPoints - i - 1] = e_position[1];
1299 splineZ[nVirtPoints - i - 1] = e_position[2];
1300 }
1301
1302 _splines.x.setPoints(splineTime, splineX);
1303 _splines.y.setPoints(splineTime, splineY);
1304 _splines.z.setPoints(splineTime, splineZ);
1305 }
1306 else if (_trajectoryType == TrajectoryType::Csv)
1307 {
1308 if (auto csvData = getInputValue<CsvData>(INPUT_PORT_INDEX_CSV);
1309 csvData && csvData->v->lines.size() >= 2)
1310 {
1311 if (auto startTime = getTimeFromCsvLine(csvData->v->lines.front(), csvData->v->description))
1312 {
1313 if (!startTime) { return false; }
1314 _startTime = *startTime;
1315 }
1316
1317 constexpr size_t nVirtPoints = 0;
1318 splineTime.resize(nVirtPoints); // Preallocate points to make the spline start at the right point
1319 std::vector<long double> splineX(splineTime.size());
1320 std::vector<long double> splineY(splineTime.size());
1321 std::vector<long double> splineZ(splineTime.size());
1322 std::vector<long double> splineRoll(splineTime.size());
1323 std::vector<long double> splinePitch(splineTime.size());
1324 std::vector<long double> splineYaw(splineTime.size());
1325
1326 for (size_t i = 0; i < csvData->v->lines.size(); i++)
1327 {
1328 auto insTime = getTimeFromCsvLine(csvData->v->lines[i], csvData->v->description);
1329 if (!insTime)
1330 {
1331 LOG_ERROR("{}: Data in time column not found in line {}", nameId(), i + 2);
1332 return false;
1333 }
1334 // LOG_DATA("{}: Time {}", nameId(), *insTime);
1335 auto time = static_cast<double>((*insTime - _startTime).count());
1336
1337 auto e_pos = e_getPositionFromCsvLine(csvData->v->lines[i], csvData->v->description);
1338 if (!e_pos)
1339 {
1340 LOG_ERROR("{}: Data in position column not found in line {}", nameId(), i + 2);
1341 return false;
1342 }
1343 // LOG_DATA("{}: e_pos {}", nameId(), *e_pos);
1344
1345 auto n_Quat_b = n_getAttitudeQuaternionFromCsvLine_b(csvData->v->lines[i], csvData->v->description);
1346 if (!n_Quat_b)
1347 {
1348 LOG_ERROR("{}: Data in attitude column not found in line {}", nameId(), i + 2);
1349 return false;
1350 }
1351 // LOG_DATA("{}: n_Quat_b {}", nameId(), *n_Quat_b);
1352
1353 splineTime.push_back(time);
1354 splineX.push_back(e_pos->x());
1355 splineY.push_back(e_pos->y());
1356 splineZ.push_back(e_pos->z());
1357
1358 auto rpy = trafo::quat2eulerZYX(*n_Quat_b);
1359 // LOG_DATA("{}: RPY {} [deg] (from CSV)", nameId(), rad2deg(rpy).transpose());
1360 splineRoll.push_back(i > 0 ? unwrapAngle(rpy(0), splineRoll.back(), M_PI) : rpy(0));
1361 splinePitch.push_back(i > 0 ? unwrapAngle(rpy(1), splinePitch.back(), M_PI_2) : rpy(1));
1362 splineYaw.push_back(i > 0 ? unwrapAngle(rpy(2), splineYaw.back(), M_PI) : rpy(2));
1363 // LOG_DATA("{}: R {}, P {}, Y {} [deg] (in Spline)", nameId(), rad2deg(splineRoll.back()), rad2deg(splinePitch.back()), rad2deg(splineYaw.back()));
1364 }
1365 _csvDuration = static_cast<double>(splineTime.back());
1366
1367 auto dt = static_cast<double>(splineTime[nVirtPoints + 1] - splineTime[nVirtPoints]);
1368 for (size_t i = 0; i < nVirtPoints; i++)
1369 {
1370 double h = 0.001;
1371 splineTime[nVirtPoints - i - 1] = splineTime[nVirtPoints - i] - h;
1372 splineX[nVirtPoints - i - 1] = splineX[nVirtPoints - i] - h * (splineX[nVirtPoints + 1] - splineX[nVirtPoints]) / dt;
1373 splineY[nVirtPoints - i - 1] = splineY[nVirtPoints - i] - h * (splineY[nVirtPoints + 1] - splineY[nVirtPoints]) / dt;
1374 splineZ[nVirtPoints - i - 1] = splineZ[nVirtPoints - i] - h * (splineZ[nVirtPoints + 1] - splineZ[nVirtPoints]) / dt;
1375 splineRoll[nVirtPoints - i - 1] = splineRoll[nVirtPoints - i] - h * (splineRoll[nVirtPoints + 1] - splineRoll[nVirtPoints]) / dt;
1376 splinePitch[nVirtPoints - i - 1] = splinePitch[nVirtPoints - i] - h * (splinePitch[nVirtPoints + 1] - splinePitch[nVirtPoints]) / dt;
1377 splineYaw[nVirtPoints - i - 1] = splineYaw[nVirtPoints - i] - h * (splineYaw[nVirtPoints + 1] - splineYaw[nVirtPoints]) / dt;
1378 splineTime.push_back(splineTime[splineX.size() - 1] + h);
1379 splineX.push_back(splineX[splineX.size() - 1] + h * (splineX[splineX.size() - 1] - splineX[splineX.size() - 2]) / dt);
1380 splineY.push_back(splineY[splineY.size() - 1] + h * (splineY[splineY.size() - 1] - splineY[splineY.size() - 2]) / dt);
1381 splineZ.push_back(splineZ[splineZ.size() - 1] + h * (splineZ[splineZ.size() - 1] - splineZ[splineZ.size() - 2]) / dt);
1382 splineRoll.push_back(splineRoll[splineRoll.size() - 1] + h * (splineRoll[splineRoll.size() - 1] - splineRoll[splineRoll.size() - 2]) / dt);
1383 splinePitch.push_back(splinePitch[splinePitch.size() - 1] + h * (splinePitch[splinePitch.size() - 1] - splinePitch[splinePitch.size() - 2]) / dt);
1384 splineYaw.push_back(splineYaw[splineYaw.size() - 1] + h * (splineYaw[splineYaw.size() - 1] - splineYaw[splineYaw.size() - 2]) / dt);
1385 }
1386
1387 _splines.x.setPoints(splineTime, splineX);
1388 _splines.y.setPoints(splineTime, splineY);
1389 _splines.z.setPoints(splineTime, splineZ);
1390
1391 _splines.roll.setPoints(splineTime, splineRoll);
1392 _splines.pitch.setPoints(splineTime, splinePitch);
1393 _splines.yaw.setPoints(splineTime, splineYaw);
1394 }
1395 else
1396 {
1397 LOG_ERROR("{}: Can't calculate the data without a connected CSV file with at least two datasets", nameId());
1398 return false;
1399 }
1400 }
1401
1402 if (_trajectoryType == TrajectoryType::Circular || _trajectoryType == TrajectoryType::RoseFigure)
1403 {
1404 std::vector<long double> splineRoll(splineTime.size());
1405 std::vector<long double> splinePitch(splineTime.size());
1406 std::vector<long double> splineYaw(splineTime.size());
1407
1408 for (uint64_t i = 0; i < splineTime.size(); i++)
1409 {
1410 Eigen::Vector3d e_pos{ static_cast<double>(_splines.x(splineTime[i])),
1411 static_cast<double>(_splines.y(splineTime[i])),
1412 static_cast<double>(_splines.z(splineTime[i])) };
1413 Eigen::Vector3d e_vel{ static_cast<double>(_splines.x.derivative(1, splineTime[i])),
1414 static_cast<double>(_splines.y.derivative(1, splineTime[i])),
1415 static_cast<double>(_splines.z.derivative(1, splineTime[i])) };
1416
1417 Eigen::Vector3d lla_position = trafo::ecef2lla_WGS84(e_pos);
1418 Eigen::Vector3d n_velocity = trafo::n_Quat_e(lla_position(0), lla_position(1)) * e_vel;
1419
1420 Eigen::Vector3d e_normalVectorCenterCircle{ std::cos(_startPosition.latLonAlt()(0)) * std::cos(_startPosition.latLonAlt()(1)),
1421 std::cos(_startPosition.latLonAlt()(0)) * std::sin(_startPosition.latLonAlt()(1)),
1422 std::sin(_startPosition.latLonAlt()(0)) };
1423
1424 Eigen::Vector3d e_normalVectorCurrentPosition{ std::cos(lla_position(0)) * std::cos(lla_position(1)),
1425 std::cos(lla_position(0)) * std::sin(lla_position(1)),
1426 std::sin(lla_position(0)) };
1427
1428 auto yaw = calcYawFromVelocity(n_velocity);
1429
1430 splineYaw[i] = i > 0 ? unwrapAngle(yaw, splineYaw[i - 1], M_PI) : yaw;
1431 splineRoll[i] = 0.0;
1432 splinePitch[i] = n_velocity.head<2>().norm() > 1e-8 ? calcPitchFromVelocity(n_velocity) : 0;
1433 // LOG_DATA("{}: [t={:.3f}] yaw = {:.3f}°", nameId(), static_cast<double>(splineTime.at(i)), rad2deg(splineYaw[i]));
1434 }
1435
1436 _splines.roll.setPoints(splineTime, splineRoll);
1437 _splines.pitch.setPoints(splineTime, splinePitch);
1438 _splines.yaw.setPoints(splineTime, splineYaw);
1439 }
1440
1441 return true;
1442}
1443
1444bool NAV::ImuSimulator::initialize()
1445{
1446 LOG_TRACE("{}: called", nameId());
1447
1448 return initializeSplines();
1449}
1450
1451void NAV::ImuSimulator::deinitialize()
1452{
1453 LOG_TRACE("{}: called", nameId());
1454}
1455
1456bool NAV::ImuSimulator::resetNode()
1457{
1458 LOG_TRACE("{}: called", nameId());
1459
1460 _imuInternalUpdateCnt = 0;
1461 _imuUpdateCnt = 0;
1462 _gnssUpdateCnt = 0;
1463
1464 _imuLastUpdateTime = 0.0;
1465 _gnssLastUpdateTime = 0.0;
1466 _lla_imuLastLinearPosition = _startPosition.latLonAlt();
1467 _lla_gnssLastLinearPosition = _startPosition.latLonAlt();
1468
1469 _p_lastImuAccelerationMeas = Eigen::Vector3d::Ones() * std::nan("");
1470 _p_lastImuAngularRateMeas = Eigen::Vector3d::Ones() * std::nan("");
1471
1472 if (_trajectoryType == TrajectoryType::Csv)
1473 {
1474 if (auto csvData = getInputValue<CsvData>(INPUT_PORT_INDEX_CSV);
1475 csvData && !csvData->v->lines.empty())
1476 {
1477 if (auto startTime = getTimeFromCsvLine(csvData->v->lines.front(), csvData->v->description))
1478 {
1479 if (!startTime) { return false; }
1480 _startTime = *startTime;
1481 }
1482 LOG_DEBUG("{}: Start Time set to {}", nameId(), _startTime);
1483 }
1484 else
1485 {
1486 LOG_ERROR("{}: Can't reset the ImuSimulator without a connected CSV file", nameId());
1487 return false;
1488 }
1489 }
1490 else if (_startTimeSource == StartTimeSource::CurrentComputerTime)
1491 {
1492 std::time_t t = std::time(nullptr);
1493 std::tm* now = std::localtime(&t); // NOLINT(concurrency-mt-unsafe)
1494
1495 _startTime = InsTime{ static_cast<uint16_t>(now->tm_year + 1900), static_cast<uint16_t>(now->tm_mon), static_cast<uint16_t>(now->tm_mday),
1496 static_cast<uint16_t>(now->tm_hour), static_cast<uint16_t>(now->tm_min), static_cast<long double>(now->tm_sec) };
1497 LOG_DEBUG("{}: Start Time set to {}", nameId(), _startTime);
1498 }
1499
1500 return true;
1501}
1502
1503bool NAV::ImuSimulator::checkStopCondition(double time, const Eigen::Vector3d& lla_position)
1504{
1505 if (_trajectoryType == TrajectoryType::Csv)
1506 {
1507 return time > _csvDuration;
1508 }
1509 if (_simulationStopCondition == StopCondition::Duration
1510 || _trajectoryType == TrajectoryType::Fixed)
1511 {
1512 return time > _simulationDuration;
1513 }
1514 if (_simulationStopCondition == StopCondition::DistanceOrCirclesOrRoses)
1515 {
1516 if (_trajectoryType == TrajectoryType::Linear)
1517 {
1518 auto horizontalDistance = calcGeographicalDistance(_startPosition.latitude(), _startPosition.longitude(),
1519 lla_position(0), lla_position(1));
1520 auto distance = std::sqrt(std::pow(horizontalDistance, 2) + std::pow(_startPosition.altitude() - lla_position(2), 2));
1521 return distance > _linearTrajectoryDistanceForStop;
1522 }
1523 if (_trajectoryType == TrajectoryType::Circular)
1524 {
1525 double omega = _trajectoryHorizontalSpeed / _trajectoryRadius;
1526 double simDuration = _circularTrajectoryCircleCountForStop * 2 * M_PI / omega;
1527 return time > simDuration;
1528 }
1529 if (_trajectoryType == TrajectoryType::RoseFigure)
1530 {
1531 return time > _roseSimDuration;
1532 }
1533 }
1534 return false;
1535}
1536
1537std::shared_ptr<const NAV::NodeData> NAV::ImuSimulator::pollImuObs(size_t /* pinIdx */, bool peek)
1538{
1539 double imuUpdateTime = static_cast<double>(_imuUpdateCnt) / _imuFrequency;
1540
1541 // -------------------------------------------------- Check if a stop condition is met -----------------------------------------------------
1542 if (checkStopCondition(imuUpdateTime, lla_calcPosition(imuUpdateTime)))
1543 {
1544 return nullptr;
1545 }
1546 // ------------------------------------- Early return in case of peeking to avoid heavy calculations ---------------------------------------
1547 if (peek)
1548 {
1549 auto obs = std::make_shared<NodeData>();
1550 obs->insTime = _startTime + std::chrono::duration<double>(imuUpdateTime);
1551 return obs;
1552 }
1553
1554 auto obs = std::make_shared<ImuObsSimulated>(_imuPos);
1555 obs->insTime = _startTime + std::chrono::duration<double>(imuUpdateTime);
1556 LOG_DATA("{}: Simulated IMU data for time [{}] (_imuUpdateCnt {}, _imuInternalUpdateCnt {})", nameId(), obs->insTime.toYMDHMS(),
1557 _imuUpdateCnt, _imuInternalUpdateCnt);
1558
1559 double dTime = 0.0;
1560 Eigen::Vector3d p_deltaVel = Eigen::Vector3d::Zero();
1561 Eigen::Vector3d p_deltaTheta = Eigen::Vector3d::Zero();
1562
1563 double imuInternalUpdateTime = 0.0;
1564 do {
1565 imuInternalUpdateTime = static_cast<double>(_imuInternalUpdateCnt) / _imuInternalFrequency - 1.0 / _imuFrequency;
1566 LOG_DATA("{}: Simulated internal IMU data for time [{}] (_imuUpdateCnt {}, _imuInternalUpdateCnt {})", nameId(),
1567 (_startTime + std::chrono::duration<double>(imuInternalUpdateTime)).toYMDHMS(), _imuUpdateCnt, _imuInternalUpdateCnt);
1568
1569 // --------------------------------------------------------- Calculation of data -----------------------------------------------------------
1570 Eigen::Vector3d lla_position = lla_calcPosition(imuInternalUpdateTime);
1571 LOG_DATA("{}: [{:8.3f}] lla_position = {}°, {}°, {} m", nameId(), imuInternalUpdateTime, rad2deg(lla_position(0)), rad2deg(lla_position(1)), lla_position(2));
1572 Eigen::Vector3d e_position = trafo::lla2ecef_WGS84(lla_position);
1573 Eigen::Quaterniond n_Quat_e = trafo::n_Quat_e(lla_position(0), lla_position(1));
1574
1575 Eigen::Vector3d n_vel = n_calcVelocity(imuInternalUpdateTime, n_Quat_e);
1576 LOG_DATA("{}: [{:8.3f}] n_vel = {} [m/s]", nameId(), imuInternalUpdateTime, n_vel.transpose());
1577
1578 auto [roll, pitch, yaw] = calcFlightAngles(imuInternalUpdateTime);
1579 LOG_DATA("{}: [{:8.3f}] roll = {}°, pitch = {}°, yaw = {}°", nameId(), imuInternalUpdateTime, rad2deg(roll), rad2deg(pitch), rad2deg(yaw));
1580
1581 Eigen::Quaterniond b_Quat_n = trafo::b_Quat_n(roll, pitch, yaw);
1582
1583 Eigen::Vector3d n_omega_ie = n_Quat_e * InsConst::e_omega_ie;
1584 LOG_DATA("{}: [{:8.3f}] n_omega_ie = {} [rad/s]", nameId(), imuInternalUpdateTime, n_omega_ie.transpose());
1585 auto R_N = calcEarthRadius_N(lla_position(0));
1586 LOG_DATA("{}: [{:8.3f}] R_N = {} [m]", nameId(), imuInternalUpdateTime, R_N);
1587 auto R_E = calcEarthRadius_E(lla_position(0));
1588 LOG_DATA("{}: [{:8.3f}] R_E = {} [m]", nameId(), imuInternalUpdateTime, R_E);
1589 Eigen::Vector3d n_omega_en = n_calcTransportRate(lla_position, n_vel, R_N, R_E);
1590 LOG_DATA("{}: [{:8.3f}] n_omega_en = {} [rad/s]", nameId(), imuInternalUpdateTime, n_omega_en.transpose());
1591
1592 // ------------------------------------------------------------ Accelerations --------------------------------------------------------------
1593
1594 // Force to keep vehicle on track
1595 Eigen::Vector3d n_trajectoryAccel = n_calcTrajectoryAccel(imuInternalUpdateTime, n_Quat_e, lla_position, n_vel);
1596 LOG_DATA("{}: [{:8.3f}] n_trajectoryAccel = {} [m/s^2]", nameId(), imuInternalUpdateTime, n_trajectoryAccel.transpose());
1597
1598 // Measured acceleration in local-navigation frame coordinates [m/s^2]
1599 Eigen::Vector3d n_accel = n_trajectoryAccel;
1600 if (_coriolisAccelerationEnabled) // Apply Coriolis Acceleration
1601 {
1602 Eigen::Vector3d n_coriolisAcceleration = n_calcCoriolisAcceleration(n_omega_ie, n_omega_en, n_vel);
1603 LOG_DATA("{}: [{:8.3f}] n_coriolisAcceleration = {} [m/s^2]", nameId(), imuInternalUpdateTime, n_coriolisAcceleration.transpose());
1604 n_accel += n_coriolisAcceleration;
1605 }
1606
1607 // Mass attraction of the Earth (gravitation)
1608 Eigen::Vector3d n_gravitation = n_calcGravitation(lla_position, _gravitationModel);
1609 LOG_DATA("{}: [{:8.3f}] n_gravitation = {} [m/s^2] ({})", nameId(), imuInternalUpdateTime, n_gravitation.transpose(), NAV::to_string(_gravitationModel));
1610 n_accel -= n_gravitation; // Apply the local gravity vector
1611
1612 if (_centrifgalAccelerationEnabled) // Centrifugal acceleration caused by the Earth's rotation
1613 {
1614 Eigen::Vector3d e_centrifugalAcceleration = e_calcCentrifugalAcceleration(e_position);
1615 LOG_DATA("{}: [{:8.3f}] e_centrifugalAcceleration = {} [m/s^2]", nameId(), imuInternalUpdateTime, e_centrifugalAcceleration.transpose());
1616 Eigen::Vector3d n_centrifugalAcceleration = n_Quat_e * e_centrifugalAcceleration;
1617 LOG_DATA("{}: [{:8.3f}] n_centrifugalAcceleration = {} [m/s^2]", nameId(), imuInternalUpdateTime, n_centrifugalAcceleration.transpose());
1618 n_accel += n_centrifugalAcceleration;
1619 }
1620
1621 // Acceleration measured by the accelerometer in platform coordinates
1622 Eigen::Vector3d p_accel = _imuPos.p_quat_b() * b_Quat_n * n_accel;
1623 LOG_DATA("{}: [{:8.3f}] p_accel = {} [m/s^2]", nameId(), imuInternalUpdateTime, p_accel.transpose());
1624
1625 // ------------------------------------------------------------ Angular rates --------------------------------------------------------------
1626
1627 Eigen::Vector3d n_omega_nb = n_calcOmega_nb(imuInternalUpdateTime, Eigen::Vector3d{ roll, pitch, yaw }, b_Quat_n.conjugate());
1628
1629 // ω_ib_n = ω_in_n + ω_nb_n = (ω_ie_n + ω_en_n) + ω_nb_n
1630 Eigen::Vector3d n_omega_ib = n_omega_nb;
1631 if (_angularRateEarthRotationEnabled)
1632 {
1633 n_omega_ib += n_omega_ie;
1634 }
1635 if (_angularRateTransportRateEnabled)
1636 {
1637 n_omega_ib += n_omega_en;
1638 }
1639
1640 // ω_ib_b = b_Quat_n * ω_ib_n
1641 // = 0
1642 // ω_ip_p = p_Quat_b * (ω_ib_b + ω_bp_b) = p_Quat_b * ω_ib_b
1643 Eigen::Vector3d p_omega_ip = _imuPos.p_quat_b() * b_Quat_n * n_omega_ib;
1644 LOG_DATA("{}: [{:8.3f}] p_omega_ip = {} [rad/s]", nameId(), imuInternalUpdateTime, p_omega_ip.transpose());
1645
1646 // -------------------------------------------------- Construct the message to send out ----------------------------------------------------
1647
1648 obs->p_acceleration = p_accel.cast<double>();
1649 obs->p_angularRate = p_omega_ip.cast<double>();
1650 // obs->p_magneticField.emplace(0, 0, 0);
1651
1652 Eigen::Quaterniond e_Quat_n = n_Quat_e.conjugate();
1653
1654 obs->n_accelDynamics = n_trajectoryAccel.cast<double>();
1655 obs->n_angularRateDynamics = n_omega_nb.cast<double>();
1656
1657 obs->e_accelDynamics = (e_Quat_n * n_trajectoryAccel).cast<double>();
1658 obs->e_angularRateDynamics = (e_Quat_n * n_omega_nb).cast<double>();
1659
1660 double dt = 1.0 / _imuInternalFrequency;
1661 if (_imuInternalUpdateCnt != 0)
1662 {
1663 dTime += dt;
1664
1665 p_deltaVel += (p_accel + _p_lastImuAccelerationMeas) / 2 * dt;
1666 p_deltaTheta += (p_omega_ip + _p_lastImuAngularRateMeas) / 2 * dt;
1667 }
1668 _p_lastImuAccelerationMeas = p_accel;
1669 _p_lastImuAngularRateMeas = p_omega_ip;
1670
1671 _imuInternalUpdateCnt++;
1672 } while (imuInternalUpdateTime + 1e-6 < imuUpdateTime);
1673
1674 obs->dtime = dTime;
1675 obs->dvel = p_deltaVel.cast<double>();
1676 obs->dtheta = p_deltaTheta.cast<double>();
1677 LOG_DATA("{}: dtime = {}, dvel = {}, dtheta = {}", nameId(), obs->dtime, obs->dvel.transpose(), obs->dtheta.transpose());
1678
1679 _imuUpdateCnt++;
1680 invokeCallbacks(OUTPUT_PORT_INDEX_IMU_OBS, obs);
1681
1682 return obs;
1683}
1684
1685std::shared_ptr<const NAV::NodeData> NAV::ImuSimulator::pollPosVelAtt(size_t /* pinIdx */, bool peek)
1686{
1687 auto gnssUpdateTime = static_cast<double>(_gnssUpdateCnt) / _gnssFrequency;
1688
1689 Eigen::Vector3d lla_position = lla_calcPosition(gnssUpdateTime);
1690
1691 // -------------------------------------------------- Check if a stop condition is met -----------------------------------------------------
1692 if (checkStopCondition(gnssUpdateTime, lla_position))
1693 {
1694 return nullptr;
1695 }
1696
1697 // ------------------------------------- Early return in case of peeking to avoid heavy calculations ---------------------------------------
1698 if (peek)
1699 {
1700 auto obs = std::make_shared<NodeData>();
1701 obs->insTime = _startTime + std::chrono::duration<double>(gnssUpdateTime);
1702 return obs;
1703 }
1704 auto obs = std::make_shared<PosVelAtt>();
1705 obs->insTime = _startTime + std::chrono::duration<double>(gnssUpdateTime);
1706 LOG_DATA("{}: Simulating GNSS data for time [{}]", nameId(), obs->insTime.toYMDHMS());
1707
1708 // --------------------------------------------------------- Calculation of data -----------------------------------------------------------
1709 LOG_DATA("{}: [{:8.3f}] lla_position = {}°, {}°, {} m", nameId(), gnssUpdateTime, rad2deg(lla_position(0)), rad2deg(lla_position(1)), lla_position(2));
1710 Eigen::Quaterniond n_Quat_e = trafo::n_Quat_e(lla_position(0), lla_position(1));
1711 Eigen::Vector3d n_vel = n_calcVelocity(gnssUpdateTime, n_Quat_e);
1712 LOG_DATA("{}: [{:8.3f}] n_vel = {} [m/s]", nameId(), gnssUpdateTime, n_vel.transpose());
1713 auto [roll, pitch, yaw] = calcFlightAngles(gnssUpdateTime);
1714 LOG_DATA("{}: [{:8.3f}] roll = {}°, pitch = {}°, yaw = {}°", nameId(), gnssUpdateTime, rad2deg(roll), rad2deg(pitch), rad2deg(yaw));
1715
1716 // -------------------------------------------------- Construct the message to send out ----------------------------------------------------
1717
1718 obs->setPosVelAtt_n(lla_position.cast<double>(), n_vel.cast<double>(), trafo::n_Quat_b(roll, pitch, yaw).cast<double>());
1719
1720 _gnssUpdateCnt++;
1721 invokeCallbacks(OUTPUT_PORT_INDEX_POS_VEL_ATT, obs);
1722
1723 return obs;
1724}
1725
1726std::array<double, 3> NAV::ImuSimulator::calcFlightAngles(double time) const
1727{
1728 return {
1729 static_cast<double>(_splines.roll(time)),
1730 static_cast<double>(_splines.pitch(time)),
1731 static_cast<double>(_splines.yaw(time))
1732 };
1733}
1734
1735Eigen::Vector3d NAV::ImuSimulator::lla_calcPosition(double time) const
1736{
1737 Eigen::Vector3d e_pos(static_cast<double>(_splines.x(time)),
1738 static_cast<double>(_splines.y(time)),
1739 static_cast<double>(_splines.z(time)));
1740 return trafo::ecef2lla_WGS84(e_pos);
1741}
1742
1743Eigen::Vector3d NAV::ImuSimulator::n_calcVelocity(double time, const Eigen::Quaterniond& n_Quat_e) const
1744{
1745 Eigen::Vector3d e_vel(static_cast<double>(_splines.x.derivative(1, time)),
1746 static_cast<double>(_splines.y.derivative(1, time)),
1747 static_cast<double>(_splines.z.derivative(1, time)));
1748 return n_Quat_e * e_vel;
1749}
1750
1751Eigen::Vector3d NAV::ImuSimulator::n_calcTrajectoryAccel(double time,
1752 const Eigen::Quaterniond& n_Quat_e,
1753 const Eigen::Vector3d& lla_position,
1754 const Eigen::Vector3d& n_velocity) const
1755{
1756 Eigen::Vector3d e_accel(static_cast<double>(_splines.x.derivative(2, time)),
1757 static_cast<double>(_splines.y.derivative(2, time)),
1758 static_cast<double>(_splines.z.derivative(2, time)));
1759 Eigen::Quaterniond e_Quat_n = n_Quat_e.conjugate();
1760 Eigen::Vector3d e_vel = e_Quat_n * n_velocity;
1761
1762 // Math: \dot{C}_n^e = C_n^e \cdot \Omega_{en}^n
1763 Eigen::Matrix3d n_DCM_dot_e = e_Quat_n.toRotationMatrix()
1764 * math::skewSymmetricMatrix(n_calcTransportRate(lla_position, n_velocity,
1765 calcEarthRadius_N(lla_position(0)),
1766 calcEarthRadius_E(lla_position(0))));
1767
1768 // Math: \dot{C}_e^n = (\dot{C}_n^e)^T
1769 Eigen::Matrix3d e_DCM_dot_n = n_DCM_dot_e.transpose();
1770
1771 // Math: a^n = \frac{\partial}{\partial t} \left( \dot{x}^n \right) = \frac{\partial}{\partial t} \left( C_e^n \cdot \dot{x}^e \right) = \dot{C}_e^n \cdot \dot{x}^e + C_e^n \cdot \ddot{x}^e
1772 return e_DCM_dot_n * e_vel + n_Quat_e * e_accel;
1773}
1774
1775Eigen::Vector3d NAV::ImuSimulator::n_calcOmega_nb(double time, const Eigen::Vector3d& rollPitchYaw, const Eigen::Quaterniond& n_Quat_b) const
1776{
1777 const auto& R = rollPitchYaw(0);
1778 const auto& P = rollPitchYaw(1);
1779
1780 // #########################################################################################################################################
1781
1782 auto R_dot = static_cast<double>(_splines.roll.derivative(1, time));
1783 auto Y_dot = static_cast<double>(_splines.yaw.derivative(1, time));
1784 auto P_dot = static_cast<double>(_splines.pitch.derivative(1, time));
1785
1786 auto C_3 = [](auto R) {
1787 // Eigen::Matrix3d C;
1788 // C << 1, 0 , 0 ,
1789 // 0, std::cos(R), std::sin(R),
1790 // 0, -std::sin(R), std::cos(R);
1791 // return C;
1792 return Eigen::AngleAxisd{ -R, Eigen::Vector3d::UnitX() };
1793 };
1794 auto C_2 = [](auto P) {
1795 // Eigen::Matrix3d C;
1796 // C << std::cos(P), 0 , -std::sin(P),
1797 // 0 , 1 , 0 ,
1798 // std::sin(P), 0 , std::cos(P);
1799 // return C;
1800 return Eigen::AngleAxisd{ -P, Eigen::Vector3d::UnitY() };
1801 };
1802
1803 // ω_nb_b = [∂/∂t R] + C_3 [ 0 ] + C_3 C_2 [ 0 ]
1804 // [ 0 ] [∂/∂t P] [ 0 ]
1805 // [ 0 ] [ 0 ] [∂/∂t Y]
1806 Eigen::Vector3d b_omega_nb = Eigen::Vector3d{ R_dot, 0, 0 }
1807 + C_3(R) * Eigen::Vector3d{ 0, P_dot, 0 }
1808 + C_3(R) * C_2(P) * Eigen::Vector3d{ 0, 0, Y_dot };
1809
1810 return n_Quat_b * b_omega_nb;
1811}
1812
1813const char* NAV::ImuSimulator::to_string(TrajectoryType value)
1814{
1815 switch (value)
1816 {
1817 case TrajectoryType::Fixed:
1818 return "Fixed";
1819 case TrajectoryType::Linear:
1820 return "Linear";
1821 case TrajectoryType::Circular:
1822 return "Circular";
1823 case TrajectoryType::Csv:
1824 return "CSV";
1825 case TrajectoryType::RoseFigure:
1826 return "Rose Figure";
1827 case TrajectoryType::COUNT:
1828 return "";
1829 }
1830 return "";
1831}
1832
1833const char* NAV::ImuSimulator::to_string(Direction value)
1834{
1835 switch (value)
1836 {
1837 case Direction::CW:
1838 return "Clockwise (CW)";
1839 case Direction::CCW:
1840 return "Counterclockwise (CCW)";
1841 case Direction::COUNT:
1842 return "";
1843 }
1844 return "";
1845}
Ellipsoid.hpp
Functions concerning the ellipsoid model.
FlowManager.hpp
Save/Load the Nodes.
json
nlohmann::json json
json namespace
Definition FlowManager.hpp:21
Gravity.hpp
Different Gravity Models.
HelpMarker.hpp
Text Help Marker (?) with Tooltip.
Functions.hpp
Inertial Navigation Helper Functions.
ImuObsSimulated.hpp
Data storage class for simulated IMU observations.
ImuSimulator.hpp
Imu Observation Simulator.
InsTime.hpp
The class is responsible for all time-related tasks.
Mechanization.hpp
Inertial Navigation Mechanization Functions in local navigation frame.
Logger.hpp
Utility class for logging to console and file.
LOG_DEBUG
#define LOG_DEBUG
Debug information. Should not be called on functions which receive observations (spamming)
Definition Logger.hpp:67
LOG_DATA
#define LOG_DATA
All output which occurs repeatedly every time observations are received.
Definition Logger.hpp:29
LOG_ERROR
#define LOG_ERROR
Error occurred, which stops part of the program to work, but not everything.
Definition Logger.hpp:73
LOG_TRACE
#define LOG_TRACE
Detailled info to trace the execution of the program. Should not be called on functions which receive...
Definition Logger.hpp:65
Math.hpp
Simple Math functions.
NodeManager.hpp
Manages all Nodes.
PosVelAtt.hpp
Position, Velocity and Attitude Storage Class.
NAV::CsvData::CsvLine
std::vector< CsvElement > CsvLine
CSV Line with splitted entries.
Definition CsvData.hpp:34
NAV::CsvData::type
static std::string type()
Returns the type of the data class.
Definition CsvData.hpp:28
NAV::ImuObsSimulated::type
static std::string type()
Returns the type of the data class.
Definition ImuObsSimulated.hpp:31
NAV::ImuSimulator::_startPosition
gui::widgets::PositionWithFrame _startPosition
Definition ImuSimulator.hpp:152
NAV::ImuSimulator::_roseSimDuration
double _roseSimDuration
Simulation duration needed for the rose figure.
Definition ImuSimulator.hpp:203
NAV::ImuSimulator::pitch
CubicSpline< long double > pitch
Pitch angle [rad].
Definition ImuSimulator.hpp:276
NAV::ImuSimulator::restore
void restore(const json &j) override
Restores the node from a json object.
Definition ImuSimulator.cpp:698
NAV::ImuSimulator::ImuSimulator
ImuSimulator()
Default constructor.
Definition ImuSimulator.cpp:36
NAV::ImuSimulator::deinitialize
void deinitialize() override
Deinitialize the node.
Definition ImuSimulator.cpp:1451
NAV::ImuSimulator::TrajectoryType
TrajectoryType
Types of Trajectories available for simulation.
Definition ImuSimulator.hpp:132
NAV::ImuSimulator::TrajectoryType::Linear
@ Linear
Linear movement with constant velocity.
Definition ImuSimulator.hpp:134
NAV::ImuSimulator::TrajectoryType::Fixed
@ Fixed
Static position without movement.
Definition ImuSimulator.hpp:133
NAV::ImuSimulator::TrajectoryType::COUNT
@ COUNT
Amount of items in the enum.
Definition ImuSimulator.hpp:138
NAV::ImuSimulator::TrajectoryType::Csv
@ Csv
Get the input from the CsvData pin.
Definition ImuSimulator.hpp:136
NAV::ImuSimulator::TrajectoryType::RoseFigure
@ RoseFigure
Movement along a mathmatical rose figure.
Definition ImuSimulator.hpp:137
NAV::ImuSimulator::pollPosVelAtt
std::shared_ptr< const NodeData > pollPosVelAtt(size_t pinIdx, bool peek)
Polls the next simulated data.
Definition ImuSimulator.cpp:1685
NAV::ImuSimulator::_imuInternalUpdateCnt
uint64_t _imuInternalUpdateCnt
Counter to calculate the internal IMU update time.
Definition ImuSimulator.hpp:285
NAV::ImuSimulator::_trajectoryRotationAngle
double _trajectoryRotationAngle
In the GUI selected origin angle of the circular trajectory in [rad].
Definition ImuSimulator.hpp:182
NAV::ImuSimulator::yaw
CubicSpline< long double > yaw
Yaw angle [rad].
Definition ImuSimulator.hpp:277
NAV::ImuSimulator::roll
CubicSpline< long double > roll
Roll angle [rad].
Definition ImuSimulator.hpp:275
NAV::ImuSimulator::StartTimeSource::CurrentComputerTime
@ CurrentComputerTime
Gets the current computer time as start time.
Definition ImuSimulator.hpp:107
NAV::ImuSimulator::StartTimeSource::CustomTime
@ CustomTime
Custom time selected by the user.
Definition ImuSimulator.hpp:106
NAV::ImuSimulator::_lla_imuLastLinearPosition
Eigen::Vector3d _lla_imuLastLinearPosition
Last calculated position for the IMU in linear mode for iterative calculations as latitude,...
Definition ImuSimulator.hpp:299
NAV::ImuSimulator::_circularHarmonicAmplitudeFactor
double _circularHarmonicAmplitudeFactor
Harmonic Oscillation Amplitude Factor of the circle radius [-].
Definition ImuSimulator.hpp:164
NAV::ImuSimulator::OUTPUT_PORT_INDEX_POS_VEL_ATT
static constexpr size_t OUTPUT_PORT_INDEX_POS_VEL_ATT
Flow (PosVelAtt)
Definition ImuSimulator.hpp:75
NAV::ImuSimulator::_trajectoryDirection
Direction _trajectoryDirection
In the GUI selected direction of the circular trajectory (used by circular and rose figure)
Definition ImuSimulator.hpp:179
NAV::ImuSimulator::z
CubicSpline< long double > z
ECEF Z Position [m].
Definition ImuSimulator.hpp:274
NAV::ImuSimulator::_imuInternalFrequency
double _imuInternalFrequency
Frequency to calculate the delta IMU values in [Hz].
Definition ImuSimulator.hpp:122
NAV::ImuSimulator::_csvDuration
double _csvDuration
Duration from the CSV file in [s].
Definition ImuSimulator.hpp:221
NAV::ImuSimulator::_gnssLastUpdateTime
double _gnssLastUpdateTime
Last time the GNSS message was calculated in [s].
Definition ImuSimulator.hpp:297
NAV::ImuSimulator::save
json save() const override
Saves the node into a json object.
Definition ImuSimulator.cpp:651
NAV::ImuSimulator::~ImuSimulator
~ImuSimulator() override
Destructor.
Definition ImuSimulator.cpp:48
NAV::ImuSimulator::n_getAttitudeQuaternionFromCsvLine_b
std::optional< Eigen::Quaterniond > n_getAttitudeQuaternionFromCsvLine_b(const CsvData::CsvLine &line, const std::vector< std::string > &description) const
Get the Attitude quaternion n_quat_b from a CSV line.
Definition ImuSimulator.cpp:928
NAV::ImuSimulator::_circularHarmonicFrequency
int _circularHarmonicFrequency
Harmonic Oscillation Frequency on the circular trajectory [cycles/revolution].
Definition ImuSimulator.hpp:161
NAV::ImuSimulator::_rosePetNum
int _rosePetNum
In the GUI selected numerator of petals (2*k for even k, k for uneven k) of the rose figure.
Definition ImuSimulator.hpp:194
NAV::ImuSimulator::OUTPUT_PORT_INDEX_IMU_OBS
static constexpr size_t OUTPUT_PORT_INDEX_IMU_OBS
Flow (ImuObs)
Definition ImuSimulator.hpp:74
NAV::ImuSimulator::_startTimeEditFormat
gui::widgets::TimeEditFormat _startTimeEditFormat
Time Format to input the start time with.
Definition ImuSimulator.hpp:114
NAV::ImuSimulator::_startTime
InsTime _startTime
Global starttime.
Definition ImuSimulator.hpp:117
NAV::ImuSimulator::_linearTrajectoryDistanceForStop
double _linearTrajectoryDistanceForStop
Distance in [m] to the start position to stop the simulation.
Definition ImuSimulator.hpp:224
NAV::ImuSimulator::category
static std::string category()
String representation of the Class Category.
Definition ImuSimulator.cpp:63
NAV::ImuSimulator::n_calcVelocity
Eigen::Vector3d n_calcVelocity(double time, const Eigen::Quaterniond &n_Quat_e) const
Calculates the velocity in local-navigation frame coordinates at the given time depending on the traj...
Definition ImuSimulator.cpp:1743
NAV::ImuSimulator::e_getPositionFromCsvLine
std::optional< Eigen::Vector3d > e_getPositionFromCsvLine(const CsvData::CsvLine &line, const std::vector< std::string > &description) const
Get the Position from a CSV line.
Definition ImuSimulator.cpp:892
NAV::ImuSimulator::_angularRateEarthRotationEnabled
bool _angularRateEarthRotationEnabled
Apply the Earth rotation rate to the measured angular rates.
Definition ImuSimulator.hpp:243
NAV::ImuSimulator::_imuLastUpdateTime
double _imuLastUpdateTime
Last time the IMU message was calculated in [s].
Definition ImuSimulator.hpp:295
NAV::ImuSimulator::_trajectoryVerticalSpeed
double _trajectoryVerticalSpeed
Vertical speed of the vehicle in the tangential plane in [m/s] (used by circular and rose figure)
Definition ImuSimulator.hpp:188
NAV::ImuSimulator::typeStatic
static std::string typeStatic()
String representation of the Class Type.
Definition ImuSimulator.cpp:53
NAV::ImuSimulator::_rosePetDenom
int _rosePetDenom
In the GUI selected denominator of petals (2*k for even k, k for uneven k) of the rose figure.
Definition ImuSimulator.hpp:197
NAV::ImuSimulator::INPUT_PORT_INDEX_CSV
static constexpr size_t INPUT_PORT_INDEX_CSV
Object (CsvData)
Definition ImuSimulator.hpp:73
NAV::ImuSimulator::_trajectoryType
TrajectoryType _trajectoryType
Selected trajectory type in the GUI.
Definition ImuSimulator.hpp:146
NAV::ImuSimulator::Direction
Direction
Possible directions for the circular trajectory.
Definition ImuSimulator.hpp:168
NAV::ImuSimulator::Direction::COUNT
@ COUNT
Amount of items in the enum.
Definition ImuSimulator.hpp:171
NAV::ImuSimulator::_roseTrajectoryCountForStop
double _roseTrajectoryCountForStop
Amount of rose figures to simulate before stopping.
Definition ImuSimulator.hpp:230
NAV::ImuSimulator::_roseStepLengthMax
double _roseStepLengthMax
Maxmimum step length for the spline points for the rose figure [m]. Points will be spaced between [L/...
Definition ImuSimulator.hpp:200
NAV::ImuSimulator::guiConfig
void guiConfig() override
ImGui config window which is shown on double click.
Definition ImuSimulator.cpp:68
NAV::ImuSimulator::y
CubicSpline< long double > y
ECEF Y Position [m].
Definition ImuSimulator.hpp:273
NAV::ImuSimulator::_centrifgalAccelerationEnabled
bool _centrifgalAccelerationEnabled
Apply the centrifugal acceleration to the measured accelerations.
Definition ImuSimulator.hpp:240
NAV::ImuSimulator::_simulationDuration
double _simulationDuration
Duration to simulate in [s].
Definition ImuSimulator.hpp:218
NAV::ImuSimulator::resetNode
bool resetNode() override
Resets the node. Moves the read cursor to the start.
Definition ImuSimulator.cpp:1456
NAV::ImuSimulator::DistanceOrCirclesOrRoses
@ DistanceOrCirclesOrRoses
Distance for Linear trajectory / Circle count for Circular / Count for rose figure trajectory.
Definition ImuSimulator.hpp:211
NAV::ImuSimulator::Duration
@ Duration
Time Duration.
Definition ImuSimulator.hpp:210
NAV::ImuSimulator::calcFlightAngles
std::array< double, 3 > calcFlightAngles(double time) const
Calculates the flight angles (roll, pitch, yaw)
Definition ImuSimulator.cpp:1726
NAV::ImuSimulator::initializeSplines
bool initializeSplines()
Initializes the spline values.
Definition ImuSimulator.cpp:966
NAV::ImuSimulator::_circularTrajectoryCircleCountForStop
double _circularTrajectoryCircleCountForStop
Amount of circles to simulate before stopping.
Definition ImuSimulator.hpp:227
NAV::ImuSimulator::pollImuObs
std::shared_ptr< const NodeData > pollImuObs(size_t pinIdx, bool peek)
Polls the next simulated data.
Definition ImuSimulator.cpp:1537
NAV::ImuSimulator::_n_linearTrajectoryStartVelocity
Eigen::Vector3d _n_linearTrajectoryStartVelocity
Start Velocity of the vehicle in local-navigation frame cooridnates in [m/s].
Definition ImuSimulator.hpp:158
NAV::ImuSimulator::_imuUpdateCnt
uint64_t _imuUpdateCnt
Counter to calculate the IMU update time.
Definition ImuSimulator.hpp:287
NAV::ImuSimulator::n_calcOmega_nb
Eigen::Vector3d n_calcOmega_nb(double time, const Eigen::Vector3d &rollPitchYaw, const Eigen::Quaterniond &n_Quat_b) const
Calculates ω_nb_n, the turn rate of the body with respect to the navigation system expressed in NED c...
Definition ImuSimulator.cpp:1775
NAV::ImuSimulator::lla_calcPosition
Eigen::Vector3d lla_calcPosition(double time) const
Calculates the position in latLonAlt at the given time depending on the trajectoryType.
Definition ImuSimulator.cpp:1735
NAV::ImuSimulator::_angularRateTransportRateEnabled
bool _angularRateTransportRateEnabled
Apply the transport rate to the measured angular rates.
Definition ImuSimulator.hpp:246
NAV::ImuSimulator::to_string
static const char * to_string(TrajectoryType value)
Converts the enum to a string.
Definition ImuSimulator.cpp:1813
NAV::ImuSimulator::type
std::string type() const override
String representation of the Class Type.
Definition ImuSimulator.cpp:58
NAV::ImuSimulator::_splines
struct NAV::ImuSimulator::@112323040341172005327360071040367216370163224314 _splines
Assign a variable that holds the Spline information.
NAV::ImuSimulator::_gravitationModel
GravitationModel _gravitationModel
Gravitation model selected in the GUI.
Definition ImuSimulator.hpp:234
NAV::ImuSimulator::checkStopCondition
bool checkStopCondition(double time, const Eigen::Vector3d &lla_position)
Checks the selected stop condition.
Definition ImuSimulator.cpp:1503
NAV::ImuSimulator::_gnssUpdateCnt
uint64_t _gnssUpdateCnt
Counter to calculate the GNSS update time.
Definition ImuSimulator.hpp:289
NAV::ImuSimulator::_lla_gnssLastLinearPosition
Eigen::Vector3d _lla_gnssLastLinearPosition
Last calculated position for the GNSS in linear mode for iterative calculations as latitude,...
Definition ImuSimulator.hpp:301
NAV::ImuSimulator::_simulationStopCondition
StopCondition _simulationStopCondition
Condition which has to be met to stop the simulation.
Definition ImuSimulator.hpp:215
NAV::ImuSimulator::_trajectoryRadius
double _trajectoryRadius
In the GUI selected radius of the circular trajectory (used by circular and rose figure)
Definition ImuSimulator.hpp:191
NAV::ImuSimulator::_startTimeSource
StartTimeSource _startTimeSource
Source for the start time, selected in the GUI.
Definition ImuSimulator.hpp:111
NAV::ImuSimulator::_p_lastImuAccelerationMeas
Eigen::Vector3d _p_lastImuAccelerationMeas
Last calculated acceleration measurement in platform coordinates [m/s²].
Definition ImuSimulator.hpp:303
NAV::ImuSimulator::_trajectoryHorizontalSpeed
double _trajectoryHorizontalSpeed
Horizontal speed of the vehicle in the tangential plane in [m/s] (used by circular and rose figure)
Definition ImuSimulator.hpp:185
NAV::ImuSimulator::_coriolisAccelerationEnabled
bool _coriolisAccelerationEnabled
Apply the coriolis acceleration to the measured accelerations.
Definition ImuSimulator.hpp:237
NAV::ImuSimulator::_p_lastImuAngularRateMeas
Eigen::Vector3d _p_lastImuAngularRateMeas
Last calculated angular rate measurement in platform coordinates [rad/s].
Definition ImuSimulator.hpp:305
NAV::ImuSimulator::n_calcTrajectoryAccel
Eigen::Vector3d n_calcTrajectoryAccel(double time, const Eigen::Quaterniond &n_Quat_e, const Eigen::Vector3d &lla_position, const Eigen::Vector3d &n_velocity) const
Calculates the acceleration in local-navigation frame coordinates at the given time depending on the ...
Definition ImuSimulator.cpp:1751
NAV::ImuSimulator::initialize
bool initialize() override
Initialize the node.
Definition ImuSimulator.cpp:1444
NAV::ImuSimulator::getTimeFromCsvLine
std::optional< InsTime > getTimeFromCsvLine(const CsvData::CsvLine &line, const std::vector< std::string > &description) const
Get the Time from a CSV line.
Definition ImuSimulator.cpp:846
NAV::ImuSimulator::x
CubicSpline< long double > x
ECEF X Position [m].
Definition ImuSimulator.hpp:272
NAV::ImuSimulator::_gnssFrequency
double _gnssFrequency
Frequency to sample the position with in [Hz].
Definition ImuSimulator.hpp:126
NAV::ImuSimulator::_imuFrequency
double _imuFrequency
Frequency to sample the IMU with in [Hz].
Definition ImuSimulator.hpp:124
NAV::ImuSimulator::_fixedTrajectoryStartOrientation
Eigen::Vector3d _fixedTrajectoryStartOrientation
Orientation of the vehicle [roll, pitch, yaw] [rad].
Definition ImuSimulator.hpp:155
NAV::Imu::save
json save() const override
Saves the node into a json object.
Definition Imu.cpp:93
NAV::Imu::restore
void restore(const json &j) override
Restores the node from a json object.
Definition Imu.cpp:104
NAV::Imu::guiConfig
void guiConfig() override
ImGui config window which is shown on double click.
Definition Imu.cpp:55
NAV::Imu::Imu
Imu(const Imu &)=delete
Copy constructor.
NAV::Imu::_imuPos
ImuPos _imuPos
Position and rotation information for conversion from platform to body frame.
Definition Imu.hpp:57
NAV::InsConst::e_omega_ie
static const Eigen::Vector3< double > e_omega_ie
ω_ie_e = ω_ie_i Nominal mean angular velocity of the Earth in [rad/s], in earth coordinates
Definition Constants.hpp:222
NAV::InsTime
The class is responsible for all time-related tasks.
Definition InsTime.hpp:710
NAV::Node::doDeinitialize
bool doDeinitialize(bool wait=false)
Asks the node worker to deinitialize the node.
Definition Node.cpp:395
NAV::Node::_guiConfigDefaultWindowSize
ImVec2 _guiConfigDefaultWindowSize
Definition Node.hpp:410
NAV::Node::getInputValue
std::optional< InputPin::IncomingLink::ValueWrapper< T > > getInputValue(size_t portIndex) const
Get Input Value connected on the pin. Only const data types.
Definition Node.hpp:290
NAV::Node::inputPins
std::vector< InputPin > inputPins
List of input pins.
Definition Node.hpp:397
NAV::Node::nameId
std::string nameId() const
Node name and id.
Definition Node.cpp:253
NAV::Node::name
std::string name
Name of the Node.
Definition Node.hpp:395
NAV::Node::invokeCallbacks
void invokeCallbacks(size_t portIndex, const std::shared_ptr< const NodeData > &data)
Calls all registered callbacks on the specified output port.
Definition Node.cpp:180
NAV::Node::_hasConfig
bool _hasConfig
Flag if the config window should be shown.
Definition Node.hpp:413
NAV::PosVelAtt::type
static std::string type()
Returns the type of the data class.
Definition PosVelAtt.hpp:29
NAV::gui::NodeEditorApplication::windowFontRatio
static float windowFontRatio()
Ratio to multiply for GUI window elements.
Definition NodeEditorApplication.cpp:1916
NAV::gui::NodeEditorApplication::m_RoseFigure
static ImTextureID m_RoseFigure
Pointer to the texture for the rose figure (ImuSimulator node)
Definition NodeEditorApplication.hpp:111
imgui_ex.hpp
ImGui extensions.
ImGui::InputIntL
bool InputIntL(const char *label, int *v, int v_min, int v_max, int step, int step_fast, ImGuiInputTextFlags flags)
Shows a value limited InputText GUI element for 'int'.
Definition imgui_ex.cpp:242
ImGui::DragDouble
bool DragDouble(const char *label, double *v, float v_speed, double v_min, double v_max, const char *format, ImGuiSliderFlags flags)
Shows a Drag GUI element for 'double'.
Definition imgui_ex.cpp:19
ImGui::InputDoubleL
bool InputDoubleL(const char *label, double *v, double v_min, double v_max, double step, double step_fast, const char *format, ImGuiInputTextFlags flags)
Shows a value limited InputText GUI element for 'double'.
Definition imgui_ex.cpp:294
NAV::NodeManager::CreateOutputPin
OutputPin * CreateOutputPin(Node *node, const char *name, Pin::Type pinType, const std::vector< std::string > &dataIdentifier, OutputPin::PinData data=static_cast< void * >(nullptr), int idx=-1)
Create an Output Pin object.
Definition NodeManager.cpp:202
NAV::NodeManager::DeleteInputPin
bool DeleteInputPin(InputPin &pin)
Deletes the input pin. Invalidates the pin reference given.
Definition NodeManager.cpp:234
NAV::NodeManager::CreateInputPin
InputPin * CreateInputPin(Node *node, const char *name, Pin::Type pinType, const std::vector< std::string > &dataIdentifier={}, InputPin::Callback callback=static_cast< InputPin::FlowFirableCallbackFunc >(nullptr), InputPin::FlowFirableCheckFunc firable=nullptr, int priority=0, int idx=-1)
Create an Input Pin object.
Definition NodeManager.cpp:176
NAV::flow::ApplyChanges
void ApplyChanges()
Signals that there have been changes to the flow.
Definition FlowManager.cpp:442
NAV::gui::widgets::BeginHelpMarker
bool BeginHelpMarker(const char *symbol="(?)", float textWrapLength=35.0F)
Begins a Text Help Marker, e.g. '(?)', with custom content.
Definition HelpMarker.cpp:28
NAV::gui::widgets::TimeEdit
bool TimeEdit(const char *str_id, InsTime &insTime, TimeEditFormat &timeEditFormat, float itemWidth=170.0F)
Inputs to edit an InsTime object.
Definition TimeEdit.cpp:52
NAV::gui::widgets::EndHelpMarker
void EndHelpMarker(bool wrapText=true)
Ends a Text Help Marker with custom content.
Definition HelpMarker.cpp:44
NAV::gui::widgets::PositionInput
bool PositionInput(const char *str, PositionWithFrame &position, PositionInputLayout layout=PositionInputLayout::SINGLE_COLUMN, float itemWidth=140.0F)
Inputs to edit an Position object.
Definition PositionInput.cpp:55
NAV::gui::widgets::HelpMarker
void HelpMarker(const char *desc, const char *symbol="(?)")
Text Help Marker, e.g. '(?)', with Tooltip.
Definition HelpMarker.cpp:14
NAV::math::calcEllipticalIntegral
double calcEllipticalIntegral(double phi, double m)
Calculates the incomplete elliptical integral of the second kind.
Definition Math.cpp:53
NAV::math::skewSymmetricMatrix
Eigen::Matrix< typename Derived::Scalar, 3, 3 > skewSymmetricMatrix(const Eigen::MatrixBase< Derived > &a)
Calculates the skew symmetric matrix of the given vector. This is needed to perform the cross product...
Definition Math.hpp:90
NAV::trafo::ecef2lla_WGS84
Eigen::Vector3< typename Derived::Scalar > ecef2lla_WGS84(const Eigen::MatrixBase< Derived > &e_position)
Converts Earth-centered-Earth-fixed coordinates into latitude, longitude and altitude using WGS84.
Definition CoordinateFrames.hpp:420
NAV::trafo::b_Quat_n
Eigen::Quaternion< Scalar > b_Quat_n(const Scalar &roll, const Scalar &pitch, const Scalar &yaw)
Quaternion for rotations from navigation to body frame.
Definition CoordinateFrames.hpp:331
NAV::trafo::e_Quat_n
Eigen::Quaternion< Scalar > e_Quat_n(const Scalar &latitude, const Scalar &longitude)
Quaternion for rotations from navigation to Earth-fixed frame.
Definition CoordinateFrames.hpp:305
NAV::trafo::n_Quat_e
Eigen::Quaternion< Scalar > n_Quat_e(const Scalar &latitude, const Scalar &longitude)
Quaternion for rotations from Earth-fixed to navigation frame.
Definition CoordinateFrames.hpp:320
NAV::trafo::quat2eulerZYX
Eigen::Vector3< typename Derived::Scalar > quat2eulerZYX(const Eigen::QuaternionBase< Derived > &q)
Converts the quaternion to Euler rotation angles with rotation sequence ZYX.
Definition CoordinateFrames.hpp:141
NAV::trafo::lla2ecef_WGS84
Eigen::Vector3< typename Derived::Scalar > lla2ecef_WGS84(const Eigen::MatrixBase< Derived > &lla_position)
Converts latitude, longitude and altitude into Earth-centered-Earth-fixed coordinates using WGS84.
Definition CoordinateFrames.hpp:402
NAV::trafo::n_Quat_b
Eigen::Quaternion< Scalar > n_Quat_b(Scalar roll, Scalar pitch, Scalar yaw)
Quaternion for rotations from body to navigation frame.
Definition CoordinateFrames.hpp:357
NAV::UTC
@ UTC
Coordinated Universal Time.
Definition TimeSystem.hpp:29
NAV::n_calcGravitation
Eigen::Vector3< typename Derived::Scalar > n_calcGravitation(const Eigen::MatrixBase< Derived > &lla_position, GravitationModel gravitationModel=GravitationModel::EGM96)
Calculates the gravitation (acceleration due to mass attraction of the Earth)
Definition Gravity.hpp:193
NAV::e_calcCentrifugalAcceleration
Eigen::Vector3< typename DerivedA::Scalar > e_calcCentrifugalAcceleration(const Eigen::MatrixBase< DerivedA > &e_position, const Eigen::MatrixBase< DerivedB > &e_omega_ie=InsConst::e_omega_ie)
Calculates the centrifugal acceleration in [m/s^2] (acceleration that makes a body follow a curved pa...
Definition Functions.hpp:72
NAV::n_calcCoriolisAcceleration
Eigen::Vector3< typename DerivedA::Scalar > n_calcCoriolisAcceleration(const Eigen::MatrixBase< DerivedA > &n_omega_ie, const Eigen::MatrixBase< DerivedB > &n_omega_en, const Eigen::MatrixBase< DerivedC > &n_velocity)
Calculates the coriolis acceleration in [m/s^2] (acceleration due to motion in rotating reference fra...
Definition Functions.hpp:90
NAV::calcGeographicalDistance
Scalar calcGeographicalDistance(Scalar lat1, Scalar lon1, Scalar lat2, Scalar lon2)
Measure the distance between two points over an ellipsoidal-surface.
Definition Ellipsoid.hpp:107
NAV::calcEarthRadius_N
Scalar calcEarthRadius_N(const Scalar &latitude, const Scalar &a=InsConst::WGS84::a, const Scalar &e_squared=InsConst::WGS84::e_squared)
Calculates the North/South (meridian) earth radius.
Definition Ellipsoid.hpp:42
NAV::to_string
const char * to_string(gui::widgets::PositionWithFrame::ReferenceFrame refFrame)
Converts the enum to a string.
Definition PositionInput.cpp:25
NAV::n_calcTransportRate
Eigen::Vector3< typename DerivedA::Scalar > n_calcTransportRate(const Eigen::MatrixBase< DerivedA > &lla_position, const Eigen::MatrixBase< DerivedB > &n_velocity, const auto &R_N, const auto &R_E)
Calculates the transport rate ω_en_n (rotation rate of the Earth frame relative to the navigation fra...
Definition Functions.hpp:39
NAV::ComboGravitationModel
bool ComboGravitationModel(const char *label, GravitationModel &gravitationModel)
Shows a ComboBox to select the gravitation model.
Definition Gravity.cpp:38
NAV::calcPitchFromVelocity
Derived::Scalar calcPitchFromVelocity(const Eigen::MatrixBase< Derived > &n_velocity)
Calculates the Pitch angle from the trajectory defined by the given velocity.
Definition Functions.hpp:169
NAV::deg2rad
constexpr auto deg2rad(const T &deg)
Convert Degree to Radians.
Definition Units.hpp:21
NAV::calcYawFromVelocity
Derived::Scalar calcYawFromVelocity(const Eigen::MatrixBase< Derived > &n_velocity)
Calculates the Yaw angle from the trajectory defined by the given velocity.
Definition Functions.hpp:153
NAV::lla_calcTimeDerivativeForPosition
Eigen::Vector3< typename Derived::Scalar > lla_calcTimeDerivativeForPosition(const Eigen::MatrixBase< Derived > &n_velocity, const auto &phi, const auto &h, const auto &R_N, const auto &R_E)
Calculates the time derivative of the curvilinear position.
Definition Mechanization.hpp:119
NAV::rad2deg
constexpr auto rad2deg(const T &rad)
Convert Radians to Degree.
Definition Units.hpp:39
NAV::calcEarthRadius_E
Scalar calcEarthRadius_E(const Scalar &latitude, const Scalar &a=InsConst::WGS84::a, const Scalar &e_squared=InsConst::WGS84::e_squared)
Calculates the East/West (prime vertical) earth radius.
Definition Ellipsoid.hpp:58
NAV::Pin::Type::Flow
@ Flow
NodeData Trigger.
Definition Pin.hpp:52
NAV::Pin::Type::Object
@ Object
Generic Object.
Definition Pin.hpp:57