CN107387064A - 一种新的排爆机器人隧进定位方法 - Google Patents

一种新的排爆机器人隧进定位方法 Download PDF

Info

Publication number
CN107387064A
CN107387064A CN201710621214.4A CN201710621214A CN107387064A CN 107387064 A CN107387064 A CN 107387064A CN 201710621214 A CN201710621214 A CN 201710621214A CN 107387064 A CN107387064 A CN 107387064A
Authority
CN
China
Prior art keywords
mrow
msubsup
msub
mover
mtd
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
CN201710621214.4A
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.)
Henan Institute of Science and Technology
Original Assignee
Henan Institute of Science and Technology
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 Henan Institute of Science and Technology filed Critical Henan Institute of Science and Technology
Priority to CN201710621214.4A priority Critical patent/CN107387064A/zh
Publication of CN107387064A publication Critical patent/CN107387064A/zh
Pending legal-status Critical Current

Links

Classifications

    • E—FIXED CONSTRUCTIONS
    • E21—EARTH OR ROCK DRILLING; MINING
    • E21B—EARTH OR ROCK DRILLING; OBTAINING OIL, GAS, WATER, SOLUBLE OR MELTABLE MATERIALS OR A SLURRY OF MINERALS FROM WELLS
    • E21B47/00—Survey of boreholes or wells
    • E21B47/09—Locating or determining the position of objects in boreholes or wells, e.g. the position of an extending arm; Identifying the free or blocked portions of pipes

