欢迎您访问程序员文章站本站旨在为大家提供分享程序员计算机编程知识!
您现在的位置是: 首页

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

程序员文章站 2022-07-12 10:19:32
...

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

相关标签: JAVA