CN114459472B - 一种容积卡尔曼滤波器和离散灰色模型的组合导航方法 - Google Patents
一种容积卡尔曼滤波器和离散灰色模型的组合导航方法 Download PDFInfo
- Publication number
- CN114459472B CN114459472B CN202210137511.2A CN202210137511A CN114459472B CN 114459472 B CN114459472 B CN 114459472B CN 202210137511 A CN202210137511 A CN 202210137511A CN 114459472 B CN114459472 B CN 114459472B
- Authority
- CN
- China
- Prior art keywords
- value
- volume
- gray model
- discrete gray
- calculating
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 45
- 239000011159 matrix material Substances 0.000 claims description 36
- 238000000354 decomposition reaction Methods 0.000 claims description 32
- 238000004364 calculation method Methods 0.000 claims description 19
- 238000005259 measurement Methods 0.000 claims description 11
- 238000004590 computer program Methods 0.000 claims description 7
- 238000011161 development Methods 0.000 claims description 7
- 230000004044 response Effects 0.000 claims description 3
- 230000010354 integration Effects 0.000 claims 1
- 230000007704 transition Effects 0.000 claims 1
- 230000001174 ascending effect Effects 0.000 abstract description 2
- 230000008569 process Effects 0.000 description 8
- 230000009286 beneficial effect Effects 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 238000009499 grossing Methods 0.000 description 2
- 238000013507 mapping Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 230000003068 static effect Effects 0.000 description 2
- 238000012546 transfer Methods 0.000 description 2
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 description 1
- KLDZYURQCUYZBL-UHFFFAOYSA-N 2-[3-[(2-hydroxyphenyl)methylideneamino]propyliminomethyl]phenol Chemical compound OC1=CC=CC=C1C=NCCCN=CC1=CC=CC=C1O KLDZYURQCUYZBL-UHFFFAOYSA-N 0.000 description 1
- 230000002159 abnormal effect Effects 0.000 description 1
- 238000013528 artificial neural network Methods 0.000 description 1
- 201000001098 delayed sleep phase syndrome Diseases 0.000 description 1
- 208000033921 delayed sleep phase type circadian rhythm sleep disease Diseases 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 230000006870 function Effects 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000001360 synchronised effect Effects 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
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
- G01C21/165—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 combined with non-inertial navigation instruments
-
- 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
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)
- Position Fixing By Use Of Radio Waves (AREA)
Abstract
本发明提供一种容积卡尔曼滤波器和离散灰色模型的组合导航方法，获取已经过解算的GNSS数据及原始IMU数据；根据状态维数初始化容积点和权重及相关参数矩阵；利用容积型卡尔曼滤波器初始化得到容积点误差；进行时间更新并进行载噪比判别；在载噪比低于第一预设值的卫星数量小于第二预设值的情况下，建立离散灰色模型并计算k+1时刻的高度预测值；基于k+1时刻的高度预测值进行状态更新、容积点更新，计算出全球卫星导航系统与惯性导航系统的组合导航结果。本发明能够降低传统CKF精度随系统上升而下降的误差，同时提高系统在GNSS数据缺失时的水平定位精度，使用DGM(1,1)在垂直通道进行辅助，抑制滤波器在高度定位结果时的误差，能在遮挡环境下更好地进行工作。
Description
技术领域
本发明涉及卫星定位技术领域，尤其涉及一种容积卡尔曼滤波器和离散灰色模型的组合导航方法。
背景技术
GNSS(全球导航卫星系统，Global Navigation Satellite System)/INS(惯性导航系统，Inertial Navigation System)组合导航系统，减缓了GNSS易受遮挡，INS误差随时间累积的问题，利用前者的高精度与后者的稳定性得到了精确且连续的定位结果，在各领域内得到了广泛的应用。目前解决城市环境下的遮挡问题很多集中增加额外的设备，如SLAM(simultaneous localization and mapping，同步定位与建图)、视觉传感器等的应用，或者使用神经网络预测遮挡数据，这无疑会增加系统的成本与计算量。随着MEMS(微机电系统,Micro-Electro-Mechanical System)器件的推广，需要一种简单可行的组合方案。在以松组合为框架的组合方式中。
发明内容
针对现有技术存在的问题，本发明提供一种容积卡尔曼滤波器和离散灰色模型的组合导航方法。
第一方面，本发明实施例提供一种容积卡尔曼滤波器和离散灰色模型的组合导航方法，包括：
获取已经过解算的GNSS数据及原始IMU数据；
根据状态维数初始化容积点和权重及相关参数矩阵；
利用容积型卡尔曼滤波器初始化得到容积点误差；
基于初始化的容积点误差及相关参数矩阵进行时间更新并进行载噪比判别；
在载噪比低于第一预设值的卫星数量小于第二预设值的情况下，建立离散灰色模型并利用所述离散灰色模型计算k+1时刻的高度预测值；
基于k+1时刻的高度预测值进行状态更新、容积点更新，计算出全球卫星导航系统与惯性导航系统的组合导航结果。
在一些实施方式中，所述相关参数矩阵，包括系统状态向量，预测协方差，系统噪声矩阵，量测噪声矩阵及系统转移矩阵。
在一些实施方式中，所述利用容积型卡尔曼滤波器初始化得到容积点误差，包括：
利用容积型卡尔曼滤波器初始化得到容积点误差
计算式如下：
其中，
为第i个容积点元素，i＝1，……N，N是容积点的数量＝2n，/>
为CKF预测得到的一步状态预测值；
将容积点误差
赋值给/>
在一些实施方式中，所述时间更新，包括：
基于初始化得到的所述相关参数矩阵及所述容积点误差，计算一步预测状态向量及一步预测状态协方差，并计算一步预测状态协方差与系统噪声矩阵的差值，对该差值做Cholesky分解，得到第一分解值
所述状态更新，包括：
基于一步预测状态向量及一步预测状态协方差，计算预测状态向量、预测状态向量协方差及量测预测向量，并计算预测状态向量协方差与量测噪声矩阵的差值，对该差值做Cholesky分解，得到第二分解值
所述容积点更新，包括：
基于第一分解值
第二分解值/>
更新容积点误差/>
在载噪比低于第一预设值的卫星数量小于第二预设值的情况下，将所述一步预测状态向量中的高度预测值替换为利用所述离散灰色模型计算的k+1时刻的高度预测值进行状态更新。
在一些实施方式中，所述基于第一分解值
第二分解值/>
更新容积点误差
采用如下计算式：
其中，
的值等于前一时刻的容积点误差值。
在一些实施方式中，所述建立离散灰色模型并利用所述离散灰色模型计算k+1时刻的预测值，包括：
取历史第一时长的GNSS数据构建初始数据序列，所述初始数据序列中的各列代表一组GNSS数据，将各行数据进行一次累加得到平滑数据序列；
基于所述平滑数据序列建立离散灰色模型DGM(1，1)，该离散灰色模型DGM(1，1)中的发展系数、灰色系数满足最小二乘形式；
根据发展系数、灰色系数及离散灰色模型的时间响应序列，计算k+1时刻的预测值。
在一些实施方式中，所述方法还包括：在载噪比低于第一预设值的卫星数量不小于第二预设值的情况下，直接进行状态更新、容积点更新，计算出全球卫星导航系统与惯性导航系统的组合导航结果。
第二方面，本发明实施例提供一种容积卡尔曼滤波器和离散灰色模型的组合导航装置，包括：
数据获取模块，用于获取已经过解算的GNSS数据及原始IMU数据；
第一初始化模块，用于根据状态维数初始化容积点和权重及相关参数矩阵；
第二初始化模块，用于利用容积型卡尔曼滤波器初始化得到容积点误差；
更新及判别模块，用于基于容积点误差及相关参数矩阵进行时间更新，并进行载噪比判别；
第一计算模块，用于在载噪比低于第一预设值的卫星数量小于第二预设值的情况下，建立离散灰色模型并利用所述离散灰色模型计算k+1时刻的高度预测值；
第二计算模块，用于基于k+1时刻的高度预测值进行状态更新、容积点更新，计算出全球卫星导航系统与惯性导航系统的组合导航结果。
第三方面，本发明实施例提供一种计算机可读存储介质，所述计算机可读存储介质上存储有计算机程序，所述计算机程序被一个或多个处理器执行时，实现如第一方面所述的容积卡尔曼滤波器和离散灰色模型的组合导航方法。
第四方面，本发明实施例提供一种电子设备，包括存储器和一个或多个处理器，所述存储器上存储有计算机程序，所述计算机程序被所述一个或多个处理器执行时实现如第一方面所述的容积卡尔曼滤波器和离散灰色模型的组合导航方法。
与现有技术相比，本发明的FLS与UKF相结合的组合导航方法至少能够带来如下有益效果：
本发明能够降低传统CKF精度随系统上升而下降的误差，同时提高系统在GNSS数据缺失时的水平定位精度，先验概率密度PDF限制了CKF的垂直精度，因此使用DGM(1，1)在垂直通道上进行了辅助，很好抑制了滤波器在高度定位结果时的误差，能在遮挡环境下更好地进行组合导航计算工作。
附图说明
为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对范围的限定。
图1是本发明实施例提供的容积卡尔曼滤波器和离散灰色模型的组合导航方法流程图；
图2是本发明实施例提供的容积卡尔曼滤波器和离散灰色模型的组合导航方法原理示意图；
图3是本发明实施例的方法所用数据的整段卫星信号载噪比变化图；
图4是本发明实施例提供的容积卡尔曼滤波器和离散灰色模型的组合导航装置框图。
具体实施方式
以下通过特定的具体实例说明本发明的实施方式，本领域技术人员可由本说明书所揭露的内容轻易地了解本发明的其他优点与功效。本发明还可以通过另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本发明的精神下进行各种修饰或改变。
需要说明的是，本实施例中所提供的图示仅以示意方式说明本发明的基本构想，遂图式中仅显示与本发明中有关的组件而非按照实际实施时的组件数目、形状及尺寸绘制，其实际实施时各组件的型态、数量及比例可为一种随意的改变，且其组件布局型态也可能更为复杂。
实施例一
本实施例提供一种容积卡尔曼滤波器和离散灰色模型的组合导航方法，如图1和图2所示，包括：
步骤S101、获取已经过解算的GNSS数据及原始IMU数据。
在一些实施方式中，通过矢量跟踪获取已经过解算的GNSS数据及原始IMU(惯性测量单元)数据，GNSS数据包括GNSS速度、位置及载噪比数据，原始IMU数据包括三轴陀螺仪及加速度计输出。
步骤S102、根据状态维数n初始化容积点e和权重w及相关参数矩阵。
在一些情况下，容积点e和权重ω的计算式如下：
其中，In×n为n维单位矩阵，本实施例中n取15。
相关参数矩阵包括系统状态向量Xk，预测协方差Pk，系统噪声矩阵Qk，量测噪声矩阵Rk及系统转移矩阵Fk，
Xk＝O15×1 (3)
Pk＝[davp9×1；wdb3×1] (4)
其中，O15×1为15维全零列向量，davp9×1为9维初始姿态、速度、位置误差列向量，web3×1为3维陀螺仪误差列向量，wdb3×1为3维加速度计误差列向量，可由数据手册阅得。
步骤S103、利用容积型卡尔曼滤波器(CKF)初始化得到容积点误差
在一些实施方式中，利用容积型卡尔曼滤波器初始化得到容积点误差，包括：
利用容积型卡尔曼滤波器初始化得到容积点误差
计算式如下：
其中，
为第i个容积点元素，i＝1，……N，N是容积点的数量＝2n，/>
为CKF预测得到的一步状态预测值；
并将初始化的容积点误差
赋值给/>
其中Pk-1，
由上一时刻的Pk，Xk代入，ei为第i个容积点。
步骤S104、基于初始化的容积点误差及相关参数矩阵进行时间更新并进行载噪比判别。
在一些实施方式中，时间更新过程，包括：
基于初始化得到的相关参数矩阵及容积点误差，计算一步预测状态向量及一步预测状态协方差；具体地，系统状态向量Xk(首次时间更新将步骤S102得到的Xk，其他时间是状态更新得到的Xk)、
Fk、Qk作为输入，得到一步预测状态向量Xk/k-1及一步预测状态协方差Pk/k-1，k表示当前时刻。
其中，f(·)为系统的非线性转换矩阵；
计算一步预测状态协方差Pk/k-1与系统噪声矩阵Qk的差值，对该差值做Cholesky分解，得到第一分解值
计算容积点误差
在一些情况下，第一预设值取40dB，第二预设值取4。通过输入的GNSS数据中的载噪比(CNO)对当前信号的受遮挡情况进行判别，当载噪比低于40dB的卫星数量小于4时，执行步骤S105。应当理解的是，载噪比低于40dB说明可能被遮挡。
步骤S105、在载噪比低于第一预设值的卫星数量小于第二预设值的情况下，建立离散灰色模型并利用离散灰色模型计算k+1时刻的高度预测值。
在一些实施方式中，建立离散灰色模型并利用离散灰色模型计算k+1时刻的预测值，包括：
步骤S105a、取历史第一时长(例如3秒)的GNSS数据(包括位置、速度)构建初始数据序列Z(0)，初始数据序列Z(0)中的各列代表一组GNSS数据，将各行数据进行一次累加(1-AGO)得到平滑数据序列Z(1)。
其中，L，λ，h分别为经解算得到的GNSS纬度、经度和高度，E，N，U分别为经解算得到的GNSS东向、北向和天向速度。各数据上标表示所属数据序列下标表示所取数据组编号。
步骤S105b、基于平滑数据序列Z(1)建立离散灰色模型DGM(1，1)，该离散灰色模型DGM(1，1)中的发展系数β1、灰色系数β2满足最小二乘形式。
定义DGM(1，1)的模型如下
z(1)(k+1)＝β1z(1)(k)+β2 (14)
其中β1为发展系数，β2为灰色指数，其满足如下最小二乘形式
其中
为参数序列[β1，β2]T，”T”为转置符号，Y矩阵及B矩阵的形式如下
步骤S105c、根据发展系数β1、灰色系数β2及离散灰色模型的时间响应序列，计算k+1时刻的预测值。
其中，z(1)(k+1)代表数据平滑序列k+1时刻的预测值，发展系数β1的上标k代表k次幂。
并通过k+1与k时刻数据平滑序列预测值的差值得到原始数据序列在k+1时刻的预测值。
步骤S106、基于k+1时刻的高度预测值进行状态更新、容积点更新，计算出全球卫星导航系统与惯性导航系统的组合导航结果。
状态更新过程，包括：
基于一步预测状态向量Xk/k-1及一步预测状态协方差Pk/k-1，预测状态向量Xk、状态向量协方差Pk及量测向量Zk，并计算预测的状态向量协方差Pk与量测噪声矩阵Rk的差值，对该差值做Cholesky分解，得到第二分解值
量测向量的协方差
计算Pk与Rk的差值并作Cholesky分解得到
容积点更新过程，包括：
式(10)所得的第一分解值
式(25)所得的第二分解值/>
更新容积点误差/>
将
的值赋予/>
在载噪比低于第一预设值的卫星数量小于第二预设值的情况下，将一步预测状态向量中的高度预测值替换为利用离散灰色模型计算的k+1时刻的高度预测值进行状态更新。具体的，将步骤S105c得到的原始数据序列在k+1时刻的预测值替代状态更新过程中的GNSS数据，计算全球卫星导航系统与惯性导航系统的组合导航结果。而在载噪比低于预设值的卫星数量不小于预设值的情况下，则直接进行状态更新、容积点更新，计算出全球卫星导航系统与惯性导航系统的组合导航结果。
检查数据是否全部计算，若未全部计算返回步骤S104；若全部计算则结束全过程。、
本实施例的方法在遮挡环境下提高了水平及垂直定位精度，即在卫星信号载噪比下降时对GNSS/INS数据进行组合提高水平定位精度，利用DGM(1.1)(离散灰色模型)预测高度值，代替异常值与INS数据进行组合，提高垂直定位精度缓解轨迹漂移问题。通过一步协方差与系统噪声、预测协方差与量测噪声分解，更新容积点的方式，预测得到的容积点具有比传统CKF更多的误差信息，能够有效提高系统的水平定位精度，在GNSS信号受遮挡时减少轨迹漂移现象，离散灰色模型能够抑制垂直通道上的波动，使系统在遮挡环境下准确地输出水平及垂直定位结果。
图3是本方法所用数据的整段卫星信号载噪比变化图。经实验验证，相较于KF，CKF，本方法分别在东北、北向及天向平均提升11.71％，15.16％及42.61％，证明该本方法中容积卡尔曼滤波器和离散灰色模型的组合导航结构在遮挡环境下的应用有效性。
实施例二
本实施例提供一种容积卡尔曼滤波器和离散灰色模型的组合导航装置，如图4所示，包括：
数据获取模块201，用于获取已经过解算的GNSS数据及原始IMU数据；
第一初始化模块202，用于根据状态维数初始化容积点和权重及相关参数矩阵；
第二初始化模块203，用于利用容积型卡尔曼滤波器初始化得到容积点误差；
更新及判别模块204，用于基于初始化的容积点误差及相关参数矩阵进行时间更新并进行载噪比判别；
第一计算模块205，用于在载噪比低于预设值的卫星数量小于预设值的情况下，建立离散灰色模型并利用离散灰色模型计算k+1时刻的高度预测值；
第二计算模块206，用于基于k+1时刻的高度预测值进行状态更新、容积点更新，计算出全球卫星导航系统与惯性导航系统的组合导航结果。
本实施例中的装置的各模块功能的实现内容可参照实施例一的具体内容，且具有实施例一的全部有益效果，此处不再赘述。
实施例三
本实施例提供一种计算机可读存储介质，计算机可读存储介质上存储有计算机程序，计算机程序被一个或多个处理器执行时，实现实施例一的MEMS-INS辅助GNSS矢量环路跟踪方法。
本实施例中，计算机可读存储介质可以由任何类型的易失性或非易失性存储设备或者它们的组合实现，例如静态随机存取存储器(Static Random Access Memory，简称SRAM)，电可擦除可编程只读存储器(Electrically Erasable Programmable Read-OnlyMemory，简称EEPROM)，可擦除可编程只读存储器(Erasable Programmable Read-OnlyMemory，简称EPROM)，可编程只读存储器(Programmable Read-Only Memory，简称PROM)，只读存储器(Read-Only Memory，简称ROM)，磁存储器，快闪存储器，磁盘或光盘。方法的内容详见实施例一，此次不再赘述。
实施例四
本实施例提供一种电子设备，包括存储器和一个或多个处理器，所述存储器上存储有计算机程序，计算机程序被一个或多个处理器执行时实现实施例一的MEMS-INS辅助GNSS矢量环路跟踪方法。
本实施例中，处理器可以是专用集成电路(Application Specific IntegratedCircuit，简称ASIC)、数字信号处理器(Digital Signal Processor，简称DSP)、数字信号处理设备(Digital Signal Processing Device，简称DSPD)、可编程逻辑器件(ProgrammableLogic Device，简称PLD)、现场可编程门阵列(Field Programmable Gate Array，简称FPGA)、控制器、微控制器、微处理器或其他电子元件实现，用于执行上述实施例中的方法。在处理器上运行的计算机程序被执行时所实现的方法可参照本发明前述实施例提供的方法的具体实施例，此处不再赘述。在实际应用中，电子设备可以是提供计算服务的设备。
在本发明实施例所提供的实施例中，应该理解到，所揭露的装置、方法，也可以通过其它的方式实现。以上所描述的系统实施例仅仅是示意性的。
需要说明的是，在本文中，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。
上述实施例仅例示性说明本发明的原理及其功效，而非用于限制本发明。任何熟悉此技术的人士皆可在不违背本发明的精神及范畴下，对上述实施例进行修饰或改变。因此，举凡所属技术领域中具有通常知识者在未脱离本发明所揭示的精神与技术思想下所完成的一切等效修饰或改变，仍应由本发明的权利要求所涵盖。
Claims (8)
1.一种容积卡尔曼滤波器和离散灰色模型的组合导航方法，其特征在于，包括：
获取已经过解算的GNSS数据及原始IMU数据；
根据状态维数初始化容积点和权重及相关参数矩阵；
利用容积型卡尔曼滤波器初始化得到容积点误差；
基于初始化的容积点误差及相关参数矩阵进行时间更新并进行载噪比判别；
在载噪比低于第一预设值的卫星数量小于第二预设值的情况下，建立离散灰色模型并利用所述离散灰色模型计算k+1时刻的高度预测值；
基于k+1时刻的高度预测值进行状态更新、容积点更新，计算出全球卫星导航系统与惯性导航系统的组合导航结果；
所述时间更新，包括：
基于初始化得到的所述相关参数矩阵及所述容积点误差，计算一步预测状态向量及一步预测状态协方差，并计算一步预测状态协方差与系统噪声矩阵的差值，对该差值做Cholesky分解，得到第一分解值
所述状态更新，包括：
基于一步预测状态向量及一步预测状态协方差，计算预测状态向量、预测状态向量协方差及量测预测向量，并计算预测状态向量协方差与量测噪声矩阵的差值，对该差值做Cholesky分解，得到第二分解值
所述容积点更新，包括：
基于第一分解值
第二分解值/>
更新容积点误差/>
在载噪比低于第一预设值的卫星数量小于第二预设值的情况下，将一步预测状态向量中的高度预测值替换为利用所述离散灰色模型计算的k+1时刻的高度预测值进行状态更新；
所述基于第一分解值
第二分解值/>
更新容积点误差/>
采用如下计算式：
其中，
的值等于前一时刻的容积点误差值。
2.根据权利要求1所述的容积卡尔曼滤波器和离散灰色模型的组合导航方法，其特征在于，所述相关参数矩阵，包括系统状态向量，预测协方差，系统噪声矩阵，量测噪声矩阵及系统转移矩阵。
3.根据权利要求1所述的容积卡尔曼滤波器和离散灰色模型的组合导航方法，其特征在于，所述利用容积型卡尔曼滤波器初始化得到容积点误差，包括：
利用容积型卡尔曼滤波器初始化得到容积点误差
计算式如下：
其中，
为第i个容积点元素，i＝1，……N，N是容积点的数量＝2n，/>
为CKF预测得到的一步状态预测值；
将容积点误差
赋值给/>
4.根据权利要求1所述的容积卡尔曼滤波器和离散灰色模型的组合导航方法，其特征在于，所述建立离散灰色模型并利用所述离散灰色模型计算k+1时刻的预测值，包括：
取历史第一时长的GNSS数据构建初始数据序列，所述初始数据序列中的各列代表一组GNSS数据，将各行数据进行一次累加得到平滑数据序列；
基于所述平滑数据序列建立离散灰色模型DGM(1,1)，该离散灰色模型DGM(1,1)中的发展系数、灰色系数满足最小二乘形式；
根据发展系数、灰色系数及离散灰色模型的时间响应序列，计算k+1时刻的预测值。
5.根据权利要求1所述的容积卡尔曼滤波器和离散灰色模型的组合导航方法，其特征在于，还包括：在载噪比低于第一预设值的卫星数量不小于第二预设值的情况下，直接进行状态更新、容积点更新，计算出全球卫星导航系统与惯性导航系统的组合导航结果。
6.一种容积卡尔曼滤波器和离散灰色模型的组合导航装置，其特征在于，包括：
数据获取模块，用于获取已经过解算的GNSS数据及原始IMU数据；
第一初始化模块，用于根据状态维数初始化容积点和权重及相关参数矩阵；
第二初始化模块，用于利用容积型卡尔曼滤波器初始化得到容积点误差；
更新及判别模块，用于基于初始化的容积点误差及相关参数矩阵进行时间更新并进行载噪比判别；
第一计算模块，用于在载噪比低于第一预设值的卫星数量小于第二预设值的情况下，建立离散灰色模型并利用所述离散灰色模型计算k+1时刻的高度预测值；
第二计算模块，用于基于k+1时刻的高度预测值进行状态更新、容积点更新，计算出全球卫星导航系统与惯性导航系统的组合导航结果；
所述时间更新，包括：
基于初始化得到的所述相关参数矩阵及所述容积点误差，计算一步预测状态向量及一步预测状态协方差，并计算一步预测状态协方差与系统噪声矩阵的差值，对该差值做Cholesky分解，得到第一分解值
所述状态更新，包括：
基于一步预测状态向量及一步预测状态协方差，计算预测状态向量、预测状态向量协方差及量测预测向量，并计算预测状态向量协方差与量测噪声矩阵的差值，对该差值做Cholesky分解，得到第二分解值
所述容积点更新，包括：
基于第一分解值
第二分解值/>
更新容积点误差/>
在载噪比低于第一预设值的卫星数量小于第二预设值的情况下，将一步预测状态向量中的高度预测值替换为利用所述离散灰色模型计算的k+1时刻的高度预测值进行状态更新；
所述基于第一分解值
第二分解值/>
更新容积点误差/>
采用如下计算式：
其中，
的值等于前一时刻的容积点误差值。
7.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质上存储有计算机程序，所述计算机程序被一个或多个处理器执行时，实现如权利要求1至5中任一项所述的容积卡尔曼滤波器和离散灰色模型的组合导航方法。
8.一种电子设备，其特征在于，包括存储器和一个或多个处理器，所述存储器上存储有计算机程序，所述计算机程序被所述一个或多个处理器执行时实现如权利要求1至5中任一项所述的容积卡尔曼滤波器和离散灰色模型的组合导航方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210137511.2A CN114459472B (zh) | 2022-02-15 | 2022-02-15 | 一种容积卡尔曼滤波器和离散灰色模型的组合导航方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210137511.2A CN114459472B (zh) | 2022-02-15 | 2022-02-15 | 一种容积卡尔曼滤波器和离散灰色模型的组合导航方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114459472A CN114459472A (zh) | 2022-05-10 |
| CN114459472B true CN114459472B (zh) | 2023-07-04 |
Family
ID=81413026
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210137511.2A Active CN114459472B (zh) | 2022-02-15 | 2022-02-15 | 一种容积卡尔曼滤波器和离散灰色模型的组合导航方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114459472B (zh) |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106885569A (zh) * | 2017-02-24 | 2017-06-23 | 南京理工大学 | 一种强机动条件下的弹载深组合arckf滤波方法 |
| CN109000642A (zh) * | 2018-05-25 | 2018-12-14 | 哈尔滨工程大学 | 一种改进的强跟踪容积卡尔曼滤波组合导航方法 |
| CN114018262A (zh) * | 2021-10-25 | 2022-02-08 | 南宁桂电电子科技研究院有限公司 | 一种改进的衍生容积卡尔曼滤波组合导航方法 |
Family Cites Families (17)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US9217643B1 (en) * | 2009-01-08 | 2015-12-22 | Trex Enterprises Corp. | Angles only navigation system |
| CN103885386A (zh) * | 2014-04-08 | 2014-06-25 | 北京工业大学 | 一种基于卡尔曼滤波的灰色模型热误差数据处理方法 |
| US10114464B2 (en) * | 2015-06-30 | 2018-10-30 | Stmicroelectronics S.R.L. | Device and method for determination of angular position in three-dimensional space, and corresponding electronic apparatus |
| US20170108612A1 (en) * | 2015-10-15 | 2017-04-20 | King Saud University | Inertial system for gravity difference measurement |
| CN105509739B (zh) * | 2016-02-04 | 2018-04-06 | 济南大学 | 采用固定区间crts平滑的ins/uwb紧组合导航系统及方法 |
| EP3532869A4 (en) * | 2016-10-26 | 2020-06-24 | The Charles Stark Draper Laboratory, Inc. | VISION INNER NAVIGATION WITH TRACKING REMAINING WITH VARIABLE CONTRAST |
| CN107314768B (zh) * | 2017-07-06 | 2020-06-09 | 上海海洋大学 | 水下地形匹配辅助惯性导航定位方法及其定位系统 |
| CN107643534B (zh) * | 2017-09-11 | 2019-07-12 | 东南大学 | 一种基于gnss/ins深组合导航的双速率卡尔曼滤波方法 |
| CN109000640B (zh) * | 2018-05-25 | 2021-09-28 | 东南大学 | 基于离散灰色神经网络模型的车辆gnss/ins组合导航方法 |
| CN108645415A (zh) * | 2018-08-03 | 2018-10-12 | 上海海事大学 | 一种船舶航迹预测方法 |
| US11808867B2 (en) * | 2019-04-15 | 2023-11-07 | The Regents Of The University Of California | Simultaneous tracking and navigation using LEO satellite signals |
| CN110260858A (zh) * | 2019-06-25 | 2019-09-20 | 哈尔滨工程大学 | 一种基于最优阶数灰色自适应动态滤波的航迹跟踪方法 |
| CN110567455B (zh) * | 2019-09-25 | 2023-01-03 | 哈尔滨工程大学 | 一种求积更新容积卡尔曼滤波的紧组合导航方法 |
| CN110780326A (zh) * | 2019-09-26 | 2020-02-11 | 上海瀚所信息技术有限公司 | 一种车载组合导航系统和定位方法 |
| CN111965683A (zh) * | 2020-08-19 | 2020-11-20 | 上海海事大学 | 一种卫星定位和惯导定位的组合定位方法、装置及存储介质 |
| CN112748450A (zh) * | 2020-12-30 | 2021-05-04 | 上海海事大学 | 一种导航卫星接收机的信号载波环跟踪方法 |
| CN112985388B (zh) * | 2021-02-08 | 2022-08-19 | 福州大学 | 基于大位移光流法的组合导航方法及系统 |
-
2022
- 2022-02-15 CN CN202210137511.2A patent/CN114459472B/zh active Active
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106885569A (zh) * | 2017-02-24 | 2017-06-23 | 南京理工大学 | 一种强机动条件下的弹载深组合arckf滤波方法 |
| CN109000642A (zh) * | 2018-05-25 | 2018-12-14 | 哈尔滨工程大学 | 一种改进的强跟踪容积卡尔曼滤波组合导航方法 |
| CN114018262A (zh) * | 2021-10-25 | 2022-02-08 | 南宁桂电电子科技研究院有限公司 | 一种改进的衍生容积卡尔曼滤波组合导航方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114459472A (zh) | 2022-05-10 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Georgy et al. | Enhanced MEMS-IMU/odometer/GPS integration using mixture particle filter | |
| CN111156987B (zh) | 基于残差补偿多速率ckf的惯性/天文组合导航方法 | |
| Ryu et al. | Navigation system heading and position accuracy improvement through GPS and INS data fusion | |
| CN109000640B (zh) | 基于离散灰色神经网络模型的车辆gnss/ins组合导航方法 | |
| CN112146655B (zh) | 一种BeiDou/SINS紧组合导航系统弹性模型设计方法 | |
| CN106772524A (zh) | 一种基于秩滤波的农业机器人组合导航信息融合方法 | |
| CN111694040B (zh) | 一种卫星/惯性组合导航系统定位方法与装置 | |
| CN112798021B (zh) | 基于激光多普勒测速仪的惯导系统行进间初始对准方法 | |
| CN101871782A (zh) | 基于set2fnn的gps/mems-ins组合导航系统定位误差预测方法 | |
| CN111795708B (zh) | 晃动基座条件下陆用惯性导航系统的自适应初始对准方法 | |
| CN114689047A (zh) | 基于深度学习的组合导航方法、装置、系统及存储介质 | |
| CN111750865A (zh) | 一种用于双功能深海无人潜器导航系统的自适应滤波导航方法 | |
| CN116007620A (zh) | 一种组合导航滤波方法、系统、电子设备及存储介质 | |
| CN111832690B (zh) | 基于粒子群优化算法的惯导系统的陀螺测量值计算方法 | |
| Nagui et al. | Improved GPS/IMU loosely coupled integration scheme using two kalman filter-based cascaded stages | |
| CN113551666A (zh) | 自动驾驶多传感器融合定位方法和装置、设备及介质 | |
| CN113432612B (zh) | 飞行体的导航方法、装置及系统 | |
| CN114459472B (zh) | 一种容积卡尔曼滤波器和离散灰色模型的组合导航方法 | |
| Yu et al. | Data fusion for a GPS/INS tightly coupled positioning system with equality and inequality constraints using an aggregate constraint unscented Kalman filter | |
| Park et al. | Grid support adaptation for point mass filter based terrain referenced navigation using mutual information | |
| CN114858166B (zh) | 基于最大相关熵卡尔曼滤波器的imu姿态解算方法 | |
| Wang et al. | A novel robust iterated CKF for GNSS/SINS integrated navigation applications | |
| CN114897942A (zh) | 点云地图的生成方法、设备及相关存储介质 | |
| Mok et al. | Terrain slope estimation methods using the least squares approach for terrain referenced navigation | |
| Nassar et al. | Land-vehicle INS/GPS accurate positioning during GPS signal blockage periods |
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 |