Hi everyone, I am trying to obtain the orientation of a robot using 3-axis accelerometer, 3-axis magnetometer and 3-axis gyroscope. I implemented a filter to obtain yaw, pitch and roll. However, for what yaw is concerned, I have some problem when pitch and roll reach 90 degrees (gimbal lock?). Thus, i tried to use quaternions, however, their values seem very strange to me, i do not understand where i am wrong in their computation. Can anybody help me? thank you for you time.
here the entire code:
#include <Pozyx.h>
#include <Pozyx_definitions.h>
#include <Wire.h>
/////////////////////////////////////////////////////////
////////////////// PARAMETERS ////////////////////////
/////////////////////////////////////////////////////////
boolean remote = false; // boolean to indicate if we want to read sensor data from the attached pozyx shield (value 0) or from a remote pozyx device (value 1)
uint16_t remote_id = 0x6833; // the network id of the other pozyx device: fill in the network id of the other device
uint32_t last_millis; // used to compute the measurement interval in milliseconds
int dt; //definition in milliseconds of the interval of time between consecutive measurements
float roll;
float yaw;
float pitch;
float roll_acc;
float pitch_acc;
float yaw_acc1;
float yaw_acc2;
float yaw_n;
float yaw_g;
float yaw_filtered;
struct {
double x;
double y;
double z;
} b;
struct {
double w;
double x;
double y;
double z;
} qam; //q_accelerometer_magnetometer
struct {
double w = 1;
double x = 0;
double y = 0;
double z = 0;
} qw; //q_omega (angular velocities)
struct {
double w = 1;
double x = 0;
double y = 0;
double z = 0;
} q; //final quaternion
bool start = false;
int count_start = 0;
//float alfa = 0.8;
//float beta = 0.9;
float const_roll = 0.95;
float const_pitch = 0.95;
float const_yaw1 = 0.05;
float const_yaw = 0.7;
float m[3]; //for calibration
float scaler; //*
boolean scaler_flag = false; // This are for sphere stabilisation
float normal_vector_length; //*/
/////////////////////////////////////////////////////////
////////////////// SETUP /////////////////////////////
/////////////////////////////////////////////////////////
void setup()
{
Serial.begin(115200);
if(Pozyx.begin(false, MODE_INTERRUPT, POZYX_INT_MASK_IMU) == POZYX_FAILURE){
delay(100);
abort();
}
if(!remote)
remote_id = NULL;
last_millis = millis();
delay(5000);
}
/////////////////////////////////////////////////////////
////////////////// LOOP //////////////////////////////
/////////////////////////////////////////////////////////
void loop(){
sensor_raw_t sensor_raw;
Pozyx.getRawSensorData(&sensor_raw);
ComputeQuaternionGyr(sensor_raw.angular_vel[0], sensor_raw.angular_vel[1], sensor_raw.angular_vel[2]);
ComputeRollPitch(sensor_raw.acceleration[0],sensor_raw.acceleration[1],sensor_raw.acceleration[2], sensor_raw.angular_vel[0], sensor_raw.angular_vel[1], sensor_raw.angular_vel[2]);
float rx = sensor_raw.magnetic[0];
float ry = sensor_raw.magnetic[1];
float rz = sensor_raw.magnetic[2];
float uncal[3] = {rx,ry,rz};
ComputeYaw(uncal, sensor_raw.angular_vel[2]);
ComputeQuaternionAccMag();
q.w = ((1-const_yaw1)*qw.w + const_yaw1*qam.w);
q.x = ((1-const_yaw1)*qw.x + const_yaw1*qam.x);
q.y = ((1-const_yaw1)*qw.y + const_yaw1*qam.y);
q.z = ((1-const_yaw1)*qw.z + const_yaw1*qam.z);
float norm = sqrt(q.w*q.w + q.x*q.x + q.y*q.y + q.z*q.z);
q.w = q.w/norm;
q.x = q.x/norm;
q.y = q.y/norm;
q.z = q.z/norm;
Serial.print("yaw_acc:");
Serial.print(yaw);
Serial.print(",");
Serial.print("yaw_gyro:");
Serial.print(yaw_g);
Serial.print(",");
Serial.print("yaw_filtered:");
Serial.print(yaw_n);
float yaw_filtered = atan2(2.0f*(q.y*q.x + q.w*q.z),1.0f - 2.0f*(q.y*q.y + q.z*q.z))*RAD_TO_DEG;
Serial.print(",");
Serial.print("yaw_filtered_quat:");
Serial.println(yaw_filtered);
dt = millis() - last_millis;
last_millis += dt;
delay(100);
}
/////////////////////////////////////////////////////////
////////////////// FUNCTIONS //////////////////////////
/////////////////////////////////////////////////////////
/*
* ROLL and PITCH COMPUTATION
*/
void ComputeRollPitch(float ax, float ay, float az, float gx, float gy, float gz){ //angles in radians
long factor_p = sqrt((long)ay*ay+(long)az*az);
long factor_r = sqrt((long)ax*ax+(long)az*az);
roll_acc = atan2(ay, az)*RAD_TO_DEG;
roll = const_roll*(roll + (gx/16.0f)*(dt/1000.0f)) + (1-const_roll)*roll_acc;
pitch_acc = atan2(-ax, factor_p)*RAD_TO_DEG;
pitch = const_pitch*(pitch + (-gy/16.0f)*(dt/1000.0f)) + (1-const_pitch)*pitch_acc;
}
/*
* YAW COMPUTATION
*/
void ComputeYaw(float notcalibrated[3],float gz){ //angle in radians
//aplly calibration to magnetic raw measures
transformation(notcalibrated);
vector_length_stabilasation();
float mx = m[0];//16.0f;
float my = m[1];//16.0f;
float mz = m[2];//16.0f;
b.x = mx*cos(roll*DEG_TO_RAD) + my*sin(roll*DEG_TO_RAD)*sin(pitch*DEG_TO_RAD) + mz*sin(roll*DEG_TO_RAD)*cos(pitch*DEG_TO_RAD);
b.y = my*cos(pitch*DEG_TO_RAD) - mz*sin(pitch*DEG_TO_RAD);
b.z = -mx*sin(roll*DEG_TO_RAD) + my*cos(roll*DEG_TO_RAD)*sin(pitch*DEG_TO_RAD) + mz*cos(roll*DEG_TO_RAD)*cos(pitch*DEG_TO_RAD);
yaw = atan2(-b.y,b.x)*RAD_TO_DEG;
yaw_g += ((+gz/16.0f)*(dt/1000.0f));
yaw_n = const_yaw*yaw_g + (1-const_yaw)*yaw;
}
/*
* MAGNETIC CALIBRATION FOR YAW
*/
void transformation(float uncalibrated_values[3]){
float calibration_matrix[3][3] =
{
{1.047, -0.039, 0.167},
{0.283, 0.96, -0.104},
{-0.004, -0.027, 1.037}
};
float bias[3] =
{
-39.318,
43.556,
-2.184
};
for (int i=0; i<3; ++i) uncalibrated_values[i] -= bias[i];
float result[3] = {0, 0, 0};
for (int i=0; i<3; ++i){
for (int j=0; j<3; ++j){
result[i] += calibration_matrix[i][j] * uncalibrated_values[j];
}
}
for (int i=0; i<3; ++i) {
m[i] = result[i];
}
}
void vector_length_stabilasation(){
//calculate the normal vector length
if (scaler_flag == false)
{
normal_vector_length = sqrt(m[0]*m[0] + m[1]*m[1] + m[2]*m[2]);
scaler_flag = true;
}
//calculate the current scaler
scaler = normal_vector_length/sqrt(m[0]*m[0] + m[1]*m[1] + m[2]*m[2]);
//apply the current scaler to the calibrated coordinates (global array calibrated_values)
m[0] = m[0]*scaler;
m[1] = m[1]*scaler;
m[2] = m[2]*scaler;
}
/*
* QUATERNION BASED ON ACC AND MAG (STATIC REFERENCE)
*/
void ComputeQuaternionAccMag(){
qam.w = cos(pitch/2)*cos(roll/2)*cos(yaw/2)+sin(pitch/2)*sin(roll/2)*sin(yaw/2);
qam.x = sin(pitch/2)*cos(roll/2)*cos(yaw/2)-cos(pitch/2)*sin(roll/2)*sin(yaw/2);
qam.y = cos(pitch/2)*sin(roll/2)*cos(yaw/2)+sin(pitch/2)*cos(roll/2)*sin(yaw/2);
qam.z = cos(pitch/2)*cos(roll/2)*sin(yaw/2)-sin(pitch/2)*sin(roll/2)*cos(yaw/2);
}
/*
* QUATERNION BASED ON GYRO (DYNAMIC REFERENCE)
*/
void ComputeQuaternionGyr(float gx, float gy, float gz){
gx = gx/16.0f;
gy = gy/16.0f;
gz = gz/16.0f;
int dt1 = 0.5*(dt/(1000.0f));
qw.w = q.w - dt1*gx*q.x - dt1*gy*q.y - dt1*gz*q.z;
qw.x = q.x + dt1*gx*q.w - dt1*gy*q.z + dt1*gz*q.y;
qw.y = q.y + dt1*gx*q.z + dt1*gy*q.w - dt1*gz*q.x;
qw.z = q.z - dt1*gx*q.y + dt1*gy*q.x + dt1*gz*q.w;
}