[发明专利]一种基于双MEMS-IMU的行人自主导航解算算法

权利要求书

1.一种基于双MEMS-IMU的行人自主导航解算算法，其特征在于，它由以下步骤实现： 

步骤一：将基于双MEMS-IMU的行人自主导航系统中的两个IMU系统分别固定于行人的两只脚上，手持PDA实时接收并存储行人运动时两个IMU系统输出的量测信息； 

步骤二：使用步骤一中存储的行人自主导航系统输出数据，利用捷联惯性导航系统导航解算方法求出任意k时刻基于双MEMS-IMU的行人自主导航系统中每个IMU系统的状态

步骤三：使用零速检测算法检测到IMU为静止时，利用零速校正卡尔曼滤波器，采用输出校正的方式对传感器的量测结果和导航解算结果进行修正，零速校正卡尔曼滤波模型为： 

得到修正后的IMU状态两个IMU独立进行零速校正； 

步骤四：利用步骤三中估计出的双IMU导航系统导航状态及最大步长不等式，判断IMU输出是否满足最大步长约束，若不满足则执行步骤五，若满足约束则返回步骤三； 

步骤五：利用公式求取将不满足约束条件的双MEMS-IMU行人自主导航系统的导航解算输出映射到满足映射条件的范围内的映射方程 

步骤六：利用公式将不满足步骤四中最大步长不等式约束的双MEMS-IMU系统导航状态的估计值约束到子空间{x∈R^m:||L·x||²≤γ²}中，得到双MEMS-IMU行人自主导航系统状态约束值

步骤七：利用公式计算经状态约束后的双MEMS-IMU行人自主导航系统导航解算结果的协方差阵以更新卡尔曼滤波的协方差阵； 

步骤八：构造卡尔曼滤波动态误差修正模型，利用基于双MEMS-IMU的行人自主导航系统的导航解算联合误差传播特性方程： 

δx_k=F_kδx_k-1+G_kw_k

得到行人自主导航参数的最优估计值。 

2.如权利要求1所述的基于双MEMS-IMU的行人自主导航解算算法，其特征在于，在步骤一中，任意时刻k接收到的两个IMU输出信息为： 

其中，i＝1、2，表示双系统中的IMU1系统、IMU2系统； 为MEMS三轴陀螺仪输出的角速率信息； 为MEMS三轴加速度计输出的比力信息；T表示转置操作。 

3.如权利要求1所述的基于双MEMS-IMU的行人自主导航解算算法，其特征在于，在步骤二中，任意k时刻基于双MEMS-IMU的行人自主导航系统中每个IMU系统的状态为： 

其中，v、s、三维位置向量、速度向量、姿态向量，即

4.如权利要求1所述的基于双MEMS-IMU的行人自主导航解算算法，其特征在于，在步骤三中，零速校正卡尔曼滤波模型中，δX_k为被估计状态向量： 

δX＝[Φ^T δω^T δr^T δv^T δa^T] 

其中，φ^T为姿态误差、δω^T为陀螺漂移、δr^T为位置误差、δv^T为 速度误差、δa^T为加速度计输出误差，以上每一项都是三维的； 

W_k-1为系统噪声序列；量测量Z_k为IMU静止时，双MEMS-IMU输出量经惯性导航解算算法得到的速度分量；N_k为量测噪声序列；H为量测阵： 

H＝[0 0 I 0 0]^T

F为状态转移矩阵: 

式中，I(0)为单位(零)阵；为沿地理系的载体运动加速度的反对称阵；为载体坐标系相对于导航坐标系的状态转移矩阵。