use roll std from locationd (#23449)

* use roll std from locationd

* cleaner

* add todo

* new ref

Co-authored-by: Harald Schafer <harald.the.engineer@gmail.com>
old-commit-hash: f808b174afbcc1fc6924d49fb0c32da5c02aad33
This commit is contained in:
Vivek Aithal
2022-01-12 21:37:09 -08:00
committed by GitHub
parent f112aef5a7
commit e4c2c2cf2e
3 changed files with 8 additions and 7 deletions
+3 -1
View File
@@ -45,10 +45,12 @@ class ParamsLearner:
yaw_rate_std = msg.angularVelocityCalibrated.std[2]
localizer_roll = msg.orientationNED.value[0]
localizer_roll_std = msg.orientationNED.std[0]
roll_valid = msg.orientationNED.valid and ROLL_MIN < localizer_roll < ROLL_MAX
if roll_valid:
roll = localizer_roll
roll_std = np.radians(1.0)
# Experimentally found multiplier of 2 to be best trade-off between stability and accuracy or similar?
roll_std = 2 * localizer_roll_std
else:
# This is done to bound the road roll estimate when localizer values are invalid
roll = 0.0