kalman滤波测试——波形显示
硬件:1、加速度——LIS3LV02DQ
2、陀螺仪——LY530AL
3、单片机——m168
软件:
1、上位机软件——串口示波器。下载,使用参考:http://www.ourdev.cn/bbs/bbs_content.jsp?bbs_sn=4164014&bbs_page_no=1&bbs_id=1025
2、单片机软件,网上找的kalman滤波,具体代码如下
static const float dt = 0.02;
static float P = {{ 1, 0 },
{ 0, 1 }};
float angle;
float q_bias;
float rate;
static const float R_angle = 0.5 ;
static const float Q_angle = 0.001;
static const float Q_gyro= 0.003;
float stateUpdate(const float gyro_m){
float q;
float Pdot;
q = gyro_m - q_bias;
Pdot = Q_angle - P - P; /* 0,0 */
Pdot = - P; /* 0,1 */
Pdot = - P; /* 1,0 */
Pdot = Q_gyro; /* 1,1 */
rate = q;
angle += q * dt;
P += Pdot * dt;
P += Pdot * dt;
P += Pdot * dt;
P += Pdot * dt;
return angle;
}
float kalmanUpdate(const float incAngle)
{
float angle_m = incAngle;
float angle_err = angle_m - angle;
float h_0 = 1;
const float PHt_0 = h_0*P; /* + h_1*P = 0*/
const float PHt_1 = h_0*P; /* + h_1*P = 0*/
float E = R_angle +(h_0 * PHt_0);
float K_0 = PHt_0 / E;
float K_1 = PHt_1 / E;
float Y_0 = PHt_0;/*h_0 * P*/
float Y_1 = h_0 * P;
P -= K_0 * Y_0;
P -= K_0 * Y_1;
P -= K_1 * Y_0;
P -= K_1 * Y_1;
angle += K_0 * angle_err;
q_bias += K_1 * angle_err;
return angle;
}
测试说明——单片机采集加速度和陀螺仪的信号,并使用上面的kalman滤波,计算出最优倾角,通过串口发送到pc机,pc机运行的串口示波器将相关波形显示出来。
1、红色为加速度换算后的角度。
2、黄色为陀螺仪直接积分后的角度。
3、蓝色为kalman滤波后的角度。
下面是硬件展示:
http://cache.amobbs.com/bbs_upload782111/files_32/ourdev_573236.jpg
(原文件名:100806A000.jpg)
http://cache.amobbs.com/bbs_upload782111/files_32/ourdev_573237.jpg
(原文件名:100806A001.jpg)
http://cache.amobbs.com/bbs_upload782111/files_32/ourdev_573238.jpg
(原文件名:100806A002.jpg)
http://cache.amobbs.com/bbs_upload782111/files_32/ourdev_573239.jpg
(原文件名:100806A003.jpg)
下面是陀螺仪、加速度传感器datasheet: 点击此处下载 ourdev_573240.pdf(文件大小:364K) (原文件名:LIS3LV02.pdf)
点击此处下载 ourdev_573241.pdf(文件大小:576K) (原文件名:Ly530AL.pdf) 调整R_angle 、Q_angle和Q_gyro 后输出波形会有所差异,以下是分别为0.5、0.001、 0.003的波形(慢速连续不断改地变倾角),滤波后角度的变化会优先于加速度的角度。 谢谢,参考一下 不好意思,总是点错按钮,占用了很多楼层
http://cache.amobbs.com/bbs_upload782111/files_32/ourdev_573251.JPG
(原文件名:sb1.JPG) 标记 再上几张,上面sb1的陀螺仪角度偏离加速度角度(向上平移了一段)是由于初始时传感器偏离了一个角度,示波刚开始时角度已经显示了一个角度,但由于传感器没有移动,加速度积分的值依旧是零,所以传感器移动后就出现两个传感器之间有平移错位。sb2是水平启动示波时的波形。另外有几点不明白请高人评评
测量的是x轴,可是沿y轴水平运动时(sb3),沿x轴水平运动(sb4)和沿z轴垂直移动(sb5)时,加速度测量角度很容易达到最大角度值。
http://cache.amobbs.com/bbs_upload782111/files_32/ourdev_573262.JPG
(原文件名:sb2.JPG)
http://cache.amobbs.com/bbs_upload782111/files_32/ourdev_573263.JPG
(原文件名:sb3.JPG)
http://cache.amobbs.com/bbs_upload782111/files_32/ourdev_573264.JPG
(原文件名:sb4.JPG)
http://cache.amobbs.com/bbs_upload782111/files_32/ourdev_573265.JPG
(原文件名:sb5.JPG) 相当牛~ 回复【6楼】yangyh75
-----------------------------------------------------------------------
加速度计需要使用低通滤波抑制运动加速度的脉冲响应
因为加速度计仅仅是提供一个长期稳定,因此低截止频率的低通是可以的 楼主贴个再完整一点的代码吧! mark 回复【楼主位】yangyh75
-----------------------------------------------------------------------
mark 应9楼要求,将main.c原码附上
#include <avr/io.h>
//#include <avr/delay.h>
#include <avr/pgmspace.h>
#include <avr/interrupt.h>
#include <util/twi.h>
#include <math.h>
#include <stdlib.h>
#include"crc16.c"
#include"uart.c"
#include"kalman.c"
#include"twimaster.c"
unsigned char Array={0x01,0x02,0x03,0x04,0x05,0x06,0x07,0x08};
unsigned char Rcvbuf;
static int i;
float y1,y2;
int y3=0;
//输出端口初始化
void PORT_initial(void)
{
DDRB=0B00110000;
PINB=0X00;
PORTB=0X00;
DDRD=0B01100000;
PIND=0X00;
PORTD=0X00;
}
//Timer1初始化
void Timer1Init(void)
{
TCCR1A=0;
TCCR1B=_BV(WGM12)|_BV(CS11);
OCR1A=0x9C40; //20ms //0x4E20; //周期是:10毫秒
TIMSK1|=_BV(OCIE1A);
}
//Timer1 CTC中断,此中断发生周期是:10毫秒
ISR(TIMER1_COMPA_vect)
{
//gyro,acc为外部变量
stateUpdate(gyro);//外部函数
y1=yyh1+(gyro*0.02);
y2=(int)kalmanUpdate(acc);//外部函数
Array=acc;
Array=acc>>8;
Array=(int)y1;
Array=(int)y1>>8;
Array=y2;
Array=y>>8;
Array=y3;//
Array=y3>>8;
CRC16(Array, Rcvbuf,8);//外部函数
for(i=0;i<8;i++)USART_Transmit(Array);//外部函数
USART_Transmit(Rcvbuf);//外部函数
USART_Transmit(Rcvbuf);//外部函数
}
int main(void)
{
PORT_initial();
Timer1Init();
I2C_Init();
USART_initial();
I2C_Start(); //启动twi的中断模式。
SREG|=0b10000000; //开总中断
while(1)
{
;
}
} 关注,黄线是滤波后的波形么? 关注~~ 发现LZ用ARDUINO的哈哈,好亲切啊,想学习卡尔曼滤波,不知道LZ是否方便将ARDUINO下的滤波代码(包括库文件)发给我学习一下,非常感谢!
xfzheng@139.com 我用的是arduino的板子,但编程环境是winavr,arduino就硬件而言就是avr。用arduino的ide只能做简单的事,无法完成复杂的工作,此测试程序没有使用其ide,故无arduino程序。另外重复一下
1、红色为加速度换算后的角度。
2、黄色为陀螺仪直接积分后的角度。
3、蓝色为kalman滤波后的角度。
目前正在研究怎样评估kalman的滤波效果,即怎样的波形才是最优波形。 那能否把完整的代码发我一份呢,包括include的库文件,纯学习用 我也用LIS3LV02DQ测试了,但不知道是否LIS3LV02DQ焊坏了,只有X轴输出正确,Y,Z轴一直保持2047(0x7FF)。另外发现VS2010多了一个Chart控件,再加上原来已有的SerialPort,做一个波形显示很简单。
http://cache.amobbs.com/bbs_upload782111/files_32/ourdev_573702.jpg
(原文件名:acce.jpg)
这个图是X轴输出,正在显示是X输出+2000的波形 回楼上——LIS3LV02DQ是数字接口,如果有x轴信号,芯片不太应该有问题,看看20h寄存器设置是否正确,如果设置为c1h则只有x轴工作,要三轴工作20h寄存器要设置为c7h。另外,当初为了看波形,我也使用VB6(mschart)编写过示波软件,但mschart抖动厉害,看了网上普遍存在抖动问题,加之放大缩小到功能,实在相比这个串口示波器差太远,于是放弃了。
再次附上一个用kb6mcc算法的波形——
http://cache.amobbs.com/bbs_upload782111/files_32/ourdev_573796.JPG
(原文件名:sb_kb6mcc.JPG)
这个算法和先前贴出的算法同样有效,如果不会有效调节R_angle 、Q_angle和Q_gyro 的话,使用这个算法会有着更优秀的输出效果。
以下是kb6mcc算法代码——感谢kb6mcc无私奉献
//----------------------------------------------------------------
void matrix_multiply(float* A, float* B, int m, int p, int n, float* C)
{
int i, j, k;
for (i=0;i<m;i++)
for(j=0;j<n;j++)
{
C=0;
for (k=0;k<p;k++)
C= C+A*B;
}
}
static void matrix_addition(float* A, float* B, int m, int n, float* C)
{
int i, j;
for (i=0;i<m;i++)
for(j=0;j<n;j++)
C=A+B;
}
void matrix_subtraction(float* A, float* B, int m, int n, float* C)
{
int i, j;
for (i=0;i<m;i++)
for(j=0;j<n;j++)
C=A-B;
}
void matrix_transpose(float* A, int m, int n, float* C)
{
int i, j;
for (i=0;i<m;i++)
for(j=0;j<n;j++)
C=A;
}
int matrix_inversion(float* A, int n, float* AInverse)
{
int i, j, iPass, imx, icol, irow;
float det, temp, pivot, factor=0;
float* ac = (float*)calloc(n*n, sizeof(float));
det = 1;
for (i = 0; i < n; i++)
{
for (j = 0; j < n; j++)
{
AInverse = 0;
ac = A;
}
AInverse = 1;
}
for (iPass = 0; iPass < n; iPass++)
{
imx = iPass;
for (irow = iPass; irow < n; irow++)
{
if (fabs(A) > fabs(A)) imx = irow;
}
if (imx != iPass)
{
for (icol = 0; icol < n; icol++)
{
temp = AInverse;
AInverse = AInverse;
AInverse = temp;
if (icol >= iPass)
{
temp = A;
A = A;
A = temp;
}
}
}
pivot = A;
det = det * pivot;
if (det == 0)
{
free(ac);
return 0;
}
for (icol = 0; icol < n; icol++)
{
AInverse = AInverse / pivot;
if (icol >= iPass) A = A / pivot;
}
for (irow = 0; irow < n; irow++)
{
if (irow != iPass) factor = A;
for (icol = 0; icol < n; icol++)
{
if (irow != iPass)
{
AInverse -= factor * AInverse;
A -= factor * A;
}
}
}
}
free(ac);
return 1;
}
float kalman_update(float gyroscope_rate, float accelerometer_angle)
{
staticfloat A = {{1.0, -0.02}, {0.0, 1.0}};
staticfloat B = {{0.02}, {0.0}};
static float C = {{1.0, 0.0}};
static float Sz = {{57.5}};
static float Sw = {{0.005, 0.005}, {0.005, 0.005}};
static float xhat = {{0.0}, {0.0}};
static float P = {{0.005, 0.005}, {0.005, 0.005}};
float u;
float y;
float AP;
float CT;
float APCT;
float CP;
float CPCT;
float CPCTSz;
float CPCTSzInv;
float K;
float Cxhat;
float yCxhat;
float KyCxhat;
float Axhat;
float Bu;
float AxhatBu;
float AT;
float APAT;
float APATSw;
float KC;
float KCP;
float KCPAT;
u = gyroscope_rate;
y = accelerometer_angle;
matrix_multiply((float*) A, (float*) xhat, 2, 2, 1, (float*) Axhat);
matrix_multiply((float*) B, (float*) u, 2, 1, 1, (float*) Bu);
matrix_addition((float*) Axhat, (float*) Bu, 2, 1, (float*) AxhatBu);
matrix_multiply((float*) C, (float*) xhat, 1, 2, 1, (float*) Cxhat);
matrix_subtraction((float*) y, (float*) Cxhat, 1, 1, (float*) yCxhat);
//matrix_multiply((float*) C, (float*) AxhatBu, 1, 2, 1, (float*) Cxhat);
//matrix_subtraction((float*) y, (float*) Cxhat, 1, 1, (float*) yCxhat);
matrix_transpose((float*) C, 1, 2, (float*) CT);
matrix_multiply((float*) C, (float*) P, 1, 2, 2, (float*) CP);
matrix_multiply((float*) CP, (float*) CT, 1, 2, 1, (float*) CPCT);
matrix_addition((float*) CPCT, (float*) Sz, 1, 1, (float*) CPCTSz);
matrix_multiply((float*) A, (float*) P, 2, 2, 2, (float*) AP);
matrix_multiply((float*) AP, (float*) CT, 2, 2, 1, (float*) APCT);
matrix_inversion((float*) CPCTSz, 1, (float*) CPCTSzInv);
matrix_multiply((float*) APCT, (float*) CPCTSzInv, 2, 1, 1, (float*) K);
matrix_multiply((float*) K, (float*) yCxhat, 2, 1, 1, (float*) KyCxhat);
matrix_addition((float*) AxhatBu, (float*) KyCxhat, 2, 1, (float*) xhat);
matrix_transpose((float*) A, 2, 2, (float*) AT);
matrix_multiply((float*) AP, (float*) AT, 2, 2, 2, (float*) APAT);
matrix_addition((float*) APAT, (float*) Sw, 2, 2, (float*) APATSw);
matrix_multiply((float*) K, (float*) C, 2, 1, 2, (float*) KC);
matrix_multiply((float*) KC, (float*) P, 2, 2, 2, (float*) KCP);
matrix_multiply((float*) KCP, (float*) AT, 2, 2, 2, (float*) KCPAT);
matrix_subtraction((float*) APATSw, (float*) KCPAT, 2, 2, (float*) P);
return xhat;
}
///////////////////////////////////////////////////////////////////////////// 回楼上,上电后我就将0x20寄存器设成0xc7, 而且读出来也是0xc7, Y轴有时很大力的甩会有变化,但Z轴不管如何,从没变化过。等我将540陀螺和这个加速度计搞好,跟你学习一下kalman滤波。 研究了好几天,这个kalman滤波非常难缠,始终没能找到怎样评估滤波波形的方法。今天把kalman运用到自平衡小车,发现调高R_angle,滤波会滞后。另外,发现kalman滤波与陀螺仪的零点无关,上个波形——小车能平衡站立,黄色是陀螺仪直接积分的结果,因为静止时陀螺仪输出为一小的负值,故随时间推移,积分曲线一直下探。
http://cache.amobbs.com/bbs_upload782111/files_32/ourdev_574662.JPG
(原文件名:sb_xc.JPG) 学习学习 再学习 真不错,LZ可否试验下用指尖敲击陀螺仪?
前面我用LY530ALH时发现抗震性能不敢恭维! 回复【21楼】yangyh75
-----------------------------------------------------------------------
请问调用stateUpdate(const float gyro_m)时,gyro_m是什么单位?是(degrees/sec)吗?还有kalmanUpdate(const float incAngle)时,incAngle是直接从LIS3LV02DQ起出的值吗?
还请问方便留下email或QQ联系? 回复【21楼】yangyh75
研究了好几天,这个kalman滤波非常难缠,始终没能找到怎样评估滤波波形的方法。今天把kalman运用到自平衡小车,发现调高r_angle,滤波会滞后。另外,发现kalman滤波与陀螺仪的零点无关,上个波形——小车能平衡站立,黄色是陀螺仪直接积分的结果,因为静止时陀螺仪输出为一小的负值,故随时间推移,积分曲线一直下探。
(原文件名:sb_xc.jpg)
引用图片
-----------------------------------------------------------------------
用MEMS器件做惯导应用系统,低通滤波很重要。
你说的“静止时陀螺仪输出为一小的负值”这个是陀螺仪的零点漂移,多产生于MEMS器件、运放及ADC采样等模拟电路的温漂。
解决办法主要有动态与静态两方面:
静态的需要调整陀螺仪、运放及ADC采样电路的工作点,要求精度高的可加入温度补偿反馈,这是硬件方法
软件可以在ADC后得到的数字量后加入零偏寄存器矫正静态误差,这个在很多IMU产品中都是最常用的方法
比如ADIS全系列数字IMU都有这样的寄存器。
动态的多用软件算法实现,比如德国四轴中的长时加权平均求中点的方法,其实就是软件实现一阶积分低通滤波。 请教楼主 发送的数据是无符号的字符数组 怎么显示出小于0的波形啊? 顶一下 Mark ST的元件電路有個140HZ的低通濾波,加上後抑制震動信號很有用,原廠說不加可以。但是用過之後,還是不要偷懶。 mark mark 看来也要装个VS2010了,有CHART方便多了 学习,顶 mark MARK mark 楼主 能发下串口通信的子程序吗困饶了好几天了!!!!!! 发送到示波器显示,要CRC,难道不要字头标识吗?请楼主指点下 还是我来解释一下吧:
1、关于函数void CRC16(unsigned char *Array, unsigned char *Rcvbuf,unsigned int Len)需要明白以下:
a、*Array是数组Array的首地址,这个数组存放你所需要发送的4个通道数据,一个通道有高位和低位,所以4个通道需
要8个unsigned char。
b、*Rcvbuf是数组Rcvbuf的首地址,这个数组存放校验码,同样也是高位和低位。
c、Len是你发送的位长,调用时指定为8就ok。
调用语句如下:CRC16(Array, Rcvbuf,8);
紧接着发送 Array数组,然后发送Rcvbuf数组,就ok了。
12楼的例程里面——CRC16(Array, Rcvbuf,8);//外部函数
for(i=0;i<8;i++)USART_Transmit(Array);//外部函数
USART_Transmit(Rcvbuf);//外部函数
USART_Transmit(Rcvbuf);//外部函数
这里再说明一下——发送Array数组时从下标0到下标7顺序发送,而发送Rcvbuf数组时先发送Rcvbuf,再发送Rcvbuf
这是因为CRC16函数的Rcvbuf是计算出来的校验码高位,Rcvbuf是低位。这个串口示波器要求先发
送低位在发送高位。
它会将顺序接收到的两个8位数据拼成一个WORD,前4个WORD分别对应4个通道,最后的一个WORD用于检验数据是否正确。
2、要使用哪个串口示波器应该将你的COM口设置为COM1,波特率设置为9600,因为这是不出钱的。
3、示波器的菜单栏的setup——communication protocol——设置为CRC16。
这样解释是否明白了,它要求每次发送10组数据,其中8组是你的4个通道数据,2组是校验码。
祝好运。 回复【39楼】yangyh75
-----------------------------------------------------------------------
现在的示波器还是没有波形显示,画面没有一点动作。设置没问题CRC16,COM1,9600。不过选CHECK SUM方式,有杂乱波形,选CRC16没显示。
我用串口调试观察 ,数据发送的正确,就是不知道CRC对不对,我设置的前8组数据每次都一样,为什么CRC的后两组的值还是一直变化,是这个问题吗?下面是我的程序,给纠正下!!!
void main()
{
InitSysCtrl();
#ifdef FLASH
MemCopy(&RamfuncsLoadStart, &RamfuncsLoadEnd, &RamfuncsRunStart);
InitFlash();
#endif
SpiInitGpio();
SciaInitGpio();
DINT;
InitPieCtrl();
IER = 0x0000;
IFR = 0x0000;
InitPieVectTable();
ADCSetup();
spi_init();
SCIAInit();
GpioDataRegs.GPADAT.bit.GPIO1 = 1;
n=0;
for(;;)
{ //adcspi(8);
//spi_rdytx(rdata);
//scitx(rdata);
ADCTX(); 这是对8组数据的设置,SCI-TX到。
//crc_num();
//crc();
CRC16(); CRC程序,求出CRC的两组数据。
scitx_ready(); 发送子程序 ,依次发送10组数据。
}
}
void ADCTX()
{ //Uint16 t,x;
//t= AdcResult.ADCRESULT0;
//x=t*3/5;
//sci_tx = x<<4;
//sci_tx = x>>4;
//sci_tx=(t&0x0f80)>>7;
//sci_tx=t&0x007f;
sci_tx = 0xff; 这是成每次都是一样的数据 ,共八组。
sci_tx = 0xff;
sci_tx = 0xff;
sci_tx = 0xff;
sci_tx = 0x00;
sci_tx = 0x00;
sci_tx = 0x00;
sci_tx = 0x00;
}
void CRC16()
{
//unsigned short CRC;
unsigned char i,j;
CRC = 0xffff;
for (i=0;i<10; i++){
CRC ^= sci_tx;
for (j=0;j<8;j++) {
if (CRC & 0x01)
CRC = (CRC >>1 ) ^ 0xa001;
else
CRC = CRC >> 1;
} CRC的子程序,把2组CRC数据排在后两组,第8和9组。
}
sci_tx=(CRC&0xff00)>>8;//高位
sci_tx=(CRC&0x00ff);//低位
}
void scitx_ready()
{ int i;
for(i=0;i<10;i++)
{
while(SciaRegs.SCICTL2.bit.TXRDY != 1); 串口发送 一次发送10组!!
SciaRegs.SCITXBUF = sci_tx;
}
} 兄台,你这个自己改写的CRC16函数肯定不对嘛。for (i=0;i<10; i++){
——你想要表达什么?10个BIT还是什么?你需要检验的是8 BIT,产生2 BIT的检验码,一共发送10 BIT。
拜托,请不要怀疑我给出的代码,只要你按我给的代码,它不可能不显示波形的。你要自己改写函数,为什么不先用我给的函数,测试出波形后自己再改写呢,因为你不能保证你改写正确,这样自己会有很多疑虑。 顶起来
lz 的万用板 多少钱一块
发现电子市场卖的这种板 较贵2块钱一块
大家有打板的没有 可以分点 回复【41楼】yangyh75
-----------------------------------------------------------------------
确实错误 ,一直没发现 再试试 mark,好贴 mark 楼主可以发个这个AVR的源程序吗? 回复【19楼】yangyh75
-----------------------------------------------------------------------
您好:请问一下R_angle 、Q_angle这些参数具体的含义呢?调节的意义在哪里? mark 正需要这个,收藏。 MARK 需要,标记一下 需要,标记一下 mark 膜拜下传说中的卡尔曼滤波器~~ kalman mark 学习 mark mark一下,我也在研究卡尔曼,最后发现很慢 好资料啊,搞了半天用互补滤波很不理想 不错,经典,好贴啊 mark mark {:handshake:}
关注~~ 楼主给个完整程序吧。 yangyh75 发表于 2010-8-8 19:19
回楼上——LIS3LV02DQ是数字接口,如果有x轴信号,芯片不太应该有问题,看看20h寄存器设置是否正确,如果设 ...
楼主,你这个程序的卡尔曼滤波函数入口参数只有两个,float kalman_update(float gyroscope_rate, float accelerometer_angle)。代表的是什么啊?同一个方向加速度计个陀螺仪输出的角度值?
页:
[1]