基于FPGA的去偏转换量测卡尔曼滤波器的设计方法

摘要

本发明公开了一种基于FPGA的去偏转换量测卡尔曼滤波器的设计方法，首先设计基于FPGA的去偏转换量测卡尔曼滤波的系统，增益矩阵模块包含平均真实协方差子模块和增益矩阵子模块；状态更新模块包含坐标转换子模块、平均真实偏差子模块、新息子模块和状态更新子模块；所述的三角函数模块、一步预测模块、预测误差协方差模块、滤波误差协方差模块、平均真实协方差子模块、增益矩阵子模块、坐标转换子模块、平均真实偏差子模块、新息子模块和状态更新子模块分别调用了浮点加、减、乘、除运算模块。本发明采用结构层次化设计，底层模块运用VHDL输入，顶层采用原理图输入方式，可提高代码的可读性，易于模块划分，且方便在设计过程中进行仿真。

说明书

技术领域

本发明属于信号处理技术领域，雷达数据处理方法，可用于目标跟踪，特别是一种基于FPGA的去偏转换量测卡尔曼滤波器的设计方法。

背景技术

现代雷达系统一般都由以下两个部分组成，即：雷达信号处理部分和雷达数据处理部分。雷达信号处理器作为第一次处理，将处理后的信号送入雷达数据处理器作第二次处理。目标跟踪作为雷达数据处理的功能之一，已经成为现代雷达跟踪系统中一个重要组成部分。

在实际雷达目标跟踪系统中，目标动态模型通常在笛卡尔坐标系中建模，而雷达量测一般却在极/球坐标系中得到。这样雷达目标跟踪就成为一个非线性估计问题。解决这一问题的其中一种方法为EKF，即将雷达对目标运动状态的非线性量测方程围绕目标运动状态的一步预测值进行Taylor级数展开，对其取一阶进行截断，以得到雷达量测的预测值及新息的协方差阵。但是这种近似可能会在目标运动状态估计及相应误差协方差阵的计算上引入大的误差，从而导致滤波器发散。解决这一问题的另一类常用方法就是转换量测卡尔曼滤波，即先将雷达量测通过坐标变换表示成笛卡尔坐标系中量测的伪线性形式，然后估计转换量测误差的前两阶矩并基于卡尔曼滤波完成目标跟踪。

去偏转换量测Kalman滤波器(DCMKF)在雷达目标跟踪系统中有着极其广泛的应用，但是由于其算法包含大量的矩阵加、减、乘、求逆等运算，计算量较大。传统的使用方法是利用数字信号处理器(DSP)实现DCMKF，DSP芯片是基于软件可编程的，依靠一条一条软件指令去串行执行，因此不可避免的问题就是功耗大，处理能力受到主频的限制等不足，因此采用传统的软件方法实现DCMKF难以保证雷达目标跟踪系统的实时性。

发明内容

本发明的目的在于提供一种基于FPGA的去偏转换量测卡尔曼滤波器的设计方法，以充分利用FPGA内部越来越多地内嵌了乘法器以及支持大规模并行运算的特点，在保证滤波精度的同时提高运算速度。

实现本发明目的的技术解决方案为：一种基于FPGA的去偏转换量测卡尔曼滤波器的设计方法，首先设计基于FPGA的去偏转换量测卡尔曼滤波的系统，包括三角函数模块、一步预测模块、预测误差协方差模块、增益矩阵模块、状态更新模块、滤波误差协方差模块和FIFO模块，其中增益矩阵模块包含平均真实协方差子模块和增益矩阵子模块；状态更新模块包含坐标转换子模块、平均真实偏差子模块、新息子模块和状态更新子模块；所述的三角函数模块、一步预测模块、预测误差协方差模块、滤波误差协方差模块、平均真实协方差子模块、增益矩阵子模块、坐标转换子模块、平均真实偏差子模块、新息子模块和状态更新子模块分别调用了浮点加、减、乘、除运算模块；

三角函数模块：三角函数模块的输入端口为目标的俯仰角和方位角、时钟信号、复位信号和输入使能信号；输出端口为俯仰角与方位角的三角函数值和下一模块的握手信号；在三角函数模块中，俯仰角与方位角的三角函数值计算一次，然后在坐标转换模块、量测转换的平均真实偏差模块和量测转换的平均真实协方差的模块需要时读取这些数据，牺牲少量的寄存器来换取宝贵的乘法单元；

一步预测模块：输入端口为上一时刻目标的状态估计值、时钟信号、复位信号和输入使能信号；输出端口为一步提前预测值和下一模块的握手信号；其中复位信号的作用是每处理完一帧数据，对内部计数器进行复位；输入使能信号的作用是提示并允许接收上一时刻目标状态估计值；握手信号的作用是使能状态更新模块、新息子模块接收一步预测值；

