UM6传感器的互补滤波器问题。 （偏航不过滤） [英] Problem with Complementary filter for UM6 sensor. (yaw does not filter )

问题描述

Data.Roll -= ((float)Data.Gyro_Raw[0] / GYROSCOPE_SENSITIVITY) * dt;    // Angle around the Y-axis

Data.Pitch += ((float)Data.Gyro_Raw[1] / GYROSCOPE_SENSITIVITY) * dt; // Angle around the X-axis

Data.Yaw += ((float)Data.Gyro_Raw[2] / GYROSCOPE_SENSITIVITY) * dt;

									 
roll =     sqrt(((float)Data.Accel_Raw[0]*(float)Data.Accel_Raw[0])+((float)Data.Accel_Raw[2]*(float)Data.Accel_Raw[2]));

pitch = sqrt(((float)Data.Accel_Raw[1]*(float)Data.Accel_Raw[1])+((float)Data.Accel_Raw[2]*(float)Data.Accel_Raw[2]));

yaw = sqrt(((float)Data.Accel_Raw[0]*(float)Data.Accel_Raw[0])+((float)Data.Accel_Raw[1]*(float)Data.Accel_Raw[1]));
									
int forceMagnitudeApprox = abs(Data.Accel_Raw[0]) + abs(Data.Accel_Raw[1]) + abs(Data.Accel_Raw[2]);
if (forceMagnitudeApprox > 8192 && forceMagnitudeApprox < 32768)
	{

// Turning around the Y axis results in a vector on the X-axis
rollAcc = atan2f((float)Data.Accel_Raw[1], roll) * 180 / M_PI;
Data.Roll = (Data.Roll) * 0.98f + rollAcc * 0.02f;
cout <<Data.Roll<<"\t";
				
// Turning around the X axis results in a vector on the Y-axis
pitchAcc = atan2f((float)Data.Accel_Raw[0], pitch) * 180 / M_PI;
Data.Pitch = (Data.Pitch) * 0.98f + pitchAcc * 0.02f;
cout <<Data.Pitch<<"\t";

yawAcc = atan2f(yaw, (float)Data.Accel_Raw[2]) * 180 / M_PI;
Data.Yaw = (Data.Yaw) * 0.98f + yawAcc * 0.02f;
cout <<Data.Yaw<<"\t\n";
				
}

加速计传感器的灵敏度为1 mg / LSB +/- 2g

Accelerometer sensor has sensitivity 1 mg/LSB with +/- 2g

推荐答案

这篇关于UM6传感器的互补滤波器问题。 （偏航不过滤）的文章就介绍到这了，希望我们推荐的答案对大家有所帮助，也希望大家多多支持IT屋！