CN107506332A

CN107506332A - Kalman滤波器快速实现方法

Info

Publication number: CN107506332A
Application number: CN201710612236.4A
Authority: CN
Inventors: 敖永才; 杨俊波; 谭平; 严明; 谢黛茜
Original assignee: Sichuan Aerospace System Engineering Research Institute
Current assignee: SICHUAN ACADEMY OF AEROSPACE TECHNOLOGY; Sichuan Aerospace System Engineering Research Institute
Priority date: 2017-07-25
Filing date: 2017-07-25
Publication date: 2017-12-22
Anticipated expiration: 2037-07-25
Also published as: CN107506332B

Abstract

本发明公开了一种Kalman滤波器快速实现方法，包括：设计相应的矩阵运算硬件模块；调用各矩阵运算硬件模块，在FPGA内部实现标准卡尔曼滤波算法；通过编写标准卡尔曼滤波IP核的驱动程序，在SOC芯片中的ARM部分直接调用标准卡尔曼滤波IP核，实现包含波形跟踪和位置预测应用的硬件加速，用于降低计算时间。本发明采用并行和流水结合的方式，实现了点积模块，提高系统运行效率；流程模块与运算模块独立分开，简化了系统设计，节约了系统资源；流程模块化，仅与算法的过程相关，与运算无关，方便系统扩展升级。

Description

Kalman滤波器快速实现方法

技术领域

本发明涉及滤波器技术领域，具体涉及一种Kalman滤波器快速实现方法。

背景技术

卡尔曼滤波器是一种由卡尔曼(Kalman)提出的用于时变线性系统的递归滤波器。这个系统可用包含正交状态变量的微分方程模型来描述，这种滤波器是将过去的测量估计误差合并到新的测量误差中来估计将来的误差。

发明内容

本发明克服了现有技术的不足，提供一种节约系统资源、提高运行效率的Kalman滤波器快速实现方法。

考虑到现有技术的上述问题，根据本发明公开的一个方面，本发明采用以下技术方案：

一种Kalman滤波器快速实现方法，包括：

设计相应的矩阵运算硬件模块；

调用各矩阵运算硬件模块，在FPGA内部实现标准卡尔曼滤波算法；

通过编写标准卡尔曼滤波IP核的驱动程序，在SOC芯片中的ARM部分直接调用标准卡尔曼滤波IP核，实现包含波形跟踪和位置预测应用的硬件加速，用于降低计算时间。

为了更好地实现本发明，进一步的技术方案是：

根据本发明的一个实施方案，所述矩阵运算硬件模块包括矩阵运算、浮点运算和点积运算。

根据本发明的另一个实施方案，所述矩阵运算包括：

矩阵加/减法：用于将两个同大小的矩阵对应元素相加/减；

矩阵乘法：用于将两个矩阵相乘；

矩阵求逆：对于n阶矩阵A求逆，先对A进行LU分解，然后对两个矩阵分别求逆，在利用两个逆矩阵的乘积计算矩阵的逆。

根据本发明的另一个实施方案，所述矩阵求逆包括：。

步骤S1，三角分解，得到L和U；

步骤S2，U矩阵求逆；

步骤S3，L矩阵求逆；

步骤S4，两个逆矩阵相乘得到最终得到所求矩阵的逆。

根据本发明的另一个实施方案，所述浮点运算包含浮点加法运算、浮点乘法运算和浮点倒数运算。

根据本发明的另一个实施方案，所述点积运算采用并行加流水的方法来实现。

根据本发明的另一个实施方案，所述FPGA采用Zynq平台，其包含PS和PL两个部分。

根据本发明的另一个实施方案，所述PS部分的工作流程包括：

1)首先初始化PL；

2)将观测向量发送到PL；

3)使能迭代；

4)等待PL迭代结束；

5)读取状态向量的值；

6)重复步骤2)-5)。

本发明还可以是：

根据本发明的另一个实施方案，所述PL部分包括：

接口，用于实现PL与PS的连接，协议为AXI-Lite；

流程控制，用于控制Kalman运算过程中运算执行单元；

运算单元，用于完成矩阵的乘法、加法等运算；

求逆，用于完成协方差矩阵的三角分解及求逆。

根据本发明的另一个实施方案，还包括：

IP生成过程：使用PL的逻辑，封装成IP核；

IP核的使用：通过建立vivado工程，产生bit文件。

与现有技术相比，本发明的有益效果之一是：

本发明的一种Kalman滤波器快速实现方法，1)采用并行和流水结合的方式，实现了点积模块，大大提高了系统的运行效率，尤其是对于阶数高的矩阵；2)流程模块与运算模块独立分开，其中运算模块只与运算符相关，使所有的流程调用同一个运算模块，简化了系统设计，节约了系统资源；3)流程模块化，仅与算法的过程相关，与运算无关，方便系统扩展升级。

附图说明

为了更清楚的说明本申请文件实施例或现有技术中的技术方案，下面将对实施例或现有技术的描述中所需要使用的附图作简单的介绍，显而易见地，下面描述中的附图仅是对本申请文件中一些实施例的参考，对于本领域技术人员来讲，在不付出创造性劳动的情况下，还可以根据这些附图得到其它的附图。