预测误差协方差模块：输入端口为上一时刻的滤波误差协方差值、时钟信号、复位信号和输入使能信号；输出端口为预测误差协方差值和下一模块的握手信号；

增益矩阵模块：在计算增益矩阵时，需要先求出量测噪声的协方差矩阵，即DCMKF的平均真实协方差，然后计算增益矩阵的值，对于整个增益矩阵模块而言，输入端口为目标的俯仰角与方位角的三角函数值、径向距离、预测误差协方差值、时钟信号、复位信号和上述各值的输入使能信号；输出端口为增益矩阵的值和状态更新模块、滤波误差协方差模块的握手信号；

滤波更新模块：在计算滤波更新值时，需要经过球坐标到笛卡尔坐标的坐标变换、去偏修正、新息计算、滤波更新四个过程；对于整个滤波更新模块而言，输入端口有目标的径向距离、目标的俯仰角与方位角的三角函数值、状态一步预测值、增益矩阵的值、时钟信号、复位信号和上述各值的输入使能信号；输出端口为状态估计值和握手信号，其中该握手信号为LPM_FIFO1模块的写使能信号；

滤波误差协方差模块：输入端口为预测误差协方差值、增益矩阵的值、时钟信号、复位信号和两个数据输入使能信号；输出端口为滤波误差协方差值和握手信号，其中该握手信号为LPM_FIFO2模块的写使能信号；

FIFO模块：调用两个FIFO模块，其中LPM_FIFO1模块用于暂存每一次滤波更新值，LPM_FIFO2模块用于暂存滤波误差协方差的值，以便在下一时刻调用。

本发明与现有技术相比，其显著优点：(1)采用结构层次化设计方法，底层模块运用VHDL输入，顶层采用原理图输入方式。这种设计风格可提高代码的可读性，易于模块划分，且方便在设计过程中进行仿真。(2)在用FPGA实现DCMKF之前，先对算法进行预处理，将算法分解为简单的标量运算，便于代码的实现，并且避免了稀疏矩阵中大量的0元素参与乘加计算，可以节约大量的FPGA内部资源。(3)从总体上给出了CMKF的并行结构，由于DCMKF算法的特性，DCMKF的各运算模块的内部可以并行运算，但是各个运算模块之间并不能实现完全意义上的并行。设计时，在每个模块之间加上握手信号，用于使能下一个运算模块接收数据并运算，各模块便能按照相应的时序有条不紊的运行。(4)选用QUARTUS II软件自带的浮点加减乘除运算模块，提高了设计的性能，缩短设计的时间，大大简化了浮点数据通路的实现。

下面结合附图对本发明作进一步详细描述。

附图说明

图1为基于FPGA的去偏转换量测Kalman滤波器的结构层次示意图。

图2为基于FPGA的去偏转换量测Kalman滤波器的算法流程图。

图3为状态一步预测模块结构框图。

图4为预测误差协方差模块结构框图。

图5为平均真实偏差模块结构框图。

图6为增益矩阵模块结构框图。

图7为状态更新模块结构框图。

图8为协方差更新模块结构框图。

具体实施方式

本发明基于FPGA的去偏转换量测卡尔曼滤波器的设计方法，首先设计基于FPGA的去偏转换量测卡尔曼滤波的系统，包括三角函数模块、一步预测模块、预测误差协方差模块、增益矩阵模块、状态更新模块、滤波误差协方差模块和FIFO模块，其中增益矩阵模块包含平均真实协方差子模块和增益矩阵子模块；状态更新模块包含坐标转换子模块、平均真实偏差子模块、新息子模块和状态更新子模块；所述的三角函数模块、一步预测模块、预测误差协方差模块、滤波误差协方差模块、平均真实协方差子模块、增益矩阵子模块、坐标转换子模块、平均真实偏差子模块、新息子模块和状态更新子模块分别调用了浮点加、减、乘、除运算模块；

三角函数模块：三角函数模块的输入端口为目标的俯仰角和方位角、时钟信号、复位信号和输入使能信号；输出端口为俯仰角与方位角的三角函数值和下一模块的握手信号；在三角函数模块中，俯仰角与方位角的三角函数值计算一次，然后在坐标转换模块、量测转换的平均真实偏差模块和量测转换的平均真实协方差的模块需要时读取这些数据，牺牲少量的寄存器来换取宝贵的乘法单元；

一步预测模块：输入端口为上一时刻目标的状态估计值、时钟信号、复位信号和输入使能信号；输出端口为一步提前预测值和下一模块的握手信号；其中复位信号的作用是每处理完一帧数据，对内部计数器进行复位；输入使能信号的作用是提示并允许接收上一时刻目标状态估计值；握手信号的作用是使能状态更新模块、新息子模块接收一步预测值；

