卡尔曼滤波理论^[1]在20世纪60年代提出后, 在雷达跟踪、导航和控制等领域获得广泛应用。多维卡尔曼滤波算法的实现需要复杂的矩阵运算, 在卡尔曼滤波器的实际应用中, 软件实现方式很难满足系统对高实时性的要求, 硬件实现方式可以利用并行处理的优点, 加快滤波速度。在卡尔曼滤波器的传统硬件实现方式中, 通常利用第三方开发平台, 如借助LABVIEW FPGA和DSP Builder的图形化开发工具或者QuartusⅡ的浮点数运算IP核^[2-4], 或者通过构建矩阵运算模块来实现滤波算法^[5], 不仅增加了滤波算法对平台的依赖性, 降低了系统的可移植性和应用范围, 而且复杂的浮点数矩阵运算成为进一步提升滤波速度的瓶颈。

本文基于卡尔曼滤波器的传统实现方式, 首先根据滤波变量之间的关系将高维模型拆分为低维滤波模型; 再根据低维滤波模型的滤波参数设置情况, 将组成卡尔曼滤波器的5条滤波公式进行彻底推导并化简, 以降低滤波算法的时间复杂度; 然后设计基于IEEE754标准的浮点数运算单元FPU^[6], 并利用“自底向上”的设计思路, 以FPU作为底层模块, 对推导后的滤波公式加以实现, 进而构成整个滤波系统。

1 卡尔曼滤波器介绍和算法优化

卡尔曼滤波器用于估计离散时间过程的状态变量, 这个离散时间过程由一个线性随机差分方程描述^[7], 称为系统方程:

$ {\mathit{\boldsymbol{x}}_k} = {\mathit{\boldsymbol{A}}_{k - 1}}{\mathit{\boldsymbol{x}}_{k - 1}} + {\mathit{\boldsymbol{B}}_{k - 1}}{\mathit{\boldsymbol{u}}_{k - 1}} + {\mathit{\boldsymbol{w}}_{k - 1}} $

系统的测量方程为

$ {\mathit{\boldsymbol{z}}_k} = {\mathit{\boldsymbol{C}}_k}{\mathit{\boldsymbol{x}}_k} + {\mathit{\boldsymbol{v}}_k} $

矩阵A, B, C, u由实际滤波模型决定, 可以是确定的, 也可能随时间变化^[8], w和v分别是系统噪声和测量噪声, 为相互独立的高斯白噪声。

从形式上看, 卡尔曼滤波器由5个公式组成。其中式(1)完成状态预测, 式(2)完成协方差预测, 式(3)用来计算卡尔曼增益, 式(4)完成状态的滤波估计, 式(5)用来更新滤波协方差。

状态预测方程:

$ {\mathit{\boldsymbol{\hat x}}_{k|k - 1}} = {\mathit{\boldsymbol{A}}_{k - 1}}{\mathit{\boldsymbol{x}}_{k - 1}} + {\mathit{\boldsymbol{B}}_{k - 1}}{\mathit{\boldsymbol{u}}_{k - 1}}。 $

(1)

协方差预测方差:

$ {\mathit{\boldsymbol{\hat P}}_{k|k - 1}} = {\mathit{\boldsymbol{A}}_{k - 1}}{\mathit{\boldsymbol{P}}_{k - 1}}{\mathit{\boldsymbol{A}}_{k - 1}}^{\rm{T}} + {\mathit{\boldsymbol{Q}}_{k - 1}}。 $

(2)

卡尔曼增益方程:

$ {\mathit{\boldsymbol{H}}_k} = {{\mathit{\boldsymbol{\hat P}}}_{k|k - 1}}{\mathit{\boldsymbol{C}}_k}^{\rm{T}}{({\mathit{\boldsymbol{C}}_k}{{\mathit{\boldsymbol{\hat P}}}_{k|k - 1}}{\mathit{\boldsymbol{C}}_k}^{\rm{T}} + {\mathit{\boldsymbol{R}}_k})^{{\rm{ - }}1}}。 $

