diff --git a/Cargo.lock b/Cargo.lock index 6053e939..9ccd1c34 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -38,30 +38,6 @@ version = "0.1.3" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "250f629c0161ad8107cf89319e990051fae62832fd343083bea452d93e2205fd" -[[package]] -name = "android_system_properties" -version = "0.1.5" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "819e7219dbd41043ac279b19830f2efc897156490d7fd6ea916720117ee66311" -dependencies = [ - "libc", -] - -[[package]] -name = "ansi_term" -version = "0.12.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d52a9bb7ec0cf484c551830a7ce27bd20d67eac647e1befb56b0be4ee39a55d2" -dependencies = [ - "winapi", -] - -[[package]] -name = "anyhow" -version = "1.0.69" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "224afbd727c3d6e4b90103ece64b8d1b67fbb1973b1046c2281eed3f3803f800" - [[package]] name = "approx" version = "0.5.1" @@ -71,47 +47,6 @@ dependencies = [ "num-traits", ] -[[package]] -name = "aquamarine" -version = "0.1.12" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a941c39708478e8eea39243b5983f1c42d2717b3620ee91f4a52115fd02ac43f" -dependencies = [ - "itertools 0.9.0", - "proc-macro-error", - "proc-macro2", - "quote", - "syn", -] - -[[package]] -name = "arc-swap" -version = "1.6.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "bddcadddf5e9015d310179a59bb28c4d4b9920ad0f11e8e14dbadf654890c9a6" - -[[package]] -name = "async-recursion" -version = "1.0.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "2cda8f4bcc10624c4e85bc66b3f452cca98cfa5ca002dc83a16aad2367641bea" -dependencies = [ - "proc-macro2", - "quote", - "syn", -] - -[[package]] -name = "async-trait" -version = "0.1.60" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "677d1d8ab452a3936018a687b20e6f7cf5363d713b732b8884001317b0e48aa3" -dependencies = [ - "proc-macro2", - "quote", - "syn", -] - [[package]] name = "atty" version = "0.2.14" @@ -129,115 +64,11 @@ version = "1.1.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "d468802bab17cbc0cc575e9b053f41e72aa36bfa6b7f55e3529ffa43161b97fa" -[[package]] -name = "autocxx" -version = "0.21.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a84a9279b1a106694b5253374ba9321dea56e7a2acdbdb3025193de2f9dd4c72" -dependencies = [ - "aquamarine", - "autocxx-macro", - "cxx", - "moveit", -] - -[[package]] -name = "autocxx-bindgen" -version = "0.59.16" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "435723e14bf88f198322f8555a4fdb108363021d97a47bb6492891ca86055e79" -dependencies = [ - "bitflags", - "cexpr", - "clang-sys", - "clap 2.34.0", - "env_logger", - "itertools 0.10.5", - "lazy_static", - "lazycell", - "log", - "peeking_take_while", - "proc-macro2", - "quote", - "regex", - "rustc-hash", - "shlex", - "which", -] - -[[package]] -name = "autocxx-build" -version = "0.21.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "807778f48a2c408a1d637c41e6e122ddd370ad64996e4d02fe16222195e6c968" -dependencies = [ - "autocxx-engine", - "env_logger", - "syn", -] - -[[package]] -name = "autocxx-engine" -version = "0.21.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "4d68259e12b51c208d20f7b3df4b76f79be0bbe18b8c05d9553271a189b336bb" -dependencies = [ - "aquamarine", - "autocxx-bindgen", - "autocxx-parser", - "cc", - "cxx-gen", - "indoc", - "itertools 0.10.5", - "log", - "miette 4.7.1", - "once_cell", - "proc-macro2", - "quote", - "regex", - "serde_json", - "strum_macros", - "syn", - "tempfile", - "thiserror", - "version_check", -] - -[[package]] -name = "autocxx-macro" -version = "0.21.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0b1724f53179a0d8b733b7e1afe8a5d456237e6ddcd340c40825a14864e9885e" -dependencies = [ - "autocxx-parser", - "proc-macro-error", - "proc-macro2", - "quote", - "syn", -] - -[[package]] -name = "autocxx-parser" -version = "0.21.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b96f18764bb041f89d1879685697561be8b367ac7e199a5391ddcb80b1cef533" -dependencies = [ - "itertools 0.10.5", - "log", - "once_cell", - "proc-macro2", - "quote", - "serde", - "serde_derive", - "syn", - "thiserror", -] - [[package]] name = "autoupdater" version = "0.0.0" dependencies = [ - "clap 3.2.23", + "clap", "color-eyre", "derive_more", "futures", @@ -330,62 +161,12 @@ version = "1.0.78" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "a20104e2335ce8a659d6dd92a51a767a0c062599c73b343fd152cb401e828c3d" -[[package]] -name = "cexpr" -version = "0.6.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "6fac387a98bb7c37292057cffc56d62ecb629900026402633ae9160df93a8766" -dependencies = [ - "nom", -] - [[package]] name = "cfg-if" version = "1.0.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "baf1de4339761588bc0619e3cbc0120ee582ebb74b53b4efbf79117bd2da40fd" -[[package]] -name = "chrono" -version = "0.4.23" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "16b0a3d9ed01224b22057780a37bb8c5dbfe1be8ba48678e7bf57ec4b385411f" -dependencies = [ - "iana-time-zone", - "js-sys", - "num-integer", - "num-traits", - "time", - "wasm-bindgen", - "winapi", -] - -[[package]] -name = "clang-sys" -version = "1.4.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "fa2e27ae6ab525c3d369ded447057bca5438d86dc3a68f6faafb8269ba82ebf3" -dependencies = [ - "glob", - "libc", - "libloading", -] - -[[package]] -name = "clap" -version = "2.34.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a0610544180c38b88101fecf2dd634b174a62eef6946f84dfc6a7127512b381c" -dependencies = [ - "ansi_term", - "atty", - "bitflags", - "strsim 0.8.0", - "textwrap 0.11.0", - "unicode-width", - "vec_map", -] - [[package]] name = "clap" version = "3.2.23" @@ -394,28 +175,13 @@ checksum = "71655c45cb9845d3270c9d6df84ebe72b4dad3c2ba3f7023ad47c144e4e473a5" dependencies = [ "atty", "bitflags", - "clap_derive 3.2.18", - "clap_lex 0.2.4", + "clap_derive", + "clap_lex", "indexmap", "once_cell", - "strsim 0.10.0", - "termcolor", - "textwrap 0.16.0", -] - -[[package]] -name = "clap" -version = "4.0.32" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a7db700bc935f9e43e88d00b0850dae18a63773cfbec6d8e070fccf7fef89a39" -dependencies = [ - "bitflags", - "clap_derive 4.0.21", - "clap_lex 0.3.0", - "is-terminal", - "once_cell", - "strsim 0.10.0", + "strsim", "termcolor", + "textwrap", ] [[package]] @@ -431,19 +197,6 @@ dependencies = [ "syn", ] -[[package]] -name = "clap_derive" -version = "4.0.21" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0177313f9f02afc995627906bbd8967e2be069f5261954222dac78290c2b9014" -dependencies = [ - "heck", - "proc-macro-error", - "proc-macro2", - "quote", - "syn", -] - [[package]] name = "clap_lex" version = "0.2.4" @@ -453,25 +206,6 @@ dependencies = [ "os_str_bytes", ] -[[package]] -name = "clap_lex" -version = "0.3.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0d4198f73e42b4936b35b5bb248d81d2b595ecb170da0bac7655c54eedfa8da8" -dependencies = [ - "os_str_bytes", -] - -[[package]] -name = "codespan-reporting" -version = "0.11.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3538270d33cc669650c4b093848450d380def10c331d38c768e34cac80576e6e" -dependencies = [ - "termcolor", - "unicode-width", -] - [[package]] name = "color-eyre" version = "0.6.2" @@ -559,62 +293,6 @@ dependencies = [ "syn", ] -[[package]] -name = "cxx" -version = "1.0.85" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5add3fc1717409d029b20c5b6903fc0c0b02fa6741d820054f4a2efa5e5816fd" -dependencies = [ - "cc", - "cxxbridge-flags", - "cxxbridge-macro", - "link-cplusplus", -] - -[[package]] -name = "cxx-build" -version = "1.0.91" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "48fcaf066a053a41a81dfb14d57d99738b767febb8b735c3016e469fac5da690" -dependencies = [ - "cc", - "codespan-reporting", - "once_cell", - "proc-macro2", - "quote", - "scratch", - "syn", -] - -[[package]] -name = "cxx-gen" -version = "0.7.85" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ccca653bd8a21c5cfe696cd5347729d43f651298459b22e57c60fbae1cd49fec" -dependencies = [ - "codespan-reporting", - "proc-macro2", - "quote", - "syn", -] - -[[package]] -name = "cxxbridge-flags" -version = "1.0.85" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "69a3e162fde4e594ed2b07d0f83c6c67b745e7f28ce58c6df5e6b6bef99dfb59" - -[[package]] -name = "cxxbridge-macro" -version = "1.0.85" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3e7e2adeb6a0d4a282e581096b06e1791532b7d576dcde5ccd9382acf55db8e6" -dependencies = [ - "proc-macro2", - "quote", - "syn", -] - [[package]] name = "darling" version = "0.13.4" @@ -658,7 +336,7 @@ dependencies = [ "ident_case", "proc-macro2", "quote", - "strsim 0.10.0", + "strsim", "syn", ] @@ -706,17 +384,6 @@ dependencies = [ "syn", ] -[[package]] -name = "derivative" -version = "2.2.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "fcc3dd5e9e9c0b295d6e1e4d811fb6f157d5ffd784b8d202fc62eac8035a770b" -dependencies = [ - "proc-macro2", - "quote", - "syn", -] - [[package]] name = "derive_more" version = "0.99.17" @@ -730,12 +397,6 @@ dependencies = [ "syn", ] -[[package]] -name = "destructure_traitobject" -version = "0.2.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3c877555693c14d2f84191cfd3ad8582790fc52b5e2274b40b59cf5f5cea25c7" - [[package]] name = "digest" version = "0.10.6" @@ -746,12 +407,6 @@ dependencies = [ "crypto-common", ] -[[package]] -name = "either" -version = "1.8.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "90e5c1c8368803113bf0c9584fc495a58b86dc8a29edbf8fe877d21d9507e797" - [[package]] name = "encoding_rs" version = "0.8.31" @@ -761,40 +416,6 @@ dependencies = [ "cfg-if", ] -[[package]] -name = "env_logger" -version = "0.9.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a12e6657c4c97ebab115a42dcee77225f7f482cdd841cf7088c657a42e9e00e7" -dependencies = [ - "atty", - "humantime", - "log", - "regex", - "termcolor", -] - -[[package]] -name = "errno" -version = "0.2.8" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f639046355ee4f37944e44f60642c6f3a7efa3cf6b78c78a0d989a8ce6c396a1" -dependencies = [ - "errno-dragonfly", - "libc", - "winapi", -] - -[[package]] -name = "errno-dragonfly" -version = "0.1.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "aa68f1b12764fab894d2755d2518754e71b4fd80ecfb822714a1206c2aab39bf" -dependencies = [ - "cc", - "libc", -] - [[package]] name = "eyre" version = "0.6.8" @@ -983,7 +604,7 @@ checksum = "c05aeb6a22b8f62540c194aac980f2115af067bfe15a0734d7277a768d396b31" dependencies = [ "cfg-if", "libc", - "wasi 0.11.0+wasi-snapshot-preview1", + "wasi", ] [[package]] @@ -992,34 +613,6 @@ version = "0.27.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "dec7af912d60cdbd3677c1af9352ebae6fb8394d165568a2234df0fa00f87793" -[[package]] -name = "git-version" -version = "0.3.5" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f6b0decc02f4636b9ccad390dcbe77b722a77efedfa393caf8379a51d5c61899" -dependencies = [ - "git-version-macro", - "proc-macro-hack", -] - -[[package]] -name = "git-version-macro" -version = "0.3.5" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "fe69f1cbdb6e28af2bac214e943b99ce8a0a06b447d15d3e61161b0423139f3f" -dependencies = [ - "proc-macro-hack", - "proc-macro2", - "quote", - "syn", -] - -[[package]] -name = "glob" -version = "0.3.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9b919933a397b79c37e33b77bb2aa3dc8eb6e165ad809e58ff75bc7db2e34574" - [[package]] name = "h2" version = "0.3.15" @@ -1103,12 +696,6 @@ version = "1.0.2" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "c4a1e36c821dbe04574f602848a19f742f4fb3c98d40449f11bcad18d6b17421" -[[package]] -name = "humantime" -version = "2.1.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9a3a5bfb195931eeb336b2a7b4d761daec841b97f947d34394601737a7bba5e4" - [[package]] name = "hyper" version = "0.14.23" @@ -1146,30 +733,6 @@ dependencies = [ "tokio-native-tls", ] -[[package]] -name = "iana-time-zone" -version = "0.1.53" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "64c122667b287044802d6ce17ee2ddf13207ed924c712de9a66a5814d5b64765" -dependencies = [ - "android_system_properties", - "core-foundation-sys", - "iana-time-zone-haiku", - "js-sys", - "wasm-bindgen", - "winapi", -] - -[[package]] -name = "iana-time-zone-haiku" -version = "0.1.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0703ae284fc167426161c2e3f1da3ea71d94b21bedbcc9494e92b28e334e3dca" -dependencies = [ - "cxx", - "cxx-build", -] - [[package]] name = "ident_case" version = "1.0.1" @@ -1198,70 +761,24 @@ version = "1.9.2" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "1885e79c1fc4b10f0e172c475f458b7f7b93061064d98c3293e98c5ba0c8b399" dependencies = [ - "autocfg", - "hashbrown", -] - -[[package]] -name = "indoc" -version = "1.0.8" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "da2d6f23ffea9d7e76c53eee25dfb67bcd8fde7f1198b0855350698c9f07c780" - -[[package]] -name = "instant" -version = "0.1.12" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "7a5bbe824c507c5da5956355e86a746d82e0e1464f65d862cc5e71da70e94b2c" -dependencies = [ - "cfg-if", -] - -[[package]] -name = "io-lifetimes" -version = "1.0.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "46112a93252b123d31a119a8d1a1ac19deac4fac6e0e8b0df58f0d4e5870e63c" -dependencies = [ - "libc", - "windows-sys 0.42.0", -] - -[[package]] -name = "ipnet" -version = "2.7.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "11b0d96e660696543b251e58030cf9787df56da39dab19ad60eae7353040917e" - -[[package]] -name = "is-terminal" -version = "0.4.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "28dfb6c8100ccc63462345b67d1bbc3679177c75ee4bf59bf29c8b1d110b8189" -dependencies = [ - "hermit-abi 0.2.6", - "io-lifetimes", - "rustix", - "windows-sys 0.42.0", + "autocfg", + "hashbrown", ] [[package]] -name = "itertools" -version = "0.9.0" +name = "instant" +version = "0.1.12" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "284f18f85651fe11e8a991b2adb42cb078325c996ed026d994719efcfca1d54b" +checksum = "7a5bbe824c507c5da5956355e86a746d82e0e1464f65d862cc5e71da70e94b2c" dependencies = [ - "either", + "cfg-if", ] [[package]] -name = "itertools" -version = "0.10.5" +name = "ipnet" +version = "2.7.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b0fd2260e829bddf4cb6ea802289de2f86d6a7a690192fbe91b3f46e0f2c8473" -dependencies = [ - "either", -] +checksum = "11b0d96e660696543b251e58030cf9787df56da39dab19ad60eae7353040917e" [[package]] name = "itoa" @@ -1284,12 +801,6 @@ version = "1.4.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "e2abad23fbc42b3700f2f279844dc832adb2b2eb069b2df918f455c4e18cc646" -[[package]] -name = "lazycell" -version = "1.3.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "830d08ce1d1d941e6b30645f1a0eb5643013d835ce3779a5fc208261dbe10f55" - [[package]] name = "libc" version = "0.2.139" @@ -1312,27 +823,12 @@ version = "0.2.6" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "348108ab3fba42ec82ff6e9564fc4ca0247bdccdc68dd8af9764bbc79c3c8ffb" -[[package]] -name = "link-cplusplus" -version = "1.0.8" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ecd207c9c713c34f95a097a5b029ac2ce6010530c7b49d7fea24d977dede04f5" -dependencies = [ - "cc", -] - [[package]] name = "linked-hash-map" version = "0.5.6" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "0717cef1bc8b636c6e1c1bbdefc09e6322da8a9321966e8928ef80d20f7f770f" -[[package]] -name = "linux-raw-sys" -version = "0.1.4" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f051f77a7c8e6957c0696eac88f26b0117e54f52d3fc682ab19397a8812846a4" - [[package]] name = "lock_api" version = "0.4.9" @@ -1350,39 +846,6 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "abb12e687cfb44aa40f41fc3978ef76448f9b6038cad6aef4259d3c095a2382e" dependencies = [ "cfg-if", - "serde", -] - -[[package]] -name = "log-mdc" -version = "0.1.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a94d21414c1f4a51209ad204c1776a3d0765002c76c6abcb602a6f09f1e881c7" - -[[package]] -name = "log4rs" -version = "1.2.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d36ca1786d9e79b8193a68d480a0907b612f109537115c6ff655a3a1967533fd" -dependencies = [ - "anyhow", - "arc-swap", - "chrono", - "derivative", - "fnv", - "humantime", - "libc", - "log", - "log-mdc", - "parking_lot", - "serde", - "serde-value", - "serde_json", - "serde_yaml", - "thiserror", - "thread-id", - "typemap-ors", - "winapi", ] [[package]] @@ -1400,64 +863,12 @@ version = "2.5.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "2dffe52ecf27772e601905b7522cb4ef790d2cc203488bbd0e2fe85fcb74566d" -[[package]] -name = "miette" -version = "4.7.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "1c90329e44f9208b55f45711f9558cec15d7ef8295cc65ecd6d4188ae8edc58c" -dependencies = [ - "miette-derive 4.7.1", - "once_cell", - "thiserror", - "unicode-width", -] - -[[package]] -name = "miette" -version = "5.5.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "4afd9b301defa984bbdbe112b4763e093ed191750a0d914a78c1106b2d0fe703" -dependencies = [ - "miette-derive 5.5.0", - "once_cell", - "thiserror", - "unicode-width", -] - -[[package]] -name = "miette-derive" -version = "4.7.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "6b5bc45b761bcf1b5e6e6c4128cd93b84c218721a8d9b894aa0aff4ed180174c" -dependencies = [ - "proc-macro2", - "quote", - "syn", -] - -[[package]] -name = "miette-derive" -version = "5.5.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "97c2401ab7ac5282ca5c8b518a87635b1a93762b0b90b9990c509888eeccba29" -dependencies = [ - "proc-macro2", - "quote", - "syn", -] - [[package]] name = "mime" version = "0.3.16" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "2a60c7ce501c71e03a9c9c0d35b861413ae925bd979cc7a4e30d060069aaac8d" -[[package]] -name = "minimal-lexical" -version = "0.2.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "68354c5c6bd36d73ff3feceb05efa59b6acb7626617f4962be322a825e61f79a" - [[package]] name = "miniz_oxide" version = "0.6.2" @@ -1475,19 +886,10 @@ checksum = "e5d732bc30207a6423068df043e3d02e0735b155ad7ce1a6f76fe2baa5b158de" dependencies = [ "libc", "log", - "wasi 0.11.0+wasi-snapshot-preview1", + "wasi", "windows-sys 0.42.0", ] -[[package]] -name = "moveit" -version = "0.5.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d7d756ffe4e38013507d35bf726a93fcdae2cae043ab5ce477f13857a335030d" -dependencies = [ - "cxx", -] - [[package]] name = "nalgebra" version = "0.30.1" @@ -1631,25 +1033,6 @@ dependencies = [ "tempfile", ] -[[package]] -name = "nom" -version = "7.1.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a8903e5a29a317527874d0402f867152a3d21c908bb0b933e416c65e301d4c36" -dependencies = [ - "memchr", - "minimal-lexical", -] - -[[package]] -name = "normpath" -version = "0.3.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "04aaf5e9cb0fbf883cc0423159eacdf96a9878022084b35c462c428cab73bcaf" -dependencies = [ - "winapi", -] - [[package]] name = "num-complex" version = "0.4.2" @@ -1771,15 +1154,6 @@ dependencies = [ "vcpkg", ] -[[package]] -name = "ordered-float" -version = "2.10.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "7940cf2ca942593318d07fcf2596cdca60a85c9e7fab408a5e21a4f9dcd40d87" -dependencies = [ - "num-traits", -] - [[package]] name = "os_str_bytes" version = "6.4.1" @@ -1809,33 +1183,6 @@ dependencies = [ "syn", ] -[[package]] -name = "ovr_overlay" -version = "0.0.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0bca515c469ad7eb699480b5dc61d1c83e1e9694e7c39435ce458a3f9fbdce14" -dependencies = [ - "derive_more", - "lazy_static", - "log", - "nalgebra 0.30.1", - "ovr_overlay_sys", - "slice-of-array", - "thiserror", -] - -[[package]] -name = "ovr_overlay_sys" -version = "0.0.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "16b5e4888d82a67dd8c3d47103aa751083bfe88de063ece9846715c2a78fcbab" -dependencies = [ - "autocxx", - "autocxx-build", - "cxx", - "normpath", -] - [[package]] name = "owo-colors" version = "3.5.0" @@ -1889,12 +1236,6 @@ dependencies = [ "once_cell", ] -[[package]] -name = "peeking_take_while" -version = "0.1.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "19b17cddbe7ec3f8bc800887bab5e717348c95ea2ca0b1bf0837fb964dc67099" - [[package]] name = "percent-encoding" version = "2.2.0" @@ -1959,12 +1300,6 @@ dependencies = [ "version_check", ] -[[package]] -name = "proc-macro-hack" -version = "0.5.20+deprecated" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "dc375e1527247fe1a97d8b7156678dfe7c1af2fc075c9a4db3690ecd2a148068" - [[package]] name = "proc-macro2" version = "1.0.49" @@ -2103,12 +1438,6 @@ version = "0.1.21" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "7ef03e0a2b150c7a90d01faf6254c9c48a41e95fb2a8c2ac1c6f0d2b9aefc342" -[[package]] -name = "rustc-hash" -version = "1.1.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "08d43f7aa6b08d49f382cde6a7982047c3426db949b1424bc4b7ec9ae12c6ce2" - [[package]] name = "rustc_version" version = "0.4.0" @@ -2118,26 +1447,6 @@ dependencies = [ "semver", ] -[[package]] -name = "rustix" -version = "0.36.5" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a3807b5d10909833d3e9acd1eb5fb988f79376ff10fce42937de71a449c4c588" -dependencies = [ - "bitflags", - "errno", - "io-lifetimes", - "libc", - "linux-raw-sys", - "windows-sys 0.42.0", -] - -[[package]] -name = "rustversion" -version = "1.0.11" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5583e89e108996506031660fe09baa5011b9dd0341b89029313006d1fb508d70" - [[package]] name = "ryu" version = "1.0.12" @@ -2169,12 +1478,6 @@ version = "1.1.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "d29ab0c6d3fc0ee92fe66e2d99f700eab17a8d57d1c1d3b748380fb20baa78cd" -[[package]] -name = "scratch" -version = "1.0.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ddccb15bcce173023b3fedd9436f882a0739b8dfb45e4f6b6002bee5929f61b2" - [[package]] name = "security-framework" version = "2.7.0" @@ -2243,16 +1546,6 @@ version = "0.1.2" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "fd2930103714ccef4f1fe5b6a5f2b6fdcfe462a6c802464714bd41e5b5097c33" -[[package]] -name = "serde-value" -version = "0.7.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f3a1a3341211875ef120e117ea7fd5228530ae7e7036a779fdc9117be6b3282c" -dependencies = [ - "ordered-float", - "serde", -] - [[package]] name = "serde_derive" version = "1.0.152" @@ -2319,12 +1612,6 @@ dependencies = [ "lazy_static", ] -[[package]] -name = "shlex" -version = "1.1.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "43b2853a4d09f215c24cc5489c992ce46052d359b5109343cbafbf26bc62f8a3" - [[package]] name = "signal-hook-registry" version = "1.4.0" @@ -2366,7 +1653,7 @@ version = "0.0.0" dependencies = [ "approx", "derive_more", - "nalgebra 0.31.4", + "nalgebra 0.32.1", "num-derive", "num-traits", "petgraph", @@ -2393,34 +1680,6 @@ dependencies = [ "autocfg", ] -[[package]] -name = "slice-of-array" -version = "0.3.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a4f120bb98cb4cb0dab21c882968c3cbff79dd23b46f07b1cf5c25044945ce84" - -[[package]] -name = "slimevr_overlay" -version = "0.0.0" -dependencies = [ - "clap 4.0.32", - "color-eyre", - "eyre", - "git-version", - "lazy_static", - "log", - "log4rs", - "nalgebra 0.30.1", - "num-derive", - "num-traits", - "ovr_overlay", - "solarxr", - "stackvec", - "tokio", - "tokio-graceful-shutdown", - "winapi", -] - [[package]] name = "smallvec" version = "1.10.0" @@ -2465,31 +1724,12 @@ version = "0.2.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "a28337dadadf1f595e7472e02ed7e27cbc790828b10336f144b5790d21152c16" -[[package]] -name = "strsim" -version = "0.8.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8ea5119cdb4c55b55d432abb513a0429384878c15dde60cc77b1c99de1a95a6a" - [[package]] name = "strsim" version = "0.10.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "73473c0e59e6d5812c5dfe2a064a6444949f089e20eec9a2e5506596494e4623" -[[package]] -name = "strum_macros" -version = "0.24.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "1e385be0d24f186b4ce2f9982191e7101bb737312ad61c1f2f984f34bcf85d59" -dependencies = [ - "heck", - "proc-macro2", - "quote", - "rustversion", - "syn", -] - [[package]] name = "syn" version = "1.0.107" @@ -2530,15 +1770,6 @@ dependencies = [ "winapi-util", ] -[[package]] -name = "textwrap" -version = "0.11.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d326610f408c7a4eb6f51c37c330e496b08506c9457c9d34287ecc38809fb060" -dependencies = [ - "unicode-width", -] - [[package]] name = "textwrap" version = "0.16.0" @@ -2565,17 +1796,6 @@ dependencies = [ "syn", ] -[[package]] -name = "thread-id" -version = "4.0.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5fdfe0627923f7411a43ec9ec9c39c3a9b4151be313e0922042581fb6c9b717f" -dependencies = [ - "libc", - "redox_syscall", - "winapi", -] - [[package]] name = "thread_local" version = "1.1.4" @@ -2585,17 +1805,6 @@ dependencies = [ "once_cell", ] -[[package]] -name = "time" -version = "0.1.45" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "1b797afad3f312d1c66a56d11d0316f916356d11bd158fbc6ca6389ff6bf805a" -dependencies = [ - "libc", - "wasi 0.10.0+wasi-snapshot-preview1", - "winapi", -] - [[package]] name = "tinyvec" version = "1.6.0" @@ -2631,23 +1840,6 @@ dependencies = [ "windows-sys 0.42.0", ] -[[package]] -name = "tokio-graceful-shutdown" -version = "0.11.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0ff81b9e7ba1103cf0bd37dffe8c0e7c243a7944a6d5b7e8de77c8c7a6b71718" -dependencies = [ - "async-recursion", - "async-trait", - "futures", - "log", - "miette 5.5.0", - "pin-project-lite", - "thiserror", - "tokio", - "tokio-util", -] - [[package]] name = "tokio-macros" version = "1.8.2" @@ -2775,15 +1967,6 @@ dependencies = [ "utf-8", ] -[[package]] -name = "typemap-ors" -version = "1.0.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a68c24b707f02dd18f1e4ccceb9d49f2058c2fb86384ef9972592904d7a28867" -dependencies = [ - "unsafe-any-ors", -] - [[package]] name = "typenum" version = "1.16.0" @@ -2817,21 +2000,6 @@ version = "1.10.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "1dd624098567895118886609431a7c3b8f516e41d30e0643f03d94592a147e36" -[[package]] -name = "unicode-width" -version = "0.1.10" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c0edd1e5b14653f783770bce4a4dabb4a5108a5370a5f5d8cfe8710c361f6c8b" - -[[package]] -name = "unsafe-any-ors" -version = "1.0.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e0a303d30665362d9680d7d91d78b23f5f899504d4f08b3c4cf08d055d87c0ad" -dependencies = [ - "destructure_traitobject", -] - [[package]] name = "url" version = "2.3.1" @@ -2862,12 +2030,6 @@ version = "0.2.15" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "accd4ea62f7bb7a82fe23066fb0957d48ef677f6eeb8215f372f52e48bb32426" -[[package]] -name = "vec_map" -version = "0.8.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f1bddf1187be692e79c5ffeab891132dfb0f236ed36a43c7ed39f1165ee20191" - [[package]] name = "version_check" version = "0.9.4" @@ -2878,7 +2040,7 @@ checksum = "49874b5167b65d7193b8aba1567f5c7d93d001cafc34600cee003eda787e483f" name = "vqf" version = "0.0.0" dependencies = [ - "nalgebra 0.32.1", + "nalgebra 0.31.4", "num-traits", ] @@ -2892,12 +2054,6 @@ dependencies = [ "try-lock", ] -[[package]] -name = "wasi" -version = "0.10.0+wasi-snapshot-preview1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "1a143597ca7c7793eff794def352d41792a93c481eb1042423ff7ff72ba2c31f" - [[package]] name = "wasi" version = "0.11.0+wasi-snapshot-preview1" @@ -2980,17 +2136,6 @@ dependencies = [ "wasm-bindgen", ] -[[package]] -name = "which" -version = "4.3.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "1c831fbbee9e129a8cf93e7747a82da9d95ba8e16621cae60ec2cdc849bacb7b" -dependencies = [ - "either", - "libc", - "once_cell", -] - [[package]] name = "wide" version = "0.7.5" diff --git a/Cargo.toml b/Cargo.toml index 97b10e7f..9ac2c39f 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -4,12 +4,11 @@ members = [ "networking/firmware_protocol", "networking/solarxr", "networking/tokio_shutdown", - "overlay", "skeletal_model/rust", "skeletal_model/napi", "vqf", ] -exclude = ["da_demo", "nrf_demo", "firmware"] +exclude = ["da_demo", "nrf_demo", "firmware", "overlay",] default-members = [ "autoupdater", "networking/firmware_protocol", @@ -30,12 +29,12 @@ authors = ["Ryan Butler "] repository = "https://github.com/SlimeVR/SlimeVR-Rust" edition = "2021" -rust-version = "1.65" # GATs and let-else +rust-version = "1.73" # These may be opted into use by members of the workspace [workspace.dependencies] log = "0.4" eyre = "0.6" -nalgebra = "0.31" +nalgebra = "0.32" feature_macros = { git = "https://github.com/TheButlah/feature_macros" } diff --git a/firmware/Cargo.lock b/firmware/Cargo.lock index 7cde0829..1af3cc3f 100644 --- a/firmware/Cargo.lock +++ b/firmware/Cargo.lock @@ -1089,6 +1089,7 @@ dependencies = [ "smoltcp", "static_cell", "toml", + "vqf", "xtensa-lx", ] @@ -2168,6 +2169,14 @@ dependencies = [ "vcell", ] +[[package]] +name = "vqf" +version = "0.0.0" +dependencies = [ + "nalgebra", + "num-traits", +] + [[package]] name = "winnow" version = "0.4.1" diff --git a/firmware/Cargo.toml b/firmware/Cargo.toml index 83e2d931..8e71797a 100644 --- a/firmware/Cargo.toml +++ b/firmware/Cargo.toml @@ -63,9 +63,9 @@ mcu-nrf52832 = [ ] # Wi-fi dependencies -net-wifi = ["esp-wifi?/wifi"] # use wifi +net-wifi = ["esp-wifi?/wifi"] # use wifi net-ble = ["esp-wifi?/ble"] -net-stubbed = [] # Stubs out network +net-stubbed = [] # Stubs out network # Supported IMUs imu-bmi160 = [] @@ -80,6 +80,7 @@ log-uart = ["defmt_esp_println?/uart"] # Fusion algorithms for unfused imus fusion-stubbed = [] # Stubs out fusion so it returns the same pose every time fusion-dcm = [] +fusion-vqf = [] # Enable to flash without needing `espflash` direct-boot = ["esp32c3-hal?/direct-boot"] @@ -171,7 +172,11 @@ esp-wifi = { git = "https://github.com/esp-rs/esp-wifi.git", rev = "76ba312", fe smoltcp = { version = "0.9", default-features = false, features = [ ], optional = true } embassy-net = { version = "*", optional = true, features = [ - "nightly", "tcp", "udp", "dhcpv4", "medium-ethernet" + "nightly", + "tcp", + "udp", + "dhcpv4", + "medium-ethernet", ] } # Generic BLE @@ -205,6 +210,7 @@ bmi160 = "0.1" # Sensor fusion dcmimu = "0.2" +vqf = { path = "../vqf" } # Other crates static_cell = "1" @@ -268,7 +274,7 @@ opt-level = 'z' [profile.release] debug = true # Symbols get stripped when flashing anyway -lto = false # LTO doesn't work for esp-wifi +lto = false # LTO doesn't work for esp-wifi ################### diff --git a/firmware/build.rs b/firmware/build.rs index d0624fd9..a5274426 100644 --- a/firmware/build.rs +++ b/firmware/build.rs @@ -67,7 +67,7 @@ fn check_features_compatible() { mandatory_and_unique!("imu-stubbed", "imu-mpu6050", "imu-bmi160"); mandatory_and_unique!("log-rtt", "log-usb-serial", "log-uart"); mandatory_and_unique!("net-wifi", "net-ble", "net-stubbed"); - mandatory_and_unique!("fusion-stubbed", "fusion-dcm"); + mandatory_and_unique!("fusion-stubbed", "fusion-dcm", "fusion-vqf"); #[cfg(any(feature = "mcu-nrf52840", feature = "mcu-nrf52832"))] mandatory_and_unique!( diff --git a/firmware/src/imu/fusion/mod.rs b/firmware/src/imu/fusion/mod.rs index 0614ea91..8c338b89 100644 --- a/firmware/src/imu/fusion/mod.rs +++ b/firmware/src/imu/fusion/mod.rs @@ -1,8 +1,10 @@ mod dcm; mod stubbed; +mod vqf; pub use self::dcm::Dcm; pub use self::stubbed::Stubbed; +pub use self::vqf::VqfFusion; use crate::imu::{FusedData, Imu, UnfusedData}; @@ -36,9 +38,11 @@ impl, F: Fuser> Imu for FusedImu { /// Builds a new fuser. The concrete impl is determined by a feature flag. pub fn new_fuser() -> impl Fuser { #[cfg(feature = "fusion-stubbed")] - let f = Stubbed::new(); + let fusion_algorithm = Stubbed::new(); #[cfg(feature = "fusion-dcm")] - let f = Dcm::new(); + let fusion_algorithm = Dcm::new(); + #[cfg(feature = "fusion-vqf")] + let fusion_algorithm = VqfFusion::new(); - f + fusion_algorithm } diff --git a/firmware/src/imu/fusion/vqf.rs b/firmware/src/imu/fusion/vqf.rs new file mode 100644 index 00000000..b52415d9 --- /dev/null +++ b/firmware/src/imu/fusion/vqf.rs @@ -0,0 +1,28 @@ +use vqf::{Vqf, VqfParameters}; + +use crate::imu::fusion::Fuser; +use crate::imu::{FusedData, UnfusedData}; + +pub struct VqfFusion { + vqf: Vqf, +} + +impl VqfFusion { + #[allow(dead_code)] + pub fn new() -> Self { + let param = VqfParameters::default(); + Self { + vqf: Vqf::new(0.01389, 0.01389, 0.01389, param), + } + } +} + +impl Fuser for VqfFusion { + fn process(&mut self, unfused: &UnfusedData) -> FusedData { + self.vqf + .update(unfused.gyro.clone(), unfused.accel.clone(), None); + + let q = self.vqf.getQuat6D(); + FusedData { q } + } +} diff --git a/rust-toolchain.toml b/rust-toolchain.toml index c0c765a5..1128c1a8 100644 --- a/rust-toolchain.toml +++ b/rust-toolchain.toml @@ -1,4 +1,4 @@ [toolchain] -channel = "1.65" # See workspace Cargo.toml +channel = "1.73" # See workspace Cargo.toml components = ["rust-src"] profile = "default" diff --git a/vqf/Cargo.lock b/vqf/Cargo.lock new file mode 100644 index 00000000..55fc1e28 --- /dev/null +++ b/vqf/Cargo.lock @@ -0,0 +1,188 @@ +# This file is automatically @generated by Cargo. +# It is not intended for manual editing. +version = 3 + +[[package]] +name = "approx" +version = "0.5.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cab112f0a86d568ea0e627cc1d6be74a1e9cd55214684db5561995f6dad897c6" +dependencies = [ + "num-traits", +] + +[[package]] +name = "autocfg" +version = "1.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d468802bab17cbc0cc575e9b053f41e72aa36bfa6b7f55e3529ffa43161b97fa" + +[[package]] +name = "az" +version = "1.2.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7b7e4c2464d97fe331d41de9d5db0def0a96f4d823b8b32a2efd503578988973" + +[[package]] +name = "bytemuck" +version = "1.14.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "374d28ec25809ee0e23827c2ab573d729e293f281dfe393500e7ad618baa61c6" + +[[package]] +name = "cfg-if" +version = "1.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "baf1de4339761588bc0619e3cbc0120ee582ebb74b53b4efbf79117bd2da40fd" + +[[package]] +name = "cordic" +version = "0.1.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0ed0a176c0b8c5c95fa0523177530364c5b68a8895d9745730dbfa692a7412d0" +dependencies = [ + "fixed", +] + +[[package]] +name = "crunchy" +version = "0.2.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7a81dae078cea95a014a339291cec439d2f232ebe854a9d672b796c6afafa9b7" + +[[package]] +name = "fixed" +version = "1.24.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "02c69ce7e7c0f17aa18fdd9d0de39727adb9c6281f2ad12f57cbe54ae6e76e7d" +dependencies = [ + "az", + "bytemuck", + "half", + "typenum", +] + +[[package]] +name = "fixed-sqrt" +version = "0.2.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "af725dbdc8dd186b6914026b51e43d99b332e8e816d3b495b84bf8304ac17b73" +dependencies = [ + "fixed", + "integer-sqrt", + "typenum", +] + +[[package]] +name = "half" +version = "2.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bc52e53916c08643f1b56ec082790d1e86a32e58dc5268f897f313fbae7b4872" +dependencies = [ + "cfg-if", + "crunchy", +] + +[[package]] +name = "integer-sqrt" +version = "0.1.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "276ec31bcb4a9ee45f58bec6f9ec700ae4cf4f4f8f2fa7e06cb406bd5ffdd770" +dependencies = [ + "num-traits", +] + +[[package]] +name = "libm" +version = "0.2.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4ec2a862134d2a7d32d7983ddcdd1c4923530833c9f2ea1a44fc5fa473989058" + +[[package]] +name = "nalgebra" +version = "0.32.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "307ed9b18cc2423f29e83f84fd23a8e73628727990181f18641a8b5dc2ab1caa" +dependencies = [ + "approx", + "num-complex", + "num-rational", + "num-traits", + "simba", + "typenum", +] + +[[package]] +name = "num-complex" +version = "0.4.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1ba157ca0885411de85d6ca030ba7e2a83a28636056c7c699b07c8b6f7383214" +dependencies = [ + "num-traits", +] + +[[package]] +name = "num-integer" +version = "0.1.45" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "225d3389fb3509a24c93f5c29eb6bde2586b98d9f016636dff58d7c6f7569cd9" +dependencies = [ + "autocfg", + "num-traits", +] + +[[package]] +name = "num-rational" +version = "0.4.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0638a1c9d0a3c0914158145bc76cff373a75a627e6ecbfb71cbe6f453a5a19b0" +dependencies = [ + "autocfg", + "num-integer", + "num-traits", +] + +[[package]] +name = "num-traits" +version = "0.2.17" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "39e3200413f237f41ab11ad6d161bc7239c84dcb631773ccd7de3dfe4b5c267c" +dependencies = [ + "autocfg", + "libm", +] + +[[package]] +name = "paste" +version = "1.0.14" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "de3145af08024dea9fa9914f381a17b8fc6034dfb00f3a84013f7ff43f29ed4c" + +[[package]] +name = "simba" +version = "0.8.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "061507c94fc6ab4ba1c9a0305018408e312e17c041eb63bef8aa726fa33aceae" +dependencies = [ + "approx", + "num-complex", + "num-traits", + "paste", +] + +[[package]] +name = "typenum" +version = "1.17.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "42ff0bf0c66b8238c6f3b578df37d0b7848e55df8577b3f74f92a69acceeb825" + +[[package]] +name = "vqf" +version = "0.0.0" +dependencies = [ + "cordic", + "fixed", + "fixed-sqrt", + "nalgebra", + "num-traits", +] diff --git a/vqf/Cargo.toml b/vqf/Cargo.toml index 9572a567..783392c6 100644 --- a/vqf/Cargo.toml +++ b/vqf/Cargo.toml @@ -5,6 +5,12 @@ edition = "2021" license = "MIT" publish = false +[features] +default = ["single-precision"] +debug-state = [] +single-precision = [] + + [dependencies] -nalgebra = { version = "0.32", default-features = false, features = ["libm"] } +nalgebra = { version = "0.31", default-features = false, features = ["libm"] } num-traits = { version = "0.2", default-features = false, features = ["libm"] } diff --git a/vqf/src/lib.rs b/vqf/src/lib.rs index 6149bf4d..8a0215b6 100644 --- a/vqf/src/lib.rs +++ b/vqf/src/lib.rs @@ -1,54 +1,169 @@ //! This crate reimplements most of the relevant parts of the VQF algorithm from //! -//! Currently this is just copy-pasted from the python code, but it should be made more -//! idiomatic before actually using it. I have marked areas most likely to contain bugs +//! Currently this is just copy-pasted from the python code, and some of the cpp code but it should +//! be made more idiomatic before actually using it. I have marked areas most likely to contain bugs //! with ඞ //! //! The original code is licensed under the MIT license, so this crate is also licensed under the MIT license. - #![no_std] -#![allow(non_snake_case)] - -use core::f32::consts::PI; +#![allow(non_snake_case)] //The vqf given names are used +#![warn(missing_docs)] use nalgebra::{ArrayStorage, U2, U9}; +use num_traits::float::Float; -type Quat = nalgebra::UnitQuaternion; -type Vec2 = nalgebra::Vector2; -type Vec3 = nalgebra::Vector3; -type Mat2x3 = nalgebra::Matrix2x3; -type Mat2x9 = nalgebra::Matrix>; -type Mat2x2 = nalgebra::Matrix2; -type Mat3x3 = nalgebra::Matrix3; - -const EPS: f32 = 1e-6; - +#[cfg(feature = "single-precision")] +use core::f32::consts::PI; +#[cfg(feature = "single-precision")] +/// Calculating resolution, use feature 'f64' for f64 calculations +#[allow(non_camel_case_types)] +pub type VQF_Real_T = f32; + +#[cfg(not(feature = "single-precision"))] +use core::f64::consts::PI; +#[cfg(not(feature = "single-precision"))] +/// Calculating resolution, using feature 'f64' for f64 calculations +#[allow(non_camel_case_types)] +pub type VQF_Real_T = f64; + +use core::f64::consts::PI as PI64; + +/// A Quaternion type with capacity VQF_Real_T +pub type Quat = nalgebra::UnitQuaternion; +/// A 2 variable vector with type VQF_Real_T +pub type Vec2 = nalgebra::Vector2; +/// A 3 variable vector with type VQF_Real_T +pub type Vec3 = nalgebra::Vector3; +type Vec2Double = nalgebra::Vector2; +type Vec3Double = nalgebra::Vector3; +type Mat2x3Double = nalgebra::Matrix2x3; +type Mat2x9Double = nalgebra::Matrix>; +type Mat2x2Double = nalgebra::Matrix2; +type Mat3x3 = nalgebra::Matrix3; + +const EPS: VQF_Real_T = VQF_Real_T::EPSILON; + +/// Constructor that initializes the struct with the default parameters. +#[derive(Clone)] pub struct VqfParameters { - pub tauAcc: f32, - pub tauMag: f32, + /// Time constant for accelerometer low-pass filtering in seconds. + /// Small values for imply trust on the accelerometer measurements and while large values of + /// imply trust on the gyroscope measurements. + /// Default value: 3.0 s + pub tauAcc: VQF_Real_T, + /// Time constant for magnetometer update in seconds. + /// Small values for imply trust on the magnetometer measurements and while large values of + /// imply trust on the gyroscope measurements. + /// Default value: 9.0 s + pub tauMag: VQF_Real_T, + /// Enables gyroscope bias estimation during motion phases. + /// If set to true (default), gyroscope bias is estimated based on the inclination correction only, + /// i.e. without using magnetometer measurements. pub motionBiasEstEnabled: bool, + /// Enables rest detection and gyroscope bias estimation during rest phases. + /// If set to true (default), phases in which the IMU is at rest are detected. During rest, + /// the gyroscope bias is estimated from the low-pass filtered gyroscope readings. pub restBiasEstEnabled: bool, + /// Enables magnetic disturbance detection and magnetic disturbance rejection. + /// If set to true (default), the magnetic field is analyzed. For short disturbed phases, the + /// magnetometer-based correction is disabled totally. If the magnetic field is always regarded + /// as disturbed or if the duration of the disturbances exceeds magMaxRejectionTime, + /// magnetometer-based updates are performed, + /// but with an increased time constant. pub magDistRejectionEnabled: bool, - pub biasSigmaInit: f32, - pub biasForgettingTime: f32, - pub biasClip: f32, - pub biasSigmaMotion: f32, - pub biasVerticalForgettingFactor: f32, - pub biasSigmaRest: f32, - pub restMinT: f32, - pub restFilterTau: f32, - pub restThGyr: f32, - pub restThAcc: f32, - pub magCurrentTau: f32, - pub magRefTau: f32, - pub magNormTh: f32, - pub magDipTh: f32, - pub magNewTime: f32, - pub magNewFirstTime: f32, - pub magNewMinGyr: f32, - pub magMinUndisturbedTime: f32, - pub magMaxRejectionTime: f32, - pub magRejectionFactor: f32, + /// Standard deviation of the initial bias estimation uncertainty (in degrees per second). + /// Default value: 0.5 °/s + pub biasSigmaInit: VQF_Real_T, + /// Time in which the bias estimation uncertainty increases from 0 °/s to 0.1 °/s (in seconds). + /// This value determines the system noise assumed by the Kalman filter. + /// Default value: 100.0 s + pub biasForgettingTime: VQF_Real_T, + /// Maximum expected gyroscope bias (in degrees per second). + /// This value is used to clip the bias estimate and the measurement error in the bias estimationupdate step. + /// It is further used by the rest detection algorithm in order to not regard measurements with a large but + /// constant angular rate as rest. + /// Default value: 2.0 °/s + pub biasClip: VQF_Real_T, + /// Standard deviation of the converged bias estimation uncertainty during motion (in degrees per second). + /// This value determines the trust on motion bias estimation updates. A small value leads to fast convergence. + /// Default value: 0.1 °/s + pub biasSigmaMotion: VQF_Real_T, + /// Forgetting factor for unobservable bias in vertical direction during motion. + /// As magnetometer measurements are deliberately not used during motion bias estimation, gyroscope bias + /// is not observable in vertical direction. This value is the relative weight of an artificial zero + /// measurement that ensures that the bias estimate in the unobservable direction will eventually decay to zero. + /// Default value: 0.0001 + pub biasVerticalForgettingFactor: VQF_Real_T, + /// Standard deviation of the converged bias estimation uncertainty during rest (in degrees per second). + /// This value determines the trust on rest bias estimation updates. A small value leads to fast convergence. + /// Default value: 0.03 ° + pub biasSigmaRest: VQF_Real_T, + /// Time threshold for rest detection (in seconds). + /// Rest is detected when the measurements have been close to the low-pass filtered reference for the given time. + /// Default value: 1.5 s + pub restMinT: VQF_Real_T, + /// Time constant for the low-pass filter used in rest detection (in seconds). + /// This time constant characterizes a second-order Butterworth low-pass filter used to obtain the reference for rest detection. + /// Default value: 0.5 s + pub restFilterTau: VQF_Real_T, + /// Angular velocity threshold for rest detection (in °/s). + /// For rest to be detected, the norm of the deviation between measurement and reference must be below the given threshold. + /// (Furthermore, the absolute value of each component must be below biasClip). + /// Default value: 2.0 °/s + pub restThGyr: VQF_Real_T, + /// Acceleration threshold for rest detection (in m/s²). + /// For rest to be detected, the norm of the deviation between measurement and reference must be below the given threshold. + /// Default value: 0.5 m/s² + pub restThAcc: VQF_Real_T, + /// Time constant for current norm/dip value in magnetic disturbance detection (in seconds). + /// This (very fast) low-pass filter is intended to provide additional robustness when the magnetometer measurements + /// are noisy or not sampled perfectly in sync with the gyroscope measurements. Set to -1 to disable the low-pass filter + /// and directly use the magnetometer measurements. + /// Default value: 0.05 s + pub magCurrentTau: VQF_Real_T, + /// Time constant for the adjustment of the magnetic field reference (in seconds). + /// This adjustment allows the reference estimate to converge to the observed undisturbed field. + /// Default value: 20.0 s + pub magRefTau: VQF_Real_T, + /// Relative threshold for the magnetic field strength for magnetic disturbance detection. + /// This value is relative to the reference norm. + /// Default value: 0.1 (10%) + pub magNormTh: VQF_Real_T, + /// Threshold for the magnetic field dip angle for magnetic disturbance detection (in degrees). + /// Default vaule: 10 ° + pub magDipTh: VQF_Real_T, + /// Duration after which to accept a different homogeneous magnetic field (in seconds). + /// A different magnetic field reference is accepted as the new field when the measurements are + /// within the thresholds magNormTh and magDipTh for the given time. Additionally, only phases with + /// sufficient movement, specified by magNewMinGyr, count. + /// Default value: 20.0 + pub magNewTime: VQF_Real_T, + /// Duration after which to accept a homogeneous magnetic field for the first time (in seconds). + /// This value is used instead of magNewTime when there is no current estimate in order to allow + /// for the initial magnetic field reference to be obtained faster. + /// Default value: 5.0 + pub magNewFirstTime: VQF_Real_T, + /// Minimum angular velocity needed in order to count time for new magnetic field acceptance (in °/s). + /// Durations for which the angular velocity norm is below this threshold do not count towards reaching magNewTime. + /// Default value: 20.0 °/s + pub magNewMinGyr: VQF_Real_T, + /// Minimum duration within thresholds after which to regard the field as undisturbed again (in seconds). + /// Default value: 0.5 s + pub magMinUndisturbedTime: VQF_Real_T, + /// Maximum duration of full magnetic disturbance rejection (in seconds). + /// For magnetic disturbances up to this duration, heading correction is fully disabled and heading + /// changes are tracked by gyroscope only. After this duration (or for many small disturbed phases + /// without sufficient time in the undisturbed field in between), the heading correction is performed + /// with an increased time constant (see magRejectionFactor). + /// Default value: 60.0 s + pub magMaxRejectionTime: VQF_Real_T, + /// Factor by which to slow the heading correction during long disturbed phases. + /// After magMaxRejectionTime of full magnetic disturbance rejection, heading correction is performed + /// with an increased time constant. This parameter (approximately) specifies the factor of the increase. + /// Furthermore, after spending magMaxRejectionTime/magRejectionFactor seconds in an undisturbed magnetic + /// field, the time is reset and full magnetic disturbance rejection will be performed for up to magMaxRejectionTime again. + /// Default value: 2.0 + pub magRejectionFactor: VQF_Real_T, } impl Default for VqfParameters { @@ -83,36 +198,99 @@ impl Default for VqfParameters { } } +/// Struct containing the filter state of the VQF class. +/// The relevant parts of the state can be accessed via functions of the VQF class, e.g. +/// VQF::getQuat6D(), VQF::getQuat9D(), VQF::getGyrBiasEstimate(), VQF::setGyrBiasEstimate(), +/// VQF::getRestDetected() and VQF::getMagDistDetected(). +/// To reset the state to the initial values, use VQF::resetState(). + +/// Direct access to the full state is typically not needed but can be useful in some cases, +/// e.g. for debugging. For this purpose, the state can be accessed by VQF::getState() and set by VQF::setState(). +#[derive(Clone)] pub struct VqfState { + /// Angular velocity strapdown integration quaternion pub gyrQuat: Quat, + /// Inclination correction quaternion pub accQuat: Quat, - pub delta: f32, + /// Heading difference δ between ϵi and ϵ + pub delta: VQF_Real_T, + /// True if it has been detected that the IMU is currently at rest. + /// Used to switch between rest and motion gyroscope bias estimation pub restDetected: bool, + /// True if magnetic disturbances have been detected. pub magDistDetected: bool, + /// Last low-pass filtered acceleration in the Ii frame pub lastAccLp: Vec3, - pub accLpState: Mat2x3, - pub kMagInit: f32, - pub lastMagDisAngle: f32, - pub lastMagCorrAngularRate: f32, + /// Internal low-pass filter state for lastAccLp + pub accLpState: Mat2x3Double, + /// Last inclination correction angular rate. + /// Change to inclination correction quaternion + /// performed in the last accelerometer update, expressed as an angular rate (in rad/s). + pub lastAccCorrAngularRate: VQF_Real_T, + /// Gain used for heading correction to ensure fast initial convergence. + /// This value is used as the gain for heading correction in the beginning if it is + /// larger than the normal filter gain. It is initialized to 1 and then updated to + /// 0.5, 0.33, 0.25, … After VQFParams::tauMag seconds, it is set to zero. + pub kMagInit: VQF_Real_T, + /// Last heading disagreement angle. Disagreement between the heading δ^ + /// estimated from the last magnetometer sample and the state δ (in rad) + pub lastMagDisAngle: VQF_Real_T, + /// Last heading correction angular rate. Change to heading + /// performed in the last magnetometer update, expressed as an angular rate δ (in rad/s). + pub lastMagCorrAngularRate: VQF_Real_T, + /// Current gyroscope bias estimate (in rad/s). pub bias: Vec3, + /// Covariance matrix of the gyroscope bias estimate. The 3x3 matrix is stored in row-major order. + /// Note that for numeric reasons the internal unit used is 0.01 °/s, i.e. + /// to get the standard deviation in degrees per second use (equation in vqf docs) pub biasP: Mat3x3, - pub motionBiasEstRLpState: Mat2x9, - pub motionBiasEstBiasLpState: Mat2x2, + /// Internal state of the Butterworth low-pass filter for the rotation matrix + /// coefficients used in motion bias estimation. + pub motionBiasEstRLpState: Mat2x9Double, + /// Internal low-pass filter state for the rotated bias estimate used in motion bias estimation. + pub motionBiasEstBiasLpState: Mat2x2Double, + /// Last (squared) deviations from the reference of the last sample used in rest detection. + /// Looking at those values can be useful to understand how rest detection is working and which + /// thresholds are suitable. The array contains the last values for gyroscope and accelerometer + /// in the respective units. Note that the values are squared. + /// The method VQF::getRelativeRestDeviations() provides an easier way to obtain and interpret those values. pub restLastSquaredDeviations: Vec2, - pub restT: f32, + /// The current duration for which all sensor readings are within the rest detection thresholds. + /// Rest is detected if this value is larger or equal to VQFParams::restMinT + pub restT: VQF_Real_T, + /// Last low-pass filtered gyroscope measurement used as the reference for rest detection. + /// Note that this value is also used for gyroscope bias estimation when rest is detected. pub restLastGyrLp: Vec3, - pub restGyrLpState: Mat2x3, + /// Internal low-pass filter state for restLastGyrLp. + pub restGyrLpState: Mat2x3Double, + /// Last low-pass filtered accelerometer measurement used as the reference for rest detection. pub restLastAccLp: Vec3, - pub restAccLpState: Mat2x3, - pub magRefNorm: f32, - pub magRefDip: f32, - pub magUndisturbedT: f32, - pub magRejectT: f32, - pub magCandidateNorm: f32, - pub magCandidateDip: f32, - pub magCandidateT: f32, + /// Internal low-pass filter state for restLastAccLp. + pub restAccLpState: Mat2x3Double, + /// Norm of the currently accepted magnetic field reference. + /// A value of -1 indicates that no homogeneous field is found yet. + pub magRefNorm: VQF_Real_T, + /// Dip angle of the currently accepted magnetic field reference. + pub magRefDip: VQF_Real_T, + /// The current duration for which the current norm and dip are close to the reference. + /// The magnetic field is regarded as undisturbed when this value reaches VQFParams::magMinUndisturbedTime. + pub magUndisturbedT: VQF_Real_T, + /// The current duration for which the magnetic field was rejected. + /// If the magnetic field is disturbed and this value is smaller than VQFParams::magMaxRejectionTime, heading correction updates are fully disabled. + pub magRejectT: VQF_Real_T, + /// Norm of the alternative magnetic field reference currently being evaluated. + pub magCandidateNorm: VQF_Real_T, + /// Dip angle of the alternative magnetic field reference currently being evaluated. + pub magCandidateDip: VQF_Real_T, + /// The current duration for which the norm and dip are close to the candidate. + /// If this value exceeds VQFParams::magNewTime (or VQFParams::magNewFirstTime if magRefNorm < 0), + /// the current candidate is accepted as the new reference. + pub magCandidateT: VQF_Real_T, + /// Norm and dip angle of the current magnetometer measurements. + /// Slightly low-pass filtered, see VQFParams::magCurrentTau. pub magNormDip: Vec2, - pub magNormDipLpState: Mat2x2, + /// Internal low-pass filter state for the current norm and dip angle. + pub magNormDipLpState: Mat2x2Double, } impl Default for VqfState { @@ -124,20 +302,21 @@ impl Default for VqfState { restDetected: false, magDistDetected: true, lastAccLp: Vec3::zeros(), - accLpState: Mat2x3::repeat(f32::NAN), + accLpState: Mat2x3Double::repeat(f64::NAN), + lastAccCorrAngularRate: 0.0, kMagInit: 1.0, lastMagDisAngle: 0.0, lastMagCorrAngularRate: 0.0, bias: Vec3::zeros(), - biasP: Mat3x3::repeat(f32::NAN), - motionBiasEstRLpState: Mat2x9::repeat(f32::NAN), - motionBiasEstBiasLpState: Mat2x2::repeat(f32::NAN), + biasP: Mat3x3::repeat(VQF_Real_T::NAN), + motionBiasEstRLpState: Mat2x9Double::repeat(f64::NAN), + motionBiasEstBiasLpState: Mat2x2Double::repeat(f64::NAN), restLastSquaredDeviations: Vec2::zeros(), restT: 0.0, restLastGyrLp: Vec3::zeros(), - restGyrLpState: Mat2x3::repeat(f32::NAN), + restGyrLpState: Mat2x3Double::repeat(f64::NAN), restLastAccLp: Vec3::zeros(), - restAccLpState: Mat2x3::repeat(f32::NAN), + restAccLpState: Mat2x3Double::repeat(f64::NAN), magRefNorm: 0.0, magRefDip: 0.0, magUndisturbedT: 0.0, @@ -146,30 +325,53 @@ impl Default for VqfState { magCandidateDip: 0.0, magCandidateT: 0.0, magNormDip: Vec2::zeros(), - magNormDipLpState: Mat2x2::repeat(f32::NAN), + magNormDipLpState: Mat2x2Double::repeat(f64::NAN), } } } +/// Coefficients are values that depend on the parameters and the sampling times, +/// but do not change during update steps. They are calculated in VQF::setup(). +#[derive(Clone)] pub struct VQFCoefficients { - pub gyrTs: f32, - pub accTs: f32, - pub magTs: f32, - pub accLpB: Vec3, - pub accLpA: Vec2, - pub kMag: f32, - pub biasP0: f32, - pub biasV: f32, - pub biasMotionW: f32, - pub biasVerticalW: f32, - pub biasRestW: f32, - pub restGyrLpB: Vec3, - pub restGyrLpA: Vec2, - pub restAccLpB: Vec3, - pub restAccLpA: Vec2, - pub kMagRef: f32, - pub magNormDipLpB: Vec3, - pub magNormDipLpA: Vec2, + /// Sampling time of the gyroscope measurements (in seconds). + pub gyrTs: VQF_Real_T, + /// Sampling time of the accelerometer measurements (in seconds). + pub accTs: VQF_Real_T, + /// Sampling time of the magnetometer measurements (in seconds). + pub magTs: VQF_Real_T, + /// Numerator coefficients of the acceleration low-pass filter. + /// The array contains [b0, b1, b2] + pub accLpB: Vec3Double, + /// Denominator coefficients of the acceleration low-pass filter. + /// The array contains [a1, a2] and a0 = 1 + pub accLpA: Vec2Double, + /// Gain of the first-order filter used for heading correction. + pub kMag: VQF_Real_T, + /// Variance of the initial gyroscope bias estimate. + pub biasP0: VQF_Real_T, + /// System noise variance used in gyroscope bias estimation. + pub biasV: VQF_Real_T, + /// Measurement noise variance for the motion gyroscope bias estimation update. + pub biasMotionW: VQF_Real_T, + /// Measurement noise variance for the motion gyroscope bias estimation update in vertical direction. + pub biasVerticalW: VQF_Real_T, + /// Measurement noise variance for the rest gyroscope bias estimation update. + pub biasRestW: VQF_Real_T, + /// Numerator coefficients of the gyroscope measurement low-pass filter for rest detection. + pub restGyrLpB: Vec3Double, + /// Denominator coefficients of the gyroscope measurement low-pass filter for rest detection. + pub restGyrLpA: Vec2Double, + /// Numerator coefficients of the accelerometer measurement low-pass filter for rest detection. + pub restAccLpB: Vec3Double, + /// Denominator coefficients of the accelerometer measurement low-pass filter for rest detection. + pub restAccLpA: Vec2Double, + /// Gain of the first-order filter used for to update the magnetic field reference and candidate. + pub kMagRef: VQF_Real_T, + /// Numerator coefficients of the low-pass filter for the current magnetic norm and dip. + pub magNormDipLpB: Vec3Double, + /// Denominator coefficients of the low-pass filter for the current magnetic norm and dip. + pub magNormDipLpA: Vec2Double, } impl Default for VQFCoefficients { @@ -178,25 +380,26 @@ impl Default for VQFCoefficients { gyrTs: 0.0, accTs: 0.0, magTs: 0.0, - accLpB: Vec3::repeat(f32::NAN), - accLpA: Vec2::repeat(f32::NAN), + accLpB: Vec3Double::repeat(f64::NAN), + accLpA: Vec2Double::repeat(f64::NAN), kMag: -1.0, biasP0: -1.0, biasV: -1.0, biasMotionW: -1.0, biasVerticalW: -1.0, biasRestW: -1.0, - restGyrLpB: Vec3::repeat(f32::NAN), - restGyrLpA: Vec2::repeat(f32::NAN), - restAccLpB: Vec3::repeat(f32::NAN), - restAccLpA: Vec2::repeat(f32::NAN), + restGyrLpB: Vec3Double::repeat(f64::NAN), + restGyrLpA: Vec2Double::repeat(f64::NAN), + restAccLpB: Vec3Double::repeat(f64::NAN), + restAccLpA: Vec2Double::repeat(f64::NAN), kMagRef: -1.0, - magNormDipLpB: Vec3::repeat(f32::NAN), - magNormDipLpA: Vec2::repeat(f32::NAN), + magNormDipLpB: Vec3Double::repeat(f64::NAN), + magNormDipLpA: Vec2Double::repeat(f64::NAN), } } } +/// Internally used struct to manage vqf pub struct Vqf { _params: VqfParameters, _state: VqfState, @@ -204,20 +407,64 @@ pub struct Vqf { } impl Vqf { - pub fn new(gyrTs: f32, accTs: f32, magTs: f32, params: VqfParameters) -> Vqf { - Vqf { + /// Used to create a new instance of VQF + pub fn new( + gyrTs: VQF_Real_T, + accTs: VQF_Real_T, + magTs: VQF_Real_T, + params: VqfParameters, + ) -> Vqf { + let mut coeffs: VQFCoefficients = Default::default(); + assert!(gyrTs > 0.0); + assert!(accTs > 0.0); + assert!(magTs > 0.0); + + coeffs.gyrTs = gyrTs; + coeffs.accTs = accTs; + coeffs.magTs = magTs; + + (coeffs.accLpB, coeffs.accLpA) = filterCoeffs(params.tauAcc, coeffs.accTs); + coeffs.kMag = gainFromTau(params.tauMag, coeffs.magTs); + + coeffs.biasP0 = (params.biasSigmaInit * 100.0).powi(2); + + // the system noise increases the variance from 0 to (0.1 °/s)^2 in biasForgettingTime seconds + coeffs.biasV = (0.1 * 100.0).powi(2) * coeffs.accTs / params.biasForgettingTime; + + let pMotion = (params.biasSigmaMotion * 100.0).powi(2); + coeffs.biasMotionW = (pMotion).powi(2) / coeffs.biasV + pMotion; + coeffs.biasVerticalW = + coeffs.biasMotionW / params.biasVerticalForgettingFactor.max(1e-10); + + let pRest = (params.biasSigmaRest * 100.0).powi(2); + coeffs.biasRestW = (pRest).powi(2) / coeffs.biasV + pRest; + + (coeffs.restGyrLpB, coeffs.restGyrLpA) = + filterCoeffs(params.restFilterTau, coeffs.gyrTs); + (coeffs.restAccLpB, coeffs.restAccLpA) = + filterCoeffs(params.restFilterTau, coeffs.accTs); + + coeffs.kMagRef = gainFromTau(params.magRefTau, coeffs.magTs); + if params.magCurrentTau > 0.0 { + (coeffs.magNormDipLpB, coeffs.magNormDipLpA) = + filterCoeffs(params.magCurrentTau, coeffs.magTs); + } + + let mut vqf = Vqf { _params: params, _state: Default::default(), - _coeffs: VQFCoefficients { - gyrTs, - accTs, - magTs, - ..Default::default() - }, - } + _coeffs: coeffs, + }; + vqf.resetState(); + vqf } + /// Preforms gyroscope update step + /// It is only necessary to call this function directly if gyroscope, accelerometers + /// and magnetometers have different smpling rates. Otherwise simply use update() + /// Parameters: gyr - gyroscope measument in rad/s pub fn updateGyr(&mut self, gyr: Vec3) { + // rest detection if self._params.restBiasEstEnabled || self._params.magDistRejectionEnabled { let gyrLp = filterVec( gyr, @@ -252,12 +499,17 @@ impl Vqf { let c = (angle / 2.0).cos(); let s = (angle / 2.0).sin() / gyrNorm; let gyrStepQuat = Quat::from_quaternion( - [c, s * gyrNoBias[0], s * gyrNoBias[1], s * gyrNoBias[2]].into(), + [s * gyrNoBias[0], s * gyrNoBias[1], s * gyrNoBias[2], c].into(), ); self._state.gyrQuat = self._state.gyrQuat * gyrStepQuat; + self._state.gyrQuat.renormalize_fast(); } } + /// Preforms accelerometer update step + /// It is only necessary to call this function directly if gyroscope, accelerometers + /// and magnetometers have different smpling rates. Otherwise simply use update() + /// Paramters: acc - accelerometer measurement in m/s² pub fn updateAcc(&mut self, acc: Vec3) { if acc == Vec3::zeros() { return; @@ -305,22 +557,23 @@ impl Vqf { ); // transform to 6D earth frame and normalize - let accEarth = self._state.accQuat * self._state.lastAccLp; + let accEarth = (self._state.accQuat * self._state.lastAccLp).normalize(); // inclination correction let q_w = ((accEarth[2] + 1.0) / 2.0).sqrt(); let accCorrQuat; - if q_w > EPS { + if q_w > 1e-6 { accCorrQuat = Quat::from_quaternion( - [q_w, 0.5 * accEarth[1] / q_w, -0.5 * accEarth[0] / q_w, 0.0].into(), + [0.5 * accEarth[1] / q_w, -0.5 * accEarth[0] / q_w, 0.0, q_w].into(), ); } else { - accCorrQuat = Quat::from_quaternion([0.0, 1.0, 0.0, 0.0].into()); + accCorrQuat = Quat::from_quaternion([1.0, 0.0, 0.0, 0.0].into()); } self._state.accQuat = accCorrQuat * self._state.accQuat; + self._state.accQuat.renormalize_fast(); // calculate correction angular rate to facilitate debugging - // self._state.lastAccCorrAngularRate = (accEarth[2]).acos() / self._coeffs.accTs; + self._state.lastAccCorrAngularRate = (accEarth[2]).acos() / self._coeffs.accTs; // bias estimation if self._params.motionBiasEstEnabled || self._params.restBiasEstEnabled { @@ -329,7 +582,16 @@ impl Vqf { // get rotation matrix corresponding to accGyrQuat let accGyrQuat = self.getQuat6D(); - // ඞ + // ඞ but works + let accGyrQuat = Quat::from_quaternion( + [ + -accGyrQuat.coords.x, + -accGyrQuat.coords.y, + accGyrQuat.coords.z, + accGyrQuat.coords.w, + ] + .into(), + ); let R = accGyrQuat.to_rotation_matrix().into_inner(); // calculate R*b_hat (only the x and y component, as z is not needed) @@ -372,7 +634,6 @@ impl Vqf { - R[5] * bias[2], -R[6] * bias[0] - R[7] * bias[1] - R[8] * bias[2], )); - R = Mat3x3::repeat(f32::NAN); w = Some(Vec3::new( self._coeffs.biasMotionW, self._coeffs.biasMotionW, @@ -402,17 +663,29 @@ impl Vqf { let e = Vec3::from_fn(|x, y| e[(x, y)].clamp(-biasClip, biasClip)); // step 2: K = P R^T inv(W + R P R^T) - let K = self._state.biasP + let mut K = self._state.biasP * R.transpose() * (Mat3x3::from_diagonal(&w) + R * self._state.biasP * R.transpose()) .pseudo_inverse(EPS) .unwrap(); + // ඞ why are the signs messed up also K is transposed but works for the math + K[(0, 2)] *= -1.0; + K[(1, 2)] *= -1.0; + K[(2, 0)] *= -1.0; + K[(2, 1)] *= -1.0; + // step 3: bias = bias + K (y - R bias) = bias + K e bias += K * e; // step 4: P = P - K R P - self._state.biasP -= K * R * self._state.biasP; + let mut biasP_change = K * R * self._state.biasP; + // ඞ why are the signs messed up again + biasP_change[(0, 2)] *= -1.0; + biasP_change[(1, 2)] *= -1.0; + biasP_change[(2, 2)] *= -1.0; + + self._state.biasP -= biasP_change; // clip bias estimate to -2..2 °/s bias = Vec3::from_fn(|x, y| bias[(x, y)].clamp(-biasClip, biasClip)); @@ -423,6 +696,10 @@ impl Vqf { } } + /// Preforms magnetometer update step + /// It is only necessary to call this function directly if gyroscope, accelerometers + /// and magnetometers have different smpling rates. Otherwise simply use update() + /// Parameters: mag - magnetometer measurement in arbitrary units pub fn updateMag(&mut self, mag: Vec3) { if mag == Vec3::zeros() { return; @@ -563,6 +840,7 @@ impl Vqf { } } + /// Bulk update the gyro, accel and optionally the mag pub fn update(&mut self, gyr: Vec3, acc: Vec3, mag: Option) { self.updateGyr(gyr); self.updateAcc(acc); @@ -571,34 +849,283 @@ impl Vqf { } } + /// Returns the angular velocity strapdown integration quaternion pub fn getQuat3D(&self) -> Quat { self._state.gyrQuat } + /// Returns the 6D (magnetometer-free) orientation quaternion pub fn getQuat6D(&self) -> Quat { self._state.accQuat * self.getQuat3D() } + + /// Returns the 9D (with magnetometers) orientation quaternion + pub fn getQuat9D(&self) -> Quat { + quatApplyDelta(self.getQuat6D(), self._state.delta) + } + + /// Returns the heading difference δ between ϵi and ϵ in rad + pub fn getDelta(&self) -> VQF_Real_T { + self._state.delta + } + + /// Returns the current gyroscope bias estimate and the uncetainty + /// The returned standard deviation sigma represents the estimation uncertainty + /// in the worst direction and is based on an upper bound of the largest + /// eigenvalue of the covariance matrix + /// VQF_Real_T - standard deviation sigma of the estimation uncertainty (rad/s) + /// Vec3 - output array for the gyroscope bias estimate (rad/s) + pub fn getBiasEstimate(&self) -> (VQF_Real_T, Vec3) { + let sum1 = self._state.biasP[(0, 0)].abs() + + self._state.biasP[(0, 1)].abs() + + self._state.biasP[(0, 2)].abs(); + let sum2 = self._state.biasP[(1, 0)].abs() + + self._state.biasP[(1, 1)].abs() + + self._state.biasP[(1, 2)].abs(); + let sum3 = self._state.biasP[(2, 0)].abs() + + self._state.biasP[(2, 1)].abs() + + self._state.biasP[(2, 2)].abs(); + + let max: Vec3 = [sum1, sum2, sum3].into(); + let p = max.max().min(self._coeffs.biasP0); + + let std_dev = p.sqrt() * (PI / 100.0 / 180.0); + (std_dev, self._state.bias) + } + + /// Sets the current gyroscope bias estimate and the uncertainty + /// If a value for the uncertainty sigma is given, the covariance + /// matrix is set to a corresponding scale identity matrix + /// bias - gyroscope bias estimate (rad/s) + /// sigma - standard deviation of the estimation uncertainty (rad/s) + /// - set to -1 (default) in order to not change the estimation + /// - covariance matrix + pub fn setBiasEstimate(&self, _bias: Vec3, _sigma: Option) { + //if sigma none then = -1.0; + todo!(); + } + + /// Returns true if rest was detected + pub fn getRestDetected(&self) -> bool { + self._state.restDetected + } + + /// Returns true if a disturbed magnetic field was detected + pub fn getMagDistDetected(&self) -> bool { + self._state.magDistDetected + } + + /// Returns the relative deviations used in rest detection + /// Looking at those values can be useful to understand how rest detection is + /// working and which thresholds are suitable. The output array is fulled with + /// the last values for gyroscope and accelerometer, relative to the threshold. + /// In order for rest to be detected, both values must stay below 1 + /// Returns - relative rest deviations + pub fn getRelativeRestDeviations(&self) -> Vec2 { + todo!(); + } + + /// Returns the norm of the currently accepted magnetic field reference + pub fn getMagRefNorm(&self) -> VQF_Real_T { + self._state.magRefNorm + } + + /// Returns the dip angle of the currently accepted magnetic field reference + pub fn getMagRefDip(&self) -> VQF_Real_T { + self._state.magRefDip + } + + /// Overwrites the current magnetic field reference + pub fn setMagRef(mut self, norm: VQF_Real_T, dip: VQF_Real_T) { + self._state.magRefNorm = norm; + self._state.magRefDip = dip; + } + + /// Sets the time constant for accelerometer low-pass filtering (in seconds) + pub fn setTauAcc(mut self, tauAcc: VQF_Real_T) { + self._params.tauAcc = tauAcc; + todo!(); + } + + /// Sets the time constant for the magnetometer update (in seconds) + pub fn setTauMag(mut self, tauMag: VQF_Real_T) { + self._params.tauMag = tauMag; + todo!(); + } + + /// Enables/disables gyroscope bias estimation during motion + pub fn setMotionBiasEstEnabled(mut self, enabled: bool) { + if self._params.motionBiasEstEnabled == enabled { + return; + } + self._params.motionBiasEstEnabled = enabled; + self._state.motionBiasEstRLpState = Mat2x9Double::repeat(f64::NAN); + self._state.motionBiasEstBiasLpState = Mat2x2Double::repeat(f64::NAN); + } + + /// Enables/disables rest detection and bias estimation during rest + pub fn setRestBiasEstEnabled(mut self, enabled: bool) { + if self._params.restBiasEstEnabled == enabled { + return; + } + self._params.restBiasEstEnabled = enabled; + self._state.restDetected = false; + self._state.restLastSquaredDeviations = Vec2::zeros(); + self._state.restT = 0.0; + self._state.restLastGyrLp = Vec3::zeros(); + self._state.restGyrLpState = Mat2x3Double::repeat(f64::NAN); + self._state.restLastAccLp = Vec3::zeros(); + self._state.restAccLpState = Mat2x3Double::repeat(f64::NAN); + } + + /// Enables/disables magnetic disturbance detection and rejection + pub fn setMagDistRejectionEnabled(mut self, enabled: bool) { + if self._params.magDistRejectionEnabled == enabled { + return; + } + self._params.magDistRejectionEnabled = enabled; + self._state.magDistDetected = true; + self._state.magRefNorm = 0.0; + self._state.magRefDip = 0.0; + self._state.magUndisturbedT = 0.0; + self._state.magRejectT = self._params.magMaxRejectionTime; + self._state.magCandidateNorm = -1.0; + self._state.magCandidateDip = 0.0; + self._state.magCandidateT = 0.0; + self._state.magNormDipLpState = Mat2x2Double::repeat(f64::NAN); + } + + /// Sets the current thresholds for rest detection + pub fn setRestDetectionThresholds(mut self, thGyr: VQF_Real_T, thAcc: VQF_Real_T) { + self._params.restThGyr = thGyr; + self._params.restThAcc = thAcc; + } + + /// Returns the current parameters (read only) + pub fn getParams(&self) -> VqfParameters { + self._params.clone() + } + + /// Returns the coefficients used by the algorithm (read only) + #[cfg(feature = "debug-state")] + pub fn getCoeffs(&self) -> VQFCoefficients { + self._coeffs.clone() + } + + /// Returns the current state (read only) + #[cfg(feature = "debug-state")] + pub fn getState(&self) -> VqfState { + self._state.clone() + } + + /// Overwrites the current state + /// This method allows to set a completely arbitrary filter state and + /// is intended for debugging purposes. In combination with getState() + /// individual elements of the state can be modified + #[cfg(feature = "debug-state")] + pub fn setState(mut self, state: VqfState) { + self._state = state; + } + + /// Resets the state to the defualt values at initialization + /// Resetting the state is equivalent to creating a new instance of this class + pub fn resetState(&mut self) { + self._state.gyrQuat = Quat::identity(); + self._state.accQuat = Quat::identity(); + self._state.delta = 0.0; + + self._state.restDetected = false; + self._state.magDistDetected = true; + + self._state.lastAccLp = Vec3::zeros(); + self._state.accLpState = Mat2x3Double::repeat(f64::NAN); + self._state.lastAccCorrAngularRate = 0.0; + + self._state.kMagInit = 1.0; + self._state.lastMagDisAngle = 0.0; + self._state.lastMagCorrAngularRate = 0.0; + + self._state.bias = Vec3::zeros(); + self._state.biasP = Mat3x3::identity() * self._coeffs.biasP0; + self._state.motionBiasEstRLpState = Mat2x9Double::repeat(f64::NAN); + self._state.motionBiasEstBiasLpState = Mat2x2Double::repeat(f64::NAN); + + self._state.restLastSquaredDeviations = Vec2::zeros(); + self._state.restT = 0.0; + self._state.restLastGyrLp = Vec3::zeros(); + self._state.restGyrLpState = Mat2x3Double::repeat(f64::NAN); + self._state.restLastAccLp = Vec3::zeros(); + self._state.restAccLpState = Mat2x3Double::repeat(f64::NAN); + + self._state.magRefNorm = 0.0; + self._state.magRefDip = 0.0; + self._state.magUndisturbedT = 0.0; + self._state.magRejectT = self._params.magMaxRejectionTime; + self._state.magCandidateNorm = -1.0; + self._state.magCandidateDip = 0.0; + self._state.magCandidateT = 0.0; + self._state.magNormDip = Vec2::zeros(); + self._state.magNormDipLpState = Mat2x2Double::repeat(f64::NAN); + } +} + +fn quatApplyDelta(q: Quat, delta: VQF_Real_T) -> Quat { + let c = (delta / 2.0).cos(); + let s = (delta / 2.0).sin(); + let w = c * q.coords.w - s * q.coords.z; + let x = c * q.coords.x - s * q.coords.y; + let y = c * q.coords.y + s * q.coords.x; + let z = c * q.coords.z + s * q.coords.w; + Quat::from_quaternion([x, y, z, w].into()) +} + +fn filterCoeffs(tau: VQF_Real_T, Ts: VQF_Real_T) -> (Vec3Double, Vec2Double) { + assert!(tau > 0.0); + assert!(Ts > 0.0); + // second order Butterworth filter based on https://stackoverflow.com/a/52764064 + let fc: f64 = ((2.0 as f64).sqrt() / (2.0 * PI64)) / tau as f64; // time constant of dampened, non-oscillating part of step response + let C = (PI64 * fc * Ts as f64).tan(); + let D = C.powi(2) + (2.0 as f64).sqrt() * C + 1.0; + let b0 = C.powi(2) / D; + let outB: Vec3Double = [b0, 2.0 * b0, b0].into(); + + // a0 = 1.0 + let a1 = 2.0 * (C.powi(2) - 1.0) / D; + let a2 = (1.0 - (2.0 as f64).sqrt() * C + C.powi(2)) / D; + let outA: Vec2Double = [a1, a2].into(); + return (outB, outA); +} + +fn gainFromTau(tau: VQF_Real_T, Ts: VQF_Real_T) -> VQF_Real_T { + assert!(Ts > 0.0); + if tau < 0.0 { + return 0.0; // k=0 for negative tau (disable update) + } else if tau == 0.0 { + return 1.0; //k=1 for tau=0 + } else { + return 1.0 - (-Ts / tau).exp(); // fc = 1/(2*pi*tau) + } } fn filterVec( - x: nalgebra::SVector, - tau: f32, - Ts: f32, - b: Vec3, - a: Vec2, + x: nalgebra::SVector, + tau: VQF_Real_T, + Ts: VQF_Real_T, + b: Vec3Double, + a: Vec2Double, state: &mut nalgebra::Matrix< - f32, + f64, nalgebra::Const, nalgebra::Const, - ArrayStorage, + ArrayStorage, >, ) -> nalgebra::Matrix< - f32, + VQF_Real_T, nalgebra::Const, nalgebra::Const<1>, - ArrayStorage, + ArrayStorage, > { - assert!(M >= N); + assert!(N >= M); // to avoid depending on a single sample, average the first samples (for duration tau) // and then use this average to calculate the filter initial state if state[(0, 0)].is_nan() { @@ -614,11 +1141,11 @@ fn filterVec( let mut out = nalgebra::SMatrix::zeros(); // ඞ for (i, x) in x.iter().enumerate() { - state[(1, i)] += *x; - out[i] = state[(1, i)] / state[(0, 1)]; + state[(1, i)] += *x as f64; + out[i] = (state[(1, i)] / state[(0, 1)]) as VQF_Real_T; } - if state[(0, 1)] * Ts >= tau { + if state[(0, 1)] as VQF_Real_T * Ts >= tau { for i in 0..N { let init = filterInitialState(out[i], b, a); // ඞ @@ -633,36 +1160,484 @@ fn filterVec( filterStep(x, b, a, state) } -fn filterInitialState(x0: f32, b: Vec3, a: Vec2) -> Vec2 { - Vec2::new(x0 * (1.0 - b[0]), x0 * (b[2] - a[1])) +fn filterInitialState(x0: VQF_Real_T, b: Vec3Double, a: Vec2Double) -> Vec2Double { + Vec2Double::new(x0 as f64 * (1.0 - b[0]), x0 as f64 * (b[2] - a[1])) } fn filterStep( - x: nalgebra::SVector, - b: Vec3, - a: Vec2, + x: nalgebra::SVector, + b: Vec3Double, + a: Vec2Double, state: &mut nalgebra::Matrix< - f32, + f64, nalgebra::Const, nalgebra::Const, - ArrayStorage, + ArrayStorage, >, ) -> nalgebra::Matrix< - f32, + VQF_Real_T, nalgebra::Const, nalgebra::Const<1>, - ArrayStorage, + ArrayStorage, > { // difference equations based on scipy.signal.lfilter documentation // assumes that a0 == 1.0 - let y = b[0] * x + nalgebra::SMatrix::repeat(state[0]); + let x_64 = x.map(|x| x as f64); + let y_64: nalgebra::Matrix< + f64, + nalgebra::Const, + nalgebra::Const<1>, + ArrayStorage, + > = b[0] * x_64 + nalgebra::SMatrix::repeat(state[0]); + let y: nalgebra::Matrix< + VQF_Real_T, + nalgebra::Const, + nalgebra::Const<1>, + ArrayStorage, + > = y_64.map(|x| x as VQF_Real_T); + // ඞ for i in 0..N { - state[(0, i)] = b[1] * x[i] - a[0] * y[i] + state[(1, i)]; + state[(0, i)] = b[1] * x[i] as f64 - a[0] * y_64[i] + state[(1, i)]; } // ඞ for i in 0..N { - state[(1, i)] = b[2] * x[i] - a[1] * y[i]; + state[(1, i)] = b[2] * x[i] as f64 - a[1] * y_64[i]; } return y; } + +#[no_mangle] +fn fminf(a: f32, b: f32) -> f32 { + if a < b { + a + } else { + b + } +} + +#[no_mangle] +fn fmaxf(a: f32, b: f32) -> f32 { + if a > b { + a + } else { + b + } +} + +#[cfg(test)] +mod test { + use crate::Quat; + use crate::Vec3; + use crate::Vqf; + use crate::VqfParameters; + + #[test] + fn quat_ordering() { + let quat = Quat::from_quaternion([1.0, 2.0, 3.0, 4.0].into()); + assert!(quat.coords.x == quat[0]); + assert!((quat.coords.x - 0.182574183).abs() < 1e-7); + assert!(quat.coords.y == quat[1]); + assert!((quat.coords.y - 0.365148365).abs() < 1e-7); + assert!(quat.coords.z == quat[2]); + assert!((quat.coords.z - 0.547722518).abs() < 1e-7); + assert!(quat.coords.w == quat[3]); + assert!((quat.coords.w - 0.730296731).abs() < 1e-7); + } + + #[test] + fn single_same_3D_quat() { + let param = VqfParameters::default(); + let mut vqf = Vqf::new(0.01, 0.01, 0.01, param); + + let gyr: Vec3 = [0.021, 0.021, 0.021].into(); + vqf.updateGyr(gyr); + + let quat = vqf.getQuat3D(); + assert!((quat.coords.w - 1.0).abs() < 1e-6); + assert!((quat.coords.x - 0.000105).abs() < 1e-6); + assert!((quat.coords.y - 0.000105).abs() < 1e-6); + assert!((quat.coords.z - 0.000105).abs() < 1e-6); + } + + #[test] + fn single_x_3D_quat() { + let param = VqfParameters::default(); + let mut vqf = Vqf::new(0.01, 0.01, 0.01, param); + + let gyr: Vec3 = [0.25, 0.0, 0.0].into(); + vqf.updateGyr(gyr); + + let quat = vqf.getQuat3D(); + assert!((quat.coords.w - 0.9999999).abs() < 1e-6); + assert!((quat.coords.x - 0.00125).abs() < 1e-6); + assert!((quat.coords.y - 0.0).abs() < 1e-6); + assert!((quat.coords.z - 0.0).abs() < 1e-6); + } + + #[test] + fn single_y_3D_quat() { + let param = VqfParameters::default(); + let mut vqf = Vqf::new(0.01, 0.01, 0.01, param); + + let gyr: Vec3 = [0.0, 0.25, 0.0].into(); + vqf.updateGyr(gyr); + + let quat = vqf.getQuat3D(); + assert!((quat.coords.w - 0.9999999).abs() < 1e-6); + assert!((quat.coords.x - 0.0).abs() < 1e-6); + assert!((quat.coords.y - 0.00125).abs() < 1e-6); + assert!((quat.coords.z - 0.0).abs() < 1e-6); + } + + #[test] + fn single_z_3D_quat() { + let param = VqfParameters::default(); + let mut vqf = Vqf::new(0.01, 0.01, 0.01, param); + + let gyr: Vec3 = [0.0, 0.0, 0.25].into(); + vqf.updateGyr(gyr); + + let quat = vqf.getQuat3D(); + assert!((quat.coords.w - 0.9999999).abs() < 1e-6); + assert!((quat.coords.x - 0.0).abs() < 1e-6); + assert!((quat.coords.y - 0.0).abs() < 1e-6); + assert!((quat.coords.z - 0.00125).abs() < 1e-6); + } + + #[test] + fn single_different_3D_quat() { + let param = VqfParameters::default(); + let mut vqf = Vqf::new(0.01, 0.01, 0.01, param); + + let gyr: Vec3 = [0.054, 0.012, -0.9].into(); + vqf.updateGyr(gyr); + + let quat = vqf.getQuat3D(); + assert!((quat.coords.w - 0.99999).abs() < 1e-6); + assert!((quat.coords.x - 0.000269999).abs() < 1e-6); + assert!((quat.coords.y - 5.99998e-5).abs() < 1e-6); + assert!((quat.coords.z - -0.00449998).abs() < 1e-6); + } + + #[test] + fn many_same_3D_quat() { + let param = VqfParameters::default(); + let mut vqf = Vqf::new(0.01, 0.01, 0.01, param); + + let gyr: Vec3 = [0.021, 0.021, 0.021].into(); + for _ in 0..10_000 { + vqf.updateGyr(gyr); + } + + let quat = vqf.getQuat3D(); + assert!((quat.coords.w - -0.245327).abs() < 1e-6); //slightly different results + assert!((quat.coords.x - 0.559707).abs() < 1e-6); + assert!((quat.coords.y - 0.559707).abs() < 1e-6); + assert!((quat.coords.z - 0.559707).abs() < 1e-6); + } + + #[test] + fn many_different_3D_quat() { + let param = VqfParameters::default(); + let mut vqf = Vqf::new(0.01, 0.01, 0.01, param); + + let gyr: Vec3 = [0.054, 0.012, -0.09].into(); + for _ in 0..10_000 { + vqf.updateGyr(gyr); + } + + let quat = vqf.getQuat3D(); + assert!((quat.coords.w - 0.539342).abs() < 1e-6); //slightly different results + assert!((quat.coords.x - -0.430446).abs() < 1e-6); + assert!((quat.coords.y - -0.0956546).abs() < 1e-6); + assert!((quat.coords.z - 0.71741).abs() < 1e-6); + } + + #[test] + fn single_same_6D_quat() { + let param = VqfParameters::default(); + let mut vqf = Vqf::new(0.01, 0.01, 0.01, param); + + let gyr: Vec3 = [0.021, 0.021, 0.021].into(); + let acc: Vec3 = [5.663806, 5.663806, 5.663806].into(); + vqf.updateGyr(gyr); + vqf.updateAcc(acc); + + let quat = vqf.getQuat6D(); + assert!((quat.coords.w - 0.888074).abs() < 1e-6); + assert!((quat.coords.x - 0.325117).abs() < 1e-6); + assert!((quat.coords.y - -0.324998).abs() < 1e-6); + assert!((quat.coords.z - 0.00016151).abs() < 1e-6); + } + + #[test] + fn single_x_6D_quat() { + let param = VqfParameters::default(); + let mut vqf = Vqf::new(0.01, 0.01, 0.01, param); + + let gyr: Vec3 = [0.021, 0.021, 0.021].into(); + let acc: Vec3 = [9.81, 0.0, 0.0].into(); + vqf.update(gyr, acc, None); + + let quat = vqf.getQuat6D(); + assert!((quat.coords.w - 0.707107).abs() < 1e-6); + assert!((quat.coords.x - 0.000148508).abs() < 1e-6); + assert!((quat.coords.y - -0.707107).abs() < 1e-6); + assert!((quat.coords.z - 0.000148508).abs() < 1e-6); + } + + #[test] + fn single_y_6D_quat() { + let param = VqfParameters::default(); + let mut vqf = Vqf::new(0.01, 0.01, 0.01, param); + + let gyr: Vec3 = [0.021, 0.021, 0.021].into(); + let acc: Vec3 = [0.0, 9.81, 0.0].into(); + vqf.update(gyr, acc, None); + + let quat = vqf.getQuat6D(); + assert!((quat.coords.w - 0.707107).abs() < 1e-6); + assert!((quat.coords.x - 0.707107).abs() < 1e-6); + assert!((quat.coords.y - 0.000148477).abs() < 1e-6); + assert!((quat.coords.z - 0.000148477).abs() < 1e-6); + } + + #[test] + fn single_z_6D_quat() { + let param = VqfParameters::default(); + let mut vqf = Vqf::new(0.01, 0.01, 0.01, param); + + let gyr: Vec3 = [0.021, 0.021, 0.021].into(); + let acc: Vec3 = [0.0, 0.0, 9.81].into(); + vqf.update(gyr, acc, None); + + let quat = vqf.getQuat6D(); + assert!((quat.coords.w - 1.0).abs() < 1e-6); + assert!((quat.coords.x - -1.72732e-20).abs() < 1e-6); + assert!((quat.coords.y - -4.06576e-20).abs() < 1e-6); + assert!((quat.coords.z - 0.000105).abs() < 1e-6); + } + + #[test] + fn single_different_6D_quat() { + let param = VqfParameters::default(); + let mut vqf = Vqf::new(0.01, 0.01, 0.01, param); + + let gyr: Vec3 = [0.021, 0.021, 0.021].into(); + let acc: Vec3 = [4.5, 6.7, 3.2].into(); + vqf.update(gyr, acc, None); + + let quat = vqf.getQuat6D(); + assert!((quat.coords.w - 0.827216).abs() < 1e-6); + assert!((quat.coords.x - 0.466506).abs() < 1e-6); + assert!((quat.coords.y - -0.313187).abs() < 1e-6); + assert!((quat.coords.z - 0.000168725).abs() < 1e-6); + } + + #[test] + fn many_same_6D_quat() { + let param = VqfParameters::default(); + let mut vqf = Vqf::new(0.01, 0.01, 0.01, param); + + let gyr: Vec3 = [0.021, 0.021, 0.021].into(); + let acc: Vec3 = [5.663806, 5.663806, 5.663806].into(); + + for _ in 0..10_000 { + vqf.update(gyr, acc, None); + } + + let quat = vqf.getQuat6D(); + assert!((quat.coords.w - 0.887649).abs() < 1e-6); //Look into why there is so + assert!((quat.coords.x - 0.334951).abs() < 1e-6); // much difference between them + assert!((quat.coords.y - -0.314853).abs() < 1e-6); // we use f32 math, they use mostly double math + assert!((quat.coords.z - 0.0274545).abs() < 1e-6); + } + + #[test] + fn many_different_6D_quat() { + let param = VqfParameters::default(); + let mut vqf = Vqf::new(0.01, 0.01, 0.01, param); + + let gyr: Vec3 = [0.021, 0.021, 0.021].into(); + let acc: Vec3 = [4.5, 6.7, 3.2].into(); + + for _ in 0..10_000 { + vqf.update(gyr, acc, None); + } + + let quat = vqf.getQuat6D(); + assert!((quat.coords.w - 0.826852).abs() < 1e-6); //Look into why there is so + assert!((quat.coords.x - 0.475521).abs() < 1e-6); // much difference between them + assert!((quat.coords.y - -0.299322).abs() < 1e-6); // we use f32 math, they use mostly double math + assert!((quat.coords.z - 0.0245133).abs() < 1e-6); + } + + #[test] + fn single_same_9D_quat() { + let param = VqfParameters::default(); + let mut vqf = Vqf::new(0.01, 0.01, 0.01, param); + + let gyr: Vec3 = [0.021, 0.021, 0.021].into(); + let acc: Vec3 = [5.663806, 5.663806, 5.663806].into(); + let mag: Vec3 = [10.0, 10.0, 10.0].into(); + vqf.updateGyr(gyr); + vqf.updateAcc(acc); + vqf.updateMag(mag); + + let quat = vqf.getQuat9D(); + assert!((quat.coords.w - 0.86428).abs() < 1e-6); + assert!((quat.coords.x - 0.391089).abs() < 1e-6); + assert!((quat.coords.y - -0.241608).abs() < 1e-6); + assert!((quat.coords.z - 0.204195).abs() < 1e-6); + } + + #[test] + fn single_x_9D_quat() { + let param = VqfParameters::default(); + let mut vqf = Vqf::new(0.01, 0.01, 0.01, param); + + let gyr: Vec3 = [0.021, 0.021, 0.021].into(); + let acc: Vec3 = [5.663806, 5.663806, 5.663806].into(); + let mag: Vec3 = [10.0, 0.0, 0.0].into(); + vqf.update(gyr, acc, Some(mag)); + + let quat = vqf.getQuat9D(); + assert!((quat.coords.w - 0.540625).abs() < 1e-6); + assert!((quat.coords.x - 0.455768).abs() < 1e-6); + assert!((quat.coords.y - 0.060003).abs() < 1e-6); + assert!((quat.coords.z - 0.704556).abs() < 1e-6); + } + + #[test] + fn single_y_9D_quat() { + let param = VqfParameters::default(); + let mut vqf = Vqf::new(0.01, 0.01, 0.01, param); + + let gyr: Vec3 = [0.021, 0.021, 0.021].into(); + let acc: Vec3 = [5.663806, 5.663806, 5.663806].into(); + let mag: Vec3 = [0.0, 10.0, 0.0].into(); + vqf.update(gyr, acc, Some(mag)); + + let quat = vqf.getQuat9D(); + assert!((quat.coords.w - 0.880476).abs() < 1e-6); + assert!((quat.coords.x - 0.279848).abs() < 1e-6); + assert!((quat.coords.y - -0.364705).abs() < 1e-6); + assert!((quat.coords.z - -0.115917).abs() < 1e-6); + } + + #[test] + fn single_z_9D_quat() { + let param = VqfParameters::default(); + let mut vqf = Vqf::new(0.01, 0.01, 0.01, param); + + let gyr: Vec3 = [0.021, 0.021, 0.021].into(); + let acc: Vec3 = [5.663806, 5.663806, 5.663806].into(); + let mag: Vec3 = [0.0, 0.0, 10.0].into(); + vqf.update(gyr, acc, Some(mag)); + + let quat = vqf.getQuat9D(); + assert!((quat.coords.w - 0.339851).abs() < 1e-6); + assert!((quat.coords.x - -0.17592).abs() < 1e-6); + assert!((quat.coords.y - -0.424708).abs() < 1e-6); + assert!((quat.coords.z - -0.820473).abs() < 1e-6); + } + + #[test] + fn single_different_9D_quat() { + let param = VqfParameters::default(); + let mut vqf = Vqf::new(0.01, 0.01, 0.01, param); + + let gyr: Vec3 = [0.021, 0.021, 0.021].into(); + let acc: Vec3 = [5.663806, 5.663806, 5.663806].into(); + let mag: Vec3 = [3.54, 6.32, -2.34].into(); + vqf.update(gyr, acc, Some(mag)); + + let quat = vqf.getQuat9D(); + assert!((quat.coords.w - 0.864117).abs() < 1e-6); + assert!((quat.coords.x - 0.391281).abs() < 1e-6); + assert!((quat.coords.y - -0.241297).abs() < 1e-6); + assert!((quat.coords.z - 0.204882).abs() < 1e-6); + } + + #[test] + fn many_same_9D_quat() { + let param = VqfParameters::default(); + let mut vqf = Vqf::new(0.01, 0.01, 0.01, param); + + let gyr: Vec3 = [0.021, 0.021, 0.021].into(); + let acc: Vec3 = [5.663806, 5.663806, 5.663806].into(); + let mag: Vec3 = [10.0, 10.0, 10.0].into(); + + for _ in 0..10_000 { + vqf.update(gyr, acc, Some(mag)); + } + + let quat = vqf.getQuat9D(); + assert!((quat.coords.w - 0.338005).abs() < 1e-6); //Look into why there is so + assert!((quat.coords.x - -0.176875).abs() < 1e-6); // much difference between them + assert!((quat.coords.y - -0.424311).abs() < 1e-6); // we use f32 math, they use mostly double math + assert!((quat.coords.z - -0.821236).abs() < 1e-6); + } + + #[test] + fn many_different_9D_quat() { + let param = VqfParameters::default(); + let mut vqf = Vqf::new(0.01, 0.01, 0.01, param); + + let gyr: Vec3 = [0.021, 0.021, 0.021].into(); + let acc: Vec3 = [5.663806, 5.663806, 5.663806].into(); + let mag: Vec3 = [3.54, 6.32, -2.34].into(); + + for _ in 0..10_000 { + vqf.update(gyr, acc, Some(mag)); + } + + let quat = vqf.getQuat9D(); + assert!((quat.coords.w - 0.864111).abs() < 1e-6); //Look into why there is so + assert!((quat.coords.x - 0.391288).abs() < 1e-6); // much difference between them + assert!((quat.coords.y - -0.241286).abs() < 1e-6); // we use f32 math, they use mostly double math + assert!((quat.coords.z - 0.204906).abs() < 1e-6); + } + + #[test] + fn run_vqf_cpp_example() { + let param = VqfParameters::default(); + let mut vqf = Vqf::new(0.01, 0.01, 0.01, param); + + let gyr: Vec3 = Vec3::repeat(0.01745329); + let acc: Vec3 = Vec3::repeat(5.663806); + + for _ in 0..6000 { + vqf.update(gyr, acc, None); + } + + let quat = vqf.getQuat6D(); + assert!((quat.coords.w - 0.887781).abs() < 1e-6); + assert!((quat.coords.x - 0.333302).abs() < 1e-6); + assert!((quat.coords.y - -0.316598).abs() < 1e-6); + assert!((quat.coords.z - 0.0228175).abs() < 1e-6); + } + + #[test] + fn run_vqf_cpp_example_basic() { + let mut param = VqfParameters::default(); + param.restBiasEstEnabled = false; + param.motionBiasEstEnabled = false; + param.magDistRejectionEnabled = false; + let mut vqf = Vqf::new(0.01, 0.01, 0.01, param); + + let gyr: Vec3 = Vec3::repeat(0.01745329); + let acc: Vec3 = Vec3::repeat(5.663806); + + for _ in 0..6000 { + vqf.update(gyr, acc, None); + } + + let quat = vqf.getQuat6D(); + assert!((quat.coords.w - 0.547223).abs() < 1e-6); + assert!((quat.coords.x - 0.456312).abs() < 1e-6); + assert!((quat.coords.y - 0.055717).abs() < 1e-6); + assert!((quat.coords.z - 0.699444).abs() < 1e-6); + } +}