预测误差协方差模块：输入端口为上一时刻的滤波误差协方差值、时钟信号、复位信号和输入使能信号；输出端口为预测误差协方差值和下一模块的握手信号；

增益矩阵模块：在计算增益矩阵时，需要先求出量测噪声的协方差矩阵，即DCMKF的平均真实协方差，然后计算增益矩阵的值，对于整个增益矩阵模块而言，输入端口为目标的俯仰角与方位角的三角函数值、径向距离、预测误差协方差值、时钟信号、复位信号和上述各值的输入使能信号；输出端口为增益矩阵的值和状态更新模块、滤波误差协方差模块的握手信号；

滤波更新模块：在计算滤波更新值时，需要经过球坐标到笛卡尔坐标的坐标变换、去偏修正、新息计算、滤波更新四个过程；对于整个滤波更新模块而言，输入端口有目标的径向距离、目标的俯仰角与方位角的三角函数值、状态一步预测值、增益矩阵的值、时钟信号、复位信号和上述各值的输入使能信号；输出端口为状态估计值和握手信号，其中该握手信号为LPM_FIFO1模块的写使能信号；

滤波误差协方差模块：输入端口为预测误差协方差值、增益矩阵的值、时钟信号、复位信号和两个数据输入使能信号；输出端口为滤波误差协方差值和握手信号，其中该握手信号为LPM_FIFO2模块的写使能信号；

FIFO模块：调用两个FIFO模块，其中LPM_FIFO1模块用于暂存每一次滤波更新值，LPM_FIFO2模块用于暂存滤波误差协方差的值，以便在下一时刻调用。

在用FPGA实现DCMKF之前，需要对算法进行预处理，将DCMKF算法由矩阵运算转化为简单的标量加、减、乘、除运算。本发明采用结构化设计思想，利用FPGA实现浮点DCMKF的设计。在保证实时性的前提下，在模块内部对运算单元进行分时复用，模块之间通过握手信号协调工作，解决了采用数字信号处理器(DSP)软件方法实现过程中存在的并行性和速度问题，并保证了运算的精度。

本发明基于FPGA的去偏转换量测Kalman滤波并行实现的步骤如下：由于DCMKF算法的特性，DCMKF的各运算模块的内部可以并行运算，但是各个运算模块之间并不能实现完全意义上的并行，但有些模块可以并行运算。其中平均真实偏差模块，平均真实协方差模块，一步预测模块，预测误差协方差模块，三角函数模块可以并行运算。设计时，在每个模块之间加上握手信号，用于使能下一个运算模块接收数据并运算，各模块便能按照相应的时序有条不紊的运行。因此，各个模块之间不仅有相应的数据输入输出端口，还应该有握手信号、时钟信号和复位信号。

(1)三角函数模块，在求俯仰角和方位角三角函数值时，利用Taylor公式求出当前时刻测量值的正弦值和余弦值；

(2)平均真实偏差模块，利用雷达测量到的径向距离、俯仰角和方位角以及各自的噪声方差求去转换量测的平均真实偏差；

(3)平均真实协方差模块，利用雷达测量到的径向距离、俯仰角和方位角以及各自的噪声方差求去转换量测的平均真实协方差；

(4)一步预测模块，将上一时刻的状态估计值代入状态预测方程，得到当前时刻的状态预测值；

(5)预测误差协方差模块，利用上一时刻的滤波误差协方差以及状态噪声协方差代入预测误差协方差方程，得到预测误差协方差值；

(6)增益模块，利用预测误差协方差值和量测转换的平均真实协方差值代入增益矩阵方程，得到增益；

(7)新息模块，利用去偏转换量测后的量测值和状态预测值，求取新息；

(8)状态更新值模块，利用状态预测值、增益以及新息，代入状态更新方程，得到状态更新值；

(9)滤波误差协方差模块，利用预测误差协方差值和增益，代入滤波误差协方差方程，得到滤波误差协方差值；

(10)返回到步骤(1)进行下一时刻的计算。

结合图3，本发明的一步预测模块分为数据分配模块和数据运算模块，当系统接收到当前时刻测量值且一步预测模块接收完前一时刻状态估计值之后，开始启动数据分配模块，每个时钟周期将相应的数据赋给对应的寄存器，经过n+m+m个时钟周期之后，每个时钟输出一个状态预测值；其中一步预测模块共占用两个浮点加法单元和两个浮点乘法运算单元；在QUARTUS II的参数可设置模块(LPM)中，浮点加、乘法器的周期参数分别设置为m、n个时钟周期；在数据运算模块的第一级浮点乘法运算时，需要将未参与运算的数据同样设置为n个时钟的延时；在数据运算模块的第二级浮点加法运算时，也需要将相应的上一级运算结果设置为m个时钟的延时；在输入端口，每个时钟周期输入一个数据，当接收完九个状态值后，每个时钟将相应的乘数和被乘数传送到对应的寄存器并使能运算模块，这样每个数据在运算模块中经过n+m+m个时钟周期的运算延时后，每个时钟周期输出一个数据，其中m≥7，n≥5。