(3)

滤波估计方程:

$ {\mathit{\boldsymbol{x}}_k} = {{\mathit{\boldsymbol{\hat x}}}_{k|k - 1}} + {\mathit{\boldsymbol{H}}_k}({\mathit{\boldsymbol{z}}_k} - {\mathit{\boldsymbol{C}}_k}{{\mathit{\boldsymbol{\hat x}}}_{k|k - z}})。 $

(4)

协方差更新方程:

$ {\mathit{\boldsymbol{P}}_k} = (\mathit{\boldsymbol{I}} - {\mathit{\boldsymbol{H}}_k}{\mathit{\boldsymbol{C}}_k}){{\mathit{\boldsymbol{\hat P}}}_{k|k - 1}}。 $

(5)

我们考虑一个雷达实时跟踪系统, 假设有一个在三维空间x-y-z内运动的飞行器M, 设γ, △θ和△φ分别为M的距离、方位角误差和仰角误差。考虑γ, △θ和△φ是时间的函数, 并且一阶导数和二阶导数分别表示为$\dot \gamma, \Delta \dot \theta, \Delta \dot \varphi $和$\dot \gamma, \Delta \dot \theta, \Delta \dot \varphi, \Delta t$为采样时间单位。对该系统有

$ \mathit{\boldsymbol{x}}(k) = {\left[{\begin{array}{*{20}{c}} {{\gamma _k}}&{{{\dot \gamma }_k}}&{{{\ddot \gamma }_k}}&{\Delta {\theta _k}}&{\Delta {{\dot \theta }_k}}&{\Delta {{\ddot \theta }_k}}&{\Delta {\varphi _k}}&{\Delta {{\dot \varphi }_k}}&{\Delta {{\ddot \varphi }_k}} \end{array}} \right]^{\rm{T}}}, $

$ {\mathit{\boldsymbol{A}}_{k - 1}} = \left[{\begin{array}{*{20}{c}} 1&{\Delta t}&{\Delta {t^2}/2}&0&0&0&0&0&0\\ 0&1&{\Delta t}&0&0&0&0&0&0\\ 0&0&1&0&0&0&0&0&0\\ 0&0&0&1&{\Delta t}&{\Delta {t^2}/2}&0&0&0\\ 0&0&0&0&1&{\Delta t}&0&0&0\\ 0&0&0&0&0&1&0&0&0\\ 0&0&0&0&0&0&1&{\Delta t}&{\Delta {t^2}/2}\\ 0&0&0&0&0&0&0&1&{\Delta t}\\ 0&0&0&0&0&0&0&0&1 \end{array}} \right]\;。 $

对于该模型, 系统无额外控制量输入, 即有控制量u为零矩阵, 此处基于算法完整性考虑, 假设控制矩阵B的维度为n×1, 控制量u为1×1矩阵。所以有

$ {\mathit{\boldsymbol{B}}_{k - 1}} = {\left[{\begin{array}{*{20}{c}} {{B_0}}&{{B_1}}&{{B_2}}&{{B_3}}&{{B_4}}&{{B_5}}&{{B_6}}&{{B_7}}&{{B_8}} \end{array}} \right]^{\rm{T}}}. $

假设原点位置有一雷达O对飞行器M进行目标跟踪, 有

$ \mathit{\boldsymbol{z}}(k) = {\left[{\begin{array}{*{20}{c}} {{\gamma _k}}&{\Delta {\theta _k}}&{\Delta {\varphi _k}} \end{array}} \right]^{\rm{T}}}, $

$ {\mathit{\boldsymbol{C}}_k} = \left[{\begin{array}{*{20}{c}} 1&0&0&0&0&0&0&0&0\\ 0&0&0&1&0&0&0&0&0\\ 0&0&0&0&0&0&1&0&0 \end{array}} \right]。 $

容易证明该系统可以分解为3个具有相似状态空间描述的子系统^[9], 即这个九维滤波问题可以拆分成3个三维滤波模型:距离跟踪模型、方位角跟踪模型和仰角跟踪模型, 并且每个滤波模型的转换矩阵和测量矩阵都相同, 分解之后的三维模型为

