1、基于FPGA的Kalman滤波器实现研究：卡尔曼kalman滤波计算精度和速度是工程应用中是否成功的决定性条件，为进一步提高kalman滤波算法在更复杂的环境下使用的性能，并能够同时满足实时性和精度的要求，采用现场可编程逻辑阵列fpga技术，设计了kalman滤波算法在fpga上的实现方案，选择了一种可以同时满足精度和实时性的方案进行实现，对算法中的矩阵相乘、状态机的应用以及资源分时复用等关键技术进行了设计。通过与matlab及dsp的计算结果相比照，验证了在fpga内实现kalman滤波器的优势。关键词：fpga； kalman滤波器； ip核； 实时性 kalman滤波理论1在20世纪60
2、年代一经提出，便得到了军事、控制、通信等领域的极广泛的应用。它可以实现随机干扰下的线性动态系统的最优估计，目前kalman滤波器的实现方式主要有两种，一是在pc机上实现，可以同时满足计算精度和实时性的要求，但是pc机体积大，质量重，本钱高；二是通过dsp等芯片来实现，用这种方式实现的kalman滤波器虽然体积小，质量轻，但是因其指令顺序执行的cpu架构，在系统复杂时无法满足系统的实时性要求。随着控制系统的复杂性的提高，系统的阶次变大，如组合导航系统的滤波，其滤波的阶次一般都要18阶，如果对系统进一步细化建模或增加其复杂性，其滤波阶次可以到达几十阶。因此，kalman滤波器在工程应用中的实现遇到
3、了系统体积、重量、本钱和系统精度、速度等性能不能兼顾的问题。随着现代电子技术的开展，fpga具有系统结构和逻辑单元灵活、集成度高以及适用范围宽等特点，可以很好地解决这个难题。因为fpga采用的是硬件并行算法，能很好的解决速度和实时性的问题，并且其具有灵活的可配置特性和优良的抗干扰能力，使得fpga构成的数字信号处理系统非常易于修改、测试及硬件升级。随着fpga技术的不断成熟，其内嵌资源不断丰富，硬核乘法器和块ram的数目不断增长，使得fpga实现复杂的数字信号处理算法变得更为简单和快速。因此，本文对fpga技术和kalman滤波算法进行结合研究，探索kalman滤波算法在fpga中的实现方式并
4、进行性能验证，以对基于fpga的kalman滤波算法的工程实现提供参考。1kalman滤波算法理论kalman滤波是在时域内以信号的一、二阶统计特性为前提、以均方误差极小为判据，能自动跟踪信号统计性质的非平稳变化，具有递归性质的一种算法。它处理的对象是随机系统，并能正确估计出有用信号。设离散系统差分方程如下：xk+1=k+1,kxk+tk+1,kuk+k+1wkzk+1=hk+1xk+1+vk+1那么kalman滤波方程组如下：状态一步预测方程：k+1/k=k+1,kk/k+tk+1,kuk(1)一步预测均方误差方程：pk+1/k=k+1,kpk/ktk+1.k+k+1,kqktk+1,k(2
5、)滤波增益方程：kk+1=pk+1/khtk+1(hk+1pk+1/khtk+1+rk+1)-1(3)状态估值方程： k+1/k+1=k+1/k+kk+1(zk+1-hk+1k+1/k)(4)估计均方误差方程：pk+1/k+1=(i-kk+1hk+1)pk+1/k(i-kk+1hk+1)t+kk+1rk+1ktk+1(5)或pk+1/k+1=(i-kk+1hk+1)pk+1/k(6)从式16可知，假设利用传统的处理器实现kalman滤波算法，由于其指令执行的顺序性，至少需要分为5步来实现，其中每一步还都需要进行至少1次的加法和乘法等运算，每次运算都要顺序执行，其执行速度和效率很低；如果利用fp
6、ga来进行kalman滤波，根据其各步的逻辑关系，可以分为3步来实现，即第一步计算状态一步预测值k+1/k和一步预测均方误差pk+1/k，第二步计算滤波增益kk+1，第三步计算状态最优估值k+1/k+1和估计均方误差pk+1/k+1。由此可知，利用fpga技术可以实现kalman滤波的并行计算2，压缩计算时间，提高解算速度。因此，对fpga的kalman滤波进行研究开发，可实现基于fpga的快速kalman滤波解算，满足在对实时性要求更高的环境中使用。2在fpga中实现kalman滤波算法研究由于fpga实现kalman滤波3解算速度非常快，假设利用fpga 的串行口依次输入观测值，由于数据串
7、行输入的特点，会使fpga的解算局部等待数据接收完毕才能执行滤波解算，导致整体的解算时间过长。为检验fpga实现kalman滤波器的计算性能，本文预先将观测值输入并保存于fpga内的rom中，以使fpga可以连续地进行滤波解算，实现方案原理如图1所示。图1采用fpga实现 kalman滤波算法原理框图图1中，kalman滤波解算在fpga内完成，ram和rom使用fpga内嵌的硬件ram存储器，其中ram暂存每步的中间结果，rom存放滤波中的固定系数，如观测矩阵、噪声系数阵等。kalman滤波的解算过程主要利用内嵌的硬核乘法器等资源来完成。因解算速度较快，解算结果暂存于一个稍大的存储器内，同时
8、通过串行口输出到pc机上保存用于分析。该方案的关键问题是在fpga中实现kalman滤波算法。fpga实现kalman滤波器，其实质就是控制数据的转移和存储并实现矩阵的相乘、加、减、求逆等运算。其中，数据的转移控制需要有限状态机(fsm)来完成，同时fpga设计中，不可防止的会遇到资源与速度的问题。因此，需要对上述各关键技术进行研究和实现。2.1矩阵相乘在fpga 中的实现kalman滤波计算中最根本的步骤就是矩阵相乘8。对于其中最常见的d=abc型的矩阵相乘，有两种实现方式：方式一，分步相乘；方式二，直接相乘。事先将矩阵a，b，c分别存入rom1，rom2，rom3中，方式一中，首先进行两个
9、矩阵的相乘，多路选择开关mux选通rom1和rom2，依次读取其中的数据进行乘加，完成前面两个矩阵的相乘，结果存入romtemp中；然后，mux选通romtemp和rom3，利用前面同样的资源，完成三个矩阵的连乘。方式二中，rom1，rom2，rom3同时输出数据，mux根据解算需要配置乘法器和加法器的输入，所有的过程同时进行。从上面的执行过程可知，方式一的执行需要占用更多的时间，而方式二的执行会占用更多的资源。对于上述n阶的3个矩阵相乘，其占用资源和所需时间如表1所示。表13个n阶矩阵相乘时不同方式下占用资源与时间情况所需浮点乘法器所需浮点加法器所需时钟方式一nn-1n2方式二n(n+1)n
10、2-112由表1可知，对于维数越大的矩阵相乘，需要的浮点加法器越多。由于浮点加法器的生成利用fpga内的根本逻辑单元可配置逻辑块(clb)，所以其占用的clb等资源也越多。这种现象在上述方式二中尤为突出。本文中研究拟先实现二阶kalman滤波器，阶次较低，资源相对充足，为检验fpga实现kalman滤波器的快速性，选用第二种方式进行矩阵相乘，以得到最快的解算速度。2.2有限状态机的运用有限状态机是一种用来进行对象行为建模的工具，其作用主要是描述对象在它的生命周期内所经历的状态序列，以及如何响应来自外界的各种事件。kalman滤波中，由于解算过程中的逻辑关系，需要分步进行，所以需要用有限状态机来
11、控制各步的转移。根据各步间的逻辑关系，可以将其大致分为4个状态：s0，s1，s2和s3状态。其中， s0为初始化状态，之后进入s1状态，计算k+1/k和pk+1/k，然后进入s2状态，计算kk+1，最后计算解算结果k+1/k+1和pk+1/k+1。计算kk+1时，也需要将其分步实现。假设将各步的解算归于同一个状态机内，那么显得逻辑复杂。为使得各步的逻辑更加清晰，并且增加状态机的稳定性和安全性，使用交互状态机，如图2所示。图2交互状态机的运用原理图图2中，状态机的交互过程中，设置标志信号enble和finish分别用于启动和终止计算k值的状态，初始化时其值均为0。当计算完第一步进入s2时，enb
12、le置为1，启动计算k值的状态；当k值计算完成时，finish置为1，进入s3，enble和finish置为初值0，为下次状态交互做准备。进入原状态机继续进行下面的计算。书写状态机时，采用三段式写法9，一个模块采用同步时序描述状态转移，另一个模块采用组合逻辑判断转移条件并进行状态转移，第三个模块实现同步输出。三段式描述方法的状态机，做到了同步存放器输出，消除了组合逻辑输出的不稳定和毛刺现象，而且更利于时序路径分组，综合与布局布线效果更佳。2.3资源分时复用fpga设计中，资源与速度是个矛盾体。fpga中的资源是有限的，所以必须考虑资源的节省问题。由于kalman滤波可以分3步进行，所以每一步可
13、以利用其它步中相同的资源。此种方法可以在不降低总体速度的情况下，减少资源利用量；而对于阶次较高的kalman滤波，此方法可以最大限度的增加并行性，提高速度。该设计中用到大量的乘法器、加法器以及clb等资源，计算第一步时用到的资源会在第二步和第三步中用到，即同一资源被用到3次。以其中用到的某乘法器的分时复用为例，其输入端口在不同的时刻可以有不同的赋值，实现语句如下：assign mulq1_ina=(state_ni=state_ni4)state_ni_mulq1_ina:(state_ex=state_ex4)romhk_douta:romtao_douta)；3性能比照及分析为验证本文研究
14、利用fpga实现kalman滤波算法的性能，采用二阶kalman滤波器进行实际性能比照测试。建立数学模型如下：选取状态转移矩阵k+1,k=1101，无控制量，即控制阵uk=0，噪声系数矩阵k+1=0001，wk是系统动态噪声，是均值为0、方差阵为0001的白噪声随机序列，观测系数矩阵hk+1=1,0，vk+1表示观测噪声，为均值为0，方差为2的白噪声；初始状态协方差阵为p0/0=100010。对于该滤波器分别利用三种方式实现：采用matlab在pc机上实现、利用dsp实现和利用fpga实现。其中，pc机为dell dimension4700台式机，安装matlab 7.0软件；dsp选用双精度
15、浮点型tms320c6713型号的芯片10；fpga型号为xc2vp30，主频100 mhz，内嵌多达136个硬核乘法器和2 mb硬件ram。为得到更准确合理的结果，分别截取第13次、1113次、2123次的计算结果来比照。因为在pc机上利用matlab实现的kalman滤波器通过软件设置可以到达很高的计算精度，所以将其得到的结果作为标准值(真值)，分别用fpga实现和dsp实现的结果与其进行比照分析。选取估计均方误差阵pk+1/k+1的第一个元素来进行比照分析各实现方式的性能，其解算结果比照方表2和表3所示。表2kalman滤波器的3种实现方式的数据比照matlab结果dsp结果fpga结果
16、dsp绝对误差fpga绝对误差10.952 381 00.952 380 90.952 380 75.50e-082.65e-0722.192 307 72.192 307 02.192 308 03.16e-07-1.40e-0730.900 000 00.899 999 50.899 999 65.56e-074.44e-07110.846 094 10.846 093 50.846 094 16.54e-07-5.52e-08121.588 868 11.588 868 01.588 868 05.71e-085.71e-08130.846 093 90.846 093 80.846 093 71.08e-072.27e-07210.846 093 80.846 093 50.846 093 23.55e-077.09e-07221.588 867 51
