基于FPGA的Kalman滤波器的设计

电路

2010年第23卷第2期

Electr onic Sci 1&Tech 1/Feb 115,2010

收稿日期:2009208212

作者简介:米月琴(1982-),女,硕士。研究方向:伺服系统设计。黄军荣(1983-),男,硕士,工程师。研究方向:高校教育管理。

基于FPG A 的Kalman 滤波器的设计

米月琴1

,黄军荣

2

(11航天恒星空间技术应用有限公司卫星通信事业部,陕西西安　710077;

21西安电子科技大学经济管理学院,陕西西安　710071)

　　摘　要　针对电路设计中经常碰到数据的噪声干扰现象,提出了一种Kal m an 滤波的FPG A 实现方法。该方法采用

了TI 公司的高精度模数转换器ADS1251以及A ltera 公司的EP1C12,首先用卡尔曼滤波算法设计了一个滤波器,然后将该滤波器分解成简单的加、减、乘、除运算。通过基于FPG A 平台的硬件与软件的合理设计,成功地实现了数据噪声的滤除设计,并通过实践仿真计算,验证了所实现滤波的有效性。

关键词　卡尔曼;FPG A;最小方差估计

中图分类号　TP274　　文献标识码　A 　　文章编号　1007-7820(2010)02-052-04

D esi gn of the FPGA 2ba sed Ka l m an F ilter

M i Yueqin 1

,Huang Junr ong

2

(11Depart ment of Satellite Communications Services,Space Star Aer os pace

Technol ogy App licati ons Co 1L td .,Xi πan 710077,China;

21School of Econom ics and Management,Xidian University,Xi πan 710071,China )

Abstract 　In circuit design,we often encounter input data noise .A Kal man filter based on the FPG A i mp le 2mentati on is p r oposed t o achieve the de 2noising .Kal man filter is a common filtering method,which uses TI πs high 2p recisi on ADS1251ADC and EP1C12of A ltera Cor poration .A Kal man filter is designed and then deco mposed int o si m p le computations such as additi on,subtraction,multip lication,and division .A data noise filter is successfully realized based on the hardware and s oft w are of FPG A p latfor m.Si m ulation results verify its validity .

Keywords 　Kal man;FPG A;least equation esti mate

　　卡尔曼滤波是一个“Op ti m al Recursive Data Pr ocessing A lgorith m (最优化自回归数据处理算法)”,对于解决很大部分的问题,是最优化的,效率最高甚至是最有用的。传统的卡尔曼滤波是在DSP 上实现的。但是DSP 成本相对较高,而且指令是串行执行的,不能满足有些要求较高的场合。而FPG A 由于其硬件结构决定了它的并行处理方式,无论在速度还是实时性都更胜一筹。文中以基于FPG A 器件和A /D 转换器的数据采集系统为硬件平台,进行了卡尔曼滤波算法设计,详述了基于FPG A 的卡尔曼滤波器的设计实现。

1　卡尔曼滤波算法

工程中,为了了解工程对象(滤波中称为系统)

的各个物理量(滤波中称为状态)的确切数值,或为了达到对工程对象进行控制的目的,必须利用测量手段对系统的各个状态进行测量。但是,量测值可能仅是系统的部分状态或是部分状态的线性组合,且量测值中有随机误差(常称为量测噪声)。最优估计就是针对上述问题的一种解决方法。它能将仅与部分状态有关的测量进行处理,得出从统计意义上讲误差最小的更多状态的估值。误差最小的标准常称为估计准则,根据不同的估计准则和估计计算方法,有各种不同的最优估计,卡尔曼滤波是一种递推线性最小方差估计的最优估计

[1]

。

系统的状态方程可设定为

X k +1/k =AX k +BW k

(1)

式中

X k =

x k x k -1(2)

W k =w k w k -1

(3)

2

5

米月琴,等:基于FPG A 的Kal m an 滤波器的设计

电子・电路

式(3)为系统噪声。设设备的量测噪声为V k ,系

统得量测方程为

Y k +1=CX k +1/k +V k ,k =1,2,3,…,n

(4)式中,C =10

,Y k +1为设备的量测值[2]

。

噪声序列W k ,V k 满足统计特性:E (W k )=E (V k )=0;cov (W k )=Q,cov (V k )=R,cov (W k ,V k )=0,构造如下的滤波器

X k +1/k =A X k

(5)P k +1/k =A P k A T

+B Q B

T (6)K k +1=P k +1/k C T

×(C P k +1/k ×C T

+R )

-1

(7)X k +1/k =X k +1/k +K k +1[Y k +1-C X k +1/k ]

(8)P k +1=(I -K k +1C )P k +1/k

(9)

式中,X k +1/k 是利用当前状态预测的结果,X k 是当前状态最优的结果,P k +1/k 是X k +1/k 对应的covariance ,

P k 是X k 对应的covariance ,A T

表示A 的转置矩阵,Q

是系统过程的covariance 。

式(1),式(2)就是卡尔曼滤波器5个公式中的前两个,也就是对系统的预测。可以得到将来状态k +1的最优化估算值X k +1。

