NAV_INIT: # SUBROUTINE 2: MAIN FILTER ALGORITHM, INS + GNSS # === Time-Update === dt = imu.time - self.last_imu.time q0, qvec = self.last_estATT[0], self.last_estATT[1:4] C_B2N = navpy.quat2dcm(q0,qvec).T # 0. Data Acquisition f_b=np.array([0.5*(self.last_im...