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

权利要求书

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

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

上式代表第k时刻的磁环境辨识情况，其中，σ₃为第一种磁异常判别参数；σ₄为第二种磁异常判别参数；var()代表方差函数；min()代表求取最小值函数，mag₁为第一个时刻总磁场强度；代表窗口大小为N的滑动数组；N为数组大小；

数组存储总磁场强度并实时更新，总磁场强度如下式所示，由三轴磁传感器测量值的平方和开根号组成

其中：mag_i为第i时刻总磁场强度；mag_ix为x轴磁场强度；mag_iy为y轴磁场强度；mag_iz为z轴磁场强度；

步骤5：在步骤4的基础上，进行自适应卡尔曼滤波量测建模，构建一维量测模型，量测周期为1秒，量测方程如下式所示：

其中，为捷联解算出的航向角；为磁航向角；为姿态误差角；为航向噪声；H_v(t)为量测矩阵；X(t)为第t时刻状态量；V_v(t)为量测噪声；磁航向角解算公式如下式所示，其中X、Y分别为前向和横侧向所测量得到的磁信息

姿态误差角与平台误差角存在如下关系：

其中，θ为俯仰角，φ_x、φ_y、φ_y为三个平台误差角；因此：

同时利用步骤4的结果，依据下式对卡尔曼滤波器量测噪声阵V_v(t)进行修正

其中λ为设定的经验值；η为事先人为设定的增益常量；ε₃与ε₄为通过统计学方法获得的参数值，即在磁正常环境下由式(2)获得的均值；

步骤6：在步骤5的基础上，利用上述自适应卡尔曼滤波器对组合航向角进行修正，同时将陀螺估计误差反馈给陀螺误差修正模型，并返回步骤2。