久久国产成人av_抖音国产毛片_a片网站免费观看_A片无码播放手机在线观看,色五月在线观看,亚洲精品m在线观看,女人自慰的免费网址,悠悠在线观看精品视频,一级日本片免费的,亚洲精品久,国产精品成人久久久久久久

分享

MPU6050數(shù)據(jù)采集及其意義和濾波(一階互補濾波、二階互補濾波,、卡爾曼濾波)

 dwlinux_gs 2014-08-25
 本帖最后由 275891381 于 2014-6-23 14:21 編輯

1. MPU6050陀螺儀

// 陀螺儀
float angleAx,gyroGy;
MPU6050 accelgyro;
int16_t ax, ay, az, gx, gy, gz;

    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);//原始數(shù)據(jù)采集
    angleAx=atan2(ax,az)*180/PI;//加速度計算角度
    gyroGy=-gy/131.00;//陀螺儀角速度,,注意正負號與放置有關(guān)


2.濾波參數(shù)及函數(shù)
//一階互補濾波
float K1 =0.05; // 對加速度計取值的權(quán)重
float dt=20*0.001;//注意:dt的取值為濾波器采樣時間
float angle1;

void Yijielvbo(float angle_m, float gyro_m)//采集后計算的角度和角加速度
{
     angle1 = K1 * angle_m+ (1-K1) * (angle1 + gyro_m * dt);
}

//二階互補濾波
float K2 =0.2; // 對加速度計取值的權(quán)重
float x1,x2,y1;
float dt=20*0.001;//注意:dt的取值為濾波器采樣時間
float angle2;

void Erjielvbo(float angle_m,float gyro_m)//采集后計算的角度和角加速度
{
    x1=(angle_m-angle2)*(1-K2)*(1-K2);
    y1=y1+x1*dt;
    x2=y1+2*(1-K2)*(angle_m-angle2)+gyro_m;
    angle2=angle2+ x2*dt;
}


//卡爾曼濾波參數(shù)與函數(shù)
float dt=0.005;//注意:dt的取值為kalman濾波器采樣時間
float angle, angle_dot;//角度和角速度
float P[2][2] = {{ 1, 0 },
              { 0, 1 }};
float Pdot[4] ={ 0,0,0,0};
float Q_angle=0.001, Q_gyro=0.005; //角度數(shù)據(jù)置信度,角速度數(shù)據(jù)置信度
float R_angle=0.5 ,C_0 = 1;
float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;


//卡爾曼濾波
void Kalman_Filter(float angle_m, float gyro_m)//angleAx 和 gyroGy
{
angle+=(gyro_m-q_bias) * dt;
angle_err = angle_m - angle;
Pdot[0]=Q_angle - P[0][1] - P[1][0];
Pdot[1]=- P[1][1];
Pdot[2]=- P[1][1];
Pdot[3]=Q_gyro;
P[0][0] += Pdot[0] * dt;
P[0][1] += Pdot[1] * dt;
P[1][0] += Pdot[2] * dt;
P[1][1] += Pdot[3] * dt;
PCt_0 = C_0 * P[0][0];
PCt_1 = C_0 * P[1][0];
E = R_angle + C_0 * PCt_0;
K_0 = PCt_0 / E;
K_1 = PCt_1 / E;
t_0 = PCt_0;
t_1 = C_0 * P[0][1];
P[0][0] -= K_0 * t_0;
P[0][1] -= K_0 * t_1;
P[1][0] -= K_1 * t_0;
P[1][1] -= K_1 * t_1;
angle += K_0 * angle_err; //最優(yōu)角度
q_bias += K_1 * angle_err;
angle_dot = gyro_m-q_bias;//最優(yōu)角速度
}


3 三種濾波比較源代碼 //
mpu6050.png
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
#include "Timer.h"http://時間操作系統(tǒng)頭文件  本程序用作timeChange時間采集并處理一次數(shù)據(jù)

Timer t;//時間類

float timeChange=20;//濾波法采樣時間間隔毫秒
float dt=timeChange*0.001;//注意:dt的取值為濾波器采樣時間
// 陀螺儀
float angleAx,gyroGy;//計算后的角度(與x軸夾角)和角速度
MPU6050 accelgyro;//陀螺儀類
int16_t ax, ay, az, gx, gy, gz;//陀螺儀原始數(shù)據(jù) 3個加速度+3個角速度

//一階濾波
float K1 =0.05; // 對加速度計取值的權(quán)重
//float dt=20*0.001;//注意:dt的取值為濾波器采樣時間
float angle1;//一階濾波角度輸出
//二階濾波
float K2 =0.2; // 對加速度計取值的權(quán)重
float x1,x2,y1;//運算中間變量
//float dt=20*0.001;//注意:dt的取值為濾波器采樣時間
float angle2;//er階濾波角度輸出

//卡爾曼濾波參數(shù)與函數(shù)
float angle, angle_dot;//角度和角速度
float angle_0, angle_dot_0;//采集來的角度和角速度
//float dt=20*0.001;//注意:dt的取值為kalman濾波器采樣時間
//一下為運算中間變量
float P[2][2] = {{ 1, 0 },
              { 0, 1 }};
float Pdot[4] ={ 0,0,0,0};
float Q_angle=0.001, Q_gyro=0.005; //角度數(shù)據(jù)置信度,角速度數(shù)據(jù)置信度
float R_angle=0.5 ,C_0 = 1;
float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;