Landscapes

  • Physics & Mathematics (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Engineering & Computer Science (AREA)
  • Geology (AREA)
  • Mining & Mineral Resources (AREA)
  • Geophysics (AREA)
  • Environmental & Geological Engineering (AREA)
  • Fluid Mechanics (AREA)
  • General Life Sciences & Earth Sciences (AREA)
  • Geochemistry & Mineralogy (AREA)
  • Other Investigation Or Analysis Of Materials By Electrical Means (AREA)

Abstract

本发明公开了一种新的排爆机器人隧进定位方法，步骤如下：S1，采样并初始化；S2.根据UKF算法更新粒子；S3，根据EKF算法再次更新粒子；S4，计算粒子的均值和均方差，根据建议分布函数随机抽取粒子并对每个粒子赋以权值S5，对进行归一化处理；S6.再采样，删除无效粒子，得到目标的位置。本发明在进行坐标估算过程中，进行两次粒子优化更新，降低了每个粒子坐标的误差，提高了滤波精度。在初始化阶段使用粒子滤波抽样方法，粒子数量有限，避免了UKF算法受粒子数影响而程序变慢的缺点，提高了运算速度。本发明应用于对过程噪声和量测噪声分布要求不高的非高斯噪声非线性系统，滤波精度高，定位误差小，而且算法简单，非常适合地下未知环境隧进系统钻头坐标定位。

Description

一种新的排爆机器人隧进定位方法
技术领域
本发明属于排爆机器人地下隧进方位检测的技术领域，具体设计一种新的排爆机器人隧进定位方法。
背景技术
隧进定位是确定殉爆系统在二维工作环境中相对于全局坐标的位置及其本身的姿态，以便随时掌握殉爆系统所处位置，防止意外情况出现时殉爆系统陷入侵彻弹道内无法退出。
目前常用定位滤波方法有扩展卡尔曼滤波、无迹卡尔曼滤波和粒子滤波。其中扩展卡尔曼滤波利用泰勒级数实现非线性问题线性化处理，建立的数学模型不准确，随着误差累积而产生野值，致使滤波器可靠性和收敛速度下降。无迹卡尔曼滤波适应于非线性系统，但是初值选择比较敏感，不适用于非高斯噪声系统。粒子滤波解决了非线性非高斯问题，是基于随机采样的滤波方法，算法复杂响应慢，容易出现严重的粒子退化现象。
发明内容
本发明要解决的是在机场道面下混泥土复合介质环境中，未爆弹殉爆系统的隧进定位方法受到混凝土复合介质的干扰不能正常工作，定位误差大不可靠等技术问题，从而提供一种定位准确、算法简单、检测精度和可靠性高的基于改进卡尔曼滤波的隧进定位方法。
为解决上述技术问题，本发明所采用的技术方案如下：一种新的排爆机器人隧进定位方法，步骤如下：S1，采样并初始化。
具体步骤为：
S1.1，采样；根据初始先验密度函数p(x0)，从殉爆系统头部坐标的状态向量中随机抽取采样样本，获得第0时刻的N个采样粒子其中，i＝1,2,…,N。
产生初始化后的随机样本空间为随着时间推移可以产生k时刻的样本空间为
其中，表示0时刻的第i个样本集合，即粒子集合。表示为0时刻第i个粒子的权重值。
S1.2，赋初值；令：
其中，表示0时刻第i个采样粒子的均值；表示0时刻第i个随机状态向量采样值；表示i时刻第i个粒子的协方差；表示0时刻第i个粒子扩展状态噪声和量测噪声后的增广状态向量的均值；表示0时刻第i个粒子扩展状态噪声和量测噪声后的增广状态向量；a表示状态向量扩展；表示0时刻第i个粒子的增广状态向量的协方差；Q表示状态噪声的协方差；R表示量测噪声的协方差。
S2.根据UKF(Unscented Kalman Filter，无迹卡尔曼滤波)算法更新粒子。
具体步骤为：
S2.1基于粒子通过UT变换方法计算2na个σ点构造矩阵χ，矩阵χ内的向量χi为σ点，计算公式为：
λ＝α2(n+κ)-n (7)；
其中，表示k-1时刻第i个采样粒子的第j个σ点；表示k-1时刻第i个粒子扩展状态噪声和量测噪声后的增广状态向量的均值；表示k-1时刻第i个粒子的增广状态向量的协方差；表示矩阵平方根第j列；na表示σ点个数；λ表示尺度因子；α为常数，表示分布在周围的散布程度，取值范围为0.0001≤α≤1；κ为常数是一个辅助尺度因子，取值为0；表示第j个σ点计算一阶统计特性时的权值；是第j个σ点计算二阶统计特性时的权值。
S2.2，将步骤S2.1中的σ点通过系统模型和测量模型向前传递，传递方程为：
其中，表示k-1时刻第j个σ点一步状态预测值；表示k-1时刻第i个采样粒子的第j个σ点；表示非线性状态传递函数；表示k-1时刻第i个粒子第j个σ点的一步预测测量值；表示非线性量测传递函数。
S2.3，根据步骤S2.2得到一步状态预测值和协方差一步预测值
一步状态预测值的计算公式为：
其中，na表示UT变换σ点个数；表示第j个σ点计算一阶统计特性时的权值；表示k-1时刻第j个σ点状态一步预测值。
协方差一步预测值的计算公式为：
其中，表示k-1时刻第i个粒子一步预测协方差；是第j个σ点计算二阶统计特性时的权值；表示k-1时刻第i个粒子基于UKF算法计算的状态一步预测均值。
S 2.4，根据步骤S2.2计算一步预测测量值的均值
均值的计算公式为：
其中，表示k-1时刻第i个粒子一步预测测量值的均值。
S2.5，通过传感器直接获取当时新的测量值zk，并计算新的状态预测值
计算公式为：
其中，Kk表示卡尔曼增益。
卡尔曼增益Kk的计算公式为：
S3，根据EKF(Extended Kalman Filter，扩展卡尔曼滤波)算法再次更新粒子。
具体步骤为：
S3.1，根据如下公式分别计算系统模型雅可比矩阵和观测模型的
计算公式为：
其中，表示k-1时刻第i个粒子的m x n维矩阵；m表示为f函数的方程个数；n表示为x状态向量维数；表示为k-1时刻第i个粒子的不包含噪声的理想状态；表示为k-1时刻第i个粒子包含了噪声的待估计状态；f(*)表示为状态一步预测转移方程组；表示为对第m个方程求第n个状态的偏导。
S 3.2，令计算状态一步预测值和协方差一步预测值。
计算公式为：
其中，表示为第i个粒子的状态一步预测值；表示状态的一步预测函数；表示为从k-1时刻到k时刻第i个粒子的状态预测协方差；表示为k-1时刻第i个粒子的协方差；表示为的矩阵转置；Qk表示为k时刻状态噪声的协方差。
S 3.3，计算卡尔曼增益Kk；
计算公式为：
S 3.4，计算修正的状态预测量和协方差预测值；
其中，表示为第i个粒子修正后的协方差；表示为k-1时刻到k时刻第i个粒子预测值的协方差；表示为第i个粒子修正后的状态均值；表示为k-1时刻到k时刻第i个粒子状态预测值；表示k时刻的观察噪声；zk表示k时刻的真实测量值；表示为k-1时刻到k时刻第i个粒子测量估计。
S3.5，计算第i个粒子k时刻的状态预测量和协方差预测值；
S4，根据步骤S3中的粒子，计算粒子的均值和均方差，并根据粒子均值和均方差得到近似服从高斯分布的建议分布函数根据建议分布函数随机抽取粒子并对每个粒子赋以权值具体公式为：
其中，表示k时刻第i个粒子的权重；表示k时刻第i粒子量测传递概率密度函数；表示k时刻第i个粒子状态传递概率密度函数；表示第i个粒子的重要性密度函数；表示k-1时刻第i个粒子的权重。
S5，对步骤S4中的进行归一化处理，并计算出状态的最终估计值
计算公式如下：
S6.再采样，删除无效粒子，得到目标的位置。
具体步骤为：
S6.1，计算粒子退化程度；
S6.2，判断粒子退化程度当消除权值较小的粒子，复制权值较大的粒子，重新获得N个随机样本粒子
计算公式如下：
设计累积概率ci，
选择[0,1]区间中的一个随机数rj(j＝1,2,…,N)，根据ci﹥rj找到min(ci)来确定i，并令新粒子最终确定再采样后新的粒子集合。
其中，表示再采样后的第j个粒子；表示再采样前的第i个粒子；
S6.3，为每个再采样之后的样本粒子赋以相同的权值
S6.4，重复步骤S2—S5，获得任一时刻的钻头空间坐标状态量直到多种传感器探测到爆炸物结束循环，确定爆炸物坐标。
本发明的有益效果为：本发明通过分析EKF算法和UPF算法存在的问题，研究了非线性滤波方法，提出了一种新的滤波算法——混合卡尔曼粒子滤波算法(MKPF)。首先选用了非常适用于非高斯噪声非线性系统的粒子滤波理论，对隧进系统钻头坐标进行状态随机抽样。其次列出状态方程和观测方程，利用Unscented卡尔曼滤波算法收敛速度快等优点对每个粒子进行状态预测和量测更新，产生系统的无偏状态估计。然后重复使用上一步的状态方程和观测方程基于扩展卡尔曼滤波算法进一步优化该粒子的状态值，并计算出对应的协方差，得出某时刻某个粒子的最终状态估计。最后根据该最终状态估计值组建粒子滤波的建议分布，重新抽样粒子，并计算出该粒子的权值。以上步骤重复N次之后，对所有粒子的权值进行归一化处理，得出某时刻钻头最终坐标状态值。该方法在进行坐标估算过程中，进行两次粒子优化更新，降低了每个粒子坐标的误差，提高了滤波精度。在初始化阶段使用粒子滤波抽样方法，粒子数量有限，避免了UKF算法受粒子数影响而程序变慢的缺点，提高了运算速度。该方法混合了粒子算法、UKF算法和EKF算法。基本思想是首先基于粒子算法抽样N个粒子，其次通过UKF算法优化粒子的状态值，再次通过EKF算法进一步优化粒子状态值并计算协方差产生粒子滤波所需要的建议分布，然后通过粒子滤波方法进行粒子重采样，并计算新粒子的权值。最后归一化N个粒子的权值，经过加权平均计算出钻头某时刻的坐标状态，直到传感器发现爆炸物为止。通过对该滤波算法的数学建模仿真实验和分析，UPF算法和UKF算法比EKF算法有更高的跟踪滤波精度，但新提出的MKPF算法和实际轨迹最接近，滤波效果明显优于其他方法。该方法应用于对过程噪声和量测噪声分布要求不高的非高斯噪声非线性系统，滤波精度高，定位误差小，而且算法简单易于编程实现，非常适合地下未知环境隧进系统钻头坐标定位。
附图说明
图1为各种滤波方法对殉爆头部在水平方向的估计轨迹对比图。
图2为各种滤波方法对殉爆头部在深度方向的估计轨迹对比图。
图3为各种滤波方法对殉爆头部在水平方向的均方误差曲线对比图。
图4为各种滤波方法对殉爆头部在深度方向的均方误差曲线对比图。
具体实施方式
一种新的排爆机器人隧进定位方法，步骤如下：S1，采样并初始化。
具体步骤为：
S1.1，采样；根据初始先验密度函数p(x0)，从殉爆系统头部坐标的状态向量中随机抽取采样样本，获得第0时刻的N个采样粒子其中，i＝1,2,…,N。
产生初始化后的随机样本空间为随着时间推移可以产生k时刻的样本空间为
其中，表示0时刻的第i个样本集合，即粒子集合。表示为0时刻第i个粒子的权重值。
S1.2，赋初值；令：
其中，表示0时刻第i个采样粒子的均值；表示0时刻第i个随机状态向量采样值；表示i时刻第i个粒子的协方差；表示0时刻第i个粒子扩展状态噪声和量测噪声后的增广状态向量的均值；表示0时刻第i个粒子扩展状态噪声和量测噪声后的增广状态向量；a表示状态向量扩展；表示0时刻第i个粒子的增广状态向量的协方差；Q表示状态噪声的协方差；R表示量测噪声的协方差。
S2.根据UKF(Unscented Kalman Filter，无迹卡尔曼滤波)算法更新粒子。
具体步骤为：
S2.1基于粒子通过UT变换方法计算2na个σ点构造矩阵χ，矩阵χ内的向量χi为σ点，计算公式为：
λ＝α2(n+κ)-n (7)；
其中，表示k-1时刻第i个采样粒子的第j个σ点；表示k-1时刻第i个粒子扩展状态噪声和量测噪声后的增广状态向量的均值；表示k-1时刻第i个粒子的增广状态向量的协方差；表示矩阵平方根第j列；na表示σ点个数；λ表示尺度因子；α为常数，表示分布在周围的散布程度，取值范围为0.0001≤α≤1；κ为常数是一个辅助尺度因子，取值为0；表示第j个σ点计算一阶统计特性时的权值；是第j个σ点计算二阶统计特性时的权值。
S2.2，将步骤S2.1中的σ点通过系统模型和测量模型向前传递，传递方程为：
其中，表示k-1时刻第j个σ点一步状态预测值；表示k-1时刻第i个采样粒子的第j个σ点；表示非线性状态传递函数；表示k-1时刻第i个粒子第j个σ点的一步预测测量值；表示非线性量测传递函数。
S2.3，根据步骤S2.2得到一步状态预测值和协方差一步预测值
一步状态预测值的计算公式为：
其中，na表示UT变换σ点个数；表示第j个σ点计算一阶统计特性时的权值；表示k-1时刻第j个σ点状态一步预测值。
协方差一步预测值的计算公式为：
其中，表示k-1时刻第i个粒子一步预测协方差；是第j个σ点计算二阶统计特性时的权值；表示k-1时刻第i个粒子基于UKF算法计算的状态一步预测均值。
S 2.4，根据步骤S2.2计算一步预测测量值的均值
均值的计算公式为：
其中，表示k-1时刻第i个粒子一步预测测量值的均值。
S2.5，通过传感器直接获取当时新的测量值zk，并计算新的状态预测值
计算公式为：
其中，Kk表示卡尔曼增益。
卡尔曼增益Kk的计算公式为：
S3，根据EKF(Extended Kalman Filter，扩展卡尔曼滤波)算法再次更新粒子。
具体步骤为：
S3.1，根据如下公式分别计算系统模型雅可比矩阵和观测模型的
计算公式为：
其中，表示k-1时刻第i个粒子的m x n维矩阵；m表示为f函数的方程个数；n表示为x状态向量维数；表示为k-1时刻第i个粒子的不包含噪声的理想状态；表示为k-1时刻第i个粒子包含了噪声的待估计状态；f(*)表示为状态一步预测转移方程组；表示为对第m个方程求第n个状态的偏导。
S 3.2，令计算状态一步预测值和协方差一步预测值。
计算公式为：
其中，表示为第i个粒子的状态一步预测值；表示状态的一步预测函数；表示为从k-1时刻到k时刻第i个粒子的状态预测协方差；表示为k-1时刻第i个粒子的协方差；表示为的矩阵转置；Qk表示为k时刻状态噪声的协方差。
S 3.3，计算卡尔曼增益Kk；
计算公式为：
S 3.4，计算修正的状态预测量和协方差预测值；
其中，表示为第i个粒子修正后的协方差；表示为k-1时刻到k时刻第i个粒子预测值的协方差；表示为第i个粒子修正后的状态均值；表示为k-1时刻到k时刻第i个粒子状态预测值；表示k时刻的观察噪声；zk表示k时刻的真实测量值；表示为k-1时刻到k时刻第i个粒子测量估计。
S3.5，计算第i个粒子k时刻的状态预测量和协方差预测值；
S4，根据步骤S3中的粒子，计算粒子的均值和均方差，并根据粒子均值和均方差得到近似服从高斯分布的建议分布函数根据建议分布函数随机抽取粒子并对每个粒子赋以权值具体公式为：
其中，表示k时刻第i个粒子的权重；表示k时刻第i粒子量测传递概率密度函数；表示k时刻第i个粒子状态传递概率密度函数；表示第i个粒子的重要性密度函数；表示k-1时刻第i个粒子的权重。
S5，对步骤S4中的进行归一化处理，并计算出状态的最终估计值
计算公式如下：
S6.再采样，删除无效粒子，得到目标的位置。
具体步骤为：
S6.1，计算粒子退化程度；
S6.2，判断粒子退化程度当消除权值较小的粒子，复制权值较大的粒子，重新获得N个随机样本粒子
计算公式如下：
设计累积概率ci，
选择[0,1]区间中的一个随机数rj(j＝1,2,…,N)，根据ci﹥rj找到min(ci)来确定i，并令新粒子最终确定再采样后新的粒子集合。
其中，表示再采样后的第j个粒子；表示再采样前的第i个粒子；
S6.3，为每个再采样之后的样本粒子赋以相同的权值
S6.4，重复步骤S2—S5，获得任一时刻的钻头空间坐标状态量直到多种传感器探测到爆炸物结束循环，确定爆炸物坐标。
下面以实验对比进行说明
本发明利用多种非线性滤波分别对殉爆头部进行了定位跟踪，并对其轨迹进行了估计，各种滤波对水平方向的估计轨迹如图1所示，在深度方向的估计轨迹如图2所示。每个时刻产生的水平方向均方误差曲线如图3所示，深度方向的均方误差曲线如图4所示。产生的均方误差均值和方差如表1所示。
表1
由图1-4以及表1可以看出于：UPF和UKF在水平方向上估计精度相近，在深度方向上UPF要好于UKF。在深度方向上UPF和MKPF的精度接近，但MKPF效果的最为理想。一方面说明了在一般的非线性高斯环境中，如果采用UKF方法，只要合理确定α和β的值，就可能获得对状态均值和方差的估计误差在4阶水平上，能获得较好的滤波精度，且运算较快。另一方面，也说明了UPF算法性能受到一些参数的影响：通常粒子数随所估计的状态向量维数的增加而增加，但是这又极大地增加了计算量，使得程序变慢。
MKPF也就是本发明的方法，首先用Unscented卡尔曼滤波器产生系统的状态估计，然后用扩展卡尔曼滤波器重复这一过程并产生系统的最终状态估计。它可用于处理非线性非高斯模型，对系统的过程噪声和量测噪声分布要求不高，而且算法简单，并且易于编程实现，避免了UPF受粒子数影响而程序变慢的缺点。

Claims (5)

1.一种新的排爆机器人隧进定位方法，其特征在于，步骤如下：
S1，采样并初始化；
S2，根据UKF算法更新粒子；
S3，根据EKF算法再次更新粒子；
S4，根据步骤S3中的粒子，计算粒子的均值和均方差，并根据粒子均值和均方差得到近似服从高斯分布的建议分布函数，根据建议分布函数随机抽取粒子并对每个粒子赋以权值具体公式为：
<mrow> <msubsup> <mi>w</mi> <mi>k</mi> <mi>i</mi> </msubsup> <mo>=</mo> <mfrac> <mrow> <mi>p</mi> <mrow> <mo>(</mo> <msub> <mi>Z</mi> <mi>k</mi> </msub> <mo>|</mo> <msubsup> <mi>X</mi> <mi>k</mi> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> </msubsup> <mo>)</mo> </mrow> <mi>p</mi> <mrow> <mo>(</mo> <msubsup> <mi>X</mi> <mi>k</mi> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> </msubsup> <mo>|</mo> <msubsup> <mi>X</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> </msubsup> <mo>)</mo> </mrow> </mrow> <mrow> <mi>q</mi> <mrow> <mo>(</mo> <msubsup> <mi>X</mi> <mi>k</mi> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> </msubsup> <mo>|</mo> <msubsup> <mi>X</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> </msubsup> <mo>,</mo> <msub> <mi>Z</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> </mrow> </mfrac> <mo>&amp;CenterDot;</mo> <msubsup> <mi>w</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> </msubsup> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>24</mn> <mo>)</mo> </mrow> <mo>;</mo> </mrow>
其中，表示k时刻第i个粒子的权重；表示k时刻第i粒子量测传递概率密度函数；表示k时刻第i个粒子状态传递概率密度函数；表示第i个粒子的重要性密度函数；表示k-1时刻第i个粒子的权重；
S5，对步骤S4中的进行归一化处理；
计算公式如下：
<mrow> <msubsup> <mover> <mi>w</mi> <mo>^</mo> </mover> <mi>k</mi> <mi>i</mi> </msubsup> <mo>=</mo> <msubsup> <mi>w</mi> <mi>k</mi> <mi>i</mi> </msubsup> <mo>/</mo> <msub> <mi>&amp;Sigma;</mi> <mrow> <mi>j</mi> <mo>=</mo> <mn>1</mn> <mo>:</mo> <mi>N</mi> </mrow> </msub> <msubsup> <mi>w</mi> <mi>k</mi> <mi>j</mi> </msubsup> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>25</mn> <mo>)</mo> </mrow> <mo>;</mo> </mrow>
S6.再采样，删除无效粒子，得到目标的位置。
2.根据权利要求1所述的新的排爆机器人隧进定位方法，其特征在于，在步骤S1中，具体步骤为：S1.1，采样；
根据初始先验密度函数p(x0)，从殉爆系统头部坐标的状态向量中随机抽取采样样本，获得第0时刻的N个采样粒子其中，i＝1,2,…,N；
S1.2，赋初值；令：
<mrow> <msubsup> <mover> <mi>x</mi> <mo>&amp;OverBar;</mo> </mover> <mn>0</mn> <mi>i</mi> </msubsup> <mo>=</mo> <mi>E</mi> <mrow> <mo>(</mo> <msubsup> <mi>x</mi> <mn>0</mn> <mi>i</mi> </msubsup> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>1</mn> <mo>)</mo> </mrow> <mo>;</mo> </mrow>
<mrow> <msubsup> <mi>P</mi> <mn>0</mn> <mi>i</mi> </msubsup> <mo>=</mo> <mi>E</mi> <mo>&amp;lsqb;</mo> <mrow> <mo>(</mo> <msubsup> <mi>x</mi> <mn>0</mn> <mi>i</mi> </msubsup> <mo>-</mo> <msubsup> <mover> <mi>x</mi> <mo>&amp;OverBar;</mo> </mover> <mn>0</mn> <mi>i</mi> </msubsup> <mo>)</mo> </mrow> <msup> <mrow> <mo>(</mo> <msubsup> <mi>x</mi> <mn>0</mn> <mi>i</mi> </msubsup> <mo>-</mo> <msubsup> <mover> <mi>x</mi> <mo>&amp;OverBar;</mo> </mover> <mn>0</mn> <mi>i</mi> </msubsup> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mo>&amp;rsqb;</mo> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>2</mn> <mo>)</mo> </mrow> <mo>;</mo> </mrow>
<mrow> <msubsup> <mover> <mi>x</mi> <mo>&amp;OverBar;</mo> </mover> <mn>0</mn> <mrow> <mi>i</mi> <mo>,</mo> <mi>a</mi> </mrow> </msubsup> <mo>=</mo> <mi>E</mi> <mo>&amp;lsqb;</mo> <msubsup> <mi>x</mi> <mn>0</mn> <mrow> <mi>i</mi> <mo>,</mo> <mi>a</mi> </mrow> </msubsup> <mo>&amp;rsqb;</mo> <mo>=</mo> <msup> <mrow> <mo>&amp;lsqb;</mo> <msup> <mrow> <mo>(</mo> <msubsup> <mi>x</mi> <mn>0</mn> <mi>i</mi> </msubsup> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mo>,</mo> <mn>0</mn> <mo>,</mo> <mn>0</mn> <mo>&amp;rsqb;</mo> </mrow> <mi>T</mi> </msup> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>3</mn> <mo>)</mo> </mrow> <mo>;</mo> </mrow>
<mrow> <msubsup> <mi>P</mi> <mn>0</mn> <mrow> <mi>i</mi> <mo>,</mo> <mi>a</mi> </mrow> </msubsup> <mo>=</mo> <mi>E</mi> <mo>&amp;lsqb;</mo> <mrow> <mo>(</mo> <msubsup> <mi>r</mi> <mn>0</mn> <mrow> <mi>i</mi> <mo>,</mo> <mi>a</mi> </mrow> </msubsup> <mo>-</mo> <msubsup> <mover> <mi>x</mi> <mo>&amp;OverBar;</mo> </mover> <mn>0</mn> <mrow> <mi>i</mi> <mo>,</mo> <mi>a</mi> </mrow> </msubsup> <mo>)</mo> </mrow> <msup> <mrow> <mo>(</mo> <msubsup> <mi>x</mi> <mn>0</mn> <mrow> <mi>i</mi> <mo>,</mo> <mi>a</mi> </mrow> </msubsup> <mo>-</mo> <msubsup> <mover> <mi>x</mi> <mo>&amp;OverBar;</mo> </mover> <mn>0</mn> <mrow> <mi>i</mi> <mo>,</mo> <mi>a</mi> </mrow> </msubsup> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mo>&amp;rsqb;</mo> <mo>=</mo> <mi>d</mi> <mi>i</mi> <mi>a</mi> <mi>g</mi> <mfenced open = "(" close = ")"> <mtable> <mtr> <mtd> <msubsup> <mi>P</mi> <mn>0</mn> <mi>i</mi> </msubsup> </mtd> <mtd> <mi>Q</mi> </mtd> <mtd> <mi>R</mi> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>4</mn> <mo>)</mo> </mrow> <mo>;</mo> </mrow>
其中，表示0时刻第i个采样粒子的均值；表示0时刻第i个随机状态向量采样值；表示i时刻第i个粒子的协方差；表示0时刻第i个粒子扩展状态噪声和量测噪声后的增广状态向量的均值；表示0时刻第i个粒子扩展状态噪声和量测噪声后的增广状态向量；a表示状态向量扩展；表示0时刻第i个粒子的增广状态向量的协方差；Q表示状态噪声的协方差；R表示量测噪声的协方差。
3.根据权利要求1所述的新的排爆机器人隧进定位方法，其特征在于，在步骤S2中，具体步骤为：
S2.1构造矩阵χ，矩阵χ内的向量χi为σ点，计算公式为：
<mrow> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msubsup> <mi>&amp;chi;</mi> <mrow> <mn>0</mn> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mrow> <mi>i</mi> <mo>,</mo> <mi>a</mi> </mrow> </msubsup> <mo>=</mo> <msubsup> <mover> <mi>x</mi> <mo>&amp;OverBar;</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mrow> <mi>i</mi> <mo>,</mo> <mi>a</mi> </mrow> </msubsup> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msubsup> <mi>&amp;chi;</mi> <mrow> <mi>j</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mrow> <mi>i</mi> <mo>,</mo> <mi>a</mi> </mrow> </msubsup> <mo>=</mo> <msubsup> <mover> <mi>x</mi> <mo>&amp;OverBar;</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mrow> <mi>i</mi> <mo>,</mo> <mi>a</mi> </mrow> </msubsup> <mo>+</mo> <msub> <mrow> <mo>(</mo> <msqrt> <mrow> <mo>(</mo> <msub> <mi>n</mi> <mi>a</mi> </msub> <mo>+</mo> <mi>&amp;lambda;</mi> <mo>)</mo> <msubsup> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mrow> <mi>i</mi> <mo>,</mo> <mi>a</mi> </mrow> </msubsup> </mrow> </msqrt> <mo>)</mo> </mrow> <mi>j</mi> </msub> <mo>,</mo> <mi>j</mi> <mo>=</mo> <mn>1</mn> <mo>,</mo> <mn>2</mn> <mo>,</mo> <mn>...</mn> <mo>,</mo> <msub> <mi>n</mi> <mi>a</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msubsup> <mi>&amp;chi;</mi> <mrow> <mi>j</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mrow> <mi>i</mi> <mo>,</mo> <mi>a</mi> </mrow> </msubsup> <mo>=</mo> <msubsup> <mover> <mi>x</mi> <mo>&amp;OverBar;</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mrow> <mi>i</mi> <mo>,</mo> <mi>a</mi> </mrow> </msubsup> <mo>-</mo> <msub> <mrow> <mo>(</mo> <msqrt> <mrow> <mo>(</mo> <msub> <mi>n</mi> <mi>a</mi> </msub> <mo>+</mo> <mi>&amp;lambda;</mi> <mo>)</mo> <msubsup> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mrow> <mi>i</mi> <mo>,</mo> <mi>a</mi> </mrow> </msubsup> </mrow> </msqrt> <mo>)</mo> </mrow> <mi>j</mi> </msub> <mo>,</mo> <mi>j</mi> <mo>=</mo> <msub> <mi>n</mi> <mi>a</mi> </msub> <mo>+</mo> <mn>1</mn> <mo>,</mo> <msub> <mi>n</mi> <mi>a</mi> </msub> <mo>+</mo> <mn>2</mn> <mo>,</mo> <mn>...</mn> <mo>,</mo> <mn>2</mn> <msub> <mi>n</mi> <mi>a</mi> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>5</mn> <mo>)</mo> </mrow> <mo>;</mo> </mrow>
<mrow> <msub> <mi>w</mi> <mi>i</mi> </msub> <mo>=</mo> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msubsup> <mi>W</mi> <mn>0</mn> <mrow> <mo>(</mo> <mi>m</mi> <mo>)</mo> </mrow> </msubsup> <mo>=</mo> <mi>&amp;lambda;</mi> <mo>/</mo> <mrow> <mo>(</mo> <mi>n</mi> <mo>+</mo> <mi>&amp;lambda;</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msubsup> <mi>W</mi> <mn>0</mn> <mrow> <mo>(</mo> <mi>c</mi> <mo>)</mo> </mrow> </msubsup> <mo>=</mo> <mi>&amp;lambda;</mi> <mo>/</mo> <mrow> <mo>(</mo> <mi>n</mi> <mo>+</mo> <mi>&amp;lambda;</mi> <mo>)</mo> </mrow> <mo>+</mo> <mrow> <mo>(</mo> <mn>1</mn> <mo>-</mo> <msup> <mi>&amp;alpha;</mi> <mn>2</mn> </msup> <mo>+</mo> <mi>&amp;beta;</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msubsup> <mi>W</mi> <mi>i</mi> <mrow> <mo>(</mo> <mi>m</mi> <mo>)</mo> </mrow> </msubsup> <mo>=</mo> <msubsup> <mi>W</mi> <mi>i</mi> <mrow> <mo>(</mo> <mi>c</mi> <mo>)</mo> </mrow> </msubsup> <mo>=</mo> <mn>0.5</mn> <mo>/</mo> <mrow> <mo>(</mo> <mi>n</mi> <mo>+</mo> <mi>&amp;lambda;</mi> <mo>)</mo> </mrow> <mo>,</mo> <mi>i</mi> <mo>=</mo> <mn>1</mn> <mo>,</mo> <mn>2</mn> <mo>,</mo> <mo>...</mo> <mo>,</mo> <mn>2</mn> <mi>n</mi> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>6</mn> <mo>)</mo> </mrow> <mo>;</mo> </mrow>
λ＝α2(n+κ)-n (7)；
其中，表示k-1时刻第i个采样粒子的第j个σ点；表示k-1时刻第i个粒子扩展状态噪声和量测噪声后的增广状态向量的均值；表示k-1时刻第i个粒子的增广状态向量的协方差；表示矩阵平方根第j列；na表示σ点个数；λ表示尺度因子；α为常数，表示分布在周围的散布程度，取值范围为0.0001≤α≤1；κ为常数是一个辅助尺度因子，取值为0；表示第j个σ点计算一阶统计特性时的权值；是第j个σ点计算二阶统计特性时的权值；
S2.2，将步骤S2.1中的σ点通过系统模型和测量模型向前传递，传递方程为：
<mrow> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msubsup> <mi>&amp;chi;</mi> <mrow> <mi>j</mi> <mo>,</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mrow> <mi>i</mi> <mo>,</mo> <mi>a</mi> </mrow> </msubsup> <mo>=</mo> <mi>f</mi> <mrow> <mo>(</mo> <msubsup> <mi>&amp;chi;</mi> <mrow> <mi>j</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mrow> <mi>i</mi> <mo>,</mo> <mi>a</mi> </mrow> </msubsup> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msubsup> <mi>Z</mi> <mrow> <mi>j</mi> <mo>,</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mrow> <mi>i</mi> <mo>,</mo> <mi>a</mi> </mrow> </msubsup> <mo>=</mo> <mi>h</mi> <mrow> <mo>(</mo> <msubsup> <mi>&amp;chi;</mi> <mrow> <mi>j</mi> <mo>,</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mrow> <mi>i</mi> <mo>,</mo> <mi>a</mi> </mrow> </msubsup> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>8</mn> <mo>)</mo> </mrow> <mo>;</mo> </mrow>
其中，表示k-1时刻第j个σ点一步状态预测值；表示k-1时刻第i个采样粒子的第j个σ点；表示非线性状态传递函数；表示k-1时刻第i个粒子第j个σ点的一步预测测量值；表示非线性量测传递函数；
S2.3，根据步骤S2.2得到一步状态预测值和协方差一步预测值
一步状态预测值的计算公式为：
<mrow> <msubsup> <mover> <mi>x</mi> <mo>&amp;OverBar;</mo> </mover> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <msub> <mn>1</mn> <mrow> <mi>u</mi> <mi>k</mi> <mi>f</mi> </mrow> </msub> </mrow> <mi>i</mi> </msubsup> <mo>=</mo> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>j</mi> <mo>=</mo> <mn>0</mn> </mrow> <mrow> <mn>2</mn> <msub> <mi>n</mi> <mi>a</mi> </msub> </mrow> </munderover> <msubsup> <mi>w</mi> <mi>j</mi> <mrow> <mo>(</mo> <mi>m</mi> <mo>)</mo> </mrow> </msubsup> <msubsup> <mi>&amp;chi;</mi> <mrow> <mi>j</mi> <mo>,</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mrow> <mi>i</mi> <mo>,</mo> <mi>a</mi> </mrow> </msubsup> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>9</mn> <mo>)</mo> </mrow> <mo>;</mo> </mrow>
其中，na表示UT变换σ点个数；表示第j个σ点计算一阶统计特性时的权值；表示k-1时刻第j个σ点状态一步预测值；
协方差一步预测值的计算公式为：
<mrow> <msubsup> <mi>P</mi> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <msub> <mn>1</mn> <mrow> <mi>u</mi> <mi>k</mi> <mi>f</mi> </mrow> </msub> </mrow> <mi>i</mi> </msubsup> <mo>=</mo> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>j</mi> <mo>=</mo> <mn>0</mn> </mrow> <mrow> <mn>2</mn> <msub> <mi>n</mi> <mi>a</mi> </msub> </mrow> </munderover> <msubsup> <mi>w</mi> <mi>j</mi> <mrow> <mo>(</mo> <mi>c</mi> <mo>)</mo> </mrow> </msubsup> <mo>&amp;lsqb;</mo> <msubsup> <mi>&amp;chi;</mi> <mrow> <mi>j</mi> <mo>,</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mrow> <mi>i</mi> <mo>,</mo> <mi>a</mi> </mrow> </msubsup> <mo>-</mo> <msubsup> <mover> <mi>x</mi> <mo>&amp;OverBar;</mo> </mover> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <msub> <mn>1</mn> <mrow> <mi>k</mi> <mi>u</mi> <mi>f</mi> </mrow> </msub> </mrow> <mi>i</mi> </msubsup> <mo>&amp;rsqb;</mo> <msup> <mrow> <mo>&amp;lsqb;</mo> <msubsup> <mi>&amp;chi;</mi> <mrow> <mi>j</mi> <mo>,</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mrow> <mi>i</mi> <mo>,</mo> <mi>a</mi> </mrow> </msubsup> <mo>-</mo> <msubsup> <mover> <mi>x</mi> <mo>&amp;OverBar;</mo> </mover> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <msub> <mn>1</mn> <mrow> <mi>k</mi> <mi>u</mi> <mi>f</mi> </mrow> </msub> </mrow> <mi>i</mi> </msubsup> <mo>&amp;rsqb;</mo> </mrow> <mi>T</mi> </msup> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>10</mn> <mo>)</mo> </mrow> <mo>;</mo> </mrow>
其中，表示k-1时刻第i个粒子一步预测协方差；是第j个σ点计算二阶统计特性时的权值；表示k-1时刻第i个粒子基于UKF算法计算的状态一步预测均值；
S2.4，根据步骤S2.2计算一步预测测量值的均值
均值的计算公式为：
<mrow> <msubsup> <mover> <mi>z</mi> <mo>&amp;OverBar;</mo> </mover> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <msub> <mn>1</mn> <mrow> <mi>u</mi> <mi>k</mi> <mi>f</mi> </mrow> </msub> </mrow> <mi>i</mi> </msubsup> <mo>=</mo> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>j</mi> <mo>=</mo> <mn>0</mn> </mrow> <mrow> <mn>2</mn> <msub> <mi>n</mi> <mi>a</mi> </msub> </mrow> </munderover> <msubsup> <mi>W</mi> <mi>j</mi> <mrow> <mo>(</mo> <mi>m</mi> <mo>)</mo> </mrow> </msubsup> <msubsup> <mi>Z</mi> <mrow> <mi>j</mi> <mo>,</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>i</mi> </msubsup> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>11</mn> <mo>)</mo> </mrow> <mo>;</mo> </mrow>
其中，表示k-1时刻第i个粒子一步预测测量值的均值；
S2.5，通过传感器直接获取当时新的测量值zk，并计算新的状态预测值
计算公式为：
<mrow> <msubsup> <mover> <mi>x</mi> <mo>&amp;OverBar;</mo> </mover> <msub> <mi>k</mi> <mrow> <mi>u</mi> <mi>k</mi> <mi>f</mi> </mrow> </msub> <mi>i</mi> </msubsup> <mo>=</mo> <msubsup> <mover> <mi>x</mi> <mo>&amp;OverBar;</mo> </mover> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <msub> <mn>1</mn> <mrow> <mi>u</mi> <mi>k</mi> <mi>f</mi> </mrow> </msub> </mrow> <mi>i</mi> </msubsup> <mo>+</mo> <msub> <mi>K</mi> <mi>k</mi> </msub> <mrow> <mo>(</mo> <msub> <mi>z</mi> <mi>k</mi> </msub> <mo>-</mo> <msubsup> <mover> <mi>z</mi> <mo>&amp;OverBar;</mo> </mover> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <msub> <mn>1</mn> <mrow> <mi>u</mi> <mi>k</mi> <mi>f</mi> </mrow> </msub> </mrow> <mi>i</mi> </msubsup> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>12</mn> <mo>)</mo> </mrow> <mo>;</mo> </mrow>
其中，Kk表示卡尔曼增益；
卡尔曼增益Kk的计算公式为：
<mrow> <msub> <mi>K</mi> <mi>k</mi> </msub> <mo>=</mo> <msub> <mi>P</mi> <mrow> <msub> <mover> <mi>x</mi> <mo>~</mo> </mover> <mi>k</mi> </msub> <msub> <mover> <mi>z</mi> <mo>~</mo> </mover> <mi>k</mi> </msub> </mrow> </msub> <msubsup> <mi>P</mi> <mrow> <msub> <mover> <mi>z</mi> <mo>~</mo> </mover> <mi>k</mi> </msub> <msub> <mover> <mi>z</mi> <mo>~</mo> </mover> <mi>k</mi> </msub> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msubsup> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>13</mn> <mo>)</mo> </mrow> <mo>;</mo> </mrow>
<mrow> <msub> <mi>P</mi> <mrow> <msub> <mover> <mi>z</mi> <mo>~</mo> </mover> <mi>k</mi> </msub> <msub> <mover> <mi>z</mi> <mo>~</mo> </mover> <mi>k</mi> </msub> </mrow> </msub> <mo>=</mo> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>j</mi> <mo>=</mo> <mn>0</mn> </mrow> <mrow> <mn>2</mn> <msub> <mi>n</mi> <mi>a</mi> </msub> </mrow> </munderover> <msubsup> <mi>W</mi> <mi>j</mi> <mrow> <mo>(</mo> <mi>c</mi> <mo>)</mo> </mrow> </msubsup> <mo>&amp;lsqb;</mo> <msubsup> <mi>Z</mi> <mrow> <mi>j</mi> <mo>,</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>i</mi> </msubsup> <mo>-</mo> <msubsup> <mover> <mi>z</mi> <mo>&amp;OverBar;</mo> </mover> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>i</mi> </msubsup> <mo>&amp;rsqb;</mo> <msup> <mrow> <mo>&amp;lsqb;</mo> <msubsup> <mi>Z</mi> <mrow> <mi>j</mi> <mo>,</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>i</mi> </msubsup> <mo>-</mo> <msubsup> <mover> <mi>z</mi> <mo>&amp;OverBar;</mo> </mover> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>i</mi> </msubsup> <mo>&amp;rsqb;</mo> </mrow> <mi>T</mi> </msup> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>14</mn> <mo>)</mo> </mrow> <mo>;</mo> </mrow>
<mrow> <msub> <mi>P</mi> <mrow> <msub> <mover> <mi>x</mi> <mo>~</mo> </mover> <mi>k</mi> </msub> <msub> <mover> <mi>z</mi> <mo>~</mo> </mover> <mi>k</mi> </msub> </mrow> </msub> <mo>=</mo> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>j</mi> <mo>=</mo> <mn>0</mn> </mrow> <mrow> <mn>2</mn> <msub> <mi>n</mi> <mi>a</mi> </msub> </mrow> </munderover> <msubsup> <mi>W</mi> <mi>j</mi> <mrow> <mo>(</mo> <mi>c</mi> <mo>)</mo> </mrow> </msubsup> <mo>&amp;lsqb;</mo> <msubsup> <mi>&amp;chi;</mi> <mrow> <mi>j</mi> <mo>,</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>i</mi> </msubsup> <mo>-</mo> <msubsup> <mover> <mi>x</mi> <mo>&amp;OverBar;</mo> </mover> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>i</mi> </msubsup> <mo>&amp;rsqb;</mo> <msup> <mrow> <mo>&amp;lsqb;</mo> <msubsup> <mi>Z</mi> <mrow> <mi>j</mi> <mo>,</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>i</mi> </msubsup> <mo>-</mo> <msubsup> <mover> <mi>z</mi> <mo>&amp;OverBar;</mo> </mover> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>i</mi> </msubsup> <mo>&amp;rsqb;</mo> </mrow> <mi>T</mi> </msup> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>15</mn> <mo>)</mo> </mrow> <mo>.</mo> </mrow>
4.根据权利要求1所述的新的排爆机器人隧进定位方法，其特征在于，在步骤S3中，具体步骤为：
S3.1，根据如下公式分别计算系统模型雅可比矩阵和观测模型的
计算公式为：
<mrow> <msubsup> <mi>F</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>i</mi> </msubsup> <mo>=</mo> <mfrac> <mrow> <mo>&amp;part;</mo> <mi>f</mi> <mrow> <mo>(</mo> <msubsup> <mi>x</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>i</mi> </msubsup> <mo>-</mo> <msubsup> <mover> <mi>x</mi> <mo>&amp;OverBar;</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>i</mi> </msubsup> <mo>)</mo> </mrow> </mrow> <mrow> <mo>&amp;part;</mo> <msubsup> <mover> <mi>x</mi> <mo>&amp;OverBar;</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>i</mi> </msubsup> </mrow> </mfrac> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mfrac> <mrow> <mo>&amp;part;</mo> <msup> <mi>f</mi> <mn>1</mn> </msup> </mrow> <mrow> <mo>&amp;part;</mo> <msubsup> <mi>x</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mrow> <mi>i</mi> <mo>,</mo> <mn>1</mn> </mrow> </msubsup> </mrow> </mfrac> </mtd> <mtd> <mfrac> <mrow> <mo>&amp;part;</mo> <msup> <mi>f</mi> <mn>1</mn> </msup> </mrow> <mrow> <mo>&amp;part;</mo> <msubsup> <mi>x</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mrow> <mi>i</mi> <mo>,</mo> <mn>2</mn> </mrow> </msubsup> </mrow> </mfrac> </mtd> <mtd> <mn>...</mn> </mtd> <mtd> <mfrac> <mrow> <mo>&amp;part;</mo> <msup> <mi>f</mi> <mn>1</mn> </msup> </mrow> <mrow> <mo>&amp;part;</mo> <msubsup> <mi>x</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mrow> <mi>i</mi> <mo>,</mo> <mi>n</mi> </mrow> </msubsup> </mrow> </mfrac> </mtd> </mtr> <mtr> <mtd> <mfrac> <mrow> <mo>&amp;part;</mo> <msup> <mi>f</mi> <mn>2</mn> </msup> </mrow> <mrow> <mo>&amp;part;</mo> <msubsup> <mi>x</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mrow> <mi>i</mi> <mo>,</mo> <mn>1</mn> </mrow> </msubsup> </mrow> </mfrac> </mtd> <mtd> <mfrac> <mrow> <mo>&amp;part;</mo> <msup> <mi>f</mi> <mn>2</mn> </msup> </mrow> <mrow> <mo>&amp;part;</mo> <msubsup> <mi>x</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mrow> <mi>i</mi> <mo>,</mo> <mn>2</mn> </mrow> </msubsup> </mrow> </mfrac> </mtd> <mtd> <mn>...</mn> </mtd> <mtd> <mfrac> <mrow> <mo>&amp;part;</mo> <msup> <mi>f</mi> <mn>2</mn> </msup> </mrow> <mrow> <mo>&amp;part;</mo> <msubsup> <mi>x</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mrow> <mi>i</mi> <mo>,</mo> <mi>n</mi> </mrow> </msubsup> </mrow> </mfrac> </mtd> </mtr> <mtr> <mtd> <mo>.</mo> </mtd> <mtd> <mo>.</mo> </mtd> <mtd> <mo>.</mo> </mtd> <mtd> <mo>.</mo> </mtd> </mtr> <mtr> <mtd> <mo>.</mo> </mtd> <mtd> <mo>.</mo> </mtd> <mtd> <mo>.</mo> </mtd> <mtd> <mo>.</mo> </mtd> </mtr> <mtr> <mtd> <mfrac> <mrow> <mo>&amp;part;</mo> <msup> <mi>f</mi> <mi>m</mi> </msup> </mrow> <mrow> <mo>&amp;part;</mo> <msubsup> <mi>x</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mrow> <mi>i</mi> <mo>,</mo> <mn>1</mn> </mrow> </msubsup> </mrow> </mfrac> </mtd> <mtd> <mfrac> <mrow> <mo>&amp;part;</mo> <msup> <mi>f</mi> <mi>m</mi> </msup> </mrow> <mrow> <mo>&amp;part;</mo> <msubsup> <mi>x</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mrow> <mi>i</mi> <mo>,</mo> <mn>2</mn> </mrow> </msubsup> </mrow> </mfrac> </mtd> <mtd> <mn>...</mn> </mtd> <mtd> <mfrac> <mrow> <mo>&amp;part;</mo> <msup> <mi>f</mi> <mi>m</mi> </msup> </mrow> <mrow> <mo>&amp;part;</mo> <msubsup> <mi>x</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mrow> <mi>i</mi> <mo>,</mo> <mi>n</mi> </mrow> </msubsup> </mrow> </mfrac> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>16</mn> <mo>)</mo> </mrow> <mo>;</mo> </mrow>
其中，表示k-1时刻第i个粒子的mxn维矩阵；m表示为f函数的方程个数；n表示为x状态向量维数；表示为k-1时刻第i个粒子的不包含噪声的理想状态；表示为k-1时刻第i个粒子包含了噪声的待估计状态；f(*)表示为状态一步预测转移方程组；表示为对第m个方程求第n个状态的偏导；
S 3.2，令计算状态一步预测值和协方差一步预测值；
计算公式为：
<mrow> <msubsup> <mover> <mi>x</mi> <mo>&amp;OverBar;</mo> </mover> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <msub> <mn>1</mn> <mrow> <mi>e</mi> <mi>k</mi> <mi>f</mi> </mrow> </msub> </mrow> <mi>i</mi> </msubsup> <mo>=</mo> <mi>f</mi> <mrow> <mo>(</mo> <msubsup> <mover> <mi>x</mi> <mo>&amp;OverBar;</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>i</mi> </msubsup> <mo>)</mo> </mrow> <mo>=</mo> <mi>f</mi> <mrow> <mo>(</mo> <msubsup> <mover> <mi>x</mi> <mo>&amp;OverBar;</mo> </mover> <msub> <mi>k</mi> <mrow> <mi>u</mi> <mi>k</mi> <mi>f</mi> </mrow> </msub> <mi>i</mi> </msubsup> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>17</mn> <mo>)</mo> </mrow> <mo>;</mo> </mrow> 3
<mrow> <msubsup> <mi>P</mi> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <msub> <mn>1</mn> <mrow> <mi>e</mi> <mi>k</mi> <mi>f</mi> </mrow> </msub> </mrow> <mi>i</mi> </msubsup> <mo>=</mo> <msubsup> <mi>F</mi> <mi>k</mi> <mi>i</mi> </msubsup> <msubsup> <mover> <mi>P</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>i</mi> </msubsup> <msubsup> <mi>F</mi> <mi>k</mi> <mrow> <mi>T</mi> <mo>,</mo> <mi>i</mi> </mrow> </msubsup> <mo>+</mo> <msubsup> <mi>G</mi> <mi>k</mi> <mi>i</mi> </msubsup> <msub> <mi>Q</mi> <mi>k</mi> </msub> <msubsup> <mi>G</mi> <mi>k</mi> <mrow> <mi>T</mi> <mo>,</mo> <mi>i</mi> </mrow> </msubsup> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>18</mn> <mo>)</mo> </mrow> <mo>;</mo> </mrow>
其中，表示为第i个粒子的状态一步预测值；表示状态的一步预测函数；表示为从k-1时刻到k时刻第i个粒子的状态预测协方差；表示为k-1时刻第i个粒子的协方差；表示为的矩阵转置；Qk表示为k时刻状态噪声的协方差；
S 3.3，计算卡尔曼增益Kk；
计算公式为：
<mrow> <msub> <mi>K</mi> <mi>k</mi> </msub> <mo>=</mo> <msubsup> <mi>P</mi> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <msub> <mn>1</mn> <mrow> <mi>e</mi> <mi>k</mi> <mi>f</mi> </mrow> </msub> </mrow> <mi>i</mi> </msubsup> <msup> <mrow> <mo>(</mo> <msubsup> <mi>H</mi> <mi>k</mi> <mi>i</mi> </msubsup> <mo>)</mo> </mrow> <mi>T</mi> </msup> <msup> <mrow> <mo>&amp;lsqb;</mo> <msubsup> <mi>U</mi> <mi>k</mi> <mi>i</mi> </msubsup> <msub> <mi>R</mi> <mi>k</mi> </msub> <msup> <mrow> <mo>(</mo> <msubsup> <mi>U</mi> <mi>k</mi> <mi>i</mi> </msubsup> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mo>+</mo> <msubsup> <mi>H</mi> <mi>k</mi> <mi>i</mi> </msubsup> <msubsup> <mi>P</mi> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <msub> <mn>1</mn> <mrow> <mi>e</mi> <mi>k</mi> <mi>f</mi> </mrow> </msub> </mrow> <mi>i</mi> </msubsup> <msup> <mrow> <mo>(</mo> <msubsup> <mi>H</mi> <mi>k</mi> <mi>i</mi> </msubsup> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mo>&amp;rsqb;</mo> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>19</mn> <mo>)</mo> </mrow> <mo>;</mo> </mrow>
S 3.4，计算修正的状态预测量和协方差预测值；
<mrow> <msubsup> <mover> <mi>P</mi> <mo>^</mo> </mover> <mi>k</mi> <mi>i</mi> </msubsup> <mo>=</mo> <msubsup> <mi>P</mi> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>i</mi> </msubsup> <mo>-</mo> <msub> <mi>K</mi> <mi>k</mi> </msub> <msubsup> <mi>H</mi> <mi>k</mi> <mi>i</mi> </msubsup> <msubsup> <mi>P</mi> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>i</mi> </msubsup> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>20</mn> <mo>)</mo> </mrow> <mo>;</mo> </mrow>
其中，表示为第i个粒子修正后的协方差；表示为k-1时刻到k时刻第i个粒子预测值的协方差；表示为第i个粒子修正后的状态均值；表示为k-1时刻到k时刻第i个粒子状态预测值；表示k时刻的观察噪声；zk表示k时刻的真实测量值；表示为k-1时刻到k时刻第i个粒子测量估计；
S3.5，计算第i个粒子k时刻的状态预测量和协方差预测值；
<mrow> <msubsup> <mover> <mi>x</mi> <mo>&amp;OverBar;</mo> </mover> <mi>k</mi> <mi>i</mi> </msubsup> <mo>=</mo> <msubsup> <mover> <mi>x</mi> <mo>&amp;OverBar;</mo> </mover> <msub> <mi>k</mi> <mrow> <mi>e</mi> <mi>k</mi> <mi>f</mi> </mrow> </msub> <mi>i</mi> </msubsup> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>22</mn> <mo>)</mo> </mrow> <mo>;</mo> </mrow>
<mrow> <msubsup> <mover> <mi>P</mi> <mo>^</mo> </mover> <mi>k</mi> <mi>i</mi> </msubsup> <mo>=</mo> <msubsup> <mover> <mi>P</mi> <mo>^</mo> </mover> <msub> <mi>k</mi> <mrow> <mi>e</mi> <mi>k</mi> <mi>f</mi> </mrow> </msub> <mi>i</mi> </msubsup> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>23</mn> <mo>)</mo> </mrow> <mo>.</mo> </mrow>
5.根据权利要求1所述的新的排爆机器人隧进定位方法，其特征在于，在步骤S6中，具体步骤为：S6.1，计算粒子退化程度；
<mrow> <msub> <mover> <mi>N</mi> <mo>^</mo> </mover> <mrow> <mi>e</mi> <mi>f</mi> <mi>f</mi> </mrow> </msub> <mo>=</mo> <mfrac> <mn>1</mn> <mrow> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>N</mi> </munderover> <msup> <mrow> <mo>(</mo> <msubsup> <mi>q</mi> <mi>k</mi> <mi>i</mi> </msubsup> <mo>)</mo> </mrow> <mn>2</mn> </msup> </mrow> </mfrac> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>27</mn> <mo>)</mo> </mrow> <mo>;</mo> </mrow>
S6.2，判断粒子退化程度当消除权值较小的粒子，复制权值较大的粒子，重新获得N个随机样本粒子(j＝1,2,…,N)；
S6.3，为每个再采样之后的样本粒子赋以相同的权值
S6.4，重复步骤S2—S5，获得任一时刻的钻头空间坐标状态量直到多种传感器探测到爆炸物结束循环，确定爆炸物坐标。
CN201710621214.4A 2017-07-27 2017-07-27 一种新的排爆机器人隧进定位方法 Pending CN107387064A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201710621214.4A CN107387064A (zh) 2017-07-27 2017-07-27 一种新的排爆机器人隧进定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201710621214.4A CN107387064A (zh) 2017-07-27 2017-07-27 一种新的排爆机器人隧进定位方法

