基于FPGA的高维卡尔曼滤波器及其实现方法

技术领域

本发明涉及基于FPGA的高维卡尔曼滤波器及其实现方法，属于航空航天 中的导航技术领域。

背景技术

卡尔曼滤波采用状态空间法在时域内设计滤波器，是一种解决离散系统线 性滤波问题的递推最优估计算法。本世纪60年代以来,卡尔曼滤波算法得到了 深入的研究和广泛的工程应用,许多非凡技术成就都可部分地归因于卡尔曼滤 波器,它有许多军事的、工业的以及科学的应用。其中主要应用包括:宇宙飞船的 航行、导弹的制导和控制、卫星导航与定位、雷达和飞机自动驾驶仪等等。

在实际应用中，卡尔曼滤波器常采用通用的处理器来实现，如单片机，DSP 处理器以及ARM处理器。通用处理器上的卡尔曼滤波器一般通过C语言编程 实现，因而它具有设计简单的特点。但由于卡尔曼滤波器在运算的过程中存在 大量的高维矩阵加减乘除运算，同时通用的处理器采用了程序顺序执行的硬件 架构，因而在对实时性要求比较高的场合，通用处理器上的卡尔曼滤波器往往 不能够满足系统对运行速度的要求。针对通用微处理器执行速度较慢、效率偏 低的缺陷，充分利用FPGA(Field Programmable Gate Array,现场可编程门阵列) 自身高度并行性、执行速度快的特点，近些年来人们对在FPGA中定制卡尔曼 滤波算法的硬件电路展开了初步的研究。而目前基于FPGA的卡尔曼滤波器限 于FPGA芯片内部逻辑资源有限，因而所设计的滤波器的矩阵维数较低，同时 采用定点小数进行运算节省运算资源，这大大降低了卡尔曼滤波器的滤波性能。

发明内容

本发明技术解决问题：克服现有技术的不足，提供一种基于FPGA的高维 卡尔曼滤波器及其实现方法，以解决常规处理器执行高维卡尔曼滤波算法的速 度慢、精度低，从而会影响导航性能的问题，大大提高了高维卡尔曼滤波算法 的执行速度与计算精度。

本发明技术解决方案：基于FPGA的高维卡尔曼滤波器,如图1所示，包括： 滤波逻辑控制器（2），双精度运算器(3)及数据存储器(4)；所述滤波逻辑控制 器(2)由状态预测控制器(2a)，协方差预测控制器(2b)，卡尔曼增益更新控制器 (2c)，状态更新控制器(2d)及协方差更新控制器(2e)五个逻辑子控制器组成； 所述运算器由双精度乘法器(3a)，双精度除法器(3b)，双精度加法器(3c)以及 双精度减法器(3d)四个子运算器，每个子运算器有两个数据输入端口和一个数 据输出端口；

逻辑控制器(2)给定滤波时序，在滤波时序的约束下把从数据存储器(4)中 读出的数据赋值给运算器(3)的输入端口，并把运算器(3)的输出数据写入到数 据存储器(4)中；

双精度乘法器(3a)，双精度除法器(3b)，双精度加法器(3c)以及双精度减 法器(3d)四子运算器彼此独立，它们的输入输出数据均通过数据总线与数据存 储器(4)相连接；当状态预测控制器(2a)，协方差预测控制器(2b)，卡尔曼增益 更新控制器(2c)，状态更新控制器(2d)及协方差更新控制器(2e)五个逻辑子控 制器所给定时序依次执行后，则完成一次滤波，此时再循环执行，实现实时滤 波。

所述的存储器(4)采用RAM来实现，RAM的地址总线宽度为12位，寻址范 围0-4095，每个地址内存储的数据宽度为64位，表示了一个双精度浮点型数 据，RAM中存储的数据分段组成滤波算法的参数矩阵和中间变量矩阵。在此设 计结构下可以实现15维的卡尔曼滤波器，通过增大地址总线的宽度，进一步扩 大存储器的寻址范围，从而完成更高维数的卡尔曼滤波器。

