CN114637956B - 一种基于双卡尔曼滤波器实现目标位置预测的方法 - Google Patents

一种基于双卡尔曼滤波器实现目标位置预测的方法 Download PDF

Info

Publication number
CN114637956B
CN114637956B CN202210526125.2A CN202210526125A CN114637956B CN 114637956 B CN114637956 B CN 114637956B CN 202210526125 A CN202210526125 A CN 202210526125A CN 114637956 B CN114637956 B CN 114637956B
Authority
CN
China
Prior art keywords
state
kalman filter
error
value
classical
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
Application number
CN202210526125.2A
Other languages
English (en)
Other versions
CN114637956A (zh
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.)
Ruidina Wuxi Technology Co ltd
Original Assignee
Redina Nanjing Electronic Technology Co ltd
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 Redina Nanjing Electronic Technology Co ltd filed Critical Redina Nanjing Electronic Technology Co ltd
Priority to CN202210526125.2A priority Critical patent/CN114637956B/zh
Publication of CN114637956A publication Critical patent/CN114637956A/zh
Application granted granted Critical
Publication of CN114637956B publication Critical patent/CN114637956B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • 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
    • 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/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
    • 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)
  • Mathematical Physics (AREA)
  • Data Mining & Analysis (AREA)
  • Theoretical Computer Science (AREA)
  • Mathematical Analysis (AREA)
  • Computational Mathematics (AREA)
  • Pure & Applied Mathematics (AREA)
  • Mathematical Optimization (AREA)
  • Algebra (AREA)
  • Databases & Information Systems (AREA)
  • Software Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Computing Systems (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Evolutionary Biology (AREA)
  • Operations Research (AREA)
  • Probability & Statistics with Applications (AREA)
  • Filters That Use Time-Delay Elements (AREA)

Abstract

本发明公开了一种基于双卡尔曼滤波器实现目标位置预测的方法。该方法包括确定当前NLOS误差范围；根据NLOS误差范围调整当前时刻经典卡尔曼滤波器的状态误差协方差和噪声误差协方差；将调整后的状态误差协方差和噪声误差协方差输入经典卡尔曼滤波器中进行当前时刻的状态估计；将经典卡尔曼滤波器输出的状态估计值和状态误差方差输入至输入扩展卡尔曼滤波器中，扩展卡尔曼滤波器以经典卡尔曼滤波器输出的状态估计值和状态误差方差分别作为扩展卡尔曼滤波器的测量值和测量噪声方差进行预测。本发明可以较快收敛，不存在较大的系统误差波动，可以抑制短时间或较长时间持续的NLOS误差和离群值。

Description

一种基于双卡尔曼滤波器实现目标位置预测的方法
技术领域
本发明涉及滤波方法技术领域，具体涉及一种基于双卡尔曼滤波器实现目标位置预测的方法。
背景技术
1960年，R. E. Kalman发表了一篇以递归算法为基础的用于解决线性离散系统的著作，在该著作中，卡尔曼滤波摆脱了曾广泛应用的维纳滤波的缺点，自此之后，随着计算机的发明，计算能力的大幅提升，卡尔曼滤波器已经广泛地应用于科研、军事以及民用领域。卡尔曼滤波的优势在于，在不知道系统的确切模型的情况下，它不仅用于估计信号的当前状态，还可以用于预测信号的将来状态，所以卡尔曼滤波广泛适用于雷达和导航系统，对既定目标进行循迹。它的基本思想可以表述为，通过前一时刻的估计值以及当前时刻的观测值的条件下得到准确值以及预测下一时刻的估计值，整个过程的准则为最小均方差准则，过程中引入了信号与噪声的状态方程，建立了系统状态方程和系统测量方程。基本的卡尔曼滤波算法适用于解决线性离散系统的状态以及参数估计问题。实际应用中，卡尔曼滤波可以将一个系统的整个过程视为从前一个状态变为下一个状态、状态不断更新的过程，卡尔曼滤波对于系统的建模也是基于状态转换的。经典的卡尔曼滤波器的计算过程分为：预测状态，卡尔曼增益计算，估计输出。
经典卡尔曼滤波器的系统状态方程和观测方程均为线性方程，但是实际场景中状态方程或观测方程都可能为非线性方程，这时就要对状态方程或观测方程等非线性函数在当前估计状态的平均值附近进行线性化，在每个迭代过程执行线性化操作，并将得到的雅可比矩阵用于预测和更新卡尔曼滤波器算法的状态，这就是扩展卡尔曼滤波(ExtendedKalman Filter, EKF)算法的基本思路。
雅可比矩阵的计算较为复杂，计算量较大，当系统不可微时，雅可比矩阵不存在，且在高度非线性系统中，雅可比矩阵的线性化效果不好。
为了解决非线性模型中复杂的概率密度分布问题，学者提出了无迹卡尔曼滤波(Unscented Kalman Filter, UKF)算法，该算法按照一定规律采样，计算概率密度函数的均值和方差，并按照均值和方差转换为高斯密度函数进行处理，该算法的主要思想是，选取一组最小的采样点(Sigma 点)，让它们的均值和协方差与已知的概率分布相同，每个Sigma点通过非线性系统模型计算非线性变换后输出点的均值和协方差，并计算它们的经验高斯分布，用来更新新的状态值。
无迹卡尔曼滤波算法步骤分为：预测和更新步骤。其中预测步骤包含：计算Sigma点，转换Sigma点，计算预测状态值和预测误差的协方差值。更新步骤包含：从转换的Sigma点预测测量值，求预测测量值的平均值和方差，计算互协方差值和卡尔曼增益，计算修正的状态值和状态误差方差。
现有的卡尔曼滤波算法的变体，应用较广的还有容积卡尔曼滤波(CubatureKalman Filter, CKF)，该算法是由加拿大学者Arasaratnam和Haykin在2009年于硕士学位论文中提出，CKF应用了基于三阶球面径向容积准则，使用一组容积点来逼近具有附加高斯噪声的非线性系统的状态均值和协方差，是理论上当前最接近贝叶斯滤波的近似算法，是解决非线性系统状态估计的强有力工具。其中，将积分形式变换成球面径向积分形式和三阶球面径向准则是最为重要的两个步骤。
除了容积卡尔曼滤波算法之外，还有一种应用较广的卡尔曼滤波算法是自适应调整协方差值的卡尔曼滤波(Adaptive Kalman Filter, AKF)算法，该算法是1969年由学者A. P. Sage和G. W. Husa提出的一种自适应滤波算法，它在进行状态估计的同时还可以通过量测输出在线实时地估计系统的噪声参数，该算法中是同时对状态误差协方差Q和噪声误差协方差R进行调整，但是也有学者提出同时对Q和R噪声参数进行调整往往是不可能的。
对于双卡尔曼滤波的结合法，清华大学的王向华在2010年提出了一种基于两次卡尔曼滤波的观测噪声自适应算法，该算法通过任一时刻两次步长不同的卡尔曼滤波结果来判断当前对R的估计精度，并实时自适应地调整R，进而获得适当的卡尔曼滤波增益K，减少因R估计不当造成的跟踪误差。
现有技术有以下缺陷：
1、经典卡尔曼滤波算法、扩展卡尔曼滤波算法、无迹卡尔曼滤波算法、容积卡尔曼滤波算法无法实现Q和R的调整，但是在实际的目标跟踪过程中，由于目标远近等各种客观因素的影响，观测噪声是随时变化的，在以上滤波算法中Q和R均为恒定值，且需要依靠经验值提前设定，噪声方差不方便确定且不变可能会造成跟踪结果不理想。
2、经典卡尔曼滤波算法、扩展卡尔曼滤波算法、无迹卡尔曼滤波算法、容积卡尔曼滤波算法在测量值出现一个较大的NLOS距离误差时，会出现短期或持续一段时间的跟踪失效，定位效果差，难以应对复杂室内环境中非视距产生的较短或较长时间的时延对系统产生的较大误差影响。
3、经典卡尔曼滤波算法只适用于线性系统而不能用于非线性系统，扩展卡尔曼滤波、无迹卡尔曼滤波、容积卡尔曼滤波算法适用于非线性系统，其运算速度较经典卡尔曼滤波算法更慢，自适应卡尔曼滤波算法适用于线性系统，但是也可以经过改进从而适用于非线性系统。
4、自适应卡尔曼滤波算法可以在迭代过程中对噪声参数实时调整，但是Q和R不能同时进行调整，一般只对R值进行调整，且当测量值出现一个较大的NLOS距离误差时，自适应卡尔曼滤波器可能会发散，难以收敛。
5、两次不同步长的双卡尔曼滤波算法在每个采样时刻比较两种状态的后验估计值，根据估计值的比较来判断目标的运动状态是否发生了变化，自适应调整R，提高目标的跟踪精度。当测量值异常值时，虽然双卡尔曼滤波器可以自适应调整R，使其状态后验估计比标准卡尔曼滤波器的状态后验估计更接近真值，但其状态后验估计误差仍然很大。因此，双卡尔曼滤波方法的精度不能满足实际应用的要求。此外，当测量值中存在异常值时，由于双卡尔曼滤波方法中的一个滤波器步长较大，系统延迟会产生较大的跟踪误差，状态后验估计的收敛速度也会降低。
发明内容
本发明的目的是针对现有技术存在的不足，提供一种基于双卡尔曼滤波器实现目标位置预测的方法。
为实现上述目的，本发明提供了一种基于双卡尔曼滤波器实现目标位置预测的方法，包括以下步骤：
步骤1、将当前时刻经典卡尔曼滤波器的测量值与根据上一时刻经典卡尔曼滤波器的状态值估计得到的预测值进行比较，以确定NLOS误差范围；
步骤2、根据NLOS误差范围调整当前时刻经典卡尔曼滤波器的状态误差协方差
Figure 147376DEST_PATH_IMAGE001
和噪声误差协方差
Figure 90186DEST_PATH_IMAGE002
；
步骤3、将调整后的状态误差协方差
Figure 77734DEST_PATH_IMAGE001
和噪声误差协方差
Figure 364359DEST_PATH_IMAGE002
输入经典卡尔曼滤波 器中进行当前时刻的状态估计，以使所述经典卡尔曼滤波器输出当前时刻的状态估计值
Figure 957014DEST_PATH_IMAGE003
与状态变换矩阵
Figure 303682DEST_PATH_IMAGE004
的乘积和状态误差方差
Figure 277061DEST_PATH_IMAGE005
；
步骤4、将经典卡尔曼滤波器输出的当前时刻的状态估计值
Figure 367377DEST_PATH_IMAGE006
与状态变换矩阵
Figure 41635DEST_PATH_IMAGE004
的乘积和状态误差方差输入至输入扩展卡尔曼滤波器中，所述扩展卡尔曼滤波器以当 前时刻的状态估计值
Figure 450882DEST_PATH_IMAGE007
与状态变换矩阵
Figure 475339DEST_PATH_IMAGE004
的乘积作为测量值
Figure 602301DEST_PATH_IMAGE008
，且其以经典卡尔曼滤波 器输出的状态误差方差
Figure 700707DEST_PATH_IMAGE005
作为测量噪声方差
Figure 389177DEST_PATH_IMAGE009
进行预测，并输出目标的位置信息。
进一步的，所述步骤1具体包括：
计算残差绝对值
Figure 573034DEST_PATH_IMAGE010
，
Figure 772197DEST_PATH_IMAGE011
为当前时刻经典卡尔曼滤波器的 测量值，
Figure 990688DEST_PATH_IMAGE012
为根据上一时刻经典卡尔曼滤波器的状态值估计得到的预测值，k为大 于1的自然数；
根据残差绝对值
Figure 53322DEST_PATH_IMAGE013
的大小确定误差范围，具体如下：
Figure 724475DEST_PATH_IMAGE014
其中，
Figure 694705DEST_PATH_IMAGE015
为低误差阈值，
Figure 537677DEST_PATH_IMAGE016
为中误差阈值，
Figure 895846DEST_PATH_IMAGE017
为高误差阈值，
Figure 742710DEST_PATH_IMAGE018
。
进一步的，所述步骤2具体包括：
根据残差绝对值
Figure 687270DEST_PATH_IMAGE013
设置标志位flag，所述标志位flag包括0、1、2、3；
根据标志位flag将经典卡尔曼滤波器的状态误差协方差
Figure 880354DEST_PATH_IMAGE019
和噪声误差协方差
Figure 597642DEST_PATH_IMAGE020
按以下方式进行调整：
Figure 181070DEST_PATH_IMAGE021
其中，
Figure 758682DEST_PATH_IMAGE022
、
Figure 806272DEST_PATH_IMAGE023
、
Figure 945392DEST_PATH_IMAGE024
分别为第一状态误差协方差调整系数、第二状态误差协方差调整 参系数和第三状态误差协方差调整系数，
Figure 812854DEST_PATH_IMAGE025
、
Figure 256473DEST_PATH_IMAGE026
、
Figure 266892DEST_PATH_IMAGE027
分别为第一状态误差方差调整系数、第 二状态误差方差调整系数、第三状态误差方差调整系数，
Figure 907739DEST_PATH_IMAGE028
为经典卡尔曼滤波器的状 态误差协方差和噪声误差协方差的初始值。
进一步的，所述经典卡尔曼滤波器进行当前时刻的状态估计的方式如下：
Figure 590393DEST_PATH_IMAGE029
其中，
Figure 978649DEST_PATH_IMAGE030
为经典卡尔曼滤波器对当前时刻状态的预测值，E为针对于经典卡尔 曼滤波器的取期望值操作，A为经典卡尔曼滤波器的状态转移矩阵，
Figure 767876DEST_PATH_IMAGE031
为经典卡尔曼滤波 器对上一时刻状态的预测值，
Figure 685016DEST_PATH_IMAGE032
为经典卡尔曼滤波器的状态噪声；
当前时刻经典卡尔曼滤波器预测误差的均方差值
Figure 527070DEST_PATH_IMAGE033
为：
Figure 250176DEST_PATH_IMAGE034
Figure 625400DEST_PATH_IMAGE035
其中，T为转置矩阵，
Figure 510179DEST_PATH_IMAGE036
为经典卡尔曼滤波器当前时刻的预测状态与真实状 态的误差，
Figure 42792DEST_PATH_IMAGE037
为经典卡尔曼滤波器当前时刻的状态估计输出值与真实状态的误差，
Figure 726845DEST_PATH_IMAGE038
，
Figure 723620DEST_PATH_IMAGE039
为经典卡尔曼滤波器的当前真实状态，
Figure 334293DEST_PATH_IMAGE040
为经典卡尔曼滤波器输出的 当前时刻的状态估计值，
Figure 541153DEST_PATH_IMAGE041
为上一时刻的状态估计输出值与真实状态的误差，
Figure 701001DEST_PATH_IMAGE042
为当 前时刻调整后的噪声方差，
Figure 817861DEST_PATH_IMAGE043
为经典卡尔曼滤波器前一时刻的真实状态；
所述
Figure 339716DEST_PATH_IMAGE044
的计算方式如下：
Figure 705976DEST_PATH_IMAGE045
其中，
Figure 669515DEST_PATH_IMAGE046
为当前时刻的卡尔曼滤波系数，其计算方式如下：
Figure 109723DEST_PATH_IMAGE047
其中，
Figure 277180DEST_PATH_IMAGE048
为经典卡尔曼滤波器当前时刻的状态变换矩阵；
所述经典卡尔曼滤波器输出的状态误差方差
Figure 6102DEST_PATH_IMAGE005
为：
Figure 412812DEST_PATH_IMAGE049
其中，I为n阶单位矩阵，n为经典卡尔曼滤波器状态向量的维数。
进一步的，所述扩展卡尔曼滤波器进行预测方式如下：
Figure 176369DEST_PATH_IMAGE050
其中，
Figure 10333DEST_PATH_IMAGE051
为扩展卡尔曼滤波器对当前时刻状态的预测值，
Figure 790332DEST_PATH_IMAGE052
为针对于扩展卡 尔曼滤波器的取期望值操作，
Figure 938417DEST_PATH_IMAGE053
为扩展卡尔曼滤波器的状态转移矩阵，
Figure 353218DEST_PATH_IMAGE054
为扩展卡尔 曼滤波器的噪声状态,
Figure 623662DEST_PATH_IMAGE055
为扩展卡尔曼滤波器对上一时刻状态的预测值；
所述扩展卡尔曼滤波器的预测误差的均方差值
Figure 327176DEST_PATH_IMAGE056
为：
Figure 574224DEST_PATH_IMAGE057
其中，
Figure 109111DEST_PATH_IMAGE058
为
Figure 488140DEST_PATH_IMAGE054
的方差；
所述扩展卡尔曼滤波器的预测误差的卡尔曼滤波系数
Figure 475687DEST_PATH_IMAGE059
为：
Figure 27891DEST_PATH_IMAGE060
其中，
Figure 122011DEST_PATH_IMAGE061
为扩展卡尔曼滤波器当前时刻的状态变换矩阵；
Figure 468679DEST_PATH_IMAGE009
为扩展卡尔曼滤波器 的测量噪声方差，
Figure 943523DEST_PATH_IMAGE062
；
所述扩展卡尔曼滤波器预测并输出目标的位置信息为：
Figure 237101DEST_PATH_IMAGE063
Figure 856168DEST_PATH_IMAGE064
其中，
Figure 577000DEST_PATH_IMAGE065
为扩展卡尔曼滤波器输出的当前时刻的状态估计输出值，
Figure 539139DEST_PATH_IMAGE066
为扩展卡尔 曼滤波器的状态误差方差，
Figure 167567DEST_PATH_IMAGE067
，
Figure 236279DEST_PATH_IMAGE068
为扩展卡尔曼滤波器当前时刻的状态变换矩 阵，
Figure 924749DEST_PATH_IMAGE069
为m阶单位矩阵，m为扩展卡尔曼滤波器状态向量的维数。
有益效果：本发明根据观测噪声的实时变化情况对Q和R做出调整，具体是根据测距值和状态值的偏差值判断NLOS误差范围，并根据NLOS误差的范围判断Q和R系数的调整从而调整卡尔曼滤波系数K；在后验估计之前，根据状态变化情况调整噪声参数的算法对噪声参数进行调整，该算法可以较快收敛，且当NLOS误差存在时，不存在较大的系统误差波动，可以抑制短时间或较长时间持续的NLOS误差和离群值。
附图说明
图1是本发明实施例的基于双卡尔曼滤波器实现目标位置预测的方法流程示意图；
图2是根据NLOS误差范围调整噪声方差和测量噪声方差的流程图。
具体实施方式
下面结合附图和具体实施例，进一步阐明本发明，本实施例在以本发明技术方案为前提下进行实施，应理解这些实施例仅用于说明本发明而不用于限制本发明的范围。
如图1和图2所示，本发明实施例提供了一种基于双卡尔曼滤波器实现目标位置预 测的方法，该方法基于两个卡尔曼滤波器实现，第一个是经典卡尔曼滤波器，其状态量由目 标与基站之间的距离值和距离值变化率组成，测量值为目标与基站之间的测距值。第二个 是扩展卡尔曼滤波器，其状态量由标签的位置坐标(二维定位为
Figure 374185DEST_PATH_IMAGE070
，三维定位为
Figure 806304DEST_PATH_IMAGE071
)， 标签的运动速度(二维定位为
Figure 228058DEST_PATH_IMAGE072
，三维定位为
Figure 585965DEST_PATH_IMAGE073
)以及标签的运动加速度(二 维定位为
Figure 522697DEST_PATH_IMAGE074
，三维定位为
Figure 696189DEST_PATH_IMAGE075
)，其中x、y、z下标分别代表平行于x、y、z正向轴 的速度或加速度；其测量值为目标与基站之间的测距值，由经典卡尔曼滤波器的状态估计 值输入。两个卡尔曼滤波器均需要输入初始值，包括状态初始值X0、Y0，状态误差协方差初始 值
Figure 769187DEST_PATH_IMAGE076
，噪声误差协方差初始值
Figure 566504DEST_PATH_IMAGE077
，以及经典卡尔曼滤波器的初始误差方差
Figure 724953DEST_PATH_IMAGE078
。
经典卡尔曼滤波器的状态方程和测量方程分别如下：
Figure 702136DEST_PATH_IMAGE079
扩展卡尔曼滤波器的状态方程和测量方程分别如下：
Figure 629641DEST_PATH_IMAGE080
需要说明的是，经典卡尔曼滤波器以及扩展卡尔曼滤波器的状态方程和测量方程均为现有技术，在此不再对其参数进行释义。
本发明实施例的基于双卡尔曼滤波器实现目标位置预测的方法包括以下步骤：
步骤1、将当前时刻经典卡尔曼滤波器的测量值与根据上一时刻经典卡尔曼滤波 器的状态值估计得到的预测值进行比较，以确定NLOS误差范围。具体的，先计算经典卡尔曼 滤波器当前时刻测距值和由经典卡尔曼滤波器前一时刻状态估计值预测得到的状态预测 值的残差绝对值
Figure 96394DEST_PATH_IMAGE081
，根据预设的阈值范围判断差值中可能存在的NLOS距离误差值范围。其 中，
Figure 449796DEST_PATH_IMAGE082
，
Figure 27408DEST_PATH_IMAGE083
为当前时刻经典卡尔曼滤波器的测量值，
Figure 74999DEST_PATH_IMAGE084
为 根据上一时刻经典卡尔曼滤波器的状态值估计得到的预测值，k为大于1的自然数。根据残 差绝对值
Figure 447074DEST_PATH_IMAGE013
的大小确定误差范围，具体如下：
Figure 783377DEST_PATH_IMAGE014
其中，
Figure 666145DEST_PATH_IMAGE015
为低误差阈值，
Figure 568242DEST_PATH_IMAGE016
为中误差阈值，
Figure 314481DEST_PATH_IMAGE017
为高误差阈值，另有
Figure 200398DEST_PATH_IMAGE018
。
步骤2、根据NLOS误差范围调整当前时刻经典卡尔曼滤波器的噪声方差
Figure 57495DEST_PATH_IMAGE019
和测量 噪声方差
Figure 312634DEST_PATH_IMAGE020
。具体的，首先根据残差绝对值
Figure 557670DEST_PATH_IMAGE013
设置标志位flag，标志位flag包括0、1、2、3；
然后再根据标志位flag将经典卡尔曼滤波器的状态误差协方差
Figure 602987DEST_PATH_IMAGE019
和噪声误差协 方差
Figure 60513DEST_PATH_IMAGE020
按以下方式进行调整：
Figure 438667DEST_PATH_IMAGE021
其中，
Figure 589025DEST_PATH_IMAGE022
、
Figure 121638DEST_PATH_IMAGE023
、
Figure 382855DEST_PATH_IMAGE024
分别为第一状态误差协方差调整系数、第二状态误差协方差调整 参系数和第三状态误差协方差调整系数，
Figure 379630DEST_PATH_IMAGE025
、
Figure 131249DEST_PATH_IMAGE026
、
Figure 947895DEST_PATH_IMAGE027
分别为第一状态误差方差调整系数、第 二状态误差方差调整系数、第三状态误差方差调整系数，
Figure 278382DEST_PATH_IMAGE028
为经典卡尔曼滤波器的状 态误差协方差和噪声误差协方差的初始值，需要提前设定并输入至经典卡尔曼滤波器中。
步骤3、将调整后的状态误差协方差
Figure 129663DEST_PATH_IMAGE019
和噪声误差协方差
Figure 123289DEST_PATH_IMAGE020
输入经典卡尔曼滤波 器中进行当前时刻的状态估计，以使所述经典卡尔曼滤波器输出当前时刻的状态估计值
Figure 896073DEST_PATH_IMAGE003
与状态变换矩阵
Figure 233514DEST_PATH_IMAGE085
的乘积和状态误差方差
Figure 939302DEST_PATH_IMAGE005
。具体的，经典卡尔曼滤波器进行当前时 刻的状态估计的方式如下：
Figure 540047DEST_PATH_IMAGE086
其中，
Figure 829821DEST_PATH_IMAGE087
为经典卡尔曼滤波器对当前时刻状态的预测值，E为针对于经典卡尔 曼滤波器的取期望值操作，A为经典卡尔曼滤波器的状态转移矩阵，
Figure 970952DEST_PATH_IMAGE088
为经典卡尔曼滤波 器对上一时刻状态的预测值，
Figure 796826DEST_PATH_IMAGE032
为经典卡尔曼滤波器的状态噪声。
当前时刻经典卡尔曼滤波器预测误差的均方差值
Figure 630790DEST_PATH_IMAGE033
为：
Figure 348472DEST_PATH_IMAGE089
Figure 558874DEST_PATH_IMAGE090
其中，T为转置矩阵，
Figure 973674DEST_PATH_IMAGE036
为经典卡尔曼滤波器当前时刻的预测状态与真实状 态的误差，
Figure 181802DEST_PATH_IMAGE037
为经典卡尔曼滤波器当前时刻的状态估计输出值与真实状态的误差，
Figure 682053DEST_PATH_IMAGE091
，
Figure 669382DEST_PATH_IMAGE039
为经典卡尔曼滤波器的当前真实状态，
Figure 469848DEST_PATH_IMAGE040
为经典卡尔曼滤波器输出的 当前时刻的状态估计值，
Figure 645614DEST_PATH_IMAGE041
为上一时刻的状态估计输出值与真实状态的误差，
Figure 633162DEST_PATH_IMAGE042
为当 前时刻调整后的噪声方差，
Figure 624514DEST_PATH_IMAGE043
为经典卡尔曼滤波器前一时刻的真实状态。
其中，
Figure 13907DEST_PATH_IMAGE044
的计算方式如下：
Figure 360574DEST_PATH_IMAGE045
其中，
Figure 38680DEST_PATH_IMAGE046
为当前时刻的卡尔曼滤波系数，其计算方式如下：
Figure 394575DEST_PATH_IMAGE092
其中，
Figure 137010DEST_PATH_IMAGE048
为经典卡尔曼滤波器当前时刻的状态变换矩阵；
经典卡尔曼滤波器输出的状态误差方差
Figure 592262DEST_PATH_IMAGE005
为：
Figure 554402DEST_PATH_IMAGE049
其中，I为n阶单位矩阵，n为经典卡尔曼滤波器状态向量的维数。
步骤4、将经典卡尔曼滤波器输出的当前时刻的状态估计值
Figure 979567DEST_PATH_IMAGE006
与状态变换矩阵
Figure 313859DEST_PATH_IMAGE004
的乘积和状态误差方差输入至输入扩展卡尔曼滤波器中，所述扩展卡尔曼滤波器以当 前时刻的状态估计值
Figure 940012DEST_PATH_IMAGE007
与状态变换矩阵
Figure 389448DEST_PATH_IMAGE093
的乘积作为测量值
Figure 821566DEST_PATH_IMAGE008
，且其以经典卡尔曼滤波 器输出的状态误差方差
Figure 40058DEST_PATH_IMAGE005
作为测量噪声方差
Figure 884385DEST_PATH_IMAGE009
进行预测，并输出目标的位置信息。具体 的，扩展卡尔曼滤波器进行预测方式如下：
Figure 758800DEST_PATH_IMAGE094
其中，
Figure 463450DEST_PATH_IMAGE051
为扩展卡尔曼滤波器对当前时刻状态的预测值，
Figure 802028DEST_PATH_IMAGE052
为针对于扩展卡 尔曼滤波器的取期望值操作，
Figure 301142DEST_PATH_IMAGE095
为扩展卡尔曼滤波器的状态转移矩阵，
Figure 928433DEST_PATH_IMAGE054
为扩展卡尔 曼滤波器的噪声状态,
Figure 905616DEST_PATH_IMAGE096
为扩展卡尔曼滤波器对上一时刻状态的预测值。
扩展卡尔曼滤波器的预测误差的均方差值
Figure 803427DEST_PATH_IMAGE056
为：
Figure 270181DEST_PATH_IMAGE097
其中，
Figure 119188DEST_PATH_IMAGE058
为
Figure 900062DEST_PATH_IMAGE054
的方差。
扩展卡尔曼滤波器的预测误差的卡尔曼滤波系数
Figure 416494DEST_PATH_IMAGE059
为：
Figure 54149DEST_PATH_IMAGE098
其中，
Figure 154567DEST_PATH_IMAGE061
为扩展卡尔曼滤波器当前时刻的状态变换矩阵，即函数g在状态预测值对 应点附近的雅可比矩阵，
Figure 473552DEST_PATH_IMAGE009
为扩展卡尔曼滤波器的测量噪声方差,此处
Figure 844491DEST_PATH_IMAGE099
。
扩展卡尔曼滤波器预测并输出目标的位置信息为：
Figure 653047DEST_PATH_IMAGE100
Figure 476646DEST_PATH_IMAGE064
其中，
Figure 864902DEST_PATH_IMAGE065
为扩展卡尔曼滤波器输出的当前时刻的状态估计输出值，
Figure 824768DEST_PATH_IMAGE101
为扩展卡尔 曼滤波器的状态误差方差，
Figure 508953DEST_PATH_IMAGE102
，
Figure 882165DEST_PATH_IMAGE103
为扩展卡尔曼滤波器当前时刻的状态变换矩 阵，
Figure 808533DEST_PATH_IMAGE069
为m阶单位矩阵，m为扩展卡尔曼滤波器状态向量的维数。
以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，其它未具体描述的部分，属于现有技术或公知常识。在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (3)

1.一种基于双卡尔曼滤波器实现目标位置预测的方法，其特征在于，包括：
步骤1、将当前时刻经典卡尔曼滤波器的测量值与根据上一时刻经典卡尔曼滤波器的状态值估计得到的预测值进行比较，以确定NLOS误差范围，所述测量值为目标与基站之间的测距值；
步骤2、根据NLOS误差范围调整当前时刻经典卡尔曼滤波器的状态误差协方差
Figure 882419DEST_PATH_IMAGE001
和噪 声误差协方差
Figure 88273DEST_PATH_IMAGE002
；
步骤3、将调整后的状态误差协方差
Figure 80500DEST_PATH_IMAGE001
和噪声误差协方差
Figure 979185DEST_PATH_IMAGE002
输入经典卡尔曼滤波器中 进行当前时刻的状态估计，以使所述经典卡尔曼滤波器输出当前时刻的状态估计值
Figure 689653DEST_PATH_IMAGE003
与 状态变换矩阵
Figure 433618DEST_PATH_IMAGE004
的乘积和状态误差方差
Figure 545930DEST_PATH_IMAGE005
，所述状态估计值
Figure 349938DEST_PATH_IMAGE006
由目标与基站之间的距离 值和距离值变化率组成；
步骤4、将经典卡尔曼滤波器输出的当前时刻的状态估计值
Figure 547701DEST_PATH_IMAGE007
与状态变换矩阵
Figure 95357DEST_PATH_IMAGE004
的乘 积和状态误差方差输入至输入扩展卡尔曼滤波器中，所述扩展卡尔曼滤波器以当前时刻的 状态估计值
Figure 62176DEST_PATH_IMAGE008
与状态变换矩阵
Figure 296805DEST_PATH_IMAGE009
的乘积作为测量值
Figure 716285DEST_PATH_IMAGE010
，且其以经典卡尔曼滤波器输出的 状态误差方差
Figure 67632DEST_PATH_IMAGE005
作为测量噪声方差
Figure 888958DEST_PATH_IMAGE011
进行预测，并输出目标的位置信息，所述目标的位置 信息包括目标的位置坐标、目标的运动速度以及目标的运动加速度；
所述步骤1具体包括：
计算残差绝对值
Figure 300347DEST_PATH_IMAGE012
，
Figure 207124DEST_PATH_IMAGE013
为当前时刻经典卡尔曼滤波器的测量 值，
Figure 96582DEST_PATH_IMAGE014
为根据上一时刻经典卡尔曼滤波器的状态值估计得到的预测值，k为大于1 的自然数；
根据残差绝对值
Figure 37993DEST_PATH_IMAGE015
的大小确定误差范围，具体如下：
Figure 620284DEST_PATH_IMAGE016
其中，
Figure 14357DEST_PATH_IMAGE017
为低误差阈值，
Figure 707506DEST_PATH_IMAGE018
为中误差阈值，
Figure 237845DEST_PATH_IMAGE019
为高误差阈值，
Figure 991037DEST_PATH_IMAGE020
；
所述步骤2具体包括：
根据残差绝对值
Figure 872405DEST_PATH_IMAGE015
设置标志位flag，所述标志位flag包括0、1、2、3；
根据标志位flag将经典卡尔曼滤波器的状态误差协方差
Figure 369246DEST_PATH_IMAGE001
和噪声误差协方差
Figure 19670DEST_PATH_IMAGE021
按以 下方式进行调整：
Figure 678184DEST_PATH_IMAGE022
其中，
Figure 46849DEST_PATH_IMAGE023
、
Figure 81801DEST_PATH_IMAGE024
、
Figure 852311DEST_PATH_IMAGE025
分别为第一状态误差协方差调整系数、第二状态误差协方差调整参系 数和第三状态误差协方差调整系数，
Figure 681727DEST_PATH_IMAGE026
、
Figure 537687DEST_PATH_IMAGE027
、
Figure 376330DEST_PATH_IMAGE028
分别为第一状态误差方差调整系数、第二状 态误差方差调整系数、第三状态误差方差调整系数，
Figure 735767DEST_PATH_IMAGE029
为经典卡尔曼滤波器的状态误 差协方差和噪声误差协方差的初始值。
2.根据权利要求1所述的一种基于双卡尔曼滤波器实现目标位置预测的方法，其特征在于，所述经典卡尔曼滤波器进行当前时刻的状态估计的方式如下：
Figure 1664DEST_PATH_IMAGE030
其中，
Figure 79341DEST_PATH_IMAGE031
为经典卡尔曼滤波器对当前时刻状态的预测值，E为针对于经典卡尔曼滤 波器的取期望值操作，A为经典卡尔曼滤波器的状态转移矩阵，
Figure 721675DEST_PATH_IMAGE032
为经典卡尔曼滤波器对 上一时刻状态的预测值，
Figure 201198DEST_PATH_IMAGE033
为经典卡尔曼滤波器的状态噪声；
当前时刻经典卡尔曼滤波器预测误差的均方差值
Figure 637995DEST_PATH_IMAGE034
为：
Figure 202969DEST_PATH_IMAGE035
Figure 383415DEST_PATH_IMAGE036
其中，T为转置矩阵，
Figure 717444DEST_PATH_IMAGE037
为经典卡尔曼滤波器当前时刻的预测状态与真实状态的误 差，
Figure 319283DEST_PATH_IMAGE038
为经典卡尔曼滤波器当前时刻的状态估计输出值与真实状态的误差，
Figure 105974DEST_PATH_IMAGE039
，
Figure 90110DEST_PATH_IMAGE040
为经典卡尔曼滤波器的当前真实状态，
Figure 544225DEST_PATH_IMAGE041
为上一时刻的状态估计输 出值与真实状态的误差，
Figure 322826DEST_PATH_IMAGE042
为当前时刻调整后的噪声方差，
Figure 596812DEST_PATH_IMAGE043
为经典卡尔曼滤波器 前一时刻的真实状态；
所述
Figure 119060DEST_PATH_IMAGE006
的计算方式如下：
Figure 427682DEST_PATH_IMAGE044
其中，
Figure 642763DEST_PATH_IMAGE045
为当前时刻的卡尔曼滤波系数，其计算方式如下：
Figure 404045DEST_PATH_IMAGE046
；
所述经典卡尔曼滤波器输出的状态误差方差
Figure 729984DEST_PATH_IMAGE005
为：
Figure 893112DEST_PATH_IMAGE047
其中，I为n阶单位矩阵，n为经典卡尔曼滤波器状态向量的维数。
3.根据权利要求1所述的一种基于双卡尔曼滤波器实现目标位置预测的方法，其特征在于，所述扩展卡尔曼滤波器进行预测方式如下：
Figure 13515DEST_PATH_IMAGE048
其中，
Figure 527673DEST_PATH_IMAGE049
为扩展卡尔曼滤波器对当前时刻状态的预测值，
Figure 126145DEST_PATH_IMAGE050
为针对于扩展卡尔曼滤 波器的取期望值操作，
Figure 409358DEST_PATH_IMAGE051
为扩展卡尔曼滤波器的状态转移矩阵，
Figure 700662DEST_PATH_IMAGE052
为扩展卡尔曼滤波 器的噪声状态,
Figure 702117DEST_PATH_IMAGE053
为扩展卡尔曼滤波器对上一时刻状态的预测值；
所述扩展卡尔曼滤波器的预测误差的均方差值
Figure 369858DEST_PATH_IMAGE054
为：
Figure 241999DEST_PATH_IMAGE055
其中，
Figure 969784DEST_PATH_IMAGE056
为
Figure 192955DEST_PATH_IMAGE052
的方差；
所述扩展卡尔曼滤波器的预测误差的卡尔曼滤波系数
Figure 133229DEST_PATH_IMAGE057
为：
Figure 125456DEST_PATH_IMAGE058
其中，
Figure 24142DEST_PATH_IMAGE059
为扩展卡尔曼滤波器当前时刻的状态变换矩阵；
Figure 469029DEST_PATH_IMAGE011
为扩展卡尔曼滤波器的测 量噪声方差，
Figure 478574DEST_PATH_IMAGE060
；
所述扩展卡尔曼滤波器预测并输出目标的位置信息为：
Figure 590886DEST_PATH_IMAGE061
Figure 394894DEST_PATH_IMAGE062
其中，
Figure 592657DEST_PATH_IMAGE063
为扩展卡尔曼滤波器输出的当前时刻的状态估计输出值，
Figure 405893DEST_PATH_IMAGE064
为扩展卡尔曼滤 波器的状态误差方差，
Figure 107132DEST_PATH_IMAGE065
，
Figure 76182DEST_PATH_IMAGE066
为扩展卡尔曼滤波器当前时刻的状态变换矩阵，
Figure 761241DEST_PATH_IMAGE067
为m阶单位矩阵，m为扩展卡尔曼滤波器状态向量的维数。
CN202210526125.2A 2022-05-16 2022-05-16 一种基于双卡尔曼滤波器实现目标位置预测的方法 Active CN114637956B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210526125.2A CN114637956B (zh) 2022-05-16 2022-05-16 一种基于双卡尔曼滤波器实现目标位置预测的方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210526125.2A CN114637956B (zh) 2022-05-16 2022-05-16 一种基于双卡尔曼滤波器实现目标位置预测的方法

Publications (2)

Publication Number Publication Date
CN114637956A CN114637956A (zh) 2022-06-17
CN114637956B true CN114637956B (zh) 2022-09-20

Family

ID=81952899

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210526125.2A Active CN114637956B (zh) 2022-05-16 2022-05-16 一种基于双卡尔曼滤波器实现目标位置预测的方法

Country Status (1)

Country Link
CN (1) CN114637956B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117720012B (zh) * 2024-02-08 2024-05-07 泰安市特种设备检验研究院 基于扩展卡尔曼滤波的吊车系统模型预测控制方法及系统

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103152826A (zh) * 2013-03-08 2013-06-12 天津大学 一种基于nlos状态检测补偿的移动目标追踪方法
CN106793077A (zh) * 2017-01-05 2017-05-31 重庆邮电大学 一种自适应室内动态目标的uwb定位方法及系统
CN114325577A (zh) * 2022-01-06 2022-04-12 浙江大学 一种非视距定位误差修正方法与装置

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104316905B (zh) * 2014-10-20 2016-09-28 合肥工业大学 处理飞行时间测距数据的自适应卡尔曼滤波的方法
CN108318856B (zh) * 2018-02-02 2021-06-15 河南工学院 一种异构网络下快速精准的目标定位与跟踪方法

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103152826A (zh) * 2013-03-08 2013-06-12 天津大学 一种基于nlos状态检测补偿的移动目标追踪方法
CN106793077A (zh) * 2017-01-05 2017-05-31 重庆邮电大学 一种自适应室内动态目标的uwb定位方法及系统
CN114325577A (zh) * 2022-01-06 2022-04-12 浙江大学 一种非视距定位误差修正方法与装置

Also Published As

Publication number Publication date
CN114637956A (zh) 2022-06-17

Similar Documents

Publication Publication Date Title
CN109597864B (zh) 椭球边界卡尔曼滤波的即时定位与地图构建方法及系统
CN107728138B (zh) 一种基于当前统计模型的机动目标跟踪方法
CN101221238B (zh) 基于高斯均值移动配准的动态偏差估计方法
CN110501696B (zh) 一种基于多普勒量测自适应处理的雷达目标跟踪方法
CN111127523B (zh) 基于量测迭代更新的多传感器gmphd自适应融合方法
CN107229060B (zh) 一种基于自适应滤波的gps测量数据处理方法
CN108267731B (zh) 无人机目标跟踪系统的构建方法及应用
CN114637956B (zh) 一种基于双卡尔曼滤波器实现目标位置预测的方法
CN110895146A (zh) 一种移动机器人同步定位与地图构建方法
CN109115228A (zh) 一种基于加权最小二乘容积卡尔曼滤波的目标定位方法
CN110989341B (zh) 一种约束辅助粒子滤波方法及目标跟踪方法
Malleswaran et al. IMM-UKF-TFS model-based approach for intelligent navigation
CN116520380A (zh) 一种基于自适应卡尔曼滤波的动态目标组合定位方法
CN111291319A (zh) 一种应用于非高斯噪声环境下的移动机器人状态估计方法
CN117351042B (zh) 一种基于高斯和滤波的任意步时延目标跟踪方法
CN111340853B (zh) 基于ospa迭代的多传感器gmphd自适应融合方法
CN112986977A (zh) 一种克服雷达扩展卡尔曼航迹滤波发散的方法
CN110007298B (zh) 一种目标超前预测跟踪方法
Bhaumik et al. Risk-sensitive formulation of unscented Kalman filter
CN115859626A (zh) 针对周期运动目标的自适应无迹卡尔曼滤波器设计方法
CN114445459B (zh) 基于变分贝叶斯理论的连续-离散最大相关熵目标跟踪方法
CN111998854B (zh) 基于Cholesky分解计算的精确扩展Stirling插值滤波方法
Fahmedha et al. Estimation of system parameters using Kalman filter and extended Kalman filter
Huang et al. Bearing-only target tracking using a bank of MAP estimators
Sönmez et al. Analysis of performance criteria for optimization based bearing only target tracking algorithms

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
TR01 Transfer of patent right

Effective date of registration: 20221214

Address after: 501-c1, No. 19, Erquan East Road, Huizhi enterprise center, Xishan Economic and Technological Development Zone, Wuxi City, Jiangsu Province, 214000

Patentee after: Ruidina (Wuxi) Technology Co.,Ltd.

Address before: 210000 room 504, building 12, No. 29, buyue Road, Qiaolin street, Pukou District, Nanjing City, Jiangsu Province

Patentee before: Redina (Nanjing) Electronic Technology Co.,Ltd.

TR01 Transfer of patent right