FPGA的Kalman滤波器的设计

FPGA的Kalman滤波器的设计

摘要：针对电路设计中经常碰到数据的噪声干扰现象，提出了一种Kalman滤波的FPGA实现方法。该方法采用了TI公司的高精度模数转换器ADSl25l以及Altera公司的EPlCl2，首先用卡尔曼滤波算法设计了一个滤波器，然后将该滤波器分解成简单的加、减、乘、除运算。通过基于FPGA平台的硬件与软件的合理设计，成功地实现了数据噪声的滤除设计，并通过实践仿真计算，验证了所实现滤波的有效性。

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

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

式（3）为系统噪声。设设备的量测噪声为Vk，系统得量测方程为

式中，是利用当前状态预测的结果，是当前状态最优的结果，Pk+1／k是*对应的covariance，Rk是对应的covanance，表示A的转置矩阵，Q是系统过程的covariance。式(1)，式(2)就是卡尔曼滤波器5个公式中的前两个，也就是对系统的预测。可以得到将来状态k+l的最优化估算值。
    式(9)中I为单位矩阵，对于单模型单测量，I=1。当系统进入k+2状态时，Pk+1就是式(2)的Pk。这样，算法就可以自回归的运算下去。

    R为测量数据的方差；Q为系统过程噪声的方差，取值为，根据经验可设置为测量数据方差的1／10；P的初值可选为单位阵，I为单位阵，X的处置可选为(0，0)T。算法示意图，如图1所示。

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

    这些公式包含了一些加、减、乘、除的数学运算，用FPGA实现卡尔曼滤波就是用FPGA实现这些运算。

2 硬件设计
2．1 A／D选择
    ADSl251是TI公司生产的一种高精度、宽动态范围的24位△～∑乏结构的模数转换器。采样保持器的输入形式采用差分模拟输入方式，设计了一个简单的电平转换电路使得端和端之间的电压波动范围为士2．5 V。ADSl251的时钟频率由8 MHz的外部晶振提供，系统时钟由FPGA给出。ADSl251的内部结构图，如图2所示。

    输出有3种模式：同步、异步及掉电模式，异步模式与Kalman滤波器数据输入接口连接比较方便。其时序如图3所示。

2．2 FPGA设计
    FPGA的特点在于灵活，通用。如果资源够用，可以实现各种数字组合，包括CPU。本系统采用的：FPGA是Ahera公司低价位的EP1C12，该芯片内含有两个PLLs，12060LEs及239616 Total RAMbits。在本系统的设计中，FPGA内主要实现以下几个功能：Kalman状态控制信号、Kalm-an滤波运算、A／D控制模块、时钟及波特率设置等。卡尔曼滤波器的系统结构图，如图4所示。

    通过A／D转换芯片采集转换数据后，进入FPGA的A／D控制模块，在Kalman状态控制信号的作用下，进行滤波运算，然后通过控制接口将滤波数据输出。由此可见，卡尔曼滤波的实现包括两部分：Kalman状态控制信号和Kalman滤波运算。

3 Kalmaft滤波器的设计
    在FPGA中实现Kalman滤波器，重点在于平衡资源利用率和处理速度、数据运算精度之间的矛盾，难点在于浮点加、减、乘、除的硬逻辑及卡尔曼滤波流程控制的实现。一般地，实际应用中，数字采样重复频率是待采样信号最高频率成分的5倍以上能保证较好恢复原信号，本设计中出于后期升级的考虑，将采样率设置在800次／s，是输出信号带宽的20倍。
    通过第2节中的分解，Kalman滤波器由16次加法、20次乘法、1次除法以及必要的输入、输出及循环控制组成，共45步。状态控制器(Kal-man状态控制信号)实际是一个有限状态机，分别控制滤波器中的45步运算，决定其执行顺序，每一步利用计数器为其提供必要的时钟周期。
    Kalman滤波运算主要是浮点计算，浮点表示常用的标准是IEEE 754，IEEE二进制浮点数算术标准(IEEE754)是最广泛使用的浮点数运算标准，为许多CPU与浮点运算器所采用。IEEE 754规定了4种表示浮点数值的方式：单精确度(32位元)、双精确度(64位元)、延伸单精确度(43位元以上，较少使用)与延伸双精确度(79位元以上，通常以80位元实做)。
    二进制浮点数是以符号数值表示法格式储存，将最高效位元指定为符号位元(Sign Bit)；“指数部分”，即次高效的e位元，为浮点数中经指数偏差(Exponent Bias)处理过后的指数；“小数部分”，即剩下的f位元，为有效位数(Significand)减掉有效位数本身的最高效位元，如图5所示。

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