基于FPGA的高维卡尔曼滤波器实现方法，首先对存储器（4）中的滤波算 法相关矩阵进行初始化，然后在不断从外部读入观测量的条件下重复以下五个 步骤，从而完成卡尔曼滤波过程，具体实现为：

（1）状态预测控制器完成状态预测

状态预测控制器(2a)需要控制读取存储器(4)中与状态预测有关的数据，送 至所需要的双精度乘法器(3a)，双精度除法器(3b)，双精度加法器(3c)以及双 精度减法器(3d)四个子运算器进行计算，并把计算结果写入到存储器(4)中的状 态预测结果存储区域，在此过程完成1次“方阵×列向量”运算；

（2）协方差预测控制器完成协方差预测

协方差预测控制器需要依次控制读取存储器(4)中与协方差预测有关的数 据，送至所需要的双精度乘法器(3a)，双精度除法器(3b)，双精度加法器(3c) 以及双精度减法器(3d)四个子运算器进行计算，并把计算结果写入到存储器(4) 中的协方差预测结果存储区域，在此过程完成1次“方阵×方阵”运算，1次“方 阵×方阵”运算和1次“方阵+方阵”运算；

（3）卡尔曼增益更新控制器完成卡尔曼增益更新

卡尔曼增益更新控制器需要依次控制读取存储器(4)中与卡尔曼增益更新 有关的数据，送至所需要的双精度乘法器(3a)，双精度除法器(3b)，双精度加 法器(3c)以及双精度减法器(3d)四个子运算器进行计算，并把计算结果写入到 存储器(4)中的卡尔曼增益更新结果存储区域，在此过程完成1次“方阵×方阵” 运算，1次“方阵×方阵”运算，1次“方阵+方阵”运算，1次矩阵求逆运算和 1次“方阵×方阵”运算；

（4）状态更新控制器完成状态更新

状态更新控制器需要依次控制读取存储器(4)中与状态更新有关的数据，送 至所需要的双精度乘法器(3a)，双精度除法器(3b)，双精度加法器(3c)以及双 精度减法器(3d)四个子运算器进行计算，并把计算结果写入到存储器(4)中的状 态更新结果存储区域，在此过程完成1次“方阵×列向量”运算，1次“列向量 -列向量”运算，1次“方阵×列向量”运算和1次“列向量+列向量”运算；

（5）协方差更新控制器完成协方差更新

协方差更新控制器需要依次控制读取存储器(4)中与协方差更新有关的数 据，送至所需要的双精度乘法器(3a)，双精度除法器(3b)，双精度加法器(3c) 以及双精度减法器(3d)四个子运算器进行计算，并把计算结果写入到存储器(4) 中的协方差更新结果存储区域，在此过程完成1次“方阵×方阵”运算,1次“方 阵-方阵”运算和1次“方阵×方阵”运算。

本发明与现有技术相比的优点在于：

（1）本发明克服了通用处理器中在卡尔曼滤波算法执行速度低的不足，四 个子运算器可以在单个时钟下独立并行地完成一次双精度的滤波计算，大大提 高了高维卡尔曼滤波算法的执行速度与计算精度。

（2）本发明在既有的硬件结构基础上可以通过改变存储器地址总线的宽度 来对滤波维数进行调整，兼容低维的卡尔曼滤波器设计，具有较好的扩展性。

以上所述的“列向量+列向量”以及“方阵+方阵”运算通过FPGA设计高维 矩阵相加的逻辑控制状态机来实现,“列向量-列向量”以及“方阵-方阵”运 算通过FPGA设计高维矩阵相减的逻辑控制状态机来实现,,“方阵×列向量”以 及“方阵×方阵”运算通过FPGA设计高维矩阵相乘的逻辑控制状态机来实现， 而矩阵求逆运算通过FPGA设计高维矩阵求逆的逻辑控制状态机来实现。

附图说明

图1为本发明基于FPGA的高维卡尔曼滤波器硬件结构设计示意图；

