还有大侠做姿态融合吗？帮忙看看9250数据融合的一个问题
本帖最后由 jssd 于 2021-1-21 09:29 编辑硬件为mpu9250，算法为mahonyUpdate，9轴数据融合后，姿态挺稳，但有一个问题，
当偏航角转到0时再陆续变小的话，偏航角会瞬间变为-300度，然后再继续变小，直到-360度时再瞬变为+330度（差不多这个角度，但没到360）。
反之，当角度增加到-300度时也会瞬变为0度。也就是说，imu转一圈会出现两次偏航角瞬变，其他角度正常。
请问是什么原因？
代码和图片明天贴上来。先行谢过！
//=============================================================================================
// MahonyAHRS.c
//=============================================================================================
//
// Madgwick's implementation of Mayhony's AHRS algorithm.
// See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
//
// From the x-io website "Open-source resources available on this website are
// provided under the GNU General Public Licence unless an alternative licence
// is provided in source."
//
// Date Author Notes
// 29/09/2011 SOH Madgwick Initial release
// 02/10/2011 SOH Madgwick Optimised for reduced CPU load
//
// Algorithm paper:
// http://ieeexplore.ieee.org/xpl/login.jsp?tp=&arnumber=4608934&url=http%3A%2F%2Fieeexplore.ieee.org%2Fstamp%2Fstamp.jsp%3Ftp%3D%26arnumber%3D4608934
//
//=============================================================================================
//-------------------------------------------------------------------------------------------
// Header files
#include "MahonyAHRS.h"
#include <math.h>
//-------------------------------------------------------------------------------------------
// Definitions
float twoKi; // 2 * integral gain (Ki)
float q0, q1, q2, q3; // quaternion of sensor frame relative to auxiliary frame
float integralFBx, integralFBy, integralFBz;// integral error terms scaled by Ki
float invSampleFreq;
float roll, pitch, yaw;
char anglesComputed;
//#define twoKpDef (5.0f * 0.5f) // 2 * proportional gain
//#define twoKiDef (0.5f * 1.0f) // 2 * integral gain
#define twoKpDef (10.0f * 0.5f) // 2 * proportional gain
#define twoKiDef (0.5f * 1.0f) // 2 * integral gain
void Mahony_Init(float sampleFrequency)
{
twoKi = twoKiDef; // 2 * integral gain (Ki)
q0 = 1.0f;
q1 = 0.0f;
q2 = 0.0f;
q3 = 0.0f;
integralFBx = 0.0f;
integralFBy = 0.0f;
integralFBz = 0.0f;
anglesComputed = 0;
invSampleFreq = 1.0f / sampleFrequency;
}
float Mahony_invSqrt(float x)
{
float halfx = 0.5f * x;
float y = x;
long i = *(long*)&y;
i = 0x5f3759df - (i>>1);
y = *(float*)&i;
y = y * (1.5f - (halfx * y * y));
y = y * (1.5f - (halfx * y * y));
return y;
}
void Mahony_update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)
{
float recipNorm;
float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
float hx, hy, bx, bz;
float halfvx, halfvy, halfvz, halfwx, halfwy, halfwz;
float halfex, halfey, halfez;
float qa, qb, qc;
// Compute feedback only if accelerometer measurement valid
// (avoids NaN in accelerometer normalisation)
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
// Normalise accelerometer measurement
recipNorm = Mahony_invSqrt(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
// Normalise magnetometer measurement
recipNorm = Mahony_invSqrt(mx * mx + my * my + mz * mz);
mx *= recipNorm;
my *= recipNorm;
mz *= recipNorm;
// Auxiliary variables to avoid repeated arithmetic
q0q0 = q0 * q0;
q0q1 = q0 * q1;
q0q2 = q0 * q2;
q0q3 = q0 * q3;
q1q1 = q1 * q1;
q1q2 = q1 * q2;
q1q3 = q1 * q3;
q2q2 = q2 * q2;
q2q3 = q2 * q3;
q3q3 = q3 * q3;
// Reference direction of Earth's magnetic field
hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));
bx = sqrtf(hx * hx + hy * hy);
bz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2));
// Estimated direction of gravity and magnetic field
halfvx = q1q3 - q0q2;
halfvy = q0q1 + q2q3;
halfvz = q0q0 - 0.5f + q3q3;
halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);
halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);
// Error is sum of cross product between estimated direction
// and measured direction of field vectors
halfex = (ay * halfvz - az * halfvy) + (my * halfwz - mz * halfwy);
halfey = (az * halfvx - ax * halfvz) + (mz * halfwx - mx * halfwz);
halfez = (ax * halfvy - ay * halfvx) + (mx * halfwy - my * halfwx);
// Compute and apply integral feedback if enabled
if(twoKi > 0.0f) {
// integral error scaled by Ki
integralFBx += twoKi * halfex * invSampleFreq;
integralFBy += twoKi * halfey * invSampleFreq;
integralFBz += twoKi * halfez * invSampleFreq;
gx += integralFBx; // apply integral feedback
gy += integralFBy;
gz += integralFBz;
} else {
integralFBx = 0.0f; // prevent integral windup
integralFBy = 0.0f;
integralFBz = 0.0f;
}
// Apply proportional feedback
gx += twoKpDef * halfex;
gy += twoKpDef * halfey;
gz += twoKpDef * halfez;
}
// Integrate rate of change of quaternion
gx *= (0.5f * invSampleFreq); // pre-multiply common factors
gy *= (0.5f * invSampleFreq);
gz *= (0.5f * invSampleFreq);
qa = q0;
qb = q1;
qc = q2;
q0 += (-qb * gx - qc * gy - q3 * gz);
q1 += (qa * gx + qc * gz - q3 * gy);
q2 += (qa * gy - qb * gz + q3 * gx);
q3 += (qa * gz + qb * gy - qc * gx);
// Normalise quaternion
recipNorm = Mahony_invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
anglesComputed = 0;
}
void Mahony_computeAngles()
{
roll = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2);
pitch = asinf(-2.0f * (q1*q3 - q0*q2));
yaw = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3);
anglesComputed = 1;
}
float getRoll() {
if (!anglesComputed) Mahony_computeAngles();
return roll * 57.29578f;
}
float getPitch() {
if (!anglesComputed) Mahony_computeAngles();
return pitch * 57.29578f;
}
float getYaw() {
if (!anglesComputed) Mahony_computeAngles();
return yaw * 57.29578f + 180.0f;
}
static float acc_x,acc_y,acc_z;
static float groy_x,groy_y,groy_z;
static float mag_x,mag_y,mag_z;
void Mahony_send_ANO(uint8_t fun,uint8_t* p,int len)
{
uint8_t buf;
int L=0;
uint8_t ver = 0;
buf = 0xAA; ver += buf;
buf = 0x05; ver += buf;
buf = 0xAF; ver += buf;
buf = fun;ver += buf;
buf = len;ver += buf;
for(int i=0;i<len;i++){
buf = p; ver += buf;
}
buf = ver;
ESB_SendBuff(buf,L);
}
void Mahony_send_ANO_STATUS(void)
{
uint8_t buf;
uint8_t L=0;
int16_t roll= (int16_t)(getRoll()*100);
int16_t pitch = (int16_t)(getPitch()*100);
int16_t yaw = (int16_t)(getYaw()*100);
buf = (uint8_t)(roll>>8);
buf = (uint8_t)(roll>>0);
buf = (uint8_t)(pitch>>8);
buf = (uint8_t)(pitch>>0);
buf = (uint8_t)(yaw>>8);
buf = (uint8_t)(yaw>>0);
buf = 0;
buf = 0;
buf = 0;
buf = 0;
buf = 0;
buf = 0;
buf = 0;
Mahony_send_ANO(0x01,buf,L);
}
void Mahony_send_ANO_SENSER(int16_t gx, int16_t gy, int16_t gz, int16_t ax, int16_t ay, int16_t az, int16_t mx, int16_t my, int16_t mz)
{
uint8_t buf;
uint8_t L=0;
buf = (uint8_t)(ax>>8);
buf = (uint8_t)(ax>>0);
buf = (uint8_t)(ay>>8);
buf = (uint8_t)(ay>>0);
buf = (uint8_t)(az>>8);
buf = (uint8_t)(az>>0);
buf = (uint8_t)(gx>>8);
buf = (uint8_t)(gx>>0);
buf = (uint8_t)(gy>>8);
buf = (uint8_t)(gy>>0);
buf = (uint8_t)(gz>>8);
buf = (uint8_t)(gz>>0);
buf = (uint8_t)(mx>>8);
buf = (uint8_t)(mx>>0);
buf = (uint8_t)(my>>8);
buf = (uint8_t)(my>>0);
buf = (uint8_t)(mz>>8);
buf = (uint8_t)(mz>>0);
Mahony_send_ANO(0x02,buf,L);
}
void Mahony_process(int16_t gx, int16_t gy, int16_t gz, int16_t ax, int16_t ay, int16_t az, int16_t mx, int16_t my, int16_t mz)
{
// Convert gyroscope degrees/sec to radians/sec
gx = gx+9; gy = gy+13; gz = gz+66;
// ax = ax+40; ay = ay-20; az = az+20;
mx = mx-200; my = my+150; mz = mz+150;
groy_x = gx*3.1415926/16.4f/180.0f;
groy_y = gy*3.1415926/16.4f/180.0f;
groy_z = gz*3.1415926/16.4f/180.0f;
// groy_x = gx*0.030517f*0.0174533f;
// groy_y = gy*0.030517f*0.0174533f;
// groy_z = gz*0.030517f*0.0174533f;
acc_x = ax/2048.0f;
acc_y = ay/2048.0f;
acc_z = az/2048.0f;
mag_x = my/1.0f;
mag_y = mx/1.0f;
mag_z = -mz/1.0f;
// mag_x = 0;
// mag_y = 0;
// mag_z = 0;
Mahony_update(groy_x,groy_y,groy_z,acc_x,acc_y,acc_z,mag_x,mag_y,mag_z);
Mahony_send_ANO_STATUS();
Mahony_send_ANO_SENSER(gx,gy,gz,ax,ay,az,mx,my,mz);
}
//============================================================================================
// END OF CODE
//============================================================================================
搞定了，算法没问题，问题出在182行
return yaw * 57.29578f + 180.0f;改为 return yaw * 57.29578f; jssd 发表于 2021-1-21 10:30
搞定了，算法没问题，问题出在182行
return yaw * 57.29578f + 180.0f;改为 return yaw * 57.29578f; ...
流弊极了！ 问下lz用的上位机是哪款软件 jssd 发表于 2021-1-21 10:30
搞定了，算法没问题，问题出在182行
return yaw * 57.29578f + 180.0f;改为 return yaw * 57.29578f; ...
(引用自2楼)
完整的代码，可以分享吗？
页:
[1]