基于FPGA的无迹卡尔曼滤波系统及并行实现方法.pdf

基于FPGA的无迹卡尔曼滤波系统及并行实现方法
    【技术领域】

    本发明属于信号处理技术领域，涉及非线性滤波方法，可用于目标跟踪和参数估计。

    背景技术

    滤波理论是在对系统可观测信号进行测量的基础上，根据一定的滤波准则，对系统的状态进行估计的理论和方法。根据贝叶斯理论，最优估计只是一个理想化的解，一般情况下，无法得到它的解析形式。卡尔曼(Kalman)滤波是目前最经典、应用最广泛的线性滤波器，当系统为线性高斯分布时，它可以得到递归贝叶斯估计的最小均方误差解。然而，实际的系统往往是非线性、非高斯的，无法采用卡尔曼滤波求解，为此提出了大量非线性滤波方法，主要可分为粒子滤波和高斯滤波两大类。

    粒子滤波是一种贝叶斯框架下基于采样的非线性滤波方法，它使用一些带权值的样本粒子来近似模拟先验和后验概率分布。这里的样本粒子是随机采样得到的，当样本数目足够大时，粒子滤波趋于最优贝叶斯估计，可以得到很好的滤波精度。但是，粒子滤波计算量太大，实时性差，难以在工程中应用。

    高斯滤波以扩展卡尔曼滤波(Extended Kalman Filter，EKF)和无迹卡尔曼滤波(Unscented Kalman Filter，UKF)为代表。前者是一种弱非线性滤波算法，它将非线性函数通过一定的规则(如泰勒级数展开等)进行线性化处理，在一定程度上提高了非线性目标的跟踪性能，具有很好的实时性，目前已在军用和民用领域得到广泛应用。然而，当系统呈现强非线性时，一阶线性化近似会带来较大的误差，可能导致滤波器不稳定甚至发散。

    UKF(Unscented Kalman Filter)是一种基于UT(Unscented Transform)变换的新滤波算法。该算法摒弃了对非线性函数进行线性化的传统做法，转而从统计学的角度寻找解决问题的思路。与粒子滤波的随机采样方式不同，UKF选取的Sigma样本点很少，且是按照一定规则确定性选取的，实时性显著优于粒子滤波。与EKF算法相比，UKF算法对于高斯输入的非线性函数的近似可以精确到三阶，对于非高斯输入的非线性函数的近似，至少可以精确到二阶，精度上明显优于EKF。因此，UKF可以在滤波精度和实时性之间做到良好的折衷，具有广泛的应用前景。

    虽然与粒子滤波相比，UKF的实时性已有了明显提高，但UKF中仍涉及到诸如矩阵求逆和Cholesky分解等复杂运算，当系统状态维数和观测维数较大时，运算速度明显降低，硬件实现难度远大于EKF。

    FPGA具有大容量的Block RAM资源可用于存储大量数据和实现查找表等功能；大量的DSP48E资源可以实现高效的浮点数加法、减法和乘法等运算；丰富的逻辑资源可以实现大规模并行运算。随着超大规模集成电路技术的发展，FPGA的性能也在不断提高，充分利用FPGA的这一特点会大大提高算法的运算速度。

    【发明内容】

    本发明的目的是针对上述问题，提出一种基于FPGA的无迹卡尔曼滤波系统及并行实现方法，以充分利用FPGA易于实现大规模并行运算的特点，在保证滤波精度的同时提高运算速度，降低硬件实现复杂度。

    为实现上述目的，本发明基于FPGA的无迹卡尔曼滤波系统，包括：

    协方差矩阵Cholesky分解模块A，用于对分块对角协方差矩阵的对角线上的各子矩阵分别进行Cholesky分解，得到下三角矩阵，将下三角矩阵的L个行向量，分成K组，分别输入到Sigma点产生模块，其中L＝2m+n，m为状态量的维数，n为观测量的维数，K≥1；

    Sigma点产生模块B，用于接收上一时刻的状态估计值，利用A模块得到的结果产生K组Sigma点，分别输入到时间更新模块，其中第一组有2L/K+1个采样点，其余各组有2L/K个采样点；

    时间更新模块C，用于把接收到的Sigma点代入系统模型的状态方程，得到状态预测值，输入到观测预测模块；

    观测预测模块D，用于把状态预测值代入观测模型的观测方程，得到观测预测值，并将观测预测值和状态预测值输入到部分均值和协方差矩阵计算模块；

    部分均值和协方差矩阵计算模块E，用于对观测预测值和状态预测值加权求和，分别计算部分状态预测协方差矩阵、部分观测预测协方差矩阵和部分互协方差矩阵，并将其计算结果输入到总体均值计算模块和总体协方差矩阵计算模块；

    总体均值计算模块F，用于接收部分均值和协方差矩阵计算模块的结果，分别计算状态预测均值和观测预测均值，并将其计算结果输入到总体协方差矩阵计算模块和状态量估计和状态协方差矩阵估计模块；

    总体协方差矩阵计算模块G，用于接收部分均值和协方差矩阵计算模块和总体均值计算模块的结果，分别计算状态预测协方差矩阵、观测预测协方差矩阵和互协方差矩阵，并将观测预测协方差矩阵输入到观测预测协方差矩阵求逆模块，将互协方差矩阵输入到增益计算模块，将状态预测协方差矩阵和观测预测协方差矩阵输入到状态量估计和状态协方差矩阵估计模块；

    观测预测协方差矩阵求逆模块H，用于对观测预测协方差矩阵采用奇异值分解方法求逆，并将求逆结果输入到增益计算模块；

    增益计算模块I，用于接收互协方差矩阵和观测预测协方差矩阵的逆矩阵，计算增益，并将增益输入到状态量估计和状态协方差矩阵估计模块；

    状态量估计和状态协方差矩阵估计模块J，用于接收增益、状态预测均值、状态预测协方差矩阵、观测预测协方差矩阵和当前时刻的观测数据，计算状态估计值和状态协方差矩阵，并将状态估计值作为当前时刻的最终结果输出，将状态协方差矩阵扩维后输入到A模块。

    为实现上述目的，本发明基于FPGA的无迹卡尔曼滤波并行实现方法，包括如下步骤：

    (1)协方差矩阵Cholesky分解步骤，对分块对角协方差矩阵的对角线上的各子矩阵分别进行Cholesky分解，得到下三角矩阵，将下三角矩阵的L个行向量，分成K组，其中L＝2m+n，m为状态量的维数，n为观测量的维数，K≥1；

    (2)Sigma点产生步骤，接收上一时刻的状态估计值，利用Cholesky分解得到的L个向量产生K组Sigma点；

    (3)时间更新步骤，将Sigma点代入系统模型的状态方程，得到状态预测值；

    (4)观测预测步骤，将状态预测值代入观测模型的观测方程，得到观测预测值；

    (5)部分均值和协方差矩阵计算步骤，对观测预测值和状态预测值进行加权求和，分别计算出部分状态预测协方差矩阵、部分观测预测协方差矩阵和部分互协方差矩阵；

    (6)总体均值计算步骤，计算状态预测均值和观测预测均值；

    (7)总体协方差矩阵计算步骤，分别计算状态预测协方差矩阵、观测预测协方差矩阵和互协方差矩阵；

    (8)观测预测协方差矩阵求逆步骤，对观测预测协方差矩阵采用奇异值分解方法求逆；

    (9)增益计算步骤，将互协方差矩阵和观测预测协方差矩阵的逆矩阵相乘得到增益；

    (10)状态量估计和状态协方差矩阵估计步骤，利用增益、状态预测均值、状态预测协方差矩阵、观测预测协方差矩阵和当前时刻的观测数据，计算状态估计值和状态协方差矩阵，并将状态估计值作为当前时刻的最终结果输出，对状态协方差矩阵扩维，返回到步骤(1)进行下一时刻的计算。

    本发明具有如下优点：

    本发明的滤波系统由于从总体上给出了UKF的并行结构，提高了滤波的速度；

    本发明的滤波方法由于将分块对角矩阵的Cholesky分解转化为对对角线上各子矩阵的Cholesky分解，降低了进行Cholesky分解的矩阵维数，易于硬件实现；

    本发明由于给出了Cholesky分解矩阵各元素在FPGA中的并行运算方法，提高了滤波的速度。

    本发明由于采用奇异值分解实现矩阵求逆，并将求逆矩阵分为多个处理2×2维子矩阵的并行运算单元在FPGA中同时进行运算，降低了硬件实现复杂度。

    【附图说明】

    图1是本发明的无迹卡尔曼滤波系统结构图；

    图2是本发明的无迹卡尔曼滤波方法流程图；

    图3是现有M×M维矩阵奇异值分解各子矩阵之间进行元素交换图。

    【具体实施方式】

    参照图1，本发明基于FPGA的无迹卡尔曼滤波系统包括：协方差矩阵Cholesky分解模块A、Sigma点产生模块B、时间更新模块C、观测预测模块D、部分均值和协方差矩阵计算模块E、总体均值计算模块F、总体协方差矩阵计算模块G、观测预测协方差矩阵求逆模块H、增益计算模块I和状态量估计和状态协方差矩阵估计模块J。其中，模块B包含K个Sigma点产生子模块B_i，i＝1，2，...，K，B₁，B₂，...，B_K采用K个并行运算单元结构，模块C包含K个时间更新子模块C_i，i＝1，2，...，K，C₁，C₂，...，C_K采用K个并行运算单元结构，模块D包含K个观测预测子模块D_i，i＝1，2，...，K，D₁，D₂，...，D_K采用K个并行运算单元结构，模块E包含K个部分均值和协方差矩阵计算子模块E_i，i＝1，2，...，K，E₁，E₂，...，E_K采用K个并行运算单元结构。模块A实现协方差矩阵Cholesky分解，并将分解得到的下三角矩阵的行向量分成K组，分别送入模块B的K个子模块，模块B的K个子模块B_i分别产生一组Sigma点，送入模块C_i，模块C_i分别对该组Sigma点进行时间更新产生一组状态预测值送入模块D_i，模块D_i将该组状态预测值代入观测方程产生一组观测预测值并将状态预测值和观测预测值一起送入模块E_i，模块E_i对该组观测预测值和状态预测值加权求和，分别计算部分状态预测协方差矩阵、部分观测预测协方差矩阵和部分互协方差矩阵。将E₁，E₂，...，E_K的计算结果送入模块F和模块G，模块F采用E₁，E₂，...，E_K输入的K组计算结果实现总体均值的计算，并将计算结果送入模块G，模块G实现总体协方差矩阵计算，并将计算结果送入模块H，模块H对观测预测协方差矩阵求逆，并将计算结果送入模块I，模块I计算增益，并将计算结果送入模块J，模块J计算状态估计值和状态协方差矩阵，并将状态估计值输出，将状态协方差矩阵扩维后送入模块A。

    本系统的工作原理如下：

    协方差矩阵Cholesky分解模块A采用Cholesky分解方法将协方差矩阵(L+λ)P_x分解为一个上三角矩阵Δ和下三角矩阵Δ^T的乘积，即(L+λ)P_x＝ΔΔ^T。由于P_x是由状态协方差矩阵P_k‑1、状态噪声协方差矩阵Q和观测噪声协方差矩阵R扩维后得到的，所以(L+λ)P_x矩阵是分块对角矩阵，对分块对角协方差矩阵的对角线上的各子矩阵P_k‑1、Q和R分别进行Cholesky分解，得到下三角矩阵Δ^T，其中L＝2m+n，m为状态量的维数，n为观测量的维数，λ＝α²(L+k)‑L，α为确定采样点的散布程度，k为影响采样点分布的四阶矩；

    Sigma点产生子模块B_i，i＝1，2，...，K，接收上一时刻的状态估计值并利用模块A的第i组结果(表示Δ^T的第j个行向量，j＝1+L(i‑1)/K，...，Li/K)，按照1)式分别产生L个Sigma点χ_j和L个Sigma点χ_j+L，其中子模块B₁产生2L/K+1个Sigma点，包含其余子模块产生2L/K个Sigma点；

     ${\mathrm{\χ}}_j={\hat{x}}_{k-1}+{\mathrm{\Δ}}_j^T,$j＝1+L(i‑1)/K，...，Li/K

                                                1)

     ${\mathrm{\χ}}_{j+L}={\hat{x}}_{k-1}-{\mathrm{\Δ}}_j^T,$i＝1，...，K

    时间更新子模块C_i，i＝1，2，...，K，将来自Bi的一组Sigma点代入系统模型的状态方程，得到状态预测值对于模块C₁，j＝0，1，...，2L/K，对于其余子模块，j＝1，...，2L/K；

    观测预测子模块D_i，i＝1，2，...，K，把来自C_i的一组状态预测值代入观测模型的观测方程，得到观测预测值对于模块D₁，j＝0，1，...，2L/K，对于其余子模块，j＝1，...，2L/K；

    部分均值和协方差矩阵计算子模块E_i，i＝1，2，...，K，分别计算状态预测样本点的加权和观测预测样本点的加权和状态预测协方差矩阵观测预测协方差矩阵以及互协方差矩阵计算公式如下：

     ${\hat{x}}_{k|k-1}^i={\hat{x}}_{k|k-1}^i+W_j^{\left(m\right)}{\mathrm{\χ}}_{k|k-1,i}^{x\left(j\right)}---2)$

     ${\hat{y}}_{k|k-1}^i={\hat{y}}_{k|k-1}^i+W_j^{\left(m\right)}{y}_{k|k-1,i}^{\left(j\right)}---3)$

     $P_{{\hat{x}}_k{\hat{x}}_k}^i=P_{{\hat{x}}_k{\hat{x}}_k}^i+W_j^{\left(c\right)}{\mathrm{\χ}}_{k|k-1,i}^{x\left(j\right)}{\left({\mathrm{\χ}}_{k|k-1,i}^{x\left(j\right)}\right)}^T---4)$

     $P_{{\hat{y}}_k{\hat{y}}_k}^i=P_{{\hat{y}}_k{\hat{y}}_k}^i+W_j^{\left(c\right)}{y}_{k|k-1,i}^{\left(j\right)}{\left(y_{k|k-1,i}^{\left(j\right)}\right)}^T---5)$

     $P_{{\hat{x}}_k{\hat{y}}_k}^i=P_{{\hat{x}}_k{\hat{y}}_k}^i+W_j^{\left(c\right)}{\mathrm{\χ}}_{k|k-1,i}^{x\left(j\right)}{\left(y_{k|k-1,i}^{\left(j\right)}\right)}^T---6)$

    其中，对于模块E₁，j＝0，1，...，2L/K，对于其余子模块，j＝1，...，2L/K，$W_0^{\left(m\right)}=\mathrm{\λ}/(L+\mathrm{\λ}),$$W_0^{\left(c\right)}=\mathrm{\λ}/(L+\mathrm{\λ})+(1-{\mathrm{\α}}^2+\mathrm{\β}),$当j≠0时，$W_j^{\left(m\right)}=W_j^{\left(c\right)}=1/\left\{2(L+\mathrm{\λ})\right\},$α＞0，β≥0；

    总体均值计算模块F，按式7)和式8)计算状态预测均值和观测预测均值

     ${\hat{x}}_{k|k-1}={\hat{x}}_{k|k-1}+{\hat{x}}_{k|k-1}^i---7)$

     ${\hat{y}}_{k|k-1}={\hat{y}}_{k|k-1}+{\hat{y}}_{k|k-1}^i---8)$

    其中，i＝1，2，...，K；

    总体协方差矩阵计算模块G，对K个模块E_i计算得到的部分状态预测协方差矩阵部分观测预测协方差矩阵以及部分互协方差矩阵分别求和，得到状态预测协方差矩阵观测预测协方差矩阵和互协方差矩阵然后按式9)～式11)更新各个协方差矩阵：

     $P_{{\hat{x}}_k{\hat{x}}_k}=P_{{\hat{x}}_k{\hat{x}}_k}+(\mathrm{\β}-{\mathrm{\α}}^2){\hat{x}}_{k|k-1}{\hat{x}}_{k|k-1}^T-(1-{\mathrm{\α}}^2+\mathrm{\β})({\hat{x}}_{k|k-1}{\left({\mathrm{\χ}}_{k|k-\mathrm{1,0}}^{x\left(0\right)}\right)}^T+\left({\mathrm{\χ}}_{k|k-\mathrm{1,0}}^{x\left(0\right)}\right){\hat{x}}_{k|k-1}^T)---9)$

     $P_{{\hat{y}}_k{\hat{y}}_k}=P_{{\hat{y}}_k{\hat{y}}_k}+(\mathrm{\β}-{\mathrm{\α}}^2){\hat{y}}_{k|k-1}{\hat{y}}_{k|k-1}^T-(1-{\mathrm{\α}}^2+\mathrm{\β})({\hat{y}}_{k|k-1}{\left(y_{k|k-\mathrm{1,0}}^{\left(0\right)}\right)}^T+\left(y_{k|k-\mathrm{1,0}}^{\left(0\right)}\right){\hat{y}}_{k|k-1}^T)---10)$

     $P_{{\hat{x}}_k{\hat{y}}_k}=P_{{\hat{x}}_k{\hat{y}}_k}+(\mathrm{\β}-{\mathrm{\α}}^2){\hat{x}}_{k|k-1}{\hat{y}}_{k|k-1}^T-(1-{\mathrm{\α}}^2+\mathrm{\β})({\hat{x}}_{k|k-1}{\left(y_{k|k-\mathrm{1,0}}^{\left(0\right)}\right)}^T+\left({\mathrm{\χ}}_{k|k-\mathrm{1,0}}^{x\left(0\right)}\right){\hat{y}}_{k|k-1}^T)---11)$

    其中，i＝1，2，...，K，α，β为权值中的参数，α＞0，β≥0，为模块C₁计算出的第一个状态预测值，为模块D₁计算出的第一个观测预测值；

    观测预测协方差矩阵求逆模块H，对观测预测协方差矩阵进行奇异值分解，得到正交矩阵U和正交矩阵V以及对角矩阵∑，对∑求逆得到∑‑1，则的逆矩阵的计算公式如下；

     $P_{{\hat{y}}_k{\hat{y}}_k}^{-1}=V{\mathrm{\Σ}}^{-1}{U}^T---12)$

    增益计算模块I，将互协方差矩阵和观测预测协方差矩阵的逆矩阵相乘，得到增益K，计算公式如下；

     $K=P_{{\hat{x}}_k{\hat{y}}_k}{P}_{{\hat{y}}_k{\hat{y}}_k}^{-1}---13)$

    状态协方差矩阵估计模块J，接收增益K、状态预测均值观测预测均值状态预测协方差矩阵观测预测协方差矩阵和当前时刻的观测数据y_k，计算状态估计值和状态协方差矩阵P_k，并将作为当前时刻的最终结果输出，将P_k扩维后输入到协方差矩阵Cholesky分解模块A，计算公式如下。

     ${\hat{x}}_k={\hat{x}}_{k|k-1}+K(y_k-{\hat{y}}_{k|k-1})---14)$

     $P_k=P_{{\hat{x}}_k{\hat{x}}_k}-{\mathrm{KP}}_{{\hat{y}}_k{\hat{y}}_k}{K}^T---15)$

    参照图2，本发明的基于FPGA的无迹卡尔曼滤波方法，包括以下步骤：

    步骤1，对分块对角协方差矩阵的对角线上的各子矩阵分别进行Cholesky分解。

    1.1)计算矩阵第i列的第一个非零元素，i≥1，并根据第一个非零元素同时计算该列的其它非零元素；

    1.2)计算矩阵第i＝i+1列的第一个非零元素，并根据第一个非零元素同时计算该列的其它非零元素，i≤N，N为子矩阵维数。

    步骤2，接收上一时刻，即k‑1时刻的状态估计值利用步骤1Cholesky分解得到的L个向量产生K组Sigma点：其中和分别为χ_k‑1中对应于状态量、过程噪声和观测噪声的分量。

    步骤3，将步骤2得到的K组Sigma点χ_k‑1中对应于状态量的分量和对应于过程噪声的分量代入到如式16)所示的系统模型的状态方程中，得到K组状态预测值：

     ${\mathrm{\χ}}_{k|k-1}^x=F[{\mathrm{\χ}}_{k-1}^x,{\mathrm{\χ}}_{k-1}^w].---16)$

    步骤4，将步骤3得到的K组状态预测值和K组Sigma点χ_k‑1中对应于观测噪声的分量代入到如式17)所示的观测模型的观测方程中，得到观测预测值：

     $y_{k|k-1}=H[{\mathrm{\χ}}_{k|k-1}^x,{\mathrm{\χ}}_{k-1}^v].---17)$

    步骤5，对步骤4得到的K组观测预测值y_k|k‑1和步骤3得到的状态预测值进行加权求和，得到K组状态预测样本点的加权和和K组观测预测样本点的加权和按式4)～6)分别计算部分状态预测协方差矩阵部分观测预测协方差矩阵以及部分互协方差矩阵其中i＝1，2，...，K。

    步骤6，将步骤5得到的K组状态预测样本点的加权和求和得到状态预测均值将步骤5得到的K组观测预测样本点的加权和求和得到观测预测均值其中i＝1，2，...，K。

    步骤7，计算总体协方差矩阵。

    7.1)将步骤5得到的K组部分状态预测协方差矩阵K组部分观测预测协方差矩阵以及K组部分互协方差矩阵分别求和，得到状态预测协方差矩阵观测预测协方差矩阵和互协方差矩阵其中i＝1，2，...，K；

    7.2)按式9)～式11)更新各个协方差矩阵。

    步骤8，对步骤7得到的观测预测协方差矩阵求逆。

    8.1)若步骤7.2)得到的更新后的观测预测协方差矩阵维数为奇数，则对该矩阵补一行和一列零元素使其维数为偶数，否则不变；

    8.2)将划分为M/2×M/2个由2×2维的子矩阵$P_{\mathrm{ij}}=\left[\begin{array}{cc}{a}_{2i-\mathrm{1,2}j-1}& a_{2i-\mathrm{1,2}j}\\ a_{2i,2j-1}& a_{2i,2j}\end{array}\right]$组成的分块矩阵，M为观测预测协方差矩阵维数，a_{2i‑1，2j‑1}、a_2i‑1，2j、a_2i，2j‑1和a_2i，2j为观测预测协方差矩阵对应位置上的元素，为了表述方便将P_ij表示为$\left[\begin{array}{cc}{\mathrm{\α}}_{\mathrm{ij}}& {\mathrm{\β}}_{\mathrm{ij}}\\ {\mathrm{\γ}}_{\mathrm{ij}}& {\mathrm{\δ}}_{\mathrm{ij}}\end{array}\right],$其中α_ij＝a_{2i‑1，2j‑1}，β_ij＝a_2i‑1，2j，γ_ij＝a_2i，2j‑1，δ_ij＝a_2i，2j，i，j＝1，…，

    8.3)将待求的正交矩阵U划分为M/2×M/2个由2×2维的子矩阵$U_{\mathrm{ij}}=\left[\begin{array}{cc}{\mathrm{\μ}}_{\mathrm{ij}}& {\mathrm{\ν}}_{\mathrm{ij}}\\ {\mathrm{\σ}}_{\mathrm{ij}}& {\mathrm{\τ}}_{\mathrm{ij}}\end{array}\right],$(i，j＝1，…，)组成的分块矩阵，其中，当i≠j时，U_ij的初始值为μ_ij＝ν_ij＝σ_ij＝τ_ij＝0，当i＝j时，U_ij的初始值为μ_ii＝τ_ii＝1，σ_ii＝ν_ii＝0，将待求的正交矩阵V划分为M/2×M/2个由2×2维的子矩阵$V_{\mathrm{ij}}=\left[\begin{array}{cc}{\mathrm{\η}}_{\mathrm{ij}}& {\mathrm{\ω}}_{\mathrm{ij}}\\ {\mathrm{\ρ}}_{\mathrm{ij}}& {\mathrm{\ϵ}}_{\mathrm{ij}}\end{array}\right],$(i，j＝1，…，)组成的分块矩阵，其中，当i≠j时，V_ij的初始值为η_ij＝ω_ij＝ρ_ij＝ε_ij＝0，当i＝j时，V_ij的初始值为η_ii＝ε_ii＝1，ω_ii＝ρ_ii＝0；

    8.4)按如下公式计算φ+θ和φ‑θ，并计算cosθ、sinθ、cosφ和sinφ：

     $\mathrm{\φ}+\mathrm{\θ}={\tan }^{-1}\frac{{\mathrm{\γ}}_{\mathrm{ii}}+{\mathrm{\β}}_{\mathrm{ii}}}{{\mathrm{\δ}}_{\mathrm{ii}}-{\mathrm{\α}}_{\mathrm{ii}}}---18)$

     $\mathrm{\φ}-\mathrm{\θ}={\tan }^{-1}\frac{{\mathrm{\γ}}_{\mathrm{ii}}-{\mathrm{\β}}_{\mathrm{ii}}}{{\mathrm{\δ}}_{\mathrm{ii}}+{\mathrm{\α}}_{\mathrm{ii}}}---19)$

    其中，α_ii、β_ii、γ_ii和δ_ii分别是子矩阵$P_{\mathrm{ii}}=\left[\begin{array}{cc}{\mathrm{\α}}_{\mathrm{ii}}& {\mathrm{\β}}_{\mathrm{ii}}\\ {\mathrm{\γ}}_{\mathrm{ii}}& {\mathrm{\δ}}_{\mathrm{ii}}\end{array}\right]$对应位置上的元素，i＝1，…，

    8.5)将对角线上的子矩阵P_ii(i＝1，…，)按如下公式进行更新：

     $\left[\begin{array}{cc}{\mathrm{\α}}_{\mathrm{ii}}^{\′}& 0\\ 0& {\mathrm{\δ}}_{\mathrm{ii}}^{\′}\end{array}\right]={\left[\begin{array}{cc}{c}_i^L& s_i^L\\ -s_i^L& c_i^L\end{array}\right]}^T\left[\begin{array}{cc}{\mathrm{\α}}_{\mathrm{ii}}& {\mathrm{\β}}_{\mathrm{ii}}\\ {\mathrm{\γ}}_{\mathrm{ii}}& {\mathrm{\δ}}_{\mathrm{ii}}\end{array}\right]\left[\begin{array}{cc}{c}_i^R& s_i^R\\ -s_i^R& c_i^R\end{array}\right]---20)$

    其中，表示cosθ，表示sinθ，表示cosφ，表示sinφ，α_ii、β_ii、γ_ii和δ_ii分别是子矩阵$P_{\mathrm{ii}}=\left[\begin{array}{cc}{\mathrm{\α}}_{\mathrm{ii}}& {\mathrm{\β}}_{\mathrm{ii}}\\ {\mathrm{\γ}}_{\mathrm{ii}}& {\mathrm{\δ}}_{\mathrm{ii}}\end{array}\right]$更新前对应位置上的元素，α′_ii和δ′_ii分别是按式20)更新后子矩阵$P_{\mathrm{ii}}=\left[\begin{array}{cc}{\mathrm{\α}}_{\mathrm{ii}}^{\′}& 0\\ 0& {\mathrm{\δ}}_{\mathrm{ii}}^{\′}\end{array}\right]$对应位置上的元素，i＝1，…，

    8.6)将非对角线上的子矩阵P_ij(i≠j)按如下公式进行更新：

     $\left[\begin{array}{cc}{\mathrm{\α}}_{\mathrm{ij}}^{\′}& {\mathrm{\β}}_{\mathrm{ij}}^{\′}\\ {\mathrm{\γ}}_{\mathrm{ij}}^{\′}& {\mathrm{\δ}}_{\mathrm{ij}}^{\′}\end{array}\right]={\left[\begin{array}{cc}{c}_i^L& s_i^L\\ {-s}_i^L& c_i^L\end{array}\right]}^T\left[\begin{array}{cc}{\mathrm{\α}}_{\mathrm{ij}}& {\mathrm{\β}}_{\mathrm{ij}}\\ {\mathrm{\γ}}_{\mathrm{ij}}& {\mathrm{\δ}}_{\mathrm{ij}}\end{array}\right]\left[\begin{array}{cc}{c}_j^R& s_j^R\\ {-s}_j^R& c_j^R\end{array}\right]---21)$

    其中，表示cosθ，表示sinθ，表示cosφ，表示sinφ，α_ij、β_ij、γ_ij和δ_ij分别是子矩阵$P_{\mathrm{ij}}=\left[\begin{array}{cc}{\mathrm{\α}}_{\mathrm{ij}}& {\mathrm{\β}}_{\mathrm{ij}}\\ {\mathrm{\γ}}_{\mathrm{ij}}& {\mathrm{\δ}}_{\mathrm{ij}}\end{array}\right]$更新前对应位置上的元素，α′_ij、β′_ij、γ′_ij和δ′_ij是按式21)更新后子矩阵$P_{\mathrm{ij}}=\left[\begin{array}{cc}{\mathrm{\α}}_{\mathrm{ij}}^{\′}& {\mathrm{\β}}_{\mathrm{ij}}^{\′}\\ {\mathrm{\γ}}_{\mathrm{ij}}^{\′}& {\mathrm{\δ}}_{\mathrm{ij}}^{\′}\end{array}\right]$对应位置上的元素，i，j＝1，…，且i≠j；

    8.7)将U_ij按如下公式进行更新：

     $\left[\begin{array}{cc}{\mathrm{\μ}}_{\mathrm{ij}}^{\′}& {\mathrm{\ν}}_{\mathrm{ij}}^{\′}\\ {\mathrm{\σ}}_{\mathrm{ij}}^{\′}& {\mathrm{\τ}}_{\mathrm{ij}}^{\′}\end{array}\right]=\left[\begin{array}{cc}{\mathrm{\μ}}_{\mathrm{ij}}& {\mathrm{\ν}}_{\mathrm{ij}}\\ {\mathrm{\σ}}_{\mathrm{ij}}& {\mathrm{\τ}}_{\mathrm{ij}}\end{array}\right]\left[\begin{array}{cc}{c}_j^L& s_j^L\\ -s_j^L& c_j^L\end{array}\right]---22)$

    其中，表示cosθ，表示sinθ，表示cosφ，表示sinφ，μ_ij、ν_ij、σ_ij和τ_ij是更新前$U_{\mathrm{ij}}=\left[\begin{array}{cc}{\mathrm{\μ}}_{\mathrm{ij}}& {\mathrm{\ν}}_{\mathrm{ij}}\\ {\mathrm{\σ}}_{\mathrm{ij}}& {\mathrm{\τ}}_{\mathrm{ij}}\end{array}\right]$对应位置上的元素，μ′_ij、ν′_ij、σ′_ij和τ′_ij是按式22)更新后$U_{\mathrm{ij}}=\left[\begin{array}{cc}{\mathrm{\μ}}_{\mathrm{ij}}^{\′}& {\mathrm{\ν}}_{\mathrm{ij}}^{\′}\\ {\mathrm{\σ}}_{\mathrm{ij}}^{\′}& {\mathrm{\τ}}_{\mathrm{ij}}^{\′}\end{array}\right]$对应位置上的元素，i，j＝1，…，

    8.8)将V_ij按如下公式进行更新：

     $\left[\begin{array}{cc}{\mathrm{\η}}_{\mathrm{ij}}^{\′}& {\mathrm{\ω}}_{\mathrm{ij}}^{\′}\\ {\mathrm{\ρ}}_{\mathrm{ij}}^{\′}& {\mathrm{\ϵ}}_{\mathrm{ij}}^{\′}\end{array}\right]=\left[\begin{array}{cc}{\mathrm{\η}}_{\mathrm{ij}}& {\mathrm{\ω}}_{\mathrm{ij}}\\ {\mathrm{\ρ}}_{\mathrm{ij}}& {\mathrm{\ϵ}}_{\mathrm{ij}}\end{array}\right]\left[\begin{array}{cc}{c}_j^R& s_j^R\\ -s_j^R& c_j^R\end{array}\right]---23)$

    其中，表示cosθ，表示sinθ，表示cosφ，表示sinφ，η_ij、ω_ij、ρ_ij和ε_ij是更新前$V_{\mathrm{ij}}=\left[\begin{array}{cc}{\mathrm{\η}}_{\mathrm{ij}}& {\mathrm{\ω}}_{\mathrm{ij}}\\ {\mathrm{\ρ}}_{\mathrm{ij}}& {\mathrm{\ϵ}}_{\mathrm{ij}}\end{array}\right]$对应位置上的元素，η′_ij、ω′_ij、ρ′_ij和ε′_ij是按式23)更新后$V_{\mathrm{ij}}=\left[\begin{array}{cc}{\mathrm{\η}}_{\mathrm{ij}}^{\′}& {\mathrm{\ω}}_{\mathrm{ij}}^{\′}\\ {\mathrm{\ρ}}_{\mathrm{ij}}^{\′}& {\mathrm{\ϵ}}_{\mathrm{ij}}^{\′}\end{array}\right]$对应位置上的元素，i，j＝1，…，

    8.9)将步骤8.5)～8.8)更新后的矩阵U和V的子矩阵内部进行元素交换，交换规则如下：

    当i＝1，j＝1时，$\left[\begin{array}{c}{\mathrm{\α}}^{\′}\←\mathrm{\α};{\mathrm{\β}}^{\′}\←\mathrm{\β};\\ {\mathrm{\γ}}^{\′}\←\mathrm{\γ};{\mathrm{\δ}}^{\′}\←\mathrm{\δ};\end{array}\right]---24)$

    当i＝1，j≠1时，$\left[\begin{array}{c}{\mathrm{\α}}^{\′}\←\mathrm{\β};{\mathrm{\β}}^{\′}\←\mathrm{\α};\\ {\mathrm{\γ}}^{\′}\←\mathrm{\δ};{\mathrm{\δ}}^{\′}\←\mathrm{\γ};\end{array}\right]---25)$

    当i≠1，j＝1时，$\left[\begin{array}{c}{\mathrm{\α}}^{\′}\←\mathrm{\γ};{\mathrm{\β}}^{\′}\←\mathrm{\δ};\\ {\mathrm{\γ}}^{\′}\←\mathrm{\α};{\mathrm{\δ}}^{\′}\←\mathrm{\β};\end{array}\right]---26)$

    其它情况$\left[\begin{array}{c}{\mathrm{\α}}^{\′}\←\mathrm{\δ};{\mathrm{\β}}^{\′}\←\mathrm{\γ};\\ {\mathrm{\γ}}^{\′}\←\mathrm{\β};{\mathrm{\δ}}^{\′}\←\mathrm{\α};\end{array}\right]---27)$

    其中，α、β、γ和δ是交换前子矩阵$\left[\begin{array}{cc}\mathrm{\α}& \mathrm{\β}\\ \mathrm{\γ}& \mathrm{\δ}\end{array}\right]$对应位置上的元素，α′、β′、γ′和δ′是交换后子矩阵$\left[\begin{array}{cc}{\mathrm{\α}}^{\′}& {\mathrm{\β}}^{\′}\\ {\mathrm{\γ}}^{\′}& {\mathrm{\δ}}^{\′}\end{array}\right]$对应位置上的元素，i，j是子矩阵的下标，i，j＝1，…，

    8.10)将矩阵U和V各子矩阵之间进行元素交换，交换规则如图3所示，并将交换后的矩阵U和V代入步骤8.4)；

    8.11)将步骤8.4)～8.9)重复S(M‑1)次，最后得到S(M‑1)次更新后的正交矩阵U和V，并由S(M‑1)次更新后的得到对角矩阵∑，其中，S＝log₂M；

    8.12)将对角矩阵∑的对角线上的非零元素求倒数，得到∑‑1，则的逆矩阵为V∑^‑1U^T。

    步骤9，将步骤7得到的互协方差矩阵和步骤8得到的观测预测协方差矩阵的逆矩阵相乘，得到增益K。

    步骤10，将步骤9得到的增益K、步骤6得到的状态预测均值观测预测均值和当前时刻(k时刻)的观测数据y_k代入14)式得到状态估计值将步骤7得到的状态预测协方差矩阵观测预测协方差矩阵和步骤9得到的增益K代入15)式得到状态协方差矩阵估计值P_k，并将状态估计值作为当前时刻的最终结果输出，对状态协方差矩阵P_k扩维，返回到步骤1进行下一时刻的计算。

    本发明的效果可通过如下仿真实验说明：

    1.仿真条件

    本实验采用二维平面内静止布置的三个被动传感器测量同一平面内的单个运动目标，三个被动传感器成正三角形分布，各传感器位置坐标分别为(20，20)、(30，20)和(25，28.66)，单位为km，每个传感器的采样周期T为10ms。目标在坐标轴上的初始位置为x₀＝0km，y₀＝23km，目标以速度作匀速直线运动。系统模型的状态方程为：

    x_k＝F_k，k‑1x_k‑1+w_k‑1                        28)

    其中，为k时刻的目标状态，w_k‑1为高斯分布的状态噪声，F为目标状态转移矩阵。

     $F=\left[\begin{array}{cccccc}1& 0& T& 0& T^2/2& 0\\ 0& 1& 0& T& 0& T^2/2\\ 0& 0& 1& 0& T& 0\\ 0& 0& 0& 1& 0& T\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\end{array}\right],$T为采样周期

    29)

    观测模型的观测方程为：

    z_ik＝H_i[x_k]+v_ik                            30)

    其中，z_ik是第i(i＝1，2，3)个被动观测站的方位观测，y_i和x_i分别为观测站在直角坐标中两个方向上的位置，v_ik是观测噪声，假设v_ik为零均值白噪声，方差为σ_i，观测噪声协方差矩阵为$R_k=\left[\begin{array}{ccc}{\mathrm{\σ}}_1& 0& 0\\ 0& {\mathrm{\σ}}_2& 0\\ 0& 0& {\mathrm{\σ}}_3\end{array}\right].$

    在Xilinx公司的FPGA芯片XC4VFX140上用本发明提出的无迹卡尔曼滤波系统实现对该运动目标的跟踪，将UKF算法在FPGA中采用本发明提出的并行结构进行处理。其中FPGA芯片的速度级别为‑11，主时钟频率为100MHz，运算过程中涉及的乘法器、加法器、减法器、除法器和开方运算器均采用Xilinx提供的浮点数IP核实现，反正切、正弦和余弦的计算均采用Xilinx提供的CORDIC算法IP核实现。

    2.仿真结果

    表1给出了FPGA的运算时间与并行单元个数之间的关系。从表1中可以看出并行单元个数越多，运算时间越短，因而本发明的基于FPGA的无迹卡尔曼滤波系统及并行实现方法具有良好的实时性。

    表1不同并行单元个数对应的运算时间

       并行单元个数  1  2  3  5  6  时间(us)  122.10  68.13  52.70  43.22  41.29