[发明专利]一种基于自适应卡尔曼滤波的行人航向最优融合方法

说明书

本发明公开了一种基于自适应卡尔曼滤波器的行人航向最优融合方法，属于行人导航技术领域。本方法构建了9阶自适应卡尔曼滤波算法模型，每到量测时刻，融合捷联航向角与磁航向角，利用磁传感器实时统计信息，建立磁异常辨识模型，进而在不同的辨识状态下实时修正自适应卡尔曼滤波量测噪声阵，实现捷联航向角与磁航向角的最优融合，保证组合航向角的高精度。本发明采用低精度的消费级传感器芯片，无论室内外，均能有效的保证行人航向角高精度，实用性较强。

技术领域

本发明涉及一种基于自适应卡尔曼滤波的行人航向最优融合方法，属于行人导航技术领域。

背景技术

行人导航技术已掀起国内外研究热潮，其中航向作为核心技术之一，对行人导航定位精度起着至关重要的作用。目前较为普遍的做法是利用陀螺仪获得角速率，进而四元素解算得到角度，但是低成本的惯性传感器，其误差较大且发散严重，长时间内将会导致行人定位失效。地磁场作为地球的固有属性，可以用于计算磁航向角，但是室内地磁受到钢筋、管道等其他金属制品的影响，会产生较大的误差，无法满足所有路段高精度航向的要求。目前，国内外主要考察磁航向角与陀螺解算出的角度的差值大小，当这一差值大于某一预定阈值时，即为判定有磁场异常情况。但是这种判别方法受限于磁航向角的解算精度，本身磁传感器尚未标定完全会带有误差，投影解算航向时又会引入二次误差，判别精度较低。同时针对磁航向角与捷联解算出来的航向角融合方面，其仅仅只有完全融合与完全不融合两种情况，其灵活性与适应性较差，当磁异常情况微弱时，可以尝试提取有效信息进行航向融合。

发明内容

本发明采用低精度的陀螺传感器与磁传感器，提出了一种基于自适应卡尔曼滤波的行人航向最优融合方法，解决不同磁环境下的航向融合问题。利用磁异常辨识算法有效区分磁异常环境，进而在不同环境下恰当的选取量测噪声阵，构建自适应卡尔曼滤波器灵活实现捷联航向角与磁航向角的最优融合，实现室内外不同环境下行人航向的高精度。

本发明为解决其技术问题采用如下技术方案：

一种基于自适应卡尔曼滤波的行人航向最优融合方法，包括如下步骤：

步骤1，首先建立自适应卡尔曼滤波状态方程，选用“东北天”地理坐标系，构建9阶状态模型，如下式所示

其中为东向平台角误差；为北向平台角误差；为天向平台角误差；ε_bx为x轴陀螺随机常数；ε_by为y轴陀螺随机常数；ε_bz为z轴陀螺随机常数；ε_rx为x轴陀螺一阶马尔科夫过程；ε_ry为y轴陀螺一阶马尔科夫过程；ε_rz为z轴陀螺一阶马尔科夫过程；W为系统随机过程噪声序列；A为系统矩阵；G为系统噪声矩阵；W为系统噪声序列；X为状态量；为状态量导数；w_gx为x轴随机白噪声驱动；w_gy为y轴随机白噪声驱动；w_gz为z轴随机白噪声驱动；w_rx为x轴马尔科夫白噪声驱动；w_ry为y轴马尔科夫白噪声驱动；w_rz为z轴马尔科夫白噪声驱动；

步骤2，在步骤1自适应卡尔曼滤波状态方程建立好的基础之上，开始导航；利用陀螺仪每0.005秒采集一次数据，经误差修正后，通过四元素解算得出当前捷联航向角，利用误差修正后的磁传感器信息解算出当前磁航向角；

步骤3，在步骤2的基础上，判断当前解算时间是否达到1秒，无则返回步骤2，有则进行步骤4；

步骤4，在步骤3的基础上，研究利用磁传感器实时统计信息进行磁异常辨识，再利用二维椭圆标定算法修正磁传感器信息之后，按照下式构造磁异常辨识模型