本发明涉及一种基于自适应卡尔曼滤波的行人航向最优融合方法，属于行人导航技术领域。
背景技术：
行人导航技术已掀起国内外研究热潮，其中航向作为核心技术之一，对行人导航定位精度起着至关重要的作用。目前较为普遍的做法是利用陀螺仪获得角速率，进而四元素解算得到角度，但是低成本的惯性传感器，其误差较大且发散严重，长时间内将会导致行人定位失效。地磁场作为地球的固有属性，可以用于计算磁航向角，但是室内地磁受到钢筋、管道等其他金属制品的影响，会产生较大的误差，无法满足所有路段高精度航向的要求。目前，国内外主要考察磁航向角与陀螺解算出的角度的差值大小，当这一差值大于某一预定阈值时，即为判定有磁场异常情况。但是这种判别方法受限于磁航向角的解算精度，本身磁传感器尚未标定完全会带有误差，投影解算航向时又会引入二次误差，判别精度较低。同时针对磁航向角与捷联解算出来的航向角融合方面，其仅仅只有完全融合与完全不融合两种情况，其灵活性与适应性较差，当磁异常情况微弱时，可以尝试提取有效信息进行航向融合。
技术实现要素：
本发明采用低精度的陀螺传感器与磁传感器，提出了一种基于自适应卡尔曼滤波的行人航向最优融合方法，解决不同磁环境下的航向融合问题。利用磁异常辨识算法有效区分磁异常环境，进而在不同环境下恰当的选取量测噪声阵，构建自适应卡尔曼滤波器灵活实现捷联航向角与磁航向角的最优融合，实现室内外不同环境下行人航向的高精度。
本发明为解决其技术问题采用如下技术方案：
一种基于自适应卡尔曼滤波的行人航向最优融合方法，包括如下步骤：
步骤1，首先建立自适应卡尔曼滤波状态方程，选用“东北天”地理坐标系，构建9阶状态模型，如下式所示
其中为东向平台角误差；为北向平台角误差；为天向平台角误差；εbx为x轴陀螺随机常数；εby为y轴陀螺随机常数；εbz为z轴陀螺随机常数；εrx为x轴陀螺一阶马尔科夫过程；εry为y轴陀螺一阶马尔科夫过程；εrz为z轴陀螺一阶马尔科夫过程；W为系统随机过程噪声序列；A为系统矩阵；G为系统噪声矩阵；W为系统噪声序列；X为状态量；为状态量导数；wgx为x轴随机白噪声驱动；wgy为y轴随机白噪声驱动；wgz为z轴随机白噪声驱动；wrx为x轴马尔科夫白噪声驱动；wry为y轴马尔科夫白噪声驱动；wrz为z轴马尔科夫白噪声驱动；
步骤2，在步骤1自适应卡尔曼滤波状态方程建立好的基础之上，开始导航；利用陀螺仪每0.005秒采集一次数据，经误差修正后，通过四元素解算得出当前捷联航向角，利用误差修正后的磁传感器信息解算出当前磁航向角；
步骤3，在步骤2的基础上，判断当前解算时间是否达到1秒，无则返回步骤2，有则进行步骤4；
步骤4，在步骤3的基础上，研究利用磁传感器实时统计信息进行磁异常辨识，再利用二维椭圆标定算法修正磁传感器信息之后，按照下式构造磁异常辨识模型
上式代表第k时刻的磁环境辨识情况，其中，σ3为第一种磁异常判别参数；σ4为第二种磁异常判别参数；var()代表方差函数；min()代表求取最小值函数；mag1为第一个时刻总磁场强度；代表窗口大小为N的滑动数组；N为数组大小；
数组存储总磁场强度并实时更新，总磁场强度如下式所示，由三轴磁传感器测量值的平方和开根号组成。
其中：magi为第i时刻总磁场强度；magix为x轴磁场强度；magiy为y轴磁场强度；magiz为z轴磁场强度；
步骤5：在步骤4的基础上，进行自适应卡尔曼滤波量测建模，构建一维量测模型，量测周期为1秒，量测方程如下式所示：
其中，为捷联解算出的航向角；为磁航向角；为姿态误差角；为航向噪声；Hv(t)为量测矩阵；X(t)为状态量；Vv(t)为量测噪声；磁航向角解算公式如下式所示，其中X、Y分别为前向和横侧向所测量得到的磁信息。
姿态误差角与平台误差角存在如下关系：
其中，θ为俯仰角，φx、φy、φy为三个平台误差角。因此：
同时利用步骤4的结果，依据下式对卡尔曼滤波器量测噪声阵Vv(t)进行修正。
其中λ为设定的经验值；η为事先人为设定的增益常量；ε3与ε4为通过统计学方法获得的参数值，即在磁正常环境下由式(2)获得的均值。
步骤6：在步骤5的基础上，利用上述自适应卡尔曼滤波器对组合航向角进行修正，同时将陀螺估计误差反馈给陀螺误差修正模型，并返回步骤2。
本发明的有益效果如下：
1.采用低精度的消费级传感器芯片，无论室内外，均能有效的保证行人航向角高精度，实用性较强。
2.解决了磁异常环境下磁航向角误用的问题。
3.传感器成本低且普及较广，算法的实用性与推广性较强。
附图说明
图1为自适应卡尔曼滤波的行人航向最优融合流程示意图。
具体实施方案
下面结合附图对本发明创造做进一步详细说明。
如图1所示，导航解算过程中，陀螺仪可以实时获得角速率信息，通过四元素解算，实时获得捷联航向角，解算频率一般可设置为50Hz。磁传感器实时采集磁信息，主要分为两部分，一部分用于解算出磁航向角，一部分用于磁异常辨识，实时灵活调整量测噪声阵。以捷联航向角为主，每过1秒，利用自适应卡尔曼滤波器，利用磁航向角建立观测方程，实时修正捷联航向角误差，提高航向角精度，并返回估计误差对惯性传感器进行误差修正。
基于自适应卡尔曼滤波的行人航向最优融合算法主要包括以下三个步骤：
步骤一：自适应卡尔曼滤波器状态建模
选用“东北天”地理坐标系，构建9阶状态模型，如式(1)所示。
其中为东向平台角误差；为北向平台角误差；为天向平台角误差；εbx为x轴陀螺随机常数；εby为y轴陀螺随机常数；εbz为z轴陀螺随机常数；εrx为x轴陀螺一阶马尔科夫过程；εry为y轴陀螺一阶马尔科夫过程；εrz为z轴陀螺一阶马尔科夫过程；W为系统随机过程噪声序列；A为系统矩阵；G为系统噪声矩阵；W为系统噪声序列；X为状态量；为状态量导数；wgx为x轴随机白噪声驱动；wgy为y轴随机白噪声驱动；wgz为z轴随机白噪声驱动；wrx为x轴马尔科夫白噪声驱动；wry为y轴马尔科夫白噪声驱动；wrz为z轴马尔科夫白噪声驱动。
步骤二：磁异常辨识模型
研究利用磁传感器实时统计信息进行磁异常辨识，磁异常环境下，无论是磁场强度模值还是磁场强度变化趋势，均与磁正常环境有着较大差异。再利用二维椭圆标定算法修正磁传感器信息之后，按照(2)式构造磁异常辨识模型。
上式代表第k时刻的磁环境辨识情况。其中，σ3为第一种磁异常判别参数；σ4为第二种磁异常判别参数；var()代表方差函数；min()代表求取最小值函数；mag1为第一时刻总磁场强度；代表窗口大小为N的滑动数组；N为数组大小；数组存储总磁场强度并实时更新，总磁场强度如式(3)所示，由三轴磁传感器测量值的平方和开根号组成。
其中：magi为第i时刻总磁场强度；magix为x轴磁场强度大小；magiy为y轴磁场强度大小；magiz为z轴磁场强度大小。
步骤三：自适应卡尔曼滤波器量测建模
构建一维量测模型，量测周期为1秒，量测方程如下式所示：
其中，为捷联解算出的航向角；为磁航向角；为姿态误差角；为航向噪声；Hv(t)为量测矩阵；X(t)为状态量；Vv(t)为量测噪声。磁航向角解算公式如下式(5)所示，其中X、Y分别为前向和横侧向所测量得到的磁信息。
姿态误差角与平台误差角存在如下关系：
其中，θ为俯仰角，φx、φy、φy为三个平台误差角。因此：
卡尔曼滤波器量测噪声阵Vv(t)为
其中λ为设定的经验值，η为事先人为设定的增益常量，ε3与ε4为通过统计学方法获得的参数值(即在磁正常环境下由式(2)获得的均值)。
至此构建了自适应卡尔曼滤波器，实时的针对当前环境进行磁异常辨识，在不同的辨识结果下，适应性的调整卡尔曼滤波器量测噪声阵，实现捷联航向角与磁航向角的最优融合。
整个解算流程如下：
1、首先依据状态方程和量测方程构建自适应卡尔曼滤波器模型；
2、开始导航：利用陀螺仪采集数据，经误差修正后，通过四元素解算得出当前捷联航向角，利用误差修正后的磁传感器信息解算出当前磁航向角；
3、是否达到1秒，无则返回步骤2，是则进行步骤4、5、6；
4、利用磁异常辨识算法辨识出磁正常状态和异常状态，根据辨识结果实时调整自适应卡尔曼滤波器中的量测噪声阵；
5、滤波，获得修正过后的组合航向角，同时将陀螺估计误差反馈给陀螺误差修正模型；
6、返回步骤2。