[发明专利]Kalman滤波器快速实现方法

权利要求书

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

设计相应的矩阵运算硬件模块；所述矩阵运算硬件模块包括矩阵运算、浮点运算和点积运算；

所述矩阵运算包括：

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

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

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

所述矩阵求逆包括：

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

步骤S2，U矩阵求逆；

步骤S3，L矩阵求逆；

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

调用各矩阵运算硬件模块，在FPGA内部实现标准卡尔曼滤波算法；所述FPGA采用Zynq平台，其包含PS和PL两个部分，所述PS部分的工作流程包括：

1）首先初始化PL；

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

3）使能迭代；

4）等待PL迭代结束；

5）读取状态向量的值；

6）重复步骤2）-5）；

所述PL部分包括：

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

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

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

求逆，用于完成协方差矩阵的三角分解及求逆；具体包括：

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

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

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

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

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

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

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

LU分解具体包括：

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的元素值，如果是最后一行，则代表完成了分解，进入下一状态；

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

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

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

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

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

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