基于Faddeev算法的卡尔曼滤波FPGA实现——QMJ

一、设计目的

（1） 使滤波程序中数据计算流程模块化，结构清晰。

（2） 使程序可以完成矩阵之间加、减、乘和求逆运算。

（3） 运用求解矩阵舒尔补方法设计算法。

二、设计思路及算法原理

运用算法搭建出流水线脉动阵列（PSA）,该阵列通过高斯消元方式求解出矩阵的舒尔补。

算法是将四个输入矩阵A、B、C和D组合成一个新的矩阵M，然后对矩阵进行高斯消元法进行矩阵求解。其中A为非奇异矩阵，设矩阵M为

算法是通过对矩阵M进行矩阵行变换，使M变为上三角矩阵，-C变为零矩阵

对M矩阵进行矩阵变换，得到矩阵E，式

（A的Schur）。

只要A、B、C和D矩阵给定，矩阵E就能通过算法计算出来。故只要恰当的选择A、B、C和D，就可以实现矩阵加、减、乘和求逆等运算。图1每个式子的左端四个矩阵分别对应于矩阵A、B、C和D，右端表示通过算法计算得到的结果。（矩阵I表示单位矩阵）

图1 算法可以实现的矩阵运算 三、实现步骤介绍

本次实验中A、B、C和D四个输入矩阵都采用的是2*2矩阵。

第一步，设计算法脉冲阵列的边界单元（ cell）和内部单元（ cell）

图2 边界单元和内部单元的操作

第二步，搭建脉动阵列

图3 2*2矩阵脉动阵列

第三步，算法运用到卡尔曼滤波器，图4中滤波的每一步都使用一次算法。将待处理数据输入到对应的A,B,C,D矩阵寄存器中，数据计算流程如图3所示。

图4 卡尔曼滤波算法的矩阵替换 四、实验结果

1）使用语言搭建出算法，处理一组数据所需要的时钟周期是286。实验结果如图5，图6所示

图5 算法输入和输出

图5中in1、in2、in3、in4代表的时序图，表示的是输入数据。out1、out2表示的是输出数据。该算法计算结果正确。

图6 算法的循环运算

经测验，算法的每次循环迭代运算结果正确。

2）使用算法搭建出滤波器，程序设计如图4所示。仿真结果如图7所示。

图7 滤波器时序图

图7中表示滤波数据输入，out1表示滤波数据输出。

3）基于直接除法的滤波器和基于算法的滤波器分别处理拉压力传感器采集的数据，实验得到的数据结果的分析通过绘图进行呈现。（两种滤波器区别在于，算法是矩阵运算）

五、结果分析

通过对数据的分析，可以查看出两种滤波器处理后得到的数据结果误差接近于零。

其中基于算法的滤波器的程序设计，需要花费大量的时钟来进行除法运算，故每处理一个16位二进制拉压力传感器采集的数据，需要约44微秒。但是设计的算法具有很强的通用性，适用于矩阵运算。

基于直接除法的滤波器在每处理一个16位二进制拉压力传感器采集的数据，需要约1微秒。