CN101777887A - 基于fpga的无迹卡尔曼滤波系统及并行实现方法 - Google Patents
基于fpga的无迹卡尔曼滤波系统及并行实现方法 Download PDFInfo
- Publication number
- CN101777887A CN101777887A CN 201010013568 CN201010013568A CN101777887A CN 101777887 A CN101777887 A CN 101777887A CN 201010013568 CN201010013568 CN 201010013568 CN 201010013568 A CN201010013568 A CN 201010013568A CN 101777887 A CN101777887 A CN 101777887A
- Authority
- CN
- China
- Prior art keywords
- covariance matrix
- module
- matrix
- observation
- prime
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Images
Landscapes
- Management, Administration, Business Operations System, And Electronic Commerce (AREA)
Abstract
本发明公开了一种基于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得到当前结果。本发明具有滤波速度快和易于硬件实现的优点,可用于目标跟踪和参数估计。
Description
技术领域
本发明属于信号处理技术领域,涉及非线性滤波方法,可用于目标跟踪和参数估计。
背景技术
滤波理论是在对系统可观测信号进行测量的基础上,根据一定的滤波准则,对系统的状态进行估计的理论和方法。根据贝叶斯理论,最优估计只是一个理想化的解,一般情况下,无法得到它的解析形式。卡尔曼(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点产生子模块Bi,i=1,2,...,K,B1,B2,...,BK采用K个并行运算单元结构,模块C包含K个时间更新子模块Ci,i=1,2,...,K,C1,C2,...,CK采用K个并行运算单元结构,模块D包含K个观测预测子模块Di,i=1,2,..,K,D1,D2,...,DK采用K个并行运算单元结构,模块E包含K个部分均值和协方差矩阵计算子模块Ei,i=1,2,...,K,E1,E2,...,EK采用K个并行运算单元结构。模块A实现协方差矩阵Cholesky分解,并将分解得到的下三角矩阵的行向量分成K组,分别送入模块B的K个子模块,模块B的K个子模块Bi分别产生一组Sigma点,送入模块Ci,模块Ci分别对该组Sigma点进行时间更新产生一组状态预测值送入模块Di,模块Di将该组状态预测值代入观测方程产生一组观测预测值并将状态预测值和观测预测值一起送入模块Ei,模块Ei对该组观测预测值和状态预测值加权求和,分别计算部分状态预测协方差矩阵、部分观测预测协方差矩阵和部分互协方差矩阵。将E1,E2,...,EK的计算结果送入模块F和模块G,模块F采用E1,E2,...,EK输入的K组计算结果实现总体均值的计算,并将计算结果送入模块G,模块G实现总体协方差矩阵计算,并将计算结果送入模块H,模块H对观测预测协方差矩阵求逆,并将计算结果送入模块I,模块I计算增益,并将计算结果送入模块J,模块J计算状态估计值和状态协方差矩阵,并将状态估计值输出,将状态协方差矩阵扩维后送入模块A。
本系统的工作原理如下:
协方差矩阵Cholesky分解模块A采用Cholesky分解方法将协方差矩阵(L+λ)Px分解为一个上三角矩阵Δ和下三角矩阵ΔT的乘积,即(L+λ)Px=ΔΔT。由于Px是由状态协方差矩阵Pk-1、状态噪声协方差矩阵Q和观测噪声协方差矩阵R扩维后得到的,所以(L+λ)Px矩阵是分块对角矩阵,对分块对角协方差矩阵的对角线上的各子矩阵Pk-1、Q和R分别进行Cholesky分解,得到下三角矩阵ΔT,其中L=2m+n,m为状态量的维数,n为观测量的维数,λ=α2(L+k)-L,α为确定采样点的散布程度,k为影响采样点分布的四阶矩;
Sigma点产生子模块Bi,i=1,2,...,K,接收上一时刻的状态估计值并利用模块A的第i组结果{Δj T}(Δj T表示ΔT的第j个行向量,j=1+L(i-1)/K,...,Li/K),按照1)式分别产生L个Sigma点χj和L个Sigma点χj+L,其中子模块B0产生2L/K+1个Sigma点,包含 其余子模块产生2L/K个Sigma点;
时间更新子模块Ci,i=1,2,...,K,将来自Bi的一组Sigma点代入系统模型的状态方程,得到状态预测值{χk|k-1,i x(j)},对于模块C0,j=0,1,...,2L/K,对于其余子模块,j=1,...,2L/K;
观测预测子模块Di,i=1,2,...,K,把来自Ci的一组状态预测值代入观测模型的观测方程,得到观测预测值{yk|k-1,i (j)},对于模块D0,j=0,1,...,2L/K,对于其余子模块,j=1,...,2L/K;
其中, 对于模块E0,j=0,1,...,2L/K,对于其余子模块,j=1,...,2L/K,i=1,2,...,K;
其中,i=1,2,...,K;
总体协方差矩阵计算模块G,对K个模块Ei计算得到的部分状态预测协方差矩阵部分观测预测协方差矩阵以及部分互协方差矩阵分别求和,得到状态预测协方差矩阵观测预测协方差矩阵和互协方差矩阵然后按式9)~式11)更新各个协方差矩阵:
其中,i=1,2,...,K,α,β为权值 中的参数,α>0,β≥0,χk|k-1,0 x(0)为模块C0计算出的第一个状态预测值,yk|k-1,0 (0)为模块D0计算出的第一个观测预测值;
状态协方差矩阵估计模块J,接收增益K、状态预测均值观测预测均值状态预测协方差矩阵观测预测协方差矩阵和当前时刻的观测数据yk,计算状态估计值和状态协方差矩阵Pk,并将作为当前时刻的最终结果输出,将Pk扩维后输入到协方差矩阵Cholesky分解模块A,计算公式如下。
参照图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 x、χk-1 w和χk-1 v分别为χk-1中对应于状态量、过程噪声和观测噪声的分量。
步骤3,将步骤2得到的K组Sigma点χk-1中对应于状态量的χk-1 x分量和对应于过程噪声的χk-1 w分量代入到如式16)所示的系统模型的状态方程中,得到K组状态预测值:
步骤4,将步骤3得到的K组状态预测值χk|k-1 x和K组Sigma点χk-1中对应于观测噪声的χk-1 v分量代入到如式17)所示的观测模型的观测方程中,得到观测预测值:
步骤5,对步骤4得到的K组观测预测值yk|k-1和步骤3得到的状态预测值χk|k-1 x进行加权求和,得到K组状态预测样本点的加权和和K组观测预测样本点的加权和按式4)~6)分别计算部分状态预测协方差矩阵部分观测预测协方差矩阵以及部分互协方差矩阵其中i=1,2,...,K。
步骤7,计算总体协方差矩阵。
7.2)按式9)~式11)更新各个协方差矩阵。
8.1)若步骤7.2)得到的更新后的观测预测协方差矩阵维数为奇数,则对该矩阵补一行和一列零元素使其维数为偶数,否则不变;
8.2)将划分为M/2×M/2个由2×2维的子矩阵 组成的分块矩阵,M为观测预测协方差矩阵维数,a2i-1,2j-1、a2i-1,2j、a2i,2j-1和a2i,2j为观测预测协方差矩阵对应位置上的元素,为了表述方便将Pij表示为其中αij=a2i-1,2j-1,βij=a2i-1, 2j,γij=a2i,2j-1,δij=a2i,2j,
8.3)将待求的正交矩阵U划分为M/2×M/2个由2×2维的子矩阵 组成的分块矩阵,其中,当i≠j时,Uij的初始值为μij=νij=σij=τij=0,当i=j时,Uij的初始值为μii=τii=1,σii=νii=0,将待求的正交矩阵V划分为M/2×M/2个由2×2维的子矩阵 组成的分块矩阵,其中,当i≠j时,Vij的初始值为ηij=ωij=ρij=εij=0,当i=j时,Vij的初始值为ηii=εii=1,ωii=ρii=0;
8.4)按如下公式计算φ+θ和φ-θ,并计算cosθ、sinθ、cosφ和sinφ:
其中,αii、βii、γii和δii分别是子矩阵 对应位置上的元素,
8.5)将对角线上的子矩阵 按如下公式进行更新:
其中,ci L表示cosθ,si L表示sinθ,ci R表示cosφ,si R表示sinφ,αii、βii、γii和δii分别是子矩阵 更新前对应位置上的元素,αii′和δii′分别是按式20)更新后子矩阵 对应位置上的元素,
8.6)将非对角线上的子矩阵Pij(i≠j)按如下公式进行更新:
其中,ci L表示cosθ,si L表示sinθ,ci R表示cosφ,si R表示sinφ,αij、βij、γij和δij分别是子矩阵 更新前对应位置上的元素,αij′、βij′、γij′和δij′是按式21)更新后子矩阵 对应位置上的元素, 且i≠j;
8.7)将Uij按如下公式进行更新:
其中,ci L表示cosθ,si L表示sinθ,ci R表示cosφ,si R表示sinφ,μij、νij、σij和τij是更新前 对应位置上的元素,μij′、νij′、σij′和τij′是按式22)更新后 对应位置上的元素,
8.8)将Vij按如下公式进行更新:
其中,ci L表示cosθ,si L表示sinθ,ci R表示cosφ,si R表示sinφ,ηij、ωij、ρij和εij是更新前 对应位置上的元素,ηij′、ωij′、ρij′和εij′是按式23)更新后 对应位置上的元素,
8.9)将步骤8.5)~8.8)更新后的矩阵U和V的子矩阵内部进行元素交换,交换规则如下:
步骤10,将步骤9得到的增益K、步骤6得到的状态预测均值观测预测均值和当前时刻(k时刻)的观测数据yk代入14)式得到状态估计值将步骤7得到的状态预测协方差矩阵观测预测协方差矩阵和步骤9得到的增益K代入15)式得到状态协方差矩阵估计值Pk,并将状态估计值作为当前时刻的最终结果输出,对状态协方差矩阵Pk扩维,返回到步骤1进行下一时刻的计算。
本发明的效果可通过如下仿真实验说明:
1.仿真条件
本实验采用二维平面内静止布置的三个被动传感器测量同一平面内的单个运动目标,三个被动传感器成正三角形分布,各传感器位置坐标分别为(20,20)、(30,20)和(25,28.66),单位为km,每个传感器的采样周期T为10ms。目标在坐标轴上的初始位置为x0=0km,y0=23km,目标以速度 作匀速直线运动。系统模型的状态方程为:
xk=Fk,k-1xk-1+wk-1 28)
其中, 为k时刻的目标状态,wk-1为高斯分布的状态噪声,F为目标状态转移矩阵。
观测模型的观测方程为:
zik=Hi[xk]+vik 30)
在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 |
Claims (8)
1.一种基于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模块。
2.根据权利要求1所述的基于FPGA的无迹卡尔曼滤波系统,其中模块B包含K个Sigma点产生子模块Bi,i=1,2,...,K,B1,B2,...,BK采用K个并行运算单元结构。
3.根据权利要求1所述的基于FPGA的无迹卡尔曼滤波系统,其中模块C包含K个时间更新子模块Ci,i=1,2,...,K,C1,C2,...,CK采用K个并行运算单元结构。
4.根据权利要求1所述的基于FPGA的无迹卡尔曼滤波系统,其中模块D包含K个观测预测子模块Di,i=1,2,...,K,D1,D2,...,DK采用K个并行运算单元结构。
5.根据权利要求1所述的基于FPGA的无迹卡尔曼滤波系统,其中模块E包含K个部分均值和协方差矩阵计算子模块Ei,i=1,2,...,K,E1,E2,...,EK采用K个并行运算单元结构。
6.一种基于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)进行下一时刻的计算。
7.根据权利要求6所述的无迹卡尔曼滤波的并行实现方法,其中步骤(1)所述的子矩阵Cholesky分解,先计算矩阵第i列的第一个非零元素,i≥1,并根据第一个非零元素同时计算该列的其它非零元素;再计算矩阵第i=i+1列的第一个非零元素,并根据第一个非零元素同时计算该列的其它非零元素,i≤N,N为子矩阵维数。
8.根据权利要求6所述的无迹卡尔曼滤波的并行实现方法,其中步骤(8)所述的对观测预测协方差矩阵采用奇异值分解方法求逆,按如下步骤进行:
(8a)若观测预测协方差矩阵维数为奇数,则对该矩阵补一行和一列零元素使其维数为偶数,否则不变;
(8b)将观测预测协方差矩阵划分为M/2×M/2个由2×2维的子矩阵 组成的分块矩阵,M为观测预测协方差矩阵维数,a2i-1,2j-1、a2i-1,2j、a2i,2j-1和a2i,2j为观测预测协方差矩阵对应位置上的元素,为了表述方便将Pij表示为其中αij=a2i-1,2j-1,βij=a2i-1,2j,γij=a2i,2j-1,δij=a2i,2j,
(8c)将待求的正交矩阵U划分为M/2×M/2个由2×2维的子矩阵 组成的分块矩阵,其中,当i≠j时,Uij的初始值为μij=νij=σij=τij=0,当i=j时,Uij的初始值为μii=τii=1,σii=νii=0,将待求的正交矩阵V划分为M/2×M/2个由2×2维的子矩阵 组成的分块矩阵,其中,当i≠j时,Vij的初始值为ηij=ωij=ρij=εij=0,当i=j时,Vij的初始值为ηii=εii=1,ωii=ρii=0;
(8d)按如下公式计算φ+θ和φ-θ,并计算cosθ、sinθ、cosφ和sinφ:
其中,αii、βii、γii和δii分别是子矩阵 对应位置上的元素,
(8e)将对角线上的子矩阵Pii按如下公式进行更新:
其中,ci L表示cosθ,si L表示sinθ,ci R表示cosφ,si R表示sinφ,αii、βii、γii和δii分别是子矩阵 更新前对应位置上的元素,αii′和δii′分别是按上式更新后子矩阵 对应位置上的元素,
(8f)将非对角线上的子矩阵Pij(i≠j)按如下公式进行更新:
其中,ci L表示cosθ,si L表示sinθ,ci R表示cosφ,si R表示sinφ,αij、βij、γij和δij分别是子矩阵 更新前对应位置上的元素,αij′、βij′、γij′和δij′是按上式更新后子矩阵 对应位置上的元素, 且i≠j;
(8g)将Uij按如下公式进行更新:
其中,ci L表示cosθ,si L表示sinθ,ci R表示cosφ,si R表示sinφ,μij、νij、σij和τij是更新前 对应位置上的元素,μij′、νij′、σij′和τij′是按上式更新后 对应位置上的元素,
(8h)将Vij按如下公式进行更新:
其中,ci L表示cosθ,si L表示sinθ,ci R表示cosφ,si R表示sinφ,ηij、ωij、ρij和εij是更新前 对应位置上的元素,ηij′、ωij′、ρij′和εij′是按式23)更新后 对应位置上的元素,
(8l)将对角矩阵∑的对角线上的非零元素求倒数,得到∑-1,则的逆矩阵为V∑-1UT。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN 201010013568 CN101777887B (zh) | 2010-01-08 | 2010-01-08 | 基于fpga的无迹卡尔曼滤波系统及并行实现方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN 201010013568 CN101777887B (zh) | 2010-01-08 | 2010-01-08 | 基于fpga的无迹卡尔曼滤波系统及并行实现方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN101777887A true CN101777887A (zh) | 2010-07-14 |
CN101777887B CN101777887B (zh) | 2013-04-03 |
Family
ID=42514244
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN 201010013568 Expired - Fee Related CN101777887B (zh) | 2010-01-08 | 2010-01-08 | 基于fpga的无迹卡尔曼滤波系统及并行实现方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN101777887B (zh) |
Cited By (16)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102064799A (zh) * | 2010-12-31 | 2011-05-18 | 南京理工大学 | 基于fpga的去偏转换量测卡尔曼滤波器的设计方法 |
CN102129420A (zh) * | 2011-03-07 | 2011-07-20 | 哈尔滨工业大学 | 基于Cholesky分解解决最小二乘问题的FPGA实现装置 |
CN104038180A (zh) * | 2014-05-22 | 2014-09-10 | 中国科学院重庆绿色智能技术研究院 | 基于高阶矩匹配的无迹卡尔曼滤波器的多项式方法 |
CN104143017A (zh) * | 2014-07-07 | 2014-11-12 | 燕山大学 | 基于fpga的ukf算法及其对大脑动力学模型的滤波 |
CN104038180B (zh) * | 2014-05-22 | 2016-11-30 | 中国科学院重庆绿色智能技术研究院 | 基于高阶矩匹配的无迹卡尔曼滤波器的多项式方法 |
CN107025609A (zh) * | 2017-03-16 | 2017-08-08 | 河海大学 | 基于奇异值分解cdkf的电力系统动态状态估计方法 |
CN107506332A (zh) * | 2017-07-25 | 2017-12-22 | 四川航天系统工程研究所 | Kalman滤波器快速实现方法 |
CN107947761A (zh) * | 2017-12-18 | 2018-04-20 | 西安理工大学 | 基于最小均方四阶的变阈值比例更新自适应滤波算法 |
CN105611288B (zh) * | 2015-12-28 | 2018-08-21 | 电子科技大学 | 一种基于有约束插值技术的低码率图像编码方法 |
CN108733627A (zh) * | 2018-04-30 | 2018-11-02 | 南京大学 | 一种正定矩阵Cholesky分解的FPGA实现方法 |
CN110109049A (zh) * | 2019-03-27 | 2019-08-09 | 北京邮电大学 | 用于大规模天线角度估计的无迹卡尔曼滤波方法及装置 |
CN110297127A (zh) * | 2019-05-28 | 2019-10-01 | 许昌许继软件技术有限公司 | 一种交流信号滤波方法及装置 |
CN112242145A (zh) * | 2019-07-17 | 2021-01-19 | 南京人工智能高等研究院有限公司 | 语音滤波方法、装置、介质和电子设备 |
CN112507597A (zh) * | 2020-11-02 | 2021-03-16 | 中国南方电网有限责任公司超高压输电公司广州局 | 基于多簇粒子滤波的多端柔性直流输电系统状态评估方法 |
CN113422593A (zh) * | 2021-07-05 | 2021-09-21 | 北京信息科技大学 | 滤波方法、滤波器、计算机可读存储介质、处理器和fpga |
CN114338664A (zh) * | 2021-11-30 | 2022-04-12 | 鹏城实验室 | 基于分布式架构获取目标状态的方法、装置及存储介质 |
Family Cites Families (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101055563A (zh) * | 2007-05-21 | 2007-10-17 | 北京理工大学 | 基于多建议分布的粒子滤波方法 |
CN201365232Y (zh) * | 2009-01-15 | 2009-12-16 | 北京航空航天大学 | 一种基于现场可编程门阵列的卡尔曼滤波器 |
-
2010
- 2010-01-08 CN CN 201010013568 patent/CN101777887B/zh not_active Expired - Fee Related
Cited By (23)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102064799A (zh) * | 2010-12-31 | 2011-05-18 | 南京理工大学 | 基于fpga的去偏转换量测卡尔曼滤波器的设计方法 |
CN102064799B (zh) * | 2010-12-31 | 2014-06-04 | 南京理工大学 | 基于fpga的去偏转换量测卡尔曼滤波的系统 |
CN102129420A (zh) * | 2011-03-07 | 2011-07-20 | 哈尔滨工业大学 | 基于Cholesky分解解决最小二乘问题的FPGA实现装置 |
CN102129420B (zh) * | 2011-03-07 | 2013-03-20 | 哈尔滨工业大学 | 基于Cholesky分解解决最小二乘问题的FPGA实现装置 |
CN104038180A (zh) * | 2014-05-22 | 2014-09-10 | 中国科学院重庆绿色智能技术研究院 | 基于高阶矩匹配的无迹卡尔曼滤波器的多项式方法 |
CN104038180B (zh) * | 2014-05-22 | 2016-11-30 | 中国科学院重庆绿色智能技术研究院 | 基于高阶矩匹配的无迹卡尔曼滤波器的多项式方法 |
CN104143017A (zh) * | 2014-07-07 | 2014-11-12 | 燕山大学 | 基于fpga的ukf算法及其对大脑动力学模型的滤波 |
CN105611288B (zh) * | 2015-12-28 | 2018-08-21 | 电子科技大学 | 一种基于有约束插值技术的低码率图像编码方法 |
CN107025609B (zh) * | 2017-03-16 | 2020-10-16 | 河海大学 | 基于奇异值分解cdkf的电力系统动态状态估计方法 |
CN107025609A (zh) * | 2017-03-16 | 2017-08-08 | 河海大学 | 基于奇异值分解cdkf的电力系统动态状态估计方法 |
CN107506332A (zh) * | 2017-07-25 | 2017-12-22 | 四川航天系统工程研究所 | Kalman滤波器快速实现方法 |
CN107506332B (zh) * | 2017-07-25 | 2021-01-29 | 四川航天系统工程研究所 | Kalman滤波器快速实现方法 |
CN107947761A (zh) * | 2017-12-18 | 2018-04-20 | 西安理工大学 | 基于最小均方四阶的变阈值比例更新自适应滤波算法 |
CN107947761B (zh) * | 2017-12-18 | 2021-09-10 | 西安理工大学 | 基于最小均方四阶的变阈值比例更新自适应滤波方法 |
CN108733627A (zh) * | 2018-04-30 | 2018-11-02 | 南京大学 | 一种正定矩阵Cholesky分解的FPGA实现方法 |
CN110109049A (zh) * | 2019-03-27 | 2019-08-09 | 北京邮电大学 | 用于大规模天线角度估计的无迹卡尔曼滤波方法及装置 |
CN110297127A (zh) * | 2019-05-28 | 2019-10-01 | 许昌许继软件技术有限公司 | 一种交流信号滤波方法及装置 |
CN112242145A (zh) * | 2019-07-17 | 2021-01-19 | 南京人工智能高等研究院有限公司 | 语音滤波方法、装置、介质和电子设备 |
CN112507597A (zh) * | 2020-11-02 | 2021-03-16 | 中国南方电网有限责任公司超高压输电公司广州局 | 基于多簇粒子滤波的多端柔性直流输电系统状态评估方法 |
CN113422593A (zh) * | 2021-07-05 | 2021-09-21 | 北京信息科技大学 | 滤波方法、滤波器、计算机可读存储介质、处理器和fpga |
CN113422593B (zh) * | 2021-07-05 | 2024-04-26 | 北京信息科技大学 | 滤波方法、滤波器、计算机可读存储介质、处理器和fpga |
CN114338664A (zh) * | 2021-11-30 | 2022-04-12 | 鹏城实验室 | 基于分布式架构获取目标状态的方法、装置及存储介质 |
CN114338664B (zh) * | 2021-11-30 | 2023-09-26 | 鹏城实验室 | 基于分布式架构获取目标状态的方法、装置及存储介质 |
Also Published As
Publication number | Publication date |
---|---|
CN101777887B (zh) | 2013-04-03 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN101777887B (zh) | 基于fpga的无迹卡尔曼滤波系统及并行实现方法 | |
Wang et al. | An adaptive Kalman filter estimating process noise covariance | |
Liu et al. | Off-grid DOA estimation with nonconvex regularization via joint sparse representation | |
Attia et al. | Static switched output feedback stabilization for linear discrete-time switched systems | |
US9047227B2 (en) | Operation circuit and method thereof | |
CN105608059A (zh) | 一种基于改进的按位替换法求矩阵三角分解的模块 | |
Tovkach et al. | Adaptive filtration of radio source movement parameters with complex use of sensor network data based on TDOA and RSS methods | |
Lingsch et al. | Vandermonde neural operators | |
Kermani et al. | A new stability analysis for discrete-time switched time-delay systems | |
Lingsch et al. | A structured matrix method for nonequispaced neural operators | |
CN113030945B (zh) | 一种基于线性序贯滤波的相控阵雷达目标跟踪方法 | |
Givon et al. | Variance reduction for particle filters of systems with time scale separation | |
Fang et al. | Fornasini-Marchesini model realization of MIMO radar image by elementary operation approach | |
Rao et al. | Efficient Computation of Extreme Excursion Probabilities for Dynamical Systems through Rice's Formula | |
Mitrouli | Estimating matrix functionals via extrapolation | |
Ng et al. | Sparse matrix computation for air quality forecast data assimilation | |
Rao et al. | Efficient computation of extreme excursion probabilities for dynamical systems | |
Konda et al. | Source identification of puff-based dispersion models using convex optimization | |
Arcucci et al. | Toward a preconditioned scalable 3DVAR for assimilating Sea Surface Temperature collected into the Caspian Sea. | |
Xiaochuan et al. | Grid evolution: An iterative reweighted algorithm for off-grid DOA estimation with gain/phase uncertainties | |
Chen et al. | On condition numbers for the weighted Moore-Penrose inverse and the weighted least squares problem involving Kronecker products | |
Lv et al. | Udut continuous-discrete unscented kalman filtering | |
Ruhe | Rational Krylov for real pencils with complex eigenvalues | |
Zhang et al. | Explicit Solution For Queue Length Distribution Of M/T-Sph/1 Queue | |
Sunarto et al. | Half-sweep Gauss-Seidel iteration applied to time-fractional diffusion equations |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
C14 | Grant of patent or utility model | ||
GR01 | Patent grant | ||
CF01 | Termination of patent right due to non-payment of annual fee | ||
CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20130403 Termination date: 20190108 |