IQ.Lvbs/IQ.Pilot
8
7
Fork 2
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

IQ.Pilot Release Commit @ cd83f5a

Browse Source
This commit is contained in:
IQ.Lvbs CI [bot]
2026-08-23 11:32:03 -05:00
parent 58039e647c
commit a80e124cb8
116 changed files with 1657 additions and 4066 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored
View File

@@ -29,7 +29,6 @@ a.out
/iqdbc
/msgq
/openpilot
/rednose
/teleoprtc
/tinygrad

11
SConstruct
View File

@@ -9,7 +9,6 @@ import numpy as np
import iqdbc
import msgq as msgq_package
import panda
import rednose as rednose_package
import tinygrad
import SCons.Errors
@@ -110,8 +109,6 @@ env = Environment(
iqdbc.INCLUDE_PATH,
msgq_package.INCLUDE_PATH,
panda.INCLUDE_PATH,
rednose_package.INCLUDE_PATH,
os.path.dirname(rednose_package.__file__),
"#iqpilot/cereal/gen/cpp",
"#iqpilot/third_party",
"#iqpilot/third_party/json11",
@@ -132,9 +129,8 @@ env = Environment(
RPATH=[],
CYTHONCFILESUFFIX=".cpp",
COMPILATIONDB_USE_ABSPATH=True,
REDNOSE_ROOT=rednose_package.INCLUDE_PATH,
tools=["default", "cython", "compilation_db", "rednose_filter"],
toolpath=["#iqpilot/tools/scons/site_tools", rednose_package.SCONS_TOOL_PATH],
tools=["default", "cython", "compilation_db"],
toolpath=["#iqpilot/tools/scons/site_tools"],
)
# Arch-specific flags and paths
@@ -249,9 +245,6 @@ Import('socketmaster')
messaging = [socketmaster, msgq, 'capnp', 'kj',]
Export('messaging')
rednose = File(rednose_package.LIB_PATH)
Export('rednose')
# Build system services
SConscript([
'iqpilot/system/loggerd/SConscript',

114
artifacts/iqpilot_hephaestusd_private/manifest.json
View File

@@ -76,8 +76,8 @@
},
"python/_iqclosure/iqpilot/common/realtime.py": {
"mode": 420,
"sha256": "95b2e4eb3d78b607945cc6fad29c97a2e133eff4c427620d5c04b044f78abe10",
"size": 3763
"sha256": "f137ef62c1602a1f7c329731883dc518c62288bd25596891858d460ae3209ae2",
"size": 3966
},
"python/_iqclosure/iqpilot/common/spinner.py": {
"mode": 420,
@@ -151,18 +151,18 @@
},
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/__init__.py": {
"mode": 420,
"sha256": "e5ddcbc7b4dc348957c3d141c6bf8e7c321067f2ac56aecec24f6de107f9c2d3",
"size": 81
"sha256": "5865243a95ca4557d2a960302124a9898545cc3a8387e88ef569308e3d0f7077",
"size": 119
},
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/fetcher.py": {
"mode": 420,
"sha256": "0487789aec270f9a712a19afafe0b686f825f06f57006c2346c052d3622ac5f8",
"size": 619
"sha256": "23622cdec9eed5649a05422ba19eae956ba16a63850927af4a222c3d9030a1bb",
"size": 399
},
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/helpers.py": {
"mode": 420,
"sha256": "5ddd486e47cdf811c0c293c460aba2d3b15bb30059ff6da8da775c5cd236465b",
"size": 10945
"sha256": "ce5d23a33b0e2d9eb81525ad63a88823d20a0578191ceab48c20e24594f8f690",
"size": 10952
},
"python/_iqclosure/iqpilot/selfdrive/locationd/__init__.py": {
"mode": 420,
@@ -186,8 +186,8 @@
},
"python/_iqclosure/iqpilot/selfdrive/selfdrived/events.py": {
"mode": 420,
"sha256": "7694cfb588e978a30c78a58039b087754e352215240b5e4bde953d848419122b",
"size": 36515
"sha256": "5da7fc165b88cca63457f1aa4d62cef38b2585039e1b77184fbd5ff4b2fcc244",
"size": 36716
},
"python/_iqclosure/iqpilot/selfdrive/ui/__init__.py": {
"mode": 420,
@@ -291,8 +291,8 @@
},
"python/_iqclosure/iqpilot/system/manager/process_config.py": {
"mode": 420,
"sha256": "810bbd9bf9d211b1c54bfaaf56992b52e9a08a4cd0d1919d43a0a3b907401313",
"size": 11656
"sha256": "71594d16c0c104ede7bad3145d19d6af2d89d6c4aa4e9b5e6fdedd941ce35e41",
"size": 11679
},
"python/_iqclosure/iqpilot/system/micd.py": {
"mode": 420,
@@ -321,8 +321,8 @@
},
"python/_iqclosure/iqpilot/system/ui/lib/os_update.py": {
"mode": 420,
"sha256": "96760f2b5935a4429395628b64779efcfe80a7f50d98d57dda6c63855de1b2c6",
"size": 4236
"sha256": "f5b87e4e4b43d591eff61472922781ac41f2d43d10d2fe095207e7c14951dbd6",
"size": 4444
},
"python/_iqclosure/iqpilot/system/ui/lib/wifi_manager.py": {
"mode": 420,
@@ -331,8 +331,8 @@
},
"python/_iqclosure/iqpilot/system/version.py": {
"mode": 420,
"sha256": "b39a686854c9bfa3c6db9158f68734d7044760cf7ee79980102b7d23230836fd",
"size": 5909
"sha256": "3a6bc7520dae7944a1da6f911cd090704a463e011f39949ea6c9df51b0e8c616",
"size": 6033
},
"python/iqpilot_private/__init__.py": {
"mode": 420,
@@ -351,27 +351,27 @@
},
"python/iqpilot_private/konn3kt/backups/archive_codec.cpython-312-aarch64-linux-gnu.so": {
"mode": 493,
"sha256": "80cd58a10f6356fcad68063a474458653acbae17dfc8a66791256ec1fc109036",
"sha256": "2ec8be1fb7b3b4e657a79c8085996729a7e7b1d9cf8c37f5008a62f4aa952aab",
"size": 135552
},
"python/iqpilot_private/konn3kt/backups/backup_keys.cpython-312-aarch64-linux-gnu.so": {
"mode": 493,
"sha256": "81e12476cedca56039fe9bf194cc9fd1a80b223c987e212a3208c64ff1390b26",
"sha256": "87e315903c8dbe8803111e2fe21c2bd367ff69cfb05ab88458e12130022a6c2c",
"size": 67664
},
"python/iqpilot_private/konn3kt/backups/backup_orchestrator.cpython-312-aarch64-linux-gnu.so": {
"mode": 493,
"sha256": "9d7e7fd9d1a3d2ff6ba8140fbe3743502151768f7d2c22625ffe2a78b3c8cd7b",
"sha256": "187b5e13f2b21def80888ee353be65dee1aced7b2b1c7a4f8c962ce8895f8f89",
"size": 204640
},
"python/iqpilot_private/konn3kt/backups/cbc_vault.cpython-312-aarch64-linux-gnu.so": {
"mode": 493,
"sha256": "c39b482f192406ab8f3ec4733f2da8131ec1d9d5c791189035e42504d8e6132f",
"sha256": "9338a83201734ff572d431de5f56c8279fc4a656d10d0a67ea44e8057bb88016",
"size": 69768
},
"python/iqpilot_private/konn3kt/backups/imahelper.cpython-312-aarch64-linux-gnu.so": {
"mode": 493,
"sha256": "5524a08ebc9918fabf6d8c7d2ccdb89ac67cbf8f017e4d00989bf88dc9720b12",
"sha256": "df671d37153130a8dcf67fd8a7ae7b024206d19aaa6db85b48811a259289481d",
"size": 203088
},
"python/iqpilot_private/konn3kt/flockd/__init__.py": {
@@ -381,12 +381,12 @@
},
"python/iqpilot_private/konn3kt/flockd/flockd.cpython-312-aarch64-linux-gnu.so": {
"mode": 493,
"sha256": "364a06ba9338c61fe904c6c93e3be7bc567b0881cffaaa62b7cba0d67a5f3c04",
"sha256": "a67c26048adfb32e76168bdb6a260e11dedbf05257a66eba5eef3b4a623ce1e6",
"size": 202184
},
"python/iqpilot_private/konn3kt/flockd/signatures.cpython-312-aarch64-linux-gnu.so": {
"mode": 493,
"sha256": "08a7990bb84f88505eb5e58745b339b59ada3cc2b802c5f2396281cb03af1c84",
"sha256": "e09bb698e3128449a4da3c2d767621928e531140181b20acd2adcf4186617953",
"size": 68080
},
"python/iqpilot_private/konn3kt/hephaestus/__init__.py": {
@@ -396,67 +396,67 @@
},
"python/iqpilot_private/konn3kt/hephaestus/_vendor/localapi_runtime.zip": {
"mode": 420,
"sha256": "ec5e60ae7ebed56e6613216115b25e3f7388afd374619a656642118b0506642a",
"sha256": "cb21ff749633ca4b01aef63c178df260d51fff631aed83923b223d70bf6533bd",
"size": 2100238
},
"python/iqpilot_private/konn3kt/hephaestus/ble_auth.cpython-312-aarch64-linux-gnu.so": {
"mode": 493,
"sha256": "b9d5402b525987bc9939bf30113debde997d07a7ab0a5922bf1954cea7cb7757",
"sha256": "85bf27b0c19387fdb52b493f259e84a23d31c73b5e275e964dbeb934d329c821",
"size": 268080
},
"python/iqpilot_private/konn3kt/hephaestus/ble_gatt.cpython-312-aarch64-linux-gnu.so": {
"mode": 493,
"sha256": "68787d021840f0e1ee1babd3e4e781324c9b4e1bb8de38e78ac9fb57d967ba7c",
"sha256": "1525c92a7fe8dd898a07188fcdc05dc608a5b77b35ab2fc9d250fcc46bf69999",
"size": 406176
},
"python/iqpilot_private/konn3kt/hephaestus/ble_rpc_dispatch.cpython-312-aarch64-linux-gnu.so": {
"mode": 493,
"sha256": "405fbafd6e385b7f6fa5f20b1ef1fa38cbd04952edef764efffbecbbebcd8f14",
"sha256": "f5163ae4cbaa77c09ccbea174f659bf719fa9f9c403db478af33ab09a8a9673b",
"size": 68048
},
"python/iqpilot_private/konn3kt/hephaestus/ble_transportd.cpython-312-aarch64-linux-gnu.so": {
"mode": 493,
"sha256": "2449bf7779155e546ea14a5c10ee16be174da6993f6ab25e2aa59de007b5078b",
"sha256": "c69f659c3736c14a57065e118c35cf06c262520a35c37afea3e7bbc43844c129",
"size": 335776
},
"python/iqpilot_private/konn3kt/hephaestus/bt_gamepad.cpython-312-aarch64-linux-gnu.so": {
"mode": 493,
"sha256": "951fbff54e5fd1c667ca73359ea015e9cf79dcd55abb7f8b1bc857a38e11a54a",
"sha256": "543534177922974da9872b56ba91b5f63b26ab9cb3b275f8e7bd5517e2971ca6",
"size": 271696
},
"python/iqpilot_private/konn3kt/hephaestus/cloud_routes.cpython-312-aarch64-linux-gnu.so": {
"mode": 493,
"sha256": "2fa9a1e9352721a03f2eaa4ecf3719787baafc35fb37d71e0b6372656cde6d5f",
"sha256": "53a637f886ac6c465c2e3a5ec74b7f2d4e624bbfdb0186c11f29f8d50a6af8d2",
"size": 134408
},
"python/iqpilot_private/konn3kt/hephaestus/hephaestusd.cpython-312-aarch64-linux-gnu.so": {
"mode": 493,
"sha256": "d342ef73b4fd9d60407c60d0c3768668e8bdbb0d9702f163aec4893f8d49af6c",
"sha256": "52b6306bc0b6a69198ea139eb311e3c70a7f7d0b2093d17cae75e280dfdf52d1",
"size": 3715880
},
"python/iqpilot_private/konn3kt/hephaestus/kwp2000.cpython-312-aarch64-linux-gnu.so": {
"mode": 493,
"sha256": "cf6cc790e765adc5913c23ff32dea23588c1fb8608c6a8043fbc9b1bb2b662e8",
"sha256": "1dab38fc494466cac932289b9805266d211b1b510fe838907f3cb3afe53ed93c",
"size": 136472
},
"python/iqpilot_private/konn3kt/hephaestus/manage_hephaestusd.cpython-312-aarch64-linux-gnu.so": {
"mode": 493,
"sha256": "4d66dbd069d90ee956feac23130c5a14338f3d66492180dd3cd945f8a5c4ddad",
"sha256": "1adfdd3fa1ebd5e0f83e147dadf6b0a91e044bce00ea2a8c90c539c1ad0970dc",
"size": 68208
},
"python/iqpilot_private/konn3kt/hephaestus/motd.cpython-312-aarch64-linux-gnu.so": {
"mode": 493,
"sha256": "d5227394abae38d2118d7e1df8a142ad2137a28e02679d29d2426c3ac8a60ba8",
"sha256": "af1119b628946776d468bad4c6f05126a9f1b76010e76d84615f004fb10ea0cd",
"size": 67840
},
"python/iqpilot_private/konn3kt/hephaestus/tp20.cpython-312-aarch64-linux-gnu.so": {
"mode": 493,
"sha256": "59cba66846b23370b9c7b20d108db8bf6f1c7dbc8a49958621905e0cb322fbe9",
"sha256": "e5e0fa2832d0ace95843e5d732ecd9e1de370c00d5dc41a55f33103236194125",
"size": 135664
},
"python/iqpilot_private/konn3kt/hephaestus/vw_pq_flasher.cpython-312-aarch64-linux-gnu.so": {
"mode": 493,
"sha256": "cc1be0247694a84b596e1333d53fa23a44fa2353b8b635f95b0329a5ffef5585",
"sha256": "f20bd4b330e641b481db8f545b72d472007402c709666b687d4e9897c42d0dc0",
"size": 269600
},
"python/iqpilot_private/konn3kt/uploaderd/__init__.py": {
@@ -466,7 +466,7 @@
},
"python/iqpilot_private/konn3kt/uploaderd/iquploaderd.cpython-312-aarch64-linux-gnu.so": {
"mode": 493,
"sha256": "8a4926ba1d3bcf80326f699b1f9c775cd1fbe9d4c7981ba47d8232a8d4aedf79",
"sha256": "eee4e9e58787962b9cb5e63f9b946fa07ac1121c3c29373f947d3d461db3f1e3",
"size": 204296
},
"runtime": {
@@ -510,25 +510,25 @@
}
},
"signatures": {
"python/iqpilot_private/konn3kt/backups/archive_codec.cpython-312-aarch64-linux-gnu.so": "tV3zvq8PZd4MgIU1hjzjpVxS8puOkVUUsJsSYb2fjeOqkBv6KG3oIlN+TPGiwwO7YX8dO8dFivZWGnktKWKPCg==",
"python/iqpilot_private/konn3kt/backups/backup_keys.cpython-312-aarch64-linux-gnu.so": "kG8Ifpx65b0dhdTdfFhLr65vGa1O5enjiCI9N3aGd5+aOE7DdKJPID4Q3vIIjMZqMdoKPTdBLj3MaPEYXL3pCg==",
"python/iqpilot_private/konn3kt/backups/backup_orchestrator.cpython-312-aarch64-linux-gnu.so": "1Dh3vRWc91DfHWg7pw9GhZ6O5eT335f4utjxSzHh8Y1wvwButYfsEO5DUZ2KNss5IAKDy7O4/CpnACD/64UYDg==",
"python/iqpilot_private/konn3kt/backups/cbc_vault.cpython-312-aarch64-linux-gnu.so": "6Fdo8Du4ZUMt8241uRDjLVoy1a2flzQNeXMfaVuRaMU8aYZZixqvaYPeanZDPOTvpy6PxaFVZYYnBR/cqokNCw==",
"python/iqpilot_private/konn3kt/backups/imahelper.cpython-312-aarch64-linux-gnu.so": "YTL4x/JaFnZCALPoyET84LvOcISGB5vrpTYn5b87kSDdcWPDwiTyEAXUuueOQGRQXVzZJj4UC+zal2DEA2QlAw==",
"python/iqpilot_private/konn3kt/flockd/flockd.cpython-312-aarch64-linux-gnu.so": "KHqjlPIClO9hg2LxyOzRismA38ysUPxci0KCiHrydE8W7Nbd3LMMR1/9/rUvPUfRYY/szpkfvkZTlMwo26R5BQ==",
"python/iqpilot_private/konn3kt/flockd/signatures.cpython-312-aarch64-linux-gnu.so": "Uas3ReVuTgjJxr1elZ/26R8/6StRaFrMPD16t7GWGj6PTG+CiUOPn4oiiJgfumevnrYw4xGGLRsfZQhFHaUHAA==",
"python/iqpilot_private/konn3kt/hephaestus/ble_auth.cpython-312-aarch64-linux-gnu.so": "39bSI5v9Mg9VQLo3PmfUBmfHE1hKjTD2Jfgak/W3m1FJ7eXROhUjts6bsY5pXsHDx6MTcEw35IUr7Q8v+3bQBg==",
"python/iqpilot_private/konn3kt/hephaestus/ble_gatt.cpython-312-aarch64-linux-gnu.so": "hB1aGUsLCYcFSLxGxg1nuRzbrA1Uv/StOQv0V/0L8v73Z7xakXYFW8h/C5xD2/hNIGFMaNc7GLOyc2GwN7tVAg==",
"python/iqpilot_private/konn3kt/hephaestus/ble_rpc_dispatch.cpython-312-aarch64-linux-gnu.so": "LlBVzMoPeK8ypvzgXXYkqmooV6V1lHtS6Ym5VPj/fNFn79hWfhbsjwY9A463hs3QzaxGh5OWwcc1pmFc6USODw==",
"python/iqpilot_private/konn3kt/hephaestus/ble_transportd.cpython-312-aarch64-linux-gnu.so": "nq7AH2XYZuAxhGfHfgc3haBGbu5bDwOI15mI++Be3VYsVah0P+Fqd0F5G6dZjIFo5fbuO0FH7/LHOlEnxRQcBw==",
"python/iqpilot_private/konn3kt/hephaestus/bt_gamepad.cpython-312-aarch64-linux-gnu.so": "m2y/Lozqs16IZsrpL+OOQ0MQCWEB+EuyacYa/TFch4KxwPHVrEnh+zKtGdeIhcd7pVkUd1IRkFVnWGGvBx+pCg==",
"python/iqpilot_private/konn3kt/hephaestus/cloud_routes.cpython-312-aarch64-linux-gnu.so": "+njDWbhjEsFL1WzGlL0fN5ygbqGbfwInwaAquDeC5aYgbrwvJOdnwonJgxV0fd0ABzIwfuUFzSMZg6F3qI84BA==",
"python/iqpilot_private/konn3kt/hephaestus/hephaestusd.cpython-312-aarch64-linux-gnu.so": "G8sfe52XB1m4Lc197oLD51bwbXOZXkpOtAsgiMHZHwbayEFsPeI2Ls/R4ZOcPYTjztXDKYt68Ig8QJblRanTCQ==",
"python/iqpilot_private/konn3kt/hephaestus/kwp2000.cpython-312-aarch64-linux-gnu.so": "jr8rsgKsxDYBuY58XqIqMkb9pW78Jb6rBGaEHLZRtWS6z58td37+PBesMzzg4J1GNov4RKcZfMbaANBui1OnAA==",
"python/iqpilot_private/konn3kt/hephaestus/manage_hephaestusd.cpython-312-aarch64-linux-gnu.so": "QbprGqwfKC9bo4xs8pNieYI/TOwLOUNv4YVHhXPS/2xVU5bS+aDr71aD/o4PF3UmkJrMW/94b8X22Dy19SYEDQ==",
"python/iqpilot_private/konn3kt/hephaestus/motd.cpython-312-aarch64-linux-gnu.so": "Iv9RL4rXHell9BfRp6PoWxD3TnT+WLugypT3aG4Ukf/iMqPVPizGtqfJY/axQY8jItBGTXcIyfZA9O6juGpfBg==",
"python/iqpilot_private/konn3kt/hephaestus/tp20.cpython-312-aarch64-linux-gnu.so": "KKD5TdLEDBtDy+YoUgglFkpHVJjvn9K4xlzUF7doWIAo1+n3ySTYQ4V7+iJ04H0Axs3Gsp0d0ytDui5qw09+Cw==",
"python/iqpilot_private/konn3kt/hephaestus/vw_pq_flasher.cpython-312-aarch64-linux-gnu.so": "C4po9GxTKUlww19PaUJUi+kUTBTTVYyUMjAvAhNYAVcOS6mpVNMIBX7UCt3XcTtlLU+q+goE0Mw68LvoMza4AQ==",
"python/iqpilot_private/konn3kt/uploaderd/iquploaderd.cpython-312-aarch64-linux-gnu.so": "l1lWA8GLEToT4GDa96aEt6TFO8RDP/WSHgXV+9mge8cHQ82UVrVJKX98andFvKDaNGPjZgR+gK5gslEMt2dVCg=="
"python/iqpilot_private/konn3kt/backups/archive_codec.cpython-312-aarch64-linux-gnu.so": "l+KcwF57otKYCL17nXSTJ8s0so+z9q4b/SsOfagBc9G+TbGDO9qHYcbsMtWSKoIj7Np6NGwAhBBKGubGNA6NDw==",
"python/iqpilot_private/konn3kt/backups/backup_keys.cpython-312-aarch64-linux-gnu.so": "7gttAqiZR2SOutahE8XnQEH+es5E7nHt8TprLg3QAv/q/aYVTvJMHM/uMYUr7CVqtNSk9NXzi4kuahry96A2DQ==",
"python/iqpilot_private/konn3kt/backups/backup_orchestrator.cpython-312-aarch64-linux-gnu.so": "otqtacOKGvys+mxrA0GSs/C2gvL/RoyNVXXeTtwhYYBYsBwWEacuM6xEAANZtnkowt2EIrY22RdIDS9091KBBQ==",
"python/iqpilot_private/konn3kt/backups/cbc_vault.cpython-312-aarch64-linux-gnu.so": "4qPIHhjLGAGjKTXtvAWbdrOqxLL5vJtZnF3xVUBQiB/3mQo20i0hPT8+6164HQXMyf596FsetmzUclPPlAeoBQ==",
"python/iqpilot_private/konn3kt/backups/imahelper.cpython-312-aarch64-linux-gnu.so": "jPid/JOUWsCYqtCVG4JD5c86nLtadg9L3YICEhsodZiaVPx2Yga0wxPvS7wkaNKCBohIGD20QxJqBUgrKnXQCA==",
"python/iqpilot_private/konn3kt/flockd/flockd.cpython-312-aarch64-linux-gnu.so": "Wl2IcgkvLI/bHQfozQP4vcMhbhMhph3rb/Zu4Mg7E6XKNfxw/N8/frq+8x8QV9N2kRro9/6eANyAamOsQUFuBw==",
"python/iqpilot_private/konn3kt/flockd/signatures.cpython-312-aarch64-linux-gnu.so": "nzibDoeERknQpLGPUCUQ0qa7FxkoJ2UNXHA8pgcIFWCLMUXDPODjGbKdx2mpSQDU7Q5E5xNx6gjSU0JgZvpZAA==",
"python/iqpilot_private/konn3kt/hephaestus/ble_auth.cpython-312-aarch64-linux-gnu.so": "uuROebZDc2abXyiuhL8kvQszS5/aADOnEG1wP0cssD2I6CRPqJb47y6mwXKlUJrgasod2XDhD22mCbn9sDuVDA==",
"python/iqpilot_private/konn3kt/hephaestus/ble_gatt.cpython-312-aarch64-linux-gnu.so": "5wGiTrEA8uaDEkk5W0eHztI+gaixGHIT4sbgUKeiW8nUlJYAsC7pDXodOZMmcmcjtQTtgTFrzhDnA9O1aaC+Ag==",
"python/iqpilot_private/konn3kt/hephaestus/ble_rpc_dispatch.cpython-312-aarch64-linux-gnu.so": "P6k37fMBW5ryhdHxReumrOSIuRnIb7rqaq+3OOVpgwol/lDwST0WL/wZfSNsnGB8tQL+YZOLOTw6daJe6BanBQ==",
"python/iqpilot_private/konn3kt/hephaestus/ble_transportd.cpython-312-aarch64-linux-gnu.so": "lP6OEHgbOtQu8AskEdKgy+773g7F/NWGrkeD4LeC7zcdHmdmfg2dYcN+Wdrs8FrlvAjBilFqyv3wZVfoKv8dBg==",
"python/iqpilot_private/konn3kt/hephaestus/bt_gamepad.cpython-312-aarch64-linux-gnu.so": "q1XPOv/SdBC4xfSsiDSf8yR2mdfgWbseTBrUbl9xilFYUd565cv8iauUqeh3/EsVV8qZvcIZViTWuCpy00VZAQ==",
"python/iqpilot_private/konn3kt/hephaestus/cloud_routes.cpython-312-aarch64-linux-gnu.so": "zY8c7e8Qq5vSDlbymf3o6hsPz+slQ8oxqiLk8qpKDNFfP6hJxK2eUf/9WHQGGTvGg99d/QoQVyozCu1XRGoFCA==",
"python/iqpilot_private/konn3kt/hephaestus/hephaestusd.cpython-312-aarch64-linux-gnu.so": "xu/P0Ekw0d1yjINg542+10BROwGUU5G6qktfwnhFymcF0jhWGaThSndJmPeFmWq0k9bs4asAXimgq5/BUAIABw==",
"python/iqpilot_private/konn3kt/hephaestus/kwp2000.cpython-312-aarch64-linux-gnu.so": "zFPl0ip4i/baX0GJzDnPGrm9vAgeXXMIotBr0VArZOxtOM1pOyGwOaJpgWBtb4kVUsNLepjfPgva6UVhrvb6Dg==",
"python/iqpilot_private/konn3kt/hephaestus/manage_hephaestusd.cpython-312-aarch64-linux-gnu.so": "Ai6K1+DGajQIi6tEBl0wfPoHWHA499mXbLrEZQZZb0X7dlASbwu+MVW1jovY3kgskMI9JcVANNEWPfSTvPnyAg==",
"python/iqpilot_private/konn3kt/hephaestus/motd.cpython-312-aarch64-linux-gnu.so": "GMyhhOO0gGxbbLiB0yjUKVOwV6JeeTfhKNnjytjAboFDUj4ueh/Kuf9L6CRuFZf9xU6uvB6QXSBuUr+FHEYQDQ==",
"python/iqpilot_private/konn3kt/hephaestus/tp20.cpython-312-aarch64-linux-gnu.so": "+fLA6hdx0m3WISNW+mvi8s8Lk/N+ZOvfyEF69ZmQ8lFWVdN08g52wbaH6lR2hh2bl8y8rekqFGSHKj38NnYcAg==",
"python/iqpilot_private/konn3kt/hephaestus/vw_pq_flasher.cpython-312-aarch64-linux-gnu.so": "DGU9axrJW6Mtcpt43w07LgP9u+fk67pOZ4pfmiTGddnsZkw1IEJJZneguVTO5mUN7OZq9iWH2KsqV+N1aaLbDw==",
"python/iqpilot_private/konn3kt/uploaderd/iquploaderd.cpython-312-aarch64-linux-gnu.so": "SSw7fJoNsbQ1GuomiW9rETXgMVqHKgqv7wuPIbzL+Y+pNJyYgvmikeyTFaloTIhllvr9n0XzSp/KT+qr41weCw=="
}
}

2
artifacts/iqpilot_hephaestusd_private/manifest.json.sig
View File

@@ -1 +1 @@
HlmJ/WkPaO0L0omQG7BWUtHYKzp2ZTFvaVu4BDpACrkluSoWeyX0suOZlU29fJvRI+nWhBEkRW7807toLyrNAQ==
aT5ZgWVbq0qWHt3m8wQmPWVfaFivYO6zCFB2LxmMC6bn1ZS+k/tNYZTbIANNFwhKGM4Q6zdQeGVicoDreqtYBw==

6
artifacts/iqpilot_hephaestusd_private/python/_iqclosure/iqpilot/common/realtime.py
View File

@@ -41,6 +41,12 @@ def config_realtime_process(cores: int | list[int], priority: int) -> None:
set_core_affinity(c)
def config_background_thread() -> None:
if sys.platform == 'linux' and not PC:
os.sched_setscheduler(0, os.SCHED_OTHER, os.sched_param(0))
set_core_affinity(list(range(os.cpu_count() or 1)))
def lock_memory() -> None:
"""mlockall this process so memory reclaim/compaction can't stall it. RT control
procs only (locking ui/modeld would worsen pressure). Best-effort."""

2
artifacts/iqpilot_hephaestusd_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/__init__.py
View File

@@ -1,3 +1,3 @@
"""
IQ model selection and runner support that is actively used by iqmodeld.
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
"""

7
artifacts/iqpilot_hephaestusd_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/fetcher.py
View File

@@ -1,11 +1,8 @@
#!/usr/bin/env python3
"""
Copyright (c) IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
Public entry point for the model-manifest fetcher: prefers the compiled private
bundle, falling back to the in-tree source. The default-runner fallback lives in
ManifestDecoder now, so no post-import patching is needed.
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
"""
from iqpilot._proprietary_loader import ProprietaryModuleMissing, load_private_module
try:

6
artifacts/iqpilot_hephaestusd_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/helpers.py
View File

@@ -40,7 +40,6 @@ _DEFAULT_BUNDLE_REF = "default"
def get_default_model_bundle(_bundles):
"""Legacy compatibility hook: stock default is preinstalled, not a manifest bundle."""
return None
@@ -239,10 +238,13 @@ def select_default_model(params: Params = None) -> None:
def seed_default_bundle_if_unset(params: Params = None) -> None:
params = Params() if params is None else params
if params.get(_ACTIVE_BUNDLE_KEY) or params.get(_DOWNLOAD_INDEX_KEY) is not None:
if params.get(_ACTIVE_BUNDLE_KEY):
return
queued_download = params.get(_DOWNLOAD_INDEX_KEY)
try:
select_default_model(params)
if queued_download is not None:
params.put(_DOWNLOAD_INDEX_KEY, queued_download)
cloudlog.warning("default_model: seeded Default (CD210) as active bundle")
except Exception as e:
cloudlog.exception(f"default_model: failed to seed default bundle: {e}")

