[发明专利]基于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，用于对观测预测值和状态预测值加权求和，分 别计算部分状态预测协方差矩阵、部分观测预测协方差矩阵和部分互协方差矩阵，并将其 计算结果输入到总体均值计算模块和总体协方差矩阵计算模块；