图1为根据本发明一个实施例的加法单元时序示意图。

图2为根据本发明一个实施例的点积模块拓扑示意图。

图3为根据本发明一个实施例的点积模块部分时序示意图。

图4为根据本发明一个实施例的程序流程示意图。

图5为根据本发明一个实施例的PS端主要流程示意图。

图6为根据本发明一个实施例的初始化流程示意图。

图7为根据本发明一个实施例的矩阵分解及求逆流程示意图。

图8为根据本发明一个实施例的LU分解流程示意图。

图9为根据本发明一个实施例的矩阵U的元素求解逻辑示意图。

图10为根据本发明一个实施例的UDE求解过程中主要信号逻辑时序示意图。

图11为根据本发明一个实施例的矩阵L的元素求解逻辑示意图。

图12为根据本发明一个实施例的L元素求解过程逻辑中主要信号时序示意图。

图13为根据本发明一个实施例的流程控制模块流程示意图。

图14为根据本发明一个实施例的运算单元流程示意图。

具体实施方式

下面结合实施例对本发明作进一步地详细说明，但本发明的实施方式不限于此。

本发明是针对卡尔曼滤波器算法的基本运算单元——矩阵浮点运算(加、减、乘、转置和求逆)进行深入分析，研究其在FPGA中的硬件实现方法，设计相应的矩阵运算硬件模块，在此基础上，调用各硬件功能模块，在FPGA内部实现标准卡尔曼滤波算法。通过编写标准卡尔曼滤波IP核的驱动程序，可在SOC芯片中的ARM部分直接调用标准卡尔曼滤波IP核，实现波形跟踪、位置预测等典型应用的硬件加速，显著降低其计算时间。

本发明可基于Xilinx的ZC706开发板，使用vivado和SDK软件开发环境。主要性能要求：对于不超过15阶的Kalman滤波器迭代一次时间不超过500us，运算为单精度浮点数。

标准卡尔曼算法，其流程如下：

S1：初始状态估计和初始协方差P(0|0)赋值；

S2：k时刻的状态估计和估计的协方差P(k|k)；

S3：状态和协方差的一步预测，由下面两式给出

P(k+1|k)＝F(k)P(k|k)F'(k)+Q(k)

S4：量测的预测

S5：新息计算

S6：新息协方差

S(k+1)＝H(k+1)P(k+1|k)H'(k+1)+R(k+1)

S7：计算增益矩阵

K(k+1)＝P(k+1|k)H'(k+1)S^-1(k+1)

S8：状态更新方程及协方差更新方程

P(k+1|k+1)＝P(k+1|k)-K(k+1)H(k)P(k+1|k)

不超过15阶是指流程中矩阵的最大阶数不超过15阶。决定阶数为状态向量X个数m和观测向量的个数n。m，n一旦确定，则涉及到的矩阵阶数分别为

状态转移矩阵F，m×m。

协方差相关矩阵P，m×m。

观测矩阵H，m×n。

过程噪声相关矩阵Q，m×m。

观测噪声相关矩阵R，n×n。

信息协方差矩阵S，n×n。

增益矩阵K，m×n。

矩阵运算：

从流程中可以看出，整个过程包含了矩阵的乘法、加法、减法以及求逆。

矩阵加/减法：

矩阵加法是两个同大小的矩阵对应元素相加，即如果C＝A+B，则c_ij＝a_ij+b_ij，对于m×n 的矩阵，共有mn次浮点加法。

矩阵乘法：

矩阵乘法是两个矩阵相乘，即如果C＝AB，则要求A的列数等于B的行数，且有

其中K为A的列数。

每计算一个元素，需要K次浮点乘法，K-1次浮点加法。因此如果A为MK阶，B为KN阶，则需要的计算量为MNK次浮点乘法，MN(K-1)次浮点加法。

矩阵求逆：

求逆的算法采用三角分解法，其原理如下所述。

矩阵求逆是其中最复杂的一个运算。对于n阶矩阵A求逆，采用LU分解的形式。即首先对A进行LU分解，然后对两个矩阵分别求逆，在利用两个逆矩阵的乘积计算矩阵的逆。

如果

则有

对于上三角矩阵R，设V＝R^-1，则有

对于下三角阵，可以对其作如下操作，

W＝L^-1＝((L^T)^T)^-1＝((L^T)^-1)^T

即先计算下三角阵L的转置，然后求逆，在转置。

对于分解之后的三角矩阵，求逆之后可以通过将两个逆矩阵相乘得到A的逆，即A^-1＝R^-1L^-1＝VW。由于V和W都是三角矩阵，因此可以简化矩阵的乘法运算。

因上，矩阵求逆的流程如下：

S1)三角分解，得到L和U；

S2)U矩阵求逆；

S3)L矩阵求逆；

S4)两个逆矩阵相乘得到最终得到所求矩阵的逆。

三角分解：

从公式中可以看出，分解时首先对第一行的U进行赋值，然后计算L的第一列，然后再计算U的第二行，然后是L的第二行，依次类推。

对于L矩阵，对角线上元素全为1。因此两个矩阵加起来的元素与原矩阵相同。

对于U的元素，求解过程为