12
artifacts/iqpilot_hephaestusd_private/python/_iqclosure/iqpilot/selfdrive/selfdrived/events.py
View File

@@ -648,8 +648,16 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
},
EventName.wrongGear: {
ET.SOFT_DISABLE: user_soft_disable_alert("Gear not D"),
ET.NO_ENTRY: NoEntryAlert("Gear not D"),
ET.SOFT_DISABLE: Alert(
"",
"",
AlertStatus.normal, AlertSize.none,
Priority.LOWEST, VisualAlert.none, AudibleAlert.none, 0.),
ET.NO_ENTRY: Alert(
"",
"",
AlertStatus.normal, AlertSize.none,
Priority.LOWEST, VisualAlert.none, AudibleAlert.none, 0.),
},
# This alert is thrown when the calibration angles are outside of the acceptable range.

2
artifacts/iqpilot_hephaestusd_private/python/_iqclosure/iqpilot/system/manager/process_config.py
View File

@@ -195,7 +195,7 @@ procs = [
procs += [
# Models
BundleProcess("models_manager", "iqpilot_model_selector_private", "iqpilot_private.models.manager", and_(only_offroad, not_low_power)),
NativeProcess("iqmodeld", "iqpilot/selfdrive/iqmodeld", ["./iqmodeld"], and_(only_onroad, is_tinygrad_model)),
NativeProcess("iqmodeld", "iqpilot/selfdrive/iqmodeld", ["./iqmodeld"], and_(only_onroad, is_tinygrad_model), restart_if_crash=True),
BundleProcess("backup_manager_k3", "iqpilot_hephaestusd_private", "iqpilot_private.konn3kt.backups.backup_orchestrator",
and_(only_offroad, hephaestus_ready_shim, not_low_power)),

11
artifacts/iqpilot_hephaestusd_private/python/_iqclosure/iqpilot/system/ui/lib/os_update.py
View File

@@ -42,10 +42,17 @@ def required_agnos_version(install_path: str) -> str:
return ""
def _hardware_dir(install_path: str) -> str:
nested = os.path.join(install_path, "iqpilot", "system", "hardware", "tici")
if os.path.isdir(nested):
return nested
return os.path.join(install_path, "system", "hardware", "tici")
def agnos_manifest_path(install_path: str, device_type: str) -> str:
# comma 3 (tici) uses a different AGNOS manifest than comma 3x (tizi) / comma 4 (mici).
fname = "agnos_tici_15_1.json" if device_type == "tici" else "agnos.json"
return os.path.join(install_path, "system", "hardware", "tici", fname)
return os.path.join(_hardware_dir(install_path), fname)
def os_update_needed(install_path: str) -> tuple[bool, str, str]:
@@ -64,7 +71,7 @@ def run_agnos_update(install_path: str, device_type: str, progress_cb: ProgressC
via progress_cb(percent, note). Returns True on success. The device must be
rebooted by the caller afterward for the new slot to take effect."""
manifest = agnos_manifest_path(install_path, device_type)
agnos_py = os.path.join(install_path, "system", "hardware", "tici", "agnos.py")
agnos_py = os.path.join(_hardware_dir(install_path), "agnos.py")
if not os.path.isfile(manifest) or not os.path.isfile(agnos_py):
progress_cb(0, "manifest_missing")
return False

9
artifacts/iqpilot_hephaestusd_private/python/_iqclosure/iqpilot/system/version.py
View File

@@ -33,8 +33,13 @@ def get_version(path: str = BASEDIR) -> str:
def get_release_notes(path: str = BASEDIR) -> str:
with open(os.path.join(path, "docs", "CHANGELOG.md")) as f:
return f.read().split('\n\n', 1)[0]
for rel in (("iqpilot", "docs", "CHANGELOG.md"), ("docs", "CHANGELOG.md")):
try:
with open(os.path.join(path, *rel)) as f:
return f.read().split('\n\n', 1)[0]
except OSError:
continue
return ""
@cache

BIN
artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/backups/archive_codec.cpython-312-aarch64-linux-gnu.so
View File

Binary file not shown.

BIN
artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/backups/backup_keys.cpython-312-aarch64-linux-gnu.so
View File

Binary file not shown.

BIN
artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/backups/backup_orchestrator.cpython-312-aarch64-linux-gnu.so
View File

Binary file not shown.

BIN
artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/backups/cbc_vault.cpython-312-aarch64-linux-gnu.so
View File

Binary file not shown.

BIN
artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/backups/imahelper.cpython-312-aarch64-linux-gnu.so
View File

Binary file not shown.

BIN
artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/flockd/flockd.cpython-312-aarch64-linux-gnu.so
View File

Binary file not shown.

BIN
artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/flockd/signatures.cpython-312-aarch64-linux-gnu.so
View File

Binary file not shown.

BIN
artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/_vendor/localapi_runtime.zip
View File

Binary file not shown.

BIN
artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/ble_auth.cpython-312-aarch64-linux-gnu.so
View File

Binary file not shown.

BIN
artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/ble_gatt.cpython-312-aarch64-linux-gnu.so
View File

Binary file not shown.

BIN
artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/ble_rpc_dispatch.cpython-312-aarch64-linux-gnu.so
View File

Binary file not shown.

BIN
artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/ble_transportd.cpython-312-aarch64-linux-gnu.so
View File

Binary file not shown.

BIN
artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/bt_gamepad.cpython-312-aarch64-linux-gnu.so
View File

Binary file not shown.

BIN
artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/cloud_routes.cpython-312-aarch64-linux-gnu.so
View File

Binary file not shown.

BIN
artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/hephaestusd.cpython-312-aarch64-linux-gnu.so
View File

Binary file not shown.

BIN
artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/kwp2000.cpython-312-aarch64-linux-gnu.so
View File

Binary file not shown.

BIN
artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/manage_hephaestusd.cpython-312-aarch64-linux-gnu.so
View File

Binary file not shown.

BIN
artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/motd.cpython-312-aarch64-linux-gnu.so
View File

Binary file not shown.

BIN
artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/tp20.cpython-312-aarch64-linux-gnu.so
View File

Binary file not shown.

BIN
artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/vw_pq_flasher.cpython-312-aarch64-linux-gnu.so
View File

Binary file not shown.

BIN
artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/uploaderd/iquploaderd.cpython-312-aarch64-linux-gnu.so
View File

Binary file not shown.

4
artifacts/package_sources/iqdbc/iqdbc/car/tesla/carcontroller.py
View File

@@ -58,7 +58,7 @@ class CarController(CarControllerBase):
# Longitudinal control
if self.CP.openpilotLongitudinalControl:
if self.frame % 4 == 0:
state = 13 if CC.cruiseControl.cancel or CS.das_accCancel else 4 # 4=ACC_ON, 13=ACC_CANCEL_GENERIC_SILENT
state = 13 if CC.cruiseControl.cancel else 4 # 4=ACC_ON, 13=ACC_CANCEL_GENERIC_SILENT
accel = float(np.clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX))
if not CC.longActive:
accel = 0.
@@ -70,7 +70,7 @@ class CarController(CarControllerBase):
comfort_mode=bool(getattr(CC, "longComfortMode", False)),
stopping=getattr(actuators, "longControlState", None) ==
structs.CarControl.Actuators.LongControlState.stopping,
cancel=CC.cruiseControl.cancel or CS.das_accCancel))
cancel=CC.cruiseControl.cancel))
else:
# Increment counter so cancel is prioritized even without openpilot longitudinal

23
artifacts/package_sources/iqdbc/iqdbc/car/tesla/carstate.py
View File

@@ -1,6 +1,6 @@
import copy
from iqdbc.can import CANDefine, CANParser
from iqdbc.car import Bus, create_button_events, structs
from iqdbc.car import Bus, structs
from iqdbc.car.common.conversions import Conversions as CV
from iqdbc.car.interfaces import CarStateBase
from iqdbc.car.tesla import TESLA_BLINKERS
@@ -10,8 +10,6 @@ from iqdbc.lvbs.car.tesla.iq_carstate import IQCarState
from iqdbc.lvbs.car.tesla.values import TeslaFlagsIQ, TeslaSafetyFlagsIQ
from iqpilot.common.params import Params
ButtonType = structs.CarState.ButtonEvent.Type
def stock_autosteer_invalid(CP, CP_IQ, autopilot_state: int) -> bool:
return (not (CP.flags & TeslaFlags.MISSING_DAS_SETTINGS) and
@@ -33,9 +31,6 @@ class CarState(CarStateBase, IQCarState):
self.cruise_enabled_prev = False
self.hands_on_level = 0
self.acc_state_last = 0
self.das_accCancel = False
self.das_cancel_last = True
self.das_control = None
self.das_body_controls_dat = b""
self._odometer_store = iq_lvbs_alc.create_vehicle_odometer_store(CP, Params())
@@ -99,25 +94,12 @@ class CarState(CarStateBase, IQCarState):
# Cruise state
cruise_state = self.can_define.dv["DI_state"]["DI_cruiseState"].get(int(cp_party.vl["DI_state"]["DI_cruiseState"]), None)
speed_units = self.can_define.dv["DI_state"]["DI_speedUnits"].get(int(cp_party.vl["DI_state"]["DI_speedUnits"]), None)
acc_state = cp_ap_party.vl["DAS_control"]["DAS_accState"]
summon_state = self.can_define.dv["DI_state"]["DI_autoparkState"].get(int(cp_party.vl["DI_state"]["DI_autoparkState"]), None)
cruise_enabled = cruise_state in ("ENABLED", "STANDSTILL", "OVERRIDE", "PRE_FAULT", "PRE_CANCEL")
self.cruise_override = cruise_state in ("OVERRIDE")
self.update_summon_state(summon_state, cruise_enabled)
# Respect all stock DAS cancel states, not just ACC_CANCEL_GENERIC_SILENT(13).
# ELDA/ELK triggers ACC_CANCEL_GENERIC(0) which must also be forwarded.
# The stock AP is isolated from the party bus while the relay is closed, so its accState
# free-runs between ACC_ON and ACC_CANCEL_GENERIC. Only a rising edge while ACC is engaged
# is a real cancel; level-forwarding it pins DI_cruiseState to UNAVAILABLE and blocks engaging.
das_cancel = acc_state in (0, 1, 2, 12, 13, 14, 15)
if not cruise_enabled:
self.das_accCancel = False
elif das_cancel and not self.das_cancel_last:
self.das_accCancel = True
self.das_cancel_last = das_cancel
# Match panda safety cruise engaged logic
ret.cruiseState.enabled = cruise_enabled and not self.summon
if speed_units == "KPH":
@@ -130,9 +112,6 @@ class CarState(CarStateBase, IQCarState):
ret.standstill = cp_party.vl["ESP_B"]["ESP_vehicleStandstillSts"] == 1
ret.accFaulted = cruise_state == "FAULT"
ret.buttonEvents = [*create_button_events(acc_state, self.acc_state_last, {0: ButtonType.cancel, 13: ButtonType.cancel})]
self.acc_state_last = acc_state
# Gear
ret.gearShifter = GEAR_MAP[self.can_define.dv["DI_systemStatus"]["DI_gear"].get(int(cp_party.vl["DI_systemStatus"]["DI_gear"]), "DI_GEAR_INVALID")]

3
artifacts/package_sources/iqdbc/iqdbc/lvbs/car/tesla/iq_carstate.py
View File

@@ -35,7 +35,8 @@ class IQCarState:
prev_infotainment_3_finger_press = self.infotainment_3_finger_press
self.infotainment_3_finger_press = int(cp_adas.vl["UI_status2"]["UI_activeTouchPoints"])
ret.buttonEvents = [*create_button_events(self.infotainment_3_finger_press, prev_infotainment_3_finger_press,
ret.buttonEvents = [*ret.buttonEvents,
*create_button_events(self.infotainment_3_finger_press, prev_infotainment_3_finger_press,
{3: ButtonType.lkas})]
bms_soc_ui = float(cp_adas.vl["ID292BMS_SOC"].get("SOCUI292", 0.0))

2
artifacts/package_sources/panda/board/drivers/can_common.h
View File

@@ -307,7 +307,7 @@ void ignition_can_hook(CANPacket_t *msg) {
vw_meb_gateway_out_of_p = (fahrstufe >= 6) && (fahrstufe <= 14);
}
if ((msg->addr == 0x13DU) && (len == 32)) {
if ((msg->addr == 0xC0U) && (len == 32)) {
vw_meb_seen = true;
}

1
artifacts/package_sources/rednose/.dockerignore
View File

@@ -1 +0,0 @@
.sconsign.dblite

11
artifacts/package_sources/rednose/.editorconfig
View File

@@ -1,11 +0,0 @@
root = true
[*]
end_of_line = lf
insert_final_newline = true
trim_trailing_whitespace = true
[{*.py, *.pyx, *pxd}]
charset = utf-8
indent_style = space
indent_size = 2

151
artifacts/package_sources/rednose/.gitignore vendored
View File

@@ -1,151 +0,0 @@
generated/
.sconsign.dblite
*.swp
*.tmp
# Cython intermediates
*_pyx.cpp
*_pyx.h
*_pyx_api.h
*.os
# Byte-compiled / optimized / DLL files
__pycache__/
*.py[cod]
*$py.class
# C extensions
*.a
*.o
*.so
# Distribution / packaging
.Python
build/
develop-eggs/
dist/
downloads/
eggs/
.eggs/
lib/
lib64/
parts/
sdist/
var/
wheels/
share/python-wheels/
*.egg-info/
.installed.cfg
*.egg
MANIFEST
# PyInstaller
# Usually these files are written by a python script from a template
# before PyInstaller builds the exe, so as to inject date/other infos into it.
*.manifest
*.spec
# Installer logs
pip-log.txt
pip-delete-this-directory.txt
# Unit test / coverage reports
htmlcov/
.tox/
.nox/
.coverage
.coverage.*
.cache
nosetests.xml
coverage.xml
*.cover
*.py,cover
.hypothesis/
.pytest_cache/
cover/
# Translations
*.mo
*.pot
# Django stuff:
*.log
local_settings.py
db.sqlite3
db.sqlite3-journal
# Flask stuff:
instance/
.webassets-cache
# Scrapy stuff:
.scrapy
# Sphinx documentation
docs/_build/
# PyBuilder
.pybuilder/
target/
# Jupyter Notebook
.ipynb_checkpoints
# IPython
profile_default/
ipython_config.py
# pyenv
# For a library or package, you might want to ignore these files since the code is
# intended to run in multiple environments; otherwise, check them in:
# .python-version
# pipenv
# According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control.
# However, in case of collaboration, if having platform-specific dependencies or dependencies
# having no cross-platform support, pipenv may install dependencies that don't work, or not
# install all needed dependencies.
#Pipfile.lock
# PEP 582; used by e.g. github.com/David-OConnor/pyflow
__pypackages__/
# Celery stuff
celerybeat-schedule
celerybeat.pid
# SageMath parsed files
*.sage.py
# Environments
.env
.venv
env/
venv/
ENV/
env.bak/
venv.bak/
# Spyder project settings
.spyderproject
.spyproject
# Rope project settings
.ropeproject
# mkdocs documentation
/site
# mypy
.mypy_cache/
.dmypy.json
dmypy.json
# Pyre type checker
.pyre/
# pytype static type analyzer
.pytype/
# Cython debug symbols
cython_debug/

21
artifacts/package_sources/rednose/.pre-commit-config.yaml
View File

@@ -1,21 +0,0 @@
repos:
- repo: https://github.com/pre-commit/pre-commit-hooks
rev: v4.0.1
hooks:
- id: check-ast
- id: check-json
- id: check-xml
- id: check-yaml
- id: check-merge-conflict
- id: check-symlinks
- id: check-executables-have-shebangs
- id: check-shebang-scripts-are-executable
- repo: https://github.com/pre-commit/mirrors-mypy
rev: v1.4.0
hooks:
- id: mypy
additional_dependencies: ['numpy']
- repo: https://github.com/astral-sh/ruff-pre-commit
rev: v0.2.2
hooks:
- id: ruff

14
artifacts/package_sources/rednose/Dockerfile
View File

@@ -1,14 +0,0 @@
FROM ubuntu:24.04
ENV DEBIAN_FRONTEND=noninteractive
RUN apt-get update && apt-get install -y capnproto libcapnp-dev clang wget git autoconf libtool curl make build-essential libssl-dev zlib1g-dev libbz2-dev libreadline-dev libsqlite3-dev llvm libncurses5-dev libncursesw5-dev xz-utils tk-dev libffi-dev liblzma-dev python3-openssl libeigen3-dev python3-pip python3-dev
WORKDIR /project
ENV PYTHONPATH=/project
COPY . .
RUN rm -rf .git
RUN pip3 install --break-system-packages --no-cache-dir -r requirements.txt
RUN python3 setup.py install
RUN scons -c && scons -j$(nproc)

21
artifacts/package_sources/rednose/LICENSE
View File

@@ -1,21 +0,0 @@
MIT License
Copyright (c) 2020 comma.ai
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.

4
artifacts/package_sources/rednose/MANIFEST.in
View File

@@ -1,4 +0,0 @@
include SConstruct
graft rednose
graft site_scons
global-exclude __pycache__ *.pyc *.o *.os *.d

51
artifacts/package_sources/rednose/README.md
View File

@@ -1,51 +0,0 @@
## Introduction
The kalman filter framework described here is an incredibly powerful tool for any optimization problem,
but particularly for visual odometry, sensor fusion localization or SLAM. It is designed to provide very
accurate results, work online or offline, be fairly computationally efficient, be easy to design filters with in
python.
![](examples/kinematic_kf.png)
## Feature walkthrough
### Extended Kalman Filter with symbolic Jacobian computation
Most dynamic systems can be described as a Hidden Markov Process. To estimate the state of such a system with noisy
measurements one can use a Recursive Bayesian estimator. For a linear Markov Process a regular linear Kalman filter is optimal.
Unfortunately, a lot of systems are non-linear. Extended Kalman Filters can model systems by linearizing the non-linear
system at every step, this provides a close to optimal estimator when the linearization is good enough. If the linearization
introduces too much noise, one can use an Iterated Extended Kalman Filter, Unscented Kalman Filter or a Particle Filter. For
most applications those estimators are overkill. They add a lot of complexity and require a lot of additional compute.
Conventionally Extended Kalman Filters are implemented by writing the system's dynamic equations and then manually symbolically
calculating the Jacobians for the linearization. For complex systems this is time consuming and very prone to calculation errors.
This library symbolically computes the Jacobians using sympy to simplify the system's definition and remove the possibility of introducing calculation errors.
### Error State Kalman Filter
3D localization algorithms usually also require estimating orientation of an object in 3D. Orientation is generally represented
with euler angles or quaternions.
Euler angles have several problems, there are multiple ways to represent the same orientation,
gimbal lock can cause the loss of a degree of freedom and lastly their behaviour is very non-linear when errors are large.
Quaternions with one strictly positive dimension don't suffer from these issues, but have another set of problems.
Quaternions need to be normalized otherwise they will grow unbounded, but this cannot be cleanly enforced in a kalman filter.
Most importantly though a quaternion has 4 dimensions, but only represents 3 degrees of freedom, so there is one redundant dimension.
Kalman filters are designed to minimize the error of the system's state. It is possible to have a kalman filter where state and the error of the state are represented in a different space. As long as there is an error function that can compute the error based on the true state and estimated state. It is problematic to have redundant dimensions in the error of the kalman filter, but not in the state. A good compromise then, is to use the quaternion to represent the system's attitude state and use euler angles to describe the error in attitude. This library supports and defining an arbitrary error that is in a different space than the state. [Joan Solà](https://arxiv.org/abs/1711.02508) has written a comprehensive description of using ESKFs for robust 3D orientation estimation.
### Multi-State Constraint Kalman Filter
How do you integrate feature-based visual odometry with a Kalman filter? The problem is that one cannot write an observation equation for 2D feature observations in image space for a localization kalman filter. One needs to give the feature observation a depth so it has a 3D position, then one can write an obvervation equation in the kalman filter. This is possible by tracking the feature across frames and then estimating the depth. However, the solution is not that simple, the depth estimated by tracking the feature across frames depends on the location of the camera at those frames, and thus the state of the kalman filter. This creates a positive feedback loop where the kalman filter wrongly gains confidence in it's position because the feature position updates reinforce it.
The solution is to use an [MSCKF](http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.437.1085&rep=rep1&type=pdf), which this library fully supports.
### Rauch–Tung–Striebel smoothing
When doing offline estimation with a kalman filter there can be an initialization period where states are badly estimated.
Global estimators don't suffer from this, to make our kalman filter competitive with global optimizers we can run the filter
backwards using an RTS smoother. Those combined with potentially multiple forward and backwards passes of the data should make
performance very close to global optimization.
### Mahalanobis distance outlier rejector
A lot of measurements do not come from a Gaussian distribution and as such have outliers that do not fit the statistical model
of the Kalman filter. This can cause a lot of performance issues if not dealt with. This library allows the use of a mahalanobis
distance statistical test on the incoming measurements to deal with this. Note that good initialization is critical to prevent
good measurements from being rejected.

64
artifacts/package_sources/rednose/SConstruct
View File

@@ -1,64 +0,0 @@
import os
import platform
import subprocess
import sysconfig
import numpy as np
arch = subprocess.check_output(["uname", "-m"], encoding='utf8').rstrip()
if platform.system() == "Darwin":
arch = "Darwin"
common = ''
python_path = sysconfig.get_paths()['include']
cpppath = [
'#',
'#rednose',
'#rednose/examples/generated',
'/usr/lib/include',
python_path,
np.get_include(),
]
if platform.system() == "Darwin":
cpppath.append('/opt/homebrew/include')
env = Environment(
ENV=os.environ,
CC='clang',
CXX='clang++',
CCFLAGS=[
"-g",
"-fPIC",
"-O2",
"-Werror=implicit-function-declaration",
"-Werror=incompatible-pointer-types",
"-Werror=int-conversion",
"-Werror=return-type",
"-Werror=format-extra-args",
"-Wshadow",
],
LIBPATH=["#rednose/examples/generated"],
CFLAGS="-std=gnu11",
CXXFLAGS="-std=c++1z",
CPPPATH=cpppath,
REDNOSE_ROOT=Dir("#").abspath,
tools=["default", "cython", "rednose_filter"],
)
# Cython build enviroment
envCython = env.Clone()
envCython["CCFLAGS"] += ["-Wno-#warnings", "-Wno-shadow", "-Wno-deprecated-declarations"]
envCython["LIBS"] = []
if arch == "Darwin":
envCython["LINKFLAGS"] = ["-bundle", "-undefined", "dynamic_lookup"]
elif arch == "aarch64":
envCython["LINKFLAGS"] = ["-shared"]
envCython["LIBS"] = [os.path.basename(python_path)]
else:
envCython["LINKFLAGS"] = ["-pthread", "-shared"]
Export('env', 'envCython', 'common')
SConscript(['#rednose/SConscript'])
SConscript(['#examples/SConscript'])

19
artifacts/package_sources/rednose/examples/SConscript
View File

@@ -1,19 +0,0 @@
Import('env')
gen_dir = Dir('generated/').abspath
env.RednoseCompileFilter(
target="live",
filter_gen_script="live_kf.py",
output_dir=gen_dir,
)
env.RednoseCompileFilter(
target="kinematic",
filter_gen_script="kinematic_kf.py",
output_dir=gen_dir,
)
env.RednoseCompileFilter(
target="compare",
filter_gen_script="test_compare.py",
output_dir=gen_dir,
)

0
artifacts/package_sources/rednose/examples/__init__.py
View File

BIN
artifacts/package_sources/rednose/examples/kinematic_kf.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 184 KiB

81
artifacts/package_sources/rednose/examples/kinematic_kf.py
View File

@@ -1,81 +0,0 @@
#!/usr/bin/env python3
import sys
import numpy as np
import sympy as sp
from rednose.helpers.kalmanfilter import KalmanFilter
if __name__ == '__main__': # generating sympy code
from rednose.helpers.ekf_sym import gen_code
else:
from rednose.helpers.ekf_sym_pyx import EKF_sym_pyx # pylint: disable=no-name-in-module
class ObservationKind():
UNKNOWN = 0
NO_OBSERVATION = 1
POSITION = 1
names = [
'Unknown',
'No observation',
'Position'
]
@classmethod
def to_string(cls, kind):
return cls.names[kind]
class States():
POSITION = slice(0, 1)
VELOCITY = slice(1, 2)
class KinematicKalman(KalmanFilter):
name = 'kinematic'
initial_x = np.array([0.5, 0.0])
# state covariance
initial_P_diag = np.array([1.0**2, 1.0**2])
# process noise
Q = np.diag([0.1**2, 2.0**2])
obs_noise = {ObservationKind.POSITION: np.atleast_2d(0.1**2)}
@staticmethod
def generate_code(generated_dir):
name = KinematicKalman.name
dim_state = KinematicKalman.initial_x.shape[0]
state_sym = sp.MatrixSymbol('state', dim_state, 1)
state = sp.Matrix(state_sym)
position = state[States.POSITION, :][0,:]
velocity = state[States.VELOCITY, :][0,:]
dt = sp.Symbol('dt')
state_dot = sp.Matrix(np.zeros((dim_state, 1)))
state_dot[States.POSITION.start, 0] = velocity
f_sym = state + dt * state_dot
obs_eqs = [
[sp.Matrix([position]), ObservationKind.POSITION, None],
]
gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state)
def __init__(self, generated_dir):
dim_state = self.initial_x.shape[0]
dim_state_err = self.initial_P_diag.shape[0]
# init filter
self.filter = EKF_sym_pyx(generated_dir, self.name, self.Q, self.initial_x, np.diag(self.initial_P_diag), dim_state, dim_state_err)
if __name__ == "__main__":
generated_dir = sys.argv[2]
KinematicKalman.generate_code(generated_dir)

342
artifacts/package_sources/rednose/examples/live_kf.py
View File

@@ -1,342 +0,0 @@
#!/usr/bin/env python3
import sys
import numpy as np
from rednose.helpers import KalmanError
if __name__ == '__main__': # Generating sympy
import sympy as sp
from rednose.helpers.sympy_helpers import euler_rotate, quat_matrix_r, quat_rotate
from rednose.helpers.ekf_sym import gen_code
else:
from rednose.helpers.ekf_sym_pyx import EKF_sym_pyx # pylint: disable=no-name-in-module
EARTH_GM = 3.986005e14 # m^3/s^2 (gravitational constant * mass of earth)
class ObservationKind():
UNKNOWN = 0
NO_OBSERVATION = 1
GPS_NED = 2
ODOMETRIC_SPEED = 3
PHONE_GYRO = 4
GPS_VEL = 5
PSEUDORANGE_GPS = 6
PSEUDORANGE_RATE_GPS = 7
SPEED = 8
NO_ROT = 9
PHONE_ACCEL = 10
ORB_POINT = 11
ECEF_POS = 12
CAMERA_ODO_TRANSLATION = 13
CAMERA_ODO_ROTATION = 14
ORB_FEATURES = 15
MSCKF_TEST = 16
FEATURE_TRACK_TEST = 17
LANE_PT = 18
IMU_FRAME = 19
PSEUDORANGE_GLONASS = 20
PSEUDORANGE_RATE_GLONASS = 21
PSEUDORANGE = 22
PSEUDORANGE_RATE = 23
names = [
'Unknown',
'No observation',
'GPS NED',
'Odometric speed',
'Phone gyro',
'GPS velocity',
'GPS pseudorange',
'GPS pseudorange rate',
'Speed',
'No rotation',
'Phone acceleration',
'ORB point',
'ECEF pos',
'camera odometric translation',
'camera odometric rotation',
'ORB features',
'MSCKF test',
'Feature track test',
'Lane ecef point',
'imu frame eulers',
'GLONASS pseudorange',
'GLONASS pseudorange rate',
]
@classmethod
def to_string(cls, kind):
return cls.names[kind]
class States():
ECEF_POS = slice(0, 3) # x, y and z in ECEF in meters
ECEF_ORIENTATION = slice(3, 7) # quat for pose of phone in ecef
ECEF_VELOCITY = slice(7, 10) # ecef velocity in m/s
ANGULAR_VELOCITY = slice(10, 13) # roll, pitch and yaw rates in device frame in radians/s
GYRO_BIAS = slice(13, 16) # roll, pitch and yaw biases
ODO_SCALE = slice(16, 17) # odometer scale
ACCELERATION = slice(17, 20) # Acceleration in device frame in m/s**2
IMU_OFFSET = slice(20, 23) # imu offset angles in radians
# Error-state has different slices because it is an ESKF
ECEF_POS_ERR = slice(0, 3)
ECEF_ORIENTATION_ERR = slice(3, 6) # euler angles for orientation error
ECEF_VELOCITY_ERR = slice(6, 9)
ANGULAR_VELOCITY_ERR = slice(9, 12)
GYRO_BIAS_ERR = slice(12, 15)
ODO_SCALE_ERR = slice(15, 16)
ACCELERATION_ERR = slice(16, 19)
IMU_OFFSET_ERR = slice(19, 22)
class LiveKalman():
name = 'live'
initial_x = np.array([-2.7e6, 4.2e6, 3.8e6,
1, 0, 0, 0,
0, 0, 0,
0, 0, 0,
0, 0, 0,
1,
0, 0, 0,
0, 0, 0])
# state covariance
initial_P_diag = np.array([10000**2, 10000**2, 10000**2,
10**2, 10**2, 10**2,
10**2, 10**2, 10**2,
1**2, 1**2, 1**2,
0.05**2, 0.05**2, 0.05**2,
0.02**2,
1**2, 1**2, 1**2,
(0.01)**2, (0.01)**2, (0.01)**2])
# process noise
Q = np.diag([0.03**2, 0.03**2, 0.03**2,
0.0**2, 0.0**2, 0.0**2,
0.0**2, 0.0**2, 0.0**2,
0.1**2, 0.1**2, 0.1**2,
(0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2,
(0.02 / 100)**2,
3**2, 3**2, 3**2,
(0.05 / 60)**2, (0.05 / 60)**2, (0.05 / 60)**2])
@staticmethod
def generate_code(generated_dir):
name = LiveKalman.name
dim_state = LiveKalman.initial_x.shape[0]
dim_state_err = LiveKalman.initial_P_diag.shape[0]
state_sym = sp.MatrixSymbol('state', dim_state, 1)
state = sp.Matrix(state_sym)
x, y, z = state[States.ECEF_POS, :]
q = state[States.ECEF_ORIENTATION, :]
v = state[States.ECEF_VELOCITY, :]
vx, vy, vz = v
omega = state[States.ANGULAR_VELOCITY, :]
vroll, vpitch, vyaw = omega
roll_bias, pitch_bias, yaw_bias = state[States.GYRO_BIAS, :]
odo_scale = state[States.ODO_SCALE, :][0,:]
acceleration = state[States.ACCELERATION, :]
imu_angles = state[States.IMU_OFFSET, :]
dt = sp.Symbol('dt')
# calibration and attitude rotation matrices
quat_rot = quat_rotate(*q)
# Got the quat predict equations from here
# A New Quaternion-Based Kalman Filter for
# Real-Time Attitude Estimation Using the Two-Step
# Geometrically-Intuitive Correction Algorithm
A = 0.5 * sp.Matrix([[0, -vroll, -vpitch, -vyaw],
[vroll, 0, vyaw, -vpitch],
[vpitch, -vyaw, 0, vroll],
[vyaw, vpitch, -vroll, 0]])
q_dot = A * q
# Time derivative of the state as a function of state
state_dot = sp.Matrix(np.zeros((dim_state, 1)))
state_dot[States.ECEF_POS, :] = v
state_dot[States.ECEF_ORIENTATION, :] = q_dot
state_dot[States.ECEF_VELOCITY, 0] = quat_rot * acceleration
# Basic descretization, 1st order intergrator
# Can be pretty bad if dt is big
f_sym = state + dt * state_dot
state_err_sym = sp.MatrixSymbol('state_err', dim_state_err, 1)
state_err = sp.Matrix(state_err_sym)
quat_err = state_err[States.ECEF_ORIENTATION_ERR, :]
v_err = state_err[States.ECEF_VELOCITY_ERR, :]
omega_err = state_err[States.ANGULAR_VELOCITY_ERR, :]
acceleration_err = state_err[States.ACCELERATION_ERR, :]
# Time derivative of the state error as a function of state error and state
quat_err_matrix = euler_rotate(quat_err[0], quat_err[1], quat_err[2])
q_err_dot = quat_err_matrix * quat_rot * (omega + omega_err)
state_err_dot = sp.Matrix(np.zeros((dim_state_err, 1)))
state_err_dot[States.ECEF_POS_ERR, :] = v_err
state_err_dot[States.ECEF_ORIENTATION_ERR, :] = q_err_dot
state_err_dot[States.ECEF_VELOCITY_ERR, :] = quat_err_matrix * quat_rot * (acceleration + acceleration_err)
f_err_sym = state_err + dt * state_err_dot
# Observation matrix modifier
H_mod_sym = sp.Matrix(np.zeros((dim_state, dim_state_err)))
H_mod_sym[States.ECEF_POS, States.ECEF_POS_ERR] = np.eye(States.ECEF_POS.stop - States.ECEF_POS.start)
H_mod_sym[States.ECEF_ORIENTATION, States.ECEF_ORIENTATION_ERR] = 0.5 * quat_matrix_r(state[3:7])[:, 1:]
H_mod_sym[States.ECEF_ORIENTATION.stop:, States.ECEF_ORIENTATION_ERR.stop:] = np.eye(dim_state - States.ECEF_ORIENTATION.stop)
# these error functions are defined so that say there
# is a nominal x and true x:
# true x = err_function(nominal x, delta x)
# delta x = inv_err_function(nominal x, true x)
nom_x = sp.MatrixSymbol('nom_x', dim_state, 1)
true_x = sp.MatrixSymbol('true_x', dim_state, 1)
delta_x = sp.MatrixSymbol('delta_x', dim_state_err, 1)
err_function_sym = sp.Matrix(np.zeros((dim_state, 1)))
delta_quat = sp.Matrix(np.ones(4))
delta_quat[1:, :] = sp.Matrix(0.5 * delta_x[States.ECEF_ORIENTATION_ERR, :])
err_function_sym[States.ECEF_POS, :] = sp.Matrix(nom_x[States.ECEF_POS, :] + delta_x[States.ECEF_POS_ERR, :])
err_function_sym[States.ECEF_ORIENTATION, 0] = quat_matrix_r(nom_x[States.ECEF_ORIENTATION, 0]) * delta_quat
err_function_sym[States.ECEF_ORIENTATION.stop:, :] = sp.Matrix(nom_x[States.ECEF_ORIENTATION.stop:, :] + delta_x[States.ECEF_ORIENTATION_ERR.stop:, :])
inv_err_function_sym = sp.Matrix(np.zeros((dim_state_err, 1)))
inv_err_function_sym[States.ECEF_POS_ERR, 0] = sp.Matrix(-nom_x[States.ECEF_POS, 0] + true_x[States.ECEF_POS, 0])
delta_quat = quat_matrix_r(nom_x[States.ECEF_ORIENTATION, 0]).T * true_x[States.ECEF_ORIENTATION, 0]
inv_err_function_sym[States.ECEF_ORIENTATION_ERR, 0] = sp.Matrix(2 * delta_quat[1:])
inv_err_function_sym[States.ECEF_ORIENTATION_ERR.stop:, 0] = sp.Matrix(-nom_x[States.ECEF_ORIENTATION.stop:, 0] + true_x[States.ECEF_ORIENTATION.stop:, 0])
eskf_params = [[err_function_sym, nom_x, delta_x],
[inv_err_function_sym, nom_x, true_x],
H_mod_sym, f_err_sym, state_err_sym]
#
# Observation functions
#
imu_rot = euler_rotate(*imu_angles)
h_gyro_sym = imu_rot * sp.Matrix([vroll + roll_bias,
vpitch + pitch_bias,
vyaw + yaw_bias])
pos = sp.Matrix([x, y, z])
gravity = quat_rot.T * ((EARTH_GM / ((x**2 + y**2 + z**2)**(3.0 / 2.0))) * pos)
h_acc_sym = imu_rot * (gravity + acceleration)
h_phone_rot_sym = sp.Matrix([vroll, vpitch, vyaw])
speed = sp.sqrt(vx**2 + vy**2 + vz**2)
h_speed_sym = sp.Matrix([speed * odo_scale])
h_pos_sym = sp.Matrix([x, y, z])
h_imu_frame_sym = sp.Matrix(imu_angles)
h_relative_motion = sp.Matrix(quat_rot.T * v)
obs_eqs = [[h_speed_sym, ObservationKind.ODOMETRIC_SPEED, None],
[h_gyro_sym, ObservationKind.PHONE_GYRO, None],
[h_phone_rot_sym, ObservationKind.NO_ROT, None],
[h_acc_sym, ObservationKind.PHONE_ACCEL, None],
[h_pos_sym, ObservationKind.ECEF_POS, None],
[h_relative_motion, ObservationKind.CAMERA_ODO_TRANSLATION, None],
[h_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, None],
[h_imu_frame_sym, ObservationKind.IMU_FRAME, None]]
gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state_err, eskf_params)
def __init__(self, generated_dir):
self.dim_state = self.initial_x.shape[0]
self.dim_state_err = self.initial_P_diag.shape[0]
self.obs_noise = {ObservationKind.ODOMETRIC_SPEED: np.atleast_2d(0.2**2),
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_ROTATION: np.diag([0.05**2, 0.05**2, 0.05**2]),
ObservationKind.IMU_FRAME: np.diag([0.05**2, 0.05**2, 0.05**2]),
ObservationKind.NO_ROT: np.diag([0.00025**2, 0.00025**2, 0.00025**2]),
ObservationKind.ECEF_POS: np.diag([5**2, 5**2, 5**2])}
# init filter
self.filter = EKF_sym_pyx(generated_dir, self.name, self.Q, self.initial_x, np.diag(self.initial_P_diag), self.dim_state, self.dim_state_err)
@property
def x(self):
return self.filter.state()
@property
def t(self):
return self.filter.filter_time
@property
def P(self):
return self.filter.covs()
def rts_smooth(self, estimates):
return self.filter.rts_smooth(estimates, norm_quats=True)
def init_state(self, state, covs_diag=None, covs=None, filter_time=None):
if covs_diag is not None:
P = np.diag(covs_diag)
elif covs is not None:
P = covs
else:
P = self.filter.covs()
self.filter.init_state(state, P, filter_time)
def predict_and_observe(self, t, kind, data):
if len(data) > 0:
data = np.atleast_2d(data)
if kind == ObservationKind.CAMERA_ODO_TRANSLATION:
r = self.predict_and_update_odo_trans(data, t, kind)
elif kind == ObservationKind.CAMERA_ODO_ROTATION:
r = self.predict_and_update_odo_rot(data, t, kind)
elif kind == ObservationKind.ODOMETRIC_SPEED:
r = self.predict_and_update_odo_speed(data, t, kind)
else:
r = self.filter.predict_and_update_batch(t, kind, data, self.get_R(kind, len(data)))
# Normalize quats
quat_norm = np.linalg.norm(self.filter.x[3:7, 0])
# Should not continue if the quats behave this weirdly
if not (0.1 < quat_norm < 10):
raise KalmanError("Kalman filter quaternions unstable")
self.filter.x[States.ECEF_ORIENTATION, 0] = self.filter.x[States.ECEF_ORIENTATION, 0] / quat_norm
return r
def get_R(self, kind, n):
obs_noise = self.obs_noise[kind]
dim = obs_noise.shape[0]
R = np.zeros((n, dim, dim))
for i in range(n):
R[i, :, :] = obs_noise
return R
def predict_and_update_odo_speed(self, speed, t, kind):
z = np.array(speed)
R = np.zeros((len(speed), 1, 1))
for i, _ in enumerate(z):
R[i, :, :] = np.diag([0.2**2])
return self.filter.predict_and_update_batch(t, kind, z, R)
def predict_and_update_odo_trans(self, trans, t, kind):
z = trans[:, :3]
R = np.zeros((len(trans), 3, 3))
for i, _ in enumerate(z):
R[i, :, :] = np.diag(trans[i, 3:]**2)
return self.filter.predict_and_update_batch(t, kind, z, R)
def predict_and_update_odo_rot(self, rot, t, kind):
z = rot[:, :3]
R = np.zeros((len(rot), 3, 3))
for i, _ in enumerate(z):
R[i, :, :] = np.diag(rot[i, 3:]**2)
return self.filter.predict_and_update_batch(t, kind, z, R)
if __name__ == "__main__":
generated_dir = sys.argv[2]
LiveKalman.generate_code(generated_dir)

125
artifacts/package_sources/rednose/examples/test_compare.py
View File

@@ -1,125 +0,0 @@
#!/usr/bin/env python3
import pytest
import os
import sys
import sympy as sp
import numpy as np
if __name__ == '__main__': # generating sympy code
from rednose.helpers.ekf_sym import gen_code
else:
from rednose.helpers.ekf_sym_pyx import EKF_sym_pyx # pylint: disable=no-name-in-module
from rednose.helpers.ekf_sym import EKF_sym as EKF_sym2
GENERATED_DIR = os.path.abspath(os.path.join(os.path.dirname(__file__), 'generated'))
class ObservationKind:
UNKNOWN = 0
NO_OBSERVATION = 1
POSITION = 1
names = [
'Unknown',
'No observation',
'Position'
]
@classmethod
def to_string(cls, kind):
return cls.names[kind]
class States:
POSITION = slice(0, 1)
VELOCITY = slice(1, 2)
class CompareFilter:
name = "compare"
initial_x = np.array([0.5, 0.0])
initial_P_diag = np.array([1.0**2, 1.0**2])
Q = np.diag([0.1**2, 2.0**2])
obs_noise = {ObservationKind.POSITION: np.atleast_2d(0.1**2)}
@staticmethod
def generate_code(generated_dir):
name = CompareFilter.name
dim_state = CompareFilter.initial_x.shape[0]
state_sym = sp.MatrixSymbol('state', dim_state, 1)
state = sp.Matrix(state_sym)
position = state[States.POSITION, :][0,:]
velocity = state[States.VELOCITY, :][0,:]
dt = sp.Symbol('dt')
state_dot = sp.Matrix(np.zeros((dim_state, 1)))
state_dot[States.POSITION.start, 0] = velocity
f_sym = state + dt * state_dot
obs_eqs = [
[sp.Matrix([position]), ObservationKind.POSITION, None],
]
gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state)
def __init__(self, generated_dir):
dim_state = self.initial_x.shape[0]
dim_state_err = self.initial_P_diag.shape[0]
# init filter
self.filter_py = EKF_sym_pyx(generated_dir, self.name, self.Q, self.initial_x, np.diag(self.initial_P_diag), dim_state, dim_state_err)
self.filter_pyx = EKF_sym2(generated_dir, self.name, self.Q, self.initial_x, np.diag(self.initial_P_diag), dim_state, dim_state_err)
def get_R(self, kind, n):
obs_noise = self.obs_noise[kind]
dim = obs_noise.shape[0]
R = np.zeros((n, dim, dim))
for i in range(n):
R[i, :, :] = obs_noise
return R
class TestCompare:
def test_compare(self):
np.random.seed(0)
kf = CompareFilter(GENERATED_DIR)
# Simple simulation
dt = 0.01
ts = np.arange(0, 5, step=dt)
xs = np.empty(ts.shape)
# Simulate
x = 0.0
for i, v in enumerate(np.sin(ts * 5)):
xs[i] = x
x += v * dt
# insert late observation
switch = (20, 40)
ts[switch[0]], ts[switch[1]] = ts[switch[1]], ts[switch[0]]
xs[switch[0]], xs[switch[1]] = xs[switch[1]], xs[switch[0]]
for t, x in zip(ts, xs):
# get measurement
meas = np.random.normal(x, 0.1)
z = np.array([[meas]])
R = kf.get_R(ObservationKind.POSITION, 1)
# Update kf
kf.filter_py.predict_and_update_batch(t, ObservationKind.POSITION, z, R)
kf.filter_pyx.predict_and_update_batch(t, ObservationKind.POSITION, z, R)
assert kf.filter_py.get_filter_time() == pytest.approx(kf.filter_pyx.get_filter_time())
assert np.allclose(kf.filter_py.state(), kf.filter_pyx.state())
assert np.allclose(kf.filter_py.covs(), kf.filter_pyx.covs())
if __name__ == "__main__":
generated_dir = sys.argv[2]
CompareFilter.generate_code(generated_dir)

82
artifacts/package_sources/rednose/examples/test_kinematic_kf.py
View File

@@ -1,82 +0,0 @@
import pytest
import os
import numpy as np
from .kinematic_kf import KinematicKalman, ObservationKind, States
GENERATED_DIR = os.path.abspath(os.path.join(os.path.dirname(__file__), 'generated'))
class TestKinematic:
def test_kinematic_kf(self):
np.random.seed(0)
kf = KinematicKalman(GENERATED_DIR)
# Simple simulation
dt = 0.01
ts = np.arange(0, 5, step=dt)
vs = np.sin(ts * 5)
x = 0.0
xs = []
xs_meas = []
xs_kf = []
vs_kf = []
xs_kf_std = []
vs_kf_std = []
for t, v in zip(ts, vs):
xs.append(x)
# Update kf
meas = np.random.normal(x, 0.1)
xs_meas.append(meas)
kf.predict_and_observe(t, ObservationKind.POSITION, [meas])
# Retrieve kf values
state = kf.x
xs_kf.append(float(state[States.POSITION].item()))
vs_kf.append(float(state[States.VELOCITY].item()))
std = np.sqrt(kf.P)
xs_kf_std.append(float(std[States.POSITION, States.POSITION].item()))
vs_kf_std.append(float(std[States.VELOCITY, States.VELOCITY].item()))
# Update simulation
x += v * dt
xs, xs_meas, xs_kf, vs_kf, xs_kf_std, vs_kf_std = (np.asarray(a) for a in (xs, xs_meas, xs_kf, vs_kf, xs_kf_std, vs_kf_std))
assert xs_kf[-1] == pytest.approx(-0.010866289677966417)
assert xs_kf_std[-1] == pytest.approx(0.04477103863330089)
assert vs_kf[-1] == pytest.approx(-0.8553720537261753)
assert vs_kf_std[-1] == pytest.approx(0.6695762270974388)
if "PLOT" in os.environ:
import matplotlib.pyplot as plt # pylint: disable=import-error
plt.figure()
plt.subplot(2, 1, 1)
plt.plot(ts, xs, 'k', label='Simulation')
plt.plot(ts, xs_meas, 'k.', label='Measurements')
plt.plot(ts, xs_kf, label='KF')
ax = plt.gca()
ax.fill_between(ts, xs_kf - xs_kf_std, xs_kf + xs_kf_std, alpha=.2, color='C0')
plt.xlabel("Time [s]")
plt.ylabel("Position [m]")
plt.legend()
plt.subplot(2, 1, 2)
plt.plot(ts, vs, 'k', label='Simulation')
plt.plot(ts, vs_kf, label='KF')
ax = plt.gca()
ax.fill_between(ts, vs_kf - vs_kf_std, vs_kf + vs_kf_std, alpha=.2, color='C0')
plt.xlabel("Time [s]")
plt.ylabel("Velocity [m/s]")
plt.legend()
plt.show()

37
artifacts/package_sources/rednose/pyproject.toml
View File

@@ -1,37 +0,0 @@
[project]
name = "rednose"
version = "0.0.1"
description = "Kalman filter library"
requires-python = ">=3.11,<3.13"
license = "MIT"
dependencies = ["numpy", "cffi", "sympy"]
[project.optional-dependencies]
dev = ["scipy"]
[build-system]
requires = ["setuptools>=64", "Cython", "scons", "numpy"]
build-backend = "setuptools.build_meta"
[tool.setuptools.packages.find]
include = ["rednose", "rednose.*"]
[tool.setuptools.package-data]
rednose = ["helpers/*.h", "helpers/*.a", "helpers/*.so", "helpers/*.dylib", "helpers/chi2_lookup_table.npy", "templates/*", "site_scons/site_tools/*.py"]
# https://beta.ruff.rs/docs/configuration/#using-pyprojecttoml
[tool.ruff]
line-length = 160
target-version="py311"
[tool.ruff.lint]
select = ["E", "F", "W", "PIE", "C4", "ISC", "RUF100", "A"]
ignore = ["W292", "E741", "E402", "C408", "ISC003"]
flake8-implicit-str-concat.allow-multiline=false
[tool.ruff.lint.flake8-tidy-imports.banned-api]
"pytest.main".msg = "pytest.main requires special handling that is easy to mess up!"
"unittest".msg = "Use pytest"
[tool.pytest.ini_options]
addopts = "--durations=10 -n auto"

5
artifacts/package_sources/rednose/rednose/.gitignore vendored
View File

@@ -1,5 +0,0 @@
# Cython intermediates
*_pyx.cpp
*_pyx.h
*_pyx_api.h
*.os

17
artifacts/package_sources/rednose/rednose/SConscript
View File

@@ -1,17 +0,0 @@
Import('env', 'envCython', 'common')
cc_sources = [
"helpers/ekf_load.cc",
"helpers/ekf_sym.cc",
]
libs = ["dl"]
if common != "":
# for SWAGLOG support
libs += [common, 'zmq']
ekf_objects = env.SharedObject(cc_sources)
rednose = env.Library("helpers/ekf_sym", ekf_objects, LIBS=libs)
rednose_python = envCython.Program("helpers/ekf_sym_pyx.so", ["helpers/ekf_sym_pyx.pyx", ekf_objects],
LIBS=libs + envCython["LIBS"])
Export('rednose', 'rednose_python')

6
artifacts/package_sources/rednose/rednose/__init__.py
View File

@@ -1,6 +0,0 @@
import os
INCLUDE_PATH = os.path.abspath(os.path.join(os.path.dirname(__file__), ".."))
LIB_PATH = os.path.join(os.path.dirname(__file__), "helpers", "libekf_sym.a")
SCONS_TOOL_PATH = os.path.join(os.path.dirname(__file__), "site_scons", "site_tools")

35
artifacts/package_sources/rednose/rednose/helpers/__init__.py
View File

@@ -1,35 +0,0 @@
import os
import platform
from cffi import FFI
TEMPLATE_DIR = os.path.abspath(os.path.join(os.path.dirname(__file__), '..', 'templates'))
def write_code(folder, name, code, header):
if not os.path.exists(folder):
os.mkdir(folder)
with open(os.path.join(folder, f"{name}.cpp"), 'w', encoding='utf-8') as f:
f.write(code)
with open(os.path.join(folder, f"{name}.h"), 'w', encoding='utf-8') as f:
f.write(header)
def load_code(folder, name):
shared_ext = "dylib" if platform.system() == "Darwin" else "so"
shared_fn = os.path.join(folder, f"lib{name}.{shared_ext}")
header_fn = os.path.join(folder, f"{name}.h")
with open(header_fn, encoding='utf-8') as f:
header = f.read()
# is the only thing that can be parsed by cffi
header = "\n".join([line for line in header.split("\n") if line.startswith("void ")])
ffi = FFI()
ffi.cdef(header)
return (ffi, ffi.dlopen(shared_fn))
class KalmanError(Exception):
pass

22
artifacts/package_sources/rednose/rednose/helpers/chi2_lookup.py
View File

@@ -1,22 +0,0 @@
import os
import numpy as np
def gen_chi2_ppf_lookup(max_dim=200):
from scipy.stats import chi2
table = np.zeros((max_dim, 98))
for dim in range(1, max_dim):
table[dim] = chi2.ppf(np.arange(.01, .99, .01), dim)
np.save('chi2_lookup_table', table)
def chi2_ppf(p, dim):
table = np.load(os.path.dirname(os.path.realpath(__file__)) + '/chi2_lookup_table.npy')
result = np.interp(p, np.arange(.01, .99, .01), table[dim])
return result
if __name__ == "__main__":
gen_chi2_ppf_lookup()

BIN
artifacts/package_sources/rednose/rednose/helpers/chi2_lookup_table.npy
View File

Binary file not shown.

42
artifacts/package_sources/rednose/rednose/helpers/ekf.h
View File

@@ -1,42 +0,0 @@
#pragma once
#include <iostream>
#include <cassert>
#include <string>
#include <vector>
#include <deque>
#include <unordered_map>
#include <map>
#include <cmath>
#include <eigen3/Eigen/Dense>
typedef void (*extra_routine_t)(double *, double *);
struct EKF {
std::string name;
std::vector<int> kinds;
std::vector<int> feature_kinds;
void (*f_fun)(double *, double, double *);
void (*F_fun)(double *, double, double *);
void (*err_fun)(double *, double *, double *);
void (*inv_err_fun)(double *, double *, double *);
void (*H_mod_fun)(double *, double *);
void (*predict)(double *, double *, double *, double);
std::unordered_map<int, void (*)(double *, double *, double *)> hs = {};
std::unordered_map<int, void (*)(double *, double *, double *)> Hs = {};
std::unordered_map<int, void (*)(double *, double *, double *, double *, double *)> updates = {};
std::unordered_map<int, void (*)(double *, double *, double *)> Hes = {};
std::unordered_map<std::string, void (*)(double)> sets = {};
std::unordered_map<std::string, extra_routine_t> extra_routines = {};
};
#define ekf_lib_init(ekf) \
extern "C" void* ekf_get() { \
return (void*) &ekf; \
} \
extern void __attribute__((weak)) ekf_register(const EKF* ptr); \
static void __attribute__((constructor)) do_ekf_init_ ## ekf(void) { \
if (ekf_register) ekf_register(&ekf); \
}

39
artifacts/package_sources/rednose/rednose/helpers/ekf_load.cc
View File

@@ -1,39 +0,0 @@
#include "ekf_load.h"
#include <dlfcn.h>
std::vector<const EKF*>& ekf_get_all() {
static std::vector<const EKF*> vec;
return vec;
}
void ekf_register(const EKF* ekf) {
ekf_get_all().push_back(ekf);
}
const EKF* ekf_lookup(const std::string& ekf_name) {
for (const auto& ekfi : ekf_get_all()) {
if (ekf_name == ekfi->name) {
return ekfi;
}
}
return NULL;
}
void ekf_load_and_register(const std::string& ekf_directory, const std::string& ekf_name) {
if (ekf_lookup(ekf_name)) {
return;
}
#ifdef __APPLE__
std::string dylib_ext = ".dylib";
#else
std::string dylib_ext = ".so";
#endif
std::string ekf_path = ekf_directory + "/lib" + ekf_name + dylib_ext;
void* handle = dlopen(ekf_path.c_str(), RTLD_NOW);
assert(handle);
void* (*ekf_get)() = (void*(*)())dlsym(handle, "ekf_get");
assert(ekf_get != NULL);
const EKF* ekf = (const EKF*)ekf_get();
ekf_register(ekf);
}

9
artifacts/package_sources/rednose/rednose/helpers/ekf_load.h
View File

@@ -1,9 +0,0 @@
#include <vector>
#include <string>
#include "ekf.h"
std::vector<const EKF*>& ekf_get_all();
const EKF* ekf_lookup(const std::string& ekf_name);
void ekf_register(const EKF* ekf);
void ekf_load_and_register(const std::string& ekf_directory, const std::string& ekf_name);

223
artifacts/package_sources/rednose/rednose/helpers/ekf_sym.cc
View File

@@ -1,223 +0,0 @@
#include "ekf_sym.h"
#include "logger/logger.h"
using namespace EKFS;
using namespace Eigen;
EKFSym::EKFSym(std::string name, Map<MatrixXdr> Q, Map<VectorXd> x_initial, Map<MatrixXdr> P_initial, int dim_main,
int dim_main_err, int N, int dim_augment, int dim_augment_err, std::vector<int> maha_test_kinds,
std::vector<int> quaternion_idxs, std::vector<std::string> global_vars, double max_rewind_age)
{
// TODO: add logger
this->ekf = ekf_lookup(name);
assert(this->ekf);
this->msckf = N > 0;
this->N = N;
this->dim_augment = dim_augment;
this->dim_augment_err = dim_augment_err;
this->dim_main = dim_main;
this->dim_main_err = dim_main_err;
this->dim_x = x_initial.rows();
this->dim_err = P_initial.rows();
assert(dim_main + dim_augment * N == dim_x);
assert(dim_main_err + dim_augment_err * N == this->dim_err);
assert(Q.rows() == P_initial.rows() && Q.cols() == P_initial.cols());
// kinds that should get mahalanobis distance
// tested for outlier rejection
this->maha_test_kinds = maha_test_kinds;
// quaternions need normalization
this->quaternion_idxs = quaternion_idxs;
this->global_vars = global_vars;
// Process noise
this->Q = Q;
this->max_rewind_age = max_rewind_age;
this->init_state(x_initial, P_initial, NAN);
}
void EKFSym::init_state(Map<VectorXd> state, Map<MatrixXdr> covs, double init_filter_time) {
this->x = state;
this->P = covs;
this->filter_time = init_filter_time;
this->augment_times = VectorXd::Zero(this->N);
this->reset_rewind();
}
VectorXd EKFSym::state() {
return this->x;
}
MatrixXdr EKFSym::covs() {
return this->P;
}
void EKFSym::set_filter_time(double t) {
this->filter_time = t;
}
double EKFSym::get_filter_time() {
return this->filter_time;
}
void EKFSym::normalize_quaternions() {
for(std::size_t i = 0; i < this->quaternion_idxs.size(); ++i) {
this->normalize_slice(this->quaternion_idxs[i], this->quaternion_idxs[i] + 4);
}
}
void EKFSym::normalize_slice(int slice_start, int slice_end_ex) {
this->x.block(slice_start, 0, slice_end_ex - slice_start, this->x.cols()).normalize();
}
void EKFSym::set_global(std::string global_var, double val) {
this->ekf->sets.at(global_var)(val);
}
std::optional<Estimate> EKFSym::predict_and_update_batch(double t, int kind, std::vector<Map<VectorXd>> z_map,
std::vector<Map<MatrixXdr>> R_map, std::vector<std::vector<double>> extra_args, bool augment)
{
// TODO handle rewinding at this level
std::deque<Observation> rewound;
if (!std::isnan(this->filter_time) && t < this->filter_time) {
if (this->rewind_t.empty() || t < this->rewind_t.front() || t < this->rewind_t.back() - this->max_rewind_age) {
LOGD("observation too old at %f with filter at %f, ignoring!", t, this->filter_time);
return std::nullopt;
}
rewound = this->rewind(t);
}
Observation obs;
obs.t = t;
obs.kind = kind;
obs.extra_args = extra_args;
for (Map<VectorXd> zi : z_map) {
obs.z.push_back(zi);
}
for (Map<MatrixXdr> Ri : R_map) {
obs.R.push_back(Ri);
}
std::optional<Estimate> res = std::make_optional(this->predict_and_update_batch(obs, augment));
// optional fast forward
while (!rewound.empty()) {
this->predict_and_update_batch(rewound.front(), false);
rewound.pop_front();
}
return res;
}
void EKFSym::reset_rewind() {
this->rewind_obscache.clear();
this->rewind_t.clear();
this->rewind_states.clear();
}
std::deque<Observation> EKFSym::rewind(double t) {
std::deque<Observation> rewound;
// rewind observations until t is after previous observation
while (this->rewind_t.back() > t) {
rewound.push_front(this->rewind_obscache.back());
this->rewind_t.pop_back();
this->rewind_states.pop_back();
this->rewind_obscache.pop_back();
}
// set the state to the time right before that
this->filter_time = this->rewind_t.back();
this->x = this->rewind_states.back().first;
this->P = this->rewind_states.back().second;
return rewound;
}
void EKFSym::checkpoint(Observation& obs) {
// push to rewinder
this->rewind_t.push_back(this->filter_time);
this->rewind_states.push_back(std::make_pair(this->x, this->P));
this->rewind_obscache.push_back(obs);
// only keep a certain number around
if (this->rewind_t.size() > REWIND_TO_KEEP) {
this->rewind_t.pop_front();
this->rewind_states.pop_front();
this->rewind_obscache.pop_front();
}
}
Estimate EKFSym::predict_and_update_batch(Observation& obs, bool augment) {
assert(obs.z.size() == obs.R.size());
assert(obs.z.size() == obs.extra_args.size());
this->predict(obs.t);
Estimate res;
res.t = obs.t;
res.kind = obs.kind;
res.z = obs.z;
res.extra_args = obs.extra_args;
res.xk1 = this->x;
res.Pk1 = this->P;
// update batch
std::vector<VectorXd> y;
for (int i = 0; i < obs.z.size(); i++) {
assert(obs.z[i].rows() == obs.R[i].rows());
assert(obs.z[i].rows() == obs.R[i].cols());
// update state
y.push_back(this->update(obs.kind, obs.z[i], obs.R[i], obs.extra_args[i]));
}
res.xk = this->x;
res.Pk = this->P;
res.y = y;
assert(!augment); // TODO
// if (augment) {
// this->augment();
// }
this->checkpoint(obs);
return res;
}
void EKFSym::predict(double t) {
// initialize time
if (std::isnan(this->filter_time)) {
this->filter_time = t;
}
// predict
double dt = t - this->filter_time;
assert(dt >= 0.0);
this->ekf->predict(this->x.data(), this->P.data(), this->Q.data(), dt);
this->normalize_quaternions();
this->filter_time = t;
}
VectorXd EKFSym::update(int kind, VectorXd z, MatrixXdr R, std::vector<double> extra_args) {
this->ekf->updates.at(kind)(this->x.data(), this->P.data(), z.data(), R.data(), extra_args.data());
this->normalize_quaternions();
if (this->msckf && std::find(this->feature_track_kinds.begin(), this->feature_track_kinds.end(), kind) != this->feature_track_kinds.end()) {
return z.head(z.rows() - extra_args.size());
}
return z;
}
extra_routine_t EKFSym::get_extra_routine(const std::string& routine) {
return this->ekf->extra_routines.at(routine);
}

113
artifacts/package_sources/rednose/rednose/helpers/ekf_sym.h
View File

@@ -1,113 +0,0 @@
#pragma once
#include <iostream>
#include <cassert>
#include <string>
#include <vector>
#include <deque>
#include <unordered_map>
#include <map>
#include <cmath>
#include <optional>
#include <eigen3/Eigen/Dense>
#include "ekf.h"
#include "ekf_load.h"
#define REWIND_TO_KEEP 512
namespace EKFS {
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> MatrixXdr;
typedef struct Observation {
double t;
int kind;
std::vector<Eigen::VectorXd> z;
std::vector<MatrixXdr> R;
std::vector<std::vector<double>> extra_args;
} Observation;
typedef struct Estimate {
Eigen::VectorXd xk1;
Eigen::VectorXd xk;
MatrixXdr Pk1;
MatrixXdr Pk;
double t;
int kind;
std::vector<Eigen::VectorXd> y;
std::vector<Eigen::VectorXd> z;
std::vector<std::vector<double>> extra_args;
} Estimate;
class EKFSym {
public:
EKFSym(std::string name, Eigen::Map<MatrixXdr> Q, Eigen::Map<Eigen::VectorXd> x_initial,
Eigen::Map<MatrixXdr> P_initial, int dim_main, int dim_main_err, int N = 0, int dim_augment = 0,
int dim_augment_err = 0, std::vector<int> maha_test_kinds = std::vector<int>(),
std::vector<int> quaternion_idxs = std::vector<int>(),
std::vector<std::string> global_vars = std::vector<std::string>(), double max_rewind_age = 1.0);
void init_state(Eigen::Map<Eigen::VectorXd> state, Eigen::Map<MatrixXdr> covs, double filter_time);
Eigen::VectorXd state();
MatrixXdr covs();
void set_filter_time(double t);
double get_filter_time();
void normalize_quaternions();
void normalize_slice(int slice_start, int slice_end_ex);
void set_global(std::string global_var, double val);
void reset_rewind();
void predict(double t);
std::optional<Estimate> predict_and_update_batch(double t, int kind, std::vector<Eigen::Map<Eigen::VectorXd>> z,
std::vector<Eigen::Map<MatrixXdr>> R, std::vector<std::vector<double>> extra_args = {{}}, bool augment = false);
extra_routine_t get_extra_routine(const std::string& routine);
private:
std::deque<Observation> rewind(double t);
void checkpoint(Observation& obs);
Estimate predict_and_update_batch(Observation& obs, bool augment);
Eigen::VectorXd update(int kind, Eigen::VectorXd z, MatrixXdr R, std::vector<double> extra_args);
// stuct with linked sympy generated functions
const EKF *ekf = NULL;
Eigen::VectorXd x; // state
MatrixXdr P; // covs
bool msckf;
int N;
int dim_augment;
int dim_augment_err;
int dim_main;
int dim_main_err;
// state
int dim_x;
int dim_err;
double filter_time;
std::vector<int> maha_test_kinds;
std::vector<int> quaternion_idxs;
std::vector<std::string> global_vars;
// process noise
MatrixXdr Q;
// rewind stuff
double max_rewind_age;
std::deque<double> rewind_t;
std::deque<std::pair<Eigen::VectorXd, MatrixXdr>> rewind_states;
std::deque<Observation> rewind_obscache;
Eigen::VectorXd augment_times;
std::vector<int> feature_track_kinds;
};
}

690
artifacts/package_sources/rednose/rednose/helpers/ekf_sym.py
View File

@@ -1,690 +0,0 @@
import os
import logging
from bisect import bisect_right
import numpy as np
import sympy as sp
from numpy import dot
from rednose.helpers.sympy_helpers import sympy_into_c
from rednose.helpers import TEMPLATE_DIR, load_code
from rednose.helpers.chi2_lookup import chi2_ppf
def solve(a, b):
if a.shape[0] == 1 and a.shape[1] == 1:
return b / a[0][0]
else:
return np.linalg.solve(a, b)
def null(H, eps=1e-12):
_, s, vh = np.linalg.svd(H)
padding = max(0, np.shape(H)[1] - np.shape(s)[0])
null_mask = np.concatenate(((s <= eps), np.ones((padding,), dtype=bool)), axis=0)
null_space = np.compress(null_mask, vh, axis=0)
return np.transpose(null_space)
def gen_code(folder, name, f_sym, dt_sym, x_sym, obs_eqs, dim_x, dim_err, eskf_params=None, msckf_params=None, # pylint: disable=dangerous-default-value
maha_test_kinds=[], quaternion_idxs=[], global_vars=None, extra_routines=[]):
# optional state transition matrix, H modifier
# and err_function if an error-state kalman filter (ESKF)
# is desired. Best described in "Quaternion kinematics
# for the error-state Kalman filter" by Joan Sola
if eskf_params:
err_eqs = eskf_params[0]
inv_err_eqs = eskf_params[1]
H_mod_sym = eskf_params[2]
f_err_sym = eskf_params[3]
x_err_sym = eskf_params[4]
else:
nom_x = sp.MatrixSymbol('nom_x', dim_x, 1)
true_x = sp.MatrixSymbol('true_x', dim_x, 1)
delta_x = sp.MatrixSymbol('delta_x', dim_x, 1)
err_function_sym = sp.Matrix(nom_x + delta_x)
inv_err_function_sym = sp.Matrix(true_x - nom_x)
err_eqs = [err_function_sym, nom_x, delta_x]
inv_err_eqs = [inv_err_function_sym, nom_x, true_x]
H_mod_sym = sp.Matrix(np.eye(dim_x))
f_err_sym = f_sym
x_err_sym = x_sym
# This configures the multi-state augmentation
# needed for EKF-SLAM with MSCKF (Mourikis et al 2007)
if msckf_params:
msckf = True
dim_main = msckf_params[0] # size of the main state
dim_augment = msckf_params[1] # size of one augment state chunk
dim_main_err = msckf_params[2]
dim_augment_err = msckf_params[3]
N = msckf_params[4]
feature_track_kinds = msckf_params[5]
assert dim_main + dim_augment * N == dim_x
assert dim_main_err + dim_augment_err * N == dim_err
else:
msckf = False
dim_main = dim_x
dim_augment = 0
dim_main_err = dim_err
dim_augment_err = 0
N = 0
# linearize with jacobians
F_sym = f_err_sym.jacobian(x_err_sym)
if eskf_params:
for sym in x_err_sym:
F_sym = F_sym.subs(sym, 0)
assert dt_sym in F_sym.free_symbols
for i in range(len(obs_eqs)):
obs_eqs[i].append(obs_eqs[i][0].jacobian(x_sym))
if msckf and obs_eqs[i][1] in feature_track_kinds:
obs_eqs[i].append(obs_eqs[i][0].jacobian(obs_eqs[i][2]))
else:
obs_eqs[i].append(None)
# collect sympy functions
sympy_functions = []
# extra routines
sympy_functions += extra_routines
# error functions
sympy_functions.append(('err_fun', err_eqs[0], [err_eqs[1], err_eqs[2]]))
sympy_functions.append(('inv_err_fun', inv_err_eqs[0], [inv_err_eqs[1], inv_err_eqs[2]]))
# H modifier for ESKF updates
sympy_functions.append(('H_mod_fun', H_mod_sym, [x_sym]))
# state propagation function
sympy_functions.append(('f_fun', f_sym, [x_sym, dt_sym]))
sympy_functions.append(('F_fun', F_sym, [x_sym, dt_sym]))
# observation functions
for h_sym, kind, ea_sym, H_sym, He_sym in obs_eqs:
sympy_functions.append(('h_%d' % kind, h_sym, [x_sym, ea_sym]))
sympy_functions.append(('H_%d' % kind, H_sym, [x_sym, ea_sym]))
if msckf and kind in feature_track_kinds:
sympy_functions.append(('He_%d' % kind, He_sym, [x_sym, ea_sym]))
# Generate and wrap all th c code
sympy_header, code = sympy_into_c(sympy_functions, global_vars)
header = "#pragma once\n"
header += "#include \"rednose/helpers/ekf.h\"\n"
header += "extern \"C\" {\n"
pre_code = f"#include \"{name}.h\"\n"
pre_code += "\nnamespace {\n"
pre_code += "#define DIM %d\n" % dim_x
pre_code += "#define EDIM %d\n" % dim_err
pre_code += "#define MEDIM %d\n" % dim_main_err
pre_code += "typedef void (*Hfun)(double *, double *, double *);\n"
if global_vars is not None:
for var in global_vars:
pre_code += f"\ndouble {var.name};\n"
pre_code += f"\nvoid set_{var.name}(double x){{ {var.name} = x;}}\n"
post_code = "\n}\n" # namespace
post_code += "extern \"C\" {\n\n"
for h_sym, kind, ea_sym, H_sym, He_sym in obs_eqs:
if msckf and kind in feature_track_kinds:
He_str = 'He_%d' % kind
# ea_dim = ea_sym.shape[0]
else:
He_str = 'NULL'
# ea_dim = 1 # not really dim of ea but makes c function work
maha_thresh = chi2_ppf(0.95, int(h_sym.shape[0])) # mahalanobis distance for outlier detection
maha_test = kind in maha_test_kinds
pre_code += f"const static double MAHA_THRESH_{kind} = {maha_thresh};\n"
header += f"void {name}_update_{kind}(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);\n"
post_code += f"void {name}_update_{kind}(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) {{\n"
post_code += f" update<{h_sym.shape[0]}, 3, {int(maha_test)}>(in_x, in_P, h_{kind}, H_{kind}, {He_str}, in_z, in_R, in_ea, MAHA_THRESH_{kind});\n"
post_code += "}\n"
# For ffi loading of specific functions
for line in sympy_header.split("\n"):
if line.startswith("void "): # sympy functions
func_call = line[5: line.index(')') + 1]
header += f"void {name}_{func_call};\n"
post_code += f"void {name}_{func_call} {{\n"
post_code += f" {func_call.replace('double *', '').replace('double', '')};\n"
post_code += "}\n"
header += f"void {name}_predict(double *in_x, double *in_P, double *in_Q, double dt);\n"
post_code += f"void {name}_predict(double *in_x, double *in_P, double *in_Q, double dt) {{\n"
post_code += " predict(in_x, in_P, in_Q, dt);\n"
post_code += "}\n"
if global_vars is not None:
for var in global_vars:
header += f"void {name}_set_{var.name}(double x);\n"
post_code += f"void {name}_set_{var.name}(double x) {{\n"
post_code += f" set_{var.name}(x);\n"
post_code += "}\n"
post_code += "}\n\n" # extern c
funcs = ['f_fun', 'F_fun', 'err_fun', 'inv_err_fun', 'H_mod_fun', 'predict']
func_lists = {
'h': [kind for _, kind, _, _, _ in obs_eqs],
'H': [kind for _, kind, _, _, _ in obs_eqs],
'update': [kind for _, kind, _, _, _ in obs_eqs],
'He': [kind for _, kind, _, _, _ in obs_eqs if msckf and kind in feature_track_kinds],
'set': [var.name for var in global_vars] if global_vars is not None else [],
}
func_extra = [x[0] for x in extra_routines]
# For dynamic loading of specific functions
post_code += f"const EKF {name} = {{\n"
post_code += f" .name = \"{name}\",\n"
post_code += f" .kinds = {{ {', '.join([str(kind) for _, kind, _, _, _ in obs_eqs])} }},\n"
post_code += f" .feature_kinds = {{ {', '.join([str(kind) for _, kind, _, _, _ in obs_eqs if msckf and kind in feature_track_kinds])} }},\n"
for func in funcs:
post_code += f" .{func} = {name}_{func},\n"
for group, kinds in func_lists.items():
post_code += f" .{group}s = {{\n"
for kind in kinds:
str_kind = f"\"{kind}\"" if isinstance(kind, str) else kind
post_code += f" {{ {str_kind}, {name}_{group}_{kind} }},\n"
post_code += " },\n"
post_code += " .extra_routines = {\n"
for f in func_extra:
post_code += f" {{ \"{f}\", {name}_{f} }},\n"
post_code += " },\n"
post_code += "};\n\n"
post_code += f"ekf_lib_init({name})\n"
# merge code blocks
header += "}"
with open(os.path.join(TEMPLATE_DIR, "ekf_c.c"), encoding='utf-8') as f:
code = "\n".join([pre_code, code, f.read(), post_code])
# write to file
if not os.path.exists(folder):
os.mkdir(folder)
with open(os.path.join(folder, f"{name}.h"), 'w', encoding='utf-8') as f:
f.write(header) # header is used for ffi import
with open(os.path.join(folder, f"{name}.cpp"), 'w', encoding='utf-8') as f:
f.write(code)
class EKF_sym():
def __init__(self, folder, name, Q, x_initial, P_initial, dim_main, dim_main_err, # pylint: disable=dangerous-default-value
N=0, dim_augment=0, dim_augment_err=0, maha_test_kinds=[], quaternion_idxs=[], global_vars=None, max_rewind_age=1.0, logger=logging):
"""Generates process function and all observation functions for the kalman filter."""
self.msckf = N > 0
self.N = N
self.dim_augment = dim_augment
self.dim_augment_err = dim_augment_err
self.dim_main = dim_main
self.dim_main_err = dim_main_err
self.logger = logger
# state
x_initial = x_initial.reshape((-1, 1))
self.dim_x = x_initial.shape[0]
self.dim_err = P_initial.shape[0]
assert dim_main + dim_augment * N == self.dim_x
assert dim_main_err + dim_augment_err * N == self.dim_err
assert Q.shape == P_initial.shape
# kinds that should get mahalanobis distance
# tested for outlier rejection
self.maha_test_kinds = maha_test_kinds
# quaternions need normalization
self.quaternion_idxs = quaternion_idxs
# process noise
self.Q = Q
# rewind stuff
self.max_rewind_age = max_rewind_age
self.rewind_t = []
self.rewind_states = []
self.rewind_obscache = []
self.init_state(x_initial, P_initial, None)
ffi, lib = load_code(folder, name)
kinds, self.feature_track_kinds = [], []
for func in dir(lib):
if func[:len(name) + 3] == f'{name}_h_':
kinds.append(int(func[len(name) + 3:]))
if func[:len(name) + 4] == f'{name}_He_':
self.feature_track_kinds.append(int(func[len(name) + 4:]))
# wrap all the sympy functions
def wrap_1lists(func_name):
func = eval(f"lib.{name}_{func_name}", {"lib": lib}) # pylint: disable=eval-used
def ret(lst1, out):
func(ffi.cast("double *", lst1.ctypes.data),
ffi.cast("double *", out.ctypes.data))
return ret
def wrap_2lists(func_name):
func = eval(f"lib.{name}_{func_name}", {"lib": lib}) # pylint: disable=eval-used
def ret(lst1, lst2, out):
func(ffi.cast("double *", lst1.ctypes.data),
ffi.cast("double *", lst2.ctypes.data),
ffi.cast("double *", out.ctypes.data))
return ret
def wrap_1list_1float(func_name):
func = eval(f"lib.{name}_{func_name}", {"lib": lib}) # pylint: disable=eval-used
def ret(lst1, fl, out):
func(ffi.cast("double *", lst1.ctypes.data),
ffi.cast("double", fl),
ffi.cast("double *", out.ctypes.data))
return ret
self.f = wrap_1list_1float("f_fun")
self.F = wrap_1list_1float("F_fun")
self.err_function = wrap_2lists("err_fun")
self.inv_err_function = wrap_2lists("inv_err_fun")
self.H_mod = wrap_1lists("H_mod_fun")
self.hs, self.Hs, self.Hes = {}, {}, {}
for kind in kinds:
self.hs[kind] = wrap_2lists(f"h_{kind}")
self.Hs[kind] = wrap_2lists(f"H_{kind}")
if self.msckf and kind in self.feature_track_kinds:
self.Hes[kind] = wrap_2lists(f"He_{kind}")
self.set_globals = {}
if global_vars is not None:
for global_var in global_vars:
self.set_globals[global_var] = getattr(lib, f"{name}_set_{global_var}")
# wrap the C++ predict function
def _predict_blas(x, P, dt):
func = eval(f"lib.{name}_predict", {"lib": lib}) # pylint: disable=eval-used
func(ffi.cast("double *", x.ctypes.data),
ffi.cast("double *", P.ctypes.data),
ffi.cast("double *", self.Q.ctypes.data),
ffi.cast("double", dt))
return x, P
# wrap the C++ update function
def fun_wrapper(f, kind):
f = eval(f"lib.{name}_{f}", {"lib": lib}) # pylint: disable=eval-used
def _update_inner_blas(x, P, z, R, extra_args):
f(ffi.cast("double *", x.ctypes.data),
ffi.cast("double *", P.ctypes.data),
ffi.cast("double *", z.ctypes.data),
ffi.cast("double *", R.ctypes.data),
ffi.cast("double *", extra_args.ctypes.data))
if self.msckf and kind in self.feature_track_kinds:
y = z[:-len(extra_args)]
else:
y = z
return x, P, y
return _update_inner_blas
self._updates = {}
for kind in kinds:
self._updates[kind] = fun_wrapper("update_%d" % kind, kind)
def _update_blas(x, P, kind, z, R, extra_args=[]): # pylint: disable=dangerous-default-value
return self._updates[kind](x, P, z, R, extra_args)
# assign the functions
self._predict = _predict_blas
# self._predict = self._predict_python
self._update = _update_blas
# self._update = self._update_python
def init_state(self, state, covs, filter_time):
self.x = np.array(state.reshape((-1, 1))).astype(np.float64)
self.P = np.array(covs).astype(np.float64)
self.filter_time = filter_time
self.augment_times = [0] * self.N
self.rewind_obscache = []
self.rewind_t = []
self.rewind_states = []
def reset_rewind(self):
self.rewind_obscache = []
self.rewind_t = []
self.rewind_states = []
def augment(self):
# TODO this is not a generalized way of doing this and implies that the augmented states
# are simply the first (dim_augment_state) elements of the main state.
assert self.msckf
d1 = self.dim_main
d2 = self.dim_main_err
d3 = self.dim_augment
d4 = self.dim_augment_err
# push through augmented states
self.x[d1:-d3] = self.x[d1 + d3:]
self.x[-d3:] = self.x[:d3]
assert self.x.shape == (self.dim_x, 1)
# push through augmented covs
assert self.P.shape == (self.dim_err, self.dim_err)
P_reduced = self.P
P_reduced = np.delete(P_reduced, np.s_[d2:d2 + d4], axis=1)
P_reduced = np.delete(P_reduced, np.s_[d2:d2 + d4], axis=0)
assert P_reduced.shape == (self.dim_err - d4, self.dim_err - d4)
to_mult = np.zeros((self.dim_err, self.dim_err - d4))
to_mult[:-d4, :] = np.eye(self.dim_err - d4)
to_mult[-d4:, :d4] = np.eye(d4)
self.P = to_mult.dot(P_reduced.dot(to_mult.T))
self.augment_times = self.augment_times[1:]
self.augment_times.append(self.filter_time)
assert self.P.shape == (self.dim_err, self.dim_err)
def state(self):
return np.array(self.x).flatten()
def covs(self):
return self.P
def set_filter_time(self, t):
self.filter_time = t
def get_filter_time(self):
return self.filter_time
def normalize_quaternions(self):
for idx in self.quaternion_idxs:
self.normalize_slice(idx, idx+4)
def normalize_slice(self, slice_start, slice_end_ex):
self.x[slice_start:slice_end_ex] /= np.linalg.norm(self.x[slice_start:slice_end_ex])
def get_augment_times(self):
return self.augment_times
def set_global(self, global_var, val):
self.set_globals[global_var](val)
def rewind(self, t):
# find where we are rewinding to
idx = bisect_right(self.rewind_t, t)
assert self.rewind_t[idx - 1] <= t
assert self.rewind_t[idx] > t # must be true, or rewind wouldn't be called
# set the state to the time right before that
self.filter_time = self.rewind_t[idx - 1]
self.x[:] = self.rewind_states[idx - 1][0]
self.P[:] = self.rewind_states[idx - 1][1]
# return the observations we rewound over for fast forwarding
ret = self.rewind_obscache[idx:]
# throw away the old future
# TODO: is this making a copy?
self.rewind_t = self.rewind_t[:idx]
self.rewind_states = self.rewind_states[:idx]
self.rewind_obscache = self.rewind_obscache[:idx]
return ret
def checkpoint(self, obs):
# push to rewinder
self.rewind_t.append(self.filter_time)
self.rewind_states.append((np.copy(self.x), np.copy(self.P)))
self.rewind_obscache.append(obs)
# only keep a certain number around
REWIND_TO_KEEP = 512
self.rewind_t = self.rewind_t[-REWIND_TO_KEEP:]
self.rewind_states = self.rewind_states[-REWIND_TO_KEEP:]
self.rewind_obscache = self.rewind_obscache[-REWIND_TO_KEEP:]
def predict(self, t):
# initialize time
if self.filter_time is None:
self.filter_time = t
# predict
dt = t - self.filter_time
assert dt >= 0
self.x, self.P = self._predict(self.x, self.P, dt)
self.normalize_quaternions()
self.filter_time = t
def predict_and_update_batch(self, t, kind, z, R, extra_args=[[]], augment=False): # pylint: disable=dangerous-default-value
# TODO handle rewinding at this level"
# rewind
if self.filter_time is not None and t < self.filter_time:
if len(self.rewind_t) == 0 or t < self.rewind_t[0] or t < self.rewind_t[-1] - self.max_rewind_age:
self.logger.error(f"observation too old at {t:.3f} with filter at {self.filter_time:.3f}, ignoring")
return None
rewound = self.rewind(t)
else:
rewound = []
ret = self._predict_and_update_batch(t, kind, z, R, extra_args, augment)
# optional fast forward
for r in rewound:
self._predict_and_update_batch(*r)
return ret
def _predict_and_update_batch(self, t, kind, z, R, extra_args, augment=False):
"""The main kalman filter function
Predicts the state and then updates a batch of observations
dim_x: dimensionality of the state space
dim_z: dimensionality of the observation and depends on kind
n: number of observations
Args:
t (float): Time of observation
kind (int): Type of observation
z (vec [n,dim_z]): Measurements
R (mat [n,dim_z, dim_z]): Measurement Noise
extra_args (list, [n]): Values used in H computations
"""
assert z.shape[0] == R.shape[0]
assert z.shape[1] == R.shape[1]
assert z.shape[1] == R.shape[2]
# initialize time
if self.filter_time is None:
self.filter_time = t
# predict
dt = t - self.filter_time
assert dt >= 0
self.x, self.P = self._predict(self.x, self.P, dt)
self.filter_time = t
xk_km1, Pk_km1 = np.copy(self.x).flatten(), np.copy(self.P)
# update batch
y = []
for i in range(len(z)):
# these are from the user, so we canonicalize them
z_i = np.array(z[i], dtype=np.float64, order='F')
R_i = np.array(R[i], dtype=np.float64, order='F')
extra_args_i = np.array(extra_args[i], dtype=np.float64, order='F')
# update
self.x, self.P, y_i = self._update(self.x, self.P, kind, z_i, R_i, extra_args=extra_args_i)
self.normalize_quaternions()
y.append(y_i)
xk_k, Pk_k = np.copy(self.x).flatten(), np.copy(self.P)
if augment:
self.augment()
# checkpoint
self.checkpoint((t, kind, z, R, extra_args))
return xk_km1, xk_k, Pk_km1, Pk_k, t, kind, y, z, extra_args
def _predict_python(self, x, P, dt):
x_new = np.zeros(x.shape, dtype=np.float64)
self.f(x, dt, x_new)
F = np.zeros(P.shape, dtype=np.float64)
self.F(x, dt, F)
if not self.msckf:
P = dot(dot(F, P), F.T)
else:
# Update the predicted state covariance:
# Pk+1|k = |F*Pii*FT + Q*dt F*Pij |
# |PijT*FT Pjj |
# Where F is the jacobian of the main state
# predict function, Pii is the main state's
# covariance and Q its process noise. Pij
# is the covariance between the augmented
# states and the main state.
#
d2 = self.dim_main_err # known at compile time
F_curr = F[:d2, :d2]
P[:d2, :d2] = (F_curr.dot(P[:d2, :d2])).dot(F_curr.T)
P[:d2, d2:] = F_curr.dot(P[:d2, d2:])
P[d2:, :d2] = P[d2:, :d2].dot(F_curr.T)
P += dt * self.Q
return x_new, P
def _update_python(self, x, P, kind, z, R, extra_args=[]): # pylint: disable=dangerous-default-value
# init vars
z = z.reshape((-1, 1))
h = np.zeros(z.shape, dtype=np.float64)
H = np.zeros((z.shape[0], self.dim_x), dtype=np.float64)
# C functions
self.hs[kind](x, extra_args, h)
self.Hs[kind](x, extra_args, H)
# y is the "loss"
y = z - h
# *** same above this line ***
if self.msckf and kind in self.Hes:
# Do some algebraic magic to decorrelate
He = np.zeros((z.shape[0], len(extra_args)), dtype=np.float64)
self.Hes[kind](x, extra_args, He)
# TODO: Don't call a function here, do projection locally
A = null(He.T)
y = A.T.dot(y)
H = A.T.dot(H)
R = A.T.dot(R.dot(A))
# TODO If nullspace isn't the dimension we want
if A.shape[1] + He.shape[1] != A.shape[0]:
self.logger.warning('Warning: null space projection failed, measurement ignored')
return x, P, np.zeros(A.shape[0] - He.shape[1])
# if using eskf
H_mod = np.zeros((x.shape[0], P.shape[0]), dtype=np.float64)
self.H_mod(x, H_mod)
H = H.dot(H_mod)
# Do mahalobis distance test
# currently just runs on msckf observations
# could run on anything if needed
if self.msckf and kind in self.maha_test_kinds:
a = np.linalg.inv(H.dot(P).dot(H.T) + R)
maha_dist = y.T.dot(a.dot(y))
if maha_dist > chi2_ppf(0.95, y.shape[0]):
R = 10e16 * R
# *** same below this line ***
# Outlier resilient weighting as described in:
# "A Kalman Filter for Robust Outlier Detection - Jo-Anne Ting, ..."
weight = 1 # (1.5)/(1 + np.sum(y**2)/np.sum(R))
S = dot(dot(H, P), H.T) + R / weight
K = solve(S, dot(H, P.T)).T
I_KH = np.eye(P.shape[0]) - dot(K, H)
# update actual state
delta_x = dot(K, y)
P = dot(dot(I_KH, P), I_KH.T) + dot(dot(K, R), K.T)
# inject observed error into state
x_new = np.zeros(x.shape, dtype=np.float64)
self.err_function(x, delta_x, x_new)
return x_new, P, y.flatten()
def maha_test(self, x, P, kind, z, R, extra_args=[], maha_thresh=0.95): # pylint: disable=dangerous-default-value
# init vars
z = z.reshape((-1, 1))
h = np.zeros(z.shape, dtype=np.float64)
H = np.zeros((z.shape[0], self.dim_x), dtype=np.float64)
# C functions
self.hs[kind](x, extra_args, h)
self.Hs[kind](x, extra_args, H)
# y is the "loss"
y = z - h
# if using eskf
H_mod = np.zeros((x.shape[0], P.shape[0]), dtype=np.float64)
self.H_mod(x, H_mod)
H = H.dot(H_mod)
a = np.linalg.inv(H.dot(P).dot(H.T) + R)
maha_dist = y.T.dot(a.dot(y))
if maha_dist > chi2_ppf(maha_thresh, y.shape[0]):
return False
else:
return True
def rts_smooth(self, estimates, norm_quats=False):
'''
Returns rts smoothed results of
kalman filter estimates
If the kalman state is augmented with
old states only the main state is smoothed
'''
xk_n = estimates[-1][0]
Pk_n = estimates[-1][2]
Fk_1 = np.zeros(Pk_n.shape, dtype=np.float64)
states_smoothed = [xk_n]
covs_smoothed = [Pk_n]
for k in range(len(estimates) - 2, -1, -1):
xk1_n = xk_n
if norm_quats:
xk1_n[3:7] /= np.linalg.norm(xk1_n[3:7])
Pk1_n = Pk_n
xk1_k, _, Pk1_k, _, t2, _, _, _, _ = estimates[k + 1]
_, xk_k, _, Pk_k, t1, _, _, _, _ = estimates[k]
dt = t2 - t1
self.F(xk_k, dt, Fk_1)
d1 = self.dim_main
d2 = self.dim_main_err
Ck = np.linalg.solve(Pk1_k[:d2, :d2], Fk_1[:d2, :d2].dot(Pk_k[:d2, :d2].T)).T
xk_n = xk_k
delta_x = np.zeros((Pk_n.shape[0], 1), dtype=np.float64)
self.inv_err_function(xk1_k, xk1_n, delta_x)
delta_x[:d2] = Ck.dot(delta_x[:d2])
x_new = np.zeros((xk_n.shape[0], 1), dtype=np.float64)
self.err_function(xk_k, delta_x, x_new)
xk_n[:d1] = x_new[:d1, 0]
Pk_n = Pk_k
Pk_n[:d2, :d2] = Pk_k[:d2, :d2] + Ck.dot(Pk1_n[:d2, :d2] - Pk1_k[:d2, :d2]).dot(Ck.T)
states_smoothed.append(xk_n)
covs_smoothed.append(Pk_n)
return np.flipud(np.vstack(states_smoothed)), np.stack(covs_smoothed, 0)[::-1]

195
artifacts/package_sources/rednose/rednose/helpers/ekf_sym_pyx.pyx
View File

@@ -1,195 +0,0 @@
# cython: language_level=3
# cython: profile=True
# distutils: language = c++
cimport cython
from libcpp.string cimport string
from libcpp.vector cimport vector
from libcpp cimport bool
cimport numpy as np
import numpy as np
cdef extern from "<optional>" namespace "std" nogil:
cdef cppclass optional[T]:
ctypedef T value_type
bool has_value()
T& value()
cdef extern from "rednose/helpers/ekf_load.h":
cdef void ekf_load_and_register(string directory, string name)
cdef extern from "rednose/helpers/ekf_sym.h" namespace "EKFS":
cdef cppclass MapVectorXd "Eigen::Map<Eigen::VectorXd>":
MapVectorXd(double*, int)
cdef cppclass MapMatrixXdr "Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> >":
MapMatrixXdr(double*, int, int)
cdef cppclass VectorXd "Eigen::VectorXd":
VectorXd()
double* data()
int rows()
cdef cppclass MatrixXdr "Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>":
MatrixXdr()
double* data()
int rows()
int cols()
ctypedef struct Estimate:
VectorXd xk1
VectorXd xk
MatrixXdr Pk1
MatrixXdr Pk
double t
int kind
vector[VectorXd] y
vector[VectorXd] z
vector[vector[double]] extra_args
cdef cppclass EKFSym:
EKFSym(string name, MapMatrixXdr Q, MapVectorXd x_initial, MapMatrixXdr P_initial, int dim_main,
int dim_main_err, int N, int dim_augment, int dim_augment_err, vector[int] maha_test_kinds,
vector[int] quaternion_idxs, vector[string] global_vars, double max_rewind_age)
void init_state(MapVectorXd state, MapMatrixXdr covs, double filter_time)
VectorXd state()
MatrixXdr covs()
void set_filter_time(double t)
double get_filter_time()
void set_global(string name, double val)
void reset_rewind()
void predict(double t)
optional[Estimate] predict_and_update_batch(double t, int kind, vector[MapVectorXd] z, vector[MapMatrixXdr] z,
vector[vector[double]] extra_args, bool augment)
# Functions like `numpy_to_matrix` are not possible, cython requires default
# constructor for return variable types which aren't available with Eigen::Map
@cython.wraparound(False)
@cython.boundscheck(False)
cdef np.ndarray[np.float64_t, ndim=2, mode="c"] matrix_to_numpy(MatrixXdr arr):
cdef double[:,:] mem_view = <double[:arr.rows(),:arr.cols()]>arr.data()
return np.copy(np.asarray(mem_view, dtype=np.double, order="C"))
@cython.wraparound(False)
@cython.boundscheck(False)
cdef np.ndarray[np.float64_t, ndim=1, mode="c"] vector_to_numpy(VectorXd arr):
cdef double[:] mem_view = <double[:arr.rows()]>arr.data()
return np.copy(np.asarray(mem_view, dtype=np.double, order="C"))
cdef class EKF_sym_pyx:
cdef EKFSym* ekf
def __cinit__(self, str gen_dir, str name, np.ndarray[np.float64_t, ndim=2] Q,
np.ndarray[np.float64_t, ndim=1] x_initial, np.ndarray[np.float64_t, ndim=2] P_initial, int dim_main,
int dim_main_err, int N=0, int dim_augment=0, int dim_augment_err=0, list maha_test_kinds=[],
list quaternion_idxs=[], list global_vars=[], double max_rewind_age=1.0, logger=None):
# TODO logger
ekf_load_and_register(gen_dir.encode('utf8'), name.encode('utf8'))
cdef np.ndarray[np.float64_t, ndim=2, mode='c'] Q_b = np.ascontiguousarray(Q, dtype=np.double)
cdef np.ndarray[np.float64_t, ndim=1, mode='c'] x_initial_b = np.ascontiguousarray(x_initial, dtype=np.double)
cdef np.ndarray[np.float64_t, ndim=2, mode='c'] P_initial_b = np.ascontiguousarray(P_initial, dtype=np.double)
self.ekf = new EKFSym(
name.encode('utf8'),
MapMatrixXdr(<double*> Q_b.data, Q.shape[0], Q.shape[1]),
MapVectorXd(<double*> x_initial_b.data, x_initial.shape[0]),
MapMatrixXdr(<double*> P_initial_b.data, P_initial.shape[0], P_initial.shape[1]),
dim_main,
dim_main_err,
N,
dim_augment,
dim_augment_err,
maha_test_kinds,
quaternion_idxs,
[x.encode('utf8') for x in global_vars],
max_rewind_age
)
def init_state(self, np.ndarray[np.float64_t, ndim=1] state, np.ndarray[np.float64_t, ndim=2] covs, filter_time):
cdef np.ndarray[np.float64_t, ndim=1, mode='c'] state_b = np.ascontiguousarray(state, dtype=np.double)
cdef np.ndarray[np.float64_t, ndim=2, mode='c'] covs_b = np.ascontiguousarray(covs, dtype=np.double)
self.ekf.init_state(
MapVectorXd(<double*> state_b.data, state.shape[0]),
MapMatrixXdr(<double*> covs_b.data, covs.shape[0], covs.shape[1]),
np.nan if filter_time is None else filter_time
)
def state(self):
cdef np.ndarray res = vector_to_numpy(self.ekf.state())
return res
def covs(self):
return matrix_to_numpy(self.ekf.covs())
def set_filter_time(self, double t):
self.ekf.set_filter_time(t)
def get_filter_time(self):
return self.ekf.get_filter_time()
def set_global(self, str global_var, double val):
self.ekf.set_global(global_var.encode('utf8'), val)
def reset_rewind(self):
self.ekf.reset_rewind()
def predict(self, double t):
self.ekf.predict(t)
def predict_and_update_batch(self, double t, int kind, z, R, extra_args=[[]], bool augment=False):
cdef vector[MapVectorXd] z_map
cdef np.ndarray[np.float64_t, ndim=1, mode='c'] zi_b
for zi in z:
zi_b = np.ascontiguousarray(zi, dtype=np.double)
z_map.push_back(MapVectorXd(<double*> zi_b.data, zi.shape[0]))
cdef vector[MapMatrixXdr] R_map
cdef np.ndarray[np.float64_t, ndim=2, mode='c'] Ri_b
for Ri in R:
Ri_b = np.ascontiguousarray(Ri, dtype=np.double)
R_map.push_back(MapMatrixXdr(<double*> Ri_b.data, Ri.shape[0], Ri.shape[1]))
cdef vector[vector[double]] extra_args_map
cdef vector[double] args_map
for args in extra_args:
args_map.clear()
for a in args:
args_map.push_back(a)
extra_args_map.push_back(args_map)
cdef optional[Estimate] res = self.ekf.predict_and_update_batch(t, kind, z_map, R_map, extra_args_map, augment)
if not res.has_value():
return None
cdef VectorXd tmpvec
return (
vector_to_numpy(res.value().xk1),
vector_to_numpy(res.value().xk),
matrix_to_numpy(res.value().Pk1),
matrix_to_numpy(res.value().Pk),
res.value().t,
res.value().kind,
[vector_to_numpy(tmpvec) for tmpvec in res.value().y],
z, # TODO: take return values?
extra_args,
)
def augment(self):
raise NotImplementedError() # TODO
def get_augment_times(self):
raise NotImplementedError() # TODO
def rts_smooth(self, estimates, norm_quats=False):
raise NotImplementedError() # TODO
def maha_test(self, x, P, kind, z, R, extra_args=[], maha_thresh=0.95):
raise NotImplementedError() # TODO
def __dealloc__(self):
del self.ekf

52
artifacts/package_sources/rednose/rednose/helpers/kalmanfilter.py
View File

@@ -1,52 +0,0 @@
from typing import Any
import numpy as np
class KalmanFilter:
name = "<name>"
initial_x = np.zeros((0, 0))
initial_P_diag = np.zeros((0, 0))
Q = np.zeros((0, 0))
obs_noise: dict[int, Any] = {}
# Should be initialized when initializating a KalmanFilter implementation
filter = None
@property
def x(self):
return self.filter.state()
@property
def t(self):
return self.filter.get_filter_time()
@property
def P(self):
return self.filter.covs()
def init_state(self, state, covs_diag=None, covs=None, filter_time=None):
if covs_diag is not None:
P = np.diag(covs_diag)
elif covs is not None:
P = covs
else:
P = self.filter.covs()
self.filter.init_state(state, P, filter_time)
def get_R(self, kind, n):
obs_noise = self.obs_noise[kind]
dim = obs_noise.shape[0]
R = np.zeros((n, dim, dim))
for i in range(n):
R[i, :, :] = obs_noise
return R
def predict_and_observe(self, t, kind, data, R=None):
if len(data) > 0:
data = np.atleast_2d(data)
if R is None:
R = self.get_R(kind, len(data))
self.filter.predict_and_update_batch(t, kind, data, R)

162
artifacts/package_sources/rednose/rednose/helpers/sympy_helpers.py
View File

@@ -1,162 +0,0 @@
import sympy as sp
import numpy as np
# TODO: remove code duplication between openpilot.common.orientation
def quat2rot(quats):
quats = np.array(quats)
input_shape = quats.shape
quats = np.atleast_2d(quats)
Rs = np.zeros((quats.shape[0], 3, 3))
q0 = quats[:, 0]
q1 = quats[:, 1]
q2 = quats[:, 2]
q3 = quats[:, 3]
Rs[:, 0, 0] = q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3
Rs[:, 0, 1] = 2 * (q1 * q2 - q0 * q3)
Rs[:, 0, 2] = 2 * (q0 * q2 + q1 * q3)
Rs[:, 1, 0] = 2 * (q1 * q2 + q0 * q3)
Rs[:, 1, 1] = q0 * q0 - q1 * q1 + q2 * q2 - q3 * q3
Rs[:, 1, 2] = 2 * (q2 * q3 - q0 * q1)
Rs[:, 2, 0] = 2 * (q1 * q3 - q0 * q2)
Rs[:, 2, 1] = 2 * (q0 * q1 + q2 * q3)
Rs[:, 2, 2] = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3
if len(input_shape) < 2:
return Rs[0]
else:
return Rs
def euler2quat(eulers):
eulers = np.array(eulers)
if len(eulers.shape) > 1:
output_shape = (-1,4)
else:
output_shape = (4,)
eulers = np.atleast_2d(eulers)
gamma, theta, psi = eulers[:,0], eulers[:,1], eulers[:,2]
q0 = np.cos(gamma / 2) * np.cos(theta / 2) * np.cos(psi / 2) + \
np.sin(gamma / 2) * np.sin(theta / 2) * np.sin(psi / 2)
q1 = np.sin(gamma / 2) * np.cos(theta / 2) * np.cos(psi / 2) - \
np.cos(gamma / 2) * np.sin(theta / 2) * np.sin(psi / 2)
q2 = np.cos(gamma / 2) * np.sin(theta / 2) * np.cos(psi / 2) + \
np.sin(gamma / 2) * np.cos(theta / 2) * np.sin(psi / 2)
q3 = np.cos(gamma / 2) * np.cos(theta / 2) * np.sin(psi / 2) - \
np.sin(gamma / 2) * np.sin(theta / 2) * np.cos(psi / 2)
quats = np.array([q0, q1, q2, q3]).T
for i in range(len(quats)):
if quats[i,0] < 0: # pylint: disable=unsubscriptable-object
quats[i] = -quats[i] # pylint: disable=unsupported-assignment-operation,unsubscriptable-object
return quats.reshape(output_shape)
def euler2rot(eulers):
return quat2rot(euler2quat(eulers))
rotations_from_quats = quat2rot
def cross(x):
ret = sp.Matrix(np.zeros((3, 3)))
ret[0, 1], ret[0, 2] = -x[2], x[1]
ret[1, 0], ret[1, 2] = x[2], -x[0]
ret[2, 0], ret[2, 1] = -x[1], x[0]
return ret
def rot_to_euler(R):
gamma = sp.atan2(R[2, 1], R[2, 2])
theta = sp.asin(-R[2, 0])
psi = sp.atan2(R[1, 0], R[0, 0])
return sp.Matrix([gamma, theta, psi])
def rot_matrix(roll, pitch, yaw):
cr, sr = np.cos(roll), np.sin(roll)
cp, sp = np.cos(pitch), np.sin(pitch)
cy, sy = np.cos(yaw), np.sin(yaw)
rr = np.array([[1,0,0],[0, cr,-sr],[0, sr, cr]])
rp = np.array([[cp,0,sp],[0, 1,0],[-sp, 0, cp]])
ry = np.array([[cy,-sy,0],[sy, cy,0],[0, 0, 1]])
return ry.dot(rp.dot(rr))
def euler_rotate(roll, pitch, yaw):
# make symbolic rotation matrix from eulers
matrix_roll = sp.Matrix([[1, 0, 0],
[0, sp.cos(roll), -sp.sin(roll)],
[0, sp.sin(roll), sp.cos(roll)]])
matrix_pitch = sp.Matrix([[sp.cos(pitch), 0, sp.sin(pitch)],
[0, 1, 0],
[-sp.sin(pitch), 0, sp.cos(pitch)]])
matrix_yaw = sp.Matrix([[sp.cos(yaw), -sp.sin(yaw), 0],
[sp.sin(yaw), sp.cos(yaw), 0],
[0, 0, 1]])
return matrix_yaw * matrix_pitch * matrix_roll
def quat_rotate(q0, q1, q2, q3):
# make symbolic rotation matrix from quat
return sp.Matrix([[q0**2 + q1**2 - q2**2 - q3**2, 2 * (q1 * q2 + q0 * q3), 2 * (q1 * q3 - q0 * q2)],
[2 * (q1 * q2 - q0 * q3), q0**2 - q1**2 + q2**2 - q3**2, 2 * (q2 * q3 + q0 * q1)],
[2 * (q1 * q3 + q0 * q2), 2 * (q2 * q3 - q0 * q1), q0**2 - q1**2 - q2**2 + q3**2]]).T
def quat_matrix_l(p):
return sp.Matrix([[p[0], -p[1], -p[2], -p[3]],
[p[1], p[0], -p[3], p[2]],
[p[2], p[3], p[0], -p[1]],
[p[3], -p[2], p[1], p[0]]])
def quat_matrix_r(p):
return sp.Matrix([[p[0], -p[1], -p[2], -p[3]],
[p[1], p[0], p[3], -p[2]],
[p[2], -p[3], p[0], p[1]],
[p[3], p[2], -p[1], p[0]]])
def sympy_into_c(sympy_functions, global_vars=None):
from sympy.utilities import codegen
routines = []
for name, expr, args in sympy_functions:
r = codegen.make_routine(name, expr, language="C99", global_vars=global_vars)
# argument ordering input to sympy is broken with function with output arguments
nargs = []
# reorder the input arguments
for aa in args:
if aa is None:
nargs.append(codegen.InputArgument(sp.Symbol('unused'), dimensions=[1, 1]))
continue
found = False
for a in r.arguments:
if str(aa.name) == str(a.name):
nargs.append(a)
found = True
break
if not found:
# [1,1] is a hack for Matrices
nargs.append(codegen.InputArgument(aa, dimensions=[1, 1]))
# add the output arguments
for a in r.arguments:
if type(a) == codegen.OutputArgument:
nargs.append(a)
# assert len(r.arguments) == len(args)+1
r.arguments = nargs
# add routine to list
routines.append(r)
[(_, c_code), (_, c_header)] = codegen.get_code_generator('C', 'ekf', 'C99').write(routines, "ekf")
c_header = '\n'.join(x for x in c_header.split("\n") if len(x) > 0 and x[0] != '#')
c_code = '\n'.join(x for x in c_code.split("\n") if len(x) > 0 and x[0] != '#')
return c_header, c_code

20
artifacts/package_sources/rednose/rednose/logger/logger.h
View File

@@ -1,20 +0,0 @@
#pragma once
#ifdef SWAGLOG
#include SWAGLOG
#else
#define CLOUDLOG_DEBUG 10
#define CLOUDLOG_INFO 20
#define CLOUDLOG_WARNING 30
#define CLOUDLOG_ERROR 40
#define CLOUDLOG_CRITICAL 50
#define cloudlog(lvl, fmt, ...) printf(fmt "\n", ## __VA_ARGS__)
#define LOGD(fmt, ...) cloudlog(CLOUDLOG_DEBUG, fmt, ## __VA_ARGS__)
#define LOG(fmt, ...) cloudlog(CLOUDLOG_INFO, fmt, ## __VA_ARGS__)
#define LOGW(fmt, ...) cloudlog(CLOUDLOG_WARNING, fmt, ## __VA_ARGS__)
#define LOGE(fmt, ...) cloudlog(CLOUDLOG_ERROR, fmt, ## __VA_ARGS__)
#endif

0
artifacts/package_sources/rednose/rednose/site_scons/__init__.py
View File

0
artifacts/package_sources/rednose/rednose/site_scons/site_tools/__init__.py
View File

40
artifacts/package_sources/rednose/rednose/site_scons/site_tools/rednose_filter.py
View File

@@ -1,40 +0,0 @@
import platform
from SCons.Script import Dir, File
def compile_single_filter(env, target, filter_gen_script, output_dir, extra_gen_artifacts, script_deps):
generated_src_files = [File(f) for f in [f'{output_dir}/{target}.cpp', f'{output_dir}/{target}.h']]
extra_generated_files = [File(f'{output_dir}/{x}') for x in extra_gen_artifacts]
generator_file = File(filter_gen_script)
action = f"{File(generator_file).relpath} {target} {Dir(output_dir).relpath}"
if hasattr(env, 'PrettyAction'):
action = env.PrettyAction(action, 'GEN')
env.Command(generated_src_files + extra_generated_files, [generator_file] + script_deps, action)
return File(generated_src_files[:1])
class BaseRednoseCompileMethod:
def __init__(self, base_py_deps, base_cc_deps):
self.base_py_deps = base_py_deps
self.base_cc_deps = base_cc_deps
class CompileFilterMethod(BaseRednoseCompileMethod):
def __call__(self, env, target, filter_gen_script, output_dir, extra_gen_artifacts=[], gen_script_deps=[]):
objects = compile_single_filter(env, target, filter_gen_script, output_dir, extra_gen_artifacts, self.base_py_deps + gen_script_deps)
linker_flags = env.get("LINKFLAGS", [])
if platform.system() == "Darwin":
linker_flags = ["-undefined", "dynamic_lookup"]
return env.SharedLibrary(f'{output_dir}/{target}', [self.base_cc_deps, objects], LINKFLAGS=linker_flags)
def generate(env):
templates = env.Glob("$REDNOSE_ROOT/rednose/templates/*")
sympy_helpers = env.File("$REDNOSE_ROOT/rednose/helpers/sympy_helpers.py")
ekf_sym = env.File("$REDNOSE_ROOT/rednose/helpers/ekf_sym.py")
env.AddMethod(CompileFilterMethod(templates + [sympy_helpers, ekf_sym], []), "RednoseCompileFilter")
def exists(env):
return True

52
artifacts/package_sources/rednose/rednose/templates/compute_pos.c
View File

@@ -1,52 +0,0 @@
#include <eigen3/Eigen/QR>
#include <eigen3/Eigen/Dense>
#include <iostream>
typedef Eigen::Matrix<double, KDIM*2, 3, Eigen::RowMajor> R3M;
typedef Eigen::Matrix<double, KDIM*2, 1> R1M;
typedef Eigen::Matrix<double, 3, 1> O1M;
typedef Eigen::Matrix<double, 3, 3, Eigen::RowMajor> M3D;
void gauss_newton(double *in_x, double *in_poses, double *in_img_positions) {
double res[KDIM*2] = {0};
double jac[KDIM*6] = {0};
O1M x(in_x);
O1M delta;
int counter = 0;
while ((delta.squaredNorm() > 0.0001 and counter < 30) or counter == 0){
res_fun(in_x, in_poses, in_img_positions, res);
jac_fun(in_x, in_poses, in_img_positions, jac);
R1M E(res); R3M J(jac);
delta = (J.transpose()*J).inverse() * J.transpose() * E;
x = x - delta;
memcpy(in_x, x.data(), 3 * sizeof(double));
counter = counter + 1;
}
}
void compute_pos(double *to_c, double *poses, double *img_positions, double *param, double *pos) {
param[0] = img_positions[KDIM*2-2];
param[1] = img_positions[KDIM*2-1];
param[2] = 0.1;
gauss_newton(param, poses, img_positions);
Eigen::Quaterniond q;
q.w() = poses[KDIM*7-4];
q.x() = poses[KDIM*7-3];
q.y() = poses[KDIM*7-2];
q.z() = poses[KDIM*7-1];
M3D RC(to_c);
Eigen::Matrix3d R = q.normalized().toRotationMatrix();
Eigen::Matrix3d rot = R * RC.transpose();
pos[0] = param[0]/param[2];
pos[1] = param[1]/param[2];
pos[2] = 1.0/param[2];
O1M ecef_offset(poses + KDIM*7-7);
O1M ecef_output(pos);
ecef_output = rot*ecef_output + ecef_offset;
memcpy(pos, ecef_output.data(), 3 * sizeof(double));
}

123
artifacts/package_sources/rednose/rednose/templates/ekf_c.c
View File

@@ -1,123 +0,0 @@
#include <eigen3/Eigen/Dense>
#include <iostream>
typedef Eigen::Matrix<double, DIM, DIM, Eigen::RowMajor> DDM;
typedef Eigen::Matrix<double, EDIM, EDIM, Eigen::RowMajor> EEM;
typedef Eigen::Matrix<double, DIM, EDIM, Eigen::RowMajor> DEM;
void predict(double *in_x, double *in_P, double *in_Q, double dt) {
typedef Eigen::Matrix<double, MEDIM, MEDIM, Eigen::RowMajor> RRM;
double nx[DIM] = {0};
double in_F[EDIM*EDIM] = {0};
// functions from sympy
f_fun(in_x, dt, nx);
F_fun(in_x, dt, in_F);
EEM F(in_F);
EEM P(in_P);
EEM Q(in_Q);
RRM F_main = F.topLeftCorner(MEDIM, MEDIM);
P.topLeftCorner(MEDIM, MEDIM) = (F_main * P.topLeftCorner(MEDIM, MEDIM)) * F_main.transpose();
P.topRightCorner(MEDIM, EDIM - MEDIM) = F_main * P.topRightCorner(MEDIM, EDIM - MEDIM);
P.bottomLeftCorner(EDIM - MEDIM, MEDIM) = P.bottomLeftCorner(EDIM - MEDIM, MEDIM) * F_main.transpose();
P = P + dt*Q;
// copy out state
memcpy(in_x, nx, DIM * sizeof(double));
memcpy(in_P, P.data(), EDIM * EDIM * sizeof(double));
}
// note: extra_args dim only correct when null space projecting
// otherwise 1
template <int ZDIM, int EADIM, bool MAHA_TEST>
void update(double *in_x, double *in_P, Hfun h_fun, Hfun H_fun, Hfun Hea_fun, double *in_z, double *in_R, double *in_ea, double MAHA_THRESHOLD) {
typedef Eigen::Matrix<double, ZDIM, ZDIM, Eigen::RowMajor> ZZM;
typedef Eigen::Matrix<double, ZDIM, DIM, Eigen::RowMajor> ZDM;
typedef Eigen::Matrix<double, Eigen::Dynamic, EDIM, Eigen::RowMajor> XEM;
//typedef Eigen::Matrix<double, EDIM, ZDIM, Eigen::RowMajor> EZM;
typedef Eigen::Matrix<double, Eigen::Dynamic, 1> X1M;
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> XXM;
double in_hx[ZDIM] = {0};
double in_H[ZDIM * DIM] = {0};
double in_H_mod[EDIM * DIM] = {0};
double delta_x[EDIM] = {0};
double x_new[DIM] = {0};
// state x, P
Eigen::Matrix<double, ZDIM, 1> z(in_z);
EEM P(in_P);
ZZM pre_R(in_R);
// functions from sympy
h_fun(in_x, in_ea, in_hx);
H_fun(in_x, in_ea, in_H);
ZDM pre_H(in_H);
// get y (y = z - hx)
Eigen::Matrix<double, ZDIM, 1> pre_y(in_hx); pre_y = z - pre_y;
X1M y; XXM H; XXM R;
if (Hea_fun){
typedef Eigen::Matrix<double, ZDIM, EADIM, Eigen::RowMajor> ZAM;
double in_Hea[ZDIM * EADIM] = {0};
Hea_fun(in_x, in_ea, in_Hea);
ZAM Hea(in_Hea);
XXM A = Hea.transpose().fullPivLu().kernel();
y = A.transpose() * pre_y;
H = A.transpose() * pre_H;
R = A.transpose() * pre_R * A;
} else {
y = pre_y;
H = pre_H;
R = pre_R;
}
// get modified H
H_mod_fun(in_x, in_H_mod);
DEM H_mod(in_H_mod);
XEM H_err = H * H_mod;
// Do mahalobis distance test
if (MAHA_TEST){
XXM a = (H_err * P * H_err.transpose() + R).inverse();
double maha_dist = y.transpose() * a * y;
if (maha_dist > MAHA_THRESHOLD){
R = 1.0e16 * R;
}
}
// Outlier resilient weighting.
double weight = 1;//(1.5)/(1 + y.squaredNorm()/R.sum());
// kalman gains and I_KH
XXM S = ((H_err * P) * H_err.transpose()) + R/weight;
XEM KT = S.fullPivLu().solve(H_err * P.transpose());
//EZM K = KT.transpose(); TODO: WHY DOES THIS NOT COMPILE?
//EZM K = S.fullPivLu().solve(H_err * P.transpose()).transpose();
//std::cout << "Here is the matrix rot:\n" << K << std::endl;
EEM I_KH = Eigen::Matrix<double, EDIM, EDIM>::Identity() - (KT.transpose() * H_err);
// update state by injecting dx
Eigen::Matrix<double, EDIM, 1> dx(delta_x);
dx = (KT.transpose() * y);
memcpy(delta_x, dx.data(), EDIM * sizeof(double));
err_fun(in_x, delta_x, x_new);
Eigen::Matrix<double, DIM, 1> x(x_new);
// update cov
P = ((I_KH * P) * I_KH.transpose()) + ((KT.transpose() * R) * KT);
// copy out state
memcpy(in_x, x.data(), DIM * sizeof(double));
memcpy(in_P, P.data(), EDIM * EDIM * sizeof(double));
memcpy(in_z, y.data(), y.rows() * sizeof(double));
}

56
artifacts/package_sources/rednose/rednose/templates/feature_handler.c
View File

@@ -1,56 +0,0 @@
bool sane(double track [K + 1][5]) {
double diffs_x [K-1];
double diffs_y [K-1];
int i;
for (i = 0; i < K-1; i++) {
diffs_x[i] = fabs(track[i+2][2] - track[i+1][2]);
diffs_y[i] = fabs(track[i+2][3] - track[i+1][3]);
}
for (i = 1; i < K-1; i++) {
if (((diffs_x[i] > 0.05 or diffs_x[i-1] > 0.05) and
(diffs_x[i] > 2*diffs_x[i-1] or
diffs_x[i] < .5*diffs_x[i-1])) or
((diffs_y[i] > 0.05 or diffs_y[i-1] > 0.05) and
(diffs_y[i] > 2*diffs_y[i-1] or
diffs_y[i] < .5*diffs_y[i-1]))){
return false;
}
}
return true;
}
void merge_features(double *tracks, double *features, long long *empty_idxs) {
double feature_arr [3000][5];
memcpy(feature_arr, features, 3000 * 5 * sizeof(double));
double track_arr [6000][K + 1][5];
memcpy(track_arr, tracks, (K+1) * 6000 * 5 * sizeof(double));
int match;
int empty_idx = 0;
int idx;
for (int i = 0; i < 3000; i++) {
match = feature_arr[i][4];
if (track_arr[match][0][1] == match and track_arr[match][0][2] == 0){
track_arr[match][0][0] = track_arr[match][0][0] + 1;
track_arr[match][0][1] = feature_arr[i][1];
track_arr[match][0][2] = 1;
idx = track_arr[match][0][0];
memcpy(track_arr[match][idx], feature_arr[i], 5 * sizeof(double));
if (idx == K){
// label complete
track_arr[match][0][3] = 1;
if (sane(track_arr[match])){
// label valid
track_arr[match][0][4] = 1;
}
}
} else {
// gen new track with this feature
track_arr[empty_idxs[empty_idx]][0][0] = 1;
track_arr[empty_idxs[empty_idx]][0][1] = feature_arr[i][1];
track_arr[empty_idxs[empty_idx]][0][2] = 1;
memcpy(track_arr[empty_idxs[empty_idx]][1], feature_arr[i], 5 * sizeof(double));
empty_idx = empty_idx + 1;
}
}
memcpy(tracks, track_arr, (K+1) * 6000 * 5 * sizeof(double));
}

10
artifacts/package_sources/rednose/requirements.txt
View File

@@ -1,10 +0,0 @@
ruff
sympy
numpy
scipy
cffi
scons
pre-commit
Cython
pytest
pytest-xdist

19
artifacts/package_sources/rednose/setup.py
View File

@@ -1,19 +0,0 @@
import os
import subprocess
from setuptools import Distribution, setup
from setuptools.command.build_py import build_py
class BinaryDistribution(Distribution):
def has_ext_modules(self):
return True
class BuildPyWithScons(build_py):
def run(self):
subprocess.check_call(["scons", f"-j{os.cpu_count() or 1}", "rednose"], cwd=os.path.dirname(os.path.abspath(__file__)))
super().run()
setup(cmdclass={"build_py": BuildPyWithScons}, distclass=BinaryDistribution)

72
artifacts/package_sources/rednose/site_scons/site_tools/cython.py
View File

@@ -1,72 +0,0 @@
import re
import SCons
from SCons.Action import Action
from SCons.Scanner import Scanner
pyx_from_import_re = re.compile(r'^from\s+(\S+)\s+cimport', re.M)
pyx_import_re = re.compile(r'^cimport\s+(\S+)', re.M)
cdef_import_re = re.compile(r'^cdef extern from\s+.(\S+).:', re.M)
def pyx_scan(node, env, path, arg=None):
contents = node.get_text_contents()
# from <module> cimport ...
matches = pyx_from_import_re.findall(contents)
# cimport <module>
matches += pyx_import_re.findall(contents)
# Modules can be either .pxd or .pyx files
files = [m.replace('.', '/') + '.pxd' for m in matches]
files += [m.replace('.', '/') + '.pyx' for m in matches]
# cdef extern from <file>
files += cdef_import_re.findall(contents)
# Handle relative imports
cur_dir = str(node.get_dir())
files = [cur_dir + f if f.startswith('/') else f for f in files]
# Filter out non-existing files (probably system imports)
files = [f for f in files if env.File(f).exists()]
return env.File(files)
pyxscanner = Scanner(function=pyx_scan, skeys=['.pyx', '.pxd'], recursive=True)
cythonAction = Action("$CYTHONCOM")
def create_builder(env):
try:
cython = env['BUILDERS']['Cython']
except KeyError:
cython = SCons.Builder.Builder(
action=cythonAction,
emitter={},
suffix=cython_suffix_emitter,
single_source=1
)
env.Append(SCANNERS=pyxscanner)
env['BUILDERS']['Cython'] = cython
return cython
def cython_suffix_emitter(env, source):
return "$CYTHONCFILESUFFIX"
def generate(env):
env["CYTHON"] = "cythonize"
env["CYTHONCOM"] = "$CYTHON $CYTHONFLAGS $SOURCE"
env["CYTHONCFILESUFFIX"] = ".cpp"
c_file, _ = SCons.Tool.createCFileBuilders(env)
c_file.suffix['.pyx'] = cython_suffix_emitter
c_file.add_action('.pyx', cythonAction)
c_file.suffix['.py'] = cython_suffix_emitter
c_file.add_action('.py', cythonAction)
create_builder(env)
def exists(env):
return True

51
artifacts/package_sources/rednose/site_scons/site_tools/rednose_filter.py
View File

@@ -1,51 +0,0 @@
import platform
from SCons.Script import Dir, File
def compile_single_filter(env, target, filter_gen_script, output_dir, extra_gen_artifacts, script_deps):
generated_src_files = [File(f) for f in [f'{output_dir}/{target}.cpp', f'{output_dir}/{target}.h']]
extra_generated_files = [File(f'{output_dir}/{x}') for x in extra_gen_artifacts]
generator_file = File(filter_gen_script)
action = f"{File(generator_file).relpath} {target} {Dir(output_dir).relpath}"
if hasattr(env, 'PrettyAction'): # short colored line when the top-level pretty tool is present
action = env.PrettyAction(action, 'GEN')
env.Command(generated_src_files + extra_generated_files,
[generator_file] + script_deps, action)
generated_cc_file = File(generated_src_files[:1])
return generated_cc_file
class BaseRednoseCompileMethod:
def __init__(self, base_py_deps, base_cc_deps):
self.base_py_deps = base_py_deps
self.base_cc_deps = base_cc_deps
class CompileFilterMethod(BaseRednoseCompileMethod):
def __call__(self, env, target, filter_gen_script, output_dir, extra_gen_artifacts=[], gen_script_deps=[]):
objects = compile_single_filter(env, target, filter_gen_script, output_dir, extra_gen_artifacts, self.base_py_deps + gen_script_deps)
linker_flags = env.get("LINKFLAGS", [])
if platform.system() == "Darwin":
linker_flags = ["-undefined", "dynamic_lookup"]
lib_target = env.SharedLibrary(f'{output_dir}/{target}', [self.base_cc_deps, objects], LINKFLAGS=linker_flags)
return lib_target
def generate(env):
templates = env.Glob("$REDNOSE_ROOT/rednose/templates/*")
sympy_helpers = env.File("$REDNOSE_ROOT/rednose/helpers/sympy_helpers.py")
ekf_sym = env.File("$REDNOSE_ROOT/rednose/helpers/ekf_sym.py")
gen_script_deps = templates + [sympy_helpers, ekf_sym]
filter_lib_deps = []
env.AddMethod(CompileFilterMethod(gen_script_deps, filter_lib_deps), "RednoseCompileFilter")
def exists(env):
return True

7
artifacts/runtime/env_sync.sh
View File

@@ -46,9 +46,9 @@ sync_python_env() {
fi
VENV_SITE_PACKAGES="$("$DIR/.venv/bin/python3" -c 'import site; print(site.getsitepackages()[0])' 2>/dev/null || true)"
PACKAGES_READY=0
if [ -d "$DIR/artifacts/package_runtime" ] && PYTHONPATH="$DIR/artifacts/package_runtime" /usr/local/venv/bin/python3 -c "import iqdbc, msgq, panda, rednose, teleoprtc, tinygrad" 2>/dev/null; then
if [ -d "$DIR/artifacts/package_runtime" ] && PYTHONPATH="$DIR/artifacts/package_runtime" /usr/local/venv/bin/python3 -c "import iqdbc, msgq, panda, teleoprtc, tinygrad" 2>/dev/null; then
PACKAGES_READY=1
elif [ "$PACKAGE_LOCK_SHA" = "$INSTALLED_PACKAGE_LOCK_SHA" ] && "$DIR/.venv/bin/python3" -c "import iqdbc, msgq, panda, rednose, teleoprtc, tinygrad" 2>/dev/null \
elif [ "$PACKAGE_LOCK_SHA" = "$INSTALLED_PACKAGE_LOCK_SHA" ] && "$DIR/.venv/bin/python3" -c "import iqdbc, msgq, panda, teleoprtc, tinygrad" 2>/dev/null \
&& "$DIR/.venv/bin/python3" "$DIR/iqpilot/system/runtime_packages_verify.py"; then
# a top-level import passes on a partially extracted install (lazy backends),
# so readiness also requires every wheel RECORD file to exist on disk
@@ -74,8 +74,7 @@ sync_python_env() {
fi
PACKAGE_BUILD_PYTHONPATH="$BASE_SITE_PACKAGES:$VENV_SITE_PACKAGES"
# The base AGNOS venv ships Eigen as a Python package instead of under
# /usr/include. rednose includes <eigen3/Eigen/Dense>, so source-package
# builds need the package's install directory on the compiler include path.
# /usr/include, so source-package builds need its install directory on the compiler include path.
# Resolve it through Python rather than pinning the Python minor version.
EIGEN_INCLUDE_ROOT="$(/usr/local/venv/bin/python3 -c \
'from pathlib import Path; import eigen; root = Path(eigen.__file__).resolve().parent / "install"; assert (root / "eigen3/Eigen/Dense").is_file(); print(root)' \

77
iqpilot/selfdrive/controls/lib/helpers/lateral_edge_guard.py
View File

@@ -2,14 +2,16 @@
Lateral Edge Guard uses the model's lateral road-edge geometry to withhold lane
changes that lack room for a target lane. The model standard deviation remains
in metres: measurements above the validity limit are rejected, while valid
measurements use a two-sigma lower confidence bound for conservative clearance.
measurements use a one-sigma lower confidence bound for conservative clearance.
Unavailable geometry briefly holds the last output, then fails open because a
model dropout is not geometric evidence of a nearby edge.
model dropout is not geometric evidence of a nearby edge. A visible outer lane
line on the target side is direct evidence that a lane exists and overrides the
edge-distance inference.
"""
from __future__ import annotations
import math
from dataclasses import dataclass
from dataclasses import dataclass, replace
from enum import IntEnum
from typing import Any
@@ -20,13 +22,22 @@ from iqpilot.common.swaglog import cloudlog
MIN_ACTIVE_SPEED_MPS = 20.0 * CV.MPH_TO_MS # Matches the lane-change speed gate and excludes parking manoeuvres.
MAX_VALID_ROAD_EDGE_STD_M = 1.0 # A 2-sigma bound beyond 2 m cannot distinguish an adjacent 3.5 m lane reliably.
EDGE_CONFIDENCE_SIGMA = 2.0 # 97.7% one-sided confidence under the model's Gaussian uncertainty assumption.
# roadEdgeStd describes a single edge point, but it is applied to a 5-40 m minimum that already absorbs the
# spatial worst case; 1 sigma covers ~1.1x the measured p99 frame-to-frame spread of that minimum, 2 sigma 2.2x.
EDGE_CONFIDENCE_SIGMA = 1.0
ROAD_EDGE_LOOKAHEAD_MIN_M = 5.0 # Ignore near-field edge points dominated by vehicle-body perspective.
ROAD_EDGE_LOOKAHEAD_MAX_M = 40.0 # Covers about 2 s at the 20 m/s model-training reference speed.
LANE_CENTER_OFFSET_M = 3.5 # Typical freeway lane width and the target-centre lateral displacement.
# CarParams exposes neither width nor track; 0.95 m is half of an assumed conservative 1.90 m body width.
VEHICLE_LATERAL_HALF_WIDTH_M = 1.90 / 2.0
EDGE_CLEARANCE_MARGIN_M = 0.25 # Additional lateral separation between the vehicle body and detected road edge.
ADJACENT_LANE_LINE_PROB = 0.5
EGO_LANE_LINE_PROB_MIN = 0.5
MIN_MEASURED_LANE_WIDTH_M = 2.5
MAX_MEASURED_LANE_WIDTH_M = 4.5
# modelV2 lane lines are ordered outer-left, ego-left, ego-right, outer-right.
OUTER_LANE_LINE_INDEX = (0, 3)
EGO_LANE_LINE_INDEX = (1, 2)
REQUIRED_ROAD_EDGE_DISTANCE_M = LANE_CENTER_OFFSET_M + VEHICLE_LATERAL_HALF_WIDTH_M + EDGE_CLEARANCE_MARGIN_M
BLOCK_DEBOUNCE_S = 0.30 # Six model frames reject a transient close-edge prediction before blocking.
CLEAR_DEBOUNCE_S = 0.50 # Ten model frames make release slower than assertion for conservative hysteresis.
@@ -60,7 +71,8 @@ class _SideState:
fallback_reported: bool = False
def evaluate_road_edge(edge: Any, std_m: Any, direction: int) -> RoadEdgeMeasurement:
def evaluate_road_edge(edge: Any, std_m: Any, direction: int,
lane_width_m: float = LANE_CENTER_OFFSET_M) -> RoadEdgeMeasurement:
if edge is None or std_m is None:
return RoadEdgeMeasurement(RoadEdgeDataState.UNAVAILABLE)
@@ -103,11 +115,12 @@ def evaluate_road_edge(edge: Any, std_m: Any, direction: int) -> RoadEdgeMeasure
return RoadEdgeMeasurement(RoadEdgeDataState.UNAVAILABLE)
conservative_distance_m = lateral_distance_m - EDGE_CONFIDENCE_SIGMA * std
required_distance_m = lane_width_m + VEHICLE_LATERAL_HALF_WIDTH_M + EDGE_CLEARANCE_MARGIN_M
return RoadEdgeMeasurement(
RoadEdgeDataState.VALID,
lateral_distance_m,
conservative_distance_m,
conservative_distance_m < REQUIRED_ROAD_EDGE_DISTANCE_M,
conservative_distance_m < required_distance_m,
)
@@ -160,12 +173,60 @@ class LateralEdgeGuard:
except (AttributeError, TypeError):
return None, None
@staticmethod
def _lane_line_prob(modeldata: Any, index: int) -> float | None:
if modeldata is None:
return None
try:
probs = modeldata.laneLineProbs
if len(probs) <= index:
return None
value = float(probs[index])
except (AttributeError, TypeError, IndexError, ValueError):
return None
return value if math.isfinite(value) else None
@classmethod
def _adjacent_lane_visible(cls, modeldata: Any, side_index: int) -> bool:
prob = cls._lane_line_prob(modeldata, OUTER_LANE_LINE_INDEX[side_index])
return prob is not None and prob > ADJACENT_LANE_LINE_PROB
@classmethod
def _measured_lane_width(cls, modeldata: Any) -> float:
left_prob = cls._lane_line_prob(modeldata, EGO_LANE_LINE_INDEX[0])
right_prob = cls._lane_line_prob(modeldata, EGO_LANE_LINE_INDEX[1])
if left_prob is None or right_prob is None:
return LANE_CENTER_OFFSET_M
if left_prob <= EGO_LANE_LINE_PROB_MIN or right_prob <= EGO_LANE_LINE_PROB_MIN:
return LANE_CENTER_OFFSET_M
try:
lines = modeldata.laneLines
left_y = float(lines[EGO_LANE_LINE_INDEX[0]].y[0])
right_y = float(lines[EGO_LANE_LINE_INDEX[1]].y[0])
except (AttributeError, TypeError, IndexError, ValueError):
return LANE_CENTER_OFFSET_M
width = abs(right_y - left_y)
if not math.isfinite(width):
return LANE_CENTER_OFFSET_M
return min(max(width, MIN_MEASURED_LANE_WIDTH_M), MAX_MEASURED_LANE_WIDTH_M)
@staticmethod
def _apply_lane_evidence(measurement: RoadEdgeMeasurement, lane_visible: bool) -> RoadEdgeMeasurement:
if lane_visible and measurement.state == RoadEdgeDataState.VALID and measurement.should_block:
return replace(measurement, should_block=False)
return measurement
def update(self, modeldata: Any, v_ego_mps: float, dt_s: float) -> None:
dt = max(float(dt_s), 0.0)
left_edge, left_std = self._model_side(modeldata, 0)
right_edge, right_std = self._model_side(modeldata, 1)
self.left_measurement = evaluate_road_edge(left_edge, left_std, LaneChangeDirection.left)
self.right_measurement = evaluate_road_edge(right_edge, right_std, LaneChangeDirection.right)
lane_width_m = self._measured_lane_width(modeldata)
self.left_measurement = self._apply_lane_evidence(
evaluate_road_edge(left_edge, left_std, LaneChangeDirection.left, lane_width_m),
self._adjacent_lane_visible(modeldata, 0))
self.right_measurement = self._apply_lane_evidence(
evaluate_road_edge(right_edge, right_std, LaneChangeDirection.right, lane_width_m),
self._adjacent_lane_visible(modeldata, 1))
speed_active = math.isfinite(v_ego_mps) and v_ego_mps >= MIN_ACTIVE_SPEED_MPS
self._left, left_fallback = step_side_guard(self._left, self.left_measurement, speed_active, dt)
self._right, right_fallback = step_side_guard(self._right, self.right_measurement, speed_active, dt)

66
iqpilot/selfdrive/controls/lib/tests/test_lateral_edge_guard.py
View File

@@ -9,12 +9,16 @@ from iqpilot.common.realtime import DT_MDL
from iqpilot.selfdrive.controls.lib.desire_helper import DesireHelper
from iqpilot.selfdrive.controls.lib.helpers.lane_change import AutoLaneChangeMode
from iqpilot.selfdrive.controls.lib.helpers.lateral_edge_guard import (
ADJACENT_LANE_LINE_PROB,
BLOCK_DEBOUNCE_S,
CLEAR_DEBOUNCE_S,
MAX_VALID_ROAD_EDGE_STD_M,
MIN_ACTIVE_SPEED_MPS,
REQUIRED_ROAD_EDGE_DISTANCE_M,
UNAVAILABLE_HOLD_S,
LANE_CENTER_OFFSET_M,
MAX_MEASURED_LANE_WIDTH_M,
MIN_MEASURED_LANE_WIDTH_M,
LateralEdgeGuard,
RoadEdgeDataState,
evaluate_road_edge,
@@ -35,6 +39,25 @@ class ModelData:
roadEdgeStds: list[float]
@dataclass
class LaneModelData:
roadEdges: list[Edge]
roadEdgeStds: list[float]
laneLines: list[Edge]
laneLineProbs: list[float]
def lane_model(left_distance_m: float = 4.0, outer_prob: float = 0.0,
ego_width_m: float = 3.5, ego_prob: float = 0.9) -> LaneModelData:
xs = [5.0, 20.0, 40.0]
base = edge_model(left_distance_m, left_distance_m)
half = ego_width_m / 2.0
lines = [Edge(xs, [-(half + 3.0)] * 3), Edge(xs, [-half] * 3),
Edge(xs, [half] * 3), Edge(xs, [half + 3.0] * 3)]
return LaneModelData(base.roadEdges, base.roadEdgeStds, lines,
[outer_prob, ego_prob, ego_prob, outer_prob])
class CarState:
def __init__(self, left_blindspot: bool = False) -> None:
self.vEgo = MIN_ACTIVE_SPEED_MPS + 1.0
@@ -86,11 +109,15 @@ def test_unavailable_and_invalid_are_distinct() -> None:
assert invalid.should_block is None
def test_two_sigma_bound_uses_std_in_metres() -> None:
def test_one_sigma_bound_uses_std_in_metres() -> None:
measurement = evaluate_road_edge(edge_model(5.0).roadEdges[0], 0.2, log.LaneChangeDirection.left)
assert measurement.lateral_distance_m == 5.0
assert measurement.conservative_distance_m == 4.6
assert measurement.should_block is True
assert measurement.conservative_distance_m == 4.8
assert measurement.should_block is False
blocking = evaluate_road_edge(edge_model(4.5).roadEdges[0], 0.2, log.LaneChangeDirection.left)
assert blocking.conservative_distance_m == 4.3
assert blocking.should_block is True
def test_distance_threshold_on_either_side() -> None:
@@ -193,3 +220,36 @@ def test_published_edge_block_maps_to_distinct_event_and_alert() -> None:
alert = EVENTS_IQ[event_name][ET.WARNING]
assert alert.alert_text_1 == "Lane Change Blocked"
assert alert.alert_text_2 == "Road edge detected"
def test_visible_outer_lane_line_overrides_edge_block() -> None:
blocking = lane_model(4.0, outer_prob=0.0)
guard = LateralEdgeGuard()
update_for(guard, blocking, BLOCK_DEBOUNCE_S)
assert guard.block_for_direction(log.LaneChangeDirection.left) != custom.IQLateralEdgeBlock.none
guard = LateralEdgeGuard()
update_for(guard, lane_model(4.0, outer_prob=ADJACENT_LANE_LINE_PROB + 0.2), BLOCK_DEBOUNCE_S * 4)
assert guard.block_for_direction(log.LaneChangeDirection.left) == custom.IQLateralEdgeBlock.none
def test_outer_lane_line_below_threshold_still_blocks() -> None:
guard = LateralEdgeGuard()
update_for(guard, lane_model(4.0, outer_prob=ADJACENT_LANE_LINE_PROB - 0.1), BLOCK_DEBOUNCE_S)
assert guard.block_for_direction(log.LaneChangeDirection.left) != custom.IQLateralEdgeBlock.none
def test_narrow_measured_lane_relaxes_required_distance() -> None:
narrow = evaluate_road_edge(edge_model(4.3).roadEdges[0], 0.0, log.LaneChangeDirection.left, 3.0)
wide = evaluate_road_edge(edge_model(4.3).roadEdges[0], 0.0, log.LaneChangeDirection.left, LANE_CENTER_OFFSET_M)
assert narrow.should_block is False
assert wide.should_block is True
def test_measured_lane_width_is_clamped_and_falls_back() -> None:
assert LateralEdgeGuard._measured_lane_width(None) == LANE_CENTER_OFFSET_M
assert LateralEdgeGuard._measured_lane_width(edge_model(4.0)) == LANE_CENTER_OFFSET_M
assert LateralEdgeGuard._measured_lane_width(lane_model(4.0, ego_prob=0.1)) == LANE_CENTER_OFFSET_M
assert LateralEdgeGuard._measured_lane_width(lane_model(4.0, ego_width_m=9.0)) == MAX_MEASURED_LANE_WIDTH_M
assert LateralEdgeGuard._measured_lane_width(lane_model(4.0, ego_width_m=0.5)) == MIN_MEASURED_LANE_WIDTH_M
assert LateralEdgeGuard._measured_lane_width(lane_model(4.0, ego_width_m=3.2)) == 3.2

33
iqpilot/selfdrive/iqlocd/SConscript
View File

@@ -1,37 +1,8 @@
Import('env', 'arch', 'common', 'messaging', 'rednose', 'transformations')
Import('env', 'arch', 'common', 'messaging', 'transformations')
loc_libs = [messaging, common, 'pthread', 'dl']
# build ekf models
rednose_gen_dir = 'models/generated'
rednose_gen_deps = [
"models/constants.py",
]
orbit_filter = env.RednoseCompileFilter(
target='orbit',
filter_gen_script='models/orbit_kf.py',
output_dir=rednose_gen_dir,
extra_gen_artifacts=['orbit_state_constants.h'],
gen_script_deps=rednose_gen_deps,
)
car_ekf = env.RednoseCompileFilter(
target='car',
filter_gen_script='models/car_kf.py',
output_dir=rednose_gen_dir,
extra_gen_artifacts=[],
gen_script_deps=rednose_gen_deps,
)
# iqlocd build
iqlocd_sources = ["atlas_loc_core.cc", "models/orbit_kf.cc"]
lenv = env.Clone()
# ekf filter libraries need to be linked, even if no symbols are used
if arch != "Darwin":
lenv["LINKFLAGS"] += ["-Wl,--no-as-needed"]
lenv["LIBPATH"].append(Dir(rednose_gen_dir).abspath)
lenv["RPATH"].append(Dir(rednose_gen_dir).abspath)
iqlocd = lenv.Program("iqlocd", iqlocd_sources, LIBS=["orbit", rednose] + loc_libs + transformations)
lenv.Depends(iqlocd, rednose)
lenv.Depends(iqlocd, orbit_filter)
iqlocd = lenv.Program("iqlocd", iqlocd_sources, LIBS=loc_libs + transformations)

1
iqpilot/selfdrive/iqlocd/atlas_loc_core.cc
View File

@@ -7,7 +7,6 @@
#include <cmath>
#include <vector>
using namespace EKFS;
using namespace Eigen;
ExitHandler do_exit;

222
iqpilot/selfdrive/iqlocd/models/car_kf.py Executable file → Normal file
View File

@@ -1,75 +1,63 @@
#!/usr/bin/env python3
"""
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
"""
import math
import sys
from typing import Any
import numpy as np
from iqpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY
from iqpilot.selfdrive.iqlocd.models.constants import ObservationKind
from iqpilot.common.swaglog import cloudlog
from rednose.helpers.kalmanfilter import KalmanFilter
if __name__ == '__main__': # Generating sympy
import sympy as sp
from rednose.helpers.ekf_sym import gen_code
else:
from rednose.helpers.ekf_sym_pyx import EKF_sym_pyx
i = 0
def _slice(n):
global i
s = slice(i, i + n)
i += n
return s
from iqpilot.selfdrive.state_estimation import EstimatorModel, ModelDefinition, StateEstimator
try:
from iqpilot.selfdrive.state_estimation.native_binding_pyx import car_predict, car_update
except ModuleNotFoundError:
car_predict = None
car_update = None
class States:
# Vehicle model params
STIFFNESS = _slice(1) # [-]
STEER_RATIO = _slice(1) # [-]
ANGLE_OFFSET = _slice(1) # [rad]
ANGLE_OFFSET_FAST = _slice(1) # [rad]
VELOCITY = _slice(2) # (x, y) [m/s]
YAW_RATE = _slice(1) # [rad/s]
STEER_ANGLE = _slice(1) # [rad]
ROAD_ROLL = _slice(1) # [rad]
STIFFNESS = slice(0, 1)
STEER_RATIO = slice(1, 2)
ANGLE_OFFSET = slice(2, 3)
ANGLE_OFFSET_FAST = slice(3, 4)
VELOCITY = slice(4, 6)
YAW_RATE = slice(6, 7)
STEER_ANGLE = slice(7, 8)
ROAD_ROLL = slice(8, 9)
class CarKalman(KalmanFilter):
name = 'car'
def _transition(state: np.ndarray, dt: float, values: dict[str, float]) -> np.ndarray:
result = state.copy()
stiffness = state[0]
steer_ratio = state[1]
angle = state[7] - state[2] - state[3]
speed, lateral_speed = state[4:6]
yaw_rate = state[6]
mass = values["mass"]
inertia = values["rotational_inertia"]
front = values["center_to_front"]
rear = values["center_to_rear"]
front_stiffness = stiffness * values["stiffness_front"]
rear_stiffness = stiffness * values["stiffness_rear"]
lateral_dot = -(front_stiffness + rear_stiffness) * lateral_speed / (mass * speed)
lateral_dot += (-(front_stiffness * front - rear_stiffness * rear) / (mass * speed) - speed) * yaw_rate
lateral_dot += front_stiffness * angle / (mass * steer_ratio) - ACCELERATION_DUE_TO_GRAVITY * state[8]
yaw_dot = -(front_stiffness * front - rear_stiffness * rear) * lateral_speed / (inertia * speed)
yaw_dot -= (front_stiffness * front**2 + rear_stiffness * rear**2) * yaw_rate / (inertia * speed)
yaw_dot += front_stiffness * front * angle / (inertia * steer_ratio)
result[5] += dt * lateral_dot
result[6] += dt * yaw_dot
return result
initial_x = np.array([
1.0,
15.0,
0.0,
0.0,
10.0, 0.0,
0.0,
0.0,
0.0
])
# process noise
Q = np.diag([
(.05 / 100)**2,
.01**2,
math.radians(0.02)**2,
math.radians(0.25)**2,
.1**2, .01**2,
math.radians(0.1)**2,
math.radians(0.1)**2,
math.radians(1)**2,
])
class CarKalman(EstimatorModel):
name = "car"
initial_x = np.array([1.0, 15.0, 0.0, 0.0, 10.0, 0.0, 0.0, 0.0, 0.0])
Q = np.diag([(.05 / 100)**2, .01**2, math.radians(0.02)**2, math.radians(0.25)**2,
.1**2, .01**2, math.radians(0.1)**2, math.radians(0.1)**2, math.radians(1)**2])
P_initial = Q.copy()
obs_noise: dict[int, Any] = {
ObservationKind.STEER_ANGLE: np.atleast_2d(math.radians(0.05)**2),
ObservationKind.ANGLE_OFFSET_FAST: np.atleast_2d(math.radians(10.0)**2),
@@ -79,102 +67,28 @@ class CarKalman(KalmanFilter):
ObservationKind.ROAD_FRAME_X_SPEED: np.atleast_2d(0.1**2),
}
global_vars = [
'mass',
'rotational_inertia',
'center_to_front',
'center_to_rear',
'stiffness_front',
'stiffness_rear',
]
def __init__(self):
self.native_parameters = np.zeros(6)
measurements = {
ObservationKind.ROAD_FRAME_YAW_RATE: lambda state, _: state[6:7],
ObservationKind.ROAD_FRAME_XY_SPEED: lambda state, _: state[4:6],
ObservationKind.ROAD_FRAME_X_SPEED: lambda state, _: state[4:5],
ObservationKind.STEER_ANGLE: lambda state, _: state[7:8],
ObservationKind.ANGLE_OFFSET_FAST: lambda state, _: state[3:4],
ObservationKind.STEER_RATIO: lambda state, _: state[1:2],
ObservationKind.STIFFNESS: lambda state, _: state[0:1],
ObservationKind.ROAD_ROLL: lambda state, _: state[8:9],
}
def native_predict(state, covariance, dt, process_noise, _):
car_predict(state, covariance, process_noise, dt, self.native_parameters)
@staticmethod
def generate_code(generated_dir):
dim_state = CarKalman.initial_x.shape[0]
name = CarKalman.name
model = ModelDefinition(9, 9, _transition, measurements, self.Q, self.obs_noise,
native_predict=native_predict if car_predict is not None else None, native_update=car_update)
super().__init__(StateEstimator(model, self.initial_x, self.P_initial, max_rewind_age=0.8))
# Linearized single-track lateral dynamics, equations 7.211-7.213
# Massimo Guiggiani, The Science of Vehicle Dynamics: Handling, Braking, and Ride of Road and Race Cars
# Springer Cham, 2023. doi: https://doi.org/10.1007/978-3-031-06461-6
# globals
global_vars = [sp.Symbol(name) for name in CarKalman.global_vars]
m, j, aF, aR, cF_orig, cR_orig = global_vars
# make functions and jacobians with sympy
# state variables
state_sym = sp.MatrixSymbol('state', dim_state, 1)
state = sp.Matrix(state_sym)
# Vehicle model constants
sf = state[States.STIFFNESS, :][0, 0]
cF, cR = sf * cF_orig, sf * cR_orig
angle_offset = state[States.ANGLE_OFFSET, :][0, 0]
angle_offset_fast = state[States.ANGLE_OFFSET_FAST, :][0, 0]
theta = state[States.ROAD_ROLL, :][0, 0]
sa = state[States.STEER_ANGLE, :][0, 0]
sR = state[States.STEER_RATIO, :][0, 0]
u, v = state[States.VELOCITY, :]
r = state[States.YAW_RATE, :][0, 0]
A = sp.Matrix(np.zeros((2, 2)))
A[0, 0] = -(cF + cR) / (m * u)
A[0, 1] = -(cF * aF - cR * aR) / (m * u) - u
A[1, 0] = -(cF * aF - cR * aR) / (j * u)
A[1, 1] = -(cF * aF**2 + cR * aR**2) / (j * u)
B = sp.Matrix(np.zeros((2, 1)))
B[0, 0] = cF / m / sR
B[1, 0] = (cF * aF) / j / sR
C = sp.Matrix(np.zeros((2, 1)))
C[0, 0] = ACCELERATION_DUE_TO_GRAVITY
C[1, 0] = 0
x = sp.Matrix([v, r]) # lateral velocity, yaw rate
x_dot = A * x + B * (sa - angle_offset - angle_offset_fast) - C * theta
dt = sp.Symbol('dt')
state_dot = sp.Matrix(np.zeros((dim_state, 1)))
state_dot[States.VELOCITY.start + 1, 0] = x_dot[0]
state_dot[States.YAW_RATE.start, 0] = x_dot[1]
# Basic descretization, 1st order integrator
# Can be pretty bad if dt is big
f_sym = state + dt * state_dot
#
# Observation functions
#
obs_eqs = [
[sp.Matrix([r]), ObservationKind.ROAD_FRAME_YAW_RATE, None],
[sp.Matrix([u, v]), ObservationKind.ROAD_FRAME_XY_SPEED, None],
[sp.Matrix([u]), ObservationKind.ROAD_FRAME_X_SPEED, None],
[sp.Matrix([sa]), ObservationKind.STEER_ANGLE, None],
[sp.Matrix([angle_offset_fast]), ObservationKind.ANGLE_OFFSET_FAST, None],
[sp.Matrix([sR]), ObservationKind.STEER_RATIO, None],
[sp.Matrix([sf]), ObservationKind.STIFFNESS, None],
[sp.Matrix([theta]), ObservationKind.ROAD_ROLL, None],
]
gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state, global_vars=global_vars)
def __init__(self, generated_dir):
dim_state, dim_state_err = CarKalman.initial_x.shape[0], CarKalman.P_initial.shape[0]
self.filter = EKF_sym_pyx(generated_dir, CarKalman.name, CarKalman.Q, CarKalman.initial_x, CarKalman.P_initial,
dim_state, dim_state_err, global_vars=CarKalman.global_vars, logger=cloudlog)
def set_globals(self, mass, rotational_inertia, center_to_front, center_to_rear, stiffness_front, stiffness_rear):
self.filter.set_global("mass", mass)
self.filter.set_global("rotational_inertia", rotational_inertia)
self.filter.set_global("center_to_front", center_to_front)
self.filter.set_global("center_to_rear", center_to_rear)
self.filter.set_global("stiffness_front", stiffness_front)
self.filter.set_global("stiffness_rear", stiffness_rear)
if __name__ == "__main__":
generated_dir = sys.argv[2]
CarKalman.generate_code(generated_dir)
def set_globals(self, mass: float, rotational_inertia: float, center_to_front: float, center_to_rear: float,
stiffness_front: float, stiffness_rear: float) -> None:
self.native_parameters[:] = mass, rotational_inertia, center_to_front, center_to_rear, stiffness_front, stiffness_rear
for name, value in locals().copy().items():
if name != "self":
self.filter.set_global(name, value)

4
iqpilot/selfdrive/iqlocd/models/constants.py
View File

@@ -1,7 +1,3 @@
import os
GENERATED_DIR = os.path.abspath(os.path.join(os.path.dirname(__file__), 'generated'))
class ObservationKind:
UNKNOWN = 0
NO_OBSERVATION = 1

281
iqpilot/selfdrive/iqlocd/models/orbit_kf.cc
View File

@@ -1,122 +1,225 @@
/*
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
*/
#include "iqpilot/selfdrive/iqlocd/models/orbit_kf.h"
using namespace EKFS;
using namespace Eigen;
#include <cmath>
Eigen::Map<Eigen::VectorXd> get_mapvec(const Eigen::VectorXd &vec) {
return Eigen::Map<Eigen::VectorXd>((double*)vec.data(), vec.rows(), vec.cols());
using Eigen::Matrix3d;
using Eigen::Quaterniond;
using Eigen::Vector3d;
using Eigen::VectorXd;
using iqpilot::state_estimation::ModelDefinition;
using iqpilot::state_estimation::StateEstimator;
namespace {
constexpr double EARTH_GM = 3.986005e14;
Matrix3d rotation(const VectorXd &state) {
return Quaterniond(state(3), state(4), state(5), state(6)).normalized().toRotationMatrix();
}
Eigen::Map<MatrixXdr> get_mapmat(const MatrixXdr &mat) {
return Eigen::Map<MatrixXdr>((double*)mat.data(), mat.rows(), mat.cols());
Matrix3d skew(const Vector3d &value) {
Matrix3d result;
result << 0.0, -value.z(), value.y(), value.z(), 0.0, -value.x(), -value.y(), value.x(), 0.0;
return result;
}
std::vector<Eigen::Map<Eigen::VectorXd>> get_vec_mapvec(const std::vector<Eigen::VectorXd> &vec_vec) {
std::vector<Eigen::Map<Eigen::VectorXd>> res;
for (const Eigen::VectorXd &vec : vec_vec) {
res.push_back(get_mapvec(vec));
}
return res;
VectorXd transition(const VectorXd &state, double dt) {
VectorXd result = state;
const Quaterniond orientation(state(3), state(4), state(5), state(6));
const Vector3d omega = state.segment<3>(10);
const Quaterniond derivative(0.0, omega.x(), omega.y(), omega.z());
const Quaterniond rate = orientation * derivative;
result.segment<3>(0) += dt * state.segment<3>(7);
result.segment<4>(3) += 0.5 * dt * (VectorXd(4) << rate.w(), rate.x(), rate.y(), rate.z()).finished();
result.segment<3>(7) += dt * rotation(state) * state.segment<3>(16);
return result;
}
VectorXd normalize(const VectorXd &state) {
VectorXd result = state;
result.segment<4>(3) /= result.segment<4>(3).norm();
return result;
}
VectorXd inject(const VectorXd &state, const VectorXd &delta) {
VectorXd result = state;
result.segment<3>(0) += delta.segment<3>(0);
const Quaterniond orientation(state(3), state(4), state(5), state(6));
Quaterniond error(1.0, 0.5 * delta(3), 0.5 * delta(4), 0.5 * delta(5));
const Quaterniond updated = error * orientation;
result.segment<4>(3) << updated.w(), updated.x(), updated.y(), updated.z();
result.segment(7, 15) += delta.segment(6, 15);
return normalize(result);
}
MatrixXdr error_projection(const VectorXd &state) {
MatrixXdr projection = MatrixXdr::Zero(22, 21);
projection.block<3, 3>(0, 0).setIdentity();
const double w = state(3);
const double x = state(4);
const double y = state(5);
const double z = state(6);
projection.block<4, 3>(3, 3) << -0.5 * x, -0.5 * y, -0.5 * z,
0.5 * w, 0.5 * z, -0.5 * y,
-0.5 * z, 0.5 * w, 0.5 * x,
0.5 * y, -0.5 * x, 0.5 * w;
projection.block(7, 6, 15, 15).setIdentity();
return projection;
}
MatrixXdr orbit_error_transition(const VectorXd &state, double dt) {
MatrixXdr result = MatrixXdr::Identity(21, 21);
const Matrix3d transform = rotation(state);
result.block<3, 3>(0, 6) = Matrix3d::Identity() * dt;
result.block<3, 3>(3, 3) += -dt * skew(transform * state.segment<3>(10));
result.block<3, 3>(3, 9) = dt * transform;
result.block<3, 3>(6, 3) = -dt * skew(transform * state.segment<3>(16));
result.block<3, 3>(6, 15) = dt * transform;
return result;
}
MatrixXdr selected_jacobian(int start) {
MatrixXdr result = MatrixXdr::Zero(3, 21);
result.block<3, 3>(0, start).setIdentity();
return result;
}
VectorXd phone_acceleration(const VectorXd &state) {
const Vector3d position = state.segment<3>(0);
const Vector3d gravity = rotation(state).transpose() * (EARTH_GM * position / std::pow(position.squaredNorm(), 1.5));
return gravity + state.segment<3>(16) + state.segment<3>(19);
}
MatrixXdr diagonal(std::initializer_list<double> values) {
VectorXd vector(values.size());
int index = 0;
for (double value : values) vector(index++) = value;
return vector.asDiagonal();
}
std::vector<Eigen::Map<MatrixXdr>> get_vec_mapmat(const std::vector<MatrixXdr> &mat_vec) {
std::vector<Eigen::Map<MatrixXdr>> res;
for (const MatrixXdr &mat : mat_vec) {
res.push_back(get_mapmat(mat));
}
return res;
}
OrbitKalman::OrbitKalman() {
this->dim_state = orbit_initial_x.rows();
this->dim_state_err = orbit_initial_P_diag.rows();
this->initial_x = orbit_initial_x;
this->initial_P = orbit_initial_P_diag.asDiagonal();
this->fake_gps_pos_cov = orbit_fake_gps_pos_cov_diag.asDiagonal();
this->fake_gps_vel_cov = orbit_fake_gps_vel_cov_diag.asDiagonal();
this->reset_orientation_P = orbit_reset_orientation_diag.asDiagonal();
this->Q = orbit_Q_diag.asDiagonal();
for (auto& pair : orbit_obs_noise_diag) {
this->obs_noise[pair.first] = pair.second.asDiagonal();
}
// init filter
this->filter = std::make_shared<EKFSym>(this->name, get_mapmat(this->Q), get_mapvec(this->initial_x),
get_mapmat(initial_P), this->dim_state, this->dim_state_err, 0, 0, 0, std::vector<int>(),
std::vector<int>{3}, std::vector<std::string>(), 0.8);
initial_x.resize(22);
initial_x << 3.88e6, -3.37e6, 3.76e6, 0.42254641, -0.31238054, -0.83602975, -0.15788347,
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;
initial_P = diagonal({100.0, 100.0, 100.0, 0.0001, 0.0001, 0.0001, 100.0, 100.0, 100.0,
1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 10000.0, 10000.0, 10000.0, 0.0001, 0.0001, 0.0001});
fake_gps_pos_cov = diagonal({1e6, 1e6, 1e6});
fake_gps_vel_cov = diagonal({100.0, 100.0, 100.0});
reset_orientation_P = diagonal({1.0, 1.0, 1.0});
obs_noise = {
{OBSERVATION_PHONE_GYRO, diagonal({0.000625, 0.000625, 0.000625})},
{OBSERVATION_PHONE_ACCEL, diagonal({0.25, 0.25, 0.25})},
{OBSERVATION_CAMERA_ODO_ROTATION, diagonal({0.0025, 0.0025, 0.0025})},
{OBSERVATION_CAMERA_ODO_TRANSLATION, diagonal({0.25, 0.25, 0.25})},
{OBSERVATION_NO_ROT, diagonal({0.000025, 0.000025, 0.000025})},
{OBSERVATION_NO_ACCEL, diagonal({0.0025, 0.0025, 0.0025})},
{OBSERVATION_ECEF_POS, diagonal({25.0, 25.0, 25.0})},
{OBSERVATION_ECEF_VEL, diagonal({0.25, 0.25, 0.25})},
{OBSERVATION_ECEF_ORIENTATION_FROM_GPS, diagonal({0.04, 0.04, 0.04, 0.04})},
};
const MatrixXdr process_noise = diagonal({0.0009, 0.0009, 0.0009, 0.000001, 0.000001, 0.000001,
0.0001, 0.0001, 0.0001, 0.01, 0.01, 0.01,
2.5e-9, 2.5e-9, 2.5e-9, 9.0, 9.0, 9.0, 0.000025, 0.000025, 0.000025});
std::unordered_map<int, std::function<VectorXd(const VectorXd &)>> measurements = {
{OBSERVATION_PHONE_GYRO, [](const VectorXd &state) { return state.segment<3>(10) + state.segment<3>(13); }},
{OBSERVATION_NO_ROT, [](const VectorXd &state) { return state.segment<3>(10); }},
{OBSERVATION_PHONE_ACCEL, phone_acceleration},
{OBSERVATION_ECEF_POS, [](const VectorXd &state) { return state.segment<3>(0); }},
{OBSERVATION_ECEF_VEL, [](const VectorXd &state) { return state.segment<3>(7); }},
{OBSERVATION_ECEF_ORIENTATION_FROM_GPS, [](const VectorXd &state) { return state.segment<4>(3); }},
{OBSERVATION_CAMERA_ODO_TRANSLATION, [](const VectorXd &state) { return rotation(state).transpose() * state.segment<3>(7); }},
{OBSERVATION_CAMERA_ODO_ROTATION, [](const VectorXd &state) { return state.segment<3>(10); }},
{OBSERVATION_NO_ACCEL, [](const VectorXd &state) { return state.segment<3>(16); }},
};
std::unordered_map<int, std::function<MatrixXdr(const VectorXd &)>> observation_jacobians = {
{OBSERVATION_PHONE_GYRO, [](const VectorXd &) {
MatrixXdr result = selected_jacobian(9);
result.block<3, 3>(0, 12).setIdentity();
return result;
}},
{OBSERVATION_NO_ROT, [](const VectorXd &) { return selected_jacobian(9); }},
{OBSERVATION_PHONE_ACCEL, [](const VectorXd &state) {
MatrixXdr result = MatrixXdr::Zero(3, 21);
const Vector3d position = state.segment<3>(0);
const double radius_squared = position.squaredNorm();
const double radius = std::sqrt(radius_squared);
const Vector3d gravity = EARTH_GM * position / (radius_squared * radius);
result.block<3, 3>(0, 0) = rotation(state).transpose() * EARTH_GM *
(Matrix3d::Identity() / (radius_squared * radius) -
3.0 * position * position.transpose() / (radius_squared * radius_squared * radius));
result.block<3, 3>(0, 3) = rotation(state).transpose() * skew(gravity);
result.block<3, 3>(0, 15).setIdentity();
result.block<3, 3>(0, 18).setIdentity();
return result;
}},
{OBSERVATION_ECEF_POS, [](const VectorXd &) { return selected_jacobian(0); }},
{OBSERVATION_ECEF_VEL, [](const VectorXd &) { return selected_jacobian(6); }},
{OBSERVATION_ECEF_ORIENTATION_FROM_GPS, [](const VectorXd &state) { return error_projection(state).block(3, 0, 4, 21); }},
{OBSERVATION_CAMERA_ODO_TRANSLATION, [](const VectorXd &state) {
MatrixXdr result = MatrixXdr::Zero(3, 21);
result.block<3, 3>(0, 3) = rotation(state).transpose() * skew(state.segment<3>(7));
result.block<3, 3>(0, 6) = rotation(state).transpose();
return result;
}},
{OBSERVATION_CAMERA_ODO_ROTATION, [](const VectorXd &) { return selected_jacobian(9); }},
{OBSERVATION_NO_ACCEL, [](const VectorXd &) { return selected_jacobian(15); }},
};
ModelDefinition model{22, 21, transition, measurements, process_noise, obs_noise, inject, error_projection, normalize,
orbit_error_transition, observation_jacobians};
filter = std::make_shared<StateEstimator>(std::move(model), initial_x, initial_P);
}
void OrbitKalman::init_state(const VectorXd &state, const VectorXd &covs_diag, double filter_time) {
MatrixXdr covs = covs_diag.asDiagonal();
this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time);
filter->init_state(state, covs_diag.asDiagonal(), filter_time);
}
void OrbitKalman::init_state(const VectorXd &state, const MatrixXdr &covs, double filter_time) {
this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time);
filter->init_state(state, covs, filter_time);
}
void OrbitKalman::init_state(const VectorXd &state, double filter_time) {
MatrixXdr covs = this->filter->covs();
this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time);
filter->init_state(state, filter->covariance(), filter_time);
}
VectorXd OrbitKalman::get_x() {
return this->filter->state();
}
MatrixXdr OrbitKalman::get_P() {
return this->filter->covs();
}
double OrbitKalman::get_filter_time() {
return this->filter->get_filter_time();
}
VectorXd OrbitKalman::get_x() { return filter->state(); }
MatrixXdr OrbitKalman::get_P() { return filter->covariance(); }
double OrbitKalman::get_filter_time() { return filter->time(); }
std::vector<MatrixXdr> OrbitKalman::get_R(int kind, int n) {
std::vector<MatrixXdr> R;
for (int i = 0; i < n; i++) {
R.push_back(this->obs_noise[kind]);
}
return R;
return std::vector<MatrixXdr>(n, obs_noise.at(kind));
}
std::optional<Estimate> OrbitKalman::predict_and_observe(double t, int kind, const std::vector<VectorXd> &meas, std::vector<MatrixXdr> R) {
std::optional<Estimate> r;
if (R.size() == 0) {
R = this->get_R(kind, meas.size());
}
r = this->filter->predict_and_update_batch(t, kind, get_vec_mapvec(meas), get_vec_mapmat(R));
return r;
return filter->predict_and_observe(t, kind, meas, R);
}
void OrbitKalman::predict(double t) {
this->filter->predict(t);
}
const Eigen::VectorXd &OrbitKalman::get_initial_x() {
return this->initial_x;
}
const MatrixXdr &OrbitKalman::get_initial_P() {
return this->initial_P;
}
const MatrixXdr &OrbitKalman::get_fake_gps_pos_cov() {
return this->fake_gps_pos_cov;
}
const MatrixXdr &OrbitKalman::get_fake_gps_vel_cov() {
return this->fake_gps_vel_cov;
}
const MatrixXdr &OrbitKalman::get_reset_orientation_P() {
return this->reset_orientation_P;
}
void OrbitKalman::predict(double t) { filter->predict(t); }
const VectorXd &OrbitKalman::get_initial_x() { return initial_x; }
const MatrixXdr &OrbitKalman::get_initial_P() { return initial_P; }
const MatrixXdr &OrbitKalman::get_fake_gps_pos_cov() { return fake_gps_pos_cov; }
const MatrixXdr &OrbitKalman::get_fake_gps_vel_cov() { return fake_gps_vel_cov; }
const MatrixXdr &OrbitKalman::get_reset_orientation_P() { return reset_orientation_P; }
MatrixXdr OrbitKalman::H(const VectorXd &in) {
assert(in.size() == 6);
Matrix<double, 3, 6, Eigen::RowMajor> res;
this->filter->get_extra_routine("H")((double*)in.data(), res.data());
return res;
if (in.size() != 6) throw std::invalid_argument("local velocity input dimension mismatch");
auto function = [](const VectorXd &value) {
const Matrix3d transform = (Eigen::AngleAxisd(value(2), Vector3d::UnitZ()) * Eigen::AngleAxisd(value(1), Vector3d::UnitY()) *
Eigen::AngleAxisd(value(0), Vector3d::UnitX())).toRotationMatrix();
return transform.transpose() * value.segment<3>(3);
};
MatrixXdr result(3, 6);
for (int index = 0; index < 6; ++index) {
const double step = std::cbrt(Eigen::NumTraits<double>::epsilon()) * std::max(1.0, std::abs(in(index)));
VectorXd upper = in;
VectorXd lower = in;
upper(index) += step;
lower(index) -= step;
result.col(index) = (function(upper) - function(lower)) / (2.0 * step);
}
return result;
}

38
iqpilot/selfdrive/iqlocd/models/orbit_kf.h
View File

@@ -1,66 +1,46 @@
/*
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
*/
#pragma once
#include <string>
#include <cmath>
#include <memory>
#include <optional>
#include <unordered_map>
#include <vector>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>
#include "generated/orbit_state_constants.h"
#include "rednose/helpers/ekf_sym.h"
#include "iqpilot/selfdrive/iqlocd/models/orbit_kf_constants.h"
#include "iqpilot/selfdrive/state_estimation/estimator.h"
#define EARTH_GM 3.986005e14 // m^3/s^2 (gravitational constant * mass of earth)
using namespace EKFS;
Eigen::Map<Eigen::VectorXd> get_mapvec(const Eigen::VectorXd &vec);
Eigen::Map<MatrixXdr> get_mapmat(const MatrixXdr &mat);
std::vector<Eigen::Map<Eigen::VectorXd>> get_vec_mapvec(const std::vector<Eigen::VectorXd> &vec_vec);
std::vector<Eigen::Map<MatrixXdr>> get_vec_mapmat(const std::vector<MatrixXdr> &mat_vec);
using MatrixXdr = iqpilot::state_estimation::Matrix;
using Estimate = iqpilot::state_estimation::Estimate;
class OrbitKalman {
public:
OrbitKalman();
void init_state(const Eigen::VectorXd &state, const Eigen::VectorXd &covs_diag, double filter_time);
void init_state(const Eigen::VectorXd &state, const MatrixXdr &covs, double filter_time);
void init_state(const Eigen::VectorXd &state, double filter_time);
Eigen::VectorXd get_x();
MatrixXdr get_P();
double get_filter_time();
std::vector<MatrixXdr> get_R(int kind, int n);
std::optional<Estimate> predict_and_observe(double t, int kind, const std::vector<Eigen::VectorXd> &meas, std::vector<MatrixXdr> R = {});
std::optional<Estimate> predict_and_update_odo_speed(std::vector<Eigen::VectorXd> speed, double t, int kind);
std::optional<Estimate> predict_and_update_odo_trans(std::vector<Eigen::VectorXd> trans, double t, int kind);
std::optional<Estimate> predict_and_update_odo_rot(std::vector<Eigen::VectorXd> rot, double t, int kind);
void predict(double t);
const Eigen::VectorXd &get_initial_x();
const MatrixXdr &get_initial_P();
const MatrixXdr &get_fake_gps_pos_cov();
const MatrixXdr &get_fake_gps_vel_cov();
const MatrixXdr &get_reset_orientation_P();
MatrixXdr H(const Eigen::VectorXd &in);
private:
std::string name = "orbit";
std::shared_ptr<EKFSym> filter;
int dim_state;
int dim_state_err;
std::shared_ptr<iqpilot::state_estimation::StateEstimator> filter;
Eigen::VectorXd initial_x;
MatrixXdr initial_P;
MatrixXdr fake_gps_pos_cov;
MatrixXdr fake_gps_vel_cov;
MatrixXdr reset_orientation_P;
MatrixXdr Q; // process noise
std::unordered_map<int, MatrixXdr> obs_noise;
};

242
iqpilot/selfdrive/iqlocd/models/orbit_kf.py
View File

@@ -1,242 +0,0 @@
#!/usr/bin/env python3
import sys
import os
import numpy as np
from iqpilot.selfdrive.iqlocd.models.constants import ObservationKind
import sympy as sp
import inspect
from rednose.helpers.sympy_helpers import euler_rotate, quat_matrix_r, quat_rotate
from rednose.helpers.ekf_sym import gen_code
EARTH_GM = 3.986005e14 # m^3/s^2 (gravitational constant * mass of earth)
def numpy2eigenstring(arr):
assert(len(arr.shape) == 1)
arr_str = np.array2string(arr, precision=20, separator=',')[1:-1].replace(' ', '').replace('\n', '')
return f"(Eigen::VectorXd({len(arr)}) << {arr_str}).finished()"
class States:
ECEF_POS = slice(0, 3) # x, y and z in ECEF in meters
ECEF_ORIENTATION = slice(3, 7) # quat for pose of phone in ecef
ECEF_VELOCITY = slice(7, 10) # ecef velocity in m/s
ANGULAR_VELOCITY = slice(10, 13) # roll, pitch and yaw rates in device frame in radians/s
GYRO_BIAS = slice(13, 16) # roll, pitch and yaw biases
ACCELERATION = slice(16, 19) # Acceleration in device frame in m/s**2
ACC_BIAS = slice(19, 22) # Acceletometer bias in m/s**2
# Error-state has different slices because it is an ESKF
ECEF_POS_ERR = slice(0, 3)
ECEF_ORIENTATION_ERR = slice(3, 6) # euler angles for orientation error
ECEF_VELOCITY_ERR = slice(6, 9)
ANGULAR_VELOCITY_ERR = slice(9, 12)
GYRO_BIAS_ERR = slice(12, 15)
ACCELERATION_ERR = slice(15, 18)
ACC_BIAS_ERR = slice(18, 21)
class OrbitScopeModel:
name = 'orbit'
initial_x = np.array([3.88e6, -3.37e6, 3.76e6,
0.42254641, -0.31238054, -0.83602975, -0.15788347, # NED [0,0,0] -> ECEF Quat
0, 0, 0,
0, 0, 0,
0, 0, 0,
0, 0, 0,
0, 0, 0])
# state covariance
initial_P_diag = np.array([10**2, 10**2, 10**2,
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])
# state covariance when resetting midway in a segment
reset_orientation_diag = np.array([1**2, 1**2, 1**2])
# fake observation covariance, to ensure the uncertainty estimate of the filter is under control
fake_gps_pos_cov_diag = np.array([1000**2, 1000**2, 1000**2])
fake_gps_vel_cov_diag = np.array([10**2, 10**2, 10**2])
# process noise
Q_diag = np.array([0.03**2, 0.03**2, 0.03**2,
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])
obs_noise_diag = {ObservationKind.PHONE_GYRO: np.array([0.025**2, 0.025**2, 0.025**2]),
ObservationKind.PHONE_ACCEL: np.array([.5**2, .5**2, .5**2]),
ObservationKind.CAMERA_ODO_ROTATION: np.array([0.05**2, 0.05**2, 0.05**2]),
ObservationKind.NO_ROT: np.array([0.005**2, 0.005**2, 0.005**2]),
ObservationKind.NO_ACCEL: np.array([0.05**2, 0.05**2, 0.05**2]),
ObservationKind.ECEF_POS: np.array([5**2, 5**2, 5**2]),
ObservationKind.ECEF_VEL: np.array([.5**2, .5**2, .5**2]),
ObservationKind.ECEF_ORIENTATION_FROM_GPS: np.array([.2**2, .2**2, .2**2, .2**2])}
@staticmethod
def generate_code(generated_dir):
name = OrbitScopeModel.name
dim_state = OrbitScopeModel.initial_x.shape[0]
dim_state_err = OrbitScopeModel.initial_P_diag.shape[0]
state_sym = sp.MatrixSymbol('state', dim_state, 1)
state = sp.Matrix(state_sym)
x, y, z = state[States.ECEF_POS, :]
q = state[States.ECEF_ORIENTATION, :]
v = state[States.ECEF_VELOCITY, :]
vx, vy, vz = v
omega = state[States.ANGULAR_VELOCITY, :]
vroll, vpitch, vyaw = omega
roll_bias, pitch_bias, yaw_bias = state[States.GYRO_BIAS, :]
acceleration = state[States.ACCELERATION, :]
acc_bias = state[States.ACC_BIAS, :]
dt = sp.Symbol('dt')
# calibration and attitude rotation matrices
quat_rot = quat_rotate(*q)
# Got the quat predict equations from here
# A New Quaternion-Based Kalman Filter for
# Real-Time Attitude Estimation Using the Two-Step
# Geometrically-Intuitive Correction Algorithm
A = 0.5 * sp.Matrix([[0, -vroll, -vpitch, -vyaw],
[vroll, 0, vyaw, -vpitch],
[vpitch, -vyaw, 0, vroll],
[vyaw, vpitch, -vroll, 0]])
q_dot = A * q
# Time derivative of the state as a function of state
state_dot = sp.Matrix(np.zeros((dim_state, 1)))
state_dot[States.ECEF_POS, :] = v
state_dot[States.ECEF_ORIENTATION, :] = q_dot
state_dot[States.ECEF_VELOCITY, 0] = quat_rot * acceleration
# Basic descretization, 1st order intergrator
# Can be pretty bad if dt is big
f_sym = state + dt * state_dot
state_err_sym = sp.MatrixSymbol('state_err', dim_state_err, 1)
state_err = sp.Matrix(state_err_sym)
quat_err = state_err[States.ECEF_ORIENTATION_ERR, :]
v_err = state_err[States.ECEF_VELOCITY_ERR, :]
omega_err = state_err[States.ANGULAR_VELOCITY_ERR, :]
acceleration_err = state_err[States.ACCELERATION_ERR, :]
# Time derivative of the state error as a function of state error and state
quat_err_matrix = euler_rotate(quat_err[0], quat_err[1], quat_err[2])
q_err_dot = quat_err_matrix * quat_rot * (omega + omega_err)
state_err_dot = sp.Matrix(np.zeros((dim_state_err, 1)))
state_err_dot[States.ECEF_POS_ERR, :] = v_err
state_err_dot[States.ECEF_ORIENTATION_ERR, :] = q_err_dot
state_err_dot[States.ECEF_VELOCITY_ERR, :] = quat_err_matrix * quat_rot * (acceleration + acceleration_err)
f_err_sym = state_err + dt * state_err_dot
# Observation matrix modifier
H_mod_sym = sp.Matrix(np.zeros((dim_state, dim_state_err)))
H_mod_sym[States.ECEF_POS, States.ECEF_POS_ERR] = np.eye(States.ECEF_POS.stop - States.ECEF_POS.start)
H_mod_sym[States.ECEF_ORIENTATION, States.ECEF_ORIENTATION_ERR] = 0.5 * quat_matrix_r(state[3:7])[:, 1:]
H_mod_sym[States.ECEF_ORIENTATION.stop:, States.ECEF_ORIENTATION_ERR.stop:] = np.eye(dim_state - States.ECEF_ORIENTATION.stop)
# these error functions are defined so that say there
# is a nominal x and true x:
# true x = err_function(nominal x, delta x)
# delta x = inv_err_function(nominal x, true x)
nom_x = sp.MatrixSymbol('nom_x', dim_state, 1)
true_x = sp.MatrixSymbol('true_x', dim_state, 1)
delta_x = sp.MatrixSymbol('delta_x', dim_state_err, 1)
err_function_sym = sp.Matrix(np.zeros((dim_state, 1)))
delta_quat = sp.Matrix(np.ones(4))
delta_quat[1:, :] = sp.Matrix(0.5 * delta_x[States.ECEF_ORIENTATION_ERR, :])
err_function_sym[States.ECEF_POS, :] = sp.Matrix(nom_x[States.ECEF_POS, :] + delta_x[States.ECEF_POS_ERR, :])
err_function_sym[States.ECEF_ORIENTATION, 0] = quat_matrix_r(nom_x[States.ECEF_ORIENTATION, 0]) * delta_quat
err_function_sym[States.ECEF_ORIENTATION.stop:, :] = sp.Matrix(nom_x[States.ECEF_ORIENTATION.stop:, :] + delta_x[States.ECEF_ORIENTATION_ERR.stop:, :])
inv_err_function_sym = sp.Matrix(np.zeros((dim_state_err, 1)))
inv_err_function_sym[States.ECEF_POS_ERR, 0] = sp.Matrix(-nom_x[States.ECEF_POS, 0] + true_x[States.ECEF_POS, 0])
delta_quat = quat_matrix_r(nom_x[States.ECEF_ORIENTATION, 0]).T * true_x[States.ECEF_ORIENTATION, 0]
inv_err_function_sym[States.ECEF_ORIENTATION_ERR, 0] = sp.Matrix(2 * delta_quat[1:])
inv_err_function_sym[States.ECEF_ORIENTATION_ERR.stop:, 0] = sp.Matrix(-nom_x[States.ECEF_ORIENTATION.stop:, 0] + true_x[States.ECEF_ORIENTATION.stop:, 0])
eskf_params = [[err_function_sym, nom_x, delta_x],
[inv_err_function_sym, nom_x, true_x],
H_mod_sym, f_err_sym, state_err_sym]
#
# Observation functions
#
h_gyro_sym = sp.Matrix([
vroll + roll_bias,
vpitch + pitch_bias,
vyaw + yaw_bias])
pos = sp.Matrix([x, y, z])
gravity = quat_rot.T * ((EARTH_GM / ((x**2 + y**2 + z**2)**(3.0 / 2.0))) * pos)
h_acc_sym = (gravity + acceleration + acc_bias)
h_acc_stationary_sym = acceleration
h_phone_rot_sym = sp.Matrix([vroll, vpitch, vyaw])
h_pos_sym = sp.Matrix([x, y, z])
h_vel_sym = sp.Matrix([vx, vy, vz])
h_orientation_sym = q
h_relative_motion = sp.Matrix(quat_rot.T * v)
obs_eqs = [[h_gyro_sym, ObservationKind.PHONE_GYRO, None],
[h_phone_rot_sym, ObservationKind.NO_ROT, None],
[h_acc_sym, ObservationKind.PHONE_ACCEL, None],
[h_pos_sym, ObservationKind.ECEF_POS, None],
[h_vel_sym, ObservationKind.ECEF_VEL, None],
[h_orientation_sym, ObservationKind.ECEF_ORIENTATION_FROM_GPS, None],
[h_relative_motion, ObservationKind.CAMERA_ODO_TRANSLATION, None],
[h_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, None],
[h_acc_stationary_sym, ObservationKind.NO_ACCEL, None]]
# this returns a sympy routine for the jacobian of the observation function of the local vel
in_vec = sp.MatrixSymbol('in_vec', 6, 1) # roll, pitch, yaw, vx, vy, vz
h = euler_rotate(in_vec[0], in_vec[1], in_vec[2]).T * (sp.Matrix([in_vec[3], in_vec[4], in_vec[5]]))
extra_routines = [('H', h.jacobian(in_vec), [in_vec])]
gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state_err, eskf_params, extra_routines=extra_routines)
# write constants to extra header file for use in cpp
orbit_header = "#pragma once\n\n"
orbit_header += "#include <unordered_map>\n"
orbit_header += "#include <eigen3/Eigen/Dense>\n\n"
for state, slc in inspect.getmembers(States, lambda x: isinstance(x, slice)):
assert(slc.step is None) # unsupported
orbit_header += f'#define STATE_{state}_START {slc.start}\n'
orbit_header += f'#define STATE_{state}_END {slc.stop}\n'
orbit_header += f'#define STATE_{state}_LEN {slc.stop - slc.start}\n'
orbit_header += "\n"
for kind, val in inspect.getmembers(ObservationKind, lambda x: isinstance(x, int)):
orbit_header += f'#define OBSERVATION_{kind} {val}\n'
orbit_header += "\n"
orbit_header += f"static const Eigen::VectorXd orbit_initial_x = {numpy2eigenstring(OrbitScopeModel.initial_x)};\n"
orbit_header += f"static const Eigen::VectorXd orbit_initial_P_diag = {numpy2eigenstring(OrbitScopeModel.initial_P_diag)};\n"
orbit_header += f"static const Eigen::VectorXd orbit_fake_gps_pos_cov_diag = {numpy2eigenstring(OrbitScopeModel.fake_gps_pos_cov_diag)};\n"
orbit_header += f"static const Eigen::VectorXd orbit_fake_gps_vel_cov_diag = {numpy2eigenstring(OrbitScopeModel.fake_gps_vel_cov_diag)};\n"
orbit_header += f"static const Eigen::VectorXd orbit_reset_orientation_diag = {numpy2eigenstring(OrbitScopeModel.reset_orientation_diag)};\n"
orbit_header += f"static const Eigen::VectorXd orbit_Q_diag = {numpy2eigenstring(OrbitScopeModel.Q_diag)};\n"
orbit_header += "static const std::unordered_map<int, Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> orbit_obs_noise_diag = {\n"
for kind, noise in OrbitScopeModel.obs_noise_diag.items():
orbit_header += f" {{ {kind}, {numpy2eigenstring(noise)} }},\n"
orbit_header += "};\n\n"
open(os.path.join(generated_dir, "orbit_state_constants.h"), 'w').write(orbit_header)
if __name__ == "__main__":
generated_dir = sys.argv[2]
OrbitScopeModel.generate_code(generated_dir)

42
iqpilot/selfdrive/iqlocd/models/orbit_kf_constants.h Normal file
View File

@@ -0,0 +1,42 @@
/*
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
*/
#pragma once
#define STATE_ECEF_POS_START 0
#define STATE_ECEF_POS_LEN 3
#define STATE_ECEF_ORIENTATION_START 3
#define STATE_ECEF_ORIENTATION_LEN 4
#define STATE_ECEF_VELOCITY_START 7
#define STATE_ECEF_VELOCITY_LEN 3
#define STATE_ANGULAR_VELOCITY_START 10
#define STATE_ANGULAR_VELOCITY_LEN 3
#define STATE_GYRO_BIAS_START 13
#define STATE_GYRO_BIAS_LEN 3
#define STATE_ACCELERATION_START 16
#define STATE_ACCELERATION_LEN 3
#define STATE_ACC_BIAS_START 19
#define STATE_ACC_BIAS_LEN 3
#define STATE_ECEF_POS_ERR_START 0
#define STATE_ECEF_POS_ERR_LEN 3
#define STATE_ECEF_ORIENTATION_ERR_START 3
#define STATE_ECEF_ORIENTATION_ERR_LEN 3
#define STATE_ECEF_VELOCITY_ERR_START 6
#define STATE_ECEF_VELOCITY_ERR_LEN 3
#define STATE_ANGULAR_VELOCITY_ERR_START 9
#define STATE_ANGULAR_VELOCITY_ERR_LEN 3
#define STATE_GYRO_BIAS_ERR_START 12
#define STATE_GYRO_BIAS_ERR_LEN 3
#define STATE_ACCELERATION_ERR_START 15
#define STATE_ACCELERATION_ERR_LEN 3
#define STATE_ACC_BIAS_ERR_START 18
#define STATE_ACC_BIAS_ERR_LEN 3
#define OBSERVATION_PHONE_GYRO 4
#define OBSERVATION_NO_ROT 9
#define OBSERVATION_PHONE_ACCEL 10
#define OBSERVATION_ECEF_POS 12
#define OBSERVATION_CAMERA_ODO_TRANSLATION 13
#define OBSERVATION_CAMERA_ODO_ROTATION 14
#define OBSERVATION_ECEF_ORIENTATION_FROM_GPS 32
#define OBSERVATION_NO_ACCEL 33
#define OBSERVATION_ECEF_VEL 35

24
iqpilot/selfdrive/locationd/SConscript
View File

@@ -1,21 +1,5 @@
Import('env', 'rednose')
Import('env', 'envCython')
# build ekf models
rednose_gen_dir = 'models/generated'
rednose_gen_deps = [
"models/constants.py",
]
pose_ekf = env.RednoseCompileFilter(
target='pose',
filter_gen_script='models/pose_kf.py',
output_dir=rednose_gen_dir,
extra_gen_artifacts=[],
gen_script_deps=rednose_gen_deps,
)
car_ekf = env.RednoseCompileFilter(
target='car',
filter_gen_script='models/car_kf.py',
output_dir=rednose_gen_dir,
extra_gen_artifacts=[],
gen_script_deps=rednose_gen_deps,
)
native_kernel = env.StaticLibrary("../state_estimation/native_kernels", ["../state_estimation/native_kernels.cc"])
native_binding_env = envCython.Clone(CYTHONFLAGS=["--cplus"])
native_binding_env.Program("../state_estimation/native_binding_pyx.so", ["../state_estimation/native_binding_pyx.pyx"], LIBS=[native_kernel] + envCython["LIBS"])

4
iqpilot/selfdrive/locationd/locationd.py
View File

@@ -15,7 +15,7 @@ from iqpilot.common.swaglog import cloudlog
from iqpilot.selfdrive.locationd.calibration_helpers import get_calibrated_rpy
from iqpilot.selfdrive.locationd.helpers import rotate_std
from iqpilot.selfdrive.locationd.models.pose_kf import PoseKalman, States
from iqpilot.selfdrive.locationd.models.constants import ObservationKind, GENERATED_DIR
from iqpilot.selfdrive.locationd.models.constants import ObservationKind
ACCEL_SANITY_CHECK = 100.0 # m/s^2
ROTATION_SANITY_CHECK = 10.0 # rad/s
@@ -51,7 +51,7 @@ class HandleLogResult(Enum):
class LocationEstimator:
def __init__(self, debug: bool):
self.kf = PoseKalman(GENERATED_DIR, MAX_FILTER_REWIND_TIME)
self.kf = PoseKalman(MAX_FILTER_REWIND_TIME)
self.debug = debug

222
iqpilot/selfdrive/locationd/models/car_kf.py Executable file → Normal file
View File

@@ -1,75 +1,63 @@
#!/usr/bin/env python3
"""
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
"""
import math
import sys
from typing import Any
import numpy as np
from iqpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY
from iqpilot.selfdrive.locationd.models.constants import ObservationKind
from iqpilot.common.swaglog import cloudlog
from rednose.helpers.kalmanfilter import KalmanFilter
if __name__ == '__main__': # Generating sympy
import sympy as sp
from rednose.helpers.ekf_sym import gen_code
else:
from rednose.helpers.ekf_sym_pyx import EKF_sym_pyx
i = 0
def _slice(n):
global i
s = slice(i, i + n)
i += n
return s
from iqpilot.selfdrive.state_estimation import EstimatorModel, ModelDefinition, StateEstimator
try:
from iqpilot.selfdrive.state_estimation.native_binding_pyx import car_predict, car_update
except ModuleNotFoundError:
car_predict = None
car_update = None
class States:
# Vehicle model params
STIFFNESS = _slice(1) # [-]
STEER_RATIO = _slice(1) # [-]
ANGLE_OFFSET = _slice(1) # [rad]
ANGLE_OFFSET_FAST = _slice(1) # [rad]
VELOCITY = _slice(2) # (x, y) [m/s]
YAW_RATE = _slice(1) # [rad/s]
STEER_ANGLE = _slice(1) # [rad]
ROAD_ROLL = _slice(1) # [rad]
STIFFNESS = slice(0, 1)
STEER_RATIO = slice(1, 2)
ANGLE_OFFSET = slice(2, 3)
ANGLE_OFFSET_FAST = slice(3, 4)
VELOCITY = slice(4, 6)
YAW_RATE = slice(6, 7)
STEER_ANGLE = slice(7, 8)
ROAD_ROLL = slice(8, 9)
class CarKalman(KalmanFilter):
name = 'car'
def _transition(state: np.ndarray, dt: float, values: dict[str, float]) -> np.ndarray:
result = state.copy()
stiffness = state[0]
steer_ratio = state[1]
angle = state[7] - state[2] - state[3]
speed, lateral_speed = state[4:6]
yaw_rate = state[6]
mass = values["mass"]
inertia = values["rotational_inertia"]
front = values["center_to_front"]
rear = values["center_to_rear"]
front_stiffness = stiffness * values["stiffness_front"]
rear_stiffness = stiffness * values["stiffness_rear"]
lateral_dot = -(front_stiffness + rear_stiffness) * lateral_speed / (mass * speed)
lateral_dot += (-(front_stiffness * front - rear_stiffness * rear) / (mass * speed) - speed) * yaw_rate
lateral_dot += front_stiffness * angle / (mass * steer_ratio) - ACCELERATION_DUE_TO_GRAVITY * state[8]
yaw_dot = -(front_stiffness * front - rear_stiffness * rear) * lateral_speed / (inertia * speed)
yaw_dot -= (front_stiffness * front**2 + rear_stiffness * rear**2) * yaw_rate / (inertia * speed)
yaw_dot += front_stiffness * front * angle / (inertia * steer_ratio)
result[5] += dt * lateral_dot
result[6] += dt * yaw_dot
return result
initial_x = np.array([
1.0,
15.0,
0.0,
0.0,
10.0, 0.0,
0.0,
0.0,
0.0
])
# process noise
Q = np.diag([
(.05 / 100)**2,
.01**2,
math.radians(0.02)**2,
math.radians(0.25)**2,
.1**2, .01**2,
math.radians(0.1)**2,
math.radians(0.1)**2,
math.radians(1)**2,
])
class CarKalman(EstimatorModel):
name = "car"
initial_x = np.array([1.0, 15.0, 0.0, 0.0, 10.0, 0.0, 0.0, 0.0, 0.0])
Q = np.diag([(.05 / 100)**2, .01**2, math.radians(0.02)**2, math.radians(0.25)**2,
.1**2, .01**2, math.radians(0.1)**2, math.radians(0.1)**2, math.radians(1)**2])
P_initial = Q.copy()
obs_noise: dict[int, Any] = {
ObservationKind.STEER_ANGLE: np.atleast_2d(math.radians(0.05)**2),
ObservationKind.ANGLE_OFFSET_FAST: np.atleast_2d(math.radians(10.0)**2),
@@ -79,102 +67,28 @@ class CarKalman(KalmanFilter):
ObservationKind.ROAD_FRAME_X_SPEED: np.atleast_2d(0.1**2),
}
global_vars = [
'mass',
'rotational_inertia',
'center_to_front',
'center_to_rear',
'stiffness_front',
'stiffness_rear',
]
def __init__(self):
self.native_parameters = np.zeros(6)
measurements = {
ObservationKind.ROAD_FRAME_YAW_RATE: lambda state, _: state[6:7],
ObservationKind.ROAD_FRAME_XY_SPEED: lambda state, _: state[4:6],
ObservationKind.ROAD_FRAME_X_SPEED: lambda state, _: state[4:5],
ObservationKind.STEER_ANGLE: lambda state, _: state[7:8],
ObservationKind.ANGLE_OFFSET_FAST: lambda state, _: state[3:4],
ObservationKind.STEER_RATIO: lambda state, _: state[1:2],
ObservationKind.STIFFNESS: lambda state, _: state[0:1],
ObservationKind.ROAD_ROLL: lambda state, _: state[8:9],
}
def native_predict(state, covariance, dt, process_noise, _):
car_predict(state, covariance, process_noise, dt, self.native_parameters)
@staticmethod
def generate_code(generated_dir):
dim_state = CarKalman.initial_x.shape[0]
name = CarKalman.name
model = ModelDefinition(9, 9, _transition, measurements, self.Q, self.obs_noise,
native_predict=native_predict if car_predict is not None else None, native_update=car_update)
super().__init__(StateEstimator(model, self.initial_x, self.P_initial, max_rewind_age=0.8))
# Linearized single-track lateral dynamics, equations 7.211-7.213
# Massimo Guiggiani, The Science of Vehicle Dynamics: Handling, Braking, and Ride of Road and Race Cars
# Springer Cham, 2023. doi: https://doi.org/10.1007/978-3-031-06461-6
# globals
global_vars = [sp.Symbol(name) for name in CarKalman.global_vars]
m, j, aF, aR, cF_orig, cR_orig = global_vars
# make functions and jacobians with sympy
# state variables
state_sym = sp.MatrixSymbol('state', dim_state, 1)
state = sp.Matrix(state_sym)
# Vehicle model constants
sf = state[States.STIFFNESS, :][0, 0]
cF, cR = sf * cF_orig, sf * cR_orig
angle_offset = state[States.ANGLE_OFFSET, :][0, 0]
angle_offset_fast = state[States.ANGLE_OFFSET_FAST, :][0, 0]
theta = state[States.ROAD_ROLL, :][0, 0]
sa = state[States.STEER_ANGLE, :][0, 0]
sR = state[States.STEER_RATIO, :][0, 0]
u, v = state[States.VELOCITY, :]
r = state[States.YAW_RATE, :][0, 0]
A = sp.Matrix(np.zeros((2, 2)))
A[0, 0] = -(cF + cR) / (m * u)
A[0, 1] = -(cF * aF - cR * aR) / (m * u) - u
A[1, 0] = -(cF * aF - cR * aR) / (j * u)
A[1, 1] = -(cF * aF**2 + cR * aR**2) / (j * u)
B = sp.Matrix(np.zeros((2, 1)))
B[0, 0] = cF / m / sR
B[1, 0] = (cF * aF) / j / sR
C = sp.Matrix(np.zeros((2, 1)))
C[0, 0] = ACCELERATION_DUE_TO_GRAVITY
C[1, 0] = 0
x = sp.Matrix([v, r]) # lateral velocity, yaw rate
x_dot = A * x + B * (sa - angle_offset - angle_offset_fast) - C * theta
dt = sp.Symbol('dt')
state_dot = sp.Matrix(np.zeros((dim_state, 1)))
state_dot[States.VELOCITY.start + 1, 0] = x_dot[0]
state_dot[States.YAW_RATE.start, 0] = x_dot[1]
# Basic descretization, 1st order integrator
# Can be pretty bad if dt is big
f_sym = state + dt * state_dot
#
# Observation functions
#
obs_eqs = [
[sp.Matrix([r]), ObservationKind.ROAD_FRAME_YAW_RATE, None],
[sp.Matrix([u, v]), ObservationKind.ROAD_FRAME_XY_SPEED, None],
[sp.Matrix([u]), ObservationKind.ROAD_FRAME_X_SPEED, None],
[sp.Matrix([sa]), ObservationKind.STEER_ANGLE, None],
[sp.Matrix([angle_offset_fast]), ObservationKind.ANGLE_OFFSET_FAST, None],
[sp.Matrix([sR]), ObservationKind.STEER_RATIO, None],
[sp.Matrix([sf]), ObservationKind.STIFFNESS, None],
[sp.Matrix([theta]), ObservationKind.ROAD_ROLL, None],
]
gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state, global_vars=global_vars)
def __init__(self, generated_dir):
dim_state, dim_state_err = CarKalman.initial_x.shape[0], CarKalman.P_initial.shape[0]
self.filter = EKF_sym_pyx(generated_dir, CarKalman.name, CarKalman.Q, CarKalman.initial_x, CarKalman.P_initial,
dim_state, dim_state_err, global_vars=CarKalman.global_vars, logger=cloudlog)
def set_globals(self, mass, rotational_inertia, center_to_front, center_to_rear, stiffness_front, stiffness_rear):
self.filter.set_global("mass", mass)
self.filter.set_global("rotational_inertia", rotational_inertia)
self.filter.set_global("center_to_front", center_to_front)
self.filter.set_global("center_to_rear", center_to_rear)
self.filter.set_global("stiffness_front", stiffness_front)
self.filter.set_global("stiffness_rear", stiffness_rear)
if __name__ == "__main__":
generated_dir = sys.argv[2]
CarKalman.generate_code(generated_dir)
def set_globals(self, mass: float, rotational_inertia: float, center_to_front: float, center_to_rear: float,
stiffness_front: float, stiffness_rear: float) -> None:
self.native_parameters[:] = mass, rotational_inertia, center_to_front, center_to_rear, stiffness_front, stiffness_rear
for name, value in locals().copy().items():
if name not in {"self"}:
self.filter.set_global(name, value)

4
iqpilot/selfdrive/locationd/models/constants.py
View File

@@ -1,7 +1,3 @@
import os
GENERATED_DIR = os.path.abspath(os.path.join(os.path.dirname(__file__), 'generated'))
class ObservationKind:
UNKNOWN = 0
NO_OBSERVATION = 1

148
iqpilot/selfdrive/locationd/models/pose_kf.py Executable file → Normal file
View File

@@ -1,111 +1,67 @@
#!/usr/bin/env python3
"""
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
"""
import sys
import numpy as np
from iqpilot.common.transformations.orientation import euler_from_rot, rot_from_euler
from iqpilot.selfdrive.locationd.models.constants import ObservationKind
from iqpilot.selfdrive.state_estimation import EstimatorModel, ModelDefinition, StateEstimator
try:
from iqpilot.selfdrive.state_estimation.native_binding_pyx import pose_predict, pose_update
except ModuleNotFoundError:
pose_predict = None
pose_update = None
from rednose.helpers.kalmanfilter import KalmanFilter
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
NED_ORIENTATION = slice(0, 3)
DEVICE_VELOCITY = slice(3, 6)
ANGULAR_VELOCITY = slice(6, 9)
GYRO_BIAS = slice(9, 12)
ACCELERATION = slice(12, 15)
ACCEL_BIAS = slice(15, 18)
class PoseKalman(KalmanFilter):
def _transition(state: np.ndarray, dt: float, _: dict[str, float]) -> np.ndarray:
result = state.copy()
result[States.DEVICE_VELOCITY] += dt * state[States.ACCELERATION]
rotation = rot_from_euler(state[States.NED_ORIENTATION]) @ rot_from_euler(dt * state[States.ANGULAR_VELOCITY])
result[States.NED_ORIENTATION] = euler_from_rot(rotation)
return result
def _phone_acceleration(state: np.ndarray, _: dict[str, float]) -> np.ndarray:
device_from_ned = rot_from_euler(state[States.NED_ORIENTATION]).T
centripetal = np.cross(state[States.ANGULAR_VELOCITY], state[States.DEVICE_VELOCITY])
return device_from_ned @ np.array([0.0, 0.0, -EARTH_G]) + state[States.ACCELERATION] + centripetal + state[States.ACCEL_BIAS]
class PoseKalman(EstimatorModel):
name = "pose"
initial_x = np.zeros(18)
initial_P = np.diag([0.01**2] * 3 + [10**2] * 3 + [1**2] * 6 + [100**2] * 3 + [0.01**2] * 3)
Q = np.diag([0.001**2] * 3 + [0.01**2] * 3 + [0.1**2] * 3 + [(0.005 / 100)**2] * 3 + [3**2] * 3 + [0.005**2] * 3)
obs_noise = {
ObservationKind.PHONE_GYRO: np.diag([0.025**2] * 3),
ObservationKind.PHONE_ACCEL: np.diag([0.5**2] * 3),
ObservationKind.CAMERA_ODO_TRANSLATION: np.diag([0.5**2] * 3),
ObservationKind.CAMERA_ODO_ROTATION: np.diag([0.05**2] * 3),
}
# 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])
def __init__(self, max_rewind_age: float):
measurements = {
ObservationKind.PHONE_GYRO: lambda state, _: state[States.ANGULAR_VELOCITY] + state[States.GYRO_BIAS],
ObservationKind.PHONE_ACCEL: _phone_acceleration,
ObservationKind.CAMERA_ODO_TRANSLATION: lambda state, _: state[States.DEVICE_VELOCITY],
ObservationKind.CAMERA_ODO_ROTATION: lambda state, _: state[States.ANGULAR_VELOCITY],
}
def native_predict(state, covariance, dt, process_noise, _):
pose_predict(state, covariance, process_noise, dt)
# 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])
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])}
@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)
model = ModelDefinition(18, 18, _transition, measurements, self.Q, self.obs_noise,
native_predict=native_predict if pose_predict is not None else None, native_update=pose_update)
super().__init__(StateEstimator(model, self.initial_x, self.initial_P, max_rewind_age=max_rewind_age))

