[发明专利]一种行人自主导航定位的自适应卡尔曼滤波方法

权利要求书

1.一种行人自主导航定位的自适应卡尔曼滤波方法，其特征在于：包括以下步骤：

步骤(1)，将MEMS-IMU系统固定连接于人体，IMU初始对准后，行人开始运动，运动过程中采集IMU数据；

其中所述的初始对准选取北东地为导航坐标系，俯仰角θ和滚转角γ由经过补偿后的三轴加速度计输出值计算获得，偏航角由磁场强度B、俯仰角θ和滚转角γ计算获得；

步骤(2)，合理选取观测量建立融合人体运动特征的自适应卡尔曼滤波模型；

其中，自适应卡尔曼滤波模型为包括3个位置误差、3个姿态角误差、3个重力不确定误差、三轴陀螺仪零偏误差和三轴加速度计零偏误差，共计18维的状态空间方程；所述3个姿态角为：俯仰角θ、滚转角γ和偏航角

W(t)为系统过程噪声矩阵，其表达式为：其中为行人坐标系b系到导航坐标系n系的方向余弦矩阵，ω^b为行人角速度，a^b为行人加速度；G(t)为对应的噪声矩阵系数；F(t)为系统状态矩阵；Z(t)为观测量；H(t)为观测矩阵；V(t)为观测噪声矩阵；

被估计状态向量为：

其中，δr为位置误差，δv为速度误差，δψ为姿态角误差，为三轴加速度计零偏误差，ε_b为三轴陀螺仪零偏误差，δg为重力不确定误差；

观测向量和观测矩阵为：

Z_k＝[Δv_k Δω_k Δψ_k]^T

其中，Δv_k为选取的瞬间静止时刻速度计算值的误差，Δω_k为选取的瞬间静止时刻三轴陀螺仪的角速度输出误差；Δψ_k包括磁力计计算的偏航角与捷联解算出的偏航角作差获得的偏航角误差，利用三轴加速度计计算出的俯仰角θ和滚转角γ与捷联解算出的姿态角作差获得的俯仰角和滚转角误差；

步骤(3)，同时满足“四条件”时进行自适应卡尔曼滤波，同步修正晃动噪声干扰，完成各状态量的估计；

其中，“四条件”包括：

(1)三轴加速度计合成的加速度幅值输出量满足阈值条件；

(2)三轴加速度计z轴加速度输出量满足阈值条件；

(3)三轴陀螺仪合成的角速度幅值输出量满足阈值条件；

(4)三轴陀螺仪y轴角速度输出量满足阈值条件；

其中条件(2)和条件(4)中的阈值在初始静止阶段分别以取三轴加速度计输出的平均值和三轴陀螺仪输出的平均值而设定；

其中的自适应卡尔曼滤波是利用观测数据进行递推滤波的同时，通过时变噪声统计估计器实时估计和修正系统噪声以及观测噪声的统计特性，从而降低模型误差、抑制滤波发散，提高滤波精度；

自适应卡尔曼滤波的算法描述为：

上式中，分别是k时刻系统状态X_k的当前和预测线性最小方差估计值，是k-1时刻的系统状态X_k-1的当前线性最小方差估计值；Φ_k,k-1是状态过渡矩阵，是Φ_k,k-1的转置矩阵；H_k是观测矩阵，Z_k是观测量，是观测量估计误差；K_k是滤波增益；P_k、P_k/k-1分别是X_k和X_k/k-1对应的状态估计协方差阵；分别是系统过程噪声和观测噪声的时变均值，分别是系统过程噪声和观测噪声的时变协方差阵；

其中，由时变噪声统计估值器递推获得：

式中，0＜b＜1为遗忘因子，针对本方法取b＝0.98；

此处分母中的b^k+1表示b的(k+1)次幂；k为离散时间点，k＝1,2,3…N，N为采样次数；

步骤(4)，利用估计误差修正运动人体信息；

其中，通过公式和反馈修正行人速度、位置和姿态信息。