基于零速修正与姿态自观测的惯性行人导航算法

针对惯性行人导航中航向角发散致使导航精度降低的问题，提出了一种基于零速修正与姿态自观测的惯性行人导航算法。通过四条件零速检测算法对行走步态中的零速区间进行检测。在检测得到的零速区间内，利用零速修正算法原理构造速度误差的观测量；利用零速区间内行人脚部与地面保持静止、只受到重力加速度及姿态角不变的特性，构造姿态角误差的观测量。应用卡尔曼滤波对零速区间内的姿态角、速度及位置的误差进行估计。利用得到的误差状态估计结果对行人导航进行误差校正，提高惯性行人导航的精度。实验表明:小范围矩形路径中，所提算法的导航轨迹相对误差平均值仅占总路程的0.98%，比零速修正算法减小了78.11%；导航轨迹误差标准差仅为0.14 m，比零速修正算法减小了88.62%；400 m标准操场闭合路径中解算终点相对位置误差仅为1.18%。解算轨迹与实际轨迹匹配度较高，具有良好的应用价值。 

Inertial pedestrian navigation algorithm based on zero velocity update and attitude self-observation

Aiming at the problem of reduced navigation accuracy caused by the divergence of the heading angle in inertial pedestrian navigation, an inertial pedestrian navigation algorithm based on zero velocity correction and attitude self-observation is proposed. A four-condition zero velocity detection algorithm is used to detect the zero velocity interval in the walking gait. In the detected zero velocity interval, the principle of the zero velocity update is used to construct the observation of the velocity error; the characteristic that only gravity acts and the heading angle remains unchanged in the zero velocity intervals is used to construct the observation of the attitude angle error. Then, the attitude angle, velocity and position error in the zero velocity interval are estimated by Kalman filtering. The error correction of pedestrian navigation is carried out using the obtained state estimation to further improve the accuracy of inertial pedestrian navigation. Actual walking experiments show that in the rectangular path, the average value of navigation trajectory relative error of this algorithm is only 0.98%, which is reduced by 78.11% compared with the zero velocity update algorithm and the standard deviation of navigation trajectory error of this algorithm is only 0.14 m, which is reduced by 88.62% compared with the zero velocity update algorithm. In the closed loop path of the classical 400 m playground, the relative position error of the solution end point is only 1.18%. The solved trajectory has a high degree of matching with the actual trajectory, which has good application value.