功能：用于惯性导航的位置速度姿态时间更新 输入： t1 %时刻(s)，GPST r1 %位置，纬度(rad)、经度L(rad)、高程(m) v1 %当地北东地坐标系n 中的速度(m/s) q1 %n 系至 b 系的旋转单位四元数，无量纲 f1 %b系中的视加速度(m/s^2) w1 %b系相对于i 系的旋转角速度矢量在b系中的值(rad/s) dt %时间间隔(s) 输出： r2 %位置，纬度(rad)、经度L(rad)、高程(m) v2 %当地北东地坐标系n 中的速度(m