3
iqpilot/selfdrive/locationd/paramsd.py
View File

@@ -8,7 +8,6 @@ from iqpilot.common.issue_debug import log_issue_limited
from iqpilot.common.params import Params
from iqpilot.common.realtime import DT_MDL
from iqpilot.selfdrive.locationd.models.car_kf import CarKalman, ObservationKind, States
from iqpilot.selfdrive.locationd.models.constants import GENERATED_DIR
from iqpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose
from iqpilot.common.swaglog import cloudlog
@@ -26,7 +25,7 @@ LOW_ACTIVE_SPEED = 10.0
class VehicleParamsEstimator:
def __init__(self, CP: car.CarParams, steer_ratio: float, stiffness_factor: float, angle_offset: float, P_initial: np.ndarray | None = None):
self.kf = CarKalman(GENERATED_DIR)
self.kf = CarKalman()
self.x_initial = CarKalman.initial_x.copy()
self.x_initial[States.STEER_RATIO] = steer_ratio

7
iqpilot/selfdrive/state_estimation/__init__.py Normal file
View File

@@ -0,0 +1,7 @@
"""
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
"""
from iqpilot.selfdrive.state_estimation.estimator import EstimatorModel, ModelDefinition, Observation, StateEstimator
__all__ = ["EstimatorModel", "ModelDefinition", "Observation", "StateEstimator"]

