前言：
此时姐妹们对“卡尔曼滤波 arduino”可能比较注重，兄弟们都想要剖析一些“卡尔曼滤波 arduino”的相关知识。那么小编同时在网摘上搜集了一些有关“卡尔曼滤波 arduino””的相关资讯，希望你们能喜欢，姐妹们一起来了解一下吧！1 连线图示
Arduino uno
MPU6050
5V端口
VCC
GND
GND
A5
SCL
A4
SDA
2 实物图片
3 程序
#include "Wire.h"#include "I2Cdev.h"#include "MPU6050.h" MPU6050 accelgyro;unsigned long time;unsigned long now, lastTime = 0;float dt; //微分时间 int16_t ax, ay, az, gx, gy, gz; //加速度计陀螺仪原始数据float aax=0, aay=0,aaz=0, agx=0, agy=0, agz=0; //角度变量long axo = 0, ayo = 0, azo = 0; //加速度计偏移量long gxo = 0, gyo = 0, gzo = 0; //陀螺仪偏移量 float pi = 3.1415926;float AcceRatio = 16384.0; //加速度计比例系数float GyroRatio = 131.0; //陀螺仪比例系数 uint8_t n_sample = 8; //加速度计滤波算法采样个数float aaxs[8] = {0}, aays[8] = {0}, aazs[8] = {0}; //x,y轴采样队列long aax_sum, aay_sum,aaz_sum; //x,y轴采样和 float a_x[10]={0}, a_y[10]={0},a_z[10]={0} ,g_x[10]={0} ,g_y[10]={0},g_z[10]={0}; //加速度计协方差计算队列float Px=1, Rx, Kx, Sx, Vx, Qx; //x轴卡尔曼变量float Py=1, Ry, Ky, Sy, Vy, Qy; //y轴卡尔曼变量float Pz=1, Rz, Kz, Sz, Vz, Qz; //z轴卡尔曼变量 const int MPU = 0x68; // MPU6050 I2C addressfloat AccX, AccY, AccZ;float GyroX, GyroY, GyroZ;float accAngleX, accAngleY, gyroAngleX, gyroAngleY, gyroAngleZ;float roll, pitch, yaw;float AccErrorX, AccErrorY, GyroErrorX, GyroErrorY, GyroErrorZ;float elapsedTime, currentTime, previousTime;int c = 0;int data1=0;void setup(){ time = millis(); Serial.begin(19200); { Serial.println(); Serial.print("please dont move ur mpu6050 in 5 secs"); } Wire.begin(); // Initialize comunication Wire.beginTransmission(MPU); // Start communication with MPU6050 // MPU=0x68 Wire.write(0x6B); // Talk to the register 6B Wire.write(0x00); // Make reset - place a 0 into the 6B register Wire.endTransmission(true); //end the transmission accelgyro.initialize(); //初始化 unsigned short times = 200; //采样次数 for(int i=0;i<times;i++) { accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); //读取六轴原始数值 axo += ax; ayo += ay; azo += az; //采样和 gxo += gx; gyo += gy; gzo += gz; } axo /= times; ayo /= times; azo /= times; //计算加速度计偏移 gxo /= times; gyo /= times; gzo /= times; //计算陀螺仪偏移} void loop(){ float a; float b; time = millis(); unsigned long now = millis(); //当前时间(ms) dt = (now - lastTime) / 1000.0; //微分时间(s) lastTime = now; //上一次采样时间(ms) accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); //读取六轴原始数值 float accx = ax / AcceRatio; //x轴加速度 float accy = ay / AcceRatio; //y轴加速度 float accz = az / AcceRatio; //z轴加速度 aax = atan(accy / accz) * (-180) / pi; //y轴对于z轴的夹角 aay = atan(accx / accz) * 180 / pi; //x轴对于z轴的夹角 aaz = atan(accz / accy) * 180 / pi; //z轴对于y轴的夹角 aax_sum = 0; // 对于加速度计原始数据的滑动加权滤波算法 aay_sum = 0; aaz_sum = 0; for(int i=1;i<n_sample;i++) { aaxs[i-1] = aaxs[i]; aax_sum += aaxs[i] * i; aays[i-1] = aays[i]; aay_sum += aays[i] * i; } aaxs[n_sample-1] = aax; aax_sum += aax * n_sample; aax = (aax_sum / (11*n_sample/2.0)) * 9 / 7.0; //角度调幅至0-90° aays[n_sample-1] = aay; //此处应用实验法取得合适的系数 aay_sum += aay * n_sample; //本例系数为9/7 aay = (aay_sum / (11*n_sample/2.0)) * 9 / 7.0; float gyrox = - (gx-gxo) / GyroRatio * dt; //x轴角速度 float gyroy = - (gy-gyo) / GyroRatio * dt; //y轴角速度 agx += gyrox; //x轴角速度积分 agy += gyroy; //x轴角速度积分 /* kalman start */ Sx = 0; Rx = 0; Sy = 0; Ry = 0; Sz = 0; Rz = 0; for(int i=1;i<10;i++) { //测量值平均值运算 a_x[i-1] = a_x[i]; //即加速度平均值 Sx += a_x[i]; a_y[i-1] = a_y[i]; Sy += a_y[i]; } a_x[9] = aax; Sx += aax; Sx /= 10; //x轴加速度平均值 a_y[9] = aay; Sy += aay; Sy /= 10; //y轴加速度平均值 for(int i=0;i<10;i++) { Rx += sq(a_x[i] - Sx); Ry += sq(a_y[i] - Sy); Rz += sq(a_z[i] - Sz); } Rx = Rx / 9; //得到方差 Ry = Ry / 9; Px = Px + 0.0025; // 0.0025在下面有说明... Kx = Px / (Px + Rx); //计算卡尔曼增益 agx = agx + Kx * (aax - agx); //陀螺仪角度与加速度计速度叠加 Px = (1 - Kx) * Px; //更新p值 Py = Py + 0.0025; Ky = Py / (Py + Ry); agy = agy + Ky * (aay - agy); Py = (1 - Ky) * Py; // === 读取加速度计数据 === // Wire.beginTransmission(MPU); Wire.write(0x3B); // Start with register 0x3B (ACCEL_XOUT_H) Wire.endTransmission(false); Wire.requestFrom(MPU, 6, true); // Read 6 registers total, each axis value is stored in 2 registers //For a range of +-2g, we need to divide the raw values by 16384, according to the datasheet AccX = (Wire.read() << 8 | Wire.read()) / 16384.0; // X-axis value AccY = (Wire.read() << 8 | Wire.read()) / 16384.0; // Y-axis value AccZ = (Wire.read() << 8 | Wire.read()) / 16384.0; // Z-axis value // Calculating Roll and Pitch from the accelerometer data accAngleX = (atan(AccY / sqrt(pow(AccX, 2) + pow(AccZ, 2))) * 180 / PI) - 0.58; // AccErrorX ~(0.58) See the calculate_IMU_error()custom function for more details accAngleY = (atan(-1 * AccX / sqrt(pow(AccY, 2) + pow(AccZ, 2))) * 180 / PI) + 1.58; // AccErrorY ~(-1.58) // === 读取重力加速度计 === // previousTime = currentTime; // Previous time is stored before the actual time read currentTime = millis(); // Current time actual time read elapsedTime = (currentTime - previousTime) / 1000; // Divide by 1000 to get seconds Wire.beginTransmission(MPU); Wire.write(0x43); // Gyro data first register address 0x43 Wire.endTransmission(false); Wire.requestFrom(MPU, 6, true); GyroX = (Wire.read() << 8 | Wire.read()) / 131.0; GyroY = (Wire.read() << 8 | Wire.read()) / 131.0; GyroZ = (Wire.read() << 8 | Wire.read()) / 131.0; GyroZ = GyroZ; if (time<=3000){ //获得第3秒的数据 yaw = yaw + GyroZ * elapsedTime; a = yaw; } if (time<=2000){ //获得第2秒的数据 b = yaw; } if (time>3000){ yaw = yaw + (GyroZ - (a-b)) * elapsedTime; //修正公式 Serial.print(agx,1);Serial.print("/"); //输出x轴数据 Serial.print(agy,1);Serial.print("/"); //输出y轴数据 Serial.println(yaw,1); //输出y轴数据 delay(50); } if (time>=2000, time<=3000){ Serial.print(a,5); Serial.println(b,5); //输出前3秒的数据 delay (5); } else{ }}//作者:MrPumpkinsss//代码前部分基于Techblog of HaoWANG修改的 >>> 4 串口获取数据
版权声明：
本站文章均来自互联网搜集，如有侵犯您的权益，请联系我们删除，谢谢。
标签： #卡尔曼滤波 arduino
