From 74e0d5d21e8ed0eaa8e6e2e96d6b802a85f33560 Mon Sep 17 00:00:00 2001 From: TheNoobInventor Date: Mon, 19 Feb 2024 21:44:38 +0100 Subject: [PATCH] Add mpu6050 covariance program to generate IMU covariances --- README.md | 8 ++ lidarbot_bringup/CMakeLists.txt | 8 +- lidarbot_bringup/config/controllers.yaml | 8 +- lidarbot_bringup/src/mpu6050_covariances.cpp | 110 +++++++++++++++++++ 4 files changed, 126 insertions(+), 8 deletions(-) create mode 100644 lidarbot_bringup/src/mpu6050_covariances.cpp diff --git a/README.md b/README.md index d915fc1..1628545 100644 --- a/README.md +++ b/README.md @@ -31,6 +31,7 @@ Hardware components are written for the Waveshare Motor Driver HAT and MPU6050 s - [Motor Driver HAT](#motor-driver-hat) - [Raspberry Pi Camera](#raspberry-pi-camera) - [MPU6050 offsets](#mpu6050-offsets) + - [MPU6050 covariances](#mpu6050-covariances) - [📡 Network Configuration](#network-configuration) - [Differential Drive Controller](#differential-drive-controller) - [IMU Sensor Broadcaster](#imu-sensor-broadcaster) @@ -552,6 +553,13 @@ Prior to using the [Imu sensor broadcaster](https://index.ros.org/p/imu_sensor_b The MPU6050 module is set to its most sensitive gyroscope and accelerometer ranges, which can be confirmed (or changed) at the top of the `mpu6050_lib.h` file. +#### MPU6050 covariances + +TODO: +Quick difference between variance and covariances + +IMU covariances required in `controllers.yaml` + ## 📡 Network Configuration TODO: update with Mini Router setup diff --git a/lidarbot_bringup/CMakeLists.txt b/lidarbot_bringup/CMakeLists.txt index 45c82c0..7c0eab5 100644 --- a/lidarbot_bringup/CMakeLists.txt +++ b/lidarbot_bringup/CMakeLists.txt @@ -52,15 +52,19 @@ ament_export_libraries(mpu6050_hardware_interface) include_directories(include) -# Create Cpp executable +# Create Cpp executables add_executable(mpu6050_offsets src/mpu6050_lib.cpp src/mpu6050_offsets.cpp) +add_executable(mpu6050_covariances src/mpu6050_lib.cpp src/mpu6050_covariances.cpp) # Install Cpp executables install(TARGETS + mpu6050_offsets + mpu6050_covariances DESTINATION lib/${PROJECT_NAME} ) -# Link i2c to mpu6050_offsets +# Link i2c to mpu6050_offsets and mpu6050_covariances targets target_link_libraries(mpu6050_offsets i2c) +target_link_libraries(mpu6050_covariances i2c) ament_package() diff --git a/lidarbot_bringup/config/controllers.yaml b/lidarbot_bringup/config/controllers.yaml index c90569b..75ae35c 100644 --- a/lidarbot_bringup/config/controllers.yaml +++ b/lidarbot_bringup/config/controllers.yaml @@ -40,12 +40,8 @@ imu_broadcaster: sensor_name: mpu6050 frame_id: imu_link - - # static_covariance_orientation: [0.005, 0.0, 0.0, 0.0, 0.005, 0.0, 0.0, 0.0, 0.005] - # static_covariance_angular_velocity: [5.76e-04, 0.0, 0.0, 0.0, 5.76e-04, 0.0, 0.0, 0.0, 5.76e-04] - # static_covariance_linear_acceleration: [9.8067e-03, 0.0, 0.0, 0.0, 9.8067e-03, 0.0, 0.0, 0.0, 9.8067e-03] - - # 500 data points + + # 500 data points used to calculated covariances static_covariance_orientation: [2.63882e-06, 0.0, 0.0, 0.0, 7.50018e-06, 0.0, 0.0, 0.0, 2.89257e-09] static_covariance_angular_velocity: [2.71413e-07, 0.0, 0.0, 0.0, 6.79488e-07, 0.0, 0.0, 0.0, 4.37879e-07] static_covariance_linear_acceleration: [0.00133755, 0.0, 0.0, 0.0, 0.000209753, 0.0, 0.0, 0.0, 0.00143276] \ No newline at end of file diff --git a/lidarbot_bringup/src/mpu6050_covariances.cpp b/lidarbot_bringup/src/mpu6050_covariances.cpp new file mode 100644 index 0000000..2eab7dc --- /dev/null +++ b/lidarbot_bringup/src/mpu6050_covariances.cpp @@ -0,0 +1,110 @@ +// MPU6050 covariance program +// Currently only variances are considered for the imu covariance arrays + +#include "lidarbot_bringup/mpu6050_lib.h" + +MPU6050 device(0x68); + +// Function to find mean +float mean(float array[], int n) { + float sum = 0.0f; + + for (int i = 0; i < n; i++) + sum += array[i]; + return sum / n; +} + +// Function to find variance +float variance(float array[], int n) { + float sum = 0.0f; + + float array_mean = mean(array, n); + + for (int i = 0; i < n; i++) + sum += (array[i] - array_mean) * (array[i] - array_mean); + return sum / (n - 1); +} + +int main() { + + // Initialize variables and arrays + float roll, pitch, yaw; // Angle values + float gx, gy, gz; // Gyro values + float ax, ay, az; // Accel values + int sample_size = 500; // Sample data size to calculate variance on + float orient_var[3]; // Orientation variances + float ang_vel_var[3]; // Angular velocity variances + float lin_accel_var[3]; // Linear acceleration variances + float roll_array[sample_size]; // Array of roll values + float pitch_array[sample_size]; // Array of pitch values + float yaw_array[sample_size]; // Array of yaw values + float ang_vel_x_array[sample_size]; // Array of angular velocity values in x-axis + float ang_vel_y_array[sample_size]; // Array of angular velocity values in y-axis + float ang_vel_z_array[sample_size]; // Array of angular velocity values in z-axis + float lin_accel_x_array[sample_size]; // Array of linear acceleration values in x-axis + float lin_accel_y_array[sample_size]; // Array of linear acceleration values in y-axis + float lin_accel_z_array[sample_size]; // Array of linear acceleration values in z-axis + + sleep(1); // Wait for the MPU6050 to stabilize + + device.calc_yaw = true; + + std::cout << "\nPlease keep the MPU6050 module level and still. \n"; + std::cout << "Reading and appending " << sample_size << " sensor data points to respective arrays" << ", it may take a while ... \n\n"; + + // Read and append sensor data to arrays + int i; // loop variable + + for (i = 0; i < sample_size; i++) { + // Read roll, pitch and yaw angles + device.getAngle(0, &roll); + device.getAngle(1, &pitch); + device.getAngle(2, &yaw); + + roll_array[i] = roll; + pitch_array[i] = pitch; + yaw_array[i] = yaw; + + // Read gyro values + device.getGyro(&gx, &gy, &gz); + + ang_vel_x_array[i] = gx; + ang_vel_y_array[i] = gy; + ang_vel_z_array[i] = gz; + + // Read accel values + device.getAccel(&ax, &ay, &az); + + lin_accel_x_array[i] = ax; + lin_accel_y_array[i] = ay; + lin_accel_z_array[i] = az; + + usleep(250000); // 0.25sec delay to read non-consecutive values + } + + std::cout << "Calculating variances ...\n\n"; + sleep(1); + + // Calculate variances + orient_var[0] = variance(roll_array, sample_size); + orient_var[1] = variance(pitch_array, sample_size); + orient_var[2] = variance(yaw_array, sample_size); + ang_vel_var[0] = variance(ang_vel_x_array, sample_size); + ang_vel_var[1] = variance(ang_vel_y_array, sample_size); + ang_vel_var[2] = variance(ang_vel_z_array, sample_size); + lin_accel_var[0] = variance(lin_accel_x_array, sample_size); + lin_accel_var[1] = variance(lin_accel_y_array, sample_size); + lin_accel_var[2] = variance(lin_accel_z_array, sample_size); + + // Output variance + std::cout << "static_covariance_orientation: [" << orient_var[0] << ", 0.0, 0.0, " + << orient_var[1] << ", 0.0, 0.0, " << orient_var[2] << ", 0.0, 0.0]\n"; + std::cout << "static_covariance_angular_velocity: [" << ang_vel_var[0] << ", 0.0, 0.0, " + << ang_vel_var[1] << ", 0.0, 0.0, " << ang_vel_var[2] << ", 0.0, 0.0]\n"; + std::cout << "static_covariance_linear_acceleration: [" << lin_accel_var[0] << ", 0.0, 0.0, " + << lin_accel_var[1] << ", 0.0, 0.0, " << lin_accel_var[2] << ", 0.0, 0.0]\n\n"; + + std::cout << "Paste covariance arrays in the imu_broadcaster ros__parameters section in lidarbot_bringup/config/controllers.yaml.\n"; + + return 0; +} \ No newline at end of file