mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-02-18 22:23:56 +08:00
Code Issues Packages Projects Releases Wiki Activity

preinit locationd filter (#2569)

Browse Source 
* preinit

* update ref commit
old-commit-hash: 9f5dfb74f9
This commit is contained in:
HaraldSchafer
2020-11-19 15:14:35 -08:00
committed by GitHub
parent 444080d1b6
commit c4d50c0bf0
2 changed files with 7 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
selfdrive/locationd/locationd.py
View File

@@ -196,14 +196,14 @@ class Localizer():
orientation_ned = ned_euler_from_ecef(ecef_pos, orientation_ecef)
orientation_ned_gps = np.array([0, 0, np.radians(log.bearing)])
orientation_error = np.mod(orientation_ned - orientation_ned_gps - np.pi, 2*np.pi) - np.pi
initial_pose_ecef_quat = quat_from_euler(ecef_euler_from_ned(ecef_pos, orientation_ned_gps))
if np.linalg.norm(ecef_vel) > 5 and np.linalg.norm(orientation_error) > 1:
cloudlog.error("Locationd vs ubloxLocation orientation difference too large, kalman reset")
initial_pose_ecef_quat = quat_from_euler(ecef_euler_from_ned(ecef_pos, orientation_ned_gps))
self.reset_kalman(init_orient=initial_pose_ecef_quat)
self.reset_kalman(init_pos=ecef_pos, init_orient=initial_pose_ecef_quat)
self.update_kalman(current_time, ObservationKind.ECEF_ORIENTATION_FROM_GPS, initial_pose_ecef_quat)
elif gps_est_error > 50:
cloudlog.error("Locationd vs ubloxLocation position difference too large, kalman reset")
self.reset_kalman()
self.reset_kalman(init_pos=ecef_pos, init_orient=initial_pose_ecef_quat)
self.update_kalman(current_time, ObservationKind.ECEF_POS, ecef_pos, R=ecef_pos_R)
self.update_kalman(current_time, ObservationKind.ECEF_VEL, ecef_vel, R=ecef_vel_R)
@@ -267,12 +267,14 @@ class Localizer():
self.calib_from_device = self.device_from_calib.T
self.calibrated = log.calStatus == 1
def reset_kalman(self, current_time=None, init_orient=None):
def reset_kalman(self, current_time=None, init_orient=None, init_pos=None):
self.filter_time = current_time
init_x = LiveKalman.initial_x.copy()
# too nonlinear to init on completely wrong
if init_orient is not None:
init_x[3:7] = init_orient
if init_pos is not None:
init_x[:3] = init_pos
self.kf.init_state(init_x, covs=np.diag(LiveKalman.initial_P_diag), filter_time=current_time)
self.observation_buffer = []

2
selfdrive/test/process_replay/ref_commit
View File

@@ -1 +1 @@
aeb870b1d4f523506570f66e76dde0b036c58f40
8cc45567f52bb3bff7d354b8c387b2dc51c90731