$ \mathit{\boldsymbol{x}}(k) = {\left[{\begin{array}{*{20}{c}} {{\gamma _k}}&{{{\dot \gamma }_k}}&{{{\ddot \gamma }_k}} \end{array}} \right]^{\rm{T}}}, $

$ z(k) = {\gamma _k} $

$ {A_{k - 1}} = \left[{\begin{array}{*{20}{c}} 1&{\Delta t}&{\Delta {t^2}/2}\\ 0&1&{\Delta t}\\ 0&0&1 \end{array}} \right] $

$ {\mathit{\boldsymbol{B}}_{k - 1}} = {\left[{\begin{array}{*{20}{c}} {{B_0}}&{{B_1}}&{{B_2}} \end{array}} \right]^{\rm{T}}}, $

$ {\mathit{\boldsymbol{C}}_k} = \left[{\begin{array}{*{20}{c}} 1&0&0 \end{array}} \right]. $

根据矩阵运算的性质, 将上述滤波参数带入滤波公式, 对三维滤波模型的5个公式做彻底推导并简化, 从而将矩阵运算转化为加、减、乘、除运算, 这里给出式(1)的推导结果, 其余4个公式可做类似推导:

$ {{\mathit{\boldsymbol{\hat x}}}_{k|k - 1}} = {\mathit{\boldsymbol{A}}_{k - 1}}{\mathit{\boldsymbol{x}}_{k - 1}} + {\mathit{\boldsymbol{B}}_{k - 1}}{\mathit{\boldsymbol{u}}_{k - 1}} = \left[ {\begin{array}{*{20}{c}} {{{\hat x}_{k|k - 1}}\_0}\\ {{{\hat x}_{k|k - 1}}\_1}\\ {{{\hat x}_{k|k - 1}}\_2} \end{array}} \right]。 $

将矩阵运算展开得各元素如下:

$ \begin{array}{l} {{\mathit{\boldsymbol{\hat x}}}_{k|k - 1}}\_0 = {x_{k - 1}}\_0 + \Delta t \cdot {x_{k - 1}}\_1 + \Delta {t^2} \cdot {x_{k - 1}}\_2/2{\rm{ + }}{u_{k - 1}} \cdot {B_{k - 1}}\_0\\ {{\mathit{\boldsymbol{\hat x}}}_{k|k - 1}}\_1 = {x_{k - 1}}\_1 + \Delta t \cdot {x_{k - 1}}\_2{\rm{ + }}{u_{k - 1}} \cdot {B_{k - 1}}\_1\\ {{\mathit{\boldsymbol{\hat x}}}_{k|k - 1}}\_2 = {x_{k - 1}}\_2{\rm{ + }}{u_{k - 1}} \cdot {B_{k - 1}}\_2。 \end{array} $

2 电路设计 2.1 系统结构

系统的结构如图 1所示, 需要说明的是, 图中所有与滤波算法有关的变量或者信号均是矩阵形式, 此处为方便表示, 只用一个信号引脚表示整个矩阵。

CLKGEN模块利用外来时钟信号生成系统时钟信号, 并产生系统初始化信号。CAL_1, CAL_2, CAL_3, CAL_4和CAL_5分别完成滤波算法5个公式的内容, 即状态预测, 协方差预测, 卡尔曼增益的计算, 状态的滤波估计以及滤波协方差的更新。MUX_X和MUX_P分别用来控制算法迭代过程中的状态更新和协方差更新。寄存器组REGS_X和REGS_P由若干寄存器构成, 用来完成不同模块之间的数据寄存和时序调整。AFIFO为异步FIFO (first in first out), 用来缓存输入数据, 匹配数据输入速度和滤波速度^[10]。

FPU的结构如图 2所示, FPU模块完成基于IEEE754标准的32位浮点数的加、减、乘、除运算, 支持对两个操作数的加、减、乘、除运算, 是卡尔曼滤波器的底层基础运算模块。

