mpu6050數(shù)據(jù)輕松分析(參考版)
2025-06-28 21:09本頁面
【正文】 。紅色的是陀螺儀讀出的角度，綠色的是加速度計讀出的角度。 /* 加速度計取值范圍 */ setSleepEnabled(false)。 setFullScaleGyroRange(MPU6050_GYRO_FS_250)。}Mpu6050取值范圍調(diào)整的方法,修改initialize函數(shù)中標成紅色的行。 (Angle)。 (total_angle)。 (ax_angle)。 dt=time/。gx_angle=gx_angle/。 gyro=gy/。 /* 陀螺儀量程范圍設(shè)置250 131 LSB//s * 陀螺儀角度計算公式:* 小車傾斜角度是gx_angle,陀螺儀讀數(shù)是y,時間是dt* gx_angle +=(y/(131*1000))*dt */ gy = GX_ZERO。/* 加速度量程范圍設(shè)置2g 16384 LSB/g* 計算公式：* 前邊已經(jīng)推導(dǎo)過這里再列出來一次* x是小車傾斜的角度,y是加速度計讀出的值* sinx = **x/180 = y/16384* x=180*y/(**16384)=*/ ax = AX_ZERO。gy, amp。az, amp。ax, amp。 } mictime=millis()。 if(pretime==0) { pretime=millis()。 static unsigned long pretime=0。 unsigned long time=0。}void loop() { // read raw accel/gyro measurements from device double ax_angle=。 Angle += K_0 * Angle_err。 PP[1][0] = K_1 * t_0。 PP[0][0] = K_0 * t_0。 } t_0 = PCt_0。 if(E!=0) { K_0 = PCt_0 / E。 PCt_1 = C_0 * PP[1][0]。 Angle_err = Accel Angle。 PP[1][0] += Pdot[2] * dt。 PP[0][0] += Pdot[0] * dt。 Pdot[2]= PP[1][1]。 Pdot[0]=Q_angle PP[0][1] PP[1][0]。float PP[2][2] = { { 1, 0 },{ 0, 1 } }。float K_0=, K_1=, t_0=, t_1=。float Q_bias, Angle_err。 /* dt為kalman濾波器采樣時間。float R_angle=。 float Q_angle=。}/* 通過卡爾曼濾波得到的最終角度 */f
環(huán)評公示相關(guān)推薦
鄂ICP備17016276號-1