结合图4，本发明的预测误差协方差模块分为数据分配模块和数据运算模块，当系统接收到当前时刻测量值且预测误差协方差模块接收完前一时刻滤波误差协方差值后，开始启动数据分配模块，每个时钟周期将相应的数赋给对应的寄存器，经过n+m+m+m+m个时钟周期之后，每个时钟输出一个预测误差协方差值；其中预测误差协方差模块共占用九个浮点加法单元和八个浮点乘法运算单元；在QUARTUS II的参数可设置模块(LPM)中，浮点加、乘法器的周期参数分别设置为m、n个时钟周期；在数据运算模块的第一级浮点乘法运算时，需要将未参与运算的数据同样设置为n个时钟的延时；在数据运算模块的第三、四级浮点加法运算时，也需要将相应的上一级运算结果设置为m个时钟的延时；在输入端口，每个时钟周期输入一个数据，当接收完前一时刻滤波误差协方差值后，每个时钟将相应的乘数和被乘数传送到对应的寄存器并使能运算模块，这样每个数据在运算模块中经过n+m+m+m+m个时钟周期的运算延时后，每个时钟周期输出一个数据，其中m≥7，n≥5。

结合图5，本发明的平均真实偏差模块分为数据分配模块和数据运算模块，当平均真实偏差模块接收完径向距离和俯仰角与方位角的三角函数值之后，开始启动数据分配模块，每个时钟周期将相应的数赋给对应的寄存器，经过n+n个时钟周期之后，每个时钟输出一个平均真实偏差值；其中平均真实偏差模块共占用三个浮点乘法运算单元；在QUARTUS II的参数可设置模块(LPM)中，浮点乘法器的周期参数分别设置为n个时钟周期；在输入端口，每个时钟周期输入一个数据，当接收完径向距离和俯仰角与方位角的三角函数值之后，每个时钟将相应的乘数和被乘数传送到对应的寄存器并使能数据运算模块，这样每个数据在数据运算模块中经过n+n个时钟周期的运算延时后，每个时钟周期输出一个数据，其中m≥7，n≥5。

结合图6，本发明的增益矩阵模块分为数据分配模块和数据运算模块，当增益矩阵模块接收完预测误差协方差值之后，开始启动数据分配模块，每个时钟周期将相应的数赋给对应的寄存器，经过n+m+m个时钟周期之后，每个时钟输出一个增益值；其中增益矩阵模块共占用两个浮点加法单元和三个浮点乘法运算单元；在QUARTUS II的参数可设置模块(LPM)中，浮点加、乘法器的周期参数分别设置为m、n个时钟周期；在数据运算模块的第二级浮点加法运算时，需要将相应的上一级运算结果设置为m个时钟的延时；在输入端口，每个时钟周期输入一个数据，当接收完预测误差协方差值和新息之后，每个时钟将相应的乘数和被乘数传送到对应的寄存器并使能运算模块，这样每个数据在数据运算模块中经过n+m+m个时钟周期的运算延时后，每个时钟周期输出一个数据，其中m≥7，n≥5。

结合图7本发明的状态更新模块分为数据分配模块和数据运算模块，当状态更新模块接收完状态预测值和增益矩阵和新息之后，开始启动数据分配模块，每个时钟周期将相应的数赋给对应的寄存器，经过n+m+m个时钟周期之后，每个时钟输出一个状态估计值；其中状态更新模块共占用三个浮点加法单元和三个浮点乘法运算单元；在QUARTUS II的参数可设置模块(LPM)中，浮点加、乘法器的周期参数分别设置为m、n个时钟周期；在数据运算模块的第一级浮点乘法运算时，需要将未参与运算的数据同样设置为n个时钟的延时；在输入端口，每个时钟周期输入一个数据，当接收完状态预测值和增益矩阵和新息后，每个时钟将相应的乘数和被乘数传送到对应的寄存器并使能数据运算模块，这样每个数据在数据运算模块中经过n+m+m个时钟周期的运算延时后，每个时钟周期输出一个数据，其中m≥7，n≥5。