其中，31位是符号位；0表示正；1表示负；30～23位为阶数；22～0表示数值的有效位。偏正值为+127。其表示的具体值可用式(27)表示
   
    指数部分采用一个无符号的正数值存储。单精度的指数部分是-126～+127加上127，指数值的大小为1～254(0和255是特殊值)。浮点小数计算时，指数值减去偏正值将是实际的指数大小。

    由于资源的限制，滤波中的运算过程只能以有限的精度进行，但过低的精度容易引起滤波发散，同时为了兼顾A／D高达24 bit的分辨率精度，在IEEE754单精度浮点数格式的基础上按照浮点表示的原则做了改造，将指数部分减少到6位，尾数增加到25位，25位尾数可以完全表示A／D输出的24位采样值，并削弱舍入误差的影响，其格式如图7所示。

    本系统对处理速度的要求并不高，运算部件的设计并不考虑速度优化。
    加法器按以下通用的规则设计：
    (1)阶码相减：两个操作数阶码相减，求得阶差d=|Ea-Eb|；
    (2)尾数对齐：阶码小的操作数的尾数右移d位；
    (3)尾数相加：完成移位处理后的尾数相加运算；
    (4)转换：尾数相加的结果为负数时，转换为符号尾数的表示方式；
    (5)前导0／1的判定：判定由于减法结果产生的左移位数，对于加法判定是否右移一位或不移，对前导0／1判定的结果进行编码以驱动规格化移位；
    (6)规格化：规格化有效位并且根据移位的方向和位数修改最终的阶码Ef；
    (7)舍入：根据IEEE标准判定最终结果的舍入，如果需要入，则最末有效位加1；如舍入引起溢出，则需要有效位右移1位，同时阶码Ef加1。
    乘法器的设计相对简单，与加法器一样采用简单的设计规则：
    (1)指数相加：完成两个操作数的指数相加运算；
    (2)尾数调整：将尾数f调整为l／f的补码格式；
    (3)尾数相乘：完成两个操作数的尾数相乘运算；
    (4)规格化：根据尾数运算结果调整指数位，对尾数进行舍入截位操作，规格化输出结果。
    采用Altera提供的IP核，除法器能完成IEEE754单精度除法运算。数据进入该除法器前需要将自定义的浮点格式的阶码扩展为8位加上95，尾数右移两位，转换为标准单精度浮点，计算结果再由标准单精度浮点的按相反的方法转换为自定义浮点。
    文中硬件编程语言是用Verilog实现的。

4 实测结果分析与结论
    从系统输出数据中随机抽取一段数据用Matlab分析，其结果如图8所示。实线C表示的是滤波前的噪声：虚线B表示的是滤波后的噪声。很容易看出经过滤波后噪声降低了很多。仿真与实测对比分析如表1所示。

5 结束语
    卡尔曼滤波器在很多领域具有重要的作用，可以实现数字信号处理功能，并能满足实时性的要求。用FPGA实现Kalman滤波过程，实现简便、成本较低、滤波效果好，被广泛应用于图像视频、陀螺惯导去噪、电子罗盘、雷达接收机目标跟踪处理、目标预测等图像、数据采集与处理领域。