CN111426322A - 同时估计状态和输入的自适应目标跟踪滤波方法及系统 - Google Patents
同时估计状态和输入的自适应目标跟踪滤波方法及系统 Download PDFInfo
- Publication number
- CN111426322A CN111426322A CN202010274152.6A CN202010274152A CN111426322A CN 111426322 A CN111426322 A CN 111426322A CN 202010274152 A CN202010274152 A CN 202010274152A CN 111426322 A CN111426322 A CN 111426322A
- Authority
- CN
- China
- Prior art keywords
- information
- target
- time
- matrix
- maneuvering target
- 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
- 238000001914 filtration Methods 0.000 title claims abstract description 47
- 238000000034 method Methods 0.000 title claims abstract description 30
- 230000003044 adaptive effect Effects 0.000 title claims description 27
- 230000001133 acceleration Effects 0.000 claims abstract description 63
- 238000012937 correction Methods 0.000 claims abstract description 59
- 238000005259 measurement Methods 0.000 claims abstract description 48
- 239000011159 matrix material Substances 0.000 claims description 120
- 238000004422 calculation algorithm Methods 0.000 claims description 12
- 230000007704 transition Effects 0.000 claims description 12
- 238000010586 diagram Methods 0.000 description 9
- 238000011160 research Methods 0.000 description 2
- 238000004364 calculation method Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 230000000750 progressive effect Effects 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
- 238000012795 verification Methods 0.000 description 1
Images
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
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/18—Complex mathematical operations for evaluating statistical data, e.g. average values, frequency distributions, probability functions, regression analysis
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Data Mining & Analysis (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Mathematical Physics (AREA)
- Mathematical Analysis (AREA)
- Theoretical Computer Science (AREA)
- Computational Mathematics (AREA)
- Pure & Applied Mathematics (AREA)
- Mathematical Optimization (AREA)
- Probability & Statistics with Applications (AREA)
- Evolutionary Biology (AREA)
- Operations Research (AREA)
- Bioinformatics & Cheminformatics (AREA)
- Life Sciences & Earth Sciences (AREA)
- Algebra (AREA)
- Automation & Control Theory (AREA)
- Databases & Information Systems (AREA)
- Software Systems (AREA)
- General Engineering & Computer Science (AREA)
- Bioinformatics & Computational Biology (AREA)
- Radar Systems Or Details Thereof (AREA)
- Feedback Control In General (AREA)
Abstract
本发明涉及一种同时估计状态和输入的自适应目标跟踪滤波方法。该方法包括:建立机动目标的运动模型和传感器的观测模型;根据上述模型得到当前时刻的量测信息和运行信息;获取上一时刻状态估计器的修正步信息和输入估计器的目标加速度估计信息;根据上一时刻的修正步信息、上一时刻的目标加速度估计信息、当前时刻的量测信息和当前时刻的运行信息,确定当前时刻状态估计器的预测步信息;根据当前时刻的预测步信息和当前时刻的量测信息,确定当前时刻状态估计器的修正步信息,修正步信息为当前时刻滤波后的目标跟踪结果;根据状态估计器的预测步信息,确定输入估计器的目标加速度的估计信息。本发明能够获得良好的目标跟踪结果,提高滤波的精度。
Description
技术领域
本发明涉及自适应目标跟踪滤波领域,特别是涉及一种同时估计状态和输入的自适应目标跟踪滤波方法及系统。
背景技术
目标跟踪技术是当今科学研究中的一个重要领域,目前在军事、工业和生活等领域均具有重要的作用和广泛的应用,也是学者们研究的热点。例如在军事上,执行拦截敌方导弹任务时会对目标进行定位、导航和打击。如果不能有效地获取目标的运动状态信息,后续的拦截就无从谈起;当敌方目标做机动运动时,如果不能准确的获得目标的加速度(输入)信息时,就无法对敌方目标的导弹进行预测,使拦截任务的成功率降低。因此,在完成对目标的跟踪时,目标的状态和输入都是很重要的信息,需要被估计。
卡尔曼滤波器及它的各种变形已经为目标跟踪提供了完善的技术基础,它们能够在有过程噪声和观测噪声的情况下获得目标状态的最优估计。这些技术往往假设噪声与干扰都符合高斯白噪声的模型,尤其是把系统的输入也视为符合高斯白噪声模型或马尔科夫随机过程,在观测的目标存在确定性的输入的情况下,会导致目标跟踪的结果不理想,甚至发散。因此需要在估计机动目标状态的同时估计机动目标的输入来获得良好的目标跟踪结果,提高滤波的精度。
发明内容
本发明的目的是提供一种同时估计状态和输入的自适应目标跟踪滤波方法及系统,能够获得良好的目标跟踪结果,提高滤波的精度。
为实现上述目的,本发明提供了如下方案:
一种同时估计状态和输入的自适应目标跟踪滤波方法,包括:
建立机动目标的运动模型和传感器的观测模型;
根据所述运动模型,得到当前时刻的量测信息;
根据所述观测模型,得到当前时刻的运行信息;
获取上一时刻状态估计器的修正步信息和上一时刻输入估计器的目标加速度估计信息;
根据所述上一时刻的修正步信息、所述上一时刻的目标加速度估计信息、所述当前时刻的量测信息和所述当前时刻的运行信息,确定当前时刻状态估计器的预测步信息;
根据所述当前时刻的预测步信息和所述当前时刻的量测信息,确定当前时刻状态估计器的修正步信息,所述修正步信息为当前时刻滤波后的目标跟踪结果;
根据所述状态估计器的预测步信息,确定输入估计器的目标加速度的估计信息。
可选的,所述建立机动目标的运动模型和传感器的观测模型,具体包括:
建立机动目标的运动模型:
X(k)=AX(k-1)+Bu(k-1)+D1w(k)
其中,X(k)为机动目标的运动状态,u(k)为机动目标的输入,w(k)为目标的系统噪声,A为机动目标的运动模型的状态转移矩阵,B为机动目标的运动模型的输入矩阵,D1为机动目标的运动模型的系统噪声驱动矩阵;
建立传感器的观测模型:
y(k)=CX(k)+D2v(k)
其中,y(k)为机动目标的观测状态,v(k)为传感器的观测噪声,C为传感器的观测矩阵,D2为传感器的观测噪声驱动矩阵。
可选的,所述根据所述上一时刻的修正步信息、所述上一时刻的目标加速度估计信息、所述当前时刻的量测信息和所述当前时刻的运行信息,确定当前时刻状态估计器的预测步信息,具体包括:
根据所述上一时刻的修正步信息、所述上一时刻的目标加速度估计信息、所述当前时刻的量测信息和所述当前时刻的运行信息通过公式Ωp(k)=(AΩc(k-1)AT+Q(k))-1、 和z(k)=yp(k)-y(k)确定当前时刻状态估计器的预测步信息;
其中,Ωp(k)为机动目标在k时刻预测步的信息矩阵,A为机动目标的运动模型的状态转移矩阵,Ωc(k-1)为机动目标在k-1时刻修正步的信息矩阵,Q(k)为机动目标的协方差矩阵,ξp(k)为机动目标在k时刻预测步的信息向量,ξc(k-1)为机动目标在k-1时刻修正步的信息向量,B为机动目标的运动模型的输入矩阵,为k-1时刻机动目标加速度的估计,Xp(k)为机动目标在k时刻预测步的运动状态,z(k)为机动目标在k时刻的观测误差,C为为传感器的观测矩阵,y(k)为机动目标的观测状态,yp(k)=CXp(k),yp(k)为机动目标在k时刻预测步的量测状态。
可选的,所述根据所述当前时刻的预测步信息和所述当前时刻的量测信息,确定当前时刻状态估计器的修正步信息,所述修正步信息为当前时刻滤波后的目标跟踪结果,具体包括:
其中,Ωc(k)为机动目标在k时刻修正步的信息矩阵,Ωp(k)为机动目标在k时刻预测步的信息矩阵,C为传感器的观测矩阵,R为观测噪声的协方差矩阵,ξp(k)为机动目标在k时刻预测步的信息向量,ξc(k)为机动目标在k时刻修正步的信息向量,y(k)为机动目标的观测状态,Xc(k)为机动目标在k时刻修正步的运动状态,即k时刻算法滤波后的目标跟踪结果。
可选的,所述根据所述状态估计器的预测步信息,确定输入估计器的目标加速度的估计信息,具体包括:
根据所述状态估计器的预测步信息,确定输入估计器的回归信息矩阵;
一种同时估计状态和输入的自适应目标跟踪滤波系统,包括:
模型建立模块,用于建立机动目标的运动模型和传感器的观测模型;
量测信息确定模块,用于根据所述运动模型,得到当前时刻的量测信息;
运行信息确定模块,用于根据所述观测模型,得到当前时刻的运行信息;
获取模块,用于获取上一时刻状态估计器的修正步信息和上一时刻输入估计器的目标加速度估计信息;
预测步信息确定模块,用于根据所述上一时刻的修正步信息、所述上一时刻的目标加速度估计信息、所述当前时刻的量测信息和所述当前时刻的运行信息,确定当前时刻状态估计器的预测步信息;
修正步信息确定模块,用于根据所述当前时刻的预测步信息和所述当前时刻的量测信息,确定当前时刻状态估计器的修正步信息,所述修正步信息为当前时刻滤波后的目标跟踪结果;
目标加速度估计模块,用于根据所述状态估计器的预测步信息,确定输入估计器的目标加速度的估计信息。
可选的,所述模型建立模块,具体包括:
运动模型建立单元,用于建立机动目标的运动模型:
X(k)=AX(k-1)+Bu(k-1)+D1w(k)
其中,X(k)为机动目标的运动状态,u(k)为机动目标的输入,w(k)为目标的系统噪声,A为机动目标的运动模型的状态转移矩阵,B为机动目标的运动模型的输入矩阵,D1为机动目标的运动模型的系统噪声驱动矩阵;
观测模型建立单元,用于建立传感器的观测模型:
y(k)=CX(k)+D2v(k)
其中,y(k)为机动目标的观测状态,v(k)为传感器的观测噪声,C为传感器的观测矩阵,D2为传感器的观测噪声驱动矩阵。
可选的,所述预测步信息确定模块,具体包括:
预测步信息确定单元,用于根据所述上一时刻的修正步信息、所述上一时刻的目标加速度估计信息、所述当前时刻的量测信息和所述当前时刻的运行信息通过公式Ωp(k)=(AΩc(k-1)AT+Q(k))-1、 和z(k)=yp(k)-y(k),确定当前时刻状态估计器的预测步信息;
其中,Ωp(k)为机动目标在k时刻预测步的信息矩阵,A为机动目标的运动模型的状态转移矩阵,Ωc(k-1)为机动目标在k-1时刻修正步的信息矩阵,Q(k)为机动目标的协方差矩阵,ξp(k)为机动目标在k时刻预测步的信息向量,ξc(k-1)为机动目标在k-1时刻修正步的信息向量,B为机动目标的运动模型的输入矩阵,为k-1时刻机动目标加速度的估计,Xp(k)为机动目标在k时刻预测步的运动状态,z(k)为机动目标在k时刻的观测误差,C为为传感器的观测矩阵,y(k)为机动目标的观测状态,yp(k)=CXp(k),yp(k)为机动目标在k时刻预测步的量测状态。
可选的,所述修正步信息确定模块,具体包括:
修正步信息确定单元,用于根据所述当前时刻的预测步信息和所述当前时刻的量测信息采用公式Ωc(k)=Ωp(k)+CTR-1(k)C、ξc(k)=ξp(k)+CTR-1(k)y(k)和确定当前时刻状态估计器的修正步信息;
其中,Ωc(k)为机动目标在k时刻修正步的信息矩阵,Ωp(k)为机动目标在k时刻预测步的信息矩阵,C为传感器的观测矩阵,R为观测噪声的协方差矩阵,ξp(k)为机动目标在k时刻预测步的信息向量,ξc(k)为机动目标在k时刻修正步的信息向量,y(k)为机动目标的观测状态,Xc(k)为机动目标在k时刻修正步的运动状态,即k时刻算法滤波后的目标跟踪结果。
可选的,所述目标加速度估计模块,具体包括:
回归信息矩阵确定单元,用于根据所述状态估计器的预测步信息,确定输入估计器的回归信息矩阵;
根据本发明提供的具体实施例,本发明公开了以下技术效果:
本发明提出了一种同时估计状态和输入的自适应目标跟踪滤波方法及系统,通过构建状态估计器和输入估计器两个子系统来分别估计机动目标的状态与输入,使得在不需要机动目标输入(加速度)的先验信息的情况下,能够准确地同时对机动目标的状态和输入进行估计,完成目标跟踪任务,提高滤波的精度。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案,下面将对实施例中所需要使用的附图作简单地介绍,显而易见地,下面描述中的附图仅仅是本发明的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动性的前提下,还可以根据这些附图获得其他的附图。
图1为本发明同时估计状态和输入的自适应目标跟踪滤波方法流程图;
图2为本发明自适应滤波方法流程框架示意图;
图3机动目标x轴输入估计结果示意图;
图4机动目标y轴输入估计结果示意图;
图5为本发明机动目标的位置估计结果示意图;
图6为本发明同时估计状态和输入的自适应目标跟踪滤波系统结构图。
具体实施方式
下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本发明一部分实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例,都属于本发明保护的范围。
本发明的目的是提供一种同时估计状态和输入的自适应目标跟踪滤波方法及系统,能够获得良好的目标跟踪结果,提高滤波的精度。
为使本发明的上述目的、特征和优点能够更加明显易懂,下面结合附图和具体实施方式对本发明作进一步详细的说明。
本发明提出一种同时估计状态和输入的自适应目标跟踪滤波方法,以在观测目标存在机动的情况下,利用传感器数据对机动目标的状态与输入进行估计,实现对目标的准确跟踪。本方法构建了状态估计器和输入估计器两个子系统来分别估计机动目标的状态与输入。图1为本发明同时估计状态和输入的自适应目标跟踪滤波方法流程图。如图1所示,一种同时估计状态和输入的自适应目标跟踪滤波方法包括:
步骤101:建立机动目标的运动模型和传感器的观测模型,具体包括:
建立机动目标的运动模型:
X(k)=AX(k-1)+Bu(k-1)+D1w(k)
其中,X(k)为机动目标的运动状态,u(k)为机动目标的输入,w(k)为目标的系统噪声,A为机动目标的运动模型的状态转移矩阵,B为机动目标的运动模型的输入矩阵,D1为机动目标的运动模型的系统噪声驱动矩阵。
建立传感器的观测模型:
y(k)=CX(k)+D2v(k)
其中,y(k)为机动目标的观测状态,v(k)为传感器的观测噪声,C为传感器的观测矩阵,D2为传感器的观测噪声驱动矩阵。
步骤102:根据所述运动模型,得到当前时刻的量测信息。
步骤103:根据所述观测模型,得到当前时刻的运行信息。
步骤102-103中对传感器所获得的目标信息进行采样并储存,得到量测信息和运行信息,并对滤波算法进行初始化。
为方便介绍,不失一般性,假设观测的机动目标在二维平面上运动。则此时对应的机动目标运动状态X(k)和输入u(k)具体如下:
其中,x1(k)和x2(k)为运动目标在x方向和y方向的位置信息,v1(k)和v2(k)为运动目标在x方向和y方向的速度信息,u1(k)和u2(k)为运动目标在x方向和y方向的加速度信息。
在k时刻传感器可以获得机动目标的量测信息如下:
y1(k)和y2(k)为传感器观测的运动目标在x方向和y方向的位置信息。
定义信息矩阵Ω(k)=P-1(k),P(k)为滤波的协方差矩阵;定义信息向量ξ(k)=ξ(k)=P-1(k)x(k)。初始化算法的信息矩阵Ωc(0)和信息向量ξc(0)。
步骤104:获取上一时刻状态估计器的修正步信息和上一时刻输入估计器的目标加速度估计信息。
步骤105:根据所述上一时刻的修正步信息、所述上一时刻的目标加速度估计信息、所述当前时刻的量测信息和所述当前时刻的运行信息,确定当前时刻状态估计器的预测步信息,具体包括:
根据所述上一时刻的修正步信息、所述上一时刻的目标加速度估计信息、所述当前时刻的量测信息和所述当前时刻的运行信息通过公式Ωp(k)=(AΩc(k-1)AT+Q(k))-1、 和确定当前时刻状态估计器的预测步信息。
其中,Ωp(k)为机动目标在k时刻预测步的信息矩阵,A为机动目标的运动模型的状态转移矩阵,Ωc(k-1)为机动目标在k-1时刻修正步的信息矩阵,Q(k)为机动目标的协方差矩阵,ξp(k)为机动目标在k时刻预测步的信息向量,ξc(k-1)为机动目标在k-1时刻修正步的信息向量,B为机动目标的运动模型的输入矩阵,为k-1时刻机动目标加速度的估计,Xp(k)为机动目标在k时刻预测步的运动状态,z(k)为机动目标在k时刻的观测误差,C为为传感器的观测矩阵,y(k)为机动目标的观测状态,yp(k)=CXp(k),yp(k)为机动目标在k时刻预测步的量测状态。
步骤106:根据所述当前时刻的预测步信息和所述当前时刻的量测信息,确定当前时刻状态估计器的修正步信息,所述修正步信息为当前时刻滤波后的目标跟踪结果,具体包括:
其中,Ωc(k)为机动目标在k时刻修正步的信息矩阵,Ωp(k)为机动目标在k时刻预测步的信息矩阵,C为传感器的观测矩阵,R为观测噪声的协方差矩阵,ξp(k)为机动目标在k时刻预测步的信息向量,ξc(k)为机动目标在k时刻修正步的信息向量,y(k)为机动目标的观测状态,Xc(k)为机动目标在k时刻修正步的运动状态,即k时刻算法滤波后的目标跟踪结果。
步骤107:根据所述状态估计器的预测步信息,确定输入估计器的目标加速度的估计信息,具体包括:
根据所述状态估计器的预测步信息,确定输入估计器的回归信息矩阵。
其中,为估计的机动目标加速度,θ(k)为输入估计器的系数,φ(k-1)为回归信息矩阵, 其中,与均为输入估计器的系数,为历史的输入信息,z(k)···z(k-ne)为当前时刻和历史的观测误差信息。回归信息矩阵中包含估计加速度和观测误差的历史信息。
选取自适应输入估计器阶数ne和参与计算的时延ks,两者均为正整数。
再计算利用回溯代价优化的重构输入估计:
U*(k-1)=-[HTRzH+η(k)Ru]-1×HTRz[Z(k)-HU(k-1)]
选取正整数d为i1,...,ih中的任意一个,u*(k-d)为U*(k-1)中对应的元素,此时通过最小均方估计的原理,可以得到更新输入估计器参数θ(k)的公式如下:
Kk(k)=Prc(k-1)φ(k-d-1)×[φT(k-d-1)Prc(k-1)φ(k-d-1)+λ]-1
Prc(k)=[I-Kk(k)φT(k-d-1)]Prc(k-1)
θT(k)=θT(k-1)+Kk(k)[θ(k)φ(k-d-1)-u*(k-d)]T
其中,Kk(k)为输入估计器的增益,Prc(k)为估计器参数的协方差矩阵,λ∈(0,1]为遗忘因子。
最后,计算得到k时刻下机动目标加速度的估计:
与传统的卡尔曼滤波算法相比,状态估计器采取了信息滤波的形式,在观测不到目标时误差协方差为零,而不是卡尔曼滤波算法中的无限大导致滤波发散,具有更好的稳定性,也更适合被拓展应用到多传感器的分布式滤波算法上。输入估计器则基于回溯代价优化和最小均方误差估计的思想,通过历史的输入和误差信息,在不需要输入的先验信息的情况下,仅通过观测的位置信息自适应地完成对机动目标输入的准确估计。将状态估计器和输入估计器结合起来可以同时估计对机动目标状态与输入,实现对机动目标的准确跟踪。
图2为本发明自适应滤波方法流程框架示意图。如图2所示,其中步骤105和步骤106组成了状态估计器,步骤107则为输入估计器,以完成同时对机动目标状态和加速度的估计。为了证明本发明的有效性,对提出的自适应滤波算法进行仿真验证,机动目标x轴输入(加速度)估计结果如图3所示,机动目标y轴输入(加速度)估计结果如图4所示,机动目标的位置估计结果如图5所示。图3机动目标x轴输入估计结果示意图;图4机动目标y轴输入估计结果示意图;图5为本发明机动目标的位置估计结果示意图。通过图3-图5可以看出本发明提出的同时估计状态和输入自适应滤波方法可以准确的估计出机动目标的状态和输入,说明了该方法的可行性与有效性。
本发明还提供一种同时估计状态和输入的自适应目标跟踪滤波系统。图6为本发明同时估计状态和输入的自适应目标跟踪滤波系统结构图。如图6所示,一种同时估计状态和输入的自适应目标跟踪滤波系统包括:
模型建立模块201,用于建立机动目标的运动模型和传感器的观测模型。
量测信息确定模块202,用于根据所述运动模型,得到当前时刻的量测信息。
运行信息确定模块203,用于根据所述观测模型,得到当前时刻的运行信息。
获取模块204,用于获取上一时刻状态估计器的修正步信息和上一时刻输入估计器的目标加速度估计信息。
预测步信息确定模块205,用于根据所述上一时刻的修正步信息、所述上一时刻的目标加速度估计信息、所述当前时刻的量测信息和所述当前时刻的运行信息,确定当前时刻状态估计器的预测步信息。
修正步信息确定模块206,用于根据所述当前时刻的预测步信息和所述当前时刻的量测信息,确定当前时刻状态估计器的修正步信息,所述修正步信息为当前时刻滤波后的目标跟踪结果。
目标加速度估计模块207,用于根据所述状态估计器的预测步信息,确定输入估计器的目标加速度的估计信息。
所述模型建立模块201,具体包括:
运动模型建立单元,用于建立机动目标的运动模型:
X(k)=AX(k-1)+Bu(k-1)+D1w(k)
其中,X(k)为机动目标的运动状态,u(k)为机动目标的输入,w(k)为目标的系统噪声,A为机动目标的运动模型的状态转移矩阵,B为机动目标的运动模型的输入矩阵,D1为机动目标的运动模型的系统噪声驱动矩阵。
观测模型建立单元,用于建立传感器的观测模型:
y(k)=CX(k)+D2v(k)
其中,y(k)为机动目标的观测状态,v(k)为传感器的观测噪声,C为传感器的观测矩阵,D2为传感器的观测噪声驱动矩阵。
所述预测步信息确定模块205,具体包括:
预测步信息确定单元,用于根据所述上一时刻的修正步信息、所述上一时刻的目标加速度估计信息、所述当前时刻的量测信息和所述当前时刻的运行信息通过公式Ωp(k)=(AΩc(k-1)AT+Q(k))-1、 和z(k)=yp(k)-y(k),确定当前时刻状态估计器的预测步信息。
其中,Ωp(k)为机动目标在k时刻预测步的信息矩阵,A为机动目标的运动模型的状态转移矩阵,Ωc(k-1)为机动目标在k-1时刻修正步的信息矩阵,Q(k)为机动目标的协方差矩阵,ξp(k)为机动目标在k时刻预测步的信息向量,ξc(k-1)为机动目标在k-1时刻修正步的信息向量,B为机动目标的运动模型的输入矩阵,为k-1时刻机动目标加速度的估计,Xp(k)为机动目标在k时刻预测步的运动状态,z(k)为机动目标在k时刻的观测误差,C为为传感器的观测矩阵,y(k)为机动目标的观测状态,yp(k)=CXp(k),yp(k)为机动目标在k时刻预测步的量测状态。
所述修正步信息确定模块206,具体包括:
修正步信息确定单元,用于根据所述当前时刻的预测步信息和所述当前时刻的量测信息采用公式Ωc(k)=Ωp(k)+CTR-1(k)C、ξc(k)=ξp(k)+CTR-1(k)y(k)和确定当前时刻状态估计器的修正步信息。
其中,Ωc(k)为机动目标在k时刻修正步的信息矩阵,Ωp(k)为机动目标在k时刻预测步的信息矩阵,C为传感器的观测矩阵,R为观测噪声的协方差矩阵,ξp(k)为机动目标在k时刻预测步的信息向量,ξc(k)为机动目标在k时刻修正步的信息向量,y(k)为机动目标的观测状态,Xc(k)为机动目标在k时刻修正步的运动状态,即k时刻算法滤波后的目标跟踪结果。
所述目标加速度估计模块207,具体包括:
回归信息矩阵确定单元,用于根据所述状态估计器的预测步信息,确定输入估计器的回归信息矩阵。
本说明书中各个实施例采用递进的方式描述,每个实施例重点说明的都是与其他实施例的不同之处,各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言,由于其与实施例公开的方法相对应,所以描述的比较简单,相关之处参见方法部分说明即可。
本文中应用了具体个例对本发明的原理及实施方式进行了阐述,以上实施例的说明只是用于帮助理解本发明的方法及其核心思想;同时,对于本领域的一般技术人员,依据本发明的思想,在具体实施方式及应用范围上均会有改变之处。综上所述,本说明书内容不应理解为对本发明的限制。
Claims (10)
1.一种同时估计状态和输入的自适应目标跟踪滤波方法,其特征在于,包括:
建立机动目标的运动模型和传感器的观测模型;
根据所述运动模型,得到当前时刻的量测信息;
根据所述观测模型,得到当前时刻的运行信息;
获取上一时刻状态估计器的修正步信息和上一时刻输入估计器的目标加速度估计信息;
根据所述上一时刻的修正步信息、所述上一时刻的目标加速度估计信息、所述当前时刻的量测信息和所述当前时刻的运行信息,确定当前时刻状态估计器的预测步信息;
根据所述当前时刻的预测步信息和所述当前时刻的量测信息,确定当前时刻状态估计器的修正步信息,所述修正步信息为当前时刻滤波后的目标跟踪结果;
根据所述状态估计器的预测步信息,确定输入估计器的目标加速度的估计信息。
2.根据权利要求1所述的同时估计状态和输入的自适应目标跟踪滤波方法,其特征在于,所述建立机动目标的运动模型和传感器的观测模型,具体包括:
建立机动目标的运动模型:
X(k)=AX(k-1)+Bu(k-1)+D1w(k)
其中,X(k)为机动目标的运动状态,u(k)为机动目标的输入,w(k)为目标的系统噪声,A为机动目标的运动模型的状态转移矩阵,B为机动目标的运动模型的输入矩阵,D1为机动目标的运动模型的系统噪声驱动矩阵;
建立传感器的观测模型:
y(k)=CX(k)+D2v(k)
其中,y(k)为机动目标的观测状态,v(k)为传感器的观测噪声,C为传感器的观测矩阵,D2为传感器的观测噪声驱动矩阵。
3.根据权利要求1所述的同时估计状态和输入的自适应目标跟踪滤波方法,其特征在于,所述根据所述上一时刻的修正步信息、所述上一时刻的目标加速度估计信息、所述当前时刻的量测信息和所述当前时刻的运行信息,确定当前时刻状态估计器的预测步信息,具体包括:
根据所述上一时刻的修正步信息、所述上一时刻的目标加速度估计信息、所述当前时刻的量测信息和所述当前时刻的运行信息通过公式Ωp(k)=(AΩc(k-1)AT+Q(k))-1、 和z(k)=yp(k)-y(k),确定当前时刻状态估计器的预测步信息;
4.根据权利要求1所述的同时估计状态和输入的自适应目标跟踪滤波方法,其特征在于,所述根据所述当前时刻的预测步信息和所述当前时刻的量测信息,确定当前时刻状态估计器的修正步信息,所述修正步信息为当前时刻滤波后的目标跟踪结果,具体包括:
其中,Ωc(k)为机动目标在k时刻修正步的信息矩阵,Ωp(k)为机动目标在k时刻预测步的信息矩阵,C为传感器的观测矩阵,R为观测噪声的协方差矩阵,ξp(k)为机动目标在k时刻预测步的信息向量,ξc(k)为机动目标在k时刻修正步的信息向量,y(k)为机动目标的观测状态,Xc(k)为机动目标在k时刻修正步的运动状态,即k时刻算法滤波后的目标跟踪结果。
6.一种同时估计状态和输入的自适应目标跟踪滤波系统,其特征在于,包括:
模型建立模块,用于建立机动目标的运动模型和传感器的观测模型;
量测信息确定模块,用于根据所述运动模型,得到当前时刻的量测信息;
运行信息确定模块,用于根据所述观测模型,得到当前时刻的运行信息;
获取模块,用于获取上一时刻状态估计器的修正步信息和上一时刻输入估计器的目标加速度估计信息;
预测步信息确定模块,用于根据所述上一时刻的修正步信息、所述上一时刻的目标加速度估计信息、所述当前时刻的量测信息和所述当前时刻的运行信息,确定当前时刻状态估计器的预测步信息;
修正步信息确定模块,用于根据所述当前时刻的预测步信息和所述当前时刻的量测信息,确定当前时刻状态估计器的修正步信息,所述修正步信息为当前时刻滤波后的目标跟踪结果;
目标加速度估计模块,用于根据所述状态估计器的预测步信息,确定输入估计器的目标加速度的估计信息。
7.根据权利要求6所述的同时估计状态和输入的自适应目标跟踪滤波系统,其特征在于,所述模型建立模块,具体包括:
运动模型建立单元,用于建立机动目标的运动模型:
X(k)=AX(k-1)+Bu(k-1)+D1w(k)
其中,X(k)为机动目标的运动状态,u(k)为机动目标的输入,w(k)为目标的系统噪声,A为机动目标的运动模型的状态转移矩阵,B为机动目标的运动模型的输入矩阵,D1为机动目标的运动模型的系统噪声驱动矩阵;
观测模型建立单元,用于建立传感器的观测模型:
y(k)=CX(k)+D2v(k)
其中,y(k)为机动目标的观测状态,v(k)为传感器的观测噪声,C为传感器的观测矩阵,D2为传感器的观测噪声驱动矩阵。
8.根据权利要求6所述的同时估计状态和输入的自适应目标跟踪滤波系统,其特征在于,所述预测步信息确定模块,具体包括:
预测步信息确定单元,用于根据所述上一时刻的修正步信息、所述上一时刻的目标加速度估计信息、所述当前时刻的量测信息和所述当前时刻的运行信息通过公式Ωp(k)=(AΩc(k-1)AT+Q(k))-1、 和z(k)=yp(k)-y(k),确定当前时刻状态估计器的预测步信息;
9.根据权利要求6所述的同时估计状态和输入的自适应目标跟踪滤波系统,其特征在于,所述修正步信息确定模块,具体包括:
修正步信息确定单元,用于根据所述当前时刻的预测步信息和所述当前时刻的量测信息采用公式Ωc(k)=Ωp(k)+CTR-1(k)C、ξc(k)=ξp(k)+CTR-1(k)y(k)和确定当前时刻状态估计器的修正步信息;
其中,Ωc(k)为机动目标在k时刻修正步的信息矩阵,Ωp(k)为机动目标在k时刻预测步的信息矩阵,C为传感器的观测矩阵,R为观测噪声的协方差矩阵,ξp(k)为机动目标在k时刻预测步的信息向量,ξc(k)为机动目标在k时刻修正步的信息向量,y(k)为机动目标的观测状态,Xc(k)为机动目标在k时刻修正步的运动状态,即k时刻算法滤波后的目标跟踪结果。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010274152.6A CN111426322B (zh) | 2020-04-09 | 2020-04-09 | 同时估计状态和输入的自适应目标跟踪滤波方法及系统 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010274152.6A CN111426322B (zh) | 2020-04-09 | 2020-04-09 | 同时估计状态和输入的自适应目标跟踪滤波方法及系统 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN111426322A true CN111426322A (zh) | 2020-07-17 |
CN111426322B CN111426322B (zh) | 2020-12-18 |
Family
ID=71556065
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202010274152.6A Active CN111426322B (zh) | 2020-04-09 | 2020-04-09 | 同时估计状态和输入的自适应目标跟踪滤波方法及系统 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN111426322B (zh) |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113155122A (zh) * | 2021-04-01 | 2021-07-23 | 广州大学 | 基于自适应滤波的机动目标跟踪方法 |
CN114840801A (zh) * | 2022-04-13 | 2022-08-02 | 北京航空航天大学 | 一种适用于有向切换拓扑的分布式目标跟踪方法及系统 |
Citations (12)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH09113610A (ja) * | 1995-10-23 | 1997-05-02 | Japan Radio Co Ltd | 目標追尾レーダ装置 |
WO2012166757A2 (en) * | 2011-05-31 | 2012-12-06 | Massachusetts Institute Of Technology | Tracking a body by nonlinear and non-gaussian parametric filtering |
CN103529425A (zh) * | 2013-10-23 | 2014-01-22 | 北京工商大学 | 一种实现室内目标快速跟踪的方法 |
CN104331087A (zh) * | 2014-10-24 | 2015-02-04 | 浙江大学 | 一种鲁棒的水下传感器网络目标跟踪方法 |
CN107168925A (zh) * | 2017-06-21 | 2017-09-15 | 同济大学 | 一种线性系统的状态与未知输入估计方法 |
CN107994885A (zh) * | 2017-11-21 | 2018-05-04 | 郑州鼎创智能科技有限公司 | 一种同时估计未知输入和状态的分布式融合滤波方法 |
CN108490433A (zh) * | 2018-02-07 | 2018-09-04 | 哈尔滨工业大学 | 基于序贯滤波的空时偏差联合估计与补偿方法及系统 |
CN109240289A (zh) * | 2018-09-01 | 2019-01-18 | 哈尔滨工程大学 | 波浪滑翔器艏摇信息自适应滤波方法 |
CN110208792A (zh) * | 2019-06-26 | 2019-09-06 | 哈尔滨工业大学 | 同时估计目标状态和轨迹参数的任意直线约束跟踪方法 |
CN110471282A (zh) * | 2019-08-05 | 2019-11-19 | 电子科技大学 | 一种同时估计系统状态和扰动输入的输出反馈控制方法 |
CN110865334A (zh) * | 2019-11-26 | 2020-03-06 | 北京航空航天大学 | 一种基于噪声统计特性的多传感器目标跟踪方法及系统 |
CN110958639A (zh) * | 2019-01-31 | 2020-04-03 | 北京航空航天大学 | 一种目标状态估计方法及系统 |
-
2020
- 2020-04-09 CN CN202010274152.6A patent/CN111426322B/zh active Active
Patent Citations (12)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH09113610A (ja) * | 1995-10-23 | 1997-05-02 | Japan Radio Co Ltd | 目標追尾レーダ装置 |
WO2012166757A2 (en) * | 2011-05-31 | 2012-12-06 | Massachusetts Institute Of Technology | Tracking a body by nonlinear and non-gaussian parametric filtering |
CN103529425A (zh) * | 2013-10-23 | 2014-01-22 | 北京工商大学 | 一种实现室内目标快速跟踪的方法 |
CN104331087A (zh) * | 2014-10-24 | 2015-02-04 | 浙江大学 | 一种鲁棒的水下传感器网络目标跟踪方法 |
CN107168925A (zh) * | 2017-06-21 | 2017-09-15 | 同济大学 | 一种线性系统的状态与未知输入估计方法 |
CN107994885A (zh) * | 2017-11-21 | 2018-05-04 | 郑州鼎创智能科技有限公司 | 一种同时估计未知输入和状态的分布式融合滤波方法 |
CN108490433A (zh) * | 2018-02-07 | 2018-09-04 | 哈尔滨工业大学 | 基于序贯滤波的空时偏差联合估计与补偿方法及系统 |
CN109240289A (zh) * | 2018-09-01 | 2019-01-18 | 哈尔滨工程大学 | 波浪滑翔器艏摇信息自适应滤波方法 |
CN110958639A (zh) * | 2019-01-31 | 2020-04-03 | 北京航空航天大学 | 一种目标状态估计方法及系统 |
CN110208792A (zh) * | 2019-06-26 | 2019-09-06 | 哈尔滨工业大学 | 同时估计目标状态和轨迹参数的任意直线约束跟踪方法 |
CN110471282A (zh) * | 2019-08-05 | 2019-11-19 | 电子科技大学 | 一种同时估计系统状态和扰动输入的输出反馈控制方法 |
CN110865334A (zh) * | 2019-11-26 | 2020-03-06 | 北京航空航天大学 | 一种基于噪声统计特性的多传感器目标跟踪方法及系统 |
Non-Patent Citations (3)
Title |
---|
HUAZHEN FANG 等: ""On stable simultaneous input and state estimation for discrete-time linear systems"", 《INTERNATIONAL JOURNAL OF ADAPTIVE CONTROL AND SIGNAL PROCESSING》 * |
HUAZHEN FANG 等: "A new algorithm for simultaneous input and state estimation", 《2008 AMERICAN CONTROL CONFERENCE》 * |
XIAOHANG LI等: ""State Estimation and Simultaneous Unknown Input and Measurement Noise Reconstruction based on Adaptive H ∞ Observer"", 《INTERNATIONAL JOURNAL OF CONTROL, AUTOMATION AND SYSTEM》 * |
Cited By (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113155122A (zh) * | 2021-04-01 | 2021-07-23 | 广州大学 | 基于自适应滤波的机动目标跟踪方法 |
CN114840801A (zh) * | 2022-04-13 | 2022-08-02 | 北京航空航天大学 | 一种适用于有向切换拓扑的分布式目标跟踪方法及系统 |
CN114840801B (zh) * | 2022-04-13 | 2024-05-31 | 北京航空航天大学 | 一种适用于有向切换拓扑的分布式目标跟踪方法及系统 |
Also Published As
Publication number | Publication date |
---|---|
CN111426322B (zh) | 2020-12-18 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109472418B (zh) | 基于卡尔曼滤波的机动目标状态预测优化方法 | |
CN110849369B (zh) | 机器人跟踪方法、装置、设备及计算机可读存储介质 | |
CN111426322B (zh) | 同时估计状态和输入的自适应目标跟踪滤波方法及系统 | |
Wu et al. | A self-adaptive unscented Kalman filtering for underwater gravity aided navigation | |
CN115143954A (zh) | 一种基于多源信息融合的无人车导航方法 | |
Wang et al. | A multi-target trajectory planning of a 6-dof free-floating space robot via reinforcement learning | |
CN111025229B (zh) | 一种水下机器人纯方位目标估计方法 | |
CN113709662B (zh) | 一种基于超宽带的自主式三维反演定位方法 | |
CN111969979A (zh) | 一种最小误差熵cdkf滤波器方法 | |
CN109764876B (zh) | 无人平台的多模态融合定位方法 | |
CN113763434B (zh) | 一种基于卡尔曼滤波多运动模型切换的目标轨迹预测方法 | |
CN114608585A (zh) | 一种移动机器人同步定位与建图方法及装置 | |
Jing et al. | Process noise identification based particle filter: an efficient method to track highly manoeuvring targets | |
CN114323007A (zh) | 一种载体运动状态估计方法及装置 | |
Firouzi et al. | Real-time monocular vision-based object tracking with object distance and motion estimation | |
CN109885073A (zh) | 一种针对空间非合作目标自由漂浮运动状态的预测方法 | |
CN117369507A (zh) | 一种自适应粒子群算法的无人机动态路径规划方法 | |
CN118111430A (zh) | 基于最小误差熵卡尔曼的交互多模型auv组合导航方法 | |
Liu et al. | Navigation algorithm based on PSO-BP UKF of autonomous underwater vehicle | |
CN114764830A (zh) | 一种基于四元数ekf和未标定手眼系统的物体位姿估算方法 | |
CN115469314A (zh) | 一种均匀圆环阵稳健水下目标方位跟踪方法及系统 | |
CN114741659A (zh) | 一种自适应模型在线重构建鲁棒滤波方法、设备及系统 | |
CN114061592A (zh) | 基于多模型的自适应鲁棒auv导航方法 | |
CN110543919A (zh) | 一种机器人定位控制方法、终端设备及存储介质 | |
JP6757227B2 (ja) | 運動パラメータ推定装置、運動パラメータ推定方法及びプログラム |
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 | ||
CB03 | Change of inventor or designer information |
Inventor after: Ren Zhang Inventor after: Wang Hong Inventor after: Dong Xiwang Inventor after: Han Liang Inventor after: Li Qingdong Inventor after: Song Xun Inventor before: Ren Zhang Inventor before: Wang Hong Inventor before: Dong Xiwang Inventor before: Han Liang Inventor before: Li Qingdong |
|
CB03 | Change of inventor or designer information |