CN108332751B - 一种多源融合定位方法、装置、电子设备及存储介质 - Google Patents

一种多源融合定位方法、装置、电子设备及存储介质 Download PDF

Info

Publication number
CN108332751B
CN108332751B CN201810016973.2A CN201810016973A CN108332751B CN 108332751 B CN108332751 B CN 108332751B CN 201810016973 A CN201810016973 A CN 201810016973A CN 108332751 B CN108332751 B CN 108332751B
Authority
CN
China
Prior art keywords
target state
state vector
observation
posterior
axis
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
CN201810016973.2A
Other languages
English (en)
Other versions
CN108332751A (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.)
Beijing University of Posts and Telecommunications
Original Assignee
Beijing University of Posts and Telecommunications
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 Beijing University of Posts and Telecommunications filed Critical Beijing University of Posts and Telecommunications
Priority to CN201810016973.2A priority Critical patent/CN108332751B/zh
Publication of CN108332751A publication Critical patent/CN108332751A/zh
Application granted granted Critical
Publication of CN108332751B publication Critical patent/CN108332751B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • 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
    • G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)

Abstract

本发明实施例提供了一种多源融合定位方法、装置、电子设备及存储介质，该方法包括：构建待定位点的目标状态向量；并根据预先获得的所述待定位点的目标观测数据构建观测向量；利用卡尔曼滤波模型获得所述目标状态向量的后验值的均值；基于相对熵方法，建立所述目标状态向量中各变量和观测向量中各观测值的两两之间的耦合度因子；根据所述耦合度因子，更新所述目标状态向量的后验值的均值。本发明实施例分析了多源异构定位信息间的耦合度，建立多源异构定位信息间的相对熵，从而提高了多源融合定位算法的稳定性与多源融合定位的精度。

Description