可以看出对于n阶方阵，需要的浮点乘法和加法都约为(n³+3n²-n)/6。

对于L的元素，只少了n次乘法和加法，但增加了n次除法。

因此三角分解运算量要少于相同的两个矩阵的乘法运算，但逻辑上比较复杂。

三角矩阵求逆：

矩阵求逆元素的公式为

矩阵求逆运算于三角分解运算大体相同，但每个元素都少了减法，同时，对于L矩阵，每个元素都少了一次乘法运算。因此求逆运算量比三角分解的运算量还要低。

同时，由于求逆时，从公式中可以看出，是按照对角线从下往上计算，然后计算第二对角线，依次类推，直到计算最上角一个元素。

对于U矩阵和L矩阵有不同的求逆推算公式，但如果各自求逆，则两者的逻辑是不同的，给实现带来一定的麻烦，而求转置则很容易实现，因此这里将两个三角矩阵的求逆按照U矩阵求逆来实现。

三角逆矩阵相乘：

求逆的最后一步是两个三角逆矩阵相乘，计算量为n³次乘法和加法，随着n的增加，乘法运算的计算量增加远远大于矩阵三角分解和三角矩阵求逆。

浮点运算：

从矩阵运算中可以看出，最基本的浮点运算单元包括了加法/减法、乘法、求倒数(除法以求倒数相乘来实现)。

浮点运算采用XIlinx芯片自带的IP核floating point IP V7.1。

浮点加法：

配置时，精度为单精度，最高速度，DSP Slice采用max usage模式。

采用blocking模式，不需要输出反馈。

latency采用最大值。

浮点加法模块的接口如下：

aclk：系统时钟

s_axis_a_tvalid：输入A有效

s_axis_a_tready：A可以输入指示

s_axis_a_tdata：数据A

s_axis_b_tvalid：输入B有效

s_axis_b_tready：B可以输入指示

s_axis_b_tdata：数据B

m_axis_result_tvalid：结果有效

m_axis_result_tdata：结果数据

从实际运行来看，如果将Latecy设置成1，结果依然是在12个时钟之后输出。从综合的结果来看，其最差负时序裕量WNS和总的负时序裕量TNS都已经变成了红色，意味着时序不满足要求(这里的时序约束为100MHz的时钟)。

对于加法单元，其时序可以表示如图1所示，Latency是可以设置的结果。这里设置成了12，即在输入有效后的第12个时钟的时候，输出才有效。

浮点乘法：浮点乘法设置与加法基本相同，浮点乘法的接口与加法类似，因此不在详述。

浮点倒数：倒数与加法不同的地方是只有一个输入，且延时时间更长，使用的DSPSlice 更多。同时运算过程中，如果U矩阵的对角元素为0，则不可逆，因此增加了Dividedby zero 指示，如果有效，则说明矩阵不可逆。

点积运算：

从矩阵运算中可以看出，矩阵乘法、三角分解、三角矩阵求逆等过程涉及最多的运算除了加法、乘法之外，还可以将点积运算看成一个基本的运算单元，即

该运算包含K次乘法和K-1次加法。在实现时有多种方法，这里采用并行加流水的方法来实现。

以N＝8为例。运算单元包括8个乘法器M0～M8和15个加法器A0～A6以及一个选择器MUX。

乘法器为两个需要点积的矢量的输入，加法器按照4，2，1分成三级。如果A的列大于4，则最后一级加法器A6的输出为最终结果；如果A的列大于2，则第二级的第一个加法器A4的输出为最终结果；如果A的列大于1，则第一级第一个加法器A0的输出结果为最终结果；否则，第一个乘法器M0的输出为最终结果。M0、A0、A4、A6的结果都输入到选择器MUX，选择器矢量长度来选择相应的结果，点积模块的拓扑如图2所示。

对于本项目中的点积模块为长度为16的模块，即相对上图，乘法器扩大一倍，加法器扩大一倍的同时，在增加一级加法器输出。点积模块的时序示意如图3所示。

图3中所示为一阶乘法和一阶加法的结果，即D0＝C0+C1＝A0*B0+A1*B1。输出为一阶乘法的延迟LatencyMult和一阶加法的延迟LatencyAdd之和。如果是16阶，则结果是一阶乘法的延迟和4阶加法的延迟。

在计算过程中，由于采用了流水设计，所以N个输入可以连续输入，得到的结果也是连续输出。

逻辑流程：

Zynq平台分为了PS+PL两个部分，即可编程的系统和可编程的逻辑。实现主要集中在 PL部分，PS只完成初始化的过程，以及更新观测向量。系统流程如图4所示。

PS端逻辑流程：

PS端程序主要完成系统的初始化，读取迭代完成后状态变量的值。工作流程如图5所示：

1)首先初始化PL；

2)将观测向量发送到PL；

3)使能迭代；

4)等待PL迭代结束；

5)读取状态向量的值；

6)重复步骤2)-5)。

寄存器设置：

在PS端设置8个寄存器，同PL的接口端一样。分别是REG0～REG7。其中REG0为状态变量以及观测变量个数，其中高16位为观测变量个数，低16位为状态变量个数。由于在C 语言中计数从0开始，到n-1结束，因此在传递实际值时要减去1。例如，15个状态变量，则传递给PL端的值应该为14。

