CN108562290B - 导航数据的滤波方法、装置、计算机设备及存储介质 - Google Patents

导航数据的滤波方法、装置、计算机设备及存储介质 Download PDF

Info

Publication number
CN108562290B
CN108562290B CN201810770760.9A CN201810770760A CN108562290B CN 108562290 B CN108562290 B CN 108562290B CN 201810770760 A CN201810770760 A CN 201810770760A CN 108562290 B CN108562290 B CN 108562290B
Authority
CN
China
Prior art keywords
value
sequence number
time sequence
state quantity
covariance
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
CN201810770760.9A
Other languages
English (en)
Other versions
CN108562290A (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.)
Shenzhen Daison Intelligence Technology Co ltd
Original Assignee
Shenzhen Daison Intelligence 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 Shenzhen Daison Intelligence Technology Co ltd filed Critical Shenzhen Daison Intelligence Technology Co ltd
Priority to CN201810770760.9A priority Critical patent/CN108562290B/zh
Publication of CN108562290A publication Critical patent/CN108562290A/zh
Application granted granted Critical
Publication of CN108562290B publication Critical patent/CN108562290B/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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices

Landscapes

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

Abstract

本发明涉及导航数据的滤波方法、装置、计算机设备及存储介质，该方法包括建立并初始化惯性***参数误差模型，设置下一滤波周期的时间序号为指定值；获取时间序号为指定值的状态量预测值、状态量协方差预测值；获取时间序号为指定值的量测值估计值、量测值协方差估计值、状态量与量测值互协方差；计算高斯核的系数；获取时间序号为指定值的状态量后验估计值；更新时间序号为指定值的状态量协方差；补偿参数；设置指定值为指定值加一，返回获取时间序号为指定值的状态量预测值、时间序号为指定值的状态协方差预测值。通过实施本发明实施例的方法可实现对非高斯噪声高阶项的捕获，提高误差的估计准确率，提升定位精度。

Description