Publications (1)

Publication Number Publication Date
CN107387064A true CN107387064A (zh) 2017-11-24

Family

ID=60341308

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201710621214.4A Pending CN107387064A (zh) 2017-07-27 2017-07-27 一种新的排爆机器人隧进定位方法

Country Status (1)

Country Link
CN (1) CN107387064A (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109917292A (zh) * 2019-03-28 2019-06-21 首都师范大学 一种基于daupf的锂离子电池寿命预测方法
CN114046785A (zh) * 2021-11-10 2022-02-15 广东微电科技有限公司 磁探测信号线性噪声滤波方法及系统、计算机可读存储介质、磁导航传感器、agv小车

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101055563A (zh) * 2007-05-21 2007-10-17 北京理工大学 基于多建议分布的粒子滤波方法
CN101615794A (zh) * 2009-08-05 2009-12-30 河海大学 基于无迹变换卡尔曼滤波的电力系统动态状态估计方法
CN101644758A (zh) * 2009-02-24 2010-02-10 中国科学院声学研究所 一种目标定位跟踪系统及方法
CN101852615A (zh) * 2010-05-18 2010-10-06 南京航空航天大学 一种用于惯性组合导航系统中的改进混合高斯粒子滤波方法
CN102103815A (zh) * 2009-12-17 2011-06-22 上海电机学院 移动机器人的粒子定位方法及其装置
CN103389094A (zh) * 2013-07-15 2013-11-13 哈尔滨工程大学 一种改进的粒子滤波方法
CN105891820A (zh) * 2016-03-17 2016-08-24 四川长虹电器股份有限公司 基于ukf和iufir的机动目标跟踪方法
CN106054170A (zh) * 2016-05-19 2016-10-26 哈尔滨工业大学 一种约束条件下的机动目标跟踪方法

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101055563A (zh) * 2007-05-21 2007-10-17 北京理工大学 基于多建议分布的粒子滤波方法
CN101644758A (zh) * 2009-02-24 2010-02-10 中国科学院声学研究所 一种目标定位跟踪系统及方法
CN101615794A (zh) * 2009-08-05 2009-12-30 河海大学 基于无迹变换卡尔曼滤波的电力系统动态状态估计方法
CN102103815A (zh) * 2009-12-17 2011-06-22 上海电机学院 移动机器人的粒子定位方法及其装置
CN101852615A (zh) * 2010-05-18 2010-10-06 南京航空航天大学 一种用于惯性组合导航系统中的改进混合高斯粒子滤波方法
CN103389094A (zh) * 2013-07-15 2013-11-13 哈尔滨工程大学 一种改进的粒子滤波方法
CN105891820A (zh) * 2016-03-17 2016-08-24 四川长虹电器股份有限公司 基于ukf和iufir的机动目标跟踪方法
CN106054170A (zh) * 2016-05-19 2016-10-26 哈尔滨工业大学 一种约束条件下的机动目标跟踪方法

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109917292A (zh) * 2019-03-28 2019-06-21 首都师范大学 一种基于daupf的锂离子电池寿命预测方法
CN114046785A (zh) * 2021-11-10 2022-02-15 广东微电科技有限公司 磁探测信号线性噪声滤波方法及系统、计算机可读存储介质、磁导航传感器、agv小车

Similar Documents

Publication Publication Date Title
Hu et al. Unscented kalman filter with process noise covariance estimation for vehicular ins/gps integration system
CN111798491B (zh) 一种基于Elman神经网络的机动目标跟踪方法
CN103940433B (zh) 一种基于改进的自适应平方根ukf算法的卫星姿态确定方法
Yang et al. Particle filter for sensor fusion in a land vehicle navigation system
CN102393881B (zh) 一种实时多传感温度数据融合的高精度检测方法
CN107084714A (zh) 一种基于RoboCup3D的多机器人协作目标定位方法
CN102795323B (zh) 一种基于ukf的水下机器人状态和参数联合估计方法
CN104066179B (zh) 一种改进的自适应迭代ukf的wsn节点定位方法
CN106599368A (zh) 基于改进粒子提议分布和自适应粒子重采样的FastSLAM方法
CN103278813A (zh) 一种基于高阶无迹卡尔曼滤波的状态估计方法
CN104613965B (zh) 一种基于双向滤波平滑技术的步进式行人导航方法
CN105785358B (zh) 一种方向余弦坐标系下带多普勒量测的雷达目标跟踪方法
CN103973263B (zh) 一种逼近滤波方法
CN106772524A (zh) 一种基于秩滤波的农业机器人组合导航信息融合方法
CN104237853B (zh) 一种针对多帧检测前跟踪点迹序列的粒子滤波方法
CN104298857B (zh) 一种多因素耦合作用下的机构可靠度计算方法
CN111551897B (zh) 传感器位置误差下基于加权多维标度和多项式求根的tdoa定位方法
CN107387064A (zh) 一种新的排爆机器人隧进定位方法
CN104363649A (zh) 带有约束条件的ukf的wsn节点定位方法
CN106403953A (zh) 一种用于水下自主导航与定位的方法
CN103296995B (zh) 任意维高阶(≥4阶)无味变换与无味卡尔曼滤波方法
CN113709662B (zh) 一种基于超宽带的自主式三维反演定位方法
Dai et al. A novel STSOSLAM algorithm based on strong tracking second order central difference Kalman filter
CN108303095A (zh) 适用于非高斯系统的鲁棒容积目标协同定位方法
CN113449254B (zh) 任意网型变形监测稳定性分析方法及监控点位置确定方法

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: 20171124