38
iqpilot/selfdrive/state_estimation/benchmark.py Normal file
View File

@@ -0,0 +1,38 @@
"""
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
"""
import json
import time
import numpy as np
from iqpilot.selfdrive.locationd.models.car_kf import CarKalman
from iqpilot.selfdrive.locationd.models.constants import ObservationKind
from iqpilot.selfdrive.locationd.models.pose_kf import PoseKalman
def measure(function, count: int) -> dict[str, float]:
samples = np.empty(count)
for index in range(count):
started = time.perf_counter_ns()
function(index)
samples[index] = (time.perf_counter_ns() - started) / 1000.0
return {"p50_us": float(np.percentile(samples, 50)), "p99_us": float(np.percentile(samples, 99)), "mean_us": float(samples.mean())}
def main() -> None:
car = CarKalman()
car.set_globals(1800.0, 2500.0, 1.2, 1.6, 90000.0, 100000.0)
car.init_state(CarKalman.initial_x, CarKalman.P_initial, 0.0)
pose = PoseKalman(0.8)
pose.init_state(PoseKalman.initial_x, PoseKalman.initial_P, 0.0)
result = {
"car": measure(lambda index: car.predict_and_observe(index * 0.01, ObservationKind.ROAD_FRAME_X_SPEED, np.array([15.0])), 1000),
"pose": measure(lambda index: pose.predict_and_observe(index * 0.01, ObservationKind.PHONE_GYRO, np.zeros(3)), 1000),
}
print(json.dumps(result, sort_keys=True))
if __name__ == "__main__":
main()