REG1为矩阵片选，也代表了矩阵的地址，矩阵的地址如下：

MATRIX_F＝5'h1；

MATRIX_H＝5'h2；

MATRIX_Q＝5'h3；

MATRIX_R＝5'h4；

MATRIX_P＝5'h5；

MATRIX_T＝5'h6；

MATRIX_S＝5'h7；

MATRIX_K＝5'h8；

MATRIX_I＝5'h9；

MATRIX_U＝5'ha；

MATRIX_L＝5'hb；

MATRIX_G＝5'hc；

VEC_X＝5'h10；

VEC_V＝5'h11；

VEC_Z＝5'h12；

REG2：预留；

REG3：使能有效指示及工作模式设置。为1时，系统工作在中断状态，为2时，系统工作在查询状态；

REG4：中断寄存器。PS端接收到中断后读取该寄存器，判断中断源。其中

bit 0：最低位表示迭代是否完成，高电平表示完成；

bit 1：矩阵不可逆指示，高电平表示矩阵不可逆；

REG5：PL流程的状态指示，用以在查询模式中判断迭代是否完成，为0时表示迭代完成；

REG6：时间计数，表示PL端完成一次迭代所需的时间计数，单位是10ns。

REG7：数据操作，用来读写数据。

初始化：

初始化部分的程序代码kalman.c，详细说明见《程序说明》。

初始化的目的是将固定不变的矩阵传输到PL端，如噪声矩阵R和Q。还包括状态向量初始值X0、协方差矩阵初始值P0。初始化的流程如图6所示。

初始化过程中，必须先设置REG0，后面矩阵的设置顺序可以改变。初始化函数为

kalmanInit((float*)x0,(float*)matrixP,(float*)matrixH,(float*)matrixQ, (float*)matrixR,int STATENO,int OBSERVENO)；

上述函数中，输入分别为

float*x0：初始化状态向量首地址；

float*matrixP：初始化协方差矩阵首地址；

float*matrixH：初始化观测矩阵首地址；

float*matrixQ：初始化状态噪声首地址；

float*matrixR：初始化观测噪声首地址；

int STATENO：状态向量个数；

int OBSERVENO：观测向量个数；

在该函数中，初始化包括X0、P、H、Q、R等向量和矩阵，在初始化过程中，通过寄存器REG7来读写数据，在写数据之前首先写入寄存器REG1来确定写入那个寄存器，之后按照矩阵大小，且按照行依次写入矩阵的值。

对于矩阵阵，可以使用

void matrixInit(float*matrix,u32 matrixSel,u32 lineNo,u32 columnNo)

float*matrix：矩阵首地址；

u32 matrixSel：矩阵片选；

u32 lineNo：行数；

u32 columnNo：列数；

以写入F矩阵为例，其写入过程为

浮点操作寄存器函数系统没有自带，因此自定义两个函数来实现浮点操作。

写函数：

读函数

对于矢量，例如观测矢量，可以使用初始化矢量函数

void vectInit(float*vector,u32 matrixSel,u32 length)

相对于矩阵，这个输入参数只包含了向量的长度。

使能迭代：

使能迭代用来打开或关闭一次迭代过程，函数为

void sysEn(int mode)

mode为工作模式选择，0为关闭，1为打开为中断模式；2为打开为查询模式。

状态查询：

状态查询函数是用来在查询模式下判断系统的工作状态，函数为

int statusCheck(void)

该函数的返回值为0，代表迭代完成，可以读取状态变量结果了。

矩阵回读读函数：

读函数是用来读取矩阵的值的函数，可以用来判断矩阵是否写入成功。该函数的声明如下。

void matrixRead(float*matrix,u32 matrixSel,u32 lineNo,u32 columnNo)

对于矢量，回读函数为

void matrixRead(float*vector,u32 matrixSel,u32 lineNo)

中断函数：

中断函数主要包括使能、关闭中断，初始化中断，以及中断响应几个函数。分别是

中断使能函数

void kalman_interrupt_Enable()

中断关闭函数

void kalman_interrupt_disable()

中断初始化函数

void kalman_interrupt_init()

中断设置函数

int ScuGicInit(u16 DeviceId,u32 int_id,Xil_ExceptionHandlerDeviceDriverHandler)

中断响应函数

void DeviceDriverHandler1(void*CallbackRef)

如果系统设置为中断模式，则需要的处理在中断响应函数中完成。

PL端逻辑流程：

为实现Kalman算法，将PL端分成如下几部分。

1)接口，用来实现PL与PS的连接，协议为AXI-Lite。

2)流程控制，用来控制Kalman运算过程中运算执行单元。

3)运算单元，用来完成矩阵的乘法、加法等运算。

4)求逆，用来完成协方差矩阵的三角分解及求逆。

求逆：

该部分的程序代码为inverse.v，说明文档为《程序说明》。

求逆模块是用来完成协主差矩阵的三角分解，以及分解后三角矩阵的求逆。两个逆矩阵相乘不在该模块完成。

该模块的逻辑流程图如图7所示。

流程分为：

