- #include "kalman.h"
- #include "mpu6050.h"
- #include "math.h"
- short aacx,aacy,aacz; /*加速度传感器原始数据*/
- short gyrox,gyroy,gyroz; /*陀螺仪原始数据*/
- short temperature; /*陀螺仪温度数据*/
- float Accel_x; /*X轴加速度值暂存*/
- float Accel_y; /*Y轴加速度值暂存*/
- float Accel_z; /*Z轴加速度值暂存*/
- float Gyro_x; /*X轴陀螺仪数据暂存*/
- float Gyro_y; /*Y轴陀螺仪数据暂存*/
- float Gyro_z; /*Z轴陀螺仪数据暂存*/
- float Angle_x_temp; /*由加速度计算的x倾斜角度*/
- float Angle_y_temp; /*由加速度计算的y倾斜角度*/
- float Angle_X_Final; /*X最终倾斜角度*/
- float Angle_Y_Final; /*Y最终倾斜角度*/
- /**
- * [url=home.php?mod=space&uid=247401]@brief[/url] 读取数据预处理
- *
- * @param NULL
- *
- * @retval NULL
- */
- void Angle_Calcu(void)
- {
- /*1.原始数据读取*/
- float accx,accy,accz; /*三方向角加速度值*/
- MPU_Get_Accelerometer(&aacx,&aacy,&aacz); /*得到加速度传感器数据*/
- MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz); /*得到陀螺仪数据*/
- temperature = MPU_Get_Temperature(); /*得到温度值*/
- Accel_x = aacx;
- Accel_y = aacy;
- Accel_z = aacz;
- Gyro_x = gyrox;
- Gyro_y = gyroy;
- Gyro_z = gyroz;
-
- /*2.角加速度原始值处理过程*/
- /*加速度传感器配置寄存器0X1C内写入0x01,设置范围为±2g。换算关系：2^16/4 = 16384LSB/g*/
- if(Accel_x<32764) accx=Accel_x/16384;
- else accx=1-(Accel_x-49152)/16384;
- if(Accel_y<32764) accy=Accel_y/16384;
- else accy=1-(Accel_y-49152)/16384;
- if(Accel_z<32764) accz=Accel_z/16384;
- else accz=(Accel_z-49152)/16384;
- /*加速度反正切公式计算三个轴和水平面坐标系之间的夹角*/
- Angle_x_temp=(atan(accy/accz))*180/3.14;
- Angle_y_temp=(atan(accx/accz))*180/3.14;
- /*判断计算后角度的正负号*/
- if(Accel_x<32764) Angle_y_temp = +Angle_y_temp;
- if(Accel_x>32764) Angle_y_temp = -Angle_y_temp;
- if(Accel_y<32764) Angle_x_temp = +Angle_x_temp;
- if(Accel_y>32764) Angle_x_temp = -Angle_x_temp;
-
- /*3.角速度原始值处理过程*/
- /*陀螺仪配置寄存器0X1B内写入0x18，设置范围为±2000deg/s。换算关系：2^16/4000=16.4LSB/(°/S)*/
- /*计算角速度*/
- if(Gyro_x<32768) Gyro_x=-(Gyro_x/16.4);
- if(Gyro_x>32768) Gyro_x=+(65535-Gyro_x)/16.4;
- if(Gyro_y<32768) Gyro_y=-(Gyro_y/16.4);
- if(Gyro_y>32768) Gyro_y=+(65535-Gyro_y)/16.4;
- if(Gyro_z<32768) Gyro_z=-(Gyro_z/16.4);
- if(Gyro_z>32768) Gyro_z=+(65535-Gyro_z)/16.4;
-
- /*4.调用卡尔曼函数*/
- Kalman_Filter_X(Angle_x_temp,Gyro_x); /*卡尔曼滤波计算X倾角*/
- Kalman_Filter_Y(Angle_y_temp,Gyro_y); /*卡尔曼滤波计算Y倾角*/
- }
- /************************ 卡尔曼参数 ****************************/
- float Q_angle = 0.001; /*角度数据置信度，角度噪声的协方差*/
- float Q_gyro = 0.003; /*角速度数据置信度，角速度噪声的协方差*/
- float R_angle = 0.5; /*加速度计测量噪声的协方差*/
- float dt = 0.02; /*滤波算法计算周期，由定时器定时20ms*/
- char C_0 = 1; /*H矩阵值*/
- float Q_bias, Angle_err; /*Q_bias:陀螺仪的偏差 Angle_err:角度偏量*/
- float PCt_0, PCt_1, E; /*计算的过程量*/
- float K_0, K_1, t_0, t_1; /*卡尔曼增益 K_0:用于计算最优估计值 K_1:用于计算最优估计值的偏差 t_0/1:中间变量*/
- float P[4] ={0,0,0,0}; /*过程协方差矩阵的微分矩阵，中间变量*/
- float PP[2][2] = { { 1, 0 },{ 0, 1 } };/*过程协方差矩阵P*/
- /**
- * @brief 卡尔曼函数
- *
- * @param Accel
- * @param Gyro
- *
- * @retval NULL
- */
- void Kalman_Filter_X(float Accel,float Gyro)
- {
- /*
- 步骤一，先验估计
- 公式：X(k|k-1) = AX(k-1|k-1) + BU(k)
- X = (Angle,Q_bias)
- A(1,1) = 1,A(1,2) = -dt
- A(2,1) = 0,A(2,2) = 1
- */
- Angle_X_Final += (Gyro - Q_bias) * dt; /*状态方程,角度值等于上次最优角度加角速度减零漂后积分*/
-
- /*
- 步骤二，计算过程协方差矩阵的微分矩阵
- 公式：P(k|k-1)=AP(k-1|k-1)A^T + Q
- Q(1,1) = cov(Angle,Angle) Q(1,2) = cov(Q_bias,Angle)
- Q(2,1) = cov(Angle,Q_bias) Q(2,2) = cov(Q_bias,Q_bias)
- */
- P[0]= Q_angle - PP[0][1] - PP[1][0];
- P[1]= -PP[1][1]; /*先验估计误差协方差*/
- P[2]= -PP[1][1];
- P[3]= Q_gyro;
- PP[0][0] += P[0] * dt;
- PP[0][1] += P[1] * dt;
- PP[1][0] += P[2] * dt;
- PP[1][1] += P[3] * dt;
-
- /*
- 步骤三，计算卡尔曼增益
- 公式：Kg(k)= P(k|k-1)H^T/(HP(k|k-1)H^T+R)
- Kg = (K_0,K_1) 对应Angle,Q_bias增益
- H = (1,0) 可由z=HX+v求出z:Accel
- */
- PCt_0 = C_0 * PP[0][0];
- PCt_1 = C_0 * PP[1][0];
- E = R_angle + C_0 * PCt_0;
- K_0 = PCt_0 / E;
- K_1 = PCt_1 / E;
-
- /*
- 步骤四，后验估计误差协方差
- 公式：P(k|k)=(I-Kg(k)H)P(k|k-1)
- 也可写为：P(k|k)=P(k|k-1)-Kg(k)HP(k|k-1)
- */
- t_0 = PCt_0;
- t_1 = C_0 * PP[0][1];
- PP[0][0] -= K_0 * t_0;
- PP[0][1] -= K_0 * t_1;
- PP[1][0] -= K_1 * t_0;
- PP[1][1] -= K_1 * t_1;
-
- /*
- 步骤五，计算最优角速度值
- 公式：X(k|k)= X(k|k-1)+Kg(k)(Z(k)-X(k|k-1))
- */
- Angle_err = Accel - Angle_X_Final; /*Z(k)先验估计 计算角度偏差*/
- Angle_X_Final += K_0 * Angle_err; /*后验估计，给出最优估计值*/
- Q_bias += K_1 * Angle_err; /*后验估计，跟新最优估计值偏差*/
- Gyro_x = Gyro - Q_bias;
- }
- void Kalman_Filter_Y(float Accel,float Gyro)
- {
- Angle_Y_Final += (Gyro - Q_bias) * dt;
- P[0]=Q_angle - PP[0][1] - PP[1][0];
- P[1]=-PP[1][1];
- P[2]=-PP[1][1];
- P[3]=Q_gyro;
- PP[0][0] += P[0] * dt;
- PP[0][1] += P[1] * dt;
- PP[1][0] += P[2] * dt;
- PP[1][1] += P[3] * dt;
- Angle_err = Accel - Angle_Y_Final;
- PCt_0 = C_0 * PP[0][0];
- PCt_1 = C_0 * PP[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 * PP[0][1];
- PP[0][0] -= K_0 * t_0;
- PP[0][1] -= K_0 * t_1;
- PP[1][0] -= K_1 * t_0;
- PP[1][1] -= K_1 * t_1;
- Angle_Y_Final += K_0 * Angle_err;
- Q_bias += K_1 * Angle_err;
- Gyro_y = Gyro - Q_bias;
- }
- 复制代码
- /**
- ******************************************************************************
- * [url=home.php?mod=space&uid=288409]@file[/url] main.c
- * [url=home.php?mod=space&uid=187600]@author[/url]
- * [url=home.php?mod=space&uid=895143]@version[/url] V1.0
- * [url=home.php?mod=space&uid=212281]@date[/url] 2023-4-7
- * @brief MPU6050陀螺仪数据解算（卡尔曼滤波）
- ******************************************************************************
- *
- *
- *
- ******************************************************************************
- */
- #include "main.h"
- /**
- * @brief 主函数
- * @param 无
- * @retval 无
- */
- int main(void)
- {
- SYS_Init();
- while(1)
- {
- /*MPU6050数据上报*/
- DATA_Report();
- }
- }
- /**
- * @brief 系统初始化总函数
- * @param 无
- * @retval 无
- */
- void SYS_Init(void)
- {
- delay_init();
- NVIC_PriorityGroupConfig(NVIC_PriorityGroup_1);
- uart_init(115200);
- LED_Init();
- MPU_Init();
- /*10Khz的计数频率，计数到200为20ms*/
- TIM4_Int_Init(199,7199);
- }
- /**
- * @brief MPU6050数据上报
- * @param 无
- * @retval 无
- */
- void DATA_Report(void)
- {
- /*串口发送实时俯仰角，横滚角，XYZ三轴角加速度原始值，XYZ三轴角速度原始值*/
- printf("俯仰角:%.4f 横滚角:%.4f \
- X加速度:%5d y加速度:%5d z加速度:%5d \
- x速度:%5d y速度:%5d z速度:%5d\r\n",\
- Angle_X_Final,Angle_Y_Final,aacx,aacy,aacz,\
- gyrox,gyroy,gyroz);
-
-
- }
楼主