一种多源融合定位方法、装置、电子设备及存储介质
技术领域
本发明涉及计算机技术领域，特别是涉及一种多源融合定位方法、装置、电子设备及存储介质。
背景技术
传统的多源融合定位技术，将来自不同导航源的同构或者异构的导航信息按照相应的融合算法进行融合，从而得到最佳的融合定位结果。相对于传统的单一导航源，多源融合导航可以充分利用每一个导航源的优势，从而提供更优的定位与导航服务。在导航系统中，为了提高导航定位精度，普遍应用卡尔曼滤波技术来计算导航系统的误差状态，再用误差状态的最优估计值去校正系统。
由于导航系统的状态方程是时变的，而且状态转移矩阵中含有导航信息及惯性元件测量值，这些含有误差的参数使得滤波器模型不准确。另外，系统噪声与观测噪声的统计特性等很难精确地估计或测定，所以采用常规卡尔曼滤波器时常常会发散，从而导致稳定性与可靠性差。
为了解决此问题，通常采用自适应滤波技术，但现有的自适应滤波算法，大多采用在线估计系统噪声和观测噪声的协方差阵来估计状态变量，算法结构复杂，而且在室内复杂环境下的稳定性差，进而影响多源融合定位的精度，难以适应将来的智能导航系统。
发明内容
本发明实施例的目的在于提供一种多源融合定位方法、装置、电子设备及存储介质，以提高多源融合定位算法的稳定性与多源融合定位的精度。具体技术方案如下：
为达到上述目的，第一方面，本发明实施提供了一种多源融合定位方法，包括：
构建待定位点的目标状态向量；其中，所述目标状态向量中的变量包括：所述待定位点的位置、速度、加速度、姿态以及陀螺漂移误差；
根据预先获得的所述待定位点的目标观测数据构建观测向量；
利用卡尔曼滤波模型获得所述目标状态向量的后验值的均值；
基于相对熵方法，建立所述目标状态向量中各变量和观测向量中各观测值的两两之间的耦合度因子；
根据所述耦合度因子，更新所述目标状态向量的后验值的均值。
可选地，获得所述待定位点的目标观测数据的过程，包括：
获取针对待定位点的多源观测数据；
对所述多源观测数据进行滤波和野值处理，得到所述待定位点的目标观测数据。
可选地，所述利用卡尔曼滤波模型获得所述目标状态变量的后验值的均值，包括：
建立所述目标状态向量的离散卡尔曼状态方程和观测方程；
根据所述离散卡尔曼状态方程，预测所述目标状态向量的后验估计值以及所述后验估计值的协方差矩阵；
当所述目标状态向量服从高斯分布时，利用GGF方程、所述观测方程以及所述后验估计值的协方差矩阵估计所述目标状态向量的后验值的均值。
可选地，所述基于相对熵方法，建立所述目标状态向量中各变量和所述观测向量中各观测值的两两之间的耦合度因子，包括：
基于相对熵方法，建立所述目标状态向量和所述观测向量的相对熵；
根据所述相对熵建立所述目标状态向量中的各变量和所述观测向量中的各观测值的两两之间的耦合度因子；
所述根据所述耦合度因子，更新所述目标状态向量的后验值的均值，包括：
将所述各耦合度因子中小于预设阈值的耦合度因子作为目标耦合度因子；
根据所述目标耦合度因子更新所述目标状态向量的后验值的均值。
可选地，在所述利用卡尔曼滤波模型获得所述目标状态向量的后验值的均值的步骤之后，所述方法还包括：
利用卡尔曼滤波模型获得所述目标状态向量的后验值的方差。
可选地，在所述根据所述耦合度因子，更新所述目标状态向量的后验值的均值的步骤之后，所述方法还包括：
根据所述耦合度因子，更新所述目标状态向量的后验值的方差。
第二方面，本发明实施例提供了一种多源融合定位装置，包括：
第一构建模块，用于构建待定位点的目标状态向量；其中，所述目标状态向量中的变量包括：所述待定位点的位置、速度、加速度、姿态以及陀螺漂移误差；
第二构建模块，用于根据预先获得的所述待定位点的目标观测数据构建观测向量；
第一获得模块，用于利用卡尔曼滤波模型获得所述目标状态向量的后验值的均值；
建立模块，用于基于相对熵方法，建立所述目标状态向量中各变量和观测向量中各观测值的两两之间的耦合度因子；
第一更新模块，用于根据所述耦合度因子，更新所述目标状态向量的后验值的均值。
可选地，所述装置还包括：
第二获得模块，用于获取针对待定位点的多源观测数据；
对所述多源观测数据进行滤波和野值处理，得到所述待定位点的目标观测数据。
可选地，所述第一获得模块，包括：
第三建立子模块，用于建立所述目标状态向量的离散卡尔曼状态方程和观测方程；
预测子模块，用于根据所述离散卡尔曼状态方程，预测所述目标状态向量的后验估计值以及所述后验估计值的协方差矩阵；
估计子模块，用于当所述目标状态向量服从高斯分布时，利用GGF方程、所述观测方程以及所述后验估计值的协方差矩阵估计所述目标状态向量的后验值的均值。
可选地，所述建立模块，包括：
第一建立子模块，用于基于相对熵方法，建立所述目标状态向量和所述观测向量的相对熵；
第二建立子模块，用于根据所述相对熵建立所述目标状态向量中的各变量和所述观测向量中的各观测值的两两之间的耦合度因子；
所述第一更新模块，具体用于将所述各耦合度因子中小于预设阈值的耦合度因子作为目标耦合度因子；根据所述目标耦合度因子更新所述目标状态向量的后验值的均值。
可选地，所述装置还包括：
第三获得模块，用于在所述第一获得模块利用卡尔曼滤波模型获得所述目标状态向量的后验值的均值之后，利用卡尔曼滤波模型获得所述目标状态向量的后验值的方差。
可选地，所述装置还包括：
第二更新模块，用于在所述第一更新模块根据所述耦合度因子，更新所述目标状态向量的后验值的均值之后，根据所述耦合度因子，更新所述目标状态向量的后验值的方差。
第三方面，本发明实施例提供了一种电子设备，包括处理器、通信接口、存储器和通信总线，其中，处理器，通信接口，存储器通过通信总线完成相互间的通信；
存储器，用于存放计算机程序；
处理器，用于执行存储器上所存放的程序时，实现如上第一方面所述的多源融合定位的方法步骤。
第四方面，本发明实施例提供了一种计算机可读存储介质，所述计算机可读存储介质中存储有指令，当其在计算机上运行时，使得计算机执行如上第一方面所述的多源融合定位的方法步骤。
第五方面，本发明实施例提供了一种包含指令的计算机程序产品，当其在计算机上运行时，使得计算机执行如上第一方面所述的多源融合定位的方法步骤。
本发明实施例提供的多源融合定位方法、装置、电子设备及存储介质，通过构建待定位点的目标状态向量；并根据预先获得的所述待定位点的目标观测数据构建观测向量；利用卡尔曼滤波模型获得所述目标状态向量的后验值的均值；然后基于相对熵方法，建立所述目标状态向量中的各变量和观测向量中的各观测值的两两之间的耦合度因子；最后根据所述耦合度因子，更新所述目标状态向量的后验值的均值。本发明实施例分析了多源异构定位信息间的耦合度，建立多源异构定位信息间的相对熵，从而提高了多源融合定位算法的稳定性与多源融合定位的精度。
当然，实施本发明的任一产品或方法必不一定需要同时达到以上所述的所有优点。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍。
图1a为本发明实施例提供的一种多源融合定位方法的流程图；
图1b为本发明实施例中获得待定位点的目标观测数据的流程图；
图1c为本发明实施例中利用卡尔曼滤波模型获得目标状态向量的后验值的均值的流程图；
图1d为本发明实施例中基于相对熵方法建立目标状态向量中各变量和观测向量中各观测值的两两之间的耦合度因子的流程图；
图1e为本发明实施例中根据耦合度因子更新目标状态向量的后验值的均值的流程图；
图2为本发明实施例提供的另一种多源融合定位方法的流程图；
图3为本发明实施例提供的一种多源融合定位装置的结构图；
图4为本发明实施例提供的另一种多源融合定位装置的结构图；
图5为本发明实施例提供的一种电子设备的结构图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本发明实施例提供的多源融合定位方法，可以基于待定位点的多源观测数据对待定位点进行室内外无缝定位。
图1a为本发明实施例提供的一种多源融合定位方法的流程图，该方法包括：
S101，构建待定位点的目标状态向量。
目标状态向量中的变量可以包括：待定位点的位置、速度、加速度、姿态以及陀螺漂移误差。
本实施例中，设定Xk为待定位点第k时刻的目标状态向量，其表达式具体为：
Xk＝[δx，δy，δz，δvx，δvy，δvz，δφx，δφy，δφz，δαx，δαy，δαz，δεx，δεy，δεz]T；
其中，δx，δy，δz分别为待定位点在东北天坐标系中x轴、y轴和z轴的位置分量；δvx，δvy，δvz分别为待定位点在东北天坐标系中x轴、y轴和z轴的速度分量；δφx，δφy，δφz分别为待定位点在东北天坐标系中的滚转角、俯仰角和偏航角；δαx，δαy，δαz分别为待定位点在东北天坐标系中x轴、y轴和z轴的加速度分量；δεx，δεy，δεz分别为待定位点在东北天坐标系中x轴、y轴和z轴的陀螺漂移误差。
通过构建待定位点的目标状态向量，可以用于进一步建立目标状态向量的离散卡尔曼状态方程。
S102，根据预先获得的待定位点的目标观测数据构建观测向量。
本实施例中，如图1b所示，获得待定位点的目标观测数据的过程，可以包括以下步骤：
S1021，获取针对待定位点的多源观测数据；
多源观测数据可以包括：待定位点的位置数据和速度数据等。具体的，可以获取通过惯性器件采集待定位点的位置数据和速度数据，通过全球定位系统 (GlobalPositioning System，GPS)或中国北斗卫星导航系统(BeiDou Navigation SatelliteSystem，BDS)采集待定位点的位置数据和速度数据，或通过时分-码分正交频分复用(TimeCode Division-Orthogonal Frequency Division Multiplexing，TC-OFDM)测距技术采集待定位点的位置数据和速度数据。
S1022，对多源观测数据进行滤波和野值处理，得到待定位点的目标观测数据。
可以理解，在获取的多源观测数据中可能存在着离群数据，这将影响融合定位的精度。因此，在本发明实施例中，在进行多源融合定位前，可以对多源观测数据进行预处理，以改进数据的质量，提高融合定位的效率和精度。
具体的，可以对多源观测数据进行滤波和野值处理，从而得到待定位点的目标观测数据。该滤波和野值处理可通过现有方法实现。
本实施例中，设定Zk为第k时刻的观测向量，其表达式具体为：
Zk＝[Δx，Δy,Δz,Δvx，Δvy，Δva]；
其中，
Figure BDA0001541630160000071
xins，yins，zins分别为惯性器件采集的待定位点在东北天坐标系中x轴、y轴和z轴的位置分量， xT&G，yT&G，zT&G分别为将GPS/BDS与TC-OFDM采集的位置数据融合后的待定位点在东北天坐标系中x轴、y轴和z轴的位置分量；vx,ins，vy,ins，vz,ins分别为惯性器件采集的待定位点在东北天坐标系中x轴、y轴和z轴的速度分量，vx,T&G，vy,T&G，vz,T&G分别为将GPS/BDS与TC-OFDM采集的速度数据融合后的待定位点在东北天坐标系中x轴、y轴和z轴的速度分量。
需要说明的是，上述GPS/BDS表示GPS系统或BDS系统。上述融合具体可以是按照预设融合规则将GPS/BDS采集的位置/速度数据与TC-OFDM采集的位置/速度数据进行融合。预设融合规则可以是将上述两种系统采集的数据进行加权融合等。本发明对此不做限制。
通过构建观测向量，可以用于进一步建立目标状态向量的观测方程。
S103，利用卡尔曼滤波模型获得目标状态向量的后验值的均值。
本实施例中，如图1c所示，利用卡尔曼滤波模型获得目标状态变量的后验值的均值，可以包括以下步骤：
S1031，建立目标状态向量的离散卡尔曼状态方程和观测方程；
离散卡尔曼状态方程为：
Xk＝Fk|k-1Xk-1+Gk-1Wk-1；
其中，Xk-1为待定位点第k-1时刻的目标状态向量；Fk|k-1为第k-1时刻到第k时刻的状态转移矩阵；Gk-1为第k-1时刻的过程噪声矩阵，Wk-1为第 k-1时刻的过程噪声向量。
观测方程为：
Zk＝ΦkXk+Vk；
其中，Φk为第k时刻的观测矩阵；Vk为第k时刻的观测噪声向量。
S1032，根据离散卡尔曼状态方程，预测目标状态向量的后验估计值以及后验估计值的协方差矩阵；
本实施例中，设定
Figure BDA0001541630160000081
为Xk的后验估计值，其表达式具体为：
Figure BDA0001541630160000082
其中，
Figure BDA0001541630160000083
为Xk的先验值，也可以理解为Xk-1的最优估计值。
本实施例中，设定为Pk|k-1为
Figure BDA0001541630160000084
的协方差矩阵，其表达式具体为：
Figure BDA0001541630160000091
其中，Pk-1|k-1为
Figure BDA0001541630160000092
的协方差；Qk-1为第k-1时刻的过程噪声协方差矩阵。
上述Pk|k-1也可以理解为第k-1时刻到第k时刻的目标状态向量的协方差矩阵。
S1033，当目标状态向量服从高斯分布时，利用通用高斯滤波器 (GeneralGaussian filter，GGF)方程、观测方程以及后验估计值的协方差矩阵估计目标状态向量的后验值的均值。
本实施例中，Xk服从高斯分布，可以表示为：
fx＝N(x|μ-，P-)；
其中，μ-和P-分别为
Figure BDA0001541630160000093
的均值和方差。
相应的，Xk的后验值服从如下分布：
fx|z∝fxN(z|Φ，R)；
其中，Φ为观测矩阵，R为测量噪声协方差矩阵。
本实施例中，设定μ+为Xk的后验值的均值，其表达式具体为：
μ+＝μ-+K(Zk-Zk-1)；
其中，Zk-1为第k-1时刻的观测向量；K为卡尔曼增益，其表达式为：
Figure BDA0001541630160000094
其中，Rk为第k时刻的测量噪声协方差矩阵。
S104，基于相对熵方法，建立目标状态向量中各变量和观测向量中各观测值的两两之间的耦合度因子。
本实施例中，如图1d所示，基于相对熵方法，建立目标状态向量中各变量和观测向量中各观测值的两两之间的耦合度因子，包括以下步骤：
S1041，基于相对熵方法，建立目标状态向量和观测向量的相对熵；
本实施例中，设定
Figure BDA0001541630160000107
为目标状态向量和观测向量的相对熵，其表达式具体为：
Figure BDA0001541630160000108
实际使用中，
Figure BDA0001541630160000109
的另一种表达式可以为：
Figure BDA0001541630160000101
其中，χ＝∑X∑Z((Zk-Zk-1)(Zk-Zk-1)T-ΨT(P-)-1Ψ)，
Ψ＝(Xk-μ-)(Zk-Zk-1)T。
S1042，根据相对熵建立目标状态向量中各变量和观测向量中各观测值的两两之间的耦合度因子。
本实施例中，设定
Figure BDA0001541630160000102
为目标状态向量中各变量和观测向量中各观测值之间的第i个耦合度因子，其表达式具体为：
Figure BDA0001541630160000103
其中，
Figure BDA0001541630160000104
是
Figure 1
的对角线上的第i个元素，
Figure BDA0001541630160000106
是χ的对角化矩阵。
S105，根据耦合度因子，更新目标状态向量的后验值的均值。
本实施例中，如图1e所示，根据耦合度因子，更新目标状态向量的后验值的均值，包括以下步骤：
S1051，将各耦合度因子中小于预设阈值的耦合度因子作为目标耦合度因子；
本领域技术人员可以理解，两个向量间的相对熵越小，表示两个向量的耦合度越高。因而，本实施例中，针对各耦合度因子设置了预设阈值，可以只将小于预设阈值的耦合度因子用于更新目标状态向量的后验值的均值，从而可以提高融合定位的精度。
S1052，根据目标耦合度因子更新目标状态向量的后验值的均值。
在步骤S1052中，首先，根据目标耦合度因子更新Zk。具体的，可以将步骤S1051中的除了目标耦合度因子之外的其他耦合度因子对应的Zk和Zk-1中的观测值去掉，形成更新后的Zk和Zk-1，用
Figure 2
和
Figure BDA0001541630160000112
和来表示。
设定
Figure BDA0001541630160000113
为更新后的目标状态向量的后验值的均值，其表达式具体为：
Figure BDA0001541630160000114
其中，
Figure BDA0001541630160000115
为更新后的卡尔曼增益，其表达式为：
Figure BDA0001541630160000116
其中，
Figure BDA0001541630160000117
为对角化因子。
则
Figure BDA0001541630160000118
即为在第k个时刻对待定位点融合定位的结果。
本发明实施例提供的多源融合定位方法，通过构建待定位点的目标状态向量；并根据预先获得的待定位点的目标观测数据构建观测向量；利用卡尔曼滤波模型获得目标状态向量的后验值的均值；然后基于相对熵方法，建立目标状态向量中各变量和观测向量中各观测值的两两之间的耦合度因子；最后根据耦合度因子，更新目标状态向量的后验值的均值。本发明实施例分析了多源异构定位信息间的耦合度，建立多源异构定位信息间的相对熵，从而提高了多源融合定位算法的稳定性与多源融合定位的精度。
图2为本发明实施例提供的另一种多源融合定位方法的流程图，该方法包括：
S201，构建待定位点的目标状态向量。
S202，根据预先获得的待定位点的目标观测数据构建观测向量。
S203，利用卡尔曼滤波模型获得目标状态向量的后验值的均值。
本实施例中，上述步骤S201-S203可以分别与图1所示实施例中的步骤 S101-S103完全相同，这里不再赘述。
S204，利用卡尔曼滤波模型获得目标状态向量的后验值的方差。
本实施例中，设定P+为目标状态向量的后验值的方差，其表达式具体为：
P+＝P--K∑Z((Zk-Zk-1)(Zk-Zk-1)T+R)KT。
S205，基于相对熵方法，建立目标状态向量中各变量和观测向量中各观测值的两两之间的耦合度因子。
S206，根据耦合度因子，更新目标状态向量的后验值的均值。
本实施例中，上述步骤S205-S206可以分别与图1所示实施例中的步骤 S104-S105完全相同，这里不再赘述。
S207，根据耦合度因子，更新目标状态向量的后验值的方差。
本实施例中，设定
Figure BDA0001541630160000121
为更新后的目标状态向量的后验值的方差，其表达式具体为：
Figure BDA0001541630160000122
本发明实施例提供的多源融合定位方法，利用卡尔曼滤波模型获得目标状态向量的后验值的方差，并进一步根据根据耦合度因子，更新目标状态向量的后验值的方差。从而可以利用更新后的目标状态向量的后验值的方差，进一步估计下一时刻的目标状态向量的后验值的均值。
图3为本发明实施例提供的一种多源融合定位装置的结构图，该装置包括：
第一构建模块301，用于构建待定位点的目标状态向量；其中，目标状态向量中的变量包括：待定位点的位置、速度、加速度、姿态以及陀螺漂移误差；
第二构建模块302，用于根据预先获得的待定位点的目标观测数据构建观测向量；
第一获得模块303，用于利用卡尔曼滤波模型获得目标状态向量的后验值的均值；
建立模块304，用于基于相对熵方法，建立目标状态向量中各变量和观测向量中各观测值的两两之间的耦合度因子；
第一更新模块305，用于根据耦合度因子，更新目标状态向量的后验值的均值。
本发明实施例提供的多源融合定位装置，通过构建待定位点的目标状态向量；并根据预先获得的待定位点的目标观测数据构建观测向量；利用卡尔曼滤波模型获得目标状态向量的后验值的均值；然后基于相对熵方法，建立目标状态向量中各变量和观测向量中各观测值的两两之间的耦合度因子；最后根据耦合度因子，更新目标状态向量的后验值的均值。本发明实施例分析了多源异构定位信息间的耦合度，建立多源异构定位信息间的相对熵，从而提高了多源融合定位算法的稳定性与多源融合定位的精度。
可选地，上述装置还包括：
第二获得模块(图中未示出)，用于获取针对待定位点的多源观测数据；
对多源观测数据进行滤波和野值处理，得到待定位点的目标观测数据。
可选地，第一获得模块303，包括：
第三建立子模块，用于建立目标状态向量的离散卡尔曼状态方程和观测方程；
预测子模块，用于根据离散卡尔曼状态方程，预测目标状态向量的后验估计值以及后验估计值的协方差矩阵；
估计子模块，用于当目标状态向量服从高斯分布时，利用GGF方程、观测方程以及后验估计值的协方差矩阵估计目标状态向量的后验值的均值。
可选地，建立模块304，包括：
第一建立子模块，用于基于相对熵方法，建立目标状态向量和观测向量的相对熵；
第二建立子模块，用于根据相对熵建立目标状态向量中各变量和观测向量中各观测值的两两之间的耦合度因子；
第一更新模块305，具体用于将各耦合度因子中小于预设阈值的耦合度因子作为目标耦合度因子；根据目标耦合度因子更新目标状态向量的后验值的均值。
图4为本发明实施例提供的另一种多源融合定位装置的结构图，该装置包括：第一构建模块401、第二构建模块402、第一获得模块403、第三获得模块 404、建立模块405、第一更新模块406，第二更新模块407，其中，第一构建模块401、第二构建模块402、第一获得模块403、建立模块405、第一更新模块406 可以分别与图3实施例中的第一构建模块301、第二构建模块302、第一获得模块303、建立模块304、第一更新模块305完全相同，这里不再重复。
本实施例中，第三获得模块404，用于在第一获得模403块利用卡尔曼滤波模型获得目标状态向量的后验值的均值之后，利用卡尔曼滤波模型获得目标状态向量的后验值的方差。
第二更新模块407，用于在第一更新模块406根据耦合度因子，更新目标状态向量的后验值的均值之后，根据耦合度因子，更新目标状态向量的后验值的方差。
本发明实施例提供的多源融合定位装置，利用卡尔曼滤波模型获得目标状态向量的后验值的方差，并进一步根据根据耦合度因子，更新目标状态向量的后验值的方差。从而可以利用更新后的目标状态向量的后验值的方差，进一步估计下一时刻的目标状态向量的后验值的均值。
本发明实施例还提供了一种电子设备，如图5所示，包括处理器501、通信接口502、存储器503和通信总线504，其中，处理器501，通信接口502，存储器503通过通信总线504完成相互间的通信，
存储器503，用于存放计算机程序；
处理器501，用于执行存储器503上所存放的程序时，实现如下步骤：
构建待定位点的目标状态向量；其中，目标状态向量中的变量包括：待定位点的位置、速度、加速度、姿态以及陀螺漂移误差；
根据预先获得的待定位点的目标观测数据构建观测向量；
利用卡尔曼滤波模型获得目标状态向量的后验值的均值；
基于相对熵方法，建立目标状态向量中各变量和观测向量中各观测值的两两之间的耦合度因子；
根据耦合度因子，更新目标状态向量的后验值的均值。
本发明实施例提供的电子设备，处理器通过执行存储器上所存放的程序时，构建待定位点的目标状态向量；并根据预先获得的待定位点的目标观测数据构建观测向量；利用卡尔曼滤波模型获得目标状态向量的后验值的均值；然后基于相对熵方法，建立目标状态向量中各变量和观测向量中各观测值的两两之间的耦合度因子；最后根据耦合度因子，更新目标状态向量的后验值的均值。本发明实施例分析了多源异构定位信息间的耦合度，建立多源异构定位信息间的相对熵，从而提高了多源融合定位算法的稳定性与多源融合定位的精度。
上述电子设备提到的通信总线可以是外设部件互连标准(Peripheral ComponentInterconnect，简称PCI)总线或扩展工业标准结构(Extended Industry StandardArchitecture，简称EISA)总线等。该通信总线可以分为地址总线、数据总线、控制总线等。为便于表示，图中仅用一条粗线表示，但并不表示仅有一根总线或一种类型的总线。
通信接口用于上述电子设备与其他设备之间的通信。
存储器可以包括随机存取存储器(Random Access Memory，简称RAM)，也可以包括非易失性存储器(non-volatile memory)，例如至少一个磁盘存储器。可选的，存储器还可以是至少一个位于远离前述处理器的存储装置。
上述的处理器可以是通用处理器，包括中央处理器(Central Processing Unit，简称CPU)、网络处理器(Network Processor，简称NP)等；还可以是数字信号处理器(Digital Signal Processing，简称DSP)、专用集成电路(Application SpecificIntegrated Circuit，简称ASIC)、现场可编程门阵列(Field- Programmable Gate Array，简称FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。
在本发明提供的又一实施例中，还提供了一种计算机可读存储介质，该计算机可读存储介质中存储有指令，当其在计算机上运行时，使得计算机执行上述实施例中任一的多源融合定位方法。
本发明实施例提供的计算机可读存储介质内存储的指令，当其在计算机上运行时，构建待定位点的目标状态向量；并根据预先获得的待定位点的目标观测数据构建观测向量；利用卡尔曼滤波模型获得目标状态向量的后验值的均值；然后基于相对熵方法，建立目标状态向量中各变量和观测向量中各观测值的两两之间的耦合度因子；最后根据耦合度因子，更新目标状态向量的后验值的均值。本发明实施例分析了多源异构定位信息间的耦合度，建立多源异构定位信息间的相对熵，从而提高了多源融合定位算法的稳定性与多源融合定位的精度。
在本发明提供的又一实施例中，还提供了一种包含指令的计算机程序产品，当其在计算机上运行时，使得计算机执行上述实施例中任一的多源融合定位方法。
本发明实施例提供的包含指令的计算程序产品，当其在计算机上运行时，构建待定位点的目标状态向量；并根据预先获得的待定位点的目标观测数据构建观测向量；利用卡尔曼滤波模型获得目标状态向量的后验值的均值；然后基于相对熵方法，建立目标状态向量中各变量和观测向量中各观测值的两两之间的耦合度因子；最后根据耦合度因子，更新目标状态向量的后验值的均值。本发明实施例分析了多源异构定位信息间的耦合度，建立多源异构定位信息间的相对熵，从而提高了多源融合定位算法的稳定性与多源融合定位的精度。
在上述实施例中，可以全部或部分地通过软件、硬件、固件或者其任意组合来实现。当使用软件实现时，可以全部或部分地以计算机程序产品的形式实现。计算机程序产品包括一个或多个计算机指令。在计算机上加载和执行计算机程序指令时，全部或部分地产生按照本发明实施例的流程或功能。计算机可以是通用计算机、专用计算机、计算机网络、或者其他可编程装置。计算机指令可以存储在计算机可读存储介质中，或者从一个计算机可读存储介质向另一个计算机可读存储介质传输，例如，计算机指令可以从一个网站站点、计算机、服务器或数据中心通过有线(例如同轴电缆、光纤、数字用户线(DSL))或无线(例如红外、无线、微波等)方式向另一个网站站点、计算机、服务器或数据中心进行传输。计算机可读存储介质可以是计算机能够存取的任何可用介质或者是包含一个或多个可用介质集成的服务器、数据中心等数据存储设备。可用介质可以是磁性介质，(例如，软盘、硬盘、磁带)、光介质(例如，DVD)、或者半导体介质(例如固态硬盘Solid State Disk(SSD))等。
需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个......”限定的要素，并不排除在包括要素的过程、方法、物品或者设备中还存在另外的相同要素。
本说明书中的各个实施例均采用相关的方式描述，各个实施例之间相同相似的部分互相参见即可，每个实施例重点说明的都是与其他实施例的不同之处。尤其，对于装置/电子设备/存储介质/计算机程序产品实施例而言，由于其基本相似于方法实施例，所以描述的比较简单，相关之处参见方法实施例的部分说明即可。
以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内所作的任何修改、等同替换、改进等，均包含在本发明的保护范围内。

Claims (9)

1.一种多源融合定位方法，其特征在于，包括：
构建待定位点的目标状态向量；其中，所述目标状态向量中的变量包括：所述待定位点的位置、速度、加速度、姿态以及陀螺漂移误差；
根据预先获得的所述待定位点的目标观测数据构建观测向量；
利用卡尔曼滤波模型获得所述目标状态向量的后验值的均值；
基于相对熵方法，建立所述目标状态向量中各变量和所述观测向量中各观测值的两两之间的耦合度因子；
根据所述耦合度因子，更新所述目标状态向量的后验值的均值；
所述观测向量的表达式为：
Zk＝[Δx，Δy，Δz，Δvx，Δvy，Δvz]；
其中，
Figure FDA0002647207080000011
xins，yins，zins分别为惯性器件采集的待定位点在东北天坐标系中x轴、y轴和z轴的位置分量，xT&G，yT&G，zT&G分别为将GPS/BDS与TC-OFDM采集的位置数据融合后的待定位点在东北天坐标系中x轴、y轴和z轴的位置分量；vx，ins，vy，ins，vz，ins分别为惯性器件采集的待定位点在东北天坐标系中x轴、y轴和z轴的速度分量，vx，T&G，vy，T&G，vz，T&G分别为将GPS/BDS与TC-OFDM采集的速度数据融合后的待定位点在东北天坐标系中x轴、y轴和z轴的速度分量；
其中，所述利用卡尔曼滤波模型获得所述目标状态向量的后验值的均值，包括：
建立所述目标状态向量的离散卡尔曼状态方程和观测方程；
根据所述离散卡尔曼状态方程，预测所述目标状态向量的后验估计值以及所述后验估计值的协方差矩阵；
当所述目标状态向量服从高斯分布时，利用通用高斯滤波器GGF方程、所述观测方程以及所述后验估计值的协方差矩阵估计所述目标状态向量的后验值的均值。
2.根据权利要求1所述的方法，其特征在于，获得所述待定位点的目标观测数据的过程，包括：
获取针对待定位点的多源观测数据；
对所述多源观测数据进行滤波和野值处理，得到所述待定位点的目标观测数据。
3.根据权利要求1所述的方法，其特征在于，所述基于相对熵方法，建立所述目标状态向量中各变量和所述观测向量中各观测值的两两之间的耦合度因子，包括：
基于相对熵方法，建立所述目标状态向量和所述观测向量的相对熵；
根据所述相对熵建立所述目标状态向量中的各变量和所述观测向量中的各观测值的两两之间的耦合度因子；
所述根据所述耦合度因子，更新所述目标状态向量的后验值的均值，包括：
将各耦合度因子中小于预设阈值的耦合度因子作为目标耦合度因子；
根据所述目标耦合度因子更新所述目标状态向量的后验值的均值。
4.根据权利要求1所述的方法，其特征在于，在所述利用卡尔曼滤波模型获得所述目标状态向量的后验值的均值的步骤之后，所述方法还包括：
利用卡尔曼滤波模型获得所述目标状态向量的后验值的方差。
5.根据权利要求4所述的方法，其特征在于，在所述根据所述耦合度因子，更新所述目标状态向量的后验值的均值的步骤之后，所述方法还包括：
根据所述耦合度因子，更新所述目标状态向量的后验值的方差。
6.一种多源融合定位装置，其特征在于，包括：
第一构建模块，用于构建待定位点的目标状态向量；其中，所述目标状态向量中的变量包括：所述待定位点的位置、速度、加速度、姿态以及陀螺漂移误差；
第二构建模块，用于根据预先获得的所述待定位点的目标观测数据构建观测向量；
第一获得模块，用于利用卡尔曼滤波模型获得所述目标状态向量的后验值的均值；
建立模块，用于基于相对熵方法，建立所述目标状态向量中各变量和观测向量中各观测值的两两之间的耦合度因子；
第一更新模块，用于根据所述耦合度因子，更新所述目标状态向量的后验值的均值；
所述观测向量的表达式为：
Zk＝[Δx，Δy，Δz，Δvx，Δvy，Δvz]；
其中，
Figure FDA0002647207080000031
xins，yins，Zins分别为惯性器件采集的待定位点在东北天坐标系中x轴、y轴和z轴的位置分量，xT&G，yT&G，zT&G分别为将GPS/BDS与TC-OFDM采集的位置数据融合后的待定位点在东北天坐标系中x轴、y轴和z轴的位置分量；vx，ins，vy，ins，vz，ins分别为惯性器件采集的待定位点在东北天坐标系中x轴、y轴和z轴的速度分量，vx，T&G，vy，T&G，vz，T&G分别为将GPS/BDS与TC-OFDM采集的速度数据融合后的待定位点在东北天坐标系中x轴、y轴和z轴的速度分量；
其中，所述第一获得模块，包括：
第三建立子模块，用于建立所述目标状态向量的离散卡尔曼状态方程和观测方程；
预测子模块，用于根据所述离散卡尔曼状态方程，预测所述目标状态向量的后验估计值以及所述后验估计值的协方差矩阵；
估计子模块，用于当所述目标状态向量服从高斯分布时，利用通用高斯滤波器GGF方程、所述观测方程以及所述后验估计值的协方差矩阵估计所述目标状态向量的后验值的均值。
7.根据权利要求6所述的装置，其特征在于，所述建立模块，包括：
第一建立子模块，用于基于相对熵方法，建立所述目标状态向量和所述观测向量的相对熵；
第二建立子模块，用于根据所述相对熵建立所述目标状态向量中的各变量和所述观测向量中的各观测值的两两之间的耦合度因子；
所述第一更新模块，具体用于将各耦合度因子中小于预设阈值的耦合度因子作为目标耦合度因子；根据所述目标耦合度因子更新所述目标状态向量的后验值的均值。
8.一种电子设备，其特征在于，包括处理器、通信接口、存储器和通信总线，其中，处理器，通信接口，存储器通过通信总线完成相互间的通信；
存储器，用于存放计算机程序；
处理器，用于执行存储器上所存放的程序时，实现权利要求1-5任一所述的方法。
9.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质内存储有计算机程序，所述计算机程序被处理器执行时实现权利要求1-5任一项所述的方法。
CN201810016973.2A 2018-01-08 2018-01-08 一种多源融合定位方法、装置、电子设备及存储介质 Active CN108332751B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810016973.2A CN108332751B (zh) 2018-01-08 2018-01-08 一种多源融合定位方法、装置、电子设备及存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810016973.2A CN108332751B (zh) 2018-01-08 2018-01-08 一种多源融合定位方法、装置、电子设备及存储介质

Publications (2)

Publication Number Publication Date
CN108332751A CN108332751A (zh) 2018-07-27
CN108332751B true CN108332751B (zh) 2020-11-20

Family

ID=62924042

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810016973.2A Active CN108332751B (zh) 2018-01-08 2018-01-08 一种多源融合定位方法、装置、电子设备及存储介质

Country Status (1)

Country Link
CN (1) CN108332751B (zh)

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105737828A (zh) * 2016-05-09 2016-07-06 郑州航空工业管理学院 一种基于强跟踪的相关熵扩展卡尔曼滤波的组合导航方法
CN105809718A (zh) * 2016-03-14 2016-07-27 西南交通大学 一种轨迹熵最小化的对象跟踪方法
CN106291645A (zh) * 2016-07-19 2017-01-04 东南大学 适于高维gnss/ins深耦合的容积卡尔曼滤波方法
CN106482728A (zh) * 2016-09-14 2017-03-08 西安交通大学 基于最大互相关熵准则无迹卡尔曼滤波的通信保障航天器相对状态估计方法
CN106599368A (zh) * 2016-11-14 2017-04-26 浙江大学 基于改进粒子提议分布和自适应粒子重采样的FastSLAM方法
CN106597017A (zh) * 2016-12-16 2017-04-26 上海拓攻机器人有限公司 一种基于扩展卡尔曼滤波的无人机角加速度估计方法及装置

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US8010535B2 (en) * 2008-03-07 2011-08-30 Microsoft Corporation Optimization of discontinuous rank metrics

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105809718A (zh) * 2016-03-14 2016-07-27 西南交通大学 一种轨迹熵最小化的对象跟踪方法
CN105737828A (zh) * 2016-05-09 2016-07-06 郑州航空工业管理学院 一种基于强跟踪的相关熵扩展卡尔曼滤波的组合导航方法
CN106291645A (zh) * 2016-07-19 2017-01-04 东南大学 适于高维gnss/ins深耦合的容积卡尔曼滤波方法
CN106482728A (zh) * 2016-09-14 2017-03-08 西安交通大学 基于最大互相关熵准则无迹卡尔曼滤波的通信保障航天器相对状态估计方法
CN106599368A (zh) * 2016-11-14 2017-04-26 浙江大学 基于改进粒子提议分布和自适应粒子重采样的FastSLAM方法
CN106597017A (zh) * 2016-12-16 2017-04-26 上海拓攻机器人有限公司 一种基于扩展卡尔曼滤波的无人机角加速度估计方法及装置

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
基于信息熵的多无源传感器数据关联;曹乐 等;《传感器与微系统》;20151231;第34卷(第11期);第33-37页 *
车载自组网中车辆定位导航的混合协作定位和融合技术;Georges HASSANA MAIGARY;《万方博士学位论文期刊库》;20170103;第1-162页 *

Also Published As

Publication number Publication date
CN108332751A (zh) 2018-07-27

Similar Documents

Publication Publication Date Title
CN111896914A (zh) 一种协同定位方法、装置、设备和存储介质
CN114018274B (zh) 车辆定位方法、装置及电子设备
JP2016114603A (ja) Ｇｐｓドリフトの処理方法、およびそのシステム
CN111578928B (zh) 一种基于多源融合定位系统的定位方法及装置
CN108108335B (zh) 一种野值剔除方法及装置
CN115457152A (zh) 外参标定方法、装置、电子设备及存储介质
CN115656989A (zh) 外参标定方法、装置、电子设备及存储介质
CN111678513A (zh) 一种超宽带/惯导紧耦合的室内定位装置与系统
CN113219505B (zh) 用于车路协同隧道场景的采集gps坐标的方法、装置和设备
CN108120452B (zh) Mems陀螺仪动态数据的滤波方法
CN114139109A (zh) 一种目标跟踪方法、系统、设备、介质及数据处理终端
CN108332751B (zh) 一种多源融合定位方法、装置、电子设备及存储介质
CN115790665B (zh) 一种陀螺误差补偿方法、装置、电子设备及存储介质
CN106153046B (zh) 一种基于自适应Kalman滤波的陀螺随机噪声AR建模方法
Silva et al. Integration of a low-cost global navigation satellite system to a single-board computer using Kalman filtering
CN109581284B (zh) 基于交互多模型的非视距误差消除方法
CN114199236A (zh) 定位数据处理的方法、装置、电子设备和自动驾驶车辆
CN110033088B (zh) 估计gps数据的方法和装置
JP6954346B2 (ja) パラメータ推定装置、パラメータ推定方法、及びプログラム
CN111090281A (zh) 一种基于改进粒子滤波算法估算移动机器人精确方位的方法和装置
CN112859139A (zh) 一种姿态测量方法、装置及电子设备
CN116380056B (zh) 惯性定位方法、装置、电子设备及存储介质
CN112996105B (zh) 一种基于同时定位与标定的目标定位方法及装置
TWI518351B (zh) 室內定位方法和設備
CN117687006A (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
GR01 Patent grant
GR01 Patent grant