diff --git a/.gitignore b/.gitignore index cb1b91cfa..3b0cb3f47 100644 --- a/.gitignore +++ b/.gitignore @@ -29,7 +29,6 @@ a.out /iqdbc /msgq /openpilot -/rednose /teleoprtc /tinygrad diff --git a/SConstruct b/SConstruct index 88706fa77..cd6c9981d 100644 --- a/SConstruct +++ b/SConstruct @@ -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', diff --git a/artifacts/iqpilot_hephaestusd_private/manifest.json b/artifacts/iqpilot_hephaestusd_private/manifest.json index 730bc0cc4..080d369fe 100644 --- a/artifacts/iqpilot_hephaestusd_private/manifest.json +++ b/artifacts/iqpilot_hephaestusd_private/manifest.json @@ -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==" } } diff --git a/artifacts/iqpilot_hephaestusd_private/manifest.json.sig b/artifacts/iqpilot_hephaestusd_private/manifest.json.sig index c4378d3c6..aa65720a7 100644 --- a/artifacts/iqpilot_hephaestusd_private/manifest.json.sig +++ b/artifacts/iqpilot_hephaestusd_private/manifest.json.sig @@ -1 +1 @@ -HlmJ/WkPaO0L0omQG7BWUtHYKzp2ZTFvaVu4BDpACrkluSoWeyX0suOZlU29fJvRI+nWhBEkRW7807toLyrNAQ== +aT5ZgWVbq0qWHt3m8wQmPWVfaFivYO6zCFB2LxmMC6bn1ZS+k/tNYZTbIANNFwhKGM4Q6zdQeGVicoDreqtYBw== diff --git a/artifacts/iqpilot_hephaestusd_private/python/_iqclosure/iqpilot/common/realtime.py b/artifacts/iqpilot_hephaestusd_private/python/_iqclosure/iqpilot/common/realtime.py index c2c09295a..9fd2f695d 100644 --- a/artifacts/iqpilot_hephaestusd_private/python/_iqclosure/iqpilot/common/realtime.py +++ b/artifacts/iqpilot_hephaestusd_private/python/_iqclosure/iqpilot/common/realtime.py @@ -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.""" diff --git a/artifacts/iqpilot_hephaestusd_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/__init__.py b/artifacts/iqpilot_hephaestusd_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/__init__.py index c7d1e92cc..5ece1c074 100644 --- a/artifacts/iqpilot_hephaestusd_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/__init__.py +++ b/artifacts/iqpilot_hephaestusd_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/__init__.py @@ -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/ """ diff --git a/artifacts/iqpilot_hephaestusd_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/fetcher.py b/artifacts/iqpilot_hephaestusd_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/fetcher.py index 8ce6aff59..d06c7c3dd 100644 --- a/artifacts/iqpilot_hephaestusd_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/fetcher.py +++ b/artifacts/iqpilot_hephaestusd_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/fetcher.py @@ -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: diff --git a/artifacts/iqpilot_hephaestusd_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/helpers.py b/artifacts/iqpilot_hephaestusd_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/helpers.py index 191d8a1b1..186c9d623 100644 --- a/artifacts/iqpilot_hephaestusd_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/helpers.py +++ b/artifacts/iqpilot_hephaestusd_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/helpers.py @@ -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}") diff --git a/artifacts/iqpilot_hephaestusd_private/python/_iqclosure/iqpilot/selfdrive/selfdrived/events.py b/artifacts/iqpilot_hephaestusd_private/python/_iqclosure/iqpilot/selfdrive/selfdrived/events.py index f67aab3c3..2e36bfe34 100644 --- a/artifacts/iqpilot_hephaestusd_private/python/_iqclosure/iqpilot/selfdrive/selfdrived/events.py +++ b/artifacts/iqpilot_hephaestusd_private/python/_iqclosure/iqpilot/selfdrive/selfdrived/events.py @@ -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. diff --git a/artifacts/iqpilot_hephaestusd_private/python/_iqclosure/iqpilot/system/manager/process_config.py b/artifacts/iqpilot_hephaestusd_private/python/_iqclosure/iqpilot/system/manager/process_config.py index 0ac3555fe..7e1492154 100644 --- a/artifacts/iqpilot_hephaestusd_private/python/_iqclosure/iqpilot/system/manager/process_config.py +++ b/artifacts/iqpilot_hephaestusd_private/python/_iqclosure/iqpilot/system/manager/process_config.py @@ -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)), diff --git a/artifacts/iqpilot_hephaestusd_private/python/_iqclosure/iqpilot/system/ui/lib/os_update.py b/artifacts/iqpilot_hephaestusd_private/python/_iqclosure/iqpilot/system/ui/lib/os_update.py index 738b9157f..806beee73 100644 --- a/artifacts/iqpilot_hephaestusd_private/python/_iqclosure/iqpilot/system/ui/lib/os_update.py +++ b/artifacts/iqpilot_hephaestusd_private/python/_iqclosure/iqpilot/system/ui/lib/os_update.py @@ -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 diff --git a/artifacts/iqpilot_hephaestusd_private/python/_iqclosure/iqpilot/system/version.py b/artifacts/iqpilot_hephaestusd_private/python/_iqclosure/iqpilot/system/version.py index 980a250bd..cb731d009 100644 --- a/artifacts/iqpilot_hephaestusd_private/python/_iqclosure/iqpilot/system/version.py +++ b/artifacts/iqpilot_hephaestusd_private/python/_iqclosure/iqpilot/system/version.py @@ -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 diff --git a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/backups/archive_codec.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/backups/archive_codec.cpython-312-aarch64-linux-gnu.so index d31e8e105..9e509d962 100755 Binary files a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/backups/archive_codec.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/backups/archive_codec.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/backups/backup_keys.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/backups/backup_keys.cpython-312-aarch64-linux-gnu.so index 3b626e98a..b9dd2e505 100755 Binary files a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/backups/backup_keys.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/backups/backup_keys.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/backups/backup_orchestrator.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/backups/backup_orchestrator.cpython-312-aarch64-linux-gnu.so index 728e491c1..c13f3ee64 100755 Binary files a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/backups/backup_orchestrator.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/backups/backup_orchestrator.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/backups/cbc_vault.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/backups/cbc_vault.cpython-312-aarch64-linux-gnu.so index 2d09d1131..d00dd8fa1 100755 Binary files a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/backups/cbc_vault.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/backups/cbc_vault.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/backups/imahelper.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/backups/imahelper.cpython-312-aarch64-linux-gnu.so index 4014ef7a3..e756cdcca 100755 Binary files a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/backups/imahelper.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/backups/imahelper.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/flockd/flockd.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/flockd/flockd.cpython-312-aarch64-linux-gnu.so index 5e3a5d067..7a685ad8a 100755 Binary files a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/flockd/flockd.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/flockd/flockd.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/flockd/signatures.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/flockd/signatures.cpython-312-aarch64-linux-gnu.so index b54f83a81..53ac0901d 100755 Binary files a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/flockd/signatures.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/flockd/signatures.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/_vendor/localapi_runtime.zip b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/_vendor/localapi_runtime.zip index 1e140de80..4b8b44f6c 100644 Binary files a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/_vendor/localapi_runtime.zip and b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/_vendor/localapi_runtime.zip differ diff --git a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/ble_auth.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/ble_auth.cpython-312-aarch64-linux-gnu.so index 8c3cfafcd..97dd631d2 100755 Binary files a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/ble_auth.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/ble_auth.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/ble_gatt.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/ble_gatt.cpython-312-aarch64-linux-gnu.so index 63217dc34..24f76e3b5 100755 Binary files a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/ble_gatt.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/ble_gatt.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/ble_rpc_dispatch.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/ble_rpc_dispatch.cpython-312-aarch64-linux-gnu.so index 0aff36088..a2c9b1a4f 100755 Binary files a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/ble_rpc_dispatch.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/ble_rpc_dispatch.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/ble_transportd.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/ble_transportd.cpython-312-aarch64-linux-gnu.so index 7117e1e6e..76f4eb238 100755 Binary files a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/ble_transportd.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/ble_transportd.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/bt_gamepad.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/bt_gamepad.cpython-312-aarch64-linux-gnu.so index 585d73905..54bab2f3c 100755 Binary files a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/bt_gamepad.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/bt_gamepad.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/cloud_routes.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/cloud_routes.cpython-312-aarch64-linux-gnu.so index 4ffb115e5..7144fd042 100755 Binary files a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/cloud_routes.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/cloud_routes.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/hephaestusd.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/hephaestusd.cpython-312-aarch64-linux-gnu.so index a8ad800d6..0789a9306 100755 Binary files a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/hephaestusd.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/hephaestusd.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/kwp2000.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/kwp2000.cpython-312-aarch64-linux-gnu.so index 6a39f2e49..e4b8fe4c4 100755 Binary files a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/kwp2000.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/kwp2000.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/manage_hephaestusd.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/manage_hephaestusd.cpython-312-aarch64-linux-gnu.so index fd0e3a50f..8fb55495e 100755 Binary files a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/manage_hephaestusd.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/manage_hephaestusd.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/motd.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/motd.cpython-312-aarch64-linux-gnu.so index 0c5a37f10..f194e08d8 100755 Binary files a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/motd.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/motd.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/tp20.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/tp20.cpython-312-aarch64-linux-gnu.so index cf62c67c3..58c6ae02f 100755 Binary files a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/tp20.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/tp20.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/vw_pq_flasher.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/vw_pq_flasher.cpython-312-aarch64-linux-gnu.so index cd09ea69f..7a31ce003 100755 Binary files a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/vw_pq_flasher.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/vw_pq_flasher.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/uploaderd/iquploaderd.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/uploaderd/iquploaderd.cpython-312-aarch64-linux-gnu.so index 6acb35511..320ff90e2 100755 Binary files a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/uploaderd/iquploaderd.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/uploaderd/iquploaderd.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/package_sources/iqdbc/iqdbc/car/tesla/carcontroller.py b/artifacts/package_sources/iqdbc/iqdbc/car/tesla/carcontroller.py index 5d9653b37..5b91e9371 100644 --- a/artifacts/package_sources/iqdbc/iqdbc/car/tesla/carcontroller.py +++ b/artifacts/package_sources/iqdbc/iqdbc/car/tesla/carcontroller.py @@ -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 diff --git a/artifacts/package_sources/iqdbc/iqdbc/car/tesla/carstate.py b/artifacts/package_sources/iqdbc/iqdbc/car/tesla/carstate.py index 8004eb74d..bc5d0362d 100644 --- a/artifacts/package_sources/iqdbc/iqdbc/car/tesla/carstate.py +++ b/artifacts/package_sources/iqdbc/iqdbc/car/tesla/carstate.py @@ -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")] diff --git a/artifacts/package_sources/iqdbc/iqdbc/lvbs/car/tesla/iq_carstate.py b/artifacts/package_sources/iqdbc/iqdbc/lvbs/car/tesla/iq_carstate.py index 90be56407..357e0c4fd 100644 --- a/artifacts/package_sources/iqdbc/iqdbc/lvbs/car/tesla/iq_carstate.py +++ b/artifacts/package_sources/iqdbc/iqdbc/lvbs/car/tesla/iq_carstate.py @@ -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)) diff --git a/artifacts/package_sources/panda/board/drivers/can_common.h b/artifacts/package_sources/panda/board/drivers/can_common.h index a8b64c457..7d4b57575 100644 --- a/artifacts/package_sources/panda/board/drivers/can_common.h +++ b/artifacts/package_sources/panda/board/drivers/can_common.h @@ -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; } diff --git a/artifacts/package_sources/rednose/.dockerignore b/artifacts/package_sources/rednose/.dockerignore deleted file mode 100644 index 5b2d46270..000000000 --- a/artifacts/package_sources/rednose/.dockerignore +++ /dev/null @@ -1 +0,0 @@ -.sconsign.dblite diff --git a/artifacts/package_sources/rednose/.editorconfig b/artifacts/package_sources/rednose/.editorconfig deleted file mode 100644 index e0da06102..000000000 --- a/artifacts/package_sources/rednose/.editorconfig +++ /dev/null @@ -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 diff --git a/artifacts/package_sources/rednose/.gitignore b/artifacts/package_sources/rednose/.gitignore deleted file mode 100644 index bf2b50d22..000000000 --- a/artifacts/package_sources/rednose/.gitignore +++ /dev/null @@ -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/ diff --git a/artifacts/package_sources/rednose/.pre-commit-config.yaml b/artifacts/package_sources/rednose/.pre-commit-config.yaml deleted file mode 100644 index b908de296..000000000 --- a/artifacts/package_sources/rednose/.pre-commit-config.yaml +++ /dev/null @@ -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 diff --git a/artifacts/package_sources/rednose/Dockerfile b/artifacts/package_sources/rednose/Dockerfile deleted file mode 100644 index 57bd4c1ea..000000000 --- a/artifacts/package_sources/rednose/Dockerfile +++ /dev/null @@ -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) diff --git a/artifacts/package_sources/rednose/LICENSE b/artifacts/package_sources/rednose/LICENSE deleted file mode 100644 index 6f1774f1d..000000000 --- a/artifacts/package_sources/rednose/LICENSE +++ /dev/null @@ -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. diff --git a/artifacts/package_sources/rednose/MANIFEST.in b/artifacts/package_sources/rednose/MANIFEST.in deleted file mode 100644 index bb5ef2292..000000000 --- a/artifacts/package_sources/rednose/MANIFEST.in +++ /dev/null @@ -1,4 +0,0 @@ -include SConstruct -graft rednose -graft site_scons -global-exclude __pycache__ *.pyc *.o *.os *.d diff --git a/artifacts/package_sources/rednose/README.md b/artifacts/package_sources/rednose/README.md deleted file mode 100644 index 264958bfc..000000000 --- a/artifacts/package_sources/rednose/README.md +++ /dev/null @@ -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. diff --git a/artifacts/package_sources/rednose/SConstruct b/artifacts/package_sources/rednose/SConstruct deleted file mode 100644 index 82bc95b3c..000000000 --- a/artifacts/package_sources/rednose/SConstruct +++ /dev/null @@ -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']) diff --git a/artifacts/package_sources/rednose/examples/SConscript b/artifacts/package_sources/rednose/examples/SConscript deleted file mode 100644 index 3d178ade1..000000000 --- a/artifacts/package_sources/rednose/examples/SConscript +++ /dev/null @@ -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, -) diff --git a/artifacts/package_sources/rednose/examples/__init__.py b/artifacts/package_sources/rednose/examples/__init__.py deleted file mode 100644 index e69de29bb..000000000 diff --git a/artifacts/package_sources/rednose/examples/kinematic_kf.png b/artifacts/package_sources/rednose/examples/kinematic_kf.png deleted file mode 100644 index acb0e442c..000000000 Binary files a/artifacts/package_sources/rednose/examples/kinematic_kf.png and /dev/null differ diff --git a/artifacts/package_sources/rednose/examples/kinematic_kf.py b/artifacts/package_sources/rednose/examples/kinematic_kf.py deleted file mode 100755 index 29049a7b9..000000000 --- a/artifacts/package_sources/rednose/examples/kinematic_kf.py +++ /dev/null @@ -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) diff --git a/artifacts/package_sources/rednose/examples/live_kf.py b/artifacts/package_sources/rednose/examples/live_kf.py deleted file mode 100755 index 6e3c21d93..000000000 --- a/artifacts/package_sources/rednose/examples/live_kf.py +++ /dev/null @@ -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) diff --git a/artifacts/package_sources/rednose/examples/test_compare.py b/artifacts/package_sources/rednose/examples/test_compare.py deleted file mode 100755 index 291f29e82..000000000 --- a/artifacts/package_sources/rednose/examples/test_compare.py +++ /dev/null @@ -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) diff --git a/artifacts/package_sources/rednose/examples/test_kinematic_kf.py b/artifacts/package_sources/rednose/examples/test_kinematic_kf.py deleted file mode 100644 index 05d4d3efd..000000000 --- a/artifacts/package_sources/rednose/examples/test_kinematic_kf.py +++ /dev/null @@ -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() diff --git a/artifacts/package_sources/rednose/pyproject.toml b/artifacts/package_sources/rednose/pyproject.toml deleted file mode 100644 index c181daa8c..000000000 --- a/artifacts/package_sources/rednose/pyproject.toml +++ /dev/null @@ -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" diff --git a/artifacts/package_sources/rednose/rednose/.gitignore b/artifacts/package_sources/rednose/rednose/.gitignore deleted file mode 100644 index d771ae601..000000000 --- a/artifacts/package_sources/rednose/rednose/.gitignore +++ /dev/null @@ -1,5 +0,0 @@ -# Cython intermediates -*_pyx.cpp -*_pyx.h -*_pyx_api.h -*.os diff --git a/artifacts/package_sources/rednose/rednose/SConscript b/artifacts/package_sources/rednose/rednose/SConscript deleted file mode 100644 index 52e36e06f..000000000 --- a/artifacts/package_sources/rednose/rednose/SConscript +++ /dev/null @@ -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') diff --git a/artifacts/package_sources/rednose/rednose/__init__.py b/artifacts/package_sources/rednose/rednose/__init__.py deleted file mode 100644 index dba1dbc53..000000000 --- a/artifacts/package_sources/rednose/rednose/__init__.py +++ /dev/null @@ -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") diff --git a/artifacts/package_sources/rednose/rednose/helpers/__init__.py b/artifacts/package_sources/rednose/rednose/helpers/__init__.py deleted file mode 100644 index 3acc14aff..000000000 --- a/artifacts/package_sources/rednose/rednose/helpers/__init__.py +++ /dev/null @@ -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 diff --git a/artifacts/package_sources/rednose/rednose/helpers/chi2_lookup.py b/artifacts/package_sources/rednose/rednose/helpers/chi2_lookup.py deleted file mode 100644 index e22cc9729..000000000 --- a/artifacts/package_sources/rednose/rednose/helpers/chi2_lookup.py +++ /dev/null @@ -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() diff --git a/artifacts/package_sources/rednose/rednose/helpers/chi2_lookup_table.npy b/artifacts/package_sources/rednose/rednose/helpers/chi2_lookup_table.npy deleted file mode 100644 index 6f1bd959c..000000000 Binary files a/artifacts/package_sources/rednose/rednose/helpers/chi2_lookup_table.npy and /dev/null differ diff --git a/artifacts/package_sources/rednose/rednose/helpers/ekf.h b/artifacts/package_sources/rednose/rednose/helpers/ekf.h deleted file mode 100644 index 2afe6dd2b..000000000 --- a/artifacts/package_sources/rednose/rednose/helpers/ekf.h +++ /dev/null @@ -1,42 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -typedef void (*extra_routine_t)(double *, double *); - -struct EKF { - std::string name; - std::vector kinds; - std::vector 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 hs = {}; - std::unordered_map Hs = {}; - std::unordered_map updates = {}; - std::unordered_map Hes = {}; - std::unordered_map sets = {}; - std::unordered_map 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); \ -} diff --git a/artifacts/package_sources/rednose/rednose/helpers/ekf_load.cc b/artifacts/package_sources/rednose/rednose/helpers/ekf_load.cc deleted file mode 100644 index 882b0b4a6..000000000 --- a/artifacts/package_sources/rednose/rednose/helpers/ekf_load.cc +++ /dev/null @@ -1,39 +0,0 @@ -#include "ekf_load.h" -#include - -std::vector& ekf_get_all() { - static std::vector 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); -} diff --git a/artifacts/package_sources/rednose/rednose/helpers/ekf_load.h b/artifacts/package_sources/rednose/rednose/helpers/ekf_load.h deleted file mode 100644 index 89180b802..000000000 --- a/artifacts/package_sources/rednose/rednose/helpers/ekf_load.h +++ /dev/null @@ -1,9 +0,0 @@ -#include -#include - -#include "ekf.h" - -std::vector& 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); diff --git a/artifacts/package_sources/rednose/rednose/helpers/ekf_sym.cc b/artifacts/package_sources/rednose/rednose/helpers/ekf_sym.cc deleted file mode 100644 index 38d419d07..000000000 --- a/artifacts/package_sources/rednose/rednose/helpers/ekf_sym.cc +++ /dev/null @@ -1,223 +0,0 @@ -#include "ekf_sym.h" -#include "logger/logger.h" - -using namespace EKFS; -using namespace Eigen; - -EKFSym::EKFSym(std::string name, Map Q, Map x_initial, Map P_initial, int dim_main, - int dim_main_err, int N, int dim_augment, int dim_augment_err, std::vector maha_test_kinds, - std::vector quaternion_idxs, std::vector 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 state, Map 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 EKFSym::predict_and_update_batch(double t, int kind, std::vector> z_map, - std::vector> R_map, std::vector> extra_args, bool augment) -{ - // TODO handle rewinding at this level - - std::deque 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 zi : z_map) { - obs.z.push_back(zi); - } - for (Map Ri : R_map) { - obs.R.push_back(Ri); - } - - std::optional 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 EKFSym::rewind(double t) { - std::deque 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 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 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); -} diff --git a/artifacts/package_sources/rednose/rednose/helpers/ekf_sym.h b/artifacts/package_sources/rednose/rednose/helpers/ekf_sym.h deleted file mode 100644 index b83d7f3a1..000000000 --- a/artifacts/package_sources/rednose/rednose/helpers/ekf_sym.h +++ /dev/null @@ -1,113 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include "ekf.h" -#include "ekf_load.h" - -#define REWIND_TO_KEEP 512 - -namespace EKFS { - -typedef Eigen::Matrix MatrixXdr; - -typedef struct Observation { - double t; - int kind; - std::vector z; - std::vector R; - std::vector> extra_args; -} Observation; - -typedef struct Estimate { - Eigen::VectorXd xk1; - Eigen::VectorXd xk; - MatrixXdr Pk1; - MatrixXdr Pk; - double t; - int kind; - std::vector y; - std::vector z; - std::vector> extra_args; -} Estimate; - -class EKFSym { -public: - EKFSym(std::string name, Eigen::Map Q, Eigen::Map x_initial, - Eigen::Map P_initial, int dim_main, int dim_main_err, int N = 0, int dim_augment = 0, - int dim_augment_err = 0, std::vector maha_test_kinds = std::vector(), - std::vector quaternion_idxs = std::vector(), - std::vector global_vars = std::vector(), double max_rewind_age = 1.0); - void init_state(Eigen::Map state, Eigen::Map 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 predict_and_update_batch(double t, int kind, std::vector> z, - std::vector> R, std::vector> extra_args = {{}}, bool augment = false); - - extra_routine_t get_extra_routine(const std::string& routine); - -private: - std::deque 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 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 maha_test_kinds; - std::vector quaternion_idxs; - - std::vector global_vars; - - // process noise - MatrixXdr Q; - - // rewind stuff - double max_rewind_age; - std::deque rewind_t; - std::deque> rewind_states; - std::deque rewind_obscache; - - Eigen::VectorXd augment_times; - - std::vector feature_track_kinds; -}; - -} diff --git a/artifacts/package_sources/rednose/rednose/helpers/ekf_sym.py b/artifacts/package_sources/rednose/rednose/helpers/ekf_sym.py deleted file mode 100644 index 9dffe50e1..000000000 --- a/artifacts/package_sources/rednose/rednose/helpers/ekf_sym.py +++ /dev/null @@ -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] diff --git a/artifacts/package_sources/rednose/rednose/helpers/ekf_sym_pyx.pyx b/artifacts/package_sources/rednose/rednose/helpers/ekf_sym_pyx.pyx deleted file mode 100644 index f552ea735..000000000 --- a/artifacts/package_sources/rednose/rednose/helpers/ekf_sym_pyx.pyx +++ /dev/null @@ -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 "" 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": - MapVectorXd(double*, int) - - cdef cppclass MapMatrixXdr "Eigen::Map >": - MapMatrixXdr(double*, int, int) - - cdef cppclass VectorXd "Eigen::VectorXd": - VectorXd() - double* data() - int rows() - - cdef cppclass MatrixXdr "Eigen::Matrix": - 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 = 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 = 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( Q_b.data, Q.shape[0], Q.shape[1]), - MapVectorXd( x_initial_b.data, x_initial.shape[0]), - MapMatrixXdr( 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( state_b.data, state.shape[0]), - MapMatrixXdr( 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( 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( 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 diff --git a/artifacts/package_sources/rednose/rednose/helpers/kalmanfilter.py b/artifacts/package_sources/rednose/rednose/helpers/kalmanfilter.py deleted file mode 100644 index fc9fd2bf3..000000000 --- a/artifacts/package_sources/rednose/rednose/helpers/kalmanfilter.py +++ /dev/null @@ -1,52 +0,0 @@ -from typing import Any - -import numpy as np - - -class KalmanFilter: - 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) diff --git a/artifacts/package_sources/rednose/rednose/helpers/sympy_helpers.py b/artifacts/package_sources/rednose/rednose/helpers/sympy_helpers.py deleted file mode 100644 index 7f8813e8a..000000000 --- a/artifacts/package_sources/rednose/rednose/helpers/sympy_helpers.py +++ /dev/null @@ -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 diff --git a/artifacts/package_sources/rednose/rednose/logger/logger.h b/artifacts/package_sources/rednose/rednose/logger/logger.h deleted file mode 100644 index 8f541a2b4..000000000 --- a/artifacts/package_sources/rednose/rednose/logger/logger.h +++ /dev/null @@ -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 diff --git a/artifacts/package_sources/rednose/rednose/site_scons/__init__.py b/artifacts/package_sources/rednose/rednose/site_scons/__init__.py deleted file mode 100644 index e69de29bb..000000000 diff --git a/artifacts/package_sources/rednose/rednose/site_scons/site_tools/__init__.py b/artifacts/package_sources/rednose/rednose/site_scons/site_tools/__init__.py deleted file mode 100644 index e69de29bb..000000000 diff --git a/artifacts/package_sources/rednose/rednose/site_scons/site_tools/rednose_filter.py b/artifacts/package_sources/rednose/rednose/site_scons/site_tools/rednose_filter.py deleted file mode 100644 index fef6309ad..000000000 --- a/artifacts/package_sources/rednose/rednose/site_scons/site_tools/rednose_filter.py +++ /dev/null @@ -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 diff --git a/artifacts/package_sources/rednose/rednose/templates/compute_pos.c b/artifacts/package_sources/rednose/rednose/templates/compute_pos.c deleted file mode 100644 index 742c7d618..000000000 --- a/artifacts/package_sources/rednose/rednose/templates/compute_pos.c +++ /dev/null @@ -1,52 +0,0 @@ -#include -#include -#include - -typedef Eigen::Matrix R3M; -typedef Eigen::Matrix R1M; -typedef Eigen::Matrix O1M; -typedef Eigen::Matrix 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)); -} diff --git a/artifacts/package_sources/rednose/rednose/templates/ekf_c.c b/artifacts/package_sources/rednose/rednose/templates/ekf_c.c deleted file mode 100644 index 20437a8d5..000000000 --- a/artifacts/package_sources/rednose/rednose/templates/ekf_c.c +++ /dev/null @@ -1,123 +0,0 @@ -#include -#include - -typedef Eigen::Matrix DDM; -typedef Eigen::Matrix EEM; -typedef Eigen::Matrix DEM; - -void predict(double *in_x, double *in_P, double *in_Q, double dt) { - typedef Eigen::Matrix 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 -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 ZZM; - typedef Eigen::Matrix ZDM; - typedef Eigen::Matrix XEM; - //typedef Eigen::Matrix EZM; - typedef Eigen::Matrix X1M; - typedef Eigen::Matrix 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 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 pre_y(in_hx); pre_y = z - pre_y; - X1M y; XXM H; XXM R; - if (Hea_fun){ - typedef Eigen::Matrix 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::Identity() - (KT.transpose() * H_err); - - // update state by injecting dx - Eigen::Matrix 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 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)); -} - - diff --git a/artifacts/package_sources/rednose/rednose/templates/feature_handler.c b/artifacts/package_sources/rednose/rednose/templates/feature_handler.c deleted file mode 100644 index 9c580c2ea..000000000 --- a/artifacts/package_sources/rednose/rednose/templates/feature_handler.c +++ /dev/null @@ -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)); -} diff --git a/artifacts/package_sources/rednose/requirements.txt b/artifacts/package_sources/rednose/requirements.txt deleted file mode 100644 index 3077a329a..000000000 --- a/artifacts/package_sources/rednose/requirements.txt +++ /dev/null @@ -1,10 +0,0 @@ -ruff -sympy -numpy -scipy -cffi -scons -pre-commit -Cython -pytest -pytest-xdist diff --git a/artifacts/package_sources/rednose/setup.py b/artifacts/package_sources/rednose/setup.py deleted file mode 100644 index a58f4b54c..000000000 --- a/artifacts/package_sources/rednose/setup.py +++ /dev/null @@ -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) diff --git a/artifacts/package_sources/rednose/site_scons/site_tools/cython.py b/artifacts/package_sources/rednose/site_scons/site_tools/cython.py deleted file mode 100644 index c29147553..000000000 --- a/artifacts/package_sources/rednose/site_scons/site_tools/cython.py +++ /dev/null @@ -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 cimport ... - matches = pyx_from_import_re.findall(contents) - # cimport - 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 - 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 diff --git a/artifacts/package_sources/rednose/site_scons/site_tools/rednose_filter.py b/artifacts/package_sources/rednose/site_scons/site_tools/rednose_filter.py deleted file mode 100644 index e5b048822..000000000 --- a/artifacts/package_sources/rednose/site_scons/site_tools/rednose_filter.py +++ /dev/null @@ -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 diff --git a/artifacts/runtime/env_sync.sh b/artifacts/runtime/env_sync.sh index 5c26e2341..e40b1be87 100755 --- a/artifacts/runtime/env_sync.sh +++ b/artifacts/runtime/env_sync.sh @@ -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 , 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)' \ diff --git a/iqpilot/selfdrive/controls/lib/helpers/lateral_edge_guard.py b/iqpilot/selfdrive/controls/lib/helpers/lateral_edge_guard.py index 5d6e3a52e..f1b8a24e6 100644 --- a/iqpilot/selfdrive/controls/lib/helpers/lateral_edge_guard.py +++ b/iqpilot/selfdrive/controls/lib/helpers/lateral_edge_guard.py @@ -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) diff --git a/iqpilot/selfdrive/controls/lib/tests/test_lateral_edge_guard.py b/iqpilot/selfdrive/controls/lib/tests/test_lateral_edge_guard.py index c49bb5159..56f7efe09 100644 --- a/iqpilot/selfdrive/controls/lib/tests/test_lateral_edge_guard.py +++ b/iqpilot/selfdrive/controls/lib/tests/test_lateral_edge_guard.py @@ -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 diff --git a/iqpilot/selfdrive/iqlocd/SConscript b/iqpilot/selfdrive/iqlocd/SConscript index e1ef211d0..761fda7ae 100644 --- a/iqpilot/selfdrive/iqlocd/SConscript +++ b/iqpilot/selfdrive/iqlocd/SConscript @@ -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) diff --git a/iqpilot/selfdrive/iqlocd/atlas_loc_core.cc b/iqpilot/selfdrive/iqlocd/atlas_loc_core.cc index d40c1eb4d..f18f86c4c 100644 --- a/iqpilot/selfdrive/iqlocd/atlas_loc_core.cc +++ b/iqpilot/selfdrive/iqlocd/atlas_loc_core.cc @@ -7,7 +7,6 @@ #include #include -using namespace EKFS; using namespace Eigen; ExitHandler do_exit; diff --git a/iqpilot/selfdrive/iqlocd/models/car_kf.py b/iqpilot/selfdrive/iqlocd/models/car_kf.py old mode 100755 new mode 100644 index e59bf94af..9bb86b358 --- a/iqpilot/selfdrive/iqlocd/models/car_kf.py +++ b/iqpilot/selfdrive/iqlocd/models/car_kf.py @@ -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) diff --git a/iqpilot/selfdrive/iqlocd/models/constants.py b/iqpilot/selfdrive/iqlocd/models/constants.py index 6d328ce6f..8e95ff82e 100644 --- a/iqpilot/selfdrive/iqlocd/models/constants.py +++ b/iqpilot/selfdrive/iqlocd/models/constants.py @@ -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 diff --git a/iqpilot/selfdrive/iqlocd/models/orbit_kf.cc b/iqpilot/selfdrive/iqlocd/models/orbit_kf.cc index c39fc0af7..f3f010d8c 100644 --- a/iqpilot/selfdrive/iqlocd/models/orbit_kf.cc +++ b/iqpilot/selfdrive/iqlocd/models/orbit_kf.cc @@ -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 -Eigen::Map get_mapvec(const Eigen::VectorXd &vec) { - return Eigen::Map((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 get_mapmat(const MatrixXdr &mat) { - return Eigen::Map((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> get_vec_mapvec(const std::vector &vec_vec) { - std::vector> 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 values) { + VectorXd vector(values.size()); + int index = 0; + for (double value : values) vector(index++) = value; + return vector.asDiagonal(); } -std::vector> get_vec_mapmat(const std::vector &mat_vec) { - std::vector> 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(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(), - std::vector{3}, std::vector(), 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> 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> 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(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 OrbitKalman::get_R(int kind, int n) { - std::vector R; - for (int i = 0; i < n; i++) { - R.push_back(this->obs_noise[kind]); - } - return R; + return std::vector(n, obs_noise.at(kind)); } std::optional OrbitKalman::predict_and_observe(double t, int kind, const std::vector &meas, std::vector R) { - std::optional 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 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::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; } diff --git a/iqpilot/selfdrive/iqlocd/models/orbit_kf.h b/iqpilot/selfdrive/iqlocd/models/orbit_kf.h index a87accb72..aa9c23514 100644 --- a/iqpilot/selfdrive/iqlocd/models/orbit_kf.h +++ b/iqpilot/selfdrive/iqlocd/models/orbit_kf.h @@ -1,66 +1,46 @@ +/* +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos +*/ #pragma once -#include -#include #include +#include #include #include -#include #include -#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 get_mapvec(const Eigen::VectorXd &vec); -Eigen::Map get_mapmat(const MatrixXdr &mat); -std::vector> get_vec_mapvec(const std::vector &vec_vec); -std::vector> get_vec_mapmat(const std::vector &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 get_R(int kind, int n); - std::optional predict_and_observe(double t, int kind, const std::vector &meas, std::vector R = {}); - std::optional predict_and_update_odo_speed(std::vector speed, double t, int kind); - std::optional predict_and_update_odo_trans(std::vector trans, double t, int kind); - std::optional predict_and_update_odo_rot(std::vector 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 filter; - - int dim_state; - int dim_state_err; - + std::shared_ptr 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 obs_noise; }; diff --git a/iqpilot/selfdrive/iqlocd/models/orbit_kf.py b/iqpilot/selfdrive/iqlocd/models/orbit_kf.py deleted file mode 100755 index 38374d6c4..000000000 --- a/iqpilot/selfdrive/iqlocd/models/orbit_kf.py +++ /dev/null @@ -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 \n" - orbit_header += "#include \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> 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) diff --git a/iqpilot/selfdrive/iqlocd/models/orbit_kf_constants.h b/iqpilot/selfdrive/iqlocd/models/orbit_kf_constants.h new file mode 100644 index 000000000..6de364494 --- /dev/null +++ b/iqpilot/selfdrive/iqlocd/models/orbit_kf_constants.h @@ -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 diff --git a/iqpilot/selfdrive/locationd/SConscript b/iqpilot/selfdrive/locationd/SConscript index e8eeff7e0..632ac2872 100644 --- a/iqpilot/selfdrive/locationd/SConscript +++ b/iqpilot/selfdrive/locationd/SConscript @@ -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"]) diff --git a/iqpilot/selfdrive/locationd/locationd.py b/iqpilot/selfdrive/locationd/locationd.py index ac7cce0af..bd7864b82 100755 --- a/iqpilot/selfdrive/locationd/locationd.py +++ b/iqpilot/selfdrive/locationd/locationd.py @@ -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 diff --git a/iqpilot/selfdrive/locationd/models/car_kf.py b/iqpilot/selfdrive/locationd/models/car_kf.py old mode 100755 new mode 100644 index 80e51c168..387799bda --- a/iqpilot/selfdrive/locationd/models/car_kf.py +++ b/iqpilot/selfdrive/locationd/models/car_kf.py @@ -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) diff --git a/iqpilot/selfdrive/locationd/models/constants.py b/iqpilot/selfdrive/locationd/models/constants.py index 6d328ce6f..8e95ff82e 100644 --- a/iqpilot/selfdrive/locationd/models/constants.py +++ b/iqpilot/selfdrive/locationd/models/constants.py @@ -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 diff --git a/iqpilot/selfdrive/locationd/models/pose_kf.py b/iqpilot/selfdrive/locationd/models/pose_kf.py old mode 100755 new mode 100644 index 2b4ffe5eb..6dbf0f091 --- a/iqpilot/selfdrive/locationd/models/pose_kf.py +++ b/iqpilot/selfdrive/locationd/models/pose_kf.py @@ -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)) diff --git a/iqpilot/selfdrive/locationd/paramsd.py b/iqpilot/selfdrive/locationd/paramsd.py index fa3948727..8a42de568 100755 --- a/iqpilot/selfdrive/locationd/paramsd.py +++ b/iqpilot/selfdrive/locationd/paramsd.py @@ -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 diff --git a/iqpilot/selfdrive/state_estimation/__init__.py b/iqpilot/selfdrive/state_estimation/__init__.py new file mode 100644 index 000000000..958a822d4 --- /dev/null +++ b/iqpilot/selfdrive/state_estimation/__init__.py @@ -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"] diff --git a/iqpilot/selfdrive/state_estimation/benchmark.py b/iqpilot/selfdrive/state_estimation/benchmark.py new file mode 100644 index 000000000..9db7fabf3 --- /dev/null +++ b/iqpilot/selfdrive/state_estimation/benchmark.py @@ -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() diff --git a/iqpilot/selfdrive/state_estimation/estimator.h b/iqpilot/selfdrive/state_estimation/estimator.h new file mode 100644 index 000000000..ad8cdaf2b --- /dev/null +++ b/iqpilot/selfdrive/state_estimation/estimator.h @@ -0,0 +1,157 @@ +/* +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos +*/ +#pragma once + +#include +#include +#include +#include +#include +#include + +#include + +namespace iqpilot::state_estimation { + +using Matrix = Eigen::Matrix; +using Vector = Eigen::VectorXd; + +struct Estimate { + double time; + Vector state; + Matrix covariance; + std::vector innovations; +}; + +struct ModelDefinition { + int state_size; + int error_size; + std::function transition; + std::unordered_map> measurements; + Matrix process_noise; + std::unordered_map observation_noise; + std::function inject_error; + std::function error_projection; + std::function normalize; + std::function error_transition; + std::unordered_map> 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 predict_and_observe(double time, int kind, const std::vector &measurements, + const std::vector &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 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 &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::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; +}; + +} diff --git a/iqpilot/selfdrive/state_estimation/estimator.py b/iqpilot/selfdrive/state_estimation/estimator.py new file mode 100644 index 000000000..696bdd737 --- /dev/null +++ b/iqpilot/selfdrive/state_estimation/estimator.py @@ -0,0 +1,311 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos +""" + +from collections.abc import Callable +from dataclasses import dataclass +import math + +import numpy as np + + +Array = np.ndarray +Prediction = Callable[[Array, float, dict[str, float]], Array] +Measurement = Callable[[Array, dict[str, float]], Array] +Injection = Callable[[Array, Array], Array] +NativePrediction = Callable[[Array, Array, float, Array, dict[str, float]], None] +NativeUpdate = Callable[[Array, Array, int, Array, Array], Array] + + +@dataclass(frozen=True) +class Observation: + kind: int + values: Array + noise: Array + + +@dataclass(frozen=True) +class ModelDefinition: + state_size: int + error_size: int + transition: Prediction + measurements: dict[int, Measurement] + process_noise: Array + observation_noise: dict[int, Array] + inject_error: Injection | None = None + error_projection: Callable[[Array], Array] | None = None + normalize: Callable[[Array], Array] | None = None + native_predict: NativePrediction | None = None + native_update: NativeUpdate | None = None + + +@dataclass +class _Snapshot: + time: float + state: Array + covariance: Array + + +@dataclass +class _Event: + time: float + observation: Observation + order: int + + +class StateEstimator: + def __init__(self, model: ModelDefinition, initial_state: Array, initial_covariance: Array, + max_rewind_age: float = 0.0): + self.model = model + self.parameters: dict[str, float] = {} + self.max_rewind_age = max_rewind_age + self._order = 0 + self.init_state(initial_state, initial_covariance, None) + + @property + def x(self) -> Array: + return self._state.copy() + + @property + def P(self) -> Array: + return self._covariance.copy() + + @property + def t(self) -> float: + return self._time + + def set_global(self, name: str, value: float) -> None: + self.parameters[name] = float(value) + + def init_state(self, state: Array, covs: Array, filter_time: float | None) -> None: + state = np.asarray(state, dtype=np.float64).reshape(-1) + covariance = np.asarray(covs, dtype=np.float64) + self._validate_state(state, covariance) + self._state = self._normalize(state.copy()) + self._covariance = self._stabilize(covariance.copy()) + self._time = math.nan if filter_time is None else float(filter_time) + self._events: list[_Event] = [] + self._snapshots = [_Snapshot(self._time, self._state.copy(), self._covariance.copy())] + + def set_filter_time(self, filter_time: float | None) -> None: + self._time = math.nan if filter_time is None else float(filter_time) + + def reset_rewind(self) -> None: + self._events.clear() + self._snapshots = [_Snapshot(self._time, self._state.copy(), self._covariance.copy())] + + def predict(self, time: float) -> None: + time = float(time) + if math.isnan(self._time): + self._time = time + return + if time < self._time: + raise ValueError("prediction time precedes estimator time") + dt = time - self._time + if dt == 0.0: + return + if self.model.native_predict is not None: + self.model.native_predict(self._state, self._covariance, dt, self.model.process_noise, self.parameters) + self._time = time + return + previous = self._state.copy() + transition_jacobian = self._jacobian(lambda value: self.model.transition(value, dt, self.parameters), previous) + predicted = self.model.transition(previous, dt, self.parameters) + projection = self._error_projection(previous) + if self.model.error_size == self.model.state_size: + error_transition = transition_jacobian + else: + error_transition = np.linalg.pinv(self._error_projection(predicted)) @ transition_jacobian @ projection + self._state = self._normalize(predicted) + self._covariance = self._stabilize(error_transition @ self._covariance @ error_transition.T + dt * self.model.process_noise) + self._time = time + + def predict_and_observe(self, time: float, kind: int, measurements: Array, noise: Array | None = None): + values = self._measurement_batch(kind, measurements) + noises = self._noise_batch(kind, len(values), noise) + event = _Event(float(time), Observation(kind, values, noises), self._order) + self._order += 1 + if not math.isnan(self._time) and event.time < self._time: + if self.max_rewind_age <= 0.0 or self._time - event.time > self.max_rewind_age: + return None + return self._rewind(event) + result = self._apply_event(event) + self._events.append(event) + self._snapshots.append(_Snapshot(self._time, self._state.copy(), self._covariance.copy())) + self._trim_history() + return result + + def _apply_event(self, event: _Event): + self.predict(event.time) + prior_state = self._state.copy() + prior_covariance = self._covariance.copy() + innovations = [] + for measurement, noise in zip(event.observation.values, event.observation.noise, strict=True): + innovations.append(self._update(event.observation.kind, measurement, noise)) + return (event.time, self.x, prior_state, self.P, prior_covariance, event.observation.kind, + tuple(innovations), event.observation.values.copy(), event.observation.noise.copy()) + + def _update(self, kind: int, measurement: Array, noise: Array) -> Array: + measurement_function = self.model.measurements.get(kind) + if measurement_function is None: + raise KeyError(f"unknown observation kind {kind}") + measurement = np.asarray(measurement, dtype=np.float64).reshape(-1) + if noise.shape != (measurement.size, measurement.size): + raise ValueError("observation noise dimension mismatch") + if self.model.native_update is not None: + innovation = self.model.native_update(self._state, self._covariance, kind, measurement, noise) + return innovation + predicted = np.asarray(measurement_function(self._state, self.parameters), dtype=np.float64).reshape(-1) + if predicted.shape != measurement.shape: + raise ValueError("measurement dimension mismatch") + innovation = measurement - predicted + state_jacobian = self._jacobian(lambda value: measurement_function(value, self.parameters), self._state) + observation_jacobian = state_jacobian @ self._error_projection(self._state) + innovation_covariance = observation_jacobian @ self._covariance @ observation_jacobian.T + noise + gain = np.linalg.solve(innovation_covariance, observation_jacobian @ self._covariance).T + delta = gain @ innovation + self._state = self._normalize(self._inject(self._state, delta)) + identity = np.eye(self.model.error_size) + residual = identity - gain @ observation_jacobian + self._covariance = self._stabilize(residual @ self._covariance @ residual.T + gain @ noise @ gain.T) + self._require_finite() + return innovation + + def _rewind(self, new_event: _Event): + events = sorted(self._events + [new_event], key=lambda event: (event.time, event.order)) + base_index = max(i for i, snapshot in enumerate(self._snapshots) if math.isnan(snapshot.time) or snapshot.time <= new_event.time) + base = self._snapshots[base_index] + retained = self._events[:base_index] + retained_orders = {event.order for event in retained} + replay = [event for event in events if event.order not in retained_orders] + self._state = base.state.copy() + self._covariance = base.covariance.copy() + self._time = base.time + self._events = retained.copy() + self._snapshots = self._snapshots[:base_index + 1] + result = None + for event in replay: + current = self._apply_event(event) + self._events.append(event) + self._snapshots.append(_Snapshot(self._time, self._state.copy(), self._covariance.copy())) + if event is new_event: + result = current + self._trim_history() + return result + + def _trim_history(self) -> None: + if self.max_rewind_age <= 0.0 or math.isnan(self._time): + return + cutoff = self._time - self.max_rewind_age + remove = 0 + while remove < len(self._events) and self._events[remove].time < cutoff: + remove += 1 + if remove: + self._events = self._events[remove:] + self._snapshots = self._snapshots[remove:] + + def _measurement_batch(self, kind: int, measurements: Array) -> Array: + measurement_function = self.model.measurements.get(kind) + if measurement_function is None: + raise KeyError(f"unknown observation kind {kind}") + if self.model.native_update is not None and kind in self.model.observation_noise: + expected = self.model.observation_noise[kind].shape[0] + else: + expected = np.asarray(measurement_function(self._state, self.parameters)).size + values = np.asarray(measurements, dtype=np.float64) + if values.ndim == 1: + values = values.reshape(1, -1) + elif values.ndim != 2: + raise ValueError("measurements must be one or two dimensional") + if values.shape[1] != expected: + raise ValueError("measurement dimension mismatch") + return values + + def _noise_batch(self, kind: int, count: int, noise: Array | None) -> Array: + if noise is None: + base = self.model.observation_noise.get(kind) + if base is None: + raise KeyError(f"missing observation noise for kind {kind}") + return np.repeat(np.asarray(base, dtype=np.float64)[None, :, :], count, axis=0) + noises = np.asarray(noise, dtype=np.float64) + if noises.ndim == 2: + noises = noises[None, :, :] + if noises.shape[0] == 1 and count > 1: + noises = np.repeat(noises, count, axis=0) + if noises.shape[0] != count: + raise ValueError("observation noise batch mismatch") + return noises + + def _jacobian(self, function: Callable[[Array], Array], value: Array) -> Array: + output = np.asarray(function(value), dtype=np.float64).reshape(-1) + result = np.empty((output.size, value.size), dtype=np.float64) + for index in range(value.size): + step = np.cbrt(np.finfo(np.float64).eps) * max(1.0, abs(value[index])) + upper = value.copy() + lower = value.copy() + upper[index] += step + lower[index] -= step + result[:, index] = (np.asarray(function(upper)).reshape(-1) - np.asarray(function(lower)).reshape(-1)) / (2.0 * step) + return result + + def _inject(self, state: Array, delta: Array) -> Array: + if self.model.inject_error is None: + return state + delta + return self.model.inject_error(state, delta) + + def _error_projection(self, state: Array) -> Array: + if self.model.error_projection is None: + return np.eye(self.model.state_size, self.model.error_size) + return np.asarray(self.model.error_projection(state), dtype=np.float64) + + def _normalize(self, state: Array) -> Array: + if self.model.normalize is None: + return np.asarray(state, dtype=np.float64).reshape(-1) + return np.asarray(self.model.normalize(state), dtype=np.float64).reshape(-1) + + def _stabilize(self, covariance: Array) -> Array: + covariance = (covariance + covariance.T) * 0.5 + eigenvalues, eigenvectors = np.linalg.eigh(covariance) + if eigenvalues[0] < -1e-10: + raise FloatingPointError("covariance is not positive semidefinite") + return (eigenvectors * np.maximum(eigenvalues, 0.0)) @ eigenvectors.T + + def _validate_state(self, state: Array, covariance: Array) -> None: + if state.shape != (self.model.state_size,): + raise ValueError("state dimension mismatch") + if covariance.shape != (self.model.error_size, self.model.error_size): + raise ValueError("covariance dimension mismatch") + if self.model.process_noise.shape != covariance.shape: + raise ValueError("process noise dimension mismatch") + if not np.isfinite(state).all() or not np.isfinite(covariance).all(): + raise ValueError("state and covariance must be finite") + + def _require_finite(self) -> None: + if not np.isfinite(self._state).all() or not np.isfinite(self._covariance).all(): + raise FloatingPointError("estimator produced non-finite values") + + +class EstimatorModel: + def __init__(self, estimator: StateEstimator): + self.filter = estimator + + @property + def x(self) -> Array: + return self.filter.x + + @property + def P(self) -> Array: + return self.filter.P + + @property + def t(self) -> float: + return self.filter.t + + def init_state(self, state: Array, covs: Array, filter_time: float | None) -> None: + self.filter.init_state(state, covs, filter_time) + + def predict(self, time: float) -> None: + self.filter.predict(time) + + def predict_and_observe(self, time: float, kind: int, measurements: Array, noise: Array | None = None): + return self.filter.predict_and_observe(time, kind, measurements, noise) diff --git a/iqpilot/selfdrive/state_estimation/native_binding_pyx.pyx b/iqpilot/selfdrive/state_estimation/native_binding_pyx.pyx new file mode 100644 index 000000000..329a3b0a9 --- /dev/null +++ b/iqpilot/selfdrive/state_estimation/native_binding_pyx.pyx @@ -0,0 +1,48 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos +""" + +import numpy as np +cimport numpy as np + + +cdef extern from "iqpilot/selfdrive/state_estimation/native_kernels.h": + void iq_estimator_car_predict(double *, double *, const double *, double, const double *) + void iq_estimator_car_update(double *, double *, int, const double *, const double *, double *) + void iq_estimator_pose_predict(double *, double *, const double *, double) + void iq_estimator_pose_update(double *, double *, int, const double *, const double *, double *) + + +def car_predict(np.ndarray[np.float64_t, ndim=1, mode="c"] state, + np.ndarray[np.float64_t, ndim=2, mode="c"] covariance, + np.ndarray[np.float64_t, ndim=2, mode="c"] process_noise, + double dt, + np.ndarray[np.float64_t, ndim=1, mode="c"] parameters): + iq_estimator_car_predict(&state[0], &covariance[0, 0], &process_noise[0, 0], dt, ¶meters[0]) + + +def car_update(np.ndarray[np.float64_t, ndim=1, mode="c"] state, + np.ndarray[np.float64_t, ndim=2, mode="c"] covariance, + int kind, + np.ndarray[np.float64_t, ndim=1, mode="c"] measurement, + np.ndarray[np.float64_t, ndim=2, mode="c"] noise): + cdef np.ndarray[np.float64_t, ndim=1, mode="c"] innovation = np.empty(measurement.size) + iq_estimator_car_update(&state[0], &covariance[0, 0], kind, &measurement[0], &noise[0, 0], &innovation[0]) + return innovation + + +def pose_predict(np.ndarray[np.float64_t, ndim=1, mode="c"] state, + np.ndarray[np.float64_t, ndim=2, mode="c"] covariance, + np.ndarray[np.float64_t, ndim=2, mode="c"] process_noise, + double dt): + iq_estimator_pose_predict(&state[0], &covariance[0, 0], &process_noise[0, 0], dt) + + +def pose_update(np.ndarray[np.float64_t, ndim=1, mode="c"] state, + np.ndarray[np.float64_t, ndim=2, mode="c"] covariance, + int kind, + np.ndarray[np.float64_t, ndim=1, mode="c"] measurement, + np.ndarray[np.float64_t, ndim=2, mode="c"] noise): + cdef np.ndarray[np.float64_t, ndim=1, mode="c"] innovation = np.empty(measurement.size) + iq_estimator_pose_update(&state[0], &covariance[0, 0], kind, &measurement[0], &noise[0, 0], &innovation[0]) + return innovation diff --git a/iqpilot/selfdrive/state_estimation/native_kernels.cc b/iqpilot/selfdrive/state_estimation/native_kernels.cc new file mode 100644 index 000000000..7449a42ae --- /dev/null +++ b/iqpilot/selfdrive/state_estimation/native_kernels.cc @@ -0,0 +1,231 @@ +/* +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos +*/ +#include "iqpilot/selfdrive/state_estimation/native_kernels.h" + +#include +#include +#include + +#include + +namespace { + +using Matrix = Eigen::Matrix; +using Vector = Eigen::VectorXd; + +template +using FixedMatrix = Eigen::Matrix; + +template +using FixedVector = Eigen::Matrix; + +template +void predict(double *state_data, double *covariance_data, const double *noise_data, double dt, Function function) { + Eigen::Map> state(state_data); + Eigen::Map> covariance(covariance_data); + Eigen::Map> noise(noise_data); + const FixedVector previous = state; + FixedMatrix jacobian; + for (int index = 0; index < N; ++index) { + const double step = std::cbrt(Eigen::NumTraits::epsilon()) * std::max(1.0, std::abs(previous(index))); + FixedVector upper = previous; + FixedVector lower = previous; + upper(index) += step; + lower(index) -= step; + jacobian.col(index) = (function(upper, dt) - function(lower, dt)) / (2.0 * step); + } + state = function(previous, dt); + covariance = jacobian * covariance * jacobian.transpose() + dt * noise; + covariance = (covariance + covariance.transpose()).eval() * 0.5; +} + +template +void update(double *state_data, double *covariance_data, const double *measurement_data, const double *noise_data, + double *innovation_data, const Eigen::Matrix &jacobian, const Eigen::Matrix &expected) { + Eigen::Map> state(state_data); + Eigen::Map> covariance(covariance_data); + Eigen::Map> measurement(measurement_data); + Eigen::Map> noise(noise_data); + const Eigen::Matrix innovation_covariance = jacobian * covariance * jacobian.transpose() + noise; + const Eigen::Matrix gain = innovation_covariance.ldlt().solve(jacobian * covariance).transpose(); + const Eigen::Matrix innovation = measurement - expected; + state += gain * innovation; + const FixedMatrix residual = FixedMatrix::Identity() - gain * jacobian; + covariance = residual * covariance * residual.transpose() + gain * noise * gain.transpose(); + covariance = (covariance + covariance.transpose()).eval() * 0.5; + Eigen::Map> innovation_output(innovation_data); + innovation_output = innovation; +} + +FixedVector<9> car_transition(const FixedVector<9> &state, double dt, const double *values) { + FixedVector<9> result = state; + const double stiffness = state(0); + const double steer_ratio = state(1); + const double angle = state(7) - state(2) - state(3); + const double speed = state(4); + const double lateral_speed = state(5); + const double yaw_rate = state(6); + const double mass = values[0]; + const double inertia = values[1]; + const double front = values[2]; + const double rear = values[3]; + const double front_stiffness = stiffness * values[4]; + const double rear_stiffness = stiffness * values[5]; + double 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) - 9.81 * state(8); + double yaw_dot = -(front_stiffness * front - rear_stiffness * rear) * lateral_speed / (inertia * speed); + yaw_dot -= (front_stiffness * front * front + rear_stiffness * rear * rear) * 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; +} + +Eigen::Matrix3d rotation(const Eigen::Vector3d &euler) { + return (Eigen::AngleAxisd(euler.z(), Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(euler.y(), Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(euler.x(), Eigen::Vector3d::UnitX())).toRotationMatrix(); +} + +Eigen::Vector3d euler(const Eigen::Matrix3d &matrix) { + const double pitch = std::asin(-matrix(2, 0)); + return {std::atan2(matrix(2, 1), matrix(2, 2)), pitch, std::atan2(matrix(1, 0), matrix(0, 0))}; +} + +Eigen::Matrix pose_orientation_jacobian(const Eigen::Vector3d &orientation, + const Eigen::Vector3d &angular_velocity, double dt) { + const Eigen::Matrix3d x_rotation = Eigen::AngleAxisd(orientation.x(), Eigen::Vector3d::UnitX()).toRotationMatrix(); + const Eigen::Matrix3d y_rotation = Eigen::AngleAxisd(orientation.y(), Eigen::Vector3d::UnitY()).toRotationMatrix(); + const Eigen::Matrix3d z_rotation = Eigen::AngleAxisd(orientation.z(), Eigen::Vector3d::UnitZ()).toRotationMatrix(); + const Eigen::Vector3d delta = dt * angular_velocity; + const Eigen::Matrix3d delta_x = Eigen::AngleAxisd(delta.x(), Eigen::Vector3d::UnitX()).toRotationMatrix(); + const Eigen::Matrix3d delta_y = Eigen::AngleAxisd(delta.y(), Eigen::Vector3d::UnitY()).toRotationMatrix(); + const Eigen::Matrix3d delta_z = Eigen::AngleAxisd(delta.z(), Eigen::Vector3d::UnitZ()).toRotationMatrix(); + const Eigen::Matrix3d first = z_rotation * y_rotation * x_rotation; + const Eigen::Matrix3d second = delta_z * delta_y * delta_x; + const Eigen::Matrix3d combined = first * second; + Eigen::Matrix3d generator_x = Eigen::Matrix3d::Zero(); + Eigen::Matrix3d generator_y = Eigen::Matrix3d::Zero(); + Eigen::Matrix3d generator_z = Eigen::Matrix3d::Zero(); + generator_x(1, 2) = -1.0; + generator_x(2, 1) = 1.0; + generator_y(0, 2) = 1.0; + generator_y(2, 0) = -1.0; + generator_z(0, 1) = -1.0; + generator_z(1, 0) = 1.0; + std::array derivatives = { + z_rotation * y_rotation * x_rotation * generator_x * second, + z_rotation * y_rotation * generator_y * x_rotation * second, + z_rotation * generator_z * y_rotation * x_rotation * second, + first * delta_z * delta_y * delta_x * generator_x * dt, + first * delta_z * delta_y * generator_y * delta_x * dt, + first * delta_z * generator_z * delta_y * delta_x * dt, + }; + Eigen::Matrix result; + for (int index = 0; index < 6; ++index) { + const Eigen::Matrix3d &derivative = derivatives[index]; + result(0, index) = (combined(2, 2) * derivative(2, 1) - combined(2, 1) * derivative(2, 2)) / + (combined(2, 1) * combined(2, 1) + combined(2, 2) * combined(2, 2)); + result(1, index) = -derivative(2, 0) / std::sqrt(1.0 - combined(2, 0) * combined(2, 0)); + result(2, index) = (combined(0, 0) * derivative(1, 0) - combined(1, 0) * derivative(0, 0)) / + (combined(1, 0) * combined(1, 0) + combined(0, 0) * combined(0, 0)); + } + return result; +} + +FixedVector<18> pose_transition(const FixedVector<18> &state, double dt) { + FixedVector<18> result = state; + result.segment<3>(3) += dt * state.segment<3>(12); + result.segment<3>(0) = euler(rotation(state.segment<3>(0)) * rotation(dt * state.segment<3>(6))); + return result; +} + +Eigen::Vector3d pose_acceleration(const FixedVector<18> &state) { + return rotation(state.segment<3>(0)).transpose() * Eigen::Vector3d(0.0, 0.0, -9.81) + state.segment<3>(12) + + state.segment<3>(6).cross(state.segment<3>(3)) + state.segment<3>(15); +} + +} + +extern "C" void iq_estimator_car_predict(double *state, double *covariance, const double *process_noise, double dt, const double *parameters) { + predict<9>(state, covariance, process_noise, dt, [parameters](const FixedVector<9> &value, double step) { + return car_transition(value, step, parameters); + }); +} + +extern "C" void iq_estimator_car_update(double *state_data, double *covariance, int kind, const double *measurement, const double *noise, double *innovation) { + Eigen::Map> state(state_data); + if (kind == 24) { + Eigen::Matrix jacobian = Eigen::Matrix::Zero(); + jacobian(0, 4) = 1.0; + jacobian(1, 5) = 1.0; + update<9, 2>(state_data, covariance, measurement, noise, innovation, jacobian, state.segment<2>(4)); + return; + } + int index = -1; + if (kind == 25) index = 6; + if (kind == 30) index = 4; + if (kind == 26) index = 7; + if (kind == 27) index = 3; + if (kind == 29) index = 1; + if (kind == 28) index = 0; + if (kind == 31) index = 8; + if (index < 0) throw std::invalid_argument("unknown car observation"); + Eigen::Matrix jacobian = Eigen::Matrix::Zero(); + jacobian(0, index) = 1.0; + Eigen::Matrix expected; + expected(0) = state(index); + update<9, 1>(state_data, covariance, measurement, noise, innovation, jacobian, expected); +} + +extern "C" void iq_estimator_pose_predict(double *state, double *covariance, const double *process_noise, double dt) { + Eigen::Map> mapped_state(state); + Eigen::Map> mapped_covariance(covariance); + Eigen::Map> noise(process_noise); + const FixedVector<18> previous = mapped_state; + FixedMatrix<18> jacobian = FixedMatrix<18>::Identity(); + const Eigen::Matrix orientation_jacobian = pose_orientation_jacobian(previous.segment<3>(0), previous.segment<3>(6), dt); + jacobian.block<3, 3>(0, 0) = orientation_jacobian.leftCols<3>(); + jacobian.block<3, 3>(0, 6) = orientation_jacobian.rightCols<3>(); + jacobian.block<3, 3>(3, 12) = Eigen::Matrix3d::Identity() * dt; + mapped_state = pose_transition(previous, dt); + mapped_covariance = jacobian * mapped_covariance * jacobian.transpose() + dt * noise; + mapped_covariance = (mapped_covariance + mapped_covariance.transpose()).eval() * 0.5; +} + +extern "C" void iq_estimator_pose_update(double *state_data, double *covariance, int kind, const double *measurement, const double *noise, double *innovation) { + Eigen::Map> state(state_data); + Eigen::Matrix jacobian = Eigen::Matrix::Zero(); + Eigen::Vector3d expected; + if (kind == 4) { + jacobian.block<3, 3>(0, 6).setIdentity(); + jacobian.block<3, 3>(0, 9).setIdentity(); + expected = state.segment<3>(6) + state.segment<3>(9); + } else if (kind == 10) { + expected = pose_acceleration(state); + for (int index = 0; index < 3; ++index) { + const double step = std::cbrt(Eigen::NumTraits::epsilon()) * std::max(1.0, std::abs(state(index))); + FixedVector<18> upper = state; + FixedVector<18> lower = state; + upper(index) += step; + lower(index) -= step; + jacobian.col(index) = (pose_acceleration(upper) - pose_acceleration(lower)) / (2.0 * step); + } + const Eigen::Vector3d velocity = state.segment<3>(3); + const Eigen::Vector3d omega = state.segment<3>(6); + jacobian.block<3, 3>(0, 3) << 0.0, -omega.z(), omega.y(), omega.z(), 0.0, -omega.x(), -omega.y(), omega.x(), 0.0; + jacobian.block<3, 3>(0, 6) << 0.0, velocity.z(), -velocity.y(), -velocity.z(), 0.0, velocity.x(), velocity.y(), -velocity.x(), 0.0; + jacobian.block<3, 3>(0, 12).setIdentity(); + jacobian.block<3, 3>(0, 15).setIdentity(); + } else if (kind == 13) { + jacobian.block<3, 3>(0, 3).setIdentity(); + expected = state.segment<3>(3); + } else if (kind == 14) { + jacobian.block<3, 3>(0, 6).setIdentity(); + expected = state.segment<3>(6); + } else { + throw std::invalid_argument("unknown pose observation"); + } + update<18, 3>(state_data, covariance, measurement, noise, innovation, jacobian, expected); +} diff --git a/iqpilot/selfdrive/state_estimation/native_kernels.h b/iqpilot/selfdrive/state_estimation/native_kernels.h new file mode 100644 index 000000000..9015c2306 --- /dev/null +++ b/iqpilot/selfdrive/state_estimation/native_kernels.h @@ -0,0 +1,11 @@ +/* +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos +*/ +#pragma once + +extern "C" { +void iq_estimator_car_predict(double *state, double *covariance, const double *process_noise, double dt, const double *parameters); +void iq_estimator_car_update(double *state, double *covariance, int kind, const double *measurement, const double *noise, double *innovation); +void iq_estimator_pose_predict(double *state, double *covariance, const double *process_noise, double dt); +void iq_estimator_pose_update(double *state, double *covariance, int kind, const double *measurement, const double *noise, double *innovation); +} diff --git a/iqpilot/selfdrive/state_estimation/test_estimator.py b/iqpilot/selfdrive/state_estimation/test_estimator.py new file mode 100644 index 000000000..a0adb83d6 --- /dev/null +++ b/iqpilot/selfdrive/state_estimation/test_estimator.py @@ -0,0 +1,92 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos +""" + +import numpy as np +import pytest + +from iqpilot.selfdrive.state_estimation import ModelDefinition, StateEstimator + + +def linear_model(process_noise: float = 0.2, observation_noise: float = 0.5) -> ModelDefinition: + return ModelDefinition( + state_size=2, + error_size=2, + transition=lambda state, dt, _: np.array([state[0] + dt * state[1], state[1]]), + measurements={1: lambda state, _: state[:1]}, + process_noise=np.eye(2) * process_noise, + observation_noise={1: np.array([[observation_noise]])}, + ) + + +def test_linear_prediction_matches_closed_form() -> None: + estimator = StateEstimator(linear_model(), np.array([2.0, 3.0]), np.diag([4.0, 5.0])) + estimator.init_state(np.array([2.0, 3.0]), np.diag([4.0, 5.0]), 1.0) + estimator.predict(1.25) + transition = np.array([[1.0, 0.25], [0.0, 1.0]]) + np.testing.assert_allclose(estimator.x, np.array([2.75, 3.0]), atol=1e-10) + np.testing.assert_allclose(estimator.P, transition @ np.diag([4.0, 5.0]) @ transition.T + 0.25 * np.eye(2) * 0.2, atol=1e-10) + + +def test_linear_update_matches_closed_form() -> None: + estimator = StateEstimator(linear_model(), np.array([0.0, 0.0]), np.diag([2.0, 3.0])) + estimator.init_state(np.array([0.0, 0.0]), np.diag([2.0, 3.0]), 0.0) + estimator.predict_and_observe(0.0, 1, np.array([4.0])) + gain = 2.0 / 2.5 + np.testing.assert_allclose(estimator.x, np.array([gain * 4.0, 0.0]), atol=1e-10) + np.testing.assert_allclose(estimator.P, np.diag([(1.0 - gain) * 2.0, 3.0]), atol=1e-10) + + +def test_zero_innovation_does_not_change_state() -> None: + estimator = StateEstimator(linear_model(), np.array([4.0, 2.0]), np.eye(2)) + estimator.init_state(np.array([4.0, 2.0]), np.eye(2), 0.0) + estimator.predict_and_observe(0.0, 1, np.array([4.0])) + np.testing.assert_array_equal(estimator.x, np.array([4.0, 2.0])) + + +def test_larger_observation_noise_reduces_correction() -> None: + low = StateEstimator(linear_model(observation_noise=0.1), np.zeros(2), np.eye(2)) + high = StateEstimator(linear_model(observation_noise=10.0), np.zeros(2), np.eye(2)) + low.predict_and_observe(0.0, 1, np.array([1.0])) + high.predict_and_observe(0.0, 1, np.array([1.0])) + assert abs(low.x[0]) > abs(high.x[0]) + + +def test_larger_process_noise_increases_uncertainty() -> None: + low = StateEstimator(linear_model(process_noise=0.1), np.zeros(2), np.eye(2)) + high = StateEstimator(linear_model(process_noise=2.0), np.zeros(2), np.eye(2)) + low.init_state(np.zeros(2), np.eye(2), 0.0) + high.init_state(np.zeros(2), np.eye(2), 0.0) + low.predict(1.0) + high.predict(1.0) + assert np.all(np.diag(high.P) > np.diag(low.P)) + + +def test_batch_update_preserves_covariance_properties() -> None: + estimator = StateEstimator(linear_model(), np.zeros(2), np.eye(2)) + estimator.predict_and_observe(0.0, 1, np.array([[1.0], [0.5], [-0.2]])) + np.testing.assert_allclose(estimator.P, estimator.P.T, atol=1e-12) + assert np.linalg.eigvalsh(estimator.P).min() >= -1e-10 + assert np.isfinite(estimator.x).all() + assert np.isfinite(estimator.P).all() + + +def test_delayed_observation_replays_deterministically() -> None: + chronological = StateEstimator(linear_model(), np.zeros(2), np.eye(2), max_rewind_age=2.0) + delayed = StateEstimator(linear_model(), np.zeros(2), np.eye(2), max_rewind_age=2.0) + chronological.init_state(np.zeros(2), np.eye(2), 0.0) + delayed.init_state(np.zeros(2), np.eye(2), 0.0) + chronological.predict_and_observe(0.5, 1, np.array([1.0])) + chronological.predict_and_observe(1.0, 1, np.array([2.0])) + delayed.predict_and_observe(1.0, 1, np.array([2.0])) + delayed.predict_and_observe(0.5, 1, np.array([1.0])) + np.testing.assert_allclose(delayed.x, chronological.x, atol=1e-10) + np.testing.assert_allclose(delayed.P, chronological.P, atol=1e-10) + + +def test_invalid_dimensions_fail_deterministically() -> None: + with pytest.raises(ValueError, match="state dimension mismatch"): + StateEstimator(linear_model(), np.zeros(3), np.eye(2)) + estimator = StateEstimator(linear_model(), np.zeros(2), np.eye(2)) + with pytest.raises(ValueError, match="measurement dimension mismatch"): + estimator.predict_and_observe(0.0, 1, np.zeros(2)) diff --git a/iqpilot/selfdrive/state_estimation/test_models.py b/iqpilot/selfdrive/state_estimation/test_models.py new file mode 100644 index 000000000..c6717e2c8 --- /dev/null +++ b/iqpilot/selfdrive/state_estimation/test_models.py @@ -0,0 +1,58 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos +""" + +import numpy as np + +from iqpilot.selfdrive.locationd.models.car_kf import CarKalman, States as CarStates +from iqpilot.selfdrive.locationd.models.constants import ObservationKind +from iqpilot.selfdrive.locationd.models.pose_kf import PoseKalman, States as PoseStates + + +def configured_car() -> CarKalman: + estimator = CarKalman() + estimator.set_globals(1800.0, 2500.0, 1.2, 1.6, 90000.0, 100000.0) + estimator.init_state(CarKalman.initial_x, CarKalman.P_initial, 0.0) + return estimator + + +def test_car_mutable_parameters_affect_prediction() -> None: + light = configured_car() + heavy = configured_car() + heavy.set_globals(3600.0, 5000.0, 1.2, 1.6, 90000.0, 100000.0) + state = CarKalman.initial_x.copy() + state[CarStates.STEER_ANGLE] = 0.1 + light.init_state(state, CarKalman.P_initial, 0.0) + heavy.init_state(state, CarKalman.P_initial, 0.0) + light.predict(0.01) + heavy.predict(0.01) + assert abs(light.x[CarStates.YAW_RATE].item()) > abs(heavy.x[CarStates.YAW_RATE].item()) + + +def test_car_long_sequence_stays_finite() -> None: + estimator = configured_car() + for index in range(500): + time = index * 0.01 + estimator.predict_and_observe(time, ObservationKind.STEER_ANGLE, np.array([0.02 * np.sin(time)])) + estimator.predict_and_observe(time, ObservationKind.ROAD_FRAME_X_SPEED, np.array([15.0])) + assert np.isfinite(estimator.x).all() + assert np.isfinite(estimator.P).all() + assert np.linalg.eigvalsh(estimator.P).min() >= -1e-10 + + +def test_pose_delayed_sensor_sequence_is_stable() -> None: + estimator = PoseKalman(0.8) + estimator.init_state(PoseKalman.initial_x, PoseKalman.initial_P, 0.0) + estimator.predict_and_observe(0.02, ObservationKind.PHONE_GYRO, np.array([0.01, -0.02, 0.03])) + estimator.predict_and_observe(0.04, ObservationKind.PHONE_ACCEL, np.array([0.0, 0.0, -9.81])) + estimator.predict_and_observe(0.03, ObservationKind.CAMERA_ODO_ROTATION, np.array([0.01, -0.02, 0.03])) + assert np.isfinite(estimator.x).all() + assert np.isfinite(estimator.P).all() + np.testing.assert_allclose(estimator.P, estimator.P.T, atol=1e-12) + + +def test_pose_zero_rotation_preserves_orientation() -> None: + estimator = PoseKalman(0.8) + estimator.init_state(PoseKalman.initial_x, PoseKalman.initial_P, 0.0) + estimator.predict(1.0) + np.testing.assert_allclose(estimator.x[PoseStates.NED_ORIENTATION], np.zeros(3), atol=1e-12) diff --git a/iqpilot/system/ble-transportd.service b/iqpilot/system/ble-transportd.service index 3876227d6..d6892417a 100644 --- a/iqpilot/system/ble-transportd.service +++ b/iqpilot/system/ble-transportd.service @@ -5,8 +5,10 @@ Description=Konn3kt BLE Device Settings Transport Documentation=https://gitlvb.teallvbs.xyz/teal/iqpilot After=bluetooth.service dbus.service Wants=bluetooth.service dbus.service -StartLimitIntervalSec=300 -StartLimitBurst=10 +# Never give up: /data/openpilot is a symlink created by the boot-time rename +# migration, so early starts fail "failed to locate repo root". With a burst +# limit those failures permanently kill BLE for the whole boot. +StartLimitIntervalSec=0 [Service] Type=simple @@ -18,6 +20,7 @@ Environment="PYTHONSAFEPATH=1" Environment="PATH=/usr/local/venv/bin:/usr/sbin:/usr/bin:/sbin:/bin" WorkingDirectory=/data/openpilot ExecStartPre=/bin/bash -c 'for i in $(seq 1 120); do if [ -x /usr/libexec/iqpilot/iqpilot_bundle_runner ]; then exit 0; fi; echo "Waiting for iqpilot_bundle_runner..."; sleep 5; done; exit 1' +ExecStartPre=/bin/bash -c 'for i in $(seq 1 120); do if [ -e /data/openpilot/iqpilot/system ] && [ -e /data/openpilot/iqpilot/common ]; then exit 0; fi; echo "Waiting for repo root..."; sleep 5; done; exit 1' ExecStartPre=/bin/bash -c 'if [ -f /data/openpilot/artifacts/runtime/ensure_private_installed.sh ]; then bash /data/openpilot/artifacts/runtime/ensure_private_installed.sh || true; fi' ExecStart=/usr/libexec/iqpilot/iqpilot_bundle_runner --bundle iqpilot_hephaestusd_private --mode python-module --entry iqpilot_private.konn3kt.hephaestus.ble_transportd --daemon-name ble_transportd TimeoutStartSec=600 diff --git a/iqpilot/system/flockd.service b/iqpilot/system/flockd.service index b35bfb2a5..f694f2132 100644 --- a/iqpilot/system/flockd.service +++ b/iqpilot/system/flockd.service @@ -5,8 +5,7 @@ Description=Konn3kt Flock/ALPR RF Detector Documentation=https://gitlvb.teallvbs.xyz/teal/iqpilot After=bluetooth.service dbus.service NetworkManager.service Wants=bluetooth.service dbus.service -StartLimitIntervalSec=300 -StartLimitBurst=10 +StartLimitIntervalSec=0 [Service] Type=simple @@ -18,6 +17,7 @@ Environment="PYTHONSAFEPATH=1" Environment="PATH=/usr/local/venv/bin:/usr/sbin:/usr/bin:/sbin:/bin" WorkingDirectory=/data/openpilot ExecStartPre=/bin/bash -c 'for i in $(seq 1 120); do if [ -x /usr/libexec/iqpilot/iqpilot_bundle_runner ]; then exit 0; fi; echo "Waiting for iqpilot_bundle_runner..."; sleep 5; done; exit 1' +ExecStartPre=/bin/bash -c 'for i in $(seq 1 120); do if [ -e /data/openpilot/iqpilot/system ] && [ -e /data/openpilot/iqpilot/common ]; then exit 0; fi; echo "Waiting for repo root..."; sleep 5; done; exit 1' ExecStartPre=/bin/bash -c 'if [ -f /data/openpilot/artifacts/runtime/ensure_private_installed.sh ]; then bash /data/openpilot/artifacts/runtime/ensure_private_installed.sh || true; fi' ExecStart=/usr/libexec/iqpilot/iqpilot_bundle_runner --bundle iqpilot_hephaestusd_private --mode python-module --entry iqpilot_private.konn3kt.flockd.flockd --daemon-name flockd TimeoutStartSec=600 diff --git a/iqpilot/system/hephaestusd.service b/iqpilot/system/hephaestusd.service index 827c7e786..044709de8 100644 --- a/iqpilot/system/hephaestusd.service +++ b/iqpilot/system/hephaestusd.service @@ -16,6 +16,7 @@ Environment="PATH=/usr/local/venv/bin:/usr/sbin:/usr/bin:/sbin:/bin" WorkingDirectory=/data/openpilot ExecStartPre=/bin/bash -c 'for i in $(seq 1 120); do if [ -x /usr/libexec/iqpilot/iqpilot_bundle_runner ]; then exit 0; fi; echo "Waiting for iqpilot_bundle_runner..."; sleep 5; done; exit 1' +ExecStartPre=/bin/bash -c 'for i in $(seq 1 120); do if [ -e /data/openpilot/iqpilot/system ] && [ -e /data/openpilot/iqpilot/common ]; then exit 0; fi; echo "Waiting for repo root..."; sleep 5; done; exit 1' ExecStartPre=/bin/bash -c 'if [ -f /data/openpilot/artifacts/runtime/ensure_private_installed.sh ]; then bash /data/openpilot/artifacts/runtime/ensure_private_installed.sh || true; fi' ExecStart=/usr/libexec/iqpilot/iqpilot_bundle_runner --bundle iqpilot_hephaestusd_private --mode python-module --entry iqpilot_private.konn3kt.hephaestus.manage_hephaestusd --daemon-name manage_hephaestusd diff --git a/iqpilot/system/proprietary_runtime/rootfs_integrity.json b/iqpilot/system/proprietary_runtime/rootfs_integrity.json index 13f43e756..3b703f883 100644 --- a/iqpilot/system/proprietary_runtime/rootfs_integrity.json +++ b/iqpilot/system/proprietary_runtime/rootfs_integrity.json @@ -39,15 +39,15 @@ "mode": 420, "path": "/usr/lib/systemd/system/hephaestusd.service", "relative_to": "absolute", - "sha256": "4ae9a2ad76345019633c2fcf9698f48089576c9b7c009078a732e2a7c36006e4", - "size": 1418 + "sha256": "3b70e8873c7eb813e70798300b76c153dddda9947c1301cbba7e103962461fa2", + "size": 1627 }, { "mode": 420, "path": "/usr/lib/systemd/system/ble-transportd.service", "relative_to": "absolute", - "sha256": "dfb9e18320e264c847efc01a2d58ee1850a83c49ed614527eafa7634de4ac51a", - "size": 1500 + "sha256": "6b5bf61ebfebffe3d125bb766c13c391b83e8feda04b89f47de2725c6fe186e3", + "size": 1907 } ] } diff --git a/iqpilot/system/proprietary_runtime/rootfs_integrity.json.sig b/iqpilot/system/proprietary_runtime/rootfs_integrity.json.sig index 85a3ae858..319c33e40 100644 --- a/iqpilot/system/proprietary_runtime/rootfs_integrity.json.sig +++ b/iqpilot/system/proprietary_runtime/rootfs_integrity.json.sig @@ -1 +1 @@ -dmb/R9bZCCHK8J72JIgm8TMY7nBVVo1exmvFIodw0LSFVcGZGDrZNB74xhQTAXgzAgeYVdFQyXs0qdwpvQa2Aw== +exS3jBbVETgFQtzbRGWymtflNgeWLbjHvcMVsKs5NrwpmMdx9sWpcZ2FO/pB6RQaCniXGDwT7GhjvGxpfrP5Cg== diff --git a/iqpilot/system/runtime_package_sources.py b/iqpilot/system/runtime_package_sources.py index 292882334..1d73f5737 100644 --- a/iqpilot/system/runtime_package_sources.py +++ b/iqpilot/system/runtime_package_sources.py @@ -4,7 +4,7 @@ import sys from pathlib import Path -PACKAGE_NAMES = ("msgq", "iqdbc", "panda", "rednose", "teleoprtc", "tinygrad") +PACKAGE_NAMES = ("msgq", "iqdbc", "panda", "teleoprtc", "tinygrad") root = Path(sys.argv[1]).resolve() text = (root / "pyproject.toml").read_text() for name in PACKAGE_NAMES: diff --git a/iqpilot/system/runtime_packages_verify.py b/iqpilot/system/runtime_packages_verify.py index df389512a..0ea99abb3 100644 --- a/iqpilot/system/runtime_packages_verify.py +++ b/iqpilot/system/runtime_packages_verify.py @@ -11,7 +11,7 @@ import sys from importlib import metadata from pathlib import Path -PACKAGES = ("iqdbc", "msgq", "panda", "rednose", "teleoprtc", "tinygrad") +PACKAGES = ("iqdbc", "msgq", "panda", "teleoprtc", "tinygrad") missing = [] for name in PACKAGES: diff --git a/iqpilot/system/version.py b/iqpilot/system/version.py index 9cca623d4..cb731d009 100755 --- a/iqpilot/system/version.py +++ b/iqpilot/system/version.py @@ -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, "iqpilot", "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 diff --git a/pyproject.toml b/pyproject.toml index 6eb4f8d1c..8ed1c6c2e 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -13,7 +13,7 @@ dependencies = [ "sounddevice", # micd + soundd "pyserial", # pigeond + qcomgpsd "requests", # many one-off uses - "sympy", # rednose + friends + "sympy", "crcmod-plus", # cars + qcomgpsd "tqdm", # cars (fw_versions.py) on start + many one-off uses @@ -27,7 +27,6 @@ dependencies = [ "msgq", "iqdbc", "panda", - "rednose", "teleoprtc", "tinygrad", @@ -256,6 +255,5 @@ not-subscriptable = "ignore" msgq = { path = "artifacts/package_sources/msgq" } iqdbc = { path = "artifacts/package_sources/iqdbc" } panda = { path = "artifacts/package_sources/panda" } -rednose = { path = "artifacts/package_sources/rednose" } teleoprtc = { path = "artifacts/package_sources/teleoprtc" } tinygrad = { path = "artifacts/package_sources/tinygrad" } diff --git a/uv.lock b/uv.lock index f78da2047..53d2d590a 100644 --- a/uv.lock +++ b/uv.lock @@ -601,7 +601,6 @@ dependencies = [ { name = "pyzmq" }, { name = "qrcode" }, { name = "raylib" }, - { name = "rednose" }, { name = "requests" }, { name = "scons" }, { name = "sentry-sdk" }, @@ -708,7 +707,6 @@ requires-dist = [ { name = "pyzmq" }, { name = "qrcode" }, { name = "raylib", specifier = ">5.5.0.3" }, - { name = "rednose", directory = "artifacts/package_sources/rednose" }, { name = "requests" }, { name = "ruff", marker = "extra == 'testing'" }, { name = "scons" }, @@ -3956,25 +3954,6 @@ wheels = [ { url = "https://files.pythonhosted.org/packages/2c/66/a307e61c953ace906ba68ba1174ed8f1e90e68d5fc3e3af9fb7dc46d68d1/raylib-5.5.0.4-cp312-cp312-win_amd64.whl", hash = "sha256:553043a050a31f2ef072f26d3a70373f838a04733f7c5b26a4e9ee3f8caf06ec", size = 1708354, upload-time = "2025-12-11T15:27:45.979Z" }, ] -[[package]] -name = "rednose" -version = "0.0.1" -source = { directory = "artifacts/package_sources/rednose" } -dependencies = [ - { name = "cffi" }, - { name = "numpy" }, - { name = "sympy" }, -] - -[package.metadata] -requires-dist = [ - { name = "cffi" }, - { name = "numpy" }, - { name = "scipy", marker = "extra == 'dev'" }, - { name = "sympy" }, -] -provides-extras = ["dev"] - [[package]] name = "requests" version = "2.32.5"