结合图8本发明的滤波误差协方差模块分为数据分配模块和数据运算模块，当滤波误差协方差模块接收完预测误差协方差值和增益矩阵值之后，开始启动数据分配模块，每个时钟周期将相应的数赋给对应的寄存器，经过n+m+m个时钟周期之后，每个时钟输出一个滤波误差协方差值；其中滤波误差协方差模块共占用一个浮点加法单元、两个浮点减法单元和三个浮点乘法运算单元；在QUARTUS II的参数可设置模块(LPM)中，浮点加、减、乘法器的周期参数分别设置为m、m、n个时钟周期；在数据运算模块的第一级浮点乘法运算时，需要将未参与运算的数据同样设置为n个时钟的延时；在输入端口，每个时钟周期输入一个数据，当接收完预测误差协方差值和增益矩阵值后，每个时钟将相应的乘数和被乘数传送到对应的寄存器并使能数据运算模块，这样每个数据在运算模块中经过n+m+m个时钟周期的运算延时后，每个时钟周期输出一个数据，其中m≥7，n≥5。

实施例

参照图1，本发明基于FPGA的去偏转换量测卡尔曼滤波器系统包括：三角函数模块、一步预测模块、预测误差协方差模块、增益矩阵模块、状态更新模块、滤波误差协方差模块和FIFO模块。其中增益矩阵模块包含平均真实协方差子模块和增益矩阵子模块；状态更新模块包含坐标转换子模块、平均真实偏差子模块、新息子模块和状态更新子模块。其中在各运算模块分别调用了浮点加、减、乘、除运算模块。

本实施例采用雷达对近程目标进行跟踪，其中雷达数据输出周期T为0.8192ms。选择Singer加速度模型作为目标的动态模型。系统状态方程为：

X_k+1＝ΦX_k+Γ_kW_k                                 (1)

观测方程为：

Z_k＝H_kX_k+V_k                                      (2)

其中为系统的状态向量，包括目标在X轴、Y轴、Z轴方向上的坐标位置、速度和加速度；Φ为状态转移矩阵；Γ_k为噪声矩阵；W_k为系统的状态噪声；Z_k为系统量测值；H_k为量测矩阵；V_k为量测噪声。

$\Phi =\left(\begin{array}{ccccccccc}1& 0& 0& T& 0& 0& {\phi }_{17}& 0& 0\\ 0& 1& 0& 0& T& 0& 0& {\phi }_{28}& 0\\ 0& 0& 1& 0& 0& T& 0& 0& {\phi }_{39}\\ 0& 0& 0& 1& 0& 0& {\phi }_{47}& 0& 0\\ 0& 0& 0& 0& 1& 0& 0& {\phi }_{58}& 0\\ 0& 0& 0& 0& 0& 1& 0& 0& {\phi }_{69}\\ 0& 0& 0& 0& 0& 0& e^{{-\alpha }_{x}T}& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& e^{{-\alpha }_{y}T}& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& e^{{-\alpha }_{z}T}\end{array}\right)---\left(3\right)$

其中

${\phi }_{17}=({\alpha }_{x}T-1+e^{-{\alpha }_{x}T})/{{\alpha }_X}^2,$${\phi }_{28}=({\alpha }_{y}T-1+e^{-{\alpha }_{y}T})/{{\alpha }_y}^2,$

${\phi }_{39}=({\alpha }_{z}T-1+e^{{-\alpha }_{z}T})/{{\alpha }_z}^2,$${\phi }_{47}=(1-e^{-{\alpha }_{x}T})/{\alpha }_x,---\left(4\right)$

${\phi }_{58}=(1-e^{-{\alpha }_{y}T})/{\alpha }_y,$${\phi }_{69}=(1-e^{-{\alpha }_{z}T})/{\alpha }_z.$

${\Gamma }_k=\left(\begin{array}{ccc}{\Gamma }_1& 0_{3\times 1}& 0_{3\times 1}\\ 0_{3\times 1}& {\Gamma }_2& 0_{3\times 1}\\ 0_{3\times 1}& 0_{3\times 1}& {\Gamma }_3\end{array}\right)---\left(5\right)$

其中

${\Gamma }_1=\left(\begin{array}{c}{\gamma }_x[1-{\alpha }_{x}T-e^{-{\alpha }_{x}T}+(T^2{{\alpha }_x}^2/2)]/{{\alpha }_x}^3\\ {\gamma }_y[1-{\alpha }_{y}T-e^{-{\alpha }_{y}T}+(T^2{{\alpha }_y}^2/2)]/{{\alpha }_y}^3\\ {\gamma }_z[1-{\alpha }_{z}T-e^{-{\alpha }_{z}T}+(T^2{{\alpha }_z}^2/2)]/{{\alpha }_z}^3\end{array}\right)$