式(9)中I 为单位矩阵,对于单模型单测量,I =1。当系统进入k +2状态时,P k +1就是式(2)的P k 。这样,算法就可以自回归的运算下去。

R 为测量数据的方差;Q 为系统过程噪声的方

差,取值为

δ2a

0δ2

a

,根据经验可设置为测量数据方

差的1/10;P 的初值可选为单位阵,I 为单位阵,X

的处置可选为(0,0)T

。算法示意图,如图1所示。

图1　算法示意图

将算法矩阵展开并写成乘加形式,令

A =

112312-012312

10(10)B =

1-01396

00

(11)δa =0100719;R =010224

(12)A 1=a 1×a 1

(13)P 11=A 1×P 00+a 1×a 2×a 3×P 02+A 2×P 03+δa

(14)

P 12=a 1×a 3×P 00+a 2×a 3×P 02

(15)

P 21=a 1×a 3×P 00+a 2×a 3×P 01

(16)P 22=A 3×P 00(17)W =1/(P 11+R )(18)P 0=P 11-P 11×W ×P 11(19)P 1=P 12-P 11×W ×P 12(20)P 2=P 21-P 11×W ×P 21(21)P 3=P 22-P 12×W ×P 21

(22)I 2=X 01,

I 1=X 00

(23)X 1=a 1×I 2+a 2×I 1+P 11×

W ×(Y -I 2)

(24)X 0=a 3×I 2+P 21×W ×(Y -I 2)

(25)Y S =X 1

(26)

这些公式包含了一些加、减、乘、除的数算,

用FPG A 实现卡尔曼滤波就是用FPG A 实现这些运算。

2　硬件设计

211　A /D 选择

ADS1251是TI 公司生产的一种高精度、宽动态

范围的24位△～

∑乏结构的模数转换器。采样保

持器的输入形式采用差分模拟输入方式,设计了一个

简单的电平转换电路使得端和端之间的电压波动范围为士215V 。ADS1251的时钟频率由8MHz 的外部晶振提供,系统时钟由FPG A 给出。ADS1251的内部结构图,如图2所示

[3]

。

图2　ADS1251的内部结构图

输出有3种模式:同步、异步及掉电模式,异步

模式与Kal m an 滤波器数据输入接口连接比较方便。其时序如图3所示。212　FPG A 设计

FPG A 的特点在于灵活,通用。如果资源够用,可以实现各种数字组合,包括CP U 。本系统采用的FPG A 是A ltera 公司低价位的EP1C12,该芯片内含有两个P LL s,12060LEs 及239616T otal RAMbits [4]

。在本系统的设计中,FPG A 内主要实现以下几个功能:Kal m an 状态控制信号、Kal m an 滤波运算、A /D 控制

模块、时钟及波特率设置等[5,6]

。卡尔曼滤波器的系统结构图,如图4所示。

3

5

电子・电路

米月琴,等:基于FPG A 的Kal m an

滤波器的设计

图3　

异步模式时序图

图4　系统结构框图

通过A /D 转换芯片采集转换数据后,进入FPG A

的A /D 控制模块,在Kal m an 状态控制信号的作用下,进行滤波运算,然后通过控制接口将滤波数据输出[7]

。由此可见,卡尔曼滤波的实现包括两部分:Kal m an 状态控制信号和Kal m an 滤波运算。

3　Kal m an 滤波器的设计

在FPG A 中实现Kal m an 滤波器,重点在于平衡资源利用率和处理速度、数据运算精度之间的矛盾,难点在于浮点加、减、乘、除的硬逻辑及卡尔曼滤波流

程控制的实现[8]

。一般地,实际应用中,数字采样重复频率是待采样信号最高频率成分的5倍以上能保证较好恢复原信号,本设计中出于后期升级的考虑,将

采样率设置在800次/s,是输出信号带宽的20倍[9]

。

通过第2节中的分解,Kal m an 滤波器由16次加法、20次乘法、1次除法以及必要的输入、输出及循环控制组成,共45步。状态控制器(Kal m an 状态控制信号)实际是一个有限状态机,分别控制滤波器中的45步运算,决定其执行顺序,每一步利用计数器为其提供必要的时钟周期。

Kal m an 滤波运算主要是浮点计算,浮点表示常用的标准是I EEE 754,I EEE 二进制浮点数算术标准(I EEE 754)是最广泛使用的浮点数运算标准,为许多CP U 与浮点运算器所采用。I EEE 754规定了4种表示

浮点数值的方式:单精确度(32位元)、双精确度(

位元)、延伸单精确度(43位元以上,较少使用)与延伸双精确度(79位元以上,通常以80位元实做)。

二进制浮点数是以符号数值表示法格式储存,将最高效位元指定为符号位元(Sign B it );“指数部分”,即次高效的e 位元,为浮点数中经指数偏差(Expo 2nent B ias )处理过后的指数;“小数部分”,即剩下的f

位元,为有效位数(Significand )减掉有效位数本身的最高效位元,如图5所示

。

图5　I EEE 754浮点格式

