[发明专利]一种基于正反向滤波的惯性行人定位系统误差抑制方法

权利要求书

1.一种基于正反向滤波的惯性行人定位系统误差抑制方法，其特征在于，包括以下步骤：

(1)将惯性器件固定在行人脚部，打开电源并初始化系统；

(2)系统实时采集惯性器件的输出数据；

(3)系统实时检测并记录行人脚部的运动状态，用ZUPT(m)表示第m时刻脚步运动状态，当第m时刻脚部处于离地状态时，记作ZUPT(m)＝0；当第m时刻行人脚部触地时，记作ZUPT(m)＝1，理论上认为此时脚部与地面相对静止，即脚部运动速度为0，故将这一段相对静止的时间称为零速区间；

(4)行人定位系统对惯性器件输出信息进行解算,由陀螺仪输出的角速度计算行人姿态信息，得到其中b表示载体坐标系，n表示导航坐标系，表示b系到n系的转换矩阵；利用将加速度计输出的加速度信息投影到导航坐标系后，有其中f表示加速度信息，f^b表示载体坐标系下的加速度信息，fⁿ表示导航坐标系下的加速度信息；加速度信息经过一次积分求解出速度信息：νⁿ＝∫fⁿdt，再次积分求解出位置信息：pⁿ＝∫vⁿdt；其中ν表示速度信息，p表示位置信息，pⁿ表示导航坐标系下的位置信息，t表示时间；

(5)根据步骤(3)中零速检测的结果，如果当前时刻ZUPT＝1，则转至步骤(6)；如果当前时刻ZUPT＝0，判断前N时刻是否一直有ZUPT＝1，如果是，则转至步骤(7)；如果不是，直接将步骤(4)中解算的行人位姿信息作为最终的导航输出；

(6)记录行人的加速度和角速度信息，并记录零速区间长度；

(7)读取步骤(6)中加速度f、角速度ω和零速区间长度L，并扩展零速区间；

(8)根据步骤(7)中加速度和角速度信息，解算行人的姿态ε和速度ν，理论上行人脚部触地时有ν＝0，ν＝δν，其中δν为速度误差；

(9)构造Kalman滤波器，以速度误差δν、位置误差δp和姿态误差δε为状态量，以速度误差为观测量，对零速区间内数据进行滤波；

(10)利用步骤(9)中解算出的δν，δp，δε补偿并将补偿后的结果作为最终导航输出，其中，分别为系统解算的ν，p，ε的值；

(11)重复步骤(1)到步骤(10)，直至惯性行人定位系统导航结束；

所述的步骤(7)具体为：

读取步骤(6)中零速区间长度L，读取区间内的数据f、ω；记第i组数据为u_i＝[f_i ω_i]^T，则零速区间中数据可写作U＝[u₁ u₂ … u_L]；

记D＝[-u_L -u_L-1 … -u₁]，扩展零速区间至sL，得到一般取s＝5，则有Data＝[U D U D U]。