CN105509739B - 采用固定区间crts平滑的ins/uwb紧组合导航系统及方法 - Google Patents
采用固定区间crts平滑的ins/uwb紧组合导航系统及方法 Download PDFInfo
- Publication number
- CN105509739B CN105509739B CN201610078964.7A CN201610078964A CN105509739B CN 105509739 B CN105509739 B CN 105509739B CN 201610078964 A CN201610078964 A CN 201610078964A CN 105509739 B CN105509739 B CN 105509739B
- Authority
- CN
- China
- Prior art keywords
- mrow
- mtd
- msubsup
- msub
- mtr
- 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
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
- 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)
- Other Investigation Or Analysis Of Materials By Electrical Means (AREA)
- Navigation (AREA)
Abstract
本发明公开了一种采用固定区间CRTS平滑的INS/UWB紧组合导航系统及方法，包括：惯性导航器件INS和UWB无线标签分别设置在行人的鞋子上，UWB无线参考节点和参考系统分别设置在设定位置，惯性导航器件INS、UWB无线标签和参考系统分别与数据处理系统连接；数据处理系统包括：局部数据融合滤波器、容积卡尔曼滤波器、伪距数据处理模块、RTS平滑模块和均值滤波模块；本发明有益效果：有效的降低的传统紧组合导航模型中忽略泰勒展开式的高次项可能引入截断误差的可能性。
Description
技术领域
本发明涉及复杂环境下组合定位技术领域，尤其涉及一种采用固定区间CRTS平滑的INS/UWB紧组合导航系统及方法。
背景技术
近年来，行人导航(Pedestrian Navigation,PN)作为导航技术应用的新兴领域，正越来越受到各国学者的重视，并逐渐成为该领域的研究热点。然而在隧道、大型仓库、地下停车场等室内环境下，外界无线电信号微弱、电磁干扰强烈等因素都会对目标行人导航信息获取的准确性、实时性及鲁棒性有很大影响。如何将室内环境下获取的有限信息进行有效的融合以消除室内复杂环境影响，保证行人导航精度的持续稳定，具有重要的科学理论意义和实际应用价值。
在现有的定位方式中，全球卫星导航系统(Global Navigation SatelliteSystem,GNSS)是最为常用的一种方式。虽然GNSS能够通过精度持续稳定的位置信息，但是其易受电磁干扰、遮挡等外界环境影响的缺点限制了其应用范围，特别是在室内、地下巷道等一些密闭的、环境复杂的场景，GNSS信号被严重遮挡，无法进行有效的工作。近年来，UWB以其在复杂环境下定位精度高的特点在短距离局部定位领域表现出很大的潜力。学者们提出将基于UWB的目标跟踪应用于GNSS失效环境下的行人导航。这种方式虽然能够实现室内定位，但是由于室内环境复杂多变，UWB信号十分容易受到干扰而导致定位精度下降甚至失锁；与此同时，由于UWB采用的通信技术通常为短距离无线通信技术，因此若想完成大范围的室内目标跟踪定位，需要大量的网络节点共同完成，这必将引入网络组织结构优化设计、多节点多簇网络协同通信等一系列问题。因此现阶段基于UWB的目标跟踪在室内导航领域仍旧面临很多挑战。
在导航模型方面，目前在室内行人组合导航领域应用较多的为松组合导航模型。该模型具有容易实现的优点，但是需要指出的是，该模型的实现需要参与组合导航的多种技术能够独立的完成导航定位。例如，需要UWB设备能够提供行人的导航信息，这就要求目标行人所处的环境必须能够获取至少3个参考节点信息，这大大的降低的组合导航模型的应用范围，与此同时，参与导航的子技术独立完成定位，也引入了新的误差，不利于组合导航技术精度的提高。为了克服这一问题，学者们提出将紧组合模型应用于室内行人导航领域，紧组合模型直接将参与组合导航的子技术的原始传感器数据应用于最后的导航信息的解算，减少了子技术自行解算引入新误差的风险，提高了组合导航的精度，但是需要指出的是，传统的紧组合导航模型都是建立在一阶泰勒展开的基础上的，这不可避免的引入了系统的截断误差，这在普遍采用低成本传感器的行人导航领域，严重影响了组合导航系统的精度。
发明内容
本发明的目的就是为了解决上述难题，提供了一种采用固定区间CRTS平滑的INS/UWB紧组合导航系统及方法，采用改进的INS/UWB紧组合模型，该模型以INS和UWB分别测量得到的伪距的平方之差作为系统观测量；在此基础上，通过容积卡尔曼滤波(CKF)将INS和UWB获取的导航信息进行数据融合；当到达输出时间时，将CKF得到的上一输出时间到现输出时间之间的INS误差预估先通过RTS平滑之后再进行均值滤波，最后将数据输出，与当前时刻INS解算的导航信息做差，最终得到当前时刻最优的导航信息。该系统及方法能满足室内行人导航的中高精度定位和定向的要求。
为实现上述目的，本发明的具体方案如下：
一种采用固定区间CRTS平滑的INS/UWB紧组合导航系统，包括：
惯性导航器件INS、UWB无线标签、UWB无线参考节点、参考系统和数据处理系统；
所述惯性导航器件INS和UWB无线标签分别设置在行人的鞋子上，所述UWB无线参考节点和参考系统分别设置在设定位置，所述惯性导航器件INS、UWB无线标签和参考系统分别与数据处理系统连接；
所述数据处理系统包括：局部数据融合滤波器、容积卡尔曼滤波器、伪距数据处理模块、RTS平滑模块和均值滤波模块；
所述惯性导航器件INS与局部数据融合滤波器连接，所述惯性导航器件INS和UWB无线标签分别与伪距数据处理模块连接，伪距数据处理模块的输出连接所述容积卡尔曼滤波器；所述容积卡尔曼滤波器的输出依次连接RTS平滑模块和均值滤波模块后与减法器的一个输入端连接；所述局部数据融合滤波器的输出与减法器的另一个输入端连接。
进一步地，所述伪距数据处理模块求取惯性导航器件INS和UWB无线标签测量得到的伪距的平方之差。
进一步地，所述参考系统包括码盘和电子罗盘。
一种采用固定区间CRTS平滑的INS/UWB紧组合导航方法，包括：
(1)将人的运行状态分为静止和运动两个状态，通过惯性导航器件INS对行人的当前运动状态进行判断；
(2)在鞋子处于静止状态时，通过局部数据融合滤波器对惯性导航器件INS解算的导航信息误差进行预估和补偿，得到当前时刻惯性导航器件INS解算的位置、速度和姿态的导航信息的最优估计；
(3)以惯性导航器件INS的误差向量作为状态量，以惯性导航器件INS和UWB无线标签分别测量得到的伪距的平方之差作为系统观测量，构建INS/UWB紧组合模型；
(4)通过容积卡尔曼滤波器将惯性导航器件INS和UWB无线标签在本地相对坐标系中获取的导航信息进行数据融合；
(5)当到达输出时间时，将容积卡尔曼滤波器得到的上一输出时间到现输出时间之间的INS误差预估通过RTS平滑之后再进行均值滤波处理；
(6)将经过均值滤波处理后的数据与当前时刻INS解算的导航信息做差，最终得到当前时刻最优的导航信息。
进一步地，所述步骤(1)中，通过惯性导航器件INS对行人的当前运动状态进行判断的方法为：
通过惯性导航器件INS自身携带的3个加速度传感器采集到的数据，得到当前时刻行人的加速度模值，通过加速度模值的大小判断鞋子的运动状态：
其中，加速度模值：ax、ay、az分别为在载体坐标系下的x、y和z三个方向的加速度值。
进一步地，所述步骤(2)中，通过局部数据融合滤波器对惯性导航器件INS解算的导航信息误差进行预估和补偿；
局部数据融合滤波器的状态方程为：
其中，为12维INS误差向量； 为惯性导航器件INS测量得到的导航坐标系下的东向、北向和天向三个方向的加速度误差；φZ,k、φZ,k+1分别为k时刻和k+1时刻惯性导航器件INS测量得到的导航坐标系下的纵摇、横摇和航向角误差；
分别为k时刻和k+1时刻惯性导航器件INS测量得到的导航坐标系下的东向、北向和天向三个方向的速度误差；分别为k时刻和k+1时刻导航坐标系下的东向、北向和天向三个方向的加速度误差；分别为k时刻和k+1时刻导航坐标系下的东向、北向和天向三个方向的角速度误差；ωZ,k为状态噪声；T为采样周期；I为单位矩阵；为从载体系到导航系的状态转移矩阵，其中(γ,θ,ψ)分别为纵摇、横摇和航向角；ωZ,k为状态噪声；T为采样周期；I为单位矩阵；
局部数据融合滤波器的观测方程为：
其中，为INS速度误差观测值；为INS解算的速度值；ηZ,k为观测噪声；I为单位矩阵。
进一步地，所述步骤(3)中，INS/UWB紧组合模型的状态方程为：
其中，为15维INS误差向量； 为惯性导航器件INS测量得到的导航坐标系下的东向、北向和天向三个方向的加速度误差；φk、φk+1分别为k时刻和k+1时刻惯性导航器件INS测量得到的导航坐标系下的横摇、纵摇和航向角误差；
分别为k时刻和k+1时刻INS测量得到的导航坐标系下的东向、北向和天向三个方向的速度误差；分别为k时刻和k+1时刻导航坐标系下的东向、北向和天向三个方向的加速度误差；分别为k时刻和k+1时刻导航坐标系下的东向、北向和天向三个方向的角速度误差；分别为k时刻和k+1时刻导航坐标系下的东向、北向和天向三个方向的位置误差；ωk为状态噪声；T为采样周期；I为单位矩阵；为从载体系到导航系的状态转移矩阵。
进一步地，所述步骤(3)中，INS/UWB紧组合模型的观测方程为：
其中，为k时刻INS解算的东向和北向位置；为k时刻INS解算的东向和北向位置误差；为k时刻INS测量得到的未知节点到第i个参考节点之间的伪距；为k时刻UWB测量得到的未知节点到第i个参考节点之间的伪距；为k时刻UWB测量得到的未知节点到第i个参考节点之间的伪距的噪声；为第i个参考节点的坐标；为INS和UWB分别测量的未知节点到参考节点之间的伪距的平方之差。
进一步地，所述步骤(4)的方法具体为：
a.对容积卡尔曼滤波器接收到的数据进行cubature采样，得到基本容积点及其对应的权值；
b.时间更新，根据第i个cubature点在k-1时刻的状态变量计算通过非状态方程传播的容积点；根据状态方程传播的容积点计算预测状态和预测方差；
c.对得到的预测方差进行因式分解，根据得到的分解矩阵和预测状态计算第i个cubature点在k时刻的状态变量；
d.根据计算的第i个cubature点在k时刻的状态变量计算通过非线性量测方程传播的容积点；
通过非线性量测方程传播的容积点计算量测预测值、新息方差和协方差估计；
e.计算引入渐消因子后的状态预测协方差阵；通过非线性测量函数重新传播可重新计算引入渐消因子后的输出预测自协方差阵和互协方差阵增益矩阵；
f.在获得新的量测值后，进行滤波更新，得到k时刻的状态向量的最优估计。
进一步地，所述步骤(5)中，对容积卡尔曼滤波器得到的上一输出时间到现输出时间之间的INS误差预估进行RTS平滑的具体方法为：
分别求取逆向平滑过程中的状态变量和逆向平滑过程中的滤波器增益；根据上述值求取逆向平滑过程中的概率。
本发明的有益效果：
1、组合导航方法采用改进的INS/UWB紧组合模型，该模型以INS和UWB分别测量得到的伪距的平方之差作为系统观测量；有效的降低的传统紧组合导航模型中忽略泰勒展开式的高次项可能引入截断误差的可能性。
2、通过容积卡尔曼滤波(CKF)将INS和UWB获取的导航信息进行数据融合；当到达输出时间时，将CKF得到的上一输出时间到现输出时间之间的INS误差预估先通过RTS平滑之后再进行均值滤波，最后将数据输出，与当前时刻INS解算的导航信息做差，最终得到当前时刻最优的导航信息。
3、可用于室内环境下的智能鞋子的中高精度无缝定位和定向。
附图说明
图1为采用固定区间CRTS平滑的INS/UWB紧组合导航的系统结构示意图。
图2为采用固定区间CRTS平滑的INS/UWB紧组合导航方法过程示意图。
具体实施方式：
下面结合附图对本发明进行详细说明：
如图1所示，用于一种采用固定区间CRTS平滑的INS/UWB紧组合导航的方法的系统，包括：惯性导航器件INS(即足部IMU)、UWB无线标签、UWB无线参考节点、参考系统和数据处理系统(即计算机)；参考系统包括码盘和电子罗盘。
惯性导航器件INS和UWB无线标签分别设置在行人的鞋子上，UWB无线参考节点和参考系统分别设置在设定位置，惯性导航器件INS、UWB无线标签和参考系统分别与数据处理系统连接；
数据处理系统包括：局部数据融合滤波器、容积卡尔曼滤波器、伪距数据处理模块、RTS平滑模块和均值滤波模块；
惯性导航器件INS与局部数据融合滤波器连接，惯性导航器件INS和UWB无线标签分别与伪距数据处理模块连接，伪距数据处理模块的输出连接所述容积卡尔曼滤波器；所述容积卡尔曼滤波器的输出依次连接RTS平滑模块和均值滤波模块后与减法器的一个输入端连接；所述局部数据融合滤波器的输出与减法器的另一个输入端连接。
伪距数据处理模块求取惯性导航器件INS和UWB无线标签测量得到的伪距的平方之差。
如图2所示，在一种采用固定区间CRTS平滑的INS/UWB紧组合导航的方法使用卡尔曼滤波器完成系统的数据融合，具体步骤如下：
一种采用固定区间CRTS平滑的INS/UWB紧组合导航的方法，属于复杂环境下组合定位技术领域，其特征是，包括以下步骤：
(1)将惯性导航器件(INS)与UWB无线标签模块安装在行人的鞋上，将UWB无线参考节点模块固定在已知位置；
(2)将人的运行状态分为静止和运动两个状态，通过惯性导航器件INS对行人的当前运动状态进行判断；在鞋子处于静止状态时，通过局部数据融合滤波器对INS解算的导航信息误差进行预估和补偿，得到当前时刻INS解算的位置、速度和姿态的导航信息的最优估计；
其中，惯性导航器件INS解算的导航信息误差包括姿态误差、位置误差、速度误差、加速度计误差和陀螺仪误差。
通过足部IMU自身携带的3个加速度传感器采集到的数据，得到当前时刻行人的加速度模值，通过加速度模值的大小判断鞋子的运动状态：
其中，加速度模值：ax、ay、az分别为在载体坐标系下的x、y和z三个方向的加速度值。
通过局部数据融合滤波器对导航信息误差进行预估，局部数据融合滤波器的状态方程为：
其中，为12维INS误差向量； 为惯性导航器件INS测量得到的导航坐标系下的东向、北向和天向三个方向的加速度误差；φZ,k、φZ,k+1分别为k时刻和k+1时刻惯性导航器件INS测量得到的导航坐标系下的纵摇、横摇和航向角误差；
分别为k时刻和k+1时刻惯性导航器件INS测量得到的导航坐标系下的东向、北向和天向三个方向的速度误差；分别为k时刻和k+1时刻导航坐标系下的东向、北向和天向三个方向的加速度误差；分别为k时刻和k+1时刻导航坐标系下的东向、北向和天向三个方向的角速度误差；ωZ,k为状态噪声；T为采样周期；I为单位矩阵；为从载体系到导航系的状态转移矩阵，其中(γ,θ,ψ)分别为纵摇、横摇和航向角；ωZ,k为状态噪声；T为采样周期；I为单位矩阵；
局部数据融合滤波器的观测方程为：
其中，为INS速度误差观测值；为INS解算的速度值；ηZ,k为观测噪声；I为单位矩阵。
(3)该组合导航方法采用改进的INS/UWB紧组合模型，该组合导航模型以15维INS误差向量作为状态量，以INS和UWB分别测量得到的伪距的平方之差作为系统观测量，并细化了组合观测噪声；
INS/UWB紧组合模型的状态方程为：
其中，为15维INS误差向量； 为惯性导航器件INS测量得到的导航坐标系下的东向、北向和天向三个方向的加速度误差；φk、φk+1分别为k时刻和k+1时刻惯性导航器件INS测量得到的导航坐标系下的横摇、纵摇和航向角误差；
分别为k时刻和k+1时刻INS测量得到的导航坐标系下的东向、北向和天向三个方向的速度误差；分别为k时刻和k+1时刻导航坐标系下的东向、北向和天向三个方向的加速度误差；分别为k时刻和k+1时刻导航坐标系下的东向、北向和天向三个方向的角速度误差；分别为k时刻和k+1时刻导航坐标系下的东向、北向和天向三个方向的位置误差；ωk为状态噪声；T为采样周期；I为单位矩阵；为从载体系到导航系的状态转移矩阵。
其中(γ,θ,ψ)分别为纵摇、横摇和航向角。
INS/UWB紧组合模型的观测方程为：
其中，为k时刻INS解算的东向和北向位置；为k时刻INS解算的东向和北向位置误差；为k时刻INS测量得到的未知节点到第i个参考节点之间的伪距；为k时刻UWB测量得到的未知节点到第i个参考节点之间的伪距；为k时刻UWB测量得到的未知节点到第i个参考节点之间的伪距的噪声；为第i个参考节点的坐标；为INS和UWB分别测量的未知节点到参考节点之间的伪距的平方之差。
(4)通过容积卡尔曼滤波(Cubature Kalman filter，CKF)将INS、UWB在本地相对坐标系中获取的导航信息进行数据融合；具体方法为：
a.对容积卡尔曼滤波器接收到的数据进行cubature采样，得到基本容积点及其对应的权值；
使用三阶cubature规则获得如下的基本容积点ξ和相应权值；
式中：i＝1，2，…，m，m表示cubature采样点个数，使用三阶cubature规则，cubature采样点总数是状态维数的2倍，即m＝2nx，nx为状态向量维数。[1]i表示点集中在[1]的第i个点，其中符号[1]表示完整全对称点集，表示对nx维单位向量e＝[1 0 0 0 0 00 0 0 0 0 0 0 0 0]T的元素进行全排列和改变元素符号所产生的点集；
b.时间更新，根据第i个cubature点在k-1时刻的状态变量计算通过非状态方程传播的容积点；根据状态方程传播的容积点计算预测状态和预测方差；
其中为k-1时刻的状态估计值，Sk-1为状态向量协方差矩阵的平方根形式，由求得；Xi,k-1为第i个cubature点在k-1时刻的状态变量；
计算通过非线性状态方程传播的容积点
计算预测状态和预测方差
其中，为引入渐消因子前的状态预测协方差阵，Qk-1为系统噪声的协方差阵。
c.对得到的预测方差进行因式分解，根据得到的分解矩阵和预测状态计算第i个cubature点在k时刻的状态变量；
分解因式
其中，chol(·)为求矩阵·的Cholesky分解矩阵；
计算cubature点
d.根据计算的第i个cubature点在k时刻的状态变量计算通过非线性量测方程传播的容积点；
通过非线性量测方程传播的容积点计算量测预测值、新息方差和协方差估计；
计算通过非线性量测方程传播的容积点
Zi,k＝h(Xi,k) (9)
计算量测预测值、新息方差和协方差估计
记为引入渐消因子前的输出预测协方差阵，为引入渐消因子前的互协方差阵。
计算渐消因子λk+1，计算引入渐消因子后的状态预测协方差阵
将Pk/k-1代替式(6)中的再利用和Pk/k-1按照式(8)-(12)重新计算cubature点，通过非线性测量函数重新传播可重新计算引入渐消因子后的输出预测自协方差阵Pzz,k和互协方差阵增益矩阵Pxz,k。
e.量测更新
在获得新的量测值后，利用以下公式进行滤波更新
Kk＝Pxz,k/Pzz,k (13)
根据上述计算结果得到k时刻的状态向量的最优估计。
(5)当到达输出时间时，将CKF得到的上一输出时间到现输出时间之间的INS误差预估先通过RTS平滑之后再进行均值滤波，最后将数据输出，与当前时刻INS解算的导航信息做差，最终得到当前时刻最优的导航信息。
当到达输出时间时，将CKF得到的上一输出时间到现输出时间之间的INS误差预估先通过RTS平滑，即在对式(14)和(15)得到的状态预估做逆向平滑，其步骤为：
其中，为逆向平滑过程中的k时刻的状态变量；为逆向平滑过程中的k+1时刻的状态变量；为逆向平滑过程中的滤波器增益，为逆向平滑过程中的概率；
将RTS平滑之后的INS误差预估做平均。
上述虽然结合附图对本发明的具体实施方式进行了描述，但并非对本发明保护范围的限制，所属领域技术人员应该明白，在本发明的技术方案的基础上，本领域技术人员不需要付出创造性劳动即可做出的各种修改或变形仍在本发明的保护范围以内。
Claims (8)
1.一种采用固定区间CRTS平滑的INS/UWB紧组合导航系统，其特征是，包括：
惯性导航器件INS、UWB无线标签、UWB无线参考节点、参考系统和数据处理系统；
所述惯性导航器件INS和UWB无线标签分别设置在行人的鞋子上，所述UWB无线参考节点和参考系统分别设置在设定位置，所述惯性导航器件INS、UWB无线标签和参考系统分别与数据处理系统连接；
所述数据处理系统包括：局部数据融合滤波器、容积卡尔曼滤波器、伪距数据处理模块、RTS平滑模块和均值滤波模块；
所述惯性导航器件INS与局部数据融合滤波器连接，所述惯性导航器件INS和UWB无线标签分别与伪距数据处理模块连接，伪距数据处理模块的输出连接所述容积卡尔曼滤波器；所述容积卡尔曼滤波器的输出依次连接RTS平滑模块和均值滤波模块后与减法器的一个输入端连接；所述局部数据融合滤波器的输出与减法器的另一个输入端连接；
所述伪距数据处理模块求取惯性导航器件INS和UWB无线标签测量得到的伪距的平方之差；
所述参考系统包括码盘和电子罗盘。
2.一种采用固定区间CRTS平滑的INS/UWB紧组合导航方法，其特征是，包括：
(1)将人的运行状态分为静止和运动两个状态，通过惯性导航器件INS对行人的当前运动状态进行判断；
(2)在鞋子处于静止状态时，通过局部数据融合滤波器对惯性导航器件INS解算的导航信息误差进行预估和补偿，得到当前时刻惯性导航器件INS解算的位置、速度和姿态的导航信息的最优估计；
(3)以惯性导航器件INS的误差向量作为状态量，以惯性导航器件INS和UWB无线标签分别测量得到的伪距的平方之差作为系统观测量，构建INS/UWB紧组合模型；
(4)通过容积卡尔曼滤波器将惯性导航器件INS和UWB无线标签在本地相对坐标系中获取的导航信息进行数据融合；
(5)当到达输出时间时，将容积卡尔曼滤波器得到的上一输出时间到现输出时间之间的INS误差预估通过RTS平滑之后再进行均值滤波处理；
(6)将经过均值滤波处理后的数据与当前时刻INS解算的导航信息做差，最终得到当前时刻最优的导航信息。
3.如权利要求2所述的一种采用固定区间CRTS平滑的INS/UWB紧组合导航方法，其特征是，所述步骤(1)中，通过惯性导航器件INS对行人的当前运动状态进行判断的方法为：
通过惯性导航器件INS自身携带的3个加速度传感器采集到的数据，得到当前时刻行人的加速度模值，通过加速度模值的大小判断鞋子的运动状态：
其中，加速度模值：ax、ay、az分别为在载体坐标系下的x、y和z三个方向的加速度值。
4.如权利要求2所述的一种采用固定区间CRTS平滑的INS/UWB紧组合导航方法，其特征是，所述步骤(2)中，通过局部数据融合滤波器对惯性导航器件INS解算的导航信息误差进行预估和补偿；
局部数据融合滤波器的状态方程为：
<mrow>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>&phi;</mi>
<mrow>
<mi>Z</mi>
<mo>,</mo>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
</mrow>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msubsup>
<mi>&delta;V</mi>
<mrow>
<mi>Z</mi>
<mo>,</mo>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
</mrow>
<mi>n</mi>
</msubsup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<msubsup>
<mo>&dtri;</mo>
<mrow>
<mi>Z</mi>
<mo>,</mo>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
</mrow>
<mi>b</mi>
</msubsup>
</mtd>
</mtr>
<mtr>
<mtd>
<msubsup>
<mi>&epsiv;</mi>
<mrow>
<mi>Z</mi>
<mo>,</mo>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
</mrow>
<mi>b</mi>
</msubsup>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>I</mi>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msup>
<msub>
<mi>I</mi>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
<mo>&CenterDot;</mo>
</msup>
<msup>
<msubsup>
<mi>C</mi>
<mi>b</mi>
<mi>n</mi>
</msubsup>
<mo>&CenterDot;</mo>
</msup>
<mi>T</mi>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>S</mi>
<msup>
<mrow>
<mo>(</mo>
<msubsup>
<mi>f</mi>
<mi>k</mi>
<mi>n</mi>
</msubsup>
<mo>)</mo>
</mrow>
<mo>&CenterDot;</mo>
</msup>
<mi>T</mi>
</mrow>
</mtd>
<mtd>
<msub>
<mi>I</mi>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<mrow>
<msub>
<mi>I</mi>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
<msup>
<msubsup>
<mi>C</mi>
<mi>b</mi>
<mi>n</mi>
</msubsup>
<mo>&CenterDot;</mo>
</msup>
<mi>T</mi>
</mrow>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mi>I</mi>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mi>I</mi>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>&phi;</mi>
<mrow>
<mi>Z</mi>
<mo>,</mo>
<mi>k</mi>
</mrow>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msubsup>
<mi>&delta;V</mi>
<mrow>
<mi>Z</mi>
<mo>,</mo>
<mi>k</mi>
</mrow>
<mi>n</mi>
</msubsup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<msubsup>
<mo>&dtri;</mo>
<mrow>
<mi>Z</mi>
<mo>,</mo>
<mi>k</mi>
</mrow>
<mi>b</mi>
</msubsup>
</mtd>
</mtr>
<mtr>
<mtd>
<msubsup>
<mi>&epsiv;</mi>
<mrow>
<mi>Z</mi>
<mo>,</mo>
<mi>k</mi>
</mrow>
<mi>b</mi>
</msubsup>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>+</mo>
<msub>
<mi>&omega;</mi>
<mrow>
<mi>Z</mi>
<mo>,</mo>
<mi>k</mi>
</mrow>
</msub>
</mrow>
其中，为12维INS误差向量；为惯性导航器件INS测量得到的导航坐标系下的东向、北向和天向三个方向的加速度误差；φZ,k、φZ,k+1分别为k时刻和k+1时刻惯性导航器件INS测量得到的导航坐标系下的纵摇、横摇和航向角误差；
分别为k时刻和k+1时刻惯性导航器件INS测量得到的导航坐标系下的东向、北向和天向三个方向的速度误差；分别为k时刻和k+1时刻载体坐标系下的东向、北向和天向三个方向的加速度误差；分别为k时刻和k+1时刻载体坐标系下的东向、北向和天向三个方向的角速度误差；ωZ,k为状态噪声；T为采样周期；I为单位矩阵；为从载体系到导航系的状态转移矩阵，其中(γ,θ,ψ)分别为纵摇、横摇和航向角；
局部数据融合滤波器的观测方程为：
<mrow>
<mo>&lsqb;</mo>
<mi>&delta;</mi>
<msubsup>
<mover>
<mi>V</mi>
<mo>~</mo>
</mover>
<mrow>
<mi>Z</mi>
<mo>,</mo>
<mi>k</mi>
</mrow>
<mi>n</mi>
</msubsup>
<mo>&rsqb;</mo>
<mo>=</mo>
<mo>&lsqb;</mo>
<msubsup>
<mi>V</mi>
<mi>k</mi>
<mi>n</mi>
</msubsup>
<mo>-</mo>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>&rsqb;</mo>
<mo>=</mo>
<mo>&lsqb;</mo>
<mtable>
<mtr>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mi>I</mi>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
</mtr>
</mtable>
<mo>&rsqb;</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>&phi;</mi>
<mrow>
<mi>Z</mi>
<mo>,</mo>
<mi>k</mi>
</mrow>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msubsup>
<mi>&delta;V</mi>
<mrow>
<mi>Z</mi>
<mo>,</mo>
<mi>k</mi>
</mrow>
<mi>n</mi>
</msubsup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<msubsup>
<mo>&dtri;</mo>
<mrow>
<mi>Z</mi>
<mo>,</mo>
<mi>k</mi>
</mrow>
<mi>b</mi>
</msubsup>
</mtd>
</mtr>
<mtr>
<mtd>
<msubsup>
<mi>&epsiv;</mi>
<mrow>
<mi>Z</mi>
<mo>,</mo>
<mi>k</mi>
</mrow>
<mi>b</mi>
</msubsup>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>+</mo>
<msub>
<mi>&eta;</mi>
<mrow>
<mi>Z</mi>
<mo>,</mo>
<mi>k</mi>
</mrow>
</msub>
</mrow>
其中，为INS速度误差观测值；为INS解算的速度值；ηZ,k为观测噪声；I为单位矩阵。
5.如权利要求2所述的一种采用固定区间CRTS平滑的INS/UWB紧组合导航方法，其特征是，所述步骤(3)中，INS/UWB紧组合模型的状态方程为：
<mrow>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>&phi;</mi>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
</mrow>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msubsup>
<mi>&delta;V</mi>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
</mrow>
<mi>n</mi>
</msubsup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msubsup>
<mi>&delta;P</mi>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
</mrow>
<mi>n</mi>
</msubsup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<msubsup>
<mo>&dtri;</mo>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
</mrow>
<mi>b</mi>
</msubsup>
</mtd>
</mtr>
<mtr>
<mtd>
<msubsup>
<mi>&epsiv;</mi>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
</mrow>
<mi>b</mi>
</msubsup>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>I</mi>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msup>
<msub>
<mi>I</mi>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
<mo>&CenterDot;</mo>
</msup>
<msup>
<msubsup>
<mi>C</mi>
<mi>b</mi>
<mi>n</mi>
</msubsup>
<mo>&CenterDot;</mo>
</msup>
<mi>T</mi>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>S</mi>
<msup>
<mrow>
<mo>(</mo>
<msubsup>
<mi>f</mi>
<mi>k</mi>
<mi>n</mi>
</msubsup>
<mo>)</mo>
</mrow>
<mo>&CenterDot;</mo>
</msup>
<mi>T</mi>
</mrow>
</mtd>
<mtd>
<msub>
<mi>I</mi>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<mrow>
<msub>
<mi>I</mi>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
<msup>
<msubsup>
<mi>C</mi>
<mi>b</mi>
<mi>n</mi>
</msubsup>
<mo>&CenterDot;</mo>
</msup>
<mi>T</mi>
</mrow>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<mrow>
<msup>
<msub>
<mi>I</mi>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
<mo>&CenterDot;</mo>
</msup>
<mi>T</mi>
</mrow>
</mtd>
<mtd>
<msub>
<mi>I</mi>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mi>I</mi>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mi>I</mi>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>&phi;</mi>
<mi>k</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msubsup>
<mi>&delta;V</mi>
<mi>k</mi>
<mi>n</mi>
</msubsup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msubsup>
<mi>&delta;P</mi>
<mi>k</mi>
<mi>n</mi>
</msubsup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<msubsup>
<mo>&dtri;</mo>
<mi>k</mi>
<mi>b</mi>
</msubsup>
</mtd>
</mtr>
<mtr>
<mtd>
<msubsup>
<mi>&epsiv;</mi>
<mi>k</mi>
<mi>b</mi>
</msubsup>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>+</mo>
<msub>
<mi>&omega;</mi>
<mi>k</mi>
</msub>
</mrow>
其中，为15维INS误差向量；为惯性导航器件INS测量得到的导航坐标系下的东向、北向和天向三个方向的加速度误差；φk、φk+1分别为k时刻和k+1时刻惯性导航器件INS测量得到的导航坐标系下的横摇、纵摇和航向角误差；
分别为k时刻和k+1时刻INS测量得到的导航坐标系下的东向、北向和天向三个方向的速度误差；分别为k时刻和k+1时刻载体坐标系下的东向、北向和天向三个方向的加速度误差；分别为k时刻和k+1时刻载体坐标系下的东向、北向和天向三个方向的角速度误差；分别为k时刻和k+1时刻导航坐标系下的东向、北向和天向三个方向的位置误差；ωk为状态噪声；T为采样周期；I为单位矩阵；为从载体系到导航系的状态转移矩阵。
6.如权利要求2所述的一种采用固定区间CRTS平滑的INS/UWB紧组合导航方法，其特征是，所述步骤(3)中，INS/UWB紧组合模型的观测方程为：
<mrow>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<msubsup>
<mi>&Delta;&rho;</mi>
<mrow>
<mn>1</mn>
<mo>,</mo>
<mi>k</mi>
</mrow>
<mn>2</mn>
</msubsup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msubsup>
<mi>&Delta;&rho;</mi>
<mrow>
<mn>2</mn>
<mo>,</mo>
<mi>k</mi>
</mrow>
<mn>2</mn>
</msubsup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mo>.</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<mo>.</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<mo>.</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msubsup>
<mi>&Delta;&rho;</mi>
<mrow>
<mi>m</mi>
<mo>,</mo>
<mi>k</mi>
</mrow>
<mn>2</mn>
</msubsup>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<msup>
<mrow>
<mo>(</mo>
<msubsup>
<mi>&rho;</mi>
<mrow>
<mn>1</mn>
<mo>,</mo>
<mi>k</mi>
</mrow>
<mrow>
<mi>I</mi>
<mi>N</mi>
<mi>S</mi>
</mrow>
</msubsup>
<mo>)</mo>
</mrow>
<mn>2</mn>
</msup>
<mo>-</mo>
<msup>
<mrow>
<mo>(</mo>
<msubsup>
<mi>&rho;</mi>
<mrow>
<mn>1</mn>
<mo>,</mo>
<mi>k</mi>
</mrow>
<mrow>
<mi>U</mi>
<mi>W</mi>
<mi>B</mi>
</mrow>
</msubsup>
<mo>)</mo>
</mrow>
<mn>2</mn>
</msup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msup>
<mrow>
<mo>(</mo>
<msubsup>
<mi>&rho;</mi>
<mrow>
<mn>2</mn>
<mo>,</mo>
<mi>k</mi>
</mrow>
<mrow>
<mi>I</mi>
<mi>N</mi>
<mi>S</mi>
</mrow>
</msubsup>
<mo>)</mo>
</mrow>
<mn>2</mn>
</msup>
<mo>-</mo>
<msup>
<mrow>
<mo>(</mo>
<msubsup>
<mi>&rho;</mi>
<mrow>
<mn>2</mn>
<mo>,</mo>
<mi>k</mi>
</mrow>
<mrow>
<mi>U</mi>
<mi>W</mi>
<mi>B</mi>
</mrow>
</msubsup>
<mo>)</mo>
</mrow>
<mn>2</mn>
</msup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mo>.</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<mo>.</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<mo>.</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msup>
<mrow>
<mo>(</mo>
<msubsup>
<mi>&rho;</mi>
<mrow>
<mi>m</mi>
<mo>,</mo>
<mi>k</mi>
</mrow>
<mrow>
<mi>I</mi>
<mi>N</mi>
<mi>S</mi>
</mrow>
</msubsup>
<mo>)</mo>
</mrow>
<mn>2</mn>
</msup>
<mo>-</mo>
<msup>
<mrow>
<mo>(</mo>
<msubsup>
<mi>&rho;</mi>
<mrow>
<mi>m</mi>
<mo>,</mo>
<mi>k</mi>
</mrow>
<mrow>
<mi>U</mi>
<mi>W</mi>
<mi>B</mi>
</mrow>
</msubsup>
<mo>)</mo>
</mrow>
<mn>2</mn>
</msup>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<mn>2</mn>
<mrow>
<mo>(</mo>
<msubsup>
<mi>P</mi>
<mrow>
<mi>E</mi>
<mo>,</mo>
<mi>k</mi>
</mrow>
<mi>n</mi>
</msubsup>
<mo>-</mo>
<msubsup>
<mi>P</mi>
<mrow>
<mi>E</mi>
<mo>,</mo>
<mn>1</mn>
</mrow>
<mrow>
<mi>R</mi>
<mi>N</mi>
</mrow>
</msubsup>
<mo>)</mo>
</mrow>
<msubsup>
<mi>&delta;P</mi>
<mrow>
<mi>E</mi>
<mo>,</mo>
<mi>k</mi>
</mrow>
<mi>n</mi>
</msubsup>
<mo>+</mo>
<mn>2</mn>
<mrow>
<mo>(</mo>
<msubsup>
<mi>P</mi>
<mrow>
<mi>N</mi>
<mo>,</mo>
<mi>k</mi>
</mrow>
<mi>n</mi>
</msubsup>
<mo>-</mo>
<msubsup>
<mi>P</mi>
<mrow>
<mi>N</mi>
<mo>,</mo>
<mn>1</mn>
</mrow>
<mrow>
<mi>R</mi>
<mi>N</mi>
</mrow>
</msubsup>
<mo>)</mo>
</mrow>
<msubsup>
<mi>&delta;P</mi>
<mrow>
<mi>N</mi>
<mo>,</mo>
<mi>k</mi>
</mrow>
<mi>n</mi>
</msubsup>
<mo>-</mo>
<mrow>
<mo>(</mo>
<msup>
<mrow>
<mo>(</mo>
<mrow>
<msubsup>
<mi>&delta;P</mi>
<mrow>
<mi>E</mi>
<mo>,</mo>
<mi>k</mi>
</mrow>
<mi>n</mi>
</msubsup>
</mrow>
<mo>)</mo>
</mrow>
<mn>2</mn>
</msup>
<mo>+</mo>
<msup>
<mrow>
<mo>(</mo>
<mrow>
<msubsup>
<mi>&delta;P</mi>
<mrow>
<mi>N</mi>
<mo>,</mo>
<mi>k</mi>
</mrow>
<mi>n</mi>
</msubsup>
</mrow>
<mo>)</mo>
</mrow>
<mn>2</mn>
</msup>
<mo>)</mo>
</mrow>
<mo>-</mo>
<mn>2</mn>
<msubsup>
<mi>&rho;</mi>
<mrow>
<mn>1</mn>
<mo>,</mo>
<mi>k</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>U</mi>
<mi>W</mi>
<mi>B</mi>
<mo>)</mo>
</mrow>
</msubsup>
<msub>
<mi>&upsi;</mi>
<msubsup>
<mi>&rho;</mi>
<mrow>
<mn>1</mn>
<mo>,</mo>
<mi>k</mi>
</mrow>
<mrow>
<mi>U</mi>
<mi>W</mi>
<mi>B</mi>
</mrow>
</msubsup>
</msub>
<mo>+</mo>
<msubsup>
<mi>&upsi;</mi>
<msubsup>
<mi>&rho;</mi>
<mrow>
<mn>1</mn>
<mo>,</mo>
<mi>k</mi>
</mrow>
<mrow>
<mi>U</mi>
<mi>W</mi>
<mi>B</mi>
</mrow>
</msubsup>
<mn>2</mn>
</msubsup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mn>2</mn>
<mrow>
<mo>(</mo>
<msubsup>
<mi>P</mi>
<mrow>
<mi>E</mi>
<mo>,</mo>
<mi>k</mi>
</mrow>
<mi>n</mi>
</msubsup>
<mo>-</mo>
<msubsup>
<mi>P</mi>
<mrow>
<mi>E</mi>
<mo>,</mo>
<mn>2</mn>
</mrow>
<mrow>
<mi>R</mi>
<mi>N</mi>
</mrow>
</msubsup>
<mo>)</mo>
</mrow>
<msubsup>
<mi>&delta;P</mi>
<mrow>
<mi>E</mi>
<mo>,</mo>
<mi>k</mi>
</mrow>
<mi>n</mi>
</msubsup>
<mo>+</mo>
<mn>2</mn>
<mrow>
<mo>(</mo>
<msubsup>
<mi>P</mi>
<mrow>
<mi>N</mi>
<mo>,</mo>
<mi>k</mi>
</mrow>
<mi>n</mi>
</msubsup>
<mo>-</mo>
<msubsup>
<mi>P</mi>
<mrow>
<mi>N</mi>
<mo>,</mo>
<mn>2</mn>
</mrow>
<mrow>
<mi>R</mi>
<mi>N</mi>
</mrow>
</msubsup>
<mo>)</mo>
</mrow>
<msubsup>
<mi>&delta;P</mi>
<mrow>
<mi>N</mi>
<mo>,</mo>
<mi>k</mi>
</mrow>
<mi>n</mi>
</msubsup>
<mo>-</mo>
<mrow>
<mo>(</mo>
<msup>
<mrow>
<mo>(</mo>
<mrow>
<msubsup>
<mi>&delta;P</mi>
<mrow>
<mi>E</mi>
<mo>,</mo>
<mi>k</mi>
</mrow>
<mi>n</mi>
</msubsup>
</mrow>
<mo>)</mo>
</mrow>
<mn>2</mn>
</msup>
<mo>+</mo>
<msup>
<mrow>
<mo>(</mo>
<mrow>
<msubsup>
<mi>&delta;P</mi>
<mrow>
<mi>N</mi>
<mo>,</mo>
<mi>k</mi>
</mrow>
<mi>n</mi>
</msubsup>
</mrow>
<mo>)</mo>
</mrow>
<mn>2</mn>
</msup>
<mo>)</mo>
</mrow>
<mo>-</mo>
<mn>2</mn>
<msubsup>
<mi>&rho;</mi>
<mrow>
<mn>2</mn>
<mo>,</mo>
<mi>k</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>U</mi>
<mi>W</mi>
<mi>B</mi>
<mo>)</mo>
</mrow>
</msubsup>
<msub>
<mi>&upsi;</mi>
<msubsup>
<mi>&rho;</mi>
<mrow>
<mn>2</mn>
<mo>,</mo>
<mi>k</mi>
</mrow>
<mrow>
<mi>U</mi>
<mi>W</mi>
<mi>B</mi>
</mrow>
</msubsup>
</msub>
<mo>+</mo>
<msubsup>
<mi>&upsi;</mi>
<msubsup>
<mi>&rho;</mi>
<mrow>
<mn>2</mn>
<mo>,</mo>
<mi>k</mi>
</mrow>
<mrow>
<mi>U</mi>
<mi>W</mi>
<mi>B</mi>
</mrow>
</msubsup>
<mn>2</mn>
</msubsup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mo>.</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<mo>.</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<mo>.</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mn>2</mn>
<mrow>
<mo>(</mo>
<msubsup>
<mi>P</mi>
<mrow>
<mi>E</mi>
<mo>,</mo>
<mi>k</mi>
</mrow>
<mi>n</mi>
</msubsup>
<mo>-</mo>
<msubsup>
<mi>P</mi>
<mrow>
<mi>E</mi>
<mo>,</mo>
<mi>m</mi>
</mrow>
<mrow>
<mi>R</mi>
<mi>N</mi>
</mrow>
</msubsup>
<mo>)</mo>
</mrow>
<msubsup>
<mi>&delta;P</mi>
<mrow>
<mi>E</mi>
<mo>,</mo>
<mi>k</mi>
</mrow>
<mi>n</mi>
</msubsup>
<mo>+</mo>
<mn>2</mn>
<mrow>
<mo>(</mo>
<msubsup>
<mi>P</mi>
<mrow>
<mi>N</mi>
<mo>,</mo>
<mi>k</mi>
</mrow>
<mi>n</mi>
</msubsup>
<mo>-</mo>
<msubsup>
<mi>P</mi>
<mrow>
<mi>N</mi>
<mo>,</mo>
<mi>m</mi>
</mrow>
<mrow>
<mi>R</mi>
<mi>N</mi>
</mrow>
</msubsup>
<mo>)</mo>
</mrow>
<msubsup>
<mi>&delta;P</mi>
<mrow>
<mi>N</mi>
<mo>,</mo>
<mi>k</mi>
</mrow>
<mi>n</mi>
</msubsup>
<mo>-</mo>
<mrow>
<mo>(</mo>
<msup>
<mrow>
<mo>(</mo>
<mrow>
<msubsup>
<mi>&delta;P</mi>
<mrow>
<mi>E</mi>
<mo>,</mo>
<mi>k</mi>
</mrow>
<mi>n</mi>
</msubsup>
</mrow>
<mo>)</mo>
</mrow>
<mn>2</mn>
</msup>
<mo>+</mo>
<msup>
<mrow>
<mo>(</mo>
<mrow>
<msubsup>
<mi>&delta;P</mi>
<mrow>
<mi>N</mi>
<mo>,</mo>
<mi>k</mi>
</mrow>
<mi>n</mi>
</msubsup>
</mrow>
<mo>)</mo>
</mrow>
<mn>2</mn>
</msup>
<mo>)</mo>
</mrow>
<mo>-</mo>
<mn>2</mn>
<msubsup>
<mi>&rho;</mi>
<mrow>
<mi>m</mi>
<mo>,</mo>
<mi>k</mi>
</mrow>
<mrow>
<mo>(</mo>
<mi>U</mi>
<mi>W</mi>
<mi>B</mi>
<mo>)</mo>
</mrow>
</msubsup>
<msub>
<mi>&upsi;</mi>
<msubsup>
<mi>&rho;</mi>
<mrow>
<mi>m</mi>
<mo>,</mo>
<mi>k</mi>
</mrow>
<mrow>
<mi>U</mi>
<mi>W</mi>
<mi>B</mi>
</mrow>
</msubsup>
</msub>
<mo>+</mo>
<msubsup>
<mi>&upsi;</mi>
<msubsup>
<mi>&rho;</mi>
<mrow>
<mi>m</mi>
<mo>,</mo>
<mi>k</mi>
</mrow>
<mrow>
<mi>U</mi>
<mi>W</mi>
<mi>B</mi>
</mrow>
</msubsup>
<mn>2</mn>
</msubsup>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
其中，为k时刻INS解算的东向和北向位置；为k时刻INS解算的东向和北向位置误差；为k时刻INS测量得到的未知节点到第i个参考节点之间的伪距；为k时刻UWB测量得到的未知节点到第i个参考节点之间的伪距；为k时刻UWB测量得到的未知节点到第i个参考节点之间的伪距的噪声；为第i个参考节点的坐标；为INS和UWB分别测量的未知节点到参考节点之间的伪距的平方之差。
7.如权利要求2所述的一种采用固定区间CRTS平滑的INS/UWB紧组合导航方法，其特征是，所述步骤(4)的方法具体为：
a.对容积卡尔曼滤波器接收到的数据进行cubature采样，得到基本容积点及其对应的权值；
b.时间更新，根据第i个cubature点在k-1时刻的状态变量计算通过非状态方程传播的容积点；根据状态方程传播的容积点计算预测状态和预测方差；
c.对得到的预测方差进行因式分解，根据得到的分解矩阵和预测状态计算第i个cubature点在k时刻的状态变量；
d.根据计算的第i个cubature点在k时刻的状态变量计算通过非线性量测方程传播的容积点；
通过非线性量测方程传播的容积点计算量测预测值、新息方差和协方差估计；
e.计算引入渐消因子后的状态预测协方差阵；通过非线性测量函数重新传播可重新计算引入渐消因子后的输出预测自协方差阵和互协方差阵增益矩阵；
f.在获得新的量测值后，进行滤波更新，得到k时刻的状态向量的最优估计。
8.如权利要求2所述的一种采用固定区间CRTS平滑的INS/UWB紧组合导航方法，其特征是，所述步骤(5)中，对容积卡尔曼滤波器得到的上一输出时间到现输出时间之间的INS误差预估进行RTS平滑的具体方法为：
分别求取逆向平滑过程中的状态变量和逆向平滑过程中的滤波器增益；根据求得的逆向平滑过程中的状态变量和逆向平滑过程中的滤波器增益，求取逆向平滑过程中的概率。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201610078964.7A CN105509739B (zh) | 2016-02-04 | 2016-02-04 | 采用固定区间crts平滑的ins/uwb紧组合导航系统及方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201610078964.7A CN105509739B (zh) | 2016-02-04 | 2016-02-04 | 采用固定区间crts平滑的ins/uwb紧组合导航系统及方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN105509739A CN105509739A (zh) | 2016-04-20 |
| CN105509739B true CN105509739B (zh) | 2018-04-06 |
Family
ID=55717896
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201610078964.7A Active CN105509739B (zh) | 2016-02-04 | 2016-02-04 | 采用固定区间crts平滑的ins/uwb紧组合导航系统及方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN105509739B (zh) |
Families Citing this family (20)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106052684B (zh) * | 2016-06-16 | 2023-07-11 | 济南大学 | 采用多模式描述的移动机器人imu/uwb/码盘松组合导航系统及方法 |
| CN106482736B (zh) * | 2016-07-11 | 2019-04-09 | 安徽工程大学 | 一种基于平方根容积卡尔曼滤波的多机器人协同定位算法 |
| WO2018106311A2 (en) * | 2016-09-22 | 2018-06-14 | The Regents Of The University Of California | Signals of opportunity aided inertial navigation |
| CN106908759A (zh) * | 2017-01-23 | 2017-06-30 | 南京航空航天大学 | 一种基于uwb技术的室内行人导航方法 |
| CN106871893A (zh) * | 2017-03-03 | 2017-06-20 | 济南大学 | 分布式ins/uwb紧组合导航系统及方法 |
| CN106680765B (zh) * | 2017-03-03 | 2024-02-20 | 济南大学 | 基于分布式组合滤波ins/uwb行人导航系统及方法 |
| CN107389060A (zh) * | 2017-05-27 | 2017-11-24 | 哈尔滨工业大学 | 基于CKF的IMU/Wi‑Fi信号的超紧组合室内导航方法 |
| CN107894234A (zh) * | 2017-11-22 | 2018-04-10 | 哈尔滨工业大学 | 一种基于双向滤波的监控导航方法 |
| CN110658515A (zh) * | 2018-06-28 | 2020-01-07 | 北京金坤科创技术有限公司 | 一种基于uwb单基站的多用户imu定位对齐方法 |
| CN109270932A (zh) * | 2018-09-30 | 2019-01-25 | 成都精位科技有限公司 | 自动驾驶定位系统及自动驾驶系统 |
| CN109655060B (zh) * | 2019-02-19 | 2021-07-06 | 济南大学 | 基于kf/fir和ls-svm融合的ins/uwb组合导航算法及系统 |
| CN109883423B (zh) * | 2019-02-25 | 2021-02-09 | 广州市香港科大霍英东研究院 | 基于卡尔曼滤波的定位方法、系统、设备及存储介质 |
| CN109781098B (zh) * | 2019-03-08 | 2021-04-06 | 兰州交通大学 | 一种列车定位的方法和系统 |
| CN109828510A (zh) * | 2019-03-13 | 2019-05-31 | 桂林电子科技大学 | 一种基于超宽带和惯性导航技术的agv定位系统及定位方法 |
| CN110686671B (zh) * | 2019-09-29 | 2021-11-09 | 同济大学 | 基于多传感器信息融合的室内3d实时定位方法及装置 |
| CN110879069A (zh) * | 2019-12-11 | 2020-03-13 | 济南大学 | 一种面向UWB SLAM的Kalman/R-T-S混合定位方法及系统 |
| CN113074739B (zh) * | 2021-04-09 | 2022-09-02 | 重庆邮电大学 | 基于动态鲁棒容积卡尔曼的uwb/ins融合定位方法 |
| CN113970331A (zh) * | 2021-09-06 | 2022-01-25 | 济南大学 | 一种基于重构观测量的四旋翼定位方法及系统 |
| CN114459472B (zh) * | 2022-02-15 | 2023-07-04 | 上海海事大学 | 一种容积卡尔曼滤波器和离散灰色模型的组合导航方法 |
| CN116182873B (zh) * | 2023-05-04 | 2023-07-11 | 长沙驰芯半导体科技有限公司 | 室内定位方法、系统及计算机可读介质 |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103471595A (zh) * | 2013-09-26 | 2013-12-25 | 东南大学 | 一种面向ins/wsn室内移动机器人紧组合导航的迭代扩展rts均值滤波方法 |
| CN104864865A (zh) * | 2015-06-01 | 2015-08-26 | 济南大学 | 一种面向室内行人导航的ahrs/uwb无缝组合导航方法 |
| CN104897157A (zh) * | 2015-06-25 | 2015-09-09 | 济南大学 | 基于足部航姿参考和肩部电子罗盘的个人导航系统及方法 |
| CN104970489A (zh) * | 2014-04-02 | 2015-10-14 | 北京三星通信技术研究有限公司 | 位于足底的穿戴式智能设备 |
| CN205384029U (zh) * | 2016-02-04 | 2016-07-13 | 济南大学 | 采用固定区间crts平滑的ins/uwb紧组合导航系统 |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US8203487B2 (en) * | 2009-08-03 | 2012-06-19 | Xsens Holding, B.V. | Tightly coupled UWB/IMU pose estimation system and method |
-
2016
- 2016-02-04 CN CN201610078964.7A patent/CN105509739B/zh active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103471595A (zh) * | 2013-09-26 | 2013-12-25 | 东南大学 | 一种面向ins/wsn室内移动机器人紧组合导航的迭代扩展rts均值滤波方法 |
| CN104970489A (zh) * | 2014-04-02 | 2015-10-14 | 北京三星通信技术研究有限公司 | 位于足底的穿戴式智能设备 |
| CN104864865A (zh) * | 2015-06-01 | 2015-08-26 | 济南大学 | 一种面向室内行人导航的ahrs/uwb无缝组合导航方法 |
| CN104897157A (zh) * | 2015-06-25 | 2015-09-09 | 济南大学 | 基于足部航姿参考和肩部电子罗盘的个人导航系统及方法 |
| CN205384029U (zh) * | 2016-02-04 | 2016-07-13 | 济南大学 | 采用固定区间crts平滑的ins/uwb紧组合导航系统 |
Non-Patent Citations (2)
| Title |
|---|
| Tightly Coupled Model for Indoor Positioning based on UWB/INS;Fan Qigao 等;《IJCSI International Journal of Computer Science Issues》;20150731;第12卷(第4期);第11-16页 * |
| Tightly-coupled model for INS/WSN integrated navigation based on Kalman filter;Xu Yuan 等;《Journal of Southeast University (English Edition)》;20111231;第27卷(第4期);第384-387页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN105509739A (zh) | 2016-04-20 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN105509739B (zh) | 采用固定区间crts平滑的ins/uwb紧组合导航系统及方法 | |
| Georgy et al. | Modeling the stochastic drift of a MEMS-based gyroscope in gyro/odometer/GPS integrated navigation | |
| CN105928518B (zh) | 采用伪距和位置信息的室内行人uwb/ins紧组合导航系统及方法 | |
| CN104713554B (zh) | 一种基于mems惯性器件与安卓智能手机融合的室内定位方法 | |
| CN102445200B (zh) | 微小型个人组合导航系统及其导航定位方法 | |
| CN109991636A (zh) | 基于gps、imu以及双目视觉的地图构建方法及系统 | |
| CN106772524B (zh) | 一种基于秩滤波的农业机器人组合导航信息融合方法 | |
| CN104864865B (zh) | 一种面向室内行人导航的ahrs/uwb无缝组合导航方法 | |
| CN103968827A (zh) | 一种可穿戴式人体步态检测的自主定位方法 | |
| CN101949703A (zh) | 一种捷联惯性/卫星组合导航滤波方法 | |
| CN105865452B (zh) | 一种基于间接卡尔曼滤波的移动平台位姿估计方法 | |
| CN104596504A (zh) | 应急救援场景下快速构建地图辅助室内定位方法及系统 | |
| CN106507913B (zh) | 用于管道测绘的组合定位方法 | |
| CN106052684A (zh) | 采用多模式描述的移动机器人imu/uwb/码盘松组合导航系统及方法 | |
| CN107490378A (zh) | 一种基于mpu6050与智能手机的室内定位与导航的方法 | |
| CN107966143A (zh) | 一种基于多窗口的自适应efir数据融合方法 | |
| CN107702712A (zh) | 基于惯性测量双层wlan指纹库的室内行人组合定位方法 | |
| CN106840211A (zh) | 一种基于kf和stupf组合滤波的sins大方位失准角初始对准方法 | |
| CN104613966B (zh) | 一种地籍测量事后数据处理方法 | |
| CN104316058B (zh) | 一种采用交互多模型的移动机器人wsn/ins组合导航方法 | |
| CN104296741B (zh) | 采用距离平方和距离平方变化率的wsn/ahrs紧组合方法 | |
| CN109959374A (zh) | 一种行人惯性导航全时全程逆向平滑滤波方法 | |
| CN104374389B (zh) | 一种面向室内移动机器人的imu/wsn组合导航方法 | |
| CN108759825A (zh) | 面向有数据缺失INS/UWB行人导航的自适应预估Kalman滤波算法及系统 | |
| CN117289322A (zh) | 一种基于imu、gps与uwb的高精度定位算法 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| C06 | Publication | ||
| PB01 | Publication | ||
| C10 | Entry into substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |