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

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

【技术实现步骤摘要】
基于FPGA的高维卡尔曼滤波器及其实现方法
本专利技术涉及基于FPGA的高维卡尔曼滤波器及其实现方法，属于航空航天中的导航

技术介绍
卡尔曼滤波采用状态空间法在时域内设计滤波器，是一种解决离散系统线性滤波问题的递推最优估计算法。本世纪60年代以来,卡尔曼滤波算法得到了深入的研究和广泛的工程应用,许多非凡技术成就都可部分地归因于卡尔曼滤波器,它有许多军事的、工业的以及科学的应用。其中主要应用包括:宇宙飞船的航行、导弹的制导和控制、卫星导航与定位、雷达和飞机自动驾驶仪等等。在实际应用中，卡尔曼滤波器常采用通用的处理器来实现，如单片机，DSP处理器以及ARM处理器。通用处理器上的卡尔曼滤波器一般通过C语言编程实现，因而它具有设计简单的特点。但由于卡尔曼滤波器在运算的过程中存在大量的高维矩阵加减乘除运算，同时通用的处理器采用了程序顺序执行的硬件架构，因而在对实时性要求比较高的场合，通用处理器上的卡尔曼滤波器往往不能够满足系统对运行速度的要求。针对通用微处理器执行速度较慢、效率偏低的缺陷，充分利用FPGA(FieldProgrammableGateArray,现场可编程门阵列)自身高度并行性、执行速度快的特点，近些年来人们对在FPGA中定制卡尔曼滤波算法的硬件电路展开了初步的研究。而目前基于FPGA的卡尔曼滤波器限于FPGA芯片内部逻辑资源有限，因而所设计的滤波器的矩阵维数较低，同时采用定点小数进行运算节省运算资源，这大大降低了卡尔曼滤波器的滤波性能。
技术实现思路
本专利技术技术解决问题：克服现有技术的不足，提供一种基于FPGA的高维卡尔曼滤波器及其实现方法，以解决常规处理器执行高维卡尔曼滤波算法的速度慢、精度低，从而会影响导航性能的问题，大大提高了高维卡尔曼滤波算法的执行速度与计算精度。本专利技术技术解决方案：基于FPGA的高维卡尔曼滤波器,如图1所示，包括：滤波逻辑控制器，运算器及存储器；所述滤波逻辑控制器由状态预测控制器(2a)，协方差预测控制器(2b)，卡尔曼增益更新控制器(2d)，状态更新控制器(2c)及协方差更新控制器(2e)五个逻辑子控制器组成；所述运算器由双精度乘法器(3a)，双精度除法器(3b)，双精度加法器(3c)以及双精度减法器(3d)四个子运算器构成，每个子运算器有两个数据输入端口和一个数据输出端口；逻辑控制器给定滤波时序，在滤波时序的约束下把从数据存储器中读出的数据赋值给运算器的输入端口，并把运算器的输出数据写入到数据存储器中；双精度乘法器(3a)，双精度除法器(3b)，双精度加法器(3c)以及双精度减法器(3d)四子运算器的输入输出数据均通过数据总线与存储器相连接；当状态预测控制器(2a)，协方差预测控制器(2b)，卡尔曼增益更新控制器(2d)，状态更新控制器(2c)及协方差更新控制器(2e)五个逻辑子控制器所给定时序依次执行后，则完成一次滤波，此时再循环执行，实现实时滤波。所述的存储器采用RAM来实现，RAM的地址总线宽度为12位，寻址范围0-4095，每个地址内存储的数据宽度为64位，表示了一个双精度浮点型数据，RAM中存储的数据分段组成滤波算法的参数矩阵和中间变量矩阵。在此设计结构下可以实现15维的卡尔曼滤波器，然后再通过增大地址总线的宽度，进一步扩大存储器的寻址范围，从而完成更高维数的卡尔曼滤波器。基于FPGA的高维卡尔曼滤波器实现方法，首先对存储器中的滤波算法相关矩阵进行初始化，然后在不断从外部读入观测量的条件下重复以下五个步骤，从而完成卡尔曼滤波过程，具体实现为：(1)状态预测控制器完成状态预测状态预测控制器(2a)需要控制读取存储器中与状态预测有关的数据，送至所需要的双精度乘法器(3a)，双精度除法器(3b)，双精度加法器(3c)以及双精度减法器(3d)四个子运算器进行计算，并把计算结果写入到存储器中的状态预测结果存储区域，在此过程完成1次“方阵×列向量”运算；(2)协方差预测控制器完成协方差预测协方差预测控制器需要依次控制读取存储器中与协方差预测有关的数据，送至所需要的双精度乘法器(3a)，双精度除法器(3b)，双精度加法器(3c)以及双精度减法器(3d)四个子运算器进行计算，并把计算结果写入到存储器中的协方差预测结果存储区域，在此过程完成1次“方阵×方阵”运算，1次“方阵×方阵”运算和1次“方阵+方阵”运算；(3)卡尔曼增益更新控制器完成卡尔曼增益更新卡尔曼增益更新控制器需要依次控制读取存储器中与卡尔曼增益更新有关的数据，送至所需要的双精度乘法器(3a)，双精度除法器(3b)，双精度加法器(3c)以及双精度减法器(3d)四个子运算器进行计算，并把计算结果写入到存储器中的卡尔曼增益更新结果存储区域，在此过程完成1次“方阵×方阵”运算，1次“方阵×方阵”运算，1次“方阵+方阵”运算，1次矩阵求逆运算和1次“方阵×方阵”运算；(4)状态更新控制器完成状态更新状态更新控制器需要依次控制读取存储器中与状态更新有关的数据，送至所需要的双精度乘法器(3a)，双精度除法器(3b)，双精度加法器(3c)以及双精度减法器(3d)四个子运算器进行计算，并把计算结果写入到存储器中的状态更新结果存储区域，在此过程完成1次“方阵×列向量”运算，1次“列向量-列向量”运算，1次“方阵×列向量”运算和1次“列向量+列向量”运算；(5)协方差更新控制器完成协方差更新协方差更新控制器需要依次控制读取存储器中与协方差更新有关的数据，送至所需要的双精度乘法器(3a)，双精度除法器(3b)，双精度加法器(3c)以及双精度减法器(3d)四个子运算器进行计算，并把计算结果写入到存储器中的协方差更新结果存储区域，在此过程完成1次“方阵×方阵”运算,1次“方阵-方阵”运算和1次“方阵×方阵”运算。本专利技术与现有技术相比的优点在于：(1)本专利技术克服了通用处理器中在卡尔曼滤波算法执行速度低的不足，四个子运算器可以在单个时钟下独立并行地完成一次双精度的滤波计算，大大提高了高维卡尔曼滤波算法的执行速度与计算精度。(2)本专利技术在既有的硬件结构基础上可以通过改变存储器地址总线的宽度来对滤波维数进行调整，兼容低维的卡尔曼滤波器设计，具有较好的扩展性。以上所述的“列向量+列向量”以及“方阵+方阵”运算通过FPGA设计高维矩阵相加的逻辑控制状态机来实现,“列向量-列向量”以及“方阵-方阵”运算通过FPGA设计高维矩阵相减的逻辑控制状态机来实现,“方阵×列向量”以及“方阵×方阵”运算通过FPGA设计高维矩阵相乘的逻辑控制状态机来实现，而矩阵求逆运算通过FPGA设计高维矩阵求逆的逻辑控制状态机来实现。附图说明图1为本专利技术基于FPGA的高维卡尔曼滤波器硬件结构设计示意图；图2为本专利技术滤波器执行高维矩阵相加的逻辑控制状态机示意图；图3为本专利技术滤波器执行高维矩阵相减的逻辑控制状态机示意图；图4为本专利技术滤波器执行高维矩阵求逆的逻辑控制状态机示意图；图5为本专利技术滤波器执行高维矩阵相乘的逻辑控制状态机示意图；图6为本专利技术硬件结构读写RAM存储器的时序控制示意图。具体实施方式卡尔曼滤波的实质是由测量值重构系统的状态向量，它以“预测—实测—修正”的顺序递推，根据系统的测量值来消除随机干扰，得到最优估计值。下面来具体介绍一下卡尔曼滤波器的算法。首本文档来自技高网...

【技术保护点】
基于FPGA的高维卡尔曼滤波器,其特征在于包括：滤波逻辑控制器（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)五个逻辑子控制器所给定时序依次执行后，则完成一次滤波，此时再循环执行，实现实时滤波。

【技术特征摘要】
1.基于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中，根据以上的矩阵空间分配原则，对维数不同的卡尔曼滤波器计算出相应矩阵的偏移地址，当维数...

【专利技术属性】
技术研发人员：陶飞，刘轶龙，王聪，张霖，
申请(专利权)人：北京航空航天大学，
类型：发明
国别省市：