2.2 系统设计思路

系统采用“自底向上”的层次化设计方法, 首先设计滤波算法所需的浮点数运算单元FPU，然后以FPU为底层模块分别构建5个滤波公式模块CAL_1, CAL_2, CAL_3, CAL_4和CAL_5, 进而构成最顶层的卡尔曼滤波系统。

以矩阵乘法运算为例, 一个m×n与n×p的矩阵相乘, 如果所有元素参与运算的话, 需要的乘法运算次数是m×p×n, 需要的加法运算次数是m×p× (n-1)。针对传统矩阵运算法的不足, 我们根据滤波模型的具体参数设置和矩阵运算的性质, 将构成滤波算法的5个公式进行推导和简化, 并用FPU直接实现推导、简化后的滤波公式, 从而减少滤波算法所需的运算次数, 提升硬件处理速度。

卡尔曼滤波器5个公式的数据传递情况如图 3所示, 根据滤波公式之间的数据引用情况, 可以将5个公式分成3个执行状态和时序。其中, CAL_1和CAL_2组成第1个执行时序, CAL_3组成第2个执行时序, CAL_4和CAL_5组成第3个执行时序。

系统各模块的处理速度与FPU的运算速度紧密相关, 在设计FPU时, 我们综合考虑浮点数运算的速度和精度, 设计的FPU执行一条加、减、乘、除运算需要的初始化时间是4个时钟周期, 完成初始化之后, 每个时钟周期即可完成一次加、减、乘、除运算。

由FPU构建的上层滤波运算模块CAL_1, CAL_2, CAL_3, CAL_4和CAL_5初始化所需时间由各模块运算最慢的元素决定, 具体计算各个公式的各个元素时, 暂不参加计算的数据采用模块内部的寄存器组暂时保存。表 1是使用公式推导法和传统矩阵运算法时, 组成卡尔曼滤波器的5个滤波公式模块完成模块初始化需要的时间对比。

各个滤波模块的初始化时间会影响系统的整体滤波速度。在系统时钟频率为100 MHz的情况下, 使用传统矩阵运算法设计完成的卡尔曼滤波器, 完成一次滤波运算需要840 ns; 运用公式推导法设计完成的卡尔曼滤波器, 完成一次滤波仅需400 ns。可以看出, 使用本方法是使用传统矩阵运算法速度的2.1倍。

2.3 FPGA平台参数评估

图 4为利用Quartus Ⅱ软件, 对系统进行分析和综合后得到的在FPGA平台下的参数评估结果, 显示该系统的组合逻辑、时序逻辑等参数的资源占用情况。

3 应用结果

图 5是卡尔曼滤波器的系统输入值measure_ input, 包含系统噪声和测量噪声。从图中可以看出, 该输入信号包含一定的噪声干扰, 需要通过卡尔曼滤波器对其进行滤波处理。

图 6是经过卡尔曼滤波算法滤波优化之后的原变量输出值x_now_0及其一阶导数值x_now_1和二阶导数值x_now_2。可以看出, x_now_0, x_now_1和x_now_2在经过一定次数的滤波算法迭代后, 都得到很好的去噪效果。

图 7是预测误差矩阵p_pred_00, p_pred_11和p_pred_22的曲线图, 可以看出, 误差随滤波算法迭代次数的增加而变化。

4 总结

本文根据滤波变量之间的关系将高维模型拆分为低维滤波模型, 再根据低维滤波模型的滤波参数设置情况, 将组成卡尔曼滤波器的5个滤波公式进行彻底推导和简化, 然后设计浮点数运算单元FPU, 并利用“自底向上”的设计思路, 以FPU作为底层模块对推导后的滤波公式加以实现, 进而构成整个滤波系统。采用此方法设计的卡尔曼滤波器在保证滤波精度的同时, 还摆脱了滤波平台的依赖性, 增加了系统的可移植性和应用范围, 并且提升了滤波速度，可以更好地满足实际工程应用中对卡尔曼滤波器的高实时性要求。