[发明专利]一种行人惯性导航全时全程逆向平滑滤波方法

摘要

一种行人惯性导航全时全程逆向平滑滤波方法：在系统进行初始对准之后，在运动状态时，利用加速度和角速度信息进行捷联解算，当进入下一个零速区间时，即脚部着地时，利用RTS平滑算法，对导航系统状态误差进行最优估计，并修正系统状态；然后利用逆向捷联解算，反向计算系统的导航状态变量，计算到上一个零速静止区间，然后再利用卡尔曼滤波器将前向捷联解算的系统状态信息与逆向捷联解算的系统状态信息进行融合，获得精度更高的状态信息；最后，利用卡尔曼滤波得到的运动状态信息，对零速区间再次进行RTS平滑滤波，得到系统状态信息的最优估计。

主权项

1.一种行人惯性导航全时全程逆向平滑滤波方法，其特征在于：包括以下步骤：步骤1，以当地地理坐标系为导航坐标系，建立系统的误差传递模型，模型的15维状态变量包括：3维位置误差、3维速度误差、3维姿态误差、3维加速度计输出误差、3维陀螺仪输出误差。步骤2，初始的静止区间，进行系统初始对准，利用加速度计和陀螺仪传感器信息，计算载体的初始姿态角，包括俯仰角、滚转角和偏航角，确定初始四元数。步骤3，进入运动区间，利用加速度和角速度信息进行捷联解算，计算系统的3维位置、3维速度和3维姿态信息。步骤4，进入下一个静止区间，利用RTS平滑算法，在系统的零速状态下进行平滑滤波，对系统的15维状态误差进行最佳估计，然后利用估计的误差信息对系统的位置、速度、姿态、加速度输出和陀螺仪输出进行校正。步骤5，从第二个静止区间的开始时刻，记作Tj，利用逆向导航算法，以Tj时刻的系统状态作为初始状态，并利用步骤4中估计的传感器误差对运动区间的传感器输出进行修正，然后逆向解算系统的状态信息。步骤6，逆向解算到达上一个静止区间的最后时刻，记作Ti，此时已获得系统运动区间的正向捷联解算状态和逆向捷联解算状态，利用卡尔曼滤波器，将两个方向解算的系统状态信息进行融合，得到更加准确的系统状态信息。步骤7，从Tj时刻开始，再次进行RTS平滑滤波，此时利用的传感器输出为步骤4中的校正后的传感器输出，Tj时刻的系统状态为步骤6中双向融合后的系统状态信息。上述步骤就是本发明从一个零速区间到另一个零速区间的逆向平滑滤波方法的一个完整周期，后续滤波重复上述周期即可。