void setup() {
    Wire.begin();//初始化
    Serial.begin(9600);//初始化
    accelgyro.initialize();//初始化

    int tickEvent1=t.every(timeChange, getangle);//本語句執(zhí)行以后timeChange毫秒執(zhí)行回調(diào)函數(shù)getangle

    int tickEvent2=t.every(50, printout) ;//本語句執(zhí)行以后50毫秒執(zhí)行回調(diào)函數(shù)printout,串口輸出
}
void loop() {

    t.update();//時間操作系統(tǒng)運行

}
void printout()
{
     Serial.print(angleAx);Serial.print(',');
    Serial.print(angle1);Serial.print(',');
    Serial.print(angle2);Serial.print(',');
    // Serial.print(gx/131.00);Serial.print(',');
    Serial.println(angle);//Serial.print(',');

//   Serial.println(Output);
}


void getangle()
{
    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);//讀取原始6個數(shù)據(jù)
    angleAx=atan2(ax,az)*180/PI;//計算與x軸夾角
    gyroGy=-gy/131.00;//計算角速度
    Yijielvbo(angleAx,gyroGy);//一階濾波
    Erjielvbo(angleAx,gyroGy);//二階濾波
    Kalman_Filter(angleAx,gyroGy);   //卡爾曼濾波
}



void Yijielvbo(float angle_m, float gyro_m)
{
    angle1 = K1 * angle_m+ (1-K1) * (angle1 + gyro_m * dt);
}

void Erjielvbo(float angle_m,float gyro_m)
{
    x1=(angle_m-angle2)*(1-K2)*(1-K2);
    y1=y1+x1*dt;
    x2=y1+2*(1-K2)*(angle_m-angle2)+gyro_m;
    angle2=angle2+ x2*dt;
}

void Kalman_Filter(double angle_m,double gyro_m)
{
angle+=(gyro_m-q_bias) * dt;
angle_err = angle_m - angle;
Pdot[0]=Q_angle - P[0][1] - P[1][0];
Pdot[1]=- P[1][1];
Pdot[2]=- P[1][1];
Pdot[3]=Q_gyro;
P[0][0] += Pdot[0] * dt;
P[0][1] += Pdot[1] * dt;
P[1][0] += Pdot[2] * dt;
P[1][1] += Pdot[3] * dt;
PCt_0 = C_0 * P[0][0];
PCt_1 = C_0 * P[1][0];
E = R_angle + C_0 * PCt_0;
K_0 = PCt_0 / E;
K_1 = PCt_1 / E;
t_0 = PCt_0;
t_1 = C_0 * P[0][1];
P[0][0] -= K_0 * t_0;
P[0][1] -= K_0 * t_1;
P[1][0] -= K_1 * t_0;
P[1][1] -= K_1 * t_1;
angle += K_0 * angle_err; //最優(yōu)角度
q_bias += K_1 * angle_err;
angle_dot = gyro_m-q_bias;//最優(yōu)角速度
}

4 數(shù)據(jù)繪圖
123.png

5.附件  mp6050+時間輪換頭文件+數(shù)據(jù)繪圖
串口數(shù)據(jù)采集.rar (4.27 MB, 下載次數(shù): 112)
頭文件.rar (53.17 KB, 下載次數(shù): 109)
6.部分使用幫助
定時器常用函數(shù).rar (10.68 KB, 下載次數(shù): 81)
mpu6050關(guān)于16384.0 131.0 兩個參數(shù)來歷
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);//讀取原始6個數(shù)據(jù)
使勁晃陀螺儀看看最大數(shù)據(jù)時多大
就知道該除以多少換算成g和度/秒
我的為 +—32768  +—250所以判定 頭文件初始化的為最大小量程  +—2g +—250
ax/16384.0=x方向的加速度 單位g ,,加速度計算角度用上面的方法
gx/131.0=x軸向角速度  單位 度/秒
來歷:
MPU-6050.pdf (1.6 MB, 下載次數(shù): 51)
截圖出來
1.png
2.png
6.定時器pid//非全代碼

#include "Timer.h"
//pid參數(shù)
float Setpoint;
float Input,Output,errSum,lastErr;
float kp, ki, kd;
float timeChange=10;//pid采樣時間間隔毫秒

Timer t;//定時器時間

void setup(void)
{
t.every(timeChange, PIDCalc) ;//每10毫秒pid一次

}
void PIDCalc ()
{
   float error = Setpoint - Input;
   errSum += (error * timeChange);
   float dErr = (error - lastErr) / timeChange;
   Output = kp * error +ki * errSum + kd * dErr;
   lastErr = error;
}


void loop(void)
{
    t.update();
}





    本站是提供個人知識管理的網(wǎng)絡(luò)存儲空間,,所有內(nèi)容均由用戶發(fā)布,不代表本站觀點,。請注意甄別內(nèi)容中的聯(lián)系方式,、誘導購買等信息,謹防詐騙,。如發(fā)現(xiàn)有害或侵權(quán)內(nèi)容,,請點擊一鍵舉報。
    轉(zhuǎn)藏 分享 獻花(0

    0條評論

    發(fā)表

    請遵守用戶 評論公約

    類似文章 更多