I'm using an Arduino Nano 33 IoT to sense movements. Firstly I tried to compute manually pitch-roll-yaw (yes I know yaw cannot be computed without a magnetometer but I don't have one in this IMU), then I found that to avoid gimbal lock it is better use quaternions, so I used the MadgwickAHRS library to extract them. This library also compute the euler angles but when I try to print it I find only zeros.
What am I missing?
#include <Arduino_LSM6DS3.h>
#include <ArduinoBLE.h>
#include <BasicLinearAlgebra.h>
#include <SimpleKalmanFilter.h>
#include <MadgwickAHRS.h>
// Program Variables
const int sampling = 1000; // Sampling period in milliseconds (1 Hz)
const int window = 5000; // Sliding window size in ms (5 seconds)
float threshold = 1.02; // Threshold to detect significant movement (adjust based on your needs)
float acc_Buffer[1000]; // Buffer for acceleration
int bufferIndex_a = 0;
float ax, ay, az; // x, y, z accelerometer
float gx, gy, gz; // x, y, z gyroscope
float pitch = 0.0, roll = 0.0, yaw = 0.0; // Orientation angles
float displacement[3] = {0.0, 0.0, 0.0};
//float G[3] = {0.0, 0.0, 9.81};
float R_inv[3][3];
using namespace BLA;
Madgwick filter; // Create Madgwick filter object
float mea_e = 2; // Measurement Uncertainty
float est_e = 2; // Estimation Uncertainty
float q = 0.01; // Process Noise
SimpleKalmanFilter simpleKalmanFilter(mea_e, est_e, q);
const long SERIAL_REFRESH_TIME = 100;
long refresh_time;
float filtered = 0;
void setup() {
Serial.begin(115200);
BLE.begin();
pinMode(LED_BUILTIN, OUTPUT);
if (!IMU.begin()) {
Serial.println("Failed to initialize IMU!");
while (1);
}
Serial.println("IMU initializad.");
}
void loop() {
static long preMillis = 0;
if (central)
{
while (central.connected())
{
long curMillis = millis();
if (preMillis>curMillis) preMillis=0; // millis() rollover?
if (curMillis - preMillis >= sampling) // check values every 10mS
{
preMillis = curMillis;
updateValues(); // call function for updating value to send to central
}
} // still here while central connected
// central disconnected:
digitalWrite(LED_BUILTIN, LOW);
Serial.print(F("Disconnected from central: "));
Serial.println(central.address());
} // no central
}
void updateValues(){
if (IMU.accelerationAvailable()) {
IMU.readAcceleration(ax, ay, az);
BLA::Matrix<3, 1> acc_vector = {ax, ay, az};
BLA::Matrix<3, 1> G = {0.0, 0.0, 9.81};
// Calculate pitch and roll from accelerometer
//pitch = atan2(-ax, sqrt(ay * ay + az * az));
//roll = atan2(-ay, sqrt(ax * ax + az * az));
IMU.readGyroscope(gx, gy, gz);
gx = radians(gx);
gy = radians(gy);
gz = radians(gz);
filter.updateIMU(gx, gy, gz, ax, ay, az);
// Get the quaternion values. In the library, locate MadgwickAHRS.h and change from private to public
float qw = filter.q0;
float qx = filter.q1;
float qy = filter.q2;
float qz = filter.q3;
float pitch = filter.pitch;
float roll = filter.roll;
float yaw = filter.yaw;
//yaw += gz * sampling; // Adjust yaw over time using gyro Z data
//BLA::Matrix<3, 3> Rx = {1, 0, 0, 0, cos(pitch), -sin(pitch), 0, sin(pitch), cos(pitch)};
//BLA::Matrix<3, 3> Ry = {cos(roll), 0, sin(roll), 0, 1, 0, -sin(roll), 0, cos(roll)};
//BLA::Matrix<3, 3> Rz = {cos(yaw), -sin(yaw), 0, sin(yaw), cos(yaw), 0, 0, 0, 1};
//BLA::Matrix<3, 3> R = Rz * Ry * Rx;
// From https://web.archive.org/web/20180112063505/http://www.chrobotics.com/library/accel-position-velocity
// to compensate the gravity I must compute displacement = R_inv*acc_vector + (0,0,g)
BLA::Matrix<3, 3> R = { qw*qw + qx*qx - qy*qy -qz*qz, 2*qx*qy - 2*qw*qz, 2*qx*qy + 2*qw*qy,
2*qx*qy + 2*qw*qz , qw*qw - qx*qx + qy*qy - qz*qz , 2*qy*qz - 2*qw*qx,
2*qx*qz - 2*qw*qy, 2*qy*qz + 2*qw*qz, qw*qw - qx*qx - qy*qy + qz*qz};
BLA::Matrix<3, 3> R_inv = Inverse(R);
BLA::Matrix<3, 1> displacement = R_inv * acc_vector;
//displacement += G;
float abs = sqrt(displacement(1)*displacement(1) + displacement(2)*displacement(2) + displacement(3)*displacement(3));
//float abs = sqrt(qw*qw + qx*qx + qy*qy + qz*qz);
float filtered = simpleKalmanFilter.updateEstimate(abs);
acc_Buffer[bufferIndex_a] = abs;
bufferIndex_a = (bufferIndex_a + 1) % (window / sampling);
// Print counting and call count function
if (bufferIndex_a == 0) {
int n_movements = counting(acc_Buffer, window / sampling);
Serial.print("# movements per minute: ");
Serial.println(n_movements);
Serial.print(" Abs: ");
Serial.println(abs);
Serial.print(" Filtered: ");
Serial.println(filtered);
Serial.print(" x_a: ");
Serial.println(pitch);
Serial.print(" y_a: ");
Serial.println(roll);
Serial.print(" z_a: ");
Serial.println(yaw);
}
}
}
// Function to calculate displacement rate from acceleration data
int counting(float *buffer, int size) {
int zeroCrossings = 0;
for (int i = 1; i < size; i++) {
if ((buffer[i - 1] < threshold && buffer[i] > threshold) || (buffer[i - 1] > threshold && buffer[i] < threshold)) {
zeroCrossings++;
}
}
// Convert zero crossings to movements per minute
int movementsPerMinute = (zeroCrossings / 2) * (60000 / window);
chaBR.writeValue(movementsPerMinute);
return movementsPerMinute;
}
