原版地址：IMU9轴卡尔曼滤波
增加mpu6050 陀螺仪零飘矫正，imu算法优化
KalmAndAndIMU 类：
import java.util.Vector;
public class KalmAndAndIMU {
kalman mkalman;
float[] am_angle_mat = {0, 0, 0, 0, 0, 0, 0, 0, 0};
float[] gyro_angle_mat = {0, 0, 0, 0, 0, 0, 0, 0, 0};
float q0 = 1, q1 = 0, q2 = 0, q3 = 0; // 四元数
float halfT = 0.5f, upTime = 0.01f, GryLevel = 1880;
float[] GryDrifts = new float[3], OldGryData = new float[3];
float[] gryeData = new float[]{0, 0, 0};
short grycount, errorData, maxAcc, minAcc;
// private ReadWriteLock AngleReadWriteLock = new ReentrantReadWriteLock();
float q0q0 = 1, q1q1 = 0, q2q2 = 0, q3q3 = 0, q0q1 = 0, q2q3 = 0, q1q3 = 0, q0q2 = 0, q1q2 = 0, q0q3 = 0;
public KalmanAndIMU(short acc, short gry, short level) {
//level 0：5
mkalman = new kalman();
switch (level) {
case 2:
upTime = 0.025f;
break;
case 3:
upTime = 0.01666f;
break;
case 4:
upTime = 0.0125f;
break;
case 5:
upTime = 0.01f;
break;
case 6:
upTime = 0.005f;
break;
default:
upTime = 0.05f;
break;
}
switch (acc) {
case 0:
minAcc = 14800;
maxAcc = 18000;
break;
case 1:
minAcc = 7400;
maxAcc = 9000;
break;
case 2:
minAcc = 3700;
maxAcc = 4500;
break;
default:
minAcc = 1850;
maxAcc = 2250;
break;
}
switch (gry) {
case 0:
GryLevel = 7520;
errorData = 100;
break;
case 1:
GryLevel = 3760;
errorData = 50;
break;
case 2:
GryLevel = 1880;
errorData = 25;
break;
default:
GryLevel = 940;
errorData = 13;
break;
}
}
public synchronized void Uupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
float acc_norm = (float) Math.sqrt(ax * ax + ay * ay + az * az); //acc数据归一化
if (Math.abs(OldGryData[0] - gx) < errorData && Math.abs(OldGryData[1] - gy) < errorData && Math.abs(OldGryData[2] - gz) < errorData && grycount < 5000) {
gryeData[0] += gx;
gryeData[1] += gy;
gryeData[2] += gz;
if (++grycount > 30) {
GryDrifts[0] = -gryeData[0] / grycount;
GryDrifts[1] = -gryeData[1] / grycount;
GryDrifts[2] = -gryeData[2] / grycount;
}
} else {
grycount = 0;
gryeData[0] = 0;
gryeData[1] = 0;
gryeData[2] = 0;
}
OldGryData[0] = gx;
OldGryData[1] = gy;
OldGryData[2] = gz;
if (acc_norm != 0) {
gx += GryDrifts[0];
gy += GryDrifts[1];
gz += GryDrifts[2];
gx = gx / GryLevel;
gy = gy / GryLevel;
gz = gz / GryLevel;
ax = ax / acc_norm;
ay = ay / acc_norm;
az = az / acc_norm;
q0 = (-q1 * gx - q2 * gy - q3 * gz) * upTime + q0;
q1 = (q0 * gx + q2 * gz - q3 * gy) * upTime + q1;
q2 = (q0 * gy - q1 * gz + q3 * gx) * upTime + q2;
q3 = (q0 * gz + q1 * gy - q2 * gx) * upTime + q3;
float ex = 0, ey = 0, ez = 0;
float vx, vy, vz;
vx = 2 * (q1q3 - q0q2); //四元素中xyz的表示
vy = 2 * (q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3;
if (acc_norm > minAcc && acc_norm < maxAcc) {
ex = (ay * vz - az * vy); //向量外积在相减得到差分就是误差
ey = (az * vx - ax * vz);
ez = (ax * vy - ay * vx);
}
//注意！！！！以下内容默认地磁已用求圆算法矫正
float mag_norm = (float) Math.sqrt(mx * mx + my * my + mz * mz);
float hx, hy, hz, bx, bz;
float wx, wy, wz;
Log.d("mMagDrifts", "Uupdate: " + mx + ":::" + my + ":::" + mz + ":::" + mag_norm);
mx = mx / mag_norm;
my = my / mag_norm;
mz = mz / mag_norm;
hx = 2 * mx * (0.5f - q2q2 - q3q3) + 2 * my * (q1q2 - q0q3) + 2 * mz * (q1q3 + q0q2);
hy = 2 * mx * (q1q2 + q0q3) + 2 * my * (0.5f - q1q1 - q3q3) + 2 * mz * (q2q3 - q0q1);
hz = 2 * mx * (q1q3 - q0q2) + 2 * my * (q2q3 + q0q1) + 2 * mz * (0.5f - q1q1 - q2q2);
bx = (float) Math.sqrt((hx * hx) + (hy * hy));
bz = hz;
wx = 2 * bx * (0.5f - q2q2 - q3q3) + 2 * bz * (q1q3 - q0q2);
wy = 2 * bx * (q1q2 - q0q3) + 2 * bz * (q0q1 + q2q3);
wz = 2 * bx * (q0q2 + q1q3) + 2 * bz * (0.5f - q1q1 - q2q2);
ex += (my * wz - mz * wy);
ey += (mz * wx - mx * wz);
ez += (mx * wy - my * wx);
q0 = q0 + (-q1 * ex - q2 * ey - q3 * ez) * halfT;
q1 = q1 + (q0 * ex + q2 * ez - q3 * ey) * halfT;
q2 = q2 + (q0 * ey - q1 * ez + q3 * ex) * halfT;
q3 = q3 + (q0 * ez + q1 * ey - q2 * ex) * halfT;
float q_norm = (float) Math.sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 = q0 / q_norm;
q1 = q1 / q_norm;
q2 = q2 / q_norm;
q3 = q3 / q_norm;
q0q0 = q0 * q0;
q0q3 = q0 * q3;
q1q1 = q1 * q1;
q2q2 = q2 * q2;
q3q3 = q3 * q3;
q0q1 = q0 * q1;
q2q3 = q2 * q3;
q1q3 = q1 * q3;
q0q2 = q0 * q2;
q1q2 = q1 * q2;
am_angle_mat[0] = (float) Math.atan2(2 * (q1q2 + q0q3), q0q0 + q1q1 - q2q2 - q3q3) * 57.3f; // yaw
am_angle_mat[4] = (float) Math.asin(-2 * q1q3 + 2 * q0q2) * 57.3f; // pitch
am_angle_mat[8] = (float) Math.atan2(2 * q2q3 + 2 * q0q1, -2 * q1q1 - 2 * q2q2 + 1) * 57.3f; // roll
gyro_angle_mat[0] = gy;
gyro_angle_mat[4] = -gx;
gyro_angle_mat[8] = -gz;
mkalman.KalmanFilter(am_angle_mat, gyro_angle_mat);
//角度储存在mkalman.xk
Log.d("kalman", "Uupdate: yaw:" + mkalman.xk[0] + " roll:" + mkalman.xk[4] + " pitch:" + mkalman.xk[8]);
}
}
}
kalman类：
public class kalman {
//kalman.c
// float dtTimer = 0.008f;
public float[] xk = {0, 0, 0, 0, 0, 0, 0, 0, 0};
float[] pk = {1, 0, 0, 0, 1, 0, 0, 0, 0};
float[] I = {1, 0, 0, 0, 1, 0, 0, 0, 1};
float[] R = {0.5f, 0, 0, 0, 0.5f, 0, 0, 0, 0.01f};
float[] Q = {0.005f, 0, 0, 0, 0.005f, 0, 0, 0, 0.001f};
void matrix_add(float[] mata, float[] matb, float[] matc) {
short i, j;
for (i = 0; i < 3; i++) {
for (j = 0; j < 3; j++) {
matc[i * 3 + j] = mata[i * 3 + j] + matb[i * 3 + j];
}
}
}
void matrix_sub(float[] mata, float[] matb, float[] matc) {
short i, j;
for (i = 0; i < 3; i++) {
for (j = 0; j < 3; j++) {
matc[i * 3 + j] = mata[i * 3 + j] - matb[i * 3 + j];
}
}
}
void matrix_multi(float[] mata, float[] matb, float[] matc) {
int i, j, m;
for (i = 0; i < 3; i++) {
for (j = 0; j < 3; j++) {
matc[i * 3 + j] = 0;
for (m = 0; m < 3; m++) {
matc[i * 3 + j] += mata[i * 3 + m] * matb[m * 3 + j];
}
}
}
}
public void KalmanFilter(float[] am_angle_mat, float[] gyro_angle_mat) {
int i, j;
float[] yk = new float[9];
float[] pk_new = new float[9];
float[] K = new float[9];
float[] KxYk = new float[9];
float[] I_K = new float[9];
float[] S = new float[9];
float[] S_invert = new float[9];
float sdet;
//xk = xk + uk
matrix_add(xk, gyro_angle_mat, xk);
//pk = pk + Q
matrix_add(pk, Q, pk);
//yk = xnew - xk
matrix_sub(am_angle_mat, xk, yk);
//S=Pk + R
matrix_add(pk, R, S);
//S求逆invert
sdet = S[0] * S[4] * S[8]
+ S[1] * S[5] * S[6]
+ S[2] * S[3] * S[7]
- S[2] * S[4] * S[6]
- S[5] * S[7] * S[0]
- S[8] * S[1] * S[3];
S_invert[0] = (S[4] * S[8] - S[5] * S[7]) / sdet;
S_invert[1] = (S[2] * S[7] - S[1] * S[8]) / sdet;
S_invert[2] = (S[1] * S[7] - S[4] * S[6]) / sdet;
S_invert[3] = (S[5] * S[6] - S[3] * S[8]) / sdet;
S_invert[4] = (S[0] * S[8] - S[2] * S[6]) / sdet;
S_invert[5] = (S[2] * S[3] - S[0] * S[5]) / sdet;
S_invert[6] = (S[3] * S[7] - S[4] * S[6]) / sdet;
S_invert[7] = (S[1] * S[6] - S[0] * S[7]) / sdet;
S_invert[8] = (S[0] * S[4] - S[1] * S[3]) / sdet;
//K = Pk * S_invert
matrix_multi(pk, S_invert, K);
//KxYk = K * Yk
matrix_multi(K, yk, KxYk);
//xk = xk + K * yk
matrix_add(xk, KxYk, xk);
//pk = (I - K)*(pk)
matrix_sub(I, K, I_K);
matrix_multi(I_K, pk, pk_new);
//update pk
//pk = pk_new;
for (i = 0; i < 3; i++) {
for (j = 0; j < 3; j++) {
pk[i * 3 + j] = pk_new[i * 3 + j];
}
}
}
}
Cocos+u3d开发交流群：521643513
扫一扫
1787
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
