什么是卡爾曼濾波

　　卡爾曼濾波（Kalmanfiltering）一種利用線性系統(tǒng)狀態(tài)方程，通過系統(tǒng)輸入輸出觀測數(shù)據(jù)，對系統(tǒng)狀態(tài)進行最優(yōu)估計的算法。由于觀測數(shù)據(jù)中包括系統(tǒng)中的噪聲和干擾的影響，所以最優(yōu)估計也可看作是濾波過程。

　　傳統(tǒng)的濾波方法，只能是在有用信號與噪聲具有不同頻帶的條件下才能實現(xiàn)．20世紀40年代，N．維納和A．H．柯爾莫哥羅夫把信號和噪聲的統(tǒng)計性質引進了濾波理論，在假設信號和噪聲都是平穩(wěn)過程的條件下，利用最優(yōu)化方法對信號真值進行估計，達到濾波目的，從而在概念上與傳統(tǒng)的濾波方法聯(lián)系起來，被稱為維納濾波。這種方法要求信號和噪聲都必須是以平穩(wěn)過程為條件。60年代初，卡爾曼（R．E．Kalman）和布塞（R．S．Bucy）發(fā)表了一篇重要的論文《線性濾波和預測理論的新成果》，提出了一種新的線性濾波和預測理由論，被稱之為卡爾曼濾波。特點是在線性狀態(tài)空間表示的基礎上對有噪聲的輸入和觀測信號進行處理，求取系統(tǒng)狀態(tài)或真實信號。

　　這種理論是在時間域上來表述的，基本的概念是：在線性系統(tǒng)的狀態(tài)空間表示基礎上，從輸出和輸入觀測數(shù)據(jù)求系統(tǒng)狀態(tài)的最優(yōu)估計。這里所說的系統(tǒng)狀態(tài)，是總結系統(tǒng)所有過去的輸入和擾動對系統(tǒng)的作用的最小參數(shù)的集合，知道了系統(tǒng)的狀態(tài)就能夠與未來的輸入與系統(tǒng)的擾動一起確定系統(tǒng)的整個行為。

　　卡爾曼濾波不要求信號和噪聲都是平穩(wěn)過程的假設條件。對于每個時刻的系統(tǒng)擾動和觀測誤差（即噪聲），只要對它們的統(tǒng)計性質作某些適當?shù)募俣?，通過對含有噪聲的觀測信號進行處理，就能在平均的意義上，求得誤差為最小的真實信號的估計值。因此，自從卡爾曼濾波理論問世以來，在通信系統(tǒng)、電力系統(tǒng)、航空航天、環(huán)境污染控制、工業(yè)控制、雷達信號處理等許多部門都得到了應用，取得了許多成功應用的成果。例如在圖像處理方面，應用卡爾曼濾波對由于某些噪聲影響而造成模糊的圖像進行復原。在對噪聲作了某些統(tǒng)計性質的假定后，就可以用卡爾曼的算法以遞推的方式從模糊圖像中得到均方差最小的真實圖像，使模糊的圖像得到復原。

　　卡爾曼濾波的性質

　　①卡爾曼濾波是一個算法，它適用于線性、離散和有限維系統(tǒng)。每一個有外部變量的自回歸移動平均系統(tǒng)（ARMAX）或可用有理傳遞函數(shù)表示的系統(tǒng)都可以轉換成用狀態(tài)空間表示的系統(tǒng)，從而能用卡爾曼濾波進行計算。

　　②任何一組觀測數(shù)據(jù)都無助于消除x（t）的確定性。增益K（t）也同樣地與觀測數(shù)據(jù)無關。

　?、郛斢^測數(shù)據(jù)和狀態(tài)聯(lián)合服從高斯分布時用卡爾曼遞歸公式計算得到的是高斯隨機變量的條件均值和條件方差，從而卡爾曼濾波公式給出了計算狀態(tài)的條件概率密度的更新過程線性最小方差估計，也就是最小方差估計。

　　卡爾曼濾波的應用

　　比如，在雷達中，人們感興趣的是跟蹤目標，但目標的位置、速度、加速度的測量值往往在任何時候都有噪聲?？柭鼮V波利用目標的動態(tài)信息，設法去掉噪聲的影響，得到一個關于目標位置的好的估計。這個估計可以是對當前目標位置的估計（濾波），也可以是對于將來位置的估計（預測），也可以是對過去位置的估計（插值或平滑）。

　　mpu6050卡爾曼濾波分析