${\Gamma }_2=\left(\begin{array}{c}{\gamma }_x({\alpha }_{x}T+e^{-{\alpha }_{x}T}-1)/{\alpha }_x^2\\ {\gamma }_y(\mathrm{\alpha T}+e^{-{\alpha }_{y}T}-1)/{{\alpha }_y}^2\\ {\gamma }_z(\mathrm{\alpha T}+e^{-{\alpha }_{z}T}-1)/{{\alpha }_z}^2\end{array}\right)$

${\Gamma }_3=\left(\begin{array}{c}{\gamma }_x(1-e^{-{\alpha }_{x}T})/{\alpha }_x\\ {\gamma }_y(1-e^{-{\alpha }_{y}T})/{\alpha }_y\\ {\gamma }_z(1-e^{-{\alpha }_{z}T})/{\alpha }_z\end{array}\right)$

$H_k=\left(\begin{array}{ccccccccc}1& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0& 0& 0& 0\end{array}\right)---\left(6\right)$

其中γ_x＝γ_y＝γ_z＝γ，α_x＝α_y＝α_z＝α为描述来袭目标在直角坐标系中运动加速度的一阶成型滤波器参数；T为量测周期。

在ALTERA公司的FPGA芯片EP3C120F484C8N上实现本发明提出的去偏转换量测卡尔曼滤波器，运算过程中分别调用了QUARTUS II软件自带的浮点加、减、乘、除浮点运算模块。

图2为基于FPGA的DCMKF滤波器的算法流程图，从图中可以明显看出，笛卡尔坐标系下坐标、量测转换平均真实偏差以及量测转换平均真实协方差是完全可以并行运算的，因此在利用FPGA设计DCMKF时，当俯仰角与方位角的三角函数计算完成之后，同时开始这三个计算模块的计算，以达到这三个模块之间并行运算的目的。而增益矩阵与量测转换平均真实协方差则不可以并行计算，因为增益矩阵的值依赖于量测转换平均真实协方差，当且仅当量测转换平均真实协方差与预测误差协方差值全部计算完成后，才可以开始增益矩阵的计算。其它各模块的计算也是采用上述的设计思想，模块之间通过握手信号协调工作。

本系统的工作原理如下：

三角函数模块采用式(7)和式(8)求取目标俯仰角和方位角的正弦值和余弦值。在坐标转换模块、DCMKF的平均真实偏差模块和DCMKF的平均真实协方差的模块中，均需用到俯仰角与方位角的三角函数值，为了节省资源，只需要计算一次，然后在需要时读取这些数据，以牺牲少量的寄存器换取宝贵的乘法单元。

sinα＝α-0.1667α³+0.0083α⁵                             (7)

cosα＝1-0.5α²+0.0417α⁴                                 (8)

坐标转换子模块：利用式(9)将目标球坐标下的测量值r_m、η_m和θ_m先直接转换为笛卡尔坐标系下测量值x_m、y_m和z_m。

$\left(\begin{array}{c}{x}_m=r_m\cos {\eta }_m{\cos \theta }_m\\ y_m=r_m{\cos \eta }_m{\sin \theta }_m\\ z_m=r_m{\sin \eta }_m\end{array}\right)---\left(9\right)$

状态一步预测模块利用上一时刻的状态估计值根据式(10)求取当前时刻的状态预测值将式(10)简化为标量加、减、乘、除的形式。图3所示为状态一步预测模块结构框图，分为数据分配模块和数据计算模块。当系统接收到当前时刻测量值且状态一步预测模块接收完前一时刻状态估计值之后，开始启动数据分配模块，每个时钟周期将相应的数赋给对应的寄存器，经过19个时钟周期之后，每个时钟输出一个状态预测值。其中状态一步预测模块共占用两个浮点加法单元和两个浮点乘法运算单元。在参数可设置模块(LPM)中，浮点加、乘法器的周期参数分别设置为7、5个时钟周期。因为输入端并不恰好是2的N次方个数据，所以为了保证数据的同步，在运算模块的第一级浮点乘法运算时，需要将未参与运算的数据同样设置为5个时钟的延时；在运算模块的第二级浮点加法运算时，也需要将相应的上一级运算结果设置为7个时钟的延时。在输入端口，每个时钟周期输入一个数据，当接收完九个状态值后，每个时钟将相应的乘数和被乘数传送到对应的寄存器并使能运算模块，这样每个数据在运算模块中经过19(5+7+7)个时钟周期的运算延时后，每个时钟周期输出一个数据。

${\hat{X}}_{k|k-1}=\Phi {\hat{X}}_{k-1|k-1}---\left(10\right)$

