diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..6ad8c0a --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,162 @@ +cmake_minimum_required(VERSION 3.8) +project(imu_simulator_package) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(diagnostic_msgs REQUIRED) +find_package(tf2_ros REQUIRED) + +find_package(eigen3_cmake_module REQUIRED) +find_package(Eigen3 REQUIRED) + +include_directories( + include/imu_simulator_package + ${EIGEN3_INCLUDE_DIR} + ${YAML_CPP_INCLUDE_DIR}) + +set(HEADER_FILES + include/imu_simulator_package/imu_simulator.h) + +add_library(imu_simulator SHARED + src/imu_simulator.cpp + ${HEADER_FILES}) + +# add imu_simulator_node +add_executable(${PROJECT_NAME}_node src/imu_simulator_node.cpp) + +ament_target_dependencies(${PROJECT_NAME}_node PUBLIC + rclcpp + std_msgs + sensor_msgs + nav_msgs + geometry_msgs + diagnostic_msgs + tf2_ros + Eigen3) + +target_link_libraries(${PROJECT_NAME}_node PUBLIC imu_simulator) + +target_include_directories(${PROJECT_NAME}_node PUBLIC + $ + $) + +target_compile_features(${PROJECT_NAME}_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 + +install(TARGETS + ${PROJECT_NAME}_node + DESTINATION + lib/${PROJECT_NAME}) + +# add csv_test_executable +add_executable(imu_simulator_static_csv src/imu_simulator_static_csv.cpp) + +ament_target_dependencies(imu_simulator_static_csv PUBLIC + Eigen3) + +target_link_libraries(imu_simulator_static_csv PUBLIC imu_simulator) + +target_include_directories(imu_simulator_static_csv PUBLIC + $ + $) + +target_compile_features(imu_simulator_static_csv PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 + +install(TARGETS + imu_simulator_static_csv + DESTINATION lib/${PROJECT_NAME}) + +install(TARGETS imu_simulator + EXPORT imu_simulator + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include) + +# add odometry test publisher +add_executable(ground_truth_test_publisher_node src/ground_truth_test_publisher_node.cpp) + +ament_target_dependencies(ground_truth_test_publisher_node PUBLIC + rclcpp + nav_msgs + geometry_msgs + tf2_ros + Eigen3) + +target_link_libraries(ground_truth_test_publisher_node PUBLIC imu_simulator) + +target_include_directories(ground_truth_test_publisher_node PUBLIC + $ + $) + +target_compile_features(ground_truth_test_publisher_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 + +install(TARGETS + ground_truth_test_publisher_node + DESTINATION lib/${PROJECT_NAME}) + +install(DIRECTORY + launch + config + DESTINATION + share/${PROJECT_NAME}/) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + + # the following line skips uncrustify (source code formatting) + # set(ament_cmake_uncrustify_FOUND TRUE) + + find_package(ament_cmake_gtest REQUIRED) + + set(TEST_FILES + test/main.cpp + test/imu_simulator_test.cpp) + + ament_add_gtest(${PROJECT_NAME}_test ${TEST_FILES}) + + target_link_libraries(${PROJECT_NAME}_test imu_simulator) + + install(TARGETS + ${PROJECT_NAME}_test + DESTINATION lib/${PROJECT_NAME}) + + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + + list(APPEND AMENT_LINT_AUTO_EXCLUDE + ament_cmake_uncrustify) + + # enforce linters and static code analyzers defined in ament_lint_common package + ament_lint_auto_find_test_dependencies() + + # uncomment to include uncrustify explicitly + # find_package(ament_cmake_uncrustify) + # ament_uncrustify(CONFIG_FILE "./uncrustify.cfg" TESTNAME "custom_uncrustify") + +endif() + +ament_package() \ No newline at end of file diff --git a/CODE_OF_CONDUCT b/CODE_OF_CONDUCT new file mode 100644 index 0000000..ea370e0 --- /dev/null +++ b/CODE_OF_CONDUCT @@ -0,0 +1,24 @@ +# Contributor Code of Conduct + +As contributors and maintainers of this project, we pledge to respect all people who contribute through reporting issues, posting feature requests, updating documentation, submitting pull requests, and other activities. + +We are committed to making participation in this project a harassment-free experience for everyone, regardless of level of experience, gender, gender identity and expression, sexual orientation, disability, personal appearance, body size, race, ethnicity, age, religion, or nationality. + +Examples of unacceptable behavior by participants include: + +- The use of sexualized language or imagery +- Personal attacks +- Trolling or insulting/derogatory comments +- Public or private harassment +- Publishing others' private information, such as physical or electronic addresses, without explicit permission +- Other unethical or unprofessional conduct + +Project maintainers have the right and responsibility to remove, edit, or reject comments, commits, code, wiki edits, issues, and other contributions that are not aligned to this Code of Conduct. Project maintainers who do not follow the Code of Conduct may be removed from the project team. + +This Code of Conduct applies both within project spaces and in public spaces when an individual is representing the project or its community. + +Instances of abusive, harassing, or otherwise unacceptable behavior may be reported by contacting the project team at [m.nitsch@irt.rwth-aachen.de](mailto:m.nitsch@irt.rwth-aachen.de). All complaints will be reviewed and investigated and will result in a response that is deemed necessary and appropriate to the circumstances. The project team is obligated to maintain confidentiality with regard to the reporter of an incident. Further details of specific enforcement policies may be posted separately. + +Project maintainers are responsible for clarifying the standards of acceptable behavior and are expected to take appropriate and fair corrective action in response to any instances of unacceptable behavior. + +This Code of Conduct is adapted from the Contributor Covenant, version 1.4, available at https://www.contributor-covenant.org/version/1/4/code-of-conduct.html diff --git a/CONTRIBUTING b/CONTRIBUTING new file mode 100644 index 0000000..63eed73 --- /dev/null +++ b/CONTRIBUTING @@ -0,0 +1,42 @@ +# Contributing to Project Name + +Thank you for considering contributing to our project! Here are some guidelines to help you get started. + +## Code of Conduct + +Please note that this project is governed by our [Code of Conduct](CODE_OF_CONDUCT.md). Please review it before contributing. + +## Getting Started + +To start contributing, follow these steps: + +1. Fork the repository on GitHub. +2. Clone your forked repository to your local machine. +3. Create a new branch for your contribution: `git checkout -b feature/my-feature`. +4. Make your changes and commit them: `git commit -m "Add new feature"`. +5. Push your changes to your forked repository: `git push origin feature/my-feature`. +6. Open a pull request on GitHub. + +## Reporting Issues + +If you encounter any issues with the project, please [open an issue](https://github.com/rwth-irt/IMU-Simulator/issues) on GitHub and provide as much detail as possible. + +## Code Style + +Follow the existing code style and conventions of the project. + +Please follow these code style and conventions: +- [Google C++ Style Guide](https://google.github.io/styleguide/cppguide.html) +- [ROS 2 Code Style Guide](http://docs.ros.org/en/humble/The-ROS2-Project/Contributing/Code-Style-Language-Versions.html) +- [ROS 2 Developer Guide](https://docs.ros.org/en/rolling/The-ROS2-Project/Contributing/Developer-Guide.html#package-layout) + +Make sure your code is well-formatted and documented. Please use [Doxygen](https://www.doxygen.nl/) for commenting. + +## Testing + +Make sure your code is well-tested. Please use [GoogleTest](http://google.github.io/googletest/) for the core functionalities. +Before submitting a pull request, make sure all tests pass and add new tests for any new functionality or changes. + +## Licensing + +By contributing to this project, you agree to license your contributions under the [LICENSE](LICENSE) of the project. diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..f871b5b --- /dev/null +++ b/LICENSE @@ -0,0 +1,35 @@ +BSD 3-Clause License + +Copyright (c) [2023], [Maximilian Nitsch] +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, + this list of conditions and the following disclaimer. + +2. Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + +3. Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +------------------------------------------------------------------------ +This software is provided by [Maximilian Nitsch] at [Institute of Automatic Control - RWTH Aachen University]. + +Contact Information: +[m.nitsch@irt.rwth-aachen.de] + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/README.md b/README.md new file mode 100644 index 0000000..301547f --- /dev/null +++ b/README.md @@ -0,0 +1,179 @@ +# IRT C++/ROS 2 IMU-Simulator + +[![License](https://img.shields.io/badge/license-BSD--3-blue.svg)](https://opensource.org/licenses/BSD-3-Clause) + +![](./data/icon.png){width=25%} + + +**Author:** +- Maximilian Nitsch + +**Affiliation:** Institute of Automatic Control - RWTH Aachen University + +**Maintainer:** + - Maximilian Nitsch + + +## Description +This project provides a high-fidelity IMU simulator written in C++. + +The simulator implements the following features: +- Accelerometer and gyroscope measurement simulation +- WELMEC gravity model (accelerometer) +- WGS84 Earth angular velocity model (gyroscope) +- Transport rate angular velocity model (gyroscope) +- Turn-on bias +- Scaling errors +- Misalignment and orthogonality errors +- Stochastic noise (colored/non-white noise) + - Velocity/angular random walk + - Bias instability + - Acceleration/rate random walk +- Saturation +- Quantization errors +- All parameters for an IMU can be configured in a YAML file +- All models and effects can be enabled/disabled separately + +Example configs from real data of a STIM300 IMU and a 3DM-GX5-25 LORD Microstrain IMU is provided. + +MATLAB scripts are provided to extract the stochastic noise components using Allan variance analysis. +For this, you need to provide a long-term (min. 6h) dataset of acceleration/gyroscope measurements. +The IMU needs be static, temperature- and vibration-compensated during the recording. + +## Table of Contents + +- [Dependencies](#dependencies) +- [Installation](#installation) +- [Usage](#usage) +- [ROS 2 Nodes](#ros-2-nodes) + - [Publisher Node](#publisher-node) + - [Subscriber Node](#subscriber-node) +- [Coding Guidelines](#coding-guidelines) +- [References](#references) +- [Contributing](#contributing) +- [License](#license) + +# Dependencies + +This project depends on the following literature and libraries: + +- **Eigen3**: Eigen is a C++ template library for linear algebra: [Eigen website](https://eigen.tuxfamily.org/). +- **ROS 2 Humble**: ROS 2 is a set of software libraries and tools for building robot applications: [ROS 2 Installation page](https://docs.ros.org/en/humble/Installation.html)). + + +# Installation + +To install the `imu_simulator_package`, you need to follow these steps: + +1. **Install Eigen3**: Eigen3 is a dependency for your package. You can install it using your package manager. For example, on Ubuntu, you can install it using the following command: + + ```bash + sudo apt-get install libeigen3-dev + ``` + +2. **Install ROS 2 Humble**: Make sure you have ROS 2 (Humble) installed. You can follow the official installation instructions provided by ROS 2. Visit [ROS 2 Humble Installation page](https://docs.ros.org/en/humble/Installation.html) for detailed installation instructions tailored to your platform. + +3. **Clone the Package**: Clone the package repository to your ROS 2 workspace. If you don't have a ROS 2 workspace yet, you can create one using the following commands: + + ```bash + mkdir -p /path/to/ros2_workspace/src + cd /path/to/ros2_workspace/src + ``` + + Now, clone the package repository: + + ```bash + git clone + ``` + + Replace `` with the URL of your package repository. + +4. **Build the Package**: Once the package is cloned, you need to build it using colcon, the default build system for ROS 2. Navigate to your ROS 2 workspace and run the following command: + + ```bash + cd /path/to/ros2_workspace + colcon build + ``` + + This command will build all the packages in your workspace, including the newly added package. + +5. **Source the Workspace**: After building the package, you need to source your ROS 2 workspace to make the package available in your ROS 2 environment. Run the following command: + + ```bash + source /path/to/ros2_workspace/install/setup.bash + ``` + + Replace `/path/to/ros2_workspace` with the actual path to your ROS 2 workspace. + +That's it! Your `imu_simulator_package` should now be installed along with its dependencies, and ready to use in your ROS 2 environment. + +## Usage + +1. **Configure your YAML file** for your IMU or use the default file. + +2. **Start the IMU simulator** with the launch file: + ```bash + ros2 launch imu_simulator_package imu_simulator.launch.py + ``` + The IMU simulator prints your settings and now waits for a ground truth odometry message. + +3. **Provide an odometry publisher** from you vehicle simulation. + For testing, you can launch the odometry_test_publisher node: + ```bash + ros2 launch imu_simulator_package odometry_test_publisher.py + ``` + +4. The IMU values should now be published. + + +**Important Usage Information**: +- The odometry message needs to be published with at least the IMU data rate/sample time. +- The message `/imu/diagnostic` will show `WARN` if odometry rate is lower. +- If no odometry message is published, the message `/imu/diagnostic` will show `STALE`. +- If everything is correct `/imu/diagnostic` will show `OK`. + +## ROS 2 Nodes + +The IMU simulator node implements two publishers and subscribes to one topic. +ROS 2 services or actions are not provided. + +### Publisher Node + +This node publishes the following topics: + +| Topic Name | Message Type | Description | +|------------------|---------------------|------------------------------------| +| `/nanoauv/sensor/navigation/imu/data` | `sensor_msgs/Imu.msg` | Publishes IMU sensor data.| +| `/nanoauv/sensor/navigation/imu/diagnostic` | `diagnostic_msgs/DiagnosticStatus.msg` | Publishes the diagnostic status of the IMU data. + +### Subscriber Node + +This node subscribes to the following topics: + +| Topic Name | Message Type | Description | +|-------------------|---------------------|------------------------------------| +| `/nanoauv/odometry`| `nav_msgs/Odometry.msg`| Subscribes to ground truth vehicle odometry.| + +## Coding Guidelines + +This project follows these coding guidelines: +- https://google.github.io/styleguide/cppguide.html +- http://docs.ros.org/en/humble/The-ROS2-Project/Contributing/Code-Style-Language-Versions.html + +## References + +The IMU simulator implementation closely follows the work: +- J. A. Farrell, F. O. Silva, F. Rahman and J. Wendel, "Inertial Measurement Unit Error Modeling Tutorial: Inertial Navigation System State Estimation with Real-Time Sensor Calibration," in IEEE Control Systems Magazine, vol. 42, no. 6, pp. 40-66, Dec. 2022, [DOI: 10.1109/MCS.2022.3209059](https://doi.org/10.1109/MCS.2022.3209059). +- J. A. Farrell, "Aided Navigation Systems: GPS and High Rate Sensors," New York, NY, McGraw-Hill, 552 pages, 2008. + +The MATLAB scripts are in parts taken from: +- [AV-MATLAB-SW](https://github.com/jaffarrell/AV-Matlab-SW) + +## Contributing + +If you would like to contribute to the project, see the [CONTRIBUTING](CONTRIBUTING) file for details. + +## License + +This project is licensed under the BSD-3-Clause License. See the [LICENSE](LICENSE) file for details. + diff --git a/config/stim300.yaml b/config/stim300.yaml new file mode 100644 index 0000000..e8a6790 --- /dev/null +++ b/config/stim300.yaml @@ -0,0 +1,63 @@ +# @license BSD-3 https://opensource.org/licenses/BSD-3-Clause +# Copyright (c) 2024, Institute of Automatic Control - RWTH Aachen University +# Maximilian Nitsch (m.nitsch@irt.rwth-aachen.de) +# All rights reserved. + +imu_simulator: + general_settings: + sample_time: 0.008 # (s) + seed: 42 + use_constant_seed: false + calc_accel_from_odometry_velocity: false # set to calculate acceleration from numerical differentiation of odometry velocity + + model_parameter_settings: + accelerometer: + N: [0.00144813183998857, 0.00144218525538188, 0.00145192879193856] # (m/s^(3/2)) + B: [0.000259469978374161, 0.000284359672626612, 0.000266572030700551] # (m/s^2) + K: [9.72968195066955e-06, 1.50448806164571e-05, 5.61621872457085e-06] # (m/s^(5/2)) + correlation_time: [44.2425330327468, 29.931356998205, 175.918924554171] + interval_turn_on_bias: [0.04903325, 0.04903325, 0.04903325] # (m/s^2) + measurement_range: [49.03325, 49.03325, 49.03325] # (m/s^2) + interval_scale_factor: [0.2, 0.2, 0.2] # (%) + interval_misalignment: [1e-3, 1e-3, 1e-3] # (rad) + interval_non_orthogonality: [2e-04, 2e-04, 2e-04] # (rad) + resolution: [5.8447634e-06, 5.8447634e-06, 5.8447634e-06] # (m/s^2) + + gyroscope: + N: [0.000207479860047933, 0.000243411420514079, 0.000187943045943727] # (rad/s^(1/2)) + B: [8.601448819572e-07, 9.49331921834923e-07, 9.20618084887883e-07] # (rad/s) + K: [1.85470342042531e-07, 1.1738725498127e-07, 2.26095960190654e-07] # (rad/s^(3/2)) + correlation_time: [1315.9775377824, 5263.99037881102, 124.14459722317] # (s) + interval_turn_on_bias: [ + 4.84813681109536e-04, + 4.84813681109536e-04, + 4.84813681109536e-04, + ] # (rad/s) + measurement_range: [6.98131700797732, 6.98131700797732, 6.98131700797732] # (rad/s) + interval_scale_factor: [0.5, 0.5, 0.5] # (%) + interval_misalignment: [1e-3, 1e-3, 1e-3] # (rad) + interval_non_orthogonality: [2e-04, 2e-04, 2e-04] # (rad) + resolution: [8.32e-07, 8.32e-07, 8.32e-07] # (rad/LSB) + + model_enable_settings: + accelerometer: + enable_local_ref_vec: false + enable_turn_on_bias: true + enable_scaling: true + enable_misalignment: true + enable_stochastic_error: true + enable_saturation: true + enable_quantization: true + + gyroscope: + enable_local_ref_vec: true + enable_turn_on_bias: true + enable_scaling: true + enable_misalignment: true + enable_stochastic_error: true + enable_saturation: true + enable_quantization: true + + environmental_settings: + start_geo_position_llh: [-70.667997328, -8.266998932, 40.0] # (deg, deg, m) + start_geo_velocity_ned: [0.0, 0.0, 0.0] # (m/s, m/s, m/s) diff --git a/data/icon.png b/data/icon.png new file mode 100644 index 0000000..9d63754 Binary files /dev/null and b/data/icon.png differ diff --git a/data/icon.svg b/data/icon.svg new file mode 100644 index 0000000..9803110 --- /dev/null +++ b/data/icon.svg @@ -0,0 +1,1355 @@ + +image/svg+xmlIMUSIMULATOR diff --git a/include/imu_simulator_package/imu_simulator.h b/include/imu_simulator_package/imu_simulator.h new file mode 100644 index 0000000..1aeda0a --- /dev/null +++ b/include/imu_simulator_package/imu_simulator.h @@ -0,0 +1,199 @@ +/*@license BSD-3 https://opensource.org/licenses/BSD-3-Clause +Copyright (c) 2024, Institute of Automatic Control - RWTH Aachen University +Maximilian Nitsch (m.nitsch@irt.rwth-aachen.de) +All rights reserved. +*/ + +#pragma once + +#include +#include + +#include "imu_simulator_structures.h" + +namespace imu_simulator { + +class ImuSimulator { + public: + // Constructor + explicit ImuSimulator( + ImuSimParams accSimParams = ImuSimParams(), + ImuSimParams gyroSimParams = ImuSimParams(), + ImuModelEnableSettings imuModelEnableSettings = ImuModelEnableSettings(), + Eigen::Vector3d geoPositionLlh = Eigen::Vector3d::Zero(), + Eigen::Vector3d v_eb_n = Eigen::Vector3d::Zero(), double dt = 0.01, + unsigned int seed = 42); + + // Destructor + ~ImuSimulator(); + + // Measurement generation functions + Eigen::Vector3d generateAccelerationMeasurement( + const Eigen::Vector3d& a_ib_b_true, const Eigen::Quaterniond q_b_n_true); + Eigen::Vector3d generateGyroscopeMeasurement( + const Eigen::Vector3d& w_nb_b_true, const Eigen::Quaterniond q_b_n_true); + + // Reset function to reset Gauss-Markov processes and constant errors + void resetImuSimulator(); + + // Getter functions + ImuSimParams getImuSimParams(const MeasurementType& measurementType) const; + EnvironmentalParameters getEnvironmentalParameters() const; + Eigen::Vector3d getConstTurnOnBias( + const MeasurementType& measurementType) const; + ImuStochasticErrors getImuStochasticErrors( + const MeasurementType& measurementType) const; + Eigen::Vector3d getImuGeoPositionLlh() const; + Eigen::Vector3d getImuGeoVelocity() const; + Eigen::Vector3d getConstScaleFactor( + const MeasurementType& MeasurementType) const; + Eigen::Vector3d getConstMisAlignment( + const MeasurementType& MeasurementType) const; + Eigen::Matrix getConstNonOrthogonality( + const MeasurementType& MeasurementType) const; + + // Setter functions + void setImuSimParameters(const ImuSimParams& accSimParams, + const ImuSimParams& gyroSimParams); + void setEnvironmentalParameters( + const EnvironmentalParameters& environmentalParameters); + + void setSimulatorSeed(const int seed); + void setImuSampleTime(const double dt); + + void setImuGeoPositionLlh(const Eigen::Vector3d geoPositionLlh); + void setImuGeoVelocity(const Eigen::Vector3d v_eb_n); + + void setImuEnableSettings( + const ImuModelEnableSettings& imuModelEnableSettings); + + void setAccEnableGravity(const bool enable); + void setAccEnableTurnOnBias(const bool enable); + void setAccEnableScaling(const bool enable); + void setAccEnableMisAlignment(const bool enable); + void setAccEnableStochasticError(const bool enable); + void setAccEnableSaturation(const bool enable); + void setAccEnableQuantization(const bool enable); + + void setGyroEnableEarthAngularVelocity(const bool enable); + void setGyroEnableTurnOnBias(const bool enable); + void setGyroEnableScaling(const bool enable); + void setGyroEnableMisAlignment(const bool enable); + void setGyroEnableStochasticError(const bool enable); + void setGyroEnableSaturation(const bool enable); + void setGyroEnableQuantization(const bool enable); + + void setUseFixedRandomNumbersFlag(const bool enable); + + // Print functions + std::stringstream printImuSimulatorParameters(); + + private: + // IMU simulation parameters + ImuSimParams accSimParams_; + ImuSimParams gyroSimParams_; + + // IMU Stochastic error states vectors (XYZ-axis) + std::shared_ptr pAccStochasticErrors_; + std::shared_ptr pGyroStochasticErrors_; + + // IMU constant errors (after random sampling) + Eigen::Vector3d accConstTurnOnBias_; + Eigen::Vector3d accConstScaleFactor_; + Eigen::Matrix accConstNonOrthogonality_; + Eigen::Vector3d accConstMisAlignment_; + + Eigen::Vector3d gyroConstTurnOnBias_; + Eigen::Vector3d gyroConstScaleFactor_; + Eigen::Matrix gyroConstNonOrthogonality_; + Eigen::Vector3d gyroConstMisAlignment_; + + // IMU model settings (enable/disable stochastic errors, etc.) + ImuModelEnableSettings imuModelEnableSettings_; + + // Flag indicating if constant errors have been initialized + bool constErrorsInitialized_; + + // Environmental parameters + EnvironmentalParameters environmentalParameters_; + + // Geodetic position (latitude, longitude, height) of body frame (IMU) + Eigen::Vector3d geoPositionLlh_; + + // Velocity of body frame (IMU) with respect to the ECEF frame + Eigen::Vector3d v_eb_n_; + + // Transport velocity of NED frame with respect to ECEF frame + Eigen::Vector3d w_en_n_; + + // IMU sampling time + double dt_; + + // Random number generator for normal distribution and uniform distribution + std::mt19937 randomNumberGenerator_; + std::normal_distribution<> normalDistribution_; + std::uniform_real_distribution<> uniformDistribution_; + + // Flag indicating if fixed random numbers are be used (for testing/debugging) + bool useFixedRandomNumbers_; + + Eigen::Vector3d calcAccelerometerGravityModel( + const Eigen::Vector3d& a_ib_b, const Eigen::Quaterniond& q_b_n_true); + + Eigen::Vector3d calcGyroscopeEarthAngularVelocityModel( + const Eigen::Vector3d& w_nb_b_true, const Eigen::Quaterniond& q_b_n_true); + + // WELMEC gravity model + Eigen::Vector3d calcGravityVectorWelmec( + const Eigen::Vector3d& geoPositionLlh); + + // Earth angular velocity model + Eigen::Vector3d calcAngularVelocityOfEarth( + const Eigen::Vector3d& geoPositionLlh); + + // Transport angular velocity model + Eigen::Vector3d calcTransportAngularVelocity( + const Eigen::Vector3d& geoPositionLlh, const Eigen::Vector3d& v_eb_n); + + // Turn-on bias model + Eigen::Vector3d calcConstTurnOnBiasModel( + const Eigen::Vector3d& measurement, + const MeasurementType& measurementType); + + Eigen::Vector3d calcScaleFactorModel(const Eigen::Vector3d& measurement, + const Eigen::Vector3d& scaleFactor); + + // Scale factor and misalignment model + Eigen::Vector3d calcMisalignmentModel( + const Eigen::Vector3d& measurement, + const Eigen::Matrix& nonOrthogonality, + const Eigen::Vector3d& misAlignment); + + // Stochastic error model (colored noise) + Eigen::Vector3d calcStochasticErrorModel( + const Eigen::Vector3d& measurement, + const MeasurementType& measurementType); + + // Update the stochastic error state space model (Gauß-Markov process) + double updateDiscreteTimeEquivalentStateSpaceModel( + std::shared_ptr pImuStochasticErrors, const double N, + const double B, const double K, const double corrTime, + const AxisIdentifier axisId); + + // Saturation model + Eigen::Vector3d calcSaturationModel(const Eigen::Vector3d& measurement, + const Eigen::Vector3d& measRange); + + // Quantization model + Eigen::Vector3d calcQuantizationModel(const Eigen::Vector3d& measurement, + const Eigen::Vector3d& resolution); + + // Rancom constant initialization (turn-on bias, scale factor, misalignment) + void initializeRandomConstantErrors(); + + // Random number generation functions + double drawRandNormalDistNum(); + double drawRandUniformDistNumFromInterval(const double interval); +}; + +} // namespace imu_simulator diff --git a/include/imu_simulator_package/imu_simulator_node.h b/include/imu_simulator_package/imu_simulator_node.h new file mode 100644 index 0000000..2206036 --- /dev/null +++ b/include/imu_simulator_package/imu_simulator_node.h @@ -0,0 +1,128 @@ +/*@license BSD-3 https://opensource.org/licenses/BSD-3-Clause +Copyright (c) 2024, Institute of Automatic Control - RWTH Aachen University +Maximilian Nitsch (m.nitsch@irt.rwth-aachen.de) +All rights reserved. +*/ + +#include "rclcpp/rclcpp.hpp" + +#include "diagnostic_msgs/msg/diagnostic_array.hpp" +#include "geometry_msgs/msg/accel_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "sensor_msgs/msg/imu.hpp" +#include "tf2_ros/static_transform_broadcaster.h" + +#include "imu_simulator.h" + +namespace imu_simulator { + +class ImuSimulatorNode : public rclcpp::Node { + public: + // Constructor with default config file path + explicit ImuSimulatorNode(std::shared_ptr pImuSimulator); + + // Destructor + ~ImuSimulatorNode() {} + + private: + // IMU simulator class object + std::shared_ptr pImuSimulator_; + + // Ground truth odometry message + nav_msgs::msg::Odometry::SharedPtr groundTruthOdomMsg_; + + // Ground truth acceleration message + geometry_msgs::msg::AccelStamped::SharedPtr groundTruthAccelMsg_; + + // Last odometry timestamp + rclcpp::Time lastOdomTimestamp_; + + // Last acceleration timestamp + rclcpp::Time lastAccelTimestamp_; + + // IMU message publisher + rclcpp::Publisher::SharedPtr pImuPublisher_; + + // IMU visualization publisher + rclcpp::Publisher::SharedPtr + pImuAccelStampedPublisher_; + + // Diagnostic message publisher + rclcpp::Publisher::SharedPtr + pDiagnosticPublisher_; + + // True acceleration publisher + rclcpp::Publisher::SharedPtr + pTrueAccelerationPublisher_; + + // True angular velocity publisher + rclcpp::Publisher::SharedPtr + pTrueAngularRatePublisher_; + + // Vehicle odometry subscriber + rclcpp::Subscription::SharedPtr pOdometrySubscriber_; + + // Vehicle acceleration subscriber + rclcpp::Subscription::SharedPtr + pAccelerationSubscriber_; + + // Static tf2 broadcaster + std::shared_ptr pStaticTf2Broadcaster_; + + // Diagnostic message + diagnostic_msgs::msg::DiagnosticStatus diagnosticMsg_; + + // Timers + rclcpp::TimerBase::SharedPtr pTimer_; + rclcpp::TimerBase::SharedPtr pOdometryTimeOutTimer_; + rclcpp::TimerBase::SharedPtr pAccelerationTimeOutTimer_; + + // Sample time + double sampleTime_; + + // Odometry flags + bool first_odometry_received_; + bool odometry_timeout_; + + // Acceleration flags + bool first_acceleration_received_; + bool acceleration_timeout_; + + // Flag to indicate if acceleration should be calculated from odometry velocity + bool calc_acc_from_odom_vel_; + + // Linear velocity + Eigen::Vector3d v_ib_b_prev_; + + // Flag to indicate if the usable linear velocity message has been received + bool firstLinearVelocityReceived_; + + // Time of last odometry message + rclcpp::Time timeLastOdom_; + + // Decleration and retrieval functions for parameters from YAML file + void declareAndRetrieveGeneralSettings(); + void declareAndRetrieveImuParameters(); + void declareAndRetrieveEnableSettings(); + void declareAndRetrieveEnvironmentalSettings(); + + // Timer callback function + void imuSimulatorLoopCallback(); + + // Odometry callback functions + void odometryCallback(const nav_msgs::msg::Odometry::SharedPtr msg); + void odometryTimeOutCallback(); + + // Acceleration callback functions + void accelerationCallback( + const geometry_msgs::msg::AccelStamped::SharedPtr msg); + void accelerationTimeOutCallback(); + + // tf2 publisher + void publishTf2Transforms() const; + + // Helper functions + Eigen::Vector3d doubleVectorToEigenVector3(const std::vector& vec); +}; + +} // namespace imu_simulator \ No newline at end of file diff --git a/include/imu_simulator_package/imu_simulator_structures.h b/include/imu_simulator_package/imu_simulator_structures.h new file mode 100644 index 0000000..c363781 --- /dev/null +++ b/include/imu_simulator_package/imu_simulator_structures.h @@ -0,0 +1,59 @@ +/*@license BSD-3 https://opensource.org/licenses/BSD-3-Clause +Copyright (c) 2023, Institute of Automatic Control - RWTH Aachen University +Maximilian Nitsch (m.nitsch@irt.rwth-aachen.de) +All rights reserved. +*/ + +#pragma once + +#include + +namespace imu_simulator { + +struct ImuStochasticErrors { + Eigen::Vector3d z_N; // Velocity/angular random walk + Eigen::Vector3d z_B; // Bias instability + Eigen::Vector3d z_K; // Accel./rate random walk +}; + +struct ImuSimParams { + Eigen::Vector3d N; // Vel./ang. random walk (m/s^(3/2))/(rad/s^(1/2)) + Eigen::Vector3d B; // Bias instability (m/s^2)/(rad/s) + Eigen::Vector3d K; // Accel./rate random walk (m/s^(5/2)))/(rad/s^(3/2))) + Eigen::Vector3d corrTime; // Correlation time (s) + Eigen::Vector3d intervalTurnOnBias; // Interval turn-on bias (m/s^2)/(rad/s) + Eigen::Vector3d measRange; // Measurement range (m/s^2)/(rad/s) + Eigen::Vector3d intervalMisAlignment; // Interval misalignment (mrad) + Eigen::Matrix + intervalNonOrthogonality; // Interval nonOrthogonality (mrad) //NOLINT + Eigen::Vector3d intervalScaleFactor; // Interval scale factor (%) + Eigen::Vector3d resolution; // Measurement resolution (m/s^2/LSB)/(rad/s/LSB) +}; + +struct ModelEnableSettings { + bool enableLocalRefVec; // on/off gravity (acc)/ Earth angular vel. (gyro) + bool enableTurnOnBias; // on/off constant turn-on bias given an interval + bool enableScaling; // on/off scaling + bool enableMisAlignment; // on/off misalignment + bool enableStochasticError; // on/off colored noise (stochastic error) + bool enableSaturation; // on/off saturation given a measurement range + bool enableQuantization; // on/off quantization given a resolution +}; + +struct ImuModelEnableSettings { + ModelEnableSettings acc; + ModelEnableSettings gyro; +}; + +struct EnvironmentalParameters { + Eigen::Vector3d g_n; // Local gravity vector (m/s²) + Eigen::Vector3d w_ie_n; // Local angular velocity of Earth (rad/s) +}; + +// Identifier for measurement axis XYZ +enum AxisIdentifier : int { axisX = 0, axisY = 1, axisZ = 2 }; + +// Identifier for measurement type (accelerometer or gyroscope) +enum MeasurementType { acc, gyro }; + +} // namespace imu_simulator diff --git a/launch/ground_truth_test_publisher.py b/launch/ground_truth_test_publisher.py new file mode 100644 index 0000000..d047342 --- /dev/null +++ b/launch/ground_truth_test_publisher.py @@ -0,0 +1,37 @@ +# @license BSD-3 https://opensource.org/licenses/BSD-3-Clause +# Copyright (c) 2024, Institute of Automatic Control - RWTH Aachen University +# Maximilian Nitsch (m.nitsch@irt.rwth-aachen.de) +# All rights reserved. + +from launch_ros.actions import Node + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration + + +def generate_launch_description(): + # Create the launch description + ld = LaunchDescription() + + topic_name_arg = DeclareLaunchArgument( + "odom_rate", + default_value="250.0", + description="Data rate (frequency) of ground truth data in Hz", + ) + + # Add the launch argument to the launch description + ld.add_action(topic_name_arg) + # Create the node + ground_truth_test_publisher_node = Node( + package="imu_simulator_package", + namespace="/nanoauv/odometry", + executable="ground_truth_test_publisher_node", + name="ground_truth_test_publisher_node", + output="screen", + parameters=[{"odom_rate": LaunchConfiguration("odom_rate")}], + ) + + ld.add_action(ground_truth_test_publisher_node) + + return ld diff --git a/launch/imu_simulator.launch.py b/launch/imu_simulator.launch.py new file mode 100644 index 0000000..ccf77d2 --- /dev/null +++ b/launch/imu_simulator.launch.py @@ -0,0 +1,64 @@ +# @license BSD-3 https://opensource.org/licenses/BSD-3-Clause +# Copyright (c) 2024, Institute of Automatic Control - RWTH Aachen University +# Maximilian Nitsch (m.nitsch@irt.rwth-aachen.de) +# All rights reserved. + +import os + +import yaml +from ament_index_python import get_package_share_directory +from launch_ros.actions import Node + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration + + +def generate_launch_description(): + # Create the launch description + ld = LaunchDescription() + + # Declare the path to the config YAML file + config_file_path = os.path.join( + get_package_share_directory("imu_simulator_package"), # noqa + "config", # noqa + "stim300.yaml", # noqa + ) + + # Open the YAML file and load the parameters + with open(config_file_path, "r") as file: + config = yaml.safe_load(file) + + topic_name_odom_arg = DeclareLaunchArgument( + "topic_name_odom", + default_value="/nanoauv/odometry", + description="Topic name of the ground truth odometry from vehicle", + ) + + topic_name_accel_arg = DeclareLaunchArgument( + "topic_name_accel", + default_value="/nanoauv/accel", + description="Topic name of the ground truth acceleration from vehicle", + ) + + # Add the launch argument to the launch description + ld.add_action(topic_name_odom_arg) + ld.add_action(topic_name_accel_arg) + + # Create the node + imu_simulator_package_node = Node( + package="imu_simulator_package", + namespace="/nanoauv/sensor/imu", + executable="imu_simulator_package_node", + name="imu_simulator_node", + output="screen", + parameters=[ + config, + {"topic_name_odom": LaunchConfiguration("topic_name_odom")}, + {"topic_name_accel": LaunchConfiguration("topic_name_accel")} + ] + ) + + ld.add_action(imu_simulator_package_node) + + return ld diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..5076b13 --- /dev/null +++ b/package.xml @@ -0,0 +1,32 @@ + + + + imu_simulator_package + 1.0.0 + ROS 2 package for the IMU simulator. + Maximilian Nitsch + BSD-3-Clause + + ament_cmake + eigen3_cmake_module + eigen + tf2_ros + + std_msgs + sensor_msgs + nav_msgs + geometry_msgs + diagnostic_msgs + rclcpp + + ros2launch + tf2_ros + + ament_lint_auto + ament_lint_common + ament_cmake_gtest + + + ament_cmake + + \ No newline at end of file diff --git a/scripts/compute_allan_var.m b/scripts/compute_allan_var.m new file mode 100644 index 0000000..a04415d --- /dev/null +++ b/scripts/compute_allan_var.m @@ -0,0 +1,114 @@ +function [avar]=allan(data,tau) +% Compute various Allan deviations for a constant-rate time series +% [avar]=allan(data,tau) +% +% INPUTS: +% data should be a struct containing the following fields: +% data.freq the time series measurements in arbitrary units +% data.rate constant rate of time series in (Hz) +% (Differently from previous versions of allan.m, +% it is not possible to compute variances for time- +% stamp data anymore.) +% tau is an array of the tau values for computing Allan deviations +% +% OUTPUTS: +% avar is a struct containing the following fields (for values of tau): +% avar.sig = standard deviation +% avar.sig2 = Allan deviation +% avar.sig2err = standard error of Allan deviation +% avar.osig = Allan deviation with overlapping estimate +% avar.osigerr = standard error of overlapping Allan deviation +% avar.msig = modified Allan deviation +% avar.msigerr = standard error of modified Allan deviation +% avar.tsig = timed Allan deviation +% avar.tsigerr = standard error of timed Allan deviation +% avar.tau1 = measurement interval in (s) +% avar.tauerr = errors in tau that might occur because of initial +% rounding +% +% NOTES: +% Calculations of modified and timed Allan deviations for very long time +% series become very slow. It is advisable to uncomment .msig* and .tsig* +% only after calculations of .sig*, .sig2* and .osig* have been proven +% sufficiently fast. +% +% No pre-processing of the data is performed. +% For constant-rate time series, the deviations are only calculated for tau +% values greater than the minimum time between samples and less than half +% the total time. +% +% versionstr = 'allan v3.0'; +% FCz OCT2009 +% v3.0 faster and very plain code, no plotting; various Allan deviations +% can be calculated; script and sample data are availabie on +% www.nbi.dk/~czerwin/files/allan.zip +% (Normal, overlapping and modified Allan deviations are calculated +% in one function, in strong contrast to MAHs approach of splitting +% up among various functions. This might be beneficial for individual +% cases though.) +% +% MAH 2009 +% v2.0 and others +% +% FCz OCT2008 +% v1.71 'lookfor' gives now useful comments; script and sample data are +% availabie on www.nbi.dk/~czerwin/files/allan.zip +% v1.7 Improve program performance by mainly predefining matrices outside +% of loops (avoiding memory allocation within loops); no changes to +% manual +% +% early program core by Alaa MAKDISSI 2003 +% (documentation might be found http://www.alamath.com/) +% revision and modification by Fabian CZERWINSKI 2009 +% +% For more information, see: +% [1] Fabian Czerwinski, Andrew C. Richardson, and Lene B. Oddershede, +% "Quantifying Noise in Optical Tweezers by Allan Variance," +% Opt. Express 17, 13255-13269 (2009) +% http://dx.doi.org/10.1364/OE.17.013255 + +n=length(data.freq); +jj=length(tau); +m=floor(tau*data.rate); + +avar.sig = zeros(1, jj); +avar.sigerr = zeros(1, jj); +avar.sig2 = zeros(1, jj); +avar.sig2err = zeros(1, jj); +avar.osig = zeros(1, jj); +avar.osigerr = zeros(1, jj); + +tic; + +for j=1:jj + + D=zeros(1,n-m(j)+1); + D(1)=sum(data.freq(1:m(j)))/m(j); + for i=2:n-m(j)+1 + D(i)=D(i-1)+(data.freq(i+m(j)-1)-data.freq(i-1))/m(j); + end + + %standard deviation + avar.sig(j)=std(D(1:m(j):n-m(j)+1)); + avar.sigerr(j)=avar.sig(j)/sqrt(n/m(j)); + + %normal Allan deviation + avar.sig2(j)=sqrt(0.5*mean((diff(D(1:m(j):n-m(j)+1)).^2))); + avar.sig2err(j)=avar.sig2(j)/sqrt(n/m(j)); + + %overlapping Allan deviation + z1=D(m(j)+1:n+1-m(j)); + z2=D(1:n+1-2*m(j)); + u=sum((z1-z2).^2); + avar.osig(j)=sqrt(u/(n+1-2*m(j))/2); + avar.osigerr(j)=avar.osig(j)/sqrt(n-m(j)); + + % toc + +end + +avar.tau1=m/data.rate; +avar.tauerr=tau-avar.tau1; + +toc; +end \ No newline at end of file diff --git a/scripts/plot_imu_simulator_allan_std.m b/scripts/plot_imu_simulator_allan_std.m new file mode 100644 index 0000000..bc4321e --- /dev/null +++ b/scripts/plot_imu_simulator_allan_std.m @@ -0,0 +1,92 @@ +clearvars; +close all; +clc; + +%% Data import +% Read IMU data from csv file +run("read_imu_simulator_csv_data.m"); + +%% Setup +% IMU sample frequency and sample time +Fs = 125; +dT = 1 / Fs; +data.rate = Fs; + +% Generate cluster time (tau) +maxNumM = 50; +L = size(accX, 1); +maxM = 2.^floor(log2(L/2)); +m = logspace(0.1, log10(maxM), maxNumM).'; +m = ceil(m); % m must be an integer. +m = unique(m); % Remove duplicates. +tau = m*dT; + +%% Compute Allan standard deviations (ASD) for Accelerometer XYZ +data.freq = accX; +asd = allan(data, tau); +tauX = asd.tau1; +asdRealX = asd.sig2; + +data.freq = accY; +asd = allan(data, tau); +tauY = asd.tau1; +asdRealY = asd.osig; + +data.freq = accZ; +asd = allan(data, tau); +tauZ = asd.tau1; +asdRealZ = asd.osig; + +%% ASD Models Plot +figure; +loglog(gca, tauX, asdRealX, 'LineWidth', 1); grid on; hold on; +loglog(gca, tauY, asdRealY, 'LineWidth', 1); +loglog(gca, tauZ, asdRealZ, 'LineWidth', 1); +xlim([10^(-1), 10^4]); +ylim([1e-4, 1e-2]); + +ylabel('ASD \sigma(\tau) / (m/s^2)') +xlabel('Cluster Time \tau / s'); +title('Allan Standard Deviation - STIM300 Accelerometer'); + +set(gcf,'color','w'); + +%% Extract Allan Parameters +[Nx, Bx, Kx, Tbx] = extract_allan_parameters(tau, asdRealX', 'Acc-X', 'Acc'); +[Ny, By, Ky, Tby] = extract_allan_parameters(tau, asdRealY', 'Acc-Y', 'Acc'); +[Nz, Bz, Kz, Tbz] = extract_allan_parameters(tau, asdRealZ', 'Acc-Z', 'Acc'); + +%% Compute Allan standard deviations (ASD) for Gyroscope XYZ +data.freq = gyroX; +asd = allan(data, tau); +tauX = asd.tau1; +asdRealX = asd.sig2; + +data.freq = gyroY; +asd = allan(data, tau); +tauY = asd.tau1; +asdRealY = asd.osig; + +data.freq = gyroZ; +asd = allan(data, tau); +tauZ = asd.tau1; +asdRealZ = asd.osig; + +%% ASD Models Plot +figure; +loglog(gca, tauX, asdRealX, 'LineWidth', 1); grid on; hold on; +loglog(gca, tauY, asdRealY, 'LineWidth', 1); +loglog(gca, tauZ, asdRealZ, 'LineWidth', 1); +xlim([10^(-1), 10^4]); +% ylim([1e-4, 1e-2]); + +ylabel('ASD \sigma(\tau) / (rad/sqrt(Hz))') +xlabel('Cluster Time \tau / s'); +title('Allan Standard Deviation - STIM300 Gyroscope'); + +set(gcf,'color','w'); + +%% Extract Allan Gyroscope Parameters +extract_allan_parameters(tau, asdRealX', 'Gyro-X', 'Gyro'); +extract_allan_parameters(tau, asdRealY', 'Gyro-Y', 'Gyro'); +extract_allan_parameters(tau, asdRealZ', 'Gyro-Z', 'Gyro'); \ No newline at end of file diff --git a/scripts/read_imu_simulator_csv_data.m b/scripts/read_imu_simulator_csv_data.m new file mode 100644 index 0000000..a6da671 --- /dev/null +++ b/scripts/read_imu_simulator_csv_data.m @@ -0,0 +1,35 @@ +%% Import data from text file +% Script for importing data from the following text file: +% +% filename: /home/nitsch/ros2-triple_ws/imu_static_data.csv +% +% Auto-generated by MATLAB on 31-Jan-2024 07:36:14 + +%% Set up the Import Options and import the data +opts = delimitedTextImportOptions("NumVariables", 6); + +% Specify range and delimiter +opts.DataLines = [1, Inf]; +opts.Delimiter = ","; + +% Specify column names and types +opts.VariableNames = ["accX", "accY", "accZ", "gyroX", "gyroY", "gyroZ"]; +opts.VariableTypes = ["double", "double", "double", "double", "double", "double"]; + +% Specify file level properties +opts.ExtraColumnsRule = "ignore"; +opts.EmptyLineRule = "read"; + +% Import the data +tbl = readtable("../../../imu_static_data.csv", opts); + +%% Convert to output type +accX = tbl.accX; +accY = tbl.accY; +accZ = tbl.accZ; +gyroX = tbl.gyroX; +gyroY = tbl.gyroY; +gyroZ = tbl.gyroZ; + +%% Clear temporary variables +clear opts tbl diff --git a/src/ground_truth_test_publisher_node.cpp b/src/ground_truth_test_publisher_node.cpp new file mode 100644 index 0000000..d1d5bb5 --- /dev/null +++ b/src/ground_truth_test_publisher_node.cpp @@ -0,0 +1,201 @@ +/*@license BSD-3 https://opensource.org/licenses/BSD-3-Clause +Copyright (c) 2024, Institute of Automatic Control - RWTH Aachen University +Maximilian Nitsch (m.nitsch@irt.rwth-aachen.de) +All rights reserved. +*/ + +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include "geometry_msgs/msg/accel_stamped.hpp" +#include "geometry_msgs/msg/point.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include "geometry_msgs/msg/quaternion.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "tf2_ros/transform_broadcaster.h" + +class GroundTruthPublisher : public rclcpp::Node { + public: + // Constructor + GroundTruthPublisher() : Node("ground_truth_publisher") { + odomPublisher_ = this->create_publisher( + "/nanoauv/odometry", 10); + + accelPublisher_ = this->create_publisher( + "/nanoauv/accel", 10); + + // Get the value of the odom_rate parameter + this->declare_parameter("odom_rate", rclcpp::PARAMETER_DOUBLE); + rate_ = this->get_parameter("odom_rate").as_double(); + + // Print the value of the odom_rate parameter + RCLCPP_INFO(this->get_logger(), "Odometry rate: %f", rate_); + + // Create a timer to publish the odometry message at user-defined rate + timer_ = this->create_wall_timer( + std::chrono::duration(1.0 / rate_), + std::bind(&GroundTruthPublisher::publishMsgs, this)); + + tf2Broadcaster_ = std::make_shared(this); + + time_since_start_ = this->now().seconds(); + } + + private: + // Publish ground truth messages and tf2 transform + void publishMsgs() { + // Get current time in seconds + double time_seconds = this->now().seconds() - time_since_start_; + + // Create odometry message + nav_msgs::msg::Odometry odometryMsg; + + // Populate odometry header message + odometryMsg.header.stamp = this->now(); + odometryMsg.header.frame_id = "world"; + odometryMsg.child_frame_id = "base_link"; + + // Populate odometry pose message with sine/cosine signals + odometryMsg.pose.pose.position.x = + 325 * std::sin(2 * M_PI * time_seconds * 0.0002); + odometryMsg.pose.pose.position.y = 0.0; + // 20.0 * std::cos(2 * M_PI * time_seconds * 0.02); + odometryMsg.pose.pose.position.z = 0.0; + // 5.0 * std::sin(2 * M_PI * time_seconds * 0.03); + + // Create Euler angles from sine/cosine signals + double roll = 120 * M_PI / 180.0 * std::sin(2 * M_PI * time_seconds * 0.01); + double pitch = 75 * M_PI / 180.0 * std::cos(2 * M_PI * time_seconds * 0.02); + double yaw = 170 * M_PI / 180.0 * std::sin(2 * M_PI * time_seconds * 0.03); + + Eigen::Vector3d sensorRotationEulerRpy(roll, pitch, yaw); + + // Convert Euler angles to rotation matrix + Eigen::Matrix3d sensorRotation; + // sensorRotation = + // Eigen::AngleAxisd(sensorRotationEulerRpy(0), Eigen::Vector3d::UnitZ()) * + // Eigen::AngleAxisd(sensorRotationEulerRpy(1), Eigen::Vector3d::UnitY()) * + // Eigen::AngleAxisd(sensorRotationEulerRpy(2), Eigen::Vector3d::UnitX()); + + // sensorRotation = Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitZ()) * + // Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * + // Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); + + // // Convert rotation matrix to quaternion + // Eigen::Quaterniond sensorRotationQuat = Eigen::Quaterniond(sensorRotation); + + // // Normalize quaternion + // sensorRotationQuat.normalize(); + + Eigen::Quaterniond sensorRotationQuat = + Eigen::Quaterniond(0.0, 1.0, 0.0, 0.0); + + // Fill quaternion + odometryMsg.pose.pose.orientation.w = sensorRotationQuat.w(); + odometryMsg.pose.pose.orientation.x = sensorRotationQuat.x(); + odometryMsg.pose.pose.orientation.y = sensorRotationQuat.y(); + odometryMsg.pose.pose.orientation.z = sensorRotationQuat.z(); + + // odometryMsg.pose.pose.orientation.w = 0.0; + // odometryMsg.pose.pose.orientation.x = 1.0; + // odometryMsg.pose.pose.orientation.y = 0.0; + // odometryMsg.pose.pose.orientation.z = 0.0; + + // Populate odometry twist message with sine/cosine signals + odometryMsg.twist.twist.linear.x = + 0.25 * std::cos(2 * M_PI * time_seconds * 0.01); + odometryMsg.twist.twist.linear.y = + 0.01 * std::sin(2 * M_PI * time_seconds * 0.02); + odometryMsg.twist.twist.linear.z = + 0.02 * std::cos(2 * M_PI * time_seconds * 0.03); + + odometryMsg.twist.twist.angular.x = + 1 * std::cos(2 * M_PI * time_seconds * 0.01); + odometryMsg.twist.twist.angular.y = + 2 * std::sin(2 * M_PI * time_seconds * 0.02); + odometryMsg.twist.twist.angular.z = + 0.5 * std::cos(2 * M_PI * time_seconds * 0.03); + + // Publish odometry message + odomPublisher_->publish(odometryMsg); + + // Create acceleration message + geometry_msgs::msg::AccelStamped accelMsg; + + accelMsg.header.stamp = this->now(); + accelMsg.header.frame_id = "base_link"; + + Eigen::Matrix3d C_b_n = sensorRotationQuat.toRotationMatrix(); + Eigen::Matrix3d C_n_b = C_b_n.transpose(); + + // Eigen::Vector3d a_ib_b(2.5 * std::cos(2 * M_PI * time_seconds * 0.01), + // 1 * std::sin(2 * M_PI * time_seconds * 0.02), + // 0 * std::cos(2 * M_PI * time_seconds * 0.03)); + + Eigen::Vector3d f_ib_b(2.0, 0.0, 0.0); + + Eigen::Vector3d a_ib_b = f_ib_b + C_n_b * Eigen::Vector3d(0.0, 0.0, -9.81); + + // // Transform acceleration to world frame to consider orientation + // Eigen::Matrix3d C_b_n = sensorRotationQuat.toRotationMatrix(); + // Eigen::Vector3d accelerationWorld = C_b_n * acceleration; + + accelMsg.accel.linear.x = a_ib_b(0); + accelMsg.accel.linear.y = a_ib_b(1); + accelMsg.accel.linear.z = a_ib_b(2); + + accelMsg.accel.angular.x = 0.0; + accelMsg.accel.angular.y = 0.0; + accelMsg.accel.angular.z = 0.0; + + // Publish acceleration message + accelPublisher_->publish(accelMsg); + + geometry_msgs::msg::TransformStamped tfMsg; + tfMsg.header.stamp = now(); + tfMsg.header.frame_id = "world"; + tfMsg.child_frame_id = "base_link"; + + tfMsg.transform.translation.x = odometryMsg.pose.pose.position.x; + tfMsg.transform.translation.y = odometryMsg.pose.pose.position.y; + tfMsg.transform.translation.z = odometryMsg.pose.pose.position.z; + + tfMsg.transform.rotation.w = odometryMsg.pose.pose.orientation.w; + tfMsg.transform.rotation.x = odometryMsg.pose.pose.orientation.x; + tfMsg.transform.rotation.y = odometryMsg.pose.pose.orientation.y; + tfMsg.transform.rotation.z = odometryMsg.pose.pose.orientation.z; + + tf2Broadcaster_->sendTransform(tfMsg); + } + + // Odometry publisher + rclcpp::Publisher::SharedPtr odomPublisher_; + + // Acceleration publisher + rclcpp::Publisher::SharedPtr + accelPublisher_; + + // Publisher timer + rclcpp::TimerBase::SharedPtr timer_; + + // rclcpp::TimerBase::SharedPtr timerTf2Broadcaster_; + std::shared_ptr tf2Broadcaster_; + + // Rate of ground truth message publication + double rate_; + + // Time since start + double time_since_start_; +}; + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + auto ground_truth_publisher = std::make_shared(); + rclcpp::spin(ground_truth_publisher); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/src/imu_simulator.cpp b/src/imu_simulator.cpp new file mode 100644 index 0000000..274c653 --- /dev/null +++ b/src/imu_simulator.cpp @@ -0,0 +1,1295 @@ +#include "imu_simulator.h" + +#include +#include + +namespace imu_simulator { + +/** + * @brief Constructor of the ImuSimulator class. + * + * @param accSimParams IMU simulation parameters for the accelerometer. + * @param gyroSimParams IMU simulation parameters for the gyroscope. + * @param imuModelEnableSettings IMU model enable settings. + * @param geoPositionLlh Geodetic position of the IMU (latitude, longitude, height) (rad,rad,m). + * @param v_eb_n Geodetic velocity of the IMU (north, east, down) (m/s). + * @param dt Sampling time (s). + * @param seed Random number generator seed. +*/ +ImuSimulator::ImuSimulator(ImuSimParams accSimParams, + ImuSimParams gyroSimParams, + ImuModelEnableSettings imuModelEnableSettings, + Eigen::Vector3d geoPositionLlh, + Eigen::Vector3d v_eb_n, double dt, unsigned int seed) + : accSimParams_(accSimParams), + gyroSimParams_(gyroSimParams), + pAccStochasticErrors_(std::make_shared()), + pGyroStochasticErrors_(std::make_shared()), + imuModelEnableSettings_(imuModelEnableSettings), + constErrorsInitialized_(false), + geoPositionLlh_(geoPositionLlh), + v_eb_n_(v_eb_n), + w_en_n_(calcTransportAngularVelocity(geoPositionLlh, v_eb_n)), + dt_(dt), + randomNumberGenerator_(seed), + normalDistribution_(0, 1), + uniformDistribution_(0, 1) { + // Initialize IMU stochastic error structs + pAccStochasticErrors_->z_N = Eigen::Vector3d::Zero(); + pAccStochasticErrors_->z_B = Eigen::Vector3d::Zero(); + pAccStochasticErrors_->z_K = Eigen::Vector3d::Zero(); + + pGyroStochasticErrors_->z_N = Eigen::Vector3d::Zero(); + pGyroStochasticErrors_->z_B = Eigen::Vector3d::Zero(); + pGyroStochasticErrors_->z_K = Eigen::Vector3d::Zero(); + + // Initialize local gravity vector + environmentalParameters_.g_n = calcGravityVectorWelmec(geoPositionLlh_); + // Initialize local angular velocity vector of Earth + environmentalParameters_.w_ie_n = calcAngularVelocityOfEarth(geoPositionLlh_); + + // Initialize IMU constant errors + initializeRandomConstantErrors(); +} + +/** + * @brief Destructor of the ImuSimulator class. +*/ +ImuSimulator::~ImuSimulator() {} + +/** + * @brief Getter for the IMU simulation parameters. + * + * @param measurementType Measurement type (acceleration or angular velocity). + * + * @return IMU simulation parameters. +*/ +ImuSimParams ImuSimulator::getImuSimParams( + const MeasurementType& measurementType) const { + if (measurementType == acc) { + return accSimParams_; + } else if (measurementType == gyro) { + return gyroSimParams_; + } else { + // Invalid measurement type + std::cerr << "getImuSimParams: Invalid measurement type!\n"; + return ImuSimParams(); + } +} + +/** +* @brief Getter for the environmental parameters. +* +* @return Environmental parameters. +*/ +EnvironmentalParameters ImuSimulator::getEnvironmentalParameters() const { + return environmentalParameters_; +} + +/** +* @brief Getter for the constant turn-on bias. +* +* @param measurementType Measurement type (acceleration or angular velocity). +* +* @return Constant turn-on bias. +*/ +Eigen::Vector3d ImuSimulator::getConstTurnOnBias( + const MeasurementType& measurementType) const { + if (measurementType == acc) { + return accConstTurnOnBias_; + } else if (measurementType == gyro) { + return gyroConstTurnOnBias_; + } else { + // Invalid measurement type + std::cerr << "getConstTurnOnBias: Invalid measurement type!\n"; + return Eigen::Vector3d(0.0, 0.0, 0.0); + } +} + +/** + * @brief Getter for the IMU stochastic errors. + * + * @param measurementType Measurement type (acceleration or angular velocity). + * + * @return IMU stochastic errors. +*/ +ImuStochasticErrors ImuSimulator::getImuStochasticErrors( + const MeasurementType& measurementType) const { + if (measurementType == acc) { + return *pAccStochasticErrors_; + } else if (measurementType == gyro) { + return *pGyroStochasticErrors_; + } else { + // Invalid measurement type + std::cerr << "getImuStochasticErrors: Invalid measurement type!\n"; + return ImuStochasticErrors(); + } +} + +/** + * @brief Getter for the geodetic position of the IMU. + * + * @return Last geodetic position of the IMU (latitude, longitude, height). +*/ +Eigen::Vector3d ImuSimulator::getImuGeoPositionLlh() const { + return geoPositionLlh_; +} + +/** + * @brief Getter for the geodetic velocity of the IMU. + * + * @return Last geodetic velocity of the IMU (north, east, down). +*/ +Eigen::Vector3d ImuSimulator::getImuGeoVelocity() const { + return v_eb_n_; +} + +/** +* @brief Getter for the constant scale factor error. +* +* @param measurementType Measurement type (acceleration or angular velocity). +* +* @return Constant scale factor error. +*/ +Eigen::Vector3d ImuSimulator::getConstScaleFactor( + const MeasurementType& measurementType) const { + if (measurementType == acc) { + return accConstScaleFactor_; + } else if (measurementType == gyro) { + return gyroConstScaleFactor_; + } else { + // Invalid measurement type + std::cerr << "getConstScaleFactor: Invalid measurement type!\n"; + return Eigen::Vector3d(0.0, 0.0, 0.0); + } +} + +/** +* @brief Getter for the constant misalignment angles. +* +* @param measurementType Measurement type (acceleration or angular velocity). +* +* @return Constant misalignment angles. +*/ +Eigen::Vector3d ImuSimulator::getConstMisAlignment( + const MeasurementType& measurementType) const { + if (measurementType == acc) { + return accConstMisAlignment_; + } else if (measurementType == gyro) { + return gyroConstMisAlignment_; + } else { + // Invalid measurement type + std::cerr << "getConstMisAlignment: Invalid measurement type!\n"; + return Eigen::Vector3d(0.0, 0.0, 0.0); + } +} + +/** +* @brief Getter for the constant orthogonality error angles. +* +* @param measurementType Measurement type (acceleration or angular velocity). +* +* @return Constant orthogonality error angles. +*/ +Eigen::Matrix ImuSimulator::getConstNonOrthogonality( + const MeasurementType& measurementType) const { + if (measurementType == acc) { + return accConstNonOrthogonality_; + } else if (measurementType == gyro) { + return gyroConstNonOrthogonality_; + } else { + // Invalid measurement type + std::cerr << "getConstNonOrthogonality: Invalid measurement type!\n"; + return Eigen::Matrix({{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}); + } +} + +/** + * @brief Setter for the IMU simulation parameters. + * + * @param accSimParams IMU simulation parameters for the accelerometer. + * @param gyroSimParams IMU simulation parameters for the gyroscope. +*/ +void ImuSimulator::setImuSimParameters(const ImuSimParams& accSimParams, + const ImuSimParams& gyroSimParams) { + accSimParams_ = accSimParams; + gyroSimParams_ = gyroSimParams; +} + +/** + * @brief Prints the IMU simulation parameters. + * + * @return String stream with the IMU simulation parameters. +*/ +std::stringstream ImuSimulator::printImuSimulatorParameters() { + // Create stringstream to store the output + std::stringstream ss; + + ss << "***************************************************************" + "**********************************************************************" + "*" + << "\n"; + ss << std::left << std::setw(50) << "Starting IMU-Simulator" + << "\n"; + ss << "***************************************************************" + "**********************************************************************" + "*" + << "\n"; + + // Accelerometer simulation parameters + ss << std::left << "Accelerometer simulation parameters:\n"; + ss << std::fixed << std::setprecision(6); + + ss << std::left << std::setw(50) << "N:" << accSimParams_.N.transpose() + << " m/s^(3/2)\n"; + ss << std::left << std::setw(50) << "B:" << accSimParams_.B.transpose() + << " m/s^2\n"; + ss << std::left << std::setw(50) << "K:" << accSimParams_.K.transpose() + << " m/s^(5/2)\n"; + ss << std::left << std::setw(50) + << "corrTime:" << accSimParams_.corrTime.transpose() << " s\n"; + ss << std::left << std::setw(50) + << "intervalTurnOnBias:" << accSimParams_.intervalTurnOnBias.transpose() + << " m/s^2\n"; + ss << std::left << std::setw(50) + << "measRange:" << accSimParams_.measRange.transpose() << " m/s^2\n"; + ss << std::left << std::setw(50) << "intervalMisAlignment:" + << accSimParams_.intervalMisAlignment.transpose() << " rad\n"; + ss << std::left << std::setw(50) << "intervalNonOrthogonality:" + << accSimParams_.intervalNonOrthogonality.transpose() << " rad\n"; + ss << std::left << std::setw(50) + << "intervalScaleFactor:" << accSimParams_.intervalScaleFactor.transpose() + << " %\n"; + ss << std::left << std::setw(50) + << "resolution:" << accSimParams_.resolution.transpose() << " m/s^2/LSB\n"; + + ss << "***************************************************************" + "**********************************************************************" + "*" + << "\n"; + + // Gyroscope simulation parameters + ss << std::left << "Gyroscope simulation parameters:\n"; + ss << std::left << std::setw(50) << "N:" << gyroSimParams_.N.transpose() + << " rad/s^(1/2)\n"; + ss << std::left << std::setw(50) << "B:" << gyroSimParams_.B.transpose() + << " rad/s\n"; + ss << std::left << std::setw(50) << "K:" << gyroSimParams_.K.transpose() + << " rad/s^(3/2)\n"; + ss << std::left << std::setw(50) + << "corrTime:" << gyroSimParams_.corrTime.transpose() << " s\n"; + ss << std::left << std::setw(50) + << "intervalTurnOnBias:" << gyroSimParams_.intervalTurnOnBias.transpose() + << " rad/s\n"; + ss << std::left << std::setw(50) + << "measRange:" << gyroSimParams_.measRange.transpose() << " rad/s\n"; + ss << std::left << std::setw(50) << "intervalMisAlignment:" + << gyroSimParams_.intervalMisAlignment.transpose() << " rad\n"; + ss << std::left << std::setw(50) << "intervalNonOrthogonality:" + << gyroSimParams_.intervalNonOrthogonality.transpose() << " rad\n"; + ss << std::left << std::setw(50) + << "intervalScaleFactor:" << gyroSimParams_.intervalScaleFactor.transpose() + << " %\n"; + ss << std::left << std::setw(50) + << "resolution:" << gyroSimParams_.resolution.transpose() + << " rad/s/LSB\n"; + + ss << "***************************************************************" + "**********************************************************************" + "*" + << "\n"; + + // IMU model enable settings + ss << std::left << "IMU model enable settings:\n"; + ss << std::left << "Accelerometer:\n"; + ss << std::left << std::setw(50) + << "Turn-on bias:" << imuModelEnableSettings_.acc.enableTurnOnBias << "\n"; + ss << std::left << std::setw(50) + << "Stochastic error:" << imuModelEnableSettings_.acc.enableStochasticError + << "\n"; + ss << std::left << std::setw(50) + << "Scaling:" << imuModelEnableSettings_.acc.enableScaling << "\n"; + ss << std::left << std::setw(50) + << "Misalignment:" << imuModelEnableSettings_.acc.enableMisAlignment + << "\n"; + ss << std::left << std::setw(50) + << "Saturation:" << imuModelEnableSettings_.acc.enableSaturation << "\n"; + ss << std::left << std::setw(50) + << "Quantization:" << imuModelEnableSettings_.acc.enableQuantization + << "\n"; + + ss << "\n"; + + ss << std::left << "Gyroscope:\n"; + ss << std::left << std::setw(50) + << "Turn-on bias:" << imuModelEnableSettings_.gyro.enableTurnOnBias + << "\n"; + ss << std::left << std::setw(50) << "Stochastic error:" + << imuModelEnableSettings_.gyro.enableStochasticError << "\n"; + ss << std::left << std::setw(50) + << "Scaling:" << imuModelEnableSettings_.gyro.enableScaling << "\n"; + ss << std::left << std::setw(50) + << "Misalignment:" << imuModelEnableSettings_.gyro.enableMisAlignment + << "\n"; + ss << std::left << std::setw(50) + << "Saturation:" << imuModelEnableSettings_.gyro.enableSaturation << "\n"; + ss << std::left << std::setw(50) + << "Quantization:" << imuModelEnableSettings_.gyro.enableQuantization + << "\n"; + ss << "***************************************************************" + "**********************************************************************" + "*" + << "\n"; + + // Environmental parameters + ss << std::left << "Environmental parameters:\n"; + ss << std::left << std::setw(50) + << "Local gravity vector:" << environmentalParameters_.g_n.transpose() + << " m/s²\n"; + ss << std::left << std::setw(50) << "Local angular velocity of Earth:" + << environmentalParameters_.w_ie_n.transpose() << " rad/s\n"; + + ss << "***************************************************************" + "**********************************************************************" + "*" + << "\n"; + + // Sampling time and frequeqncy + ss << std::left << std::setw(50) << "Sampling time:" << std::fixed + << std::setprecision(6) << dt_ << " s\n"; + ss << std::left << std::setw(50) << "Sampling frequency:" << std::fixed + << std::setprecision(6) << 1.0 / dt_ << " Hz\n"; + + ss << "***************************************************************" + "**********************************************************************" + "*" + << "\n"; + + return ss; +} + +/** +* @brief Setter for the environmental parameters. +*/ +void ImuSimulator::setEnvironmentalParameters( + const EnvironmentalParameters& environmentalParameters) { + environmentalParameters_ = environmentalParameters; +} + +/** + * @brief Setter for the IMU seed of the random number generator. +*/ +void ImuSimulator::setSimulatorSeed(const int seed) { + randomNumberGenerator_.seed(seed); +} + +/** + * @brief Setter for the IMU simulator sampling time. +*/ +void ImuSimulator::setImuSampleTime(const double dt) { + dt_ = dt; +} + +/** + * @brief Setter for the body frame (IMU) geodetic position (ECEF). +*/ +void ImuSimulator::setImuGeoPositionLlh(const Eigen::Vector3d geoPositionLlh) { + geoPositionLlh_ = geoPositionLlh; +} + +/** + * @brief Setter for the body frame (IMU) geodetic velocity (NED). +*/ +void ImuSimulator::setImuGeoVelocity(const Eigen::Vector3d v_eb_n) { + v_eb_n_ = v_eb_n; +} + +/** + * @brief Setter for the whole IMU enable settings. + * + * @param imuModelEnableSettings IMU model enable settings. +*/ +void ImuSimulator::setImuEnableSettings( + const ImuModelEnableSettings& imuModelEnableSettings) { + imuModelEnableSettings_ = imuModelEnableSettings; +} + +/** + * @brief Setter for the accelerometer gravity model enable flag. + * + * @param enable Enable/disable accelerometer gravity model. +*/ +void ImuSimulator::setAccEnableGravity(const bool enable) { + imuModelEnableSettings_.acc.enableLocalRefVec = enable; +} + +/** +* @brief Setter for the accelerometer turn-on bias model enable flag. +* +* @param enable Enable/disable accelerometer turn-on bias model. +*/ +void ImuSimulator::setAccEnableTurnOnBias(const bool enable) { + imuModelEnableSettings_.acc.enableTurnOnBias = enable; +} + +/** +* @brief Setter for the accelerometer scaling model enable flag. +* +* @param enable Enable/disable accelerometer scaling model. +*/ +void ImuSimulator::setAccEnableScaling(const bool enable) { + imuModelEnableSettings_.acc.enableScaling = enable; +} + +/** +* @brief Setter for the accelerometer misalignment model enable flag. +* +* @param enable Enable/disable accelerometer misalignment model. +*/ +void ImuSimulator::setAccEnableMisAlignment(const bool enable) { + imuModelEnableSettings_.acc.enableMisAlignment = enable; +} + +/** +* @brief Setter for the accelerometer stochastic error model enable flag. +* +* @param enable Enable/disable accelerometer stochastic error model. +*/ +void ImuSimulator::setAccEnableStochasticError(const bool enable) { + imuModelEnableSettings_.acc.enableStochasticError = enable; +} + +/** +* @brief Setter for the accelerometer saturation model enable flag. +* +* @param enable Enable/disable accelerometer saturation model. +*/ +void ImuSimulator::setAccEnableSaturation(const bool enable) { + imuModelEnableSettings_.acc.enableSaturation = enable; +} + +/** +* @brief Setter for the accelerometer quantization model enable flag. +* +* @param enable Enable/disable accelerometer quantization model. +*/ +void ImuSimulator::setAccEnableQuantization(const bool enable) { + imuModelEnableSettings_.acc.enableQuantization = enable; +} + +/** + * @brief Setter for the gyroscope Earth angular velocity model enable flag. + * + * @param enable Enable/disable gyroscope Earth angular velocity model. +*/ +void ImuSimulator::setGyroEnableEarthAngularVelocity(const bool enable) { + imuModelEnableSettings_.gyro.enableLocalRefVec = enable; +} + +/** +* @brief Setter for the gyroscope turn-on bias model enable flag. +* +* @param enable Enable/disable gyroscope turn-on bias model. +*/ +void ImuSimulator::setGyroEnableTurnOnBias(const bool enable) { + imuModelEnableSettings_.gyro.enableTurnOnBias = enable; +} + +/** +* @brief Setter for the gyroscope scaling model enable flag. +* +* @param enable Enable/disable gyroscope scaling model. +*/ +void ImuSimulator::setGyroEnableScaling(const bool enable) { + imuModelEnableSettings_.gyro.enableScaling = enable; +} + +/** +* @brief Setter for the gyroscope misalignment model enable flag. +* +* @param enable Enable/disable gyroscope misalignment model. +*/ +void ImuSimulator::setGyroEnableMisAlignment(const bool enable) { + imuModelEnableSettings_.gyro.enableMisAlignment = enable; +} + +/** +* @brief Setter for the gyroscope stochastic error model enable flag. +* +* @param enable Enable/disable gyroscope stochastic error model. +*/ +void ImuSimulator::setGyroEnableStochasticError(const bool enable) { + imuModelEnableSettings_.gyro.enableStochasticError = enable; +} + +/** +* @brief Setter for the gyroscope saturation model enable flag. +* +* @param enable Enable/disable gyroscope saturation model. +*/ +void ImuSimulator::setGyroEnableSaturation(const bool enable) { + imuModelEnableSettings_.gyro.enableSaturation = enable; +} + +/** +* @brief Setter for the gyroscope quantization model enable flag. +* +* @param enable Enable/disable gyroscope quantization model. +*/ +void ImuSimulator::setGyroEnableQuantization(const bool enable) { + imuModelEnableSettings_.gyro.enableQuantization = enable; +} + +/** + * @brief Setter for the flag indicating if fixed random numbers are used. + * + * @param enable Enable/disable fixed random numbers. +*/ +void ImuSimulator::setUseFixedRandomNumbersFlag(const bool enable) { + useFixedRandomNumbers_ = enable; +} + +/** + * @brief Initializes the IMU constant errors. + * + * This function initializes the IMU constant errors by drawing random numbers from a uniform distribution. + * The following IMU constant errors are initialized: + * - constant turn-on bias, + * - scale factor and misalignment. +*/ +void ImuSimulator::initializeRandomConstantErrors() { + // Draw const turn-on bias for each accelerometer axis + accConstTurnOnBias_(0) = + drawRandUniformDistNumFromInterval(accSimParams_.intervalTurnOnBias.x()); + accConstTurnOnBias_(1) = + drawRandUniformDistNumFromInterval(accSimParams_.intervalTurnOnBias.y()); + accConstTurnOnBias_(2) = + drawRandUniformDistNumFromInterval(accSimParams_.intervalTurnOnBias.z()); + + // Draw const turn-on bias for each gyroscope axis + gyroConstTurnOnBias_(0) = + drawRandUniformDistNumFromInterval(gyroSimParams_.intervalTurnOnBias.x()); + gyroConstTurnOnBias_(1) = + drawRandUniformDistNumFromInterval(gyroSimParams_.intervalTurnOnBias.y()); + gyroConstTurnOnBias_(2) = + drawRandUniformDistNumFromInterval(gyroSimParams_.intervalTurnOnBias.z()); + + // Draw const scale factor error for each accelerometer axis + accConstScaleFactor_(0) = + drawRandUniformDistNumFromInterval(accSimParams_.intervalScaleFactor.x()); + accConstScaleFactor_(1) = + drawRandUniformDistNumFromInterval(accSimParams_.intervalScaleFactor.y()); + accConstScaleFactor_(2) = + drawRandUniformDistNumFromInterval(accSimParams_.intervalScaleFactor.z()); + + // Draw const scale factor for each gyroscope axis + gyroConstScaleFactor_(0) = drawRandUniformDistNumFromInterval( + gyroSimParams_.intervalScaleFactor.x()); + gyroConstScaleFactor_(1) = drawRandUniformDistNumFromInterval( + gyroSimParams_.intervalScaleFactor.y()); + gyroConstScaleFactor_(2) = drawRandUniformDistNumFromInterval( + gyroSimParams_.intervalScaleFactor.z()); + + // Draw orthogonality error for each pair of accelerometer axes + accConstNonOrthogonality_(0) = drawRandUniformDistNumFromInterval( + accSimParams_.intervalNonOrthogonality[0]); // xz-plane + accConstNonOrthogonality_(1) = drawRandUniformDistNumFromInterval( + accSimParams_.intervalNonOrthogonality[1]); // xy-plane + accConstNonOrthogonality_(2) = drawRandUniformDistNumFromInterval( + accSimParams_.intervalNonOrthogonality[2]); // yz-plane + accConstNonOrthogonality_(3) = drawRandUniformDistNumFromInterval( + accSimParams_.intervalNonOrthogonality[3]); // yx-plane + accConstNonOrthogonality_(4) = drawRandUniformDistNumFromInterval( + accSimParams_.intervalNonOrthogonality[4]); // zy-plane + accConstNonOrthogonality_(5) = drawRandUniformDistNumFromInterval( + accSimParams_.intervalNonOrthogonality[5]); // zx-plane + + // Draw orthogonality error for each pair of gyroscope axes + gyroConstNonOrthogonality_(0) = drawRandUniformDistNumFromInterval( + gyroSimParams_.intervalNonOrthogonality[0]); // xz-plane + gyroConstNonOrthogonality_(1) = drawRandUniformDistNumFromInterval( + gyroSimParams_.intervalNonOrthogonality[1]); // xy-plane + gyroConstNonOrthogonality_(2) = drawRandUniformDistNumFromInterval( + gyroSimParams_.intervalNonOrthogonality[2]); // yz-plane + gyroConstNonOrthogonality_(3) = drawRandUniformDistNumFromInterval( + gyroSimParams_.intervalNonOrthogonality[3]); // yx-plane + gyroConstNonOrthogonality_(4) = drawRandUniformDistNumFromInterval( + gyroSimParams_.intervalNonOrthogonality[4]); // zy-plane + gyroConstNonOrthogonality_(5) = drawRandUniformDistNumFromInterval( + gyroSimParams_.intervalNonOrthogonality[5]); // zx-plane + + // Draw alignment error for each accelerometer axis + accConstMisAlignment_(0) = drawRandUniformDistNumFromInterval( + accSimParams_.intervalMisAlignment.x()); + accConstMisAlignment_(1) = drawRandUniformDistNumFromInterval( + accSimParams_.intervalMisAlignment.y()); + accConstMisAlignment_(2) = drawRandUniformDistNumFromInterval( + accSimParams_.intervalMisAlignment.z()); + + // Draw alignment error for each gyroscope axis + gyroConstMisAlignment_(0) = drawRandUniformDistNumFromInterval( + gyroSimParams_.intervalMisAlignment.x()); + gyroConstMisAlignment_(1) = drawRandUniformDistNumFromInterval( + gyroSimParams_.intervalMisAlignment.y()); + gyroConstMisAlignment_(2) = drawRandUniformDistNumFromInterval( + gyroSimParams_.intervalMisAlignment.z()); + + constErrorsInitialized_ = true; +} + +/** + * @brief Generates an erroneous accelerometer measurement vector. + * + * This function generates an erroneous accelerometer measurement vector. + * The measurement vector is generated by applying the following models: + * - gravity model, + * - scale factor, + * - misalignment model, + * - constant turn-on bias model, + * - stochastic error model, + * - saturation model, + * - quantization model. + * + * @param a_ib_b_true True acceleration of body frame with respect to inertial frame. + * @param q_b_n_true True quaternion of body frame with respect to navigation frame. +*/ +Eigen::Vector3d ImuSimulator::generateAccelerationMeasurement( + const Eigen::Vector3d& a_ib_b_true, const Eigen::Quaterniond q_b_n_true) { + // Check if constant errors have been initialized + if (constErrorsInitialized_ == false) { + std::cerr << "generateAccelerationMeasurement: Constant errors have not " + "been initialized!\n"; + return Eigen::Vector3d(0.0, 0.0, 0.0); + } + + // Initialize measurement vector with true acceleration + Eigen::Vector3d f_ib_b_meas = a_ib_b_true; + + // Update local gravity vector for last geodetic position + environmentalParameters_.g_n = calcGravityVectorWelmec(geoPositionLlh_); + + // Apply gravity model, now acceleration is a specific force + if (imuModelEnableSettings_.acc.enableLocalRefVec == true) { + f_ib_b_meas = calcAccelerometerGravityModel(f_ib_b_meas, q_b_n_true); + } + + // Apply constant turn-on bias model + if (imuModelEnableSettings_.acc.enableTurnOnBias == true) { + f_ib_b_meas = calcConstTurnOnBiasModel(f_ib_b_meas, acc); + } + + // Apply scale factor error model + if (imuModelEnableSettings_.acc.enableScaling == true) { + f_ib_b_meas = calcScaleFactorModel(f_ib_b_meas, accConstScaleFactor_); + } + + // Apply scale factor error and misalignment model + if (imuModelEnableSettings_.acc.enableMisAlignment == true) { + f_ib_b_meas = calcMisalignmentModel(f_ib_b_meas, accConstNonOrthogonality_, + accConstMisAlignment_); + } + + // Apply stochastic error model + if (imuModelEnableSettings_.acc.enableStochasticError == true) { + f_ib_b_meas = calcStochasticErrorModel(f_ib_b_meas, acc); + } + + // Apply saturation model + if (imuModelEnableSettings_.acc.enableSaturation == true) { + f_ib_b_meas = calcSaturationModel(f_ib_b_meas, accSimParams_.measRange); + } + + // Apply quantization model + if (imuModelEnableSettings_.acc.enableQuantization == true) { + f_ib_b_meas = calcQuantizationModel(f_ib_b_meas, accSimParams_.resolution); + } + + return f_ib_b_meas; +} + +/** + * @brief Generates an erroneous gyroscope measurement vector. + * + * This function generates an erroneous gyroscope measurement vector. + * The measurement vector is generated by applying the following models: + * - Earth angular velocity model, + * - scale factor, + * - misalignment model, + * - constant turn-on bias model, + * - stochastic error model, + * - saturation model, + * - quantization model. + * + * @param w_nb_b_true True angular velocity of body frame with respect to navigation (NED) frame. + * @param q_b_n_true True quaternion of body frame with respect to navigation frame. +*/ +Eigen::Vector3d ImuSimulator::generateGyroscopeMeasurement( + const Eigen::Vector3d& w_nb_b_true, const Eigen::Quaterniond q_b_n_true) { + // Check if constant errors have been initialized + if (constErrorsInitialized_ == false) { + std::cerr << "generateGyroscopeMeasurement: Constant errors have not been " + "initialized!\n"; + return Eigen::Vector3d(0.0, 0.0, 0.0); + } + + // Initialize measurement vector with true angular velocity + Eigen::Vector3d w_ib_b_meas = w_nb_b_true; + + // Update local angular velocity of Earth for last geodetic position + environmentalParameters_.w_ie_n = calcAngularVelocityOfEarth(geoPositionLlh_); + + // Update transport angular velocity for last geodetic position/velocity + w_en_n_ = calcTransportAngularVelocity(geoPositionLlh_, v_eb_n_); + + // Apply Earth angular velocity model + if (imuModelEnableSettings_.gyro.enableLocalRefVec == true) { + w_ib_b_meas = + calcGyroscopeEarthAngularVelocityModel(w_ib_b_meas, q_b_n_true); + } + + // Apply constant turn-on bias model + if (imuModelEnableSettings_.gyro.enableTurnOnBias == true) { + w_ib_b_meas = calcConstTurnOnBiasModel(w_ib_b_meas, gyro); + } + + // Apply scale factor error model + if (imuModelEnableSettings_.gyro.enableScaling == true) { + w_ib_b_meas = calcScaleFactorModel(w_ib_b_meas, gyroConstScaleFactor_); + } + + // Apply scale factor error and misalignment model + if (imuModelEnableSettings_.gyro.enableMisAlignment == true) { + w_ib_b_meas = calcMisalignmentModel(w_ib_b_meas, gyroConstNonOrthogonality_, + gyroConstMisAlignment_); + } + + // Apply stochastic error model + if (imuModelEnableSettings_.gyro.enableStochasticError == true) { + w_ib_b_meas = calcStochasticErrorModel(w_ib_b_meas, gyro); + } + + // Apply saturation model + if (imuModelEnableSettings_.gyro.enableSaturation == true) { + w_ib_b_meas = calcSaturationModel(w_ib_b_meas, gyroSimParams_.measRange); + } + + // Apply quantization model + if (imuModelEnableSettings_.gyro.enableQuantization == true) { + w_ib_b_meas = calcQuantizationModel(w_ib_b_meas, gyroSimParams_.resolution); + } + + return w_ib_b_meas; +} + +/** + * @brief Scaling error model for IMU measurements. + * + * Scaling errors result from imperfect calibration of the sensor's scaling factor. + * Since the sensor's onboard software already attempts to compensate for these errors, + * this function is meant to apply only to the uncompensated part. + * + * @param measurement Vector of IMU measurements (acceleration or angular velocity). + * @param scaleFactor Scale factors in all three dimensions (%). + * + * @return Measurement vector with scaling error. + */ +Eigen::Vector3d ImuSimulator::calcScaleFactorModel( + const Eigen::Vector3d& measurement, const Eigen::Vector3d& scaleFactor) { + // Apply scale factor error to measurement + Eigen::Vector3d measurementScaled = + measurement.array() * (1.0 + scaleFactor.array() / 100); + + return measurementScaled; +} + +/** + * @brief Misalignment and orthogonality error model for IMU measurements. + * + * The IMU's measurements are subject to two further types of errors: + * Non-orthogonality means that each of the axes can be tilted w.r.t. its perfect + * orthogonal position independently from other axes (in addition to misalignment). + * misalignment represents a rigid-body (orthogonal) rotational misplacement of + * The actual sensor axes with their locations marked on the sensor. + * This function applies all three of these errors to the measurement. Since the + * The sensor's onboard software already attempts to compensate for these errors; this + * function is meant to apply only to the uncompensated part. + * + * @param measurement Vector of IMU measurements (acceleration or angular velocity). +* @param nonOrthogonality Six angles between each pair of the axes (rad). + * @param misAlignment Three Euler angles representing the rigid-body rotation (rad). + * + * @return Measurement vector with misalignment and orthogonality error. + */ +Eigen::Vector3d ImuSimulator::calcMisalignmentModel( + const Eigen::Vector3d& measurement, + const Eigen::Matrix& nonOrthogonality, + const Eigen::Vector3d& misAlignment) { + + // Calculate misalignment matrix + Eigen::Matrix3d misAlignmentMatrix{ + Eigen::AngleAxisd(misAlignment(0), Eigen::Vector3d::UnitX()) * + Eigen::AngleAxisd(misAlignment(1), Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(misAlignment(2), Eigen::Vector3d::UnitZ())}; + + // Calculate non-orthogonality matrix + Eigen::Matrix3d nonOrthogonalityMatrix{ + {1.0, -nonOrthogonality(0), nonOrthogonality(1)}, + {nonOrthogonality(2), 1.0, -nonOrthogonality(3)}, + {-nonOrthogonality(4), nonOrthogonality(5), 1.0}}; + + // Apply misalignment and orthogonality error to measurement + Eigen::Vector3d misAlignedMeasurement = + misAlignmentMatrix * nonOrthogonalityMatrix * measurement; + + return misAlignedMeasurement; +} + +/** +* @brief Turn-on bias (constant bias) model for IMU measurements. +* +* An IMU shows a constant bias which changes every time the IMU is turned on. +* This function adds the constant turn-on bias to the measurement vector. +* The constant turn-on bias is drawn from a uniform distribution at class initialization. +* +* @param measurement Vector of IMU measurements (acceleration or angular velocity). +* @param measurementType Measurement type (acceleration or angular velocity). +* +* @return Measurement vector with added constant turn-on bias. +*/ +Eigen::Vector3d ImuSimulator::calcConstTurnOnBiasModel( + const Eigen::Vector3d& measurement, + const MeasurementType& measurementType) { + // Vector of measurement with constant turn-on bias + Eigen::Vector3d measurementWithBias; + + if (constErrorsInitialized_ == false) { + std::cerr << "calcConstTurnOnBiasModel: Constant errors not initialized!\n"; + return measurement; + } + + // Add const turn-on bias to measurement + if (measurementType == acc) { + measurementWithBias = measurement + accConstTurnOnBias_; + } else if (measurementType == gyro) { + measurementWithBias = measurement + gyroConstTurnOnBias_; + } else { + // Invalid measurement type + std::cerr << "calcConstTurnOnBiasModel: Invalid measurement type!\n"; + measurementWithBias = measurement; + } + + return measurementWithBias; +} + +/** + * @brief Stochastic error model for IMU measurements. + * + * This function adds the stochastic error to each axis of the measurement vector. + * The detailed implementation is in function calcDiscreteTimeEquivalentModel(). + * + * @param measurement Vector of IMU measurements (acceleration or angular velocity). + * @param measurementType Measurement type (acceleration or angular velocity). + * @param normDistNumbersMatrix Matrix of normal distributed random numbers. + * + * @return Measurement vector with added stochastic error. +*/ +Eigen::Vector3d ImuSimulator::calcStochasticErrorModel( + const Eigen::Vector3d& measurement, + const MeasurementType& measurementType) { + // Vector of measurement with stochastic error + Eigen::Vector3d measurementWithStochasticError; + + // Vector of colored noise for each axis + Eigen::Vector3d coloredNoise; + + // Update discrete-time equivalent state-space model for axes + if (measurementType == acc) { + // Get colored noise for each axis + coloredNoise(0) = updateDiscreteTimeEquivalentStateSpaceModel( + pAccStochasticErrors_, accSimParams_.N(axisX), accSimParams_.B(axisX), + accSimParams_.K(axisX), accSimParams_.corrTime(axisX), axisX); + coloredNoise(1) = updateDiscreteTimeEquivalentStateSpaceModel( + pAccStochasticErrors_, accSimParams_.N(axisY), accSimParams_.B(axisY), + accSimParams_.K(axisY), accSimParams_.corrTime(axisY), axisY); + coloredNoise(2) = updateDiscreteTimeEquivalentStateSpaceModel( + pAccStochasticErrors_, accSimParams_.N(axisZ), accSimParams_.B(axisZ), + accSimParams_.K(axisZ), accSimParams_.corrTime(axisZ), axisZ); + + // Add stochastic error to measurement + measurementWithStochasticError = measurement + coloredNoise; + + } else if (measurementType == gyro) { + // Get colored noise for each axis + coloredNoise(0) = updateDiscreteTimeEquivalentStateSpaceModel( + pGyroStochasticErrors_, gyroSimParams_.N(axisX), + gyroSimParams_.B(axisX), gyroSimParams_.K(axisX), + gyroSimParams_.corrTime(axisX), axisX); + coloredNoise(1) = updateDiscreteTimeEquivalentStateSpaceModel( + pGyroStochasticErrors_, gyroSimParams_.N(axisY), + gyroSimParams_.B(axisY), gyroSimParams_.K(axisY), + gyroSimParams_.corrTime(axisY), axisY); + coloredNoise(2) = updateDiscreteTimeEquivalentStateSpaceModel( + pGyroStochasticErrors_, gyroSimParams_.N(axisZ), + gyroSimParams_.B(axisZ), gyroSimParams_.K(axisZ), + gyroSimParams_.corrTime(axisZ), axisZ); + + // Add stochastic error to measurement + measurementWithStochasticError = measurement + coloredNoise; + + } else { + // Invalid measurement type + std::cerr << "calcStochasticErrorModel: Invalid measurement type!\n"; + measurementWithStochasticError = measurement; + } + + return measurementWithStochasticError; +} + +/** + * @brief Discrete-time equivalent (NBK) model of a Gauss-Markov process to model stochastic errors. + * + * This function calculates the discrete-time equivalent (NBK) model using a first-order Gauss-Markov process. + * An IMU shows stochastic errors which sum up to colored noise, modeled as: + * - velocity/angle random walk (N), + * - bias instability (B), + * - acceleration/rate random walk (K). + * The internal Gauss-Markov state space model is updated with these parameters if this function is called. + * The model has internal states for time-correleated bias instability and acceleration/rate random walk. + * The white noise velocity/angle random walk is added at the end to the internal states. + * The output of the Gauss-Markov model is then the sum of all noise components which yields colored noise. + * The function returns the colored noise for one axis as double (scalar). + * @see [Inertial Measurement Unit Error Modeling Tutorial](https://ieeexplore.ieee.org/document/9955423) + * + * @param pImuStochasticErrors Pointer to struct of IMU stochastic error states. + * @param N Velocity/angle random walk. + * @param B Bias instability. + * @param K Accel./rate random walk. + * @param corrTime Correlation time of Gauss-Markov error model. + * @param normDistNumbers Vector of normal distributed random numbers. + * @param axisId Axis identifier. + * + * @return Sum of noise states (colored noise). +*/ +double ImuSimulator::updateDiscreteTimeEquivalentStateSpaceModel( + std::shared_ptr pImuStochasticErrors, const double N, + const double B, const double K, const double corrTime, + const AxisIdentifier axisId) { + // PSD velocity/angle random walk + double S_N = N * N; + + // PSD bias instability + double S_B = (2 * B * B * std::log(2)) / (M_PI * 0.4365 * 0.4365 * corrTime); + + // PSD accel./rate random walk + double S_K = K * K; + + // Gauss-Markov time correlation constant + double corrConstant = 1 / corrTime; + + // Discrete-time state-space model matrices + Eigen::Matrix2d Phi_k; + Phi_k << std::exp(-corrConstant * dt_), 0, 0, 1; + + Eigen::MatrixXd H(1, 2); + H << 1, 1; + + // Discrete-time bias instability covariance + double Q_B_k = + S_B / (2 * corrConstant) * (1 - std::exp(-2 * corrConstant * dt_)); + + // Discrete-time rate random walk covariance + double Q_K_k = S_K * dt_; + + // Discrete-time measurement noise covariance (velocity/angle random walk) + double Q_N_k = S_N / dt_; + + // Define random number vector + Eigen::Vector3d randomNumbers; + + if (useFixedRandomNumbers_ == true) { + // Draw fixed random numbers for testing/debugging + randomNumbers = Eigen::Vector3d(1.0, 2.0, 3.0); + } else { + // Draw normal distributed random samples from white noise processes + randomNumbers = + Eigen::Vector3d(drawRandNormalDistNum(), drawRandNormalDistNum(), + drawRandNormalDistNum()); + } + + // Draw measurement noise scalar (velocity/angle random walk) + Eigen::VectorXd nu_N_k = + Eigen::VectorXd::Constant(1, std::sqrt(Q_N_k) * randomNumbers(0)); + + // Draw process noise vector (bias instability and accel./rate random walk) + Eigen::Vector2d w_BK_k = Eigen::Vector2d(std::sqrt(Q_B_k) * randomNumbers(1), + std::sqrt(Q_K_k) * randomNumbers(2)); + + // Create state vector for one axis + double z_B = pImuStochasticErrors->z_B(axisId); + double z_K = pImuStochasticErrors->z_K(axisId); + + Eigen::Vector2d x_k = Eigen::Vector2d(z_B, z_K); + + // Calculate new state vector + Eigen::Vector2d x_k_1 = Phi_k * x_k + w_BK_k; + + // Calculate new measurement vector + Eigen::VectorXd z_k = H * x_k + nu_N_k; + + // Update internal Gauss-Markov state space model + pImuStochasticErrors->z_N(axisId) = z_k(0); + pImuStochasticErrors->z_B(axisId) = x_k_1(0); + pImuStochasticErrors->z_K(axisId) = x_k_1(1); + + // Return colored noise as double (scalar) + return z_k(0); +} + +/** +* @brief Gravity model for IMU acceleration measurements. +* +* An accelerometer measures the sum of the true acceleration of the body frame and the gravity vector (specific force). +* The true specific force is calculated by subtracting the gravity vector from the true acceleration. +* +* @param a_ib_b True acceleration of body frame with respect to inertial frame. +* @param q_b_n_true True quaternion of body frame with respect to navigation frame. +* +* @return True specific force of body frame with respect to inertial frame. +*/ +Eigen::Vector3d ImuSimulator::calcAccelerometerGravityModel( + const Eigen::Vector3d& a_ib_b, const Eigen::Quaterniond& q_b_n_true) { + // Transform quaternion to rotation matrix + Eigen::Matrix3d C_n_b = q_b_n_true.toRotationMatrix().transpose(); + + // Calculate true specific force by subtracting gravity + Eigen::Vector3d f_ib_b = a_ib_b - C_n_b * environmentalParameters_.g_n; + + return f_ib_b; +} + +/** + * @brief Model for ideal gyroscope angular velocity measurements. + * + * A gyroscope measures the true angular velocity of the body with respect to the inertial frame. + * Earth is rotating with an angular velocity, which needs to be added to the true angular velocity. + * The rotation of the navigation frame with respect to Earth also needs to be considered (transport angular velocity). + * + * @param w_nb_b True angular velocity of body frame with respect to NED frame. + * @param q_b_n_true True quaternion of body frame with respect to navigation frame. + * + * @return True angular velocity of body frame with respect to inertial frame. +*/ +Eigen::Vector3d ImuSimulator::calcGyroscopeEarthAngularVelocityModel( + const Eigen::Vector3d& w_nb_b, const Eigen::Quaterniond& q_b_n_true) { + // Transform quaternion to rotation matrix + Eigen::Matrix3d C_n_b = q_b_n_true.toRotationMatrix().transpose(); + + // Calculate true angular velocity by adding Earth/transport angular velocity + Eigen::Vector3d w_ib_b = + w_nb_b + C_n_b * (environmentalParameters_.w_ie_n + w_en_n_); + + return w_ib_b; +} + +/** + * @brief Calculate the gravity vector in the navigation frame at a given latitude and height. + * + * This function calculates the local gravity vector in the navigation (NED) frame using + * the European WELMEC gravity model at a given latitude and height. + * @see [WELMEC Guide](https://www.welmec.org/welmec/documents/guides/2/WELMEC_Guide_2_v2015.pdf) + * + * @param geoPositionLlh Geodetic position (latitude, longitude, height) of body frame (IMU) + * + * @return Local gravity vector in the navigation frame (m/s²). + */ +Eigen::Vector3d ImuSimulator::calcGravityVectorWelmec( + const Eigen::Vector3d& geoPositionLlh) { + // Gravity reference values + const double g_0 = 9.780318; + const double g_1 = 5.3024e-3; + const double g_2 = 5.8e-6; + const double g_3 = 3.085e-6; + + // Local gravity vector according WELMEC model + Eigen::Vector3d g_n; + g_n << 0.0, 0.0, + g_0 * (1 + g_1 * std::pow(std::sin(geoPositionLlh(0)), 2) - + g_2 * std::pow(std::sin(2 * geoPositionLlh(0)), 2)) - + g_3 * geoPositionLlh(2); + + return g_n; +} + +/** + * @brief Calculate the angular velocity of Earth in the navigation frame at a given latitude. + * + * This function calculates the angular velocity of Earth in the navigation (NED) frame at a given latitude. + * @see [Aided Navigation - GPS with High Rate Sensors](https://dl.acm.org/doi/10.5555/1594745) + * + * @param geoPositionLlh Geodetic position (latitude, longitude, height) of body frame (IMU) + * + * @return Angular velocity vector of Earth in the navigation frame (rad/s). + */ +Eigen::Vector3d ImuSimulator::calcAngularVelocityOfEarth( + const Eigen::Vector3d& geoPositionLlh) { + const double omega_ie = 7.292115e-5; // Angular velocity of Earth (rad/s) + + // Angular velocity of Earth in navigation frame + Eigen::Vector3d w_ie_n; + w_ie_n << omega_ie * std::cos(geoPositionLlh(0)), 0.0, + -omega_ie * std::sin(geoPositionLlh(0)); + + return w_ie_n; +} + +/** + * @brief Calculate the transport angular velocity in the navigation frame. + * + * This function calculates the transport angular velocity in the navigation (NED) frame. + * + * @param geoPositionLlh Geodetic position (latitude, longitude, height) of body frame (IMU) + * @param v_eb_n Velocity of the body (b) frame with respect to the ECEF (e) frame (m/s). + * + * @return Transport angular velocity vector in the navigation (NED) frame (rad/s). +*/ +Eigen::Vector3d ImuSimulator::calcTransportAngularVelocity( + const Eigen::Vector3d& geoPositionLlh, const Eigen::Vector3d& v_eb_n) { + // Semi-major axis of Earth (m) + const double a = 6378137.0; + + // Semi-minor axis of Earth (m) + const double b = 6356752.31424518; + + // Flattening of Earth + const double f = (a - b) / a; + + // Eccentricity of Earth + const double e = std::sqrt(f * (2 - f)); + + // Radius of curvature in the meridian plane (m) + const double R_n = a * (1 - e * e) / + std::pow((1 - (e * e) * (std::sin(geoPositionLlh(0)) * + std::sin(geoPositionLlh(0)))), + 1.5); + + // Radius of curvature in the prime vertical (m) + const double R_e = a / std::sqrt(1 - (e * e) * (std::sin(geoPositionLlh(0)) * + std::sin(geoPositionLlh(0)))); + + // Average radius of curvature of Earth (m) + const double R_0 = std::sqrt(R_n * R_e); + + // Transport angular velocity (rad/s) + Eigen::Vector3d w_en_n; + w_en_n << v_eb_n(1) / (R_0 + geoPositionLlh(2)), + -v_eb_n(0) / (R_0 + geoPositionLlh(2)), + -(v_eb_n(1) * std::tan(geoPositionLlh(0))) / (R_0 + geoPositionLlh(2)); + + return w_en_n; +} + +/** + * @brief Saturation model for IMU measurements. + * + * This function saturates the measurement vector to the absolute values of the measRange vector. + * This replicates that an IMU can only measure values within a certain range. + * + * @param measurement Vector of IMU measurements (acceleration or angular velocity). + * @param measRange Vector of measurement ranges (acceleration or angular velocity). + * + * @return Saturated measurement vector. +*/ +Eigen::Vector3d ImuSimulator::calcSaturationModel( + const Eigen::Vector3d& measurement, const Eigen::Vector3d& measRange) { + // Saturated measurement vector + Eigen::Vector3d saturatedMeasurement; + + // Saturate elements of measurement to absolute values of measRange + saturatedMeasurement(0) = std::min(std::abs(measurement(0)), measRange(0)) * + (measurement(0) >= 0 ? 1 : -1); + saturatedMeasurement(1) = std::min(std::abs(measurement(1)), measRange(1)) * + (measurement(1) >= 0 ? 1 : -1); + saturatedMeasurement(2) = std::min(std::abs(measurement(2)), measRange(2)) * + (measurement(2) >= 0 ? 1 : -1); + + return saturatedMeasurement; +} + +/** + * @brief Quantization model for IMU measurements. + * + * This function quantizes the measurement vector to the resolution of the resolution vector. + * This replicates that an IMU can only measure values with a certain resolution (Analog/Digital conversion). + * + * @param measurement Vector of IMU measurements (acceleration or angular velocity). + * @param resolution Vector of measurement resolutions (acceleration or angular velocity). + * + * @return Quantized measurement vector. +*/ +Eigen::Vector3d ImuSimulator::calcQuantizationModel( + const Eigen::Vector3d& measurement, const Eigen::Vector3d& resolution) { + // Quantized measurement vector + Eigen::Vector3d quantizedMeasurement; + + // Quantize elements of measurement to resolution + quantizedMeasurement(0) = + std::round(measurement(0) / resolution(0)) * resolution(0); + quantizedMeasurement(1) = + std::round(measurement(1) / resolution(1)) * resolution(1); + quantizedMeasurement(2) = + std::round(measurement(2) / resolution(2)) * resolution(2); + + return quantizedMeasurement; +} + +/** +* @brief Reset the IMU simulator. +*/ +void ImuSimulator::resetImuSimulator() { + // Reset IMU stochastic errors + pAccStochasticErrors_->z_N = Eigen::Vector3d::Zero(); + pAccStochasticErrors_->z_B = Eigen::Vector3d::Zero(); + pAccStochasticErrors_->z_K = Eigen::Vector3d::Zero(); + + pGyroStochasticErrors_->z_N = Eigen::Vector3d::Zero(); + pGyroStochasticErrors_->z_B = Eigen::Vector3d::Zero(); + pGyroStochasticErrors_->z_K = Eigen::Vector3d::Zero(); + + // Reset flag indicating if IMU constant errors are initialized + constErrorsInitialized_ = false; + + // Re-Initialize IMU constant errors + initializeRandomConstantErrors(); +} + +/** + * @brief Draws a random number from a normal distribution. + * + * This function draws a random number from a normal distribution with the given mean and standard deviation. + * + * @return Single random number from the normal distribution. +*/ +double ImuSimulator::drawRandNormalDistNum() { + // Generate normal distributed random number + return normalDistribution_(randomNumberGenerator_); +} + +/** + * @brief Draws a random number from a uniform distribution. + * + * This function draws a random number from a uniform distribution in a given interval [-interval, +interval]. + * + * @param interval Interval of the uniform distribution. + * @return Single random number from the uniform distribution. +*/ +double ImuSimulator::drawRandUniformDistNumFromInterval(const double interval) { + // Generate uniform distributed random number + double randUniformNum = uniformDistribution_(randomNumberGenerator_); + + double a = -interval; + double b = interval; + + return (b - a) * randUniformNum + a; +} + +} // namespace imu_simulator diff --git a/src/imu_simulator_node.cpp b/src/imu_simulator_node.cpp new file mode 100644 index 0000000..d9b4dd4 --- /dev/null +++ b/src/imu_simulator_node.cpp @@ -0,0 +1,1079 @@ +/*@license BSD-3 https://opensource.org/licenses/BSD-3-Clause +Copyright (c) 2024, Institute of Automatic Control - RWTH Aachen University +Maximilian Nitsch (m.nitsch@irt.rwth-aachen.de) +All rights reserved. +*/ + +// NOLINTNEXTLINE(build/c++11) +#include +#include +#include + +#include "imu_simulator_node.h" + +namespace imu_simulator { + +/** + * @brief IMU simulator node constructor with default config file path + * + * @param pImuSimulator Pointer to the IMU simulator object +*/ +ImuSimulatorNode::ImuSimulatorNode(std::shared_ptr pImuSimulator) + : Node("imu_simulator_node"), + pImuSimulator_(pImuSimulator), + groundTruthOdomMsg_(nullptr), + groundTruthAccelMsg_(nullptr), + lastOdomTimestamp_(0, 0), + lastAccelTimestamp_(0, 0), + sampleTime_(0.0), + first_odometry_received_(false), + odometry_timeout_(false), + first_acceleration_received_(false), + acceleration_timeout_(false), + calc_acc_from_odom_vel_(false), + firstLinearVelocityReceived_(false), + timeLastOdom_(now()) { + RCLCPP_INFO(get_logger(), "Configuring IMU simulator node..."); + + // Declare adnd retrieve parameters and load them to IMU simulator + declareAndRetrieveGeneralSettings(); + declareAndRetrieveImuParameters(); + declareAndRetrieveEnableSettings(); + declareAndRetrieveEnvironmentalSettings(); + + RCLCPP_INFO(get_logger(), "Parameters from YAML config loaded successfully."); + + // Reset IMU simulator to ensure that constant errors are initialized + pImuSimulator_->resetImuSimulator(); + + // Print IMU simulator parameters + std::stringstream ss = pImuSimulator_->printImuSimulatorParameters(); + RCLCPP_INFO(get_logger(), "%s", ss.str().c_str()); + + // Get odometry topic name from launch file or use default + std::string groundTruthOdometryTopicName; + + // Get the value of the topic_name parameter + this->declare_parameter("topic_name_odom", rclcpp::PARAMETER_STRING); + + this->get_parameter_or("topic_name_odom", groundTruthOdometryTopicName, + std::string("/nanoauv/odometry")); + + RCLCPP_INFO(get_logger(), "Subscribing to ground truth odometry topic: %s", + groundTruthOdometryTopicName.c_str()); + + // Initialize the odometry subscriber + pOdometrySubscriber_ = this->create_subscription( + groundTruthOdometryTopicName, 10, + std::bind(&ImuSimulatorNode::odometryCallback, this, + std::placeholders::_1)); + + // Create subscriber for ground truth acceleration if enabled + if (!calc_acc_from_odom_vel_) { + // Get acceleration topic name from launch file or use default + std::string groundTruthAccelTopicName; + this->get_parameter_or("topic_name_accel", groundTruthAccelTopicName, + std::string("/nanoauv/accel")); + + this->declare_parameter("topic_name_accel", rclcpp::PARAMETER_STRING); + + RCLCPP_INFO(get_logger(), + "Subscribing to ground truth acceleration topic: %s", + groundTruthAccelTopicName.c_str()); + + // Initialize the acceleration subscriber + pAccelerationSubscriber_ = + this->create_subscription( + groundTruthAccelTopicName, 10, + std::bind(&ImuSimulatorNode::accelerationCallback, this, + std::placeholders::_1)); + } else { + // Initialize the ground truth acceleration message with zeros to circumvent null pointer + groundTruthAccelMsg_ = std::make_shared(); + + groundTruthAccelMsg_->header.stamp = now(); + groundTruthAccelMsg_->header.frame_id = "base_link"; + + groundTruthAccelMsg_->accel.linear.x = 0.0; + groundTruthAccelMsg_->accel.linear.y = 0.0; + groundTruthAccelMsg_->accel.linear.z = 0.0; + + groundTruthAccelMsg_->accel.angular.x = 0.0; + groundTruthAccelMsg_->accel.angular.y = 0.0; + groundTruthAccelMsg_->accel.angular.z = 0.0; + + first_acceleration_received_ = true; + + lastAccelTimestamp_ = now(); + + RCLCPP_INFO( + get_logger(), + "Ground truth acceleration topic is not used. Acceleration is " + "calculated by numerical differentiation of odometry velocity."); + } + + // Retrieve the namespace from the node + std::string nsStr = get_namespace(); + + // Initialize the IMU data publisher + pImuPublisher_ = + this->create_publisher(nsStr + "/data", 10); + + // Initialize the IMU visualization publisher + pImuAccelStampedPublisher_ = + this->create_publisher( + nsStr + "/data_visualization", 10); + + // Initialize the acceleration publisher + pTrueAccelerationPublisher_ = + this->create_publisher( + nsStr + "/true_linear_acceleration", 10); + + // Initialize the angular rate publisher + pTrueAngularRatePublisher_ = + this->create_publisher( + nsStr + "/true_linear_angular_velocity", 10); + + // Initialize the diagnostic status publisher + pDiagnosticPublisher_ = + this->create_publisher( + nsStr + "/diagnostic", 10); + + // Initialize the tf2 broadcaster + pStaticTf2Broadcaster_ = + std::make_shared(this); + + // Publish static tf2 transformations + this->publishTf2Transforms(); + + // Set IMU sample time with data from the parameter server + sampleTime_ = + get_parameter("imu_simulator.general_settings.sample_time").as_double(); + + // Convert sample time to milliseconds and cast to int + int sampleTimeInt = static_cast(sampleTime_ * 1e3); + + RCLCPP_INFO(get_logger(), "IMU simulator node executing with %dms.", + sampleTimeInt); + + // Create a timer to call the IMU simulator loop callback function + pTimer_ = this->create_wall_timer( + std::chrono::milliseconds(sampleTimeInt), + std::bind(&ImuSimulatorNode::imuSimulatorLoopCallback, this)); + + // Create timer for odometry subscriber + pOdometryTimeOutTimer_ = this->create_wall_timer( + std::chrono::seconds(5), + std::bind(&ImuSimulatorNode::odometryTimeOutCallback, this)); + + if (!calc_acc_from_odom_vel_) { + // Create timer for acceleration subscriber + pAccelerationTimeOutTimer_ = this->create_wall_timer( + std::chrono::seconds(5), + std::bind(&ImuSimulatorNode::accelerationTimeOutCallback, this)); + } + + RCLCPP_INFO(get_logger(), "IMU simulator node initialized."); + + if (calc_acc_from_odom_vel_) { + RCLCPP_INFO(get_logger(), + "IMU simulator node waiting for first odometry message"); + } else { + RCLCPP_INFO( + get_logger(), + "IMU simulator node waiting for first odometry and acceleration " + "message..."); + } +} + +/** + * @brief Declare and retrieve general settings from the parameter server +*/ +void ImuSimulatorNode::declareAndRetrieveGeneralSettings() { + // General settings + double sampleTime; + int seed; + bool useConstantSeed; + + // Declare general settings + this->declare_parameter("imu_simulator.general_settings.sample_time", + rclcpp::PARAMETER_DOUBLE); + this->declare_parameter("imu_simulator.general_settings.seed", + rclcpp::PARAMETER_INTEGER); + this->declare_parameter("imu_simulator.general_settings.use_constant_seed", + rclcpp::PARAMETER_BOOL); + this->declare_parameter( + "imu_simulator.general_settings." + "calc_accel_from_odometry_velocity", + rclcpp::PARAMETER_BOOL); + + // Retrieve general settings + sampleTime = this->get_parameter("imu_simulator.general_settings.sample_time") + .as_double(); + seed = this->get_parameter("imu_simulator.general_settings.seed").as_int(); + useConstantSeed = + this->get_parameter("imu_simulator.general_settings.use_constant_seed") + .as_bool(); + calc_acc_from_odom_vel_ = this->get_parameter( + "imu_simulator.general_settings." + "calc_accel_from_odometry_velocity") + .as_bool(); + + // Set IMU simulator sample time and seed + pImuSimulator_->setImuSampleTime(sampleTime); + + // Set seed depending on the useConstantSeed flag + if (!useConstantSeed) { + // Draw a random seed from the random device + std::random_device rd; + seed = rd(); + pImuSimulator_->setSimulatorSeed(seed); + RCLCPP_INFO(get_logger(), "Using random seed: %d", seed); + } else { + // Set the random number generator seed + pImuSimulator_->setSimulatorSeed(seed); + RCLCPP_INFO(get_logger(), "Using seed from config file: %d", seed); + } +} + +/** + * @brief Declare and retrieve IMU model parameters from the parameter server +*/ +void ImuSimulatorNode::declareAndRetrieveImuParameters() { + // Accelerometer and gyroscope model parameters + ImuSimParams accSimParams; + ImuSimParams gyroSimParams; + + // Declare model parameters accelerometer + this->declare_parameter( + "imu_simulator.model_parameter_settings.accelerometer.N", + rclcpp::PARAMETER_DOUBLE_ARRAY); + this->declare_parameter( + "imu_simulator.model_parameter_settings.accelerometer.B", + rclcpp::PARAMETER_DOUBLE_ARRAY); + this->declare_parameter( + "imu_simulator.model_parameter_settings.accelerometer.K", + rclcpp::PARAMETER_DOUBLE_ARRAY); + this->declare_parameter( + "imu_simulator.model_parameter_settings.accelerometer.correlation_time", + rclcpp::PARAMETER_DOUBLE_ARRAY); + this->declare_parameter( + "imu_simulator.model_parameter_settings.accelerometer.interval_turn_on_" + "bias", + rclcpp::PARAMETER_DOUBLE_ARRAY); + this->declare_parameter( + "imu_simulator.model_parameter_settings.accelerometer.measurement_range", + rclcpp::PARAMETER_DOUBLE_ARRAY); + this->declare_parameter( + "imu_simulator.model_parameter_settings.accelerometer.interval_" + "misalignment", + rclcpp::PARAMETER_DOUBLE_ARRAY); + this->declare_parameter( + "imu_simulator.model_parameter_settings.accelerometer.interval_scale_" + "factor", + rclcpp::PARAMETER_DOUBLE_ARRAY); + this->declare_parameter( + "imu_simulator.model_parameter_settings.accelerometer.resolution", + rclcpp::PARAMETER_DOUBLE_ARRAY); + + // Declare model parameters gyroscope + this->declare_parameter("imu_simulator.model_parameter_settings.gyroscope.N", + rclcpp::PARAMETER_DOUBLE_ARRAY); + + this->declare_parameter("imu_simulator.model_parameter_settings.gyroscope.B", + rclcpp::PARAMETER_DOUBLE_ARRAY); + + this->declare_parameter("imu_simulator.model_parameter_settings.gyroscope.K", + rclcpp::PARAMETER_DOUBLE_ARRAY); + this->declare_parameter( + "imu_simulator.model_parameter_settings.gyroscope.correlation_time", + rclcpp::PARAMETER_DOUBLE_ARRAY); + this->declare_parameter( + "imu_simulator.model_parameter_settings.gyroscope.interval_turn_on_bias", + rclcpp::PARAMETER_DOUBLE_ARRAY); + this->declare_parameter( + "imu_simulator.model_parameter_settings.gyroscope.measurement_range", + rclcpp::PARAMETER_DOUBLE_ARRAY); + this->declare_parameter( + "imu_simulator.model_parameter_settings.gyroscope.interval_misalignment", + rclcpp::PARAMETER_DOUBLE_ARRAY); + this->declare_parameter( + "imu_simulator.model_parameter_settings.gyroscope.interval_scale_factor", + rclcpp::PARAMETER_DOUBLE_ARRAY); + this->declare_parameter( + "imu_simulator.model_parameter_settings.gyroscope.resolution", + rclcpp::PARAMETER_DOUBLE_ARRAY); + + // Retrieve model parameters accelerometer + std::vector accN = + this->get_parameter( + "imu_simulator.model_parameter_settings.accelerometer.N") + .as_double_array(); + std::vector accB = + this->get_parameter( + "imu_simulator.model_parameter_settings.accelerometer.B") + .as_double_array(); + std::vector accK = + this->get_parameter( + "imu_simulator.model_parameter_settings.accelerometer.K") + .as_double_array(); + std::vector accCorrTime = + this->get_parameter( + "imu_simulator.model_parameter_settings.accelerometer." + "correlation_time") + .as_double_array(); + std::vector accIntervalTurnOnBias = + this->get_parameter( + "imu_simulator.model_parameter_settings.accelerometer.interval_" + "turn_on_bias") + .as_double_array(); + std::vector accMeasRange = + this->get_parameter( + "imu_simulator.model_parameter_settings.accelerometer." + "measurement_range") + .as_double_array(); + std::vector accIntervalMisAlignment = + this->get_parameter( + "imu_simulator.model_parameter_settings.accelerometer.interval_" + "misalignment") + .as_double_array(); + std::vector accIntervalScaleFactor = + this->get_parameter( + "imu_simulator.model_parameter_settings.accelerometer.interval_" + "scale_factor") + .as_double_array(); + std::vector accResolution = + this->get_parameter( + "imu_simulator.model_parameter_settings.accelerometer.resolution") + .as_double_array(); + + // Retrieve model parameters gyroscope + std::vector gyroN = + this->get_parameter("imu_simulator.model_parameter_settings.gyroscope.N") + .as_double_array(); + std::vector gyroB = + this->get_parameter("imu_simulator.model_parameter_settings.gyroscope.B") + .as_double_array(); + std::vector gyroK = + this->get_parameter("imu_simulator.model_parameter_settings.gyroscope.K") + .as_double_array(); + std::vector gyroCorrTime = + this->get_parameter( + "imu_simulator.model_parameter_settings.gyroscope." + "correlation_time") + .as_double_array(); + std::vector gyroIntervalTurnOnBias = + this->get_parameter( + "imu_simulator.model_parameter_settings.gyroscope.interval_turn_" + "on_bias") + .as_double_array(); + std::vector gyroMeasRange = + this->get_parameter( + "imu_simulator.model_parameter_settings.gyroscope.measurement_" + "range") + .as_double_array(); + std::vector gyroIntervalMisAlignment = + this->get_parameter( + "imu_simulator.model_parameter_settings.gyroscope.interval_" + "misalignment") + .as_double_array(); + std::vector gyroIntervalScaleFactor = + this->get_parameter( + "imu_simulator.model_parameter_settings.gyroscope.interval_scale_" + "factor") + .as_double_array(); + std::vector gyroResolution = + this->get_parameter( + "imu_simulator.model_parameter_settings.gyroscope.resolution") + .as_double_array(); + + // Assign model parameters to struct accelerometer + accSimParams.N = doubleVectorToEigenVector3(accN); + accSimParams.B = doubleVectorToEigenVector3(accB); + accSimParams.K = doubleVectorToEigenVector3(accK); + accSimParams.corrTime = doubleVectorToEigenVector3(accCorrTime); + accSimParams.intervalTurnOnBias = + doubleVectorToEigenVector3(accIntervalTurnOnBias); + accSimParams.measRange = doubleVectorToEigenVector3(accMeasRange); + accSimParams.intervalMisAlignment = + doubleVectorToEigenVector3(accIntervalMisAlignment); + accSimParams.intervalScaleFactor = + doubleVectorToEigenVector3(accIntervalScaleFactor); + accSimParams.resolution = doubleVectorToEigenVector3(accResolution); + + // Assign model parameters to struct gyroscope + gyroSimParams.N = doubleVectorToEigenVector3(gyroN); + gyroSimParams.B = doubleVectorToEigenVector3(gyroB); + gyroSimParams.K = doubleVectorToEigenVector3(gyroK); + gyroSimParams.corrTime = doubleVectorToEigenVector3(gyroCorrTime); + gyroSimParams.intervalTurnOnBias = + doubleVectorToEigenVector3(gyroIntervalTurnOnBias); + gyroSimParams.measRange = doubleVectorToEigenVector3(gyroMeasRange); + gyroSimParams.intervalMisAlignment = + doubleVectorToEigenVector3(gyroIntervalMisAlignment); + gyroSimParams.intervalScaleFactor = + doubleVectorToEigenVector3(gyroIntervalScaleFactor); + gyroSimParams.resolution = doubleVectorToEigenVector3(gyroResolution); + + // Set IMU simulator parameters + pImuSimulator_->setImuSimParameters(accSimParams, gyroSimParams); +} + +/** + * @brief Declare and retrieve IMU model enable settings from the parameter server +*/ +void ImuSimulatorNode::declareAndRetrieveEnableSettings() { + // Model enable settings IMU + ImuModelEnableSettings imuModelEnableSettings; + + // Model enable settings accelerometer + ModelEnableSettings accModelEnableSettings; + + // Model enable settings gyroscope + ModelEnableSettings gyroModelEnableSettings; + + // Model enable settings accelerometer + this->declare_parameter( + "imu_simulator.model_enable_settings.accelerometer.enable_local_ref_vec", + rclcpp::PARAMETER_BOOL); + this->declare_parameter( + "imu_simulator.model_enable_settings.accelerometer.enable_turn_on_bias", + rclcpp::PARAMETER_BOOL); + this->declare_parameter( + "imu_simulator.model_enable_settings.accelerometer.enable_scaling", + rclcpp::PARAMETER_BOOL); + this->declare_parameter( + "imu_simulator.model_enable_settings.accelerometer.enable_misalignment", + rclcpp::PARAMETER_BOOL); + this->declare_parameter( + "imu_simulator.model_enable_settings.accelerometer.enable_stochastic_" + "error", + rclcpp::PARAMETER_BOOL); + this->declare_parameter( + "imu_simulator.model_enable_settings.accelerometer.enable_saturation", + rclcpp::PARAMETER_BOOL); + this->declare_parameter( + "imu_simulator.model_enable_settings.accelerometer.enable_quantization", + rclcpp::PARAMETER_BOOL); + + // Model enable settings gyroscope + this->declare_parameter( + "imu_simulator.model_enable_settings.gyroscope.enable_local_ref_vec", + rclcpp::PARAMETER_BOOL); + this->declare_parameter( + "imu_simulator.model_enable_settings.gyroscope.enable_turn_on_bias", + rclcpp::PARAMETER_BOOL); + this->declare_parameter( + "imu_simulator.model_enable_settings.gyroscope.enable_scaling", + rclcpp::PARAMETER_BOOL); + this->declare_parameter( + "imu_simulator.model_enable_settings.gyroscope.enable_misalignment", + rclcpp::PARAMETER_BOOL); + this->declare_parameter( + "imu_simulator.model_enable_settings.gyroscope.enable_stochastic_error", + rclcpp::PARAMETER_BOOL); + this->declare_parameter( + "imu_simulator.model_enable_settings.gyroscope.enable_saturation", + rclcpp::PARAMETER_BOOL); + this->declare_parameter( + "imu_simulator.model_enable_settings.gyroscope.enable_quantization", + rclcpp::PARAMETER_BOOL); + + // Retrieve model enable settings accelerometer + bool accEnableLocalRefVec = + this->get_parameter( + "imu_simulator.model_enable_settings.accelerometer." + "enable_local_ref_vec") + .as_bool(); + bool accEnableTurnOnBias = + this->get_parameter( + "imu_simulator.model_enable_settings.accelerometer." + "enable_turn_on_bias") + .as_bool(); + bool accEnableScaling = + this->get_parameter( + "imu_simulator.model_enable_settings.accelerometer." + "enable_scaling") + .as_bool(); + bool accEnableMisAlignment = this->get_parameter( + "imu_simulator.model_enable_settings." + "accelerometer.enable_misalignment") + .as_bool(); + bool accEnableStochasticError = + this->get_parameter( + "imu_simulator.model_enable_settings.accelerometer." + "enable_stochastic_error") + .as_bool(); + bool accEnableSaturation = this->get_parameter( + "imu_simulator.model_enable_settings." + "accelerometer.enable_saturation") + .as_bool(); + bool accEnableQuantization = this->get_parameter( + "imu_simulator.model_enable_settings." + "accelerometer.enable_quantization") + .as_bool(); + + // Retrieve model enable settings gyroscope + bool gyroEnableLocalRefVec = + this->get_parameter( + "imu_simulator.model_enable_settings.gyroscope.enable_local_ref_" + "vec") + .as_bool(); + bool gyroEnableTurnOnBias = this->get_parameter( + "imu_simulator.model_enable_settings." + "gyroscope.enable_turn_on_bias") + .as_bool(); + bool gyroEnableScaling = this->get_parameter( + "imu_simulator." + "model_enable_settings." + "gyroscope." + "enable_scaling") + .as_bool(); + bool gyroEnableMisAlignment = this->get_parameter( + "imu_simulator.model_enable_settings." + "gyroscope.enable_misalignment") + .as_bool(); + bool gyroEnableStochasticError = + this->get_parameter( + "imu_simulator.model_enable_settings." + "gyroscope.enable_stochastic_error") + .as_bool(); + bool gyroEnableSaturation = this->get_parameter( + "imu_simulator.model_enable_settings." + "gyroscope.enable_saturation") + .as_bool(); + bool gyroEnableQuantization = this->get_parameter( + "imu_simulator.model_enable_settings." + "gyroscope.enable_quantization") + .as_bool(); + + // Assign model enable settings to struct accelerometer + accModelEnableSettings.enableLocalRefVec = accEnableLocalRefVec; + accModelEnableSettings.enableTurnOnBias = accEnableTurnOnBias; + accModelEnableSettings.enableScaling = accEnableScaling; + accModelEnableSettings.enableMisAlignment = accEnableMisAlignment; + accModelEnableSettings.enableStochasticError = accEnableStochasticError; + accModelEnableSettings.enableSaturation = accEnableSaturation; + accModelEnableSettings.enableQuantization = accEnableQuantization; + + // Assign model enable settings to struct gyroscope + gyroModelEnableSettings.enableLocalRefVec = gyroEnableLocalRefVec; + gyroModelEnableSettings.enableTurnOnBias = gyroEnableTurnOnBias; + gyroModelEnableSettings.enableScaling = gyroEnableScaling; + gyroModelEnableSettings.enableMisAlignment = gyroEnableMisAlignment; + gyroModelEnableSettings.enableStochasticError = gyroEnableStochasticError; + gyroModelEnableSettings.enableSaturation = gyroEnableSaturation; + gyroModelEnableSettings.enableQuantization = gyroEnableQuantization; + + // Assign model enable settings to struct IMU model enable settings + imuModelEnableSettings.acc = accModelEnableSettings; + imuModelEnableSettings.gyro = gyroModelEnableSettings; + + // Set IMU simulator enable settings + pImuSimulator_->setImuEnableSettings(imuModelEnableSettings); +} + +/** + * @brief Declare and retrieve environmental settings from the parameter server +*/ +void ImuSimulatorNode::declareAndRetrieveEnvironmentalSettings() { + // Geographic position and velocity + Eigen::Vector3d geoPositionLlh; + Eigen::Vector3d geoVelocityNed; + + // Declare environmental settings + this->declare_parameter( + "imu_simulator.environmental_settings.start_geo_position_llh", + std::vector{0.0, 0.0, 0.0}); + this->declare_parameter( + "imu_simulator.environmental_settings.start_geo_velocity_ned", + rclcpp::PARAMETER_DOUBLE_ARRAY); + + // Retrieve environmental settings + std::vector geoPosition = + this->get_parameter( + "imu_simulator.environmental_settings.start_geo_position_llh") + .as_double_array(); + std::vector geoVelocity = + this->get_parameter( + "imu_simulator.environmental_settings.start_geo_velocity_ned") + .as_double_array(); + + // Convert latitude and longitude to radians + geoPositionLlh << geoPosition[0] * M_PI / 180.0, + geoPosition[1] * M_PI / 180.0, geoPosition[2]; + + geoVelocityNed = doubleVectorToEigenVector3(geoVelocity); + + // Set IMU simulator parameters + pImuSimulator_->setImuGeoPositionLlh(geoPositionLlh); + pImuSimulator_->setImuGeoVelocity(geoVelocityNed); +} + +/** + * @brief IMU simulator loop callback function. + * + * This function is called periodically by the timer and publishes the IMU + * and diagnostic messages. The IMU simulator is updated with ground truth + * data and the simulated IMU data is published. +*/ +void ImuSimulatorNode::imuSimulatorLoopCallback() { + // Get current time + rclcpp::Time currentTimestamp = now(); + + // Create shared pointer for true acceleration message + std::shared_ptr trueAccelerationMsgPtr = + std::make_shared(); + + // Create shared pointer for true angular rate message + std::shared_ptr trueAngularRateMsgPtr = + std::make_shared(); + + // Crate shared pointer for diagnostic message + std::shared_ptr diagnosticMsgPtr = + std::make_shared(diagnosticMsg_); + + // Create shared for diagnostic array message + std::shared_ptr diagnosticArrayMsgPtr = + std::make_shared(); + + // Read out odometry message + Eigen::Vector3d v_nb_b_true; + Eigen::Vector3d w_ib_b_true; + Eigen::Quaterniond q_ib_true; + + // Check if ground truth odometry message is available + if (groundTruthOdomMsg_ == nullptr || groundTruthAccelMsg_ == nullptr) { + // Print STALE diagnostic message when no ground truth messages + diagnosticMsgPtr->level = diagnostic_msgs::msg::DiagnosticStatus::STALE; + diagnosticMsgPtr->name = "IMU Simulator"; + diagnosticMsgPtr->message = + "Waiting for first ground truth odometry and acceleration message!"; + + // Add diagnostic message to diagnostic array message + diagnosticArrayMsgPtr->status.push_back(*diagnosticMsgPtr); + diagnosticArrayMsgPtr->header.stamp = currentTimestamp; + + // Publish the diagnostic array message + pDiagnosticPublisher_->publish(*diagnosticArrayMsgPtr); + + // Reset odometry timeout timer since waiting for first message + pOdometryTimeOutTimer_->cancel(); + pOdometryTimeOutTimer_->reset(); + + if (!calc_acc_from_odom_vel_) { + // Reset acceleration timeout timer since waiting for first message + pAccelerationTimeOutTimer_->cancel(); + pAccelerationTimeOutTimer_->reset(); + } + + return; + + } else { + // Return if this is the first odometry message + if (!firstLinearVelocityReceived_) { + // Print STALE diagnostic message when acceleration cannot be calculated + diagnosticMsgPtr->level = diagnostic_msgs::msg::DiagnosticStatus::STALE; + diagnosticMsgPtr->name = "IMU Simulator"; + diagnosticMsgPtr->message = + "Waiting for second linear velocity message to generate " + "ground truth acceleration input for IMU simulator!"; + + // Add diagnostic message to diagnostic array message + diagnosticArrayMsgPtr->status.push_back(*diagnosticMsgPtr); + diagnosticArrayMsgPtr->header.stamp = currentTimestamp; + + // Publish the diagnostic array message + pDiagnosticPublisher_->publish(*diagnosticArrayMsgPtr); + + // Reset odometry timeout timer since waiting for first message + pOdometryTimeOutTimer_->cancel(); + pOdometryTimeOutTimer_->reset(); + + if (!calc_acc_from_odom_vel_) { + // Reset acceleration timeout timer since waiting for first message + pAccelerationTimeOutTimer_->cancel(); + pAccelerationTimeOutTimer_->reset(); + } + + // Assign current time to time stamp of last odometry message + timeLastOdom_ = currentTimestamp; + + // Set first linear velocity received flag + firstLinearVelocityReceived_ = true; + + return; + } + + // Assign ground truth odometry message to IMU simulator inputs + Eigen::Vector3d v_ib_b_true; + v_ib_b_true << groundTruthOdomMsg_.get()->twist.twist.linear.x, + groundTruthOdomMsg_.get()->twist.twist.linear.y, + groundTruthOdomMsg_.get()->twist.twist.linear.z; + + // Calculate time since last odometry message + rclcpp::Duration dt = rclcpp::Duration(now() - timeLastOdom_); + + // Assign current time to time stamp of last odometry message + timeLastOdom_ = currentTimestamp; + + Eigen::Vector3d a_ib_b_true; + if (calc_acc_from_odom_vel_) { + // Calculate linear acceleration from odometry linear velocity + a_ib_b_true = (v_ib_b_true - v_ib_b_prev_) / (dt.nanoseconds() * 1e-9); + } else { + // Extract linear acceleration from ground truth acceleration message + a_ib_b_true(0) = groundTruthAccelMsg_.get()->accel.linear.x; + a_ib_b_true(1) = groundTruthAccelMsg_.get()->accel.linear.y; + a_ib_b_true(2) = groundTruthAccelMsg_.get()->accel.linear.z; + } + + // Assign current velocity to previous velocity + v_ib_b_prev_ << v_nb_b_true(0), v_nb_b_true(1), v_nb_b_true(2); + + // Assign true angular rate + w_ib_b_true << groundTruthOdomMsg_.get()->twist.twist.angular.x, + groundTruthOdomMsg_.get()->twist.twist.angular.y, + groundTruthOdomMsg_.get()->twist.twist.angular.z; + + // Assign true acceleration to true acceleration message + geometry_msgs::msg::Vector3 trueAccelerationMsg; + + trueAccelerationMsg.x = a_ib_b_true(0); + trueAccelerationMsg.y = a_ib_b_true(1); + trueAccelerationMsg.z = a_ib_b_true(2); + + // Assign true angular rate to true angular rate message + geometry_msgs::msg::Vector3 trueAngularRateMsg; + + trueAngularRateMsg.x = w_ib_b_true(0); + trueAngularRateMsg.y = w_ib_b_true(1); + trueAngularRateMsg.z = w_ib_b_true(2); + + // Publish true acceleration message + pTrueAccelerationPublisher_->publish(trueAccelerationMsg); + + // Publish true angular rate message + pTrueAngularRatePublisher_->publish(trueAngularRateMsg); + + // Assign true orientation to quaternion + q_ib_true.w() = groundTruthOdomMsg_.get()->pose.pose.orientation.w; + q_ib_true.x() = groundTruthOdomMsg_.get()->pose.pose.orientation.x; + q_ib_true.y() = groundTruthOdomMsg_.get()->pose.pose.orientation.y; + q_ib_true.z() = groundTruthOdomMsg_.get()->pose.pose.orientation.z; + + // Generate acceleration measurement + Eigen::Vector3d f_ib_b_meas = + pImuSimulator_->generateAccelerationMeasurement(a_ib_b_true, q_ib_true); + + // Generate gyroscope measurement + Eigen::Vector3d w_ib_b_meas = + pImuSimulator_->generateGyroscopeMeasurement(w_ib_b_true, q_ib_true); + + // Fill and fill the IMU message + sensor_msgs::msg::Imu imuMsg; + + imuMsg.header.stamp = currentTimestamp; + imuMsg.header.frame_id = "imu_link"; + + // Fill quaternion and orientation covariance with zeros + imuMsg.orientation.w = 1.0; + imuMsg.orientation.x = 0.0; + imuMsg.orientation.y = 0.0; + imuMsg.orientation.z = 0.0; + + for (size_t i = 0; i < 9; i++) { + imuMsg.orientation_covariance[i] = 0.0; + } + + // Fill acceleration with simulated data + imuMsg.linear_acceleration.x = f_ib_b_meas(0); + imuMsg.linear_acceleration.y = f_ib_b_meas(1); + imuMsg.linear_acceleration.z = f_ib_b_meas(2); + + // Fill acceleration covariance with zeros + for (size_t i = 0; i < 9; i++) { + imuMsg.linear_acceleration_covariance[i] = 0.0; + } + + // Fill angular velocity with simulated data + imuMsg.angular_velocity.x = w_ib_b_meas(0); + imuMsg.angular_velocity.y = w_ib_b_meas(1); + imuMsg.angular_velocity.z = w_ib_b_meas(2); + + // Fill angular velocity covariance with zeros + for (size_t i = 0; i < 9; i++) { + imuMsg.angular_velocity_covariance[i] = 0.0; + } + + // Publish the IMU message + pImuPublisher_->publish(imuMsg); + + // Define and fill the IMU visualization message + geometry_msgs::msg::AccelStamped accelStampedMsg; + + accelStampedMsg.header.stamp = currentTimestamp; + accelStampedMsg.header.frame_id = "imu_link"; + + // Fill acceleration with simulated data + accelStampedMsg.accel.linear.x = f_ib_b_meas(0); + accelStampedMsg.accel.linear.y = f_ib_b_meas(1); + accelStampedMsg.accel.linear.z = f_ib_b_meas(2); + + accelStampedMsg.accel.angular.x = w_ib_b_meas(0); + accelStampedMsg.accel.angular.y = w_ib_b_meas(1); + accelStampedMsg.accel.angular.z = w_ib_b_meas(2); + + // Publish the visualization message + pImuAccelStampedPublisher_->publish(accelStampedMsg); + + // Fill the diagnostic message + diagnosticMsgPtr->level = diagnostic_msgs::msg::DiagnosticStatus::OK; + diagnosticMsgPtr->name = "IMU Simulator"; + diagnosticMsgPtr->message = "IMU simulator running nominal."; + } + + // Calculate time since last odometry message + rclcpp::Duration timeSinceLastOdom = + rclcpp::Duration(currentTimestamp - lastOdomTimestamp_); + + // Calculate time since last acceleration message + rclcpp::Duration timeSinceLastAccel = + rclcpp::Duration(currentTimestamp - lastAccelTimestamp_); + + // Check if odometry or acceleration message frequency is too slow + if (timeSinceLastOdom.seconds() > sampleTime_ && !odometry_timeout_ && + timeSinceLastAccel.seconds() > sampleTime_ && !acceleration_timeout_) { + diagnosticMsgPtr->level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + diagnosticMsgPtr->name = "IMU Simulator"; + diagnosticMsgPtr->message = + "Ground truth odometry or acceleration message frequency is too " + "slow! " + "IMU simulator ground truth frequency higher than " + "odometry/aceleration! Increase odometry/aceleration message " + "frequency!"; + } + + // Check if odometry or acceleration timeout flag is set + if (odometry_timeout_ || acceleration_timeout_) { + diagnosticMsgPtr->level = diagnostic_msgs::msg::DiagnosticStatus::STALE; + diagnosticMsgPtr->name = "IMU Simulator"; + diagnosticMsgPtr->message = + "No ground truth odometry or acceleration message received since " + "than " + "5 seconds! IMU simulator stalling!"; + + // Set first odometry received flag to false since too long time since last + firstLinearVelocityReceived_ = false; + } + + // Add diagnostic message to diagnostic array message + diagnosticArrayMsgPtr->status.push_back(*diagnosticMsgPtr); + diagnosticArrayMsgPtr->header.stamp = currentTimestamp; + + // Publish the diagnostic message + pDiagnosticPublisher_->publish(*diagnosticArrayMsgPtr); +} + +/** + * @brief Odometry callback function. + * + * This function is called when a new odometry message is received. The ground + * truth odometry message is assigned to the ground truth odometry message + * member variable of the node class. + * + * @param msg Pointer to the odometry message +*/ +void ImuSimulatorNode::odometryCallback( + const nav_msgs::msg::Odometry::SharedPtr msg) { + // Reset odometry timeout timer + pOdometryTimeOutTimer_->cancel(); + pOdometryTimeOutTimer_->reset(); + + // Set first odometry received flag + if (!first_odometry_received_) { + first_odometry_received_ = true; + + RCLCPP_INFO(get_logger(), "First ground truth odometry message received!"); + + // Check if first acceleration has been received + if (first_acceleration_received_) { + RCLCPP_INFO(get_logger(), "IMU simulator now running nominal!"); + } else { + RCLCPP_INFO(get_logger(), + "Waiting for first ground truth acceleration message..."); + } + } + + // Reset odometry timeout flag + if (odometry_timeout_) { + odometry_timeout_ = false; + + if (acceleration_timeout_ && !calc_acc_from_odom_vel_) { + RCLCPP_INFO(get_logger(), + "Ground truth odometry message received after timeout! Still " + "waiting for ground truth acceleration message..."); + } else { + RCLCPP_INFO(get_logger(), + "Ground truth odometry message received after timeout! IMU " + "simulator now running nominal!"); + } + } + + // Assign ground truth odometry message + groundTruthOdomMsg_ = msg; + + // Assign last odometry timestamp + lastOdomTimestamp_ = msg->header.stamp; +} + +/** + * @brief Acceleration callback function. + * + * This function is called when a new acceleration message is received. The ground + * truth acceleration message is assigned to the ground truth acceleration message + * member variable of the node class. + * + * @param msg Pointer to the acceleration message +*/ +void ImuSimulatorNode::accelerationCallback( + const geometry_msgs::msg::AccelStamped::SharedPtr msg) { + // Reset acceleration timeout timer + pAccelerationTimeOutTimer_->cancel(); + pAccelerationTimeOutTimer_->reset(); + + // Set first acceleration received flag + if (!first_acceleration_received_) { + first_acceleration_received_ = true; + RCLCPP_INFO(get_logger(), + "First ground truth acceleration message received!"); + + // Check if first odometry velocity has been received + if (first_odometry_received_) { + RCLCPP_INFO(get_logger(), "IMU simulator now running nominal!"); + } else { + RCLCPP_INFO(get_logger(), + "Waiting for first ground truth odometry message..."); + } + } + + // Reset acceleration timeout flag + if (acceleration_timeout_ == true) { + acceleration_timeout_ = false; + + if (odometry_timeout_) { + RCLCPP_INFO(get_logger(), + "Ground truth acceleration message received after timeout! " + "Still waiting for ground truth odometry message..."); + } else { + RCLCPP_INFO( + get_logger(), + "Ground truth acceleration message received after timeout! IMU " + "simulator now running nominal!"); + } + } + + // Assign ground truth acceleration message + groundTruthAccelMsg_ = msg; + + // Assign last acceleration timestamp + lastAccelTimestamp_ = msg->header.stamp; +} + +/** + * @brief Odometry timeout callback function. + * + * This function is called when no ground truth odometry message is received. + * +*/ +void ImuSimulatorNode::odometryTimeOutCallback() { + // Set odometry timeout flag + odometry_timeout_ = true; + + RCLCPP_WARN(get_logger(), + "No ground truth odometry message since more than 5 " + "seconds! IMU simulator now starting to stale!"); +} + +/** + * @brief Acceleration timeout callback function. + * + * This function is called when no ground truth acceleration message is received. + * +*/ +void ImuSimulatorNode::accelerationTimeOutCallback() { + // Set acceleration timeout flag + acceleration_timeout_ = true; + + RCLCPP_WARN(get_logger(), + "No ground truth acceleration message since more than 5 " + "seconds! IMU simulator now starting to stale!"); +} + +/** + * @brief Publish static tf2 transformations. +*/ +void ImuSimulatorNode::publishTf2Transforms() const { + // Fill tf2 transform message between base_link and imu_link + geometry_msgs::msg::TransformStamped tfMsg; + tfMsg.header.stamp = now(); + + tfMsg.header.frame_id = "base_link"; + tfMsg.child_frame_id = "imu_link"; + + tfMsg.transform.translation.x = 0.0; + tfMsg.transform.translation.y = 0.0; + tfMsg.transform.translation.z = 0.0; + + tfMsg.transform.rotation.w = 1.0; + tfMsg.transform.rotation.x = 0.0; + tfMsg.transform.rotation.y = 0.0; + tfMsg.transform.rotation.z = 0.0; + + pStaticTf2Broadcaster_->sendTransform(tfMsg); +} + +/** + * @brief Helper function to convert a vector of doubles to an Eigen vector + * + * @param vec Vector of doubles + * @return Eigen vector +*/ +Eigen::Vector3d ImuSimulatorNode::doubleVectorToEigenVector3( + const std::vector& vec) { + // Convert vector of doubles to Eigen vector + Eigen::Vector3d eigenVec; + + // Check vector size + if (vec.size() != 3) { + RCLCPP_ERROR(get_logger(), + "doubleVectorToEigenVector3: Vector size is not 3!"); + } else { + eigenVec << vec[0], vec[1], vec[2]; + } + + return eigenVec; +} + +} // namespace imu_simulator + +/** + * @brief Main function of the IMU simulator node. + * + * @param argc Number of command line arguments + * @param argv Command line arguments + * + * @return int Return value +*/ +int main(int argc, char** argv) { + // Create IMU simulator object + std::shared_ptr pImuSimulator = + std::make_shared(); + + rclcpp::init(argc, argv); + rclcpp::spin( + std::make_shared(pImuSimulator)); + rclcpp::shutdown(); + + return 0; +} diff --git a/src/imu_simulator_static_csv.cpp b/src/imu_simulator_static_csv.cpp new file mode 100644 index 0000000..1209017 --- /dev/null +++ b/src/imu_simulator_static_csv.cpp @@ -0,0 +1,212 @@ +/*@license BSD-3 https://opensource.org/licenses/BSD-3-Clause +Copyright (c) 2024, Institute of Automatic Control - RWTH Aachen University +Maximilian Nitsch (m.nitsch@irt.rwth-aachen.de) +All rights reserved. +*/ + +#include +#include +#include + +// NOLINTNEXTLINE(build/c++11) +#include +// NOLINTNEXTLINE(build/c++11) +#include + +#include "imu_simulator.h" + +// Write data to a CSV file +void writeToCsvFile(const std::vector& data) { + std::ofstream outputFile; + + outputFile.open("imu_static_data.csv", std::ios_base::app); + + // Check if the file is open + if (!outputFile.is_open()) { + std::cerr << "Error opening the file." << std::endl; + return; + } + + // add a comma at the end of each element except the last one + for (size_t i = 0; i < data.size(); i++) { + outputFile << data[i]; + + if (i < data.size() - 1) { + outputFile << ","; + } + } + + // add a new line after each row + outputFile << "\n"; + + // Close the file + outputFile.close(); +} + +// Convert an Eigen vector to a string +std::string eigenVectorToString(const Eigen::VectorXd& eigenVector) { + std::ostringstream oss; + + for (int i = 0; i < eigenVector.size(); i++) { + oss << eigenVector[i]; + + if (i < eigenVector.size() - 1) { + oss << ","; + } + } + + return oss.str(); +} + +// Convert StateVector to a vector of strings +std::vector imuMeasurementsToString( + const Eigen::VectorXd& eigenVector1, const Eigen::VectorXd& eigenVector2) { + std::vector eigenVectorSTring; + + // Convert each Eigen vector/quaternion to a string + eigenVectorSTring.push_back(eigenVectorToString(eigenVector1)); + eigenVectorSTring.push_back(eigenVectorToString(eigenVector2)); + + return eigenVectorSTring; +} + +// Main function +int main(int argc, char** argv) { + (void)argc; + (void)argv; + + const double gToMs2 = 9.80665; // (g) -> (m/s^2) + + // Accelerometer simulation parameters, identified with for STIM300 IMU + imu_simulator::ImuSimParams accSimParams; + + accSimParams.N = Eigen::Vector3d(0.00144813183998857, 0.00144218525538188, + 0.00145192879193856); + accSimParams.B = Eigen::Vector3d(0.000259469978374161, 0.000284359672626612, + 0.000266572030700551); + accSimParams.K = Eigen::Vector3d(9.72968195066955e-06, 1.50448806164571e-05, + 5.61621872457085e-06); + accSimParams.corrTime = + Eigen::Vector3d(44.2425330327468, 29.931356998205, 175.918924554171); + + accSimParams.intervalTurnOnBias = + Eigen::Vector3d(5.0e-3, 5.0e-3, 5.0e-3) * gToMs2; // (mg) -> (m/s^2) + accSimParams.measRange = + Eigen::Vector3d(5.0, 5.0, 5.0) * gToMs2; // (g) -> (m/s^2) + accSimParams.intervalMisAlignment = Eigen::Vector3d(0.0, 0.0, 0.0); + accSimParams.intervalScaleFactor = Eigen::Vector3d(0.0, 0.0, 0.0); + accSimParams.resolution = + Eigen::Vector3d(0.5960e-06, 0.5960e-06, 0.5960e-06) * + gToMs2; // (µg/LSB) -> (m/s^2) + + // Gyroscope simulation parameters, identified with for STIM300 IMU + imu_simulator::ImuSimParams gyroSimParams; + + gyroSimParams.N = Eigen::Vector3d(0.000207479860047933, 0.000243411420514079, + 0.000187943045943727); + gyroSimParams.B = Eigen::Vector3d(8.601448819572e-07, 9.49331921834923e-07, + 9.20618084887883e-07); + gyroSimParams.K = Eigen::Vector3d(1.85470342042531e-07, 1.1738725498127e-07, + 2.26095960190654e-07); + gyroSimParams.corrTime = Eigen::Vector3d(1315.9775377824, 5263.99037881102, + 124.14459722317); // (s) + gyroSimParams.intervalTurnOnBias = Eigen::Vector3d(100.0, 100.0, 100.0) * + M_PI / 180.0 * 1 / + 3600; // (deg/h) -> (rad/s) + gyroSimParams.measRange = + Eigen::Vector3d(400.0, 400.0, 400.0) * M_PI / 180.0; // (rad/s) + gyroSimParams.intervalMisAlignment = Eigen::Vector3d(0.0, 0.0, 0.0); + gyroSimParams.intervalScaleFactor = Eigen::Vector3d(0.0, 0.0, 0.0); + gyroSimParams.resolution = + Eigen::Vector3d(0.832e-06, 0.832e-06, 0.832e-06); // (rad/LSB) + + // IMU model enable settings + imu_simulator::ImuModelEnableSettings imuModelEnableSettings; + + imuModelEnableSettings.acc.enableLocalRefVec = true; + imuModelEnableSettings.acc.enableTurnOnBias = + false; // disable turn-on bias for validation + imuModelEnableSettings.acc.enableScaling = false; + imuModelEnableSettings.acc.enableMisAlignment = false; + imuModelEnableSettings.acc.enableStochasticError = true; + imuModelEnableSettings.acc.enableSaturation = false; + imuModelEnableSettings.acc.enableQuantization = false; + + imuModelEnableSettings.gyro.enableLocalRefVec = true; + imuModelEnableSettings.gyro.enableTurnOnBias = + false; // disable turn-on bias for validation + imuModelEnableSettings.gyro.enableScaling = false; + imuModelEnableSettings.gyro.enableMisAlignment = false; + imuModelEnableSettings.gyro.enableStochasticError = true; + imuModelEnableSettings.gyro.enableSaturation = false; + imuModelEnableSettings.gyro.enableQuantization = false; + + // Geographic position of Neumayer III station + double latitude = -70.667997328 * M_PI / 180.0; + double longitude = -8.266998932 * M_PI / 180.0; + double height = 40.0; + + Eigen::Vector3d geoPositionLlh = Eigen::Vector3d(latitude, longitude, height); + + // Neglect the velocity of the body frame (IMU) with respect to the ECEF frame + Eigen::Vector3d geoVelocity = Eigen::Vector3d(0.0, 0.0, 0.0); + + // IMU sample time + double sampleTime = 1.0 / 125.0; + + // Random number generator seed + unsigned int seed = 40; + + // Initialize the IMU simulator class + imu_simulator::ImuSimulator imuSimulator = imu_simulator::ImuSimulator( + accSimParams, gyroSimParams, imuModelEnableSettings, geoPositionLlh, + geoVelocity, sampleTime, seed); + + imuSimulator.setUseFixedRandomNumbersFlag(false); + + imuSimulator.resetImuSimulator(); + + std::stringstream ss = imuSimulator.printImuSimulatorParameters(); + std::cout << ss.str(); + + // Wait for 5 seconds + std::this_thread::sleep_for(std::chrono::seconds(5)); + + // Accelerometer measurement + Eigen::Vector3d a_ib_b_true = Eigen::Vector3d(0.0, 0.0, 0.0); + Eigen::Vector3d w_ib_b_true = Eigen::Vector3d(0.0, 0.0, 0.0); + + Eigen::Quaterniond q_b_n_true = Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0); + + // Duration of the simulation + double simTime = 8 * 60 * 60; // 8h + + std::cout << "Starting static IMU simulation..." << std::endl; + + // Wait for 2 second + std::this_thread::sleep_for(std::chrono::seconds(2)); + + for (int i = 0; i < simTime / sampleTime; i++) { + Eigen::Vector3d a_ib_b_meas = + imuSimulator.generateAccelerationMeasurement(a_ib_b_true, q_b_n_true); + + Eigen::Vector3d w_ib_b_meas = + imuSimulator.generateGyroscopeMeasurement(w_ib_b_true, q_b_n_true); + + std::cout << std::setw(10) << std::setprecision(10) + << a_ib_b_meas.transpose() << std::endl; + + // Create string vector for the IMU measurements + std::vector stateVectorString = + imuMeasurementsToString(a_ib_b_meas, w_ib_b_meas); + + // Write the IMU measurements to a csv file + writeToCsvFile(stateVectorString); + } + + std::cout << "Finished static IMU simulation!" << std::endl; + + imuSimulator.printImuSimulatorParameters(); + + return 0; +} diff --git a/test/imu_simulator_test.cpp b/test/imu_simulator_test.cpp new file mode 100644 index 0000000..f335f45 --- /dev/null +++ b/test/imu_simulator_test.cpp @@ -0,0 +1,919 @@ +/*@license BSD-3 https://opensource.org/licenses/BSD-3-Clause +Copyright (c) 2023, Institute of Automatic Control - RWTH Aachen University +Maximilian Nitsch (m.nitsch@irt.rwth-aachen.de) +All rights reserved. +*/ + +#include "imu_simulator.h" + +#include "gtest/gtest.h" + +/** + * @brief Test fixture for the IMU simulator class +*/ +class ImuSimulatorTest : public ::testing::Test { + protected: + void SetUp() override { + const double gToMs2 = 9.80665; // (g) -> (m/s^2) + + // Accelerometer simulation parameters, identified with for STIM300 IMU + imu_simulator::ImuSimParams accSimParams; + + accSimParams.N = Eigen::Vector3d(0.00144813183998857, 0.00144218525538188, + 0.00145192879193856); + accSimParams.B = Eigen::Vector3d(0.000259469978374161, 0.000284359672626612, + 0.000266572030700551); + accSimParams.K = Eigen::Vector3d(9.72968195066955e-06, 1.50448806164571e-05, + 5.61621872457085e-06); + accSimParams.corrTime = + Eigen::Vector3d(44.2425330327468, 29.931356998205, 175.918924554171); + + accSimParams.intervalTurnOnBias = + Eigen::Vector3d(5.0e-3, 5.0e-3, 5.0e-3) * gToMs2; // (mg) -> (m/s^2) + accSimParams.measRange = + Eigen::Vector3d(5.0, 5.0, 5.0) * gToMs2; // (g) -> (m/s^2) + accSimParams.intervalMisAlignment = + Eigen::Vector3d(0.2, 0.2, 0.2); // (rad) + accSimParams.intervalNonOrthogonality = + Eigen::Matrix(0.1, 0.1, 0.1, 0.1, 0.1, 0.1); // (rad) + accSimParams.intervalScaleFactor = Eigen::Vector3d(0.5, 0.5, 0.5); // (%) + accSimParams.resolution = + Eigen::Vector3d(0.5960e-06, 0.5960e-06, 0.5960e-06) * + gToMs2; // (µg/LSB) -> (m/s^2) + + // Gyroscope simulation parameters, identified with for STIM300 IMU + imu_simulator::ImuSimParams gyroSimParams; + + gyroSimParams.N = Eigen::Vector3d( + 0.000207479860047933, 0.000243411420514079, 0.000187943045943727); + gyroSimParams.B = Eigen::Vector3d(8.601448819572e-07, 9.49331921834923e-07, + 9.20618084887883e-07); + gyroSimParams.K = Eigen::Vector3d(1.85470342042531e-07, 1.1738725498127e-07, + 2.26095960190654e-07); + gyroSimParams.corrTime = Eigen::Vector3d(1315.9775377824, 5263.99037881102, + 124.14459722317); // (s) + gyroSimParams.intervalTurnOnBias = Eigen::Vector3d(100.0, 100.0, 100.0) * + M_PI / 180.0 * 1 / + 3600; // (deg/h) -> (rad/s) + gyroSimParams.measRange = + Eigen::Vector3d(400.0, 400.0, 400.0) * M_PI / 180.0; // (rad/s) + gyroSimParams.intervalMisAlignment = + Eigen::Vector3d(0.2, 0.2, 0.2); // (rad) + gyroSimParams.intervalNonOrthogonality = + Eigen::Matrix(0.1, 0.1, 0.1, 0.1, 0.1, 0.1); // (rad) + gyroSimParams.intervalScaleFactor = Eigen::Vector3d(0.5, 0.5, 0.5); // (%) + gyroSimParams.resolution = + Eigen::Vector3d(0.832e-06, 0.832e-06, 0.832e-06); // (rad/LSB) + + // IMU model enable settings + imu_simulator::ImuModelEnableSettings imuModelEnableSettings; + + imuModelEnableSettings.acc.enableLocalRefVec = true; + imuModelEnableSettings.acc.enableTurnOnBias = false; + imuModelEnableSettings.acc.enableScaling = false; + imuModelEnableSettings.acc.enableMisAlignment = false; + imuModelEnableSettings.acc.enableStochasticError = false; + imuModelEnableSettings.acc.enableSaturation = false; + imuModelEnableSettings.acc.enableQuantization = false; + + imuModelEnableSettings.gyro.enableLocalRefVec = true; + imuModelEnableSettings.gyro.enableTurnOnBias = false; + imuModelEnableSettings.gyro.enableScaling = false; + imuModelEnableSettings.gyro.enableMisAlignment = false; + imuModelEnableSettings.gyro.enableStochasticError = false; + imuModelEnableSettings.gyro.enableSaturation = false; + imuModelEnableSettings.gyro.enableQuantization = false; + + // Geographic position of Neumayer III station + double latitude = -70.667997328 * M_PI / 180.0; + double longitude = -8.266998932 * M_PI / 180.0; + double height = 40.0; + + Eigen::Vector3d geoPositionLlh = + Eigen::Vector3d(latitude, longitude, height); + + // Neglect velocity of body frame (IMU) with respect to ECEF frame + Eigen::Vector3d geoVelocity = Eigen::Vector3d(0.0, 0.0, 0.0); + + // IMU sample time + double sampleTime = 0.01; + + // Random number generator seed + unsigned int seed = 42; + + // Initialize the IMU simulator class + imuSimulator = imu_simulator::ImuSimulator( + accSimParams, gyroSimParams, imuModelEnableSettings, geoPositionLlh, + geoVelocity, sampleTime, seed); + } + + // Declare the class under test + imu_simulator::ImuSimulator imuSimulator; +}; + +/** + * @brief Test if the WELMEC model for the gravity vector outputs the same as MATLAB +*/ +TEST_F(ImuSimulatorTest, WelmecModelGravityTest) { + Eigen::Vector3d g_n_expected = Eigen::Vector3d(0.0, 0.0, 9.8263484510466); + + imu_simulator::EnvironmentalParameters environmentalParameters = + imuSimulator.getEnvironmentalParameters(); + + Eigen::Vector3d g_n = environmentalParameters.g_n; + + EXPECT_NEAR(g_n(0), g_n_expected(0), 1e-10); + EXPECT_NEAR(g_n(1), g_n_expected(1), 1e-10); + EXPECT_NEAR(g_n(2), g_n_expected(2), 1e-10); +} + +/** + * @brief Test if the gravity vector is correctly substracted from the acceleration measurement +*/ +TEST_F(ImuSimulatorTest, AccGravityVectorTest) { + Eigen::Vector3d f_ib_b = Eigen::Vector3d(0.0, 0.0, 0.0); + Eigen::Quaterniond q_b_n = Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0); + + imu_simulator::EnvironmentalParameters environmentalParameters = + imuSimulator.getEnvironmentalParameters(); + + Eigen::Vector3d f_ib_b_expected = + f_ib_b - q_b_n.toRotationMatrix() * environmentalParameters.g_n; + + Eigen::Vector3d f_ib_b_measured = + imuSimulator.generateAccelerationMeasurement(f_ib_b, q_b_n); + + EXPECT_NEAR(f_ib_b_measured(0), f_ib_b_expected(0), 1e-10); + EXPECT_NEAR(f_ib_b_measured(1), f_ib_b_expected(1), 1e-10); + EXPECT_NEAR(f_ib_b_measured(2), f_ib_b_expected(2), 1e-10); +} + +/** + * @brief Test if the Earth angular rate is correctly added to the gyroscope measurement +*/ +TEST_F(ImuSimulatorTest, GyroEarthAngularVelocityVectorTest) { + Eigen::Vector3d w_ib_b = Eigen::Vector3d(0.0, 0.0, 0.0); + Eigen::Quaterniond q_b_n = Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0); + + imu_simulator::EnvironmentalParameters environmentalParameters = + imuSimulator.getEnvironmentalParameters(); + + Eigen::Vector3d w_ib_b_expected = + w_ib_b + q_b_n.toRotationMatrix() * environmentalParameters.w_ie_n; + + Eigen::Vector3d w_ib_b_measured = + imuSimulator.generateGyroscopeMeasurement(w_ib_b, q_b_n); + + EXPECT_EQ(w_ib_b_measured(0), w_ib_b_expected(0)); + EXPECT_EQ(w_ib_b_measured(1), w_ib_b_expected(1)); + EXPECT_EQ(w_ib_b_measured(2), w_ib_b_expected(2)); +} + +/** + * @brief Test if the accelerometer turn-on bias is always in the configured interval +*/ +TEST_F(ImuSimulatorTest, AccTurnOnBiasInIntervalTest) { + // Enable the accelerometer turn-on bias model + imuSimulator.setAccEnableTurnOnBias(true); + + // Extract the configured interval for the accelerometer turn-on bias + imu_simulator::ImuSimParams accSimParams = + imuSimulator.getImuSimParams(imu_simulator::MeasurementType::acc); + Eigen::Vector3d intervalTurnOnBiasAcc = accSimParams.intervalTurnOnBias; + + // Loop 10000 times to check if turn-on bias is always in configured interval + for (int i = 0; i < 10000; i++) { + Eigen::Vector3d turnOnBias = + imuSimulator.getConstTurnOnBias(imu_simulator::MeasurementType::acc); + + EXPECT_LE(turnOnBias.x(), intervalTurnOnBiasAcc.x()); + EXPECT_LE(turnOnBias.y(), intervalTurnOnBiasAcc.y()); + EXPECT_LE(turnOnBias.z(), intervalTurnOnBiasAcc.z()); + + // Reset the IMU simulator to draw new random turn-on biases + imuSimulator.resetImuSimulator(); + } +} + +/** + * @brief Test if gyroscope turn-on bias is always in configured interval +*/ +TEST_F(ImuSimulatorTest, GyroTurnOnBiasInIntervalTest) { + // Enable the gyroscope turn-on bias model + imuSimulator.setGyroEnableTurnOnBias(true); + + // Extract the configured interval for the gyroscope turn-on bias + imu_simulator::ImuSimParams gyroSimParams = + imuSimulator.getImuSimParams(imu_simulator::MeasurementType::gyro); + Eigen::Vector3d intervalTurnOnBiasGyro = gyroSimParams.intervalTurnOnBias; + + // Loop 10000 times to check if turn-on bias is always in configured interval + for (int i = 0; i < 10000; i++) { + Eigen::Vector3d turnOnBias = + imuSimulator.getConstTurnOnBias(imu_simulator::MeasurementType::gyro); + + EXPECT_LE(turnOnBias.x(), intervalTurnOnBiasGyro.x()); + EXPECT_LE(turnOnBias.y(), intervalTurnOnBiasGyro.y()); + EXPECT_LE(turnOnBias.z(), intervalTurnOnBiasGyro.z()); + + // Reset the IMU simulator to draw new random turn-on biases + imuSimulator.resetImuSimulator(); + } +} + +/** + * @brief Test if the accelerometer constant turn-on bias remains constant +*/ +TEST_F(ImuSimulatorTest, AccTurnOnBiasRemainsConstantTest) { + // Enable the accelerometer turn-on bias model + imuSimulator.setAccEnableTurnOnBias(true); + + Eigen::Vector3d turnOnBiasExpected = + imuSimulator.getConstTurnOnBias(imu_simulator::MeasurementType::acc); + + // Loop 5 times to check if turn-on bias is always in configured interval + for (int i = 0; i < 5; i++) { + imuSimulator.generateAccelerationMeasurement( + Eigen::Vector3d(0.0, 0.0, 0.0), Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0)); + + Eigen::Vector3d turnOnBias = + imuSimulator.getConstTurnOnBias(imu_simulator::MeasurementType::acc); + + // Turn-on bias should remain constant + EXPECT_EQ(turnOnBias.x(), turnOnBiasExpected.x()); + EXPECT_EQ(turnOnBias.y(), turnOnBiasExpected.y()); + EXPECT_EQ(turnOnBias.z(), turnOnBiasExpected.z()); + } + + // Reset the IMU simulator to draw new random turn-on biases + imuSimulator.resetImuSimulator(); + + Eigen::Vector3d turnOnBias = + imuSimulator.getConstTurnOnBias(imu_simulator::MeasurementType::acc); + + // Now the turn-on bias should be different + EXPECT_NE(turnOnBias.x(), turnOnBiasExpected.x()); + EXPECT_NE(turnOnBias.y(), turnOnBiasExpected.y()); + EXPECT_NE(turnOnBias.z(), turnOnBiasExpected.z()); +} + +/** + * @brief Test if the gyroscope constant turn-on bias remains constant +*/ +TEST_F(ImuSimulatorTest, GyroTurnOnBiasRemainsConstantTest) { + // Enable the gyroscope turn-on bias model + imuSimulator.setGyroEnableTurnOnBias(true); + + Eigen::Vector3d turnOnBiasExpected = + imuSimulator.getConstTurnOnBias(imu_simulator::MeasurementType::gyro); + + // Loop 5 times to check if turn-on bias is always in configured interval + for (int i = 0; i < 5; i++) { + imuSimulator.generateGyroscopeMeasurement( + Eigen::Vector3d(0.0, 0.0, 0.0), Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0)); + + Eigen::Vector3d turnOnBias = + imuSimulator.getConstTurnOnBias(imu_simulator::MeasurementType::gyro); + + // Turn-on bias should remain constant + EXPECT_EQ(turnOnBias.x(), turnOnBiasExpected.x()); + EXPECT_EQ(turnOnBias.y(), turnOnBiasExpected.y()); + EXPECT_EQ(turnOnBias.z(), turnOnBiasExpected.z()); + } + + // Reset the IMU simulator to draw new random turn-on biases + imuSimulator.resetImuSimulator(); + + Eigen::Vector3d turnOnBias = + imuSimulator.getConstTurnOnBias(imu_simulator::MeasurementType::gyro); + + // Now the turn-on bias should be different + EXPECT_NE(turnOnBias.x(), turnOnBiasExpected.x()); + EXPECT_NE(turnOnBias.y(), turnOnBiasExpected.y()); + EXPECT_NE(turnOnBias.z(), turnOnBiasExpected.z()); +} + +/** + * @brief Test if the accelerometer measurements are correctly scaled and misaligned +*/ +TEST_F(ImuSimulatorTest, AccScalingModelTest) { + // Set the true test acceleration and orientation + Eigen::Vector3d f_ib_b = Eigen::Vector3d(1.0, 2.0, 3.0); + Eigen::Quaterniond q_b_n = Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0); + + // Complete the baseline measurement (without misalignment and co) + Eigen::Vector3d f_ib_b_baseline = + imuSimulator.generateAccelerationMeasurement(f_ib_b, q_b_n); + + // Enable scaling model + imuSimulator.setAccEnableScaling(true); + + // Reset to draw new random scaling values + imuSimulator.resetImuSimulator(); + + // Get the scale factor error, misalignment and orthogonality error + Eigen::Vector3d scaleFactor = + imuSimulator.getConstScaleFactor(imu_simulator::MeasurementType::acc); + + // Calculate the expected measurement for the present scaling values + Eigen::Vector3d f_ib_b_expected = + f_ib_b_baseline.array() * (1.0 + scaleFactor.array() / 100); + + // Get the accelerometer measurement and compare it with the expected value + Eigen::Vector3d f_ib_b_meas{ + imuSimulator.generateAccelerationMeasurement(f_ib_b, q_b_n)}; + + EXPECT_NEAR(f_ib_b_expected(0), f_ib_b_meas(0), 1e-10); + EXPECT_NEAR(f_ib_b_expected(1), f_ib_b_meas(1), 1e-10); + EXPECT_NEAR(f_ib_b_expected(2), f_ib_b_meas(2), 1e-10); +} + +/** + * @brief Test if the accelerometer measurements are correctly misaligned +*/ +TEST_F(ImuSimulatorTest, AccMisAlignmentModelTest) { + // Set the true test acceleration and orientation + Eigen::Vector3d f_ib_b = Eigen::Vector3d(1.0, 2.0, 3.0); + Eigen::Quaterniond q_b_n = Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0); + + // Complete the baseline measurement (without misalignment and co) + Eigen::Vector3d f_ib_b_baseline = + imuSimulator.generateAccelerationMeasurement(f_ib_b, q_b_n); + + // Enable misalignment model + imuSimulator.setAccEnableMisAlignment(true); + + // Reset to draw new random misalignment values + imuSimulator.resetImuSimulator(); + + // Get the misalignment and orthogonality error + Eigen::Vector3d misAlignment = + imuSimulator.getConstMisAlignment(imu_simulator::MeasurementType::acc); + + Eigen::Matrix nonOrthogonality = + imuSimulator.getConstNonOrthogonality( + imu_simulator::MeasurementType::acc); + + // Calculate the expected measurement for the present misalignment values + Eigen::Matrix3d misAlignmentMatrix{ + Eigen::AngleAxisd(misAlignment(0), Eigen::Vector3d::UnitX()) * + Eigen::AngleAxisd(misAlignment(1), Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(misAlignment(2), Eigen::Vector3d::UnitZ())}; + + Eigen::Matrix3d nonOrthogonalityMatrix{ + {1.0, -nonOrthogonality(0), nonOrthogonality(1)}, + {nonOrthogonality(2), 1.0, -nonOrthogonality(3)}, + {-nonOrthogonality(4), nonOrthogonality(5), 1.0}}; + + // Calculate the expected measurement for the present misalignment values + Eigen::Vector3d f_ib_b_expected = + misAlignmentMatrix * nonOrthogonalityMatrix * f_ib_b_baseline; + + // Get the accelerometer measurement and compare it with the expected value + Eigen::Vector3d f_ib_b_meas{ + imuSimulator.generateAccelerationMeasurement(f_ib_b, q_b_n)}; + + EXPECT_NEAR(f_ib_b_expected(0), f_ib_b_meas(0), 1e-10); + EXPECT_NEAR(f_ib_b_expected(1), f_ib_b_meas(1), 1e-10); + EXPECT_NEAR(f_ib_b_expected(2), f_ib_b_meas(2), 1e-10); +} + +/** + * @brief Test if the gyroscope measurements are correctly scaled +*/ +TEST_F(ImuSimulatorTest, GyroScalingModelTest) { + // Set the true test angular rate and orientation + Eigen::Vector3d w_ib_b = Eigen::Vector3d(1.0, 2.0, 3.0); + Eigen::Quaterniond q_b_n = Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0); + + // Complete the baseline measurement (without scaling) + Eigen::Vector3d w_ib_b_baseline = + imuSimulator.generateGyroscopeMeasurement(w_ib_b, q_b_n); + + // Enable scaling model + imuSimulator.setGyroEnableScaling(true); + + // Reset to draw new random scaling values + imuSimulator.resetImuSimulator(); + + // Get the scale factor error + Eigen::Vector3d scaleFactor = + imuSimulator.getConstScaleFactor(imu_simulator::MeasurementType::gyro); + + // Calculate the expected measurement for the present misalignment values + Eigen::Vector3d w_ib_b_expected = + w_ib_b_baseline.array() * (1.0 + scaleFactor.array() / 100); + + // Get the gyro measurement and compare it with the expected value + Eigen::Vector3d w_ib_b_meas = + imuSimulator.generateGyroscopeMeasurement(w_ib_b, q_b_n); + + EXPECT_NEAR(w_ib_b_expected(0), w_ib_b_meas(0), 1e-10); + EXPECT_NEAR(w_ib_b_expected(1), w_ib_b_meas(1), 1e-10); + EXPECT_NEAR(w_ib_b_expected(2), w_ib_b_meas(2), 1e-10); +} + +/** + * @brief Test if the gyroscope measurements are correctly scaled +*/ +TEST_F(ImuSimulatorTest, GyroMisAlignmentModelTest) { + // Set the true test angular rate and orientation + Eigen::Vector3d w_ib_b{Eigen::Vector3d(1.0, 2.0, 3.0)}; + Eigen::Quaterniond q_b_n{Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0)}; + + // Complete the baseline measurement (without misalignment and co) + Eigen::Vector3d w_ib_b_baseline{ + imuSimulator.generateGyroscopeMeasurement(w_ib_b, q_b_n)}; + + // Enable misalignment model + imuSimulator.setGyroEnableMisAlignment(true); + + // Reset to draw new random misalignment values + imuSimulator.resetImuSimulator(); + + // Get the misalignment and orthogonality error + Eigen::Vector3d misAlignment = + imuSimulator.getConstMisAlignment(imu_simulator::MeasurementType::gyro); + + Eigen::Matrix nonOrthogonality = + imuSimulator.getConstNonOrthogonality( + imu_simulator::MeasurementType::gyro); + + // Calculate the expected measurement for the present misalignment values + Eigen::Matrix3d misAlignmentMatrix{ + Eigen::AngleAxisd(misAlignment(0), Eigen::Vector3d::UnitX()) * + Eigen::AngleAxisd(misAlignment(1), Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(misAlignment(2), Eigen::Vector3d::UnitZ())}; + + Eigen::Matrix3d nonOrthogonalityMatrix{ + {1.0, -nonOrthogonality(0), nonOrthogonality(1)}, + {nonOrthogonality(2), 1.0, -nonOrthogonality(3)}, + {-nonOrthogonality(4), nonOrthogonality(5), 1.0}}; + + // Calculate the expected measurement for the present misalignment values + Eigen::Vector3d w_ib_b_expected = + misAlignmentMatrix * nonOrthogonalityMatrix * w_ib_b_baseline; + + // Get the gyro measurement and compare it with the expected value + Eigen::Vector3d w_ib_b_meas{ + imuSimulator.generateGyroscopeMeasurement(w_ib_b, q_b_n)}; + + EXPECT_NEAR(w_ib_b_expected(0), w_ib_b_meas(0), 1e-10); + EXPECT_NEAR(w_ib_b_expected(1), w_ib_b_meas(1), 1e-10); + EXPECT_NEAR(w_ib_b_expected(2), w_ib_b_meas(2), 1e-10); +} + +/** + * @brief Test if the accelerometer measurement is saturated to the configured range +*/ +TEST_F(ImuSimulatorTest, AccSaturationModelTest) { + // Enable the accelerometer turn-on bias model + imuSimulator.setAccEnableSaturation(true); + + // Extract the configured interval for the accelerometer turn-on bias + imu_simulator::ImuSimParams accSimParams = + imuSimulator.getImuSimParams(imu_simulator::MeasurementType::acc); + Eigen::Vector3d measRange = accSimParams.measRange; + + Eigen::Vector3d f_ib_b_meas = imuSimulator.generateAccelerationMeasurement( + Eigen::Vector3d(2.0 * measRange(0), 2.0 * measRange(1), + 2.0 * measRange(2)), + Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0)); + + // Accelerometer measurement should be saturated + EXPECT_LE(f_ib_b_meas(0), measRange(0)); + EXPECT_LE(f_ib_b_meas(1), measRange(1)); + EXPECT_LE(f_ib_b_meas(2), measRange(2)); + + // Disable the accelerometer turn-on bias model + imuSimulator.setAccEnableSaturation(false); + + f_ib_b_meas = imuSimulator.generateAccelerationMeasurement( + Eigen::Vector3d(2.0 * measRange(0), 2.0 * measRange(1), + 2.0 * measRange(2)), + Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0)); + + // Accelerometer measurement should not be saturated + EXPECT_GE(f_ib_b_meas(0), measRange(0)); + EXPECT_GE(f_ib_b_meas(1), measRange(1)); + EXPECT_GE(f_ib_b_meas(2), measRange(2)); +} + +/** + * @brief Test if the gyroscope measurement is saturated to the configured range +*/ +TEST_F(ImuSimulatorTest, GyroSaturationModelTest) { + // Enable the gyroscope turn-on bias model + imuSimulator.setGyroEnableSaturation(true); + + // Extract the configured interval for the gyroscope turn-on bias + imu_simulator::ImuSimParams gyroSimParams = + imuSimulator.getImuSimParams(imu_simulator::MeasurementType::gyro); + Eigen::Vector3d measRange = gyroSimParams.measRange; + + Eigen::Vector3d w_ib_b_meas = imuSimulator.generateGyroscopeMeasurement( + Eigen::Vector3d(2.0 * measRange(0), 2.0 * measRange(1), + 2.0 * measRange(2)), + Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0)); + + // Gyroscope measurement should be saturated + EXPECT_LE(w_ib_b_meas(0), measRange(0)); + EXPECT_LE(w_ib_b_meas(1), measRange(1)); + EXPECT_LE(w_ib_b_meas(2), measRange(2)); + + // Disable the gyroscope turn-on bias model + imuSimulator.setGyroEnableSaturation(false); + + w_ib_b_meas = imuSimulator.generateGyroscopeMeasurement( + Eigen::Vector3d(2.0 * measRange(0), 2.0 * measRange(1), + 2.0 * measRange(2)), + Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0)); + + // Gyroscope measurement should not be saturated + EXPECT_GE(w_ib_b_meas(0), measRange(0)); + EXPECT_GE(w_ib_b_meas(1), measRange(1)); + EXPECT_GE(w_ib_b_meas(2), measRange(2)); +} + +/** + * @brief Test if the accelerometer measurement is quantized to the configured resolution +*/ +TEST_F(ImuSimulatorTest, AccQuantizationModelTest) { + Eigen::Vector3d f_ib_b = Eigen::Vector3d(1.0, 2.0, 3.0); + Eigen::Quaterniond q_b_n = Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0); + + // Disable the gravity vector model + imuSimulator.setAccEnableGravity(false); + + // Enable the accelerometer quantization model + imuSimulator.setAccEnableQuantization(true); + + // Extract the IMU simulation parameters + imu_simulator::ImuSimParams accSimParams = + imuSimulator.getImuSimParams(imu_simulator::MeasurementType::acc); + imu_simulator::ImuSimParams gyroSimParams = + imuSimulator.getImuSimParams(imu_simulator::MeasurementType::gyro); + + // Set the resolution to a high value + accSimParams.resolution = Eigen::Vector3d(3, 0.5, 1.25); + + // Set new accelerometer simulation parameters + imuSimulator.setImuSimParameters(accSimParams, gyroSimParams); + + // Expected quantized measurement + Eigen::Vector3d f_ib_b_expected = Eigen::Vector3d(0.0, 2.0, 2.5); + + // Generate an accelerometer measurement + Eigen::Vector3d f_ib_b_meas = + imuSimulator.generateAccelerationMeasurement(f_ib_b, q_b_n); + + // Accelerometer measurement should be quantized + EXPECT_EQ(f_ib_b_meas(0), f_ib_b_expected(0)); + EXPECT_EQ(f_ib_b_meas(1), f_ib_b_expected(1)); + EXPECT_EQ(f_ib_b_meas(2), f_ib_b_expected(2)); +} + +/** + * @brief Test if the gyroscope measurement is quantized to the configured resolution +*/ +TEST_F(ImuSimulatorTest, GyroQuantizationModelTest) { + Eigen::Vector3d w_ib_b = Eigen::Vector3d(1.0, 2.0, 3.0); + Eigen::Quaterniond q_b_n = Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0); + + // Disable the Earth angular velocity model + imuSimulator.setGyroEnableEarthAngularVelocity(false); + + // Enable the gyroscope quantization model + imuSimulator.setGyroEnableQuantization(true); + + // Extract the IMU simulation parameters + imu_simulator::ImuSimParams accSimParams = + imuSimulator.getImuSimParams(imu_simulator::MeasurementType::acc); + imu_simulator::ImuSimParams gyroSimParams = + imuSimulator.getImuSimParams(imu_simulator::MeasurementType::gyro); + + // Set the resolution to a high value + gyroSimParams.resolution = Eigen::Vector3d(3, 0.5, 1.25); + + // Set new gyroscope simulation parameters + imuSimulator.setImuSimParameters(accSimParams, gyroSimParams); + + // Expected quantized measurement + Eigen::Vector3d w_ib_b_expected = Eigen::Vector3d(0.0, 2.0, 2.5); + + // Generate a gyroscope measurement + Eigen::Vector3d w_ib_b_meas = + imuSimulator.generateGyroscopeMeasurement(w_ib_b, q_b_n); + + // Gyroscope measurement should be quantized + EXPECT_EQ(w_ib_b_meas(0), w_ib_b_expected(0)); + EXPECT_EQ(w_ib_b_meas(1), w_ib_b_expected(1)); + EXPECT_EQ(w_ib_b_meas(2), w_ib_b_expected(2)); +} + +/** + * @brief Test if the accelerometer stochastic errors are zero when the noise parameters are zero +*/ +TEST_F(ImuSimulatorTest, AccStochasticErrorModelZeroNoiseTest) { + // Set accelerometer noise parameters to zero to disable the noise + imu_simulator::ImuSimParams accSimParams; + + accSimParams.N = Eigen::Vector3d(0.0, 0.0, 0.0); + accSimParams.B = Eigen::Vector3d(0.0, 0.0, 0.0); + accSimParams.K = Eigen::Vector3d(0.0, 0.0, 0.0); + accSimParams.corrTime = Eigen::Vector3d(100.0, 100.0, 100.0); + + // Set gyro noise parameters to zero to disable the noise + imu_simulator::ImuSimParams gyroSimParams; + + gyroSimParams.N = Eigen::Vector3d(0.0, 0.0, 0.0); + gyroSimParams.B = Eigen::Vector3d(0.0, 0.0, 0.0); + gyroSimParams.K = Eigen::Vector3d(0.0, 0.0, 0.0); + gyroSimParams.corrTime = Eigen::Vector3d(100.0, 100.0, 100.0); + + // Overwrite the IMU simulation parameters with zero noise parameters + imuSimulator.setImuSimParameters(accSimParams, gyroSimParams); + + // Disable the gravity vector model + imuSimulator.setAccEnableGravity(false); + + // Enable the accelerometer stochastic error model + imuSimulator.setAccEnableStochasticError(true); + + Eigen::Vector3d f_ib_b_meas; + imu_simulator::ImuStochasticErrors imuStochasticErrors; + + // Generate an accelerometer measurement + for (int i = 0; i < 1000; i++) { + f_ib_b_meas = imuSimulator.generateAccelerationMeasurement( + Eigen::Vector3d(0.0, 0.0, 0.0), Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0)); + + imuStochasticErrors = imuSimulator.getImuStochasticErrors( + imu_simulator::MeasurementType::acc); + } + + // Accelerometer measurement should be zero + EXPECT_EQ(f_ib_b_meas(0), 0.0); + EXPECT_EQ(f_ib_b_meas(1), 0.0); + EXPECT_EQ(f_ib_b_meas(2), 0.0); + + // Accelerometer stochastic errors should be zero + EXPECT_EQ(imuStochasticErrors.z_N(0), 0.0); + EXPECT_EQ(imuStochasticErrors.z_N(1), 0.0); + EXPECT_EQ(imuStochasticErrors.z_N(2), 0.0); + + EXPECT_EQ(imuStochasticErrors.z_B(0), 0.0); + EXPECT_EQ(imuStochasticErrors.z_B(1), 0.0); + EXPECT_EQ(imuStochasticErrors.z_B(2), 0.0); + + EXPECT_EQ(imuStochasticErrors.z_K(0), 0.0); + EXPECT_EQ(imuStochasticErrors.z_K(1), 0.0); + EXPECT_EQ(imuStochasticErrors.z_K(2), 0.0); +} + +/** + * @brief Test if the gyroscope stochastic errors are zero when the noise parameters are zero +*/ +TEST_F(ImuSimulatorTest, GyroStochasticErrorModelZeroNoiseTest) { + // Set accelerometer noise parameters to zero to disable the noise + imu_simulator::ImuSimParams accSimParams; + + accSimParams.N = Eigen::Vector3d(0.0, 0.0, 0.0); + accSimParams.B = Eigen::Vector3d(0.0, 0.0, 0.0); + accSimParams.K = Eigen::Vector3d(0.0, 0.0, 0.0); + accSimParams.corrTime = Eigen::Vector3d(100.0, 100.0, 100.0); + + // Set gyro noise parameters to zero to disable the noise + imu_simulator::ImuSimParams gyroSimParams; + + gyroSimParams.N = Eigen::Vector3d(0.0, 0.0, 0.0); + gyroSimParams.B = Eigen::Vector3d(0.0, 0.0, 0.0); + gyroSimParams.K = Eigen::Vector3d(0.0, 0.0, 0.0); + gyroSimParams.corrTime = Eigen::Vector3d(100.0, 100.0, 100.0); + + // Overwrite the IMU simulation parameters with zero noise parameters + imuSimulator.setImuSimParameters(accSimParams, gyroSimParams); + + // Disable the Earth angular velocity model + imuSimulator.setGyroEnableEarthAngularVelocity(false); + + // Enable the gyroscope stochastic error model + imuSimulator.setGyroEnableStochasticError(true); + + Eigen::Vector3d w_ib_b_meas; + imu_simulator::ImuStochasticErrors imuStochasticErrors; + + // Generate a gyroscope measurement + for (int i = 0; i < 1000; i++) { + w_ib_b_meas = imuSimulator.generateGyroscopeMeasurement( + Eigen::Vector3d(0.0, 0.0, 0.0), Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0)); + + imuStochasticErrors = imuSimulator.getImuStochasticErrors( + imu_simulator::MeasurementType::gyro); + } + + // Gyroscope measurement should be zero + EXPECT_EQ(w_ib_b_meas(0), 0.0); + EXPECT_EQ(w_ib_b_meas(1), 0.0); + EXPECT_EQ(w_ib_b_meas(2), 0.0); + + // Gyroscopes stochastic errors should be zero + EXPECT_EQ(imuStochasticErrors.z_N(0), 0.0); + EXPECT_EQ(imuStochasticErrors.z_N(1), 0.0); + EXPECT_EQ(imuStochasticErrors.z_N(2), 0.0); + + EXPECT_EQ(imuStochasticErrors.z_B(0), 0.0); + EXPECT_EQ(imuStochasticErrors.z_B(1), 0.0); + EXPECT_EQ(imuStochasticErrors.z_B(2), 0.0); + + EXPECT_EQ(imuStochasticErrors.z_K(0), 0.0); + EXPECT_EQ(imuStochasticErrors.z_K(1), 0.0); + EXPECT_EQ(imuStochasticErrors.z_K(2), 0.0); +} + +/** + * @brief Test if the accelerometer stochastic error match the manual calculations for fixed random numbers +*/ +TEST_F(ImuSimulatorTest, AccStochasticErrorModelTest) { + // Set accelerometer noise parameters to zero to disable the noise + imu_simulator::ImuSimParams accSimParams; + + accSimParams.N = Eigen::Vector3d(0.001, 0.001, 0.001); + accSimParams.B = Eigen::Vector3d(0.0001, 0.0001, 0.0001); + accSimParams.K = Eigen::Vector3d(1e-5, 1e-5, 1e-5); + accSimParams.corrTime = Eigen::Vector3d(100.0, 100.0, 100.0); + + // Set gyro noise parameters to zero to disable the noise + imu_simulator::ImuSimParams gyroSimParams; + + gyroSimParams.N = Eigen::Vector3d(0.0, 0.0, 0.0); + gyroSimParams.B = Eigen::Vector3d(0.0, 0.0, 0.0); + gyroSimParams.K = Eigen::Vector3d(0.0, 0.0, 0.0); + gyroSimParams.corrTime = Eigen::Vector3d(100.0, 100.0, 100.0); + + // Overwrite the IMU simulation parameters with zero noise parameters + imuSimulator.setImuSimParameters(accSimParams, gyroSimParams); + + // Disable the gravity vector model + imuSimulator.setAccEnableGravity(false); + + // Enable the accelerometer stochastic error model + imuSimulator.setAccEnableStochasticError(true); + + // Set the random numbers to fixed values for testing + imuSimulator.setUseFixedRandomNumbersFlag(true); + + Eigen::Vector3d f_ib_b_meas; + f_ib_b_meas = imuSimulator.generateAccelerationMeasurement( + Eigen::Vector3d(0.0, 0.0, 0.0), Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0)); + + imu_simulator::ImuStochasticErrors imuStochasticErrors; + imuStochasticErrors = + imuSimulator.getImuStochasticErrors(imu_simulator::MeasurementType::acc); + + Eigen::Vector3d z_N_expected = Eigen::Vector3d(0.01, 0.01, 0.01); + Eigen::Vector3d z_B_expected = Eigen::Vector3d( + 3.04352466221447e-06, 3.04352466221447e-06, 3.04352466221447e-06); + Eigen::Vector3d z_K_expected = Eigen::Vector3d(3e-6, 3e-6, 3e-6); + + // Check if the stochastic errors are as expected for fixed random numbers + EXPECT_NEAR(imuStochasticErrors.z_N(0), z_N_expected(0), 1e-10); + EXPECT_NEAR(imuStochasticErrors.z_N(1), z_N_expected(1), 1e-10); + EXPECT_NEAR(imuStochasticErrors.z_N(2), z_N_expected(2), 1e-10); + + EXPECT_NEAR(imuStochasticErrors.z_B(0), z_B_expected(0), 1e-10); + EXPECT_NEAR(imuStochasticErrors.z_B(1), z_B_expected(1), 1e-10); + EXPECT_NEAR(imuStochasticErrors.z_B(2), z_B_expected(2), 1e-10); + + EXPECT_NEAR(imuStochasticErrors.z_K(0), z_K_expected(0), 1e-10); + EXPECT_NEAR(imuStochasticErrors.z_K(1), z_K_expected(1), 1e-10); + EXPECT_NEAR(imuStochasticErrors.z_K(2), z_K_expected(2), 1e-10); + + // Generate second measurement + f_ib_b_meas = imuSimulator.generateAccelerationMeasurement( + Eigen::Vector3d(0.0, 0.0, 0.0), Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0)); + + imuStochasticErrors = + imuSimulator.getImuStochasticErrors(imu_simulator::MeasurementType::acc); + + // Check that stochastic errors changed after second measurement + EXPECT_FALSE(imuStochasticErrors.z_N.isApprox(z_N_expected, 1e-10)); + EXPECT_FALSE(imuStochasticErrors.z_B.isApprox(z_B_expected, 1e-10)); + EXPECT_FALSE(imuStochasticErrors.z_K.isApprox(z_K_expected, 1e-10)); + + // Reset the IMU simulator to draw new random numbers + imuSimulator.resetImuSimulator(); + + f_ib_b_meas = imuSimulator.generateAccelerationMeasurement( + Eigen::Vector3d(0.0, 0.0, 0.0), Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0)); + + imuStochasticErrors = + imuSimulator.getImuStochasticErrors(imu_simulator::MeasurementType::acc); + + // Check if stochastic errors are as expected for fixed random numbers + EXPECT_NEAR(imuStochasticErrors.z_N(0), z_N_expected(0), 1e-10); + EXPECT_NEAR(imuStochasticErrors.z_N(1), z_N_expected(1), 1e-10); + EXPECT_NEAR(imuStochasticErrors.z_N(2), z_N_expected(2), 1e-10); + + EXPECT_NEAR(imuStochasticErrors.z_B(0), z_B_expected(0), 1e-10); + EXPECT_NEAR(imuStochasticErrors.z_B(1), z_B_expected(1), 1e-10); + EXPECT_NEAR(imuStochasticErrors.z_B(2), z_B_expected(2), 1e-10); + + EXPECT_NEAR(imuStochasticErrors.z_K(0), z_K_expected(0), 1e-10); + EXPECT_NEAR(imuStochasticErrors.z_K(1), z_K_expected(1), 1e-10); + EXPECT_NEAR(imuStochasticErrors.z_K(2), z_K_expected(2), 1e-10); +} + +/** + * @brief Test if the gyroscope stochastic error match the manual calculations for fixed random numbers + * +*/ +TEST_F(ImuSimulatorTest, GyroStochasticErrorModelTest) { + // Set accelerometer noise parameters to zero to disable the noise + imu_simulator::ImuSimParams accSimParams; + + accSimParams.N = Eigen::Vector3d(0.0, 0.0, 0.0); + accSimParams.B = Eigen::Vector3d(0.0, 0.0, 0.0); + accSimParams.K = Eigen::Vector3d(0.0, 0.0, 0.0); + accSimParams.corrTime = Eigen::Vector3d(100.0, 100.0, 100.0); + + // Set gyro noise parameters to zero to disable the noise + imu_simulator::ImuSimParams gyroSimParams; + + gyroSimParams.N = Eigen::Vector3d(0.001, 0.001, 0.001); + gyroSimParams.B = Eigen::Vector3d(0.0001, 0.0001, 0.0001); + gyroSimParams.K = Eigen::Vector3d(1e-5, 1e-5, 1e-5); + gyroSimParams.corrTime = Eigen::Vector3d(100.0, 100.0, 100.0); + + // Overwrite the IMU simulation parameters with zero noise parameters + imuSimulator.setImuSimParameters(accSimParams, gyroSimParams); + + // Disable the Earth angular velocity model + imuSimulator.setGyroEnableEarthAngularVelocity(false); + + // Enable the gyroscope stochastic error model + imuSimulator.setGyroEnableStochasticError(true); + + // Set the random numbers to fixed values for testing + imuSimulator.setUseFixedRandomNumbersFlag(true); + + Eigen::Vector3d w_ib_b_meas; + w_ib_b_meas = imuSimulator.generateGyroscopeMeasurement( + Eigen::Vector3d(0.0, 0.0, 0.0), Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0)); + + imu_simulator::ImuStochasticErrors imuStochasticErrors; + imuStochasticErrors = + imuSimulator.getImuStochasticErrors(imu_simulator::MeasurementType::gyro); + + Eigen::Vector3d z_N_expected = Eigen::Vector3d(0.01, 0.01, 0.01); + Eigen::Vector3d z_B_expected = Eigen::Vector3d( + 3.04352466221447e-06, 3.04352466221447e-06, 3.04352466221447e-06); + Eigen::Vector3d z_K_expected = Eigen::Vector3d(3e-6, 3e-6, 3e-6); + + // Check if the stochastic errors are as expected for fixed random numbers + EXPECT_NEAR(imuStochasticErrors.z_N(0), z_N_expected(0), 1e-10); + EXPECT_NEAR(imuStochasticErrors.z_N(1), z_N_expected(1), 1e-10); + EXPECT_NEAR(imuStochasticErrors.z_N(2), z_N_expected(2), 1e-10); + + EXPECT_NEAR(imuStochasticErrors.z_B(0), z_B_expected(0), 1e-10); + EXPECT_NEAR(imuStochasticErrors.z_B(1), z_B_expected(1), 1e-10); + EXPECT_NEAR(imuStochasticErrors.z_B(2), z_B_expected(2), 1e-10); + + EXPECT_NEAR(imuStochasticErrors.z_K(0), z_K_expected(0), 1e-10); + EXPECT_NEAR(imuStochasticErrors.z_K(1), z_K_expected(1), 1e-10); + EXPECT_NEAR(imuStochasticErrors.z_K(2), z_K_expected(2), 1e-10); + + // Generate second measurement + w_ib_b_meas = imuSimulator.generateGyroscopeMeasurement( + Eigen::Vector3d(0.0, 0.0, 0.0), Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0)); + + imuStochasticErrors = + imuSimulator.getImuStochasticErrors(imu_simulator::MeasurementType::gyro); + + // Check that stochastic errors changed after second measurement + EXPECT_FALSE(imuStochasticErrors.z_N.isApprox(z_N_expected, 1e-10)); + EXPECT_FALSE(imuStochasticErrors.z_B.isApprox(z_B_expected, 1e-10)); + EXPECT_FALSE(imuStochasticErrors.z_K.isApprox(z_K_expected, 1e-10)); + + // Reset the IMU simulator to draw new random numbers + imuSimulator.resetImuSimulator(); + + w_ib_b_meas = imuSimulator.generateGyroscopeMeasurement( + Eigen::Vector3d(0.0, 0.0, 0.0), Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0)); + + imuStochasticErrors = + imuSimulator.getImuStochasticErrors(imu_simulator::MeasurementType::gyro); + + // Check if stochastic errors are as expected for fixed random numbers + EXPECT_NEAR(imuStochasticErrors.z_N(0), z_N_expected(0), 1e-10); + EXPECT_NEAR(imuStochasticErrors.z_N(1), z_N_expected(1), 1e-10); + EXPECT_NEAR(imuStochasticErrors.z_N(2), z_N_expected(2), 1e-10); + + EXPECT_NEAR(imuStochasticErrors.z_B(0), z_B_expected(0), 1e-10); + EXPECT_NEAR(imuStochasticErrors.z_B(1), z_B_expected(1), 1e-10); + EXPECT_NEAR(imuStochasticErrors.z_B(2), z_B_expected(2), 1e-10); + + EXPECT_NEAR(imuStochasticErrors.z_K(0), z_K_expected(0), 1e-10); + EXPECT_NEAR(imuStochasticErrors.z_K(1), z_K_expected(1), 1e-10); + EXPECT_NEAR(imuStochasticErrors.z_K(2), z_K_expected(2), 1e-10); +} diff --git a/test/main.cpp b/test/main.cpp new file mode 100644 index 0000000..776893a --- /dev/null +++ b/test/main.cpp @@ -0,0 +1,6 @@ +#include "gtest/gtest.h" + +int main(int argc, char ** argv) { + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}