3781
- 收藏
- 点赞
- 分享
- 举报
使用了网上的姿态融合算法,波动好大,求指导
我用的是arduino mega2560 和一块10 DOF IMU sensor,上面集成了MPU6050和HMC5883L,拷入代码后做了实验,pitch角从0摆到45度再恢复水平,得到的数据绘图如下
波动太大,请问各位大神我的代码有什么问题呢?代码附上
[code]
#include
#include
#include
#include
#include
#include "I2Cdev.h"
#include "MPU6050.h"
#include"math.h"
#define Kp 2.0f
#define Ki 0.001f
#define halfT 0.3f
//----------------------------------------------------------------------------------------------------
// Variable definitions
float q0 = 1, q1 = 0, q2 = 0, q3 = 0;
float exInt = 0, eyInt = 0, ezInt = 0,pitch,yaw;
const int SampleTime=600;
MPU6050 accelgyro;
int ax,ay,az;
int16_t gx,gy,gz;
float angles[3]; // yaw pitch roll
float heading,mx,my,mz;
float Ax,Ay,Az,Gx,Gy,Gz;
short temperature;
long pressure;
// Set the FreeSixIMU object
FreeSixIMU sixDOF = FreeSixIMU();
HMC5883L compass;
// Record any errors that may occur in the compass.
int error = 0;
void setup(){
Serial.begin(115200);
Wire.begin();
delay(5);
sixDOF.init(); //init the Acc and Gyro
delay(5);
compass = HMC5883L(); // init HMC5883
accelgyro.initialize();
error = compass.SetScale(1.3); // Set the scale of the compass.
error = compass.SetMeasurementMode(Measurement_Continuous); // Set the measurement mode to Continuous
if(error != 0) // If there is an error, print it out.
Serial.println(compass.GetErrorText(error));
bmp085Calibration(); // init barometric pressure sensor
}
void loop(){
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
Ax=(float)ax/16384;
Ay=(float)ay/16384;
Az=(float)az/16384;
Gx=(gx/32.8+6)/57.3;
Gy=(gy/32.8-5)/57.3;
Gz=(gz/32.8-155)/57.3;
temperature = bmp085GetTemperature(bmp085ReadUT());
pressure = bmp085GetPressure(bmp085ReadUP());
MagnetometerRaw raw = compass.ReadRawAxis();
MagnetometerScaled scaled = compass.ReadScaledAxis();
mx=scaled.XAxis;
my=scaled.YAxis;
mz=scaled.ZAxis;
AHRSupdate(Gx,Gy,Gz,Ax,Ay,Az,mx,my,mz);
PrintData();
delay(0);
}
void PrintData(){
Serial.print(pitch);
Serial.println(" ");
}
void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
unsigned long now,timeChange;
static unsigned long lastTime=0;
now = micros(); //us
timeChange =now - lastTime;
if(timeChange>=SampleTime){
lastTime = now;
float norm;
float hx, hy, hz, bx, bz;
float vx, vy, vz, wx, wy, wz;
float ex, ey, ez,lastex,lastey,lastez;
float q0q0 = q0*q0;
float q0q1 = q0*q1;
float q0q2 = q0*q2;
float q0q3 = q0*q3;
float q1q1 = q1*q1;
float q1q2 = q1*q2;
float q1q3 = q1*q3;
float q2q2 = q2*q2;
float q2q3 = q2*q3;
float q3q3 = q3*q3;
norm = sqrt(ax*ax + ay*ay + az*az);
ax = ax / norm;
ay = ay / norm;
az = az / norm;
norm = sqrt(mx*mx + my*my + mz*mz);
mx = mx / norm;
my = my / norm;
mz = mz / norm;
hx = 2*mx*(0.5 - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);
hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5 - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);
hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5 - q1q1 - q2q2);
bx = sqrt((hx*hx) + (hy*hy));
bz = hz;
vx = 2*(q1q3 - q0q2);
vy = 2*(q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3;
wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);
wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);
wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2);
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);
exInt = exInt + ex*Ki;
eyInt = eyInt + ey*Ki;
ezInt = ezInt + ez*Ki;
gx = gx + Kp*ex + exInt;
gy = gy + (Kp*ey + eyInt);
gz = gz + (Kp*ez + ezInt);
q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;
norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 = q0 / norm;
q1 = q1 / norm;
q2 = q2 / norm;
q3 = q3 / norm;
yaw= atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3;
pitch= asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;
}
}
[/code]
我来回答
回答0个
时间排序
认可量排序
暂无数据
或将文件直接拖到这里
悬赏:
E币
网盘
* 网盘链接:
* 提取码:
悬赏:
E币
Markdown 语法
- 加粗**内容**
- 斜体*内容*
- 删除线~~内容~~
- 引用> 引用内容
- 代码`代码`
- 代码块```编程语言↵代码```
- 链接[链接标题](url)
- 无序列表- 内容
- 有序列表1. 内容
- 缩进内容
- 图片![alt](url)
相关问答
-
2015-10-02 22:23:39
-
2020-06-02 16:38:37
-
2019-10-21 17:49:27
-
2015-10-01 23:58:16
-
2013-12-14 14:18:08
-
2013-12-01 13:31:47
-
2013-12-01 12:22:15
-
2019-11-07 16:07:01
-
12019-11-15 15:37:24
-
2013-06-07 10:59:06
-
12022-05-31 17:04:40
-
2013-12-01 23:38:10
-
2018-12-28 15:56:49
-
2018-12-28 16:38:16
-
2018-12-11 16:16:56
-
2018-07-30 16:23:25
-
2017-08-29 11:08:47
-
2019-09-20 16:37:47
-
2018-09-29 10:59:34
无更多相似问答 去提问
点击登录
-- 积分
-- E币
提问
—
收益
—
被采纳
—
我要提问
切换马甲
上一页
下一页
悬赏问答
-
50如何获取vpss chn的图像修改后发送至vo
-
5FPGA通过Bt1120传YUV422数据过来,vi接收不到数据——3516dv500
-
50SS928 运行PQtools 拼接 推到设备里有一半画面会异常
-
53536AV100的sample_vdec输出到CVBS显示
-
10海思板子mpp怎么在vi阶段改变视频数据尺寸
-
10HI3559AV100 多摄像头同步模式
-
9海思ss928单路摄像头vio中加入opencv处理并显示
-
10EB-RV1126-BC-191板子运行自己编码的程序
-
10求HI3519DV500_SDK_V2.0.1.1
-
5有偿求HI3516DV500 + OV5647驱动
举报反馈
举报类型
- 内容涉黄/赌/毒
- 内容侵权/抄袭
- 政治相关
- 涉嫌广告
- 侮辱谩骂
- 其他
详细说明
提醒
你的问题还没有最佳答案,是否结题,结题后将扣除20%的悬赏金
取消
确认
提醒
你的问题还没有最佳答案,是否结题,结题后将根据回答情况扣除相应悬赏金(1回答=1E币)
取消
确认