CN111047627A - 一种平滑约束无迹卡尔曼滤波方法及目标跟踪方法 - Google Patents

一种平滑约束无迹卡尔曼滤波方法及目标跟踪方法 Download PDF

Info

Publication number
CN111047627A
CN111047627A CN201911114914.XA CN201911114914A CN111047627A CN 111047627 A CN111047627 A CN 111047627A CN 201911114914 A CN201911114914 A CN 201911114914A CN 111047627 A CN111047627 A CN 111047627A
Authority
CN
China
Prior art keywords
mean
value
constraint
prior probability
variance
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.)
Pending
Application number
CN201911114914.XA
Other languages
English (en)
Inventor
张宏伟
张小虎
杨夏
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Sun Yat Sen University
Original Assignee
Sun Yat Sen University
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Sun Yat Sen University filed Critical Sun Yat Sen University
Priority to CN201911114914.XA priority Critical patent/CN111047627A/zh
Publication of CN111047627A publication Critical patent/CN111047627A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/277—Analysis of motion involving stochastic approaches, e.g. using Kalman filters

Landscapes

  • Engineering & Computer Science (AREA)
  • Multimedia (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Image Analysis (AREA)

Abstract

本发明提供了一种平滑约束无迹卡尔曼滤波方法，包括如下步骤：步骤1，根据无迹变换获取当前目标观测时刻目标状态的原始先验概率密度函数；步骤2，通过数值期望计算原始先验概率的均值和方差；步骤3，引入噪声约束信息，通过计算近似可行域的中心，以获取修正先验概率密度；步骤4，通过后验迭代优化寻求满足约束条件的高斯分布均值和方差，产生满足约束条件的新的高斯西格玛点；步骤5，对高斯西格玛点进行加权计算，完成滤波过程。本发明所述的一种平滑约束无迹卡尔曼滤波方法在准确性和鲁棒性方面具有优势，同时，其实时性方面优于粒子滤波算法。相应地，本发明还提供一种目标跟踪方法。

Description

一种平滑约束无迹卡尔曼滤波方法及目标跟踪方法
技术领域
本发明涉及目标跟踪技术领域领域，具体而言，涉及一种平滑约束无迹卡尔曼滤波方法及目标跟踪方法。
背景技术
在导航和制导系统、跟踪快速变化的无线信道的信道状态信息、跟踪飞机的实时位置等科学领域中经常需要使用滤波技术(如卡尔曼滤波等)以实现对目标的实时跟踪。
经过大量检索发现一些典型的现有技术，如申请号为201810661518.8的专利公开了一种基于开关卡尔曼滤波器的运动目标跟踪方法，该发明在目标运动状态产生突变的情况下，能够抑制因目标运动状态产生突变导致的跟踪误差，具有稳定的跟踪效果和较好的鲁棒性。又如申请号为201410134331.4的专利公开了种目标跟踪方法及扩展截断无迹卡尔曼滤波方法、装置，该发明能够解决观测函数不具有唯一反函数的问题，有效提高滤波精度且实用性较高。又如申请号为201410779717.0公开了一种于轨迹平滑的室内移动目标定位方法，该发明充分利用当前时刻与先前时刻的测量值和估计值，采用无迹卡尔曼滤波方式，提高了三边测量法的定位精度。
可见，对于使用卡尔曼滤波进行目标跟踪，其实际应用中的亟待处理的实际问题(如提高目标跟踪的准确性和鲁棒性等)还有很多未提出具体的解决方案。
发明内容
为了克服现有技术的不足提供了一种平滑约束无迹卡尔曼滤波方法及目标跟踪方法，本发明的具体技术方案如下：
一种平滑约束无迹卡尔曼滤波方法，包括如下步骤：
步骤1，根据无迹变换获取当前目标观测时刻目标状态的原始先验概率密度函数；
步骤2，通过数值期望计算原始先验概率的均值和方差；
步骤3，引入噪声约束信息，通过计算近似可行域的中心，以获取修正先验概率密度；
步骤4，通过后验迭代优化寻求满足约束条件的高斯分布均值和方差，产生满足约束条件的新的高斯西格玛点；
步骤5，对高斯西格玛点进行加权计算，完成滤波过程。
可选的，在步骤1中，根据无迹变换获取当前目标观测时刻目标状态的原始先验概率密度函数的步骤具体包括：
步骤1a，根据采样规则，利用修正先验概率的均值和方差产生满足约束条件的2nx+1个西格玛点以及相应的权值wi，具体如下式所示：
Figure BDA0002273774030000021
其中，nx是状态变量的维数，λ是尺寸因子，λ决定周围西格玛点的范围，可以为nx+λ≠0的任意值，
Figure BDA0002273774030000022
Figure BDA0002273774030000023
是(nx+λ)Px均方根的第i列，该均方根的求解可由Cholesky分解得到,wi是第i个粒子的权值，并且
Figure BDA0002273774030000024
步骤1b，将西格玛点代入状态方程，利用公式
Figure BDA0002273774030000025
以及
Figure BDA0002273774030000031
加权计算预测均值
Figure BDA0002273774030000032
和预测协方差Px；
步骤1c,时间更新，利用公式
Figure BDA0002273774030000033
Figure BDA0002273774030000034
以及
Figure BDA0002273774030000035
加权计算量测均值
Figure BDA0002273774030000036
预测协方差Pzz与测量的互协方差Pxz。
可选的，在步骤2中，通过数值期望计算原始先验概率的均值和方差的公式为：
Figure BDA0002273774030000037
以及
Figure BDA0002273774030000038
可选的，在步骤3中，引入噪声约束信息，通过计算近似可行域的中心，以获取修正先验概率密度的步骤具体为：
步骤3a,在可行域内，设加性噪声的概率密度函数具有有界的连通支持，即p(ek)＝0,
Figure BDA0002273774030000039
其中
Figure BDA00022737740300000310
表示满足约束条件的状态的n维联通区域可行区域；
步骤3b，根据最大后验估计理论，定义可行域的中心点为
Figure BDA00022737740300000311
其中上标c表示约束，K表示总观测时刻；
步骤3c,设
Figure BDA00022737740300000312
并计算
Figure BDA00022737740300000313
的近似初始优化值，采用非线性规划方法近似求解全局最小值，其中
Figure BDA00022737740300000314
和
Figure BDA00022737740300000315
分别表示
Figure BDA00022737740300000316
在
Figure BDA00022737740300000317
处的雅克比和海森矩阵；
步骤3d，选择待估计参量的L1范数为障碍函数，将目标函数可＝表达为求解一个无约束凸优化序列的形式，具体如下：
Figure BDA0002273774030000041
步骤3e，设
Figure BDA0002273774030000042
如果均值在可行域，计算均值并将其定为初值，即
Figure BDA0002273774030000043
否则，取权值最大的数值作为初值，即
Figure BDA0002273774030000044
步骤3f,通过拟牛顿迭代方法遍历搜索全局最优解为
Figure BDA0002273774030000045
其中α*是步长，di是搜索方向，若
Figure BDA0002273774030000046
则令α*＝1；
步骤3g，由可行域的中心，计算其一阶矩作为正态分布方差
Figure BDA0002273774030000047
获取修正的先验概率密度为
Figure BDA0002273774030000048
可选的，在步骤4中，通过后验迭代优化寻求满足约束条件的高斯分布均值和方差，产生满足约束条件的新的高斯西格玛点的步骤具体包括：
步骤4a，根据采样规则，利用修正先验概率的均值和方差产生满足约束条件的2nx+1个sigma点
Figure BDA0002273774030000049
以及相应的权值
Figure BDA00022737740300000410
具体如下式所示：
Figure BDA00022737740300000411
其中，nx是状态维数，λ是尺寸因子，可以为nX+λ≠0的任意值，
Figure BDA00022737740300000412
是(nx+λ)Px均方根的第i列，wi是第i个粒子的权值，并且
Figure BDA00022737740300000413
步骤4b，将西格玛点代入状态方程，利用公式
Figure BDA00022737740300000414
以及
Figure BDA0002273774030000051
加权计算预测均值
Figure BDA0002273774030000052
和预测协方差Px；
步骤4c,加权计算量测均值
Figure BDA0002273774030000053
通过公式
Figure BDA0002273774030000054
Figure BDA0002273774030000055
以及
Figure BDA0002273774030000056
预测协方差pzz以及状态与测量的互协方差pxz，；
步骤4d,利用公式
Figure BDA0002273774030000057
以及
Figure BDA0002273774030000058
计算增益Kk+1，状态均值
Figure BDA0002273774030000059
和方差Pk+1。
本发明还提供一种目标跟踪方法，包括如下步骤：
步骤1，对目标进行感测以获取当前目标观测时刻的目标观测向量；
步骤2，根据无迹变换获取当前目标观测时刻目标状态的原始先验概率密度函数；
步骤3，通过数值期望计算原始先验概率的均值和方差；
步骤4，引入噪声约束信息，通过计算近似可行域的中心，以获取修正先验概率密度；
步骤5，通过后验迭代优化寻求满足约束条件的高斯分布均值和方差，产生满足约束条件的新的高斯西格玛点；
步骤6，对高斯西格玛点进行加权计算，完成滤波过程；
步骤7，输出当前目标观测时刻的目标状态估计值，以实现对目标的跟踪。
本发明所取得的有益效果包括：
1、所述的一种平滑约束无迹卡尔曼滤波方法的均方根误差最小，在准确性和鲁棒性方面具有优势。同时，其实时性方面优于粒子滤波算法；
2、所述的一种平滑约束无迹卡尔曼滤波方法及目标跟踪方法，避免了模型细化以及雅可比计算，在保证准确性的同时有效提高了运算效率。
附图说明
从以下结合附图的描述可以进一步理解本发明，将重点放在示出实施例的原理上。
图1是本发明实施例之一中一种平滑约束无迹卡尔曼滤波方法的流程示意图；
图2是本发明实施例之一中不同滤波器均方根对比的示意图；
图3是本发明实施例之一中目标状态后验概率密度分布示意图一；
图4是本发明实施例之一中目标状态后验概率密度分布示意图二；
图5是本发明实施例之一中一种目标跟踪方法的流程示意图；
图6是本发明实施例之一中仿真轨迹设计示意图；
图7是本发明实施例之一不同观测噪声下的滤波误差分析对比示意图一；
图8是本发明实施例之一不同观测噪声下的滤波误差分析对比示意图二；
图9是本发明实施例之一不同观测噪声下的滤波误差分析对比示意图三。
具体实施方式
为了使得本发明的目的、技术方案及优点更加清楚明白，以下结合其实施例，对本发明进行进一步详细说明；应当理解，此处所描述的具体实施例仅用于解释本发明，并不用于限定本发明。对于本领域技术人员而言，在查阅以下详细描述之后，本实施例的其它系统、方法和/或特征将变得显而易见。旨在所有此类附加的系统、方法、特征和优点都包括在本说明书内、包括在本发明的范围内，并且受所附权利要求书的保护。在以下详细描述描述了所公开的实施例的另外的特征，并且这些特征根据以下将详细描述将是显而易见的。
本发明实施例的附图中相同或相似的标号对应相同或相似的部件；在本发明的描述中，需要理解的是，若有术语“上”、“下”、“左”、“右”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或组件必须具有特定的方位、以特定的方位构造和操作，因此附图中描述位置关系的用语仅用于示例性说明，不能理解为对本专利的限制，对于本领域的普通技术人员而言，可以根据具体情况理解上述术语的具体含义。
本发明为一种平滑约束无迹卡尔曼滤波方法及目标跟踪方法，根据附图所示讲述以下实施例：
实施例一：
非线性动态系统由噪声损坏的量测数据估计目标的状态，量测非线性和不确定性是两个相互关联的主要挑战。一阶扩展卡尔曼滤波器(EKF)广泛用于非线性滤波，该方法基于泰勒级数思想，易于实现。由于线性化带来不可避免的估计误差，对于强非线性动态系统，EKF的估计性能下降并可能产生发散。无迹卡尔曼滤波器(UKF)算法使用高斯点逼近后验分布，避免了非线性函数的线性化计算，估计均值及协方差更为准确。粒子滤波器(PF)可以有效解决非高斯、非线性滤波问题，基本思想是通过带权重的采样粒子以在线方式完整地表示估计状态的后验分布，粒子贫化及计算量大是有待解决的问题。近年来，诸多约束状态估计算法相继涌现。通常，没有单一的最优方法将约束结合到非线性动态状态估计过程中，较为常见的方法有截断法，数值优化法等。
无迹变换是有效处理非线性的方法，能够提供粒子滤波的精度且能够实时运算，在此基础上，本实施例提出了一种平滑约束无迹卡尔曼(SCUKF)滤波方法，其通过将量测软约束信息及最新量测信息同时融入状态更新过程，采用凸优化方法构建目标函数，同时将当前观测信息融入到更新过程，可以提高计算效率并平滑更新过程中可能存在的野值问题。
如图1所示，一种平滑约束无迹卡尔曼滤波方法，包括如下步骤：
步骤1，根据无迹变换获取当前目标观测时刻目标状态的原始先验概率密度函数；
步骤2，通过数值期望计算原始先验概率的均值和方差；
步骤3，引入噪声约束信息，通过计算近似可行域的中心，以获取修正先验概率密度；
步骤4，通过后验迭代优化寻求满足约束条件的高斯分布均值和方差，产生满足约束条件的新的高斯西格玛点；
步骤5，对高斯西格玛点进行加权计算，完成滤波过程。
其中，在步骤1中，根据无迹变换获取当前目标观测时刻目标状态的原始先验概率密度函数的步骤具体包括：
步骤1a，根据采样规则，利用修正先验概率的均值和方差产生满足约束条件的2nx+1个西格玛点以及相应的权值wi，具体如下式所示：
Figure BDA0002273774030000091
其中，nx是状态变量的维数，λ是尺寸因子，λ决定周围西格玛点的范围，可以为nx+λ≠0的任意值，
Figure BDA0002273774030000092
Figure BDA0002273774030000093
是(nx+λ)Px均方根的第i列，该均方根的求解可由Cholesky分解得到,wi是第i个粒子的权值，并且
Figure BDA0002273774030000094
步骤1b，将西格玛点代入状态方程，利用公式
Figure BDA0002273774030000095
以及
Figure BDA0002273774030000096
加权计算预测均值
Figure BDA0002273774030000097
和预测协方差Px；
步骤1c,时间更新，利用公式
Figure BDA0002273774030000098
Figure BDA0002273774030000099
以及
Figure BDA00022737740300000910
加权计算量测均值
Figure BDA00022737740300000911
预测协方差Pzz与测量的互协方差Pxz。
其中，在步骤2中，通过数值期望计算原始先验概率的均值和方差的公式为：
Figure BDA00022737740300000912
以及
Figure BDA00022737740300000913
其中，在步骤3中，引入噪声约束信息，通过计算近似可行域的中心，以获取修正先验概率密度的步骤具体为：
步骤3a,在可行域内，设加性噪声的概率密度函数具有有界的连通支持，即p(ek)＝0,
Figure BDA00022737740300000914
其中
Figure BDA00022737740300000915
表示满足约束条件的状态的n维联通区域可行区域；
步骤3b，根据最大后验估计理论，定义可行域的中心点为
Figure BDA0002273774030000101
其中上标c表示约束，K表示总观测时刻；
步骤3c,设
Figure BDA0002273774030000102
并计算
Figure BDA0002273774030000103
的近似初始优化值，采用非线性规划方法近似求解全局最小值，其中
Figure BDA0002273774030000104
和
Figure BDA0002273774030000105
分别表示
Figure BDA0002273774030000106
在
Figure BDA0002273774030000107
处的雅克比和海森矩阵；
步骤3d，选择待估计参量的L1范数为障碍函数，将目标函数可＝表达为求解一个无约束凸优化序列的形式，具体如下：
Figure BDA0002273774030000108
步骤3e，设
Figure BDA0002273774030000109
如果均值在可行域，计算均值并将其定为初值，即
Figure BDA00022737740300001010
否则，取权值最大的数值作为初值，即
Figure BDA00022737740300001011
步骤3f,通过拟牛顿迭代方法遍历搜索全局最优解为
Figure BDA00022737740300001012
其中α*是步长，di是搜索方向，若
Figure BDA00022737740300001013
则令α*＝1；
步骤3g，由可行域的中心，计算其一阶矩作为正态分布方差
Figure BDA00022737740300001014
获取修正的先验概率密度为
Figure BDA00022737740300001015
其中，在步骤4中，通过后验迭代优化寻求满足约束条件的高斯分布均值和方差，产生满足约束条件的新的高斯西格玛点的步骤具体包括：
步骤4a，根据采样规则，利用修正先验概率的均值和方差产生满足约束条件的2nx+1个sigma点
Figure BDA00022737740300001016
以及相应的权值
Figure BDA00022737740300001017
具体如下式所示：
Figure BDA0002273774030000111
其中，nx是状态维数，λ是尺寸因子，可以为nX+λ≠0的任意值，
Figure BDA0002273774030000112
是(nx+λ)Px均方根的第i列，wi是第i个粒子的权值，并且
Figure BDA0002273774030000113
步骤4b，将西格玛点代入状态方程，利用公式
Figure BDA0002273774030000114
以及
Figure BDA0002273774030000115
加权计算预测均值
Figure BDA0002273774030000116
和预测协方差Px；
步骤4c,加权计算量测均值
Figure BDA0002273774030000117
通过公式
Figure BDA0002273774030000118
Figure BDA0002273774030000119
以及
Figure BDA00022737740300001110
预测协方差pzz以及状态与测量的互协方差pxz，；
步骤4d,利用公式
Figure BDA00022737740300001111
以及
Figure BDA00022737740300001112
计算增益Kk+1，状态均值
Figure BDA00022737740300001113
和方差Pk+1。
动态系统模型中状态随时间变化过程可描述为一个离散马尔科夫过程xk＝fk(xk-1)+vk以及zk＝hk(xk)+ek。其中，k表示离散时间，xk和zk分别是k时刻系统的状态和观测序列。fk，hk分别表示相应量测空间上的一些确定的非线性函数。vk和ek分别是相互独立的过程噪声和观测噪声，为了统计简单，将其假设为加性白高斯噪声。
在实践中，噪声总是有界的，因为没有噪声可以提供无限大的值。在可行区域内，可合理假设加性噪声的概率密度函数具有有界的连通支持，数学表述为：
Figure BDA0002273774030000121
其中，
Figure BDA0002273774030000122
表示满足约束条件的状态的n维联通区域可行区域。
考虑量测软约束条件，根据贝叶斯公式可得，修正先验概率P1(·)是原始先验概率P0(·)的截断，可表示为：
Figure BDA0002273774030000123
其中，εk是归一化常数，pg(·)是指示函数。
非线性观测约束信息作为先验知识，只影响目标的位置矢量。不同于硬约束，软约束仅需要近似满足而不是完全满足边界条件。根据最大后验估计理论，定义可行域的中心点为：
Figure BDA0002273774030000124
式中，上标c表示约束。
定义
Figure BDA0002273774030000125
Figure BDA0002273774030000126
分别表示
Figure BDA0002273774030000127
在
Figure BDA0002273774030000128
处的雅克比和海森矩阵。将其展开成泰勒二阶级数来得到
Figure BDA0002273774030000129
的近似初始优化值。
本实施例在传统UKF算法的基础上，引入观测噪声约束信息，将修正先验概率限制在能反映观测输入变化的有效区域，将其扩展成平滑约束UKF；通过后验迭代优化寻求满足约束条件的高斯分布均值和方差，产生满足约束条件的新的高斯sigma点，对其进行加权以进行滤波估计。
为了比较本实施所述的一种平滑约束无迹卡尔曼滤波方法的估计性能，对于高度非线性、非静态的动态离散时间系统进行仿真比较，公式可表示为：
Figure BDA00022737740300001210
其中vk是过程噪声，假设是零均值高斯分布；观测噪声ek零均值、方差为0.01的高斯分布。模型系数为常数α＝1,β＝1,γ＝0.5。对于每次蒙特卡洛仿真，取状态初值x0为[0,1]间的均匀分布。基于粒子滤波的粒子数目取为300。
图2定性分析比较了几种算法的均方根位置误差。下表定量总结了均值、方差及100次运行时间的比较结果。
Figure BDA0002273774030000131
如同预期，均方根误差的均值及方差统计结果从大到小依次是扩展卡尔曼(EKF)，无迹卡尔曼(UKF)，粒子滤波(PF)，扩展卡尔曼粒子滤波(PF-EKF)，无迹卡尔曼粒子滤波(PF-UKF)，平滑约束无迹卡尔曼(SCUKF)。在该实验条件下，本实施例所述的一种平滑约束无迹卡尔曼滤波方法的均方根误差最小，在准确性和鲁棒性方面具有优势。同时，其实时性方面优于粒子滤波算法。
图3以及图4给出了后验概率密度分布图示。相比先验分布，后验概率分布明显集中在有效区域，也因此说明了利用约束信息的必要性。图6、图7、图8以及图9给出了不同观测噪声下的滤波误差分析，其中图6为仿真轨迹设计。根据图6、图7、图8以及图9可以知道，和截断无迹卡尔曼滤波算法以及克拉美罗界进行对比，随着观测噪声增大，滤波精度下降，而本实施例所述的一种平滑约束无迹卡尔曼滤波方法表现出稳定性。这主要是因为对观测噪声的约束截断，同时，通过数值优化近似全局最优解，误差可达到克拉美罗界值附近。
针对非线性动态系统中估计的非线性及不确定性问题，本实施例提出了一种平滑约束无迹卡尔曼滤波方法。通过有效利用约束先验知识将后验概率分布集中在有效区域，通过数值优化的方法统计递归地近似线性化观测方程，近似满足约束条件的截断先验概率为高斯分布，从其均值及方差中，采样满足约束条件的高斯采样点，进行无迹变换，以得到滤波输出。
实施例二：
如图5所示，与实施例一所述的一种平滑约束无迹卡尔曼滤波方法相对应，本实施例提供了一种目标跟踪方法，其包括如下步骤：
步骤1，对目标进行感测以获取当前目标观测时刻的目标观测向量；在这里，可以利用传感器对目标进行感测以获取当前目标观测时刻的目标观测量，其中，传感器具体可以是红外或者雷达等。
步骤2，根据无迹变换获取当前目标观测时刻目标状态的原始先验概率密度函数；
步骤3，通过数值期望计算原始先验概率的均值和方差；
步骤4，引入噪声约束信息，通过计算近似可行域的中心，以获取修正先验概率密度；
步骤5，通过后验迭代优化寻求满足约束条件的高斯分布均值和方差，产生满足约束条件的新的高斯西格玛点；
步骤6，对高斯西格玛点进行加权计算，完成滤波过程；
步骤7，输出当前目标观测时刻的目标状态估计值，以实现对目标的跟踪。跟踪的目标可以是飞机、航空飞行器或者车辆等快速移动物件。
其中，在步骤2中，根据无迹变换获取当前目标观测时刻目标状态的原始先验概率密度函数的步骤具体包括：
步骤1a，根据采样规则，利用修正先验概率的均值和方差产生满足约束条件的2nx+1个西格玛点以及相应的权值wi，具体如下式所示：
Figure BDA0002273774030000151
其中，nx是状态变量的维数，λ是尺寸因子，λ决定周围西格玛点的范围，可以为nx+λ≠0的任意值，
Figure BDA0002273774030000152
Figure BDA0002273774030000153
是(nx+λ)Px均方根的第i列，该均方根的求解可由Cholesky分解得到,wi是第i个粒子的权值，并且
Figure BDA0002273774030000154
步骤1b，将西格玛点代入状态方程，利用公式
Figure BDA0002273774030000155
以及
Figure BDA0002273774030000156
加权计算预测均值
Figure BDA0002273774030000157
和预测协方差Px；
步骤1c,时间更新，利用公式
Figure BDA0002273774030000158
Figure BDA0002273774030000159
以及
Figure BDA00022737740300001510
加权计算量测均值
Figure BDA00022737740300001511
预测协方差Pzz与测量的互协方差Pxz。
其中，在步骤3中，通过数值期望计算原始先验概率的均值和方差的公式为：
Figure BDA00022737740300001512
以及
Figure BDA00022737740300001513
其中，在步骤4中，引入噪声约束信息，通过计算近似可行域的中心，以获取修正先验概率密度的步骤具体为：
步骤3a,在可行域内，设加性噪声的概率密度函数具有有界的连通支持，即
Figure BDA00022737740300001514
其中
Figure BDA00022737740300001515
表示满足约束条件的状态的n维联通区域可行区域；
步骤3b，根据最大后验估计理论，定义可行域的中心点为
Figure BDA0002273774030000161
其中上标c表示约束，K表示总观测时刻；
步骤3c,设
Figure BDA0002273774030000162
并计算
Figure BDA0002273774030000163
的近似初始优化值，采用非线性规划方法近似求解全局最小值，其中
Figure BDA0002273774030000164
和
Figure BDA0002273774030000165
分别表示
Figure BDA0002273774030000166
在
Figure BDA0002273774030000167
处的雅克比和海森矩阵；
步骤3d，选择待估计参量的L1范数为障碍函数，将目标函数可＝表达为求解一个无约束凸优化序列的形式，具体如下：
Figure BDA0002273774030000168
步骤3e，设
Figure BDA0002273774030000169
如果均值在可行域，计算均值并将其定为初值，即
Figure BDA00022737740300001610
否则，取权值最大的数值作为初值，即
Figure BDA00022737740300001611
步骤3f,通过拟牛顿迭代方法遍历搜索全局最优解为
Figure BDA00022737740300001612
其中α*是步长，di是搜索方向，若
Figure BDA00022737740300001613
则令α*＝1；
步骤3g，由可行域的中心，计算其一阶矩作为正态分布方差
Figure BDA00022737740300001614
获取修正的先验概率密度为
Figure BDA00022737740300001615
其中，在步骤5中，通过后验迭代优化寻求满足约束条件的高斯分布均值和方差，产生满足约束条件的新的高斯西格玛点的步骤具体包括：
步骤4a，根据采样规则，利用修正先验概率的均值和方差产生满足约束条件的2nx+1个sigma点
Figure BDA00022737740300001616
以及相应的权值
Figure BDA00022737740300001617
具体如下式所示：
Figure BDA0002273774030000171
其中，nx是状态维数，λ是尺寸因子，可以为nX+λ≠0的任意值，
Figure BDA0002273774030000172
是(nx+λ)Px均方根的第i列，wi是第i个粒子的权值，并且
Figure BDA0002273774030000173
步骤4b，将西格玛点代入状态方程，利用公式
Figure BDA0002273774030000174
以及
Figure BDA0002273774030000175
加权计算预测均值
Figure BDA0002273774030000176
和预测协方差Px；
步骤4c,加权计算量测均值
Figure BDA0002273774030000177
通过公式
Figure BDA0002273774030000178
Figure BDA0002273774030000179
以及
Figure BDA00022737740300001710
预测协方差pzz以及状态与测量的互协方差pxz，；
步骤4d,利用公式
Figure BDA00022737740300001711
以及
Figure BDA00022737740300001712
计算增益Kk+1，状态均值
Figure BDA00022737740300001713
和方差Pk+1。
纯方位机动目标跟踪是典型的非线性估计问题，量测非线性和目标机动运动的不确定性是两个相互关联的主要挑战。对于约束多模型粒子滤波方法，该方法在非均匀稀疏采样环境下能够有效跟踪机动目标，不足之处是运算量过大，难以实现实时跟踪。本实施例所述的一种平滑约束无迹卡尔曼滤波方法及目标跟踪方法，避免了模型细化以及雅可比计算，在保证准确性的同时有效提高了运算效率。
综上所述，本发明公开的一种平滑约束无迹卡尔曼滤波方法及目标跟踪方法，所产生的有益技术效果包括：
1、所述的一种平滑约束无迹卡尔曼滤波方法的均方根误差最小，在准确性和鲁棒性方面具有优势。同时，其实时性方面优于粒子滤波算法；
2、所述的一种平滑约束无迹卡尔曼滤波方法及目标跟踪方法，避免了模型细化以及雅可比计算，在保证准确性的同时有效提高了运算效率。
虽然上面已经参考各种实施例描述了本发明，但是应当理解，在不脱离本发明的范围的情况下，可以进行许多改变和修改。也就是说上面讨论的方法、系统和设备是示例，各种配置可以适当地省略、替换或添加各种过程或组件。例如，在替代配置中，可以以与所描述的顺序不同的顺序执行方法和/或可以添加、省略和/或组合各种部件。而且，关于某些配置描述的特征可以以各种其他配置组合，如可以以类似的方式组合配置的不同方面和元素。此外，随着技术发展其中的元素可以更新，即许多元素是示例，并不限制本发明公开或权利要求的范围。
在说明书中给出了具体细节以提供对包括实现的示例性配置的透彻理解。然而，可以在没有这些具体细节的情况下实践配置,例如已经示出了众所周知的电路、过程、算法、结构和技术而没有不必要的细节，以避免模糊配置。该描述仅提供示例配置，并且不限制权利要求的范围，适用性或配置。相反，前面对配置的描述将为本领域技术人员提供用于实现所描述的技术的使能描述。在不脱离本发明公开的精神或范围的情况下，可以对元件的功能和布置进行各种改变。
综上，其旨在上述详细描述被认为是例示性的而非限制性的，并且应当理解，以下权利要求(包括所有等同物)旨在限定本发明的精神和范围。以上这些实施例应理解为仅用于说明本发明而不用于限制本发明的保护范围。在阅读了本发明的记载的内容之后，技术人员可以对本发明作各种改动或修改，这些等效变化和修饰同样落入本发明权利要求所限定的范围。

Claims (10)

1.一种平滑约束无迹卡尔曼滤波方法，其特征在于，包括如下步骤：
步骤1，根据无迹变换获取当前目标观测时刻目标状态的原始先验概率密度函数；
步骤2，通过数值期望计算原始先验概率的均值和方差；
步骤3，引入噪声约束信息，通过计算近似可行域的中心，以获取修正先验概率密度；
步骤4，通过后验迭代优化寻求满足约束条件的高斯分布均值和方差，产生满足约束条件的新的高斯西格玛点；
步骤5，对高斯西格玛点进行加权计算，完成滤波过程。
2.根据权利要求1所述的一种平滑约束无迹卡尔曼滤波方法，其特征在于，在步骤1中，根据无迹变换获取当前目标观测时刻目标状态的原始先验概率密度函数的步骤具体包括：
步骤1a，根据采样规则，利用修正先验概率的均值和方差产生满足约束条件的2nx+1个西格玛点以及相应的权值wi，具体如下式所示：
Figure FDA0002273774020000011
其中，nx是状态变量的维数，λ是尺寸因子，λ决定周围西格玛点的范围，可以为nx+λ≠0的任意值，
Figure FDA0002273774020000015
Figure FDA0002273774020000012
是(nx+λ)Px均方根的第i列，该均方根的求解可由Cholesky分解得到,wi是第i个粒子的权值，并且
Figure FDA0002273774020000013
步骤1b，将西格玛点代入状态方程，利用公式
Figure FDA0002273774020000014
以及
Figure FDA0002273774020000021
加权计算预测均值
Figure FDA0002273774020000022
和预测协方差Px；
步骤1c,时间更新，利用公式
Figure FDA0002273774020000023
Figure FDA0002273774020000024
以及
Figure FDA0002273774020000025
加权计算量测均值
Figure FDA0002273774020000026
预测协方差Pzz与测量的互协方差Pxz。
3.根据权利要求2所述的一种平滑约束无迹卡尔曼滤波方法，其特征在于，在步骤2中，通过数值期望计算原始先验概率的均值和方差的公式为：
Figure FDA0002273774020000027
以及
Figure FDA0002273774020000028
4.根据权利要求3所述的一种平滑约束无迹卡尔曼滤波方法，其特征在于，在步骤3中，引入噪声约束信息，通过计算近似可行域的中心，以获取修正先验概率密度的步骤具体为：
步骤3a,在可行域内，设加性噪声的概率密度函数具有有界的连通支持，即p(ek)＝0,
Figure FDA0002273774020000029
其中
Figure FDA00022737740200000210
表示满足约束条件的状态的n维联通区域可行区域；
步骤3b，根据最大后验估计理论，定义可行域的中心点为
Figure FDA00022737740200000211
其中上标c表示约束，K表示总观测时刻；
步骤3c,设
Figure FDA00022737740200000212
并计算
Figure FDA00022737740200000213
的近似初始优化值，采用非线性规划方法近似求解全局最小值，其中
Figure FDA00022737740200000214
和
Figure FDA00022737740200000215
分别表示
Figure FDA00022737740200000216
在
Figure FDA00022737740200000217
处的雅克比和海森矩阵；
步骤3d，选择待估计参量的L1范数为障碍函数，将目标函数可＝表达为求解一个无约束凸优化序列的形式，具体如下：
Figure FDA0002273774020000031
步骤3e，设
Figure FDA0002273774020000032
如果均值在可行域，计算均值并将其定为初值，即
Figure FDA0002273774020000033
否则，取权值最大的数值作为初值，即
Figure FDA0002273774020000034
步骤3f,通过拟牛顿迭代方法遍历搜索全局最优解为
Figure FDA0002273774020000035
其中α*是步长，di是搜索方向，若
Figure FDA0002273774020000036
则令α*＝1；
步骤3g，由可行域的中心，计算其一阶矩作为正态分布方差
Figure FDA0002273774020000037
获取修正的先验概率密度为
Figure FDA0002273774020000038
5.根据权利要求4所述的一种平滑约束无迹卡尔曼滤波方法，其特征在于，在步骤4中，通过后验迭代优化寻求满足约束条件的高斯分布均值和方差，产生满足约束条件的新的高斯西格玛点的步骤具体包括：
步骤4a，根据采样规则，利用修正先验概率的均值和方差产生满足约束条件的2nx+1个sigma点
Figure FDA0002273774020000039
以及相应的权值
Figure FDA00022737740200000310
具体如下式所示：
Figure FDA00022737740200000311
Figure FDA00022737740200000312
Figure FDA00022737740200000313
其中，nx是状态维数，λ是尺寸因子，可以为nX+λ≠0的任意值，
Figure FDA00022737740200000314
是(nx+λ)Px均方根的第i列，wi是第i个粒子的权值，并且
Figure FDA00022737740200000315
步骤4b，将西格玛点代入状态方程，利用公式
Figure FDA0002273774020000041
以及
Figure FDA0002273774020000042
加权计算预测均值
Figure FDA0002273774020000043
和预测协方差Px；
步骤4c,加权计算量测均值
Figure FDA0002273774020000044
通过公式
Figure FDA0002273774020000045
Figure FDA0002273774020000046
以及
Figure FDA0002273774020000047
预测协方差pzz以及状态与测量的互协方差pxz，；
步骤4d,利用公式
Figure FDA0002273774020000048
以及
Figure FDA0002273774020000049
计算增益Kk+1，状态均值
Figure FDA00022737740200000410
和方差Pk+1。
6.一种目标跟踪方法，其特征在于，包括如下步骤：
步骤1，对目标进行感测以获取当前目标观测时刻的目标观测向量；
步骤2，根据无迹变换获取当前目标观测时刻目标状态的原始先验概率密度函数；
步骤3，通过数值期望计算原始先验概率的均值和方差；
步骤4，引入噪声约束信息，通过计算近似可行域的中心，以获取修正先验概率密度；
步骤5，通过后验迭代优化寻求满足约束条件的高斯分布均值和方差，产生满足约束条件的新的高斯西格玛点；
步骤6，对高斯西格玛点进行加权计算，完成滤波过程；
步骤7，输出当前目标观测时刻的目标状态估计值，以实现对目标的跟踪。
7.根据权利要求6所述的一种目标跟踪方法，其特征在于，在步骤2中，根据无迹变换获取当前目标观测时刻目标状态的原始先验概率密度函数的步骤具体包括：
步骤1a，根据采样规则，利用修正先验概率的均值和方差产生满足约束条件的2nx+1个西格玛点以及相应的权值wi，具体如下式所示：
Figure FDA0002273774020000051
其中，nx是状态变量的维数，λ是尺寸因子，λ决定周围西格玛点的范围，可以为nx+λ≠0的任意值，
Figure FDA0002273774020000052
Figure FDA0002273774020000053
是(nx+λ)Px均方根的第i列，该均方根的求解可由Cholesky分解得到,wi是第i个粒子的权值，并且
Figure FDA0002273774020000054
步骤1b，将西格玛点代入状态方程，利用公式
Figure FDA0002273774020000055
以及
Figure FDA0002273774020000056
加权计算预测均值
Figure FDA0002273774020000057
和预测协方差Px；
步骤1c,时间更新，利用公式
Figure FDA0002273774020000058
Figure FDA0002273774020000059
以及
Figure FDA00022737740200000510
加权计算量测均值
Figure FDA00022737740200000511
预测协方差Pzz与测量的互协方差Pxz。
8.根据权利要求7所述的一种目标跟踪方法，其特征在于，在步骤3中，通过数值期望计算原始先验概率的均值和方差的公式为：
Figure FDA0002273774020000061
以及
Figure FDA0002273774020000062
9.根据权利要求8所述的一种目标跟踪方法，其特征在于，在步骤4中，引入噪声约束信息，通过计算近似可行域的中心，以获取修正先验概率密度的步骤具体为：
步骤3a,在可行域内，设加性噪声的概率密度函数具有有界的连通支持，即p(ek)＝0,
Figure FDA0002273774020000063
其中
Figure FDA0002273774020000064
表示满足约束条件的状态的n维联通区域可行区域；
步骤3b，根据最大后验估计理论，定义可行域的中心点为
Figure FDA0002273774020000065
其中上标c表示约束，K表示总观测时刻；
步骤3c,设
Figure FDA0002273774020000066
并计算
Figure FDA0002273774020000067
的近似初始优化值，采用非线性规划方法近似求解全局最小值，其中
Figure FDA0002273774020000068
和
Figure FDA0002273774020000069
分别表示
Figure FDA00022737740200000610
在
Figure FDA00022737740200000611
处的雅克比和海森矩阵；
步骤3d，选择待估计参量的L1范数为障碍函数，将目标函数可＝表达为求解一个无约束凸优化序列的形式，具体如下：
Figure FDA00022737740200000612
步骤3e，设
Figure FDA00022737740200000613
如果均值在可行域，计算均值并将其定为初值，即
Figure FDA00022737740200000614
否则，取权值最大的数值作为初值，即
Figure FDA00022737740200000615
步骤3f,通过拟牛顿迭代方法遍历搜索全局最优解为
Figure FDA0002273774020000071
其中α*是步长，di是搜索方向，若
Figure FDA0002273774020000072
则令α*＝1；
步骤3g，由可行域的中心，计算其一阶矩作为正态分布方差
Figure FDA0002273774020000073
获取修正的先验概率密度为
Figure FDA0002273774020000074
10.根据权利要求9所述的一种目标跟踪方法，其特征在于，在步骤5中，通过后验迭代优化寻求满足约束条件的高斯分布均值和方差，产生满足约束条件的新的高斯西格玛点的步骤具体包括：
步骤4a，根据采样规则，利用修正先验概率的均值和方差产生满足约束条件的2nx+1个sigma点
Figure FDA0002273774020000075
以及相应的权值
Figure FDA0002273774020000076
具体如下式所示：
Figure FDA0002273774020000077
Figure FDA0002273774020000078
Figure FDA0002273774020000079
其中，nx是状态维数，λ是尺寸因子，可以为nX+λ≠0的任意值，
Figure FDA00022737740200000710
是(nx+λ)Px均方根的第i列，wi是第i个粒子的权值，并且
Figure FDA00022737740200000711
步骤4b，将西格玛点代入状态方程，利用公式
Figure FDA00022737740200000712
以及
Figure FDA00022737740200000713
加权计算预测均值
Figure FDA00022737740200000714
和预测协方差Px；
步骤4c,加权计算量测均值
Figure FDA00022737740200000715
通过公式
Figure FDA00022737740200000716
Figure FDA00022737740200000717
以及
Figure FDA0002273774020000081
预测协方差pzz以及状态与测量的互协方差pxz，；
步骤4d,利用公式
Figure FDA0002273774020000082
以及
Figure FDA0002273774020000083
计算增益Kk+1，状态均值
Figure FDA0002273774020000084
和方差Pk+1。
CN201911114914.XA 2019-11-14 2019-11-14 一种平滑约束无迹卡尔曼滤波方法及目标跟踪方法 Pending CN111047627A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911114914.XA CN111047627A (zh) 2019-11-14 2019-11-14 一种平滑约束无迹卡尔曼滤波方法及目标跟踪方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911114914.XA CN111047627A (zh) 2019-11-14 2019-11-14 一种平滑约束无迹卡尔曼滤波方法及目标跟踪方法

Publications (1)

Publication Number Publication Date
CN111047627A true CN111047627A (zh) 2020-04-21

Family

ID=70232898

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911114914.XA Pending CN111047627A (zh) 2019-11-14 2019-11-14 一种平滑约束无迹卡尔曼滤波方法及目标跟踪方法

Country Status (1)

Country Link
CN (1) CN111047627A (zh)

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111711432A (zh) * 2020-06-16 2020-09-25 桂林理工大学 一种基于ukf和pf混合滤波的目标跟踪算法
CN111985093A (zh) * 2020-08-03 2020-11-24 哈尔滨工程大学 一种带噪声估计器的自适应无迹卡尔曼滤波状态估计方法
CN112990124A (zh) * 2021-04-26 2021-06-18 湖北亿咖通科技有限公司 一种车辆跟踪方法、装置、电子设备及存储介质
CN113452349A (zh) * 2021-06-28 2021-09-28 中山大学 一种基于贝叶斯序贯重要性积分的卡尔曼滤波方法
CN114088086A (zh) * 2021-11-23 2022-02-25 江苏科技大学 一种抗测量野值干扰的多目标鲁棒定位方法
CN115342815A (zh) * 2022-08-26 2022-11-15 哈尔滨工业大学 反大气层内或临近空间机动目标视线角速率估计方法
CN115808683A (zh) * 2023-02-08 2023-03-17 安徽隼波科技有限公司 一种雷达光电联动跟踪方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
张宏伟等: "平滑约束无迹卡尔曼滤波器", 《信号处理》 *

Cited By (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111711432B (zh) * 2020-06-16 2023-03-28 桂林理工大学 一种基于ukf和pf混合滤波的目标跟踪算法
CN111711432A (zh) * 2020-06-16 2020-09-25 桂林理工大学 一种基于ukf和pf混合滤波的目标跟踪算法
CN111985093B (zh) * 2020-08-03 2022-06-21 哈尔滨工程大学 一种带噪声估计器的自适应无迹卡尔曼滤波状态估计方法
CN111985093A (zh) * 2020-08-03 2020-11-24 哈尔滨工程大学 一种带噪声估计器的自适应无迹卡尔曼滤波状态估计方法
CN112990124B (zh) * 2021-04-26 2021-08-06 湖北亿咖通科技有限公司 一种车辆跟踪方法、装置、电子设备及存储介质
CN112990124A (zh) * 2021-04-26 2021-06-18 湖北亿咖通科技有限公司 一种车辆跟踪方法、装置、电子设备及存储介质
CN113452349A (zh) * 2021-06-28 2021-09-28 中山大学 一种基于贝叶斯序贯重要性积分的卡尔曼滤波方法
CN113452349B (zh) * 2021-06-28 2022-09-02 中山大学 一种基于贝叶斯序贯重要性积分的卡尔曼滤波方法
CN114088086A (zh) * 2021-11-23 2022-02-25 江苏科技大学 一种抗测量野值干扰的多目标鲁棒定位方法
CN114088086B (zh) * 2021-11-23 2023-11-24 江苏科技大学 一种抗测量野值干扰的多目标鲁棒定位方法
CN115342815A (zh) * 2022-08-26 2022-11-15 哈尔滨工业大学 反大气层内或临近空间机动目标视线角速率估计方法
CN115342815B (zh) * 2022-08-26 2024-04-26 哈尔滨工业大学 反大气层内或临近空间机动目标视线角速率估计方法
CN115808683A (zh) * 2023-02-08 2023-03-17 安徽隼波科技有限公司 一种雷达光电联动跟踪方法
CN115808683B (zh) * 2023-02-08 2023-04-07 安徽隼波科技有限公司 一种雷达光电联动跟踪方法

Similar Documents

Publication Publication Date Title
CN111047627A (zh) 一种平滑约束无迹卡尔曼滤波方法及目标跟踪方法
CN110503071B (zh) 基于变分贝叶斯标签多伯努利叠加模型的多目标跟踪方法
Gultekin et al. Nonlinear Kalman filtering with divergence minimization
Zhu et al. Huber-based adaptive unscented Kalman filter with non-Gaussian measurement noise
Havangi Target tracking based on improved unscented particle filter with Markov chain Monte Carlo
Amor et al. Constrained State Estimation--A Review
Chang et al. Marginalised iterated unscented Kalman filter
CN111181529B (zh) 一种应用于非线性高斯模型的平滑约束扩展卡尔曼滤波方法
Liu et al. Adaptive Gaussian sum squared-root cubature Kalman filter with split-merge scheme for state estimation
Daum et al. Particle flow with non-zero diffusion for nonlinear filters, Bayesian decisions and transport
Zuo et al. Particle filter with multimode sampling strategy
Menegaz et al. New minimum sigma set for unscented filtering
He et al. A spline filter for multidimensional nonlinear state estimation
CN113452349B (zh) 一种基于贝叶斯序贯重要性积分的卡尔曼滤波方法
He et al. An EM algorithm for target tracking with an unknown correlation coefficient of measurement noise
Yu et al. An improved dual unscented Kalman filter for state and parameter estimation
Chen et al. Kalman filtering
CN109115228A (zh) 一种基于加权最小二乘容积卡尔曼滤波的目标定位方法
Karlgaard et al. Comparison of several nonlinear filters for a benchmark tracking problem
Xiong et al. Linear fitting Kalman filter
Bhaumik et al. Risk-sensitive formulation of unscented Kalman filter
Fu et al. A novel improved cubature Kalman filter with adaptive generation of cubature points and weights for target tracking
Wang et al. Huber‐based Unscented Kalman Filters with theq‐gradient
CN114445459B (zh) 基于变分贝叶斯理论的连续-离散最大相关熵目标跟踪方法
Kang et al. Adaptive pulsar time delay estimation using wavelet-based RLS

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
RJ01 Rejection of invention patent application after publication
RJ01 Rejection of invention patent application after publication

Application publication date: 20200421