一文看懂mpu6050卡尔曼滤波程序

　　什么是卡尔曼滤波

　　卡尔曼滤波（Kalmanfiltering）一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。

　　传统的滤波方法，只能是在有用信号与噪声具有不同频带的条件下才能实现．20世纪40年代，N．维纳和A．H．柯尔莫哥罗夫把信号和噪声的统计性质引进了滤波理论，在假设信号和噪声都是平稳过程的条件下，利用最优化方法对信号真值进行估计，达到滤波目的，从而在概念上与传统的滤波方法联系起来，被称为维纳滤波。这种方法要求信号和噪声都必须是以平稳过程为条件。60年代初，卡尔曼（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）测试，测试信息通过USART1打印在电脑的超级终端。

　　*实验平台：野火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_2）;//设置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************/

　　……………………