CN108007477A - 一种基于正反向滤波的惯性行人定位系统误差抑制方法 - Google Patents
一种基于正反向滤波的惯性行人定位系统误差抑制方法 Download PDFInfo
- Publication number
- CN108007477A CN108007477A CN201711220837.7A CN201711220837A CN108007477A CN 108007477 A CN108007477 A CN 108007477A CN 201711220837 A CN201711220837 A CN 201711220837A CN 108007477 A CN108007477 A CN 108007477A
- 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.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 44
- 238000001914 filtration Methods 0.000 title claims abstract description 30
- 230000002441 reversible effect Effects 0.000 title claims abstract description 20
- 230000001133 acceleration Effects 0.000 claims abstract description 27
- 238000004364 calculation method Methods 0.000 claims abstract description 12
- 238000001514 detection method Methods 0.000 claims abstract description 11
- 239000011159 matrix material Substances 0.000 claims description 47
- 230000008859 change Effects 0.000 claims description 16
- 230000009466 transformation Effects 0.000 claims description 14
- 230000001629 suppression Effects 0.000 claims description 12
- 230000008569 process Effects 0.000 claims description 10
- 230000001174 ascending effect Effects 0.000 claims description 6
- 238000004422 calculation algorithm Methods 0.000 claims description 6
- 230000003068 static effect Effects 0.000 claims description 6
- 238000005259 measurement Methods 0.000 claims description 5
- 230000005484 gravity Effects 0.000 claims description 3
- 230000010354 integration Effects 0.000 claims description 3
- 238000005070 sampling Methods 0.000 claims description 3
- 239000013598 vector Substances 0.000 claims description 3
- 238000006243 chemical reaction Methods 0.000 claims description 2
- 238000004321 preservation Methods 0.000 claims description 2
- 230000007704 transition Effects 0.000 claims description 2
- 238000012937 correction Methods 0.000 abstract description 12
- 238000005516 engineering process Methods 0.000 abstract description 3
- 230000009286 beneficial effect Effects 0.000 description 2
- 230000004927 fusion Effects 0.000 description 2
- 230000005021 gait Effects 0.000 description 2
- 230000006872 improvement Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000000452 restraining effect Effects 0.000 description 2
- 230000008030 elimination Effects 0.000 description 1
- 238000003379 elimination reaction Methods 0.000 description 1
- 230000014509 gene expression Effects 0.000 description 1
- 230000002401 inhibitory effect Effects 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 238000013178 mathematical model Methods 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 230000005855 radiation Effects 0.000 description 1
- 230000002829 reductive effect Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
- 239000000126 substance Substances 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
Classifications
-
- 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)
- Manufacturing & Machinery (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Navigation (AREA)
- Acyclic And Carbocyclic Compounds In Medicinal Compositions (AREA)
Abstract
本发明公开了一种基于正反向滤波的惯性行人定位系统误差抑制方法，属于行人定位技术领域。将惯性器件固定在行人脚部，根据测量得的加速度和角速度解算行人的定位信息；同时，定位系统检测并记录人脚部的运动状态，当脚部触地时，认为脚部处于“零速状态”，此时需储存加速度和角速度，零速状态结束后，计算零速区间长度；之后构造Kalman滤波器，对扩展后的零速区间内的数据进行正反向滤波，并利用滤波结果校正行人的定位信息，将补偿后的结果作为最终导航输出。本发明增强了不同运动速度下惯性行人定位系统的适用性，减小了因行人运动速度加快而导致的误差补偿不充分的问题，并且不需任何外界辅助信息就可提高定位精度，计算量小，简单易操作。
Description
技术领域
本发明属于依托惯性传感器的行人定位技术领域，具体涉及一种基于正反向滤波的惯性行人定位系统误差抑制方法。
背景技术
惯性行人定位系统是一种利用以惯性组件为主的专用设备,可以为行人提供精确、实时位置信息。微惯性测量单元(Micro Inertial Measurement Unit，MIMU)是惯性行人定位系统中最常用的惯性器件，它的核心是组件是加速度计和陀螺仪。惯性行人定位系统是将MIMU直接安装在载体上，并将载体与行人足部固连，通过采集MIMU测量载体运动的线运动和角运动信息，经导航解算连续输出行人的速度、位置和姿态(俯仰角、横滚角、航向角)信息。由于惯性行人定位系统不需要任何外界信息，所以是一种全自主定位系统。由于其具有体积小、重量轻、易集成、无辐射等优点，在抢险救灾、单兵作战中等背景下具有极其重要的作用。但是，由于MIMU器件噪声的存在，系统定位误差随时间发散而不断增大，这是制约惯性行人定位系统长时间导航的重要因素之一。
为了提高系统定位精度，一方面可以提高惯性元件精度，但是由于受加工技术水平的限制，无限制的提高元件精度是很难实现的；另一方面就是采取惯性行人定位系统的误差抑制技术，自动抵消惯性器件的误差对系统精度的影响。这样就可以应用现有精度的惯性元件构成较高精度的惯性行人定位系统。
零速校正方法是一种惯性行人定位系统的误差自补偿方法，该方法通过实时检测行人脚部的运动状态，当脚部与地面存在相对运动时，对脚部的位置、速度、姿态信息(简称位姿信息)进行解算；当脚部与地面相对静止时，对脚部位姿信息进行解算与误差补偿，来抵消器件噪声对系统的影响，进而提高系统定位精度。虽然零速校正方法能够较好的抑制行人正常行进时定位误差的发散，但是当行人行进速度加快时，由于脚部与地面相对静止时间缩短(约为0.05秒)，即零速区间缩短，进而导致零速校正时间短，进而无法完全补偿定位误差，导致定位精度显著下降，无法满足行人定位的需求。
对于该方向的研究，在CNKI库中公开报道有：1.公告号为CN103616030A的中国发明专利在2014年3月5日公开的《基于捷联惯导解算和零速校正的自主导航系统定位方法》，针对零速校正过程中部分观测量不可观的问题，设计了磁力计辅助零速校正算法的惯性行人定位方案。2.《测绘工程》2017年26卷第5期由朱彩杰等人撰写的《基于MEMS的室内定位误差修正方法研究》，该文章主要对消除行人运动过程中的无效振动进行了研究，针对行走过程中存在无效振动,导致MIMU输出信号中包含明显噪声的问题，提出了一种滤除MIMU输出信号中高频噪声以提高零速校正精度的方案。3.《中国惯性技术学报》2013年22卷第4期由黄志远等人撰写的《多条件约束的行人导航零速区间检测算法》，该文章主要针对MIMU存在漂移大、器件精度低的问题，在分析行人运动步态的基础上，设计了一种改进的零速区间检测算法。在SCI库中公开检索有：1.《Information Fusion》2017年由Sen Qiu等人撰写的《Inertial/magnetic sensors based pedestrian dead reckoning by means of multi-sensor fusion》，该文章主要提出了一种惯性行人定位系统误差补偿技术，对MIMU安装误差和系统噪声进行了研究。2.《Journal of Chemical Information&Computer Sciences》2016年44卷第5期由Yuan Xu等人撰写的《Two-mode navigation method for low-costinertial measurement unit-based indoor pedestrian navigation》，该文章提出了一种抑制惯性行人定位系统误差传播的方案，对行人运动过程中不同的步态特征进行了分析，并建立了相应的数学模型。3.《IEEE Sensors Journal》2016年16卷第17期由ANorrdine等人撰写的《Step Detection for ZUPT-Aided Inertial PedestrianNavigation System Using Foot-Mounted Permanent Magnet》，该文章提出了一种改进的零速校正方法，分析了MIMU器件误差对惯性行人定位系统精度的影响，并对MIMU器件误差进行了估计。以上文献都在于抑制行人慢速运动时的定位误差，并没有提及行人快速运动对系统解算位姿信息精度及适用性的影响。
发明内容
本发明的目的在于提供可以提高定位精度，增强不同运动速度下系统解算行人定位信息适用性的一种基于正反向滤波的惯性行人定位系统误差抑制方法。
本发明的目的通过如下技术方案来实现：
在惯性行人定位系统解算行人脚部位姿(位置、速度和姿态)的过程中实时检测行人脚部的零速状态，根据零速检测的结果，扩展零速区间，通过基于正反向滤波的零速校正方法对行人的位姿信息进行补偿。
一种基于正反向滤波惯性行人定位系统定位误差抑制方法，包括以下步骤：
(1)将MIMU器件固定在行人脚部，打开电源并初始化系统；
(2)系统实时采集MIMU的输出数据；
(3)系统实时检测并记录行人脚部的运动状态，用ZUPT(m)表示第m时刻脚步运动状态。当第m时刻脚部处于离地状态时，记作ZUPT(m)＝0；当第m时刻行人脚部触地时，记作ZUPT(m)＝1，理论上认为此时脚部与地面相对静止，即脚部运动速度为0，故将这一段相对静止的时间称为“零速区间”；
(4)行人定位系统对MIMU(陀螺仪和加速度计)输出信息进行解算,由陀螺仪输出的角速度计算行人姿态信息，得到其中b表示载体坐标系，n表示导航坐标系，表示b系到n系的转换矩阵；利用将加速度计输出的加速度信息投影到导航坐标系后，有其中f表示加速度信息，fb表示载体坐标系下的加速度信息，fn表示导航坐标系下的加速度信息；加速度信息经过一次积分求解出速度信息：νn＝∫fndt，再次积分求解出位置信息：pn＝∫vndt；其中ν表示速度信息，p表示位置信息，pn表示导航坐标系下的位置信息，t表示时间；
(5)根据步骤(3)中零速检测的结果，如果当前时刻ZUPT＝1，则转至步骤(6)；如果当前时刻ZUPT＝0，判断前N时刻是否一直有ZUPT＝1，如果是，则转至步骤(7)；如果不是，直接将步骤(4)中解算的行人位姿信息作为最终的导航输出；
(6)记录行人的加速度和角速度信息，并记录零速区间长度。
(7)读取步骤(6)中加速度f、角速度ω和零速区间长度L，并扩展零速区间；
(8)：根据步骤(7)中加速度和角速度信息，解算行人的姿态ε和速度ν。由于理论上行人脚部触地时有ν＝0，所以ν＝δν，其中δν为速度误差。
(9)构造Kalman滤波器，以速度误差δν、位置误差δp和姿态误差δε为状态量，以速度误差为观测量，对零速区间内数据进行滤波；
(10)利用步骤(9)中解算出的δν，δp，δε补偿并将补偿后的结果作为最终导航输出，其中，分别为系统解算的ν，p，ε的值；
(11)重复步骤(1)到步骤(10)，直至惯性行人定位系统导航结束。
本发明的有益效果在于：
本根据惯性行人定位系统解算定位信息中位姿误差形式，在得出行人运动过程中的运动状态后，结合零速校正算法设计了正反向滤波算法，消除运动速度对误差补偿的影响，滤波后的定位信息作为最终导航解算输出信息。
增强了不同运动速度下惯性行人定位系统的适用性，减小了因行人运动速度加快而导致的误差补偿不充分的问题；不需任何外界辅助信息就可提高定位精度；计算量小，简单易操作。
附图说明
图1是本发明的方法流程图；
图2为本发明中利用MATLAB仿真得到有无正反向滤波的定位轨迹比较曲线；
图3为行人预设路线。
具体实施方式
下面结合附图对本发明的具体实施方式作进一步说明：
一种基于正反向滤波的惯性行人定位系统误差抑制方法，方法流程如图1所示，包括以下步骤：
步骤一：将MIMU器件水平固定在行人脚部，其x、y、z轴分别指向行人的右、前、上方向。上电，运行。
导航初始时刻，需初始化系统：(1)初始化惯性定位系统初值：行人三轴位置信息 (单位均为m)，三轴速度信息单位均为m/s)，以及三个姿态角信息φ0、θ0、ψ0((单位均为rad))，初始转换矩阵初始四元数q0；(2)初始化常值参量：当地重力海拔h(单位为m)，当地纬度λ(单位为rad)，零速检测阈值γ，加速度计白噪声误差σa，陀螺仪白噪声误差σω，采样时间T(单位为s)；(3)Kalman滤波器参数初值：状态变量初值均方误差阵P0，系统噪声方差阵Q，量测噪声方差阵R，量测阵H；将以上初始化信息装订至导航计算机中。
通常情况下，
姿态角φ0、θ0计算如下：
其中，φ0表示横滚角初始值，θ0表示俯仰角初始值，分别表示加速度计测量比力在载体系oxb轴、oyb轴、ozb轴上的分量。
初始转换矩阵计算如下：
其中，b表示载体坐标系，n表示导航坐标系，表示b系到n系的转换矩阵。
初始四元数q0计算如下：
令则
其中，cij(i＝1,2,3，j＝1,2,3)表示中第i行第j列矩阵元素，表示矩阵的转置；
当地重力加速度计算如下：
g＝9.780327×(1+0.0053024sin2λ-0.0000058sin22λ)-(3.0877×10-6-4×10- 9sin2λ)×h+7.2×10-14×h2
其中，0为三行三列的零矩阵；
其余初值需根据实际情况设定；
定位过程中，利用该初始信息进行更新，得到任意时刻行人的位置、速度和姿态信息；
步骤二：系统实时采集MIMU的输出数据，包括加速度计输出的三轴加速度 分别表示加速度计测量比力在载体系oxb轴、oyb轴、ozb轴上的分量(单位均为m/s2)；陀螺仪输出的三轴角速度 分别表示陀螺仪测量的角速度在载体系oxb轴、oyb轴、ozb轴上的分量(单位均为rads)；
步骤三：系统利用fb和ωb实时检测并记录行人脚部的运动状态，并用ZUPT表示运动状态。
在m时刻，若则认为脚部处于离地状态，记作ZUPT(m)＝0；反之，则认为脚部处于触地状态，记作ZUPT(m)＝1，理论上认为此时脚部与地面相对静止，即脚部运动速度为0，故将这一段相对静止的时间称为“零速区间”。其中，表示向量的2范数，γ为零速检测阈值，σa为加速度计白噪声误差，σω为陀螺仪白噪声误差；
步骤四：系统根据步骤二中采集到的加速度fb和角速度ωb进行导航解算，由ωb计算行人姿态信息，得到利用将加速度计输出的加速度信息投影到导航坐标系后，有加速度信息经过一次积分求解出速度信息：νn＝∫fndt，再次积分求解出位置信息：pn＝∫vndt；其中ν表示速度信息，p表示位置信息，pn表示导航坐标系下的位置信息，t表示时间；
在行人运动过程中，通过ωb更新转换矩阵具体为：
四元数姿态矩阵更新：
设任意时刻载体系相对导航坐标系的转动四元数为：
q＝[q0 q1 q2 q3]T (1)
其中，q为四元数；q0为q的实数部分，q1、q2、q3为q的虚数部分。
四元数q的及时修正：
其中，分别表示q0、q1、q2、q3的变化率；
根据k时刻载体坐标系相对导航坐标系的转动四元数q0(k)、q1(k)、q2(k)、q3(k)，求取k时刻转动四元数的变化率为：
在k+1时刻载体的转动四元数具体为：
其中，I为单位阵，T为采样时间，(4)式中ωx、ωy、ωz均省略了上角标b，当k＝1时，q(k)为步骤一中初始化系统时获得的载体初始四元数。
利用得到的q(k+1)中元素q0(k+1)、q1(k+1)、q2(k+1)、q3(k+1)，更新捷联矩阵
其中，(5)式中的qi(i＝1,2,3,4)为(4)式中qi(k+1)(i＝1,2,3,4)，(5)式中省略了(k+1)。
更新载体姿态信息，具体为：
将加速度计沿载体坐标系测量的比力信息，通过捷联矩阵进行投影转换：
利用下列微分方程求解载体运动速度：
其中，分别表示解算载体速度在导航系oxn轴、oyn轴、ozn轴上的分量； 表示vx、vy、vz的变化率，即载体沿导航系oxn轴、oyn轴、ozn轴的运动加速度；g为当地重力加速度。
根据k时刻的载体三轴速度vx(k)、vy(k)和vz(k)，求取k时刻载体速度变化率为：
在k+1时刻载体速度和位置分别为：
其中，分别表示载体位置在导航系oxn轴、oyn轴、ozn轴上的分量，当k＝1时，vx(1)、vy(1)、vz(1)为步骤一中初始化系统时获得的载体初始速度，为步骤一中初始化系统时获得的载体初始位置。
至此，根据(6)、(10)、(11)式，得到了行人的姿态、速度、位置。
步骤五：记当前时刻为第k时刻，根据步骤三中零速检测的结果，如果ZUPT(k)＝1，则转至步骤六；如果ZUPT(k)＝0，判断前N时刻是否一直满足ZUPT(i)≡1,(i＝k-N,k-N+1,…,k-1)，如果是，表示前N时刻脚部处于零速状态，则转至步骤七；如果不是，表示前N时刻脚部不处于零速状态，直接将步骤四中解算的行人位姿信息作为最终的导航输出。N的一般取1～2；
步骤六：记录行人的加速度fb和角速度ωb，直到ZUPT＝0停止记录，并计算零速区间长度L。
步骤七：读取步骤六中零速区间长度L，读取区间内的数据f、ω；记第i组数据为ui＝[fi ωi]T，则零速区间中数据可写作U＝[u1 u2 … uL]；
记D＝[-uL -uL-1 … -u1]，扩展零速区间至sL，得到一般取s＝5，则有Data＝[U D U D U]；
步骤八：根据步骤七中Data，解算行人的速度位置和姿态
和转换矩阵的计算与步骤四相同；
理论上行人脚部触地时，位置保持不变，k+1时刻载体速度和位置分别为：
其中，分别为ν，p，ε，的计算值。
步骤九：对Data进行正反向滤波，滤波状态量为X＝[δpn δνn δεn]，观测量为z＝δνn，滤波过程如下：
利用下列微分方程建立载体运动速度、位置和姿态的误差模型：
其中：δp为载体位置误差，且δν为载体速度误差，且δε为载体姿态角误差并且有δε＝[δφ δθ δψ]。分别为δp，δν，δε的变化率，ωb为载体角速度，且[fn×]为由加速度信息构成的反对称阵，且03×1为三行一列的零向量。
根据k时刻的载体位置误差δp(k)，载体速度误差δν(k)，载体姿态角误差δε(k)，求取k时刻载体位置误差变化率、速度误差变化率、姿态角误差变化率为：
在k+1时刻载体位置误差δp(k+1)、速度误差δν(k+1)、姿态角误差δε(k+1)分别为：
其中，式(17)和式(18)中省去了上角标n。
由于理论上行人脚部触地时，应有ν＝0，故此时系统解算出的速度信息ν为误差量，即ν＝δν，其中δν为速度误差。
根据上述误差模型，利用卡尔曼滤波器，对行人的位姿信息进行误差修正，具体算法如下：
Kk＝PkHT(HPkHT+R)-1 (18)
其中，Pk为k时刻的估计均方误差阵，Fk,k-1为k-1时刻到k时刻的为状态转移矩阵，且I为单位阵，0为零矩阵，为k-1时刻的噪声驱动阵，Kk为k时刻的滤波增益阵。
Xk＝-KkZk (19)
更新Pk：
其中，Xk为k时刻的状态变量，当k＝1时，状态量X＝[δp(1) δν(1) δε(1)]为步骤一中初始化系统时获得的载体初始状态量X0，为更新后的Pk。
保存Pk与Xk，并将其作为下一次滤波的输入。
式(17)应当在整个定位过程中实时结算，而非只针对零速区间。
步骤十：利用步骤九中解算出的δν，δp，δε补偿步骤八中的更新四元数qk，并将补偿后的结果作为最终导航输出：
位置、速度信息补偿：
姿态角补偿：
令利用公式更新转换矩阵利用式(6)求出更新后的φ，θ，ψ；
其中，为矩阵的逆运算。
更新四元数：
令则
步骤十一：重复步骤一到步骤十，直至惯性行人定位系统导航结束。
实施例：
对本发明的有益效果如下方式得以验证：
采用MTi-710系列MIMU构建惯性行人定位系统，进行惯性行人定位系统的快速行走试验。根据图3，其中，黑色方框为行人起始位置。
系统初始化参数如下：
载体三轴位置信息：
载体三轴速度信息：载体航向角信息：ψ0＝0rad；
当地重力海拔：h＝100m；
当地纬度：λ＝0.7988rad；
采样时间：T＝0.01s；
加速度计白噪声误差：σa＝0.008；
陀螺仪白噪声误差：σω＝0.00506；
零速检测阈值：γ＝30000；
最短零速区间长度：N＝2；
零速区间扩展倍数：s＝5；
Kalman滤波器初始参数：
初始状态量：
初始均方误差阵：
系统噪声方差阵：
量测噪声方差阵：
利用发明所述方法，得到行人快速行走时有、无正反向滤波的定位轨迹比较曲线，其中(a)、(b)图分别表示有正反向滤波的定位轨迹曲线，无正反向滤波的定位轨迹曲线。结果表明本发明抑制行人快速运动时定位误差能力较好，可以满足实际需求。
以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。
Claims (7)
1.一种基于正反向滤波的惯性行人定位系统误差抑制方法，其特征在于，包括以下步骤：
(1)将惯性器件固定在行人脚部，打开电源并初始化系统；
(2)系统实时采集惯性器件的输出数据；
(3)系统实时检测并记录行人脚部的运动状态，用ZUPT(m)表示第m时刻脚步运动状态，当第m时刻脚部处于离地状态时，记作ZUPT(m)＝0；当第m时刻行人脚部触地时，记作ZUPT(m)＝1，理论上认为此时脚部与地面相对静止，即脚部运动速度为0，故将这一段相对静止的时间称为“零速区间”；
(4)行人定位系统对惯性器件(陀螺仪和加速度计)输出信息进行解算,由陀螺仪输出的角速度计算行人姿态信息，得到其中b表示载体坐标系，n表示导航坐标系，表示b系到n系的转换矩阵；利用将加速度计输出的加速度信息投影到导航坐标系后，有其中f表示加速度信息，fb表示载体坐标系下的加速度信息，fn表示导航坐标系下的加速度信息；加速度信息经过一次积分求解出速度信息：νn＝∫fndt，再次积分求解出位置信息：pn＝∫vndt；其中ν表示速度信息，p表示位置信息，pn表示导航坐标系下的位置信息，t表示时间；
(5)根据步骤(3)中零速检测的结果，如果当前时刻ZUPT＝1，则转至步骤(6)；如果当前时刻ZUPT＝0，判断前N时刻是否一直有ZUPT＝1，如果是，则转至步骤(7)；如果不是，直接将步骤(4)中解算的行人位姿信息作为最终的导航输出；
(6)记录行人的加速度和角速度信息，并记录零速区间长度；
(7)读取步骤(6)中加速度f、角速度ω和零速区间长度L，并扩展零速区间；
(8)根据步骤(7)中加速度和角速度信息，解算行人的姿态ε和速度ν，理论上行人脚部触地时有ν＝0，ν＝δν，其中δν为速度误差；
(9)构造Kalman滤波器，以速度误差δν、位置误差δp和姿态误差δε为状态量，以速度误差为观测量，对零速区间内数据进行滤波；
(10)利用步骤(9)中解算出的δν，δp，δε补偿并将补偿后的结果作为最终导航输出，其中，分别为系统解算的ν，p，ε的值；
(11)重复步骤(1)到步骤(10)，直至惯性行人定位系统导航结束。
2.根据权利要求1所述的一种基于正反向滤波的惯性行人定位系统误差抑制方法，其特征在于，所述的步骤(1)具体为：
导航初始时刻，需初始化系统：(1.1)初始化惯性定位系统初值：行人三轴位置信息 (单位均为m)，三轴速度信息(单位均为m/s)，以及三个姿态角信息φ0、θ0、ψ0((单位均为rad))，初始转换矩阵初始四元数q0；(1.2)初始化常值参量：当地重力海拔h(单位为m)，当地纬度λ(单位为rad)，零速检测阈值γ，加速度计白噪声误差σa，陀螺仪白噪声误差σω，采样时间T(单位为s)；(1.3)Kalman滤波器参数初值：状态变量初值均方误差阵P0，系统噪声方差阵Q，量测噪声方差阵R，量测阵H；
通常情况下，
姿态角φ0、θ0计算如下：
其中，φ0表示横滚角初始值，θ0表示俯仰角初始值，分别表示加速度计测量比力在载体系oxb轴、oyb轴、ozb轴上的分量；
初始转换矩阵计算如下：
<mrow>
<msubsup>
<mi>C</mi>
<mi>b</mi>
<mi>n</mi>
</msubsup>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>cos&theta;</mi>
<mn>0</mn>
</msub>
<msub>
<mi>cos&psi;</mi>
<mn>0</mn>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>cos&phi;</mi>
<mn>0</mn>
</msub>
<msub>
<mi>sin&psi;</mi>
<mn>0</mn>
</msub>
<mo>+</mo>
<msub>
<mi>sin&phi;</mi>
<mn>0</mn>
</msub>
<msub>
<mi>sin&theta;</mi>
<mn>0</mn>
</msub>
<msub>
<mi>cos&psi;</mi>
<mn>0</mn>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>sin&phi;</mi>
<mn>0</mn>
</msub>
<msub>
<mi>sin&psi;</mi>
<mn>0</mn>
</msub>
<mo>+</mo>
<msub>
<mi>cos&phi;</mi>
<mn>0</mn>
</msub>
<msub>
<mi>sin&theta;</mi>
<mn>0</mn>
</msub>
<msub>
<mi>cos&psi;</mi>
<mn>0</mn>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>cos&theta;</mi>
<mn>0</mn>
</msub>
<msub>
<mi>sin&psi;</mi>
<mn>0</mn>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>cos&phi;</mi>
<mn>0</mn>
</msub>
<msub>
<mi>cos&psi;</mi>
<mn>0</mn>
</msub>
<mo>+</mo>
<msub>
<mi>sin&phi;</mi>
<mn>0</mn>
</msub>
<msub>
<mi>sin&theta;</mi>
<mn>0</mn>
</msub>
<msub>
<mi>sin&psi;</mi>
<mn>0</mn>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>sin&phi;</mi>
<mn>0</mn>
</msub>
<msub>
<mi>cos&psi;</mi>
<mn>0</mn>
</msub>
<mo>+</mo>
<msub>
<mi>cos&phi;</mi>
<mn>0</mn>
</msub>
<msub>
<mi>sin&theta;</mi>
<mn>0</mn>
</msub>
<msub>
<mi>sin&psi;</mi>
<mn>0</mn>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>sin&theta;</mi>
<mn>0</mn>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>sin&phi;</mi>
<mn>0</mn>
</msub>
<msub>
<mi>cos&theta;</mi>
<mn>0</mn>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>cos&phi;</mi>
<mn>0</mn>
</msub>
<msub>
<mi>cos&theta;</mi>
<mn>0</mn>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
其中，b表示载体坐标系，n表示导航坐标系，表示b系到n系的转换矩阵；
初始四元数q0计算如下：
令则
其中，cij(i＝1,2,3，j＝1,2,3)表示中第i行第j列矩阵元素，表示矩阵的转置；
当地重力加速度计算如下：
g＝9.780327×(1+0.0053024sin2λ-0.0000058sin22λ)-(3.0877×10-6-4×10-9sin2λ)×h+7.2×10-14×h2
<mrow>
<msub>
<mi>X</mi>
<mn>0</mn>
</msub>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<msubsup>
<mi>&delta;p</mi>
<mn>0</mn>
<mi>n</mi>
</msubsup>
</mrow>
</mtd>
<mtd>
<mrow>
<msubsup>
<mi>&delta;v</mi>
<mn>0</mn>
<mi>n</mi>
</msubsup>
</mrow>
</mtd>
<mtd>
<mrow>
<msubsup>
<mi>&delta;&epsiv;</mi>
<mn>0</mn>
<mi>n</mi>
</msubsup>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
其中，0为三行三列的零矩阵；
其余初值需根据实际情况设定。
3.根据权利要求1所述的一种基于正反向滤波的惯性行人定位系统误差抑制方法，其特征在于，所述的步骤(4)具体为：
在行人运动过程中，通过ωb更新转换矩阵具体为：
四元数姿态矩阵更新：
设任意时刻载体系相对导航坐标系的转动四元数为：
q＝[q0 q1 q2 q3]T (1)
其中，q为四元数；q0为q的实数部分，q1、q2、q3为q的虚数部分；
四元数q的及时修正：
<mrow>
<mover>
<mi>q</mi>
<mo>&CenterDot;</mo>
</mover>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mover>
<mi>q</mi>
<mo>&CenterDot;</mo>
</mover>
<mn>0</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mover>
<mi>q</mi>
<mo>&CenterDot;</mo>
</mover>
<mn>1</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mover>
<mi>q</mi>
<mo>&CenterDot;</mo>
</mover>
<mn>2</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mover>
<mi>q</mi>
<mo>&CenterDot;</mo>
</mover>
<mn>3</mn>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<mfrac>
<mn>1</mn>
<mn>2</mn>
</mfrac>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msubsup>
<mi>&omega;</mi>
<mi>x</mi>
<mi>b</mi>
</msubsup>
</mrow>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msubsup>
<mi>&omega;</mi>
<mi>y</mi>
<mi>b</mi>
</msubsup>
</mrow>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msubsup>
<mi>&omega;</mi>
<mi>z</mi>
<mi>b</mi>
</msubsup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<msubsup>
<mi>&omega;</mi>
<mi>x</mi>
<mi>b</mi>
</msubsup>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<msubsup>
<mi>&omega;</mi>
<mi>z</mi>
<mi>b</mi>
</msubsup>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msubsup>
<mi>&omega;</mi>
<mi>x</mi>
<mi>b</mi>
</msubsup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<msubsup>
<mi>&omega;</mi>
<mi>y</mi>
<mi>b</mi>
</msubsup>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msubsup>
<mi>&omega;</mi>
<mi>z</mi>
<mi>b</mi>
</msubsup>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<msubsup>
<mi>&omega;</mi>
<mi>x</mi>
<mi>b</mi>
</msubsup>
</mtd>
</mtr>
<mtr>
<mtd>
<msubsup>
<mi>&omega;</mi>
<mi>z</mi>
<mi>b</mi>
</msubsup>
</mtd>
<mtd>
<msubsup>
<mi>&omega;</mi>
<mi>y</mi>
<mi>b</mi>
</msubsup>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msubsup>
<mi>&omega;</mi>
<mi>x</mi>
<mi>b</mi>
</msubsup>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>q</mi>
<mn>0</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>q</mi>
<mn>1</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>q</mi>
<mn>2</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>q</mi>
<mn>3</mn>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>2</mn>
<mo>)</mo>
</mrow>
</mrow>
其中，分别表示q0、q1、q2、q3的变化率；
根据k时刻载体坐标系相对导航坐标系的转动四元数q0(k)、q1(k)、q2(k)、q3(k)，求取k时刻转动四元数的变化率为：
<mrow>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mover>
<mi>q</mi>
<mo>&CenterDot;</mo>
</mover>
<mn>0</mn>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mover>
<mi>q</mi>
<mo>&CenterDot;</mo>
</mover>
<mn>1</mn>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mover>
<mi>q</mi>
<mo>&CenterDot;</mo>
</mover>
<mn>2</mn>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mover>
<mi>q</mi>
<mo>&CenterDot;</mo>
</mover>
<mn>3</mn>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<mfrac>
<mn>1</mn>
<mn>2</mn>
</mfrac>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msubsup>
<mi>&omega;</mi>
<mi>x</mi>
<mi>b</mi>
</msubsup>
</mrow>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msubsup>
<mi>&omega;</mi>
<mi>y</mi>
<mi>b</mi>
</msubsup>
</mrow>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msubsup>
<mi>&omega;</mi>
<mi>z</mi>
<mi>b</mi>
</msubsup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<msubsup>
<mi>&omega;</mi>
<mi>x</mi>
<mi>b</mi>
</msubsup>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<msubsup>
<mi>&omega;</mi>
<mi>z</mi>
<mi>b</mi>
</msubsup>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msubsup>
<mi>&omega;</mi>
<mi>x</mi>
<mi>b</mi>
</msubsup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<msubsup>
<mi>&omega;</mi>
<mi>y</mi>
<mi>b</mi>
</msubsup>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msubsup>
<mi>&omega;</mi>
<mi>z</mi>
<mi>b</mi>
</msubsup>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<msubsup>
<mi>&omega;</mi>
<mi>x</mi>
<mi>s</mi>
</msubsup>
</mtd>
</mtr>
<mtr>
<mtd>
<msubsup>
<mi>&omega;</mi>
<mi>z</mi>
<mi>b</mi>
</msubsup>
</mtd>
<mtd>
<msubsup>
<mi>&omega;</mi>
<mi>y</mi>
<mi>b</mi>
</msubsup>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msubsup>
<mi>&omega;</mi>
<mi>x</mi>
<mi>b</mi>
</msubsup>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>q</mi>
<mn>0</mn>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>q</mi>
<mn>1</mn>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>q</mi>
<mn>2</mn>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>q</mi>
<mn>3</mn>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>3</mn>
<mo>)</mo>
</mrow>
</mrow>
在k+1时刻载体的转动四元数具体为：
<mrow>
<mi>q</mi>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mo>{</mo>
<mi>cos</mi>
<mfrac>
<mrow>
<msqrt>
<mrow>
<msubsup>
<mi>&omega;</mi>
<mi>x</mi>
<mn>2</mn>
</msubsup>
<mo>+</mo>
<msubsup>
<mi>&omega;</mi>
<mi>y</mi>
<mn>2</mn>
</msubsup>
<mo>+</mo>
<msubsup>
<mi>&omega;</mi>
<mi>z</mi>
<mn>2</mn>
</msubsup>
</mrow>
</msqrt>
<mi>T</mi>
</mrow>
<mn>2</mn>
</mfrac>
<mi>I</mi>
<mo>+</mo>
<mfrac>
<mrow>
<mi>sin</mi>
<mfrac>
<mrow>
<msqrt>
<mrow>
<msubsup>
<mi>&omega;</mi>
<mi>x</mi>
<mn>2</mn>
</msubsup>
<mo>+</mo>
<msubsup>
<mi>&omega;</mi>
<mi>y</mi>
<mn>2</mn>
</msubsup>
<mo>+</mo>
<msubsup>
<mi>&omega;</mi>
<mi>z</mi>
<mn>2</mn>
</msubsup>
</mrow>
</msqrt>
<mi>T</mi>
</mrow>
<mn>2</mn>
</mfrac>
</mrow>
<msqrt>
<mrow>
<msubsup>
<mi>&omega;</mi>
<mi>x</mi>
<mn>2</mn>
</msubsup>
<mo>+</mo>
<msubsup>
<mi>&omega;</mi>
<mi>y</mi>
<mn>2</mn>
</msubsup>
<mo>+</mo>
<msubsup>
<mi>&omega;</mi>
<mi>z</mi>
<mn>2</mn>
</msubsup>
</mrow>
</msqrt>
</mfrac>
<mi>O</mi>
<mi>M</mi>
<mi>E</mi>
<mi>G</mi>
<mi>A</mi>
<mo>}</mo>
<mi>q</mi>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>4</mn>
<mo>)</mo>
</mrow>
</mrow>
其中，I为单位阵，T为采样时间，(4)式中ωx、ωy、ωz均省略了上角标b，当k＝1时，q(k)为步骤一中初始化系统时获得的载体初始四元数；
利用得到的q(k+1)中元素q0(k+1)、q1(k+1)、q2(k+1)、q3(k+1)，更新捷联矩阵
<mrow>
<msubsup>
<mi>C</mi>
<mi>b</mi>
<mi>n</mi>
</msubsup>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<msubsup>
<mi>q</mi>
<mn>0</mn>
<mn>2</mn>
</msubsup>
<mo>+</mo>
<msubsup>
<mi>q</mi>
<mn>1</mn>
<mn>2</mn>
</msubsup>
<mo>-</mo>
<msubsup>
<mi>q</mi>
<mn>2</mn>
<mn>2</mn>
</msubsup>
<mo>-</mo>
<msubsup>
<mi>q</mi>
<mn>3</mn>
<mn>2</mn>
</msubsup>
</mrow>
</mtd>
<mtd>
<mrow>
<mn>2</mn>
<mrow>
<mo>(</mo>
<msub>
<mi>q</mi>
<mn>1</mn>
</msub>
<msub>
<mi>q</mi>
<mn>2</mn>
</msub>
<mo>-</mo>
<msub>
<mi>q</mi>
<mn>0</mn>
</msub>
<msub>
<mi>q</mi>
<mn>3</mn>
</msub>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
<mtd>
<mrow>
<mn>2</mn>
<mrow>
<mo>(</mo>
<msub>
<mi>q</mi>
<mn>1</mn>
</msub>
<msub>
<mi>q</mi>
<mn>3</mn>
</msub>
<mo>+</mo>
<msub>
<mi>q</mi>
<mn>0</mn>
</msub>
<msub>
<mi>q</mi>
<mn>2</mn>
</msub>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mn>2</mn>
<mrow>
<mo>(</mo>
<msub>
<mi>q</mi>
<mn>1</mn>
</msub>
<msub>
<mi>q</mi>
<mn>2</mn>
</msub>
<mo>+</mo>
<msub>
<mi>q</mi>
<mn>0</mn>
</msub>
<msub>
<mi>q</mi>
<mn>3</mn>
</msub>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
<mtd>
<mrow>
<msubsup>
<mi>q</mi>
<mn>0</mn>
<mn>2</mn>
</msubsup>
<mo>-</mo>
<msubsup>
<mi>q</mi>
<mn>1</mn>
<mn>2</mn>
</msubsup>
<mo>+</mo>
<msubsup>
<mi>q</mi>
<mn>2</mn>
<mn>2</mn>
</msubsup>
<mo>-</mo>
<msubsup>
<mi>q</mi>
<mn>3</mn>
<mn>2</mn>
</msubsup>
</mrow>
</mtd>
<mtd>
<mrow>
<mn>2</mn>
<mrow>
<mo>(</mo>
<msub>
<mi>q</mi>
<mn>2</mn>
</msub>
<msub>
<mi>q</mi>
<mn>3</mn>
</msub>
<mo>-</mo>
<msub>
<mi>q</mi>
<mn>0</mn>
</msub>
<msub>
<mi>q</mi>
<mn>1</mn>
</msub>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mn>2</mn>
<mrow>
<mo>(</mo>
<msub>
<mi>q</mi>
<mn>1</mn>
</msub>
<msub>
<mi>q</mi>
<mn>3</mn>
</msub>
<mo>-</mo>
<msub>
<mi>q</mi>
<mn>0</mn>
</msub>
<msub>
<mi>q</mi>
<mn>2</mn>
</msub>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
<mtd>
<mrow>
<mn>2</mn>
<mrow>
<mo>(</mo>
<msub>
<mi>q</mi>
<mn>2</mn>
</msub>
<msub>
<mi>q</mi>
<mn>3</mn>
</msub>
<mo>+</mo>
<msub>
<mi>q</mi>
<mn>0</mn>
</msub>
<msub>
<mi>q</mi>
<mn>1</mn>
</msub>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
<mtd>
<mrow>
<msubsup>
<mi>q</mi>
<mn>0</mn>
<mn>2</mn>
</msubsup>
<mo>-</mo>
<msubsup>
<mi>q</mi>
<mn>1</mn>
<mn>2</mn>
</msubsup>
<mo>-</mo>
<msubsup>
<mi>q</mi>
<mn>2</mn>
<mn>2</mn>
</msubsup>
<mo>+</mo>
<msubsup>
<mi>q</mi>
<mn>3</mn>
<mn>2</mn>
</msubsup>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>5</mn>
<mo>)</mo>
</mrow>
</mrow>
其中，(5)式中的qi(i＝1,2,3,4)为(4)式中qi(k+1)(i＝1,2,3,4)，(5)式中省略了(k+1)；
更新载体姿态信息，具体为：
<mrow>
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mrow>
<mi>&phi;</mi>
<mo>=</mo>
<mi>a</mi>
<mi>r</mi>
<mi>c</mi>
<mi>t</mi>
<mi>a</mi>
<mi>n</mi>
<mrow>
<mo>(</mo>
<msub>
<mi>c</mi>
<mn>32</mn>
</msub>
<mo>/</mo>
<msub>
<mi>c</mi>
<mn>33</mn>
</msub>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>&theta;</mi>
<mo>=</mo>
<mi>arcsin</mi>
<mrow>
<mo>(</mo>
<mo>-</mo>
<msub>
<mi>c</mi>
<mn>31</mn>
</msub>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>&psi;</mi>
<mo>=</mo>
<mi>arctan</mi>
<mrow>
<mo>(</mo>
<msub>
<mi>c</mi>
<mn>21</mn>
</msub>
<mo>/</mo>
<msub>
<mi>c</mi>
<mn>11</mn>
</msub>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>6</mn>
<mo>)</mo>
</mrow>
</mrow>
将加速度计沿载体坐标系测量的比力信息，通过捷联矩阵进行投影转换：
<mrow>
<msup>
<mi>f</mi>
<mi>n</mi>
</msup>
<mo>=</mo>
<msubsup>
<mi>C</mi>
<mi>b</mi>
<mi>n</mi>
</msubsup>
<msup>
<mi>f</mi>
<mi>b</mi>
</msup>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>7</mn>
<mo>)</mo>
</mrow>
</mrow>
利用下列微分方程求解载体运动速度：
<mrow>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msubsup>
<mover>
<mi>v</mi>
<mo>&CenterDot;</mo>
</mover>
<mi>x</mi>
<mi>n</mi>
</msubsup>
</mtd>
</mtr>
<mtr>
<mtd>
<msubsup>
<mover>
<mi>v</mi>
<mo>&CenterDot;</mo>
</mover>
<mi>y</mi>
<mi>n</mi>
</msubsup>
</mtd>
</mtr>
<mtr>
<mtd>
<msubsup>
<mover>
<mi>v</mi>
<mo>&CenterDot;</mo>
</mover>
<mi>z</mi>
<mi>n</mi>
</msubsup>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msubsup>
<mi>f</mi>
<mi>x</mi>
<mi>n</mi>
</msubsup>
</mtd>
</mtr>
<mtr>
<mtd>
<msubsup>
<mi>f</mi>
<mi>y</mi>
<mi>n</mi>
</msubsup>
</mtd>
</mtr>
<mtr>
<mtd>
<msubsup>
<mi>f</mi>
<mi>z</mi>
<mi>n</mi>
</msubsup>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mi>g</mi>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>8</mn>
<mo>)</mo>
</mrow>
</mrow>
其中，分别表示解算载体速度在导航系oxn轴、oyn轴、ozn轴上的分量； 表示vx、vy、vz的变化率，即载体沿导航系oxn轴、oyn轴、ozn轴的运动加速度；g为当地重力加速度；
根据k时刻的载体三轴速度vx(k)、vy(k)和vz(k)，求取k时刻载体速度变化率为：
<mrow>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msubsup>
<mover>
<mi>v</mi>
<mo>&CenterDot;</mo>
</mover>
<mi>x</mi>
<mi>n</mi>
</msubsup>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<msubsup>
<mover>
<mi>v</mi>
<mo>&CenterDot;</mo>
</mover>
<mi>y</mi>
<mi>n</mi>
</msubsup>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<msubsup>
<mover>
<mi>v</mi>
<mo>&CenterDot;</mo>
</mover>
<mi>z</mi>
<mi>n</mi>
</msubsup>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msubsup>
<mi>f</mi>
<mi>x</mi>
<mi>n</mi>
</msubsup>
</mtd>
</mtr>
<mtr>
<mtd>
<msubsup>
<mi>f</mi>
<mi>y</mi>
<mi>n</mi>
</msubsup>
</mtd>
</mtr>
<mtr>
<mtd>
<msubsup>
<mi>f</mi>
<mi>z</mi>
<mi>n</mi>
</msubsup>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mi>g</mi>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>9</mn>
<mo>)</mo>
</mrow>
</mrow>
在k+1时刻载体速度和位置分别为：
<mrow>
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mrow>
<msubsup>
<mi>v</mi>
<mi>x</mi>
<mi>n</mi>
</msubsup>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
<mo>=</mo>
<msubsup>
<mi>v</mi>
<mi>x</mi>
<mi>n</mi>
</msubsup>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msubsup>
<mover>
<mi>v</mi>
<mo>&CenterDot;</mo>
</mover>
<mi>x</mi>
<mi>n</mi>
</msubsup>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mi>T</mi>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msubsup>
<mi>v</mi>
<mi>y</mi>
<mi>n</mi>
</msubsup>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
<mo>=</mo>
<msubsup>
<mi>v</mi>
<mi>y</mi>
<mi>n</mi>
</msubsup>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msubsup>
<mover>
<mi>v</mi>
<mo>&CenterDot;</mo>
</mover>
<mi>y</mi>
<mi>n</mi>
</msubsup>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mi>T</mi>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msubsup>
<mi>v</mi>
<mi>z</mi>
<mi>n</mi>
</msubsup>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
<mo>=</mo>
<msubsup>
<mi>v</mi>
<mi>z</mi>
<mi>n</mi>
</msubsup>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msubsup>
<mover>
<mi>v</mi>
<mo>&CenterDot;</mo>
</mover>
<mi>z</mi>
<mi>n</mi>
</msubsup>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mi>T</mi>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>10</mn>
<mo>)</mo>
</mrow>
</mrow>
<mrow>
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mrow>
<msubsup>
<mi>p</mi>
<mi>x</mi>
<mi>n</mi>
</msubsup>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
<mo>=</mo>
<msubsup>
<mi>v</mi>
<mi>x</mi>
<mi>n</mi>
</msubsup>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msubsup>
<mover>
<mi>v</mi>
<mo>&CenterDot;</mo>
</mover>
<mi>x</mi>
<mi>n</mi>
</msubsup>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mi>T</mi>
<mo>+</mo>
<msubsup>
<mi>f</mi>
<mi>x</mi>
<mi>n</mi>
</msubsup>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<msup>
<mi>T</mi>
<mn>2</mn>
</msup>
<mo>/</mo>
<mn>2</mn>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msubsup>
<mi>p</mi>
<mi>y</mi>
<mi>n</mi>
</msubsup>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
<mo>=</mo>
<msubsup>
<mi>v</mi>
<mi>y</mi>
<mi>n</mi>
</msubsup>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msubsup>
<mover>
<mi>v</mi>
<mo>&CenterDot;</mo>
</mover>
<mi>y</mi>
<mi>n</mi>
</msubsup>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mi>T</mi>
<mo>+</mo>
<msubsup>
<mi>f</mi>
<mi>y</mi>
<mi>n</mi>
</msubsup>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<msup>
<mi>T</mi>
<mn>2</mn>
</msup>
<mo>/</mo>
<mn>2</mn>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msubsup>
<mi>p</mi>
<mi>z</mi>
<mi>n</mi>
</msubsup>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
<mo>=</mo>
<msubsup>
<mi>v</mi>
<mi>z</mi>
<mi>n</mi>
</msubsup>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msubsup>
<mover>
<mi>v</mi>
<mo>&CenterDot;</mo>
</mover>
<mi>z</mi>
<mi>n</mi>
</msubsup>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mi>T</mi>
<mo>+</mo>
<msubsup>
<mi>f</mi>
<mi>z</mi>
<mi>n</mi>
</msubsup>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<msup>
<mi>T</mi>
<mn>2</mn>
</msup>
<mo>/</mo>
<mn>2</mn>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>11</mn>
<mo>)</mo>
</mrow>
</mrow>
其中，分别表示载体位置在导航系oxn轴、oyn轴、ozn轴上的分量，当k＝1时，vx(1)、vy(1)、vz(1)为步骤一中初始化系统时获得的载体初始速度，为步骤一中初始化系统时获得的载体初始位置。
4.根据权利要求1所述的一种基于正反向滤波的惯性行人定位系统误差抑制方法，其特征在于，所述的步骤(7)具体为：
读取步骤(6)中零速区间长度L，读取区间内的数据f、ω；记第i组数据为ui＝[fi ωi]T，则零速区间中数据可写作U＝[u1 u2 … uL]；
记D＝[-uL -uL-1 … -u1]，扩展零速区间至sL，得到一般取s＝5，则有Data＝[U D U D U]。
5.根据权利要求1所述的一种基于正反向滤波的惯性行人定位系统误差抑制方法，其特征在于，所述的步骤(8)具体为：
根据步骤七中Data，解算行人的速度位置和姿态
和转换矩阵的计算与步骤四相同；
理论上行人脚部触地时，位置保持不变，k+1时刻载体速度和位置分别为：
<mrow>
<msubsup>
<mover>
<mi>v</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
</mrow>
<mi>n</mi>
</msubsup>
<mo>=</mo>
<msubsup>
<mi>v</mi>
<mi>k</mi>
<mi>n</mi>
</msubsup>
</mrow>
<mrow>
<msubsup>
<mover>
<mi>p</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
</mrow>
<mi>n</mi>
</msubsup>
<mo>=</mo>
<msubsup>
<mi>p</mi>
<mi>k</mi>
<mi>n</mi>
</msubsup>
</mrow>
其中，分别为ν，p，ε，的计算值。
6.根据权利要求1所述的一种基于正反向滤波的惯性行人定位系统误差抑制方法，其特征在于，所述的步骤(9)具体为：
对Data进行正反向滤波，滤波状态量为X＝[δpn δνn δεn]，观测量为z＝δνn，滤波过程如下：
利用下列微分方程建立载体运动速度、位置和姿态的误差模型：
<mrow>
<mi>&delta;</mi>
<mover>
<mi>p</mi>
<mo>&CenterDot;</mo>
</mover>
<mo>=</mo>
<mi>&delta;</mi>
<mi>v</mi>
</mrow>
<mrow>
<mi>&delta;</mi>
<mover>
<mi>v</mi>
<mo>&CenterDot;</mo>
</mover>
<mo>=</mo>
<mo>&lsqb;</mo>
<msup>
<mi>f</mi>
<mi>n</mi>
</msup>
<mo>&times;</mo>
<mo>&rsqb;</mo>
<mi>&delta;</mi>
<mi>&epsiv;</mi>
</mrow>
<mrow>
<mi>&delta;</mi>
<mover>
<mi>&epsiv;</mi>
<mo>&CenterDot;</mo>
</mover>
<mo>=</mo>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>1</mn>
</mrow>
</msub>
</mrow>
其中：δp为载体位置误差，且δν为载体速度误差，且δε为载体姿态角误差并且有δε＝[δφδθδψ]；分别为δp，δν，δε的变化率，ωb为载体角速度，且[fn×]为由加速度信息构成的反对称阵，且03×1为三行一列的零向量；
根据k时刻的载体位置误差δp(k)，载体速度误差δν(k)，载体姿态角误差δε(k)，求取k时刻载体位置误差变化率、速度误差变化率、姿态角误差变化率为：
<mrow>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mi>&delta;</mi>
<mover>
<mi>p</mi>
<mo>&CenterDot;</mo>
</mover>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<mi>&delta;</mi>
<mover>
<mi>v</mi>
<mo>&CenterDot;</mo>
</mover>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<mi>&delta;</mi>
<mover>
<mi>&epsiv;</mi>
<mo>&CenterDot;</mo>
</mover>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mi>I</mi>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<mo>&lsqb;</mo>
<mi>f</mi>
<mo>&times;</mo>
<mo>&rsqb;</mo>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mi>&delta;</mi>
<mover>
<mi>p</mi>
<mo>&CenterDot;</mo>
</mover>
</mtd>
</mtr>
<mtr>
<mtd>
<mi>&delta;</mi>
<mover>
<mi>v</mi>
<mo>&CenterDot;</mo>
</mover>
</mtd>
</mtr>
<mtr>
<mtd>
<mi>&delta;</mi>
<mover>
<mi>&epsiv;</mi>
<mo>&CenterDot;</mo>
</mover>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
在k+1时刻载体位置误差δp(k+1)、速度误差δν(k+1)、姿态角误差δε(k+1)分别为：
<mrow>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mi>&delta;</mi>
<mi>p</mi>
<mo>(</mo>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
<mo>)</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<mi>&delta;</mi>
<mi>v</mi>
<mo>(</mo>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
<mo>)</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<mi>&delta;</mi>
<mi>&epsiv;</mi>
<mo>(</mo>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
<mo>)</mo>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<mrow>
<mo>(</mo>
<mrow>
<mi>I</mi>
<mo>+</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mi>I</mi>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<mo>&lsqb;</mo>
<mi>f</mi>
<mo>&times;</mo>
<mo>&rsqb;</mo>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
<mo>)</mo>
</mrow>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mi>&delta;</mi>
<mi>p</mi>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<mi>&delta;</mi>
<mi>v</mi>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<mi>&delta;</mi>
<mi>&epsiv;</mi>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
由于理论上行人脚部触地时，应有ν＝0，故此时系统解算出的速度信息ν为误差量，即ν＝δν，其中δν为速度误差；
根据上述误差模型，利用卡尔曼滤波器，对行人的位姿信息进行误差修正，具体算法如下：
<mrow>
<msub>
<mi>P</mi>
<mi>k</mi>
</msub>
<mo>=</mo>
<msub>
<mi>F</mi>
<mrow>
<mi>k</mi>
<mo>,</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<msub>
<mi>P</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<msubsup>
<mi>F</mi>
<mrow>
<mi>k</mi>
<mo>,</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
<mi>T</mi>
</msubsup>
<mo>+</mo>
<msub>
<mi>G</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<msub>
<mi>Q</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<msubsup>
<mi>G</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
<mi>T</mi>
</msubsup>
</mrow>
Kk＝PkHT(HPkHT+R)-1
其中，Pk为k时刻的估计均方误差阵，Fk,k-1为k-1时刻到k时刻的为状态转移矩阵，且I为单位阵，0为零矩阵，为k-1时刻的噪声驱动阵，Kk为k时刻的滤波增益阵；
Xk＝-KkZk
更新Pk：
<mrow>
<msub>
<mi>P</mi>
<msub>
<mi>k</mi>
<mi>new</mi>
</msub>
</msub>
<mo>=</mo>
<mrow>
<mo>(</mo>
<mi>I</mi>
<mo>-</mo>
<msub>
<mi>K</mi>
<mi>k</mi>
</msub>
<mi>H</mi>
<mo>)</mo>
</mrow>
<msub>
<mi>P</mi>
<mi>k</mi>
</msub>
</mrow>
其中，Xk为k时刻的状态变量，当k＝1时，状态量X＝[δp(1) δν(1) δε(1)]为步骤一中初始化系统时获得的载体初始状态量X0，为更新后的Pk；保存Pk与Xk，并将其作为下一次滤波的输入。
7.根据权利要求1所述的一种基于正反向滤波的惯性行人定位系统误差抑制方法，其特征在于，所述的步骤(10)具体为：
利用步骤九中解算出的δν，δp，δε补偿步骤八中的更新四元数qk，并将补偿后的结果作为最终导航输出：
位置、速度信息补偿：
<mrow>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msubsup>
<mi>v</mi>
<mi>k</mi>
<mi>n</mi>
</msubsup>
</mtd>
</mtr>
<mtr>
<mtd>
<msubsup>
<mi>p</mi>
<mi>k</mi>
<mi>n</mi>
</msubsup>
</mtd>
</mtr>
<mtr>
<mtd>
<msubsup>
<mi>&epsiv;</mi>
<mi>k</mi>
<mi>n</mi>
</msubsup>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msubsup>
<mover>
<mi>v</mi>
<mo>^</mo>
</mover>
<mi>k</mi>
<mi>n</mi>
</msubsup>
</mtd>
</mtr>
<mtr>
<mtd>
<msubsup>
<mover>
<mi>p</mi>
<mo>^</mo>
</mover>
<mi>k</mi>
<mi>n</mi>
</msubsup>
</mtd>
</mtr>
<mtr>
<mtd>
<msubsup>
<mover>
<mi>&epsiv;</mi>
<mo>^</mo>
</mover>
<mi>k</mi>
<mi>n</mi>
</msubsup>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>+</mo>
<msub>
<mi>X</mi>
<mi>k</mi>
</msub>
</mrow>
姿态角补偿：
令利用公式更新转换矩阵利用式(6)求出更新后的φ，θ，ψ；
其中，为矩阵的逆运算；
更新四元数：
令则
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201711220837.7A CN108007477B (zh) | 2017-11-29 | 2017-11-29 | 一种基于正反向滤波的惯性行人定位系统误差抑制方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201711220837.7A CN108007477B (zh) | 2017-11-29 | 2017-11-29 | 一种基于正反向滤波的惯性行人定位系统误差抑制方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN108007477A true CN108007477A (zh) | 2018-05-08 |
| CN108007477B CN108007477B (zh) | 2020-09-25 |
Family
ID=62054570
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201711220837.7A Active CN108007477B (zh) | 2017-11-29 | 2017-11-29 | 一种基于正反向滤波的惯性行人定位系统误差抑制方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN108007477B (zh) |
Cited By (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109855620A (zh) * | 2018-12-26 | 2019-06-07 | 北京壹氢科技有限公司 | 一种基于自回溯算法的室内行人导航方法 |
| CN110274592A (zh) * | 2019-07-18 | 2019-09-24 | 北京航空航天大学 | 一种腰部脚部惯性测量单元信息融合的零速区间确定方法 |
| CN111024070A (zh) * | 2019-12-23 | 2020-04-17 | 哈尔滨工程大学 | 一种基于航向自观测的惯性足绑式行人定位方法 |
| CN111189443A (zh) * | 2020-01-14 | 2020-05-22 | 电子科技大学 | 一种在线校准步长、修正运动偏差角和自适应能量管理的行人导航方法 |
| CN111521179A (zh) * | 2020-04-28 | 2020-08-11 | 中国人民解放军国防科技大学 | 基于行进速度自检测的钻探用定位定向仪孔内定位方法 |
| CN112762944A (zh) * | 2020-12-25 | 2021-05-07 | 上海商汤临港智能科技有限公司 | 零速区间检测及零速更新方法 |
| CN112781622A (zh) * | 2020-12-31 | 2021-05-11 | 厦门华源嘉航科技有限公司 | 一种行人导航mimu安装误差在线标定方法 |
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2014147544A1 (en) * | 2013-03-21 | 2014-09-25 | Sony Corporation | Recalibrating an inertial navigation system |
| CN106225786A (zh) * | 2016-08-15 | 2016-12-14 | 北京理工大学 | 一种自适应的行人导航系统零速区间检测方法 |
-
2017
- 2017-11-29 CN CN201711220837.7A patent/CN108007477B/zh active Active
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2014147544A1 (en) * | 2013-03-21 | 2014-09-25 | Sony Corporation | Recalibrating an inertial navigation system |
| CN106225786A (zh) * | 2016-08-15 | 2016-12-14 | 北京理工大学 | 一种自适应的行人导航系统零速区间检测方法 |
Non-Patent Citations (1)
| Title |
|---|
| 郭铮等: "Research on Indoor Pedestrian Location Based on Miniature Inertial Measurement Unit", 《2017 FORUM ON COOPERATIVE POSITIONING AND SERVICE (CPGPS）》 * |
Cited By (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109855620A (zh) * | 2018-12-26 | 2019-06-07 | 北京壹氢科技有限公司 | 一种基于自回溯算法的室内行人导航方法 |
| CN110274592A (zh) * | 2019-07-18 | 2019-09-24 | 北京航空航天大学 | 一种腰部脚部惯性测量单元信息融合的零速区间确定方法 |
| CN110274592B (zh) * | 2019-07-18 | 2021-07-27 | 北京航空航天大学 | 一种腰部脚部惯性测量单元信息融合的零速区间确定方法 |
| CN111024070A (zh) * | 2019-12-23 | 2020-04-17 | 哈尔滨工程大学 | 一种基于航向自观测的惯性足绑式行人定位方法 |
| CN111189443A (zh) * | 2020-01-14 | 2020-05-22 | 电子科技大学 | 一种在线校准步长、修正运动偏差角和自适应能量管理的行人导航方法 |
| CN111521179A (zh) * | 2020-04-28 | 2020-08-11 | 中国人民解放军国防科技大学 | 基于行进速度自检测的钻探用定位定向仪孔内定位方法 |
| CN111521179B (zh) * | 2020-04-28 | 2021-01-15 | 中国人民解放军国防科技大学 | 基于行进速度自检测的钻探用定位定向仪孔内定位方法 |
| CN112762944A (zh) * | 2020-12-25 | 2021-05-07 | 上海商汤临港智能科技有限公司 | 零速区间检测及零速更新方法 |
| CN112762944B (zh) * | 2020-12-25 | 2024-03-22 | 上海商汤临港智能科技有限公司 | 零速区间检测及零速更新方法 |
| CN112781622A (zh) * | 2020-12-31 | 2021-05-11 | 厦门华源嘉航科技有限公司 | 一种行人导航mimu安装误差在线标定方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN108007477B (zh) | 2020-09-25 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108007477B (zh) | 一种基于正反向滤波的惯性行人定位系统误差抑制方法 | |
| CN108362282B (zh) | 一种基于自适应零速区间调整的惯性行人定位方法 | |
| Ludwig et al. | Comparison of Euler estimate using extended Kalman filter, Madgwick and Mahony on quadcopter flight data | |
| CN105300379B (zh) | 一种基于加速度的卡尔曼滤波姿态估计方法及系统 | |
| US9026263B2 (en) | Automotive navigation system and method to utilize internal geometry of sensor position with respect to rear wheel axis | |
| CN102445200B (zh) | 微小型个人组合导航系统及其导航定位方法 | |
| Niu et al. | An accurate land‐vehicle MEMS IMU/GPS navigation system using 3D auxiliary velocity updates | |
| CN103616030A (zh) | 基于捷联惯导解算和零速校正的自主导航系统定位方法 | |
| CN103196445B (zh) | 基于匹配技术的地磁辅助惯性的载体姿态测量方法 | |
| CN110207692B (zh) | 一种地图辅助的惯性预积分行人导航方法 | |
| CN111024070A (zh) | 一种基于航向自观测的惯性足绑式行人定位方法 | |
| US20140222369A1 (en) | Simplified method for estimating the orientation of an object, and attitude sensor implementing such a method | |
| CN106662443A (zh) | 用于垂直轨迹确定的方法和系统 | |
| CN112683269B (zh) | 一种附有运动加速度补偿的marg姿态计算方法 | |
| CN113175933B (zh) | 一种基于高精度惯性预积分的因子图组合导航方法 | |
| CN111121820B (zh) | 基于卡尔曼滤波的mems惯性传感器阵列融合方法 | |
| US7711516B2 (en) | Method for estimating movement of a solid | |
| CN110672095A (zh) | 一种基于微惯导的行人室内自主定位算法 | |
| CN106370178A (zh) | 移动终端设备的姿态测量方法及装置 | |
| CN113340298A (zh) | 一种惯导和双天线gnss外参标定方法 | |
| CN109764870A (zh) | 基于变换估计量建模方案的载体初始航向估算方法 | |
| CN104897155A (zh) | 一种个人携行式多源定位信息辅助修正方法 | |
| CN103256932B (zh) | 一种替换结合外推的着陆导航方法 | |
| RU2487318C1 (ru) | Бесплатформенная инерциальная курсовертикаль на чувствительных элементах средней точности | |
| CN112325878A (zh) | 基于ukf与空中无人机节点辅助的地面载体组合导航方法 |
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 |