CN107255474A - 一种融合电子罗盘和陀螺仪的pdr航向角确定方法 - Google Patents

一种融合电子罗盘和陀螺仪的pdr航向角确定方法 Download PDF

Info

Publication number
CN107255474A
CN107255474A CN201710330467.6A CN201710330467A CN107255474A CN 107255474 A CN107255474 A CN 107255474A CN 201710330467 A CN201710330467 A CN 201710330467A CN 107255474 A CN107255474 A CN 107255474A
Authority
CN
China
Prior art keywords
mrow
msub
mtd
msubsup
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
Application number
CN201710330467.6A
Other languages
English (en)
Other versions
CN107255474B (zh
Inventor
姚瑞丽
姚英彪
冯维
严军荣
姜显阳
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Zhonglian Jinguan Information Technology Beijing Co ltd
Original Assignee
Hangzhou Dianzi University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Hangzhou Dianzi University filed Critical Hangzhou Dianzi University
Priority to CN201710330467.6A priority Critical patent/CN107255474B/zh
Publication of CN107255474A publication Critical patent/CN107255474A/zh
Application granted granted Critical
Publication of CN107255474B publication Critical patent/CN107255474B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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
    • G01C1/00—Measuring angles
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01P—MEASURING LINEAR OR ANGULAR SPEED, ACCELERATION, DECELERATION, OR SHOCK; INDICATING PRESENCE, ABSENCE, OR DIRECTION, OF MOVEMENT
    • G01P13/00—Indicating or recording presence, absence, or direction, of movement
    • G01P13/02—Indicating direction only, e.g. by weather vane
    • G01P13/025—Indicating direction only, e.g. by weather vane indicating air data, i.e. flight variables of an aircraft, e.g. angle of attack, side slip, shear, yaw

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Automation & Control Theory (AREA)
  • Navigation (AREA)
  • Traffic Control Systems (AREA)
  • Other Investigation Or Analysis Of Materials By Electrical Means (AREA)

Abstract

本发明公开了一种融合电子罗盘和陀螺仪的PDR航向角确定方法。本发明包括下面3个步骤：步骤1.通过电子罗盘得到航向角ψelec；步骤2.通过陀螺仪得到航向角ψgyro；步骤3.自适应方式融合电子罗盘和陀螺仪确定航行角ψk，3.1利用电子罗盘滤波后的数据初始化航向角，3.2利用融合算法确定航向角ψk。本发明能够提高航向角的估计精度，从而大幅度提高PDR定位精度。本发明可以应用于内置惯性传感器的便携式设备中，所内置的惯性传感器是指三轴磁力计、三轴加速度计和三轴陀螺仪三种。

Description

一种融合电子罗盘和陀螺仪的PDR航向角确定方法
技术领域
本发明属于信息技术领域，具体涉及一种融合电子罗盘和陀螺仪的PDR航向角确定方法。
背景技术
近几年，随着微电子机械系统(MEMS)的发展，惯性传感器在智能移动终端的应用越来越普及，这使得基于惯性传感器的行人室内导航系统，因其无需铺设外部设施的低成本优势，逐渐成为室内定位技术研究热门之一。
基于MEMS惯性传感器的定位技术按照计算运动距离的方法可以分为惯性导航系统(INS)和基于运动模型的行人航迹推算(PDR)两类。INS主要依靠对加速度传感器的二次积分来计算运动距离，它主要用于车载、飞机、武器等导航领域；PDR主要依靠运动模型来计算人的运动步长，从而估算运动距离，该技术主要用于室内的行人定位和导航系统中。
PDR定位包含步态检测、步长估计和航向角估计三部分。其中，航向角估计是PDR定位中的关键环节，直接影响定位精度，它的稍许误差就会导致最终定位误差的迅速增长。PDR定位中的航向角一般通过方向传感器获取的数据得到，主要包括电子罗盘(一般由磁力计和加速度计组成)和陀螺仪。电子罗盘可以获取航向角的绝对值；陀螺仪只能获得角速度的变化量，需要对获取的角速度相对时间进行积分，才可以得到航向角的变化值。在实际应用中，电子罗盘和陀螺仪获取航向角各有优缺点。电子罗盘容易受到磁介质的影响，使得观测的角度值随机误差较大；而陀螺仪通过数据积分获取的角度变化值会出现累积误差问题。因此，直接通过电子罗盘和陀螺仪获取航向角都存在较大误差，它是PDR定位误差的主要来源。
发明内容
为解决传统PDR定位航向角估计精度不高的问题，本发明公布一种融合电子罗盘和陀螺仪的PDR航向角确定方法。本发明能够提高航向角的估计精度，从而大幅度提高PDR定位精度。本发明可以应用于内置惯性传感器的便携式设备中，如智能手机、掌上电脑、个人数字设备和智能佩戴设备等。所内置的惯性传感器是指三轴磁力计、三轴加速度计和三轴陀螺仪三种。
本发明解决其技术问题所采样的技术方案具体包括下面3个步骤：
步骤1.通过电子罗盘得到航向角ψelec；
步骤2.通过陀螺仪得到航向角ψgyro；
步骤3.自适应方式融合电子罗盘和陀螺仪确定航行角ψk。
所述步骤1中的电子罗盘由加速度计和磁力计组成，所述的通过电子罗盘得到航向角ψelec具体包括下述步骤：
1.1利用便携式设备内置的三轴加速度计和三轴磁力计采集在载体坐标系下的测量值。
所述的载体坐标系(简称b系)，其中X轴是水平指向右，Y轴是垂直向上，Z轴是指向屏幕正面之外。
记加速度计在b系下采集到的X、Y和Z轴的测量值为ax、ay和az。
记磁力计在b系下采集到的X、Y和Z轴的测量值为hx、hy和hz。
1.2根据b系下的加速度计和磁力计的测量值计算地球坐标系下的航向角。
所述的地球坐标系(简称n系)，其中X轴是与地面相切指向东，Y轴是与地面相切指向北，Z轴是垂直于地面指向天空。
一般来说，行人行走过程中b系与n系并不重合，需要将b系坐标系依次经过翻滚角γ、俯仰角θ和航向角ψ三次旋转才能得到n系坐标系，该过程用下面的旋转矩阵来描述：
上述公式(1)里面的俯仰角θ和翻滚角γ根据加速度计的测量值计算得到，具体计算公式如下：
上述公式(1)里面的航向角ψ根据下面两步得到：
第1步：利用公式(2)所求得的俯仰角θ、翻滚角γ和磁力计在b系下的测量值计算磁力计在n系下X、Y轴的分量Hx和Hy。
第2步：利用下面公式计算航向角ψ，也就是电子罗盘在n系下的航向角ψelec。
1.3利用下述公式对航向角ψelec进行滑动低通滤波，降低随机噪声的影响。
其中，N是滤波窗口的大小。
所述步骤2中通过陀螺仪得到航向角ψgyro具体包括下述步骤：
2.1利用便携式设备内置的三轴陀螺仪采集在b系下的测量值。
记陀螺仪在b系下采集到的X、Y和Z轴的角速度测量值为wx、wy和wz。
2.2根据b系下的陀螺仪的测量值计算n系下的航向角ψgyro。
所述的航向角ψgyro根据下面两步得到：
第1步：利用b系下的陀螺仪的测量值更新四元数Q
所述的四元数定义如下：
分别将便携式设备初始时刻加速度计和磁力计的测量值先后带入到上述公式(2)、(4)和(1)，得到当前旋转矩阵同时四元数也能够确定旋转矩阵
利用公式(7)求出初始时刻的四元数Q，具体公式如下所示：
由于行人是移动的，所以四元数是一个变量。利用下述公式对四元数进行更新：
式中，k＝1,2,...n，dt是采样时间。
第2步：利用上步得到的四元数根据下面的公式计算陀螺仪在n系下的航向角ψgyro。
所述步骤3中的自适应方式融合电子罗盘和陀螺仪确定航行角ψ具体包括下述步骤：
3.1利用电子罗盘滤波后的数据初始化航向角，
3.2利用融合算法确定航向角ψk。
所述3.2中的航向角ψk根据下面三步得到：
第1步：通过电子罗盘得到的航向角ψelec、陀螺仪得到的航向角ψgyro以及前一时刻的航向角ψk-1得到质心Z，具体计算公式如下：
式中a1、a2、和a3分别是在计算质心时的权重，由电子罗盘和陀螺仪的特性决定。
第2步:计算自适应权重ωgyro，ωelec和ωprev
所述的权重ωgyro，ωelec和ωprev分别为陀螺仪，电子罗盘和前一时刻航向角在得到当前时刻融合算法航向角中所占的权重。
利用下面公式计算ωgyro，ωelec和ωprev：
ωprev＝1/abs(ψk-1-Zk)
第3步：利用下面公式计算航向角ψk，也就是电子罗盘和陀螺仪融合后确定的n系下的航向角。
本发明有益效果如下：
本发明公布的自适应融合电子罗盘和陀螺仪航向角的方法可以提高航向角的估计精度，从而大幅度提高PDR定位精度。具体来说，在行人运动航向角的确定中，基于电子罗盘和陀螺仪的优缺点以及行人的运动特点，利用自适应的方式实时调整权重参数，动态加权融合电子罗盘和陀螺仪的输出，提高了PDR航向角的估计精度。本发明应用于内置惯性传感器的便携式设备中，这样拥有设备的用户就能够很好在地铁、医院、超市等一些公共场所快速、有效获得用户实时位置，在现实中具有很好的实用性和应用前景。
附图说明
图1是两种坐标系的示意图；
图2是融合电子罗盘与陀螺仪算法框图；
图3是定位轨迹路线图；
图4是定位误差CDF图；
具体实施方式
下面结合智能手机Google nexus(内置三轴磁力计、三轴加速度计和三轴陀螺仪)和附图对本发明做进一步的说明。
如图2所示，一种融合电子罗盘和陀螺仪的PDR航向角确定方法，具体包括下面3个步骤：
步骤1.通过电子罗盘得到航向角ψelec；
步骤2.通过陀螺仪得到航向角ψgyro；
步骤3.自适应方式融合电子罗盘和陀螺仪确定航行角ψk。
所述步骤1中通过电子罗盘得到航向角ψelec具体包括下述步骤：
所述步骤1中的电子罗盘由加速度计和磁力计组成，所述的通过电子罗盘得到航向角ψelec具体包括下述步骤：
1.1利用智能手机内置的三轴加速度计和三轴磁力计采集b系下的测量值。
记加速度计在b系下采集到的X、Y和Z轴的测量值为ax、ay和az。
记磁力计在b系下采集到的X、Y和Z轴的测量值为hx、hy和hz。
1.2根据b系下的加速度计和磁力计的测量值计算n系下的航向角。
一般来说，行人行走过程中手持智能手机的b系与n系并不重合，需要将b系坐标系依次经过翻滚角γ、俯仰角θ和航向角ψ三次旋转才能得到n系坐标系，这个过程可以用下面的旋转矩阵来描述：
上述公式(14)里面的俯仰角θ和翻滚角γ根据加速度计的测量值计算得到，具体计算公式如下：
上述公式(14)里面的航向角ψ根据下面两步得到：
第1步：利用公式(15)所求得的俯仰角θ、翻滚角γ和磁力计在b系下的测量值计算磁力计在n系下X、Y轴的分量Hx和Hy。
第2步：利用下面公式计算航向角ψ，也就是电子罗盘在n系下的航向角ψelec。
1.3利用下述公式对航向角ψelec进行滑动低通滤波，降低随机噪声的影响。
其中，N是滤波窗口的大小，本实例中将N设置为4。
所述步骤2中通过陀螺仪得到航向角ψgyro具体包括下述步骤：
2.1利用智能手机内置的三轴陀螺仪采集b系下的测量值。
记陀螺仪在b系下X、Y和Z轴的测量值为wx、wy和wz。
2.2根据b系下的陀螺仪的测量值计算n系下的航向角ψgyro。
所述的航向角ψgyro根据下面两步得到：
第1步：利用b系下的陀螺仪的测量值更新四元数Q
所述的四元数定义如下：
分别将智能手机初始时刻加速度计和磁力计的测量值先后带入到上述公式(15)、(17)和(14)，得到当前旋转矩阵同时四元数也可以确定旋转矩阵
利用公式(20)可以求出初始时刻的四元数Q，具体公式如下所示：
由于行人是移动的，所以四元数是一个变量。利用下述公式对四元数进行更新：
式中，k＝1,2,...n，dt是采样时间。
第2步：利用上步得到的四元数根据下面的公式计算陀螺仪在n系下的航向角ψgyro。
所述步骤3中的自适应方式融合电子罗盘和陀螺仪确定航行角ψ具体包括下述步骤：
3.1利用电子罗盘滤波后的数据初始化航向角，
3.2利用融合算法确定航向角ψk。
所述3.2中的航向角ψk根据下面三步得到：
第1步：通过电子罗盘得到的航向角ψelec、陀螺仪得到的航向角ψgyro以及前一时刻的航向角ψk-1得到质心Z，具体计算公式如下：
式中a1、a2、和a3分别是在计算质心时的权重，由电子罗盘和陀螺仪的特性决定，本实例中将a1、a2、和a3分别设置为1、2和4。
第2步：计算自适应权重ωgyro，ωelec和ωprev
所述的权重ωgyro，ωelec和ωprev分别为陀螺仪、电子罗盘和前一时刻航向角在得到当前时刻融合算法航向角中所占的权重。
利用下面公式计算ωgyro，ωelec和ωprev：
ωprev＝1/abs(ψk-1-Zk)
第3步：利用下面公式计算航向角ψk，也就是电子罗盘和陀螺仪融合后确定的n系下的航向角。
实验验证
分别将所述的电子罗盘、陀螺仪和融合算法得到的航向角带入到PDR算法，以智能手机Google nexus为载体，其内置三轴磁力计、三轴加速度计和三轴陀螺仪，在我校1教4楼进行实验验证，定位轨迹路线图如图3所示。
从图3中可以看出，本发明提出的RPDR算法(融合电子罗盘和陀螺仪的PDR算法)得到的定位轨迹与标准路线最接近。相比之下，EPDR算法(基于电子罗盘的PDR算法)得到的定位轨迹在室内环境中由于磁失真和标准路线出现了偏差。而GPDR算法(基于陀螺仪的PDR算法)得到的路线基本不会受到外界环境的影响，但是随着时间的增加，积分产生的累计误差也使得GPDR算法的定位轨迹慢慢偏离标准路线。本发明提出的融合电子罗盘和陀螺仪的PDR航向角确定方法可以很好的适应传感器的变化，在复杂室内环境中也可以很好的消除电子罗盘和陀螺仪的缺陷，所以RPDR算法定位精度最高，定位误差累计分布函数如图4所示。
对于本发明方法的以下几点说明：
(1)本发明中未明确的组成部分和具体一些算法都可以用现有技术实现。
(2)本发明提供了一种融合电子罗盘和陀螺仪的PDR航向角确定方法，该方法的实施基于惯性传感器，航向角的精度取决于惯性传感器的性能。
(3)最后应当说明的是，以上内容仅用以说明本发明的技术方案，而非对本发明保护范围的限制，本领域的普通技术人员对本发明的技术方案进行的简单修改或者等同替换，均不脱离本发明技术方案的实质和范围。

Claims (3)

1.一种融合电子罗盘和陀螺仪的PDR航向角确定方法，其特征在于包括下面3个步骤：
步骤1.通过电子罗盘得到航向角ψelec；
步骤2.通过陀螺仪得到航向角ψgyro；
步骤3.自适应方式融合电子罗盘和陀螺仪确定航行角ψk；
所述步骤3中的自适应方式融合电子罗盘和陀螺仪确定航行角ψ具体包括下述步骤：
3.1利用电子罗盘滤波后的数据初始化航向角，
3.2利用融合算法确定航向角ψk；
所述3.2中的航向角ψk根据下面三步得到：
第1步：通过电子罗盘得到的航向角ψelec、陀螺仪得到的航向角ψgyro以及前一时刻的航向角ψk-1得到质心Z，具体计算公式如下：
<mrow> <msub> <mi>Z</mi> <mi>k</mi> </msub> <mo>=</mo> <mrow> <mo>(</mo> <mi>a</mi> <mn>1</mn> <mo>*</mo> <msubsup> <mi>&amp;psi;</mi> <mi>k</mi> <mrow> <mi>g</mi> <mi>y</mi> <mi>r</mi> <mi>o</mi> </mrow> </msubsup> <mo>+</mo> <mi>a</mi> <mn>2</mn> <mo>*</mo> <msub> <mi>&amp;psi;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>+</mo> <mi>a</mi> <mn>3</mn> <mo>*</mo> <msubsup> <mi>&amp;psi;</mi> <mi>k</mi> <mrow> <mi>e</mi> <mi>l</mi> <mi>e</mi> <mi>c</mi> </mrow> </msubsup> <mo>)</mo> </mrow> <mo>/</mo> <mrow> <mo>(</mo> <mi>a</mi> <mn>1</mn> <mo>+</mo> <mi>a</mi> <mn>2</mn> <mo>+</mo> <mi>a</mi> <mn>3</mn> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>1</mn> <mo>)</mo> </mrow> </mrow>
式中a1、a2、和a3分别是在计算质心时的权重，由电子罗盘和陀螺仪的特性决定；
第2步:计算自适应权重ωgyro，ωelec和ωprev
所述的权重ωgyro，ωelec和ωprev分别为陀螺仪，电子罗盘和前一时刻航向角在得到当前时刻融合算法航向角中所占的权重；
利用下面公式计算ωgyro，ωelec和ωprev：
<mrow> <mtable> <mtr> <mtd> <mrow> <msup> <mi>&amp;omega;</mi> <mrow> <mi>g</mi> <mi>y</mi> <mi>r</mi> <mi>o</mi> </mrow> </msup> <mo>=</mo> <mn>1</mn> <mo>/</mo> <mi>a</mi> <mi>b</mi> <mi>s</mi> <mrow> <mo>(</mo> <mrow> <msubsup> <mi>&amp;psi;</mi> <mi>k</mi> <mrow> <mi>g</mi> <mi>y</mi> <mi>r</mi> <mi>o</mi> </mrow> </msubsup> <mo>-</mo> <msub> <mi>Z</mi> <mi>k</mi> </msub> </mrow> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msup> <mi>&amp;omega;</mi> <mrow> <mi>e</mi> <mi>l</mi> <mi>e</mi> <mi>c</mi> </mrow> </msup> <mo>=</mo> <mn>1</mn> <mo>/</mo> <mi>a</mi> <mi>b</mi> <mi>s</mi> <mrow> <mo>(</mo> <mrow> <msubsup> <mi>&amp;psi;</mi> <mi>k</mi> <mrow> <mi>e</mi> <mi>l</mi> <mi>e</mi> <mi>c</mi> </mrow> </msubsup> <mo>-</mo> <msub> <mi>Z</mi> <mi>k</mi> </msub> </mrow> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msup> <mi>&amp;omega;</mi> <mrow> <mi>p</mi> <mi>r</mi> <mi>e</mi> <mi>v</mi> </mrow> </msup> <mo>=</mo> <mn>1</mn> <mo>/</mo> <mi>a</mi> <mi>b</mi> <mi>s</mi> <mrow> <mo>(</mo> <mrow> <msub> <mi>&amp;psi;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>-</mo> <msub> <mi>Z</mi> <mi>k</mi> </msub> </mrow> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>2</mn> <mo>)</mo> </mrow> </mrow>
第3步：利用下面公式计算航向角ψk，也就是电子罗盘和陀螺仪融合后确定的n系下的航向角；
<mrow> <msub> <mi>&amp;psi;</mi> <mi>k</mi> </msub> <mo>=</mo> <mrow> <mo>(</mo> <msup> <mi>&amp;omega;</mi> <mrow> <mi>g</mi> <mi>y</mi> <mi>r</mi> <mi>o</mi> </mrow> </msup> <mo>*</mo> <msubsup> <mi>&amp;psi;</mi> <mi>k</mi> <mrow> <mi>g</mi> <mi>y</mi> <mi>r</mi> <mi>o</mi> </mrow> </msubsup> <mo>+</mo> <msup> <mi>&amp;omega;</mi> <mrow> <mi>p</mi> <mi>r</mi> <mi>e</mi> <mi>v</mi> </mrow> </msup> <mo>*</mo> <msub> <mi>&amp;psi;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>+</mo> <msup> <mi>&amp;omega;</mi> <mrow> <mi>e</mi> <mi>l</mi> <mi>e</mi> <mi>c</mi> </mrow> </msup> <mo>*</mo> <msubsup> <mi>&amp;psi;</mi> <mi>k</mi> <mrow> <mi>e</mi> <mi>l</mi> <mi>e</mi> <mi>c</mi> </mrow> </msubsup> <mo>)</mo> </mrow> <mo>/</mo> <mrow> <mo>(</mo> <msup> <mi>&amp;omega;</mi> <mrow> <mi>g</mi> <mi>y</mi> <mi>r</mi> <mi>o</mi> </mrow> </msup> <mo>+</mo> <msup> <mi>&amp;omega;</mi> <mrow> <mi>p</mi> <mi>r</mi> <mi>e</mi> <mi>v</mi> </mrow> </msup> <mo>+</mo> <msup> <mi>&amp;omega;</mi> <mrow> <mi>e</mi> <mi>l</mi> <mi>e</mi> <mi>c</mi> </mrow> </msup> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>3</mn> <mo>)</mo> </mrow> </mrow>
2.根据权利要求1所述的一种融合电子罗盘和陀螺仪的PDR航向角确定方法，其特征在于所述步骤1中的电子罗盘由加速度计和磁力计组成，所述的通过电子罗盘得到航向角ψelec具体包括下述步骤：
1.1利用便携式设备内置的三轴加速度计和三轴磁力计采集在载体坐标系下的测量值；
所述的载体坐标系，简称b系；其中X轴是水平指向右，Y轴是垂直向上，Z轴是指向屏幕正面之外；
记加速度计在b系下采集到的X、Y和Z轴的测量值为ax、ay和az；
记磁力计在b系下采集到的X、Y和Z轴的测量值为hx、hy和hz；
1.2根据b系下的加速度计和磁力计的测量值计算地球坐标系下的航向角；
所述的地球坐标系，简称n系；其中X轴是与地面相切指向东，Y轴是与地面相切指向北，Z轴是垂直于地面指向天空；
行人行走过程中b系与n系并不重合，需要将b系坐标系依次经过翻滚角γ、俯仰角θ和航向角ψ三次旋转才能得到n系坐标系，该过程用下面的旋转矩阵来描述：
<mrow> <mtable> <mtr> <mtd> <mrow> <msubsup> <mi>C</mi> <mi>b</mi> <mi>n</mi> </msubsup> <mo>=</mo> <msup> <mrow> <mo>(</mo> <msubsup> <mi>C</mi> <mi>n</mi> <mi>b</mi> </msubsup> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mo>=</mo> <msup> <mrow> <mo>(</mo> <mrow> <msub> <mi>C</mi> <mi>&amp;gamma;</mi> </msub> <mo>&amp;CenterDot;</mo> <msub> <mi>C</mi> <mi>&amp;theta;</mi> </msub> <mo>&amp;CenterDot;</mo> <msub> <mi>C</mi> <mi>&amp;psi;</mi> </msub> </mrow> <mo>)</mo> </mrow> <mi>T</mi> </msup> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <mi>cos</mi> <mi>&amp;theta;</mi> <mi>cos</mi> <mi>&amp;psi;</mi> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <mi>cos</mi> <mi>&amp;gamma;</mi> <mi>sin</mi> <mi>&amp;psi;</mi> <mo>+</mo> <mi>sin</mi> <mi>&amp;gamma;</mi> <mi>sin</mi> <mi>&amp;theta;</mi> <mi>cos</mi> <mi>&amp;psi;</mi> </mrow> </mtd> <mtd> <mrow> <mi>sin</mi> <mi>&amp;gamma;</mi> <mi>sin</mi> <mi>&amp;psi;</mi> <mo>+</mo> <mi>cos</mi> <mi>&amp;gamma;</mi> <mi>sin</mi> <mi>&amp;theta;</mi> <mi>cos</mi> <mi>&amp;psi;</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>cos</mi> <mi>&amp;theta;</mi> <mi>sin</mi> <mi>&amp;psi;</mi> </mrow> </mtd> <mtd> <mrow> <mi>cos</mi> <mi>&amp;gamma;</mi> <mi>cos</mi> <mi>&amp;psi;</mi> <mo>+</mo> <mi>sin</mi> <mi>&amp;gamma;</mi> <mi>sin</mi> <mi>&amp;theta;</mi> <mi>sin</mi> <mi>&amp;psi;</mi> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <mi>sin</mi> <mi>&amp;gamma;</mi> <mi>cos</mi> <mi>&amp;psi;</mi> <mo>+</mo> <mi>cos</mi> <mi>&amp;gamma;</mi> <mi>sin</mi> <mi>&amp;theta;</mi> <mi>sin</mi> <mi>&amp;psi;</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>-</mo> <mi>sin</mi> <mi>&amp;theta;</mi> </mrow> </mtd> <mtd> <mrow> <mi>sin</mi> <mi>&amp;gamma;</mi> <mi>cos</mi> <mi>&amp;theta;</mi> </mrow> </mtd> <mtd> <mrow> <mi>cos</mi> <mi>&amp;gamma;</mi> <mi>cos</mi> <mi>&amp;theta;</mi> </mrow> </mtd> </mtr> </mtable> </mfenced> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>C</mi> <mn>11</mn> </msub> </mtd> <mtd> <msub> <mi>C</mi> <mn>12</mn> </msub> </mtd> <mtd> <msub> <mi>C</mi> <mn>13</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>C</mi> <mn>21</mn> </msub> </mtd> <mtd> <msub> <mi>C</mi> <mn>22</mn> </msub> </mtd> <mtd> <msub> <mi>C</mi> <mn>23</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>C</mi> <mn>31</mn> </msub> </mtd> <mtd> <msub> <mi>C</mi> <mn>32</mn> </msub> </mtd> <mtd> <msub> <mi>C</mi> <mn>33</mn> </msub> </mtd> </mtr> </mtable> </mfenced> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>4</mn> <mo>)</mo> </mrow> </mrow>
上述公式(4)里面的俯仰角θ和翻滚角γ根据加速度计的测量值计算得到，具体计算公式如下：
<mrow> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <mi>&amp;theta;</mi> <mo>=</mo> <mo>-</mo> <mi>arctan</mi> <mrow> <mo>(</mo> <mfrac> <msub> <mi>a</mi> <mi>x</mi> </msub> <msqrt> <mrow> <msubsup> <mi>a</mi> <mi>y</mi> <mn>2</mn> </msubsup> <mo>+</mo> <msubsup> <mi>a</mi> <mi>z</mi> <mn>2</mn> </msubsup> </mrow> </msqrt> </mfrac> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>&amp;gamma;</mi> <mo>=</mo> <mi>arctan</mi> <mrow> <mo>(</mo> <mfrac> <msub> <mi>a</mi> <mi>y</mi> </msub> <msub> <mi>a</mi> <mi>z</mi> </msub> </mfrac> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>5</mn> <mo>)</mo> </mrow> </mrow>
上述公式(4)里面的航向角ψ根据下面两步得到：
第1步：利用公式(5)所求得的俯仰角θ、翻滚角γ和磁力计在b系下的测量值计算磁力计在n系下X、Y轴的分量Hx和Hy；
<mrow> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>H</mi> <mi>x</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>H</mi> <mi>y</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>H</mi> <mi>z</mi> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <mi>cos</mi> <mi>&amp;theta;</mi> </mrow> </mtd> <mtd> <mrow> <mi>sin</mi> <mi>&amp;gamma;</mi> <mi>sin</mi> <mi>&amp;theta;</mi> </mrow> </mtd> <mtd> <mrow> <mi>cos</mi> <mi>&amp;gamma;</mi> <mi>sin</mi> <mi>&amp;theta;</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mi>cos</mi> <mi>&amp;gamma;</mi> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <mi>sin</mi> <mi>&amp;gamma;</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>-</mo> <mi>sin</mi> <mi>&amp;theta;</mi> </mrow> </mtd> <mtd> <mrow> <mi>sin</mi> <mi>&amp;gamma;</mi> <mi>cos</mi> <mi>&amp;theta;</mi> </mrow> </mtd> <mtd> <mrow> <mi>cos</mi> <mi>&amp;gamma;</mi> <mi>cos</mi> <mi>&amp;theta;</mi> </mrow> </mtd> </mtr> </mtable> </mfenced> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>h</mi> <mi>x</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>h</mi> <mi>y</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>h</mi> <mi>z</mi> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>6</mn> <mo>)</mo> </mrow> </mrow>
第2步：利用下面公式计算航向角ψ，也就是电子罗盘在n系下的航向角ψelec；
<mrow> <msup> <mi>&amp;psi;</mi> <mrow> <mi>e</mi> <mi>l</mi> <mi>e</mi> <mi>c</mi> </mrow> </msup> <mo>=</mo> <mi>arctan</mi> <mrow> <mo>(</mo> <mfrac> <msub> <mi>H</mi> <mi>y</mi> </msub> <msqrt> <mrow> <msubsup> <mi>H</mi> <mi>x</mi> <mn>2</mn> </msubsup> <mo>+</mo> <msubsup> <mi>H</mi> <mi>y</mi> <mn>2</mn> </msubsup> </mrow> </msqrt> </mfrac> <mo>)</mo> </mrow> <mo>=</mo> <mi>arctan</mi> <mrow> <mo>(</mo> <mfrac> <mrow> <mo>-</mo> <msub> <mi>h</mi> <mi>y</mi> </msub> <mi>c</mi> <mi>o</mi> <mi>s</mi> <mi>&amp;gamma;</mi> <mo>+</mo> <msub> <mi>h</mi> <mi>z</mi> </msub> <mi>s</mi> <mi>i</mi> <mi>n</mi> <mi>&amp;gamma;</mi> </mrow> <mrow> <msub> <mi>h</mi> <mi>x</mi> </msub> <mi>c</mi> <mi>o</mi> <mi>s</mi> <mi>&amp;theta;</mi> <mo>+</mo> <msub> <mi>h</mi> <mi>y</mi> </msub> <mi>sin</mi> <mi>&amp;gamma;</mi> <mi>s</mi> <mi>i</mi> <mi>n</mi> <mi>&amp;theta;</mi> <mo>+</mo> <msub> <mi>h</mi> <mi>z</mi> </msub> <mi>c</mi> <mi>o</mi> <mi>s</mi> <mi>&amp;gamma;</mi> <mi>s</mi> <mi>i</mi> <mi>n</mi> <mi>&amp;theta;</mi> </mrow> </mfrac> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>7</mn> <mo>)</mo> </mrow> </mrow>
1.3利用下述公式对航向角ψelec进行滑动低通滤波，降低随机噪声的影响；
<mrow> <msubsup> <mi>&amp;psi;</mi> <mi>k</mi> <mrow> <mi>e</mi> <mi>l</mi> <mi>e</mi> <mi>c</mi> </mrow> </msubsup> <mo>=</mo> <mfrac> <mn>1</mn> <mi>N</mi> </mfrac> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>N</mi> </munderover> <msubsup> <mi>&amp;psi;</mi> <mrow> <mi>k</mi> <mo>+</mo> <mi>i</mi> </mrow> <mrow> <mi>e</mi> <mi>l</mi> <mi>e</mi> <mi>c</mi> </mrow> </msubsup> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>8</mn> <mo>)</mo> </mrow> </mrow>
其中，N是滤波窗口的大小。
3.根据权利要求1所述的一种融合电子罗盘和陀螺仪的PDR航向角确定方法，其特征在于所述步骤2中通过陀螺仪得到航向角ψgyro具体包括下述步骤：
2.1利用便携式设备内置的三轴陀螺仪采集在b系下的测量值；
记陀螺仪在b系下采集到的X、Y和Z轴的角速度测量值为wx、wy和wz；
2.2根据b系下的陀螺仪的测量值计算n系下的航向角ψgyro；
所述的航向角ψgyro根据下面两步得到：
第1步：利用b系下的陀螺仪的测量值更新四元数Q
所述的四元数定义如下：
<mrow> <mi>Q</mi> <mo>=</mo> <msup> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>q</mi> <mn>0</mn> </msub> </mtd> <mtd> <msub> <mi>q</mi> <mn>1</mn> </msub> </mtd> <mtd> <msub> <mi>q</mi> <mn>2</mn> </msub> </mtd> <mtd> <msub> <mi>q</mi> <mn>3</mn> </msub> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> <mo>,</mo> <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> <mo>=</mo> <mn>1</mn> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>9</mn> <mo>)</mo> </mrow> </mrow>
分别将便携式设备初始时刻加速度计和磁力计的测量值先后带入到上述公式(5)、(7)和(4)，得到当前旋转矩阵同时四元数也能够确定旋转矩阵
<mrow> <msubsup> <mi>C</mi> <mi>b</mi> <mi>n</mi> </msubsup> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>C</mi> <mn>11</mn> </msub> </mtd> <mtd> <msub> <mi>C</mi> <mn>12</mn> </msub> </mtd> <mtd> <msub> <mi>C</mi> <mn>13</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>C</mi> <mn>21</mn> </msub> </mtd> <mtd> <msub> <mi>C</mi> <mn>22</mn> </msub> </mtd> <mtd> <msub> <mi>C</mi> <mn>23</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>C</mi> <mn>31</mn> </msub> </mtd> <mtd> <msub> <mi>C</mi> <mn>32</mn> </msub> </mtd> <mtd> <msub> <mi>C</mi> <mn>33</mn> </msub> </mtd> </mtr> </mtable> </mfenced> <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> <mrow> <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> </mrow> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <mn>2</mn> <mrow> <mo>(</mo> <mrow> <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> </mrow> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mn>2</mn> <mrow> <mo>(</mo> <mrow> <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> </mrow> <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> <mrow> <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> </mrow> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mn>2</mn> <mrow> <mo>(</mo> <mrow> <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> </mrow> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <mn>2</mn> <mrow> <mo>(</mo> <mrow> <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> </mrow> <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>10</mn> <mo>)</mo> </mrow> </mrow>
利用公式(10)求出初始时刻的四元数Q，具体公式如下所示：
<mrow> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msub> <mi>q</mi> <mn>1</mn> </msub> <mo>=</mo> <mo>&amp;PlusMinus;</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <msqrt> <mrow> <mn>1</mn> <mo>+</mo> <msub> <mi>C</mi> <mn>11</mn> </msub> <mo>-</mo> <msub> <mi>C</mi> <mn>22</mn> </msub> <mo>-</mo> <msub> <mi>C</mi> <mn>33</mn> </msub> </mrow> </msqrt> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>q</mi> <mn>2</mn> </msub> <mo>=</mo> <mo>&amp;PlusMinus;</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <msqrt> <mrow> <mn>1</mn> <mo>-</mo> <msub> <mi>C</mi> <mn>11</mn> </msub> <mo>+</mo> <msub> <mi>C</mi> <mn>22</mn> </msub> <mo>-</mo> <msub> <mi>C</mi> <mn>33</mn> </msub> </mrow> </msqrt> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>q</mi> <mn>3</mn> </msub> <mo>=</mo> <mo>&amp;PlusMinus;</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <msqrt> <mrow> <mn>1</mn> <mo>-</mo> <msub> <mi>C</mi> <mn>11</mn> </msub> <mo>-</mo> <msub> <mi>C</mi> <mn>22</mn> </msub> <mo>+</mo> <msub> <mi>C</mi> <mn>33</mn> </msub> </mrow> </msqrt> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>q</mi> <mn>0</mn> </msub> <mo>=</mo> <mo>&amp;PlusMinus;</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <msqrt> <mrow> <mn>1</mn> <mo>+</mo> <msub> <mi>C</mi> <mn>11</mn> </msub> <mo>+</mo> <msub> <mi>C</mi> <mn>22</mn> </msub> <mo>+</mo> <msub> <mi>C</mi> <mn>33</mn> </msub> </mrow> </msqrt> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>11</mn> <mo>)</mo> </mrow> </mrow>
由于行人是移动的，所以四元数是一个变量；利用下述公式对四元数进行更新：
<mrow> <msub> <mi>Q</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <mrow> <mo>(</mo> <mi>I</mi> <mi> </mi> <mi>c</mi> <mi>o</mi> <mi>s</mi> <mfrac> <mrow> <mi>&amp;Delta;</mi> <mi>&amp;theta;</mi> </mrow> <mn>2</mn> </mfrac> <mo>+</mo> <mi>&amp;Omega;</mi> <mo>(</mo> <mi>&amp;omega;</mi> <mo>)</mo> <mi>d</mi> <mi>t</mi> <mfrac> <mrow> <mi>s</mi> <mi>i</mi> <mi>n</mi> <mfrac> <mrow> <mi>&amp;Delta;</mi> <mi>&amp;theta;</mi> </mrow> <mn>2</mn> </mfrac> </mrow> <mrow> <mi>&amp;Delta;</mi> <mi>&amp;theta;</mi> </mrow> </mfrac> <mo>)</mo> </mrow> <msub> <mi>Q</mi> <mi>k</mi> </msub> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>12</mn> <mo>)</mo> </mrow> </mrow>
式中，是采样时间；
第2步：利用上步得到的四元数根据下面的公式计算陀螺仪在n系下的航向角ψgyro；
<mrow> <msup> <mi>&amp;psi;</mi> <mrow> <mi>g</mi> <mi>y</mi> <mi>r</mi> <mi>o</mi> </mrow> </msup> <mo>=</mo> <mi>arctan</mi> <mrow> <mo>(</mo> <mfrac> <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> <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> </mfrac> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>13</mn> <mo>)</mo> </mrow> <mo>.</mo> </mrow> 3
CN201710330467.6A 2017-05-11 2017-05-11 一种融合电子罗盘和陀螺仪的pdr航向角确定方法 Active CN107255474B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201710330467.6A CN107255474B (zh) 2017-05-11 2017-05-11 一种融合电子罗盘和陀螺仪的pdr航向角确定方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201710330467.6A CN107255474B (zh) 2017-05-11 2017-05-11 一种融合电子罗盘和陀螺仪的pdr航向角确定方法

Publications (2)

Publication Number Publication Date
CN107255474A true CN107255474A (zh) 2017-10-17
CN107255474B CN107255474B (zh) 2020-06-05

Family

ID=60027315

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201710330467.6A Active CN107255474B (zh) 2017-05-11 2017-05-11 一种融合电子罗盘和陀螺仪的pdr航向角确定方法

Country Status (1)

Country Link
CN (1) CN107255474B (zh)

Cited By (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108106613A (zh) * 2017-11-06 2018-06-01 上海交通大学 基于视觉辅助的定位方法与系统
CN108489489A (zh) * 2018-01-23 2018-09-04 杭州电子科技大学 一种蓝牙辅助纠正pdr的室内定位方法及系统
CN109540135A (zh) * 2018-11-09 2019-03-29 华南农业大学 水田拖拉机位姿检测和偏航角提取的方法及装置
CN110345939A (zh) * 2019-07-02 2019-10-18 山东科技大学 一种融合模糊逻辑判断及地图信息的室内定位方法
CN111278688A (zh) * 2017-11-03 2020-06-12 黑拉有限责任两合公司 驱动车辆的部件的方法、系统、计算机程序产品和计算机可读介质
CN113984046A (zh) * 2021-10-25 2022-01-28 北京航空航天大学 一种基于体域惯性传感器网多特征融合的高精度室内定位方法
CN115773750A (zh) * 2022-12-02 2023-03-10 无锡恺韵来机器人有限公司 一种磁航向角测量装置及方法
CN116793364A (zh) * 2023-06-27 2023-09-22 汇鲲化鹏(海南)科技有限公司 一种基于航位推算的室内定位系统及方法

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20130053064A1 (en) * 2011-08-31 2013-02-28 Snu R&Db Foundation Method and apparatus for determining heading angle in wireless lan
EP2778709A1 (en) * 2013-03-15 2014-09-17 Honeywell International Inc. User assisted location method
CN104121905A (zh) * 2014-07-28 2014-10-29 东南大学 一种基于惯性传感器的航向角获取方法
CN105910606A (zh) * 2016-06-20 2016-08-31 天津大学 一种基于角速度差值的方向修正方法
CN106153042A (zh) * 2015-03-31 2016-11-23 日本电气株式会社 航向角获取方法和装置
CN106225784A (zh) * 2016-06-13 2016-12-14 国家海洋局第二海洋研究所 基于低成本多传感器融合行人航位推算方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20130053064A1 (en) * 2011-08-31 2013-02-28 Snu R&Db Foundation Method and apparatus for determining heading angle in wireless lan
EP2778709A1 (en) * 2013-03-15 2014-09-17 Honeywell International Inc. User assisted location method
CN104121905A (zh) * 2014-07-28 2014-10-29 东南大学 一种基于惯性传感器的航向角获取方法
CN106153042A (zh) * 2015-03-31 2016-11-23 日本电气株式会社 航向角获取方法和装置
CN106225784A (zh) * 2016-06-13 2016-12-14 国家海洋局第二海洋研究所 基于低成本多传感器融合行人航位推算方法
CN105910606A (zh) * 2016-06-20 2016-08-31 天津大学 一种基于角速度差值的方向修正方法

Cited By (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111278688A (zh) * 2017-11-03 2020-06-12 黑拉有限责任两合公司 驱动车辆的部件的方法、系统、计算机程序产品和计算机可读介质
CN108106613A (zh) * 2017-11-06 2018-06-01 上海交通大学 基于视觉辅助的定位方法与系统
CN108489489A (zh) * 2018-01-23 2018-09-04 杭州电子科技大学 一种蓝牙辅助纠正pdr的室内定位方法及系统
CN109540135A (zh) * 2018-11-09 2019-03-29 华南农业大学 水田拖拉机位姿检测和偏航角提取的方法及装置
CN109540135B (zh) * 2018-11-09 2020-07-31 华南农业大学 水田拖拉机位姿检测和偏航角提取的方法及装置
CN110345939A (zh) * 2019-07-02 2019-10-18 山东科技大学 一种融合模糊逻辑判断及地图信息的室内定位方法
CN110345939B (zh) * 2019-07-02 2021-03-19 山东科技大学 一种融合模糊逻辑判断及地图信息的室内定位方法
CN113984046A (zh) * 2021-10-25 2022-01-28 北京航空航天大学 一种基于体域惯性传感器网多特征融合的高精度室内定位方法
CN113984046B (zh) * 2021-10-25 2023-05-30 北京航空航天大学 一种基于体域惯性传感器网多特征融合的高精度室内定位方法
CN115773750A (zh) * 2022-12-02 2023-03-10 无锡恺韵来机器人有限公司 一种磁航向角测量装置及方法
CN115773750B (zh) * 2022-12-02 2023-11-10 运来智能装备(无锡)有限公司 一种磁航向角测量装置及方法
CN116793364A (zh) * 2023-06-27 2023-09-22 汇鲲化鹏(海南)科技有限公司 一种基于航位推算的室内定位系统及方法

Also Published As

Publication number Publication date
CN107255474B (zh) 2020-06-05

Similar Documents

Publication Publication Date Title
CN107255474A (zh) 一种融合电子罗盘和陀螺仪的pdr航向角确定方法
CN104061934B (zh) 基于惯性传感器的行人室内位置跟踪方法
CN108362282B (zh) 一种基于自适应零速区间调整的惯性行人定位方法
CN104406586B (zh) 基于惯性传感器的行人导航装置和方法
CN105180937B (zh) 一种mems‑imu初始对准方法
CN105300379B (zh) 一种基于加速度的卡尔曼滤波姿态估计方法及系统
CN107796391A (zh) 一种捷联惯性导航系统/视觉里程计组合导航方法
CN106482734A (zh) 一种用于imu多传感器数据融合的滤波方法
CN104698485B (zh) 基于bd、gps及mems的组合导航系统及导航方法
CN103940425B (zh) 一种磁‑惯性组合捷联测量方法
CN105698822B (zh) 基于反向姿态跟踪的自主式惯性导航行进间初始对准方法
CN107490378B (zh) 一种基于mpu6050与智能手机的室内定位与导航的方法
CN107084718A (zh) 基于行人航迹推算的室内定位方法
CN104977006A (zh) 一种基于模糊理论和多传感器融合的室内定位方法
CN108458714B (zh) 一种姿态检测系统中不含重力加速度的欧拉角求解方法
CN105509739A (zh) 采用固定区间crts平滑的ins/uwb紧组合导航系统及方法
CN108318038A (zh) 一种四元数高斯粒子滤波移动机器人姿态解算方法
CN102445200A (zh) 微小型个人组合导航系统及其导航定位方法
CN106643715A (zh) 一种基于bp神经网络改善的室内惯性导航方法
CN104634345B (zh) 一种自适应步长的室内轨迹追踪方法
CN103557866B (zh) 一种基于地磁技术的虚拟陀螺仪及算法的方法
CN103175502A (zh) 一种基于数据手套低速运动的姿态角检测方法
CN103900608A (zh) 一种基于四元数ckf的低精度惯导初始对准方法
CN108007477A (zh) 一种基于正反向滤波的惯性行人定位系统误差抑制方法
CN104266648A (zh) Android平台MARG传感器的室内定位系统

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
TR01 Transfer of patent right

Effective date of registration: 20201222

Address after: Building loftc, West Greenland Business City, Hanyuan Avenue, Yunlong District, Xuzhou City, Jiangsu Province, 221000

Patentee after: XUZHOU XINNANHU TECHNOLOGY Co.,Ltd.

Address before: 310018 No. 2 street, Xiasha Higher Education Zone, Hangzhou, Zhejiang

Patentee before: HANGZHOU DIANZI University

TR01 Transfer of patent right
TR01 Transfer of patent right

Effective date of registration: 20221227

Address after: 1333-28, Floor 13, Building A 1, Qinghe Jiayuan East District, Haidian District, Beijing 100089

Patentee after: Zhonglian Shenghe Holding Group Co.,Ltd.

Address before: Building loftc, West Greenland Business City, Hanyuan Avenue, Yunlong District, Xuzhou City, Jiangsu Province, 221000

Patentee before: XUZHOU XINNANHU TECHNOLOGY Co.,Ltd.

TR01 Transfer of patent right
TR01 Transfer of patent right

Effective date of registration: 20230104

Address after: 3-106, 1st floor, Building 6, Qingshangyuan, Haidian District, Beijing 100089

Patentee after: Zhonglian Jinguan information technology (Beijing) Co.,Ltd.

Address before: 1333-28, Floor 13, Building A 1, Qinghe Jiayuan East District, Haidian District, Beijing 100089

Patentee before: Zhonglian Shenghe Holding Group Co.,Ltd.

TR01 Transfer of patent right