文中采用的单精度二进制小数,使用32个位元存储,如图6所示

。

图6　I EEE 754单精度二进制小数格式

其中,31位是符号位;0表示正;1表示负;30～23位为阶数;22～0表示数值的有效位。偏正值为+127。其表示的具体值可用式(27)表示

V =(-1)s ×(1/f )×2e

-127

(27)指数部分采用一个无符号的正数值存储。单精度的指数部分是-126～+127加上127,指数值的大小为1～254(0和255是特殊值)。浮点小数计算时,指数值减去偏正值将是实际的指数大小。

由于资源的,滤波中的运算过程只能以有限的精度进行,但过低的精度容易引起滤波发散,同时为了兼顾A /D 高达24bit 的分辨率精度,在I EEE 754单精度浮点数格式的基础上按照浮点表示的原则做了改造,将指数部分减少到6位,尾数增加到25位,25位尾数可以完全表示A /D 输出的24位采样

4

5

米月琴,等:基于FPG A 的Kal m an 滤波器的设计

电子・电路

值,并削弱舍入误差的影响,其格式如图7所示

。

图7　自定义浮点格式

本系统对处理速度的要求并不高,运算部件的设计并不考虑速度优化。

加法器按以下通用的规则设计[10]

:

(1)阶码相减:两个操作数阶码相减,求得阶差

d =E a -E b ;

(2)尾数对齐:阶码小的操作数的尾数右移

d 位;

(3)尾数相加:完成移位处理后的尾数相加运算;(4)转换:尾数相加的结果为负数时,转换为符

号尾数的表示方式;

(5)前导0/1的判定:判定由于减法结果产生的左移位数,对于加法判定是否右移一位或不移,对前导0/1判定的结果进行编码以驱动规格化移位;

(6)规格化:规格化有效位并且根据移位的方向和位数修改最终的阶码E f ;

(7)舍入:根据I EEE 标准判定最终结果的舍入,如果需要入,则最末有效位加1;如舍入引起溢出,则需要有效位右移1位,同时阶码E f 加1。乘法器的设计相对简单,与加法器一样采用简单的设计规则:

(1)指数相加:完成两个操作数的指数相加运算;

(2)尾数调整:将尾数f 调整为1/f 的补码格式;(3)尾数相乘:完成两个操作数的尾数相乘

运算;

(4)规格化:根据尾数运算结果调整指数位,对尾数进行舍入截位操作,规格化输出结果。

采用A ltera 提供的I P 核,除法器能完成I EEE 754单精度除法运算。数据进入该除法器前需要将自定义的浮点格式的阶码扩展为8位加上95,尾数右移两位,转换为标准单精度浮点,计算结果再由标准单精度浮点的按相反的方法转换为自定义浮点。

文中硬件编程语言是用Veril og 实现的。

4　实测结果分析与结论

从系统输出数据中随机抽取一段数据用Matlab 分析,其结果如图8所示。实线C 表示的是滤波前的噪声:虚线B 表示的是滤波后的噪声。很容易看出

经过滤波后噪声降低了很多。仿真与实测对比分析如

表1所示

。

图8　噪声数据分析表1　仿真与实测对比分析原始数据

M atlab 处理结果

本系统处理结果

均值130151613015161301516方差012981010778010927协方差

0108

010060

010099

5　结束语

卡尔曼滤波器在很多领域具有重要的作用,可以实现数字信号处理功能,并能满足实时性的要求。用FPG A 实现Kal m an 滤波过程,实现简便、成本较低、

滤波效果好,被广泛应用于图像视频、陀螺惯导去噪、电子罗盘、雷达接收机目标跟踪处理、目标预测等图像、数据采集与处理领域。参考文献

[1]　付梦印,邓志红,张继伟.Kal m an 滤波理论及其在导

航系统中的应用[M ].北京:科学出版社,2003.[2]　蒋志凯.数字滤波与卡尔曼滤波[M ].北京:中国科学

技术出版社,1993.[3]　周敏,叶敦范,梁庆九.ADS1251在高精度采集中的应

用[J ].世界电子元器件,2004(8):58-60.[4]　罗朝霞,高书莉.CP LD /FPG A 设计及应用[M ].北京:

人民邮电出版社,2007.[5]　刘波.精通Veril og HDL 语言编程[M ].北京:电子工

业出版社,2007.[6]　黄智伟,陈琼,潘礼,等.FPG A 系统设计与实现[M ].

北京:电子工业出版社,2005.[7]　陈刚,郭立,史洪生,等.最大平行结构的Kal m an 滤

波器的硬件电路设计[J ].微电子学与计算机,2006,23(6):34-37,41.[8]　李国欣,徐艳丽,蒋永洲.卡尔曼滤波器在倒立摆控制

系统中的应用[J ].长春大学学报,2009(6):12-14,34.[9]　韩梅,陈禾.卡尔曼滤波器的FPG A 实现[J ].电子工

程师,2006,32(7):38-40.

[10]Uwe Meyer Baese,刘凌,胡永生.数字信号处理的FP 2

G A 实现[M ].北京:清华大学出版社,2003.

5

5