sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
openpilot is an open source driver assistance system. openpilot performs the functions of Automated Lane Centering and Adaptive Cruise Control for over 200 supported car makes and models.
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
14697 Commits
123 Branches
82 Tags
5.0 GiB
Tag: Branch: Tree: __jenkins_loop_nostmaster5
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from '__jenkins_loop_nostmaster5'
${ noResults }
openpilot_comma/selfdrive/locationd/locationd.py

333 lines
14 KiB
Raw Permalink Normal View History Unescape

locationd no GPS (#33029) * Pose kf draft old-commit-hash: 17dd4d576e597792f0e18826498c00076739f92b * Fix it old-commit-hash: 13ac120affe58fd22e871586ea5f4d335b3e9d2b * Add translation noise old-commit-hash: 166529cb612858c4ce80649367ac35b2b6007e1d * Add gravity to acc old-commit-hash: 8fcfed544b8e090ccc86b189c13bc03c6c190613 * Use pyx old-commit-hash: 8e69e0baa0a4c43b4d0c22535711f296f05420aa * Indent old-commit-hash: 25b19a73644cdcb571ccf1a1d8acb88a0d066c67 * Reset function old-commit-hash: ca5d2736da15e4fd6539f7268133320735b7c9cc * Add device_from_ned and ned_from_device transformations old-commit-hash: a60d25da0edc311e583549dc015fa595749fd4ae * Fix rotations old-commit-hash: d6d582f7f6d19a2bc2308dbcb0c9f81363e325b6 * kms old-commit-hash: 681bc4e50374795ccc61422c3ce4ffb51389fce2 * Centripetal acceleration old-commit-hash: 6e574506d27e5b76a04b2097d94efa4ca91ead71 * Rewrite draft old-commit-hash: 4a2aad0146267460e5d30036c8cdb2bef94d1d7c * Remove old locationd stuff old-commit-hash: c2be9f7dbf22fb5cd29e437cd7891a7d52266fba * Python process now old-commit-hash: 83fac85f28c0b546b6965aafe1dd8a089e67f5b3 * Process replay fix old-commit-hash: c44f9de98583c49dad0b22497869b3bb0266fcd9 * Add checks for timing and validity old-commit-hash: aed4fbe2d00ca620e01a0e0ee99a4871c939de36 * Fixes old-commit-hash: 3f052c658c16984a34915f38afdfbfd0fb19a267 * Process replay config fixes old-commit-hash: 1c56690ee7ceb3c23c9ec2b2713352191212716e * static analysis fixes old-commit-hash: 6145e2c140ea9aa97e75069c3ddd82172cadc866 * lp in latcontrol old-commit-hash: 9abf7359d68e794c69052724f3aca14b04dd3cca * Fix SensorEvent name for acceleration old-commit-hash: 91a1ad6c604727c9c898ba4aefe9478022b167fd * Ignore sensor readings from segments with multiple imus old-commit-hash: 1f05af63d6cc605ea98d7da0d727a5bd8d7819b0 * Update shebang old-commit-hash: e3f2f5c10df3a4ba698421335bfeffc63d1a8797 * Replace llk with lp old-commit-hash: 99b6c7ba08de6b703708fef0b8fd2d8cb24b92c0 * Refactor locationd scenario test old-commit-hash: 7f5f788f071b7647e36f854df927cc5b6f819a84 * Add more debugging tools old-commit-hash: 8d4e364867e143ea35f4bfd00d8212aaf170a1d1 * Param name update old-commit-hash: 5151e8f5520f067e7808e3f0baa628fbf8fb7337 * Fix expected observations old-commit-hash: d6a0d4c1a96c438fb6893e8b6ff43becf6061b75 * Handle invalid measurements old-commit-hash: 549362571e74ad1e7ec9368f6026378c54a29adf * Fix spelling old-commit-hash: eefd7c4c92fb486452e9b83c7121d2599811852b * Include observations in debug info too old-commit-hash: 625806d1110b3bffe249cd1d03416f2a3f2c1868 * Store error instead of expected observation old-commit-hash: 1cb7a799b67e56af4eddc6608d5b0e295f2d888c * Renames old-commit-hash: a7eec74640fc5cc7a5e86172d2087b66cb93d17d * Zero the yaw old-commit-hash: 96150779590fcb8ac50e8ffe8f8df03105983179 * New state_dot for orientation old-commit-hash: c1456bf3a0c5af2f888aa6ff0b5ffc2e5516ddf7 * Fix the state transformations old-commit-hash: 7cb9b91e2f99caa4ac3fb748c7f23bb8bf9f65db * Update process in test_onroad old-commit-hash: 854afab7c39ca7dec2b42974cecbb5310b82b617 * Test polling on cameraOdometry old-commit-hash: a78e8b7d61618177f15c9892e2fa1e51620deca8 * Keep the copy of x and P returned from predict old-commit-hash: 3c4159a6a7d7383265a99f3f78f8805d2fcfc8cd * Remove polling again old-commit-hash: f7228675c5fd2de5f879c4786859f1abcec27d68 * Remove locationd.cc old-commit-hash: d7005599b2b178e688c3bd1959d6b69357d3a663 * Optim in _finite_check old-commit-hash: 58ad6a06b9380960e9f69eb98663ddb97461e8d5 * Access .t once old-commit-hash: d03252e75ed4cbdb49291a60c904568e6a3d3399 * Move the timing check to cam odo code path old-commit-hash: 6a1c26f8c201e1feb601753f0cb7299cf981b47e * Call all_checks only once old-commit-hash: 373809cebf8d9db89d1ab00f4c8c933f33038e78 * Do not sort old-commit-hash: 2984cd02c0ab76827b8c7e32f7e637b261425025 * Check sm.updated old-commit-hash: 11c48de3f0802eb4783899f6a37737078dbf2da4 * Remove test_params_gps old-commit-hash: 82db4fd1a876cc2402702edc74eba0a8ac5da858 * Increase tolerance old-commit-hash: 927d6f05249d2c8ec40b32e2a0dcde8e1a469fb3 * Fix static old-commit-hash: 2d86d62c74d5ac0ad56ec3855a126e00a26cd490 * Try separate sockets for sensors old-commit-hash: 5dade63947ab237f0b4555f45d941a8851449ab1 * sensor_all_checks old-commit-hash: e25f40dd6b37ee76cd9cc2b19be552baf1355ec3 * Fix static old-commit-hash: 328cf1ad86079746b4f3fde55539e4acb92d285e * Set the cpu limit to 25 old-commit-hash: 7ba696ff54c5d3bfa42e42624d124f2a1914a96d * Make it prettier old-commit-hash: cd6270dec80d8b9dac784ddd4767a1a46bcff4b7 * Prettier old-commit-hash: 1b17931d23d37f299dad54139eaf283a89592bf5 * Increase the cpu budget to 260 old-commit-hash: 20173afb937a2609c8a9905aee0b2b093cb8bba4 * Change trans std mult. 2 works better * Update ref commit * Update ref commit
8 months ago
#!/usr/bin/env python3
import os
import json
import time
import capnp
import numpy as np
from enum import Enum
from collections import defaultdict
from cereal import log, messaging
locationd: timing spikes resiliance (#34080) * Locationd scenario for timing spike * Add test for consistent timing spike * Resiliance to bad timing * Test update * Refactor test * fix comment * Decay based on frequency * Fix * Update comment * Only for critical services * Fix tests
5 months ago
from cereal.services import SERVICE_LIST
locationd no GPS (#33029) * Pose kf draft old-commit-hash: 17dd4d576e597792f0e18826498c00076739f92b * Fix it old-commit-hash: 13ac120affe58fd22e871586ea5f4d335b3e9d2b * Add translation noise old-commit-hash: 166529cb612858c4ce80649367ac35b2b6007e1d * Add gravity to acc old-commit-hash: 8fcfed544b8e090ccc86b189c13bc03c6c190613 * Use pyx old-commit-hash: 8e69e0baa0a4c43b4d0c22535711f296f05420aa * Indent old-commit-hash: 25b19a73644cdcb571ccf1a1d8acb88a0d066c67 * Reset function old-commit-hash: ca5d2736da15e4fd6539f7268133320735b7c9cc * Add device_from_ned and ned_from_device transformations old-commit-hash: a60d25da0edc311e583549dc015fa595749fd4ae * Fix rotations old-commit-hash: d6d582f7f6d19a2bc2308dbcb0c9f81363e325b6 * kms old-commit-hash: 681bc4e50374795ccc61422c3ce4ffb51389fce2 * Centripetal acceleration old-commit-hash: 6e574506d27e5b76a04b2097d94efa4ca91ead71 * Rewrite draft old-commit-hash: 4a2aad0146267460e5d30036c8cdb2bef94d1d7c * Remove old locationd stuff old-commit-hash: c2be9f7dbf22fb5cd29e437cd7891a7d52266fba * Python process now old-commit-hash: 83fac85f28c0b546b6965aafe1dd8a089e67f5b3 * Process replay fix old-commit-hash: c44f9de98583c49dad0b22497869b3bb0266fcd9 * Add checks for timing and validity old-commit-hash: aed4fbe2d00ca620e01a0e0ee99a4871c939de36 * Fixes old-commit-hash: 3f052c658c16984a34915f38afdfbfd0fb19a267 * Process replay config fixes old-commit-hash: 1c56690ee7ceb3c23c9ec2b2713352191212716e * static analysis fixes old-commit-hash: 6145e2c140ea9aa97e75069c3ddd82172cadc866 * lp in latcontrol old-commit-hash: 9abf7359d68e794c69052724f3aca14b04dd3cca * Fix SensorEvent name for acceleration old-commit-hash: 91a1ad6c604727c9c898ba4aefe9478022b167fd * Ignore sensor readings from segments with multiple imus old-commit-hash: 1f05af63d6cc605ea98d7da0d727a5bd8d7819b0 * Update shebang old-commit-hash: e3f2f5c10df3a4ba698421335bfeffc63d1a8797 * Replace llk with lp old-commit-hash: 99b6c7ba08de6b703708fef0b8fd2d8cb24b92c0 * Refactor locationd scenario test old-commit-hash: 7f5f788f071b7647e36f854df927cc5b6f819a84 * Add more debugging tools old-commit-hash: 8d4e364867e143ea35f4bfd00d8212aaf170a1d1 * Param name update old-commit-hash: 5151e8f5520f067e7808e3f0baa628fbf8fb7337 * Fix expected observations old-commit-hash: d6a0d4c1a96c438fb6893e8b6ff43becf6061b75 * Handle invalid measurements old-commit-hash: 549362571e74ad1e7ec9368f6026378c54a29adf * Fix spelling old-commit-hash: eefd7c4c92fb486452e9b83c7121d2599811852b * Include observations in debug info too old-commit-hash: 625806d1110b3bffe249cd1d03416f2a3f2c1868 * Store error instead of expected observation old-commit-hash: 1cb7a799b67e56af4eddc6608d5b0e295f2d888c * Renames old-commit-hash: a7eec74640fc5cc7a5e86172d2087b66cb93d17d * Zero the yaw old-commit-hash: 96150779590fcb8ac50e8ffe8f8df03105983179 * New state_dot for orientation old-commit-hash: c1456bf3a0c5af2f888aa6ff0b5ffc2e5516ddf7 * Fix the state transformations old-commit-hash: 7cb9b91e2f99caa4ac3fb748c7f23bb8bf9f65db * Update process in test_onroad old-commit-hash: 854afab7c39ca7dec2b42974cecbb5310b82b617 * Test polling on cameraOdometry old-commit-hash: a78e8b7d61618177f15c9892e2fa1e51620deca8 * Keep the copy of x and P returned from predict old-commit-hash: 3c4159a6a7d7383265a99f3f78f8805d2fcfc8cd * Remove polling again old-commit-hash: f7228675c5fd2de5f879c4786859f1abcec27d68 * Remove locationd.cc old-commit-hash: d7005599b2b178e688c3bd1959d6b69357d3a663 * Optim in _finite_check old-commit-hash: 58ad6a06b9380960e9f69eb98663ddb97461e8d5 * Access .t once old-commit-hash: d03252e75ed4cbdb49291a60c904568e6a3d3399 * Move the timing check to cam odo code path old-commit-hash: 6a1c26f8c201e1feb601753f0cb7299cf981b47e * Call all_checks only once old-commit-hash: 373809cebf8d9db89d1ab00f4c8c933f33038e78 * Do not sort old-commit-hash: 2984cd02c0ab76827b8c7e32f7e637b261425025 * Check sm.updated old-commit-hash: 11c48de3f0802eb4783899f6a37737078dbf2da4 * Remove test_params_gps old-commit-hash: 82db4fd1a876cc2402702edc74eba0a8ac5da858 * Increase tolerance old-commit-hash: 927d6f05249d2c8ec40b32e2a0dcde8e1a469fb3 * Fix static old-commit-hash: 2d86d62c74d5ac0ad56ec3855a126e00a26cd490 * Try separate sockets for sensors old-commit-hash: 5dade63947ab237f0b4555f45d941a8851449ab1 * sensor_all_checks old-commit-hash: e25f40dd6b37ee76cd9cc2b19be552baf1355ec3 * Fix static old-commit-hash: 328cf1ad86079746b4f3fde55539e4acb92d285e * Set the cpu limit to 25 old-commit-hash: 7ba696ff54c5d3bfa42e42624d124f2a1914a96d * Make it prettier old-commit-hash: cd6270dec80d8b9dac784ddd4767a1a46bcff4b7 * Prettier old-commit-hash: 1b17931d23d37f299dad54139eaf283a89592bf5 * Increase the cpu budget to 260 old-commit-hash: 20173afb937a2609c8a9905aee0b2b093cb8bba4 * Change trans std mult. 2 works better * Update ref commit * Update ref commit
8 months ago
from openpilot.common.transformations.orientation import rot_from_euler
from openpilot.common.realtime import config_realtime_process
from openpilot.common.params import Params
locationd: use cloudlog (#34508) * Use cloudlog instead of print * Changes
3 months ago
from openpilot.common.swaglog import cloudlog
locationd no GPS (#33029) * Pose kf draft old-commit-hash: 17dd4d576e597792f0e18826498c00076739f92b * Fix it old-commit-hash: 13ac120affe58fd22e871586ea5f4d335b3e9d2b * Add translation noise old-commit-hash: 166529cb612858c4ce80649367ac35b2b6007e1d * Add gravity to acc old-commit-hash: 8fcfed544b8e090ccc86b189c13bc03c6c190613 * Use pyx old-commit-hash: 8e69e0baa0a4c43b4d0c22535711f296f05420aa * Indent old-commit-hash: 25b19a73644cdcb571ccf1a1d8acb88a0d066c67 * Reset function old-commit-hash: ca5d2736da15e4fd6539f7268133320735b7c9cc * Add device_from_ned and ned_from_device transformations old-commit-hash: a60d25da0edc311e583549dc015fa595749fd4ae * Fix rotations old-commit-hash: d6d582f7f6d19a2bc2308dbcb0c9f81363e325b6 * kms old-commit-hash: 681bc4e50374795ccc61422c3ce4ffb51389fce2 * Centripetal acceleration old-commit-hash: 6e574506d27e5b76a04b2097d94efa4ca91ead71 * Rewrite draft old-commit-hash: 4a2aad0146267460e5d30036c8cdb2bef94d1d7c * Remove old locationd stuff old-commit-hash: c2be9f7dbf22fb5cd29e437cd7891a7d52266fba * Python process now old-commit-hash: 83fac85f28c0b546b6965aafe1dd8a089e67f5b3 * Process replay fix old-commit-hash: c44f9de98583c49dad0b22497869b3bb0266fcd9 * Add checks for timing and validity old-commit-hash: aed4fbe2d00ca620e01a0e0ee99a4871c939de36 * Fixes old-commit-hash: 3f052c658c16984a34915f38afdfbfd0fb19a267 * Process replay config fixes old-commit-hash: 1c56690ee7ceb3c23c9ec2b2713352191212716e * static analysis fixes old-commit-hash: 6145e2c140ea9aa97e75069c3ddd82172cadc866 * lp in latcontrol old-commit-hash: 9abf7359d68e794c69052724f3aca14b04dd3cca * Fix SensorEvent name for acceleration old-commit-hash: 91a1ad6c604727c9c898ba4aefe9478022b167fd * Ignore sensor readings from segments with multiple imus old-commit-hash: 1f05af63d6cc605ea98d7da0d727a5bd8d7819b0 * Update shebang old-commit-hash: e3f2f5c10df3a4ba698421335bfeffc63d1a8797 * Replace llk with lp old-commit-hash: 99b6c7ba08de6b703708fef0b8fd2d8cb24b92c0 * Refactor locationd scenario test old-commit-hash: 7f5f788f071b7647e36f854df927cc5b6f819a84 * Add more debugging tools old-commit-hash: 8d4e364867e143ea35f4bfd00d8212aaf170a1d1 * Param name update old-commit-hash: 5151e8f5520f067e7808e3f0baa628fbf8fb7337 * Fix expected observations old-commit-hash: d6a0d4c1a96c438fb6893e8b6ff43becf6061b75 * Handle invalid measurements old-commit-hash: 549362571e74ad1e7ec9368f6026378c54a29adf * Fix spelling old-commit-hash: eefd7c4c92fb486452e9b83c7121d2599811852b * Include observations in debug info too old-commit-hash: 625806d1110b3bffe249cd1d03416f2a3f2c1868 * Store error instead of expected observation old-commit-hash: 1cb7a799b67e56af4eddc6608d5b0e295f2d888c * Renames old-commit-hash: a7eec74640fc5cc7a5e86172d2087b66cb93d17d * Zero the yaw old-commit-hash: 96150779590fcb8ac50e8ffe8f8df03105983179 * New state_dot for orientation old-commit-hash: c1456bf3a0c5af2f888aa6ff0b5ffc2e5516ddf7 * Fix the state transformations old-commit-hash: 7cb9b91e2f99caa4ac3fb748c7f23bb8bf9f65db * Update process in test_onroad old-commit-hash: 854afab7c39ca7dec2b42974cecbb5310b82b617 * Test polling on cameraOdometry old-commit-hash: a78e8b7d61618177f15c9892e2fa1e51620deca8 * Keep the copy of x and P returned from predict old-commit-hash: 3c4159a6a7d7383265a99f3f78f8805d2fcfc8cd * Remove polling again old-commit-hash: f7228675c5fd2de5f879c4786859f1abcec27d68 * Remove locationd.cc old-commit-hash: d7005599b2b178e688c3bd1959d6b69357d3a663 * Optim in _finite_check old-commit-hash: 58ad6a06b9380960e9f69eb98663ddb97461e8d5 * Access .t once old-commit-hash: d03252e75ed4cbdb49291a60c904568e6a3d3399 * Move the timing check to cam odo code path old-commit-hash: 6a1c26f8c201e1feb601753f0cb7299cf981b47e * Call all_checks only once old-commit-hash: 373809cebf8d9db89d1ab00f4c8c933f33038e78 * Do not sort old-commit-hash: 2984cd02c0ab76827b8c7e32f7e637b261425025 * Check sm.updated old-commit-hash: 11c48de3f0802eb4783899f6a37737078dbf2da4 * Remove test_params_gps old-commit-hash: 82db4fd1a876cc2402702edc74eba0a8ac5da858 * Increase tolerance old-commit-hash: 927d6f05249d2c8ec40b32e2a0dcde8e1a469fb3 * Fix static old-commit-hash: 2d86d62c74d5ac0ad56ec3855a126e00a26cd490 * Try separate sockets for sensors old-commit-hash: 5dade63947ab237f0b4555f45d941a8851449ab1 * sensor_all_checks old-commit-hash: e25f40dd6b37ee76cd9cc2b19be552baf1355ec3 * Fix static old-commit-hash: 328cf1ad86079746b4f3fde55539e4acb92d285e * Set the cpu limit to 25 old-commit-hash: 7ba696ff54c5d3bfa42e42624d124f2a1914a96d * Make it prettier old-commit-hash: cd6270dec80d8b9dac784ddd4767a1a46bcff4b7 * Prettier old-commit-hash: 1b17931d23d37f299dad54139eaf283a89592bf5 * Increase the cpu budget to 260 old-commit-hash: 20173afb937a2609c8a9905aee0b2b093cb8bba4 * Change trans std mult. 2 works better * Update ref commit * Update ref commit
8 months ago
from openpilot.selfdrive.locationd.helpers import rotate_std
from openpilot.selfdrive.locationd.models.pose_kf import PoseKalman, States
from openpilot.selfdrive.locationd.models.constants import ObservationKind, GENERATED_DIR
ACCEL_SANITY_CHECK = 100.0 # m/s^2
ROTATION_SANITY_CHECK = 10.0 # rad/s
TRANS_SANITY_CHECK = 200.0 # m/s
CALIB_RPY_SANITY_CHECK = 0.5 # rad (+- 30 deg)
MIN_STD_SANITY_CHECK = 1e-5 # m or rad
MAX_FILTER_REWIND_TIME = 0.8 # s
MAX_SENSOR_TIME_DIFF = 0.1 # s
YAWRATE_CROSS_ERR_CHECK_FACTOR = 30
locationd: frequency based bad observation resiliance and recovery (#34476) * Improve it * Fix static * Fix test_consistent_timing_spikes test * Fix tests * Comment * Remove crap
3 months ago
INPUT_INVALID_LIMIT = 2.0 # 1 (camodo) / 9 (sensor) bad input[s] ignored
INPUT_INVALID_RECOVERY = 10.0 # ~10 secs to resume after exceeding allowed bad inputs by one
locationd no GPS (#33029) * Pose kf draft old-commit-hash: 17dd4d576e597792f0e18826498c00076739f92b * Fix it old-commit-hash: 13ac120affe58fd22e871586ea5f4d335b3e9d2b * Add translation noise old-commit-hash: 166529cb612858c4ce80649367ac35b2b6007e1d * Add gravity to acc old-commit-hash: 8fcfed544b8e090ccc86b189c13bc03c6c190613 * Use pyx old-commit-hash: 8e69e0baa0a4c43b4d0c22535711f296f05420aa * Indent old-commit-hash: 25b19a73644cdcb571ccf1a1d8acb88a0d066c67 * Reset function old-commit-hash: ca5d2736da15e4fd6539f7268133320735b7c9cc * Add device_from_ned and ned_from_device transformations old-commit-hash: a60d25da0edc311e583549dc015fa595749fd4ae * Fix rotations old-commit-hash: d6d582f7f6d19a2bc2308dbcb0c9f81363e325b6 * kms old-commit-hash: 681bc4e50374795ccc61422c3ce4ffb51389fce2 * Centripetal acceleration old-commit-hash: 6e574506d27e5b76a04b2097d94efa4ca91ead71 * Rewrite draft old-commit-hash: 4a2aad0146267460e5d30036c8cdb2bef94d1d7c * Remove old locationd stuff old-commit-hash: c2be9f7dbf22fb5cd29e437cd7891a7d52266fba * Python process now old-commit-hash: 83fac85f28c0b546b6965aafe1dd8a089e67f5b3 * Process replay fix old-commit-hash: c44f9de98583c49dad0b22497869b3bb0266fcd9 * Add checks for timing and validity old-commit-hash: aed4fbe2d00ca620e01a0e0ee99a4871c939de36 * Fixes old-commit-hash: 3f052c658c16984a34915f38afdfbfd0fb19a267 * Process replay config fixes old-commit-hash: 1c56690ee7ceb3c23c9ec2b2713352191212716e * static analysis fixes old-commit-hash: 6145e2c140ea9aa97e75069c3ddd82172cadc866 * lp in latcontrol old-commit-hash: 9abf7359d68e794c69052724f3aca14b04dd3cca * Fix SensorEvent name for acceleration old-commit-hash: 91a1ad6c604727c9c898ba4aefe9478022b167fd * Ignore sensor readings from segments with multiple imus old-commit-hash: 1f05af63d6cc605ea98d7da0d727a5bd8d7819b0 * Update shebang old-commit-hash: e3f2f5c10df3a4ba698421335bfeffc63d1a8797 * Replace llk with lp old-commit-hash: 99b6c7ba08de6b703708fef0b8fd2d8cb24b92c0 * Refactor locationd scenario test old-commit-hash: 7f5f788f071b7647e36f854df927cc5b6f819a84 * Add more debugging tools old-commit-hash: 8d4e364867e143ea35f4bfd00d8212aaf170a1d1 * Param name update old-commit-hash: 5151e8f5520f067e7808e3f0baa628fbf8fb7337 * Fix expected observations old-commit-hash: d6a0d4c1a96c438fb6893e8b6ff43becf6061b75 * Handle invalid measurements old-commit-hash: 549362571e74ad1e7ec9368f6026378c54a29adf * Fix spelling old-commit-hash: eefd7c4c92fb486452e9b83c7121d2599811852b * Include observations in debug info too old-commit-hash: 625806d1110b3bffe249cd1d03416f2a3f2c1868 * Store error instead of expected observation old-commit-hash: 1cb7a799b67e56af4eddc6608d5b0e295f2d888c * Renames old-commit-hash: a7eec74640fc5cc7a5e86172d2087b66cb93d17d * Zero the yaw old-commit-hash: 96150779590fcb8ac50e8ffe8f8df03105983179 * New state_dot for orientation old-commit-hash: c1456bf3a0c5af2f888aa6ff0b5ffc2e5516ddf7 * Fix the state transformations old-commit-hash: 7cb9b91e2f99caa4ac3fb748c7f23bb8bf9f65db * Update process in test_onroad old-commit-hash: 854afab7c39ca7dec2b42974cecbb5310b82b617 * Test polling on cameraOdometry old-commit-hash: a78e8b7d61618177f15c9892e2fa1e51620deca8 * Keep the copy of x and P returned from predict old-commit-hash: 3c4159a6a7d7383265a99f3f78f8805d2fcfc8cd * Remove polling again old-commit-hash: f7228675c5fd2de5f879c4786859f1abcec27d68 * Remove locationd.cc old-commit-hash: d7005599b2b178e688c3bd1959d6b69357d3a663 * Optim in _finite_check old-commit-hash: 58ad6a06b9380960e9f69eb98663ddb97461e8d5 * Access .t once old-commit-hash: d03252e75ed4cbdb49291a60c904568e6a3d3399 * Move the timing check to cam odo code path old-commit-hash: 6a1c26f8c201e1feb601753f0cb7299cf981b47e * Call all_checks only once old-commit-hash: 373809cebf8d9db89d1ab00f4c8c933f33038e78 * Do not sort old-commit-hash: 2984cd02c0ab76827b8c7e32f7e637b261425025 * Check sm.updated old-commit-hash: 11c48de3f0802eb4783899f6a37737078dbf2da4 * Remove test_params_gps old-commit-hash: 82db4fd1a876cc2402702edc74eba0a8ac5da858 * Increase tolerance old-commit-hash: 927d6f05249d2c8ec40b32e2a0dcde8e1a469fb3 * Fix static old-commit-hash: 2d86d62c74d5ac0ad56ec3855a126e00a26cd490 * Try separate sockets for sensors old-commit-hash: 5dade63947ab237f0b4555f45d941a8851449ab1 * sensor_all_checks old-commit-hash: e25f40dd6b37ee76cd9cc2b19be552baf1355ec3 * Fix static old-commit-hash: 328cf1ad86079746b4f3fde55539e4acb92d285e * Set the cpu limit to 25 old-commit-hash: 7ba696ff54c5d3bfa42e42624d124f2a1914a96d * Make it prettier old-commit-hash: cd6270dec80d8b9dac784ddd4767a1a46bcff4b7 * Prettier old-commit-hash: 1b17931d23d37f299dad54139eaf283a89592bf5 * Increase the cpu budget to 260 old-commit-hash: 20173afb937a2609c8a9905aee0b2b093cb8bba4 * Change trans std mult. 2 works better * Update ref commit * Update ref commit
8 months ago
POSENET_STD_INITIAL_VALUE = 10.0
POSENET_STD_HIST_HALF = 20
locationd: frequency based bad observation resiliance and recovery (#34476) * Improve it * Fix static * Fix test_consistent_timing_spikes test * Fix tests * Comment * Remove crap
3 months ago
def calculate_invalid_input_decay(invalid_limit, recovery_time, frequency):
return (1 - 1 / (2 * invalid_limit)) ** (1 / (recovery_time * frequency))
locationd no GPS (#33029) * Pose kf draft old-commit-hash: 17dd4d576e597792f0e18826498c00076739f92b * Fix it old-commit-hash: 13ac120affe58fd22e871586ea5f4d335b3e9d2b * Add translation noise old-commit-hash: 166529cb612858c4ce80649367ac35b2b6007e1d * Add gravity to acc old-commit-hash: 8fcfed544b8e090ccc86b189c13bc03c6c190613 * Use pyx old-commit-hash: 8e69e0baa0a4c43b4d0c22535711f296f05420aa * Indent old-commit-hash: 25b19a73644cdcb571ccf1a1d8acb88a0d066c67 * Reset function old-commit-hash: ca5d2736da15e4fd6539f7268133320735b7c9cc * Add device_from_ned and ned_from_device transformations old-commit-hash: a60d25da0edc311e583549dc015fa595749fd4ae * Fix rotations old-commit-hash: d6d582f7f6d19a2bc2308dbcb0c9f81363e325b6 * kms old-commit-hash: 681bc4e50374795ccc61422c3ce4ffb51389fce2 * Centripetal acceleration old-commit-hash: 6e574506d27e5b76a04b2097d94efa4ca91ead71 * Rewrite draft old-commit-hash: 4a2aad0146267460e5d30036c8cdb2bef94d1d7c * Remove old locationd stuff old-commit-hash: c2be9f7dbf22fb5cd29e437cd7891a7d52266fba * Python process now old-commit-hash: 83fac85f28c0b546b6965aafe1dd8a089e67f5b3 * Process replay fix old-commit-hash: c44f9de98583c49dad0b22497869b3bb0266fcd9 * Add checks for timing and validity old-commit-hash: aed4fbe2d00ca620e01a0e0ee99a4871c939de36 * Fixes old-commit-hash: 3f052c658c16984a34915f38afdfbfd0fb19a267 * Process replay config fixes old-commit-hash: 1c56690ee7ceb3c23c9ec2b2713352191212716e * static analysis fixes old-commit-hash: 6145e2c140ea9aa97e75069c3ddd82172cadc866 * lp in latcontrol old-commit-hash: 9abf7359d68e794c69052724f3aca14b04dd3cca * Fix SensorEvent name for acceleration old-commit-hash: 91a1ad6c604727c9c898ba4aefe9478022b167fd * Ignore sensor readings from segments with multiple imus old-commit-hash: 1f05af63d6cc605ea98d7da0d727a5bd8d7819b0 * Update shebang old-commit-hash: e3f2f5c10df3a4ba698421335bfeffc63d1a8797 * Replace llk with lp old-commit-hash: 99b6c7ba08de6b703708fef0b8fd2d8cb24b92c0 * Refactor locationd scenario test old-commit-hash: 7f5f788f071b7647e36f854df927cc5b6f819a84 * Add more debugging tools old-commit-hash: 8d4e364867e143ea35f4bfd00d8212aaf170a1d1 * Param name update old-commit-hash: 5151e8f5520f067e7808e3f0baa628fbf8fb7337 * Fix expected observations old-commit-hash: d6a0d4c1a96c438fb6893e8b6ff43becf6061b75 * Handle invalid measurements old-commit-hash: 549362571e74ad1e7ec9368f6026378c54a29adf * Fix spelling old-commit-hash: eefd7c4c92fb486452e9b83c7121d2599811852b * Include observations in debug info too old-commit-hash: 625806d1110b3bffe249cd1d03416f2a3f2c1868 * Store error instead of expected observation old-commit-hash: 1cb7a799b67e56af4eddc6608d5b0e295f2d888c * Renames old-commit-hash: a7eec74640fc5cc7a5e86172d2087b66cb93d17d * Zero the yaw old-commit-hash: 96150779590fcb8ac50e8ffe8f8df03105983179 * New state_dot for orientation old-commit-hash: c1456bf3a0c5af2f888aa6ff0b5ffc2e5516ddf7 * Fix the state transformations old-commit-hash: 7cb9b91e2f99caa4ac3fb748c7f23bb8bf9f65db * Update process in test_onroad old-commit-hash: 854afab7c39ca7dec2b42974cecbb5310b82b617 * Test polling on cameraOdometry old-commit-hash: a78e8b7d61618177f15c9892e2fa1e51620deca8 * Keep the copy of x and P returned from predict old-commit-hash: 3c4159a6a7d7383265a99f3f78f8805d2fcfc8cd * Remove polling again old-commit-hash: f7228675c5fd2de5f879c4786859f1abcec27d68 * Remove locationd.cc old-commit-hash: d7005599b2b178e688c3bd1959d6b69357d3a663 * Optim in _finite_check old-commit-hash: 58ad6a06b9380960e9f69eb98663ddb97461e8d5 * Access .t once old-commit-hash: d03252e75ed4cbdb49291a60c904568e6a3d3399 * Move the timing check to cam odo code path old-commit-hash: 6a1c26f8c201e1feb601753f0cb7299cf981b47e * Call all_checks only once old-commit-hash: 373809cebf8d9db89d1ab00f4c8c933f33038e78 * Do not sort old-commit-hash: 2984cd02c0ab76827b8c7e32f7e637b261425025 * Check sm.updated old-commit-hash: 11c48de3f0802eb4783899f6a37737078dbf2da4 * Remove test_params_gps old-commit-hash: 82db4fd1a876cc2402702edc74eba0a8ac5da858 * Increase tolerance old-commit-hash: 927d6f05249d2c8ec40b32e2a0dcde8e1a469fb3 * Fix static old-commit-hash: 2d86d62c74d5ac0ad56ec3855a126e00a26cd490 * Try separate sockets for sensors old-commit-hash: 5dade63947ab237f0b4555f45d941a8851449ab1 * sensor_all_checks old-commit-hash: e25f40dd6b37ee76cd9cc2b19be552baf1355ec3 * Fix static old-commit-hash: 328cf1ad86079746b4f3fde55539e4acb92d285e * Set the cpu limit to 25 old-commit-hash: 7ba696ff54c5d3bfa42e42624d124f2a1914a96d * Make it prettier old-commit-hash: cd6270dec80d8b9dac784ddd4767a1a46bcff4b7 * Prettier old-commit-hash: 1b17931d23d37f299dad54139eaf283a89592bf5 * Increase the cpu budget to 260 old-commit-hash: 20173afb937a2609c8a9905aee0b2b093cb8bba4 * Change trans std mult. 2 works better * Update ref commit * Update ref commit
8 months ago
def init_xyz_measurement(measurement: capnp._DynamicStructBuilder, values: np.ndarray, stds: np.ndarray, valid: bool):
assert len(values) == len(stds) == 3
locationd: replaced the loop and setattr with direct attribute assignment (#33466) replaced the loop and setattr with direct attribute assignment
7 months ago
measurement.x, measurement.y, measurement.z = map(float, values)
measurement.xStd, measurement.yStd, measurement.zStd = map(float, stds)
locationd no GPS (#33029) * Pose kf draft old-commit-hash: 17dd4d576e597792f0e18826498c00076739f92b * Fix it old-commit-hash: 13ac120affe58fd22e871586ea5f4d335b3e9d2b * Add translation noise old-commit-hash: 166529cb612858c4ce80649367ac35b2b6007e1d * Add gravity to acc old-commit-hash: 8fcfed544b8e090ccc86b189c13bc03c6c190613 * Use pyx old-commit-hash: 8e69e0baa0a4c43b4d0c22535711f296f05420aa * Indent old-commit-hash: 25b19a73644cdcb571ccf1a1d8acb88a0d066c67 * Reset function old-commit-hash: ca5d2736da15e4fd6539f7268133320735b7c9cc * Add device_from_ned and ned_from_device transformations old-commit-hash: a60d25da0edc311e583549dc015fa595749fd4ae * Fix rotations old-commit-hash: d6d582f7f6d19a2bc2308dbcb0c9f81363e325b6 * kms old-commit-hash: 681bc4e50374795ccc61422c3ce4ffb51389fce2 * Centripetal acceleration old-commit-hash: 6e574506d27e5b76a04b2097d94efa4ca91ead71 * Rewrite draft old-commit-hash: 4a2aad0146267460e5d30036c8cdb2bef94d1d7c * Remove old locationd stuff old-commit-hash: c2be9f7dbf22fb5cd29e437cd7891a7d52266fba * Python process now old-commit-hash: 83fac85f28c0b546b6965aafe1dd8a089e67f5b3 * Process replay fix old-commit-hash: c44f9de98583c49dad0b22497869b3bb0266fcd9 * Add checks for timing and validity old-commit-hash: aed4fbe2d00ca620e01a0e0ee99a4871c939de36 * Fixes old-commit-hash: 3f052c658c16984a34915f38afdfbfd0fb19a267 * Process replay config fixes old-commit-hash: 1c56690ee7ceb3c23c9ec2b2713352191212716e * static analysis fixes old-commit-hash: 6145e2c140ea9aa97e75069c3ddd82172cadc866 * lp in latcontrol old-commit-hash: 9abf7359d68e794c69052724f3aca14b04dd3cca * Fix SensorEvent name for acceleration old-commit-hash: 91a1ad6c604727c9c898ba4aefe9478022b167fd * Ignore sensor readings from segments with multiple imus old-commit-hash: 1f05af63d6cc605ea98d7da0d727a5bd8d7819b0 * Update shebang old-commit-hash: e3f2f5c10df3a4ba698421335bfeffc63d1a8797 * Replace llk with lp old-commit-hash: 99b6c7ba08de6b703708fef0b8fd2d8cb24b92c0 * Refactor locationd scenario test old-commit-hash: 7f5f788f071b7647e36f854df927cc5b6f819a84 * Add more debugging tools old-commit-hash: 8d4e364867e143ea35f4bfd00d8212aaf170a1d1 * Param name update old-commit-hash: 5151e8f5520f067e7808e3f0baa628fbf8fb7337 * Fix expected observations old-commit-hash: d6a0d4c1a96c438fb6893e8b6ff43becf6061b75 * Handle invalid measurements old-commit-hash: 549362571e74ad1e7ec9368f6026378c54a29adf * Fix spelling old-commit-hash: eefd7c4c92fb486452e9b83c7121d2599811852b * Include observations in debug info too old-commit-hash: 625806d1110b3bffe249cd1d03416f2a3f2c1868 * Store error instead of expected observation old-commit-hash: 1cb7a799b67e56af4eddc6608d5b0e295f2d888c * Renames old-commit-hash: a7eec74640fc5cc7a5e86172d2087b66cb93d17d * Zero the yaw old-commit-hash: 96150779590fcb8ac50e8ffe8f8df03105983179 * New state_dot for orientation old-commit-hash: c1456bf3a0c5af2f888aa6ff0b5ffc2e5516ddf7 * Fix the state transformations old-commit-hash: 7cb9b91e2f99caa4ac3fb748c7f23bb8bf9f65db * Update process in test_onroad old-commit-hash: 854afab7c39ca7dec2b42974cecbb5310b82b617 * Test polling on cameraOdometry old-commit-hash: a78e8b7d61618177f15c9892e2fa1e51620deca8 * Keep the copy of x and P returned from predict old-commit-hash: 3c4159a6a7d7383265a99f3f78f8805d2fcfc8cd * Remove polling again old-commit-hash: f7228675c5fd2de5f879c4786859f1abcec27d68 * Remove locationd.cc old-commit-hash: d7005599b2b178e688c3bd1959d6b69357d3a663 * Optim in _finite_check old-commit-hash: 58ad6a06b9380960e9f69eb98663ddb97461e8d5 * Access .t once old-commit-hash: d03252e75ed4cbdb49291a60c904568e6a3d3399 * Move the timing check to cam odo code path old-commit-hash: 6a1c26f8c201e1feb601753f0cb7299cf981b47e * Call all_checks only once old-commit-hash: 373809cebf8d9db89d1ab00f4c8c933f33038e78 * Do not sort old-commit-hash: 2984cd02c0ab76827b8c7e32f7e637b261425025 * Check sm.updated old-commit-hash: 11c48de3f0802eb4783899f6a37737078dbf2da4 * Remove test_params_gps old-commit-hash: 82db4fd1a876cc2402702edc74eba0a8ac5da858 * Increase tolerance old-commit-hash: 927d6f05249d2c8ec40b32e2a0dcde8e1a469fb3 * Fix static old-commit-hash: 2d86d62c74d5ac0ad56ec3855a126e00a26cd490 * Try separate sockets for sensors old-commit-hash: 5dade63947ab237f0b4555f45d941a8851449ab1 * sensor_all_checks old-commit-hash: e25f40dd6b37ee76cd9cc2b19be552baf1355ec3 * Fix static old-commit-hash: 328cf1ad86079746b4f3fde55539e4acb92d285e * Set the cpu limit to 25 old-commit-hash: 7ba696ff54c5d3bfa42e42624d124f2a1914a96d * Make it prettier old-commit-hash: cd6270dec80d8b9dac784ddd4767a1a46bcff4b7 * Prettier old-commit-hash: 1b17931d23d37f299dad54139eaf283a89592bf5 * Increase the cpu budget to 260 old-commit-hash: 20173afb937a2609c8a9905aee0b2b093cb8bba4 * Change trans std mult. 2 works better * Update ref commit * Update ref commit
8 months ago
measurement.valid = valid
class HandleLogResult(Enum):
SUCCESS = 0
TIMING_INVALID = 1
INPUT_INVALID = 2
SENSOR_SOURCE_INVALID = 3
class LocationEstimator:
def __init__(self, debug: bool):
self.kf = PoseKalman(GENERATED_DIR, MAX_FILTER_REWIND_TIME)
self.debug = debug
locationd: improve circular buffer handling with np.roll (#33470) improve circular buffer handling
7 months ago
self.posenet_stds = np.array([POSENET_STD_INITIAL_VALUE] * (POSENET_STD_HIST_HALF * 2))
locationd no GPS (#33029) * Pose kf draft old-commit-hash: 17dd4d576e597792f0e18826498c00076739f92b * Fix it old-commit-hash: 13ac120affe58fd22e871586ea5f4d335b3e9d2b * Add translation noise old-commit-hash: 166529cb612858c4ce80649367ac35b2b6007e1d * Add gravity to acc old-commit-hash: 8fcfed544b8e090ccc86b189c13bc03c6c190613 * Use pyx old-commit-hash: 8e69e0baa0a4c43b4d0c22535711f296f05420aa * Indent old-commit-hash: 25b19a73644cdcb571ccf1a1d8acb88a0d066c67 * Reset function old-commit-hash: ca5d2736da15e4fd6539f7268133320735b7c9cc * Add device_from_ned and ned_from_device transformations old-commit-hash: a60d25da0edc311e583549dc015fa595749fd4ae * Fix rotations old-commit-hash: d6d582f7f6d19a2bc2308dbcb0c9f81363e325b6 * kms old-commit-hash: 681bc4e50374795ccc61422c3ce4ffb51389fce2 * Centripetal acceleration old-commit-hash: 6e574506d27e5b76a04b2097d94efa4ca91ead71 * Rewrite draft old-commit-hash: 4a2aad0146267460e5d30036c8cdb2bef94d1d7c * Remove old locationd stuff old-commit-hash: c2be9f7dbf22fb5cd29e437cd7891a7d52266fba * Python process now old-commit-hash: 83fac85f28c0b546b6965aafe1dd8a089e67f5b3 * Process replay fix old-commit-hash: c44f9de98583c49dad0b22497869b3bb0266fcd9 * Add checks for timing and validity old-commit-hash: aed4fbe2d00ca620e01a0e0ee99a4871c939de36 * Fixes old-commit-hash: 3f052c658c16984a34915f38afdfbfd0fb19a267 * Process replay config fixes old-commit-hash: 1c56690ee7ceb3c23c9ec2b2713352191212716e * static analysis fixes old-commit-hash: 6145e2c140ea9aa97e75069c3ddd82172cadc866 * lp in latcontrol old-commit-hash: 9abf7359d68e794c69052724f3aca14b04dd3cca * Fix SensorEvent name for acceleration old-commit-hash: 91a1ad6c604727c9c898ba4aefe9478022b167fd * Ignore sensor readings from segments with multiple imus old-commit-hash: 1f05af63d6cc605ea98d7da0d727a5bd8d7819b0 * Update shebang old-commit-hash: e3f2f5c10df3a4ba698421335bfeffc63d1a8797 * Replace llk with lp old-commit-hash: 99b6c7ba08de6b703708fef0b8fd2d8cb24b92c0 * Refactor locationd scenario test old-commit-hash: 7f5f788f071b7647e36f854df927cc5b6f819a84 * Add more debugging tools old-commit-hash: 8d4e364867e143ea35f4bfd00d8212aaf170a1d1 * Param name update old-commit-hash: 5151e8f5520f067e7808e3f0baa628fbf8fb7337 * Fix expected observations old-commit-hash: d6a0d4c1a96c438fb6893e8b6ff43becf6061b75 * Handle invalid measurements old-commit-hash: 549362571e74ad1e7ec9368f6026378c54a29adf * Fix spelling old-commit-hash: eefd7c4c92fb486452e9b83c7121d2599811852b * Include observations in debug info too old-commit-hash: 625806d1110b3bffe249cd1d03416f2a3f2c1868 * Store error instead of expected observation old-commit-hash: 1cb7a799b67e56af4eddc6608d5b0e295f2d888c * Renames old-commit-hash: a7eec74640fc5cc7a5e86172d2087b66cb93d17d * Zero the yaw old-commit-hash: 96150779590fcb8ac50e8ffe8f8df03105983179 * New state_dot for orientation old-commit-hash: c1456bf3a0c5af2f888aa6ff0b5ffc2e5516ddf7 * Fix the state transformations old-commit-hash: 7cb9b91e2f99caa4ac3fb748c7f23bb8bf9f65db * Update process in test_onroad old-commit-hash: 854afab7c39ca7dec2b42974cecbb5310b82b617 * Test polling on cameraOdometry old-commit-hash: a78e8b7d61618177f15c9892e2fa1e51620deca8 * Keep the copy of x and P returned from predict old-commit-hash: 3c4159a6a7d7383265a99f3f78f8805d2fcfc8cd * Remove polling again old-commit-hash: f7228675c5fd2de5f879c4786859f1abcec27d68 * Remove locationd.cc old-commit-hash: d7005599b2b178e688c3bd1959d6b69357d3a663 * Optim in _finite_check old-commit-hash: 58ad6a06b9380960e9f69eb98663ddb97461e8d5 * Access .t once old-commit-hash: d03252e75ed4cbdb49291a60c904568e6a3d3399 * Move the timing check to cam odo code path old-commit-hash: 6a1c26f8c201e1feb601753f0cb7299cf981b47e * Call all_checks only once old-commit-hash: 373809cebf8d9db89d1ab00f4c8c933f33038e78 * Do not sort old-commit-hash: 2984cd02c0ab76827b8c7e32f7e637b261425025 * Check sm.updated old-commit-hash: 11c48de3f0802eb4783899f6a37737078dbf2da4 * Remove test_params_gps old-commit-hash: 82db4fd1a876cc2402702edc74eba0a8ac5da858 * Increase tolerance old-commit-hash: 927d6f05249d2c8ec40b32e2a0dcde8e1a469fb3 * Fix static old-commit-hash: 2d86d62c74d5ac0ad56ec3855a126e00a26cd490 * Try separate sockets for sensors old-commit-hash: 5dade63947ab237f0b4555f45d941a8851449ab1 * sensor_all_checks old-commit-hash: e25f40dd6b37ee76cd9cc2b19be552baf1355ec3 * Fix static old-commit-hash: 328cf1ad86079746b4f3fde55539e4acb92d285e * Set the cpu limit to 25 old-commit-hash: 7ba696ff54c5d3bfa42e42624d124f2a1914a96d * Make it prettier old-commit-hash: cd6270dec80d8b9dac784ddd4767a1a46bcff4b7 * Prettier old-commit-hash: 1b17931d23d37f299dad54139eaf283a89592bf5 * Increase the cpu budget to 260 old-commit-hash: 20173afb937a2609c8a9905aee0b2b093cb8bba4 * Change trans std mult. 2 works better * Update ref commit * Update ref commit
8 months ago
self.car_speed = 0.0
self.camodo_yawrate_distribution = np.array([0.0, 10.0]) # mean, std
self.device_from_calib = np.eye(3)
obs_kinds = [ObservationKind.PHONE_ACCEL, ObservationKind.PHONE_GYRO, ObservationKind.CAMERA_ODO_ROTATION, ObservationKind.CAMERA_ODO_TRANSLATION]
self.observations = {kind: np.zeros(3, dtype=np.float32) for kind in obs_kinds}
self.observation_errors = {kind: np.zeros(3, dtype=np.float32) for kind in obs_kinds}
def reset(self, t: float, x_initial: np.ndarray = PoseKalman.initial_x, P_initial: np.ndarray = PoseKalman.initial_P):
self.kf.reset(t, x_initial, P_initial)
def _validate_sensor_source(self, source: log.SensorEventData.SensorSource):
# some segments have two IMUs, ignore the second one
return source != log.SensorEventData.SensorSource.bmx055
def _validate_sensor_time(self, sensor_time: float, t: float):
# ignore empty readings
if sensor_time == 0:
return False
# sensor time and log time should be close
sensor_time_invalid = abs(sensor_time - t) > MAX_SENSOR_TIME_DIFF
if sensor_time_invalid:
locationd: use cloudlog (#34508) * Use cloudlog instead of print * Changes
3 months ago
cloudlog.warning("Sensor reading ignored, sensor timestamp more than 100ms off from log time")
locationd no GPS (#33029) * Pose kf draft old-commit-hash: 17dd4d576e597792f0e18826498c00076739f92b * Fix it old-commit-hash: 13ac120affe58fd22e871586ea5f4d335b3e9d2b * Add translation noise old-commit-hash: 166529cb612858c4ce80649367ac35b2b6007e1d * Add gravity to acc old-commit-hash: 8fcfed544b8e090ccc86b189c13bc03c6c190613 * Use pyx old-commit-hash: 8e69e0baa0a4c43b4d0c22535711f296f05420aa * Indent old-commit-hash: 25b19a73644cdcb571ccf1a1d8acb88a0d066c67 * Reset function old-commit-hash: ca5d2736da15e4fd6539f7268133320735b7c9cc * Add device_from_ned and ned_from_device transformations old-commit-hash: a60d25da0edc311e583549dc015fa595749fd4ae * Fix rotations old-commit-hash: d6d582f7f6d19a2bc2308dbcb0c9f81363e325b6 * kms old-commit-hash: 681bc4e50374795ccc61422c3ce4ffb51389fce2 * Centripetal acceleration old-commit-hash: 6e574506d27e5b76a04b2097d94efa4ca91ead71 * Rewrite draft old-commit-hash: 4a2aad0146267460e5d30036c8cdb2bef94d1d7c * Remove old locationd stuff old-commit-hash: c2be9f7dbf22fb5cd29e437cd7891a7d52266fba * Python process now old-commit-hash: 83fac85f28c0b546b6965aafe1dd8a089e67f5b3 * Process replay fix old-commit-hash: c44f9de98583c49dad0b22497869b3bb0266fcd9 * Add checks for timing and validity old-commit-hash: aed4fbe2d00ca620e01a0e0ee99a4871c939de36 * Fixes old-commit-hash: 3f052c658c16984a34915f38afdfbfd0fb19a267 * Process replay config fixes old-commit-hash: 1c56690ee7ceb3c23c9ec2b2713352191212716e * static analysis fixes old-commit-hash: 6145e2c140ea9aa97e75069c3ddd82172cadc866 * lp in latcontrol old-commit-hash: 9abf7359d68e794c69052724f3aca14b04dd3cca * Fix SensorEvent name for acceleration old-commit-hash: 91a1ad6c604727c9c898ba4aefe9478022b167fd * Ignore sensor readings from segments with multiple imus old-commit-hash: 1f05af63d6cc605ea98d7da0d727a5bd8d7819b0 * Update shebang old-commit-hash: e3f2f5c10df3a4ba698421335bfeffc63d1a8797 * Replace llk with lp old-commit-hash: 99b6c7ba08de6b703708fef0b8fd2d8cb24b92c0 * Refactor locationd scenario test old-commit-hash: 7f5f788f071b7647e36f854df927cc5b6f819a84 * Add more debugging tools old-commit-hash: 8d4e364867e143ea35f4bfd00d8212aaf170a1d1 * Param name update old-commit-hash: 5151e8f5520f067e7808e3f0baa628fbf8fb7337 * Fix expected observations old-commit-hash: d6a0d4c1a96c438fb6893e8b6ff43becf6061b75 * Handle invalid measurements old-commit-hash: 549362571e74ad1e7ec9368f6026378c54a29adf * Fix spelling old-commit-hash: eefd7c4c92fb486452e9b83c7121d2599811852b * Include observations in debug info too old-commit-hash: 625806d1110b3bffe249cd1d03416f2a3f2c1868 * Store error instead of expected observation old-commit-hash: 1cb7a799b67e56af4eddc6608d5b0e295f2d888c * Renames old-commit-hash: a7eec74640fc5cc7a5e86172d2087b66cb93d17d * Zero the yaw old-commit-hash: 96150779590fcb8ac50e8ffe8f8df03105983179 * New state_dot for orientation old-commit-hash: c1456bf3a0c5af2f888aa6ff0b5ffc2e5516ddf7 * Fix the state transformations old-commit-hash: 7cb9b91e2f99caa4ac3fb748c7f23bb8bf9f65db * Update process in test_onroad old-commit-hash: 854afab7c39ca7dec2b42974cecbb5310b82b617 * Test polling on cameraOdometry old-commit-hash: a78e8b7d61618177f15c9892e2fa1e51620deca8 * Keep the copy of x and P returned from predict old-commit-hash: 3c4159a6a7d7383265a99f3f78f8805d2fcfc8cd * Remove polling again old-commit-hash: f7228675c5fd2de5f879c4786859f1abcec27d68 * Remove locationd.cc old-commit-hash: d7005599b2b178e688c3bd1959d6b69357d3a663 * Optim in _finite_check old-commit-hash: 58ad6a06b9380960e9f69eb98663ddb97461e8d5 * Access .t once old-commit-hash: d03252e75ed4cbdb49291a60c904568e6a3d3399 * Move the timing check to cam odo code path old-commit-hash: 6a1c26f8c201e1feb601753f0cb7299cf981b47e * Call all_checks only once old-commit-hash: 373809cebf8d9db89d1ab00f4c8c933f33038e78 * Do not sort old-commit-hash: 2984cd02c0ab76827b8c7e32f7e637b261425025 * Check sm.updated old-commit-hash: 11c48de3f0802eb4783899f6a37737078dbf2da4 * Remove test_params_gps old-commit-hash: 82db4fd1a876cc2402702edc74eba0a8ac5da858 * Increase tolerance old-commit-hash: 927d6f05249d2c8ec40b32e2a0dcde8e1a469fb3 * Fix static old-commit-hash: 2d86d62c74d5ac0ad56ec3855a126e00a26cd490 * Try separate sockets for sensors old-commit-hash: 5dade63947ab237f0b4555f45d941a8851449ab1 * sensor_all_checks old-commit-hash: e25f40dd6b37ee76cd9cc2b19be552baf1355ec3 * Fix static old-commit-hash: 328cf1ad86079746b4f3fde55539e4acb92d285e * Set the cpu limit to 25 old-commit-hash: 7ba696ff54c5d3bfa42e42624d124f2a1914a96d * Make it prettier old-commit-hash: cd6270dec80d8b9dac784ddd4767a1a46bcff4b7 * Prettier old-commit-hash: 1b17931d23d37f299dad54139eaf283a89592bf5 * Increase the cpu budget to 260 old-commit-hash: 20173afb937a2609c8a9905aee0b2b093cb8bba4 * Change trans std mult. 2 works better * Update ref commit * Update ref commit
8 months ago
return not sensor_time_invalid
def _validate_timestamp(self, t: float):
kf_t = self.kf.t
invalid = not np.isnan(kf_t) and (kf_t - t) > MAX_FILTER_REWIND_TIME
if invalid:
locationd: use cloudlog (#34508) * Use cloudlog instead of print * Changes
3 months ago
cloudlog.warning("Observation timestamp is older than the max rewind threshold of the filter")
locationd no GPS (#33029) * Pose kf draft old-commit-hash: 17dd4d576e597792f0e18826498c00076739f92b * Fix it old-commit-hash: 13ac120affe58fd22e871586ea5f4d335b3e9d2b * Add translation noise old-commit-hash: 166529cb612858c4ce80649367ac35b2b6007e1d * Add gravity to acc old-commit-hash: 8fcfed544b8e090ccc86b189c13bc03c6c190613 * Use pyx old-commit-hash: 8e69e0baa0a4c43b4d0c22535711f296f05420aa * Indent old-commit-hash: 25b19a73644cdcb571ccf1a1d8acb88a0d066c67 * Reset function old-commit-hash: ca5d2736da15e4fd6539f7268133320735b7c9cc * Add device_from_ned and ned_from_device transformations old-commit-hash: a60d25da0edc311e583549dc015fa595749fd4ae * Fix rotations old-commit-hash: d6d582f7f6d19a2bc2308dbcb0c9f81363e325b6 * kms old-commit-hash: 681bc4e50374795ccc61422c3ce4ffb51389fce2 * Centripetal acceleration old-commit-hash: 6e574506d27e5b76a04b2097d94efa4ca91ead71 * Rewrite draft old-commit-hash: 4a2aad0146267460e5d30036c8cdb2bef94d1d7c * Remove old locationd stuff old-commit-hash: c2be9f7dbf22fb5cd29e437cd7891a7d52266fba * Python process now old-commit-hash: 83fac85f28c0b546b6965aafe1dd8a089e67f5b3 * Process replay fix old-commit-hash: c44f9de98583c49dad0b22497869b3bb0266fcd9 * Add checks for timing and validity old-commit-hash: aed4fbe2d00ca620e01a0e0ee99a4871c939de36 * Fixes old-commit-hash: 3f052c658c16984a34915f38afdfbfd0fb19a267 * Process replay config fixes old-commit-hash: 1c56690ee7ceb3c23c9ec2b2713352191212716e * static analysis fixes old-commit-hash: 6145e2c140ea9aa97e75069c3ddd82172cadc866 * lp in latcontrol old-commit-hash: 9abf7359d68e794c69052724f3aca14b04dd3cca * Fix SensorEvent name for acceleration old-commit-hash: 91a1ad6c604727c9c898ba4aefe9478022b167fd * Ignore sensor readings from segments with multiple imus old-commit-hash: 1f05af63d6cc605ea98d7da0d727a5bd8d7819b0 * Update shebang old-commit-hash: e3f2f5c10df3a4ba698421335bfeffc63d1a8797 * Replace llk with lp old-commit-hash: 99b6c7ba08de6b703708fef0b8fd2d8cb24b92c0 * Refactor locationd scenario test old-commit-hash: 7f5f788f071b7647e36f854df927cc5b6f819a84 * Add more debugging tools old-commit-hash: 8d4e364867e143ea35f4bfd00d8212aaf170a1d1 * Param name update old-commit-hash: 5151e8f5520f067e7808e3f0baa628fbf8fb7337 * Fix expected observations old-commit-hash: d6a0d4c1a96c438fb6893e8b6ff43becf6061b75 * Handle invalid measurements old-commit-hash: 549362571e74ad1e7ec9368f6026378c54a29adf * Fix spelling old-commit-hash: eefd7c4c92fb486452e9b83c7121d2599811852b * Include observations in debug info too old-commit-hash: 625806d1110b3bffe249cd1d03416f2a3f2c1868 * Store error instead of expected observation old-commit-hash: 1cb7a799b67e56af4eddc6608d5b0e295f2d888c * Renames old-commit-hash: a7eec74640fc5cc7a5e86172d2087b66cb93d17d * Zero the yaw old-commit-hash: 96150779590fcb8ac50e8ffe8f8df03105983179 * New state_dot for orientation old-commit-hash: c1456bf3a0c5af2f888aa6ff0b5ffc2e5516ddf7 * Fix the state transformations old-commit-hash: 7cb9b91e2f99caa4ac3fb748c7f23bb8bf9f65db * Update process in test_onroad old-commit-hash: 854afab7c39ca7dec2b42974cecbb5310b82b617 * Test polling on cameraOdometry old-commit-hash: a78e8b7d61618177f15c9892e2fa1e51620deca8 * Keep the copy of x and P returned from predict old-commit-hash: 3c4159a6a7d7383265a99f3f78f8805d2fcfc8cd * Remove polling again old-commit-hash: f7228675c5fd2de5f879c4786859f1abcec27d68 * Remove locationd.cc old-commit-hash: d7005599b2b178e688c3bd1959d6b69357d3a663 * Optim in _finite_check old-commit-hash: 58ad6a06b9380960e9f69eb98663ddb97461e8d5 * Access .t once old-commit-hash: d03252e75ed4cbdb49291a60c904568e6a3d3399 * Move the timing check to cam odo code path old-commit-hash: 6a1c26f8c201e1feb601753f0cb7299cf981b47e * Call all_checks only once old-commit-hash: 373809cebf8d9db89d1ab00f4c8c933f33038e78 * Do not sort old-commit-hash: 2984cd02c0ab76827b8c7e32f7e637b261425025 * Check sm.updated old-commit-hash: 11c48de3f0802eb4783899f6a37737078dbf2da4 * Remove test_params_gps old-commit-hash: 82db4fd1a876cc2402702edc74eba0a8ac5da858 * Increase tolerance old-commit-hash: 927d6f05249d2c8ec40b32e2a0dcde8e1a469fb3 * Fix static old-commit-hash: 2d86d62c74d5ac0ad56ec3855a126e00a26cd490 * Try separate sockets for sensors old-commit-hash: 5dade63947ab237f0b4555f45d941a8851449ab1 * sensor_all_checks old-commit-hash: e25f40dd6b37ee76cd9cc2b19be552baf1355ec3 * Fix static old-commit-hash: 328cf1ad86079746b4f3fde55539e4acb92d285e * Set the cpu limit to 25 old-commit-hash: 7ba696ff54c5d3bfa42e42624d124f2a1914a96d * Make it prettier old-commit-hash: cd6270dec80d8b9dac784ddd4767a1a46bcff4b7 * Prettier old-commit-hash: 1b17931d23d37f299dad54139eaf283a89592bf5 * Increase the cpu budget to 260 old-commit-hash: 20173afb937a2609c8a9905aee0b2b093cb8bba4 * Change trans std mult. 2 works better * Update ref commit * Update ref commit
8 months ago
return not invalid
def _finite_check(self, t: float, new_x: np.ndarray, new_P: np.ndarray):
all_finite = np.isfinite(new_x).all() and np.isfinite(new_P).all()
if not all_finite:
locationd: use cloudlog (#34508) * Use cloudlog instead of print * Changes
3 months ago
cloudlog.error("Non-finite values detected, kalman reset")
locationd no GPS (#33029) * Pose kf draft old-commit-hash: 17dd4d576e597792f0e18826498c00076739f92b * Fix it old-commit-hash: 13ac120affe58fd22e871586ea5f4d335b3e9d2b * Add translation noise old-commit-hash: 166529cb612858c4ce80649367ac35b2b6007e1d * Add gravity to acc old-commit-hash: 8fcfed544b8e090ccc86b189c13bc03c6c190613 * Use pyx old-commit-hash: 8e69e0baa0a4c43b4d0c22535711f296f05420aa * Indent old-commit-hash: 25b19a73644cdcb571ccf1a1d8acb88a0d066c67 * Reset function old-commit-hash: ca5d2736da15e4fd6539f7268133320735b7c9cc * Add device_from_ned and ned_from_device transformations old-commit-hash: a60d25da0edc311e583549dc015fa595749fd4ae * Fix rotations old-commit-hash: d6d582f7f6d19a2bc2308dbcb0c9f81363e325b6 * kms old-commit-hash: 681bc4e50374795ccc61422c3ce4ffb51389fce2 * Centripetal acceleration old-commit-hash: 6e574506d27e5b76a04b2097d94efa4ca91ead71 * Rewrite draft old-commit-hash: 4a2aad0146267460e5d30036c8cdb2bef94d1d7c * Remove old locationd stuff old-commit-hash: c2be9f7dbf22fb5cd29e437cd7891a7d52266fba * Python process now old-commit-hash: 83fac85f28c0b546b6965aafe1dd8a089e67f5b3 * Process replay fix old-commit-hash: c44f9de98583c49dad0b22497869b3bb0266fcd9 * Add checks for timing and validity old-commit-hash: aed4fbe2d00ca620e01a0e0ee99a4871c939de36 * Fixes old-commit-hash: 3f052c658c16984a34915f38afdfbfd0fb19a267 * Process replay config fixes old-commit-hash: 1c56690ee7ceb3c23c9ec2b2713352191212716e * static analysis fixes old-commit-hash: 6145e2c140ea9aa97e75069c3ddd82172cadc866 * lp in latcontrol old-commit-hash: 9abf7359d68e794c69052724f3aca14b04dd3cca * Fix SensorEvent name for acceleration old-commit-hash: 91a1ad6c604727c9c898ba4aefe9478022b167fd * Ignore sensor readings from segments with multiple imus old-commit-hash: 1f05af63d6cc605ea98d7da0d727a5bd8d7819b0 * Update shebang old-commit-hash: e3f2f5c10df3a4ba698421335bfeffc63d1a8797 * Replace llk with lp old-commit-hash: 99b6c7ba08de6b703708fef0b8fd2d8cb24b92c0 * Refactor locationd scenario test old-commit-hash: 7f5f788f071b7647e36f854df927cc5b6f819a84 * Add more debugging tools old-commit-hash: 8d4e364867e143ea35f4bfd00d8212aaf170a1d1 * Param name update old-commit-hash: 5151e8f5520f067e7808e3f0baa628fbf8fb7337 * Fix expected observations old-commit-hash: d6a0d4c1a96c438fb6893e8b6ff43becf6061b75 * Handle invalid measurements old-commit-hash: 549362571e74ad1e7ec9368f6026378c54a29adf * Fix spelling old-commit-hash: eefd7c4c92fb486452e9b83c7121d2599811852b * Include observations in debug info too old-commit-hash: 625806d1110b3bffe249cd1d03416f2a3f2c1868 * Store error instead of expected observation old-commit-hash: 1cb7a799b67e56af4eddc6608d5b0e295f2d888c * Renames old-commit-hash: a7eec74640fc5cc7a5e86172d2087b66cb93d17d * Zero the yaw old-commit-hash: 96150779590fcb8ac50e8ffe8f8df03105983179 * New state_dot for orientation old-commit-hash: c1456bf3a0c5af2f888aa6ff0b5ffc2e5516ddf7 * Fix the state transformations old-commit-hash: 7cb9b91e2f99caa4ac3fb748c7f23bb8bf9f65db * Update process in test_onroad old-commit-hash: 854afab7c39ca7dec2b42974cecbb5310b82b617 * Test polling on cameraOdometry old-commit-hash: a78e8b7d61618177f15c9892e2fa1e51620deca8 * Keep the copy of x and P returned from predict old-commit-hash: 3c4159a6a7d7383265a99f3f78f8805d2fcfc8cd * Remove polling again old-commit-hash: f7228675c5fd2de5f879c4786859f1abcec27d68 * Remove locationd.cc old-commit-hash: d7005599b2b178e688c3bd1959d6b69357d3a663 * Optim in _finite_check old-commit-hash: 58ad6a06b9380960e9f69eb98663ddb97461e8d5 * Access .t once old-commit-hash: d03252e75ed4cbdb49291a60c904568e6a3d3399 * Move the timing check to cam odo code path old-commit-hash: 6a1c26f8c201e1feb601753f0cb7299cf981b47e * Call all_checks only once old-commit-hash: 373809cebf8d9db89d1ab00f4c8c933f33038e78 * Do not sort old-commit-hash: 2984cd02c0ab76827b8c7e32f7e637b261425025 * Check sm.updated old-commit-hash: 11c48de3f0802eb4783899f6a37737078dbf2da4 * Remove test_params_gps old-commit-hash: 82db4fd1a876cc2402702edc74eba0a8ac5da858 * Increase tolerance old-commit-hash: 927d6f05249d2c8ec40b32e2a0dcde8e1a469fb3 * Fix static old-commit-hash: 2d86d62c74d5ac0ad56ec3855a126e00a26cd490 * Try separate sockets for sensors old-commit-hash: 5dade63947ab237f0b4555f45d941a8851449ab1 * sensor_all_checks old-commit-hash: e25f40dd6b37ee76cd9cc2b19be552baf1355ec3 * Fix static old-commit-hash: 328cf1ad86079746b4f3fde55539e4acb92d285e * Set the cpu limit to 25 old-commit-hash: 7ba696ff54c5d3bfa42e42624d124f2a1914a96d * Make it prettier old-commit-hash: cd6270dec80d8b9dac784ddd4767a1a46bcff4b7 * Prettier old-commit-hash: 1b17931d23d37f299dad54139eaf283a89592bf5 * Increase the cpu budget to 260 old-commit-hash: 20173afb937a2609c8a9905aee0b2b093cb8bba4 * Change trans std mult. 2 works better * Update ref commit * Update ref commit
8 months ago
self.reset(t)
def handle_log(self, t: float, which: str, msg: capnp._DynamicStructReader) -> HandleLogResult:
new_x, new_P = None, None
if which == "accelerometer" and msg.which() == "acceleration":
sensor_time = msg.timestamp * 1e-9
if not self._validate_sensor_time(sensor_time, t) or not self._validate_timestamp(sensor_time):
return HandleLogResult.TIMING_INVALID
if not self._validate_sensor_source(msg.source):
return HandleLogResult.SENSOR_SOURCE_INVALID
v = msg.acceleration.v
meas = np.array([-v[2], -v[1], -v[0]])
if np.linalg.norm(meas) >= ACCEL_SANITY_CHECK:
return HandleLogResult.INPUT_INVALID
acc_res = self.kf.predict_and_observe(sensor_time, ObservationKind.PHONE_ACCEL, meas)
if acc_res is not None:
_, new_x, _, new_P, _, _, (acc_err,), _, _ = acc_res
self.observation_errors[ObservationKind.PHONE_ACCEL] = np.array(acc_err)
self.observations[ObservationKind.PHONE_ACCEL] = meas
elif which == "gyroscope" and msg.which() == "gyroUncalibrated":
sensor_time = msg.timestamp * 1e-9
if not self._validate_sensor_time(sensor_time, t) or not self._validate_timestamp(sensor_time):
return HandleLogResult.TIMING_INVALID
if not self._validate_sensor_source(msg.source):
return HandleLogResult.SENSOR_SOURCE_INVALID
v = msg.gyroUncalibrated.v
meas = np.array([-v[2], -v[1], -v[0]])
gyro_bias = self.kf.x[States.GYRO_BIAS]
gyro_camodo_yawrate_err = np.abs((meas[2] - gyro_bias[2]) - self.camodo_yawrate_distribution[0])
gyro_camodo_yawrate_err_threshold = YAWRATE_CROSS_ERR_CHECK_FACTOR * self.camodo_yawrate_distribution[1]
gyro_valid = gyro_camodo_yawrate_err < gyro_camodo_yawrate_err_threshold
if np.linalg.norm(meas) >= ROTATION_SANITY_CHECK or not gyro_valid:
return HandleLogResult.INPUT_INVALID
gyro_res = self.kf.predict_and_observe(sensor_time, ObservationKind.PHONE_GYRO, meas)
if gyro_res is not None:
_, new_x, _, new_P, _, _, (gyro_err,), _, _ = gyro_res
self.observation_errors[ObservationKind.PHONE_GYRO] = np.array(gyro_err)
self.observations[ObservationKind.PHONE_GYRO] = meas
elif which == "carState":
self.car_speed = abs(msg.vEgo)
elif which == "liveCalibration":
if len(msg.rpyCalib) > 0:
calib = np.array(msg.rpyCalib)
if calib.min() < -CALIB_RPY_SANITY_CHECK or calib.max() > CALIB_RPY_SANITY_CHECK:
return HandleLogResult.INPUT_INVALID
self.device_from_calib = rot_from_euler(calib)
self.calibrated = msg.calStatus == log.LiveCalibrationData.Status.calibrated
elif which == "cameraOdometry":
if not self._validate_timestamp(t):
return HandleLogResult.TIMING_INVALID
rot_device = np.matmul(self.device_from_calib, np.array(msg.rot))
trans_device = np.matmul(self.device_from_calib, np.array(msg.trans))
if np.linalg.norm(rot_device) > ROTATION_SANITY_CHECK or np.linalg.norm(trans_device) > TRANS_SANITY_CHECK:
return HandleLogResult.INPUT_INVALID
rot_calib_std = np.array(msg.rotStd)
trans_calib_std = np.array(msg.transStd)
if rot_calib_std.min() <= MIN_STD_SANITY_CHECK or trans_calib_std.min() <= MIN_STD_SANITY_CHECK:
return HandleLogResult.INPUT_INVALID
if np.linalg.norm(rot_calib_std) > 10 * ROTATION_SANITY_CHECK or np.linalg.norm(trans_calib_std) > 10 * TRANS_SANITY_CHECK:
return HandleLogResult.INPUT_INVALID
locationd: improve circular buffer handling with np.roll (#33470) improve circular buffer handling
7 months ago
self.posenet_stds = np.roll(self.posenet_stds, -1)
self.posenet_stds[-1] = trans_calib_std[0]
locationd no GPS (#33029) * Pose kf draft old-commit-hash: 17dd4d576e597792f0e18826498c00076739f92b * Fix it old-commit-hash: 13ac120affe58fd22e871586ea5f4d335b3e9d2b * Add translation noise old-commit-hash: 166529cb612858c4ce80649367ac35b2b6007e1d * Add gravity to acc old-commit-hash: 8fcfed544b8e090ccc86b189c13bc03c6c190613 * Use pyx old-commit-hash: 8e69e0baa0a4c43b4d0c22535711f296f05420aa * Indent old-commit-hash: 25b19a73644cdcb571ccf1a1d8acb88a0d066c67 * Reset function old-commit-hash: ca5d2736da15e4fd6539f7268133320735b7c9cc * Add device_from_ned and ned_from_device transformations old-commit-hash: a60d25da0edc311e583549dc015fa595749fd4ae * Fix rotations old-commit-hash: d6d582f7f6d19a2bc2308dbcb0c9f81363e325b6 * kms old-commit-hash: 681bc4e50374795ccc61422c3ce4ffb51389fce2 * Centripetal acceleration old-commit-hash: 6e574506d27e5b76a04b2097d94efa4ca91ead71 * Rewrite draft old-commit-hash: 4a2aad0146267460e5d30036c8cdb2bef94d1d7c * Remove old locationd stuff old-commit-hash: c2be9f7dbf22fb5cd29e437cd7891a7d52266fba * Python process now old-commit-hash: 83fac85f28c0b546b6965aafe1dd8a089e67f5b3 * Process replay fix old-commit-hash: c44f9de98583c49dad0b22497869b3bb0266fcd9 * Add checks for timing and validity old-commit-hash: aed4fbe2d00ca620e01a0e0ee99a4871c939de36 * Fixes old-commit-hash: 3f052c658c16984a34915f38afdfbfd0fb19a267 * Process replay config fixes old-commit-hash: 1c56690ee7ceb3c23c9ec2b2713352191212716e * static analysis fixes old-commit-hash: 6145e2c140ea9aa97e75069c3ddd82172cadc866 * lp in latcontrol old-commit-hash: 9abf7359d68e794c69052724f3aca14b04dd3cca * Fix SensorEvent name for acceleration old-commit-hash: 91a1ad6c604727c9c898ba4aefe9478022b167fd * Ignore sensor readings from segments with multiple imus old-commit-hash: 1f05af63d6cc605ea98d7da0d727a5bd8d7819b0 * Update shebang old-commit-hash: e3f2f5c10df3a4ba698421335bfeffc63d1a8797 * Replace llk with lp old-commit-hash: 99b6c7ba08de6b703708fef0b8fd2d8cb24b92c0 * Refactor locationd scenario test old-commit-hash: 7f5f788f071b7647e36f854df927cc5b6f819a84 * Add more debugging tools old-commit-hash: 8d4e364867e143ea35f4bfd00d8212aaf170a1d1 * Param name update old-commit-hash: 5151e8f5520f067e7808e3f0baa628fbf8fb7337 * Fix expected observations old-commit-hash: d6a0d4c1a96c438fb6893e8b6ff43becf6061b75 * Handle invalid measurements old-commit-hash: 549362571e74ad1e7ec9368f6026378c54a29adf * Fix spelling old-commit-hash: eefd7c4c92fb486452e9b83c7121d2599811852b * Include observations in debug info too old-commit-hash: 625806d1110b3bffe249cd1d03416f2a3f2c1868 * Store error instead of expected observation old-commit-hash: 1cb7a799b67e56af4eddc6608d5b0e295f2d888c * Renames old-commit-hash: a7eec74640fc5cc7a5e86172d2087b66cb93d17d * Zero the yaw old-commit-hash: 96150779590fcb8ac50e8ffe8f8df03105983179 * New state_dot for orientation old-commit-hash: c1456bf3a0c5af2f888aa6ff0b5ffc2e5516ddf7 * Fix the state transformations old-commit-hash: 7cb9b91e2f99caa4ac3fb748c7f23bb8bf9f65db * Update process in test_onroad old-commit-hash: 854afab7c39ca7dec2b42974cecbb5310b82b617 * Test polling on cameraOdometry old-commit-hash: a78e8b7d61618177f15c9892e2fa1e51620deca8 * Keep the copy of x and P returned from predict old-commit-hash: 3c4159a6a7d7383265a99f3f78f8805d2fcfc8cd * Remove polling again old-commit-hash: f7228675c5fd2de5f879c4786859f1abcec27d68 * Remove locationd.cc old-commit-hash: d7005599b2b178e688c3bd1959d6b69357d3a663 * Optim in _finite_check old-commit-hash: 58ad6a06b9380960e9f69eb98663ddb97461e8d5 * Access .t once old-commit-hash: d03252e75ed4cbdb49291a60c904568e6a3d3399 * Move the timing check to cam odo code path old-commit-hash: 6a1c26f8c201e1feb601753f0cb7299cf981b47e * Call all_checks only once old-commit-hash: 373809cebf8d9db89d1ab00f4c8c933f33038e78 * Do not sort old-commit-hash: 2984cd02c0ab76827b8c7e32f7e637b261425025 * Check sm.updated old-commit-hash: 11c48de3f0802eb4783899f6a37737078dbf2da4 * Remove test_params_gps old-commit-hash: 82db4fd1a876cc2402702edc74eba0a8ac5da858 * Increase tolerance old-commit-hash: 927d6f05249d2c8ec40b32e2a0dcde8e1a469fb3 * Fix static old-commit-hash: 2d86d62c74d5ac0ad56ec3855a126e00a26cd490 * Try separate sockets for sensors old-commit-hash: 5dade63947ab237f0b4555f45d941a8851449ab1 * sensor_all_checks old-commit-hash: e25f40dd6b37ee76cd9cc2b19be552baf1355ec3 * Fix static old-commit-hash: 328cf1ad86079746b4f3fde55539e4acb92d285e * Set the cpu limit to 25 old-commit-hash: 7ba696ff54c5d3bfa42e42624d124f2a1914a96d * Make it prettier old-commit-hash: cd6270dec80d8b9dac784ddd4767a1a46bcff4b7 * Prettier old-commit-hash: 1b17931d23d37f299dad54139eaf283a89592bf5 * Increase the cpu budget to 260 old-commit-hash: 20173afb937a2609c8a9905aee0b2b093cb8bba4 * Change trans std mult. 2 works better * Update ref commit * Update ref commit
8 months ago
# Multiply by N to avoid to high certainty in kalman filter because of temporally correlated noise
rot_calib_std *= 10
trans_calib_std *= 2
rot_device_std = rotate_std(self.device_from_calib, rot_calib_std)
trans_device_std = rotate_std(self.device_from_calib, trans_calib_std)
rot_device_noise = rot_device_std ** 2
trans_device_noise = trans_device_std ** 2
cam_odo_rot_res = self.kf.predict_and_observe(t, ObservationKind.CAMERA_ODO_ROTATION, rot_device, rot_device_noise)
cam_odo_trans_res = self.kf.predict_and_observe(t, ObservationKind.CAMERA_ODO_TRANSLATION, trans_device, trans_device_noise)
self.camodo_yawrate_distribution = np.array([rot_device[2], rot_device_std[2]])
if cam_odo_rot_res is not None:
_, new_x, _, new_P, _, _, (cam_odo_rot_err,), _, _ = cam_odo_rot_res
self.observation_errors[ObservationKind.CAMERA_ODO_ROTATION] = np.array(cam_odo_rot_err)
self.observations[ObservationKind.CAMERA_ODO_ROTATION] = rot_device
if cam_odo_trans_res is not None:
_, new_x, _, new_P, _, _, (cam_odo_trans_err,), _, _ = cam_odo_trans_res
self.observation_errors[ObservationKind.CAMERA_ODO_TRANSLATION] = np.array(cam_odo_trans_err)
self.observations[ObservationKind.CAMERA_ODO_TRANSLATION] = trans_device
if new_x is not None and new_P is not None:
self._finite_check(t, new_x, new_P)
return HandleLogResult.SUCCESS
def get_msg(self, sensors_valid: bool, inputs_valid: bool, filter_valid: bool):
state, cov = self.kf.x, self.kf.P
std = np.sqrt(np.diag(cov))
orientation_ned, orientation_ned_std = state[States.NED_ORIENTATION], std[States.NED_ORIENTATION]
velocity_device, velocity_device_std = state[States.DEVICE_VELOCITY], std[States.DEVICE_VELOCITY]
angular_velocity_device, angular_velocity_device_std = state[States.ANGULAR_VELOCITY], std[States.ANGULAR_VELOCITY]
acceleration_device, acceleration_device_std = state[States.ACCELERATION], std[States.ACCELERATION]
msg = messaging.new_message("livePose")
msg.valid = filter_valid
livePose = msg.livePose
init_xyz_measurement(livePose.orientationNED, orientation_ned, orientation_ned_std, filter_valid)
init_xyz_measurement(livePose.velocityDevice, velocity_device, velocity_device_std, filter_valid)
init_xyz_measurement(livePose.angularVelocityDevice, angular_velocity_device, angular_velocity_device_std, filter_valid)
init_xyz_measurement(livePose.accelerationDevice, acceleration_device, acceleration_device_std, filter_valid)
if self.debug:
livePose.debugFilterState.value = state.tolist()
livePose.debugFilterState.std = std.tolist()
livePose.debugFilterState.valid = filter_valid
livePose.debugFilterState.observations = [
{'kind': k, 'value': self.observations[k].tolist(), 'error': self.observation_errors[k].tolist()}
for k in self.observations.keys()
]
old_mean = np.mean(self.posenet_stds[:POSENET_STD_HIST_HALF])
new_mean = np.mean(self.posenet_stds[POSENET_STD_HIST_HALF:])
std_spike = (new_mean / old_mean) > 4.0 and new_mean > 7.0
livePose.inputsOK = inputs_valid
livePose.posenetOK = not std_spike or self.car_speed <= 5.0
livePose.sensorsOK = sensors_valid
return msg
def sensor_all_checks(acc_msgs, gyro_msgs, sensor_valid, sensor_recv_time, sensor_alive, simulation):
cur_time = time.monotonic()
for which, msgs in [("accelerometer", acc_msgs), ("gyroscope", gyro_msgs)]:
if len(msgs) > 0:
sensor_valid[which] = msgs[-1].valid
sensor_recv_time[which] = cur_time
if not simulation:
sensor_alive[which] = (cur_time - sensor_recv_time[which]) < 0.1
else:
sensor_alive[which] = len(msgs) > 0
return all(sensor_alive.values()) and all(sensor_valid.values())
def main():
config_realtime_process([0, 1, 2, 3], 5)
DEBUG = bool(int(os.getenv("DEBUG", "0")))
SIMULATION = bool(int(os.getenv("SIMULATION", "0")))
pm = messaging.PubMaster(['livePose'])
sm = messaging.SubMaster(['carState', 'liveCalibration', 'cameraOdometry'], poll='cameraOdometry')
# separate sensor sockets for efficiency
sensor_sockets = [messaging.sub_sock(which, timeout=20) for which in ['accelerometer', 'gyroscope']]
sensor_alive, sensor_valid, sensor_recv_time = defaultdict(bool), defaultdict(bool), defaultdict(float)
params = Params()
estimator = LocationEstimator(DEBUG)
filter_initialized = False
locationd: timing spikes resiliance (#34080) * Locationd scenario for timing spike * Add test for consistent timing spike * Resiliance to bad timing * Test update * Refactor test * fix comment * Decay based on frequency * Fix * Update comment * Only for critical services * Fix tests
5 months ago
critcal_services = ["accelerometer", "gyroscope", "cameraOdometry"]
locationd no GPS (#33029) * Pose kf draft old-commit-hash: 17dd4d576e597792f0e18826498c00076739f92b * Fix it old-commit-hash: 13ac120affe58fd22e871586ea5f4d335b3e9d2b * Add translation noise old-commit-hash: 166529cb612858c4ce80649367ac35b2b6007e1d * Add gravity to acc old-commit-hash: 8fcfed544b8e090ccc86b189c13bc03c6c190613 * Use pyx old-commit-hash: 8e69e0baa0a4c43b4d0c22535711f296f05420aa * Indent old-commit-hash: 25b19a73644cdcb571ccf1a1d8acb88a0d066c67 * Reset function old-commit-hash: ca5d2736da15e4fd6539f7268133320735b7c9cc * Add device_from_ned and ned_from_device transformations old-commit-hash: a60d25da0edc311e583549dc015fa595749fd4ae * Fix rotations old-commit-hash: d6d582f7f6d19a2bc2308dbcb0c9f81363e325b6 * kms old-commit-hash: 681bc4e50374795ccc61422c3ce4ffb51389fce2 * Centripetal acceleration old-commit-hash: 6e574506d27e5b76a04b2097d94efa4ca91ead71 * Rewrite draft old-commit-hash: 4a2aad0146267460e5d30036c8cdb2bef94d1d7c * Remove old locationd stuff old-commit-hash: c2be9f7dbf22fb5cd29e437cd7891a7d52266fba * Python process now old-commit-hash: 83fac85f28c0b546b6965aafe1dd8a089e67f5b3 * Process replay fix old-commit-hash: c44f9de98583c49dad0b22497869b3bb0266fcd9 * Add checks for timing and validity old-commit-hash: aed4fbe2d00ca620e01a0e0ee99a4871c939de36 * Fixes old-commit-hash: 3f052c658c16984a34915f38afdfbfd0fb19a267 * Process replay config fixes old-commit-hash: 1c56690ee7ceb3c23c9ec2b2713352191212716e * static analysis fixes old-commit-hash: 6145e2c140ea9aa97e75069c3ddd82172cadc866 * lp in latcontrol old-commit-hash: 9abf7359d68e794c69052724f3aca14b04dd3cca * Fix SensorEvent name for acceleration old-commit-hash: 91a1ad6c604727c9c898ba4aefe9478022b167fd * Ignore sensor readings from segments with multiple imus old-commit-hash: 1f05af63d6cc605ea98d7da0d727a5bd8d7819b0 * Update shebang old-commit-hash: e3f2f5c10df3a4ba698421335bfeffc63d1a8797 * Replace llk with lp old-commit-hash: 99b6c7ba08de6b703708fef0b8fd2d8cb24b92c0 * Refactor locationd scenario test old-commit-hash: 7f5f788f071b7647e36f854df927cc5b6f819a84 * Add more debugging tools old-commit-hash: 8d4e364867e143ea35f4bfd00d8212aaf170a1d1 * Param name update old-commit-hash: 5151e8f5520f067e7808e3f0baa628fbf8fb7337 * Fix expected observations old-commit-hash: d6a0d4c1a96c438fb6893e8b6ff43becf6061b75 * Handle invalid measurements old-commit-hash: 549362571e74ad1e7ec9368f6026378c54a29adf * Fix spelling old-commit-hash: eefd7c4c92fb486452e9b83c7121d2599811852b * Include observations in debug info too old-commit-hash: 625806d1110b3bffe249cd1d03416f2a3f2c1868 * Store error instead of expected observation old-commit-hash: 1cb7a799b67e56af4eddc6608d5b0e295f2d888c * Renames old-commit-hash: a7eec74640fc5cc7a5e86172d2087b66cb93d17d * Zero the yaw old-commit-hash: 96150779590fcb8ac50e8ffe8f8df03105983179 * New state_dot for orientation old-commit-hash: c1456bf3a0c5af2f888aa6ff0b5ffc2e5516ddf7 * Fix the state transformations old-commit-hash: 7cb9b91e2f99caa4ac3fb748c7f23bb8bf9f65db * Update process in test_onroad old-commit-hash: 854afab7c39ca7dec2b42974cecbb5310b82b617 * Test polling on cameraOdometry old-commit-hash: a78e8b7d61618177f15c9892e2fa1e51620deca8 * Keep the copy of x and P returned from predict old-commit-hash: 3c4159a6a7d7383265a99f3f78f8805d2fcfc8cd * Remove polling again old-commit-hash: f7228675c5fd2de5f879c4786859f1abcec27d68 * Remove locationd.cc old-commit-hash: d7005599b2b178e688c3bd1959d6b69357d3a663 * Optim in _finite_check old-commit-hash: 58ad6a06b9380960e9f69eb98663ddb97461e8d5 * Access .t once old-commit-hash: d03252e75ed4cbdb49291a60c904568e6a3d3399 * Move the timing check to cam odo code path old-commit-hash: 6a1c26f8c201e1feb601753f0cb7299cf981b47e * Call all_checks only once old-commit-hash: 373809cebf8d9db89d1ab00f4c8c933f33038e78 * Do not sort old-commit-hash: 2984cd02c0ab76827b8c7e32f7e637b261425025 * Check sm.updated old-commit-hash: 11c48de3f0802eb4783899f6a37737078dbf2da4 * Remove test_params_gps old-commit-hash: 82db4fd1a876cc2402702edc74eba0a8ac5da858 * Increase tolerance old-commit-hash: 927d6f05249d2c8ec40b32e2a0dcde8e1a469fb3 * Fix static old-commit-hash: 2d86d62c74d5ac0ad56ec3855a126e00a26cd490 * Try separate sockets for sensors old-commit-hash: 5dade63947ab237f0b4555f45d941a8851449ab1 * sensor_all_checks old-commit-hash: e25f40dd6b37ee76cd9cc2b19be552baf1355ec3 * Fix static old-commit-hash: 328cf1ad86079746b4f3fde55539e4acb92d285e * Set the cpu limit to 25 old-commit-hash: 7ba696ff54c5d3bfa42e42624d124f2a1914a96d * Make it prettier old-commit-hash: cd6270dec80d8b9dac784ddd4767a1a46bcff4b7 * Prettier old-commit-hash: 1b17931d23d37f299dad54139eaf283a89592bf5 * Increase the cpu budget to 260 old-commit-hash: 20173afb937a2609c8a9905aee0b2b093cb8bba4 * Change trans std mult. 2 works better * Update ref commit * Update ref commit
8 months ago
observation_input_invalid = defaultdict(int)
locationd: frequency based bad observation resiliance and recovery (#34476) * Improve it * Fix static * Fix test_consistent_timing_spikes test * Fix tests * Comment * Remove crap
3 months ago
input_invalid_limit = {s: round(INPUT_INVALID_LIMIT * (SERVICE_LIST[s].frequency / 20.)) for s in critcal_services}
input_invalid_threshold = {s: input_invalid_limit[s] - 0.5 for s in critcal_services}
input_invalid_decay = {s: calculate_invalid_input_decay(input_invalid_limit[s], INPUT_INVALID_RECOVERY, SERVICE_LIST[s].frequency) for s in critcal_services}
locationd: timing spikes resiliance (#34080) * Locationd scenario for timing spike * Add test for consistent timing spike * Resiliance to bad timing * Test update * Refactor test * fix comment * Decay based on frequency * Fix * Update comment * Only for critical services * Fix tests
5 months ago
locationd no GPS (#33029) * Pose kf draft old-commit-hash: 17dd4d576e597792f0e18826498c00076739f92b * Fix it old-commit-hash: 13ac120affe58fd22e871586ea5f4d335b3e9d2b * Add translation noise old-commit-hash: 166529cb612858c4ce80649367ac35b2b6007e1d * Add gravity to acc old-commit-hash: 8fcfed544b8e090ccc86b189c13bc03c6c190613 * Use pyx old-commit-hash: 8e69e0baa0a4c43b4d0c22535711f296f05420aa * Indent old-commit-hash: 25b19a73644cdcb571ccf1a1d8acb88a0d066c67 * Reset function old-commit-hash: ca5d2736da15e4fd6539f7268133320735b7c9cc * Add device_from_ned and ned_from_device transformations old-commit-hash: a60d25da0edc311e583549dc015fa595749fd4ae * Fix rotations old-commit-hash: d6d582f7f6d19a2bc2308dbcb0c9f81363e325b6 * kms old-commit-hash: 681bc4e50374795ccc61422c3ce4ffb51389fce2 * Centripetal acceleration old-commit-hash: 6e574506d27e5b76a04b2097d94efa4ca91ead71 * Rewrite draft old-commit-hash: 4a2aad0146267460e5d30036c8cdb2bef94d1d7c * Remove old locationd stuff old-commit-hash: c2be9f7dbf22fb5cd29e437cd7891a7d52266fba * Python process now old-commit-hash: 83fac85f28c0b546b6965aafe1dd8a089e67f5b3 * Process replay fix old-commit-hash: c44f9de98583c49dad0b22497869b3bb0266fcd9 * Add checks for timing and validity old-commit-hash: aed4fbe2d00ca620e01a0e0ee99a4871c939de36 * Fixes old-commit-hash: 3f052c658c16984a34915f38afdfbfd0fb19a267 * Process replay config fixes old-commit-hash: 1c56690ee7ceb3c23c9ec2b2713352191212716e * static analysis fixes old-commit-hash: 6145e2c140ea9aa97e75069c3ddd82172cadc866 * lp in latcontrol old-commit-hash: 9abf7359d68e794c69052724f3aca14b04dd3cca * Fix SensorEvent name for acceleration old-commit-hash: 91a1ad6c604727c9c898ba4aefe9478022b167fd * Ignore sensor readings from segments with multiple imus old-commit-hash: 1f05af63d6cc605ea98d7da0d727a5bd8d7819b0 * Update shebang old-commit-hash: e3f2f5c10df3a4ba698421335bfeffc63d1a8797 * Replace llk with lp old-commit-hash: 99b6c7ba08de6b703708fef0b8fd2d8cb24b92c0 * Refactor locationd scenario test old-commit-hash: 7f5f788f071b7647e36f854df927cc5b6f819a84 * Add more debugging tools old-commit-hash: 8d4e364867e143ea35f4bfd00d8212aaf170a1d1 * Param name update old-commit-hash: 5151e8f5520f067e7808e3f0baa628fbf8fb7337 * Fix expected observations old-commit-hash: d6a0d4c1a96c438fb6893e8b6ff43becf6061b75 * Handle invalid measurements old-commit-hash: 549362571e74ad1e7ec9368f6026378c54a29adf * Fix spelling old-commit-hash: eefd7c4c92fb486452e9b83c7121d2599811852b * Include observations in debug info too old-commit-hash: 625806d1110b3bffe249cd1d03416f2a3f2c1868 * Store error instead of expected observation old-commit-hash: 1cb7a799b67e56af4eddc6608d5b0e295f2d888c * Renames old-commit-hash: a7eec74640fc5cc7a5e86172d2087b66cb93d17d * Zero the yaw old-commit-hash: 96150779590fcb8ac50e8ffe8f8df03105983179 * New state_dot for orientation old-commit-hash: c1456bf3a0c5af2f888aa6ff0b5ffc2e5516ddf7 * Fix the state transformations old-commit-hash: 7cb9b91e2f99caa4ac3fb748c7f23bb8bf9f65db * Update process in test_onroad old-commit-hash: 854afab7c39ca7dec2b42974cecbb5310b82b617 * Test polling on cameraOdometry old-commit-hash: a78e8b7d61618177f15c9892e2fa1e51620deca8 * Keep the copy of x and P returned from predict old-commit-hash: 3c4159a6a7d7383265a99f3f78f8805d2fcfc8cd * Remove polling again old-commit-hash: f7228675c5fd2de5f879c4786859f1abcec27d68 * Remove locationd.cc old-commit-hash: d7005599b2b178e688c3bd1959d6b69357d3a663 * Optim in _finite_check old-commit-hash: 58ad6a06b9380960e9f69eb98663ddb97461e8d5 * Access .t once old-commit-hash: d03252e75ed4cbdb49291a60c904568e6a3d3399 * Move the timing check to cam odo code path old-commit-hash: 6a1c26f8c201e1feb601753f0cb7299cf981b47e * Call all_checks only once old-commit-hash: 373809cebf8d9db89d1ab00f4c8c933f33038e78 * Do not sort old-commit-hash: 2984cd02c0ab76827b8c7e32f7e637b261425025 * Check sm.updated old-commit-hash: 11c48de3f0802eb4783899f6a37737078dbf2da4 * Remove test_params_gps old-commit-hash: 82db4fd1a876cc2402702edc74eba0a8ac5da858 * Increase tolerance old-commit-hash: 927d6f05249d2c8ec40b32e2a0dcde8e1a469fb3 * Fix static old-commit-hash: 2d86d62c74d5ac0ad56ec3855a126e00a26cd490 * Try separate sockets for sensors old-commit-hash: 5dade63947ab237f0b4555f45d941a8851449ab1 * sensor_all_checks old-commit-hash: e25f40dd6b37ee76cd9cc2b19be552baf1355ec3 * Fix static old-commit-hash: 328cf1ad86079746b4f3fde55539e4acb92d285e * Set the cpu limit to 25 old-commit-hash: 7ba696ff54c5d3bfa42e42624d124f2a1914a96d * Make it prettier old-commit-hash: cd6270dec80d8b9dac784ddd4767a1a46bcff4b7 * Prettier old-commit-hash: 1b17931d23d37f299dad54139eaf283a89592bf5 * Increase the cpu budget to 260 old-commit-hash: 20173afb937a2609c8a9905aee0b2b093cb8bba4 * Change trans std mult. 2 works better * Update ref commit * Update ref commit
8 months ago
initial_pose = params.get("LocationFilterInitialState")
if initial_pose is not None:
initial_pose = json.loads(initial_pose)
x_initial = np.array(initial_pose["x"], dtype=np.float64)
P_initial = np.diag(np.array(initial_pose["P"], dtype=np.float64))
estimator.reset(None, x_initial, P_initial)
while True:
sm.update()
acc_msgs, gyro_msgs = (messaging.drain_sock(sock) for sock in sensor_sockets)
if filter_initialized:
msgs = []
for msg in acc_msgs + gyro_msgs:
t, valid, which, data = msg.logMonoTime, msg.valid, msg.which(), getattr(msg, msg.which())
msgs.append((t, valid, which, data))
for which, updated in sm.updated.items():
if not updated:
continue
t, valid, data = sm.logMonoTime[which], sm.valid[which], sm[which]
msgs.append((t, valid, which, data))
for log_mono_time, valid, which, msg in sorted(msgs, key=lambda x: x[0]):
if valid:
t = log_mono_time * 1e-9
res = estimator.handle_log(t, which, msg)
locationd: timing spikes resiliance (#34080) * Locationd scenario for timing spike * Add test for consistent timing spike * Resiliance to bad timing * Test update * Refactor test * fix comment * Decay based on frequency * Fix * Update comment * Only for critical services * Fix tests
5 months ago
if which not in critcal_services:
continue
locationd no GPS (#33029) * Pose kf draft old-commit-hash: 17dd4d576e597792f0e18826498c00076739f92b * Fix it old-commit-hash: 13ac120affe58fd22e871586ea5f4d335b3e9d2b * Add translation noise old-commit-hash: 166529cb612858c4ce80649367ac35b2b6007e1d * Add gravity to acc old-commit-hash: 8fcfed544b8e090ccc86b189c13bc03c6c190613 * Use pyx old-commit-hash: 8e69e0baa0a4c43b4d0c22535711f296f05420aa * Indent old-commit-hash: 25b19a73644cdcb571ccf1a1d8acb88a0d066c67 * Reset function old-commit-hash: ca5d2736da15e4fd6539f7268133320735b7c9cc * Add device_from_ned and ned_from_device transformations old-commit-hash: a60d25da0edc311e583549dc015fa595749fd4ae * Fix rotations old-commit-hash: d6d582f7f6d19a2bc2308dbcb0c9f81363e325b6 * kms old-commit-hash: 681bc4e50374795ccc61422c3ce4ffb51389fce2 * Centripetal acceleration old-commit-hash: 6e574506d27e5b76a04b2097d94efa4ca91ead71 * Rewrite draft old-commit-hash: 4a2aad0146267460e5d30036c8cdb2bef94d1d7c * Remove old locationd stuff old-commit-hash: c2be9f7dbf22fb5cd29e437cd7891a7d52266fba * Python process now old-commit-hash: 83fac85f28c0b546b6965aafe1dd8a089e67f5b3 * Process replay fix old-commit-hash: c44f9de98583c49dad0b22497869b3bb0266fcd9 * Add checks for timing and validity old-commit-hash: aed4fbe2d00ca620e01a0e0ee99a4871c939de36 * Fixes old-commit-hash: 3f052c658c16984a34915f38afdfbfd0fb19a267 * Process replay config fixes old-commit-hash: 1c56690ee7ceb3c23c9ec2b2713352191212716e * static analysis fixes old-commit-hash: 6145e2c140ea9aa97e75069c3ddd82172cadc866 * lp in latcontrol old-commit-hash: 9abf7359d68e794c69052724f3aca14b04dd3cca * Fix SensorEvent name for acceleration old-commit-hash: 91a1ad6c604727c9c898ba4aefe9478022b167fd * Ignore sensor readings from segments with multiple imus old-commit-hash: 1f05af63d6cc605ea98d7da0d727a5bd8d7819b0 * Update shebang old-commit-hash: e3f2f5c10df3a4ba698421335bfeffc63d1a8797 * Replace llk with lp old-commit-hash: 99b6c7ba08de6b703708fef0b8fd2d8cb24b92c0 * Refactor locationd scenario test old-commit-hash: 7f5f788f071b7647e36f854df927cc5b6f819a84 * Add more debugging tools old-commit-hash: 8d4e364867e143ea35f4bfd00d8212aaf170a1d1 * Param name update old-commit-hash: 5151e8f5520f067e7808e3f0baa628fbf8fb7337 * Fix expected observations old-commit-hash: d6a0d4c1a96c438fb6893e8b6ff43becf6061b75 * Handle invalid measurements old-commit-hash: 549362571e74ad1e7ec9368f6026378c54a29adf * Fix spelling old-commit-hash: eefd7c4c92fb486452e9b83c7121d2599811852b * Include observations in debug info too old-commit-hash: 625806d1110b3bffe249cd1d03416f2a3f2c1868 * Store error instead of expected observation old-commit-hash: 1cb7a799b67e56af4eddc6608d5b0e295f2d888c * Renames old-commit-hash: a7eec74640fc5cc7a5e86172d2087b66cb93d17d * Zero the yaw old-commit-hash: 96150779590fcb8ac50e8ffe8f8df03105983179 * New state_dot for orientation old-commit-hash: c1456bf3a0c5af2f888aa6ff0b5ffc2e5516ddf7 * Fix the state transformations old-commit-hash: 7cb9b91e2f99caa4ac3fb748c7f23bb8bf9f65db * Update process in test_onroad old-commit-hash: 854afab7c39ca7dec2b42974cecbb5310b82b617 * Test polling on cameraOdometry old-commit-hash: a78e8b7d61618177f15c9892e2fa1e51620deca8 * Keep the copy of x and P returned from predict old-commit-hash: 3c4159a6a7d7383265a99f3f78f8805d2fcfc8cd * Remove polling again old-commit-hash: f7228675c5fd2de5f879c4786859f1abcec27d68 * Remove locationd.cc old-commit-hash: d7005599b2b178e688c3bd1959d6b69357d3a663 * Optim in _finite_check old-commit-hash: 58ad6a06b9380960e9f69eb98663ddb97461e8d5 * Access .t once old-commit-hash: d03252e75ed4cbdb49291a60c904568e6a3d3399 * Move the timing check to cam odo code path old-commit-hash: 6a1c26f8c201e1feb601753f0cb7299cf981b47e * Call all_checks only once old-commit-hash: 373809cebf8d9db89d1ab00f4c8c933f33038e78 * Do not sort old-commit-hash: 2984cd02c0ab76827b8c7e32f7e637b261425025 * Check sm.updated old-commit-hash: 11c48de3f0802eb4783899f6a37737078dbf2da4 * Remove test_params_gps old-commit-hash: 82db4fd1a876cc2402702edc74eba0a8ac5da858 * Increase tolerance old-commit-hash: 927d6f05249d2c8ec40b32e2a0dcde8e1a469fb3 * Fix static old-commit-hash: 2d86d62c74d5ac0ad56ec3855a126e00a26cd490 * Try separate sockets for sensors old-commit-hash: 5dade63947ab237f0b4555f45d941a8851449ab1 * sensor_all_checks old-commit-hash: e25f40dd6b37ee76cd9cc2b19be552baf1355ec3 * Fix static old-commit-hash: 328cf1ad86079746b4f3fde55539e4acb92d285e * Set the cpu limit to 25 old-commit-hash: 7ba696ff54c5d3bfa42e42624d124f2a1914a96d * Make it prettier old-commit-hash: cd6270dec80d8b9dac784ddd4767a1a46bcff4b7 * Prettier old-commit-hash: 1b17931d23d37f299dad54139eaf283a89592bf5 * Increase the cpu budget to 260 old-commit-hash: 20173afb937a2609c8a9905aee0b2b093cb8bba4 * Change trans std mult. 2 works better * Update ref commit * Update ref commit
8 months ago
if res == HandleLogResult.TIMING_INVALID:
locationd: use cloudlog (#34508) * Use cloudlog instead of print * Changes
3 months ago
cloudlog.warning(f"Observation {which} ignored due to failed timing check")
locationd: frequency based bad observation resiliance and recovery (#34476) * Improve it * Fix static * Fix test_consistent_timing_spikes test * Fix tests * Comment * Remove crap
3 months ago
observation_input_invalid[which] += 1
locationd no GPS (#33029) * Pose kf draft old-commit-hash: 17dd4d576e597792f0e18826498c00076739f92b * Fix it old-commit-hash: 13ac120affe58fd22e871586ea5f4d335b3e9d2b * Add translation noise old-commit-hash: 166529cb612858c4ce80649367ac35b2b6007e1d * Add gravity to acc old-commit-hash: 8fcfed544b8e090ccc86b189c13bc03c6c190613 * Use pyx old-commit-hash: 8e69e0baa0a4c43b4d0c22535711f296f05420aa * Indent old-commit-hash: 25b19a73644cdcb571ccf1a1d8acb88a0d066c67 * Reset function old-commit-hash: ca5d2736da15e4fd6539f7268133320735b7c9cc * Add device_from_ned and ned_from_device transformations old-commit-hash: a60d25da0edc311e583549dc015fa595749fd4ae * Fix rotations old-commit-hash: d6d582f7f6d19a2bc2308dbcb0c9f81363e325b6 * kms old-commit-hash: 681bc4e50374795ccc61422c3ce4ffb51389fce2 * Centripetal acceleration old-commit-hash: 6e574506d27e5b76a04b2097d94efa4ca91ead71 * Rewrite draft old-commit-hash: 4a2aad0146267460e5d30036c8cdb2bef94d1d7c * Remove old locationd stuff old-commit-hash: c2be9f7dbf22fb5cd29e437cd7891a7d52266fba * Python process now old-commit-hash: 83fac85f28c0b546b6965aafe1dd8a089e67f5b3 * Process replay fix old-commit-hash: c44f9de98583c49dad0b22497869b3bb0266fcd9 * Add checks for timing and validity old-commit-hash: aed4fbe2d00ca620e01a0e0ee99a4871c939de36 * Fixes old-commit-hash: 3f052c658c16984a34915f38afdfbfd0fb19a267 * Process replay config fixes old-commit-hash: 1c56690ee7ceb3c23c9ec2b2713352191212716e * static analysis fixes old-commit-hash: 6145e2c140ea9aa97e75069c3ddd82172cadc866 * lp in latcontrol old-commit-hash: 9abf7359d68e794c69052724f3aca14b04dd3cca * Fix SensorEvent name for acceleration old-commit-hash: 91a1ad6c604727c9c898ba4aefe9478022b167fd * Ignore sensor readings from segments with multiple imus old-commit-hash: 1f05af63d6cc605ea98d7da0d727a5bd8d7819b0 * Update shebang old-commit-hash: e3f2f5c10df3a4ba698421335bfeffc63d1a8797 * Replace llk with lp old-commit-hash: 99b6c7ba08de6b703708fef0b8fd2d8cb24b92c0 * Refactor locationd scenario test old-commit-hash: 7f5f788f071b7647e36f854df927cc5b6f819a84 * Add more debugging tools old-commit-hash: 8d4e364867e143ea35f4bfd00d8212aaf170a1d1 * Param name update old-commit-hash: 5151e8f5520f067e7808e3f0baa628fbf8fb7337 * Fix expected observations old-commit-hash: d6a0d4c1a96c438fb6893e8b6ff43becf6061b75 * Handle invalid measurements old-commit-hash: 549362571e74ad1e7ec9368f6026378c54a29adf * Fix spelling old-commit-hash: eefd7c4c92fb486452e9b83c7121d2599811852b * Include observations in debug info too old-commit-hash: 625806d1110b3bffe249cd1d03416f2a3f2c1868 * Store error instead of expected observation old-commit-hash: 1cb7a799b67e56af4eddc6608d5b0e295f2d888c * Renames old-commit-hash: a7eec74640fc5cc7a5e86172d2087b66cb93d17d * Zero the yaw old-commit-hash: 96150779590fcb8ac50e8ffe8f8df03105983179 * New state_dot for orientation old-commit-hash: c1456bf3a0c5af2f888aa6ff0b5ffc2e5516ddf7 * Fix the state transformations old-commit-hash: 7cb9b91e2f99caa4ac3fb748c7f23bb8bf9f65db * Update process in test_onroad old-commit-hash: 854afab7c39ca7dec2b42974cecbb5310b82b617 * Test polling on cameraOdometry old-commit-hash: a78e8b7d61618177f15c9892e2fa1e51620deca8 * Keep the copy of x and P returned from predict old-commit-hash: 3c4159a6a7d7383265a99f3f78f8805d2fcfc8cd * Remove polling again old-commit-hash: f7228675c5fd2de5f879c4786859f1abcec27d68 * Remove locationd.cc old-commit-hash: d7005599b2b178e688c3bd1959d6b69357d3a663 * Optim in _finite_check old-commit-hash: 58ad6a06b9380960e9f69eb98663ddb97461e8d5 * Access .t once old-commit-hash: d03252e75ed4cbdb49291a60c904568e6a3d3399 * Move the timing check to cam odo code path old-commit-hash: 6a1c26f8c201e1feb601753f0cb7299cf981b47e * Call all_checks only once old-commit-hash: 373809cebf8d9db89d1ab00f4c8c933f33038e78 * Do not sort old-commit-hash: 2984cd02c0ab76827b8c7e32f7e637b261425025 * Check sm.updated old-commit-hash: 11c48de3f0802eb4783899f6a37737078dbf2da4 * Remove test_params_gps old-commit-hash: 82db4fd1a876cc2402702edc74eba0a8ac5da858 * Increase tolerance old-commit-hash: 927d6f05249d2c8ec40b32e2a0dcde8e1a469fb3 * Fix static old-commit-hash: 2d86d62c74d5ac0ad56ec3855a126e00a26cd490 * Try separate sockets for sensors old-commit-hash: 5dade63947ab237f0b4555f45d941a8851449ab1 * sensor_all_checks old-commit-hash: e25f40dd6b37ee76cd9cc2b19be552baf1355ec3 * Fix static old-commit-hash: 328cf1ad86079746b4f3fde55539e4acb92d285e * Set the cpu limit to 25 old-commit-hash: 7ba696ff54c5d3bfa42e42624d124f2a1914a96d * Make it prettier old-commit-hash: cd6270dec80d8b9dac784ddd4767a1a46bcff4b7 * Prettier old-commit-hash: 1b17931d23d37f299dad54139eaf283a89592bf5 * Increase the cpu budget to 260 old-commit-hash: 20173afb937a2609c8a9905aee0b2b093cb8bba4 * Change trans std mult. 2 works better * Update ref commit * Update ref commit
8 months ago
elif res == HandleLogResult.INPUT_INVALID:
locationd: use cloudlog (#34508) * Use cloudlog instead of print * Changes
3 months ago
cloudlog.warning(f"Observation {which} ignored due to failed sanity check")
locationd no GPS (#33029) * Pose kf draft old-commit-hash: 17dd4d576e597792f0e18826498c00076739f92b * Fix it old-commit-hash: 13ac120affe58fd22e871586ea5f4d335b3e9d2b * Add translation noise old-commit-hash: 166529cb612858c4ce80649367ac35b2b6007e1d * Add gravity to acc old-commit-hash: 8fcfed544b8e090ccc86b189c13bc03c6c190613 * Use pyx old-commit-hash: 8e69e0baa0a4c43b4d0c22535711f296f05420aa * Indent old-commit-hash: 25b19a73644cdcb571ccf1a1d8acb88a0d066c67 * Reset function old-commit-hash: ca5d2736da15e4fd6539f7268133320735b7c9cc * Add device_from_ned and ned_from_device transformations old-commit-hash: a60d25da0edc311e583549dc015fa595749fd4ae * Fix rotations old-commit-hash: d6d582f7f6d19a2bc2308dbcb0c9f81363e325b6 * kms old-commit-hash: 681bc4e50374795ccc61422c3ce4ffb51389fce2 * Centripetal acceleration old-commit-hash: 6e574506d27e5b76a04b2097d94efa4ca91ead71 * Rewrite draft old-commit-hash: 4a2aad0146267460e5d30036c8cdb2bef94d1d7c * Remove old locationd stuff old-commit-hash: c2be9f7dbf22fb5cd29e437cd7891a7d52266fba * Python process now old-commit-hash: 83fac85f28c0b546b6965aafe1dd8a089e67f5b3 * Process replay fix old-commit-hash: c44f9de98583c49dad0b22497869b3bb0266fcd9 * Add checks for timing and validity old-commit-hash: aed4fbe2d00ca620e01a0e0ee99a4871c939de36 * Fixes old-commit-hash: 3f052c658c16984a34915f38afdfbfd0fb19a267 * Process replay config fixes old-commit-hash: 1c56690ee7ceb3c23c9ec2b2713352191212716e * static analysis fixes old-commit-hash: 6145e2c140ea9aa97e75069c3ddd82172cadc866 * lp in latcontrol old-commit-hash: 9abf7359d68e794c69052724f3aca14b04dd3cca * Fix SensorEvent name for acceleration old-commit-hash: 91a1ad6c604727c9c898ba4aefe9478022b167fd * Ignore sensor readings from segments with multiple imus old-commit-hash: 1f05af63d6cc605ea98d7da0d727a5bd8d7819b0 * Update shebang old-commit-hash: e3f2f5c10df3a4ba698421335bfeffc63d1a8797 * Replace llk with lp old-commit-hash: 99b6c7ba08de6b703708fef0b8fd2d8cb24b92c0 * Refactor locationd scenario test old-commit-hash: 7f5f788f071b7647e36f854df927cc5b6f819a84 * Add more debugging tools old-commit-hash: 8d4e364867e143ea35f4bfd00d8212aaf170a1d1 * Param name update old-commit-hash: 5151e8f5520f067e7808e3f0baa628fbf8fb7337 * Fix expected observations old-commit-hash: d6a0d4c1a96c438fb6893e8b6ff43becf6061b75 * Handle invalid measurements old-commit-hash: 549362571e74ad1e7ec9368f6026378c54a29adf * Fix spelling old-commit-hash: eefd7c4c92fb486452e9b83c7121d2599811852b * Include observations in debug info too old-commit-hash: 625806d1110b3bffe249cd1d03416f2a3f2c1868 * Store error instead of expected observation old-commit-hash: 1cb7a799b67e56af4eddc6608d5b0e295f2d888c * Renames old-commit-hash: a7eec74640fc5cc7a5e86172d2087b66cb93d17d * Zero the yaw old-commit-hash: 96150779590fcb8ac50e8ffe8f8df03105983179 * New state_dot for orientation old-commit-hash: c1456bf3a0c5af2f888aa6ff0b5ffc2e5516ddf7 * Fix the state transformations old-commit-hash: 7cb9b91e2f99caa4ac3fb748c7f23bb8bf9f65db * Update process in test_onroad old-commit-hash: 854afab7c39ca7dec2b42974cecbb5310b82b617 * Test polling on cameraOdometry old-commit-hash: a78e8b7d61618177f15c9892e2fa1e51620deca8 * Keep the copy of x and P returned from predict old-commit-hash: 3c4159a6a7d7383265a99f3f78f8805d2fcfc8cd * Remove polling again old-commit-hash: f7228675c5fd2de5f879c4786859f1abcec27d68 * Remove locationd.cc old-commit-hash: d7005599b2b178e688c3bd1959d6b69357d3a663 * Optim in _finite_check old-commit-hash: 58ad6a06b9380960e9f69eb98663ddb97461e8d5 * Access .t once old-commit-hash: d03252e75ed4cbdb49291a60c904568e6a3d3399 * Move the timing check to cam odo code path old-commit-hash: 6a1c26f8c201e1feb601753f0cb7299cf981b47e * Call all_checks only once old-commit-hash: 373809cebf8d9db89d1ab00f4c8c933f33038e78 * Do not sort old-commit-hash: 2984cd02c0ab76827b8c7e32f7e637b261425025 * Check sm.updated old-commit-hash: 11c48de3f0802eb4783899f6a37737078dbf2da4 * Remove test_params_gps old-commit-hash: 82db4fd1a876cc2402702edc74eba0a8ac5da858 * Increase tolerance old-commit-hash: 927d6f05249d2c8ec40b32e2a0dcde8e1a469fb3 * Fix static old-commit-hash: 2d86d62c74d5ac0ad56ec3855a126e00a26cd490 * Try separate sockets for sensors old-commit-hash: 5dade63947ab237f0b4555f45d941a8851449ab1 * sensor_all_checks old-commit-hash: e25f40dd6b37ee76cd9cc2b19be552baf1355ec3 * Fix static old-commit-hash: 328cf1ad86079746b4f3fde55539e4acb92d285e * Set the cpu limit to 25 old-commit-hash: 7ba696ff54c5d3bfa42e42624d124f2a1914a96d * Make it prettier old-commit-hash: cd6270dec80d8b9dac784ddd4767a1a46bcff4b7 * Prettier old-commit-hash: 1b17931d23d37f299dad54139eaf283a89592bf5 * Increase the cpu budget to 260 old-commit-hash: 20173afb937a2609c8a9905aee0b2b093cb8bba4 * Change trans std mult. 2 works better * Update ref commit * Update ref commit
8 months ago
observation_input_invalid[which] += 1
locationd: use cloudlog (#34508) * Use cloudlog instead of print * Changes
3 months ago
elif res == HandleLogResult.SUCCESS:
locationd: timing spikes resiliance (#34080) * Locationd scenario for timing spike * Add test for consistent timing spike * Resiliance to bad timing * Test update * Refactor test * fix comment * Decay based on frequency * Fix * Update comment * Only for critical services * Fix tests
5 months ago
observation_input_invalid[which] *= input_invalid_decay[which]
locationd no GPS (#33029) * Pose kf draft old-commit-hash: 17dd4d576e597792f0e18826498c00076739f92b * Fix it old-commit-hash: 13ac120affe58fd22e871586ea5f4d335b3e9d2b * Add translation noise old-commit-hash: 166529cb612858c4ce80649367ac35b2b6007e1d * Add gravity to acc old-commit-hash: 8fcfed544b8e090ccc86b189c13bc03c6c190613 * Use pyx old-commit-hash: 8e69e0baa0a4c43b4d0c22535711f296f05420aa * Indent old-commit-hash: 25b19a73644cdcb571ccf1a1d8acb88a0d066c67 * Reset function old-commit-hash: ca5d2736da15e4fd6539f7268133320735b7c9cc * Add device_from_ned and ned_from_device transformations old-commit-hash: a60d25da0edc311e583549dc015fa595749fd4ae * Fix rotations old-commit-hash: d6d582f7f6d19a2bc2308dbcb0c9f81363e325b6 * kms old-commit-hash: 681bc4e50374795ccc61422c3ce4ffb51389fce2 * Centripetal acceleration old-commit-hash: 6e574506d27e5b76a04b2097d94efa4ca91ead71 * Rewrite draft old-commit-hash: 4a2aad0146267460e5d30036c8cdb2bef94d1d7c * Remove old locationd stuff old-commit-hash: c2be9f7dbf22fb5cd29e437cd7891a7d52266fba * Python process now old-commit-hash: 83fac85f28c0b546b6965aafe1dd8a089e67f5b3 * Process replay fix old-commit-hash: c44f9de98583c49dad0b22497869b3bb0266fcd9 * Add checks for timing and validity old-commit-hash: aed4fbe2d00ca620e01a0e0ee99a4871c939de36 * Fixes old-commit-hash: 3f052c658c16984a34915f38afdfbfd0fb19a267 * Process replay config fixes old-commit-hash: 1c56690ee7ceb3c23c9ec2b2713352191212716e * static analysis fixes old-commit-hash: 6145e2c140ea9aa97e75069c3ddd82172cadc866 * lp in latcontrol old-commit-hash: 9abf7359d68e794c69052724f3aca14b04dd3cca * Fix SensorEvent name for acceleration old-commit-hash: 91a1ad6c604727c9c898ba4aefe9478022b167fd * Ignore sensor readings from segments with multiple imus old-commit-hash: 1f05af63d6cc605ea98d7da0d727a5bd8d7819b0 * Update shebang old-commit-hash: e3f2f5c10df3a4ba698421335bfeffc63d1a8797 * Replace llk with lp old-commit-hash: 99b6c7ba08de6b703708fef0b8fd2d8cb24b92c0 * Refactor locationd scenario test old-commit-hash: 7f5f788f071b7647e36f854df927cc5b6f819a84 * Add more debugging tools old-commit-hash: 8d4e364867e143ea35f4bfd00d8212aaf170a1d1 * Param name update old-commit-hash: 5151e8f5520f067e7808e3f0baa628fbf8fb7337 * Fix expected observations old-commit-hash: d6a0d4c1a96c438fb6893e8b6ff43becf6061b75 * Handle invalid measurements old-commit-hash: 549362571e74ad1e7ec9368f6026378c54a29adf * Fix spelling old-commit-hash: eefd7c4c92fb486452e9b83c7121d2599811852b * Include observations in debug info too old-commit-hash: 625806d1110b3bffe249cd1d03416f2a3f2c1868 * Store error instead of expected observation old-commit-hash: 1cb7a799b67e56af4eddc6608d5b0e295f2d888c * Renames old-commit-hash: a7eec74640fc5cc7a5e86172d2087b66cb93d17d * Zero the yaw old-commit-hash: 96150779590fcb8ac50e8ffe8f8df03105983179 * New state_dot for orientation old-commit-hash: c1456bf3a0c5af2f888aa6ff0b5ffc2e5516ddf7 * Fix the state transformations old-commit-hash: 7cb9b91e2f99caa4ac3fb748c7f23bb8bf9f65db * Update process in test_onroad old-commit-hash: 854afab7c39ca7dec2b42974cecbb5310b82b617 * Test polling on cameraOdometry old-commit-hash: a78e8b7d61618177f15c9892e2fa1e51620deca8 * Keep the copy of x and P returned from predict old-commit-hash: 3c4159a6a7d7383265a99f3f78f8805d2fcfc8cd * Remove polling again old-commit-hash: f7228675c5fd2de5f879c4786859f1abcec27d68 * Remove locationd.cc old-commit-hash: d7005599b2b178e688c3bd1959d6b69357d3a663 * Optim in _finite_check old-commit-hash: 58ad6a06b9380960e9f69eb98663ddb97461e8d5 * Access .t once old-commit-hash: d03252e75ed4cbdb49291a60c904568e6a3d3399 * Move the timing check to cam odo code path old-commit-hash: 6a1c26f8c201e1feb601753f0cb7299cf981b47e * Call all_checks only once old-commit-hash: 373809cebf8d9db89d1ab00f4c8c933f33038e78 * Do not sort old-commit-hash: 2984cd02c0ab76827b8c7e32f7e637b261425025 * Check sm.updated old-commit-hash: 11c48de3f0802eb4783899f6a37737078dbf2da4 * Remove test_params_gps old-commit-hash: 82db4fd1a876cc2402702edc74eba0a8ac5da858 * Increase tolerance old-commit-hash: 927d6f05249d2c8ec40b32e2a0dcde8e1a469fb3 * Fix static old-commit-hash: 2d86d62c74d5ac0ad56ec3855a126e00a26cd490 * Try separate sockets for sensors old-commit-hash: 5dade63947ab237f0b4555f45d941a8851449ab1 * sensor_all_checks old-commit-hash: e25f40dd6b37ee76cd9cc2b19be552baf1355ec3 * Fix static old-commit-hash: 328cf1ad86079746b4f3fde55539e4acb92d285e * Set the cpu limit to 25 old-commit-hash: 7ba696ff54c5d3bfa42e42624d124f2a1914a96d * Make it prettier old-commit-hash: cd6270dec80d8b9dac784ddd4767a1a46bcff4b7 * Prettier old-commit-hash: 1b17931d23d37f299dad54139eaf283a89592bf5 * Increase the cpu budget to 260 old-commit-hash: 20173afb937a2609c8a9905aee0b2b093cb8bba4 * Change trans std mult. 2 works better * Update ref commit * Update ref commit
8 months ago
else:
filter_initialized = sm.all_checks() and sensor_all_checks(acc_msgs, gyro_msgs, sensor_valid, sensor_recv_time, sensor_alive, SIMULATION)
if sm.updated["cameraOdometry"]:
locationd: frequency based bad observation resiliance and recovery (#34476) * Improve it * Fix static * Fix test_consistent_timing_spikes test * Fix tests * Comment * Remove crap
3 months ago
critical_service_inputs_valid = all(observation_input_invalid[s] < input_invalid_threshold[s] for s in critcal_services)
inputs_valid = sm.all_valid() and critical_service_inputs_valid
locationd no GPS (#33029) * Pose kf draft old-commit-hash: 17dd4d576e597792f0e18826498c00076739f92b * Fix it old-commit-hash: 13ac120affe58fd22e871586ea5f4d335b3e9d2b * Add translation noise old-commit-hash: 166529cb612858c4ce80649367ac35b2b6007e1d * Add gravity to acc old-commit-hash: 8fcfed544b8e090ccc86b189c13bc03c6c190613 * Use pyx old-commit-hash: 8e69e0baa0a4c43b4d0c22535711f296f05420aa * Indent old-commit-hash: 25b19a73644cdcb571ccf1a1d8acb88a0d066c67 * Reset function old-commit-hash: ca5d2736da15e4fd6539f7268133320735b7c9cc * Add device_from_ned and ned_from_device transformations old-commit-hash: a60d25da0edc311e583549dc015fa595749fd4ae * Fix rotations old-commit-hash: d6d582f7f6d19a2bc2308dbcb0c9f81363e325b6 * kms old-commit-hash: 681bc4e50374795ccc61422c3ce4ffb51389fce2 * Centripetal acceleration old-commit-hash: 6e574506d27e5b76a04b2097d94efa4ca91ead71 * Rewrite draft old-commit-hash: 4a2aad0146267460e5d30036c8cdb2bef94d1d7c * Remove old locationd stuff old-commit-hash: c2be9f7dbf22fb5cd29e437cd7891a7d52266fba * Python process now old-commit-hash: 83fac85f28c0b546b6965aafe1dd8a089e67f5b3 * Process replay fix old-commit-hash: c44f9de98583c49dad0b22497869b3bb0266fcd9 * Add checks for timing and validity old-commit-hash: aed4fbe2d00ca620e01a0e0ee99a4871c939de36 * Fixes old-commit-hash: 3f052c658c16984a34915f38afdfbfd0fb19a267 * Process replay config fixes old-commit-hash: 1c56690ee7ceb3c23c9ec2b2713352191212716e * static analysis fixes old-commit-hash: 6145e2c140ea9aa97e75069c3ddd82172cadc866 * lp in latcontrol old-commit-hash: 9abf7359d68e794c69052724f3aca14b04dd3cca * Fix SensorEvent name for acceleration old-commit-hash: 91a1ad6c604727c9c898ba4aefe9478022b167fd * Ignore sensor readings from segments with multiple imus old-commit-hash: 1f05af63d6cc605ea98d7da0d727a5bd8d7819b0 * Update shebang old-commit-hash: e3f2f5c10df3a4ba698421335bfeffc63d1a8797 * Replace llk with lp old-commit-hash: 99b6c7ba08de6b703708fef0b8fd2d8cb24b92c0 * Refactor locationd scenario test old-commit-hash: 7f5f788f071b7647e36f854df927cc5b6f819a84 * Add more debugging tools old-commit-hash: 8d4e364867e143ea35f4bfd00d8212aaf170a1d1 * Param name update old-commit-hash: 5151e8f5520f067e7808e3f0baa628fbf8fb7337 * Fix expected observations old-commit-hash: d6a0d4c1a96c438fb6893e8b6ff43becf6061b75 * Handle invalid measurements old-commit-hash: 549362571e74ad1e7ec9368f6026378c54a29adf * Fix spelling old-commit-hash: eefd7c4c92fb486452e9b83c7121d2599811852b * Include observations in debug info too old-commit-hash: 625806d1110b3bffe249cd1d03416f2a3f2c1868 * Store error instead of expected observation old-commit-hash: 1cb7a799b67e56af4eddc6608d5b0e295f2d888c * Renames old-commit-hash: a7eec74640fc5cc7a5e86172d2087b66cb93d17d * Zero the yaw old-commit-hash: 96150779590fcb8ac50e8ffe8f8df03105983179 * New state_dot for orientation old-commit-hash: c1456bf3a0c5af2f888aa6ff0b5ffc2e5516ddf7 * Fix the state transformations old-commit-hash: 7cb9b91e2f99caa4ac3fb748c7f23bb8bf9f65db * Update process in test_onroad old-commit-hash: 854afab7c39ca7dec2b42974cecbb5310b82b617 * Test polling on cameraOdometry old-commit-hash: a78e8b7d61618177f15c9892e2fa1e51620deca8 * Keep the copy of x and P returned from predict old-commit-hash: 3c4159a6a7d7383265a99f3f78f8805d2fcfc8cd * Remove polling again old-commit-hash: f7228675c5fd2de5f879c4786859f1abcec27d68 * Remove locationd.cc old-commit-hash: d7005599b2b178e688c3bd1959d6b69357d3a663 * Optim in _finite_check old-commit-hash: 58ad6a06b9380960e9f69eb98663ddb97461e8d5 * Access .t once old-commit-hash: d03252e75ed4cbdb49291a60c904568e6a3d3399 * Move the timing check to cam odo code path old-commit-hash: 6a1c26f8c201e1feb601753f0cb7299cf981b47e * Call all_checks only once old-commit-hash: 373809cebf8d9db89d1ab00f4c8c933f33038e78 * Do not sort old-commit-hash: 2984cd02c0ab76827b8c7e32f7e637b261425025 * Check sm.updated old-commit-hash: 11c48de3f0802eb4783899f6a37737078dbf2da4 * Remove test_params_gps old-commit-hash: 82db4fd1a876cc2402702edc74eba0a8ac5da858 * Increase tolerance old-commit-hash: 927d6f05249d2c8ec40b32e2a0dcde8e1a469fb3 * Fix static old-commit-hash: 2d86d62c74d5ac0ad56ec3855a126e00a26cd490 * Try separate sockets for sensors old-commit-hash: 5dade63947ab237f0b4555f45d941a8851449ab1 * sensor_all_checks old-commit-hash: e25f40dd6b37ee76cd9cc2b19be552baf1355ec3 * Fix static old-commit-hash: 328cf1ad86079746b4f3fde55539e4acb92d285e * Set the cpu limit to 25 old-commit-hash: 7ba696ff54c5d3bfa42e42624d124f2a1914a96d * Make it prettier old-commit-hash: cd6270dec80d8b9dac784ddd4767a1a46bcff4b7 * Prettier old-commit-hash: 1b17931d23d37f299dad54139eaf283a89592bf5 * Increase the cpu budget to 260 old-commit-hash: 20173afb937a2609c8a9905aee0b2b093cb8bba4 * Change trans std mult. 2 works better * Update ref commit * Update ref commit
8 months ago
sensors_valid = sensor_all_checks(acc_msgs, gyro_msgs, sensor_valid, sensor_recv_time, sensor_alive, SIMULATION)
msg = estimator.get_msg(sensors_valid, inputs_valid, filter_initialized)
pm.send("livePose", msg)
if __name__ == "__main__":
main()