CN108303095B - 适用于非高斯***的鲁棒容积目标协同定位方法 - Google Patents
适用于非高斯***的鲁棒容积目标协同定位方法 Download PDFInfo
- Publication number
- CN108303095B CN108303095B CN201810106433.3A CN201810106433A CN108303095B CN 108303095 B CN108303095 B CN 108303095B CN 201810106433 A CN201810106433 A CN 201810106433A CN 108303095 B CN108303095 B CN 108303095B
- Authority
- CN
- China
- Prior art keywords
- sensor
- value
- target
- maneuvering target
- measurement
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 44
- 238000001914 filtration Methods 0.000 title claims abstract description 12
- 230000004807 localization Effects 0.000 title abstract description 3
- 238000005259 measurement Methods 0.000 claims abstract description 54
- 238000012417 linear regression Methods 0.000 claims abstract description 12
- 238000013179 statistical model Methods 0.000 claims abstract description 10
- 230000001133 acceleration Effects 0.000 claims description 33
- 239000011159 matrix material Substances 0.000 claims description 30
- 238000007476 Maximum Likelihood Methods 0.000 claims description 9
- 230000008569 process Effects 0.000 claims description 9
- 230000007704 transition Effects 0.000 claims description 6
- 238000006243 chemical reaction Methods 0.000 claims description 4
- 238000005070 sampling Methods 0.000 claims description 4
- 238000009795 derivation Methods 0.000 claims description 2
- 239000000203 mixture Substances 0.000 claims description 2
- 230000010354 integration Effects 0.000 description 12
- 238000004088 simulation Methods 0.000 description 5
- 238000009826 distribution Methods 0.000 description 3
- 238000013459 approach Methods 0.000 description 2
- 238000004422 calculation algorithm Methods 0.000 description 2
- 238000004364 calculation method Methods 0.000 description 2
- 238000007796 conventional method Methods 0.000 description 2
- 238000001514 detection method Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 230000004044 response Effects 0.000 description 2
- 230000008859 change Effects 0.000 description 1
- 238000000354 decomposition reaction Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000012886 linear function Methods 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
Abstract
适用于非高斯***的鲁棒容积目标协同定位方法,属于非合作目标协同定位技术领域。本发明是为了解决现有协同定位方法认为传感器的测量噪声符合高斯白噪声,造成对目标估计精度差的问题。它基于当前统计模型建立状态方程;基于传感器测角与机动目标位置关系建立测量方程;然后基于三阶球面‑径向容积准则进行时间更新,获取机动目标状态和协方差的一步预测;再利用机动目标状态和协方差的一步预测生成容积点,根据***测量方程计算量测预测值和互协方差;最后,将测量更新转换为线性衰退的求解问题,通过求取指标函数的最小值,利用迭代方法获得机动目标状态和协方差的后验估计值,完成机动目标的协同定位。本发明用于目标的协同定位。
Description
技术领域
本发明涉及适用于非高斯***的鲁棒容积目标协同定位方法,属于非合作目标协同定位技术领域。
背景技术
目标协同定位是工程领域中一种常用的确定非合作目标位置和速度的方法。该方法以目标运动模型为基础,基于两个或多个传感器对目标进行测量,通过卡尔曼滤波等手段对来自目标运动模型的信息和带误差的测量信息进行融合,从而估计出目标的位置和速度。但传统的协同定位方法均认为传感器的测量噪声符合高斯白噪声,但在实际***中,传感器的噪声呈现为以闪烁噪声为代表的非高斯噪声。在这种情况下,传统的协同定位方法出现估计精度下降甚至发散的问题。
发明内容
本发明目的是为了解决现有协同定位方法认为传感器的测量噪声符合高斯白噪声,造成对目标估计精度差的问题,提供了一种适用于非高斯***的鲁棒容积目标协同定位方法。
本发明所述适用于非高斯***的鲁棒容积目标协同定位方法,它包括:
基于当前统计模型建立状态方程;基于传感器测角与机动目标位置关系建立测量方程;然后基于三阶球面-径向容积准则进行时间更新,获取机动目标状态和协方差的一步预测;再利用机动目标状态和协方差的一步预测生成容积点,根据***测量方程计算量测预测值和互协方差;最后,将测量更新转换为线性衰退的求解问题,通过求取指标函数的最小值,利用迭代方法获得机动目标状态和协方差的后验估计值,完成机动目标的协同定位。
本发明的优点:本发明针对目标定位过程中传感器量测受闪烁噪声等非高斯噪声污染而提出,用于提高非高斯***的目标协同定位精度。通过数学仿真表明,本发明方法更好地处理了传感器所引入的非高斯噪声,具有更高的位置和速度估计精度,可以更为有效地处理非高斯***下的目标协同定位问题。
附图说明
图1是本发明所述适用于非高斯***的鲁棒容积目标协同定位方法的协同跟踪场景示意图;
图2是分别在l1、l2和Huber范数时,代价函数ρ的函数曲线;
图3是分别在l1、l2和Huber范数时,权重函数ψ的函数曲线;
图4是采用本发明方法和传统方法进行目标位置估计的误差对比图;
图5是采用本发明方法和传统方法进行目标速度估计的误差对比图。
具体实施方式
下面结合图1至图5,对本发明的具体实施方式进行说明:
本发明所述适用于非高斯***的鲁棒容积目标协同定位方法,它包括:
基于当前统计模型建立状态方程;基于传感器测角与机动目标位置关系建立测量方程;然后基于三阶球面-径向容积准则进行时间更新,获取机动目标状态和协方差的一步预测;再利用机动目标状态和协方差的一步预测生成容积点,根据***测量方程计算量测预测值和互协方差;最后,将测量更新转换为线性衰退的求解问题,通过求取指标函数的最小值,利用迭代方法获得机动目标状态和协方差的后验估计值,完成机动目标的协同定位。
当前统计模型认为,当机动目标以加速度机动时,下一时刻的加速度取值是有限的,即只能在“当前”的加速度领域内,并且机动加速度的“当前”统计概率服从修正的瑞利分布,均值是“当前”的加速度预测。该模型本质上是一种非零均值时间相关模型。
所述机动目标的当前统计模型为:
式中X(t)为机动目标位置,为加速度当前均值,a(t)为加速度当前值,α为机动频率,即为机动时间常数的倒数,ω(t)为***噪声;
设定加速度当前均值在采样周期内为常数,则:
进而将机动目标的当前统计模型变形为:
公式(4)的模型本质上是非零均值时间相关模型,加速度的“当前”概率密度用修正的瑞利分布描述,均值为“当前”加速度预测值,随机加速度在时间轴上符合一阶时间相关过程。
将公式(4)离散化获得相应的离散形式为:
其中为机动目标状态,F(k)为状态转移矩阵,X1(k-1)为k-1时刻机动目标状态,U(k)为加速度驱动矩阵,为k时刻加速度当前均值;w(k)为过程噪声,是均值为零方差为Q(k)的高斯白噪声;
其中
式中T为采样周期;
加速度当前值a(t)的方差为:
式中amax为最大加速度正值,a-max为最大加速度负值;
可以看出,amax和a-max直接影响“当前”统计模型中“当前”加速度的方差而的大小又直接影响过程噪声的方差Q(k),从而影响滤波器的跟踪性能。当amax和a-max的绝对值取较小值时,跟踪***的***方差较小,跟踪精度高,但跟踪目标机动变化范围较窄,且响应速度较慢;当amax和a-max的绝对值取较大值时,跟踪***的***方差较大,跟踪***对较大范围机动的目标能以较大的***方差保持快速响应,但跟踪精度较低;当amax和a-max取一定值后,在目标机动加速度较大时,***方差较小,跟踪精度较高;但在机动加速度较小时,即目标弱机动情况下,***的方差很大,跟踪精度较低。
对于空间中的机动目标,定义其在惯性坐标系下的位置、速度和加速度的状态量x为:
x=[x y z vx vy vz ax ay az]T (9)
式中x为机动目标在x轴方向的值,y为机动目标在y轴方向的值,z为机动目标在z轴方向的值,vx为机动目标在x轴方向的速度,vy为机动目标在y轴方向的速度,vz为机动目标在z轴方向的速度,ax为机动目标在x轴方向的加速度,ay为机动目标在y轴方向的加速度,az为机动目标在z轴方向的加速度;
机动目标的状态方程为:
xk+1为k+1时刻机动目标状态,Φk+1/k为状态转移矩阵,xk为k时刻机动目标状态,Uk为k时刻加速度驱动矩阵,为k时刻当前加速度均值,Γk为噪声驱动矩阵,
状态转移矩阵
***噪声驱动矩阵
式中I为3×3的单位矩阵。
测量方程:
由于被动传感器仅能测量目标的视线角信息(高低角和方位角),因此常利用两个传感器进行协同探测。
采用两个传感器测量机动目标的视线高低角γ和方位角η,两个传感器的测量量z为:
z=[γ1 η1 γ2η2]T (11)
γ1为一号传感器的视线高低角,η1为一号传感器的方位角,γ2为二号传感器的视线高低角,η2为二号传感器的方位角;
一号传感器在惯性坐标系下的位置坐标xs,1为:
xs,1=[xs,1 ,ys,1, zs,1]T (12)
式中xs,1为一号传感器在x轴方向的坐标值,ys,1为一号传感器在y轴方向的坐标值,zs,1为一号传感器在z轴方向的坐标值;
二号传感器在惯性坐标系下的位置坐标xs,2为:
xs,2=[xs,2,ys,2,zs,2]T (13)
式中xs,2为一号传感器在x轴方向的坐标值,ys,2为一号传感器在y轴方向的坐标值,zs,2为一号传感器在z轴方向的坐标值;
则机动目标相对于一号传感器的位置在惯性坐标系下表示为:
机动目标相对于二号传感器的位置在惯性坐标系下表示为:
由于传感器输出的测量信息为其本体坐标系下的高低角和方位角,因此需要将机动目标相对于传感器的位置信息在传感器本体系下进行表示:
机动目标相对于一号传感器在一号传感器本体系下的位置信息为:
式中为惯性坐标系到一号传感器本体系的坐标转换矩阵;
机动目标相对于二号传感器在二号传感器本体系下的位置信息为:
为惯性坐标系到二号传感器本体系的坐标转换矩阵;
式(16)写成展开形式为:
xrs,1为一号传感器在x方向的位置分量,yrs,1为一号传感器在y方向的位置分量,zrs,1为一号传感器在z方向的位置分量,
式(17)写成展开形式为:
xrs,2为二号传感器在x方向的位置分量,yrs,2为为二号传感器在y方向的位置分量,zrs,2为二号传感器在z方向的位置分量,
根据高低角和方位角的定义,一号传感器的视线高低角γ1为:
一号传感器的方位角η1为:
η1=arctan2(-zrs,1,xrs,1) (21)
二号传感器的视线高低角γ2为:
二号传感器的方位角η2为:
η2=arctan2(-zrs,2,xrs,2) (23)
考虑传感器测量噪声的影响,可建立传感器的测量方程如下:
式中h(x)为测量方程,v为测量噪声矢量,为一号传感器高低角测量噪声?为一号传感器方位角测量噪声,为二号传感器高低角测量噪声,为二号传感器方位角测量噪声。
鲁棒容积滤波算法:
所述三阶球面-径向容积规则包括:
计算如下形式的积分运算:
式中F(t)为非线性函数,Rn为n维实数,n为***维数,t为均值为方差为Σ的随机变量,
不考虑常值部分,将式(25)中的多维积分简化为:
式中I(f)表示f的积分f(x)为***方程;
令x=ry,其中yTy=1,r∈[0,∞),通过积分转化为如下的球面-径向坐标积分:
式中r为球面半径,σ(·)为Un上的元素,Un为n维单位球面;
将积分I(f)分解,变为一个球面积分S(r)和一个径向积分R:
由此可知,该积分坐标变换是将积分计算式(26)转换为多维空间中某个几何体的容积计算,被称为容积规则。
对于球面积分和径向积分,假定分别由如下数值积分方法近似:
式中{yi,ws,i}为计算球面积分的积分点集合,Ns为相应的积分点数目;{rj,wr,j}为计算径向积分的积分点集合,Nr为相应的积分点数目;S(rj)表示球面积分;
将式(26)所示的积分I(f)近似表示为:
将三阶球面准则和三阶径向准则代入式(32)所示的积分公式中,得到三阶球面-径向容积准则:
式中,N=NrNs为积分点数目,ei∈Rn表示第i个元素为1的单位向量,An为n维单位球的表面积;
在积分权重函数为高斯概率密度函数下的三阶球面-径向容积准则为:
式中Px为***噪声,wi为容积点权重,ξi为为点集,
三阶球面-径向容积准则的积分点数目为N=2n,其积分点及权重如下:
式中χi为容积点,
其中,[1]i表示点集[1]的第i组元素,[1]为
广义极大似然估计:采用Huber提出的广义极大似然估计方法求解如下的残差函数J(x)的极小值:
式中,ρ为任意函数,m为测量量维数,ζi为关心的自变量,
极大似然估计为ρ(ζi)=-ln[f(ζi)]时,作为广义极大似然估计的特例。
对式(37)求导,得到广义极大似然估计的解为
式中中间变量φ(ζi)为:
定义权重函数ψ(ζi)和矩阵Ψ分别为:
Ψ=diag[ψ(ζi)] (41)
式(38)解的隐函数方程为
HTΨ(Hx-z)=0 (42)
式中H为测量矩阵;
在广义极大似然估计问题中,可以通过选择函数ρ的形式获取估计器的某些特定性质。广义极大似然估计的一个重要的特性就是鲁棒性,Huber选择ρ函数为:
式中,μ为可调参数,ρ函数为混合l1、l2范数最小函数;当γ→0时,ρ函数趋近于l1范数最小,当γ→∞时,ρ函数趋近于l2范数最小。图2和图3分别给出了l1、l2和Huber范数时代价函数ρ和权重函数ψ的函数曲线。
利用加权迭代法求解非线性隐函数,式(42)改写为
HTΨHx=HTΨz (44)
于是
x=(HTΨH)-1HTΨz (45)
由于Ψ依赖于残差ζi,并且ζ=Hx-y,式(42)的迭代解形式为
xj+1=(HTΨ(j)H)-1HTΨ(j)z (46)
式中,j为迭代次数,上式的收敛后的值作为状态估计;
迭代初值x(0)为:
x(0)=(HTH)-1HTz。 (47)
Huber已证明,当选择式(43)作为ρ函数的形式时,M估计滤波对测量噪声为闪烁噪声的***具有渐近最优鲁棒性。
鲁棒容积滤波算法的基本步骤:
参数初始化:
状态估计初值和误差协方差阵初值分别为
式中E[x0]为初始状态x0的均值,对k=1,2,…运行如下步骤:
计算k-1时刻的容积点Pk-1k-1:
式中Sk-1通过Pk-1k-1的Cholesky的分解得到;
计算k时刻的一步预测值及其协方差
式中Qk-1为k-1时刻***噪声协方差;
计算用于测量更新的容积点:
其中,点集ξi与式(51)一致;
计算量测预测值与互协方差Pxz:
为关于一步预测容积点的测量方程;
构造线性回归问题:
状态真值和预测值的关系为:
δk为预测值偏差;
对测量方程进行线性化
υk为测量值偏差;
其中,
Hk=[(Pkk-1)-1Pxz]T (62)
将量测更新转化为线性回归问题:
定义变量:
则线性回归模型可以转化为
yk=Mkxk+ξk (68)
求解线性回归问题:
将基于Huber滤波理论的鲁棒容积滤波测量更新转换为求解如下指标函数的最小值J(xk):
式中,ρ为指标函数,其具体形式为式(43);
ζi为ζ的第i个元素,且
指标函数J最小的解满足如下方程
式中
φ(ζi)=ρ'(ζi) (72)
定义ψ(ζi)和Ψ(ζi)分别为
Ψ(ζi)=diag[ψ(ζi)] (74)
于是,式(71)可改写为
计算状态估计值及协方差:
通过迭代的方法得到上式的解为
式中,j为迭代次数;
迭代的初值选择为:
式(76)的收敛解为鲁棒容积滤波的测量更新
估计协方差矩阵Pk为
下面通过数学仿真来说明本发明方法的效果:
考虑图1所示场景,两架安装有被动传感器的无人机对一地面目标进行协同探测,无人机之间利用数据链进行通信,传输传感器所获取的测量信息,地面目标则在公路上进行变加速直线运动。
在地面坐标下描述目标运动,地面系X轴指向东,Y轴指向上,Z轴指向南,目标朝东南方向沿15°上坡进行变加速直线运动,其轨迹满足如下方程
其中,c1=cos15°sin45°,c2=sin15°,c3=cos15°cos45°。
两无人机在目标后方2000米处拉开500米基线进行跟踪,其传感器测量帧频为10Hz,测量噪声为闪烁噪声,其概率密度函数p(x)为:
其中,ε=0.2为闪烁概率,和为相互独立的高斯分布,σ1=0.3°,σ2=1.5°。
在上述仿真场景下,基于250次独立的蒙特卡洛数学仿真,获得图4和图5所示仿真结果。
图4和图5分别对比了本专利所公开的基于鲁棒容积滤波的协同定位方法和基于扩展卡尔曼滤波的传统协同定位方法的位置和速度估计精度。可以看出,在非高斯***下,本专利所给出的协同定位方法更好地处理了传感器所引入的非高斯噪声,具有更高的位置和速度估计精度,可以更为有效地处理非高斯***下的目标协同定位问题。
Claims (4)
1.一种适用于非高斯***的鲁棒容积目标协同定位方法,其特征在于,它包括:
基于当前统计模型建立状态方程;基于传感器测角与机动目标位置关系建立测量方程;然后基于三阶球面-相径容积准则进行时间更新,获取机动目标状态和协方差的一步预测;再利用机动目标状态和协方差的一步预测生成容积点,根据***测量方程计算量测预测值和互协方差;最后,将测量更新转换为线性衰退的求解问题,通过求取指标函数的最小值,利用迭代方法获得机动目标状态和协方差的后验估计值,完成机动目标的协同定位;
采用两个传感器测量机动目标的视线高低角γ和方位角η,两个传感器的测量量Z为:
Z=[γ1 η1 γ2 η2]T (1)
γ1为一号传感器的视线高低角,η1为一号传感器的方位角,γ2为二号传感器的视线高低角,η2为二号传感器的方位角;
一号传感器在惯性坐标系下的位置坐标XS,1为:
Xs,1=[xs,1,ys,1,zs,1]T (2)
式中xs,1为一号传感器在x轴方向的坐标值,ys,1为一号传感器在y轴方向的坐标值,zs,1为一号传感器在z轴方向的坐标值;
二号传感器在惯性坐标系下的位置坐标XS,2为:
Xs,2=[xs,2,ys,2,zs,2]T (3)
式中xs,2为二号传感器在x轴方向的坐标值,ys,2为二号传感器在y轴方向的坐标值,zs,2为二号传感器在z轴方向的坐标值;
则机动目标相对于一号传感器的位置r1 i在惯性坐标系下表示为:
r1 i=x-xs,1 (4)
式中,x=[x y z]T,x为机动目标在x轴方向的位置坐标,y为机动目标在y轴方向的位置坐标,z为机动目标在z轴方向的位置坐标;
机动目标相对于二号传感器的位置在惯性坐标系下表示为:
由于传感器输出的测量信息为其本体坐标系下的视线高低角和方位角,因此需要将机动目标相对于传感器的位置信息在传感器本体系下进行表示:
机动目标相对于一号传感器在一号传感器本体系下的位置信息r1 s为:
式中为惯性坐标系到一号传感器本体系的坐标转换矩阵;
机动目标相对于二号传感器在二号传感器本体系下的位置信息为:
为惯性坐标系到二号传感器本体系的坐标转换矩阵;
式(6)写成展开形式为:
xrs,1为一号传感器在x方向的位置分量,yrs,1为一号传感器在y方向的位置分量,zrs,1为一号传感器在z方向的位置分量,
式(7)写成展开形式为:
xrs,2为二号传感器在x方向的位置分量,yrs,2为二号传感器在y方向的位置分量,zrs,2为二号传感器在z方向的位置分量,
一号传感器的视线高低角γ1为:
一号传感器的方位角η1为:
η1=arctan2(-zrs,1,xrs,1) (11)
二号传感器的视线高低角γ2为:
二号传感器的方位角η2为:
η2=arctan2(-zrs,2,xrs,2) (13)
建立传感器的测量方程如下:
式中h(X)为测量方程,v为测量噪声矢量,为一号传感器视线高低角测量噪声,为一号传感器方位角测量噪声,为二号传感器视线高低角测量噪声,为二号传感器方位角测量噪声。
2.根据权利要求1所述的适用于非高斯***的鲁棒容积目标协同定位方法,其特征在于,
所述机动目标的当前统计模型为:
式中X(t)为机动目标位置,为加速度当前均值,a(t)为加速度当前值,α为机动频率,ω(t)为***噪声;
设定加速度当前均值在采样周期内为常数,则:
进而将机动目标的当前统计模型变形为:
将公式(18)离散化获得相应的离散形式为:
其中为机动目标状态,F(k)为状态转移矩阵,X1(k-1)为k-1时刻机动目标状态,U(k)为加速度驱动矩阵,为k时刻加速度当前均值;w(k)为过程噪声,是均值为零方差为Q(k)的高斯白噪声;
其中
式中T为采样周期;
加速度当前值a(t)的方差为:
式中amax为最大加速度正值,a-max为最大加速度负值;
对于空间中的机动目标,定义其在惯性坐标系下的位置、速度和加速度的状态量X为:
X=[x y z vx vy vz ax ay az]T (23)
式中x为机动目标在x轴方向的位置坐标,y为机动目标在y轴方向的位置坐标,z为机动目标在z轴方向的位置坐标,vx为机动目标在x轴方向的速度,vy为机动目标在y轴方向的速度,vz为机动目标在z轴方向的速度,ax为机动目标在x轴方向的加速度,ay为机动目标在y轴方向的加速度,az为机动目标在z轴方向的加速度;
机动目标的状态方程为:
Xk+1为k+1时刻机动目标状态,Φk+1/k为状态转移矩阵,Xk为k时刻机动目标状态,Uk为k时刻加速度驱动矩阵,为k时刻当前加速度均值,Γk为噪声驱动矩阵,
状态转移矩阵
***噪声驱动矩阵
式中I为3×3的单位矩阵。
3.根据权利要求2所述的适用于非高斯***的鲁棒容积目标协同定位方法,其特征在于,
采用广义极大似然估计求解如下的残差函数J(x)的极小值:
式中,ρ为任意函数,m为测量维数,ζi为关心的自变量,
对式(37)求导,得到广义极大似然估计的解为
式中中间变量φ(ζi)为:
定义权重函数ψ(ζi)和矩阵Ψ分别为:
Ψ=diag[ψ(ζi)] (41)
式(38)解的隐函数方程为
HTΨ(Hx-z)=0 (42)
式中H为测量矩阵;
选择ρ函数为:
式中,μ为可调参数,ρ函数为混合l1、l2范数最小函数;
利用加权迭代法求解非线性隐函数,式(42)改写为
HTΨHx=HTΨz (44)
于是
x=(HTΨH)-1HTΨz (45)
由于Ψ依赖于残差ζi,并且ζ=Hx-y,式(42)的迭代解形式为
xj+1=(HTΨ(j)H)-1HTΨ(j)z (46)
式中,j为迭代次数,上式的收敛后的值作为状态估计;
迭代初值x(0)为:
x(0)=(HTH)-1HTz (47)。
4.根据权利要求3所述的适用于非高斯***的鲁棒容积目标协同定位方法,其特征在于,
参数初始化:
状态估计初值和误差协方差阵初值分别为
式中E[x0]为初始状态x0的均值,表示的期望,对k=1,2,…运行如下步骤:
计算k-1时刻的容积点
式中Sk-1通过Pk-1|k-1的Cholesky的分解得到,Pk-1|k-1为状态误差协方差阵,为k-1时刻的容积点,为k-1时刻的状态估计值;
计算k时刻的一步预测值及其协方差
式中Qk-1为k-1时刻***噪声协方差;
计算用于测量更新的容积点:
其中,点集ξi与式(51)一致;
计算量测预测值与互协方差Pxz:
为关于一步预测容积点的测量方程;
构造线性回归问题:
状态真值和预测值的关系为:
δk为预测值偏差;
对测量方程进行线性化
υk为测量值偏差,为k时刻的状态一步预测值的预测方程;
其中,
Hk=[(Pk|k-1)-1Pxz]T (62)
将量测更新转化为线性回归问题:
定义变量:
其中,Rk为测量噪声方差阵;
则线性回归模型可以转化为
yk=Mkxk+ξk (68)
求解线性回归问题:
将基于Huber滤波理论的鲁棒容积滤波测量更新转换为求解如下指标函数的最小值J(xk):
ζi为ζ的第i个元素,且
指标函数J最小的解满足如下方程
式中
φ(ζi)=ρ'(ζi) (72)
定义ψ(ζi)和Ψ(ζi)分别为
Ψ(ζi)=diag[ψ(ζi)] (74)
于是,式(71)可改写为
计算状态估计值及协方差:
通过迭代的方法得到上式的解为
式中,j为迭代次数;
迭代的初值选择为:
式(76)的收敛解为鲁棒容积滤波的测量更新
估计协方差矩阵Pk为
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810106433.3A CN108303095B (zh) | 2018-02-02 | 2018-02-02 | 适用于非高斯***的鲁棒容积目标协同定位方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810106433.3A CN108303095B (zh) | 2018-02-02 | 2018-02-02 | 适用于非高斯***的鲁棒容积目标协同定位方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN108303095A CN108303095A (zh) | 2018-07-20 |
CN108303095B true CN108303095B (zh) | 2019-04-19 |
Family
ID=62850939
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201810106433.3A Active CN108303095B (zh) | 2018-02-02 | 2018-02-02 | 适用于非高斯***的鲁棒容积目标协同定位方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN108303095B (zh) |
Families Citing this family (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109061579A (zh) * | 2018-08-03 | 2018-12-21 | 中国航空工业集团公司雷华电子技术研究所 | 一种带厚尾噪声的鲁棒容积滤波方法 |
CN111142100B (zh) * | 2020-01-06 | 2022-08-09 | 南京鹰目电子科技有限公司 | 一种雷达目标跟踪方法 |
CN111427022A (zh) * | 2020-05-08 | 2020-07-17 | 北京理工大学重庆创新中心 | 一种基于极大似然估计的阵列雷达测角方法 |
CN112763974B (zh) * | 2020-12-17 | 2024-04-12 | 深圳先进技术研究院 | 定位***中传感器的部署方法、装置及介质 |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
EP2113909A1 (en) * | 2001-04-27 | 2009-11-04 | Accenture LLP | Location-based services |
CN101832738A (zh) * | 2010-04-28 | 2010-09-15 | 北京航空航天大学 | 一种远程空空导弹多平台协同制导***及其实现方法 |
CN103968839A (zh) * | 2014-05-21 | 2014-08-06 | 哈尔滨工程大学 | 一种基于蜂群算法改进ckf的单点重力匹配方法 |
CN106323291A (zh) * | 2016-09-29 | 2017-01-11 | 安徽工程大学 | 一种基于平方根容积卡尔曼滤波的多机器人协同定位算法 |
CN107015944A (zh) * | 2017-03-28 | 2017-08-04 | 南京理工大学 | 一种用于目标跟踪的混合平方根容积卡尔曼滤波方法 |
CN107389060A (zh) * | 2017-05-27 | 2017-11-24 | 哈尔滨工业大学 | 基于CKF的IMU/Wi‑Fi信号的超紧组合室内导航方法 |
Family Cites Families (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104793201B (zh) * | 2015-05-04 | 2018-04-24 | 哈尔滨工业大学 | 一种跟踪临近空间高超声速目标的修正变结构网格交互多模型滤波方法 |
CN105898865B (zh) * | 2016-06-17 | 2019-06-18 | 杭州电子科技大学 | 非线性非高斯条件下基于ekf和pf的协同定位方法 |
-
2018
- 2018-02-02 CN CN201810106433.3A patent/CN108303095B/zh active Active
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
EP2113909A1 (en) * | 2001-04-27 | 2009-11-04 | Accenture LLP | Location-based services |
CN101832738A (zh) * | 2010-04-28 | 2010-09-15 | 北京航空航天大学 | 一种远程空空导弹多平台协同制导***及其实现方法 |
CN103968839A (zh) * | 2014-05-21 | 2014-08-06 | 哈尔滨工程大学 | 一种基于蜂群算法改进ckf的单点重力匹配方法 |
CN106323291A (zh) * | 2016-09-29 | 2017-01-11 | 安徽工程大学 | 一种基于平方根容积卡尔曼滤波的多机器人协同定位算法 |
CN107015944A (zh) * | 2017-03-28 | 2017-08-04 | 南京理工大学 | 一种用于目标跟踪的混合平方根容积卡尔曼滤波方法 |
CN107389060A (zh) * | 2017-05-27 | 2017-11-24 | 哈尔滨工业大学 | 基于CKF的IMU/Wi‑Fi信号的超紧组合室内导航方法 |
Non-Patent Citations (1)
Title |
---|
多水下航行器协同定位算法研究;刘亚龙;《哈尔滨工程大学》;20171231;第17-91页 |
Also Published As
Publication number | Publication date |
---|---|
CN108303095A (zh) | 2018-07-20 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN108303095B (zh) | 适用于非高斯***的鲁棒容积目标协同定位方法 | |
CN109597864B (zh) | 椭球边界卡尔曼滤波的即时定位与地图构建方法及*** | |
CN106772524B (zh) | 一种基于秩滤波的农业机器人组合导航信息融合方法 | |
CN108120438B (zh) | 一种基于imu和rfid信息融合的室内目标快速跟踪方法 | |
CN104035083B (zh) | 一种基于量测转换的雷达目标跟踪方法 | |
CN102568004A (zh) | 一种高机动目标跟踪算法 | |
Havangi et al. | Adaptive neuro-fuzzy extended Kaiman filtering for robot localization | |
CN110209180B (zh) | 一种基于HuberM-Cubature卡尔曼滤波的无人水下航行器目标跟踪方法 | |
CN111813113A (zh) | 仿生视觉自运动感知地图绘制方法、存储介质及设备 | |
CN111025229B (zh) | 一种水下机器人纯方位目标估计方法 | |
Malleswaran et al. | IMM-UKF-TFS model-based approach for intelligent navigation | |
CN114370878B (zh) | 一种基于stackf的多auv协同定位方法 | |
CN108680162B (zh) | 一种基于渐进无迹卡尔曼滤波的人体目标跟踪方法 | |
Cohen et al. | A-KIT: Adaptive Kalman-informed transformer | |
CN117369507A (zh) | 一种自适应粒子群算法的无人机动态路径规划方法 | |
He et al. | A dynamic enhanced robust cubature Kalman filter for the state estimation of an unmanned autonomous helicopter | |
Mallick et al. | Comparison of measures of nonlinearity for bearing-only and GMTI filtering | |
CN108710295B (zh) | 一种基于渐进容积信息滤波的机器人跟随方法 | |
Wei et al. | Random weighting estimation for systematic error of observation model in dynamic vehicle navigation | |
CN114565020B (zh) | 一种基于深度置信网络和扩展卡尔曼滤波的飞行器传感器信号融合方法 | |
CN115328168A (zh) | 基于自适应强跟踪的移动机器人同步定位与建图方法及*** | |
CN113689501A (zh) | 一种基于收敛点的双机协同目标机定位跟踪控制方法 | |
Zhong et al. | Random weighting estimation of kinematic model error for dynamic navigation | |
Duraisamy et al. | Influence of the sensor local track covariance on the track-to-track sensor fusion | |
Amir | Enhanced SLAM for a mobile robot using unscented Kalman filter and radial basis function neural network |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |