对卡尔曼滤波器的初略了解
3赞（博文来源：http://blog.chinaaet.com/zhaocundang）
写这篇博客只要是记录备战这个暑假7月25号全国电子设计比赛，对自动化控制类项目的了解与认知。这个对非自动化专业的学生要去了解卡尔曼滤波和PID来说，有点难，况且数学就只学了一年就没数学课了。不过我还是耐心的研究了起来，下面就以小白的身份说说卡尔曼滤波器这个东西吧。
什么是卡尔曼滤波器？首先卡尔曼是个人，这个人研究数学很牛逼，就研究出来这个滤波器，所以叫卡尔曼滤波器。
滤波器顾名思义就过滤，就像从沙子里面淘金子那样，把沙子和其他的石子去掉，就要金子。然后从数学角度来看，假如我要测一个角度值，测角度用的传感器是I2C接线的MPU6050（陀螺仪），现在我从陀螺仪里面已经读出一些角度值来了，但是有些角度值会出现漂移，一直增加，范围波动很大，不能稳定下来，所以就用了这个卡尔曼滤波器来过滤一下输出稳定的角度值。
这里使用了MPU6050陀螺仪，采用I2C 2线制，就是 VCC GND SCL SDA这几根线就能驱动陀螺仪。SCL是时钟端，负责时序，SDA是数据端，用来传输数据。
/* 什么是I2C？ Inter ingrated cruit 内部集成芯片。
IIC(Inter－Integrated Circuit)总线是一种由PHILIPS公司开发的两线式串行总线，用于连接微控制器及其外围设备。它是由数据线SDA和时钟SCL构成的串行总线，可发送和接收数据。在CPU与被控IC之间、IC与IC之间进行双向传送，高速IIC总线一般可达400kbps以上。
I2C总线在传送数据过程中共有三种类型信号， 它们分别是：开始信号、结束信号和应答信号。
开始信号：SCL为高电平时，SDA由高电平向低电平跳变，开始传送数据。
结束信号：SCL为高电平时，SDA由低电平向高电平跳变，结束传送数据。
应答信号：接收数据的IC在接收到8bit数据后，向发送数据的IC发出特定的低电平脉冲，表示已收到数据。CPU向受控单元发出一个信号后，等待受控单元发出一个应答信号，CPU接收到应答信号后，根据实际情况作出是否继续传递信号的判断。若未收到应答信号，由判断为受控单元出现故障。
*/
IIC驱动陀螺仪，这里简单贴出代码简单一看。
void I2C_Congiguration(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
I2C_InitTypeDef I2C_InitStructure;
/* 使能与 I2C1 有关的时钟 */
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);
RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C1,ENABLE);
/* PB6-I2C1_SCL、PB7-I2C1_SDA*/
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD; // 开漏输出
GPIO_Init(GPIOB, &GPIO_InitStructure);
/* I2C 配置 */
I2C_InitStructure.I2C_Mode = I2C_Mode_I2C;
I2C_InitStructure.I2C_DutyCycle = I2C_DutyCycle_2;
I2C_InitStructure.I2C_OwnAddress1 = I2C1_MPU6050;
I2C_InitStructure.I2C_Ack = I2C_Ack_Enable;
I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit;
I2C_InitStructure.I2C_ClockSpeed = I2C_Speed;
/* 使能 I2C1 */
I2C_Cmd(I2C1, ENABLE);
/* I2C1 初始化 */
I2C_Init(I2C1, &I2C_InitStructure);
/*允许1字节1应答模式*/
I2C_AcknowledgeConfig(I2C1, ENABLE);
}
void MPU6050_Init(void)
{
I2C_Congiguration();//IIC模块初始化
delay_ms(500);
I2C_WriteByte(PWR_MGMT_1,0x00);
delay_ms(10);
I2C_WriteByte(SMPLRT_DIV,0x07);
delay_ms(10);
I2C_WriteByte(CONFIG,0x06);
delay_ms(10);
I2C_WriteByte(GYRO_CONFIG,0x18);
delay_ms(10);
I2C_WriteByte(ACCEL_CONFIG,0x01);
delay_ms(1000);
}
void I2C_WriteByte(uint8_t REG_Address, uint8_t Write_Data)
{
while(I2C_GetFlagStatus(I2C1, I2C_FLAG_BUSY));
/* Send STRAT condition */
I2C_GenerateSTART(I2C1, ENABLE);
/* Test on EV5 and clear it */
while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_MODE_SELECT));
/* Send EEPROM address for write */
I2C_Send7bitAddress(I2C1, I2C1_MPU6050, I2C_Direction_Transmitter);
/* Test on EV6 and clear it */
while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED));
/* Send the EEPROM's internal address to write to */
I2C_SendData(I2C1, REG_Address);
/* Test on EV8 and clear it */
while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_TRANSMITTED));
/* Send the byte to be written */
I2C_SendData(I2C1, Write_Data);
/* Test on EV8 and clear it */
while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_TRANSMITTED));
/* Send STOP condition */
I2C_GenerateSTOP(I2C1, ENABLE);
I2C_WaitEepromStandbyState();
}
uint8_t I2C_ReadByte(uint8_t REG_Address)
{
uint8_t data_byte;
//*((u8 *)0x4001080c) |=0x80;
while(I2C_GetFlagStatus(I2C1, I2C_FLAG_BUSY));
/* Send START condition */
I2C_GenerateSTART(I2C1, ENABLE);
//*((u8 *)0x4001080c) &=~0x80;
/* Test on EV5 and clear it */
while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_MODE_SELECT));
/* Send EEPROM address for write */
I2C_Send7bitAddress(I2C1, I2C1_MPU6050, I2C_Direction_Transmitter);
/* Test on EV6 and clear it */
while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED));
/* Clear EV6 by setting again the PE bit */
I2C_Cmd(I2C1, ENABLE);
/* Send the EEPROM's internal address to write to */
I2C_SendData(I2C1, REG_Address);
/* Test on EV8 and clear it */
while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_TRANSMITTED));
/* Send STRAT condition a second time */
I2C_GenerateSTART(I2C1, ENABLE);
/* Test on EV5 and clear it */
while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_MODE_SELECT));
/* Send EEPROM address for read */
I2C_Send7bitAddress(I2C1, I2C1_MPU6050, I2C_Direction_Receiver);
/* Test on EV6 and clear it */
while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED));
/* While there is data to be read */
/* Disable Acknowledgement */
I2C_AcknowledgeConfig(I2C1, DISABLE);
/* Send STOP Condition */
I2C_GenerateSTOP(I2C1, ENABLE);
/* Test on EV7 and clear it */
if(I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_RECEIVED))
/* Read a byte from the EEPROM */
data_byte = I2C_ReceiveData(I2C1);
/* Enable Acknowledgement to be ready for another reception */
I2C_AcknowledgeConfig(I2C1, ENABLE);
//I2C_EE_WaitEepromStandbyState();
return data_byte;
}
void I2C_ReadBuffer(uint8_t* Data_Buffer, uint8_t REG_Address, uint8_t Num_Byte)
{
//*((u8 *)0x4001080c) |=0x80;
while(I2C_GetFlagStatus(I2C1, I2C_FLAG_BUSY));
/* Send START condition */
I2C_GenerateSTART(I2C1, ENABLE);
//*((u8 *)0x4001080c) &=~0x80;
/* Test on EV5 and clear it */
while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_MODE_SELECT));
/* Send EEPROM address for write */
I2C_Send7bitAddress(I2C1, I2C1_MPU6050, I2C_Direction_Transmitter);
/* Test on EV6 and clear it */
while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED));
/* Clear EV6 by setting again the PE bit */
I2C_Cmd(I2C1, ENABLE);
/* Send the EEPROM's internal address to write to */
I2C_SendData(I2C1, REG_Address);
/* Test on EV8 and clear it */
while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_TRANSMITTED));
/* Send STRAT condition a second time */
I2C_GenerateSTART(I2C1, ENABLE);
/* Test on EV5 and clear it */
while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_MODE_SELECT));
/* Send EEPROM address for read */
I2C_Send7bitAddress(I2C1, I2C1_MPU6050, I2C_Direction_Receiver);
/* Test on EV6 and clear it */
while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED));
/* While there is data to be read */
while(Num_Byte)
{
if(Num_Byte == 1)
{
/* Disable Acknowledgement */
I2C_AcknowledgeConfig(I2C1, DISABLE);
/* Send STOP Condition */
I2C_GenerateSTOP(I2C1, ENABLE);
}
/* Test on EV7 and clear it */
if(I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_RECEIVED))
{
/* Read a byte from the EEPROM */
*Data_Buffer = I2C_ReceiveData(I2C1);
/* Point to the next location where the byte read will be saved */
Data_Buffer++;
/* Decrement the read bytes counter */
Num_Byte--;
}
}
/* Enable Acknowledgement to be ready for another reception */
I2C_AcknowledgeConfig(I2C1, ENABLE);
//I2C_EE_WaitEepromStandbyState();
}
void I2C_WaitEepromStandbyState(void)
{
vu16 SR1_Tmp = 0;
do
{
/* Send START condition */
I2C_GenerateSTART(I2C1, ENABLE);
/* Read I2C1 SR1 register */
SR1_Tmp = I2C_ReadRegister(I2C1, I2C_Register_SR1);
/* Send EEPROM address for write */
I2C_Send7bitAddress(I2C1, I2C1_MPU6050, I2C_Direction_Transmitter);
}while(!(I2C_ReadRegister(I2C1, I2C_Register_SR1) & 0x0002));
/* Clear AF flag */
I2C_ClearFlag(I2C1, I2C_FLAG_AF);
/* STOP condition */
I2C_GenerateSTOP(I2C1, ENABLE);
}主要是读写时序：
现在驱动陀螺仪完事了，获取陀螺仪角度。MPU6050陀螺仪有三轴角度和三轴加速度： 横滚角roll, 俯仰角pitch, 航向角yaw，和各个角度的角速度。这里获取了x轴的加速度，换算成弧度再换算成角度，获取了y轴的角速度。
static uint8_t DataBuffer[2]; //数据缓存 /****************************加速度****************************************/ I2C_ReadBuffer(DataBuffer, ACCEL_XOUT_H, 2); Accel_x = (short)((DataBuffer[0]<<8)+DataBuffer[1]);//读取X轴加速度 Angle_ax = (Accel_x +220) /16384; //去除零点偏移,计算得到角度（弧度） Angle_ax = Angle_ax*1.2*180/3.14; //弧度转换为度 /****************************角速度****************************************/ I2C_ReadBuffer(DataBuffer, GYRO_YOUT_H, 2); Gyro_y = (short)((DataBuffer[0]<<8)+DataBuffer[1]); //静止时角速度Y轴输出为-18左右 Gyro_y = (Gyro_y + 18)/16.4; //去除零点偏移，计算角速度值 //Angle_gy = Angle_gy + Gyro_y*0.01; //角速度积分得到倾斜角度,因为卡尔曼计算带有时间dt，所以此处不用积分 /***************************卡尔曼融合*************************************/ Kalman_Filter(Angle_ax,Gyro_y); //卡尔曼滤波计算倾角
卡尔曼滤波器，主要包含5个公式，数学不好，看不太懂，就知道5个公式。（真的是数学问题）可以看看：百度百科卡尔曼滤波器
5个公式是这样的：
/*
X(k|k-1)=A X(k-1|k-1)+B U(k) ……….. (1)
式(1)中，X(k|k-1)是利用上一状态预测的结果，X(k-1|k-1)是上一状态最优的结果，U(k)为现在状态的控制量，如果没有控制量，它可以为0。
到现在为止，我们的系统结果已经更新了，可是，对应于X(k|k-1)的协方差还没更新。我们用P表示协方差(covariance)：
P(k|k-1)=A P(k-1|k-1) A’+Q ……… (2)
式(2)中，P(k|k-1)是X(k|k-1)对应的协方差，P(k-1|k-1)是X(k-1|k-1)对应的协方差，A’表示A的转置矩阵，Q是系统过程的协方差。
现在我们有了现在状态的预测结果，然后我们再收集现在状态的测量值。结合预测值和测量值，我们可以得到现在状态(k)的最优化估算值X(k|k)：
X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) ……… (3)
其中Kg为卡尔曼增益(Kalman Gain)：
Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R) ……… (4)
到现在为止，我们已经得到了k状态下最优的估算值X(k|k)。但是为了要令卡尔曼滤波器不断的运行下去直到系统过程结束，我们还要更新k状态下X(k|k)的协方差：
P(k|k)=（I-Kg(k) H）P(k|k-1) ……… (5)
*/
卡尔曼滤波器代码：将陀螺仪的x轴角度和y轴角度传参进入卡尔曼滤波器输出稳定的x轴角度和y轴角度。
void Kalman_Filter(float Accel,float Gyro)
{
Angle+=(Gyro - Q_bias) * dt; //先验估计
Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先验估计误差协方差的微分
Pdot[1]= -PP[1][1];
Pdot[2]= -PP[1][1];
Pdot[3]=Q_gyro;
PP[0][0] += Pdot[0] * dt; // Pk-先验估计误差协方差微分的积分
PP[0][1] += Pdot[1] * dt; // =先验估计误差协方差
PP[1][0] += Pdot[2] * dt;
PP[1][1] += Pdot[3] * dt;
Angle_err = Accel - Angle; //zk-先验估计
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 += K_0 * Angle_err; //后验估计
Q_bias += K_1 * Angle_err; //后验估计 Gyro_y = Gyro - Q_bias; //输出值(后验估计)的微分=角速度
}加粗的Angle 和 Gyro_y 是最终输出的稳定的x轴角度 和 y轴角度，这里Z轴角度没用，Z轴是垂直上下，没有用到，所以没有从IIC陀螺仪那里输出。
然后输出的角度就可以用了，角度值也比较稳定。
