2023年基于FPGA的Kalman滤波器实现研究.doc

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