1)复位，系统上电或复位时处于该状态，复位完成后进入空闲状态；

2)空闲，等待求逆指令。复位完成以及求逆完成后都进入该状态；求逆开始后，转入矩阵初始化状态。

3)初始化矩阵，求逆使能后将待求逆的矩阵写入缓存；写完后进入U分解状态。

4)U分解，按行对U矩阵进行求解，如果求完最后一行，则进入U矩阵求逆。否则进入L分解；

5)L分解，按列对L矩阵进行求解，求解完成后进入到U分解状态。

6)U求逆，对上三角U进行求逆，并将结果存在入内存管理中的URAM块中；完成后进入L求逆状态。

7)L求逆，对下三角L进行转置，并求逆，将结果转置后存入内存管理中的LRAM块中。完成后进入到空闲状态。

LU分解：

LU分解是整个过程中逻辑最复杂的模块，其如图8所示，具体流程：

1)从公式中可以看出，对于U矩阵的第一行，可以直接赋值，因此首先判断是否是第一行。

2)对于L的第一列，是原矩阵的第一列乘以U对角线上第一个值的倒数，因此首先要对U(1,1)求倒数，完成后进行乘法运算，得到L(:,1)的值。之后行数i加1。

3)不是第一行的时候，计算第i行的U值，通过点积完成。

4)第i行的U值计算完成后，开始并行计算U(i,i)的倒数和L(j,i)的点积。都完成后，如果不是最后一行，则相乘求L的元素值。如果是最后一行，则代表完成了分解，进入下一状态。

该模块中流程描述相对简单，但由于涉及到了数据的读取，而且包括了点积、倒数、减法等诸多运算，而每个运算的使能控制以及数据的计数都是比较复杂的逻辑。

对于U矩阵求解过程的运算逻辑如图9所示。图9说明矩阵U的元素是原始数据减去点积结果的值，这是根据公式直接可以得到的。

在计算U时，点积的输入为L矩阵的当前行和U矩阵的当前列，其中由于行不变，因此 L矩阵的数据在求U的一行时，不发生变化。该部分的数据逻辑如图10所示。

对于L矩阵求解过程增加了倒数和乘法运算，如图11所示。图11中倒数结果有效与乘法结果有效有个逻辑，只有两个运算都完成后才开始进行乘法运算。在计算L时，点积输入为U的当前列和L的当前行。由于L是按列计算，所以此时U的当前列不发生变化。对于这部分的主要信号逻辑流程如图12所示。需要注意的是，这部分逻辑中，由于点积的个数随着行数的增加而减少，因此点积和减法运算完成的时长也随着行数的增加而减少。但该逻辑中还存在有求倒数的逻辑，而求倒数逻辑的时长是固定的。因此增加了两个信号来标示减法和求倒数是否完成，只有同时完成时，才开始乘法运算。上图中，只给出了减法这后的控制信号，而没有数据信号。

在这部分逻辑中，还需要对各种计数器进行逻辑赋值。

LU分解的逻辑说明比较复杂，在《程序说明》文档中进行了描述，因此这里不在详述。

三角矩阵求逆：

从公式上看，对于U矩阵，其逆元素是点积结果乘以相应对角线元素倒数的结果。即计算公式

这里存在两个矩阵，即待求逆的矩阵u和逆矩阵v。根据前面提到的，在计算时沿对角线的数据开始计算。在第k次对线上的元素，是u矩阵的当前行与v矩阵的当前列的点积，点积长度为k。因此，该部分主要是对点积赋值。由于在求分解的过程中已经将倒数求出，因此不在单独求倒数，直接调用。对于L矩阵，由于对角元素为1，因此不存在乘法，点积结果即为结果。求逆时，主要涉及的计数器包括了点积计数器，乘法输入的计数器以及结果计数器，这部分的逻辑控制与LU分解一样，比较复杂。逻辑在程序说明中说明，不在这里详述。

流程控制：

流程控制模块主要用来实现控制PL的系统流程，包括何时进行何种操作，操作对象是谁等。模块主要接口是针对运算模块，以控制运算模块进行运算。主要流程如图13所示。流程中因为是依次执行，因此所有流程的转换条件都是操作完成，依次进入下一个流程。从图中可以看出主要流程包括：

IDLE：复位状态。

S1：计算状态的进一步预测中的临时矩阵，即T＝F*P。

S2：计算状态的进一步预测矩阵，即P＝T*F’+Q。

S3：计算新息协方差矩阵过程的临时矩阵T＝P*H’。

S4：计算新息协方差矩阵S＝H*T+R

S5：计算状态进一步预测x＝F*x

S6：计算新息过程v＝z-H*x

S7：计算两个三角矩阵的积I＝L*U

S8：计算增益矩阵K＝T*I

S9：计算状态更新方程x＝x+K*v

S10：计算协方差更新矩阵T＝K*H

S11：计算协方差更新矩阵T＝P-T*P

S12：计算最终协方差更亲矩阵P＝T*E

WAIT：等待状态，等待PS完成求逆过程，该状态在S6与S7之间。

另外，还有一个与S5并行的状态，即求逆。求逆包括了LU分解，上三角求逆。求最终逆的结果由S7完成。

