[发明专利]基于FPGA的高维卡尔曼滤波器及其实现方法

摘要

本发明涉及一种基于FPGA的高维卡尔曼滤波器及其实现方法，属于航空航天中的导航技术领域，它由滤波逻辑控制器，双精度运算器以及数据存储器组成；滤波逻辑控制器由状态预测控制器，协方差预测控制器，状态更新控制器，卡尔曼增益更新控制器以及协方差更新控制器组成；运算器主要由双精度的乘法器，除法器，加法器以及减法器组成。存储器的寻址范围能够调整，每个地址内存储的数据为六十四位的双精度浮点数据。本发明的四个子运算器可以在单个时钟下独立并行地完成一次双精度的滤波计算，大大提高了高维卡尔曼滤波算法的执行速度与计算精度。

主权项

基于FPGA的高维卡尔曼滤波器,其特征在于包括：滤波逻辑控制器，运算器及数据存储器；所述滤波逻辑控制器由状态预测控制器(2a)，协方差预测控制器(2b)，卡尔曼增益更新控制器(2d)，状态更新控制器(2c)及协方差更新控制器(2e)五个逻辑子控制器组成；所述运算器由双精度乘法器(3a)，双精度除法器(3b)，双精度加法器(3c)以及双精度减法器(3d)四个子运算器构成，每个子运算器有两个数据输入端口和一个数据输出端口；逻辑控制器给定滤波时序，在滤波时序的约束下把从数据存储器中读出的数据赋值给运算器的输入端口，并把运算器的输出数据写入到数据存储器中；双精度乘法器(3a)，双精度除法器(3b)，双精度加法器(3c)以及双精度减法器(3d)四个子运算器彼此独立，它们的输入输出数据均通过数据总线与数据存储器相连接；当状态预测控制器(2a)，协方差预测控制器(2b)，卡尔曼增益更新控制器(2d)，状态更新控制器(2c)及协方差更新控制器(2e)五个逻辑子控制器所给定时序依次执行后，则完成一次滤波，此时再循环执行，实现实时滤波；状态预测控制器(2a)控制读取存储器中与状态预测有关的数据，送至所需要的双精度乘法器(3a)，双精度除法器(3b)，双精度加法器(3c)以及双精度减法器(3d)四个子运算器进行计算，并把计算结果写入到数据存储器中的状态预测结果存储区域，在此过程完成1次“方阵×列向量”运算；协方差预测控制器(2b)依次控制读取数据存储器中与协方差预测有关的数据，送至所需要的双精度乘法器(3a)，双精度除法器(3b)，双精度加法器(3c)以及双精度减法器(3d)四个子运算器进行计算，并把计算结果写入到数据存储器中的协方差预测结果存储区域，在此过程完成1次“方阵×方阵”运算，1次“方阵×方阵”运算和1次“方阵+方阵”运算；卡尔曼增益更新控制器(2d)依次控制读取数据存储器中与卡尔曼增益更新有关的数据，送至所需要的双精度乘法器(3a)，双精度除法器(3b)，双精度加法器(3c)以及双精度减法器(3d)四个子运算器进行计算，并把计算结果写入到数据存储器中的卡尔曼增益更新结果存储区域，在此过程完成1次“方阵×方阵”运算，1次“方阵×方阵”运算，1次“方阵+方阵”运算，1次矩阵求逆运算和1次“方阵×方阵”运算；状态更新控制器(2c)依次控制读取数据存储器中与状态更新有关的数据，送至所需要的双精度乘法器(3a)，双精度除法器(3b)，双精度加法器(3c)以及双精度减法器(3d)四个子运算器进行计算，并把计算结果写入到数据存储器中的状态更新结果存储区域，在此过程完成1次“方阵×列向量”运算，1次“列向量‑列向量”运算，1次“方阵×列向量”运算和1次“列向量+列向量”运算；协方差更新控制器(2e)依次控制读取数据存储器中与协方差更新有关的数据，送至所需要的双精度乘法器(3a)，双精度除法器(3b)，双精度加法器(3c)以及双精度减法器(3d)四个子运算器进行计算，并把计算结果写入到数据存储器中的协方差更新结果存储区域，在此过程完成1次“方阵×方阵”运算,1次“方阵‑方阵”运算和1次“方阵×方阵”运算；数据存储器实现为，卡尔曼滤波器的维数为D，一个列向量所需的空间大小即为D，一个方阵所需要的空间大小为D*D，把各个矩阵依次存储在RAM中，根据以上的矩阵空间分配原则，对维数不同的卡尔曼滤波器计算出相应矩阵的偏移地址，当维数发生变化时，再另行计算，这样对卡尔曼滤波器的维数进行扩展。