原版地址：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：5mkalman = 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; // yawam_angle_mat[4] = (float) Math.asin(-2 * q1q3 + 2 * q0q2) * 57.3f; // pitcham_angle_mat[8] = (float) Math.atan2(2 * q2q3 + 2 * q0q1, -2 * q1q1 - 2 * q2q2 + 1) * 57.3f; // rollgyro_angle_mat[0] = gy;gyro_angle_mat[4] = -gx;gyro_angle_mat[8] = -gz;mkalman.KalmanFilter(am_angle_mat, gyro_angle_mat);//角度储存在mkalman.xkLog.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 + ukmatrix_add(xk, gyro_angle_mat, xk);//pk = pk + Qmatrix_add(pk, Q, pk);//yk = xnew - xkmatrix_sub(am_angle_mat, xk, yk);//S=Pk + Rmatrix_add(pk, R, S);//S求逆invertsdet = 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_invertmatrix_multi(pk, S_invert, K);//KxYk = K * Ykmatrix_multi(K, yk, KxYk);//xk = xk + K * ykmatrix_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
如果觉得《imu姿态解算+卡尔曼滤波融合JAVA版（此版本卡拉曼滤波奇点有错误）》对你有帮助，请点赞、收藏，并留下你的观点哦！