　　最近在學習卡爾曼濾波算法，算法首先靜止傳感器，先測量100次，求平均值，求出偏差Ax_offsetAz_offsetGz_offset.以后每次測量值都減去這一偏差。然后通過加速度測得的Ax，Az通過atant（Ax，Az）計算Accel_x即是Roll，K_Angle是klman以后的Roll，Gyro_y為陀螺儀Y軸加速度，K_Gyro_y為卡爾曼之后的數(shù)值，

　　klman是融合Accel_x和Gyro_y，得到的結果。

　　下圖為串口發(fā)上來的數(shù)據(jù)分析。

　　下圖為使用一個軟件得出的Roll為klman以后的角度，Pinch為原始角度?？梢钥闯鰇lman對震動表現(xiàn)較好。但是效果并不是很明顯。

　　mpu6050卡爾曼濾波輸出姿態(tài)角程序

　　/********************（C）COPYRIGHT2012WildFireTeam**************************

　　*文件名：main.c

　　*描述：I2CEEPROM（AT24C02）測試，測試信息通過USART1打印在電腦的超級終端。

　　*實驗平臺：野火STM32開發(fā)板

　　*庫版本：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;//平衡環(huán)控制相關變量

　　floatEncoder_left，Encoder_right;//速度環(huán)控制相關變量

　　intMoto1，Moto2;

　　/*

　　*函數(shù)名：main

　　*描述：主函數(shù)

　　*輸入：無

　　*輸出：無

　　*返回：無

　　*/

　　//中斷分組處理函數(shù)

　　voidNVIC_Configuration（void）

　　{

　　NVIC_PriorityGroupConfig（NVIC_PriorityGroup_2）;//設置NVIC中斷分組2:2位搶占優(yōu)先級，2位響應優(yōu)先級

　　}

　　intRead_Encoder（u8TIMX）;

　　//位置平衡PID控制

　　intbalance（floatAngle，floatGyro）

　　{

　　floatBias，kp=500，kd=1;

　　intbalance;

　　Bias=Angle-ZHONGZHI;//===求出平衡的角度中值和機械相關

　　balance=kp*Bias+Gyro*kd;//===計算平衡控制的電機PWMPD控制kp是P系數(shù)kd是D系數(shù)

　　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，減緩速度差值，減少對直立系統(tǒng)的干擾

　　Encoder+=Encoder_Least*0.3;//一階低通濾波

　　Encoder_Integral+=Encoder;//積分出位移，積分時間10ms

　　Encoder_Integral=Encoder_Integral-Movement;//接受遙控器的數(shù)據(jù)，控制正反轉

　　if（Encoder_Integral》15000）{

　　Encoder_Integral=15000;//積分限幅

　　}

　　if（Encoder_Integral《-15000）

　　{

　　Encoder_Integral=-15000;

　　}

　　Velocity=Encoder*kp+Encoder_Integral*ki;//PI控制器

　　returnVelocity;

　　}

　　//限幅函數(shù)

　　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;

　　}

　　//絕對值函數(shù)

　　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）;//根據(jù)設定參數(shù)初始化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;//設定計數(shù)器自動重裝值

　　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位搶占優(yōu)先級，2位響應優(yōu)先級

　　/*串口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軸原始數(shù)據(jù)---------%d\r\n”，Accel_X）;

　　printf（“\r\n---------加速度Y軸原始數(shù)據(jù)---------%d\r\n”，Accel_Y）;

　　printf（“\r\n---------加速度Z軸原始數(shù)據(jù)---------%d\r\n”，Accel_Z）;

　　printf（“\r\n---------陀螺儀X軸原始數(shù)據(jù)---------%d\r\n”，Gyro_X）;

　　printf（“\r\n---------陀螺儀Y軸原始數(shù)據(jù)---------%d\r\n”，Gyro_Y）;

　　printf（“\r\n---------陀螺儀Z軸原始數(shù)據(jù)---------%d\r\n”，Gyro_Z）;*/

　　//delay_ms（500）;

　　if（Gyro_Y》32768）Gyro_Y-=65536;//數(shù)據(jù)類型轉換也可通過short強制類型轉換

　　if（Gyro_Z》32768）Gyro_Z-=65536;//數(shù)據(jù)類型轉換

　　if（Accel_X》32768）Accel_X-=65536;//數(shù)據(jù)類型轉換

　　if（Accel_Z》32768）Accel_Z-=65536;//數(shù)據(jù)類型轉換

　　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************/

　　……………………