integrate.py 文件源码

def _correct(self, x):
        i = self.traj.shape[0] - 1
        d_lat = x[1] / earth.R0
        d_lon = x[0] / (earth.R0 * np.cos(self.lat_arr[i]))
        self.lat_arr[i] -= d_lat
        self.lon_arr[i] -= d_lon

        phi = x[4:7]
        phi[2] += d_lon * np.sin(self.lat_arr[i])

        VE_new = self.VE_arr[i] - x[2]
        VN_new = self.VN_arr[i] - x[3]

        self.VE_arr[i] = VE_new - phi[2] * VN_new
        self.VN_arr[i] = VN_new + phi[2] * VE_new

        self.Cnb_arr[i] = dcm.from_rv(phi).dot(self.Cnb_arr[i])
        h, p, r = dcm.to_hpr(self.Cnb_arr[i])

        self.traj.iloc[-1] = [np.rad2deg(self.lat_arr[i]),
                              np.rad2deg(self.lon_arr[i]),
                              self.VE_arr[i], self.VN_arr[i], h, p, r]