def handle_gps(self, log, current_time):
self.converter = coord.LocalCoord.from_geodetic([
log.gpsLocationExternal.latitude,
log.gpsLocationExternal.longitude, log.gpsLocationExternal.altitude
])
fix_ecef = self.converter.ned2ecef([0, 0, 0])
# initing with bad bearing allowed, maybe bad?
if not self.filter_ready and len(list(self.dog.orbits.keys(
))) > 6: # and log.gpsLocationExternal.speed > 5:
self.filter_ready = True
initial_ecef = fix_ecef
initial_state = np.zeros(29)
gps_bearing = log.gpsLocationExternal.bearing * (np.pi / 180)
initial_pose_ecef = ecef_euler_from_ned(initial_ecef,
[0, 0, gps_bearing])
initial_pose_ecef_quat = euler2quat(initial_pose_ecef)
gps_speed = log.gpsLocationExternal.speed
quat_uncertainty = 0.2**2
initial_pose_ecef_quat = euler2quat(initial_pose_ecef)
initial_state[:3] = initial_ecef
initial_state[3:7] = initial_pose_ecef_quat
initial_state[7:10] = rotations_from_quats(
initial_pose_ecef_quat).dot(np.array([gps_speed, 0, 0]))
initial_state[18] = 1
initial_state[22] = 1
covs_diag = np.array([
10**2, 10**2, 10**2, quat_uncertainty, quat_uncertainty,
quat_uncertainty, 2**2, 2**2, 2**2, 1, 1, 1, 20000000**2,
100**2, 0.01**2, 0.01**2, 0.01**2, 0.02**2, 2**2, 2**2, 2**2,
.01**2, 0.01**2, 0.01**2, 0.01**2, 10**2, 1**2, 0.2**2
])
self.kf.init_state(initial_state,
covs=np.diag(covs_diag),
filter_time=current_time)
print("Filter initialized")
elif self.filter_ready:
#self.update_kalman(current_time, ObservationKind.ECEF_POS, fix_ecef)
gps_est_error = np.sqrt((self.kf.x[0] - fix_ecef[0])**2 +
(self.kf.x[1] - fix_ecef[1])**2 +
(self.kf.x[2] - fix_ecef[2])**2)
if gps_est_error > 50:
cloudlog.info(
"Locationd vs ubloxLocation difference too large, kalman reset"
)
self.reset_kalman()
def handle_gps(self, current_time, log):
converter = coord.LocalCoord.from_geodetic(
[log.latitude, log.longitude, log.altitude])
fix_ecef = converter.ned2ecef([0, 0, 0])
# TODO initing with bad bearing not allowed, maybe not bad?
if not self.filter_ready and log.speed > 5:
self.filter_ready = True
initial_ecef = fix_ecef
gps_bearing = math.radians(log.bearing)
initial_pose_ecef = ecef_euler_from_ned(initial_ecef,
[0, 0, gps_bearing])
initial_pose_ecef_quat = euler2quat(initial_pose_ecef)
gps_speed = log.speed
quat_uncertainty = 0.2**2
initial_pose_ecef_quat = euler2quat(initial_pose_ecef)
initial_state = initial_x
initial_covs_diag = initial_P_diag
initial_state[States.ECEF_POS] = initial_ecef
initial_state[States.ECEF_ORIENTATION] = initial_pose_ecef_quat
initial_state[States.ECEF_VELOCITY] = rotations_from_quats(
initial_pose_ecef_quat).dot(np.array([gps_speed, 0, 0]))
initial_covs_diag[States.ECEF_POS_ERR] = 10**2
initial_covs_diag[States.ECEF_ORIENTATION_ERR] = quat_uncertainty
initial_covs_diag[States.ECEF_VELOCITY_ERR] = 1**2
self.kf.init_state(initial_state,
covs=np.diag(initial_covs_diag),
filter_time=current_time)
cloudlog.info("Filter initialized")
elif self.filter_ready:
self.update_kalman(current_time, ObservationKind.ECEF_POS,
fix_ecef)
gps_est_error = np.sqrt((self.kf.x[0] - fix_ecef[0])**2 +
(self.kf.x[1] - fix_ecef[1])**2 +
(self.kf.x[2] - fix_ecef[2])**2)
if gps_est_error > 50:
cloudlog.error(
"Locationd vs ubloxLocation difference too large, kalman reset"
)
self.reset_kalman()
def test_rot_euler(self):
for eul in eulers:
np.testing.assert_allclose(euler2quat(eul),
euler2quat(rot2euler(euler2rot(eul))),
rtol=1e-7)
for eul in eulers:
np.testing.assert_allclose(euler2quat(eul),
euler2quat(
rot2euler(euler2rot(list(eul)))),
rtol=1e-7)
np.testing.assert_allclose(euler2quat(eulers),
euler2quat(rot2euler(euler2rot(eulers))),
rtol=1e-7)
def test_quat_euler(self):
for i, eul in enumerate(eulers):
np.testing.assert_allclose(quats[i], euler2quat(eul), rtol=1e-7)
np.testing.assert_allclose(quats[i],
euler2quat(quat2euler(quats[i])),
rtol=1e-6)
for i, eul in enumerate(eulers):
np.testing.assert_allclose(quats[i],
euler2quat(list(eul)),
rtol=1e-7)
np.testing.assert_allclose(quats[i],
euler2quat(quat2euler(list(quats[i]))),
rtol=1e-6)
np.testing.assert_allclose(quats, euler2quat(eulers), rtol=1e-7)
np.testing.assert_allclose(quats,
euler2quat(quat2euler(quats)),
rtol=1e-6)