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

摘要

本发明公开了一种基于FPGA的无迹卡尔曼滤波系统，主要解决现有无迹卡尔曼滤波硬件实现难度大和实时性差的问题。该系统包括：协方差矩阵Cholesky分解模块A，Sigma点产生模块B、时间更新模块C、观测预测模块D、部分均值和协方差矩阵计算模块E、总体均值计算模块F、总体协方差矩阵计算模块G、观测预测协方差矩阵求逆模块H、增益计算模块I和状态量估计和状态协方差矩阵估计模块J。模块A产生K组行向量到模块B，模块B、C、D和E是串连关系，分别包含K个采用并行运算单元结构的子模块；模块F、G接收模块E的K组结果并处理，处理结果依次经过模块H、I和J得到当前结果。本发明具有滤波速度快和易于硬件实现的优点，可用于目标跟踪和参数估计。

主权项

一种基于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模块。