由于以下给出的Arduino代码，我试图从四元数值中获取Pitch，Roll和Yaw。当我在串行监视器中或通过“处理”打印这些值时，设备过于“敏感”，偏航值也不准确。它可能来自我的Q向量初始化或Kp和Ki增益。我没有尝试调整收益。
有人可以帮助我吗？真是太好了！
祝大家有美好的一天！
代码：
#include <Wire.h>
#include <SPI.h>
#include <Kalman.h>
#include <math.h>
#include <Adafruit_LSM9DS1.h>
#include <Adafruit_Sensor.h>
#define LSM9DS1_SCK A5
#define LSM9DS1_MISO 12
#define LSM9DS1_MOSI A4
#define LSM9DS1_XGCS 6
#define LSM9DS1_MCS 5
#define CONVERSIONG 3.9
// i2c
Adafruit_LSM9DS1 lsm = Adafruit_LSM9DS1();
float q[4]={1.0f,0.0f,0.0f,0.0f};
float Ki=0.005;
float Kp=2;
float eInt[3];
uint32_t timer;
double accX,accY,accZ;
double gyroX,gyroY,gyroZ;
double magX,magY,magZ;
float pitch;
float roll;
float yaw;
void setupSensor()
{
// 1.) Set the accelerometer range
lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_2G);
// 2.) Set the magnetometer sensitivity
// 3.) Setup the gyroscope
lsm.setupGyro(lsm.LSM9DS1_GYROSCALE_245DPS);
}
void setup() {
Serial.begin(115200);
while (!Serial) {
delay(1);
}
if (!lsm.begin())
{
Serial.println("Oops ... unable to initialize the LSM9DS1. Check your wiring!");
while (1);
}
Serial.println("Found LSM9DS1 9DOF");
setupSensor();
}
void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz)
{
float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; // short name local
variable for readability
float norm;
float hx, hy, bx, bz;
float vx, vy, vz, wx, wy, wz;
float ex, ey, ez;
float pa, pb, pc;
float q1q1 = q1 * q1;
float q1q2 = q1 * q2;
float q1q3 = q1 * q3;
float q1q4 = q1 * q4;
float q2q2 = q2 * q2;
float q2q3 = q2 * q3;
float q2q4 = q2 * q4;
float q3q3 = q3 * q3;
float q3q4 = q3 * q4;
float q4q4 = q4 * q4;
// Normalise accelerometer measurement
norm = sqrtf(ax * ax + ay * ay + az * az);
if (norm == 0.0f) return; // handle NaN
norm = 1.0f / norm; // use reciprocal for division
//Serial.println(norm);
ax *= norm;
ay *= norm;
az *= norm;
// Normalise magnetometer measurement
norm = sqrtf(mx * mx + my * my + mz * mz);
if (norm == 0.0f) return; // handle NaN
norm = 1.0f / norm; // use reciprocal for division
//Serial.println(norm);
mx *= norm;
my *= norm;
mz *= norm;
// Reference direction of Earth's magnetic field
hx = 2.0f * mx * (0.5f - q3q3 - q4q4) + 2.0f * my * (q2q3 - q1q4) + 2.0f * mz * (q2q4 + q1q3);
hy = 2.0f * mx * (q2q3 + q1q4) + 2.0f * my * (0.5f - q2q2 - q4q4) + 2.0f * mz * (q3q4 - q1q2);
bx = sqrtf((hx * hx) + (hy * hy));
bz = 2.0f * mx * (q2q4 - q1q3) + 2.0f * my * (q3q4 + q1q2) + 2.0f * mz * (0.5f - q2q2 - q3q3);
// Estimated direction of gravity and magnetic field
vx = 2.0f * (q2q4 - q1q3);
vy = 2.0f * (q1q2 + q3q4);
vz = q1q1 - q2q2 - q3q3 + q4q4;
wx = 2.0f * bx * (0.5f - q3q3 - q4q4) + 2.0f * bz * (q2q4 - q1q3);
wy = 2.0f * bx * (q2q3 - q1q4) + 2.0f * bz * (q1q2 + q3q4);
wz = 2.0f * bx * (q1q3 + q2q4) + 2.0f * bz * (0.5f - q2q2 - q3q3);
// Error is cross product between estimated direction and measured direction of gravity
ex = (ay * vz - az * vy) + (my * wz - mz * wy);
ey = (az * vx - ax * vz) + (mz * wx - mx * wz);
ez = (ax * vy - ay * vx) + (mx * wy - my * wx);
if (Ki > 0.0f)
{
eInt[0] += ex; // accumulate integral error
eInt[1] += ey;
eInt[2] += ez;
}
else
{
eInt[0] = 0.0f; // prevent integral wind up
eInt[1] = 0.0f;
eInt[2] = 0.0f;
}
// Apply feedback terms
gx = gx + Kp * ex + Ki * eInt[0];
gy = gy + Kp * ey + Ki * eInt[1];
gz = gz + Kp * ez + Ki * eInt[2];
// Integrate rate of change of quaternion
pa = q2;
pb = q3;
pc = q4;
double dt = (double)(micros()-timer)/1000000;
timer=micros();
q1 = q1 + (-q2 * gx - q3 * gy - q4 * gz) * (0.5f * dt);
q2 = pa + (q1 * gx + pb * gz - pc * gy) * (0.5f * dt);
q3 = pb + (q1 * gy - pa * gz + pc * gx) * (0.5f * dt);
q4 = pc + (q1 * gz + pa * gy - pb * gx) * (0.5f * dt);
// Normalise quaternion
norm = sqrtf(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);
norm = 1.0f / norm;
q[0] = q1 * norm;
q[1] = q2 * norm;
q[2] = q3 * norm;
q[3] = q4 * norm;
}
void loop() {
// put your main code here, to run repeatedly:
lsm.read(); /* ask it to read in the data */
/* Get a new sensor event */
sensors_event_t a, m, g, temp;
lsm.getEvent(&a, &m, &g, &temp);
gyroX=g.gyro.x;
gyroY=g.gyro.y;
gyroZ=g.gyro.z;
accX=a.acceleration.x;
accY=a.acceleration.y;
accZ=a.acceleration.z;
magX=m.magnetic.x;
magY=m.magnetic.y;
magZ=m.magnetic.z;
MahonyQuaternionUpdate(accX, accY, accZ, gyroX, gyroY, gyroZ, magX, magY, magZ);
pitch=atan2(2*(q[0]*q[1]+q[2]*q[3]),1-2*(q[1]*q[1]+q[2]*q[2]))*RAD_TO_DEG;
roll=asin(2*(q[0]*q[2]-q[3]*q[1]))*RAD_TO_DEG;
yaw=atan2(2*(q[0]*q[3]+q[1]*q[2]),1-2*(q[2]*q[2]+q[3]*q[3]))*RAD_TO_DEG;
Serial.print(pitch);
Serial.print(',');
Serial.print(roll);
Serial.print(',');
Serial.println(yaw);
}