3.4.1 估计姿态计算