MPU6050与卡尔曼滤波算法实践(MPU6050sketch01.ino)
#include "Wire.h"#include "I2Cdev.h"#include "MPU6050.h" MPU6050 accelgyro; 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轴卡尔曼变量 void setup(){ Wire.begin(); Serial.begin(115200); accelgyro.initialize(); //初始化 unsigned short times = 200; //采样次数 for(int i=0;i
- 2019-12-11下载
- 积分：1
|
联系客服（QQ：3324461878）