预测误差协方差模块利用上一时刻的滤波误差协方差阵P_k-1|k-1根据式(11)求取预测误差协方差阵P_k|k-1。图4所示为预测误差协方差模块结构框图。在利用式(11)计算预测误差协方差之前，需要将式(11)简化为标量加减乘除的形式。预测误差协方差模块占用八个浮点乘法运算单元和九个浮点加法运算单元。当预测误差协方差模块接收完前一时刻滤波误差协方差值之后，启动数据分配模块，33个时钟周期之后，每个时钟周期输出一个数据。

P_k|k-1＝ΦP_k-1|k-1Φ^T+ΓQΓ^T                         (11)

平均真实偏差模块利用式(12)求取坐标变换时的平均真实偏差，图5所示为平均真实偏差模块结构框图。在利用式(12)计算状态更新值之前，需要将式(12)简化为标量加减乘除的形式。平均真实偏差模块占用三个浮点乘法运算单元。当平均真实偏差模块径向距离和俯仰角与方位角的三角函数值后，启动数据分配模块，10个时钟周期之后，每个时钟周期输出一个数据。

$\left(\begin{array}{c}{\mu }_k^x=r_m{\cos \eta }_m{\cos \theta }_m(e^{-{\sigma }_{\eta }^2}{e}^{-{\sigma }_{\theta }^2}-e^{-{\sigma }_{\eta }^2/2}{e}^{-{\sigma }_{\theta }^2/2})\\ {\mu }_k^y=r_m{\cos \eta }_m\sin {\theta }_m(e^{-{\sigma }_{\eta }^2}-e^{{-\sigma }_{\theta }^2}-e^{-{\sigma }_{\eta }^2/2}{e}^{-{\sigma }_{\theta }^2/2})\\ {\mu }_k^2=r_m{\sin \eta }_m(e^{-{\sigma }_{\eta }^2}-e^{-{\sigma }_{\eta }^2/2})\end{array}\right)---\left(12\right)$

这样，球坐标系中的量测转换为笛卡尔坐标系中的量测就应当修正为：

$Z_c=Z_k-{\mu }_k=\left(\begin{array}{c}{r}_m{\cos \eta }_m{\cos \theta }_m\\ r_m{\cos \eta }_m{\sin \theta }_m\\ r_m{\sin \eta }_m\end{array}\right)-{\mu }_k---\left(13\right)$

平均真实协方差模块利用式(14)求取坐标变换的平均的真实协方差：

$R_k=\left(\begin{array}{ccc}{R}_k^{\mathrm{xx}}& R_k^{\mathrm{xy}}& R_k^{\mathrm{xz}}\\ R_k^{\mathrm{yx}}& R_k^{\mathrm{yy}}& R_k^{\mathrm{yz}}\\ R_k^{\mathrm{zx}}& R_k^{\mathrm{zy}}& R_k^{\mathrm{zz}}\end{array}\right)---\left(14\right)$

其中

$\left(\begin{array}{c}{R}_k^{\mathrm{xx}}=[{r_m}^2({\tilde{\beta }}_x{\tilde{\beta }}_{\mathrm{xy}}-{\tilde{\alpha }}_x{\tilde{\alpha }}_{\mathrm{xy}})+{\sigma }_r^2({2\tilde{\beta }}_x{\tilde{\beta }}_{\mathrm{xy}}-{\tilde{\alpha }}_x{\tilde{\alpha }}_{\mathrm{xy}})]e^{-{2\sigma }_{\theta }^2}{e}^{-2{\sigma }_{\eta }^2}\\ R_k^{\mathrm{yy}}=[{r_m}^2({\tilde{\beta }}_y{\tilde{\beta }}_{\mathrm{xy}}-{\tilde{\alpha }}_y{\tilde{\alpha }}_{\mathrm{xy}})+{\sigma }_r^2(2{\tilde{\beta }}_y{\tilde{\beta }}_{\mathrm{xy}}-{\tilde{\alpha }}_y{\tilde{\alpha }}_{\mathrm{xy}})]e^{-{2\sigma }_{\theta }^2}{e}^{-2{\sigma }_{\eta }^2}\\ R_k^{\mathrm{zz}}=[{r_m}^2({\tilde{\beta }}_z-{\tilde{\alpha }}_z)+{\sigma }_r^2(2{\tilde{\beta }}_z-{\tilde{\alpha }}_z)]e^{-2{\sigma }_{\eta }^2}\\ R_k^{\mathrm{xy}}=[{r_m}^2({\tilde{\beta }}_{\mathrm{xy}}-{\tilde{\alpha }}_{\mathrm{xy}}{e}^{{\sigma }_{\theta }^2})+{\sigma }_r^2(2{\tilde{\beta }}_{\mathrm{xy}}-{\tilde{\alpha }}_{\mathrm{xy}}{e}^{{\sigma }_{\theta }^2})]{\sin \theta }_m{\cos \theta }_m{e}^{-4{\sigma }_{\theta }^2}{e}^{-2{\sigma }_{\eta }^2}\\ R_k^{\mathrm{yz}}=[{r_m}^2(1-e^{{\sigma }_{\eta }^2})+{\sigma }_r^2(1-e^{{\sigma }_{\eta }^2})]{\sin \theta }_m{\sin \eta }_m{\cos \eta }_m{e}^{-{\sigma }_{\theta }^2}{e}^{-4{\sigma }_{\eta }^2}\\ R_k^{\mathrm{xz}}=[{r_m}^2(1-e^{{\sigma }_{\eta }^2})+{\sigma }_r^2(1-e^{{\sigma }_{\eta }^2})]{\cos \theta }_m{\sin \eta }_m{\cos \eta }_m{e}^{-{\sigma }_{\theta }^2}{e}^{-4{\sigma }_{\eta }^2}\end{array}\right)---\left(15\right)$

