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：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版（此版本卡拉曼滤波奇点有错误）相关推荐

STM32入门笔记(02)：MPU6050、MPU9250、ICM20948及姿态解算（SPL库函数版)
目录 MPU6050 什么是MPU6050? MPU6050的特点 MPU6050框图 MPU6050初始化 MPU6050寄存器电源管理寄存器1(0X6B) 陀螺仪配置寄存器(0X1B) 加速度传 ...
四元数AHRS姿态解算和IMU姿态解算分析
ref:https://blog.csdn.net/xiaoxie613520/article/details/78227170 AHRS是自动航向基准系统(Automatic Heading Ref ...
imu matlab,IMU姿态解算matlab
[实例简介] IMU姿态解算matlabIMU姿态解算matlabIMU姿态解算matlab [实例截图] [核心代码] GaitTrackingWithx-IMU └── Gait Tracking ...
ADIS16465姿态解算+卡尔曼滤波代码
工程介绍工程使用stm32f407开发,imu单元为ADIS16465-1 BMLZ,通过spi通信读取imu的输出寄存器,对读取到的三轴加速度值和三轴陀螺仪值利用四元数进行姿态解算,最后得到pit ...
关于姿态解算与融合的代码注释篇（三）
https://blog.csdn.net/sinat_23338865/article/details/53886114 加速度计和陀螺仪都能计算出姿态,但为何要对它们融合呢,是因为加速度计对振动之 ...
无人机姿态解算_扩展卡尔曼滤波（2）
一.扩展卡尔曼滤波 KF和EKF的公式对比(基本没差别) 二.扩展卡尔曼五个公式利用扩展卡尔曼滤波估计四元数. 下图是论文中的截图.可以和前面的卡尔曼滤波估计高度文章的那五个公式对应一下. 观测矩阵 ...
matlab求逆矩阵_MPU6050姿态解算2-欧拉角amp;旋转矩阵
1 IMU姿态解算 IMU,即惯性测量单元,一般包含三轴陀螺仪与三轴加速度计.之前的文章码农爱学习:MPU6050姿态解算方式1-DMPzhuanlan.zhihu.com 已将对MPU6050这 ...
stm32 MPU6050 姿态解算 Mahony互补滤波算法
文章目录 0.介绍 1,理论分析 1.1 MPU6050 1.2 Mahony算法原理 2,代码实现 1.1 MPU6050初始化及数据读取 1.2 Mahony算法c语言实现 1.3 将代码移植到你 ...
姿态解算知识（三）-陀螺仪加速度计6轴数据融合
这么久的惯导总算是没白看,加上一篇博客的指点,这两天把Mahony的九轴数据融合算法看懂了.可惜第二版硬件还没到,磁力计用不了,没法验证效果~今天先总结下陀螺仪和加速度计的六轴数据融合. 版权声明原 ...

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

原版地址：IMU9轴卡尔曼滤波

增加mpu6050 陀螺仪零飘矫正，imu算法优化

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

最新文章

热门文章