一文看懂mpu6050卡尔曼滤波程序 - 全文
时间：2022-11-09 23:00:01
卡尔曼滤波器是什么？
卡尔曼滤波(Kalmanfiltering)一种利用线性系统状态方程通过系统输入输出观测数据来优化系统状态的算法。由于观测数据包括系统中噪声和干扰的影响，最佳估计也可视为滤波过程。
传统的滤波方法只能在不同频带的有用信号和噪声条件下实现。N．维纳和A．H。柯尔莫哥罗夫将信号和噪声的统计性质引入了滤波理论。在假设信号和噪声过程稳定的情况下，使用最优化的方法来估计信号的真实值，以实现滤波的目的，从而将其概念上与传统的滤波方法联系起来，称为维纳滤波。这种方法要求信号和噪声必须基于稳定的过程。六十年代初，卡尔曼(R．E．Kalman)和布塞(R．S．Bucy)发表了一篇重要论文《线性滤波与预测理论的新成果》，提出了一种新的线性滤波与预测理由论，称为卡尔曼滤波。其特点是在表示在线状态空间的基础上处理噪声输入和观测信号，以获取系统状态或真实信号。
这一理论是在时间范围内表达的。基本概念是在线系统状态空间表示的基础上，从输出和输入观测数据中对系统状态进行最佳估计。这里提到的系统状态是系统过去所有输入和干扰的最小参数的集合。如果你知道系统的状态，你可以确定系统的整个行为与未来的输入和干扰。
假设卡尔曼滤波器不需要信号和噪声。对于每时每刻的系统扰动和观测误差（即噪声），只要适当假设其统计性质，通过处理含噪声的观测信号，就可以在平均意义上获得最小真实信号的估计值。因此，自卡尔曼滤波理论问世以来，它已应用于通信系统、电力系统、航空航天、环境污染控制、工业控制、雷达信号处理等部门，并取得了许多成功的应用成果。例如，在图像处理方面，卡尔曼滤波器的应用恢复了由于某些噪声的影响而造成的模糊图像。在对噪声作了某些统计性质的假定后，就可以用卡尔曼的算法以递推的方式从模糊图像中得到均方差最小的真实图像，使模糊的图像得到复原。
卡尔曼滤波器的性质
①卡尔曼滤波器是一种适用于线性、离散和有限维系统的算法。每个自回归移动平均系统都有外部变量(ARMAX)或者可以用合理传递函数表示的系统可以转换为用状态空间表示的系统，可以用卡尔曼滤波器计算。
②任何一组观测数据都无助于消除x(t)确定性K(t)也与观测数据无关。
③当观测数据和状态联合服从高斯分布时，用卡尔曼递归公式计算高斯随机变量的条件平均值和条件方差，因此卡尔曼滤波公式给出了条件概率密度的线性最小方差估计，即最小方差估计。
应用卡尔曼滤波器
例如，在雷达中，人们对跟踪目标感兴趣，但目标的位置、速度和加速度的测量值通常在任何时候都有噪音。卡尔曼滤波器利用目标的动态信息来消除噪声的影响，并对目标位置进行良好的估计。这种估计可以是对当前目标位置的估计（滤波器）、对未来位置的估计（预测）或对过去位置的估计（插入值或平滑）。
mpu6050卡尔曼滤波分析
最近在学习卡尔曼滤波算法，先测量静止传感器100次，寻求平均值，寻求偏差Ax_offsetAz_offsetGz_offset.这种偏差将在以后的每个测量值中减去。然后通过加速度测量Ax，Az通过atant(Ax，Az)计算Accel_x即是Roll，K_Angle是klman以后的Roll，Gyro_y加速陀螺仪Y轴，K_Gyro_y卡尔曼之后的数值，
klman是融合Accel_x和Gyro_y，得到的结果。
下图是串口发送的数据分析。
下图是使用软件获得的Roll为klman以后的角度，Pinch为原始角度。可以看出klman振动性能好。但效果不是很明显。
mpu6050卡尔曼滤波器输出姿态角程序
/********************(C)COPYRIGHT2012WildFireTeam**************************
*文件名：main.c
*描述：I2CEEPROM(AT24C02)测试信息通过USART在电脑超级终端打印。
*实验平台：野火STM32开发板
*库版本：ST3.0.0
*作者：wildfireteam
**********************************************************************************/
#include“stm32f10x.h”
#include“I2C_MPU6050.h”
#include“usart1.h”
#include“delay.h”
#include“filter.h”
#include“math.h”
#include“misc.h”
#include“TIMX.h”
#defineAIN2PBout(15)
#defineAIN1PBout(14)
#defineBIN1PBout(13)
#defineBIN2PBout(12)
#defineBITBAND(addr，bitnum)((addr&0xF0000000) 0x2000000 ((addr&0xFFFFF)《《5) (bitnum《《2))
#defineMEM_ADDR(addr)*((volaTIleunsignedlong*)(addr))
#defineBIT_ADDR(addr，bitnum)MEM_ADDR(BITBAND(addr，bitnum))
#defineGPIOA_ODR_Addr(GPIOA_BASE 12)//0x4001080C
#defineGPIOB_ODR_Addr(GPIOB_BASE 12)//0x40010C0C
#defineGPIOC_ODR_Addr(GPIOC_BASE 12)//0x4001100C
#defineGPIOD_ODR_Addr(GPIOD_BASE 12)//0x4001140C
#defineGPIOE_ODR_Addr(GPIOE_BASE 12)//0x4001180C
#defineGPIOF_ODR_Addr(GPIOF_BASE 12)//0x40011A0C
#defineGPIOG_ODR_Addr(GPIOG_BASE 12)//0x40011E0C
//#definePAout(n)BIT_ADDR(GPIOA_ODR_Addr，n)//输出
//#definePAin(n)BIT_ADDR(GPIOA_IDR_Addr，n)//输入
#definePBout(n)BIT_ADDR(GPIOB_ODR_Addr，n)//输出
//#definePBin(n)BIT_ADDR(GPIOB_IDR_Addr，n)//输入
//#definePCout(n)BIT_ADDR(GPIOC_ODR_Addr，n)//输出
//#definePCin(n)BIT_ADDR(GPIOC_IDR_Addr，n)//输入
//#definePDout(n)BIT_ADDR(GPIOD_ODR_Addr，n)//输出
//#definePDin(n)BIT_ADDR(GPIOD_IDR_Addr，n)//输入
//#definePEout(n)BIT_ADDR(GPIOE_ODR_Addr，n)//输出
//#definePEin(n)BIT_ADDR(GPIOE_IDR_Addr，n)//输入
//#definePFout(n)BIT_ADDR(GPIOF_ODR_Addr，n)//输出
//#definePFin(n)BIT_ADDR(GPIOF_IDR_Addr，n)//输入
//#definePGout(n)BIT_ADDR(GPIOG_ODR_Addr，n)//输出
//#definePGin(n)BIT_ADDR(GPIOG_IDR_Addr，n)//输入
#definePI3.14159265
#defineZHONGZHI-6
#definePWMATIM1-》CCR1//PA8
#definePWMBTIM1-》CCR4//PA11
floatAngle_Balance，Gyro_Balance，Gyro_Turn;////平衡环控制相关变量
floatEncoder_left，Encoder_right;///
intMoto1，Moto2;
/*
*函数名：main
*描述：主函数
*输入：无
*输出：无
*返回：无
*/
///中断分组处理函数
voidNVIC_Configuration(void)
{
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_二、//设置NVIC中断分组2:2抢占优先级，2响应优先级
}
intRead_Encoder(u8TIMX);
位置平衡PID控制
intbalance(floatAngle，floatGyro)
{
floatBias，kp=500，kd=1;
intbalance;
Bias=Angle-ZHONGZHI;//===求出平衡的角度中值和机械相关
balance=kp*Bias+Gyro*kd;//===计算平衡控制的电机PWMPD控制kp是P系数kd是D系数
returnbalance;
}
//速度PI控制
intvelocity(intencoder_left，intencoder_right)
{
staticfloatVelocity，Encoder_Least，Encoder，Movement;
staticfloatEncoder_Integral，Target_Velocity;
floatkp=50，ki=kp/200;
Encoder_Least=(Encoder_left+Encoder_right)-0;
Encoder*=0.7;//一阶低通滤波，上次速度差占70，本次速度差占30，减缓速度差值，减少对直立系统的干扰
Encoder+=Encoder_Least*0.3;//一阶低通滤波
Encoder_Integral+=Encoder;//积分出位移，积分时间10ms
Encoder_Integral=Encoder_Integral-Movement;//接受遥控器的数据，控制正反转
if(Encoder_Integral》15000){
Encoder_Integral=15000;//积分限幅
}
if(Encoder_Integral《-15000)
{
Encoder_Integral=-15000;
}
Velocity=Encoder*kp+Encoder_Integral*ki;//PI控制器
returnVelocity;
}
//限幅函数
voidXianfu_Pwm(void)
{
intAmplitude=6900;//===PWM满幅是7200限制在6900
if(Moto1《-Amplitude)Moto1=-Amplitude;
if(Moto1》Amplitude)Moto1=Amplitude;
if(Moto2《-Amplitude)Moto2=-Amplitude;
if(Moto2》Amplitude)Moto2=Amplitude;
}
//绝对值函数
intmyabs(inta)
{
inttemp;
if(a《0)temp=-a;
elsetemp=a;
returntemp;
}
/*voidMiniBalance_Motor_Init(void)
{
RCC-》APB2ENR|=1《《3;//PORTB时钟使能
GPIOB-》CRH&=0X0000FFFF;//PORTB12131415推挽输出
GPIOB-》CRH|=0X22220000;//PORTB12131415推挽输出
}*/
voidMiniBalance_Motor_Init(void)
{
GPIO_InitTypeDefGPIO_InitStructure;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB，ENABLE);//使能PB，PE端口时钟
GPIO_InitStructure.GPIO_Pin=GPIO_Pin_12|GPIO_Pin_13|GPIO_Pin_14|GPIO_Pin_15;//LED0--》PB.5端口配置
GPIO_InitStructure.GPIO_Mode=GPIO_Mode_Out_PP;//推挽输出
GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;//IO口速度为50MHz
GPIO_Init(GPIOB，&GPIO_InitStructure);//根据设定参数初始化GPIOB.5
//GPIO_SetBits(GPIOB，GPIO_Pin_5);
//GPIO_ResetBits(GPIOB，GPIO_Pin_6);//PB.5输出高
//GPIO_InitStructure.GPIO_Pin=GPIO_Pin_5;//LED1--》PE.5端口配置，推挽输出
//GPIO_Init(GPIOE，&GPIO_InitStructure);//推挽输出，IO口速度为50MHz
//GPIO_SetBits(GPIOE，GPIO_Pin_5);//PE.5输出高
}
voidMiniBalance_PWM_Init(u16arr，u16psc)
{
MiniBalance_Motor_Init();//初始化电机控制所需IO
RCC-》APB2ENR|=1《《11;//使能TIM1时钟
RCC-》APB2ENR|=1《《2;//PORTA时钟使能
GPIOA-》CRH&=0XFFFF0FF0;//PORTA811复用输出
GPIOA-》CRH|=0X0000B00B;//PORTA811复用输出
TIM1-》ARR=arr;//设定计数器自动重装值
TIM1-》PSC=psc;//预分频器不分频
TIM1-》CCMR2|=6《《12;//CH4PWM1模式
TIM1-》CCMR1|=6《《4;//CH1PWM1模式
TIM1-》CCMR2|=1《《11;//CH4预装载使能
TIM1-》CCMR1|=1《《3;//CH1预装载使能
TIM1-》CCER|=1《《12;//CH4输出使能
TIM1-》CCER|=1《《0;//CH1输出使能
TIM1-》BDTR|=1《《15;//TIM1必须要这句话才能输出PWM
TIM1-》CR1=0x8000;//ARPE使能
TIM1-》CR1|=0x01;//使能定时器1
}
//PWMshewzhi
voidSet_Pwm(intmoto1，intmoto2)
{
intsiqu=400;
if(moto1《0)
{
printf(“AIN反向”);
AIN1=0;
AIN2=1;
}
else
{
printf(“AINfanxaing”);
AIN2=0;
AIN1=1;
}
PWMA=myabs(moto1)+siqu;
if(moto2《0)
{
BIN1=0;
BIN2=1;
}
else
{
BIN1=1;
BIN2=0;
}
PWMB=myabs(moto2)+siqu;
}
intmain(void)
{
intAccel_Y，Accel_X，Accel_Z，Gyro_X，Gyro_Y，Gyro_Z;
floatAcceleration_Z;//Z轴加速度计
intBalance_Pwm，Velocity_Pwm;
NVIC_Configuration();//设置NVIC中断分组2:2位抢占优先级，2位响应优先级
/*串口1初始化*/
USART1_Config();
/*GPIO口初始化*/
MiniBalance_Motor_Init();
/*定时器1初始化*/
MiniBalance_PWM_Init(7199，0);
Encoder_Init_TIM2();//=====编码器接口
Encoder_Init_TIM4();//=====初始化编码器2
/*IIC接口初始化*/
I2C_MPU6050_Init();
/*陀螺仪传感器初始化*/
InitMPU6050();
/***********************************************************************/
while(1)
{
Accel_X=GetData(ACCEL_XOUT_H);
Accel_Y=GetData(ACCEL_YOUT_H);
Accel_Z=GetData(ACCEL_ZOUT_H);
Gyro_X=GetData(GYRO_XOUT_H);
Gyro_Y=GetData(GYRO_YOUT_H);
Gyro_Z=GetData(GYRO_ZOUT_H);
Encoder_left=-Read_Encoder(2);//===读取编码器的值，因为两个电机的旋转了180度的，所以对其中一个取反，保证输出极性一致
Encoder_right=Read_Encoder(4);
/*printf(“\r\n---------加速度X轴原始数据---------%d\r\n”，Accel_X);
printf(“\r\n---------加速度Y轴原始数据---------%d\r\n”，Accel_Y);
printf(“\r\n---------加速度Z轴原始数据---------%d\r\n”，Accel_Z);
printf(“\r\n---------陀螺仪X轴原始数据---------%d\r\n”，Gyro_X);
printf(“\r\n---------陀螺仪Y轴原始数据---------%d\r\n”，Gyro_Y);
printf(“\r\n---------陀螺仪Z轴原始数据---------%d\r\n”，Gyro_Z);*/
//delay_ms(500);
if(Gyro_Y》32768)Gyro_Y-=65536;//数据类型转换也可通过short强制类型转换
if(Gyro_Z》32768)Gyro_Z-=65536;//数据类型转换
if(Accel_X》32768)Accel_X-=65536;//数据类型转换
if(Accel_Z》32768)Accel_Z-=65536;//数据类型转换
Gyro_Balance=-Gyro_Y;//更新平衡角速度
Accel_Y=atan2(Accel_X，Accel_Z)*180/PI;//计算倾角
Gyro_Y=Gyro_Y/16.4;//陀螺仪量程转换
Kalman_Filter(Accel_Y，-Gyro_Y);//卡尔曼滤波
//elseif(Way_Angle==3)Yijielvbo(Accel_Y，-Gyro_Y);//互补滤波
Angle_Balance=angle;//更新平衡倾角
Gyro_Turn=Gyro_Z;//更新转向角速度
Acceleration_Z=Accel_Z;//===更新Z轴加速度计
Gyro_Balance=-Gyro_Y;
printf(“卡尔曼滤波值%f，%f\n”，Angle_Balance，Gyro_Turn);
//Balance_Pwm=balance(Angle_Balance，Gyro_Balance);
Velocity_Pwm=velocity(Encoder_left，Encoder_right);
Moto1=Velocity_Pwm;
Moto2=Velocity_Pwm;
printf(“%d，%d\n”，Moto1，Moto2);
Xianfu_Pwm();
Set_Pwm(Moto1，Moto2);
delay_ms(5);
}
}
/*******************(C)COPYRIGHT2012WildFireTeam*****ENDOFFILE************/
……………………