其中

$\left(\begin{array}{c}{\tilde{\alpha }}_x={\sin }^2{\theta }_m*\sinh {\sigma }_{\theta }^2+{\cos }^2{\theta }_m*{\cosh \sigma }_{\theta }^2\\ {\tilde{\alpha }}_y={\sin }^2{\theta }_m*{\cosh \sigma }_{\theta }^2+{\cos }^2{\theta }_m*{\sinh \sigma }_{\theta }^2\\ {\tilde{\alpha }}_z={\sin }^2{\eta }_m*{\cosh \sigma }_{\eta }^2+{\cos }^2{\eta }_m*{\sinh \sigma }_{\eta }^2\\ {\tilde{\alpha }}_{\mathrm{xy}}={\sin }^2{\eta }_m*{\sinh \sigma }_{\eta }^2+{\cos }^2{\eta }_m*{\cosh \sigma }_{\eta }^2\\ {\tilde{\beta }}_x={\sin }^2{\theta }_m*{\sinh 2\sigma }_{\theta }^2+{\cos }^2{\theta }_m*{\cosh 2\sigma }_{\theta }^2\\ {\tilde{\beta }}_y={\sin }^2{\theta }_m*{\cosh 2}_{\theta }^2+{\cos }^2{\theta }_m*{\sinh 2\sigma }_{\theta }^2\\ {\tilde{\beta }}_z={\sin }^2{\eta }_m*{\cosh 2}_{\eta }^2+{\cos }^2{\eta }_m*{\sinh 2\sigma }_{\eta }^2\\ {\tilde{\beta }}_{\mathrm{xy}}={\sin }^2{\eta }_m*{\sinh 2\sigma }_{\eta }^2+{\cos }^2{\eta }_m*{\cosh 2}_{\eta }^2\end{array}\right)---\left(16\right)$

新息模块，利用去偏转换量测后的坐标值减去观测预测值即为$Z_k-{\mu }_k-H{\hat{X}}_{k|k-1}.$

增益矩阵模块，利用预测误差协方差模块和平均真实协方差模块求增益矩阵根据式(17)求取增益矩阵。图6所示为增益矩阵模块结构框图。在利用式(17)计算状态更新值之前，需要将式(17)简化为标量加减乘除的形式。增益矩阵模块占用三个浮点乘法运算单元和两个浮点加法运算单元。当增益矩阵模块接收完预测误差协方差值后，启动数据分配模块，19个时钟周期之后，每个时钟周期输出一个数据。

K_k＝P_k|k-1H^T(HP_k|k-1H^T+R_k)^-1                      (17)

状态更新模块，利用增益模块、新息模块以及状态一步预测模块求取目标状态估计值。图7状态更新模块结构框图。在利用式(18)计算状态更新值之前，需要将式(18)简化为标量加减乘除的形式。状态更新模块占用三个浮点乘法运算单元和三个浮点加法运算单元。当状态更新模块接收完状态预测值和增益矩阵和新息后，启动数据分配模块，19个时钟周期之后，每个时钟周期输出一个数据。

${\hat{X}}_{k|k}={\hat{X}}_{k|k-1}+K_k(Z_k-{\mu }_k-H{\hat{X}}_{k|k-1})---\left(18\right)$

滤波误差协方差模块，利用增益模块和预测误差协方差模块求取滤波误差协方差值。图8滤波误差协方差结构模块。在利用式(19)计算滤波误差协方差值之前，需要将式(19)简化为标量加减乘除的形式。滤波误差协方差模块占用三个浮点乘法运算单元，一个浮点加法运算单元和两个浮点减法运算单元。当滤波误差协方差模块接收完预测误差协方差值和增益矩阵后，启动数据分配模块，19个时钟周期之后，每个时钟周期输出一个数据。

P_k|k＝P_k|k-1-K_kHP_k|k-1                            (19)