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.
14925 Commits
123 Branches
82 Tags
5.0 GiB
Tag: Branch: Tree: 490c53e2dc
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from '490c53e2dc'
${ noResults }
openpilot_comma/selfdrive/locationd/models/pose_kf.py

112 lines
4.2 KiB
Raw 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 sys
import numpy as np
from openpilot.selfdrive.locationd.models.constants import ObservationKind
locationd: make pose_kf inherit from KalmanFilter (#34982) * Read message not json for initial state * Delete lines * Fix param
3 weeks ago
from rednose.helpers.kalmanfilter import KalmanFilter
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 __name__=="__main__":
import sympy as sp
from rednose.helpers.ekf_sym import gen_code
from rednose.helpers.sympy_helpers import euler_rotate, rot_to_euler
else:
from rednose.helpers.ekf_sym_pyx import EKF_sym_pyx
EARTH_G = 9.81
class States:
NED_ORIENTATION = slice(0, 3) # roll, pitch, yaw in rad
DEVICE_VELOCITY = slice(3, 6) # ned velocity in m/s
ANGULAR_VELOCITY = slice(6, 9) # roll, pitch and yaw rates in rad/s
GYRO_BIAS = slice(9, 12) # roll, pitch and yaw gyroscope biases in rad/s
ACCELERATION = slice(12, 15) # acceleration in device frame in m/s**2
ACCEL_BIAS = slice(15, 18) # Acceletometer bias in m/s**2
locationd: make pose_kf inherit from KalmanFilter (#34982) * Read message not json for initial state * Delete lines * Fix param
3 weeks ago
class PoseKalman(KalmanFilter):
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
name = "pose"
# state
initial_x = np.array([0.0, 0.0, 0.0,
0.0, 0.0, 0.0,
0.0, 0.0, 0.0,
0.0, 0.0, 0.0,
0.0, 0.0, 0.0,
0.0, 0.0, 0.0])
# state covariance
initial_P = np.diag([0.01**2, 0.01**2, 0.01**2,
10**2, 10**2, 10**2,
1**2, 1**2, 1**2,
1**2, 1**2, 1**2,
100**2, 100**2, 100**2,
0.01**2, 0.01**2, 0.01**2])
# process noise
Q = np.diag([0.001**2, 0.001**2, 0.001**2,
0.01**2, 0.01**2, 0.01**2,
0.1**2, 0.1**2, 0.1**2,
(0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2,
3**2, 3**2, 3**2,
0.005**2, 0.005**2, 0.005**2])
locationd: make pose_kf inherit from KalmanFilter (#34982) * Read message not json for initial state * Delete lines * Fix param
3 weeks ago
obs_noise = {ObservationKind.PHONE_GYRO: np.diag([0.025**2, 0.025**2, 0.025**2]),
ObservationKind.PHONE_ACCEL: np.diag([.5**2, .5**2, .5**2]),
ObservationKind.CAMERA_ODO_TRANSLATION: np.diag([0.5**2, 0.5**2, 0.5**2]),
ObservationKind.CAMERA_ODO_ROTATION: np.diag([0.05**2, 0.05**2, 0.05**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
@staticmethod
def generate_code(generated_dir):
name = PoseKalman.name
dim_state = PoseKalman.initial_x.shape[0]
dim_state_err = PoseKalman.initial_P.shape[0]
state_sym = sp.MatrixSymbol('state', dim_state, 1)
state = sp.Matrix(state_sym)
roll, pitch, yaw = state[States.NED_ORIENTATION, :]
velocity = state[States.DEVICE_VELOCITY, :]
angular_velocity = state[States.ANGULAR_VELOCITY, :]
vroll, vpitch, vyaw = angular_velocity
gyro_bias = state[States.GYRO_BIAS, :]
acceleration = state[States.ACCELERATION, :]
acc_bias = state[States.ACCEL_BIAS, :]
dt = sp.Symbol('dt')
ned_from_device = euler_rotate(roll, pitch, yaw)
device_from_ned = ned_from_device.T
state_dot = sp.Matrix(np.zeros((dim_state, 1)))
state_dot[States.DEVICE_VELOCITY, :] = acceleration
f_sym = state + dt * state_dot
device_from_device_t1 = euler_rotate(dt*vroll, dt*vpitch, dt*vyaw)
ned_from_device_t1 = ned_from_device * device_from_device_t1
f_sym[States.NED_ORIENTATION, :] = rot_to_euler(ned_from_device_t1)
centripetal_acceleration = angular_velocity.cross(velocity)
gravity = sp.Matrix([0, 0, -EARTH_G])
h_gyro_sym = angular_velocity + gyro_bias
h_acc_sym = device_from_ned * gravity + acceleration + centripetal_acceleration + acc_bias
h_phone_rot_sym = angular_velocity
h_relative_motion_sym = velocity
obs_eqs = [
[h_gyro_sym, ObservationKind.PHONE_GYRO, None],
[h_acc_sym, ObservationKind.PHONE_ACCEL, None],
[h_relative_motion_sym, ObservationKind.CAMERA_ODO_TRANSLATION, None],
[h_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, None],
]
gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state_err)
def __init__(self, generated_dir, max_rewind_age):
dim_state, dim_state_err = PoseKalman.initial_x.shape[0], PoseKalman.initial_P.shape[0]
self.filter = EKF_sym_pyx(generated_dir, self.name, PoseKalman.Q, PoseKalman.initial_x, PoseKalman.initial_P,
dim_state, dim_state_err, max_rewind_age=max_rewind_age)
if __name__ == "__main__":
generated_dir = sys.argv[2]
PoseKalman.generate_code(generated_dir)