除IDLE、求逆和WAIT外，共有11个状态，其中涉及到的运算包括：

O1：C＝A*B，如S1、S7、S8、S10、S12

O2：C＝A*B'，如S3

O3：D＝A*B+C，如S4

O4：D＝A*B'+C，如S2

O5：b＝A*a，如S5

O6：c＝b-A*a，如S6

O7：c＝b+A*a，如S9

O8：D＝C-A*B，如S11

每个运算涉及最多4个矩阵/向量，其中每个里面只有一个矩阵乘法运算。因此每个状态里面输出包括操作的矩阵地址、操作使能等信息。例如对于状态S1，计算T＝F*P，那么操作码为O1，读A的地址为MATRIX_F，读B的地址为MATRIX_P。其余状态都相同。

运算单元：

运算单元是完成上一节提到的8种运算，每次根据流程的运算指示完成相应的运算。运算单元的流程如图14所示。复位后处于空闲状态。如果运算使能有效，运算开始。开始后首先按照流程控制单元给的矩阵片选来选择读取的矩阵，从内存管理模块中将数据读入到运算单元的临时缓存A中。之后，开始读取矩阵B的值。如果是矩阵乘以矢量的运算，则从运算单元的矢量缓存中读取数据并存入到临时缓存B中；如果是矩阵相乘则从内存管理模块中读取相应矩阵的数据，并存入到临时缓存B中；如果是矩阵相乘的时候需要转置，则读的时候行列地址互换。矩阵读取完成之后，开始进行乘法运算。在点积结果有效后，判断运算是需要计算加减，如果不需要，则系统开始输出，到运算完成时返回空闲状态；如果需要继续运算，则开始加法/减法运算，运算结果有效时开始输出。图中A*B表示两个矩阵相乘。C+D表示乘法的结果与第三个矩阵相加。系统复位后进入空闲状态，如果运算使能有效时开始读入要操作的两个矩阵的元素，存入到缓存中。之后开始计算两个矩阵的乘法，计算完成后，根据操作符判断是否需要求加/减法运算，如果需要则进行相应的运算。运算完成后进入空闲状态。

矩阵存储管理：

内存管理是用来操作PS初始化的矩阵以及在运算过程中矩阵的存储。该部分的矩阵包括：状态转移矩阵F，方阵，stateNo阶；量测矩阵H，observeNo×stateNo；过程噪声相关矩阵Q，方阵，stateNo阶；量测噪声相关矩阵R，方阵，observeNo阶；协方差矩阵P，方阵，stateNo阶；临时矩阵T，由过程决定；新息协方差矩阵S，方阵，observeNo阶；上三角矩阵的逆U，方阵，observeNo阶；下三角矩阵的逆L，方阵，observeNo阶；增益矩阵 K，stateNo×observeNo；新息协方差矩阵的逆I，方阵，observeNo阶；控制矩阵G，方阵， stateNo阶；单位矩阵E，方阵，16阶。本模块存储矩阵使用的是内部的双口RAM，大小为 32X256。之所以使用双口RAM，是因为这样容易将初始化与逻辑操作RAM分开使逻辑设计简单。其中RAM的A端口为初始化使用，B端口为逻辑使用。本项目中每个端口只使用了地址 ADDR、时钟CALK、写使能REGCE以及数据输出DOUT。

A端口：端口A是用来和PS相对接的，即将PS初始化时的矩阵通过A端口写入到相应的RAM中，也通过A端口将数据读回到PS端。当PS写入数据时，因为首先要选择矩阵片选，此时相应的矩阵写使能有效。PS操作数据时，都是操作寄存器REG7，因此没有地址总线。在内存管理单元中产生地址总线。即每操作一个数据，地址累加1。同时根据矩阵的不同，来选择行列地址累加值的不同。

B端口：B端口是运算过程中操作的端口。运算过程中会产生片选信号、读写地址、读使能以及数据。B端口根据片选来选通不同的矩阵单元。

接口逻辑：接口为AIX-Lite协议，PS端设置了8个寄存器，完成了初始化、数据读写、中断产生等功能。中断产生的中断源有两个，不可逆指示和迭代完成。两上中断源任何一个有效时，就会产生中断。即如果矩阵不可逆，则产生中断，迭代停止；迭代完成后，系统流程也处于空闲状态，等待下一次PS使能迭代，同时也会产生中断。如果系统工作在查询模式，则中断无效，即不产生中断。该部分由于使用了AXI-Lite协议，因此通讯速率较慢，经过实际测试，写入一个数据耗时约为100ns，即如果传输225个数据，耗时为22us。本项目中，在迭代过程中需要传输的矩阵为状态转换矩阵和观测向量，两者的最大数据量为240个数据，加上回读状态向量15个数据，总共是255个数据，耗时约为26us，是可以接受的。但由于系统中通讯量并不大，因此可以满足要求。

生成过程：

IP生成过程：IP生成过程主要是使用PL的逻辑，封装成IP核。首先使用vivado建立一个生成IP的工程。将PL端代码添加到工程中后，添加使用的Xilinx自带的IP核，包括浮点运算核、RAM核、时钟管理核等。由于本工程已生成。如果要修改某些地方，按照需要进行修改。修改完成后生成IP核。点击Tools->Create and Package IP。在弹出对话框中点击next。然后在弹出对话框中选择Package your current project，点击next。选择IP 生成的位置，并选择Include.xci files。点击next。在弹出对话框中选择Overwrite。