图2为本发明滤波器执行高维矩阵相加的逻辑控制状态机示意图；

图3为本发明滤波器执行高维矩阵相减的逻辑控制状态机示意图；

图4为本发明滤波器执行高维矩阵求逆的逻辑控制状态机示意图；

图5为本发明滤波器执行高维矩阵相乘的逻辑控制状态机示意图；

图6为本发明硬件结构读写RAM存储器的时序控制示意图。

具体实施方式

卡尔曼滤波的实质是由测量值重构系统的状态向量，它以“预测—实测— 修正”的顺序递推，根据系统的测量值来消除随机干扰，得到最优估计值。

下面来具体介绍一下卡尔曼滤波器的算法。

首先，引入一个离散控制过程的系统。该系统可用一个线性随机微分方程 来描述：

X(k)＝F*X(k-1)+B*U(k)+W(k)  (1)

再加上系统的测量值：

Z(k)＝H*X(k)+V(k)  (2)

上两式子中，X(k)是k时刻的系统状态，U(k)是k时刻对系统的控制量。F 和B是系统参数，对于多模型系统，它们为矩阵。Z(k)是k时刻的测量值，H 是测量系统的参数，对于多测量系统，H为矩阵。W(k)和V(k)分别表示过程和 测量的噪声，它们被假设成高斯白噪声，它们的协方差分别是Q，R。下面将用 以上的变量结合它们的协方差来估算系统的最优化输出。

首先要利用系统的过程模型，来预测下一状态的系统。假设现在的系统状 态是k，根据系统的模型，可以基于系统的上一状态而预测出现在状态：

X(k|k-1)＝F*X(k-1|k-1)+B*U(k)  (3)

式(3)中，X(k|k-1)是利用上一状态预测的结果，X(k-1|k-1)是上一状态最优的 结果，U(k)为现在状态的控制量，如果没有控制量，它可以为0。

用P表示协方差：

P(k|k-1)＝F*P(k-1|k-1)*F'+Q  (4)

式(4)中，P(k|k-1)是X(k|k-1)对应的协方差，P(k-1|k-1)是X(k-1|k-1)对应的协 方差，F’表示F的转置矩阵，Q是系统过程的协方差。式(3)，(4)就是卡尔曼 滤波器5个公式当中的前两个，也就是对系统的预测。

现在有了现在状态的预测结果，然后再收集现在状态的测量值。结合预测 值和测量值，可以得到现在状态k的最优化估算值X(k|k)：

X(k|k)＝X(k|k-1)+Kg(k)*(Z(k)-H*X(k|k-1))  (5)

其中Kg为卡尔曼增益：

Kg(k)＝P(k|k-1)*H'/(H*P(k|k-1)*H'+R)  (6)

由上式得到了k状态下最优的估算值X(k|k)。但是为了让卡尔曼滤波器不断 的运行下去直到系统过程结束，还要更新k状态下X(k|k)的协方差：

P(k|k)＝(I-Kg(k)*H)*P(k|k-1)  (7)

其中I为单位阵，对于单模型单测量，I=1。当系统进入k+1状态时，P(k|k) 就是式(4)的P(k-1|k-1)。这样，算法就可以自回归的运算下去。

因此，卡尔曼滤波的算法可以总结为以下5个公式：

$\left(\begin{array}{c}X\left(k\right|k-1)=F*X(k-1|k-1)+B*U\left(k\right)\\ P\left(k\right|k-1)=F*P(k-1|k-1)*F\prime +Q\\ X\left(k\right|k)=X\left(k\right|k-1)+\mathrm{Kg}\left(k\right)*(Z\left(k\right)-H*X\left(k\right|k-1))\\ \mathrm{Kg}\left(k\right)=P\left(k\right|k-1)*H\prime /(H*P\left(k\right|k-1)*H\prime +R)\\ P\left(k\right|k)=(I-\mathrm{Kg}\left(k\right)*H)*P\left(k\right|k-1)\end{array}\right)---\left(8\right)$

