Kaiman滤波算法在FPGA上的设计与实现

2．2 有限状态机的运用

有限状态机是一种用来进行对象行为建模的工具，其作用主要是描述对象在它的生命周期内所经历的状态序列，以及如何响应来自外界的各种事件。Kalman滤波中，由于解算过程中的逻辑关系，需要分步进行，所以需要用有限状态机来控制各步的转移。根据各步间的逻辑关系，可以将其大致分为4个状态：S0，S1，S2和S3状态。其中，S0为初始化状态，之后进入S1状态，计算和Pk+1／k，然后进入S2状态，计算Kk+1，最后计算解算结果和Pk+1／k+1。计算Kk+1时，也需要将其分步实现。若将各步的解算归于同一个状态机内，则显得逻辑复杂。为使得各步的逻辑更加清晰，并且增加状态机的稳定性和安全性，使用交互状态机，如图2所示。

图2中，状态机的交互过程中，没置标志信号enble和finish分别用于启动和终止计算K值的状态，初始化时其值均为0。当计算完第一步进入S2时，enble置为1，启动计算K值的状态；当K值计算完成时，finish置为1，进入S3，enble和finish置为初值0，为下次状态交互做准备。进入原状态机继续进行下面的计算。

书写状态机时，采用三段式写法，一个模块采用同步时序描述状态转移，另一个模块采用组合逻辑判断转移条件并进行状态转移，第三个模块实现同步输出。三段式描述方法的状态机，做到了同步寄存器输出，消除了组合逻辑输出的不稳定和毛刺现象，而且更利于时序路径分组，综合与布局布线效果更佳。

2．3 资源分时复用

FPGA设计中，资源与速度是个矛盾体。FPGA中的资源是有限的，所以必须考虑资源的节省问题。由于Kalman滤波可以分3步进行，所以每一步可以利用其它步中相同的资源。此种方法可以在不降低总体速度的情况下，减少资源利用量；而对于阶次较高的Kalman滤波，此方法可以最大限度的增加并行性，提高速度。该设计中用到大量的乘法器、加法器以及CLB等资源，计算第一步时用到的资源会在第二步和第三步中用到，即同一资源被用到3次。以其中用到的某乘法器的分时复用为例，其输入端口在不同的时刻可以有不同的赋值，实现语句如下：

3 性能对比及分析

为验证本文研究利用FPGA实现Kalman滤波算法的性能，采用二阶Kalman滤波器进行实际性能对比测试。建立数学模型如下：选取状态转移矩阵，无控制量，即控制阵Uk=0，噪声系数矩阵，Wk是系统动态噪声，是均值为0、方差阵为的白噪声随机序列。

对于该滤波器分别利用三种方式实现：采用Matlab在PC机上实现、利用DSP实现和利用FPGA实现。其中，PC机为DELL Dimension4700台式机，安装Matlab 7．0软件；DSP选用双精度浮点型TMS320C6713型号的芯片；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所示。