From f033f24dcbdff26e90c03121bfa8bb73d2ca8ddb Mon Sep 17 00:00:00 2001 From: JimOAOOO Date: Tue, 1 Apr 2025 22:18:58 -0600 Subject: [PATCH 1/5] Hope this work --- src/filters/IMU_filter.cpp | 242 ++++++++++++++++++++++++++++++++++ src/filters/IMU_filter.hpp | 40 ++++++ src/sensors/ICM20649.cpp | 24 ++-- src/sensors/ICM20649.hpp | 23 ---- src/sensors/IMUSensor.cpp | 45 ++++++- src/sensors/IMUSensor.hpp | 76 +++++++---- src/sensors/LSM6DSOX.cpp | 16 +-- src/sensors/SensorManager.cpp | 29 +--- src/sensors/SensorManager.hpp | 3 - 9 files changed, 393 insertions(+), 105 deletions(-) create mode 100644 src/filters/IMU_filter.cpp create mode 100644 src/filters/IMU_filter.hpp diff --git a/src/filters/IMU_filter.cpp b/src/filters/IMU_filter.cpp new file mode 100644 index 00000000..0df529b7 --- /dev/null +++ b/src/filters/IMU_filter.cpp @@ -0,0 +1,242 @@ +#include "IMU_filter.hpp" + +void IMU_filter::init_EKF_6axis(IMU_data data){ + // Set a initial value for the quaternion + float recipNorm = 1.0f/sqrt(data.accel_X * data.accel_X + data.accel_Y * data.accel_Y + data.accel_Z * data.accel_Z); + data.accel_X = data.accel_X * recipNorm; + data.accel_Y = data.accel_Y * recipNorm; + data.accel_Z = data.accel_Z * recipNorm;//This is the unit gravity + float axis_norm = sqrt(data.accel_Y * data.accel_Y + data.accel_X * data.accel_X); + float sin_halfangle = sinf(acosf(data.accel_Z)/2.0f); + if(axis_norm > 1e-6){ + x[0] = cosf(data.accel_Z/2.0f); + x[1] = data.accel_Y * sin_halfangle / axis_norm; + x[2] = -data.accel_X * sin_halfangle / axis_norm; + x[3] = 1 - x[0] * x[0] - x[1] * x[1] - x[2] * x[2]; + }else{ + x[0] = 1; + x[1] = 0; + x[2] = 0; + x[3] = 0; + } + // After test. within 0.5 second pitch and roll will get to the right value when IMU facing up; + + //Since the weight of Q and R for all element should be the same + Q = 1; + R = 1000; + P[0] = {1000,0,0,0}; + P[1] = {0,1000,0,0}; + P[2] = {0,0,1000,0}; + P[3] = {0,0,0,1000}; +} + +int IMU_filter::step_EKF_6axis(IMU_data data){ + // For unit Quaternions, The seperation of garvity is what q_i, q_j, q_k should be + dt = timer.delta(); + float gravity_now = sqrt(data.accel_X * data.accel_X + data.accel_Y * data.accel_Y + data.accel_Z * data.accel_Z); + float recipNorm = 1.0f/gravity_now; + float unit_accel_X = data.accel_X * recipNorm; + float unit_accel_Y = data.accel_Y * recipNorm; + float unit_accel_Z = data.accel_Z * recipNorm;//This is the unit gravity + + //Helping numbers for F + float helpgx = (data.gyro_X * dt) * 0.5f; + float helpgy = (data.gyro_Y * dt) * 0.5f; + float helpgz = (data.gyro_Z * dt) * 0.5f; + // Predict for x + float x0 = x[0]; + float x1 = x[1]; + float x2 = x[2]; + float x3 = x[3]; + + x[0] = x0 - x1*helpgx - x2*helpgy - x3*helpgz ; + x[1] = x0*helpgx + x1 + x2*helpgz - x3*helpgy ; + x[2] = x0*helpgy - x1*helpgz + x2 + x3*helpgx ; + x[3] = x0*helpgz + x1*helpgy - x2*helpgx + x3 ; + // unit x + recipNorm = 1.0f/sqrt(x[0] * x[0] + x[1] * x[1] + x[2] * x[2] + x[3] * x[3]); + x[0] *= recipNorm; + x[1] *= recipNorm; + x[2] *= recipNorm; + x[3] *= recipNorm; + + // predict for P + float F[4][4] = { + {1, -helpgx, -helpgy, -helpgz}, + {helpgx, 1, helpgz, -helpgy}, + {helpgy, -helpgz, 1, helpgx}, + {helpgz, helpgy, -helpgx, 1} + }; + // [ 1, -(dt*gx)/2, -(dt*gy)/2, -(dt*gz)/2] + // [(dt*gx)/2, 1, (dt*gz)/2, -(dt*gy)/2] + // [(dt*gy)/2, -(dt*gz)/2, 1, (dt*gx)/2] + // [(dt*gz)/2, (dt*gy)/2, -(dt*gx)/2, 1] Jacobian function F state transition model + + float FxP[4][4] = {0}; + + // Calculate FxPxF_transpose + for(int i = 0; i < 4; i++){ + for(int j = 0; j < 4; j++){ + for(int k = 0; k < 4; k++){ + FxP[i][j] += F[i][k] * P[k][j]; + } + } + } + for(int i = 0; i < 4; i++){ + for(int j = 0; j < 4; j++){ + P[i][j] = 0; + for(int k = 0; k < 4; k++){ + P[i][j] += FxP[i][k] * F[j][k]; // P = F*P*F^T + Q + if (i == j){ + P[i][j] += Q * dt; + } + } + } + } + // // Updata + float H[3][4] = { + {-2*x[2], 2*x[3], -2*x[0], 2*x[1]}, + {2*x[1], 2*x[0], 2*x[3], 2*x[2]}, + {2*x[0], -2*x[1], -2*x[2], 2*x[3]} + }; + // [-2*q2, 2*q3, -2*q0, 2*q1] + // [ 2*q1, 2*q0, 2*q3, 2*q2] + // [ 2*q0, -2*q1, -2*q2, 2*q3] Jacobian function H observation model + + float PH_transpose[4][3] = {0}; + float HPH_transpose[3][3] = {0}; + // Calculate HPH_transpose + for(int i = 0; i < 4; i++){ + for(int j = 0; j < 3; j++){ + for(int k = 0; k < 4; k++){ + PH_transpose[i][j] += P[i][k] * H[j][k]; + } + } + } + for(int i = 0; i < 3; i++){ + for(int j = 0; j < 3; j++){ + for(int k = 0; k < 4; k++){ + HPH_transpose[i][j] += H[i][k] * PH_transpose[k][j]; + if (i == j){ + HPH_transpose[i][j] += R; + } + } + } + } + float HPH_transpose_inv[3][3] = {0}; + inverse3x3(HPH_transpose, HPH_transpose_inv); + float K[4][3] = {0}; + for(int i = 0; i < 4; i++){ + for(int j = 0; j < 3; j++){ + for(int k = 0; k < 3; k++){ + K[i][j] += PH_transpose[i][k] * HPH_transpose_inv[k][j]; + } + } + } + // caulculate the error between the predicted and the measured + float g[3] = { + unit_accel_X - 2*(x[1]*x[3] - x[0]*x[2]), + unit_accel_Y - 2*(x[2]*x[3] + x[0]*x[1]), + unit_accel_Z - (1 - 2*(x[1]*x[1] + x[2]*x[2])) + }; + + if (gravity_now > 9.7 && gravity_now < 9.9){ + // Updata x + for(int i = 1; i < 3; i++){ + x[i] += K[i][0] * g[0] + K[i][1] * g[1] + K[i][2] * g[2]; + } + recipNorm = 1.0f/sqrt(x[0] * x[0] + x[1] * x[1] + x[2] * x[2] + x[3] * x[3]); + x[0] *= recipNorm; + x[1] *= recipNorm; + x[2] *= recipNorm; + x[3] *= recipNorm; + // Update P + float KH[4][4] = {0}; + float I[4][4] = {0}; + for(int i = 0; i < 4; i++){ + I[i][i] = 1.0f; + } + + for(int i = 0; i < 4; i++){ + for(int j = 0; j < 4; j++){ + for(int k = 0; k < 3; k++){ + KH[i][j] += K[i][k] * H[k][j]; + } + } + } + float temp2[4][4] = {0}; + for(int i = 0; i < 4; i++){ + for(int j = 0; j < 4; j++){ + for(int k = 0; k < 4; k++){ + temp2[i][j] += (I[i][k] - KH[i][k]) * P[k][j]; + } + } + } + for(int i = 0; i < 4; i++){ + for(int j = 0; j < 4; j++){ + P[i][j] = temp2[i][j]; + } + } + } + float temp1 = filtered_data.pitch; + float temp2 = filtered_data.roll; + float temp3 = filtered_data.yaw; + filtered_data = data; // Reset all filtered data to the raw data + filtered_data.pitch = (atan2f(2.0f * (x[0] * x[1] + x[2] * x[3]), 1.0f - 2.0f * (x[1] * x[1] + x[2] * x[2]))); + if(abs(filtered_data.pitch) > M_PI_2) { + filtered_data.roll = -(asinf(2.0f * (x[0]*x[2] - x[1]*x[3]))); + if(filtered_data.roll > 0) { + filtered_data.roll -= M_PI; + } else { + filtered_data.roll += M_PI; + } + }else{ + filtered_data.roll = asinf(2.0f * (x[0]*x[2] - x[1]*x[3])); + } + filtered_data.yaw = atan2f(2.0f*(x[0]*x[3] + x[1]*x[2]), 1.0f - 2.0f*(x[2]*x[2] + x[3]*x[3])); + filtered_data.gyro_yaw = (filtered_data.yaw - temp3)/dt; + filtered_data.gyro_roll = (filtered_data.roll - temp2)/dt; + filtered_data.gyro_pitch = (filtered_data.pitch - temp1)/dt; + // Convert to the original + filtered_data.accel_world_X = filtered_data.accel_X * (1-2*(x[2]*x[2] + x[3]*x[3])) + filtered_data.accel_Y * (2*(x[1]*x[2] - x[3]*x[0])) + filtered_data.accel_Z * (2*(x[1]*x[3] + x[0]*x[2])); + filtered_data.accel_world_Y = filtered_data.accel_X * (2*(x[1]*x[2] + x[3]*x[0])) + filtered_data.accel_Y * (1-2*(x[1]*x[1] + x[3]*x[3])) + filtered_data.accel_Z * (2*(x[2]*x[3] - x[1]*x[0])); + filtered_data.accel_world_Z = filtered_data.accel_X * (2*(x[1]*x[3] - x[2]*x[0])) + filtered_data.accel_Y * (2*(x[2]*x[3] + x[1]*x[0])) + filtered_data.accel_Z * (1 - 2*(x[1]*x[1] + x[2]*x[2])) - SENSORS_GRAVITY_EARTH; + return 0; + +} + +IMU_data* IMU_filter::get_filter_data(){ + return &filtered_data; +} + +void IMU_filter::inverse3x3(float mat[3][3], float inv[3][3]) { + float det = mat[0][0] * (mat[1][1] * mat[2][2] - mat[2][1] * mat[1][2]) - + mat[0][1] * (mat[1][0] * mat[2][2] - mat[1][2] * mat[2][0]) + + mat[0][2] * (mat[1][0] * mat[2][1] - mat[1][1] * mat[2][0]); + + if (det == 0) { + // Matrix is not invertible + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) + inv[i][j] = mat[i][j]; + + return; + } + + float invDet = 1.0f / det; + + inv[0][0] = (mat[1][1] * mat[2][2] - mat[2][1] * mat[1][2]) * invDet; + inv[0][1] = (mat[0][2] * mat[2][1] - mat[0][1] * mat[2][2]) * invDet; + inv[0][2] = (mat[0][1] * mat[1][2] - mat[0][2] * mat[1][1]) * invDet; + inv[1][0] = (mat[1][2] * mat[2][0] - mat[1][0] * mat[2][2]) * invDet; + inv[1][1] = (mat[0][0] * mat[2][2] - mat[0][2] * mat[2][0]) * invDet; + inv[1][2] = (mat[1][0] * mat[0][2] - mat[0][0] * mat[1][2]) * invDet; + inv[2][0] = (mat[1][0] * mat[2][1] - mat[2][0] * mat[1][1]) * invDet; + inv[2][1] = (mat[2][0] * mat[0][1] - mat[0][0] * mat[2][1]) * invDet; + inv[2][2] = (mat[0][0] * mat[1][1] - mat[1][0] * mat[0][1]) * invDet; +} + + + + + diff --git a/src/filters/IMU_filter.hpp b/src/filters/IMU_filter.hpp new file mode 100644 index 00000000..945d3ab8 --- /dev/null +++ b/src/filters/IMU_filter.hpp @@ -0,0 +1,40 @@ +#ifndef IMU_FILTER_H +#define IMU_FILTER_H +#include "../utils/timing.hpp" +#include "../sensors/IMUSensor.hpp" + +class IMU_filter{ + private: + Timer timer; + float dt; + + float x[4]; + + float Q; //process noise for angle + float R; //process noise for accelerometer + float chi_square; //Chi-squared test + std::array, 4> P; // estimate covariance + std::array, 4> K; // Kalman gain + IMU_data filtered_data; + void inverse3x3(float mat[3][3], float inv[3][3]); + + + + public: + /// @brief Initalize everything including filter constant + void init_EKF_6axis(IMU_data); + + int step_EKF_6axis(IMU_data); + + void init_Kalman_Mahony(IMU_data); + + void step_Kalman_Mahony(IMU_data); + + /// @brief Print out data for debugging + void print(); + /// @brief Print out data for a Python 3D visulize function + void serial_data_for_plot(); + + IMU_data* get_filter_data(); +}; +#endif \ No newline at end of file diff --git a/src/sensors/ICM20649.cpp b/src/sensors/ICM20649.cpp index 440a1049..6100dd03 100644 --- a/src/sensors/ICM20649.cpp +++ b/src/sensors/ICM20649.cpp @@ -53,23 +53,14 @@ bool ICM20649::read() { // could increase efficiency by specifying which values we need, and only assigning values to the object's members from that. // However, getEvent will read all values from the sensor regardless, and assigning these values is very fast - accel_X = accel.acceleration.x; - accel_Y = accel.acceleration.y; - accel_Z = accel.acceleration.z; - gyro_X = gyro.gyro.x; - gyro_Y = gyro.gyro.y; - gyro_Z = gyro.gyro.z; + data.accel_X = accel.acceleration.x; + data.accel_Y = accel.acceleration.y; + data.accel_Z = accel.acceleration.z; + data.gyro_X = gyro.gyro.x; + data.gyro_Y = gyro.gyro.y; + data.gyro_Z = gyro.gyro.z; - temperature = temp.temperature; - - //copy the values to the data struct - icm_sensor_data.accel_X = accel_X; - icm_sensor_data.accel_Y = accel_Y; - icm_sensor_data.accel_Z = accel_Z; - icm_sensor_data.gyro_X = gyro_X; - icm_sensor_data.gyro_Y = gyro_Y; - icm_sensor_data.gyro_Z = gyro_Z; - icm_sensor_data.temperature = temperature; + data.temperature = temp.temperature; return true; } @@ -104,3 +95,4 @@ void ICM20649::set_gyro_range(int gyro_rate_range) { break; } } + diff --git a/src/sensors/ICM20649.hpp b/src/sensors/ICM20649.hpp index efbdc348..7931a9dc 100644 --- a/src/sensors/ICM20649.hpp +++ b/src/sensors/ICM20649.hpp @@ -17,26 +17,6 @@ /// @brief SCL/SCK (SPI Clock) pin for software-SPI mode #define ICM_SCK 27 -/// @brief Structure for the ICM sensor. -struct ICMSensorData { - /// Sensor ID. - uint8_t id; - /// Acceleration in X-axis. - float accel_X; - /// Acceleration in Y-axis. - float accel_Y; - /// Acceleration in Z-axis. - float accel_Z; - /// Gyroscope reading in X-axis. - float gyro_X; - /// Gyroscope reading in Y-axis. - float gyro_Y; - /// Gyroscope reading in Z-axis. - float gyro_Z; - /// Temperature reading. - float temperature; -}; - /// @brief Sensor access for an ICM20649 IMU Sensor. Child of the abstract IMUSensor class. /// @note supports I2C and SPI communication. /// @see Adafruit library this class utilizes: https://adafruit.github.io/Adafruit_ICM20X/html/class_adafruit___i_c_m20_x.html @@ -84,9 +64,6 @@ class ICM20649 : public IMUSensor { /// @brief The selected communication protocol CommunicationProtocol protocol; - - ///ICM sensor data. - ICMSensorData icm_sensor_data; }; diff --git a/src/sensors/IMUSensor.cpp b/src/sensors/IMUSensor.cpp index d716545e..0b68ea0d 100644 --- a/src/sensors/IMUSensor.cpp +++ b/src/sensors/IMUSensor.cpp @@ -1,6 +1,19 @@ #include "IMUSensor.hpp" -// default implementation for printing data +IMU_data IMUSensor::get_data(){ + return data; +} + +void IMUSensor::fix_raw_data(){ + data.accel_X *= scale_accel; + data.accel_Y *= scale_accel; + data.accel_Z *= scale_accel; + data.gyro_X -= offset_X; + data.gyro_Y -= offset_Y; + data.gyro_Z -= offset_Z; + return; +} + void IMUSensor::print() { // Display the temperature data, measured in Celcius Serial.print("\t\tTemperature "); @@ -23,4 +36,34 @@ void IMUSensor::print() { Serial.print(get_gyro_Z()); Serial.println(" radians/s "); Serial.println(); +} + +void IMUSensor::calibration_all(){ + Serial.println("Calibrating IMU..."); + float sum_accel_x = 0; + float sum_accel_y = 0; + float sum_accel_z = 0; + float sum_x = 0; + float sum_y = 0; + float sum_z = 0; + for (int i = 0; i < CALIBRATION_NUM; i++){ + read(); + sum_accel_x += data.accel_X; + sum_accel_y += data.accel_Y; + sum_accel_z += data.accel_Z; + sum_x += data.gyro_X; + sum_y += data.gyro_Y; + sum_z += data.gyro_Z; + } + float x = sum_accel_x/CALIBRATION_NUM; + float y = sum_accel_y/CALIBRATION_NUM; + float z = sum_accel_z/CALIBRATION_NUM; + scale_accel = SENSORS_GRAVITY_EARTH/sqrt((x*x) + (y*y) + (z*z)); + set_offsets(sum_x / CALIBRATION_NUM, sum_y / CALIBRATION_NUM, sum_z / CALIBRATION_NUM); + data.pitch_bias = atan2(y,sqrt(x*x + z*z)); + data.roll_bias = atan2(-x,z); + data.accel_X *= scale_accel; + data.accel_Y *= scale_accel; + data.accel_Z *= scale_accel; + Serial.println("Calibrating Finish..."); } \ No newline at end of file diff --git a/src/sensors/IMUSensor.hpp b/src/sensors/IMUSensor.hpp index aa0157e5..5154cc81 100644 --- a/src/sensors/IMUSensor.hpp +++ b/src/sensors/IMUSensor.hpp @@ -6,6 +6,35 @@ #include #include "Sensor.hpp" +#define CALIBRATION_NUM 100000 + + +struct IMU_data{ +float accel_X = 0; //acceleration raw value +float accel_Y = 0; +float accel_Z = 0; + +float gyro_X = 0; //raw gyroscope value (rad/s) x(along roll) y(along pitch) z(up-down) +float gyro_Y = 0; +float gyro_Z = 0; + +float temperature = 0; //(c) + +float pitch = 0; //Angle (rad) +float roll = 0; +float yaw = 0; + +float accel_world_X = 0; //Acceleration in world frame (m/s) +float accel_world_Y = 0; +float accel_world_Z = 0; + +float gyro_pitch = 0; //Filtered angular velocity (rad/s) +float gyro_roll = 0; +float gyro_yaw = 0; + +float roll_bias = 0; +float pitch_bias = 0; +}; /// @brief Abstract parent class for all IMUSensors, which give acceleration and gyroscope data. class IMUSensor : public Sensor { @@ -14,39 +43,39 @@ class IMUSensor : public Sensor { IMUSensor() : Sensor(SensorType::ICM) {} - /// @brief read values from the sensor. Call this to update sensor data before accessing them from the getters. + /// @brief read values from the sensor. Call this to update sensor data before accessing them from the getters. /// @return true if successful, false if no data available virtual bool read() = 0; /// @brief Get the temperature of the sensor /// @return temperature in Celcius - inline float get_temperature() { return temperature; }; + inline float get_temperature() { return data.temperature; }; - - //POSSIBLE BUG REASON: the accelerations are not corrected for the offset possibly being the reason for the weird values. /// @brief Get the acceleration of the sensor in its local x axis. /// @return acceleration m/s^2 - inline float get_accel_X() { return accel_X; }; + inline float get_accel_X() { return data.accel_X; }; /// @brief Get the acceleration of the sensor in its local y axis /// @return acceleration m/s^2 - inline float get_accel_Y() { return accel_Y; }; + inline float get_accel_Y() { return data.accel_Y; }; /// @brief Get the acceleration of the sensor in its local z axis /// @return acceleration m/s^2 - inline float get_accel_Z() { return accel_Z; }; + inline float get_accel_Z() { return data.accel_Z; }; /// @brief Get the change in gyroscope orientation relative to the x axis /// @return gryoscope x in radians/s - inline float get_gyro_X() { return gyro_X - offset_X; }; + inline float get_gyro_X() { return data.gyro_X; }; /// @brief Get the change in gyroscope orientation relative to the y axis /// @return gyroscope y in radians/s - inline float get_gyro_Y() { return gyro_Y - offset_Y; }; + inline float get_gyro_Y() { return data.gyro_Y;}; /// @brief Get the change in gyroscope orientation relative to the z axis /// @return gyroscope z in radians/s - inline float get_gyro_Z() { return gyro_Z - offset_Z; }; + inline float get_gyro_Z() { return data.gyro_Z;}; + + IMU_data get_data(); /// @brief Set offsets that we calculate during calibration /// @param x offset in x @@ -58,9 +87,16 @@ class IMUSensor : public Sensor { offset_Z = z; } + inline void set_scale(float a){ + scale_accel = a; + } + + void fix_raw_data(); + /// @brief Print out all IMU data to Serial for debugging purposes void print(); + void calibration_all(); protected: // sensor events to read from @@ -74,21 +110,8 @@ class IMUSensor : public Sensor { sensors_event_t temp; // acceleration values assignmed after read() - /// @brief acceleration x value - float accel_X = 0; - /// @brief acceleration y value - float accel_Y = 0; - /// @brief acceleration z value - float accel_Z = 0; - - // gyroscope values assigned after read() - - /// @brief gyroscope x value - float gyro_X = 0; - /// @brief gyroscope y value - float gyro_Y = 0; - /// @brief gyroscope z value - float gyro_Z = 0; + /// @brief data set + IMU_data data; /// @brief offset x value float offset_X = 0; /// @brief offset y value @@ -96,8 +119,7 @@ class IMUSensor : public Sensor { /// @brief offset z value float offset_Z = 0; - /// @brief temperature value - float temperature = 0; + float scale_accel = 1; }; #endif \ No newline at end of file diff --git a/src/sensors/LSM6DSOX.cpp b/src/sensors/LSM6DSOX.cpp index 703c7e95..021bffbe 100644 --- a/src/sensors/LSM6DSOX.cpp +++ b/src/sensors/LSM6DSOX.cpp @@ -23,13 +23,13 @@ bool LSM6DSOX::read() { // could increase efficiency by specifying which values we need, and only assigning values to the object's members from that. // However, getEvent will read all values from the sensor regardless, and assigning these values is very fast - accel_X = accel.acceleration.x; - accel_Y = accel.acceleration.y; - accel_Z = accel.acceleration.z; - gyro_X = gyro.gyro.x; - gyro_Y = gyro.gyro.y; - gyro_Z = gyro.gyro.z; - - temperature = temp.temperature; + data.accel_X = accel.acceleration.x; + data.accel_Y = accel.acceleration.y; + data.accel_Z = accel.acceleration.z; + data.gyro_X = gyro.gyro.x; + data.gyro_Y = gyro.gyro.y; + data.gyro_Z = gyro.gyro.z; + + data.temperature = temp.temperature; return true; } \ No newline at end of file diff --git a/src/sensors/SensorManager.cpp b/src/sensors/SensorManager.cpp index 347ec73f..7f01df1b 100644 --- a/src/sensors/SensorManager.cpp +++ b/src/sensors/SensorManager.cpp @@ -46,9 +46,8 @@ void SensorManager::init(const Config* config_data) { icm_sensors[i] = new ICM20649(); icm_sensors[i]->init(icm_sensors[i]->CommunicationProtocol::SPI); icm_sensors[i]->set_gyro_range(4000); - + icm_sensors[i]->calibration_all(); } - calibrate_imus(); // initialize rev encoders for (int i = 0; i < rev_sensor_count; i++) { @@ -78,6 +77,7 @@ void SensorManager::read() { } for (int i = 0; i < icm_sensor_count; i++) { icm_sensors[i]->read(); + icm_sensors[i]->fix_raw_data(); icm_sensors[i]->print(); } @@ -126,31 +126,6 @@ D200LD14P* SensorManager::get_lidar_sensor(int index) { } } -void SensorManager::calibrate_imus() { - Serial.println("Calibrating IMU's..."); - float sum_x = 0; - float sum_y = 0; - float sum_z = 0; - - float sum_accel_x = 0; - float sum_accel_y = 0; - float sum_accel_z = 0; - - for (int i = 0; i < NUM_IMU_CALIBRATION; i++) { - icm_sensors[0]->read(); - sum_x += icm_sensors[0]->get_gyro_X(); - sum_y += icm_sensors[0]->get_gyro_Y(); - sum_z += icm_sensors[0]->get_gyro_Z(); - - sum_accel_x += icm_sensors[0]->get_accel_X(); - sum_accel_y += icm_sensors[0]->get_accel_Y(); - sum_accel_z += icm_sensors[0]->get_accel_Z(); - } - - Serial.printf("Calibrated offsets: %f, %f, %f\n", sum_x / NUM_IMU_CALIBRATION, sum_y / NUM_IMU_CALIBRATION, sum_z / NUM_IMU_CALIBRATION); - icm_sensors[0]->set_offsets(sum_x / NUM_IMU_CALIBRATION, sum_y / NUM_IMU_CALIBRATION, sum_z / NUM_IMU_CALIBRATION); -} - int SensorManager::get_num_sensors(SensorType sensor_type) { switch (sensor_type) { case SensorType::BUFFENC: diff --git a/src/sensors/SensorManager.hpp b/src/sensors/SensorManager.hpp index 4057ec95..ce9569f6 100644 --- a/src/sensors/SensorManager.hpp +++ b/src/sensors/SensorManager.hpp @@ -74,9 +74,6 @@ class SensorManager { return ref; } - /// @brief Calibrate the IMUs - void calibrate_imus(); - // TODO: Create function that will create a struct to work with the new comms private: From 38274e291a076c94edb1ec0f30420e5c7b976a79 Mon Sep 17 00:00:00 2001 From: JimOAOOO Date: Tue, 1 Apr 2025 22:47:51 -0600 Subject: [PATCH 2/5] make it documented --- src/filters/IMU_filter.cpp | 6 ++++-- src/filters/IMU_filter.hpp | 10 +++++----- src/sensors/IMUSensor.hpp | 41 ++++++++++++++++++++------------------ 3 files changed, 31 insertions(+), 26 deletions(-) diff --git a/src/filters/IMU_filter.cpp b/src/filters/IMU_filter.cpp index 0df529b7..fd6ee192 100644 --- a/src/filters/IMU_filter.cpp +++ b/src/filters/IMU_filter.cpp @@ -178,8 +178,9 @@ int IMU_filter::step_EKF_6axis(IMU_data data){ } } } - float temp1 = filtered_data.pitch; - float temp2 = filtered_data.roll; + // These are the last time data + float temp1 = filtered_data.pitch; + float temp2 = filtered_data.roll; float temp3 = filtered_data.yaw; filtered_data = data; // Reset all filtered data to the raw data filtered_data.pitch = (atan2f(2.0f * (x[0] * x[1] + x[2] * x[3]), 1.0f - 2.0f * (x[1] * x[1] + x[2] * x[2]))); @@ -194,6 +195,7 @@ int IMU_filter::step_EKF_6axis(IMU_data data){ filtered_data.roll = asinf(2.0f * (x[0]*x[2] - x[1]*x[3])); } filtered_data.yaw = atan2f(2.0f*(x[0]*x[3] + x[1]*x[2]), 1.0f - 2.0f*(x[2]*x[2] + x[3]*x[3])); + // Calculate the filtered gyro data filtered_data.gyro_yaw = (filtered_data.yaw - temp3)/dt; filtered_data.gyro_roll = (filtered_data.roll - temp2)/dt; filtered_data.gyro_pitch = (filtered_data.pitch - temp1)/dt; diff --git a/src/filters/IMU_filter.hpp b/src/filters/IMU_filter.hpp index 945d3ab8..fea33ab9 100644 --- a/src/filters/IMU_filter.hpp +++ b/src/filters/IMU_filter.hpp @@ -24,17 +24,17 @@ class IMU_filter{ /// @brief Initalize everything including filter constant void init_EKF_6axis(IMU_data); + /// @brief Do one step of the EKF filter + /// @param IMU_data is the IMU data structure that holds all the IMU data + /// @return int 0 if successful int step_EKF_6axis(IMU_data); - void init_Kalman_Mahony(IMU_data); - - void step_Kalman_Mahony(IMU_data); - /// @brief Print out data for debugging void print(); /// @brief Print out data for a Python 3D visulize function void serial_data_for_plot(); - + /// @brief get the filtered data + /// @return IMU data structure IMU_data* get_filter_data(); }; #endif \ No newline at end of file diff --git a/src/sensors/IMUSensor.hpp b/src/sensors/IMUSensor.hpp index 5154cc81..f4e71bcf 100644 --- a/src/sensors/IMUSensor.hpp +++ b/src/sensors/IMUSensor.hpp @@ -8,32 +8,33 @@ #include "Sensor.hpp" #define CALIBRATION_NUM 100000 - +/// @brief the data structure that holds all the IMU data struct IMU_data{ -float accel_X = 0; //acceleration raw value +//acceleration raw value +float accel_X = 0; float accel_Y = 0; float accel_Z = 0; - -float gyro_X = 0; //raw gyroscope value (rad/s) x(along roll) y(along pitch) z(up-down) +// raw gyroscope value (rad/s) x(along roll) y(along pitch) z(up-down) +float gyro_X = 0; float gyro_Y = 0; float gyro_Z = 0; - -float temperature = 0; //(c) - -float pitch = 0; //Angle (rad) +// Temperature in Celcius +float temperature = 0; +// Angle (rad) +float pitch = 0; float roll = 0; float yaw = 0; - -float accel_world_X = 0; //Acceleration in world frame (m/s) +// Acceleration in world frame (m/s) +float accel_world_X = 0; float accel_world_Y = 0; float accel_world_Z = 0; - -float gyro_pitch = 0; //Filtered angular velocity (rad/s) +// angular velocity (rad/s) +float gyro_pitch = 0; float gyro_roll = 0; float gyro_yaw = 0; - -float roll_bias = 0; -float pitch_bias = 0; +// Bias for roll angle (rad) +float roll_bias = 0; +float pitch_bias = 0; }; /// @brief Abstract parent class for all IMUSensors, which give acceleration and gyroscope data. @@ -74,7 +75,8 @@ class IMUSensor : public Sensor { /// @brief Get the change in gyroscope orientation relative to the z axis /// @return gyroscope z in radians/s inline float get_gyro_Z() { return data.gyro_Z;}; - + /// @brief get the data structure that holds all the IMU data + /// @return the IMU data structure IMU_data get_data(); /// @brief Set offsets that we calculate during calibration @@ -86,16 +88,17 @@ class IMUSensor : public Sensor { offset_Y = y; offset_Z = z; } - + /// @brief set the scale of the accelerometer + /// @param a the scale of the accelerometer inline void set_scale(float a){ scale_accel = a; } - + /// @brief add bias to gyro and scale to accel void fix_raw_data(); /// @brief Print out all IMU data to Serial for debugging purposes void print(); - + /// @brief Calibrate the IMU sensor void calibration_all(); protected: // sensor events to read from From d19e5bfdcd4ab0abe1504e6af803c5e685a26885 Mon Sep 17 00:00:00 2001 From: JimOAOOO Date: Tue, 1 Apr 2025 22:59:37 -0600 Subject: [PATCH 3/5] add documentation to IMU filter and sensor classes --- src/filters/IMU_filter.hpp | 27 ++++++++++++++++----------- src/sensors/IMUSensor.hpp | 31 +++++++++++++++++++++---------- 2 files changed, 37 insertions(+), 21 deletions(-) diff --git a/src/filters/IMU_filter.hpp b/src/filters/IMU_filter.hpp index fea33ab9..e4b469b1 100644 --- a/src/filters/IMU_filter.hpp +++ b/src/filters/IMU_filter.hpp @@ -2,24 +2,29 @@ #define IMU_FILTER_H #include "../utils/timing.hpp" #include "../sensors/IMUSensor.hpp" - +/// @brief the IMU filter class that filters the IMU data class IMU_filter{ private: + /// @brief the timer for dt Timer timer; + /// @brief the time step float dt; - + /// @brief the quaternion for the filter float x[4]; - - float Q; //process noise for angle - float R; //process noise for accelerometer - float chi_square; //Chi-squared test - std::array, 4> P; // estimate covariance + /// @brief process noise for model update + float Q; + /// @brief process noise for accelerometer + float R; + /// @brief the prediction matrix + std::array, 4> P; + /// @brief the Kalman gain std::array, 4> K; // Kalman gain - IMU_data filtered_data; + /// @brief the data structure that holds all the IMU data + IMU_data filtered_data = {0}; + /// @brief the function for inverse 3x3 matrix + /// @param mat input + /// @param inv output void inverse3x3(float mat[3][3], float inv[3][3]); - - - public: /// @brief Initalize everything including filter constant void init_EKF_6axis(IMU_data); diff --git a/src/sensors/IMUSensor.hpp b/src/sensors/IMUSensor.hpp index f4e71bcf..64a8e625 100644 --- a/src/sensors/IMUSensor.hpp +++ b/src/sensors/IMUSensor.hpp @@ -10,30 +10,41 @@ /// @brief the data structure that holds all the IMU data struct IMU_data{ -//acceleration raw value +/// @brief raw acceleration value (m/s^2) float accel_X = 0; -float accel_Y = 0; +/// @brief raw acceleration value (m/s^2) +float accel_Y = 0; +/// @brief raw acceleration value (m/s^2) float accel_Z = 0; -// raw gyroscope value (rad/s) x(along roll) y(along pitch) z(up-down) +/// @brief raw gyroscope value (rad/s) float gyro_X = 0; +/// @brief raw gyroscope value (rad/s) float gyro_Y = 0; +/// @brief raw gyroscope value (rad/s) float gyro_Z = 0; -// Temperature in Celcius +/// @brief Temperature in Celcius float temperature = 0; -// Angle (rad) +/// @brief Angle (rad) float pitch = 0; +/// @brief Angle (rad) float roll = 0; +/// @brief Angle (rad) float yaw = 0; -// Acceleration in world frame (m/s) -float accel_world_X = 0; +/// @brief Acceleration in world frame (m/s) +float accel_world_X = 0; +/// @brief Acceleration in world frame (m/s) float accel_world_Y = 0; +/// @brief Acceleration in world frame (m/s) float accel_world_Z = 0; -// angular velocity (rad/s) +/// @brief angular velocity (rad/s) float gyro_pitch = 0; +/// @brief angular velocity (rad/s) float gyro_roll = 0; +/// @brief angular velocity (rad/s) float gyro_yaw = 0; -// Bias for roll angle (rad) +/// @brief Bias for roll angle (rad) float roll_bias = 0; +/// @brief Bias for pitch angle (rad) float pitch_bias = 0; }; @@ -121,7 +132,7 @@ class IMUSensor : public Sensor { float offset_Y = 0; /// @brief offset z value float offset_Z = 0; - + /// @brief scale of the accelerometer float scale_accel = 1; }; From 400f9eba7d44581731f02c321397c464f911459b Mon Sep 17 00:00:00 2001 From: JimOAOOO Date: Tue, 1 Apr 2025 23:01:35 -0600 Subject: [PATCH 4/5] documented for pull request --- src/filters/IMU_filter.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/filters/IMU_filter.hpp b/src/filters/IMU_filter.hpp index e4b469b1..93190afc 100644 --- a/src/filters/IMU_filter.hpp +++ b/src/filters/IMU_filter.hpp @@ -27,8 +27,8 @@ class IMU_filter{ void inverse3x3(float mat[3][3], float inv[3][3]); public: /// @brief Initalize everything including filter constant + /// @param IMU_data is the IMU data structure that holds all the IMU data void init_EKF_6axis(IMU_data); - /// @brief Do one step of the EKF filter /// @param IMU_data is the IMU data structure that holds all the IMU data /// @return int 0 if successful @@ -39,7 +39,7 @@ class IMU_filter{ /// @brief Print out data for a Python 3D visulize function void serial_data_for_plot(); /// @brief get the filtered data - /// @return IMU data structure + /// @return pointer of filtered IMU data structure IMU_data* get_filter_data(); }; #endif \ No newline at end of file From d443fa8c85ba6ee62ba9d00494521b1d67aedd86 Mon Sep 17 00:00:00 2001 From: JimOAOOO Date: Sat, 19 Apr 2025 15:46:22 -0600 Subject: [PATCH 5/5] Add link to the detail of IMU filter --- src/filters/IMU_filter.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/filters/IMU_filter.cpp b/src/filters/IMU_filter.cpp index fd6ee192..4f31f419 100644 --- a/src/filters/IMU_filter.cpp +++ b/src/filters/IMU_filter.cpp @@ -1,4 +1,5 @@ #include "IMU_filter.hpp" +// For more detail about imu filter Check https://github.com/OAOJim/Study_Notes/blob/main/IMU_filter.md void IMU_filter::init_EKF_6axis(IMU_data data){ // Set a initial value for the quaternion @@ -8,7 +9,7 @@ void IMU_filter::init_EKF_6axis(IMU_data data){ data.accel_Z = data.accel_Z * recipNorm;//This is the unit gravity float axis_norm = sqrt(data.accel_Y * data.accel_Y + data.accel_X * data.accel_X); float sin_halfangle = sinf(acosf(data.accel_Z)/2.0f); - if(axis_norm > 1e-6){ + if(axis_norm > 1e-6){ // Just some stupid way to initialize the quaternion. Quaternion is not easy to initialize since 2 quaternion can represent the same rotation x[0] = cosf(data.accel_Z/2.0f); x[1] = data.accel_Y * sin_halfangle / axis_norm; x[2] = -data.accel_X * sin_halfangle / axis_norm; @@ -20,11 +21,10 @@ void IMU_filter::init_EKF_6axis(IMU_data data){ x[3] = 0; } // After test. within 0.5 second pitch and roll will get to the right value when IMU facing up; - //Since the weight of Q and R for all element should be the same - Q = 1; - R = 1000; - P[0] = {1000,0,0,0}; + Q = 1; // Process noise covariance for model update + R = 1000; // measure noise + P[0] = {1000,0,0,0}; // Initial P matrix P[1] = {0,1000,0,0}; P[2] = {0,0,1000,0}; P[3] = {0,0,0,1000};