157
iqpilot/selfdrive/state_estimation/estimator.h Normal file
View File

@@ -0,0 +1,157 @@
/*
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
*/
#pragma once
#include <cmath>
#include <functional>
#include <optional>
#include <stdexcept>
#include <unordered_map>
#include <vector>
#include <eigen3/Eigen/Dense>
namespace iqpilot::state_estimation {
using Matrix = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
using Vector = Eigen::VectorXd;
struct Estimate {
double time;
Vector state;
Matrix covariance;
std::vector<Vector> innovations;
};
struct ModelDefinition {
int state_size;
int error_size;
std::function<Vector(const Vector &, double)> transition;
std::unordered_map<int, std::function<Vector(const Vector &)>> measurements;
Matrix process_noise;
std::unordered_map<int, Matrix> observation_noise;
std::function<Vector(const Vector &, const Vector &)> inject_error;
std::function<Matrix(const Vector &)> error_projection;
std::function<Vector(const Vector &)> normalize;
std::function<Matrix(const Vector &, double)> error_transition;
std::unordered_map<int, std::function<Matrix(const Vector &)>> observation_jacobians;
};
class StateEstimator {
public:
StateEstimator(ModelDefinition model, Vector state, Matrix covariance) : model_(std::move(model)) {
init_state(state, covariance, NAN);
}
void init_state(const Vector &state, const Matrix &covariance, double time) {
if (state.size() != model_.state_size || covariance.rows() != model_.error_size || covariance.cols() != model_.error_size) {
throw std::invalid_argument("estimator initialization dimension mismatch");
}
state_ = normalize(state);
covariance_ = stabilize(covariance);
time_ = time;
}
void predict(double time) {
if (std::isnan(time_)) {
time_ = time;
return;
}
if (time < time_) {
throw std::invalid_argument("prediction time precedes estimator time");
}
const double dt = time - time_;
if (dt == 0.0) return;
const Vector previous = state_;
const Vector predicted = model_.transition(previous, dt);
Matrix error_transition;
if (model_.error_transition) {
error_transition = model_.error_transition(previous, dt);
} else {
const Matrix state_jacobian = jacobian([this, dt](const Vector &value) { return model_.transition(value, dt); }, previous);
error_transition = error_projection(predicted).completeOrthogonalDecomposition().pseudoInverse() * state_jacobian * error_projection(previous);
}
state_ = normalize(predicted);
covariance_ = error_transition * covariance_ * error_transition.transpose() + dt * model_.process_noise;
time_ = time;
}
std::optional<Estimate> predict_and_observe(double time, int kind, const std::vector<Vector> &measurements,
const std::vector<Matrix> &noise = {}) {
if (!std::isnan(time_) && time < time_) return std::nullopt;
predict(time);
auto measurement_function = model_.measurements.find(kind);
if (measurement_function == model_.measurements.end()) throw std::invalid_argument("unknown observation kind");
std::vector<Vector> innovations;
for (size_t index = 0; index < measurements.size(); ++index) {
const Matrix &measurement_noise = noise.empty() ? model_.observation_noise.at(kind) : noise.at(index);
const Vector expected = measurement_function->second(state_);
if (measurements[index].size() != expected.size() || measurement_noise.rows() != expected.size() || measurement_noise.cols() != expected.size()) {
throw std::invalid_argument("observation dimension mismatch");
}
const Vector innovation = measurements[index] - expected;
Matrix observation_jacobian;
auto analytic_jacobian = model_.observation_jacobians.find(kind);
if (analytic_jacobian != model_.observation_jacobians.end()) {
observation_jacobian = analytic_jacobian->second(state_);
} else {
const Matrix state_jacobian = jacobian(measurement_function->second, state_);
observation_jacobian = state_jacobian * error_projection(state_);
}
const Matrix innovation_covariance = observation_jacobian * covariance_ * observation_jacobian.transpose() + measurement_noise;
const Matrix gain = innovation_covariance.ldlt().solve(observation_jacobian * covariance_).transpose();
state_ = normalize(inject(state_, gain * innovation));
const Matrix identity = Matrix::Identity(model_.error_size, model_.error_size);
const Matrix residual = identity - gain * observation_jacobian;
covariance_ = residual * covariance_ * residual.transpose() + gain * measurement_noise * gain.transpose();
if (!state_.allFinite() || !covariance_.allFinite()) throw std::runtime_error("estimator produced non-finite values");
innovations.push_back(innovation);
}
return Estimate{time_, state_, covariance_, innovations};
}
const Vector &state() const { return state_; }
const Matrix &covariance() const { return covariance_; }
double time() const { return time_; }
private:
Matrix jacobian(const std::function<Vector(const Vector &)> &function, const Vector &value) const {
const Vector output = function(value);
Matrix result(output.size(), value.size());
for (int index = 0; index < value.size(); ++index) {
const double step = std::cbrt(Eigen::NumTraits<double>::epsilon()) * std::max(1.0, std::abs(value(index)));
Vector upper = value;
Vector lower = value;
upper(index) += step;
lower(index) -= step;
result.col(index) = (function(upper) - function(lower)) / (2.0 * step);
}
return result;
}
Vector inject(const Vector &state, const Vector &delta) const {
return model_.inject_error ? model_.inject_error(state, delta) : state + delta;
}
Matrix error_projection(const Vector &state) const {
return model_.error_projection ? model_.error_projection(state) : Matrix::Identity(model_.state_size, model_.error_size);
}
Vector normalize(const Vector &state) const {
return model_.normalize ? model_.normalize(state) : state;
}
Matrix stabilize(const Matrix &covariance) const {
Matrix symmetric = (covariance + covariance.transpose()) * 0.5;
if (!symmetric.allFinite()) throw std::runtime_error("invalid covariance");
return symmetric;
}
ModelDefinition model_;
Vector state_;
Matrix covariance_;
double time_ = NAN;
};
}

Some files were not shown because too many files have changed in this diff Show More