按照公式(8)所示的五个公式分别设计卡尔曼滤波控制器中的五个子控制 器，它们依次为状态预测控制器，协方差预测控制器，状态更新控制器，卡尔 曼滤波增益更新控制器以及协方差更新控制器。而每一个子控制器主要控制完 成“方阵×方阵”运算，“方阵+方阵”运算，“方阵-方阵”运算，矩阵求逆运 算，“方阵×列向量”运算，“列向量+列向量”运算和“列向量-列向量”运算。

以上所述的七种运算可以归结为四类矩阵运算，即矩阵的加减乘与求逆运 算，下面结合说明附图对本发明作进一步详细的描述。

矩阵相加的逻辑控制状态机如图2所示，假设需要完成的计算为矩阵C=矩 阵A+矩阵B，则首先获得矩阵A，B以及C的偏移地址，然后依次读取矩阵A， B的当前地址中的数据，并赋值给加法器的两个输入端口，随后把矩阵A，B的 当前地址递增一，最后把计算出来加法结果存储到C矩阵的当前地址中，并把 矩阵C的当前地址递增一。如此循环，当C矩阵当前地址越界从而不在C矩阵 地址范围中时，完成矩阵相加运算。

矩阵相减的逻辑控制状态机如图3所示，假设需要完成的计算为矩阵C=矩 阵A-矩阵B，则首先获得矩阵A，B以及C的偏移地址，然后依次读取矩阵A， B的当前地址中的数据，并赋值给减法器的两个输入端口，随后把矩阵A，B的 当前地址递增一，最后把计算出来减法结果存储到C矩阵的当前地址中，并把 矩阵C的当前地址递增一。如此循环，当C矩阵当前地址越界从而不在C矩阵 地址范围中时，即完成矩阵相加运算。

矩阵求逆的逻辑控制状态机如图4所示，假设需要完成的计算为矩阵B=矩 阵A，在设计中，为了计算方便，把矩阵B存储在矩阵A之后的相邻地址空间 内，这样仅通过A矩阵的地址以及矩阵的维数就可以计算到B矩阵的地址。矩 阵求逆采用了增广矩阵法，首先获得矩阵A的偏移地址，并对B矩阵进行单位 阵化处理；然后依次读取A矩阵的首行到末行的对角线元素，并同时把A，B 矩阵的对应行所有元素除以该对角线(假设不是零)元素，以使得A矩阵的该行 对角线元素归一化；之后对A，B矩阵通过同样的行变换，把A矩阵的该行对角 线元素所在列的其它所有元素归零化；如此循环，当A矩阵的所有行处理完毕 时，即完成矩阵相加运算。

矩阵相乘的逻辑控制状态机如图5所示，假设需要完成的计算为矩阵C=矩 阵A*矩阵B，则首先获得矩阵A，B以及C的偏移地址，累加器没完成一次C 矩阵元素计算后清零；然后依次读取矩阵A的一行中的每一个元素，赋值给乘 法器的一个输入端口，同时读取矩阵B的一列中的每一个元素，赋值给乘法器 的另一个输入端口；之后，把乘法结果赋值给一个累加器的输入端口，当矩阵 A的一行（或矩阵B的一列）全部读完时，累加器即完成了矩阵C中一个元素 的计算。如此循环，当矩阵A的所有行与矩阵B所有列处理组合计算完毕时， 即完成矩阵相乘运算。

在卡尔曼滤波算法中所涉及到的所有矩阵，包括系统模型中的状态更新矩 阵，观测矩阵，过程和测量的噪声的协方差矩阵，状态变量，状态预测变量， 协方差矩阵，协方差预测矩阵，卡尔曼滤波增益矩阵以及滤波算法计算过程中 的所有中间结果矩阵，都需要存在在RAM存储器中，因而需要对这些矩阵分 配合适的存储空间。