IP核的使用：该IP在使用时，首先建立一个vivado工程。在过程选择合适的器件。本项目采用的是Xilinx的开发板XC706。在选择合适的路径后，点击确定，生成相应界面，分别添加PS核和生成的IP核，系统自动连接后生成，产生bit文件。

PS调试：PS端调试主要是用来测试和使用IP核的。PS使用的工具是Xilinx的SDK。打开后，新建一个工程。此时工程中包含了上一节硬件生成过程的硬件支持包xxx_bsp以及硬件平台xxx_wrapper_hw_platform_0两个文件夹。将以上的PS端程序加入即可进行调试。

耗时分析：

由于迭代过程耗时与矩阵尺寸有关，而矩阵尺寸又跟观测向量与状态向量中元素的个数相关。在分析时，以最大15阶为例分析迭代一次的最大耗时。

矩阵乘法：矩阵乘法在运算单元中实现，而在运算单元中，首先是读取两个要相乘的矩阵，然后再进行点积。结果有效时输出。假设两个n(7<n<16)阶的矩阵相乘，那么需要消耗的时钟包括：1)在读两个矩阵时，需要2*n*n个时钟，加上流程的开销，需要约4个时钟，即读取矩阵需要2*n*n+4个时钟。然后就开始进行点积。2)从点积开始到结果输出有效的延迟时间为固定值，即4阶加法的延迟48个时钟，1阶乘法的延迟7个时钟，等待延迟2个时钟，即55个时钟。3)输出个数为结果矩阵个数，即n*n个时钟，再加上状态转换的时钟消耗，需要约2个时钟，即总共2+n*n个时钟。综上，两个n阶矩阵相乘的时间为3*n*n+62个时钟。这里需要说明的是对于矩阵相乘，消耗的时钟个数是不变的，与元素的值无关。对于 15阶的矩阵相乘，耗时为3*15*15+62＝737个时钟，即7.37us。

矩阵乘法和加法相连：

在运算中，如O8，计算D＝A*B+C，即三个矩阵运算，先计算乘法再计算加法。

该运算中，乘法和上一节相同，但在点积输出有效时，开始进行A*B的结果与矩阵C的加法，因此，相比于乘法，这里只多了一个加法的延时和1个时钟的逻辑延迟，即13个时钟的延迟。该运算耗时为756个时钟，比之前多了13个时钟，与理论相符，即7.56us。

矩阵求逆：

矩阵求逆分为三步，即三角矩阵分解、三角矩阵求逆以及三角矩阵相乘。这里相乘放在了运算单元，只有两个步骤，即三角矩阵求解与求逆。

三角矩阵分解：

三角矩阵分解过程中是U矩阵和L矩阵依次进行，而不能分开，因此将两个矩阵分解一起计时。由于在开发板上抽取信号时，深度只有1024个，而分解的时间超过了1024，因此分成几次拼起时间来看。如下图所示为15阶方阵三角分解的过程。试验中，第1行到第6行耗时为733个时钟，第7行到第11行耗时803个时钟，第12行到15行耗时600个时钟，加在一起共耗时2136个时钟，即约21.36us。

三角矩阵求逆：三角矩阵求逆运算分为U矩阵求逆和L矩阵求逆。两者在逻辑上基本相同，但由于L矩阵的对角线全为1，因此相对于U矩阵，少了对角元素倒数的乘法运算。试验中，可以看出U矩阵求逆所需时钟约为880个，即8.8us；L矩阵求逆所需时钟约为770个，即7.7us。因为每个乘法的延迟为7个时钟，15个为105个时钟，加上逻辑消耗，110个时钟与理论也是符合的。

求逆耗时：由以上可以看出，三角矩阵分解耗时21.36us，U矩阵求逆需要8.8us，L矩阵求逆需要7.7us，三者相加约为37.86us。同时，由于该部分的内存是在S求解过程中写入的，不需要将数据再读入到缓存中，而且输出是在求逆过程中每求出一个则输出一个，因此也不占用额外的时钟，即总体的时钟不超过38us。如果算上两个三角逆矩阵相乘的7.85us，则15阶矩阵的求逆总耗时约为46us。

总体耗时：从系统逻辑流程中可以看出，步骤S5和S6的执行与三角分解和求逆并行执行，因此可以不计算总体耗时时间。每个步骤的耗时如下表所示：

除了S5和S6之外还有9个运算加求逆，总体耗时为57.77us。

在实际运行时，计数结果输出如下图所示。从图中可以看出实际时间是57.75us，与分段计算吻合。且每次迭代的结果相同，这是因为FPGA处理时，只与时钟相同，而与数据无关。从上述结果可以看出，时间满足系统要求。

