imu姿态解算+卡尔曼滤波融合JAVA版（此版本卡拉曼滤波奇点有错误）

原版地址：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];
            }
        }
    }
}