互补滤波和卡尔曼滤波结果比较
本帖最后由 chengluoran 于 2013-1-29 17:28 编辑如图:两滤波结果几乎相同。黑色曲线是互补滤波结果,黄色曲线是卡尔曼滤波结果。由于两者太相似,需要把黑色线条加粗一倍才能看到,不然就被黄色曲线覆盖了。
点击图片可查看大图
以下是部分代码://每10ms执行一次
if((millis() - time) >= 10)
{
digitalWrite(14, HIGH);//用于测试程序运行时间
time = millis();
//读传感器数据
mpu_get_data();
digitalWrite(13, HIGH);
//互补滤波
angle_hb = (0.98) * (angle_hb - mpu_data.y_gyro_f * 0.01) + (0.02)*(mpu_data.x_accel_f);
//卡尔曼滤波
Kalman_Filter(mpu_data.x_accel_f, -mpu_data.y_gyro_f);
digitalWrite(13, LOW);
//串口发送数据
Serial3.print(-mpu_data.y_gyro_f);
Serial3.print(",");
Serial3.print(mpu_data.x_accel_f);
Serial3.print(",");
Serial3.print(angle_hb);
Serial3.print(",");
Serial3.print(angle);
Serial3.println("");
digitalWrite(14, LOW);
}互补滤波参考 The Balance Filter ,卡尔曼滤波参考 zlstone
mark!!!!…… MARK。。。。。 正需要,好好学习一下 因此对于一般的应用互补滤波就可以取得不错的效果。完全不需要卡尔曼滤波器。 这个能不能不用浮点数啊, 马克一下 正想用呢 图是用设么做的?????? 不要为了卡尔曼而卡尔曼 可以用q格式 用的Excel会的图吧 两者的区别在于,叫法不同。。 yiyu 发表于 2013-1-30 21:43 static/image/common/back.gif
这个能不能不用浮点数啊,
我觉得STM32算这个很快了。如果不用浮点你可以用Q格式运算,TI IQmath 不知道“收藏”按钮在哪里,只能mark了。
其实两个相当于是牛刀和杀猪刀的区别,在杀鸡上效果一样,但是如果杀牛,那就能看出来了 123bac 发表于 2013-1-31 00:00 static/image/common/back.gif
图是用设么做的??????
串口接收数据存为文本文件,然后用excel导入作图 chenjiawei7 发表于 2013-1-31 07:53 static/image/common/back.gif
不要为了卡尔曼而卡尔曼
不明白你说的什么意思 Garbage614 发表于 2013-1-31 09:56 static/image/common/back.gif
不知道“收藏”按钮在哪里,只能mark了。
其实两个相当于是牛刀和杀猪刀的区别,在杀鸡上效果一样,但是如 ...
收藏按钮在1楼末尾。
这个比喻很形象{:lol:} 弱弱问一句,这是用在那里的啊. cyxavr 发表于 2013-1-31 10:15 static/image/common/back.gif
弱弱问一句,这是用在那里的啊.
两轮平衡小车 串口接收数据存为文本文件,然后用excel导入作图教我一下这个我真不懂 123bac 发表于 2013-1-31 12:43 static/image/common/back.gif
串口接收数据存为文本文件,然后用excel导入作图教我一下这个我真不懂
额。。。单片机按照格式 “数据1,数据2,数据3,数据4 ... 换行” 发送数据,电脑端用串口调试助手或者超级终端接收,然后把接收到数据保存为txt文件。打开excel导入数据。excel会自动识别逗号和换行符。然后就可以按常规处理数据的方法作图了 {:sad:}{:sad:} {:sweat:}{:sweat:} 我最近在弄得就是单片机用串口发数据的事,把串口通信和I2C通信理解了很多遍也编不出采集数据用串口发到调试助手, 这程序死活编不出来论坛的大师说很简单的但是我就是采集不到mpu6050角度的加速的数据我有把角度和加速度显示在1602的程序怎么转到串口就不行了 #include <REG52.H>
#include <math.h> //Keil library
#include <stdio.h> //Keil library
#include <INTRINS.H>
typedef unsigned charuchar;
typedef unsigned short ushort;
typedef unsigned int uint;
//****************************************
// 定义51单片机端口
//****************************************
#define DataPort P0 //LCD1602数据端口
sbit SCL=P1^0; //IIC时钟引脚定义
sbit SDA=P1^1; //IIC数据引脚定义
sbit LCM_RS=P2^6; //LCD1602命令端口
sbit LCM_RW=P2^5; //LCD1602命令端口
sbit LCM_EN=P2^7; //LCD1602命令端口
//****************************************
// 定义MPU6050内部地址
//****************************************
#define SMPLRT_DIV 0x19 //陀螺仪采样率,典型值:0x07(125Hz)
#define CONFIG 0x1A //低通滤波频率,典型值:0x06(5Hz)
#define GYRO_CONFIG 0x1B //陀螺仪自检及测量范围,典型值:0x18(不自检,2000deg/s)
#define ACCEL_CONFIG 0x1C //加速计自检、测量范围及高通滤波频率,典型值:0x01(不自检,2G,5Hz)
#define ACCEL_XOUT_H 0x3B
#define ACCEL_XOUT_L 0x3C
#define ACCEL_YOUT_H 0x3D
#define ACCEL_YOUT_L 0x3E
#define ACCEL_ZOUT_H 0x3F
#define ACCEL_ZOUT_L 0x40
#define TEMP_OUT_H 0x41
#define TEMP_OUT_L 0x42
#define GYRO_XOUT_H 0x43
#define GYRO_XOUT_L 0x44
#define GYRO_YOUT_H 0x45
#define GYRO_YOUT_L 0x46
#define GYRO_ZOUT_H 0x47
#define GYRO_ZOUT_L 0x48
#define PWR_MGMT_1 0x6B //电源管理,典型值:0x00(正常启用)
#define WHO_AM_I 0x75 //IIC地址寄存器(默认数值0x68,只读)
#define SlaveAddress 0xD0 //IIC写入时的地址字节数据,+1为读取
//****************************************
//定义类型及变量
//****************************************
uchar dis; //显示数字(-511至512)的字符数组
int dis_data; //变量
//int Temperature,Temp_h,Temp_l; //温度及高低位数据
//****************************************
//函数声明
//****************************************
voiddelay(unsigned int k); //延时
//LCD相关函数
voidInitLcd(); //初始化lcd1602
voidlcd_printf(uchar *s,int temp_data);
voidWriteDataLCM(uchar dataW); //LCD数据
voidWriteCommandLCM(uchar CMD,uchar Attribc); //LCD指令
voidDisplayOneChar(uchar X,uchar Y,uchar DData); //显示一个字符
voidDisplayListChar(uchar X,uchar Y,uchar *DData,L); //显示字符串
//MPU6050操作函数
voidInitMPU6050(); //初始化MPU6050
voidDelay5us();
voidI2C_Start();
voidI2C_Stop();
voidI2C_SendACK(bit ack);
bit I2C_RecvACK();
voidI2C_SendByte(uchar dat);
uchar I2C_RecvByte();
voidI2C_ReadPage();
voidI2C_WritePage();
voiddisplay_ACCEL_x();
voiddisplay_ACCEL_y();
voiddisplay_ACCEL_z();
uchar Single_ReadI2C(uchar REG_Address); //读取I2C数据
voidSingle_WriteI2C(uchar REG_Address,uchar REG_data); //向I2C写入数据
//****************************************
//整数转字符串
//****************************************
void lcd_printf(uchar *s,int temp_data)
{
if(temp_data<0)
{
temp_data=-temp_data;
*s='-';
}
else *s=' ';
*++s =temp_data/100+0x30;
temp_data=temp_data%100; //取余运算
*++s =temp_data/10+0x30;
temp_data=temp_data%10; //取余运算
*++s =temp_data+0x30;
}
//****************************************
//延时
//****************************************
void delay(unsigned int k)
{
unsigned int i,j;
for(i=0;i<k;i++)
{
for(j=0;j<121;j++);
}
}
//****************************************
//LCD1602初始化
//****************************************
void InitLcd()
{
WriteCommandLCM(0x38,1);
WriteCommandLCM(0x08,1);
WriteCommandLCM(0x01,1);
WriteCommandLCM(0x06,1);
WriteCommandLCM(0x0c,1);
DisplayOneChar(0,0,'A');
DisplayOneChar(0,1,'G');
}
//****************************************
//LCD1602写允许
//****************************************
void WaitForEnable(void)
{
DataPort=0xff;
LCM_RS=0;LCM_RW=1;_nop_();
LCM_EN=1;_nop_();_nop_();
while(DataPort&0x80);
LCM_EN=0;
}
//****************************************
//LCD1602写入命令
//****************************************
void WriteCommandLCM(uchar CMD,uchar Attribc)
{
if(Attribc)WaitForEnable();
LCM_RS=0;LCM_RW=0;_nop_();
DataPort=CMD;_nop_();
LCM_EN=1;_nop_();_nop_();LCM_EN=0;
}
//****************************************
//LCD1602写入数据
//****************************************
void WriteDataLCM(uchar dataW)
{
WaitForEnable();
LCM_RS=1;LCM_RW=0;_nop_();
DataPort=dataW;_nop_();
LCM_EN=1;_nop_();_nop_();LCM_EN=0;
}
//****************************************
//LCD1602写入一个字符
//****************************************
void DisplayOneChar(uchar X,uchar Y,uchar DData)
{
Y&=1;
X&=15;
if(Y)X|=0x40;
X|=0x80;
WriteCommandLCM(X,0);
WriteDataLCM(DData);
}
//****************************************
//LCD1602显示字符串
//****************************************
void DisplayListChar(uchar X,uchar Y,uchar *DData,L)
{
uchar ListLength=0;
Y&=0x1;
X&=0xF;
while(L--)
{
DisplayOneChar(X,Y,DData);
ListLength++;
X++;
}
}
//**************************************
//延时5微秒(STC90C52RC@12M)
//不同的工作环境,需要调整此函数
//当改用1T的MCU时,请调整此延时函数
//**************************************
void Delay5us()
{
_nop_();_nop_();_nop_();_nop_();
_nop_();_nop_();_nop_();_nop_();
_nop_();_nop_();_nop_();_nop_();
_nop_();_nop_();_nop_();_nop_();
_nop_();_nop_();_nop_();_nop_();
_nop_();_nop_();_nop_();_nop_();
}
//**************************************
//I2C起始信号
//**************************************
void I2C_Start()
{
SDA = 1; //拉高数据线
SCL = 1; //拉高时钟线
Delay5us(); //延时
SDA = 0; //产生下降沿
Delay5us(); //延时
SCL = 0; //拉低时钟线
}
//**************************************
//I2C停止信号
//**************************************
void I2C_Stop()
{
SDA = 0; //拉低数据线
SCL = 1; //拉高时钟线
Delay5us(); //延时
SDA = 1; //产生上升沿
Delay5us(); //延时
}
//**************************************
//I2C发送应答信号
//入口参数:ack (0:ACK 1:NAK)
//**************************************
void I2C_SendACK(bit ack)
{
SDA = ack; //写应答信号
SCL = 1; //拉高时钟线
Delay5us(); //延时
SCL = 0; //拉低时钟线
Delay5us(); //延时
}
//**************************************
//I2C接收应答信号
//**************************************
bit I2C_RecvACK()
{
SCL = 1; //拉高时钟线
Delay5us(); //延时
CY = SDA; //读应答信号
SCL = 0; //拉低时钟线
Delay5us(); //延时
return CY;
}
//**************************************
//向I2C总线发送一个字节数据
//**************************************
void I2C_SendByte(uchar dat)
{
uchar i;
for (i=0; i<8; i++) //8位计数器
{
dat <<= 1; //移出数据的最高位
SDA = CY; //送数据口
SCL = 1; //拉高时钟线
Delay5us(); //延时
SCL = 0; //拉低时钟线
Delay5us(); //延时
}
I2C_RecvACK();
}
//**************************************
//从I2C总线接收一个字节数据
//**************************************
uchar I2C_RecvByte()
{
uchar i;
uchar dat = 0;
SDA = 1; //使能内部上拉,准备读取数据,
for (i=0; i<8; i++) //8位计数器
{
dat <<= 1;
SCL = 1; //拉高时钟线
Delay5us(); //延时
dat |= SDA; //读数据
SCL = 0; //拉低时钟线
Delay5us(); //延时
}
return dat;
}
//**************************************
//向I2C设备写入一个字节数据
//**************************************
void Single_WriteI2C(uchar REG_Address,uchar REG_data)
{
I2C_Start(); //起始信号
I2C_SendByte(SlaveAddress); //发送设备地址+写信号
I2C_SendByte(REG_Address); //内部寄存器地址,
I2C_SendByte(REG_data); //内部寄存器数据,
I2C_Stop(); //发送停止信号
}
//**************************************
//从I2C设备读取一个字节数据
//**************************************
uchar Single_ReadI2C(uchar REG_Address)
{
uchar REG_data;
I2C_Start(); //起始信号
I2C_SendByte(SlaveAddress); //发送设备地址+写信号
I2C_SendByte(REG_Address); //发送存储单元地址,从0开始
I2C_Start(); //起始信号
I2C_SendByte(SlaveAddress+1);//发送设备地址+读信号
REG_data=I2C_RecvByte(); //读出寄存器数据
I2C_SendACK(1); //接收应答信号
I2C_Stop(); //停止信号
return REG_data;
}
//**************************************
//初始化MPU6050
//**************************************
void InitMPU6050()
{
Single_WriteI2C(PWR_MGMT_1, 0x00); //解除休眠状态
Single_WriteI2C(SMPLRT_DIV, 0x07);
Single_WriteI2C(CONFIG, 0x06);
Single_WriteI2C(GYRO_CONFIG, 0x18);
Single_WriteI2C(ACCEL_CONFIG, 0x01);
}
//**************************************
//合成数据
//**************************************
int GetData(uchar REG_Address)
{
char H,L;
H=Single_ReadI2C(REG_Address);
L=Single_ReadI2C(REG_Address+1);
return (H<<8)+L; //合成数据
}
//**************************************
//在1602上显示10位数据
//**************************************
void Display10BitData(int value,uchar x,uchar y)
{
value/=64; //转换为10位数据
lcd_printf(dis, value); //转换数据显示
DisplayListChar(x,y,dis,4); //启始列,行,显示数组,显示长度
}
//**************************************
//显示温度
//**************************************
/*
void display_temp()
{ Temp_h=Single_ReadI2C(TEMP_OUT_H); //读取温度
Temp_l=Single_ReadI2C(TEMP_OUT_L); //读取温度
Temperature=Temp_h<<8|Temp_l; //合成温度
Temperature = 35+ ((double) (Temperature + 13200)) / 280; // 计算出温度
lcd_printf(dis,Temperature); //转换数据显示
DisplayListChar(11,1,dis,4); //启始列,行,显示数组,显示位数
}
*/
//*********************************************************
//主程序
//*********************************************************
void main()
{
delay(500); //上电延时
InitLcd(); //液晶初始化
InitMPU6050(); //初始化MPU6050
delay(150);
while(1)
{
Display10BitData(GetData(ACCEL_XOUT_H),2,0); //显示X轴加速度
Display10BitData(GetData(ACCEL_YOUT_H),7,0); //显示Y轴加速度
Display10BitData(GetData(ACCEL_ZOUT_H),12,0); //显示Z轴加速度
Display10BitData(GetData(GYRO_XOUT_H),2,1); //显示X轴角速度
Display10BitData(GetData(GYRO_YOUT_H),7,1); //显示Y轴角速度
Display10BitData(GetData(GYRO_ZOUT_H),12,1); //显示Z轴角速度
delay(5000);
}
}
/*
在论坛上潜水几个月了,跟大家学习两轮平衡车的制作,获益良多。
在调试传感器数据和滤波时写了一个串口工具,趁这个周末整理
了一下,共享出来,希望能对初学者们有所帮助。
功能比较简单,主要是接收单片机发送来的3个整型数据,以图形显示。
可以向单片机发送几个字节,用作控制目的。单片机发送的数据以0xFF作为
帧结束标志,每个整型数以5个字节表示,具体参照下面的函数。
没有经过全面测试,如果你想用但有bug,可以反馈给我 :)
小皮2005
2012.12.08
=============================================================
void Print_Show(int data1, int data2, int data3)
{
SeriPush2Bytes(data1);
SeriPush2Bytes(data2);
SeriPush2Bytes(data3);
uart_transmit(0xff);
}
void SeriPush2Bytes(int value)
{
unsigned char t_char;
if(value < 0) {
value = -value;
uart_transmit(0xf0);
}
else {
uart_transmit(0xf5);
}
t_char = value/256;
if(t_char > 127) uart_transmit(0x80);
else uart_transmit(0x00);
uart_transmit(t_char & 0x7f);
t_char = value%256;
if(t_char > 127) uart_transmit(0x80);
else uart_transmit(0x00);
uart_transmit(t_char & 0x7f);
}
void uart_transmit(char c)
{
while (!(UCSRA & DATA_REGISTER_EMPTY));
UDR=c;
}
chengluoran 发表于 2013-1-31 10:00 static/image/common/back.gif
收藏按钮在1楼末尾。
这个比喻很形象
谢谢!搞定 mark!!!!!! self-balancing 学习了!!! 这个要好好学习下 mark。。。。 mark{:hug:} mark!!!!!{:smile:} 留名~~以前我也就是冲着卡尔曼而用卡尔曼滤波的,在小车上应用效果是差不多的 还是搞不懂,不知道咋用,学习一下 mark
{:smile:}{:smile:} 本帖最后由 济南电子爱好者 于 2013-4-17 17:15 编辑
我对比 卡尔曼,和互补滤波,结果和楼主不太一样,卡尔曼感觉更好一些,还是说我的互补滤波程序写的不对,
bias_cf_p*=0.998; //陀螺仪零飘低通滤波;500次均值;
bias_cf_p+=gyro_m_cf*0.002;
angle_dot_cf_p=gyro_m_cf-bias_cf_p; //这里angle_dot与angle在PID控制中会起到作用
angle_cf_p=(angle_cf_p+angle_dot_cf_p*dt)*0.98+angle_m_cf*0.02;
//加速度低通滤波;50次均值;按200次每秒计算,低通4Hz;
http://player.youku.com/player.php/sid/XNTQyODUxNzU2/v.swf 学习中,mark下 {:lol:}{:lol:}好像不错的样子 mark还需要潜一段时间潜水 受教了,确实不错,高手啊 不错.学习了.
但是用在姿态解算上是啥样对比呢? 这是说明互补完全可以代替卡尔曼吗,那卡尔曼用在哪里的时候才能显出真本事呢 济南电子爱好者 发表于 2013-4-17 17:13 static/image/common/back.gif
我对比 卡尔曼,和互补滤波,结果和楼主不太一样,卡尔曼感觉更好一些,还是说我的互补滤波程序写的不对,
bias ...
一阶互补效果确实不怎么好,你可以试试二阶的。觉得楼主kalman滤波并没有用好,用好了的话kalman应该比互补好些。 个人观点:首先应该保证卡尔曼和互补滤波参数都是最优的,再进行比较。不知道楼主参数怎么确定的? 不错值得关注 暂时看不懂,留下学习 太深奥了,,, Kalman_Filter(mpu_data.x_accel_f, -mpu_data.y_gyro_f);怎么输出参数是四个呢》》?? 如何测量互补滤波的采样率?? 学习了,多谢 学习一下~ yirenonege 发表于 2013-1-31 09:21 static/image/common/back.gif
两者的区别在于,叫法不同。。
不是哟,原理上还是有区别噻。{:3_41:} 济南电子爱好者 发表于 2013-4-17 17:13 static/image/common/back.gif
我对比 卡尔曼,和互补滤波,结果和楼主不太一样,卡尔曼感觉更好一些,还是说我的互补滤波程序写的不对,
bias ...
这个软件叫什么?能发给我一个吗?binfengsxxk@gmail.com谢谢啦 为什么我得出的图形,kalman收敛不没有互补快, 学习中,mark! 路过,看看 果断收藏。。。。。。。。。。。。。 不错,正在研究卡尔曼 mark{:smile:} 好,研究下,很有用的东西了 这个比较,,很直观。。刚才还在犹豫用哪个滤波~~ 标记一下
学习中,mark下 学习中,顶一下 学习学习 wujohn 发表于 2013-8-1 22:28 static/image/common/back.gif
如何测量互补滤波的采样率??
每秒钟执行多少次就是互补滤波器的采样率。 mark下,请问楼主用的是什么型号的传感器啊?
你的校准函数是怎么样的? 楼主,我用互补滤波效果也不错,就只想知道如果把互补滤波用在四轴上,效果怎么样,楼主有试过吗 mark,,,,正好在做。 MARK!!! 123bac 发表于 2013-1-31 20:32
MPU6050是怎样校准的?我的板子平放在桌子上,z轴加速度计读数只有0.756左右,而立起来或测起来x和y的读数分别为一,但z有0.3左右,该如何校准mpu6050采集回来的数据呢? mark一下 mark一下 互补滤波和卡尔曼滤波结果比较 以后肯定会需要,果断收藏 38楼的视频,第一个明显丢失了部分数据吧,看到最后你的传感器大约在水平时,它根本还有很大角度呢。 多谢楼主分享,大爱楼主啊 这个东西只有自己用了才知道 学习了,不错,。。。。。。。。。。。。。。。。 mark!!!!…… mark!!!!…… 楼主好强大啊,能和你做个朋友,聊聊技术上的点点滴滴吗?我的邮箱,709117559@qq.com
学习中,mark下 都是大神啊,学习中
做卡尔曼的时候,楼主怎么获得系统的置信度等数据的。。。 楼主,我觉得你的融合曲线将加速度计跟随的很紧,这样可以站起来吗?我看好多都是跟陀螺仪跟随的很紧,我用的是L3G4200,MMA7361,最近在做平衡车想问一下,加速度计得的噪声大,陀螺仪的温飘存在,低通加速度计,高通陀螺仪,对不对 陀螺仪积分的累计误差没显示出来 学习一下! 请问楼主你卡尔曼滤波的参数是如何整定的?如果方便的话 请分享一下哇!先谢谢啦 学习中~~~ 有图有真象,不错! MARK 这个不能算是真正的KF 卡尔曼滤波之前也有点研究,可惜不精 这只能说你预处理和互补滤波调试选择的参数好,正好和卡尔曼增益一样。 感谢楼主分享! (0.98) * (angle_hb - mpu_data.y_gyro_f * 0.01) + (0.02)*(mpu_data.x_accel_f);这里的0.98时滤波权值吗?怎么整定的? 这样的好帖子咋收藏,哪位大侠叫一下。
页:
[1]
2