mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-07-19 16:22:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

Mesh3D: Add accelerometer bias to loc_kf (#22880)

Browse Source 
* add accel-bias to mesh3d

* remove acc scale
old-commit-hash: 5040427cb739fad364319070a0f87ab03031cd99
This commit is contained in:
Vivek Aithal
2021-11-29 19:30:14 -08:00
committed by GitHub
parent 9b0fcaeb84
commit 87f37d73a3
1 changed files with 16 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/locationd/models/loc_kf.py
+16 -7
View File
@@ -49,6 +49,7 @@ class States():
GLONASS_FREQ_SLOPE = slice(27, 28) # GLONASS bias in m expressed as bias + freq_num*freq_slope
CLOCK_ACCELERATION = slice(28, 29) # clock acceleration in light-meters/s**2,
ACCELEROMETER_SCALE = slice(29, 30) # scale of mems accelerometer
ACCELEROMETER_BIAS = slice(30, 33) # bias of mems accelerometer
# Error-state has different slices because it is an ESKF
ECEF_POS_ERR = slice(0, 3)
@@ -66,6 +67,7 @@ class States():
GLONASS_FREQ_SLOPE_ERR = slice(26, 27)
CLOCK_ACCELERATION_ERR = slice(27, 28)
ACCELEROMETER_SCALE_ERR = slice(28, 29)
ACCELEROMETER_BIAS_ERR = slice(29, 32)
class LocKalman():
@@ -82,7 +84,8 @@ class LocKalman():
0, 0, 0,
0, 0,
0,
1], dtype=np.float64)
1,
0, 0, 0], dtype=np.float64)
# state covariance
P_initial = np.diag([1e16, 1e16, 1e16,
@@ -97,7 +100,8 @@ class LocKalman():
(0.01)**2, (0.01)**2, (0.01)**2,
10**2, 1**2,
0.2**2,
0.05**2])
0.05**2,
0.05**2, 0.05**2, 0.05**2])
# process noise
Q = np.diag([0.03**2, 0.03**2, 0.03**2,
@@ -112,7 +116,8 @@ class LocKalman():
(0.05 / 60)**2, (0.05 / 60)**2, (0.05 / 60)**2,
(.1)**2, (.01)**2,
0.005**2,
(0.02 / 100)**2])
(0.02 / 100)**2,
(0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2])
# measurements that need to pass mahalanobis distance outlier rejector
maha_test_kinds = [ObservationKind.ORB_FEATURES] # , ObservationKind.PSEUDORANGE, ObservationKind.PSEUDORANGE_RATE]
@@ -154,7 +159,8 @@ class LocKalman():
glonass_bias = state[States.GLONASS_BIAS, :]
glonass_freq_slope = state[States.GLONASS_FREQ_SLOPE, :]
ca = state[States.CLOCK_ACCELERATION, :]
accel_scale = state[States.ACCELEROMETER_SCALE, :]
# accel_scale = state[States.ACCELEROMETER_SCALE, :]
accel_bias = state[States.ACCELEROMETER_BIAS, :]
dt = sp.Symbol('dt')
@@ -290,7 +296,8 @@ class LocKalman():
pos = sp.Matrix([x, y, z])
# add 1 for stability, prevent division by 0
gravity = quat_rot.T * ((EARTH_GM / ((x**2 + y**2 + z**2 + 1)**(3.0 / 2.0))) * pos)
h_acc_sym = imu_rot * (accel_scale[0] * (gravity + acceleration))
h_acc_sym = imu_rot * (gravity + acceleration + accel_bias)
h_acc_stationary_sym = acceleration
h_phone_rot_sym = sp.Matrix([vroll, vpitch, vyaw])
speed = sp.sqrt(vx**2 + vy**2 + vz**2)
@@ -320,7 +327,8 @@ class LocKalman():
[h_relative_motion, ObservationKind.CAMERA_ODO_TRANSLATION, None],
[h_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, None],
[h_imu_frame_sym, ObservationKind.IMU_FRAME, None],
[h_orb_point_sym, ObservationKind.ORB_POINT, orb_epos_sym]]
[h_orb_point_sym, ObservationKind.ORB_POINT, orb_epos_sym],
[h_acc_stationary_sym, ObservationKind.NO_ACCEL, None]]
# MSCKF configuration
if N > 0:
@@ -368,7 +376,8 @@ class LocKalman():
ObservationKind.CAMERA_ODO_ROTATION: np.diag([0.05**2, 0.05**2, 0.05**2]),
ObservationKind.IMU_FRAME: np.diag([0.05**2, 0.05**2, 0.05**2]),
ObservationKind.NO_ROT: np.diag([0.0025**2, 0.0025**2, 0.0025**2]),
ObservationKind.ECEF_POS: np.diag([5**2, 5**2, 5**2])}
ObservationKind.ECEF_POS: np.diag([5**2, 5**2, 5**2]),
ObservationKind.NO_ACCEL: np.diag([0.0025**2, 0.0025**2, 0.0025**2])}
# MSCKF stuff
self.N = N