假设卡尔曼滤波器的维数为D，那么一个列向量所需的空间大小即为D， 而一个方阵所需要的空间大小为D*D。如果把各个矩阵依次存储在RAM中， 那么根据以上的矩阵空间分配原则，可以对维数不同的卡尔曼滤波器计算出相 应矩阵的偏移地址。表1所示为基于FPGA的高维卡尔曼滤波器的维数分别为 9，13，15以及20时的矩阵偏移地址分配表。当维数发生变化时，可以再另行 计算，这样很容易对卡尔曼滤波器的维数进行扩展。

表1基于FPGA的高维卡尔曼滤波器相关矩阵偏移地址分配表

如上表1所示，矩阵变量1-8为公式(8)中的原始变量，而矩阵9-20皆为中 间变量，其命名方式规则如下所述：各个字母拆分为公式(8)中的原始矩阵变量 名，然后按照原公式代表其加减乘除结果。例如FP代表公式(8)第二个公式中 的F*P，其它依次类推。而X_Predict，S以及S_inv除外，X_Predict代表公式(8) 第一个公式的状态预测结果，S代表公式(8)第四个公式中的H*P*H'+R部分， S_inv为S的逆矩阵。需要指出的是，在任何一个子控制器完成计算的过程都 要对RAM进行读写操作，并且需要给运算器进行输入值幅值与输出值读取操 作，而这些过程需要严格的时序控制。如图6所示为在求解矩阵逆运算中行处 理过程，首先需要三个工作时钟来读取RAM中的数据，第一个时钟给RAM的 地址线信号端口赋值，第二个时钟地址线上的信号作用于RAM，第三个时钟才 能使用读取出的RAM中数据。而在执行数据相除运算时，当除数和被除数赋 值到除法器的输入端口后的下一个时钟即可使用计算出的除法结果。

在应用基于FPGA的高维卡尔曼滤波算法的过程中，首先对RAM存储器中的 滤波算法相关矩阵进行初始化，然后在不断从FPGA芯片外部读入观测量的条件 下重复以下五个步骤：

（一）状态预测控制器完成状态预测

状态预测控制器2a需要控制读取存储器4中与状态预测有关的数据，送至 所需要的子运算器3a,3b,3c,3d进行计算，并把计算结果写入到存储器4中的 状态预测结果存储区域，在此过程完成1次“方阵×列向量”运算；

（二）协方差预测控制器完成协方差预测

协方差预测控制器2b需要依次控制读取存储器4中与协方差预测有关的数 据，送至所需要的子运算器3a,3b,3c,3d进行计算，并把计算结果写入到存储 器4中的协方差预测结果存储区域，在此过程完成1次“方阵×方阵”运算，1 次“方阵×方阵”运算和1次“方阵+方阵”运算；

（三）卡尔曼增益更新控制器完成卡尔曼增益更新

卡尔曼增益更新控制器2c需要依次控制读取存储器4中与卡尔曼增益更新 有关的数据，送至所需要的子运算器3a,3b,3c,3d进行计算，并把计算结果写 入到存储器4中的卡尔曼增益更新结果存储区域，在此过程完成1次“方阵× 方阵”运算，1次“方阵×方阵”运算，1次“方阵+方阵”运算，1次矩阵求逆 运算和1次“方阵×方阵”运算；

（四）状态更新控制器完成状态更新

状态更新控制器2d需要依次控制读取存储器(4)中与状态更新有关的数 据，送至所需要的子运算器3a,3b,3c,3d进行计算，并把计算结果写入到存储 器4中的状态更新结果存储区域，在此过程完成1次“方阵×列向量”运算，1 次“列向量-列向量”运算，1次“方阵×列向量”运算和1次“列向量+列向量” 运算；

（五）协方差更新控制器完成协方差更新

协方差更新控制器2e需要依次控制读取存储器(4)中与协方差更新有关的 数据，送至所需要的子运算器3a,3b,3c,3d进行计算，并把计算结果写入到存 储器4中的协方差更新结果存储区域，在此过程完成1次“方阵×方阵”运算,1 次“方阵-方阵”运算和1次“方阵×方阵”运算。

本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有 技术。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通 技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰， 这些改进和润饰也应视为本发明的保护范围。