CN104038180A - 基于高阶矩匹配的无迹卡尔曼滤波器的多项式方法 - Google Patents
基于高阶矩匹配的无迹卡尔曼滤波器的多项式方法 Download PDFInfo
- Publication number
- CN104038180A CN104038180A CN201410219994.6A CN201410219994A CN104038180A CN 104038180 A CN104038180 A CN 104038180A CN 201410219994 A CN201410219994 A CN 201410219994A CN 104038180 A CN104038180 A CN 104038180A
- Authority
- CN
- China
- Prior art keywords
- stochastic variable
- average
- state
- sample
- covariance
- 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
Landscapes
- Complex Calculations (AREA)
- Management, Administration, Business Operations System, And Electronic Commerce (AREA)
Abstract
本发明公开了一种基于高阶矩匹配的无迹卡尔曼滤波器的多项式方法,属于非线性滤波技术领域。包括以下步骤:建立非线性系统的状态方程和测量方程;确定初始状态的随机分布特征,包括其均值、协方差以及高阶矩,噪声的分布特征,以及初始测量值;基于上一时刻的状态估计和状态方程,使用多层无迹变换计算一步状态预测的随机变量的分布特征;基于步骤三的状态预测和测量方程,使用MUT计算状态预测的量测的分布特征;使用卡尔曼增益融合状态预测以及测量数据计算最优状态的分布特征,完成非线性系统一步估计任务。该方法用于解决非线性滤波器在实际应用过程中的精度和计算稳定性问题,结合现有采样策略,使用高阶矩和多重的对称采样来提高精度。
Description
技术领域
本发明属于非线性滤波、数字信号处理、目标定位跟踪等信息融合技术领域,涉及一种基于高阶矩匹配的无迹卡尔曼滤波器的多项式方法。
背景技术
目前,在飞行器导航、目标跟踪及工业控制等领域。几乎所有的现实系统都是非线性的。例如:在目标定位跟踪过程中,利用雷达对空中目标进行观测时,雷达能够获得空中目标相对自身的方位角,但该观测含有噪声,观测方程中雷达的方位观测量是待估计目标位置参数的非线性函数,不能直接利用线性滤波方法获取目标的运动状态,其本质为非线性滤波问题,是目标跟踪、数字信号处理等研究领域的共同难题。
针对非线性滤波问题,常采用两类滤波方法:一类是对非线性函数进行线性化近似,对高阶项采用忽略或逼近的措施,其中最广泛使用的是扩展卡尔曼滤波器(ExtendedKalmanFilter,EKF),其基本思路是对非线性函数的Taylor展开式进行一阶线性化截断,从而将非线性问题转化为线性;另一类是采用采样方法近似非线性分布,常用的有粒子滤波器(ParticleFiler,PF)和无迹卡尔曼滤波器(UnscentedKalmanFilter,UKF),其基本原理是使用样本点结合其权重逼近非线性函数的随机变量的分布。
与无迹卡尔曼滤波器相对比,EKF具有以下三点不足:(1)当非线性函数Taylor展开式的高阶项无法忽略时,线性化会使系统产生较大的误差,甚至于滤波器难以稳定;(2)在许多实际问题中很难得到非线性函数的雅克比矩阵,甚至不存在;(3)EKF需要求导,所以必须清楚了解非线性函数的具体形式,无法做到黑盒封装,从而难以模块化应用。目前,虽然对EKF有众多的改进方法,如高阶截断EKF,迭代EKF等,但这些缺陷仍然难以克服。研究表明UKF给出的估计结果比EKF跟准确,能达到更高阶的计算精度,且计算量与EFK同阶次。与无迹卡尔曼滤波器相对比,粒子滤波器采用的随机样本点,需要的数量非常大,而且其样本点数量随着问题的维数呈几何级数地增长,其计算代价十分昂贵。UKF方法采取的是确定性的与其分布密切相关的典型样本点,其数量相对大大减少,一般维数为n的随机分布只需要2n+1个样本点,甚至只需n+1个样本点即可达到匹配二阶矩的精度。
常见的,无迹卡尔曼滤波器的主流采样策略主要分为四种:(1)对称采样,其特征是样本点关于均值点是对称分布的;(2)单形采样,其特征是采样sigma点分布不是关于均值点对称的,样本点极少,只比维数多一点;(3)比例修正,其特征是对已采样样本点进行比例缩放;(4)高阶抽样,其特征是采用各种策略匹配分布的高阶矩。
目前,人们在目标跟踪领域内应用的UKF都是二阶UKF方法,对于高斯非线性系统,二阶UKF的估计精度,只能达到非线性函数的三次泰勒展开,精度有限,稳定性差,而实际应用中,迫切需要兼顾精度、稳定性与计算效率的采样策略。
发明内容
有鉴于此,本发明的目的在于提供一种基于高阶矩匹配的无迹卡尔曼滤波器的多项式方法,该方法用于解决非线性滤波器在实际应用过程中的精度和计算稳定性问题,结合现有采样策略,使用高阶矩和多重的对称采样来提高精度,同时,采用比例修正使得所得的样本及其权重对给定随机变量形成一个完全概率分布的逼近,提高稳定性。
为达到上述目的,本发明提供如下技术方案:
一种基于高阶矩匹配的无迹卡尔曼滤波器的多项式方法,包括以下步骤:步骤一:根据实际工程应用,建立非线性系统的状态方程和测量方程;步骤二:初始状态:确定系统初始状态,即初始状态的随机分布特征,包括其均值、协方差以及高阶矩,噪声的分布特征,以及初始测量值;步骤三:一步状态预测:基于上一时刻的状态估计和状态方程,使用多层无迹变换(Multi-LayerUnscentedTransformation,MUT)计算一步状态预测的随机变量的分布特征;步骤四:一步量测预测:基于步骤三的状态预测和测量方程,使用多层无迹变换计算状态预测的量测的分布特征;步骤五:状态滤波更新:使用卡尔曼增益(KalmanGain)融合状态预测以及测量数据计算最优状态的分布特征,完成非线性系统一步估计任务,并迭代回到步骤三,进行下一时刻估计任务。
进一步,在步骤一中,根据实际工程应用,建立非线性系统的状态方程和测量方程如下:
其中,k表示第k步,xk为第k步的n维状态向量,zk为第k步的m维量测向量,f(·)及h(·)为非线性函数,wk-1为n维随机系统噪声,vk为m维的随机测量噪声,其中系统噪声服从均值为零,方差为Qk的高斯分布,测量噪声服从均值为零,方差为Rk的高斯分布,并且测量噪声和系统噪声互不相关,函数f(xk-1)是系统状态变换的数学模型,函数h(xk)对应系统状态测量的数学模型。
进一步,在步骤三中,所述的基于上一时刻的状态估计和状态方程,使用多层无迹变换
计算一步状态预测的随机变量的分布特征具体分为以下4个步骤:
1)根据上一步的状态估计随机变量以及噪声随机变量(xk-1,wk-1)的分布特征,即均值、协方差以及高阶矩和该随机变量的分布假设,估计该随机变量的密度函数;设(xk-1,wk-1)是一个高斯分布,其已知均值是μ,协方差是σ,则其密度函数是高斯分布N(μ,σ)对应的密度函数;
2)根据所需匹配的高阶矩,确定样本点的分层,基于该分层,使用密度函数计算样本点的权重;即:需要匹配均值向量、协方差直至2l阶的边缘中心矩,则需要l+1层样本点,为此,选择一个实值函数L(·),实值函数的选择应该满足随机变量的分布特征;设(xk-1,wk-1)是一个高斯分布N(μ,σ),则L(xk-1,wk-1)=((xk-1,wk-1)-μ)σ-1((xk-1,wk-1)-μ),以及一个正的序列0<r1<r2<…<rl,则根据以下公式计算权重:
其中,1<j≤l,ρ(·)为随机变量(xk-1,wk-1)的联合概率密度;
3)为(xk-1,wk-1)选择预样本,其中,均值点μ是最里层预样本记为第零层,紧邻均值点的第一层预样本在集合{(xk-1,wk-1)|L(xk-1,wk-1)=r1}中选择2n个正交点,为了对称性,再添加它们关于均值的对称点,记正交点、对称点为其中i=1,…,4n;类似地,对1<j≤l,第j层的预样本在集合{(xk-1,wk-1)|L(xk-1,wk-1)=rj}中选择2n个正交点以及它们关于均值的对称点,记正交点、对称点为i=1,…,4n;为了匹配高阶距,对每一层上的预选样本到均值的距离加一个调节系数cj得到sigma点i=1,…,4n,1≤j≤l;通过以下公式匹配高阶矩:
其中, 对1<j≤l, Pxw,k-1|k-1是xk-1在第k-1步的最有估计与状态噪声wk-1的联合协方差,代表xk-1的第β个随机变量的α阶距,给定向量x,(x)β表示x的第β个变量;进一步解该公式得到关于c1,c2,…cl的多项式系统方程组,从而得到了sigma点
4)随机变量状态方程变换的分布特征计算:根据变换函数,计算sigma点经过状态方程变换后的变换sigma点公式如下:
然后,根据以下公式计算变换随机变量xk|k-1=f(xk-1)+wk-1的均值向量:
根据以下公式计算变换随机变量xk|k-1的协方差:
根据以下公式计算变换随机变量的高阶矩:
当随机变量xk|k-1服从高斯分布时,均值与协方差已经完全确定xk|k-1的密度函数,此时可以通过密度函数计算高阶矩,从而可以省略该计算公式。
进一步,在步骤四中,所述的基于步骤三的状态预测和测量方程,使用多层无迹变换计算状态预测的量测的分布特征分为以下4个步骤:
1)根据上一步的状态估计随机变量以及噪声随机变量(xk|k-1,vk)的分布特征,即均值、协方差以及高阶矩和该随机变量的分布假设,估计该随机变量的密度函数;设(xk|k-1,vk)是一个高斯分布,其已知均值是μ,协方差是σ,则其密度函数是高斯分布N(μ,σ)对应的密度函数;
2)根据所需匹配的高阶矩,确定样本点的分层,基于该分层,使用密度函数计算样本点的权重;即:需要匹配均值向量、协方差直至2l阶的边缘中心矩,则需要l+1层样本点;为此,选择一个实值函数L(·),实值函数的选择应该满足随机变量的分布特征;设(xk|k-1,vk)是一个高斯分布N(μ,σ),则L(xk|k-1,vk)=((xk|k-1,vk)-μ)σ-1((xk|k-1,vk)-μ),以及一个正的序列0<r1<r2<…<rl,则根据以下公式计算权重:
其中,1<j≤l,ρ(·)为随机变量(xk|k-1,vk)的联合概率密度;
3)为(xk|k-1,vk)选择预样本,其中,均值点μ是最里层预样本记为第零层,紧邻均值点的第一层预样本在集合{(xk|k-1,vk)|L(xk|k-1,vk)=r1}中选择m+n个正交点,为了对称性,再添加它们关于均值的对称点,记正交点、对称点为其中i=1,…,2(m+n);类似地,对1<j≤l,第j层的预样本在集合{(xk|k-1,vk)|L(xk|k-1,vk)=rj}中选择m+n个正交点以及它们关于均值的对称点,记正交点、对称点为i=1,…,2(m+n);为了匹配高阶距,对每一层上的预选样本到均值的距离加一个调节系数cj得到sigma点i=1,…,2(m+n),1≤j≤l;通过以下公式匹配高阶矩:
其中, 对1<j≤l, Pvx,k|k-1是xk|k-1在步骤三的变换随机变量与量测噪声vk的联合协方差,代表xk|k-1的第β个随机变量的α阶距,给定向量x,(x)β表示x的第β个变量;进一步解公式所得的关于c1,c2,…cl的多项式系统方程组,从而得到了sigma点
4)随机变量量测方程变换的分布特征计算:根据变换函数,计算sigma点经过量测方程变换后的变换sigma点公式如下:
然后,根据以下公式计算变换随机变量zk=h(xk)+vk的均值向量:
根据以下公式计算变换随机变量xk|k-1的协方差:
根据以下公式计算变换随机变量的高阶矩:
基于sigma点截取关于xk-1和xk|k-1的取样和并根据以下公式计算xk|k-1和zk|k-1互协方差:
当随机变量zk|k-1服从高斯分布时,均值与协方差已经完全确定zk|k-1的密度函数,此时可以通过密度函数计算高阶矩。
进一步,在步骤五中,所述的使用卡尔曼增益融合状态预测以及测量数据计算最优状态的分布特征,完成非线性系统一步估计任务的具体过程为:
根据以下公式计算xk的均值:
式中: 为卡尔曼增益;
根据以下公式计算xk的最优估计的协方差Px,k|k及高阶矩
其中,Wi j是步骤四中的权重,yc是实际测量数据值。
本发明的有益效果在于:本发明充分利用随机分布的几何特征,使用比例修正的思想把正交对称样本以及多重采样的结构结合起来,通过匹配更多的高阶矩,使得近似逼近精度显著提高,同时,该方法通过引进预样本计算样本权重,使得所有权值都在[0,1]区间内,使用这类权重和预样本通过比例修正得到一个关于比例系数的多项式方程组,最终所得的样本及其权重对给定随机变量形成一个完全概率分布的逼近,其效应在于极大的提高了无迹卡尔曼性滤波器的稳定性。
附图说明
为了使本发明的目的、技术方案和有益效果更加清楚,本发明提供如下附图进行说明:
图1为本发明所述方法的流程示意图;
图2为本发明所述方法的具体步骤示意图;
图3为基于本发明提供的MUKF方法与基于UKF方法的目标跟踪应用中对目标x坐标轴方向位置估计的均方误差曲线图;
图4为基于图本发明提供的MUKF方法与基于UKF方法的目标跟踪应用中对目标y坐标轴方向位置估计的均方误差曲线图。
具体实施方式
下面将结合附图,对本发明的优选实施例进行详细的描述。
图1为本发明所述方法的流程示意图,图2为本发明所述方法的具体步骤示意图,如图所示,本方法的具体步骤如下:
步骤一:依据纯方位目标跟踪问题,建立如下描述目标跟踪系统的状态方程和观测方程:
其中,第k步的状态方程中,xk=[x1,k x2,k]T=[x y]T,表示x坐标轴和y坐标轴平面内(笛卡尔坐标系)目标的位置; 矩阵 表征了目标x方向的速度为0.9米/秒,y方向的速度为1米/秒。随机系统噪声向量wk-1服从均值为零,方差为Qk-1的高斯分布, 随机量测噪声向量vk服从均值为0,方差为Rk的高斯分布,Rk=0.025rad2。
步骤二:初始状态:确定系统初始状态,即初始状态的随机分布特征,包括其均值、协方差以及高阶矩:
初始状态:x0=[20m 5m]T;初始协方差矩阵P0=[0.1m2 0m2;0m2 0.1m2]T,P0表征了系统初始位置的不确定性;高阶距:
步骤三:一步状态预测:基于上一时刻的状态估计和状态方程,使用MUT计算一步状态预测的随机变量的分布特征。
具体为,根据k-1时刻的状态估计随机变量以及噪声随机变量(xk-1,wk-1)的分布特征,假设xk-1是一个高斯分布,其已知均值是协方差是σ=Px,k-1|k-1,特别地,当k=1时,σ=P0,则其密度函数是高斯分布对应的密度函数。
根据所需要匹配均值向量、协方差直至6阶的边缘中心矩,则需要4层样本点。则选择一个实值函数L(·),L(xk-1,wk-1)=((xk-1,wk-1)-μ)σ-1((xk-1,wk-1)-μ);以及取一个正的序列0<r1=1<r2=2<r3=3,则根据公式:
计算权重,其中,1<j≤3。
为(xk-1,wk-1)选择预样本,其均值点μ是最里层预样本其中,紧邻均值点的第一层预样本在集合{(xk-1,wk-1)|L(xk-1,wk-1)=1}中选择4个正交点,为了对称性,再添加它们关于均值的对称点,记正交点、对称点为其中i=1,…,8;类似地,对1<j≤3,第j层的预样本在集合{(xk-1,wk-1)|L(xk-1,wk-1)=j}中选择4个正交点以及它们关于均值的对称点,记正交点、对称点为i=1,…,8。为了匹配高阶距,对每一层上的预选样本到均值的距离加一个调节系数cj得到sigma点i=1,…,8,1≤j≤3。通过公式匹配高阶矩,其中,对1<j≤3,Pxw,k-1|k-1是xk-1在第k-1步的最有估计与状态噪声wk-1的联合协方差,代表xk-1的第β个随机变量的α阶距,给定向量x,(x)β表示x的第β个变量。进一步解公式所得的关于c1,c2,…cl的多项式系统方程组,从而得到了sigma点
随机变量状态方程变换的分布特征计算:根据变换函数,根据公式:
计算sigma点经过状态方程变换后的变换sigma点然后,根据公式:
计算变换随机变量xk|k-1=f(xk-1)+wk-1的均值向量,根据公式:
计算变换随机变量xk|k-1的协方差,根据公式:
计算变换随机变量的高阶矩。
步骤四:一步量测预测:基于步骤三的状态预测和测量方程,使用MUT计算状态预测的量测的分布特征。
具体为,根据步骤三的状态估计随机变量以及噪声随机变量(xk|k-1,vk)的分布特征,假设xk|k-1是一个高斯分布,其已知均值是协方差是σ=Px,k|k-1,则其密度函数是高斯分布对应的密度函数。
根据所需要匹配均值向量、协方差直至6阶的边缘中心矩,则需要4层样本点。为此,选择一个实值函数L(·),实值函数的选择应该满足随机变量的分布特征。假设(xk|k-1,vk)是一个高斯分布N(μ,σ),则L(xk|k-1,vk)=((xk|k-1,vk)-μ)σ-1((xk|k-1,vk)-μ)。以及取一个正的序列0<r1=1<r2=2<r3=3,则根据公式:
计算权重,其中,1<j≤3。
为(xk|k-1,vk)选择预样本,其均值点μ是最里层预样本其中,紧邻均值点的第一层预样本在集合{(xk|k-1,vk)|L(xk|k-1,vk)=1}中选择3个正交点,为了对称性,再添加它们关于均值的对称点,这些点记为其中i=1,…,6;类似地,对1<j≤3,第j层的预样本在集合{(xk|k-1,vk)|L(xk|k-1,vk)=j}中选择3个正交点以及它们关于均值的对称点,记这些点为i=1,…,6。为了匹配高阶距,对每一层上的预选样本到均值的距离加一个调节系数cj得到sigma点i=1,…,6,1≤j≤3。通过公式匹配高阶矩,其中,对1<j≤3,Pvx,k|k-1是xk|k-1在步骤三的变换随机变量与量测噪声vk的联合协方差,代表xk|k-1的第β个随机变量的α阶距,给定向量x,(x)β表示x的第β个变量。进一步解公式所得的关于c1,c2,…cl的多项式系统方程组,从而得到了sigma点
随机变量量测方程变换的分布特征计算:根据变换函数,根据公式:
计算sigma点经过量测方程变换后的变换sigma点然后,根据公式:
计算变换随机变量zk=h(xk)+vk的均值向量,根据公式:
计算变换随机变量xk|k-1的协方差,根据公式:
计算变换随机变量的高阶矩。
进一步,基于sigma点截取关于xk-1和xk|k-1的取样和我们根据公式:
计算xk|k-1和zk|k-1互协方差。
步骤五:状态滤波更新:使用卡尔曼增益(KalmanGain)融合状态预测以及测量数据计算最优状态的分布特征,完成非线性系统一步估计任务,并迭代回到步骤三,进行下一时刻估计任务。
具体为:根据公式:
计算xk的均值,根据以下公式计算xk的最优估计的协方差Px,k|k及高阶矩
然后判断迭代是否结束,如不结束,那么将随机变量xk带入步骤三作为上一步的状态估计,进行第k+1步的计算。
实施过程:仿真过程中采用如下定义的均方误差性能指标,比较各种滤波方法:
对目标位置估计的均方误差越小则表明目标跟踪精度越高,效果越好,其中,仿真时间为50步。
实施效果:图3、图4分别给出了纯方向目标跟踪过程中,基于本发明提供的MUKF方法与基于UKF方法的目标跟踪应用中对目标x坐标轴方向位置估计的均方误差曲线,以及基于本发明提供的MUKF方法与基于UKF方法的目标跟踪应用中对目标y坐标轴方向位置估计的均方误差曲线。显而易见,MUKF方法的均方误差不论在x坐标轴方向还是y坐标轴方向所预测精度、以及稳定性都远高于UKF方法。
最后说明的是,以上优选实施例仅用以说明本发明的技术方案而非限制,尽管通过上述优选实施例已经对本发明进行了详细的描述,但本领域技术人员应当理解,可以在形式上和细节上对其作出各种各样的改变,而不偏离本发明权利要求书所限定的范围。
Claims (5)
1.一种基于高阶矩匹配的无迹卡尔曼滤波器的多项式方法,其特征在于:包括以下步骤:
步骤一:根据实际工程应用,建立非线性系统的状态方程和测量方程;
步骤二:初始状态:确定系统初始状态,即初始状态的随机分布特征,包括其均值、协方差以及高阶矩,噪声的分布特征,以及初始测量值;
步骤三:一步状态预测:基于上一时刻的状态估计和状态方程,使用多层无迹变换计算一步状态预测的随机变量的分布特征;
步骤四:一步量测预测:基于步骤三的状态预测和测量方程,使用多层无迹变换计算状态预测的量测的分布特征;
步骤五:状态滤波更新:使用卡尔曼增益融合状态预测以及测量数据计算最优状态的分布特征,完成非线性系统一步估计任务,并迭代回到步骤三,进行下一时刻估计任务。
2.根据权利要求1所述的一种基于高阶矩匹配的无迹卡尔曼滤波器的多项式方法,其特征在于:在步骤一中,根据实际工程应用,建立非线性系统的状态方程和测量方程如下:
其中,k表示第k步,xk为第k步的n维状态向量,zk为第k步的m维量测向量,f(·)及h(·)为非线性函数,wk-1为n维随机系统噪声,vk为m维的随机测量噪声,其中系统噪声服从均值为零,方差为Qk的高斯分布,测量噪声服从均值为零,方差为Rk的高斯分布,并且测量噪声和系统噪声互不相关,函数f(xk-1)是系统状态变换的数学模型,函数h(xk)对应系统状态测量的数学模型。
3.根据权利要求2所述的一种基于高阶矩匹配的无迹卡尔曼滤波器的多项式方法,其特征在于:在步骤三中,所述的基于上一时刻的状态估计和状态方程,使用多层无迹变换计算一步状态预测的随机变量的分布特征具体分为以下4个步骤:
1)根据上一步的状态估计随机变量以及噪声随机变量(xk-1,wk-1)的分布特征,即均值、协方差以及高阶矩和该随机变量的分布假设,估计该随机变量的密度函数;设(xk-1,wk-1)是一个高斯分布,其已知均值是μ,协方差是σ,则其密度函数是高斯分布N(μ,σ)对应的密度函数;
2)根据所需匹配的高阶矩,确定样本点的分层,基于该分层,使用密度函数计算样本点的权重;即:需要匹配均值向量、协方差直至2l阶的边缘中心矩,则需要l+1层样本点,为此,选择一个实值函数L(·),实值函数的选择应该满足随机变量的分布特征;设(xk-1,wk-1)是一个高斯分布N(μ,σ),则
L(xk-1,wk-1)=((xk-1,wk-1)-μ)σ-1((xk-1,wk-1)-μ),以及一个正的序列0<r1<r2<…<rl,则根据以下公式计算权重:
其中,1<j≤l,ρ(·)为随机变量(xk-1,wk-1)的联合概率密度;
3)为(xk-1,wk-1)选择预样本,其中,均值点μ是最里层预样本记为第零层,紧邻均值点的第一层预样本在集合{(xk-1,wk-1)|L(xk-1,wk-1)=r1}中选择2n个正交点,为了对称性,再添加它们关于均值的对称点,记正交点、对称点为其中i=1,…,4n;类似地,对1<j≤l,第j层的预样本在集合
{(xk-1,wk-1)|L(xk-1,wk-1)=rj}中选择2n个正交点以及它们关于均值的对称点,记正交点、对称点为i=1,…,4n;
为了匹配高阶距,对每一层上的预选样本到均值的距离加一个调节系数cj得到sigma点i=1,…,4n,1≤j≤l;通过以下公式匹配高阶矩:
其中, 对1<j≤l, Pxw,k-1|k-1是xk-1在第k-1步的最有估计与状态噪声wk-1的联合协方差,代表xk-1的第β个随机变量的α阶距,给定向量x,(x)β表示x的第β个变量;进一步解该公式得到关于c1,c2,…cl的多项式系统方程组,从而得到了sigma点
4)随机变量状态方程变换的分布特征计算:根据变换函数,计算sigma点经过状态方程变换后的变换sigma点公式如下:
然后,根据以下公式计算变换随机变量xk|k-1=f(xk-1)+wk-1的均值向量:
根据以下公式计算变换随机变量xk|k-1的协方差:
根据以下公式计算变换随机变量的高阶矩:
4.根据权利要求3所述的一种基于高阶矩匹配的无迹卡尔曼滤波器的多项式方法,其特征在于:在步骤四中,所述的基于步骤三的状态预测和测量方程,使用多层无迹变换计算状态预测的量测的分布特征分为以下4个步骤:
1)根据上一步的状态估计随机变量以及噪声随机变量(xk|k-1,vk)的分布特征,即均值、协方差以及高阶矩和该随机变量的分布假设,估计该随机变量的密度函数;设(xk|k-1,vk)是一个高斯分布,其已知均值是μ,协方差是σ,则其密度函数是高斯分布N(μ,σ)对应的密度函数;
2)根据所需匹配的高阶矩,确定样本点的分层,基于该分层,使用密度函数计算样本点的权重;即:需要匹配均值向量、协方差直至2l阶的边缘中心矩,则需要l+1层样本点;为此,选择一个实值函数L(·),实值函数的选择应该满足随机变量的分布特征;设(xk|k-1,vk)是一个高斯分布N(μ,σ),则
L(xk|k-1,vk)=((xk|k-1,vk)-μ)σ-1((xk|k-1,vk)-μ),以及一个正的序列0<r1<r2<…<rl,则根据以下公式计算权重:
其中,1<j≤l,ρ(·)为随机变量(xk|k-1,vk)的联合概率密度;
3)为(xk|k-1,vk)选择预样本,其中,均值点μ是最里层预样本记为第零层,紧邻均值点的第一层预样本在集合{(xk|k-1,vk)|L(xk|k-1,vk)=r1}中选择m+n个正交点,为了对称性,再添加它们关于均值的对称点,记正交点、对称点为其中i=1,…,2(m+n);类似地,对1<j≤l,第j层的预样本在集合{(xk|k-1,vk)|L(xk|k-1,vk)=rj}中选择m+n个正交点以及它们关于均值的对称点,记正交点、对称点为i=1,…,2(m+n);为了匹配高阶距,对每一层上的预选样本到均值的距离加一个调节系数cj得到sigma点i=1,…,2(m+n),1≤j≤l;通过以下公式匹配高阶矩:
其中, 对1<j≤l, Pvx,k|k-1是xk|k-1在步骤三的变换随机变量与量测噪声vk的联合协方差,代表xk|k-1的第β个随机变量的α阶距,给定向量x,(x)β表示x的第β个变量;进一步解公式所得的关于c1,c2,…cl的多项式系统方程组,从而得到了sigma点
)随机变量量测方程变换的分布特征计算:根据变换函数,计算sigma点经过量测方程变换后的变换sigma点公式如下:
然后,根据以下公式计算变换随机变量zk=h(xk)+vk的均值向量:
根据以下公式计算变换随机变量xk|k-1的协方差:
根据以下公式计算变换随机变量的高阶矩:
基于sigma点和截取关于xk-1和xk|k-1的取样和并根据以下公式计算xk|k-1和zk|k-1互协方差:
5.根据权利要求4所述的一种基于高阶矩匹配的无迹卡尔曼滤波器的多项式方法,其特征在于:在步骤五中,所述的使用卡尔曼增益融合状态预测以及测量数据计算最优状态的分布特征,完成非线性系统一步估计任务的具体过程为:
根据以下公式计算xk的均值:
式中: 为卡尔曼增益;
根据以下公式计算xk的最优估计的协方差Px,k|k及高阶矩
其中,Wi j是步骤四中的权重,yc是实际测量数据值。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201410219994.6A CN104038180B (zh) | 2014-05-22 | 基于高阶矩匹配的无迹卡尔曼滤波器的多项式方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201410219994.6A CN104038180B (zh) | 2014-05-22 | 基于高阶矩匹配的无迹卡尔曼滤波器的多项式方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN104038180A true CN104038180A (zh) | 2014-09-10 |
CN104038180B CN104038180B (zh) | 2016-11-30 |
Family
ID=
Cited By (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105703740A (zh) * | 2016-01-13 | 2016-06-22 | 中国科学院重庆绿色智能技术研究院 | 基于多层重要性采样的高斯滤波方法和高斯滤波器 |
CN105842298A (zh) * | 2016-05-07 | 2016-08-10 | 天津大学 | 两相流含水率的自适应估计方法 |
CN108645415A (zh) * | 2018-08-03 | 2018-10-12 | 上海海事大学 | 一种船舶航迹预测方法 |
CN108917768A (zh) * | 2018-07-04 | 2018-11-30 | 上海应用技术大学 | 无人机定位导航方法和系统 |
CN111238484A (zh) * | 2020-02-28 | 2020-06-05 | 上海航天控制技术研究所 | 一种基于球形无迹变换的环火轨道自主导航方法 |
CN111756353A (zh) * | 2020-06-12 | 2020-10-09 | 杭州电子科技大学 | 一种基于非线性融合滤波的液位仪噪声优化方法 |
CN113534128A (zh) * | 2020-10-21 | 2021-10-22 | 中国人民解放军空军预警学院 | 一种机载预警雷达海面机动舰船目标自适应跟踪方法 |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101726295A (zh) * | 2008-10-24 | 2010-06-09 | 中国科学院自动化研究所 | 考虑加速度补偿和基于无迹卡尔曼滤波的惯性位姿跟踪方法 |
CN101777887A (zh) * | 2010-01-08 | 2010-07-14 | 西安电子科技大学 | 基于fpga的无迹卡尔曼滤波系统及并行实现方法 |
CN103278813A (zh) * | 2013-05-02 | 2013-09-04 | 哈尔滨工程大学 | 一种基于高阶无迹卡尔曼滤波的状态估计方法 |
CN103336525A (zh) * | 2013-06-18 | 2013-10-02 | 哈尔滨工程大学 | 随机系统高权值便捷ukf滤波方法 |
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101726295A (zh) * | 2008-10-24 | 2010-06-09 | 中国科学院自动化研究所 | 考虑加速度补偿和基于无迹卡尔曼滤波的惯性位姿跟踪方法 |
CN101777887A (zh) * | 2010-01-08 | 2010-07-14 | 西安电子科技大学 | 基于fpga的无迹卡尔曼滤波系统及并行实现方法 |
CN103278813A (zh) * | 2013-05-02 | 2013-09-04 | 哈尔滨工程大学 | 一种基于高阶无迹卡尔曼滤波的状态估计方法 |
CN103336525A (zh) * | 2013-06-18 | 2013-10-02 | 哈尔滨工程大学 | 随机系统高权值便捷ukf滤波方法 |
Cited By (12)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105703740A (zh) * | 2016-01-13 | 2016-06-22 | 中国科学院重庆绿色智能技术研究院 | 基于多层重要性采样的高斯滤波方法和高斯滤波器 |
CN105703740B (zh) * | 2016-01-13 | 2019-03-22 | 中国科学院重庆绿色智能技术研究院 | 基于多层重要性采样的高斯滤波方法和高斯滤波器 |
CN105842298A (zh) * | 2016-05-07 | 2016-08-10 | 天津大学 | 两相流含水率的自适应估计方法 |
CN105842298B (zh) * | 2016-05-07 | 2018-08-31 | 天津大学 | 两相流含水率的自适应估计方法 |
CN108917768A (zh) * | 2018-07-04 | 2018-11-30 | 上海应用技术大学 | 无人机定位导航方法和系统 |
CN108917768B (zh) * | 2018-07-04 | 2022-03-01 | 上海应用技术大学 | 无人机定位导航方法和系统 |
CN108645415A (zh) * | 2018-08-03 | 2018-10-12 | 上海海事大学 | 一种船舶航迹预测方法 |
CN111238484A (zh) * | 2020-02-28 | 2020-06-05 | 上海航天控制技术研究所 | 一种基于球形无迹变换的环火轨道自主导航方法 |
CN111238484B (zh) * | 2020-02-28 | 2022-04-12 | 上海航天控制技术研究所 | 一种基于球形无迹变换的环火轨道自主导航方法 |
CN111756353A (zh) * | 2020-06-12 | 2020-10-09 | 杭州电子科技大学 | 一种基于非线性融合滤波的液位仪噪声优化方法 |
CN111756353B (zh) * | 2020-06-12 | 2024-04-16 | 杭州电子科技大学 | 一种基于非线性融合滤波的液位仪噪声优化方法 |
CN113534128A (zh) * | 2020-10-21 | 2021-10-22 | 中国人民解放军空军预警学院 | 一种机载预警雷达海面机动舰船目标自适应跟踪方法 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
Kurz et al. | Recursive nonlinear filtering for angular data based on circular distributions | |
CN106599368B (zh) | 基于改进粒子提议分布和自适应粒子重采样的FastSLAM方法 | |
CN106487358B (zh) | 一种机动目标转弯跟踪方法 | |
CN106767780B (zh) | 基于Chebyshev多项式插值逼近的扩展椭球集员滤波方法 | |
CN103278813B (zh) | 一种基于高阶无迹卡尔曼滤波的状态估计方法 | |
CN103925925B (zh) | 一种用于多点定位系统的实时高精度位置解算方法 | |
CN106683122A (zh) | 一种基于高斯混合模型和变分贝叶斯的粒子滤波方法 | |
CN107315171B (zh) | 一种雷达组网目标状态与系统误差联合估计算法 | |
CN109186601A (zh) | 一种基于自适应无迹卡尔曼滤波的激光slam算法 | |
CN104330083A (zh) | 基于平方根无迹卡尔曼滤波的多机器人协同定位算法 | |
CN103776453A (zh) | 一种多模型水下航行器组合导航滤波方法 | |
CN103955600B (zh) | 一种目标跟踪方法及截断积分卡尔曼滤波方法、装置 | |
CN105954742B (zh) | 一种球坐标系下带多普勒观测的雷达目标跟踪方法 | |
CN105785358B (zh) | 一种方向余弦坐标系下带多普勒量测的雷达目标跟踪方法 | |
CN104363649A (zh) | 带有约束条件的ukf的wsn节点定位方法 | |
CN107707220A (zh) | 一种应用在gnss/ins中的改进型ckf方法 | |
CN111964667B (zh) | 一种基于粒子滤波算法的地磁_ins组合导航方法 | |
CN104166989A (zh) | 一种用于二维激光雷达点云匹配的快速icp方法 | |
CN105353351A (zh) | 一种基于多信标到达时间差改进型定位方法 | |
CN109919233B (zh) | 一种基于数据融合的跟踪滤波方法 | |
CN103296995B (zh) | 任意维高阶(≥4阶)无味变换与无味卡尔曼滤波方法 | |
CN105021199A (zh) | 基于ls 的多模型自适应状态估计方法及系统 | |
CN109115228A (zh) | 一种基于加权最小二乘容积卡尔曼滤波的目标定位方法 | |
CN107621632A (zh) | 用于nshv跟踪滤波的自适应滤波方法及系统 | |
CN102707268A (zh) | 机动雷达组网批处理式误差配准器 |
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 |
Granted publication date: 20161130 Termination date: 20210522 |
|
CF01 | Termination of patent right due to non-payment of annual fee |