资源占用分析：FPGA的资源主要包括了查找表(LUT)、存储资源(BRAM)以及DSP资源。其中LUT主要用来完成逻辑实现，BRAM用来存储数据，DSP做为浮点运算加速器。使用的LUT 为31.58％，69036个，而对于ZYNQ系列，其资源情况如图所示。可以看出至少要使用XC7Z030 的芯片才能够满足要求，此时资源会达到87％，对系统的布局布线造成一定的影响。对于几个模块的单独测试，各部分的LUT资源消耗如下表所示。

可以看出最消耗LUT资源的是三角求逆，其次是LU分解。其原因是在这两个运算中，为保证点积运算时每次运算都是两个矢量的乘法同时完成，因此要把矩阵的数据缓存到一个临时变量中。操作该临时变量的过程是最耗LUT资源。该部分是通过资源的消耗来换取了时间的加速。

正确性验证：为验证系统工作的正确性，同时使系统符合15阶状态向量，6阶观测向量，在matlab中建模。模型中分为三个维度，每个维度的观测向量包含距离和速度信息，且每个维度的状态向量包换距离、速度、加速度、加加速度和加加速度的导数，即包含距离的从0 阶到4阶导数。

每一维的距离和速度方程为

式中R₀为初始距离；

v₀为初始速度；

a₀为初始加速度；

j₀为加加速度；

k₀为加加加速度；

ε为误差。

式中所有值的初始值都为随机值。

每一维的状态转移方程可以表示为

系统的状态转移矩阵可以表示为

式中

而对于量测方程，可以表示为

由上式可得对于观测矩阵为

式中

该模型的意义是系统中可以得到带有一定误差的距离和速度值，从该值中可以得到估计的距离、速度、加速度、加加速度、加加加速度5个变量。硬件结果与matlab结果非常接近，两者的误差在10-5量级，基本符合单精度浮点数的精确度，因此可以得到硬件解算结果正常的结论。

综上所述，本发明具有：能够实现矩阵的基本运算，包括：加、减、乘、转置和求逆；满足单精度浮点运算要求；可定制矩阵阶次，可最大支持16阶；3)矩阵是否可逆有指示，并能输出逆矩阵；可与ARM部分进行高速数据通信，通过ARM部分调用标准IP核，实现算法的硬件加速等。

在本说明书中所谈到的“一个实施例”、“另一个实施例”、“实施例”、等，指的是结合该实施例描述的具体特征、结构或者特点包括在本申请概括性描述的至少一个实施例中。在说明书中多个地方出现同种表述不是一定指的是同一个实施例。进一步来说，结合任一实施例描述一个具体特征、结构或者特点时，所要主张的是结合其他实施例来实现这种特征、结构或者特点也落在本发明的范围内。

尽管这里参照本发明的多个解释性实施例对本发明进行了描述，但是，应该理解，本领域技术人员可以设计出很多其他的修改和实施方式，这些修改和实施方式将落在本申请公开的原则范围和精神之内。更具体地说，在本申请公开和权利要求的范围内，可以对主题组合布局的组成部件和/或布局进行多种变型和改进。除了对组成部件和/或布局进行的变型和改进外，对于本领域技术人员来说，其他的用途也将是明显的。

Claims

1.一种Kalman滤波器快速实现方法，其特征在于包括：

设计相应的矩阵运算硬件模块；

2.根据权利要求1所述的Kalman滤波器快速实现方法，其特征在于所述矩阵运算硬件模块包括矩阵运算、浮点运算和点积运算。

3.根据权利要求2所述的Kalman滤波器快速实现方法，其特征在于所述矩阵运算包括：

矩阵加/减法：用于将两个同大小的矩阵对应元素相加/减；

矩阵乘法：用于将两个矩阵相乘；

4.根据权利要求3所述的Kalman滤波器快速实现方法，其特征在于所述矩阵求逆包括：。

步骤S1，三角分解，得到L和U；

步骤S2，U矩阵求逆；

步骤S3，L矩阵求逆；

步骤S4，两个逆矩阵相乘得到最终得到所求矩阵的逆。

5.根据权利要求1所述的Kalman滤波器快速实现方法，其特征在于所述浮点运算包含浮点加法运算、浮点乘法运算和浮点倒数运算。

6.根据权利要求1所述的Kalman滤波器快速实现方法，其特征在于所述点积运算采用并行加流水的方法来实现。

7.根据权利要求1所述的Kalman滤波器快速实现方法，其特征在于所述FPGA采用Zynq平台，其包含PS和PL两个部分。

8.根据权利要求7所述的Kalman滤波器快速实现方法，其特征在于所述PS部分的工作流程包括：

1)首先初始化PL；

2)将观测向量发送到PL；

3)使能迭代；

4)等待PL迭代结束；

5)读取状态向量的值；

6)重复步骤2)-5)。

9.根据权利要求7所述的Kalman滤波器快速实现方法，其特征在于所述PL部分包括：

接口，用于实现PL与PS的连接，协议为AXI-Lite；

流程控制，用于控制Kalman运算过程中运算执行单元；

运算单元，用于完成矩阵的乘法、加法等运算；

求逆，用于完成协方差矩阵的三角分解及求逆。

10.根据权利要求7所述的Kalman滤波器快速实现方法，其特征在于还包括：

IP生成过程：使用PL的逻辑，封装成IP核；

IP核的使用：通过建立vivado工程，产生bit文件。