导航数据的滤波方法、装置、计算机设备及存储介质
技术领域
本发明涉及车辆组合导航***，更具体地说是指导航数据的滤波方法、装置、计算机设备及存储介质。
背景技术
惯性***的理论基础是牛顿力学定律，***的运行只需要加速度和角速度信息的融合，不需要接收其他辅助信息，也不会产生外向辐射，具有屏蔽并且不产生干扰的特点，能在较短时间内提供高精度相对位置信息。自进入21世纪，基于MEMS(Micro-Electro-Mechanical System)技术的惯性传感器被成功应用于消费市场，MEMS是微机电***，也叫做微电子机械***、微***、微机械等，指尺寸在几毫米乃至更小的高科技装置，但是把惯性传感器应用于导航***需要解决误差在计算过程中迅速积累的问题。
多种成熟的卫星导航***共存的背景下，借助于差分GPS等辅助卫星定位技术，理想情况下，汽车能通过卫星导航***获取全方位、全天候、较高精度的绝对位置信息，但城市环境中卫星信号易受建筑物遮挡，产生较大的定位误差。惯性***与卫星***的各自特性使得惯性***/卫星***组合导航***能为车辆在城市环境下的精确定位提供有效的解决方案，两种子***能通过相互冗余并修正的方式，共同提升位置估计的精度。由于子***存在自身的误差，所以开发组合导航***需要把两个子***的数据进行融合并对误差进行补偿，目前最普遍采用的卡尔曼滤波算法及多种改进算法都是基于最小均方误差准则，此类方法能实现对高斯噪声的最优滤波性能，而无法捕获非高斯噪声的高阶项，另外，还可以采用非卡尔曼滤波迭代框架的滤波算法进行数据融合和误差进行补偿，如粒子滤波和神经网络滤波算法，都需要较大的计算量，不适用于状态量维数较高的组合导航***。
因此，有必要设计一种新的方法，以实现对非高斯噪声高阶项的有效捕获，提高误差的估计准确率，提升定位精度。
发明内容
本发明的目的在于克服现有技术的缺陷，提供导航数据的滤波方法、装置、计算机设备及存储介质。
为实现上述目的，第一方面，本发明实施例提供了一种导航数据的滤波方法，其包括：
建立惯性***参数误差模型；
初始化惯性***参数误差模型，设置下一滤波周期的时间序号为指定值；
对时间序号为指定值减一的状态量进行无迹变换，获取时间序号为指定值的状态量预测值、时间序号为指定值的状态量协方差预测值；
根据时间序号为指定值的状态量预测值对时间序号为指定值的量测值进行无迹变换，获取时间序号为指定值的量测值估计值、时间序号为指定值的量测值协方差的估计值、时间序号为指定值的状态量与量测值的互协方差；
根据时间序号为指定值的状态量协方差预测值和时间序号为指定值的状态量与量测值的互协方差构造伪观测矩阵，利用伪观测矩阵对非线性量测方程进行线性化改写，获取改写公式；
根据改写公式计算高斯核的系数；
利用时间序号为指定值的状态量协方差预测值、伪观测矩阵以及高斯核的系数获取时间序号为指定值的状态量后验估计值；
更新时间序号为指定值的状态量协方差；
反馈状态量后验估计值至惯性***内，补偿惯性***的参数；
设置指定值等于指定值加一，并返回所述对时间序号为指定值减一的状态量进行无迹变换，获取时间序号为指定值的状态量预测值、时间序号为指定值的状态量协方差预测值的步骤。
第二方面，本发明实施例还提供了一种导航数据的滤波装置，其包括：
模型建立单元，用于建立惯性***参数误差模型；
初始化单元，用于初始化惯性***参数误差模型，设置下一滤波周期的时间序号为指定值；
预测值获取单元，用于对时间序号为指定值减一的状态量进行无迹变换，获取时间序号为指定值的状态量预测值、时间序号为指定值的状态量协方差预测值；
估计值获取单元，用于根据时间序号为指定值减一的状态量预测值对时间序号为指定值的量测值进行无迹变换，获取时间序号为指定值的量测值估计值、时间序号为指定值的量测值协方差的估计值、时间序号为指定值的状态量与量测值的互协方差；
改写公式获取单元，用于根据时间序号为指定值的状态量协方差预测值和时间序号为指定值的状态量与量测值的互协方差构造伪观测矩阵，利用伪观测矩阵对非线性量测方程进行线性化改写，获取改写公式；
系数获取单元，用于根据改写公式计算高斯核的系数；
后验估计值获取单元，用于利用时间序号为指定值的状态量协方差预测值、伪观测矩阵以及高斯核的系数获取时间序号为指定值的状态量后验估计值；
方差更新单元，用于更新时间序号为指定值的状态量协方差；
参数补偿单元，用于反馈状态量后验估计值至惯性***内，补偿惯性***的参数；
设置单元，用于设置指定值等于指定值加一。
第三方面，本发明实施例还提供了一种计算机设备，其包括存储器及处理器，所述存储器上存储有计算机程序，所述处理器执行所述计算机程序时实现上述方法。
第四方面，本发明实施例还提供了一种计算机可读存储介质，所述存储介质存储有计算机程序，所述计算机程序包括程序指令，所述程序指令当被处理器执行时可实现上述方法。
本发明与现有技术相比的有益效果是：本发明通过在对导航***的状态量误差进行估计时，引入最大相关熵准则对传统无迹卡尔曼滤波算法进行改进，利用相关熵能捕获非高斯噪声高阶项，对***误差进行精确估计，并通过反馈状态量后验估计值，进行校正方式，对***误差进行补偿，实现导航参数的高精度输出，提高误差的估计准确率，提升定位精度，具备高效的滤波性能。
下面结合附图和具体实施例对本发明作进一步描述。
附图说明
图1为本发明具体实施例提供的导航数据的滤波方法的示意流程图；
图2为本发明具体实施例提供的导航数据的滤波方法的子步骤示意流程图；
图3为本发明具体实施例提供的导航数据的滤波方法的子步骤示意流程图；
图4为本发明具体实施例提供的导航数据的滤波方法的子步骤示意流程图；
图5为本发明具体实施例提供的位置估计误差对比的示意图；
图6为本发明具体实施例提供的导航数据的滤波装置的示意性框图；
图7为本发明具体实施例提供的模型建立单元的示意性框图；
图8为本发明具体实施例提供的初始化单元的示意性框图；
图9为本发明具体实施例提供的后验估计值获取单元的示意性框图；
图10为本发明具体实施例提供的一种计算机设备的示意性框图。
具体实施方式
为了更充分理解本发明的技术内容，下面结合具体实施例对本发明的技术方案进一步介绍和说明，但不局限于此。
应当理解，当在本说明书和所附权利要求书中使用时，术语“包括”和“包含”指示所描述特征、整体、步骤、操作、元素和/或组件的存在，但并不排除一个或多个其它特征、整体、步骤、操作、元素、组件和/或其集合的存在或添加。
还应当理解，在此本申请说明书中所使用的术语仅仅是出于描述特定实施例的目的而并不意在限制本申请。如在本申请说明书和所附权利要求书中所使用的那样，除非上下文清楚地指明其它情况，否则单数形式的“一”、“一个”及“该”意在包括复数形式。
还应当进一步理解，在本申请说明书和所附权利要求书中使用的术语“和/或”是指相关联列出的项中的一个或多个的任何组合以及所有可能组合，并且包括这些组合。
请参阅图1，图1为本发明具体实施例提供的导航数据的滤波方法的示意流程图；该滤波方法应用于服务器内，以滤波平台的形式存在；该服务器与导航装置进行数据交互，以使得导航装置的导航数据可以进行误差补偿，提高定位精度。
图1为本发明具体实施例提供的导航数据的滤波方法的示意流程图，如图1所示，该方法包括S110～S200。
S110、建立惯性***参数误差模型。
在本发明实施例中，惯性***参数误差模型是指由惯性***中的各个参数的误差按照特定的返程组合形成的模型。
如图2所示，在一实施例中，上述的S110可包括步骤S111～S113。
S111、利用惯性***的参数误差构造状态方程；
S112、利用惯性***与卫星***的测量信息的差值建立量测方程；
S113、根据状态方程和量测方程组成惯性***参数误差模型。
其中，状态方程是指惯性***的姿态角误差、速度误差、位置误差、陀螺仪常值零偏和加速度计常值零偏组合形成的方程，量测方程是惯性***与卫星***的测量信息的差值之间的线性关系组成的方程。避免了卡尔曼滤波算法在组合导航***中的非线性不适用问题。
模型中状态量xk包含惯性***的姿态角误差、速度误差、位置误差、陀螺仪常值零偏和加速度计常值零偏，此处的状态量为n维，***模型包含状态方程和量测方程；
惯性***参数误差模型表示为
Figure GDA0002632532020000061
其中；***状态量xk表示为
Figure GDA0002632532020000062
Figure GDA0002632532020000063
表示计算过程中导航坐标系内角度计算误差；δVE、δVN、δVU表示计算过程中导航坐标系内速度计算误差；δL、δλ、δh表示计算过程中地心惯性坐标系内的位置计算误差；bwx、bwy、bwz表示载体坐标系中陀螺仪在三个轴向的固定漂移；bfx、bfy、bfz表示载体坐标系中加速度器在三个轴向的固定漂移；Fk-1和Hk分别为15*15的状态转移矩阵和6*15的量测矩阵，n＝15；qk-1表示***噪声，统计特性为高斯特性，其协方差为Qk；rk表示测量噪声，此处设置为高斯混合噪声，统计特性为非高斯特性，协方差表示为Rk；yk为量测值。
S120、初始化惯性***参数误差模型，设置下一滤波周期的时间序号为指定值。
对***进行初始化，以完成卡尔曼滤波的迭代。在本实施例中，时间序号用k表示，此时设定的下一滤波周期的时间序号为k＝1。
在一实施例中，如图3所示，上述的S120可包括有S121～S122。
S121、时间序号为零的状态量及时间序号为零的协方差进行初始化；
S122、设置下一滤波周期的时间序号为一。
对***进行初始化，设置时间序号为k＝0的***状态值，令惯性导航***采集惯性数据，卫星导航***接收卫星信号，惯性***进行惯导数据解算，并令时间序列的序号k＝1，先获取零时刻的***状态值，作为初始数据，再根据初始数据对惯性***参数误差模型进行参数初始化。
具体地，对k＝0时刻***的初始化状态量x0设置为x0＝0及初始化状态量对应的初始方差设置为P0＝0；并设置
Figure GDA0002632532020000071
对组合导航***中导航参数的误差估计，在采集了加速度计数据和陀螺仪数据后，进行了惯性***的导航解算，包括速度和位置信息，导航输出数据为惯性导航解算数据在进行误差补偿后的修正值，也就是服务器进行所有计算后，将计算结果对导航***进行误差补偿修正，形成最终的结果。
S130、对时间序号为指定值减一的状态量进行无迹变换，获取时间序号为指定值的状态量预测值、时间序号为指定值的状态量协方差预测值。
状态量预测值指的是在某一时刻，依据惯性***参数误差模型得出的状态量；状态量协方差预测值是指在某一时刻，依据状态量预测值得出进行方差计算，获取的数值。
在本实施例中，具体是对k-1时刻的状态量xk-1应用无迹变换，求出状态量及其协方差的先验预测值
Figure GDA0002632532020000072
和Pk|k-1；对于无迹变换过程，采用以下方式：
计算2n+1个sigma点χk-1(卡尔曼滤波采样点)，计算公式如下：
Figure GDA0002632532020000081
Figure GDA0002632532020000082
Figure GDA0002632532020000083
式中λ是一个复合比例因子，规定λ＝α2(n+ε)-n；其中通常设置0＜α≤1，ε是一个比例因子，一般选择为3-n。
利用状态转换矩阵对sigma点(卡尔曼滤波采样点)进行非线性变换，公式如下：
Figure GDA0002632532020000084
通过变换后的点计算先验预测值
Figure GDA0002632532020000085
和Pk|k-1，计算公式如下：
Figure GDA0002632532020000086
Figure GDA0002632532020000087
Qk是惯性***噪声的协方差，
Figure GDA0002632532020000088
是状态均值对应的权值，
Figure GDA0002632532020000089
Figure GDA00026325320200000810
是关联协方差矩阵对应的权值，计算方式如下：
Figure GDA00026325320200000811
其中，i＝1，…，2n，β与xk的先验知识相关，通常情况下取β＝2。
基于无迹卡尔曼滤波算法框架，避免了卡尔曼滤波算法在惯性***与卫星***组合的导航***中的非线性不适用问题。
S140、根据时间序号为指定值的状态量预测值对时间序号为指定值的量测值进行无迹变换，获取时间序号为指定值的量测值估计值、时间序号为指定值的量测值协方差的估计值、时间序号为指定值的状态量与量测值的互协方差。
量测值估计值是由状态量预测值代入惯性***参数误差模型内，获取的量测值；量测值协方差的估计值是指对量测值估计值进行方差计算，获取的方差值；状态量与量测值的互协方差是指某一时刻的某一状态量对应的量测值进行互协方差计算后，获取的数值。
在本实施例中，上述的量测值协方差的估计值采用以下公式计算：
Figure GDA0002632532020000091
Figure GDA0002632532020000092
其中，Rk是测量噪声的协方差，
Figure GDA0002632532020000093
是时间序号为指定值的量测值的估计值；PY,k|k-1是时间序号为指定值的量测值协方差的估计值；
Figure GDA0002632532020000094
是关联协方差矩阵对应的权值。
该步骤中的无迹变换是采用非线性变换矩阵是观测矩阵，用观测矩阵代替步骤S130中的状态转移矩阵，进行与S130步骤类似的无迹变换。
S150、根据时间序号为指定值的状态量协方差预测值和时间序号为指定值的状态量与量测值的互协方差构造伪观测矩阵，利用伪观测矩阵对非线性量测方程进行线性化改写，获取改写公式。
在本实施例中，伪观测矩阵采用以下公式构造：
Figure GDA0002632532020000095
Figure GDA0002632532020000096
其中，Pk|k-1是时间序号为指定值的状态量协方差预测值，PXY,k|k-1是时间序号为指定值的状态量与量测值的互协方差。
在本实施例中，改写公式为
Figure GDA0002632532020000097
其中，
Figure GDA0002632532020000098
Figure GDA0002632532020000099
是时间序号为指定值的状态量预测值，Pk|k-1是时间序号为指定值的状态量协方差预测值；
Figure GDA00026325320200000910
是伪观测矩阵，
Figure GDA00026325320200000911
是时间序号为指定值的状态量预测值，PY,k|k-1是时间序号为指定值的量测值协方差的估计值。
S160、根据改写公式计算高斯核的系数；
在本实施例中，该高斯核的系数
Figure GDA00026325320200000912
Figure GDA00026325320200000913
在计算该高斯核的系数时，首先参考加权最小二乘法求解卡尔曼滤波方程的过程，列写代价函数，代价函数的设计需要体现相关熵，利用高斯核函数的使用来体现相关熵，代价函数表示为：
Figure GDA0002632532020000101
为计算代价函数所表示的最大值，对代价函数计算关于
Figure GDA0002632532020000102
的微分，并令其为零。
使用最大相关熵准则来计算状态量估计值，由于高斯核函数及其核宽参数的作用，使得无迹卡尔曼滤波的迭代过程遵循最大相关熵准则，对非高斯噪声的统计二阶项和更高阶项实现有效捕获。
S170、利用时间序号为指定值的状态量协方差预测值、伪观测矩阵以及高斯核的系数获取时间序号为指定值的状态量后验估计值。
状态量后验估计值是指某一时刻的最优状态量估计值。
在一实施例中，如图4所示，上述的步骤S170可包括有S171～S172。
S171、利用时间序号为指定值的状态量协方差预测值、伪观测矩阵以及高斯核的系数获取卡尔曼滤波增益；其中，该卡尔曼滤波增益
Figure GDA0002632532020000103
Figure GDA0002632532020000104
S172、根据卡尔曼滤波增益获取时间序号为指定值的状态量后验估计值。该状态量后验估计值为
Figure GDA0002632532020000105
该数据是k时刻的最优状态量估计值。
S180、更新时间序号为指定值的状态量协方差；具体地，将时间序号为指定值的状态量协方差更新为
Figure GDA0002632532020000106
S190、反馈状态量后验估计值至惯性***内，补偿惯性***的参数。
具体地，利用惯性***所解算的数据减去对应时刻的状态量后验估计值，形成补偿数据，并把补偿数据作为下一时刻惯性***的解算基础。
S200、设置指定值等于指定值加一，并返回步骤S120。
以此不断完成卡尔曼滤波的迭代，形成对导航误差估计进行不断更新，保证了组合导航***滤波的实时性和有效性。
当整个导航***断电时，该流程结束，也就是不再进入循环。
举一个例子，以松组合方式的SINS/GNSS(惯性***/卫星***)组合导航***为例，导航初始点为西安市某点，经纬度分别设置在北纬34.14°，东经108.54°，陀螺仪三轴向的常值偏差都设置为0.03deg/h，随机漂移设置为0.001deg/sqrt(h)，加速度计常值偏差设置为0.0001g，随机偏差设置为0.0005g/sqrt(Hz)。卫星***采用GPS模拟***，位置误差方差为5m；惯性***传感器的采集频率为100Hz，GPS更新频率为1Hz。进行两组实验，其中一组是利用本实施例提供的滤波方法在三个方向(东、北、天)位置估计误差量，另一组为传统基于最小均方误差的无迹卡尔曼滤波算法(Unscented Kalman Filter，UKF)对本例数据的三个方向上的位置估计误差量。实验结果如图5所示，从总体结果看，本方法在位置估计精度上高于传统无迹卡尔曼滤波算法。
上述的导航数据的滤波方法，通过在对导航***的状态量误差进行估计时，引入最大相关熵准则对传统无迹卡尔曼滤波算法进行改进，利用相关熵能捕获非高斯噪声高阶项，对***误差进行精确估计，并通过反馈状态量后验估计值，进行校正方式，对***误差进行补偿，实现导航参数的高精度输出，提高误差的估计准确率，提升定位精度，具备高效的滤波性能。
请参阅图6，图6是本发明具体实施例提供的导航数据的滤波装置300的示意性框图；如图6所示，该装置包括：
模型建立单元310，用于建立惯性***参数误差模型。
初始化单元320，用于初始化惯性***参数误差模型，设置下一滤波周期的时间序号为指定值。
预测值获取单元330，用于对时间序号为指定值减一的状态量进行无迹变换，获取时间序号为指定值的状态量预测值、时间序号为指定值的状态量协方差预测值。
估计值获取单元340，用于根据时间序号为指定值的状态量预测值对时间序号为指定值的量测值进行无迹变换，获取时间序号为指定值的量测值估计值、时间序号为指定值的量测值协方差的估计值、时间序号为指定值的状态量与量测值的互协方差。
改写公式获取单元350，用于根据时间序号为指定值的状态量协方差预测值和时间序号为指定值的状态量与量测值的互协方差构造伪观测矩阵，利用伪观测矩阵对非线性量测方程进行线性化改写，获取改写公式。
系数获取单元360，用于根据改写公式计算高斯核的系数。
后验估计值获取单元370，用于利用时间序号为指定值的状态量协方差预测值、伪观测矩阵以及高斯核的系数获取时间序号为指定值的状态量后验估计值。
方差更新单元380，用于更新时间序号为指定值的状态量协方差。
参数补偿单元390，用于反馈状态量后验估计值至惯性***内，补偿惯性***的参数。
设置单元391，用于设置指定值等于指定值加一。
在一实施例中，如图7所示，上述的模型建立单元310包括有：
状态方程构建模块311，用于利用惯性***的参数误差构造状态方程。
量测方程构建模块312，用于利用惯性***与卫星***的测量信息的差值建立量测方程。
组合模块313，用于根据状态方程和量测方程组成惯性***参数误差模型。
在一实施例中，如图8所示，上述的初始化单元320包括有：
数据初始化模块321，用于时间序号为零的状态量及时间序号为零的协方差进行初始化；
序号设置模块322，用于设置下一滤波周期的时间序号为一。
在一实施例中，如图9所示，上述的后验估计值获取单元370包括有：
增益获取模块371，用于利用时间序号为指定值的状态量协方差预测值、伪观测矩阵以及高斯核的系数获取卡尔曼滤波增益；
数值获取模块372，用于根据卡尔曼滤波增益获取时间序号为指定值的状态量后验估计值。
需要说明的是，所属领域的技术人员可以清楚地了解到，上述导航数据的滤波装置300和各单元的具体实现过程，可以参考前述方法实施例中的相应描述，为了描述的方便和简洁，在此不再赘述。
上述导航数据的滤波装置300可以实现为一种计算机程序的形式，该计算机程序可以在如图10所示的计算机设备上运行。
请参阅图10，图10是本申请实施例提供的一种计算机设备的示意性框图。该计算机设备500是服务器，具体地，该参阅图10，该计算机设备500包括通过***总线501连接的处理器502、存储器和网络接口505，其中，存储器可以包括非易失性存储介质503和内存储器504。
该非易失性存储介质503可存储操作***5031和计算机程序5032。该计算机程序5032包括程序指令，该程序指令被执行时，可使得处理器502执行一种导航数据的滤波方法。
该处理器502用于提供计算和控制能力，以支撑整个计算机设备500的运行。
该内存储器504为非易失性存储介质503中的计算机程序5032的运行提供环境，该计算机程序5032被处理器502执行时，可使得处理器502执行一种导航数据的滤波方法。
该网络接口505用于与其它设备进行网络通信。本领域技术人员可以理解，图8中示出的结构，仅仅是与本申请方案相关的部分结构的框图，并不构成对本申请方案所应用于其上的计算机设备500的限定，具体的计算机设备500可以包括比图中所示更多或更少的部件，或者组合某些部件，或者具有不同的部件布置。
其中，所述处理器502用于运行存储在存储器中的计算机程序5032，以实现如下步骤：
建立惯性***参数误差模型；
初始化惯性***参数误差模型，设置下一滤波周期的时间序号为指定值；
对时间序号为指定值减一的状态量进行无迹变换，获取时间序号为指定值的状态量预测值、时间序号为指定值的状态量协方差预测值；
根据时间序号为指定值的状态量预测值对时间序号为指定值的量测值进行无迹变换，获取时间序号为指定值的量测值估计值、时间序号为指定值的量测值协方差的估计值、时间序号为指定值的状态量与量测值的互协方差；
根据时间序号为指定值的时刻的状态量协方差预测值和时间序号为指定值的状态量与量测值的互协方差构造伪观测矩阵，利用伪观测矩阵对非线性量测方程进行线性化改写，获取改写公式；
根据改写公式计算高斯核的系数；
利用时间序号为指定值的状态量协方差预测值、伪观测矩阵以及高斯核的系数获取时间序号为指定值的状态量后验估计值；
更新时间序号为指定值的状态量协方差；
反馈状态量后验估计值至惯性***内，补偿惯性***的参数；
设置指定值等于指定值加一，并返回所述对时间序号为指定值减一的状态量进行无迹变换，获取时间序号为指定值的状态量预测值、时间序号为指定值的状态量协方差预测值的步骤。
在一实施例中，处理器502在实现所述建立惯性***参数误差模型步骤时，具体实现如下步骤：
利用惯性***的参数误差构造状态方程；
利用惯性***与卫星***的测量信息的差值建立量测方程；
根据状态方程和量测方程组成惯性***参数误差模型。
在一实施例中，处理器502在实现所述初始化惯性***参数误差模型，设置下一滤波周期的时间序号为指定值步骤时，具体实现如下步骤：
时间序号为零的状态量及时间序号为零的协方差进行初始化；
设置下一滤波周期的时间序号为一。
上述的所述量测值协方差的估计值采用以下公式计算
Figure GDA0002632532020000151
Figure GDA0002632532020000152
其中，Rk是测量噪声的协方差，
Figure GDA0002632532020000153
是时间序号为指定值的量测值估计值；PY,k|k-1是时间序号为指定值的量测值协方差的估计值；
Figure GDA0002632532020000154
是关联协方差矩阵对应的权值。
上述的伪观测矩阵采用以下公式构造：
Figure GDA0002632532020000155
其中，Pk|k-1是时间序号为指定值的状态量协方差预测值，PXY,k|k-1是时间序号为指定值的状态量与量测值的互协方差。
上述的改写公式为
Figure GDA0002632532020000156
其中，
Figure GDA0002632532020000157
Figure GDA0002632532020000158
是时间序号为指定值的状态量预测值，Pk|k-1是时间序号为指定值的状态量协方差预测值；
Figure GDA0002632532020000159
是伪观测矩阵，
Figure GDA00026325320200001510
是时间序号为指定值的状态量预测值，PY,k|k-1是时间序号为指定值的量测值协方差的估计值。
在一实施例中，处理器502在实现所述利用时间序号为指定值的状态量协方差预测值、伪观测矩阵以及高斯核的系数获取时间序号为指定值的状态量后验估计值步骤时，具体实现如下步骤：
利用时间序号为指定值的状态量协方差预测值、伪观测矩阵以及高斯核的系数获取卡尔曼滤波增益；
根据卡尔曼滤波增益获取时间序号为指定值的状态量后验估计值。
应当理解，在本申请实施例中，处理器502可以是中央处理单元(CentralProcessing Unit，CPU)，该处理器502还可以是其他通用处理器、数字信号处理器(DigitalSignal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现成可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。其中，通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。
本领域普通技术人员可以理解的是实现上述实施例的方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成。该计算机程序包括程序指令，计算机程序可存储于一存储介质中，该存储介质为计算机可读存储介质。该程序指令被该计算机***中的至少一个处理器执行，以实现上述方法的实施例的流程步骤。
因此，本发明还提供一种存储介质。该存储介质可以为计算机可读存储介质。该存储介质存储有计算机程序，其中计算机程序包括程序指令。该程序指令被处理器执行时使处理器执行如下步骤：
建立惯性***参数误差模型；
初始化惯性***参数误差模型，设置下一滤波周期的时间序号为指定值；
对时间序号为指定值减一的状态量进行无迹变换，获取时间序号为指定值的状态量预测值、时间序号为指定值的状态量协方差预测值；
根据时间序号为指定值的状态量预测值对时间序号为指定值的量测值进行无迹变换，获取时间序号为指定值的量测值估计值、时间序号为指定值的量测值协方差的估计值、时间序号为指定值的状态量与量测值的互协方差；
根据时间序号为指定值的时刻的状态量协方差预测值和时间序号为指定值的状态量与量测值的互协方差构造伪观测矩阵，利用伪观测矩阵对非线性量测方程进行线性化改写，获取改写公式；
根据改写公式计算高斯核的系数；
利用时间序号为指定值的状态量协方差预测值、伪观测矩阵以及高斯核的系数获取时间序号为指定值的状态量后验估计值；
更新时间序号为指定值的状态量协方差；
反馈状态量后验估计值至惯性***内，补偿惯性***的参数；
设置指定值等于指定值加一，并返回所述对时间序号为指定值减一的状态量进行无迹变换，获取时间序号为指定值的状态量预测值、时间序号为指定值的状态量协方差预测值的步骤。
在一实施例中，处理器502所述处理器在执行所述程序指令而实现所述建立惯性***参数误差模型步骤时，具体实现如下步骤：
利用惯性***的参数误差构造状态方程；
利用惯性***与卫星***的测量信息的差值建立量测方程；
根据状态方程和量测方程组成惯性***参数误差模型。
在一实施例中，处理器502所述处理器在执行所述程序指令而实现所述初始化惯性***参数误差模型，设置下一滤波周期的时间序号为指定值步骤时，具体实现如下步骤：
时间序号为零的状态量及时间序号为零的协方差进行初始化；
设置下一滤波周期的时间序号为一。
上述的量测值协方差的估计值采用以下公式计算：
Figure GDA0002632532020000181
Figure GDA0002632532020000182
其中，Rk是测量噪声的协方差，
Figure GDA0002632532020000183
是时间序号为指定值的量测值估计值；PY,k|k-1是时间序号为指定值的量测值协方差的估计值；
Figure GDA0002632532020000184
是关联协方差矩阵对应的权值。
上述的伪观测矩阵采用以下公式构造：
Figure GDA0002632532020000185
其中，Pk|k-1是时间序号为指定值的状态量协方差预测值，PXY,k|k-1是时间序号为指定值的状态量与量测值的互协方差。
上述的改写公式为
Figure GDA0002632532020000186
其中，
Figure GDA0002632532020000187
Figure GDA0002632532020000188
是时间序号为指定值的状态量预测值，Pk|k-1是时间序号为指定值的状态量协方差预测值；
Figure GDA0002632532020000189
是伪观测矩阵，
Figure GDA00026325320200001810
是时间序号为指定值的状态量预测值，PY,k|k-1是时间序号为指定值的量测值协方差的估计值。
在一实施例中，处理器502所述处理器在执行所述程序指令而实现所述利用时间序号为指定值的状态量协方差预测值、伪观测矩阵以及高斯核的系数获取时间序号为指定值的状态量后验估计值步骤时，具体实现如下步骤：
利用时间序号为指定值的状态量协方差预测值、伪观测矩阵以及高斯核的系数获取卡尔曼滤波增益；
根据卡尔曼滤波增益获取时间序号为指定值的状态量后验估计值。
所述存储介质可以是U盘、移动硬盘、只读存储器(Read-Only Memory，ROM)、磁碟或者光盘等各种可以存储程序代码的计算机可读存储介质。
本领域普通技术人员可以意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，为了清楚地说明硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。
在本发明所提供的几个实施例中，应该理解到，所揭露的装置和方法，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的。例如，各个单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式。例如多个单元或组件可以结合或者可以集成到另一个***，或一些特征可以忽略，或不执行。
本发明实施例方法中的步骤可以根据实际需要进行顺序调整、合并和删减。本发明实施例装置中的单元可以根据实际需要进行合并、划分和删减。另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以是两个或两个以上单元集成在一个单元中。
该集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分，或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，终端，或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。
上述仅以实施例来进一步说明本发明的技术内容，以便于读者更容易理解，但不代表本发明的实施方式仅限于此，任何依本发明所做的技术延伸或再创造，均受本发明的保护。本发明的保护范围以权利要求书为准。

Claims (10)

1.导航数据的滤波方法，其特征在于，包括：
建立惯性***参数误差模型；
初始化惯性***参数误差模型，设置下一滤波周期的时间序号为指定值；
对时间序号为指定值减一的状态量进行无迹变换，获取时间序号为指定值的状态量预测值、时间序号为指定值的状态量协方差预测值；
根据时间序号为指定值的状态量预测值对时间序号为指定值的量测值进行无迹变换，获取时间序号为指定值的量测值估计值、时间序号为指定值的量测值协方差的估计值、时间序号为指定值的状态量与量测值的互协方差；
根据时间序号为指定值的状态量协方差预测值和时间序号为指定值的状态量与量测值的互协方差构造伪观测矩阵，利用伪观测矩阵对非线性量测方程进行线性化改写，获取改写公式；
根据改写公式计算高斯核的系数；
利用时间序号为指定值的状态量协方差预测值、伪观测矩阵以及高斯核的系数获取时间序号为指定值的状态量后验估计值；
更新时间序号为指定值的状态量协方差；
反馈状态量后验估计值至惯性***内，补偿惯性***的参数；
设置指定值等于指定值加一，并返回所述对时间序号为指定值减一的状态量进行无迹变换，获取时间序号为指定值的状态量预测值、时间序号为指定值的状态量协方差预测值的步骤；
其中，根据改写公式计算高斯核的系数的步骤包括：高斯核的系数
Figure FDA0002632532010000011
Figure FDA0002632532010000012
在计算该高斯核的系数时，首先参考加权最小二乘法求解卡尔曼滤波方程的过程，列写代价函数，代价函数的设计需要体现相关熵，利用高斯核函数的使用来体现相关熵，代价函数表示为：
Figure FDA0002632532010000021
其中，
Figure FDA0002632532010000022
为伪观测矩阵，
Figure DEST_PATH_IMAGE001
为状态量预测值，yk为量测值，
Figure FDA0002632532010000024
为量测值的估计值，
Figure FDA0002632532010000025
PY,k|k-1为量测值协方差的估计值；
为计算代价函数所表示的最大值，对代价函数计算关于
Figure FDA0002632532010000026
的微分，并令代价函数为零。
2.根据权利要求1所述的导航数据的滤波方法，其特征在于，所述建立惯性***参数误差模型的步骤，包括以下具体步骤：
利用惯性***的参数误差构造状态方程；
利用惯性***与卫星***的测量信息的差值建立量测方程；
根据状态方程和量测方程组成惯性***参数误差模型。
3.根据权利要求1所述的导航数据的滤波方法，其特征在于，所述初始化惯性***参数误差模型，设置下一滤波周期的时间序号为指定值步骤，包括以下具体步骤：
时间序号为零的状态量及时间序号为零的协方差进行初始化；
设置下一滤波周期的时间序号为一。
4.根据权利要求1所述的导航数据的滤波方法，其特征在于，所述量测值协方差的估计值采用以下公式计算：
Figure FDA0002632532010000027
其中，Rk是测量噪声的协方差，
Figure FDA0002632532010000028
是时间序号为指定值的量测值的估计值；PY,k|k-1是时间序号为指定值的量测值协方差的估计值；Wc (i)是关联协方差矩阵对应的权值。
5.根据权利要求1所述的导航数据的滤波方法，其特征在于，所述伪观测矩阵采用以下公式构造：
Figure FDA0002632532010000031
其中，Pk|k-1是时间序号为指定值的状态量协方差预测值，PXY,k|k-1是时间序号为指定值的状态量与量测值的互协方差。
6.根据权利要求4所述的导航数据的滤波方法，其特征在于，所述改写公式为
Figure FDA0002632532010000032
其中，
Figure FDA0002632532010000033
Figure FDA0002632532010000034
Figure FDA0002632532010000035
是时间序号为指定值的状态量预测值，Pk|k-1是时间序号为指定值的状态量协方差预测值；
Figure FDA0002632532010000036
是伪观测矩阵，
Figure FDA0002632532010000037
是时间序号为指定值的状态量预测值，PY,k|k-1是时间序号为指定值的量测值协方差的估计值。
7.根据权利要求1所述的导航数据的滤波方法，其特征在于，利用时间序号为指定值的状态量协方差预测值、伪观测矩阵以及高斯核的系数获取时间序号为指定值的状态量后验估计值的步骤，包括以下具体步骤：
利用时间序号为指定值的状态量协方差预测值、伪观测矩阵以及高斯核的系数获取卡尔曼滤波增益；
根据卡尔曼滤波增益获取时间序号为指定值的状态量后验估计值。
8.导航数据的滤波装置，其特征在于，包括：
模型建立单元，用于建立惯性***参数误差模型；
初始化单元，用于初始化惯性***参数误差模型，设置下一滤波周期的时间序号为指定值；
预测值获取单元，用于对时间序号为指定值减一的状态量进行无迹变换，获取时间序号为指定值的状态量预测值、时间序号为指定值的状态量协方差预测值；
估计值获取单元，用于根据时间序号为指定值的状态量预测值对时间序号为指定值的量测值进行无迹变换，获取时间序号为指定值的量测值估计值、时间序号为指定值的量测值协方差的估计值、时间序号为指定值的状态量与量测值的互协方差；
改写公式获取单元，用于根据时间序号为指定值的状态量协方差预测值和时间序号为指定值的状态量与量测值的互协方差构造伪观测矩阵，利用伪观测矩阵对非线性量测方程进行线性化改写，获取改写公式；
系数获取单元，用于根据改写公式计算高斯核的系数；
后验估计值获取单元，用于利用时间序号为指定值的状态量协方差预测值、伪观测矩阵以及高斯核的系数获取时间序号为指定值的状态量后验估计值；
方差更新单元，用于更新时间序号为指定值的状态量协方差；
参数补偿单元，用于反馈状态量后验估计值至惯性***内，补偿惯性***的参数；
设置单元，用于设置指定值等于指定值加一；
其中，所述系数获取单元包括：高斯核的系数
Figure FDA0002632532010000041
Figure FDA0002632532010000042
在计算该高斯核的系数时，首先参考加权最小二乘法求解卡尔曼滤波方程的过程，列写代价函数，代价函数的设计需要体现相关熵，利用高斯核函数的使用来体现相关熵，代价函数表示为：
Figure FDA0002632532010000043
其中，
Figure FDA0002632532010000044
为伪观测矩阵，
Figure FDA0002632532010000045
为状态量预测值，yk为量测值，
Figure FDA0002632532010000046
为量测值的估计值，
Figure FDA0002632532010000051
PY,k|k-1为量测值协方差的估计值；
为计算代价函数所表示的最大值，对代价函数计算关于
Figure FDA0002632532010000052
的微分，并令代价函数为零。
9.一种计算机设备，其特征在于，所述计算机设备包括存储器及处理器，所述存储器上存储有计算机程序，所述处理器执行所述计算机程序时实现如权利要求1至7中任一项所述的方法。
10.一种存储介质，其特征在于，所述存储介质存储有计算机程序，所述计算机程序包括程序指令，所述程序指令当被处理器执行时可实现如权利要求1至7中任一项所述的方法。
CN201810770760.9A 2018-07-13 2018-07-13 导航数据的滤波方法、装置、计算机设备及存储介质 Active CN108562290B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810770760.9A CN108562290B (zh) 2018-07-13 2018-07-13 导航数据的滤波方法、装置、计算机设备及存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810770760.9A CN108562290B (zh) 2018-07-13 2018-07-13 导航数据的滤波方法、装置、计算机设备及存储介质

Publications (2)

Publication Number Publication Date
CN108562290A CN108562290A (zh) 2018-09-21
CN108562290B true CN108562290B (zh) 2020-10-27

Family

ID=63555388

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810770760.9A Active CN108562290B (zh) 2018-07-13 2018-07-13 导航数据的滤波方法、装置、计算机设备及存储介质

Country Status (1)

Country Link
CN (1) CN108562290B (zh)

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109211277B (zh) * 2018-10-31 2021-11-16 北京旷视科技有限公司 视觉惯性里程计的状态确定方法、装置和电子设备
CN111258218B (zh) * 2020-01-17 2022-08-12 成都信息工程大学 基于最大相关熵准则的智能车辆路径跟踪方法
CN111625766A (zh) * 2020-04-27 2020-09-04 中国人民解放军63921部队 广义延拓逼近滤波方法、存储介质及处理器
CN112987560B (zh) * 2021-04-19 2021-09-10 长沙智能驾驶研究院有限公司 滤波器控制方法、装置、设备及计算机存储介质
CN113447019B (zh) * 2021-08-05 2023-01-13 齐鲁工业大学 Ins/cns组合导航方法、***、存储介质及设备

Family Cites Families (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP2315046B1 (en) * 2009-10-20 2012-11-07 TomTec Imaging Systems GmbH Method and device for pre-processing Doppler ultrasound data
CN105737828B (zh) * 2016-05-09 2018-07-31 郑州航空工业管理学院 一种基于强跟踪的相关熵扩展卡尔曼滤波的组合导航方法
CN106487358B (zh) * 2016-09-30 2019-05-10 西南大学 一种机动目标转弯跟踪方法

Also Published As

Publication number Publication date
CN108562290A (zh) 2018-09-21

Similar Documents

Publication Publication Date Title
CN108562290B (zh) 导航数据的滤波方法、装置、计算机设备及存储介质
CN112013836B (zh) 一种基于改进自适应卡尔曼滤波的航姿参考***算法
CN109931955B (zh) 基于状态相关李群滤波的捷联惯性导航***初始对准方法
CN106772524B (zh) 一种基于秩滤波的农业机器人组合导航信息融合方法
CN110006427B (zh) 一种低动态高振动环境下的bds/ins紧组合导航方法
CN110275193B (zh) 一种基于因子图的集群卫星协同导航方法
Eckenhoff et al. High-accuracy preintegration for visual-inertial navigation
US10379224B2 (en) Invariant particle filtering
Hashlamon A new adaptive extended Kalman filter for a class of nonlinear systems
CN111366156A (zh) 基于神经网络辅助的变电站巡检机器人导航方法及***
CN105157704A (zh) 一种基于贝叶斯估计的粒子滤波重力辅助惯导匹配方法
Guo et al. IMU-RGBD camera navigation using point and plane features
Poshtan et al. Cascaded Kalman and particle filters for photogrammetry based gyroscope drift and robot attitude estimation
CN116007620A (zh) 一种组合导航滤波方法、***、电子设备及存储介质
Pfeiffer et al. A computationally efficient moving horizon estimator for ultra-wideband localization on small quadrotors
Li et al. Joint localization based on split covariance intersection on the Lie group
Fernandes et al. GNSS/MEMS-INS integration for drone navigation using EKF on lie groups
Xiang et al. In-motion initial alignment method for a laser Doppler velocimeter-aided strapdown inertial navigation system based on an adaptive unscented quaternion H-infinite filter
CN112284388B (zh) 一种无人机多源信息融合导航方法
Chu et al. Performance comparison of tight and loose INS-Camera integration
CN114858166B (zh) 基于最大相关熵卡尔曼滤波器的imu姿态解算方法
CN110912535A (zh) 一种新型无先导卡尔曼滤波方法
CN112304309B (zh) 一种基于心动阵列的高超飞行器组合导航信息解算方法
Sadeghzadeh-Nokhodberiz et al. Particle filtering based gyroscope fault and attitude estimation with uncertain dynamics fusing camera information
Li et al. Nonlinearity analysis of measurement model for vision-based optical navigation system

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