diff --git a/.clang-format b/.clang-format index 82e1136c..8340806b 100644 --- a/.clang-format +++ b/.clang-format @@ -1,137 +1,307 @@ --- -# Documentation: https://releases.llvm.org/17.0.1/tools/clang/docs/ClangFormatStyleOptions.html -Language: Cpp -# BasedOnStyle: LLVM -AccessModifierOffset: -2 +BasedOnStyle: Chromium +AccessModifierOffset: -1 AlignAfterOpenBracket: Align -AlignConsecutiveMacros: false -AlignConsecutiveAssignments: true -AlignConsecutiveDeclarations: false -AlignEscapedNewlines: Right -AlignOperands: true -AlignTrailingComments: true +AlignArrayOfStructures: None +AlignConsecutiveAssignments: + Enabled: false + AcrossEmptyLines: false + AcrossComments: false + AlignCompound: false + AlignFunctionPointers: false + PadOperators: true +AlignConsecutiveBitFields: + Enabled: false + AcrossEmptyLines: false + AcrossComments: false + AlignCompound: false + AlignFunctionPointers: false + PadOperators: false +AlignConsecutiveDeclarations: + Enabled: false + AcrossEmptyLines: false + AcrossComments: false + AlignCompound: false + AlignFunctionPointers: false + PadOperators: false +AlignConsecutiveMacros: + Enabled: false + AcrossEmptyLines: false + AcrossComments: false + AlignCompound: false + AlignFunctionPointers: false + PadOperators: false +AlignConsecutiveShortCaseStatements: + Enabled: false + AcrossEmptyLines: false + AcrossComments: false + AlignCaseArrows: false + AlignCaseColons: false +AlignConsecutiveTableGenBreakingDAGArgColons: + Enabled: false + AcrossEmptyLines: false + AcrossComments: false + AlignCompound: false + AlignFunctionPointers: false + PadOperators: false +AlignConsecutiveTableGenCondOperatorColons: + Enabled: false + AcrossEmptyLines: false + AcrossComments: false + AlignCompound: false + AlignFunctionPointers: false + PadOperators: false +AlignConsecutiveTableGenDefinitionColons: + Enabled: false + AcrossEmptyLines: false + AcrossComments: false + AlignCompound: false + AlignFunctionPointers: false + PadOperators: false +AlignEscapedNewlines: Left +AlignOperands: Align +AlignTrailingComments: + Kind: Always + OverEmptyLines: 0 AllowAllArgumentsOnNextLine: true -AllowAllConstructorInitializersOnNextLine: true -AllowAllParametersOfDeclarationOnNextLine: true +AllowAllParametersOfDeclarationOnNextLine: false +AllowBreakBeforeNoexceptSpecifier: Never AllowShortBlocksOnASingleLine: Never +AllowShortCaseExpressionOnASingleLine: true AllowShortCaseLabelsOnASingleLine: false -AllowShortFunctionsOnASingleLine: All -AllowShortLambdasOnASingleLine: All +AllowShortCompoundRequirementOnASingleLine: true +AllowShortEnumsOnASingleLine: true +AllowShortFunctionsOnASingleLine: Inline AllowShortIfStatementsOnASingleLine: Never +AllowShortLambdasOnASingleLine: All AllowShortLoopsOnASingleLine: false AlwaysBreakAfterDefinitionReturnType: None -AlwaysBreakAfterReturnType: None -AlwaysBreakBeforeMultilineStrings: false -AlwaysBreakTemplateDeclarations: MultiLine -BinPackArguments: false -BinPackParameters: true +AlwaysBreakBeforeMultilineStrings: true +AttributeMacros: + - __capability +BinPackArguments: true +BinPackParameters: false +BitFieldColonSpacing: Both BraceWrapping: - AfterCaseLabel: false - AfterClass: false - AfterControlStatement: false - AfterEnum: false - AfterFunction: false - AfterNamespace: false + AfterCaseLabel: false + AfterClass: false + AfterControlStatement: Never + AfterEnum: false + AfterFunction: false + AfterNamespace: false AfterObjCDeclaration: false - AfterStruct: false - AfterUnion: false + AfterStruct: false + AfterUnion: false AfterExternBlock: false - BeforeCatch: false - BeforeElse: false - IndentBraces: false + BeforeCatch: false + BeforeElse: false + BeforeLambdaBody: false + BeforeWhile: false + IndentBraces: false SplitEmptyFunction: true SplitEmptyRecord: true SplitEmptyNamespace: true +BreakAdjacentStringLiterals: true +BreakAfterAttributes: Leave +BreakAfterJavaFieldAnnotations: false +BreakAfterReturnType: None +BreakArrays: true BreakBeforeBinaryOperators: None -BreakBeforeBraces: Stroustrup -BreakBeforeInheritanceComma: false -BreakInheritanceList: BeforeColon +BreakBeforeBraces: Attach +BreakBeforeConceptDeclarations: Always +BreakBeforeInlineASMColon: OnlyMultiline BreakBeforeTernaryOperators: true -BreakConstructorInitializersBeforeComma: true -BreakConstructorInitializers: BeforeComma -BreakAfterJavaFieldAnnotations: false +BreakConstructorInitializers: BeforeColon +BreakFunctionDefinitionParameters: false +BreakInheritanceList: BeforeColon BreakStringLiterals: true -ColumnLimit: 160 -CommentPragmas: '^ IWYU pragma:' +BreakTemplateDeclarations: Yes +ColumnLimit: 80 +CommentPragmas: "^ IWYU pragma:" CompactNamespaces: false -ConstructorInitializerAllOnOneLineOrOnePerLine: false ConstructorInitializerIndentWidth: 4 ContinuationIndentWidth: 4 Cpp11BracedListStyle: true -DeriveLineEnding: true DerivePointerAlignment: false -DisableFormat: false +DisableFormat: false +EmptyLineAfterAccessModifier: Never +EmptyLineBeforeAccessModifier: LogicalBlock ExperimentalAutoDetectBinPacking: false FixNamespaceComments: true ForEachMacros: - foreach - Q_FOREACH - BOOST_FOREACH -IncludeBlocks: Regroup +IfMacros: + - KJ_IF_MAYBE +IncludeBlocks: Preserve IncludeCategories: - - Regex: '^"(llvm|llvm-c|clang|clang-c)/' - Priority: 2 - SortPriority: 0 - - Regex: '^(<|"(gtest|gmock|isl|json)/)' - Priority: 3 - SortPriority: 0 - - Regex: '.*' - Priority: 1 - SortPriority: 0 -IncludeIsMainRegex: '(Test)?$' -IncludeIsMainSourceRegex: '' -IndentCaseLabels: false + - Regex: ^ + Priority: 2 + SortPriority: 0 + CaseSensitive: false + - Regex: ^<.*\.h> + Priority: 1 + SortPriority: 0 + CaseSensitive: false + - Regex: ^<.* + Priority: 2 + SortPriority: 0 + CaseSensitive: false + - Regex: .* + Priority: 3 + SortPriority: 0 + CaseSensitive: false +IncludeIsMainRegex: ([-_](test|unittest))?$ +IncludeIsMainSourceRegex: "" +IndentAccessModifiers: false +IndentCaseBlocks: false +IndentCaseLabels: true +IndentExternBlock: AfterExternBlock IndentGotoLabels: true IndentPPDirectives: None -IndentWidth: 2 +IndentRequiresClause: true +IndentWidth: 4 IndentWrappedFunctionNames: false +InsertBraces: false +InsertNewlineAtEOF: false +InsertTrailingCommas: None +IntegerLiteralSeparator: + Binary: 0 + BinaryMinDigits: 0 + Decimal: 0 + DecimalMinDigits: 0 + Hex: 0 + HexMinDigits: 0 JavaScriptQuotes: Leave JavaScriptWrapImports: true -KeepEmptyLinesAtTheStartOfBlocks: true -MacroBlockBegin: '' -MacroBlockEnd: '' +KeepEmptyLines: + AtEndOfFile: false + AtStartOfBlock: false + AtStartOfFile: true +LambdaBodyIndentation: Signature +Language: Cpp +LineEnding: DeriveLF +MacroBlockBegin: "" +MacroBlockEnd: "" +MainIncludeChar: Quote MaxEmptyLinesToKeep: 1 NamespaceIndentation: None -ObjCBinPackProtocolList: Auto +ObjCBinPackProtocolList: Never ObjCBlockIndentWidth: 2 +ObjCBreakBeforeNestedBlockParam: true ObjCSpaceAfterProperty: false ObjCSpaceBeforeProtocolList: true +PPIndentWidth: -1 +PackConstructorInitializers: NextLine PenaltyBreakAssignment: 2 -PenaltyBreakBeforeFirstCallParameter: 19 +PenaltyBreakBeforeFirstCallParameter: 1 PenaltyBreakComment: 300 PenaltyBreakFirstLessLess: 120 +PenaltyBreakOpenParenthesis: 0 +PenaltyBreakScopeResolution: 500 PenaltyBreakString: 1000 PenaltyBreakTemplateDeclaration: 10 PenaltyExcessCharacter: 1000000 -PenaltyReturnTypeOnItsOwnLine: 60 -PointerAlignment: Right -ReflowComments: true -SortIncludes: true -SortUsingDeclarations: true +PenaltyIndentedWhitespace: 0 +PenaltyReturnTypeOnItsOwnLine: 200 +PointerAlignment: Left +QualifierAlignment: Leave +RawStringFormats: + - Language: Cpp + Delimiters: + - cc + - CC + - cpp + - Cpp + - CPP + - c++ + - C++ + CanonicalDelimiter: "" + BasedOnStyle: google + - Language: TextProto + Delimiters: + - pb + - PB + - proto + - PROTO + EnclosingFunctions: + - EqualsProto + - EquivToProto + - PARSE_PARTIAL_TEXT_PROTO + - PARSE_TEST_PROTO + - PARSE_TEXT_PROTO + - ParseTextOrDie + - ParseTextProtoOrDie + - ParseTestProto + - ParsePartialTestProto + CanonicalDelimiter: pb + BasedOnStyle: google +ReferenceAlignment: Pointer +ReflowComments: true +RemoveBracesLLVM: false +RemoveParentheses: Leave +RemoveSemicolon: false +RequiresClausePosition: OwnLine +RequiresExpressionIndentation: OuterScope +SeparateDefinitionBlocks: Leave +ShortNamespaceLines: 1 +SkipMacroDefinitionBody: false +SortIncludes: CaseSensitive +SortJavaStaticImport: Before +SortUsingDeclarations: LexicographicNumeric SpaceAfterCStyleCast: false SpaceAfterLogicalNot: false SpaceAfterTemplateKeyword: true +SpaceAroundPointerQualifiers: Default SpaceBeforeAssignmentOperators: true +SpaceBeforeCaseColon: false SpaceBeforeCpp11BracedList: false SpaceBeforeCtorInitializerColon: true SpaceBeforeInheritanceColon: true +SpaceBeforeJsonColon: false SpaceBeforeParens: ControlStatements +SpaceBeforeParensOptions: + AfterControlStatements: true + AfterForeachMacros: true + AfterFunctionDeclarationName: false + AfterFunctionDefinitionName: false + AfterIfMacros: true + AfterOverloadedOperator: false + AfterPlacementOperator: true + AfterRequiresInClause: false + AfterRequiresInExpression: false + BeforeNonEmptyParentheses: false SpaceBeforeRangeBasedForLoopColon: true +SpaceBeforeSquareBrackets: false SpaceInEmptyBlock: false -SpaceInEmptyParentheses: false -SpacesBeforeTrailingComments: 1 -SpacesInAngles: false -SpacesInConditionalStatement: false +SpacesBeforeTrailingComments: 2 +SpacesInAngles: Never SpacesInContainerLiterals: true -SpacesInCStyleCastParentheses: false -SpacesInParentheses: false +SpacesInLineCommentPrefix: + Minimum: 1 + Maximum: -1 +SpacesInParens: Never +SpacesInParensOptions: + ExceptDoubleParentheses: false + InConditionalStatements: false + InCStyleCasts: false + InEmptyParentheses: false + Other: false SpacesInSquareBrackets: false -SpaceBeforeSquareBrackets: false -Standard: Latest +Standard: Auto +StatementAttributeLikeMacros: + - Q_EMIT StatementMacros: - Q_UNUSED - QT_REQUIRE_VERSION -TabWidth: 2 -UseCRLF: false -UseTab: Never -... +TabWidth: 8 +TableGenBreakInsideDAGArg: DontBreak +UseTab: Never +VerilogBreakBetweenInstancePorts: true +WhitespaceSensitiveMacros: + - BOOST_PP_STRINGIZE + - CF_SWIFT_NAME + - NS_SWIFT_NAME + - PP_STRINGIZE + - STRINGIZE diff --git a/.github/workflows/clang-formatter.yml b/.github/workflows/clang-formatter.yml deleted file mode 100644 index f8643aa6..00000000 --- a/.github/workflows/clang-formatter.yml +++ /dev/null @@ -1,30 +0,0 @@ -name: Run clang-format Linter - -on: - push: - branches: [ main ] - workflow_dispatch: - - # pull_request: - # types: [opened, reopened] - # branches: [ main ] -jobs: - build: - runs-on: ubuntu-latest - - steps: - - uses: actions/checkout@v4 - - uses: DoozyX/clang-format-lint-action@v0.17 - with: - source: '.' - exclude: './lib' - extensions: 'h,hpp,cpp,c' - clangFormatVersion: 17 - inplace: True - - uses: EndBug/add-and-commit@v9 - with: - author_name: Clang Robot - author_email: robot@example.com - message: 'Committing clang-format changes' - env: - GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} \ No newline at end of file diff --git a/.github/workflows/code-coverage.yml b/.github/workflows/code-coverage.yml new file mode 100644 index 00000000..bb2dbb8b --- /dev/null +++ b/.github/workflows/code-coverage.yml @@ -0,0 +1,15 @@ +name: Code Coverage +on: + push: + branches: + - main + pull_request: + branches: + - main +jobs: + call_reusable_workflow: + uses: vortexntnu/vortex-ci/.github/workflows/reusable-code-coverage.yml@main + with: + before_install_target_dependencies: './scripts/ci_install_dependencies.sh' + secrets: + CODECOV_TOKEN: ${{ secrets.CODECOV_TOKEN }} diff --git a/.github/workflows/industrial-ci.yml b/.github/workflows/industrial-ci.yml new file mode 100644 index 00000000..585ad1ff --- /dev/null +++ b/.github/workflows/industrial-ci.yml @@ -0,0 +1,13 @@ +name: Industrial CI + +on: + push: + workflow_dispatch: + schedule: + - cron: '0 1 * * *' # Runs daily to check for dependency issues or flaking tests +jobs: + call_reusable_workflow: + uses: vortexntnu/vortex-ci/.github/workflows/reusable-industrial-ci.yml@main + with: + before_install_target_dependencies: './scripts/ci_install_dependencies.sh' + ros_repo: '["testing", "main"]' diff --git a/.github/workflows/run-unit-tests.yml b/.github/workflows/run-unit-tests.yml deleted file mode 100644 index 4a4bb45d..00000000 --- a/.github/workflows/run-unit-tests.yml +++ /dev/null @@ -1,94 +0,0 @@ -# From https://github.com/marketplace/actions/ros-2-ci-action -name: Run Unit Tests - -on: - workflow_dispatch: - push: - pull_request: - -env: - PACKAGES: vortex_filtering - -jobs: - test_docker: # Iterates on all ROS 1 and ROS 2 distributions. - runs-on: ubuntu-latest - strategy: - fail-fast: false - matrix: - ros_distribution: - - humble - # - iron - - # Define the Docker image(s) associated with each ROS distribution. - # The include syntax allows additional variables to be defined, like - # docker_image in this case. See documentation: - # https://help.github.com/en/actions/reference/workflow-syntax-for-github-actions#example-including-configurations-in-a-matrix-build - # - # Platforms are defined in REP 3 and REP 2000: - # https://ros.org/reps/rep-0003.html - # https://ros.org/reps/rep-2000.html - include: - # Humble Hawksbill (May 2022 - May 2027) - - docker_image: ubuntu:jammy - ros_distribution: humble - ros_version: 2 - - # Iron Irwini (May 2023 - November 2024) - # - docker_image: ubuntu:jammy - # ros_distribution: iron - # ros_version: 2 - - # Rolling Ridley (No End-Of-Life) - # - docker_image: ubuntu:jammy - # ros_distribution: rolling - # ros_version: 2 - - container: - image: ${{ matrix.docker_image }} - steps: - - name: Checkout repository - uses: actions/checkout@v2 - - name: Setup ROS environment - uses: ros-tooling/setup-ros@v0.7 - with: - required-ros-distributions: ${{ matrix.ros_distribution }} - - name: Use gcc 13 and g++ 13 - run: | - sudo apt-get update - sudo apt-get install -y software-properties-common - sudo add-apt-repository -y ppa:ubuntu-toolchain-r/test - sudo apt-get update - sudo apt-get install -y gcc-13 g++-13 - sudo update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-13 100 --slave /usr/bin/g++ g++ /usr/bin/g++-13 - sudo update-alternatives --config gcc - - name: Build and test ROS 2 packages - uses: ros-tooling/action-ros-ci@v0.3 - id: action_ros_ci_step - with: - colcon-defaults: | - { - "build": { - "mixin": ["coverage-gcc", "coverage-pytest"] - }, - "test": { - "mixin": ["coverage-pytest"] - } - } - # If possible, pin the repository in the workflow to a specific commit to avoid - # changes in colcon-mixin-repository from breaking your tests. - colcon-mixin-repository: https://raw.githubusercontent.com/colcon/colcon-mixin-repository/1ddb69bedfd1f04c2f000e95452f7c24a4d6176b/index.yaml - - package-name: ${{ env.PACKAGES }} - target-ros2-distro: ${{ matrix.ros_distribution }} - - - uses: codecov/codecov-action@v4 - with: - token: ${{ secrets.CODECOV_TOKEN }} - files: ros_ws/lcov/total_coverage.info,ros_ws/coveragepy/.coverage - flags: unittests - name: codecov-umbrella - - uses: actions/upload-artifact@v4 - with: - name: Colcon-logs - path: ${{ steps.action_ros_ci_step.outputs.ros-workspace-directory-name }}/log - if: always() # upload the logs even when the build fails diff --git a/.github/workflows/semantic-release.yml b/.github/workflows/semantic-release.yml new file mode 100644 index 00000000..70779d49 --- /dev/null +++ b/.github/workflows/semantic-release.yml @@ -0,0 +1,9 @@ +name: Semantic Release + +on: + push: + branches: + - main +jobs: + call_reusable_workflow: + uses: vortexntnu/vortex-ci/.github/workflows/reusable-semantic-release.yml@main diff --git a/.gitignore b/.gitignore index 2abcb93d..8228c1f8 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,4 @@ .vscode build/* install/* -log/* \ No newline at end of file +log/* diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index e69de29b..e1850c7f 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -0,0 +1,85 @@ +# To use: +# +# pre-commit run -a +# +# Or: +# +# pre-commit install # (runs every time you commit in git) +# +# To update this file: +# +# pre-commit autoupdate +# +# See https://github.com/pre-commit/pre-commit + +repos: + # Standard hooks + - repo: https://github.com/pre-commit/pre-commit-hooks + rev: v5.0.0 + hooks: + - id: check-added-large-files + - id: check-ast + - id: check-case-conflict + - id: check-docstring-first + - id: check-merge-conflict + - id: check-symlinks + - id: check-xml + - id: check-yaml + args: ["--allow-multiple-documents"] + - id: debug-statements + - id: end-of-file-fixer + - id: mixed-line-ending + - id: trailing-whitespace + exclude_types: [rst] + - id: fix-byte-order-marker + - id: requirements-txt-fixer + # Python hooks + - repo: https://github.com/astral-sh/ruff-pre-commit + rev: v0.9.2 + hooks: + - id: ruff-format + - id: ruff + name: ruff-isort + args: [ + "--select=I", + "--fix" + ] + - id: ruff + name: ruff-pyupgrade + args: [ + "--select=UP", + "--fix" + ] + - id: ruff + name: ruff-pydocstyle + args: [ + "--select=D", + "--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D401", + "--fix", + ] + stages: [pre-commit] + pass_filenames: true + - id: ruff + name: ruff-check + args: [ + "--select=F,PT,B,C4,T20,S,N", + "--ignore=T201,N812,B006,S101,S311,S607,S603", + "--fix" + ] + stages: [pre-commit] + pass_filenames: true + # C++ hooks + - repo: https://github.com/pre-commit/mirrors-clang-format + rev: v19.1.7 + hooks: + - id: clang-format + args: [--style=file] + # Spellcheck in comments and docs + - repo: https://github.com/codespell-project/codespell + rev: v2.3.0 + hooks: + - id: codespell + args: ['--write-changes', '--ignore-words-list=theses,fom'] + +ci: + autoupdate_schedule: quarterly diff --git a/README.md b/README.md index 0af7be9e..8a6511eb 100644 --- a/README.md +++ b/README.md @@ -1,9 +1,11 @@ # Vortex VKF +[![Industrial CI](https://github.com/vortexntnu/vortex-vkf/actions/workflows/industrial-ci.yml/badge.svg)](https://github.com/vortexntnu/vortex-vkf/actions/workflows/industrial-ci.yml) +[![pre-commit.ci status](https://results.pre-commit.ci/badge/github/vortexntnu/vortex-vkf/main.svg)](https://results.pre-commit.ci/latest/github/vortexntnu/vortex-vkf/main) +[![codecov](https://codecov.io/github/vortexntnu/vortex-vkf/graph/badge.svg?token=93HEN6W2PM)](https://codecov.io/github/vortexntnu/vortex-vkf) + ## Introduction This is a C++ library for state estimation. VKF stands for Visual Kalman Filter which is a stupid and misleading name ## Packages - [vortex-filtering](vortex-filtering/README.md) - - diff --git a/codecov.yml b/codecov.yml index 8134ea7b..fb92ee7d 100644 --- a/codecov.yml +++ b/codecov.yml @@ -1,20 +1,19 @@ -fixes: - - "ros_ws/src/*/vortex-vkf/::" - -comment: - layout: "header, diff, flags, components" - coverage: + precision: 2 + round: down status: - project: off + project: + default: + informational: true + flags: + - unittests patch: off - -flag_management: - individual_flags: - - name: vortex-filtering - paths: - - vortex-filtering/include/vortex_filtering/ - statuses: - - type: project - target: auto - threshold: 1% +fixes: + - "ros_ws/src/vortex_vkf/::" +comment: + layout: "diff, flags, files" + behavior: default +flags: + unittests: + paths: + - "**" diff --git a/ruff.toml b/ruff.toml new file mode 100644 index 00000000..bf3554d6 --- /dev/null +++ b/ruff.toml @@ -0,0 +1,6 @@ +[lint.pydocstyle] +convention = "google" + +[format] +# Keep quotes as is +quote-style = "preserve" diff --git a/scripts/ci_install_dependencies.sh b/scripts/ci_install_dependencies.sh new file mode 100644 index 00000000..2314d0fc --- /dev/null +++ b/scripts/ci_install_dependencies.sh @@ -0,0 +1,19 @@ +#!/usr/bin/env bash +# Installs dependencies not handled by rosdep. + +set -e + +# Note: These are required by vortex-vkf +# Add Ubuntu Toolchain PPA to get gcc-13/g++-13 +sudo apt-get update -qq +sudo apt-get install -y --no-install-recommends software-properties-common +sudo add-apt-repository ppa:ubuntu-toolchain-r/test -y +sudo apt-get update -qq + +# Install and switch to GCC 13 +sudo apt-get install -y --no-install-recommends gcc-13 g++-13 lcov +sudo update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-13 100 +sudo update-alternatives --install /usr/bin/g++ g++ /usr/bin/g++-13 100 +sudo update-alternatives --install /usr/bin/gcov gcov /usr/bin/gcov-13 100 + +echo "Done installing additional dependencies." diff --git a/vortex-filtering/CMakeLists.txt b/vortex-filtering/CMakeLists.txt index e573768d..25e53ae8 100644 --- a/vortex-filtering/CMakeLists.txt +++ b/vortex-filtering/CMakeLists.txt @@ -11,7 +11,7 @@ if(NOT CMAKE_CXX_STANDARD) # === Compiler flags === if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options( - -Wall -Wextra -Wpedantic + -Wall -Wextra -Wpedantic -fopenmp # For parallel processing with Eigen -fconcepts-diagnostics-depth=3 # For better concepts error messages -Warray-bounds # For better array bounds error messages @@ -28,10 +28,10 @@ find_package(eigen3_cmake_module REQUIRED) find_package(OpenMP REQUIRED) # For parallel processing with Eigen find_package(Gnuplot REQUIRED) -find_package(Boost REQUIRED - COMPONENTS - iostreams - system +find_package(Boost REQUIRED + COMPONENTS + iostreams + system filesystem ) # for gnuplot diff --git a/vortex-filtering/README.md b/vortex-filtering/README.md index c1fa9304..436ece44 100644 --- a/vortex-filtering/README.md +++ b/vortex-filtering/README.md @@ -1,4 +1,5 @@ # vortex-filtering + ## Models Contains the models used in the filters. The models are implemented as classes that inherit from the `DynamicModelBase` class or `SensorModelBase` class. The models are implemented in the `models` namespace. [More info](include/vortex_filtering/models/README.md) @@ -53,7 +54,7 @@ classDiagram } class DynamicModelLTV { - +overide f_d() Vec_x + +override f_d() Vec_x +virtual A_d() Mat_xx +virtual Q_d() Mat_vv +vurtual G_d() Mat_xv @@ -74,4 +75,4 @@ classDiagram class CoordinatedTurn class ConstantAcceleration -``` \ No newline at end of file +``` diff --git a/vortex-filtering/include/vortex_filtering/filters/README.md b/vortex-filtering/include/vortex_filtering/filters/README.md index 4c21c3e4..e4417ec0 100644 --- a/vortex-filtering/include/vortex_filtering/filters/README.md +++ b/vortex-filtering/include/vortex_filtering/filters/README.md @@ -62,7 +62,7 @@ auto [x_est_upd, x_est_pred, z_est_pred] = EKF::step(dynamic_model, sensor_model The UKF can take any model derived from `vortex::models::DynamicModel` and `vortex::models::SensorModel`. All methods are static, so there is no need to create an instance of this class. #### Usage -The EKF and UKF share mostly the same interface and so it can be used for everything the EKF can. The main purpose of it is that it works with nonlinear models. +The EKF and UKF share mostly the same interface and so it can be used for everything the EKF can. The main purpose of it is that it works with nonlinear models. The UKF parameters $\alpha$, $\beta$ and $\kappa$ are set to 1.0, 2.0 and 0.0 by default. These can be changed by passing them as template arguments after the models. I don't know a reason for why you would want to change these, but the option is there anyways. @@ -77,7 +77,7 @@ This class represents an [Interacting Multiple Model Filter](https://github.com/ The IMM filter supports both linear and nonlinear models, using EKF for linear and UKF for nonlinear models derived from `vortex::models::DynamicModel` and `vortex::models::SensorModel`. #### Theory -The IMM filter is a filter that can switch between different models based on the probability of each model being the correct one. It does this by running multiple models in parallel and mixing the estimates from each model together. +The IMM filter is a filter that can switch between different models based on the probability of each model being the correct one. It does this by running multiple models in parallel and mixing the estimates from each model together. The workings of the IMM filter can be summarized in the following four steps: 1. Calculate Mixing Probabilities @@ -145,13 +145,13 @@ $$ \end{align*} $$ -then the IMM filter needs to know that the states $p_x$ and $p_y$ are comparable between all models, but the states $\theta$ and $a_x$ aren't. This is done by specifying the names of the states in the [IMM model](../models/README.md#imm-model). The similar states are mixed as normal as specified in Edmund Brekkes sensor fusion book, but the states that are not comparable are mixed using the method outlined in [this paper](https://www.researchgate.net/publication/289707032_Systematic_approach_to_IMM_mixing_for_unequal_dimension_states). +then the IMM filter needs to know that the states $p_x$ and $p_y$ are comparable between all models, but the states $\theta$ and $a_x$ aren't. This is done by specifying the names of the states in the [IMM model](../models/README.md#imm-model). The similar states are mixed as normal as specified in Edmund Brekkes sensor fusion book, but the states that are not comparable are mixed using the method outlined in [this paper](https://www.researchgate.net/publication/289707032_Systematic_approach_to_IMM_mixing_for_unequal_dimension_states). The method in the paper works as long as the minimum and maximum value a state can take is fed to the mixing function. Essentially a uniform distribution is created for the states that are not comparable and the mixing is done using the mean and variance of this distribution as state estimates for the missing states. For example when mixing the states of model A into model B, the states $v_x$ and $v_y$ are missing. The mixing function then creates a uniform distribution for these states from the minimum and maximum values of the $v_x$ and $v_y$ states. The mean and variance of this distribution is then used as the state estimates for the missing states before the mixing is done. If the min and max isn't provided however, the mixing function will copy the state estimates from the other model as is. -> This feature is the main reason the implementation is so much more complex than for the EKF and UKF. +> This feature is the main reason the implementation is so much more complex than for the EKF and UKF. ##### 3 - Mode Matching The mode matching step is where the Kalman filter is run for each model. This is done in the same way as for the EKF and UKF, but for each model separately. @@ -197,9 +197,9 @@ But for custom models, you will have to define the state names yourself. */ // Create the IMM model and sensor model -IMM imm_model(hold_times, switch_probs, - {CP(std_pos), cp_names}, - {CV(std_vel), cv_names}, +IMM imm_model(hold_times, switch_probs, + {CP(std_pos), cp_names}, + {CV(std_vel), cv_names}, {CT(std_vel, std_turn), ct_names}); using SensModT = vortex::models::IdentitySensorModel; @@ -226,7 +226,7 @@ Vec2d z_meas{48, 65}; // Estimate the next state using ImmFilter = vortex::filters::IMMFilter; -auto [weights_upd, x_est_upds, x_est_preds, z_est_preds] = +auto [weights_upd, x_est_upds, x_est_preds, z_est_preds] = IMM::step(imm_model, sensor_model, dt, x_est_prevs, z_meas, model_weights, states_min_max); ``` @@ -258,13 +258,13 @@ We want to measure the temperature inside a rocket drive. We know in theory, how // example how to define models using vortex::models using DynMod = vortex::models::ConstantVelocity; using SensorMod = vortex::models::IdentitySensorModel<4, 2>; -// example how to use PDAF in pratice +// example how to use PDAF in practice using PDAF = vortex::filter::PDAF; auto [x_final, inside, outside, x_pred, z_pred, x_updated] = PDAF::step(parameters...) ``` #### Previous State Estimate -The step function needs to know what the previous state was. Based on this state, the dynamic model will be used. The model will give us a predicted new state. This state will be compared to the actual measurements. The previous state must be a Gaussian distribution. +The step function needs to know what the previous state was. Based on this state, the dynamic model will be used. The model will give us a predicted new state. This state will be compared to the actual measurements. The previous state must be a Gaussian distribution. #### Measurements The perceived measurements. This parameter consists of an `Eigen::Vector`. It should hold all perceived measurements. (The dimension must be the same as defined in the models.) @@ -281,7 +281,7 @@ It is highly recommended to look into "Fundamentals of Sensor Fusion" (Chapter 7 In order for the test to work with visualization you have to uncomment following statement in the CMakeLists.txt of *vortex-filtering*. ``` if(BUILD_TESTING) - # add_compile_definitions(GNUPLOT_ENABLE=1) <- uncomment this + # add_compile_definitions(GNUPLOT_ENABLE=1) <- uncomment this add_subdirectory(test) endif() ``` @@ -298,7 +298,7 @@ The step()-function will return return the new estimated state and all outputs o ### IPDA -IPDA stand for **I**ntegrated **P**robabilistic **D**ata **A**ssociation. It uses extra steps in addition to the regular PDAF. The PDAF is extended to also include a probability value which describes the target's existence. The main reason for this is to evaluate not only the state of the target but also the probability of existence dependent on the previous states. +IPDA stand for **I**integrated **P**robabilistic **D**ata **A**ssociation. It uses extra steps in addition to the regular PDAF. The PDAF is extended to also include a probability value which describes the target's existence. The main reason for this is to evaluate not only the state of the target but also the probability of existence dependent on the previous states. IPDA works in terms of usage the same way the PDAF works. The class provides a static function (also called *step*). You can use this function by referencing the namespace. ```c++ @@ -315,4 +315,4 @@ The parameters of the step() function are mainly the same as those of the PDAF. * Probability of survival: This is a general hyperparameter that defines how likely a target is to survive. This parameter shouldn't be confused with the probability of detection. If no measurement is considered to correspond to the target, we consider the dynamic model. The track still *survives*. If a target *dies*, it can't come back, and the track will be deleted. #### Returns -In addition to all the return parameters of the PDAF the IPDA will return the estimated existence prediction for the current state (corresponding to x_final). \ No newline at end of file +In addition to all the return parameters of the PDAF the IPDA will return the estimated existence prediction for the current state (corresponding to x_final). diff --git a/vortex-filtering/include/vortex_filtering/filters/ekf.hpp b/vortex-filtering/include/vortex_filtering/filters/ekf.hpp index 13d34d3c..d4e9563c 100644 --- a/vortex-filtering/include/vortex_filtering/filters/ekf.hpp +++ b/vortex-filtering/include/vortex_filtering/filters/ekf.hpp @@ -27,95 +27,144 @@ namespace vortex::filter { * @tparam n_dim_v Process noise dimension * @tparam n_dim_w Measurement noise dimension */ -template class EKF_t { -public: - static constexpr int N_DIM_x = (int)n_dim_x; - static constexpr int N_DIM_z = (int)n_dim_z; - static constexpr int N_DIM_u = (int)n_dim_u; - static constexpr int N_DIM_v = (int)n_dim_v; - static constexpr int N_DIM_w = (int)n_dim_w; +template +class EKF_t { + public: + static constexpr int N_DIM_x = (int)n_dim_x; + static constexpr int N_DIM_z = (int)n_dim_z; + static constexpr int N_DIM_u = (int)n_dim_u; + static constexpr int N_DIM_v = (int)n_dim_v; + static constexpr int N_DIM_w = (int)n_dim_w; - using T = Types_xzuvw; + using T = Types_xzuvw; - EKF_t() = delete; + EKF_t() = delete; - /** Perform one EKF prediction step - * @param dyn_mod Dynamic model - * @param sens_mod Sensor model - * @param dt Time step - * @param x_est_prev Gauss_x Previous state estimate - * @param u Vec_x Input. Not used, set to zero. - * @return std::pair Predicted state, predicted measurement - * @throws std::runtime_error if dyn_mod or sens_mod are not of the DynamicModelT or SensorModelT type - */ - static auto predict(const auto &dyn_mod, const auto &sens_mod, double dt, const auto &x_est_prev, const T::Vec_u &u = T::Vec_u::Zero()) - -> std::pair, typename T::Gauss_z> - requires(concepts::DynamicModelLTV && - concepts::SensorModelLTV && concepts::MultiVarGaussLike) - { - using StateT = std::remove_reference_t; - StateT x_est_pred = StateT{dyn_mod.pred_from_est(dt, x_est_prev, u)}; - typename T::Gauss_z z_est_pred = sens_mod.pred_from_est(x_est_pred); - return {x_est_pred, z_est_pred}; - } + /** Perform one EKF prediction step + * @param dyn_mod Dynamic model + * @param sens_mod Sensor model + * @param dt Time step + * @param x_est_prev Gauss_x Previous state estimate + * @param u Vec_x Input. Not used, set to zero. + * @return std::pair Predicted state, predicted + * measurement + * @throws std::runtime_error if dyn_mod or sens_mod are not of the + * DynamicModelT or SensorModelT type + */ + static auto predict(const auto& dyn_mod, + const auto& sens_mod, + double dt, + const auto& x_est_prev, + const T::Vec_u& u = T::Vec_u::Zero()) + -> std::pair, + typename T::Gauss_z> + requires( + concepts:: + DynamicModelLTV && + concepts:: + SensorModelLTV && + concepts::MultiVarGaussLike) + { + using StateT = std::remove_reference_t; + StateT x_est_pred = StateT{dyn_mod.pred_from_est(dt, x_est_prev, u)}; + typename T::Gauss_z z_est_pred = sens_mod.pred_from_est(x_est_pred); + return {x_est_pred, z_est_pred}; + } - /** Perform one EKF update step - * @param sens_mod Sensor model - * @param x_est_pred Gauss_x Predicted state - * @param z_est_pred Gauss_z Predicted measurement - * @param z_meas Vec_z Measurement - * @return MultivarGauss Updated state - * @throws std::runtime_error ifsens_mod is not of the SensorModelT type - */ - static auto update(const auto &sens_mod, const auto &x_est_pred, const auto &z_est_pred, const T::Vec_z &z_meas) - -> std::remove_reference_t - requires(concepts::SensorModelLTV && concepts::MultiVarGaussLike && - concepts::MultiVarGaussLike) - { - typename T::Mat_zx C = sens_mod.C(x_est_pred.mean()); // Measurement matrix - typename T::Mat_ww R = sens_mod.R(x_est_pred.mean()); // Measurement noise covariance - typename T::Mat_zw H = sens_mod.H(x_est_pred.mean()); // Measurement noise cross-covariance - typename T::Mat_xx P = x_est_pred.cov(); // State covariance - typename T::Mat_zz S_inv = z_est_pred.cov_inv(); // Inverse of the predicted measurement covariance - typename T::Mat_xx I = T::Mat_xx::Identity(N_DIM_x, N_DIM_x); + /** Perform one EKF update step + * @param sens_mod Sensor model + * @param x_est_pred Gauss_x Predicted state + * @param z_est_pred Gauss_z Predicted measurement + * @param z_meas Vec_z Measurement + * @return MultivarGauss Updated state + * @throws std::runtime_error ifsens_mod is not of the SensorModelT type + */ + static auto update(const auto& sens_mod, + const auto& x_est_pred, + const auto& z_est_pred, + const T::Vec_z& z_meas) + -> std::remove_reference_t + requires( + concepts:: + SensorModelLTV && + concepts::MultiVarGaussLike && + concepts::MultiVarGaussLike) + { + typename T::Mat_zx C = + sens_mod.C(x_est_pred.mean()); // Measurement matrix + typename T::Mat_ww R = + sens_mod.R(x_est_pred.mean()); // Measurement noise covariance + typename T::Mat_zw H = sens_mod.H( + x_est_pred.mean()); // Measurement noise cross-covariance + typename T::Mat_xx P = x_est_pred.cov(); // State covariance + typename T::Mat_zz S_inv = + z_est_pred + .cov_inv(); // Inverse of the predicted measurement covariance + typename T::Mat_xx I = T::Mat_xx::Identity(N_DIM_x, N_DIM_x); - typename T::Mat_xz W = P * C.transpose() * S_inv; // Kalman gain - typename T::Vec_z innovation = z_meas - z_est_pred.mean(); + typename T::Mat_xz W = P * C.transpose() * S_inv; // Kalman gain + typename T::Vec_z innovation = z_meas - z_est_pred.mean(); - typename T::Vec_x state_upd_mean = x_est_pred.mean() + W * innovation; - // Use the Joseph form of the covariance update to ensure positive definiteness - typename T::Mat_xx state_upd_cov = (I - W * C) * P * (I - W * C).transpose() + W * H * R * H.transpose() * W.transpose(); + typename T::Vec_x state_upd_mean = x_est_pred.mean() + W * innovation; + // Use the Joseph form of the covariance update to ensure positive + // definiteness + typename T::Mat_xx state_upd_cov = + (I - W * C) * P * (I - W * C).transpose() + + W * H * R * H.transpose() * W.transpose(); - return {state_upd_mean, state_upd_cov}; - } + return {state_upd_mean, state_upd_cov}; + } - /** Perform one EKF prediction and update step - * @param dyn_mod Dynamic model - * @param sens_mod Sensor model - * @param dt Time step - * @param x_est_prev Gauss_x Previous state estimate - * @param z_meas Vec_z Measurement - * @param u Vec_x Input - * @return Updated state, predicted state, predicted measurement - */ - static auto step(const auto &dyn_mod, const auto &sens_mod, double dt, const auto &x_est_prev, const T::Vec_z &z_meas, const T::Vec_u &u = T::Vec_u::Zero()) - -> std::tuple, std::remove_reference_t, typename T::Gauss_z> - requires(concepts::DynamicModelLTV && - concepts::SensorModelLTV && concepts::MultiVarGaussLike) - { - auto [x_est_pred, z_est_pred] = predict(dyn_mod, sens_mod, dt, x_est_prev, u); + /** Perform one EKF prediction and update step + * @param dyn_mod Dynamic model + * @param sens_mod Sensor model + * @param dt Time step + * @param x_est_prev Gauss_x Previous state estimate + * @param z_meas Vec_z Measurement + * @param u Vec_x Input + * @return Updated state, predicted state, predicted measurement + */ + static auto step(const auto& dyn_mod, + const auto& sens_mod, + double dt, + const auto& x_est_prev, + const T::Vec_z& z_meas, + const T::Vec_u& u = T::Vec_u::Zero()) + -> std::tuple, + std::remove_reference_t, + typename T::Gauss_z> + requires( + concepts:: + DynamicModelLTV && + concepts:: + SensorModelLTV && + concepts::MultiVarGaussLike) + { + auto [x_est_pred, z_est_pred] = + predict(dyn_mod, sens_mod, dt, x_est_prev, u); - auto x_est_upd = update(sens_mod, x_est_pred, z_est_pred, z_meas); - return {x_est_upd, x_est_pred, z_est_pred}; - } + auto x_est_upd = update(sens_mod, x_est_pred, z_est_pred, z_meas); + return {x_est_upd, x_est_pred, z_est_pred}; + } }; /** * @brief Extended Kalman Filter. - * @tparam DynModT Dynamic model type derived from `vortex::models::interface::DynamicModelLTV` - * @tparam SensModT Sensor model type derived from `vortex::models::interface::SensorModelLTV` + * @tparam DynModT Dynamic model type derived from + * `vortex::models::interface::DynamicModelLTV` + * @tparam SensModT Sensor model type derived from + * `vortex::models::interface::SensorModelLTV` */ -template -using EKF = EKF_t; +template +using EKF = EKF_t; -} // namespace vortex::filter \ No newline at end of file +} // namespace vortex::filter diff --git a/vortex-filtering/include/vortex_filtering/filters/imm_filter.hpp b/vortex-filtering/include/vortex_filtering/filters/imm_filter.hpp index 9dfbd2a3..9d4c8517 100644 --- a/vortex-filtering/include/vortex_filtering/filters/imm_filter.hpp +++ b/vortex-filtering/include/vortex_filtering/filters/imm_filter.hpp @@ -27,332 +27,441 @@ namespace vortex::filter { -template class ImmFilter { -public: - static constexpr size_t N_MODELS = ImmModelT::N_MODELS; - - static constexpr auto N_DIMS_x = ImmModelT::N_DIMS_x; - static constexpr auto N_DIMS_u = ImmModelT::N_DIMS_u; - static constexpr auto N_DIMS_v = ImmModelT::N_DIMS_v; - static constexpr int N_DIM_z = SensModT::N_DIM_z; - static constexpr int N_DIM_w = SensModT::N_DIM_w; - - template using T = Types_xz; - - template using DynModT = typename ImmModelT::template DynModT; - - using Vec_n = Eigen::Vector; - using Arr_nb = Eigen::Array; - using Mat_nn = Eigen::Matrix; - using Vec_z = typename T<0>::Vec_z; - using Gauss_z = typename T<0>::Gauss_z; - using GaussTuple_x = typename ImmModelT::GaussTuple_x; - using GaussArr_z = std::array; - - /// No need to instantiate this class. All methods are static. - ImmFilter() = delete; - - /** - * Calculates the mixing probabilities for the IMM filter, following step 1 in (6.4.1) in the book. - * @param transition_matrix The discrete time transition matrix for the Markov chain. (pi_mat_d from the imm model) - * @param model_weights The weights (mode probabilities) from the previous time step. - * @return The mixing probabilities. Each element is mixing_probs[s_{k-1}, s_k] = mu_{s_{k-1}|s_k} where s is the index of the model. - */ - static Mat_nn calculate_mixing_probs(const Mat_nn &transition_matrix, const Vec_n &model_weights) - { - - // mu_{s_{k-1}|s_k} = pi_{s_{k-1}|s_k} * mu_{s_{k-1}|k-1} - Mat_nn mixing_probs = transition_matrix.cwiseProduct(model_weights.replicate(1, N_MODELS)); - - // Normalize - for (int s_k = 0; s_k < mixing_probs.cols(); s_k++) { - mixing_probs.col(s_k) /= mixing_probs.col(s_k).sum(); - } - return mixing_probs; - } - - /** - * @brief Calculate moment-based approximation, following step 2 in (6.4.1) in the book - * @param x_est_prev Gaussians from previous time step - * @param mixing_probs Mixing probabilities - * @param state_names The names of the states - * @param states_min_max The minimum and maximum value each state can take (optional, but can lead to better performance) - * @return tuple of moment-based predictions, i.e. update each model based on the state of all of the other models. - * @note - If the sizes are different, the mixing model is modified to fit the size of the target model and the possibly missing states - * are initialized with the mean and covariance from the target model or with a uniform distribution if `states_min_max` is provided. - */ - static GaussTuple_x mixing(const GaussTuple_x &x_est_prevs, const Mat_nn &mixing_probs, const StateMap &states_min_max = {}) - { - return [&](std::index_sequence) -> GaussTuple_x { - return {mix_one_component(x_est_prevs, mixing_probs.col(s_k), states_min_max)...}; - }(std::make_index_sequence{}); - - } - - /** - * @brief Calculate the Kalman filter outputs for each mode (6.36), following step 3 in (6.4.1) in the book - * @param imm_model The IMM model. - * @param sensor_model The sensor model. - * @param dt double Time step - * @param moment_based_preds Moment-based predictions - * @param z_meas Vec_z Measurement - * @return Tuple of updated states, predicted states, predicted measurements - */ - static std::tuple mode_matched_filter(const ImmModelT &imm_model, const SensModT &sensor_model, double dt, - const GaussTuple_x &moment_based_preds, const Vec_z &z_meas) - { - GaussTuple_x x_est_upds; - GaussTuple_x x_est_preds; - GaussArr_z z_est_preds; - - [&](std::index_sequence) { - ((std::tie(std::get(x_est_upds), std::get(x_est_preds), z_est_preds.at(s_k)) = - kalman_filter_step(imm_model.template get_model(), sensor_model, dt, std::get(moment_based_preds), z_meas)), - ...); - }(std::make_index_sequence{}); - - return {x_est_upds, x_est_preds, z_est_preds}; - } - - /** - * @brief Update the mode probabilites based on how well the predictions matched the measurements. - * Using (6.37) from step 3 and (6.38) from step 4 in (6.4.1) in the book - * @param transition_matrix The discrete time transition matrix for the Markov chain. (pi_mat_d from the imm model) - * @param z_preds Mode-match filter outputs - * @param z_meas Vec_z Measurement - * @param prev_weigths Vec_n Weights - * @return `Vec_n` Updated weights - */ - static Vec_n update_probabilities(const Mat_nn &transition_matrix, const GaussArr_z &z_preds, const Vec_z &z_meas, const Vec_n &prev_weights) - { - Vec_n weights_pred = transition_matrix.transpose() * prev_weights; - - Vec_n z_probs; - for (size_t s_k = 0; const Gauss_z &z_pred : z_preds) { - z_probs(s_k++) = z_pred.pdf(z_meas); +template +class ImmFilter { + public: + static constexpr size_t N_MODELS = ImmModelT::N_MODELS; + + static constexpr auto N_DIMS_x = ImmModelT::N_DIMS_x; + static constexpr auto N_DIMS_u = ImmModelT::N_DIMS_u; + static constexpr auto N_DIMS_v = ImmModelT::N_DIMS_v; + static constexpr int N_DIM_z = SensModT::N_DIM_z; + static constexpr int N_DIM_w = SensModT::N_DIM_w; + + template + using T = Types_xz; + + template + using DynModT = typename ImmModelT::template DynModT; + + using Vec_n = Eigen::Vector; + using Arr_nb = Eigen::Array; + using Mat_nn = Eigen::Matrix; + using Vec_z = typename T<0>::Vec_z; + using Gauss_z = typename T<0>::Gauss_z; + using GaussTuple_x = typename ImmModelT::GaussTuple_x; + using GaussArr_z = std::array; + + /// No need to instantiate this class. All methods are static. + ImmFilter() = delete; + + /** + * Calculates the mixing probabilities for the IMM filter, following step 1 + * in (6.4.1) in the book. + * @param transition_matrix The discrete time transition matrix for the + * Markov chain. (pi_mat_d from the imm model) + * @param model_weights The weights (mode probabilities) from the previous + * time step. + * @return The mixing probabilities. Each element is mixing_probs[s_{k-1}, + * s_k] = mu_{s_{k-1}|s_k} where s is the index of the model. + */ + static Mat_nn calculate_mixing_probs(const Mat_nn& transition_matrix, + const Vec_n& model_weights) { + // mu_{s_{k-1}|s_k} = pi_{s_{k-1}|s_k} * mu_{s_{k-1}|k-1} + Mat_nn mixing_probs = transition_matrix.cwiseProduct( + model_weights.replicate(1, N_MODELS)); + + // Normalize + for (int s_k = 0; s_k < mixing_probs.cols(); s_k++) { + mixing_probs.col(s_k) /= mixing_probs.col(s_k).sum(); + } + return mixing_probs; } - Vec_n weights_upd = z_probs.cwiseProduct(weights_pred); - weights_upd /= weights_upd.sum(); - - return weights_upd; - } - - /** - * @brief Perform one IMM filter step - * @param imm_model The IMM model. - * @param sensor_model The sensor model. - * @param dt double Time step - * @param x_est_prevs Gaussians from previous time step - * @param weights Vec_n Weights - * @param z_meas Vec_z Measurement - * @param states_min_max The minimum and maximum value each state can take (optional, but can lead to better performance) - */ - static std::tuple step(const ImmModelT &imm_model, const SensModT &sensor_model, double dt, - const GaussTuple_x &x_est_prevs, const Vec_z &z_meas, const Vec_n &weights, - const StateMap &states_min_max = {}) - { - Mat_nn transition_matrix = imm_model.get_pi_mat_d(dt); - - Mat_nn mixing_probs = calculate_mixing_probs(transition_matrix, weights); - GaussTuple_x moment_based_preds = mixing(x_est_prevs, mixing_probs, states_min_max); - auto [x_est_upds, x_est_preds, z_est_preds] = mode_matched_filter(imm_model, sensor_model, dt, moment_based_preds, z_meas); - Vec_n weights_upd = update_probabilities(mixing_probs, z_est_preds, z_meas, weights); - - return {weights_upd, x_est_upds, x_est_preds, z_est_preds}; - } - - static std::tuple kalman_filter_predictions(const ImmModelT &imm_model, const SensModT &sensor_model, double dt, - const GaussTuple_x &x_est_prevs) - { - GaussTuple_x x_est_preds; - GaussArr_z z_est_preds; - - [&](std::index_sequence) { - ((std::tie(std::get(x_est_preds), z_est_preds.at(s_k)) = - kalman_filter_predict(imm_model.template get_model(), sensor_model, dt, std::get(x_est_prevs))), - ...); - }(std::make_index_sequence{}); - - return {x_est_preds, z_est_preds}; - } - - static GaussTuple_x kalman_filter_updates(const ImmModelT &imm_model, const SensModT &sensor_model, double dt, const GaussTuple_x &x_est_preds, - const GaussArr_z &z_est_preds, const Vec_z &z_meas, const Arr_nb &gated_measurements = Arr_nb::Ones()) - { - GaussTuple_x x_est_upds; - - [&](std::index_sequence) { - ((std::get(x_est_upds) = - gated_measurements(s_k) - ? kalman_filter_update(imm_model.template get_model(), sensor_model, dt, std::get(x_est_preds), z_est_preds.at(s_k), z_meas) - : std::get(x_est_preds)), - ...); - }(std::make_index_sequence{}); - - return x_est_upds; - } - - /** - * @brief Calculate the Kalman filter outputs for one mode. If the model isn't LTV, use the ukf instead of the ekf. - * @tparam s_k Index of model - * @param imm_model The IMM model. - * @param sensor_model The sensor model. - * @param dt double Time step - * @param x_est_prev Moment-based prediction - * @param z_meas Vec_z Measurement - * @return Tuple of updated state, predicted state, predicted measurement - */ - template - static std::tuple::Gauss_x, typename T::Gauss_x, Gauss_z> - kalman_filter_step(const DynModT &dyn_model, const SensModT &sensor_model, double dt, const T::Gauss_x &x_est_prev, const Vec_z &z_meas) - { - if constexpr (concepts::DynamicModelLTVWithDefinedSizes> && concepts::SensorModelLTVWithDefinedSizes) { - using ImmSensMod = models::ImmSensorModelLTV; - using EKF = filter::EKF_t; - ImmSensMod imm_sens_mod{sensor_model}; - return EKF::step(dyn_model, imm_sens_mod, dt, x_est_prev, z_meas); - } - else { - using ImmSensMod = models::ImmSensorModel; - using UKF = filter::UKF_t; - ImmSensMod imm_sens_mod{sensor_model}; - return UKF::step(dyn_model, imm_sens_mod, dt, x_est_prev, z_meas); - } - } - - template - static std::tuple::Gauss_x, Gauss_z> kalman_filter_predict(const DynModT &dyn_model, const SensModT &sensor_model, double dt, - const T::Gauss_x &x_est_prev) - { - if constexpr (concepts::DynamicModelLTVWithDefinedSizes> && concepts::SensorModelLTVWithDefinedSizes) { - using ImmSensMod = models::ImmSensorModelLTV; - using EKF = filter::EKF_t; - ImmSensMod imm_sens_mod{sensor_model}; - return EKF::predict(dyn_model, imm_sens_mod, dt, x_est_prev); - } - else { - using ImmSensMod = models::ImmSensorModel; - using UKF = filter::UKF_t; - ImmSensMod imm_sens_mod{sensor_model}; - return UKF::predict(dyn_model, imm_sens_mod, dt, x_est_prev); - } - } - - template - static T::Gauss_x kalman_filter_update(const DynModT &dyn_model, const SensModT &sensor_model, double dt, const T::Gauss_x &x_est_pred, - const T::Gauss_z &z_est_pred, const Vec_z &z_meas) - { - if constexpr (concepts::DynamicModelLTVWithDefinedSizes> && concepts::SensorModelLTVWithDefinedSizes) { - using ImmSensMod = models::ImmSensorModelLTV; - using EKF = filter::EKF_t; - ImmSensMod imm_sens_mod{sensor_model}; - return EKF::update(imm_sens_mod, x_est_pred, z_est_pred, z_meas); - } - else { - using ImmSensMod = models::ImmSensorModel; - using UKF = filter::UKF_t; - ImmSensMod imm_sens_mod{sensor_model}; - return UKF::update(dyn_model, imm_sens_mod, dt, x_est_pred, z_est_pred, z_meas); - } - } - - /** Helper function to mix one component (mode) of the IMM filter. It mixes all the components with the target model. - * @tparam target_model_index The model to mix the other models into - * @param x_est_prevs Gaussians from previous time step - * @param weights Weights (column of the mixing_probs matrix corresponding to the target model index) - * @param states_min_max The minimum and maximum value each state can take (optional, but can lead to better performance) - * @return Gauss_x The updated model after mixing it with the other models - * @note This is the function that actually does the mixing of the models. It is called for each model in the IMM filter. - */ - template - static T::Gauss_x mix_one_component(const GaussTuple_x &x_est_prevs, const Vec_n &weights, const StateMap &states_min_max = {}) - { - auto moment_based_preds = - [&](std::index_sequence) -> std::array::Gauss_x, N_MODELS> { - return {prepare_mixing_model(x_est_prevs, states_min_max)...}; - }(std::make_index_sequence{}); - - return typename T::GaussMix_x{weights, moment_based_preds}.reduce(); - } - - /** - * @brief Fit the size and states of the mixing_model in case it doesn't have the same dimensions or states as the target model. - * A map of the minimum and maximum values for each state (`states_min_max`) can be provided to initialize the missing states. - * If the `states_min_max` map is not provided, the missing states are initialized with the mean and covariance from the target model. - * @tparam target_model_index The model to mix the other models into - * @tparam mixing_model_index The model to mix into the target model - * @param x_est_prevs Gaussians from previous time step - * @param states_min_max The minimum and maximum value each state can take (optional, but can lead to better filter performance) - * @return Gauss_x Modified `mixing_model` to fit the size and types of `target_model` - * @note - If the sizes and state names of the mixing model and the target model are the same, the mixing model is returned as is. - * @note - If the sizes are different, the mixing model is modified to fit the size of the target model and the possibly missing states - * are initialized with the mean and covariance from the target model or with a uniform distribution if `states_min_max` is provided. - */ - template - static T::Gauss_x prepare_mixing_model(const GaussTuple_x &x_est_prevs, const StateMap &states_min_max = {}) - { - if constexpr (target_model_index == mixing_model_index) { - return std::get(x_est_prevs); + /** + * @brief Calculate moment-based approximation, following step 2 in (6.4.1) + * in the book + * @param x_est_prev Gaussians from previous time step + * @param mixing_probs Mixing probabilities + * @param state_names The names of the states + * @param states_min_max The minimum and maximum value each state can take + * (optional, but can lead to better performance) + * @return tuple of moment-based predictions, i.e. update each model based + * on the state of all of the other models. + * @note - If the sizes are different, the mixing model is modified to fit + * the size of the target model and the possibly missing states are + * initialized with the mean and covariance from the target model or with a + * uniform distribution if `states_min_max` is provided. + */ + static GaussTuple_x mixing(const GaussTuple_x& x_est_prevs, + const Mat_nn& mixing_probs, + const StateMap& states_min_max = {}) { + return [&](std::index_sequence) -> GaussTuple_x { + return {mix_one_component(x_est_prevs, mixing_probs.col(s_k), + states_min_max)...}; + }(std::make_index_sequence{}); } - constexpr size_t N_DIM_target = ImmModelT::N_DIM_x(target_model_index); - constexpr size_t N_DIM_mixing = ImmModelT::N_DIM_x(mixing_model_index); - constexpr size_t N_DIM_min = std::min(N_DIM_target, N_DIM_mixing); + /** + * @brief Calculate the Kalman filter outputs for each mode (6.36), + * following step 3 in (6.4.1) in the book + * @param imm_model The IMM model. + * @param sensor_model The sensor model. + * @param dt double Time step + * @param moment_based_preds Moment-based predictions + * @param z_meas Vec_z Measurement + * @return Tuple of updated states, predicted states, predicted measurements + */ + static std::tuple + mode_matched_filter(const ImmModelT& imm_model, + const SensModT& sensor_model, + double dt, + const GaussTuple_x& moment_based_preds, + const Vec_z& z_meas) { + GaussTuple_x x_est_upds; + GaussTuple_x x_est_preds; + GaussArr_z z_est_preds; + + [&](std::index_sequence) { + ((std::tie(std::get(x_est_upds), std::get(x_est_preds), + z_est_preds.at(s_k)) = + kalman_filter_step( + imm_model.template get_model(), sensor_model, dt, + std::get(moment_based_preds), z_meas)), + ...); + }(std::make_index_sequence{}); + + return {x_est_upds, x_est_preds, z_est_preds}; + } - using Vec_x = Eigen::Vector; - using Mat_xx = Eigen::Matrix; - using Uniform = prob::Uniform<1>; - using Vec_x_b = Eigen::Vector; - using Mat_xx_b = Eigen::Matrix; + /** + * @brief Update the mode probabilities based on how well the predictions + * matched the measurements. Using (6.37) from step 3 and (6.38) from step 4 + * in (6.4.1) in the book + * @param transition_matrix The discrete time transition matrix for the + * Markov chain. (pi_mat_d from the imm model) + * @param z_preds Mode-match filter outputs + * @param z_meas Vec_z Measurement + * @param prev_weigths Vec_n Weights + * @return `Vec_n` Updated weights + */ + static Vec_n update_probabilities(const Mat_nn& transition_matrix, + const GaussArr_z& z_preds, + const Vec_z& z_meas, + const Vec_n& prev_weights) { + Vec_n weights_pred = transition_matrix.transpose() * prev_weights; + + Vec_n z_probs; + for (size_t s_k = 0; const Gauss_z& z_pred : z_preds) { + z_probs(s_k++) = z_pred.pdf(z_meas); + } + + Vec_n weights_upd = z_probs.cwiseProduct(weights_pred); + weights_upd /= weights_upd.sum(); + + return weights_upd; + } - auto target_state = std::get(x_est_prevs); - auto mixing_state = std::get(x_est_prevs); + /** + * @brief Perform one IMM filter step + * @param imm_model The IMM model. + * @param sensor_model The sensor model. + * @param dt double Time step + * @param x_est_prevs Gaussians from previous time step + * @param weights Vec_n Weights + * @param z_meas Vec_z Measurement + * @param states_min_max The minimum and maximum value each state can take + * (optional, but can lead to better performance) + */ + static std::tuple step( + const ImmModelT& imm_model, + const SensModT& sensor_model, + double dt, + const GaussTuple_x& x_est_prevs, + const Vec_z& z_meas, + const Vec_n& weights, + const StateMap& states_min_max = {}) { + Mat_nn transition_matrix = imm_model.get_pi_mat_d(dt); + + Mat_nn mixing_probs = + calculate_mixing_probs(transition_matrix, weights); + GaussTuple_x moment_based_preds = + mixing(x_est_prevs, mixing_probs, states_min_max); + auto [x_est_upds, x_est_preds, z_est_preds] = mode_matched_filter( + imm_model, sensor_model, dt, moment_based_preds, z_meas); + Vec_n weights_upd = + update_probabilities(mixing_probs, z_est_preds, z_meas, weights); + + return {weights_upd, x_est_upds, x_est_preds, z_est_preds}; + } - Vec_x x = Vec_x::Zero(); - Mat_xx P = Mat_xx::Zero(); + static std::tuple kalman_filter_predictions( + const ImmModelT& imm_model, + const SensModT& sensor_model, + double dt, + const GaussTuple_x& x_est_prevs) { + GaussTuple_x x_est_preds; + GaussArr_z z_est_preds; + + [&](std::index_sequence) { + ((std::tie(std::get(x_est_preds), z_est_preds.at(s_k)) = + kalman_filter_predict( + imm_model.template get_model(), sensor_model, dt, + std::get(x_est_prevs))), + ...); + }(std::make_index_sequence{}); + + return {x_est_preds, z_est_preds}; + } - x.template head() = mixing_state.mean().template head(); - P.template topLeftCorner() = mixing_state.cov().template topLeftCorner(); + static GaussTuple_x kalman_filter_updates( + const ImmModelT& imm_model, + const SensModT& sensor_model, + double dt, + const GaussTuple_x& x_est_preds, + const GaussArr_z& z_est_preds, + const Vec_z& z_meas, + const Arr_nb& gated_measurements = Arr_nb::Ones()) { + GaussTuple_x x_est_upds; + + [&](std::index_sequence) { + ((std::get(x_est_upds) = + gated_measurements(s_k) + ? kalman_filter_update( + imm_model.template get_model(), sensor_model, + dt, std::get(x_est_preds), z_est_preds.at(s_k), + z_meas) + : std::get(x_est_preds)), + ...); + }(std::make_index_sequence{}); + + return x_est_upds; + } - constexpr auto target_state_names = std::get(ImmModelT::ALL_STATE_NAMES); - constexpr auto mixing_state_names = std::get(ImmModelT::ALL_STATE_NAMES); - constexpr auto matching_states = models::matching_state_names(target_state_names, mixing_state_names); + /** + * @brief Calculate the Kalman filter outputs for one mode. If the model + * isn't LTV, use the ukf instead of the ekf. + * @tparam s_k Index of model + * @param imm_model The IMM model. + * @param sensor_model The sensor model. + * @param dt double Time step + * @param x_est_prev Moment-based prediction + * @param z_meas Vec_z Measurement + * @return Tuple of updated state, predicted state, predicted measurement + */ + template + static std:: + tuple::Gauss_x, typename T::Gauss_x, Gauss_z> + kalman_filter_step(const DynModT& dyn_model, + const SensModT& sensor_model, + double dt, + const T::Gauss_x& x_est_prev, + const Vec_z& z_meas) { + if constexpr (concepts::DynamicModelLTVWithDefinedSizes> && + concepts::SensorModelLTVWithDefinedSizes) { + using ImmSensMod = + models::ImmSensorModelLTV; + using EKF = + filter::EKF_t; + ImmSensMod imm_sens_mod{sensor_model}; + return EKF::step(dyn_model, imm_sens_mod, dt, x_est_prev, z_meas); + } else { + using ImmSensMod = + models::ImmSensorModel; + using UKF = + filter::UKF_t; + ImmSensMod imm_sens_mod{sensor_model}; + return UKF::step(dyn_model, imm_sens_mod, dt, x_est_prev, z_meas); + } + } - constexpr bool all_states_match = std::apply([](auto... b) { return (b && ...); }, matching_states); + template + static std::tuple::Gauss_x, Gauss_z> kalman_filter_predict( + const DynModT& dyn_model, + const SensModT& sensor_model, + double dt, + const T::Gauss_x& x_est_prev) { + if constexpr (concepts::DynamicModelLTVWithDefinedSizes> && + concepts::SensorModelLTVWithDefinedSizes) { + using ImmSensMod = + models::ImmSensorModelLTV; + using EKF = + filter::EKF_t; + ImmSensMod imm_sens_mod{sensor_model}; + return EKF::predict(dyn_model, imm_sens_mod, dt, x_est_prev); + } else { + using ImmSensMod = + models::ImmSensorModel; + using UKF = + filter::UKF_t; + ImmSensMod imm_sens_mod{sensor_model}; + return UKF::predict(dyn_model, imm_sens_mod, dt, x_est_prev); + } + } - if constexpr (all_states_match) { - return {x, P}; + template + static T::Gauss_x kalman_filter_update( + const DynModT& dyn_model, + const SensModT& sensor_model, + double dt, + const T::Gauss_x& x_est_pred, + const T::Gauss_z& z_est_pred, + const Vec_z& z_meas) { + if constexpr (concepts::DynamicModelLTVWithDefinedSizes> && + concepts::SensorModelLTVWithDefinedSizes) { + using ImmSensMod = + models::ImmSensorModelLTV; + using EKF = + filter::EKF_t; + ImmSensMod imm_sens_mod{sensor_model}; + return EKF::update(imm_sens_mod, x_est_pred, z_est_pred, z_meas); + } else { + using ImmSensMod = + models::ImmSensorModel; + using UKF = + filter::UKF_t; + ImmSensMod imm_sens_mod{sensor_model}; + return UKF::update(dyn_model, imm_sens_mod, dt, x_est_pred, + z_est_pred, z_meas); + } } - Vec_x_b matching_states_vec_b = Eigen::Map(models::matching_state_names(target_state_names, mixing_state_names).data()); - Vec_x matching_states_vec = matching_states_vec_b.template cast(); - - Mat_xx_b matching_states_mat_b = matching_states_vec_b * matching_states_vec_b.transpose(); - Mat_xx matching_states_mat = matching_states_mat_b.template cast(); - - // Set the states and covariances that are not in the target model to 0 - x = x.cwiseProduct(matching_states_vec); - P = P.cwiseProduct(matching_states_mat); - - for (size_t i = 0; i < N_DIM_target; i++) { - if (matching_states_vec(i)) - continue; - StateName state_name = target_state_names.at(i); - if (!states_min_max.contains(state_name)) { - x(i) = target_state.mean()(i); - P(i, i) = target_state.cov()(i, i); - continue; - } - double min = states_min_max.at(state_name).min; - double max = states_min_max.at(state_name).max; - Uniform initial_estimate{min, max}; - x(i) = initial_estimate.mean(); - P(i, i) = initial_estimate.cov(); + /** Helper function to mix one component (mode) of the IMM filter. It mixes + * all the components with the target model. + * @tparam target_model_index The model to mix the other models into + * @param x_est_prevs Gaussians from previous time step + * @param weights Weights (column of the mixing_probs matrix corresponding + * to the target model index) + * @param states_min_max The minimum and maximum value each state can take + * (optional, but can lead to better performance) + * @return Gauss_x The updated model after mixing it + * with the other models + * @note This is the function that actually does the mixing of the models. + * It is called for each model in the IMM filter. + */ + template + static T::Gauss_x mix_one_component( + const GaussTuple_x& x_est_prevs, + const Vec_n& weights, + const StateMap& states_min_max = {}) { + auto moment_based_preds = + [&]( + std::index_sequence) + -> std::array::Gauss_x, N_MODELS> { + return { + prepare_mixing_model( + x_est_prevs, states_min_max)...}; + }(std::make_index_sequence{}); + + return typename T::GaussMix_x{weights, + moment_based_preds} + .reduce(); } - return {x, P}; - } + /** + * @brief Fit the size and states of the mixing_model in case it doesn't + * have the same dimensions or states as the target model. A map of the + * minimum and maximum values for each state (`states_min_max`) can be + * provided to initialize the missing states. If the `states_min_max` map is + * not provided, the missing states are initialized with the mean and + * covariance from the target model. + * @tparam target_model_index The model to mix the other models into + * @tparam mixing_model_index The model to mix into the target model + * @param x_est_prevs Gaussians from previous time step + * @param states_min_max The minimum and maximum value each state can take + * (optional, but can lead to better filter performance) + * @return Gauss_x Modified `mixing_model` to fit the + * size and types of `target_model` + * @note - If the sizes and state names of the mixing model and the target + * model are the same, the mixing model is returned as is. + * @note - If the sizes are different, the mixing model is modified to fit + * the size of the target model and the possibly missing states are + * initialized with the mean and covariance from the target model or with a + * uniform distribution if `states_min_max` is provided. + */ + template + static T::Gauss_x prepare_mixing_model( + const GaussTuple_x& x_est_prevs, + const StateMap& states_min_max = {}) { + if constexpr (target_model_index == mixing_model_index) { + return std::get(x_est_prevs); + } + + constexpr size_t N_DIM_target = ImmModelT::N_DIM_x(target_model_index); + constexpr size_t N_DIM_mixing = ImmModelT::N_DIM_x(mixing_model_index); + constexpr size_t N_DIM_min = std::min(N_DIM_target, N_DIM_mixing); + + using Vec_x = Eigen::Vector; + using Mat_xx = Eigen::Matrix; + using Uniform = prob::Uniform<1>; + using Vec_x_b = Eigen::Vector; + using Mat_xx_b = Eigen::Matrix; + + auto target_state = std::get(x_est_prevs); + auto mixing_state = std::get(x_est_prevs); + + Vec_x x = Vec_x::Zero(); + Mat_xx P = Mat_xx::Zero(); + + x.template head() = + mixing_state.mean().template head(); + P.template topLeftCorner() = + mixing_state.cov().template topLeftCorner(); + + constexpr auto target_state_names = + std::get(ImmModelT::ALL_STATE_NAMES); + constexpr auto mixing_state_names = + std::get(ImmModelT::ALL_STATE_NAMES); + constexpr auto matching_states = models::matching_state_names( + target_state_names, mixing_state_names); + + constexpr bool all_states_match = + std::apply([](auto... b) { return (b && ...); }, matching_states); + + if constexpr (all_states_match) { + return {x, P}; + } + + Vec_x_b matching_states_vec_b = Eigen::Map( + models::matching_state_names(target_state_names, mixing_state_names) + .data()); + Vec_x matching_states_vec = + matching_states_vec_b.template cast(); + + Mat_xx_b matching_states_mat_b = + matching_states_vec_b * matching_states_vec_b.transpose(); + Mat_xx matching_states_mat = + matching_states_mat_b.template cast(); + + // Set the states and covariances that are not in the target model to 0 + x = x.cwiseProduct(matching_states_vec); + P = P.cwiseProduct(matching_states_mat); + + for (size_t i = 0; i < N_DIM_target; i++) { + if (matching_states_vec(i)) + continue; + StateName state_name = target_state_names.at(i); + if (!states_min_max.contains(state_name)) { + x(i) = target_state.mean()(i); + P(i, i) = target_state.cov()(i, i); + continue; + } + double min = states_min_max.at(state_name).min; + double max = states_min_max.at(state_name).max; + Uniform initial_estimate{min, max}; + x(i) = initial_estimate.mean(); + P(i, i) = initial_estimate.cov(); + } + + return {x, P}; + } }; -} // namespace vortex::filter \ No newline at end of file +} // namespace vortex::filter diff --git a/vortex-filtering/include/vortex_filtering/filters/immipda.hpp b/vortex-filtering/include/vortex_filtering/filters/immipda.hpp index da5089db..aca08ef6 100644 --- a/vortex-filtering/include/vortex_filtering/filters/immipda.hpp +++ b/vortex-filtering/include/vortex_filtering/filters/immipda.hpp @@ -16,175 +16,224 @@ namespace vortex::filter { namespace config { struct IMMIPDA { - StateMap states_min_max; + StateMap states_min_max; }; -} // namespace config - -template class IMMIPDA { -public: - template - using T = Types_xzuvw; - using ImmFilter_ = ImmFilter; - using Arr_nXd = Eigen::Array; - using Arr_1Xd = Eigen::Array; - using Arr_nXb = Eigen::Array; - - template using PDAF_ = PDAF, SensModT>; - template using IPDA_ = IPDA, SensModT>; - - struct Config { - config::PDAF pdaf; - config::IPDA ipda; - config::IMMIPDA immipda; - }; - - struct State { - ImmModelT::GaussTuple_x x_estimate; - ImmModelT::Vec_n mode_probabilities; - double existence_probability; - }; - - struct Output { - State state; - ImmModelT::GaussTuple_x x_preds; - ImmFilter_::GaussArr_z z_preds; - std::vector x_upds; - Arr_nXb gated_measurements; - }; - - IMMIPDA() = delete; - - /** - * @brief Do one step of IMMIPDA filtering - * - * @param imm_model IMM model to use - * @param sens_mod Sensor model to use - * @param timestep Time step - * @param state_est_prev Previous state estimate (previous x-estimate, mode probabilities, existence probability) - * @param z_measurements Measurements - * @param config Configuration - * @return std::tuple, std::vector, ImmModelT::GaussTuple_x, - * ImmModelT::GaussTuple_z, std::vector> - */ - static Output step(const ImmModelT &imm_model, const SensModT &sens_mod, double timestep, const State &state_est_prev, const Arr_nXd &z_measurements, - const Config &config) - { - const auto &x_est_prevs = state_est_prev.x_estimate; - const auto &mode_prob_est_prevs = state_est_prev.mode_probabilities; - const auto &existence_prob_est = state_est_prev.existence_probability; - - const size_t m_k = z_measurements.cols(); - // Calculate IMM mixing probabilities - auto transition_matrix = imm_model.get_pi_mat_d(timestep); - auto mixing_probs = ImmFilter_::calculate_mixing_probs(transition_matrix, mode_prob_est_prevs); - - // IMM mixing - auto moment_based_preds = ImmFilter_::mixing(x_est_prevs, mixing_probs, config.immipda.states_min_max); - - // IMM Kalman prediction - auto [x_est_preds, z_est_preds] = ImmFilter_::kalman_filter_predictions(imm_model, sens_mod, timestep, moment_based_preds); - - // Gate measurements - Arr_nXb gated_measurements(ImmModelT::N_MODELS, m_k); - [&](std::index_sequence) { - ((gated_measurements.row(s_k) = PDAF_::apply_gate(z_measurements, z_est_preds.at(s_k), {config.pdaf})), ...); - }(std::make_index_sequence{}); - - // Split measurements - const size_t m_k_inside = (gated_measurements.colwise().any()).count(); - Arr_nXd z_meas_inside(ImmModelT::N_MODELS, m_k_inside); - Arr_nXb gated_meas_inside(ImmModelT::N_MODELS, m_k_inside); - - for (size_t a_k = 0; const auto &z_k : z_measurements.colwise()) { - if (gated_measurements.col(a_k).any()) { - z_meas_inside.col(a_k) = z_k; - gated_meas_inside.col(a_k) = gated_measurements.col(a_k); - } - a_k++; - } - - // IMM Kalman update (7.63) - std::vector imm_estimates(m_k_inside + 1); - imm_estimates.at(0) = x_est_preds; - for (size_t a_k : std::views::iota(1u, m_k_inside + 1)) { - auto z_k = z_meas_inside.col(a_k - 1); - auto gated_k = gated_meas_inside.col(a_k - 1); - imm_estimates.at(a_k) = ImmFilter_::kalman_filter_updates(imm_model, sens_mod, timestep, x_est_preds, z_est_preds, z_k, gated_k); - } +} // namespace config + +template +class IMMIPDA { + public: + template + using T = Types_xzuvw; + using ImmFilter_ = ImmFilter; + using Arr_nXd = Eigen::Array; + using Arr_1Xd = Eigen::Array; + using Arr_nXb = Eigen::Array; + + template + using PDAF_ = PDAF, SensModT>; + template + using IPDA_ = IPDA, SensModT>; + + struct Config { + config::PDAF pdaf; + config::IPDA ipda; + config::IMMIPDA immipda; + }; - // IMM mode probabilities (6.37-6.38) - Arr_nXd predicted_mode_probabilities(ImmModelT::N_MODELS, m_k_inside + 1); - predicted_mode_probabilities.col(0) = mode_prob_est_prevs; // TODO: Find what the mode probability is when a_k = 0 - for (size_t a_k : std::views::iota(1u, m_k_inside + 1)) { - auto z_k = z_meas_inside.col(a_k - 1); - predicted_mode_probabilities.col(a_k) = ImmFilter_::update_probabilities(mixing_probs, z_est_preds, z_k, mode_prob_est_prevs); - } + struct State { + ImmModelT::GaussTuple_x x_estimate; + ImmModelT::Vec_n mode_probabilities; + double existence_probability; + }; - // Calculate the hypothesis-conditional likelihood (7.61) - // TODO: Verify what the hypothesis-conditional likelihood is when a_k = 0 - // (for now assume it is lambda(1-P_D) for all s_k) - Arr_nXd hypothesis_conditional_likelyhood(ImmModelT::N_MODELS, m_k_inside + 1); - for (size_t s_k = 0; const auto &z_pred_s_k : z_est_preds) { - double P_d = config.pdaf.prob_of_detection; - double lambda = config.pdaf.clutter_intensity; - hypothesis_conditional_likelyhood(s_k, 0) = lambda * (1.0 - P_d); - for (size_t a_k = 1; const auto &z_k : z_meas_inside.colwise()) { - hypothesis_conditional_likelyhood(s_k, a_k) = z_pred_s_k.pdf(z_k); - a_k++; - } - s_k++; - } + struct Output { + State state; + ImmModelT::GaussTuple_x x_preds; + ImmFilter_::GaussArr_z z_preds; + std::vector x_upds; + Arr_nXb gated_measurements; + }; - // Calculate the hypothesis likelihoods (7.62) - Eigen::ArrayXd hypothesis_likelihoods(m_k_inside); - for (size_t a_k : std::views::iota(0u, m_k_inside)) { - hypothesis_likelihoods(a_k) = predicted_mode_probabilities.col(a_k).matrix().dot(hypothesis_conditional_likelyhood.col(a_k).matrix()); + IMMIPDA() = delete; + + /** + * @brief Do one step of IMMIPDA filtering + * + * @param imm_model IMM model to use + * @param sens_mod Sensor model to use + * @param timestep Time step + * @param state_est_prev Previous state estimate (previous x-estimate, mode + * probabilities, existence probability) + * @param z_measurements Measurements + * @param config Configuration + * @return std::tuple, std::vector, ImmModelT::GaussTuple_x, + * ImmModelT::GaussTuple_z, std::vector> + */ + static Output step(const ImmModelT& imm_model, + const SensModT& sens_mod, + double timestep, + const State& state_est_prev, + const Arr_nXd& z_measurements, + const Config& config) { + const auto& x_est_prevs = state_est_prev.x_estimate; + const auto& mode_prob_est_prevs = state_est_prev.mode_probabilities; + const auto& existence_prob_est = state_est_prev.existence_probability; + + const size_t m_k = z_measurements.cols(); + // Calculate IMM mixing probabilities + auto transition_matrix = imm_model.get_pi_mat_d(timestep); + auto mixing_probs = ImmFilter_::calculate_mixing_probs( + transition_matrix, mode_prob_est_prevs); + + // IMM mixing + auto moment_based_preds = ImmFilter_::mixing( + x_est_prevs, mixing_probs, config.immipda.states_min_max); + + // IMM Kalman prediction + auto [x_est_preds, z_est_preds] = ImmFilter_::kalman_filter_predictions( + imm_model, sens_mod, timestep, moment_based_preds); + + // Gate measurements + Arr_nXb gated_measurements(ImmModelT::N_MODELS, m_k); + [&](std::index_sequence) { + ((gated_measurements.row(s_k) = PDAF_::apply_gate( + z_measurements, z_est_preds.at(s_k), {config.pdaf})), + ...); + }(std::make_index_sequence{}); + + // Split measurements + const size_t m_k_inside = (gated_measurements.colwise().any()).count(); + Arr_nXd z_meas_inside(ImmModelT::N_MODELS, m_k_inside); + Arr_nXb gated_meas_inside(ImmModelT::N_MODELS, m_k_inside); + + for (size_t a_k = 0; const auto& z_k : z_measurements.colwise()) { + if (gated_measurements.col(a_k).any()) { + z_meas_inside.col(a_k) = z_k; + gated_meas_inside.col(a_k) = gated_measurements.col(a_k); + } + a_k++; + } + + // IMM Kalman update (7.63) + std::vector imm_estimates(m_k_inside + + 1); + imm_estimates.at(0) = x_est_preds; + for (size_t a_k : std::views::iota(1u, m_k_inside + 1)) { + auto z_k = z_meas_inside.col(a_k - 1); + auto gated_k = gated_meas_inside.col(a_k - 1); + imm_estimates.at(a_k) = ImmFilter_::kalman_filter_updates( + imm_model, sens_mod, timestep, x_est_preds, z_est_preds, z_k, + gated_k); + } + + // IMM mode probabilities (6.37-6.38) + Arr_nXd predicted_mode_probabilities(ImmModelT::N_MODELS, + m_k_inside + 1); + predicted_mode_probabilities.col(0) = + mode_prob_est_prevs; // TODO: Find what the mode probability is + // when a_k = 0 + for (size_t a_k : std::views::iota(1u, m_k_inside + 1)) { + auto z_k = z_meas_inside.col(a_k - 1); + predicted_mode_probabilities.col(a_k) = + ImmFilter_::update_probabilities(mixing_probs, z_est_preds, z_k, + mode_prob_est_prevs); + } + + // Calculate the hypothesis-conditional likelihood (7.61) + // TODO: Verify what the hypothesis-conditional likelihood is when a_k = + // 0 (for now assume it is lambda(1-P_D) for all s_k) + Arr_nXd hypothesis_conditional_likelyhood(ImmModelT::N_MODELS, + m_k_inside + 1); + for (size_t s_k = 0; const auto& z_pred_s_k : z_est_preds) { + double P_d = config.pdaf.prob_of_detection; + double lambda = config.pdaf.clutter_intensity; + hypothesis_conditional_likelyhood(s_k, 0) = lambda * (1.0 - P_d); + for (size_t a_k = 1; const auto& z_k : z_meas_inside.colwise()) { + hypothesis_conditional_likelyhood(s_k, a_k) = + z_pred_s_k.pdf(z_k); + a_k++; + } + s_k++; + } + + // Calculate the hypothesis likelihoods (7.62) + Eigen::ArrayXd hypothesis_likelihoods(m_k_inside); + for (size_t a_k : std::views::iota(0u, m_k_inside)) { + hypothesis_likelihoods(a_k) = + predicted_mode_probabilities.col(a_k).matrix().dot( + hypothesis_conditional_likelyhood.col(a_k).matrix()); + } + + // Calculate the association probabilities (7.56) + Eigen::ArrayXd association_probabilities(m_k_inside + 1); + association_probabilities = PDAF_<0>::association_probabilities( + hypothesis_likelihoods, config.pdaf.prob_of_detection, + config.pdaf.clutter_intensity); + + // Calculate the posterior mode probabilities (7.52) + Arr_nXd posterior_mode_probabilities(ImmModelT::N_MODELS, + m_k_inside + 1); + posterior_mode_probabilities = + hypothesis_conditional_likelyhood * predicted_mode_probabilities; + posterior_mode_probabilities /= + posterior_mode_probabilities.colwise().sum().replicate( + ImmModelT::N_MODELS, 1); + + // Calculate the mode-conditional association probabilities (7.57) + Arr_nXd mode_conditional_association_probabilities = + posterior_mode_probabilities.rowwise() * + association_probabilities.transpose(); + typename ImmModelT::Vec_n mode_prob_upd = + mode_conditional_association_probabilities.rowwise().sum(); + mode_conditional_association_probabilities /= + mode_prob_upd.replicate(1, m_k_inside + 1).array(); + + // PDAF_ mixture reduction (7.58) + typename ImmModelT::GaussTuple_x x_est_upds; + + [&](std::index_sequence) { + std::tuple::Gauss_x>...> + imm_estimates_s_k; + + for (const auto& imm_estimates_a_k : imm_estimates) { + (std::get(imm_estimates_s_k) + .push_back(std::get(imm_estimates_a_k)), + ...); + } + + ((std::get(x_est_upds) = + typename T::GaussMix_x{ + mode_conditional_association_probabilities.row(s_k), + std::get(imm_estimates_s_k)} + .reduce()), + ...); + }(std::make_index_sequence{}); + + // Calculate existence probability (7.32-7.33) + double existence_prob_upd = IPDA_<0>::existence_prob_update( + hypothesis_likelihoods, existence_prob_est, + {config.pdaf, config.ipda}); + + return { + .state = + { + .x_estimate = x_est_upds, + .mode_probabilities = mode_prob_upd, + .existence_probability = existence_prob_upd, + }, + .x_preds = x_est_preds, + .z_preds = z_est_preds, + .x_upds = imm_estimates, + .gated_measurements = gated_measurements, + }; } - - // Calculate the accociation probabilities (7.56) - Eigen::ArrayXd association_probabilities(m_k_inside + 1); - association_probabilities = PDAF_<0>::association_probabilities(hypothesis_likelihoods, config.pdaf.prob_of_detection, config.pdaf.clutter_intensity); - - // Calculate the posterior mode probabilities (7.52) - Arr_nXd posterior_mode_probabilities(ImmModelT::N_MODELS, m_k_inside + 1); - posterior_mode_probabilities = hypothesis_conditional_likelyhood * predicted_mode_probabilities; - posterior_mode_probabilities /= posterior_mode_probabilities.colwise().sum().replicate(ImmModelT::N_MODELS, 1); - - // Calculate the mode-conditional association probabilities (7.57) - Arr_nXd mode_conditional_association_probabilities = posterior_mode_probabilities.rowwise() * association_probabilities.transpose(); - typename ImmModelT::Vec_n mode_prob_upd = mode_conditional_association_probabilities.rowwise().sum(); - mode_conditional_association_probabilities /= mode_prob_upd.replicate(1, m_k_inside + 1).array(); - - // PDAF_ mixture reduction (7.58) - typename ImmModelT::GaussTuple_x x_est_upds; - - [&](std::index_sequence) { - std::tuple::Gauss_x>...> imm_estimates_s_k; - - for (const auto &imm_estimates_a_k : imm_estimates) { - (std::get(imm_estimates_s_k).push_back(std::get(imm_estimates_a_k)), ...); - } - - ((std::get(x_est_upds) = typename T::GaussMix_x{mode_conditional_association_probabilities.row(s_k), std::get(imm_estimates_s_k)}.reduce()), - ...); - }(std::make_index_sequence{}); - - // Calculate existence probability (7.32-7.33) - double existence_prob_upd = IPDA_<0>::existence_prob_update(hypothesis_likelihoods, existence_prob_est, {config.pdaf, config.ipda}); - - return { - .state = - { - .x_estimate = x_est_upds, - .mode_probabilities = mode_prob_upd, - .existence_probability = existence_prob_upd, - }, - .x_preds = x_est_preds, - .z_preds = z_est_preds, - .x_upds = imm_estimates, - .gated_measurements = gated_measurements, - }; - } }; -} // namespace vortex::filter \ No newline at end of file +} // namespace vortex::filter diff --git a/vortex-filtering/include/vortex_filtering/filters/ipda.hpp b/vortex-filtering/include/vortex_filtering/filters/ipda.hpp index 736b7dc0..0df43d3c 100644 --- a/vortex-filtering/include/vortex_filtering/filters/ipda.hpp +++ b/vortex-filtering/include/vortex_filtering/filters/ipda.hpp @@ -10,141 +10,165 @@ namespace vortex::filter { namespace config { struct IPDA { - double prob_of_survival = 0.99; - bool estimate_clutter = true; - bool update_existence_probability_on_no_detection = true; + double prob_of_survival = 0.99; + bool estimate_clutter = true; + bool update_existence_probability_on_no_detection = true; }; -} // namespace config - -template class IPDA { -public: - static constexpr int N_DIM_x = DynModT::N_DIM_x; - static constexpr int N_DIM_z = SensModT::N_DIM_z; - static constexpr int N_DIM_u = DynModT::N_DIM_u; - static constexpr int N_DIM_v = DynModT::N_DIM_v; - static constexpr int N_DIM_w = SensModT::N_DIM_w; - - using T = Types_xzuvw; - - using Arr_zXd = Eigen::Array; - using Arr_1Xb = Eigen::Array; - using EKF = vortex::filter::EKF_t; - using PDAF = vortex::filter::PDAF; - - IPDA() = delete; - - struct Config { - config::PDAF pdaf; - config::IPDA ipda; - }; - - struct State { - T::Gauss_x x_estimate; - double existence_probability; - }; - - struct Output { - State state; - T::Gauss_x x_prediction; - T::Gauss_z z_prediction; - std::vector x_updates; - Arr_1Xb gated_measurements; - }; - - static double existence_prediction(double existence_prob_est, double prob_of_survival) - { - double r_km1 = existence_prob_est; - double P_s = prob_of_survival; - return P_s * r_km1; // (7.28) - } - - /** - * @brief Calculates the existence probability given the measurements and the previous existence probability. - * @param z_measurements The measurements to iterate over. - * @param z_pred The predicted measurement. - * @param existence_prob_est (r_{k-1}) The previous existence probability. - * @param config The configuration for the IPDA. - * @return The existence probability. - */ - static double existence_prob_update(const Arr_zXd &z_measurements, T::Gauss_z &z_pred, double existence_prob_pred, Config config) - { - double r_kgkm1 = existence_prob_pred; - double P_d = config.pdaf.prob_of_detection; - double lambda = config.pdaf.clutter_intensity; - - // predicted measurement probability - double z_pred_prob = 0.0; - for (const typename T::Vec_z &z_k : z_measurements.colwise()) { - z_pred_prob += z_pred.pdf(z_k); - } +} // namespace config + +template +class IPDA { + public: + static constexpr int N_DIM_x = DynModT::N_DIM_x; + static constexpr int N_DIM_z = SensModT::N_DIM_z; + static constexpr int N_DIM_u = DynModT::N_DIM_u; + static constexpr int N_DIM_v = DynModT::N_DIM_v; + static constexpr int N_DIM_w = SensModT::N_DIM_w; + + using T = Types_xzuvw; + + using Arr_zXd = Eigen::Array; + using Arr_1Xb = Eigen::Array; + using EKF = + vortex::filter::EKF_t; + using PDAF = vortex::filter::PDAF; + + IPDA() = delete; + + struct Config { + config::PDAF pdaf; + config::IPDA ipda; + }; - // posterior existence probability r_k - double L_k = 1 - P_d + P_d / lambda * z_pred_prob; // (7.33) - double r_k = (L_k * r_kgkm1) / (1 - (1 - L_k) * r_kgkm1); // (7.32) - return r_k; - } - - /** - * @brief Calculates the existence probability given the likelyhood of the measurements and the previous existence probability. - * @param z_likelyhoods (l_a_k) The likelyhood of the measurements - * @param existence_prob_est (r_{k-1}) The previous existence probability. - * @param config The configuration for the IPDA. - * @return The existence probability (r_k). - */ - static double existence_prob_update(const Eigen::ArrayXd z_likelyhoods, double existence_prob_pred, Config config) - { - double r_kgkm1 = existence_prob_pred; // r_k given k minus 1 - double P_d = config.pdaf.prob_of_detection; - double lambda = config.pdaf.clutter_intensity; - - // posterior existence probability r_k - double L_k = 1 - P_d + P_d / lambda * z_likelyhoods.sum(); // (7.33) - double r_k = (L_k * r_kgkm1) / (1 - (1 - L_k) * r_kgkm1); // (7.32) - return r_k; - } - - /** - * @brief Estimates the clutter intensity using (7.31) - * @param z_pred The predicted measurement. - * @param predicted_existence_probability (r_{k|k-1}) The predicted existence probability. - * @param num_measurements (m_k) The number of z_measurements. - * @param config The configuration for the IPDA. - * @return The clutter intensity. - */ - static double estimate_clutter_intensity(const T::Gauss_z &z_pred, double predicted_existence_probability, double num_measurements, Config config) - { - size_t m_k = num_measurements; - double P_d = config.pdaf.prob_of_detection; - double r_k = predicted_existence_probability; - double V_k = utils::Ellipsoid(z_pred, config.pdaf.mahalanobis_threshold).volume(); // gate area - - if (m_k == 0) { - return 0.0; - } - return 1 / V_k * (m_k - r_k * P_d); // (7.31) - } - - static Output step(const DynModT &dyn_mod, const SensModT &sens_mod, double timestep, const State &state_est_prev, const Arr_zXd &z_measurements, - Config &config) - { - double existence_prob_pred = existence_prediction(state_est_prev.existence_probability, config.ipda.prob_of_survival); - - if (config.ipda.estimate_clutter) { - typename T::Gauss_z z_pred; - std::tie(std::ignore, z_pred) = EKF::predict(dyn_mod, sens_mod, timestep, state_est_prev.x_estimate); - config.pdaf.clutter_intensity = estimate_clutter_intensity(z_pred, existence_prob_pred, z_measurements.cols(), config); + struct State { + T::Gauss_x x_estimate; + double existence_probability; + }; + + struct Output { + State state; + T::Gauss_x x_prediction; + T::Gauss_z z_prediction; + std::vector x_updates; + Arr_1Xb gated_measurements; + }; + + static double existence_prediction(double existence_prob_est, + double prob_of_survival) { + double r_km1 = existence_prob_est; + double P_s = prob_of_survival; + return P_s * r_km1; // (7.28) } - auto [x_post, x_pred, z_pred, x_upd, gated_measurements] = - PDAF::step(dyn_mod, sens_mod, timestep, state_est_prev.x_estimate, z_measurements, {config.pdaf}); + /** + * @brief Calculates the existence probability given the measurements and + * the previous existence probability. + * @param z_measurements The measurements to iterate over. + * @param z_pred The predicted measurement. + * @param existence_prob_est (r_{k-1}) The previous existence probability. + * @param config The configuration for the IPDA. + * @return The existence probability. + */ + static double existence_prob_update(const Arr_zXd& z_measurements, + T::Gauss_z& z_pred, + double existence_prob_pred, + Config config) { + double r_kgkm1 = existence_prob_pred; + double P_d = config.pdaf.prob_of_detection; + double lambda = config.pdaf.clutter_intensity; + + // predicted measurement probability + double z_pred_prob = 0.0; + for (const typename T::Vec_z& z_k : z_measurements.colwise()) { + z_pred_prob += z_pred.pdf(z_k); + } + + // posterior existence probability r_k + double L_k = 1 - P_d + P_d / lambda * z_pred_prob; // (7.33) + double r_k = (L_k * r_kgkm1) / (1 - (1 - L_k) * r_kgkm1); // (7.32) + return r_k; + } - Arr_zXd z_meas_inside = PDAF::get_inside_measurements(z_measurements, gated_measurements); + /** + * @brief Calculates the existence probability given the likelihood of the + * measurements and the previous existence probability. + * @param z_likelyhoods (l_a_k) The likelihood of the measurements + * @param existence_prob_est (r_{k-1}) The previous existence probability. + * @param config The configuration for the IPDA. + * @return The existence probability (r_k). + */ + static double existence_prob_update(const Eigen::ArrayXd z_likelyhoods, + double existence_prob_pred, + Config config) { + double r_kgkm1 = existence_prob_pred; // r_k given k minus 1 + double P_d = config.pdaf.prob_of_detection; + double lambda = config.pdaf.clutter_intensity; + + // posterior existence probability r_k + double L_k = 1 - P_d + P_d / lambda * z_likelyhoods.sum(); // (7.33) + double r_k = (L_k * r_kgkm1) / (1 - (1 - L_k) * r_kgkm1); // (7.32) + return r_k; + } - double existence_probability_upd = existence_prob_pred; - if (!(z_measurements.cols() == 0 && config.ipda.update_existence_probability_on_no_detection)) { - existence_probability_upd = existence_prob_update(z_meas_inside, z_pred, existence_prob_pred, config); + /** + * @brief Estimates the clutter intensity using (7.31) + * @param z_pred The predicted measurement. + * @param predicted_existence_probability (r_{k|k-1}) The predicted + * existence probability. + * @param num_measurements (m_k) The number of z_measurements. + * @param config The configuration for the IPDA. + * @return The clutter intensity. + */ + static double estimate_clutter_intensity( + const T::Gauss_z& z_pred, + double predicted_existence_probability, + double num_measurements, + Config config) { + size_t m_k = num_measurements; + double P_d = config.pdaf.prob_of_detection; + double r_k = predicted_existence_probability; + double V_k = + utils::Ellipsoid(z_pred, config.pdaf.mahalanobis_threshold) + .volume(); // gate area + + if (m_k == 0) { + return 0.0; + } + return 1 / V_k * (m_k - r_k * P_d); // (7.31) } - // clang-format off + + static Output step(const DynModT& dyn_mod, + const SensModT& sens_mod, + double timestep, + const State& state_est_prev, + const Arr_zXd& z_measurements, + Config& config) { + double existence_prob_pred = existence_prediction( + state_est_prev.existence_probability, config.ipda.prob_of_survival); + + if (config.ipda.estimate_clutter) { + typename T::Gauss_z z_pred; + std::tie(std::ignore, z_pred) = EKF::predict( + dyn_mod, sens_mod, timestep, state_est_prev.x_estimate); + config.pdaf.clutter_intensity = estimate_clutter_intensity( + z_pred, existence_prob_pred, z_measurements.cols(), config); + } + + auto [x_post, x_pred, z_pred, x_upd, gated_measurements] = + PDAF::step(dyn_mod, sens_mod, timestep, state_est_prev.x_estimate, + z_measurements, {config.pdaf}); + + Arr_zXd z_meas_inside = + PDAF::get_inside_measurements(z_measurements, gated_measurements); + + double existence_probability_upd = existence_prob_pred; + if (!(z_measurements.cols() == 0 && + config.ipda.update_existence_probability_on_no_detection)) { + existence_probability_upd = existence_prob_update( + z_meas_inside, z_pred, existence_prob_pred, config); + } + // clang-format off return { .state = { .x_estimate = x_post, @@ -155,7 +179,7 @@ template ::max(); - double prob_of_detection = 1.0; - double clutter_intensity = 1.0; + double mahalanobis_threshold = 1.0; + double min_gate_threshold = 0.0; + double max_gate_threshold = std::numeric_limits::max(); + double prob_of_detection = 1.0; + double clutter_intensity = 1.0; }; -} // namespace config - -template class PDAF { -public: - static constexpr int N_DIM_x = DynModT::N_DIM_x; - static constexpr int N_DIM_z = SensModT::N_DIM_z; - static constexpr int N_DIM_u = DynModT::N_DIM_u; - static constexpr int N_DIM_v = DynModT::N_DIM_v; - static constexpr int N_DIM_w = SensModT::N_DIM_w; - - using T = Types_xzuvw; - - using Gauss_z = typename T::Gauss_z; - using Gauss_x = typename T::Gauss_x; - using Vec_z = typename T::Vec_z; - using GaussMix_x = typename T::GaussMix_x; - using GaussMix_z = typename T::GaussMix_z; - using Arr_zXd = Eigen::Array; - using Arr_1Xb = Eigen::Array; - using Gauss_xX = std::vector; - using EKF = vortex::filter::EKF_t; - - struct Config { - config::PDAF pdaf; - }; - - struct Output { - Gauss_x x_; - Gauss_x x_prediction; - Gauss_z z_prediction; - Gauss_xX x_updates; - Arr_1Xb gated_measurements; - }; - - PDAF() = delete; - - /** - * @brief Perform one step of the Probabilistic Data Association Filter - * - * @param dyn_model The dynamic model - * @param sen_model The sensor model - * @param timestep Time step in seconds - * @param x_est The estimated state - * @param z_measurements Array of measurements - * @param config Configuration for the PDAF - * @return `Output` The result of the PDAF step and some intermediate results - */ - static Output step(const DynModT &dyn_model, const SensModT &sen_model, double timestep, const Gauss_x &x_est, const Arr_zXd &z_measurements, - const Config &config) - { - auto [x_pred, z_pred] = EKF::predict(dyn_model, sen_model, timestep, x_est); - auto gated_measurements = apply_gate(z_measurements, z_pred, config); - auto inside_meas = get_inside_measurements(z_measurements, gated_measurements); - - Gauss_xX x_updated; - for (const auto &z_k : inside_meas.colwise()) { - x_updated.push_back(EKF::update(sen_model, x_pred, z_pred, z_k)); +} // namespace config + +template +class PDAF { + public: + static constexpr int N_DIM_x = DynModT::N_DIM_x; + static constexpr int N_DIM_z = SensModT::N_DIM_z; + static constexpr int N_DIM_u = DynModT::N_DIM_u; + static constexpr int N_DIM_v = DynModT::N_DIM_v; + static constexpr int N_DIM_w = SensModT::N_DIM_w; + + using T = Types_xzuvw; + + using Gauss_z = typename T::Gauss_z; + using Gauss_x = typename T::Gauss_x; + using Vec_z = typename T::Vec_z; + using GaussMix_x = typename T::GaussMix_x; + using GaussMix_z = typename T::GaussMix_z; + using Arr_zXd = Eigen::Array; + using Arr_1Xb = Eigen::Array; + using Gauss_xX = std::vector; + using EKF = + vortex::filter::EKF_t; + + struct Config { + config::PDAF pdaf; + }; + + struct Output { + Gauss_x x_; + Gauss_x x_prediction; + Gauss_z z_prediction; + Gauss_xX x_updates; + Arr_1Xb gated_measurements; + }; + + PDAF() = delete; + + /** + * @brief Perform one step of the Probabilistic Data Association Filter + * + * @param dyn_model The dynamic model + * @param sen_model The sensor model + * @param timestep Time step in seconds + * @param x_est The estimated state + * @param z_measurements Array of measurements + * @param config Configuration for the PDAF + * @return `Output` The result of the PDAF step and some intermediate + * results + */ + static Output step(const DynModT& dyn_model, + const SensModT& sen_model, + double timestep, + const Gauss_x& x_est, + const Arr_zXd& z_measurements, + const Config& config) { + auto [x_pred, z_pred] = + EKF::predict(dyn_model, sen_model, timestep, x_est); + auto gated_measurements = apply_gate(z_measurements, z_pred, config); + auto inside_meas = + get_inside_measurements(z_measurements, gated_measurements); + + Gauss_xX x_updated; + for (const auto& z_k : inside_meas.colwise()) { + x_updated.push_back(EKF::update(sen_model, x_pred, z_pred, z_k)); + } + + Gauss_x x_final = get_weighted_average( + inside_meas, x_updated, z_pred, x_pred, + config.pdaf.prob_of_detection, config.pdaf.clutter_intensity); + return {x_final, x_pred, z_pred, x_updated, gated_measurements}; } - Gauss_x x_final = get_weighted_average(inside_meas, x_updated, z_pred, x_pred, config.pdaf.prob_of_detection, config.pdaf.clutter_intensity); - return {x_final, x_pred, z_pred, x_updated, gated_measurements}; - } - - /** - * @brief Apply gate to the measurements - * - * @param z_measurements Array of measurements - * @param z_pred Predicted measurement - * @param config Configuration for the PDAF - * @return `Arr_1Xb` Indeces of the measurements that are inside the gate - */ - static Arr_1Xb apply_gate(const Arr_zXd &z_measurements, const Gauss_z &z_pred, Config config) - { - double mahalanobis_threshold = config.pdaf.mahalanobis_threshold; - double min_gate_threshold = config.pdaf.min_gate_threshold; - double max_gate_threshold = config.pdaf.max_gate_threshold; - - Arr_1Xb gated_measurements(1, z_measurements.cols()); - - for (size_t a_k = 0; const Vec_z &z_k : z_measurements.colwise()) { - double mahalanobis_distance = z_pred.mahalanobis_distance(z_k); - double regular_distance = (z_pred.mean() - z_k).norm(); - gated_measurements(a_k++) = - (mahalanobis_distance <= mahalanobis_threshold || regular_distance <= min_gate_threshold) && regular_distance <= max_gate_threshold; + /** + * @brief Apply gate to the measurements + * + * @param z_measurements Array of measurements + * @param z_pred Predicted measurement + * @param config Configuration for the PDAF + * @return `Arr_1Xb` Indices of the measurements that are inside the gate + */ + static Arr_1Xb apply_gate(const Arr_zXd& z_measurements, + const Gauss_z& z_pred, + Config config) { + double mahalanobis_threshold = config.pdaf.mahalanobis_threshold; + double min_gate_threshold = config.pdaf.min_gate_threshold; + double max_gate_threshold = config.pdaf.max_gate_threshold; + + Arr_1Xb gated_measurements(1, z_measurements.cols()); + + for (size_t a_k = 0; const Vec_z& z_k : z_measurements.colwise()) { + double mahalanobis_distance = z_pred.mahalanobis_distance(z_k); + double regular_distance = (z_pred.mean() - z_k).norm(); + gated_measurements(a_k++) = + (mahalanobis_distance <= mahalanobis_threshold || + regular_distance <= min_gate_threshold) && + regular_distance <= max_gate_threshold; + } + return gated_measurements; } - return gated_measurements; - } - - /** - * @brief Get the measurements that are inside the gate - * - * @param z_measurements Array of measurements - * @param gated_measurements Indeces of the measurements that are inside the gate - * @return `Arr_zXd` The measurements that are inside the gate - */ - static Arr_zXd get_inside_measurements(const Arr_zXd &z_measurements, const Arr_1Xb &gated_measurements) - { - Arr_zXd inside_meas(N_DIM_z, gated_measurements.count()); - for (size_t i = 0, j = 0; bool gated : gated_measurements) { - if (gated) { - inside_meas.col(j++) = z_measurements.col(i); - } - i++; + + /** + * @brief Get the measurements that are inside the gate + * + * @param z_measurements Array of measurements + * @param gated_measurements Indices of the measurements that are inside the + * gate + * @return `Arr_zXd` The measurements that are inside the gate + */ + static Arr_zXd get_inside_measurements(const Arr_zXd& z_measurements, + const Arr_1Xb& gated_measurements) { + Arr_zXd inside_meas(N_DIM_z, gated_measurements.count()); + for (size_t i = 0, j = 0; bool gated : gated_measurements) { + if (gated) { + inside_meas.col(j++) = z_measurements.col(i); + } + i++; + } + return inside_meas; } - return inside_meas; - } - - /** - * @brief Get the weighted average of the states - * - * @param z_measurements Array of measurements - * @param updated_states Array of updated states - * @param z_pred Predicted measurement - * @param x_pred Predicted state - * @param prob_of_detection Probability of detection - * @param clutter_intensity Clutter intensity - * @return `Gauss_x` The weighted average of the states - */ - static Gauss_x get_weighted_average(const Arr_zXd &z_measurements, const Gauss_xX &updated_states, const Gauss_z &z_pred, const Gauss_x &x_pred, - double prob_of_detection, double clutter_intensity) - { - Gauss_xX states; - states.push_back(x_pred); - states.insert(states.end(), updated_states.begin(), updated_states.end()); - - Eigen::VectorXd weights = get_weights(z_measurements, z_pred, prob_of_detection, clutter_intensity); - - return GaussMix_x{weights, states}.reduce(); - } - - /** - * @brief Get the weights for the measurements - * - * @param z_measurements Array of measurements - * @param z_pred Predicted measurement - * @param prob_of_detection Probability of detection - * @param clutter_intensity Clutter intensity - * @return `Eigen::VectorXd` The weights for the measurements - */ - static Eigen::VectorXd get_weights(const Arr_zXd &z_measurements, const Gauss_z &z_pred, double prob_of_detection, double clutter_intensity) - { - double lambda = clutter_intensity; - double P_d = prob_of_detection; - - Eigen::VectorXd weights(z_measurements.cols() + 1); - - // in case no measurement assosiates with the target - if(lambda == 0.0 && z_measurements.cols() == 0) { - weights(0) = 1.0; - } else { - weights(0) = lambda * (1 - P_d); + + /** + * @brief Get the weighted average of the states + * + * @param z_measurements Array of measurements + * @param updated_states Array of updated states + * @param z_pred Predicted measurement + * @param x_pred Predicted state + * @param prob_of_detection Probability of detection + * @param clutter_intensity Clutter intensity + * @return `Gauss_x` The weighted average of the states + */ + static Gauss_x get_weighted_average(const Arr_zXd& z_measurements, + const Gauss_xX& updated_states, + const Gauss_z& z_pred, + const Gauss_x& x_pred, + double prob_of_detection, + double clutter_intensity) { + Gauss_xX states; + states.push_back(x_pred); + states.insert(states.end(), updated_states.begin(), + updated_states.end()); + + Eigen::VectorXd weights = get_weights( + z_measurements, z_pred, prob_of_detection, clutter_intensity); + + return GaussMix_x{weights, states}.reduce(); } - // measurements associating with the target - for (size_t a_k = 1; const Vec_z &z_k : z_measurements.colwise()) { - weights(a_k++) = P_d * z_pred.pdf(z_k); + /** + * @brief Get the weights for the measurements + * + * @param z_measurements Array of measurements + * @param z_pred Predicted measurement + * @param prob_of_detection Probability of detection + * @param clutter_intensity Clutter intensity + * @return `Eigen::VectorXd` The weights for the measurements + */ + static Eigen::VectorXd get_weights(const Arr_zXd& z_measurements, + const Gauss_z& z_pred, + double prob_of_detection, + double clutter_intensity) { + double lambda = clutter_intensity; + double P_d = prob_of_detection; + + Eigen::VectorXd weights(z_measurements.cols() + 1); + + // in case no measurement associates with the target + if (lambda == 0.0 && z_measurements.cols() == 0) { + weights(0) = 1.0; + } else { + weights(0) = lambda * (1 - P_d); + } + + // measurements associating with the target + for (size_t a_k = 1; const Vec_z& z_k : z_measurements.colwise()) { + weights(a_k++) = P_d * z_pred.pdf(z_k); + } + + // normalize weights + weights /= weights.sum(); + + return weights; } - // normalize weights - weights /= weights.sum(); - - return weights; - } - - /** - * @brief Get association probabilities according to Corollary 7.3.3 - * - * @param z_likelyhoods Array of likelyhoods - * @param prob_of_detection Probability of detection - * @param clutter_intensity Clutter intensity - * @return `Eigen::VectorXd` The weights for the measurements - */ - static Eigen::ArrayXd association_probabilities(const Eigen::ArrayXd &z_likelyhoods, double prob_of_detection, double clutter_intensity) - { - size_t m_k = z_likelyhoods.size(); - double lambda = clutter_intensity; - double P_d = prob_of_detection; - - Eigen::ArrayXd weights(m_k + 1); - - // Accociation probabilities (Corrolary 7.3.3) - weights(0) = lambda * (1 - P_d); - weights.tail(m_k) = P_d * z_likelyhoods; - - // normalize weights - weights /= weights.sum(); - - return weights; - } + /** + * @brief Get association probabilities according to Corollary 7.3.3 + * + * @param z_likelyhoods Array of likelyhoods + * @param prob_of_detection Probability of detection + * @param clutter_intensity Clutter intensity + * @return `Eigen::VectorXd` The weights for the measurements + */ + static Eigen::ArrayXd association_probabilities( + const Eigen::ArrayXd& z_likelyhoods, + double prob_of_detection, + double clutter_intensity) { + size_t m_k = z_likelyhoods.size(); + double lambda = clutter_intensity; + double P_d = prob_of_detection; + + Eigen::ArrayXd weights(m_k + 1); + + // Association probabilities (Corrolary 7.3.3) + weights(0) = lambda * (1 - P_d); + weights.tail(m_k) = P_d * z_likelyhoods; + + // normalize weights + weights /= weights.sum(); + + return weights; + } }; -} // namespace vortex::filter \ No newline at end of file +} // namespace vortex::filter diff --git a/vortex-filtering/include/vortex_filtering/filters/ukf.hpp b/vortex-filtering/include/vortex_filtering/filters/ukf.hpp index 89a28ac7..28a24e37 100644 --- a/vortex-filtering/include/vortex_filtering/filters/ukf.hpp +++ b/vortex-filtering/include/vortex_filtering/filters/ukf.hpp @@ -15,9 +15,9 @@ #include #include #include -#include -#include #include +#include +#include namespace vortex { namespace filter { @@ -29,274 +29,374 @@ namespace filter { * @tparam n_dim_v Process noise dimension * @tparam n_dim_w Measurement noise dimension * @tparam alpha Parameter for spread of sigma points (default 1.0) - * @tparam beta Parameter for weighting of mean in covariance calculation (default 2.0) - * @tparam kappa Parameter for adding additional spread to sigma points (default 0.0) + * @tparam beta Parameter for weighting of mean in covariance calculation + * (default 2.0) + * @tparam kappa Parameter for adding additional spread to sigma points (default + * 0.0) */ -template +template class UKF_t { -public: - static constexpr int N_DIM_x = n_dim_x; - static constexpr int N_DIM_z = n_dim_z; - static constexpr int N_DIM_u = n_dim_u; - static constexpr int N_DIM_v = n_dim_v; - static constexpr int N_DIM_w = n_dim_w; - - static constexpr int N_DIM_a = N_DIM_x + N_DIM_v + N_DIM_w; // Augmented state dimension - static constexpr size_t N_SIGMA_POINTS = 2 * N_DIM_a + 1; // Number of sigma points - - using T = Types_xzuvw; - - using Vec_a = Eigen::Vector; // Augmented state vector - using Mat_aa = Eigen::Matrix; // Augmented state covariance matrix - using Mat_x2ap1 = Eigen::Matrix; // Matrix for sigma points of x - using Mat_z2ap1 = Eigen::Matrix; // Matrix for sigma points of z - using Mat_a2ap1 = Eigen::Matrix; // Matrix for sigma points of a - - // Parameters for UKF - static constexpr double ALPHA = alpha; - static constexpr double BETA = beta; - static constexpr double KAPPA = kappa; - static constexpr double LAMBDA = ALPHA * ALPHA * (N_DIM_a + KAPPA) - N_DIM_a; // Parameter for spread of sigma points - static constexpr double GAMMA = std::sqrt(N_DIM_a + LAMBDA); // Scaling factor for spread of sigma points - - static constexpr std::array W_x = []() { - std::array W_x; - W_x[0] = LAMBDA / (N_DIM_a + LAMBDA); - for (size_t i = 1; i < N_SIGMA_POINTS; i++) { - W_x[i] = 1 / (2 * (N_DIM_a + LAMBDA)); - } - return W_x; - }(); - - static constexpr std::array W_c = []() { - std::array W_c; - W_c[0] = LAMBDA / (N_DIM_a + LAMBDA) + (1 - ALPHA * ALPHA + BETA); - for (size_t i = 1; i < N_SIGMA_POINTS; i++) { - W_c[i] = 1 / (2 * (N_DIM_a + LAMBDA)); - } - return W_c; - }(); - - UKF_t() = delete; - -private: - /** Get sigma points - * @param dyn_mod Dynamic model - * @param sens_mod Sensor model - * @param dt Time step - * @param x_est T::Gauss_x State estimate - * @return Mat_a2ap1 sigma_points - */ - static Mat_a2ap1 get_sigma_points(const auto &dyn_mod, const auto &sens_mod, double dt, const T::Gauss_x &x_est) - requires(concepts::DynamicModel && concepts::SensorModel) - { - typename T::Mat_xx P = x_est.cov(); - typename T::Mat_vv Q = dyn_mod.Q_d(dt, x_est.mean()); - typename T::Mat_ww R = sens_mod.R(x_est.mean()); - // Make augmented covariance matrix - Mat_aa P_a = Mat_aa::Zero(); - /* - P_a = | P 0 0 | - | 0 Q 0 | - | 0 0 R | - */ - P_a.template topLeftCorner() = P; - P_a.template block(N_DIM_x, N_DIM_x) = Q; - P_a.template bottomRightCorner() = R; - - Mat_aa sqrt_P_a = P_a.llt().matrixLLT(); - - // Make augmented state vector - Vec_a x_a = Vec_a::Zero(); - x_a.template head() = x_est.mean(); - - // Calculate sigma points using the symmetric sigma point set - Mat_a2ap1 sigma_points; - sigma_points.col(0) = x_a; - for (int i = 1; i <= N_DIM_a; i++) { - sigma_points.col(i) = x_a + GAMMA * sqrt_P_a.col(i - 1); - sigma_points.col(i + N_DIM_a) = x_a - GAMMA * sqrt_P_a.col(i - 1); - } - return sigma_points; - } - - /** Propagate sigma points through f - * @param dyn_mod Dynamic model - * @param dt Time step - * @param sigma_points Mat_a2ap1 Sigma points - * @param u T::Vec_u Control input (default 0) - * @return Mat_x2ap1 sigma_x_pred - */ - static Mat_x2ap1 propagate_sigma_points_f(const auto &dyn_mod, double dt, const Mat_a2ap1 &sigma_points, const T::Vec_u &u = T::Vec_u::Zero()) - requires(concepts::DynamicModel) - { - Eigen::Matrix sigma_x_pred; - for (size_t i = 0; i < N_SIGMA_POINTS; i++) { - typename T::Vec_x x_i = sigma_points.template block(0, i); - typename T::Vec_v v_i = sigma_points.template block(N_DIM_x, i); - sigma_x_pred.col(i) = dyn_mod.f_d(dt, x_i, u, v_i); + public: + static constexpr int N_DIM_x = n_dim_x; + static constexpr int N_DIM_z = n_dim_z; + static constexpr int N_DIM_u = n_dim_u; + static constexpr int N_DIM_v = n_dim_v; + static constexpr int N_DIM_w = n_dim_w; + + static constexpr int N_DIM_a = + N_DIM_x + N_DIM_v + N_DIM_w; // Augmented state dimension + static constexpr size_t N_SIGMA_POINTS = + 2 * N_DIM_a + 1; // Number of sigma points + + using T = Types_xzuvw; + + using Vec_a = Eigen::Vector; // Augmented state vector + using Mat_aa = + Eigen::Matrix; // Augmented state covariance + // matrix + using Mat_x2ap1 = + Eigen::Matrix; // Matrix for sigma + // points of x + using Mat_z2ap1 = + Eigen::Matrix; // Matrix for sigma + // points of z + using Mat_a2ap1 = + Eigen::Matrix; // Matrix for sigma + // points of a + + // Parameters for UKF + static constexpr double ALPHA = alpha; + static constexpr double BETA = beta; + static constexpr double KAPPA = kappa; + static constexpr double LAMBDA = + ALPHA * ALPHA * (N_DIM_a + KAPPA) - + N_DIM_a; // Parameter for spread of sigma points + static constexpr double GAMMA = std::sqrt( + N_DIM_a + LAMBDA); // Scaling factor for spread of sigma points + + static constexpr std::array W_x = []() { + std::array W_x; + W_x[0] = LAMBDA / (N_DIM_a + LAMBDA); + for (size_t i = 1; i < N_SIGMA_POINTS; i++) { + W_x[i] = 1 / (2 * (N_DIM_a + LAMBDA)); + } + return W_x; + }(); + + static constexpr std::array W_c = []() { + std::array W_c; + W_c[0] = LAMBDA / (N_DIM_a + LAMBDA) + (1 - ALPHA * ALPHA + BETA); + for (size_t i = 1; i < N_SIGMA_POINTS; i++) { + W_c[i] = 1 / (2 * (N_DIM_a + LAMBDA)); + } + return W_c; + }(); + + UKF_t() = delete; + + private: + /** Get sigma points + * @param dyn_mod Dynamic model + * @param sens_mod Sensor model + * @param dt Time step + * @param x_est T::Gauss_x State estimate + * @return Mat_a2ap1 sigma_points + */ + static Mat_a2ap1 get_sigma_points(const auto& dyn_mod, + const auto& sens_mod, + double dt, + const T::Gauss_x& x_est) + requires( + concepts:: + DynamicModel && + concepts:: + SensorModel) + { + typename T::Mat_xx P = x_est.cov(); + typename T::Mat_vv Q = dyn_mod.Q_d(dt, x_est.mean()); + typename T::Mat_ww R = sens_mod.R(x_est.mean()); + // Make augmented covariance matrix + Mat_aa P_a = Mat_aa::Zero(); + /* + P_a = | P 0 0 | + | 0 Q 0 | + | 0 0 R | + */ + P_a.template topLeftCorner() = P; + P_a.template block(N_DIM_x, N_DIM_x) = Q; + P_a.template bottomRightCorner() = R; + + Mat_aa sqrt_P_a = P_a.llt().matrixLLT(); + + // Make augmented state vector + Vec_a x_a = Vec_a::Zero(); + x_a.template head() = x_est.mean(); + + // Calculate sigma points using the symmetric sigma point set + Mat_a2ap1 sigma_points; + sigma_points.col(0) = x_a; + for (int i = 1; i <= N_DIM_a; i++) { + sigma_points.col(i) = x_a + GAMMA * sqrt_P_a.col(i - 1); + sigma_points.col(i + N_DIM_a) = x_a - GAMMA * sqrt_P_a.col(i - 1); + } + return sigma_points; } - return sigma_x_pred; - } - - /** Propagate sigma points through h - * @param sens_mod Sensor model - * @param sigma_points Mat_a2ap1 Sigma points - * @return Mat_z2ap1 sigma_z_pred - */ - static Mat_z2ap1 propagate_sigma_points_h(const auto &sens_mod, const Mat_a2ap1 &sigma_points) - requires(concepts::SensorModel) - { - Mat_z2ap1 sigma_z_pred; - for (size_t i = 0; i < N_SIGMA_POINTS; i++) { - typename T::Vec_x x_i = sigma_points.template block(0, i); - typename T::Vec_w w_i = sigma_points.template block(N_DIM_x + N_DIM_v, i); - sigma_z_pred.col(i) = sens_mod.h(x_i, w_i); - } - return sigma_z_pred; - } - - /** Estimate gaussian from sigma points using the unscented transform - * @param sigma_points Mat_n2ap1 Sigma points - * @tparam n_dims Dimension of the gaussian - * @return prob::MultiVarGauss - * @note This function is templated to allow for different dimensions of the gaussian - */ - template static prob::MultiVarGauss estimate_gaussian(const Eigen::Matrix &sigma_points) - { - using Vec_n = Eigen::Vector; - using Mat_nn = Eigen::Matrix; - - // Predicted State Estimate x_k- - Vec_n mean = Vec_n::Zero(); - for (size_t i = 0; i < N_SIGMA_POINTS; i++) { - mean += W_x.at(i) * sigma_points.col(i); - } - Mat_nn cov = Mat_nn::Zero(); - for (size_t i = 0; i < N_SIGMA_POINTS; i++) { - cov += W_c.at(i) * (sigma_points.col(i) - mean) * (sigma_points.col(i) - mean).transpose(); + + /** Propagate sigma points through f + * @param dyn_mod Dynamic model + * @param dt Time step + * @param sigma_points Mat_a2ap1 Sigma points + * @param u T::Vec_u Control input (default 0) + * @return Mat_x2ap1 sigma_x_pred + */ + static Mat_x2ap1 propagate_sigma_points_f( + const auto& dyn_mod, + double dt, + const Mat_a2ap1& sigma_points, + const T::Vec_u& u = T::Vec_u::Zero()) + requires(concepts:: + DynamicModel) + { + Eigen::Matrix sigma_x_pred; + for (size_t i = 0; i < N_SIGMA_POINTS; i++) { + typename T::Vec_x x_i = + sigma_points.template block(0, i); + typename T::Vec_v v_i = + sigma_points.template block(N_DIM_x, i); + sigma_x_pred.col(i) = dyn_mod.f_d(dt, x_i, u, v_i); + } + return sigma_x_pred; } - return {mean, cov}; - } - -public: - /** Perform one UKF prediction step - * @param dyn_mod Dynamic model - * @param sens_mod Sensor model - * @param dt Time step - * @param x_est_prev Previous state estimate - * @param u T::Vec_u Control input (default 0) - * @return std::pair Predicted state estimate, predicted measurement estimate - */ - static std::pair predict(const auto &dyn_mod, const auto &sens_mod, double dt, const T::Gauss_x &x_est_prev, - const T::Vec_u &u = T::Vec_u::Zero()) - requires(concepts::DynamicModel && concepts::SensorModel) - { - Mat_a2ap1 sigma_points = get_sigma_points(dyn_mod, sens_mod, dt, x_est_prev); - - // Propagate sigma points through f and h - Mat_x2ap1 sigma_x_pred = propagate_sigma_points_f(dyn_mod, dt, sigma_points, u); - Mat_z2ap1 sigma_z_pred = propagate_sigma_points_h(sens_mod, sigma_points); - - // Predicted State and Measurement Estimate x_k- and z_k- - typename T::Gauss_x x_pred = estimate_gaussian(sigma_x_pred); - typename T::Gauss_z z_pred = estimate_gaussian(sigma_z_pred); - - return {x_pred, z_pred}; - } - - /** Perform one UKF update step - * @param dyn_mod Dynamic model - * @param sens_mod Sensor model - * @param x_est_pred Predicted state estimate - * @param z_est_pred Predicted measurement estimate - * @param z_meas Measurement - * @return T::Gauss_x Updated state estimate - * @note Sigma points are generated from the predicted state estimate instead of the previous state estimate as is done in the 'step' method. - */ - static T::Gauss_x update(const auto &dyn_mod, const auto &sens_mod, double dt, const T::Gauss_x &x_est_pred, const T::Gauss_z &z_est_pred, - const T::Vec_z &z_meas) - requires(concepts::DynamicModel && concepts::SensorModel) - { - // Generate sigma points from the predicted state estimate - Mat_a2ap1 sigma_points = get_sigma_points(dyn_mod, sens_mod, dt, x_est_pred); - - // Extract the sigma points for the state - Mat_x2ap1 sigma_x_pred = sigma_points.template block(0, 0); - - // Predict measurement - Mat_z2ap1 sigma_z_pred = propagate_sigma_points_h(sens_mod, sigma_points); - - // Calculate cross-covariance - typename T::Mat_xz P_xz = T::Mat_xz::Zero(); - for (size_t i = 0; i < N_SIGMA_POINTS; i++) { - P_xz += W_c(i) * (sigma_x_pred.col(i) - x_est_pred.mean()) * (sigma_z_pred.col(i) - z_est_pred.mean()).transpose(); + + /** Propagate sigma points through h + * @param sens_mod Sensor model + * @param sigma_points Mat_a2ap1 Sigma points + * @return Mat_z2ap1 sigma_z_pred + */ + static Mat_z2ap1 propagate_sigma_points_h(const auto& sens_mod, + const Mat_a2ap1& sigma_points) + requires(concepts:: + SensorModel) + { + Mat_z2ap1 sigma_z_pred; + for (size_t i = 0; i < N_SIGMA_POINTS; i++) { + typename T::Vec_x x_i = + sigma_points.template block(0, i); + typename T::Vec_w w_i = + sigma_points.template block(N_DIM_x + N_DIM_v, i); + sigma_z_pred.col(i) = sens_mod.h(x_i, w_i); + } + return sigma_z_pred; } - // Calculate Kalman gain - typename T::Mat_zz P_zz = z_est_pred.cov(); - typename T::Mat_xz K = P_xz * P_zz.llt().solve(T::Mat_zz::Identity()); - - // Update state estimate - typename T::Vec_x x_upd_mean = x_est_pred.mean() + K * (z_meas - z_est_pred.mean()); - typename T::Mat_xx x_upd_cov = x_est_pred.cov() - K * P_zz * K.transpose(); - typename T::Gauss_x x_est_upd = {x_upd_mean, x_upd_cov}; - - return x_est_upd; - } - - /** Perform one UKF prediction and update step - * @param dyn_mod Dynamic model - * @param sens_mod Sensor model - * @param dt Time step - * @param x_est_prev Previous state estimate - * @param z_meas Measurement - * @param u T::Vec_u Control input - * @return std::tuple Updated state estimate, predicted state estimate, predicted measurement estimate - */ - static std::tuple - step(const auto &dyn_mod, const auto &sens_mod, double dt, const T::Gauss_x &x_est_prev, const T::Vec_z &z_meas, const T::Vec_u &u = T::Vec_u::Zero()) - requires(concepts::DynamicModel && concepts::SensorModel) - { - Mat_a2ap1 sigma_points = get_sigma_points(dyn_mod, sens_mod, dt, x_est_prev); - - // Propagate sigma points through f and h - Mat_x2ap1 sigma_x_pred = propagate_sigma_points_f(dyn_mod, dt, sigma_points, u); - Mat_z2ap1 sigma_z_pred = propagate_sigma_points_h(sens_mod, sigma_points); - - // Predicted State and Measurement Estimate x_k- and z_k- - typename T::Gauss_x x_pred = estimate_gaussian(sigma_x_pred); - typename T::Gauss_z z_pred = estimate_gaussian(sigma_z_pred); - - // Calculate cross-covariance - typename T::Mat_xz P_xz = T::Mat_xz::Zero(); - for (size_t i = 0; i < N_SIGMA_POINTS; i++) { - P_xz += W_c.at(i) * (sigma_x_pred.col(i) - x_pred.mean()) * (sigma_z_pred.col(i) - z_pred.mean()).transpose(); + /** Estimate gaussian from sigma points using the unscented transform + * @param sigma_points Mat_n2ap1 Sigma points + * @tparam n_dims Dimension of the gaussian + * @return prob::MultiVarGauss + * @note This function is templated to allow for different dimensions of the + * gaussian + */ + template + static prob::MultiVarGauss estimate_gaussian( + const Eigen::Matrix& sigma_points) { + using Vec_n = Eigen::Vector; + using Mat_nn = Eigen::Matrix; + + // Predicted State Estimate x_k- + Vec_n mean = Vec_n::Zero(); + for (size_t i = 0; i < N_SIGMA_POINTS; i++) { + mean += W_x.at(i) * sigma_points.col(i); + } + Mat_nn cov = Mat_nn::Zero(); + for (size_t i = 0; i < N_SIGMA_POINTS; i++) { + cov += W_c.at(i) * (sigma_points.col(i) - mean) * + (sigma_points.col(i) - mean).transpose(); + } + return {mean, cov}; } - // Calculate Kalman gain - typename T::Mat_zz P_zz = z_pred.cov(); - typename T::Mat_xz K = P_xz * P_zz.llt().solve(T::Mat_zz::Identity()); + public: + /** Perform one UKF prediction step + * @param dyn_mod Dynamic model + * @param sens_mod Sensor model + * @param dt Time step + * @param x_est_prev Previous state estimate + * @param u T::Vec_u Control input (default 0) + * @return std::pair Predicted state estimate, + * predicted measurement estimate + */ + static std::pair predict( + const auto& dyn_mod, + const auto& sens_mod, + double dt, + const T::Gauss_x& x_est_prev, + const T::Vec_u& u = T::Vec_u::Zero()) + requires( + concepts:: + DynamicModel && + concepts:: + SensorModel) + { + Mat_a2ap1 sigma_points = + get_sigma_points(dyn_mod, sens_mod, dt, x_est_prev); + + // Propagate sigma points through f and h + Mat_x2ap1 sigma_x_pred = + propagate_sigma_points_f(dyn_mod, dt, sigma_points, u); + Mat_z2ap1 sigma_z_pred = + propagate_sigma_points_h(sens_mod, sigma_points); + + // Predicted State and Measurement Estimate x_k- and z_k- + typename T::Gauss_x x_pred = estimate_gaussian(sigma_x_pred); + typename T::Gauss_z z_pred = estimate_gaussian(sigma_z_pred); + + return {x_pred, z_pred}; + } - // Update state estimate - typename T::Vec_x x_upd_mean = x_pred.mean() + K * (z_meas - z_pred.mean()); - typename T::Mat_xx x_upd_cov = x_pred.cov() - K * P_zz * K.transpose(); - typename T::Gauss_x x_est_upd = {x_upd_mean, x_upd_cov}; + /** Perform one UKF update step + * @param dyn_mod Dynamic model + * @param sens_mod Sensor model + * @param x_est_pred Predicted state estimate + * @param z_est_pred Predicted measurement estimate + * @param z_meas Measurement + * @return T::Gauss_x Updated state estimate + * @note Sigma points are generated from the predicted state estimate + * instead of the previous state estimate as is done in the 'step' method. + */ + static T::Gauss_x update(const auto& dyn_mod, + const auto& sens_mod, + double dt, + const T::Gauss_x& x_est_pred, + const T::Gauss_z& z_est_pred, + const T::Vec_z& z_meas) + requires( + concepts:: + DynamicModel && + concepts:: + SensorModel) + { + // Generate sigma points from the predicted state estimate + Mat_a2ap1 sigma_points = + get_sigma_points(dyn_mod, sens_mod, dt, x_est_pred); + + // Extract the sigma points for the state + Mat_x2ap1 sigma_x_pred = + sigma_points.template block(0, 0); + + // Predict measurement + Mat_z2ap1 sigma_z_pred = + propagate_sigma_points_h(sens_mod, sigma_points); + + // Calculate cross-covariance + typename T::Mat_xz P_xz = T::Mat_xz::Zero(); + for (size_t i = 0; i < N_SIGMA_POINTS; i++) { + P_xz += W_c(i) * (sigma_x_pred.col(i) - x_est_pred.mean()) * + (sigma_z_pred.col(i) - z_est_pred.mean()).transpose(); + } + + // Calculate Kalman gain + typename T::Mat_zz P_zz = z_est_pred.cov(); + typename T::Mat_xz K = P_xz * P_zz.llt().solve(T::Mat_zz::Identity()); + + // Update state estimate + typename T::Vec_x x_upd_mean = + x_est_pred.mean() + K * (z_meas - z_est_pred.mean()); + typename T::Mat_xx x_upd_cov = + x_est_pred.cov() - K * P_zz * K.transpose(); + typename T::Gauss_x x_est_upd = {x_upd_mean, x_upd_cov}; + + return x_est_upd; + } - return {x_est_upd, x_pred, z_pred}; - } + /** Perform one UKF prediction and update step + * @param dyn_mod Dynamic model + * @param sens_mod Sensor model + * @param dt Time step + * @param x_est_prev Previous state estimate + * @param z_meas Measurement + * @param u T::Vec_u Control input + * @return std::tuple Updated state + * estimate, predicted state estimate, predicted measurement estimate + */ + static std:: + tuple + step(const auto& dyn_mod, + const auto& sens_mod, + double dt, + const T::Gauss_x& x_est_prev, + const T::Vec_z& z_meas, + const T::Vec_u& u = T::Vec_u::Zero()) + requires( + concepts:: + DynamicModel && + concepts:: + SensorModel) + { + Mat_a2ap1 sigma_points = + get_sigma_points(dyn_mod, sens_mod, dt, x_est_prev); + + // Propagate sigma points through f and h + Mat_x2ap1 sigma_x_pred = + propagate_sigma_points_f(dyn_mod, dt, sigma_points, u); + Mat_z2ap1 sigma_z_pred = + propagate_sigma_points_h(sens_mod, sigma_points); + + // Predicted State and Measurement Estimate x_k- and z_k- + typename T::Gauss_x x_pred = estimate_gaussian(sigma_x_pred); + typename T::Gauss_z z_pred = estimate_gaussian(sigma_z_pred); + + // Calculate cross-covariance + typename T::Mat_xz P_xz = T::Mat_xz::Zero(); + for (size_t i = 0; i < N_SIGMA_POINTS; i++) { + P_xz += W_c.at(i) * (sigma_x_pred.col(i) - x_pred.mean()) * + (sigma_z_pred.col(i) - z_pred.mean()).transpose(); + } + + // Calculate Kalman gain + typename T::Mat_zz P_zz = z_pred.cov(); + typename T::Mat_xz K = P_xz * P_zz.llt().solve(T::Mat_zz::Identity()); + + // Update state estimate + typename T::Vec_x x_upd_mean = + x_pred.mean() + K * (z_meas - z_pred.mean()); + typename T::Mat_xx x_upd_cov = x_pred.cov() - K * P_zz * K.transpose(); + typename T::Gauss_x x_est_upd = {x_upd_mean, x_upd_cov}; + + return {x_est_upd, x_pred, z_pred}; + } }; /** Unscented Kalman Filter - * @tparam DynModT Dynamic model type derived from `vortex::models::interface::DynamicModel` - * @tparam SensModT Sensor model type derived from `vortex::models::interface::SensorModel` + * @tparam DynModT Dynamic model type derived from + * `vortex::models::interface::DynamicModel` + * @tparam SensModT Sensor model type derived from + * `vortex::models::interface::SensorModel` * @tparam alpha Parameter for spread of sigma points (default 1.0) - * @tparam beta Parameter for weighting of mean in covariance calculation (default 2.0) - * @tparam kappa Parameter for adding additional spread to sigma points (default 0.0) + * @tparam beta Parameter for weighting of mean in covariance calculation + * (default 2.0) + * @tparam kappa Parameter for adding additional spread to sigma points (default + * 0.0) */ -template -using UKF = UKF_t; - -} // namespace filter -} // namespace vortex \ No newline at end of file +using UKF = UKF_t; + +} // namespace filter +} // namespace vortex diff --git a/vortex-filtering/include/vortex_filtering/models/README.md b/vortex-filtering/include/vortex_filtering/models/README.md index 5ce8ecc9..fcae7795 100644 --- a/vortex-filtering/include/vortex_filtering/models/README.md +++ b/vortex-filtering/include/vortex_filtering/models/README.md @@ -1,5 +1,5 @@ # Models -This folder contains the models used in the Vortex-VKF project. The models are derived from either DynamicModelI or the SensorModelI interface. +This folder contains the models used in the Vortex-VKF project. The models are derived from either DynamicModelI or the SensorModelI interface. They define the dynamics of the system and the sensor models used in the project. All classes and functions are under the namespace `vortex::models`. @@ -15,10 +15,10 @@ The interfaces that the different models are derived from are definged under the ## Dynamic Models -Models for describing the dynamics of the system. +Models for describing the dynamics of the system. ### Dynamic Model Interfaces -The dynamic model interfaces are virtual template classes that makes it convenient to define your own dynamic models. +The dynamic model interfaces are virtual template classes that makes it convenient to define your own dynamic models. #### DynamicModel @@ -33,7 +33,7 @@ Dynamic model interface for other classes to derive from. The [UKF](../filters/R #### DynamicModelLTV Dynamic model interface for other classes to derive from. The [EKF](../filters/README.md#ekf) (and UKF) works on models derived from this class. -This interface inherits from the `DynamicModel` interface and defines the dynamics of the system as a linear time varying system. The virtual method `f_d` from the `DynamicModel` interface is implemented as a linear time varying system on the form +This interface inherits from the `DynamicModel` interface and defines the dynamics of the system as a linear time varying system. The virtual method `f_d` from the `DynamicModel` interface is implemented as a linear time varying system on the form $$ f_d(dt, x_k, u_k, v_k) = x_{k+1} = A_d(dt, x_k) x_k + B_d(dt, x_k) u_k + G_d(dt, x_k)v_k @@ -72,7 +72,7 @@ This class implements the `DynamicModelLTV` interface and defines the dynamics o ### IMM Model __Interacting Multiple Models__ -This class can store multiple `DynamicModel` objects and defines functions to calculate the probability of switching between the models. +This class can store multiple `DynamicModel` objects and defines functions to calculate the probability of switching between the models. #### Usage To instantiate a **Interacting Multiple Models (IMM) object**, you must provide four parameters: @@ -119,25 +119,25 @@ But for custom models, you will have to define the state names yourself. */ // initialize IMM with the hold times, switching probabilities, dynamic models and state names -IMM imm_model(hold_times, switch_probs, - {CP(std_pos), cp_names}, - {CV(std_vel), cv_names}, +IMM imm_model(hold_times, switch_probs, + {CP(std_pos), cp_names}, + {CV(std_vel), cv_names}, {CT(std_vel, std_turn), ct_names}); // Enjoy your very own IMM model! :) ``` #### Theory -It's important to note that the actual probability of switching from one model to another is determined through the `hold_times` vector. By treating the system as a **Continuous Time Markov Chain (CTMC)**, as detailed on [Wikipedia](https://en.wikipedia.org/wiki/Continuous-time_Markov_chain), the model calculates the switching probabilities based on the specified hold times and the switching probabilities matrix. +It's important to note that the actual probability of switching from one model to another is determined through the `hold_times` vector. By treating the system as a **Continuous Time Markov Chain (CTMC)**, as detailed on [Wikipedia](https://en.wikipedia.org/wiki/Continuous-time_Markov_chain), the model calculates the switching probabilities based on the specified hold times and the switching probabilities matrix. ### Dynamic Models -`dynamic_models.hpp` +`dynamic_models.hpp` This file contains some movement models that are commonly used in an IMM. - `ConstantVelocity`: Has states for position and velocity. The template parameter `n_spatial_dims` specifies the number of spatial dimensions. So if the model is used in 2D, `n_spatial_dims` should be set to 2 and the model will have 4 states. `x`, `y`, `v_x` and `v_y`. -- `ConstantAcceleration`: Has states for position, velocity and acceleration. The template parameter `n_spatial_dims` specifies the number of spatial dimensions. So if the model is used in 2D, `n_spatial_dims` should be set to 2 and the model will have 6 states. `x`, `y`, `v_x`, `v_y`, `a_x` and `a_y`. -- `CoordinatedTurn`: Has states for 2D position, 2D velocity and turn rate. +- `ConstantAcceleration`: Has states for position, velocity and acceleration. The template parameter `n_spatial_dims` specifies the number of spatial dimensions. So if the model is used in 2D, `n_spatial_dims` should be set to 2 and the model will have 6 states. `x`, `y`, `v_x`, `v_y`, `a_x` and `a_y`. +- `CoordinatedTurn`: Has states for 2D position, 2D velocity and turn rate. ## Sensor Models @@ -150,5 +150,3 @@ This interface defines the sensor models. The methods `h` `H` and `R` define the In order to define a new sensor model, the user must create a new class that inherits from the SensorModelI interface. The user must then implement the methods `h`, `H` and `R` as they are pure virtual. The interface is a template class with parameter `N_DIM_x` and `N_DIM_z`. `N_DIM_x` is the dimension of the state vector and `N_DIM_z` is the dimension of the measurement vector. The user must specify these when creating a new class, or derive a template class from the SensorModelI interface. - - diff --git a/vortex-filtering/include/vortex_filtering/models/dynamic_model_interfaces.hpp b/vortex-filtering/include/vortex_filtering/models/dynamic_model_interfaces.hpp index 08856cba..cb0973c8 100644 --- a/vortex-filtering/include/vortex_filtering/models/dynamic_model_interfaces.hpp +++ b/vortex-filtering/include/vortex_filtering/models/dynamic_model_interfaces.hpp @@ -1,13 +1,14 @@ /** * @file dynamic_model.hpp * @author Eirik KolÃ¥s - * @brief Dynamic model interface. Based on "Fundamentals of Sensor Fusion" by Edmund Brekke + * @brief Dynamic model interface. Based on "Fundamentals of Sensor Fusion" by + * Edmund Brekke * @version 0.1 * @date 2023-10-26 */ #pragma once #include -#include // For exp +#include // For exp #include #include #include @@ -26,336 +27,365 @@ namespace interface { * @tparam n_dim_x State dimension * @tparam n_dim_u Input dimension * @tparam n_dim_v Process noise dimension - * @note To derive from this class, you must override the following virtual functions: + * @note To derive from this class, you must override the following virtual + * functions: * @note - f_d * @note - Q_d */ -template class DynamicModel { -public: - static constexpr int N_DIM_x = (int)n_dim_x; - static constexpr int N_DIM_u = (int)n_dim_u; - static constexpr int N_DIM_v = (int)n_dim_v; - - using T = Types_xuv; - - DynamicModel() = default; - virtual ~DynamicModel() = default; - - /** Discrete time dynamics - * @param dt Time step - * @param x T::Vec_x State - * @param u T::Vec_u Input - * @param v T::Vec_v Process noise - * @return T::Vec_x Next state - */ - virtual T::Vec_x f_d(double dt, const T::Vec_x &x, const T::Vec_u &u, const T::Vec_v &v) const = 0; - - /** Discrete time process noise covariance matrix - * @param dt Time step - * @param x T::Vec_x State - */ - virtual T::Mat_vv Q_d(double dt, const T::Vec_x &x) const = 0; - - /** Sample from the discrete time dynamics - * @param dt Time step - * @param x T::Vec_x State - * @param u T::Vec_u Input - * @param gen Random number generator (For deterministic behaviour) - * @return T::Vec_x Next state - */ - T::Vec_x sample_f_d(double dt, const T::Vec_x &x, const T::Vec_u &u, std::mt19937 &gen) const - { - typename T::Gauss_v v = {T::Vec_v::Zero(), Q_d(dt, x)}; - return f_d(dt, x, u, v.sample(gen)); - } +template +class DynamicModel { + public: + static constexpr int N_DIM_x = (int)n_dim_x; + static constexpr int N_DIM_u = (int)n_dim_u; + static constexpr int N_DIM_v = (int)n_dim_v; + + using T = Types_xuv; + + DynamicModel() = default; + virtual ~DynamicModel() = default; + + /** Discrete time dynamics + * @param dt Time step + * @param x T::Vec_x State + * @param u T::Vec_u Input + * @param v T::Vec_v Process noise + * @return T::Vec_x Next state + */ + virtual T::Vec_x f_d(double dt, + const T::Vec_x& x, + const T::Vec_u& u, + const T::Vec_v& v) const = 0; + + /** Discrete time process noise covariance matrix + * @param dt Time step + * @param x T::Vec_x State + */ + virtual T::Mat_vv Q_d(double dt, const T::Vec_x& x) const = 0; + + /** Sample from the discrete time dynamics + * @param dt Time step + * @param x T::Vec_x State + * @param u T::Vec_u Input + * @param gen Random number generator (For deterministic behaviour) + * @return T::Vec_x Next state + */ + T::Vec_x sample_f_d(double dt, + const T::Vec_x& x, + const T::Vec_u& u, + std::mt19937& gen) const { + typename T::Gauss_v v = {T::Vec_v::Zero(), Q_d(dt, x)}; + return f_d(dt, x, u, v.sample(gen)); + } }; -/** Linear Time Variant Dynamic Model Interface. [x_k+1 = f_d = A_k*x_k + B_k*u_k + G_k*v_k] +/** Linear Time Variant Dynamic Model Interface. [x_k+1 = f_d = A_k*x_k + + * B_k*u_k + G_k*v_k] * @tparam n_dim_x State dimension * @tparam n_dim_u Input dimension (Default: n_dim_x) * @tparam n_dim_v Process noise dimension (Default: n_dim_x) - * @note To derive from this class, you must override the following virtual functions: + * @note To derive from this class, you must override the following virtual + * functions: * @note - f_d (optional) * @note - A_d * @note - B_d (optional) * @note - Q_d * @note - G_d (optional if n_dim_x == n_dim_v) */ -template class DynamicModelLTV : public DynamicModel { -public: - static constexpr int N_DIM_x = n_dim_x; - static constexpr int N_DIM_u = n_dim_u; - static constexpr int N_DIM_v = n_dim_v; - - using T = Types_xuv; - - /** Linear Time Variant Dynamic Model Interface. [x_k+1 = f_d = A_k*x_k + B_k*u_k + G_k*v_k] - * @tparam n_dim_x State dimension - * @tparam n_dim_u Input dimension (Default: n_dim_x) - * @tparam n_dim_v Process noise dimension (Default: n_dim_x) - * @note To derive from this class, you must override the following virtual functions: - * @note - f_d (optional) - * @note - A_d - * @note - B_d (optional) - * @note - Q_d - * @note - G_d (optional if n_dim_x == n_dim_v) - */ - DynamicModelLTV() - : DynamicModel() - { - } - virtual ~DynamicModelLTV() = default; - - /** Discrete time dynamics - * @param dt Time step - * @param x Vec_x State - * @param u Vec_u Input - * @param v Vec_v Process noise - * @return Vec_x - */ - virtual T::Vec_x f_d(double dt, const T::Vec_x &x, const T::Vec_u &u = T::Vec_u::Zero(), const T::Vec_v &v = T::Vec_v::Zero()) const override - { - typename T::Mat_xx A_d = this->A_d(dt, x); - typename T::Mat_xu B_d = this->B_d(dt, x); - typename T::Mat_xv G_d = this->G_d(dt, x); - return A_d * x + B_d * u + G_d * v; - } - - /** System matrix (Jacobian of discrete time dynamics with respect to state) - * @param dt Time step - * @param x T::Vec_x - * @return State_jac - */ - virtual T::Mat_xx A_d(double dt, const T::Vec_x &x) const = 0; - - /** Input matrix - * @param dt Time step - * @param x T::Vec_x - * @return Input_jac - */ - virtual T::Mat_xu B_d(double dt, const T::Vec_x &x) const - { - (void)dt; // unused - (void)x; // unused - return T::Mat_xu::Zero(); - } - - /** Discrete time process noise covariance matrix - * @param dt Time step - * @param x T::Vec_x State - */ - virtual T::Mat_vv Q_d(double dt, const T::Vec_x &x) const override = 0; - - /** Discrete time process noise transition matrix - * @param dt Time step - * @param x T::Vec_x State - */ - virtual T::Mat_xv G_d(double, const T::Vec_x &) const - { - if (this->N_DIM_x != this->N_DIM_v) { - throw std::runtime_error("G_d not implemented"); +template +class DynamicModelLTV : public DynamicModel { + public: + static constexpr int N_DIM_x = n_dim_x; + static constexpr int N_DIM_u = n_dim_u; + static constexpr int N_DIM_v = n_dim_v; + + using T = Types_xuv; + + /** Linear Time Variant Dynamic Model Interface. [x_k+1 = f_d = A_k*x_k + + * B_k*u_k + G_k*v_k] + * @tparam n_dim_x State dimension + * @tparam n_dim_u Input dimension (Default: n_dim_x) + * @tparam n_dim_v Process noise dimension (Default: n_dim_x) + * @note To derive from this class, you must override the following virtual + * functions: + * @note - f_d (optional) + * @note - A_d + * @note - B_d (optional) + * @note - Q_d + * @note - G_d (optional if n_dim_x == n_dim_v) + */ + DynamicModelLTV() : DynamicModel() {} + virtual ~DynamicModelLTV() = default; + + /** Discrete time dynamics + * @param dt Time step + * @param x Vec_x State + * @param u Vec_u Input + * @param v Vec_v Process noise + * @return Vec_x + */ + virtual T::Vec_x f_d(double dt, + const T::Vec_x& x, + const T::Vec_u& u = T::Vec_u::Zero(), + const T::Vec_v& v = T::Vec_v::Zero()) const override { + typename T::Mat_xx A_d = this->A_d(dt, x); + typename T::Mat_xu B_d = this->B_d(dt, x); + typename T::Mat_xv G_d = this->G_d(dt, x); + return A_d * x + B_d * u + G_d * v; + } + + /** System matrix (Jacobian of discrete time dynamics with respect to state) + * @param dt Time step + * @param x T::Vec_x + * @return State_jac + */ + virtual T::Mat_xx A_d(double dt, const T::Vec_x& x) const = 0; + + /** Input matrix + * @param dt Time step + * @param x T::Vec_x + * @return Input_jac + */ + virtual T::Mat_xu B_d(double dt, const T::Vec_x& x) const { + (void)dt; // unused + (void)x; // unused + return T::Mat_xu::Zero(); + } + + /** Discrete time process noise covariance matrix + * @param dt Time step + * @param x T::Vec_x State + */ + virtual T::Mat_vv Q_d(double dt, const T::Vec_x& x) const override = 0; + + /** Discrete time process noise transition matrix + * @param dt Time step + * @param x T::Vec_x State + */ + virtual T::Mat_xv G_d(double, const T::Vec_x&) const { + if (this->N_DIM_x != this->N_DIM_v) { + throw std::runtime_error("G_d not implemented"); + } + return T::Mat_xv::Identity(); + } + + /** Get the predicted state distribution given a state estimate + * @param dt Time step + * @param x_est T::Vec_x estimate + * @return T::Vec_x + */ + auto pred_from_est(double dt, + const auto& x_est, + const T::Vec_u& u = T::Vec_u::Zero()) const + -> std::remove_reference_t + requires(concepts::MultiVarGaussLike) + { + typename T::Mat_xx P = x_est.cov(); + typename T::Mat_xx A_d = this->A_d(dt, x_est.mean()); + typename T::Mat_xx GQGT_d = this->GQGT_d(dt, x_est.mean()); + + return {f_d(dt, x_est.mean(), u), A_d * P * A_d.transpose() + GQGT_d}; + } + + /** Get the predicted state distribution given a state + * @param dt Time step + * @param x T::Vec_x + * @return T::Vec_x + */ + T::Gauss_x pred_from_state(double dt, + const T::Vec_x& x, + const T::Vec_u& u = T::Vec_u::Zero()) const { + typename T::Gauss_x x_est_pred = {f_d(dt, x, u), GQGT_d(dt, x)}; + return x_est_pred; + } + + protected: + /** Process noise covariance matrix. For expressing the process noise in the + * state space. + * @param dt Time step + * @param x T::Vec_x State + * @return Process_noise_jac + */ + virtual T::Mat_xx GQGT_d(double dt, const T::Vec_x& x) const { + typename T::Mat_vv Q_d = this->Q_d(dt, x); + typename T::Mat_xv G_d = this->G_d(dt, x); + + return G_d * Q_d * G_d.transpose(); } - return T::Mat_xv::Identity(); - } - - /** Get the predicted state distribution given a state estimate - * @param dt Time step - * @param x_est T::Vec_x estimate - * @return T::Vec_x - */ - auto pred_from_est(double dt, const auto &x_est, const T::Vec_u &u = T::Vec_u::Zero()) const -> std::remove_reference_t - requires(concepts::MultiVarGaussLike) - { - typename T::Mat_xx P = x_est.cov(); - typename T::Mat_xx A_d = this->A_d(dt, x_est.mean()); - typename T::Mat_xx GQGT_d = this->GQGT_d(dt, x_est.mean()); - - return {f_d(dt, x_est.mean(), u), A_d * P * A_d.transpose() + GQGT_d}; - } - - /** Get the predicted state distribution given a state - * @param dt Time step - * @param x T::Vec_x - * @return T::Vec_x - */ - T::Gauss_x pred_from_state(double dt, const T::Vec_x &x, const T::Vec_u &u = T::Vec_u::Zero()) const - { - typename T::Gauss_x x_est_pred = {f_d(dt, x, u), GQGT_d(dt, x)}; - return x_est_pred; - } - -protected: - /** Process noise covariance matrix. For expressing the process noise in the state space. - * @param dt Time step - * @param x T::Vec_x State - * @return Process_noise_jac - */ - virtual T::Mat_xx GQGT_d(double dt, const T::Vec_x &x) const - { - typename T::Mat_vv Q_d = this->Q_d(dt, x); - typename T::Mat_xv G_d = this->G_d(dt, x); - - return G_d * Q_d * G_d.transpose(); - } }; -/** Continuous Time Linear Time Varying Dynamic Model Interface. It uses exact discretization for everything. So it might be slow. - * [x_dot = A_c*x + B_c*u + G_c*v] +/** Continuous Time Linear Time Varying Dynamic Model Interface. It uses exact + * discretization for everything. So it might be slow. [x_dot = A_c*x + B_c*u + + * G_c*v] * @tparam n_dim_x State dimension * @tparam n_dim_u Input dimension (Default: n_dim_x) * @tparam n_dim_v Process noise dimension (Default: n_dim_x) - * @note See https://en.wikipedia.org/wiki/Discretization for more info on how the discretization is done. + * @note See https://en.wikipedia.org/wiki/Discretization for more info on how + * the discretization is done. * @note To derive from this class, you must override the following functions: * @note - A_c * @note - B_c (optional) * @note - Q_c * @note - G_c (optional if n_dim_x == n_dim_v) */ -template class DynamicModelCTLTV : public DynamicModelLTV { -public: - static constexpr int N_DIM_x = n_dim_x; - static constexpr int N_DIM_u = n_dim_u; - static constexpr int N_DIM_v = n_dim_v; - - using T = Types_xuv; - - /** Continuous Time Linear Time Varying Dynamic Model Interface. [x_dot = A_c*x + B_c*u + G_c*v] - * @tparam n_dim_x State dimension - * @tparam n_dim_u Input dimension (Default: n_dim_x) - * @tparam n_dim_v Process noise dimension (Default: n_dim_x) - */ - DynamicModelCTLTV() - : DynamicModelLTV() - { - } - virtual ~DynamicModelCTLTV() = default; - - /** Continuous time dynamics - * @param x T::Vec_x State - * @param u T::Vec_u Input - * @param v T::Vec_v Process noise - * @return T::Vec_x State_dot - */ - T::Vec_x f_c(const T::Vec_x &x, const T::Vec_u &u = T::Vec_u::Zero(), const T::Vec_v &v = T::Vec_v::Zero()) const - { - typename T::Mat_xx A_c = this->A_c(x); - typename T::Mat_xu B_c = this->B_c(x); - typename T::Mat_xv G_c = this->G_c(x); - return A_c * x + B_c * u + G_c * v; - } - - /** System matrix (Jacobian of continuous time dynamics with respect to state) - * @param x T::Vec_x State - * @return State_jac - */ - virtual T::Mat_xx A_c(const T::Vec_x &x) const = 0; - - /** Input matrix - * @param x T::Vec_x State - * @return Input_jac - */ - virtual T::Mat_xu B_c(const T::Vec_x &x) const - { - (void)x; // unused - return T::Mat_xu::Zero(); - } - - /** Process noise transition matrix. For expressing the process noise in the state space. - * @param x T::Vec_x State - * @return Process_noise_jac - */ - virtual T::Mat_xv G_c(const T::Vec_x &x) const - { - if (N_DIM_x != N_DIM_v) { - throw std::runtime_error("G_c not implemented"); +template +class DynamicModelCTLTV : public DynamicModelLTV { + public: + static constexpr int N_DIM_x = n_dim_x; + static constexpr int N_DIM_u = n_dim_u; + static constexpr int N_DIM_v = n_dim_v; + + using T = Types_xuv; + + /** Continuous Time Linear Time Varying Dynamic Model Interface. [x_dot = + * A_c*x + B_c*u + G_c*v] + * @tparam n_dim_x State dimension + * @tparam n_dim_u Input dimension (Default: n_dim_x) + * @tparam n_dim_v Process noise dimension (Default: n_dim_x) + */ + DynamicModelCTLTV() : DynamicModelLTV() {} + virtual ~DynamicModelCTLTV() = default; + + /** Continuous time dynamics + * @param x T::Vec_x State + * @param u T::Vec_u Input + * @param v T::Vec_v Process noise + * @return T::Vec_x State_dot + */ + T::Vec_x f_c(const T::Vec_x& x, + const T::Vec_u& u = T::Vec_u::Zero(), + const T::Vec_v& v = T::Vec_v::Zero()) const { + typename T::Mat_xx A_c = this->A_c(x); + typename T::Mat_xu B_c = this->B_c(x); + typename T::Mat_xv G_c = this->G_c(x); + return A_c * x + B_c * u + G_c * v; + } + + /** System matrix (Jacobian of continuous time dynamics with respect to + * state) + * @param x T::Vec_x State + * @return State_jac + */ + virtual T::Mat_xx A_c(const T::Vec_x& x) const = 0; + + /** Input matrix + * @param x T::Vec_x State + * @return Input_jac + */ + virtual T::Mat_xu B_c(const T::Vec_x& x) const { + (void)x; // unused + return T::Mat_xu::Zero(); + } + + /** Process noise transition matrix. For expressing the process noise in the + * state space. + * @param x T::Vec_x State + * @return Process_noise_jac + */ + virtual T::Mat_xv G_c(const T::Vec_x& x) const { + if (N_DIM_x != N_DIM_v) { + throw std::runtime_error("G_c not implemented"); + } + (void)x; // unused + return T::Mat_xv::Identity(); + } + + /** Process noise covariance matrix. This has the same dimension as the + * process noise. + * @param x T::Vec_x State + */ + virtual T::Mat_vv Q_c(const T::Vec_x& x) const = 0; + + /** System dynamics (Jacobian of discrete time dynamics w.r.t. state). Using + * exact discretization. + * @param dt Time step + * @param x T::Vec_x + * @return State_jac + */ + T::Mat_xx A_d(double dt, const T::Vec_x& x) const override { + return (A_c(x) * dt).exp(); + } + + /** Input matrix (Jacobian of discrete time dynamics w.r.t. input). Using + * exact discretization. + * @param dt Time step + * @param x T::Vec_x + * @return Input_jac + */ + T::Mat_xu B_d(double dt, const T::Vec_x& x) const override { + Eigen::Matrix van_loan; + van_loan << A_c(x), B_c(x), T::Mat_ux::Zero(), T::Mat_uu::Zero(); + van_loan *= dt; + van_loan = van_loan.exp(); + + // T::Mat_xx A_d = van_loan.template block(0, 0); + typename T::Mat_xu B_d = + van_loan.template block(0, N_DIM_x); + return B_d; + } + + T::Mat_xv G_d(double dt, const T::Vec_x& x) const override { + Eigen::Matrix van_loan; + van_loan.template topLeftCorner() = A_c(x); + van_loan.template topRightCorner() = G_c(x); + van_loan.template bottomLeftCorner() = + T::Mat_vx::Zero(); + van_loan.template bottomRightCorner() = + T::Mat_vv::Zero(); + + van_loan *= dt; + van_loan = van_loan.exp(); + + // T::Mat_xx A_d = van_loan.template block(0, 0); + typename T::Mat_xv G_d = + van_loan.template block(0, N_DIM_x); + return G_d; + } + + /** Discrete time process noise covariance matrix. This is super scuffed, + * but it works... (As long as G_d^T*G_d is invertible) Overriding + * DynamicModel::Q_d + * @param dt Time step + * @param x T::Vec_x + * @return Matrix Process noise covariance + */ + T::Mat_vv Q_d(double dt, const T::Vec_x& x) const override { + typename T::Mat_xx GQGT_d = this->GQGT_d(dt, x); + typename T::Mat_xv G_d = this->G_d(dt, x); + // pseudo inverse of G_d + typename T::Mat_vx G_d_pinv = + G_d.completeOrthogonalDecomposition().pseudoInverse(); + + return G_d_pinv * GQGT_d * G_d_pinv.transpose(); + } + + T::Mat_xx GQGT_d(double dt, const T::Vec_x& x) const override { + typename T::Mat_xx A_c = this->A_c(x); + typename T::Mat_vv Q_c = this->Q_c(x); + typename T::Mat_xv G_c = this->G_c(x); + + Eigen::Matrix van_loan; + van_loan.template topLeftCorner() = -A_c; + van_loan.template topRightCorner() = + G_c * Q_c * G_c.transpose(); + van_loan.template bottomLeftCorner() = + T::Mat_xx::Zero(); + van_loan.template bottomRightCorner() = + A_c.transpose(); + + van_loan *= dt; + van_loan = van_loan.exp(); + + typename T::Mat_xx A_d = + van_loan.template block(N_DIM_x, N_DIM_x) + .transpose(); + typename T::Mat_xx A_d_inv_GQGT_d = + van_loan.template block( + 0, N_DIM_x); // A_d^(-1) * G * Q * G^T + typename T::Mat_xx GQGT_d = A_d * A_d_inv_GQGT_d; // G * Q * G^T + return GQGT_d; } - (void)x; // unused - return T::Mat_xv::Identity(); - } - - /** Process noise covariance matrix. This has the same dimension as the process noise. - * @param x T::Vec_x State - */ - virtual T::Mat_vv Q_c(const T::Vec_x &x) const = 0; - - /** System dynamics (Jacobian of discrete time dynamics w.r.t. state). Using exact discretization. - * @param dt Time step - * @param x T::Vec_x - * @return State_jac - */ - T::Mat_xx A_d(double dt, const T::Vec_x &x) const override { return (A_c(x) * dt).exp(); } - - /** Input matrix (Jacobian of discrete time dynamics w.r.t. input). Using exact discretization. - * @param dt Time step - * @param x T::Vec_x - * @return Input_jac - */ - T::Mat_xu B_d(double dt, const T::Vec_x &x) const override - { - Eigen::Matrix van_loan; - van_loan << A_c(x), B_c(x), T::Mat_ux::Zero(), T::Mat_uu::Zero(); - van_loan *= dt; - van_loan = van_loan.exp(); - - // T::Mat_xx A_d = van_loan.template block(0, 0); - typename T::Mat_xu B_d = van_loan.template block(0, N_DIM_x); - return B_d; - } - - T::Mat_xv G_d(double dt, const T::Vec_x &x) const override - { - Eigen::Matrix van_loan; - van_loan.template topLeftCorner() = A_c(x); - van_loan.template topRightCorner() = G_c(x); - van_loan.template bottomLeftCorner() = T::Mat_vx::Zero(); - van_loan.template bottomRightCorner() = T::Mat_vv::Zero(); - - van_loan *= dt; - van_loan = van_loan.exp(); - - // T::Mat_xx A_d = van_loan.template block(0, 0); - typename T::Mat_xv G_d = van_loan.template block(0, N_DIM_x); - return G_d; - } - - /** Discrete time process noise covariance matrix. This is super scuffed, but it works... (As long as G_d^T*G_d is invertible) - * Overriding DynamicModel::Q_d - * @param dt Time step - * @param x T::Vec_x - * @return Matrix Process noise covariance - */ - T::Mat_vv Q_d(double dt, const T::Vec_x &x) const override - { - typename T::Mat_xx GQGT_d = this->GQGT_d(dt, x); - typename T::Mat_xv G_d = this->G_d(dt, x); - // psuedo inverse of G_d - typename T::Mat_vx G_d_pinv = G_d.completeOrthogonalDecomposition().pseudoInverse(); - - return G_d_pinv * GQGT_d * G_d_pinv.transpose(); - } - - T::Mat_xx GQGT_d(double dt, const T::Vec_x &x) const override - { - typename T::Mat_xx A_c = this->A_c(x); - typename T::Mat_vv Q_c = this->Q_c(x); - typename T::Mat_xv G_c = this->G_c(x); - - Eigen::Matrix van_loan; - van_loan.template topLeftCorner() = -A_c; - van_loan.template topRightCorner() = G_c * Q_c * G_c.transpose(); - van_loan.template bottomLeftCorner() = T::Mat_xx::Zero(); - van_loan.template bottomRightCorner() = A_c.transpose(); - - van_loan *= dt; - van_loan = van_loan.exp(); - - typename T::Mat_xx A_d = van_loan.template block(N_DIM_x, N_DIM_x).transpose(); - typename T::Mat_xx A_d_inv_GQGT_d = van_loan.template block(0, N_DIM_x); // A_d^(-1) * G * Q * G^T - typename T::Mat_xx GQGT_d = A_d * A_d_inv_GQGT_d; // G * Q * G^T - return GQGT_d; - } }; -} // namespace interface +} // namespace interface -} // namespace vortex::models \ No newline at end of file +} // namespace vortex::models diff --git a/vortex-filtering/include/vortex_filtering/models/dynamic_models.hpp b/vortex-filtering/include/vortex_filtering/models/dynamic_models.hpp index 099d651c..e3319d3b 100644 --- a/vortex-filtering/include/vortex_filtering/models/dynamic_models.hpp +++ b/vortex-filtering/include/vortex_filtering/models/dynamic_models.hpp @@ -1,405 +1,428 @@ #pragma once #include #include -#include #include +#include namespace vortex { namespace models { -constexpr int UNUSED = 1; // For when a template parameter is required but not used. +constexpr int UNUSED = + 1; // For when a template parameter is required but not used. /** Identity Dynamic Model * @tparam n_dim_x Number of dimensions in state vector */ -template class IdentityDynamicModel : public interface::DynamicModelLTV { - using Parent = interface::DynamicModelLTV; - -public: - static constexpr int N_DIM_x = Parent::N_DIM_x; - static constexpr int N_DIM_u = Parent::N_DIM_u; - static constexpr int N_DIM_v = Parent::N_DIM_v; - - using T = vortex::Types_xuv; - - /** Identity Dynamic Model - * @param std Standard deviation of process noise - */ - IdentityDynamicModel(double std) - : Q_(T::Mat_xx::Identity() * std * std) - { - } - - /** Identity Dynamic Model - * @param Q Process noise covariance - */ - IdentityDynamicModel(T::Mat_vv Q) - : Q_(Q) - { - } - - T::Mat_xx A_d(double dt, const T::Vec_x /*x*/ &) const override { return T::Mat_xx::Identity() * dt; } - T::Mat_vv Q_d(double /*dt*/, const T::Vec_x /*x*/ &) const override { return Q_; } - -private: - T::Mat_vv Q_; +template +class IdentityDynamicModel : public interface::DynamicModelLTV { + using Parent = interface::DynamicModelLTV; + + public: + static constexpr int N_DIM_x = Parent::N_DIM_x; + static constexpr int N_DIM_u = Parent::N_DIM_u; + static constexpr int N_DIM_v = Parent::N_DIM_v; + + using T = vortex::Types_xuv; + + /** Identity Dynamic Model + * @param std Standard deviation of process noise + */ + IdentityDynamicModel(double std) : Q_(T::Mat_xx::Identity() * std * std) {} + + /** Identity Dynamic Model + * @param Q Process noise covariance + */ + IdentityDynamicModel(T::Mat_vv Q) : Q_(Q) {} + + T::Mat_xx A_d(double dt, const T::Vec_x /*x*/&) const override { + return T::Mat_xx::Identity() * dt; + } + T::Mat_vv Q_d(double /*dt*/, const T::Vec_x /*x*/&) const override { + return Q_; + } + + private: + T::Mat_vv Q_; }; /** Constant Dynamic Model * @tparam n_dim_x Number of dimensions in state vector */ -template class ConstantDynamicModel : public interface::DynamicModelLTV { - using Parent = interface::DynamicModelLTV; - -public: - static constexpr int N_DIM_x = Parent::N_DIM_x; - static constexpr int N_DIM_u = Parent::N_DIM_u; - static constexpr int N_DIM_v = Parent::N_DIM_v; - - using T = vortex::Types_xuv; - - /** Identity Dynamic Model - * @param std_dev Standard deviation of process noise - */ - explicit ConstantDynamicModel(double std_dev) - : std_dev_(std_dev) {} - - /** Get the Jacobian of the continuous state transition model with respect to the state. - * @param dt Time step - * @param x State (unused) - * @return T::Mat_xx - * @note Overriding DynamicModelLTV::A_d - */ - T::Mat_xx A_d(double /*dt*/, const T::Vec_x /*x*/ & = T::Vec_x::Zero()) const override { return T::Mat_xx::Identity(); } - - /** Get the Jacobian of the continuous state transition model with respect to the process noise. - * @param dt Time step - * @param x State (unused) - * @return T::Mat_xv - * @note Overriding DynamicModelLTV::G_d - */ - T::Mat_xv G_d(double dt, const T::Vec_x /*x*/ & = T::Vec_x::Zero()) const override - { - return 0.5 * dt * T::Mat_xx::Identity(); - } - - /** Get the continuous time process noise covariance matrix. - * @param dt Time step (unused) - * @param x State (unused) - * @return T::Mat_xx Process noise covariance - * @note Overriding DynamicModelLTV::Q_d - */ - T::Mat_vv Q_d(double /*dt*/ = 0.0, const T::Vec_x /*x*/ & = T::Vec_x::Zero()) const override { return T::Mat_vv::Identity() * std_dev_ * std_dev_; } - -private: - double std_dev_; +template +class ConstantDynamicModel : public interface::DynamicModelLTV { + using Parent = interface::DynamicModelLTV; + + public: + static constexpr int N_DIM_x = Parent::N_DIM_x; + static constexpr int N_DIM_u = Parent::N_DIM_u; + static constexpr int N_DIM_v = Parent::N_DIM_v; + + using T = vortex::Types_xuv; + + /** Identity Dynamic Model + * @param std_dev Standard deviation of process noise + */ + explicit ConstantDynamicModel(double std_dev) : std_dev_(std_dev) {} + + /** Get the Jacobian of the continuous state transition model with respect + * to the state. + * @param dt Time step + * @param x State (unused) + * @return T::Mat_xx + * @note Overriding DynamicModelLTV::A_d + */ + T::Mat_xx A_d(double /*dt*/, + const T::Vec_x /*x*/& = T::Vec_x::Zero()) const override { + return T::Mat_xx::Identity(); + } + + /** Get the Jacobian of the continuous state transition model with respect + * to the process noise. + * @param dt Time step + * @param x State (unused) + * @return T::Mat_xv + * @note Overriding DynamicModelLTV::G_d + */ + T::Mat_xv G_d(double dt, + const T::Vec_x /*x*/& = T::Vec_x::Zero()) const override { + return 0.5 * dt * T::Mat_xx::Identity(); + } + + /** Get the continuous time process noise covariance matrix. + * @param dt Time step (unused) + * @param x State (unused) + * @return T::Mat_xx Process noise covariance + * @note Overriding DynamicModelLTV::Q_d + */ + T::Mat_vv Q_d(double /*dt*/ = 0.0, + const T::Vec_x /*x*/& = T::Vec_x::Zero()) const override { + return T::Mat_vv::Identity() * std_dev_ * std_dev_; + } + + private: + double std_dev_; }; /** (Nearly) Constant Position Model * State x = [position, position] */ class ConstantPosition : public interface::DynamicModelLTV<2, UNUSED, 2> { - using Parent = interface::DynamicModelLTV<2, UNUSED, 2>; - -public: - static constexpr int N_DIM_x = Parent::N_DIM_x; - static constexpr int N_DIM_u = Parent::N_DIM_u; - static constexpr int N_DIM_v = Parent::N_DIM_v; - - using T = Types_xuv; - - using S = StateName; - using StateT = State; - - /** Constant Position Model in 2D - * x = [x, y] - * @param std_pos Standard deviation of position - */ - ConstantPosition(double std_pos) - : std_pos_(std_pos) - { - } - - /** Get the Jacobian of the continuous state transition model with respect to the state. - * @param dt Time step - * @param x State (unused) - * @return T::Mat_xx - * @note Overriding DynamicModelLTV::A_d - */ - T::Mat_xx A_d(double /*dt*/, const T::Vec_x /*x*/ & = T::Vec_x::Zero()) const override { return T::Mat_xx::Identity(); } - - /** Get the Jacobian of the continuous state transition model with respect to the process noise. - * @param dt Time step - * @param x State (unused) - * @return T::Mat_xv - * @note Overriding DynamicModelLTV::G_d - */ - T::Mat_xv G_d(double dt, const T::Vec_x /*x*/ & = T::Vec_x::Zero()) const override - { - T::Mat_xx I = T::Mat_xx::Identity(); - return 0.5 * dt * I; - } - - /** Get the continuous time process noise covariance matrix. - * @param dt Time step (unused) - * @param x State (unused) - * @return T::Mat_xx Process noise covariance - * @note Overriding DynamicModelLTV::Q_d - */ - T::Mat_vv Q_d(double /*dt*/ = 0.0, const T::Vec_x /*x*/ & = T::Vec_x::Zero()) const override { return T::Mat_vv::Identity() * std_pos_ * std_pos_; } - -private: - double std_pos_; + using Parent = interface::DynamicModelLTV<2, UNUSED, 2>; + + public: + static constexpr int N_DIM_x = Parent::N_DIM_x; + static constexpr int N_DIM_u = Parent::N_DIM_u; + static constexpr int N_DIM_v = Parent::N_DIM_v; + + using T = Types_xuv; + + using S = StateName; + using StateT = State; + + /** Constant Position Model in 2D + * x = [x, y] + * @param std_pos Standard deviation of position + */ + ConstantPosition(double std_pos) : std_pos_(std_pos) {} + + /** Get the Jacobian of the continuous state transition model with respect + * to the state. + * @param dt Time step + * @param x State (unused) + * @return T::Mat_xx + * @note Overriding DynamicModelLTV::A_d + */ + T::Mat_xx A_d(double /*dt*/, + const T::Vec_x /*x*/& = T::Vec_x::Zero()) const override { + return T::Mat_xx::Identity(); + } + + /** Get the Jacobian of the continuous state transition model with respect + * to the process noise. + * @param dt Time step + * @param x State (unused) + * @return T::Mat_xv + * @note Overriding DynamicModelLTV::G_d + */ + T::Mat_xv G_d(double dt, + const T::Vec_x /*x*/& = T::Vec_x::Zero()) const override { + T::Mat_xx I = T::Mat_xx::Identity(); + return 0.5 * dt * I; + } + + /** Get the continuous time process noise covariance matrix. + * @param dt Time step (unused) + * @param x State (unused) + * @return T::Mat_xx Process noise covariance + * @note Overriding DynamicModelLTV::Q_d + */ + T::Mat_vv Q_d(double /*dt*/ = 0.0, + const T::Vec_x /*x*/& = T::Vec_x::Zero()) const override { + return T::Mat_vv::Identity() * std_pos_ * std_pos_; + } + + private: + double std_pos_; }; /** (Nearly) Constant Pose model. * State x = [position, position, position, orientaiton] */ class ConstantPose : public interface::DynamicModelLTV<4, UNUSED, 4> { - using Parent = interface::DynamicModelLTV<4, UNUSED, 4>; + using Parent = interface::DynamicModelLTV<4, UNUSED, 4>; -public: - static constexpr int N_DIM_x = Parent::N_DIM_x; - static constexpr int N_DIM_u = Parent::N_DIM_u; - static constexpr int N_DIM_v = Parent::N_DIM_v; + public: + static constexpr int N_DIM_x = Parent::N_DIM_x; + static constexpr int N_DIM_u = Parent::N_DIM_u; + static constexpr int N_DIM_v = Parent::N_DIM_v; - using T = Types_xuv; + using T = Types_xuv; - /** Constant Pose Model - * x = [x, y, z, orientation] - * @param std_pos Standard deviation of position - * @param std_orient Standard deviation of orientation - */ + /** Constant Pose Model + * x = [x, y, z, orientation] + * @param std_pos Standard deviation of position + * @param std_orient Standard deviation of orientation + */ ConstantPose(double std_pos, double std_orient) - : std_pos_(std_pos), std_orient_(std_orient) - { - } - - /** Get the Jacobian of the state transition model with respect to the state. - * @param dt Time step (unused) - * @param x State (unused) - * @return T::Mat_xx Identity matrix representing no change to the state. - */ - T::Mat_xx A_d(double /*dt*/, const T::Vec_x& /*x*/ = T::Vec_x::Zero()) const override { - return T::Mat_xx::Identity(); - } - - /** Get the Jacobian of the state transition model with respect to the process noise. - * @param dt Time step - * @param x State (unused) - * @return T::Mat_xv Identity matrix scaled by dt. - */ - T::Mat_xv G_d(double dt, const T::Vec_x& /*x*/ = T::Vec_x::Zero()) const override { - // For a constant pose model, we assume that the noise affects the position and orientation directly. - return T::Mat_xv::Identity() * dt; - } - - /** Get the discrete-time process noise covariance matrix. - * @param dt Time step (unused) - * @param x State (unused) - * @return T::Mat_vv Process noise covariance matrix. - */ - T::Mat_vv Q_d(double /*dt*/, const T::Vec_x& /*x*/ = T::Vec_x::Zero()) const override { - T::Mat_vv Q = T::Mat_vv::Zero(); - Q.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity() * std_pos_ * std_pos_; - Q(3, 3) = std_orient_ * std_orient_; - return Q; - } - -private: - double std_pos_; - double std_orient_; + : std_pos_(std_pos), std_orient_(std_orient) {} + + /** Get the Jacobian of the state transition model with respect to the + * state. + * @param dt Time step (unused) + * @param x State (unused) + * @return T::Mat_xx Identity matrix representing no change to the state. + */ + T::Mat_xx A_d(double /*dt*/, + const T::Vec_x& /*x*/ = T::Vec_x::Zero()) const override { + return T::Mat_xx::Identity(); + } + + /** Get the Jacobian of the state transition model with respect to the + * process noise. + * @param dt Time step + * @param x State (unused) + * @return T::Mat_xv Identity matrix scaled by dt. + */ + T::Mat_xv G_d(double dt, + const T::Vec_x& /*x*/ = T::Vec_x::Zero()) const override { + // For a constant pose model, we assume that the noise affects the + // position and orientation directly. + return T::Mat_xv::Identity() * dt; + } + + /** Get the discrete-time process noise covariance matrix. + * @param dt Time step (unused) + * @param x State (unused) + * @return T::Mat_vv Process noise covariance matrix. + */ + T::Mat_vv Q_d(double /*dt*/, + const T::Vec_x& /*x*/ = T::Vec_x::Zero()) const override { + T::Mat_vv Q = T::Mat_vv::Zero(); + Q.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity() * std_pos_ * std_pos_; + Q(3, 3) = std_orient_ * std_orient_; + return Q; + } + + private: + double std_pos_; + double std_orient_; }; /** (Nearly) Constant Velocity Model. * State x = [position, position, velocity, velocity] */ class ConstantVelocity : public interface::DynamicModelLTV<4, UNUSED, 2> { - using Parent = interface::DynamicModelLTV<4, UNUSED, 2>; - -public: - static constexpr int N_DIM_x = Parent::N_DIM_x; - static constexpr int N_DIM_u = Parent::N_DIM_u; - static constexpr int N_DIM_v = Parent::N_DIM_v; - - static constexpr int N_SPATIAL_DIM = 2; - static constexpr int N_STATES = 2 * N_SPATIAL_DIM; - - using T = vortex::Types_xuv; - - using S = StateName; - using StateT = State; - - - using Vec_s = Eigen::Matrix; - using Mat_ss = Eigen::Matrix; - - - /** - * @brief Constant Velocity Model in 2D - * x = [x, y, x_dot, y_dot] - * @param std_vel Standard deviation of velocity - */ - ConstantVelocity(double std_vel) - : std_vel_(std_vel) - { - } - - /** Get the Jacobian of the continuous state transition model with respect to the state. - * @param dt Time step - * @param x State (unused) - * @return T::Mat_xx - * @note Overriding DynamicModelLTV::A_d - */ - T::Mat_xx A_d(double dt, const T::Vec_x /*x*/ & = T::Vec_x::Zero()) const override - { - Mat_ss I = Mat_ss::Identity(); - Mat_ss O = Mat_ss::Zero(); - T::Mat_xx A; - // clang-format off + using Parent = interface::DynamicModelLTV<4, UNUSED, 2>; + + public: + static constexpr int N_DIM_x = Parent::N_DIM_x; + static constexpr int N_DIM_u = Parent::N_DIM_u; + static constexpr int N_DIM_v = Parent::N_DIM_v; + + static constexpr int N_SPATIAL_DIM = 2; + static constexpr int N_STATES = 2 * N_SPATIAL_DIM; + + using T = vortex::Types_xuv; + + using S = StateName; + using StateT = State; + + using Vec_s = Eigen::Matrix; + using Mat_ss = Eigen::Matrix; + + /** + * @brief Constant Velocity Model in 2D + * x = [x, y, x_dot, y_dot] + * @param std_vel Standard deviation of velocity + */ + ConstantVelocity(double std_vel) : std_vel_(std_vel) {} + + /** Get the Jacobian of the continuous state transition model with respect + * to the state. + * @param dt Time step + * @param x State (unused) + * @return T::Mat_xx + * @note Overriding DynamicModelLTV::A_d + */ + T::Mat_xx A_d(double dt, + const T::Vec_x /*x*/& = T::Vec_x::Zero()) const override { + Mat_ss I = Mat_ss::Identity(); + Mat_ss O = Mat_ss::Zero(); + T::Mat_xx A; + // clang-format off A << I, I*dt, O, I; - // clang-format on - return A; - } - - /** Get the Jacobian of the continuous state transition model with respect to the process noise. - * @param dt Time step - * @param x State (unused) - * @return T::Mat_xv - * @note Overriding DynamicModelLTV::G_d - */ - T::Mat_xv G_d(double dt, const T::Vec_x /*x*/ & = T::Vec_x::Zero()) const override - { - Mat_ss I = Mat_ss::Identity(); - T::Mat_xv G; - // clang-format off + // clang-format on + return A; + } + + /** Get the Jacobian of the continuous state transition model with respect + * to the process noise. + * @param dt Time step + * @param x State (unused) + * @return T::Mat_xv + * @note Overriding DynamicModelLTV::G_d + */ + T::Mat_xv G_d(double dt, + const T::Vec_x /*x*/& = T::Vec_x::Zero()) const override { + Mat_ss I = Mat_ss::Identity(); + T::Mat_xv G; + // clang-format off G << 0.5*dt*dt*I, dt*I; - // clang-format on - return G; - } - - /** Get the continuous time process noise covariance matrix. - * @param dt Time step (unused) - * @param x State (unused) - * @return T::Mat_xx Process noise covariance - * @note Overriding DynamicModelLTV::Q_d - */ - T::Mat_vv Q_d(double /*dt*/ = 0.0, const T::Vec_x /*x*/ & = T::Vec_x::Zero()) const override { return T::Mat_vv::Identity() * std_vel_ * std_vel_; } - -private: - double std_vel_; + // clang-format on + return G; + } + + /** Get the continuous time process noise covariance matrix. + * @param dt Time step (unused) + * @param x State (unused) + * @return T::Mat_xx Process noise covariance + * @note Overriding DynamicModelLTV::Q_d + */ + T::Mat_vv Q_d(double /*dt*/ = 0.0, + const T::Vec_x /*x*/& = T::Vec_x::Zero()) const override { + return T::Mat_vv::Identity() * std_vel_ * std_vel_; + } + + private: + double std_vel_; }; /** (Nearly) Constant Acceleration Model. - * State vector x = [position, position, velocity, velocity, acceleration, acceleration] + * State vector x = [position, position, velocity, velocity, acceleration, + * acceleration] */ -class ConstantAcceleration : public interface::DynamicModelLTV<3 * 2, UNUSED, 2 * 2> { -public: - static constexpr int N_SPATIAL_DIM = 2; - static constexpr int N_STATES = 3 * N_SPATIAL_DIM; - - using T = vortex::Types_xv; - - using S = StateName; - using StateT = State; - - using Vec_s = Eigen::Matrix; - using Mat_ss = Eigen::Matrix; - - - /** Constant Acceleration Model - * @param std_vel Standard deviation of velocity - * @param std_acc Standard deviation of acceleration - */ - ConstantAcceleration(double std_vel, double std_acc) - : std_vel_(std_vel) - , std_acc_(std_acc) - { - } - - /** Get the Jacobian of the continuous state transition model with respect to the state. - * @param x State - * @return T::Mat_xx - * @note Overriding DynamicModelLTV::A_d - */ - T::Mat_xx A_d(double dt, const T::Vec_x /*x*/ &) const override - { - Mat_ss I = Mat_ss::Identity(); - Mat_ss O = Mat_ss::Zero(); - T::Mat_xx A; - // clang-format off +class ConstantAcceleration + : public interface::DynamicModelLTV<3 * 2, UNUSED, 2 * 2> { + public: + static constexpr int N_SPATIAL_DIM = 2; + static constexpr int N_STATES = 3 * N_SPATIAL_DIM; + + using T = vortex::Types_xv; + + using S = StateName; + using StateT = State; + + using Vec_s = Eigen::Matrix; + using Mat_ss = Eigen::Matrix; + + /** Constant Acceleration Model + * @param std_vel Standard deviation of velocity + * @param std_acc Standard deviation of acceleration + */ + ConstantAcceleration(double std_vel, double std_acc) + : std_vel_(std_vel), std_acc_(std_acc) {} + + /** Get the Jacobian of the continuous state transition model with respect + * to the state. + * @param x State + * @return T::Mat_xx + * @note Overriding DynamicModelLTV::A_d + */ + T::Mat_xx A_d(double dt, const T::Vec_x /*x*/&) const override { + Mat_ss I = Mat_ss::Identity(); + Mat_ss O = Mat_ss::Zero(); + T::Mat_xx A; + // clang-format off A << I, I*dt, I*0.5*dt*dt, O, I , I*dt , O, O , I ; - // clang-format on - return A; - } - - T::Mat_xv G_d(double dt, const T::Vec_x /*x*/ & = T::Vec_x::Zero()) const override - { - Mat_ss I = Mat_ss::Identity(); - Mat_ss O = Mat_ss::Zero(); - T::Mat_xv G; - // clang-format off + // clang-format on + return A; + } + + T::Mat_xv G_d(double dt, + const T::Vec_x /*x*/& = T::Vec_x::Zero()) const override { + Mat_ss I = Mat_ss::Identity(); + Mat_ss O = Mat_ss::Zero(); + T::Mat_xv G; + // clang-format off G << I*dt, I*0.5*dt*dt, I , I*dt , O , I ; - // clang-format on - return G; - } - - /** Get the continuous time process noise covariance matrix. - * @param dt Time step (unused) - * @param x State - * @return T::Mat_xx Process noise covariance - * @note Overriding DynamicModelLTV::Q_d - */ - T::Mat_vv Q_d(double /*dt*/ = 0.0, const T::Vec_x /*x*/ & = T::Vec_x::Zero()) const override - { - T::Vec_v D; - double var_vel = std_vel_ * std_vel_; - double var_acc = std_acc_ * std_acc_; - D << var_vel, var_vel, var_acc, var_acc; - return D.asDiagonal(); - } - -private: - double std_vel_; - double std_acc_; + // clang-format on + return G; + } + + /** Get the continuous time process noise covariance matrix. + * @param dt Time step (unused) + * @param x State + * @return T::Mat_xx Process noise covariance + * @note Overriding DynamicModelLTV::Q_d + */ + T::Mat_vv Q_d(double /*dt*/ = 0.0, + const T::Vec_x /*x*/& = T::Vec_x::Zero()) const override { + T::Vec_v D; + double var_vel = std_vel_ * std_vel_; + double var_acc = std_acc_ * std_acc_; + D << var_vel, var_vel, var_acc, var_acc; + return D.asDiagonal(); + } + + private: + double std_vel_; + double std_acc_; }; /** Coordinated Turn Model in 2D. * x = [position, position, velocity, velocity, turn_rate] */ class CoordinatedTurn : public interface::DynamicModelCTLTV<5, UNUSED, 3> { -public: - static constexpr int N_STATES = 5; - using T = vortex::Types_xv; - - using S = StateName; - using StateT = State; - - /** (Nearly) Coordinated Turn Model in 2D. (Nearly constant speed, nearly constant turn rate) - * State = [x, y, x_dot, y_dot, omega] - * @param std_vel Standard deviation of velocity - * @param std_turn Standard deviation of turn rate - */ - CoordinatedTurn(double std_vel, double std_turn) - : std_vel_(std_vel) - , std_turn_(std_turn) - { - } - - /** Get the Jacobian of the continuous state transition model with respect to the state. - * @param x State - * @return T::Mat_xx - * @note Overriding DynamicModelCTLTV::A_c - */ - T::Mat_xx A_c(const T::Vec_x /*x*/ &x) const override - { - // clang-format off + public: + static constexpr int N_STATES = 5; + using T = vortex::Types_xv; + + using S = StateName; + using StateT = State; + + /** (Nearly) Coordinated Turn Model in 2D. (Nearly constant speed, nearly + * constant turn rate) State = [x, y, x_dot, y_dot, omega] + * @param std_vel Standard deviation of velocity + * @param std_turn Standard deviation of turn rate + */ + CoordinatedTurn(double std_vel, double std_turn) + : std_vel_(std_vel), std_turn_(std_turn) {} + + /** Get the Jacobian of the continuous state transition model with respect + * to the state. + * @param x State + * @return T::Mat_xx + * @note Overriding DynamicModelCTLTV::A_c + */ + T::Mat_xx A_c(const T::Vec_x /*x*/& x) const override { + // clang-format off return T::Mat_xx{ {0, 0, 1 , 0 , 0}, {0, 0, 0 , 1 , 0}, @@ -407,17 +430,16 @@ class CoordinatedTurn : public interface::DynamicModelCTLTV<5, UNUSED, 3> { {0, 0, x(4), 0 , 0}, {0, 0, 0 , 0 , 0} }; - // clang-format on - } - - /** Get the continuous time process noise matrix - * @param x State (unused) - * return T::Mat_xv Process noise matrix - * @note Overriding DynamicModelCTLTV::G_c - */ - T::Mat_xv G_c(const T::Vec_x /*x*/ & = T::Vec_x::Zero()) const override - { - // clang-format off + // clang-format on + } + + /** Get the continuous time process noise matrix + * @param x State (unused) + * return T::Mat_xv Process noise matrix + * @note Overriding DynamicModelCTLTV::G_c + */ + T::Mat_xv G_c(const T::Vec_x /*x*/& = T::Vec_x::Zero()) const override { + // clang-format off return T::Mat_xv { {0, 0, 0}, {0, 0, 0}, @@ -425,26 +447,25 @@ class CoordinatedTurn : public interface::DynamicModelCTLTV<5, UNUSED, 3> { {0, 1, 0}, {0, 0, 1} }; - // clang-format on - } - - /** Get the continuous time process noise covariance matrix. - * @param x State - * @return T::Mat_xx Process noise covariance - * @note Overriding DynamicModelCTLTV::Q_c - */ - T::Mat_vv Q_c(const T::Vec_x /*x*/ & = T::Vec_x::Zero()) const override - { - double var_vel = std_vel_ * std_vel_; - double var_turn = std_turn_ * std_turn_; - - return T::Vec_v{var_vel, var_vel, var_turn}.asDiagonal(); - } - -private: - double std_vel_; - double std_turn_; + // clang-format on + } + + /** Get the continuous time process noise covariance matrix. + * @param x State + * @return T::Mat_xx Process noise covariance + * @note Overriding DynamicModelCTLTV::Q_c + */ + T::Mat_vv Q_c(const T::Vec_x /*x*/& = T::Vec_x::Zero()) const override { + double var_vel = std_vel_ * std_vel_; + double var_turn = std_turn_ * std_turn_; + + return T::Vec_v{var_vel, var_vel, var_turn}.asDiagonal(); + } + + private: + double std_vel_; + double std_turn_; }; -} // namespace models -} // namespace vortex \ No newline at end of file +} // namespace models +} // namespace vortex diff --git a/vortex-filtering/include/vortex_filtering/models/imm_model.hpp b/vortex-filtering/include/vortex_filtering/models/imm_model.hpp index 22d1aea1..7ecb231a 100644 --- a/vortex-filtering/include/vortex_filtering/models/imm_model.hpp +++ b/vortex-filtering/include/vortex_filtering/models/imm_model.hpp @@ -22,263 +22,308 @@ namespace vortex::models { -template constexpr std::array matching_state_names(const std::array &array1, const std::array &array2) -{ - std::array matches{}; - for (std::size_t i = 0; i < N; ++i) { - matches[i] = i < N && i < M && array1.at(i) == array2.at(i); - } - return matches; +template +constexpr std::array matching_state_names( + const std::array& array1, + const std::array& array2) { + std::array matches{}; + for (std::size_t i = 0; i < N; ++i) { + matches[i] = i < N && i < M && array1.at(i) == array2.at(i); + } + return matches; } - - /** * @brief Container class for interacting multiple models. * @tparam DynModels Dynamic models to use. */ -template class ImmModel { -public: - static constexpr std::array N_DIMS_x = {DynModels::N_DIM_x...}; - static constexpr std::array N_DIMS_u = {DynModels::N_DIM_u...}; - static constexpr std::array N_DIMS_v = {DynModels::N_DIM_v...}; - - static constexpr bool SAME_DIMS_x = (DynModels::N_DIM_x == ...); - static constexpr bool MIN_DIM_x = std::min(N_DIMS_x); - static constexpr size_t N_MODELS = sizeof...(DynModels); - - using StateNames = std::tuple...>; - - static constexpr StateNames ALL_STATE_NAMES = {{DynModels::StateT::STATE_NAMES}...}; - - using DynModTuple = std::tuple; - using GaussTuple_x = std::tuple::Gauss_x...>; - using Vec_n = Eigen::Vector; - using Mat_nn = Eigen::Matrix; - - template using DynModT = typename std::tuple_element::type; - - template using State = DynModT::State; - - template using T = Types_xuv; - - /** - * @brief Construct a new ImmModel object - * @tparam DynModels Dynamic models to use. - * @param jump_matrix Markov jump chain matrix for the transition probabilities. - * I.e. the probability of switching from model i to model j is `jump_matrix(i,j)`. Diagonal should be 0. - * @param hold_times Expected holding time in seconds for each state. Parameter is the mean of an exponential distribution. - * @param models Tuple of dynamic models - * @param state_names Tuple of std::array of state names for each model - * @note - The jump matrix specifies the probability of switching to a model WHEN a switch occurs. - * @note - The holding times specifies HOW LONG a state is expected to be held between switches. - * @note - In order to change the properties of a model, you must get the model using `get_model()` - */ - ImmModel(Mat_nn jump_matrix, Vec_n hold_times, DynModels... models) - : models_(models...) - , jump_matrix_(jump_matrix) - , hold_times_(hold_times) - { - if (!jump_matrix_.diagonal().isZero()) { - throw std::invalid_argument("Jump matrix diagonal should be zero"); +template +class ImmModel { + public: + static constexpr std::array N_DIMS_x = {DynModels::N_DIM_x...}; + static constexpr std::array N_DIMS_u = {DynModels::N_DIM_u...}; + static constexpr std::array N_DIMS_v = {DynModels::N_DIM_v...}; + + static constexpr bool SAME_DIMS_x = (DynModels::N_DIM_x == ...); + static constexpr bool MIN_DIM_x = std::min(N_DIMS_x); + static constexpr size_t N_MODELS = sizeof...(DynModels); + + using StateNames = std::tuple...>; + + static constexpr StateNames ALL_STATE_NAMES = { + {DynModels::StateT::STATE_NAMES}...}; + + using DynModTuple = std::tuple; + using GaussTuple_x = + std::tuple::Gauss_x...>; + using Vec_n = Eigen::Vector; + using Mat_nn = Eigen::Matrix; + + template + using DynModT = typename std::tuple_element::type; + + template + using State = DynModT::State; + + template + using T = Types_xuv; + + /** + * @brief Construct a new ImmModel object + * @tparam DynModels Dynamic models to use. + * @param jump_matrix Markov jump chain matrix for the transition + * probabilities. I.e. the probability of switching from model i to model j + * is `jump_matrix(i,j)`. Diagonal should be 0. + * @param hold_times Expected holding time in seconds for each state. + * Parameter is the mean of an exponential distribution. + * @param models Tuple of dynamic models + * @param state_names Tuple of std::array of state names for each model + * @note - The jump matrix specifies the probability of switching to a model + * WHEN a switch occurs. + * @note - The holding times specifies HOW LONG a state is expected to be + * held between switches. + * @note - In order to change the properties of a model, you must get the + * model using `get_model()` + */ + ImmModel(Mat_nn jump_matrix, Vec_n hold_times, DynModels... models) + : models_(models...), + jump_matrix_(jump_matrix), + hold_times_(hold_times) { + if (!jump_matrix_.diagonal().isZero()) { + throw std::invalid_argument("Jump matrix diagonal should be zero"); + } + + // Check that the jump matrix is valid + for (int i = 0; i < jump_matrix_.rows(); i++) { + if (jump_matrix_.row(i).sum() != 1.0) { + throw std::invalid_argument("Jump matrix row " + + std::to_string(i) + + " should sum to 1"); + } + } + } + + /** + * @brief Compute the continuous-time transition matrix + * @return Matrix Continuous-time transition matrix + * @note See https://en.wikipedia.org/wiki/Continuous-time_Markov_chain. + */ + Mat_nn get_pi_mat_c() const { + Vec_n hold_rates = hold_times_.cwiseInverse(); + Mat_nn pi_mat_c = hold_rates.replicate(1, N_MODELS); + + // Multiply the jump matrix with the hold times + pi_mat_c = pi_mat_c.cwiseProduct(jump_matrix_); + + // Each row should sum to zero + pi_mat_c -= hold_rates.asDiagonal(); + + return pi_mat_c; + } + + /** + * @brief Compute the discrete-time transition matrix + * @return Matrix Discrete-time transition matrix + */ + Mat_nn get_pi_mat_d(double dt) const { + // Cache the pi matrix for the same time step as it is likely to be + // reused and is expensive to compute + static double prev_dt = dt; + static Mat_nn pi_mat_d = (get_pi_mat_c() * dt).exp(); + if (dt != prev_dt) { + pi_mat_d = (get_pi_mat_c() * dt).exp(); + prev_dt = dt; + } + return pi_mat_d; + } + + /** + * @brief Get the dynamic models + * @return Reference to tuple of dynamic models + */ + const DynModTuple& get_models() const { return models_; } + + /** + * @brief Get the dynamic models (non-const) + * @return Reference to tuple of dynamic models + */ + DynModTuple& get_models() { return models_; } + + /** + * @brief Get specific dynamic model + * @tparam i Index of model + * @return DynModT model reference + */ + template + const DynModT& get_model() const { + return std::get(models_); + } + + /** + * @brief Get specific dynamic model (non-const) + * @tparam i Index of model + * @return DynModT model reference + */ + template + DynModT& get_model() { + return std::get(models_); } - // Check that the jump matrix is valid - for (int i = 0; i < jump_matrix_.rows(); i++) { - if (jump_matrix_.row(i).sum() != 1.0) { - throw std::invalid_argument("Jump matrix row " + std::to_string(i) + " should sum to 1"); - } + /** + * @brief f_d of specific dynamic model + * @tparam i Index of model + * @param dt Time step + * @param x State + * @param u Input (optional) + * @param v Noise (optional) + * @return Vec_x + */ + template + T::Vec_x f_d(double dt, + const T::Vec_x& x, + const T::Vec_u& u = T::Vec_u::Zero(), + const T::Vec_v& v = T::Vec_v::Zero()) const { + return get_model().f_d(dt, x, u, v); } - } - - /** - * @brief Compute the continuous-time transition matrix - * @return Matrix Continuous-time transition matrix - * @note See https://en.wikipedia.org/wiki/Continuous-time_Markov_chain. - */ - Mat_nn get_pi_mat_c() const - { - Vec_n hold_rates = hold_times_.cwiseInverse(); - Mat_nn pi_mat_c = hold_rates.replicate(1, N_MODELS); - - // Multiply the jump matrix with the hold times - pi_mat_c = pi_mat_c.cwiseProduct(jump_matrix_); - - // Each row should sum to zero - pi_mat_c -= hold_rates.asDiagonal(); - - return pi_mat_c; - } - - /** - * @brief Compute the discrete-time transition matrix - * @return Matrix Discrete-time transition matrix - */ - Mat_nn get_pi_mat_d(double dt) const - { - // Cache the pi matrix for the same time step as it is likely to be reused and is expensive to compute - static double prev_dt = dt; - static Mat_nn pi_mat_d = (get_pi_mat_c() * dt).exp(); - if (dt != prev_dt) { - pi_mat_d = (get_pi_mat_c() * dt).exp(); - prev_dt = dt; + + /** + * @brief Q_d of specific dynamic model + * @tparam i Index of model + * @param dt Time step + * @param x State + * @return Mat_vv + */ + template + T::Mat_vv Q_d(double dt, const T::Vec_x& x) const { + return get_model().Q_d(dt, x); + } + + static constexpr int N_DIM_x(size_t model_index) { + return N_DIMS_x.at(model_index); } - return pi_mat_d; - } - - /** - * @brief Get the dynamic models - * @return Reference to tuple of dynamic models - */ - const DynModTuple &get_models() const { return models_; } - - /** - * @brief Get the dynamic models (non-const) - * @return Reference to tuple of dynamic models - */ - DynModTuple &get_models() { return models_; } - - /** - * @brief Get specific dynamic model - * @tparam i Index of model - * @return DynModT model reference - */ - template const DynModT &get_model() const { return std::get(models_); } - - /** - * @brief Get specific dynamic model (non-const) - * @tparam i Index of model - * @return DynModT model reference - */ - template DynModT &get_model() { return std::get(models_); } - - /** - * @brief f_d of specific dynamic model - * @tparam i Index of model - * @param dt Time step - * @param x State - * @param u Input (optional) - * @param v Noise (optional) - * @return Vec_x - */ - template - T::Vec_x f_d(double dt, const T::Vec_x &x, const T::Vec_u &u = T::Vec_u::Zero(), const T::Vec_v &v = T::Vec_v::Zero()) const - { - return get_model().f_d(dt, x, u, v); - } - - /** - * @brief Q_d of specific dynamic model - * @tparam i Index of model - * @param dt Time step - * @param x State - * @return Mat_vv - */ - template T::Mat_vv Q_d(double dt, const T::Vec_x &x) const { return get_model().Q_d(dt, x); } - - - static constexpr int N_DIM_x(size_t model_index) { return N_DIMS_x.at(model_index); } - static constexpr int N_DIM_u(size_t model_index) { return N_DIMS_u.at(model_index); } - static constexpr int N_DIM_v(size_t model_index) { return N_DIMS_v.at(model_index); } - -private: - DynModTuple models_; - Mat_nn jump_matrix_; - Vec_n hold_times_; + static constexpr int N_DIM_u(size_t model_index) { + return N_DIMS_u.at(model_index); + } + static constexpr int N_DIM_v(size_t model_index) { + return N_DIMS_v.at(model_index); + } + + private: + DynModTuple models_; + Mat_nn jump_matrix_; + Vec_n hold_times_; }; /** - * @brief Class for resizing the state vector of a sensor model to fit with multiple dynamic models. + * @brief Class for resizing the state vector of a sensor model to fit with + * multiple dynamic models. * * @tparam n_dim_a The dimension of the desired state vector. * @tparam SensModT The sensor model to use. */ -template class ImmSensorModel { -public: - static constexpr int N_DIM_x = SensModT::N_DIM_x; - static constexpr int N_DIM_z = SensModT::N_DIM_z; - static constexpr int N_DIM_w = SensModT::N_DIM_w; - static constexpr int N_DIM_a = (int)n_dim_a; - - using T = Types_xzwa; - - /** - * @brief Construct a new Imm Sensor Model object - * - * @param sensor_model The sensor model to use. Must have a copy constructor. - */ - ImmSensorModel(SensModT sensor_model) - : sensor_model_(sensor_model) - { - static_assert(N_DIM_a >= SensModT::SensModI::N_DIM_x, "N_DIM_a must be greater than or equal to the state dimension of the sensor model"); - } - - T::Vec_z h(const T::Vec_a &x, const T::Vec_w &w) const { return sensor_model_.h(x.template head(), w); } - - T::Mat_ww R(const T::Vec_x &x) const { return sensor_model_.R(x.template head()); } - -private: - SensModT sensor_model_; +template +class ImmSensorModel { + public: + static constexpr int N_DIM_x = SensModT::N_DIM_x; + static constexpr int N_DIM_z = SensModT::N_DIM_z; + static constexpr int N_DIM_w = SensModT::N_DIM_w; + static constexpr int N_DIM_a = (int)n_dim_a; + + using T = Types_xzwa; + + /** + * @brief Construct a new Imm Sensor Model object + * + * @param sensor_model The sensor model to use. Must have a copy + * constructor. + */ + ImmSensorModel(SensModT sensor_model) : sensor_model_(sensor_model) { + static_assert(N_DIM_a >= SensModT::SensModI::N_DIM_x, + "N_DIM_a must be greater than or equal to the state " + "dimension of the sensor model"); + } + + T::Vec_z h(const T::Vec_a& x, const T::Vec_w& w) const { + return sensor_model_.h(x.template head(), w); + } + + T::Mat_ww R(const T::Vec_x& x) const { + return sensor_model_.R(x.template head()); + } + + private: + SensModT sensor_model_; }; /** - * @brief Class for resizing the state vector of a sensor model to fit with multiple dynamic models. - * + * @brief Class for resizing the state vector of a sensor model to fit with + * multiple dynamic models. + * * @tparam n_dim_a The dimension of the desired state vector. - * @tparam SensModT The sensor model to use. + * @tparam SensModT The sensor model to use. */ -template class ImmSensorModelLTV { -public: - static constexpr int N_DIM_x = SensModT::N_DIM_x; - static constexpr int N_DIM_z = SensModT::N_DIM_z; - static constexpr int N_DIM_w = SensModT::N_DIM_w; - static constexpr int N_DIM_a = (int)n_dim_a; - - using T = Types_xzwa; - - /** - * @brief Construct a new Imm Sensor Model LTV object - * - * @param sensor_model The sensor model to use. Must have a copy constructor. - */ - ImmSensorModelLTV(SensModT sensor_model) - : sensor_model_(sensor_model) - { - static_assert(N_DIM_a >= N_DIM_x, "N_DIM_a must be greater than or equal to the state dimension of the sensor model"); - } - - T::Vec_z h(const T::Vec_a &x, const T::Vec_w &w) const { return sensor_model_.h(x.template head(), w); } - - T::Mat_za C(const T::Vec_a &x) const - { - typename T::Mat_za C_a = T::Mat_za::Zero(); - C_a.template leftCols() = sensor_model_.C(x.template head()); - return C_a; - } - - T::Mat_zw H(const T::Vec_a &x) const { return sensor_model_.H(x.template head()); } - - T::Mat_ww R(const T::Vec_a &x) const { return sensor_model_.R(x.template head()); } - - T::Gauss_z pred_from_est(const T::Gauss_a &x_est) const - { - typename T::Vec_x mean = x_est.mean().template head(); - typename T::Mat_xx cov = x_est.cov().template topLeftCorner(); - return sensor_model_.pred_from_est(typename T::Gauss_x{mean, cov}); - } - - T::Gauss_z pred_from_state(const T::Vec_a &x) const { return sensor_model_.pred_from_state(x.template head()); } - -private: - SensModT sensor_model_; +template +class ImmSensorModelLTV { + public: + static constexpr int N_DIM_x = SensModT::N_DIM_x; + static constexpr int N_DIM_z = SensModT::N_DIM_z; + static constexpr int N_DIM_w = SensModT::N_DIM_w; + static constexpr int N_DIM_a = (int)n_dim_a; + + using T = Types_xzwa; + + /** + * @brief Construct a new Imm Sensor Model LTV object + * + * @param sensor_model The sensor model to use. Must have a copy + * constructor. + */ + ImmSensorModelLTV(SensModT sensor_model) : sensor_model_(sensor_model) { + static_assert(N_DIM_a >= N_DIM_x, + "N_DIM_a must be greater than or equal to the state " + "dimension of the sensor model"); + } + + T::Vec_z h(const T::Vec_a& x, const T::Vec_w& w) const { + return sensor_model_.h(x.template head(), w); + } + + T::Mat_za C(const T::Vec_a& x) const { + typename T::Mat_za C_a = T::Mat_za::Zero(); + C_a.template leftCols() = + sensor_model_.C(x.template head()); + return C_a; + } + + T::Mat_zw H(const T::Vec_a& x) const { + return sensor_model_.H(x.template head()); + } + + T::Mat_ww R(const T::Vec_a& x) const { + return sensor_model_.R(x.template head()); + } + + T::Gauss_z pred_from_est(const T::Gauss_a& x_est) const { + typename T::Vec_x mean = x_est.mean().template head(); + typename T::Mat_xx cov = + x_est.cov().template topLeftCorner(); + return sensor_model_.pred_from_est(typename T::Gauss_x{mean, cov}); + } + + T::Gauss_z pred_from_state(const T::Vec_a& x) const { + return sensor_model_.pred_from_state(x.template head()); + } + + private: + SensModT sensor_model_; }; namespace concepts { template -concept ImmModel = requires { - typename T::DynModTuple; -}; +concept ImmModel = requires { typename T::DynModTuple; }; -} // namespace concepts +} // namespace concepts -} // namespace vortex::models \ No newline at end of file +} // namespace vortex::models diff --git a/vortex-filtering/include/vortex_filtering/models/sensor_model_interfaces.hpp b/vortex-filtering/include/vortex_filtering/models/sensor_model_interfaces.hpp index 17d9227e..bf8a3f2b 100644 --- a/vortex-filtering/include/vortex_filtering/models/sensor_model_interfaces.hpp +++ b/vortex-filtering/include/vortex_filtering/models/sensor_model_interfaces.hpp @@ -1,7 +1,8 @@ /** * @file sensor_model.hpp * @author Eirik KolÃ¥s - * @brief Sensor model interface. Based on "Fundamentals of Sensor Fusion" by Edmund Brekke + * @brief Sensor model interface. Based on "Fundamentals of Sensor Fusion" by + * Edmund Brekke * @version 0.1 * @date 2023-10-26 * @@ -12,10 +13,10 @@ #include #include #include -#include -#include #include +#include #include +#include namespace vortex::models { namespace interface { @@ -29,42 +30,43 @@ namespace interface { * @note - h * @note - R */ -template class SensorModel { -public: - static constexpr int N_DIM_x = (int)n_dim_x; - static constexpr int N_DIM_z = (int)n_dim_z; - static constexpr int N_DIM_w = (int)n_dim_w; - - using T = vortex::Types_xzw; - - SensorModel() = default; - virtual ~SensorModel() = default; - - /** - * @brief Sensor Model - * @param x State - * @return Vec_z - */ - virtual T::Vec_z h(const T::Vec_x &x, const T::Vec_w &w = T::Vec_w::Zero()) const = 0; - - /** - * @brief Noise covariance matrix - * @param x State - * @return Mat_zz - */ - virtual T::Mat_ww R(const T::Vec_x &x) const = 0; - - /** Sample from the sensor model - * @param x State - * @param w Noise - * @param gen Random number generator (For deterministic behaviour) - * @return Vec_z - */ - T::Vec_z sample_h(const T::Vec_x &x, std::mt19937 &gen) const - { - typename T::Gauss_w w{T::Vec_w::Zero(), R(x)}; - return this->h(x, w.sample(gen)); - } +template +class SensorModel { + public: + static constexpr int N_DIM_x = (int)n_dim_x; + static constexpr int N_DIM_z = (int)n_dim_z; + static constexpr int N_DIM_w = (int)n_dim_w; + + using T = vortex::Types_xzw; + + SensorModel() = default; + virtual ~SensorModel() = default; + + /** + * @brief Sensor Model + * @param x State + * @return Vec_z + */ + virtual T::Vec_z h(const T::Vec_x& x, + const T::Vec_w& w = T::Vec_w::Zero()) const = 0; + + /** + * @brief Noise covariance matrix + * @param x State + * @return Mat_zz + */ + virtual T::Mat_ww R(const T::Vec_x& x) const = 0; + + /** Sample from the sensor model + * @param x State + * @param w Noise + * @param gen Random number generator (For deterministic behaviour) + * @return Vec_z + */ + T::Vec_z sample_h(const T::Vec_x& x, std::mt19937& gen) const { + typename T::Gauss_w w{T::Vec_w::Zero(), R(x)}; + return this->h(x, w.sample(gen)); + } }; /** @@ -78,84 +80,86 @@ template class Sensor * @note - R * @note - H (optional if N_DIM_x == N_DIM_z) */ -template class SensorModelLTV : public SensorModel { -public: - static constexpr int N_DIM_x = n_dim_x; - static constexpr int N_DIM_z = n_dim_z; - static constexpr int N_DIM_w = n_dim_w; - - using T = vortex::Types_xzw; - - virtual ~SensorModelLTV() = default; - /** Sensor Model - * Overriding SensorModel::h - * @param x State - * @param w Noise - * @return Vec_z - */ - virtual T::Vec_z h(const T::Vec_x &x, const T::Vec_w &w = T::Vec_w::Zero()) const override - { - typename T::Mat_zx C = this->C(x); - typename T::Mat_zw H = this->H(x); - return C * x + H * w; - } - - /** - * @brief Jacobian of sensor model with respect to state - * @param x State - * @return Mat_zx - */ - virtual T::Mat_zx C(const T::Vec_x &x) const = 0; - - /** - * @brief Noise matrix - * @param x State - * @return Mat_zz - */ - virtual T::Mat_zw H(const T::Vec_x & /* x */ = T::Vec_x::Zero()) const - { - if (N_DIM_x != N_DIM_z) { - throw std::runtime_error("SensorModelLTV::H not implemented"); +template +class SensorModelLTV : public SensorModel { + public: + static constexpr int N_DIM_x = n_dim_x; + static constexpr int N_DIM_z = n_dim_z; + static constexpr int N_DIM_w = n_dim_w; + + using T = vortex::Types_xzw; + + virtual ~SensorModelLTV() = default; + /** Sensor Model + * Overriding SensorModel::h + * @param x State + * @param w Noise + * @return Vec_z + */ + virtual T::Vec_z h(const T::Vec_x& x, + const T::Vec_w& w = T::Vec_w::Zero()) const override { + typename T::Mat_zx C = this->C(x); + typename T::Mat_zw H = this->H(x); + return C * x + H * w; + } + + /** + * @brief Jacobian of sensor model with respect to state + * @param x State + * @return Mat_zx + */ + virtual T::Mat_zx C(const T::Vec_x& x) const = 0; + + /** + * @brief Noise matrix + * @param x State + * @return Mat_zz + */ + virtual T::Mat_zw H(const T::Vec_x& /* x */ = T::Vec_x::Zero()) const { + if (N_DIM_x != N_DIM_z) { + throw std::runtime_error("SensorModelLTV::H not implemented"); + } + return T::Mat_zw::Identity(); + } + + /** + * @brief Noise covariance matrix + * @param x State + * @return Mat_zz + */ + virtual T::Mat_ww R(const T::Vec_x& x) const override = 0; + + /** + * @brief Get the predicted measurement distribution given a state estimate. + * Updates the covariance + * + * @param x_est TVec_x estimate + * @return prob::MultiVarGauss + */ + T::Gauss_z pred_from_est(const auto& x_est) const + requires(vortex::concepts::MultiVarGaussLike) + { + typename T::Mat_xx P = x_est.cov(); + typename T::Mat_zx C = this->C(x_est.mean()); + typename T::Mat_ww R = this->R(x_est.mean()); + typename T::Mat_zw H = this->H(x_est.mean()); + + return {this->h(x_est.mean()), + C * P * C.transpose() + H * R * H.transpose()}; + } + + /** + * @brief Get the predicted measurement distribution given a state. Does not + * update the covariance + * @param x Vec_x + * @return prob::MultiVarGauss + */ + T::Gauss_z pred_from_state(const T::Vec_x& x) const { + typename T::Mat_ww R = this->R(x); + typename T::Mat_zw H = this->H(x); + return {this->h(x), H * R * H.transpose()}; } - return T::Mat_zw::Identity(); - } - - /** - * @brief Noise covariance matrix - * @param x State - * @return Mat_zz - */ - virtual T::Mat_ww R(const T::Vec_x &x) const override = 0; - - /** - * @brief Get the predicted measurement distribution given a state estimate. Updates the covariance - * - * @param x_est TVec_x estimate - * @return prob::MultiVarGauss - */ - T::Gauss_z pred_from_est(const auto &x_est) const - requires(vortex::concepts::MultiVarGaussLike) - { - typename T::Mat_xx P = x_est.cov(); - typename T::Mat_zx C = this->C(x_est.mean()); - typename T::Mat_ww R = this->R(x_est.mean()); - typename T::Mat_zw H = this->H(x_est.mean()); - - return {this->h(x_est.mean()), C * P * C.transpose() + H * R * H.transpose()}; - } - - /** - * @brief Get the predicted measurement distribution given a state. Does not update the covariance - * @param x Vec_x - * @return prob::MultiVarGauss - */ - T::Gauss_z pred_from_state(const T::Vec_x &x) const - { - typename T::Mat_ww R = this->R(x); - typename T::Mat_zw H = this->H(x); - return {this->h(x), H * R * H.transpose()}; - } }; -} // namespace interface -} // namespace vortex::models \ No newline at end of file +} // namespace interface +} // namespace vortex::models diff --git a/vortex-filtering/include/vortex_filtering/models/sensor_models.hpp b/vortex-filtering/include/vortex_filtering/models/sensor_models.hpp index 4e5d7e41..106f6aad 100644 --- a/vortex-filtering/include/vortex_filtering/models/sensor_models.hpp +++ b/vortex-filtering/include/vortex_filtering/models/sensor_models.hpp @@ -6,62 +6,69 @@ namespace models { /** * A simple sensor model for testing purposes. - * The measurement model is simply the n_dim_z first elements of the state vector. + * The measurement model is simply the n_dim_z first elements of the state + * vector. * @tparam n_dim_x Dimension of state * @tparam n_dim_z Dimension of measurement */ -template class IdentitySensorModel : public interface::SensorModelLTV { - using Parent = interface::SensorModelLTV; -public: - static constexpr int N_DIM_x = Parent::N_DIM_x; - static constexpr int N_DIM_z = Parent::N_DIM_z; - static constexpr int N_DIM_w = Parent::N_DIM_w; - - using T = vortex::Types_xzw; - - /** Construct a new Simple Sensor Model object. - * The measurement model is simply the n_dim_z first elements of the state vector. - * @param std Standard deviation. Sets the measurement covariance matrix R to I*std^2. - * @tparam n_dim_x Dimension of state - * @tparam n_dim_z Dimension of measurement - */ - IdentitySensorModel(double std) - : R_(T::Mat_ww::Identity() * std * std) - { - } - - /** Construct a new Simple Sensor Model object. - * The measurement model is simply the n_dim_z first elements of the state vector. - * @param R Measurement covariance matrix - */ - IdentitySensorModel(T::Mat_zz R) - : R_(R) - { - } - - /** Get the Jacobian of the measurement model with respect to the state. - * @param x State (not used) - * @return Mat_zx - * @note Overriding SensorModelLTV::C - */ - T::Mat_zx C(const T::Vec_x /* x */& = T::Vec_x::Zero()) const override { return T::Mat_zx::Identity(); } - - /** Get the measurement covariance matrix. - * @param x State (not used) - * @return Mat_zz - * @note Overriding SensorModelLTV::R - */ - T::Mat_zz R(const T::Vec_x /* x */ & = T::Vec_x::Zero()) const override { return R_; } - - /** Get the Jacobian of the measurement model with respect to noise - * @param x State (not used) - * @return Mat_zw - * @note Overriding SensorModelLTV::H - */ - T::Mat_zw H(const T::Vec_x /* x */& = T::Vec_x::Zero()) const override { return T::Mat_zw::Identity(); } - -private: - const T::Mat_ww R_; // Measurement covariance matrix +template +class IdentitySensorModel + : public interface::SensorModelLTV { + using Parent = interface::SensorModelLTV; + + public: + static constexpr int N_DIM_x = Parent::N_DIM_x; + static constexpr int N_DIM_z = Parent::N_DIM_z; + static constexpr int N_DIM_w = Parent::N_DIM_w; + + using T = vortex::Types_xzw; + + /** Construct a new Simple Sensor Model object. + * The measurement model is simply the n_dim_z first elements of the state + * vector. + * @param std Standard deviation. Sets the measurement covariance matrix R + * to I*std^2. + * @tparam n_dim_x Dimension of state + * @tparam n_dim_z Dimension of measurement + */ + IdentitySensorModel(double std) : R_(T::Mat_ww::Identity() * std * std) {} + + /** Construct a new Simple Sensor Model object. + * The measurement model is simply the n_dim_z first elements of the state + * vector. + * @param R Measurement covariance matrix + */ + IdentitySensorModel(T::Mat_zz R) : R_(R) {} + + /** Get the Jacobian of the measurement model with respect to the state. + * @param x State (not used) + * @return Mat_zx + * @note Overriding SensorModelLTV::C + */ + T::Mat_zx C(const T::Vec_x /* x */& = T::Vec_x::Zero()) const override { + return T::Mat_zx::Identity(); + } + + /** Get the measurement covariance matrix. + * @param x State (not used) + * @return Mat_zz + * @note Overriding SensorModelLTV::R + */ + T::Mat_zz R(const T::Vec_x /* x */& = T::Vec_x::Zero()) const override { + return R_; + } + + /** Get the Jacobian of the measurement model with respect to noise + * @param x State (not used) + * @return Mat_zw + * @note Overriding SensorModelLTV::H + */ + T::Mat_zw H(const T::Vec_x /* x */& = T::Vec_x::Zero()) const override { + return T::Mat_zw::Identity(); + } + + private: + const T::Mat_ww R_; // Measurement covariance matrix }; /** Range-Bearing sensor model. @@ -71,69 +78,71 @@ template class IdentitySensorModel : public interface * z = [range, bearing] */ class RangeBearingSensor : public interface::SensorModelLTV<2, 2, 2> { -public: - static constexpr int N_DIM_x = 2; - static constexpr int N_DIM_z = 2; - static constexpr int N_DIM_w = 2; - - using T = vortex::Types_xzw; - - /** Range-Bearing sensor model. - * The measurement model is the range and bearing to the target. - * The first 2 elements of the state vector are assumed to be the position of the target in cartesian coordinates. - * x = [x_target, y_target, ...] - * z = [range, bearing] - * @tparam n_dim_x Dimension of state - * @param std_range Standard deviation of range measurement - * @param std_bearing Standard deviation of bearing measurement - */ - RangeBearingSensor(double std_range, double std_bearing) : std_range_(std_range), std_bearing_(std_bearing) {} - - /** Get the measurement model. - * @param x State - * @param w Noise - * @return Vec_z - * @note Overriding SensorModelLTV::h - */ - T::Vec_z h(const T::Vec_x &x, const T::Vec_w &w = T::Vec_w::Zero()) const override - { - typename T::Vec_z z{std::sqrt(x(0) * x(0) + x(1) * x(1)), std::atan2(x(1), x(0))}; - z += w; - return z; - } - - /** Get the Jacobian of the measurement model with respect to the state. State is in cartesian coordinates. - * @param x State - * @return Mat_zx - * @note Overriding SensorModelLTV::C - */ - T::Mat_zx C(const T::Vec_x &x) const override - { - // clang-format off + public: + static constexpr int N_DIM_x = 2; + static constexpr int N_DIM_z = 2; + static constexpr int N_DIM_w = 2; + + using T = vortex::Types_xzw; + + /** Range-Bearing sensor model. + * The measurement model is the range and bearing to the target. + * The first 2 elements of the state vector are assumed to be the position + * of the target in cartesian coordinates. x = [x_target, y_target, ...] z = + * [range, bearing] + * @tparam n_dim_x Dimension of state + * @param std_range Standard deviation of range measurement + * @param std_bearing Standard deviation of bearing measurement + */ + RangeBearingSensor(double std_range, double std_bearing) + : std_range_(std_range), std_bearing_(std_bearing) {} + + /** Get the measurement model. + * @param x State + * @param w Noise + * @return Vec_z + * @note Overriding SensorModelLTV::h + */ + T::Vec_z h(const T::Vec_x& x, + const T::Vec_w& w = T::Vec_w::Zero()) const override { + typename T::Vec_z z{std::sqrt(x(0) * x(0) + x(1) * x(1)), + std::atan2(x(1), x(0))}; + z += w; + return z; + } + + /** Get the Jacobian of the measurement model with respect to the state. + * State is in cartesian coordinates. + * @param x State + * @return Mat_zx + * @note Overriding SensorModelLTV::C + */ + T::Mat_zx C(const T::Vec_x& x) const override { + // clang-format off typename T::Mat_zx C{ {(x(0) / std::sqrt(x(0) * x(0) + x(1) * x(1))), (x(1) / std::sqrt(x(0) * x(0) + x(1) * x(1)))}, {(-x(1) / (x(0) * x(0) + x(1) * x(1))) , (x(0) / (x(0) * x(0) + x(1) * x(1)))} }; - // clang-format on - return C; - } - - /** Get the measurement covariance matrix. - * @param x State (not used) - * @return Mat_zz - * @note Overriding SensorModelLTV::R - */ - T::Mat_zz R(const T::Vec_x & = T::Vec_x::Zero()) const override - { - typename T::Vec_z D{std_range_ * std_range_, std_bearing_ * std_bearing_}; - typename T::Mat_zz R = D.asDiagonal(); - return R; - } - -private: - double std_range_; - double std_bearing_; + // clang-format on + return C; + } + + /** Get the measurement covariance matrix. + * @param x State (not used) + * @return Mat_zz + * @note Overriding SensorModelLTV::R + */ + T::Mat_zz R(const T::Vec_x& = T::Vec_x::Zero()) const override { + typename T::Vec_z D{std_range_ * std_range_, + std_bearing_ * std_bearing_}; + typename T::Mat_zz R = D.asDiagonal(); + return R; + } + + private: + double std_range_; + double std_bearing_; }; -} // namespace models -} // namespace vortex \ No newline at end of file +} // namespace models +} // namespace vortex diff --git a/vortex-filtering/include/vortex_filtering/models/state.hpp b/vortex-filtering/include/vortex_filtering/models/state.hpp index 2c0457dd..085783af 100644 --- a/vortex-filtering/include/vortex_filtering/models/state.hpp +++ b/vortex-filtering/include/vortex_filtering/models/state.hpp @@ -11,158 +11,170 @@ namespace vortex { -enum class StateName : size_t { none, position, velocity, acceleration, jerk, turn_rate }; +enum class StateName : size_t { + none, + position, + velocity, + acceleration, + jerk, + turn_rate +}; // structs for the type of state struct StateMinMax { - double min; - double max; + double min; + double max; }; template - requires std::is_integral_v || std::is_enum_v + requires std::is_integral_v || std::is_enum_v using StateMapT = std::map; using StateMap = StateMapT; template - requires std::is_integral_v || std::is_enum_v + requires std::is_integral_v || std::is_enum_v struct StateLocation { - StateName name; - size_t start_index; - size_t end_index; - constexpr size_t size() const { return end_index - start_index; } - bool operator==(const StateLocation &other) const { return name == other.name && start_index == other.start_index && end_index == other.end_index; } + StateName name; + size_t start_index; + size_t end_index; + constexpr size_t size() const { return end_index - start_index; } + bool operator==(const StateLocation& other) const { + return name == other.name && start_index == other.start_index && + end_index == other.end_index; + } }; -template constexpr auto index_of(const R &range, T needle) -{ - auto it = std::ranges::find(range, needle); - if (it == std::ranges::end(range)) - throw std::logic_error("Element not found!"); - return std::ranges::distance(std::ranges::begin(range), it); +template +constexpr auto index_of(const R& range, T needle) { + auto it = std::ranges::find(range, needle); + if (it == std::ranges::end(range)) + throw std::logic_error("Element not found!"); + return std::ranges::distance(std::ranges::begin(range), it); } template - requires std::is_integral_v || std::is_enum_v + requires std::is_integral_v || std::is_enum_v class State : public vortex::prob::MultiVarGauss { -public: - static constexpr size_t N_STATES = sizeof...(Sn); - - static constexpr std::array STATE_NAMES = {Sn...}; - - using T = vortex::Types_n; - using StateLoc = StateLocation; - - State(const T::Vec_n &mean, const T::Mat_nn &cov) - : vortex::prob::MultiVarGauss(mean, cov) - { - } - - explicit State(T::Gauss_n x) - : vortex::prob::MultiVarGauss(x.mean(), x.cov()) - { - } - - // private: - static constexpr size_t UNIQUE_STATES_COUNT = []() { - std::array state_names = STATE_NAMES; - std::sort(state_names.begin(), state_names.end()); - auto last = std::unique(state_names.begin(), state_names.end()); - return std::distance(state_names.begin(), last); - }(); - - // static_assert( - // []() { - // size_t n_unique = 1; - // for (size_t i = 0; i < N_STATES; i++) { - // if (i > 0 && STATE_NAMES[i] == STATE_NAMES[i - 1]) { - // n_unique++; - // } - // } - // if (n_unique != UNIQUE_STATES_COUNT) { - // return false; - // } - // return true; - // }(), - // "Groups of state names must not repeat"); - - static constexpr std::array UNIQUE_STATE_NAMES = []() { - std::array unique_state_names = {}; - size_t map_index = 0; - for (size_t i = 1; i < N_STATES; i++) { - if (STATE_NAMES[i] != STATE_NAMES[i - 1]) { - unique_state_names[map_index++] = STATE_NAMES[i - 1]; - } + public: + static constexpr size_t N_STATES = sizeof...(Sn); + + static constexpr std::array STATE_NAMES = {Sn...}; + + using T = vortex::Types_n; + using StateLoc = StateLocation; + + State(const T::Vec_n& mean, const T::Mat_nn& cov) + : vortex::prob::MultiVarGauss(mean, cov) {} + + explicit State(T::Gauss_n x) + : vortex::prob::MultiVarGauss(x.mean(), x.cov()) {} + + // private: + static constexpr size_t UNIQUE_STATES_COUNT = []() { + std::array state_names = STATE_NAMES; + std::sort(state_names.begin(), state_names.end()); + auto last = std::unique(state_names.begin(), state_names.end()); + return std::distance(state_names.begin(), last); + }(); + + // static_assert( + // []() { + // size_t n_unique = 1; + // for (size_t i = 0; i < N_STATES; i++) { + // if (i > 0 && STATE_NAMES[i] == STATE_NAMES[i - 1]) { + // n_unique++; + // } + // } + // if (n_unique != UNIQUE_STATES_COUNT) { + // return false; + // } + // return true; + // }(), + // "Groups of state names must not repeat"); + + static constexpr std::array + UNIQUE_STATE_NAMES = []() { + std::array unique_state_names = {}; + size_t map_index = 0; + for (size_t i = 1; i < N_STATES; i++) { + if (STATE_NAMES[i] != STATE_NAMES[i - 1]) { + unique_state_names[map_index++] = STATE_NAMES[i - 1]; + } + } + unique_state_names[map_index] = STATE_NAMES[N_STATES - 1]; + return unique_state_names; + }(); + + static constexpr std::array STATE_MAP = + []() { + std::array state_map = {}; + + size_t start_index = 0; + size_t map_index = 0; + + for (size_t i = 1; i < N_STATES; i++) { + if (STATE_NAMES[i] != STATE_NAMES[i - 1]) { + state_map[map_index++] = {STATE_NAMES[i - 1], start_index, + i}; + start_index = i; + } + } + state_map[map_index] = {STATE_NAMES[N_STATES - 1], start_index, + N_STATES}; + return state_map; + }(); + + static constexpr bool has_state_name(StateName S) { + return std::find(UNIQUE_STATE_NAMES.begin(), UNIQUE_STATE_NAMES.end(), + S) != UNIQUE_STATE_NAMES.end(); + } + + public: + static constexpr StateLoc state_loc(StateName S) { + return STATE_MAP[index_of(UNIQUE_STATE_NAMES, S)]; + } + + template + requires(has_state_name(S)) + using T_n = vortex::Types_n; + + template + requires(has_state_name(S)) + T_n::Vec_n mean_of() const { + constexpr StateLoc sm = state_loc(S); + return this->mean().template segment(sm.start_index); + } + + template + requires(has_state_name(S)) + void set_mean_of(const T_n::Vec_n& mean) { + constexpr StateLoc sm = state_loc(S); + this->mean().template segment(sm.start_index) = mean; } - unique_state_names[map_index] = STATE_NAMES[N_STATES - 1]; - return unique_state_names; - }(); - static constexpr std::array STATE_MAP = []() { - std::array state_map = {}; + template + requires(has_state_name(S)) + T_n::Mat_nn cov_of() const { + constexpr StateLoc sm = state_loc(S); + return this->cov().template block(sm.start_index, + sm.start_index); + } + + template + requires(has_state_name(S)) + void set_cov_of(const T_n::Mat_nn& cov) { + constexpr StateLoc sm = state_loc(S); + this->cov().template block(sm.start_index, + sm.start_index) = cov; + } - size_t start_index = 0; - size_t map_index = 0; + T::Gauss_n gauss() const { return static_cast(*this); } - for (size_t i = 1; i < N_STATES; i++) { - if (STATE_NAMES[i] != STATE_NAMES[i - 1]) { - state_map[map_index++] = {STATE_NAMES[i - 1], start_index, i}; - start_index = i; - } + template + requires(has_state_name(S)) + T_n::Gauss_n gauss_of() const { + return {mean_of(S), cov_of(S)}; } - state_map[map_index] = {STATE_NAMES[N_STATES - 1], start_index, N_STATES}; - return state_map; - }(); - - static constexpr bool has_state_name(StateName S) { return std::find(UNIQUE_STATE_NAMES.begin(), UNIQUE_STATE_NAMES.end(), S) != UNIQUE_STATE_NAMES.end(); } - -public: - static constexpr StateLoc state_loc(StateName S) { return STATE_MAP[index_of(UNIQUE_STATE_NAMES, S)]; } - - template - requires(has_state_name(S)) - using T_n = vortex::Types_n; - - template - requires(has_state_name(S)) - T_n::Vec_n mean_of() const - { - constexpr StateLoc sm = state_loc(S); - return this->mean().template segment(sm.start_index); - } - - template - requires(has_state_name(S)) - void set_mean_of(const T_n::Vec_n &mean) - { - constexpr StateLoc sm = state_loc(S); - this->mean().template segment(sm.start_index) = mean; - } - - template - requires(has_state_name(S)) - T_n::Mat_nn cov_of() const - { - constexpr StateLoc sm = state_loc(S); - return this->cov().template block(sm.start_index, sm.start_index); - } - - template - requires(has_state_name(S)) - void set_cov_of(const T_n::Mat_nn &cov) - { - constexpr StateLoc sm = state_loc(S); - this->cov().template block(sm.start_index, sm.start_index) = cov; - } - - T::Gauss_n gauss() const { return static_cast(*this); } - - template - requires(has_state_name(S)) - T_n::Gauss_n gauss_of() const - { - return {mean_of(S), cov_of(S)}; - } }; -} // namespace vortex \ No newline at end of file +} // namespace vortex diff --git a/vortex-filtering/include/vortex_filtering/numerical_integration/README.md b/vortex-filtering/include/vortex_filtering/numerical_integration/README.md index 747d174f..e853f956 100644 --- a/vortex-filtering/include/vortex_filtering/numerical_integration/README.md +++ b/vortex-filtering/include/vortex_filtering/numerical_integration/README.md @@ -1,10 +1,10 @@ # Integration Methods -A collection of integration methods. +A collection of integration methods. All methods can be used with all of the Kalman filter implementations. ## Use Cases -| Method | Pros | Cons | Use Case -| ---- | ---- | ---- | ---- +| Method | Pros | Cons | Use Case +| ---- | ---- | ---- | ---- | Forward Euler | Fast | Most inaccurate | If speed is your only concern, use this | RK4 | Accurate | 4 stages -> Slower | Should be the go-to method for most cases | RK45 | As accurate as you want | Adaptive step size -> Super slow | If you need a set accuracy. Not meant for real-time applications diff --git a/vortex-filtering/include/vortex_filtering/numerical_integration/erk_methods.hpp b/vortex-filtering/include/vortex_filtering/numerical_integration/erk_methods.hpp index a10f56fc..7d921c29 100644 --- a/vortex-filtering/include/vortex_filtering/numerical_integration/erk_methods.hpp +++ b/vortex-filtering/include/vortex_filtering/numerical_integration/erk_methods.hpp @@ -20,140 +20,169 @@ namespace integrator { * @brief Forward Euler method * @tparam n_dim_x Dimension of the state vector */ -template class Forward_Euler { -public: - using Vec_x = Eigen::Vector; - using Dyn_mod_func = std::function; - - static Vec_x integrate(Dyn_mod_func f, double dt, const Vec_x &x0, double t0 = 0.0) - { - Vec_x k1 = f(t0, x0); - return x0 + dt * k1; - } +template +class Forward_Euler { + public: + using Vec_x = Eigen::Vector; + using Dyn_mod_func = std::function; + + static Vec_x integrate(Dyn_mod_func f, + double dt, + const Vec_x& x0, + double t0 = 0.0) { + Vec_x k1 = f(t0, x0); + return x0 + dt * k1; + } }; -template using Forward_Euler_M = Forward_Euler; +template +using Forward_Euler_M = Forward_Euler; /** * @brief Runge-Kutta 4th order method * @tparam n_dim_x Dimension of the state vector */ -template class RK4 { -public: - using Vec_x = Eigen::Vector; - using Dyn_mod_func = std::function; - - /** - * @brief Integrate the function f over one step dt - * - * @param f Function to integrate, must be of the form Vec_x(double dt, const Vec_x&x_k, double t_k=0) - * @param dt Time step - * @param x0 Start state - * @param t0 Start time (optional) - * @return Vec_x Integrated state - */ - static Vec_x integrate(Dyn_mod_func f, double dt, const Vec_x &x0, double t0 = 0.0) - { - Vec_x k1 = f(t0 + 0.0 * dt, x0); - Vec_x k2 = f(t0 + 0.5 * dt, x0 + 0.5 * dt * k1); - Vec_x k3 = f(t0 + 0.5 * dt, x0 + 0.5 * dt * k2); - Vec_x k4 = f(t0 + 1.0 * dt, x0 + 1.0 * dt * k3); - - return x0 + (dt / 6.0) * (k1 + 2 * k2 + 2 * k3 + k4); - } +template +class RK4 { + public: + using Vec_x = Eigen::Vector; + using Dyn_mod_func = std::function; + + /** + * @brief Integrate the function f over one step dt + * + * @param f Function to integrate, must be of the form Vec_x(double dt, + * const Vec_x&x_k, double t_k=0) + * @param dt Time step + * @param x0 Start state + * @param t0 Start time (optional) + * @return Vec_x Integrated state + */ + static Vec_x integrate(Dyn_mod_func f, + double dt, + const Vec_x& x0, + double t0 = 0.0) { + Vec_x k1 = f(t0 + 0.0 * dt, x0); + Vec_x k2 = f(t0 + 0.5 * dt, x0 + 0.5 * dt * k1); + Vec_x k3 = f(t0 + 0.5 * dt, x0 + 0.5 * dt * k2); + Vec_x k4 = f(t0 + 1.0 * dt, x0 + 1.0 * dt * k3); + + return x0 + (dt / 6.0) * (k1 + 2 * k2 + 2 * k3 + k4); + } }; -template using RK4_M = RK4; +template +using RK4_M = RK4; /** * @brief Create an arbitrary explicit Runge-Kutta method from a Butcher table * @tparam n_x Dimension of the state vector * @tparam n_stages Number of stages in the method */ -template class Butcher { -public: - using Vec_x = Eigen::Vector; - using Dyn_mod_func = std::function; - - using Vec_n = Eigen::Vector; - using Mat_nn = Eigen::Matrix; - using Mat_xn = Eigen::Matrix; - /** - * @brief Construct an ERK method from a Butcher table. Cannot be passed as a class to a template - * as it needs the non-static members A, b and c to be intitalized in an object. - * @param A - * @param b - * @param c - */ - Butcher(Mat_nn A, Vec_n b, Vec_n c) : _A{A}, _b{b}, _c{c} - { - // Check if Butcher table is valid - if (A.isLowerTriangular() == false) { - throw std::invalid_argument("Butcher table: A is not lower triangular"); - } +template +class Butcher { + public: + using Vec_x = Eigen::Vector; + using Dyn_mod_func = std::function; - // Check if Butcher table is consistent - if (b.sum() != 1) { - throw std::invalid_argument("Butcher table: b does not sum to one"); - } - for (int i = 0; i < A.rows(); i++) { - if (std::abs(A.row(i).sum() - c(i)) > 1e-6) { - throw std::invalid_argument("Butcher table: row " + std::to_string(i) + " in A does not sum to c(" + std::to_string(i) + ")"); - } + using Vec_n = Eigen::Vector; + using Mat_nn = Eigen::Matrix; + using Mat_xn = Eigen::Matrix; + /** + * @brief Construct an ERK method from a Butcher table. Cannot be passed as + * a class to a template as it needs the non-static members A, b and c to be + * initialized in an object. + * @param A + * @param b + * @param c + */ + Butcher(Mat_nn A, Vec_n b, Vec_n c) : _A{A}, _b{b}, _c{c} { + // Check if Butcher table is valid + if (A.isLowerTriangular() == false) { + throw std::invalid_argument( + "Butcher table: A is not lower triangular"); + } + + // Check if Butcher table is consistent + if (b.sum() != 1) { + throw std::invalid_argument("Butcher table: b does not sum to one"); + } + for (int i = 0; i < A.rows(); i++) { + if (std::abs(A.row(i).sum() - c(i)) > 1e-6) { + throw std::invalid_argument( + "Butcher table: row " + std::to_string(i) + + " in A does not sum to c(" + std::to_string(i) + ")"); + } + } } - } - Vec_x integrate(Dyn_mod_func f, double dt, const Vec_x &x0, double t0 = 0.0) - { - Mat_xn k = Mat_xn::Zero(); - for (size_t i = 0; i < n_stages; i++) { - k.col(i) = f(t0 + _c(i) * dt, x0 + dt * k * _A.row(i).transpose()); + Vec_x integrate(Dyn_mod_func f, + double dt, + const Vec_x& x0, + double t0 = 0.0) { + Mat_xn k = Mat_xn::Zero(); + for (size_t i = 0; i < n_stages; i++) { + k.col(i) = f(t0 + _c(i) * dt, x0 + dt * k * _A.row(i).transpose()); + } + return x0 + dt * k * _b; } - return x0 + dt * k * _b; - } -private: - Mat_nn _A; - Vec_n _b; - Vec_n _c; + private: + Mat_nn _A; + Vec_n _b; + Vec_n _c; }; -template class ODE45 { -public: - static constexpr int n_stages = 7; - static constexpr size_t q = 5; // Order of the method - - using Vec_x = Eigen::Vector; - using Dyn_mod_func = std::function; - - using Vec_n = Eigen::Vector; - using Mat_nn = Eigen::Matrix; - using Mat_xn = Eigen::Matrix; - - /** - * @brief Variable step size Runge-Kutta method - * - * @param abs_error absolute error. Specify a value for all state variables - * @param rel_error relative error. Specify a value for all state variables - * @param max_iterations Maximum number of iterations before giving up - * @param min_step_size Minimum valid value for the step size in the integration - */ - ODE45(double abs_error = 1e-6, double rel_error = 1e-6, size_t max_iterations = 1000, double min_step_size = 1e-9) - : ODE45(Vec_x::Ones() * abs_error, Vec_x::Ones() * rel_error, max_iterations, min_step_size) - { - } - - /** - * @brief Variable step size Runge-Kutta method - * - * @param abs_error_vec absolute error vector. Specify a value for each state variable - * @param rel_error_vec relative error vector. Specify a value for each state variable - * @param max_iterations Maximum number of iterations before giving up - * @param min_step_size Minimum valid value for the step size in the integration - */ - ODE45(Vec_x abs_error_vec = Vec_x::Ones() * 1e-6, Vec_x rel_error_vec = Vec_x::Ones() * 1e-6, size_t max_iterations = 1000, double min_step_size = 1e-9) - : _Atol{abs_error_vec}, _Rtol{rel_error_vec}, _max_iter{max_iterations}, _min_step_size{min_step_size} - { - // Use the Dormand Prince (RKDP) method - // clang-format off +template +class ODE45 { + public: + static constexpr int n_stages = 7; + static constexpr size_t q = 5; // Order of the method + + using Vec_x = Eigen::Vector; + using Dyn_mod_func = std::function; + + using Vec_n = Eigen::Vector; + using Mat_nn = Eigen::Matrix; + using Mat_xn = Eigen::Matrix; + + /** + * @brief Variable step size Runge-Kutta method + * + * @param abs_error absolute error. Specify a value for all state variables + * @param rel_error relative error. Specify a value for all state variables + * @param max_iterations Maximum number of iterations before giving up + * @param min_step_size Minimum valid value for the step size in the + * integration + */ + ODE45(double abs_error = 1e-6, + double rel_error = 1e-6, + size_t max_iterations = 1000, + double min_step_size = 1e-9) + : ODE45(Vec_x::Ones() * abs_error, + Vec_x::Ones() * rel_error, + max_iterations, + min_step_size) {} + + /** + * @brief Variable step size Runge-Kutta method + * + * @param abs_error_vec absolute error vector. Specify a value for each + * state variable + * @param rel_error_vec relative error vector. Specify a value for each + * state variable + * @param max_iterations Maximum number of iterations before giving up + * @param min_step_size Minimum valid value for the step size in the + * integration + */ + ODE45(Vec_x abs_error_vec = Vec_x::Ones() * 1e-6, + Vec_x rel_error_vec = Vec_x::Ones() * 1e-6, + size_t max_iterations = 1000, + double min_step_size = 1e-9) + : _Atol{abs_error_vec}, + _Rtol{rel_error_vec}, + _max_iter{max_iterations}, + _min_step_size{min_step_size} { + // Use the Dormand Prince (RKDP) method + // clang-format off _A << 0 , 0 , 0 , 0 , 0 , 0 , 0, 1/5.0 , 0 , 0 , 0 , 0 , 0 , 0, 3/40.0 , 9/40.0 , 0 , 0 , 0 , 0 , 0, @@ -166,90 +195,101 @@ template class ODE45 { b_T <<35/384.0 , 0 , 500/1113.0 , 125/192.0, -2187/6784.0 , 11/84.0 , 0, // Error of order O(dt^5) 5179/57600.0, 0 , 7571/16695.0, 393/640.0, -92097/339200.0, 187/2100.0, 1/40.0; // Error of order O(dt^4) _b = b_T.transpose(); - + _c << 0 , 1/5.0 , 3/10.0 , 4/5.0 , 8/9.0 , 1 , 1; - // clang-format on - } - - /** - * @brief Integrate the function f over one step dt - * - * @param f Function to integrate - * @param dt Time step size / Simulation time - * @param x_k Start state - * @param t_k Start time (optional) - * @return Integrated state - * @throws std::runtime_error if the maximum number of iterations is reached - * @throws std::runtime_error if the step size becomes too small - */ - Vec_x integrate(Dyn_mod_func f, double dt, const Vec_x &x0, double t0 = 0.0) - { - // Copy t and x - const double t0pdt = t0 + dt; // Final time t0+dt - double t_i = t0; // Intermediate times - Vec_x x_i = x0; // Intermediate states - double h = dt; // Variable step size - - for (size_t iter = 0; iter < _max_iter; iter++) { - // Compute k_i - Mat_xn k = Mat_xn::Zero(); - for (size_t i = 0; i < n_stages; i++) { - k.col(i) = f(t_i + _c(i) * h, x_i + h * k * _A.row(i).transpose()); - } - - Vec_x x_ip1_hat = x_i + h * k * _b.col(0); // Higher order solution - Vec_x x_ip1 = x_i + h * k * _b.col(1); // Lower order solution - - // From https://scicomp.stackexchange.com/questions/32563/dormand-prince-54-how-to-update-the-stepsize-and-make-accept-reject-decision - - // Compute max(|x_i|, |x_i+1|) - Vec_x abs_x_i = x_i.cwiseAbs(); - Vec_x abs_x_ip1 = x_ip1.cwiseAbs(); // Using lower order solution - Vec_x max_x = abs_x_i.cwiseMax(abs_x_ip1); - - // Scaling factor - Vec_x sc = _Atol + max_x.cwiseProduct(_Rtol); - - // Estimate error - Vec_x temp_error = (x_ip1 - x_ip1_hat).cwiseQuotient(sc); - double error = std::sqrt(temp_error.dot(temp_error) / n_dim_x); - - // Accecpt step if error is within tolerance - if (error < 1) { - t_i += h; - x_i = x_ip1_hat; - - if (t_i >= t0pdt) { - return x_i; // Simulation completed successfully + // clang-format on + } + + /** + * @brief Integrate the function f over one step dt + * + * @param f Function to integrate + * @param dt Time step size / Simulation time + * @param x_k Start state + * @param t_k Start time (optional) + * @return Integrated state + * @throws std::runtime_error if the maximum number of iterations is reached + * @throws std::runtime_error if the step size becomes too small + */ + Vec_x integrate(Dyn_mod_func f, + double dt, + const Vec_x& x0, + double t0 = 0.0) { + // Copy t and x + const double t0pdt = t0 + dt; // Final time t0+dt + double t_i = t0; // Intermediate times + Vec_x x_i = x0; // Intermediate states + double h = dt; // Variable step size + + for (size_t iter = 0; iter < _max_iter; iter++) { + // Compute k_i + Mat_xn k = Mat_xn::Zero(); + for (size_t i = 0; i < n_stages; i++) { + k.col(i) = + f(t_i + _c(i) * h, x_i + h * k * _A.row(i).transpose()); + } + + Vec_x x_ip1_hat = x_i + h * k * _b.col(0); // Higher order solution + Vec_x x_ip1 = x_i + h * k * _b.col(1); // Lower order solution + + // From + // https://scicomp.stackexchange.com/questions/32563/dormand-prince-54-how-to-update-the-stepsize-and-make-accept-reject-decision + + // Compute max(|x_i|, |x_i+1|) + Vec_x abs_x_i = x_i.cwiseAbs(); + Vec_x abs_x_ip1 = x_ip1.cwiseAbs(); // Using lower order solution + Vec_x max_x = abs_x_i.cwiseMax(abs_x_ip1); + + // Scaling factor + Vec_x sc = _Atol + max_x.cwiseProduct(_Rtol); + + // Estimate error + Vec_x temp_error = (x_ip1 - x_ip1_hat).cwiseQuotient(sc); + double error = std::sqrt(temp_error.dot(temp_error) / n_dim_x); + + // Accept step if error is within tolerance + if (error < 1) { + t_i += h; + x_i = x_ip1_hat; + + if (t_i >= t0pdt) { + return x_i; // Simulation completed successfully + } + } + + // Compute new step size + double h_new; + static constexpr double f_ac = + std::pow(0.25, 1.0 / (q + 1)); // safety factor + h_new = f_ac * h * + std::pow(1 / error, 1.0 / (q + 1)); // optimal step size + h_new = std::max(h_new, 0.2 * h); // limit step size decrease + h_new = std::min(h_new, 5.0 * h); // limit step size increase + h_new = std::min( + h_new, t0pdt - t_i); // limit step size to not overshoot t_kp1 + h = h_new; + + if (h < _min_step_size) { + throw std::runtime_error( + "Could not reach tolerance within minimum step size" + + std::to_string(_min_step_size)); + } } - } - - // Compute new step size - double h_new; - static constexpr double f_ac = std::pow(0.25, 1.0 / (q + 1)); // safety factor - h_new = f_ac * h * std::pow(1 / error, 1.0 / (q + 1)); // optimal step size - h_new = std::max(h_new, 0.2 * h); // limit step size decrease - h_new = std::min(h_new, 5.0 * h); // limit step size increase - h_new = std::min(h_new, t0pdt - t_i); // limit step size to not overshoot t_kp1 - h = h_new; - - if (h < _min_step_size) { - throw std::runtime_error("Could not reach tolerance within minimum step size" + std::to_string(_min_step_size)); - } + throw std::runtime_error( + "Could not reach tolerance within maximum number of iterations " + + std::to_string(_max_iter)); } - throw std::runtime_error("Could not reach tolerance within maximum number of iterations " + std::to_string(_max_iter)); - } - -private: - Eigen::Matrix _A; - Eigen::Matrix _b; - Eigen::Vector _c; - - Vec_x _Atol; - Vec_x _Rtol; - size_t _max_iter; - double _min_step_size; + + private: + Eigen::Matrix _A; + Eigen::Matrix _b; + Eigen::Vector _c; + + Vec_x _Atol; + Vec_x _Rtol; + size_t _max_iter; + double _min_step_size; }; -} // namespace integrator -} // namespace vortex \ No newline at end of file +} // namespace integrator +} // namespace vortex diff --git a/vortex-filtering/include/vortex_filtering/probability/README.md b/vortex-filtering/include/vortex_filtering/probability/README.md index 3facf8cf..2fecbe76 100644 --- a/vortex-filtering/include/vortex_filtering/probability/README.md +++ b/vortex-filtering/include/vortex_filtering/probability/README.md @@ -1,10 +1,10 @@ # Probability -This folder contains the probability distributions used. +This folder contains the probability distributions used. All classes and functions are under the namespace `vortex::prob`. ## MultiVarGauss -This class represents a multivariate Gaussian distribution. It is a template class with parameter `N_DIM`. This is the dimension of the distribution. +This class represents a multivariate Gaussian distribution. It is a template class with parameter `N_DIM`. This is the dimension of the distribution. ### Usage The class contains methods for getting the mean and covariance, sampling from the distribution, calculating the probability density function and the log probability density function. It works with both static and dynamic dimensions. @@ -19,4 +19,4 @@ The class contains methods for sampling from the distribution, calculating the p To create an instance you need to provide a vector of weights and a vector of MultiVarGauss objects. MultiVarGauss objects must have the same dimension as the GaussianMixture object. -To reduce the gaussian mixture to a single gaussian, the methods `GaussianMixture::reduce` and `GaussianMixture::ml_estimate` can be used. The first method reduces the mixture to a single gaussian by matching the mean and covariance of the mixture to a single gaussian. The second method returns the gaussian component with the highest weight multiplied by the mean. \ No newline at end of file +To reduce the gaussian mixture to a single gaussian, the methods `GaussianMixture::reduce` and `GaussianMixture::ml_estimate` can be used. The first method reduces the mixture to a single gaussian by matching the mean and covariance of the mixture to a single gaussian. The second method returns the gaussian component with the highest weight multiplied by the mean. diff --git a/vortex-filtering/include/vortex_filtering/probability/binomial.hpp b/vortex-filtering/include/vortex_filtering/probability/binomial.hpp index 0b287ee8..ee02734d 100644 --- a/vortex-filtering/include/vortex_filtering/probability/binomial.hpp +++ b/vortex-filtering/include/vortex_filtering/probability/binomial.hpp @@ -14,51 +14,53 @@ namespace vortex::prob { class Binomial { -public: - Binomial(int n, double p) : n_(n), p_(p) {} + public: + Binomial(int n, double p) : n_(n), p_(p) {} - /** Calculate the probability of x given the Binomial distribution - * @param x - * @return double - */ - double pr(int x) const { return std::pow(p_, x) * std::pow(1 - p_, n_ - x) * factorial(n_) / (factorial(x) * factorial(n_ - x)); } + /** Calculate the probability of x given the Binomial distribution + * @param x + * @return double + */ + double pr(int x) const { + return std::pow(p_, x) * std::pow(1 - p_, n_ - x) * factorial(n_) / + (factorial(x) * factorial(n_ - x)); + } - /** Calculate the mean of the Binomial distribution - * @return double mean - */ - double mean() const { return n_ * p_; } + /** Calculate the mean of the Binomial distribution + * @return double mean + */ + double mean() const { return n_ * p_; } - /** Calculate the variance of the Binomial distribution - * @return double variance - */ - double cov() const { return n_ * p_ * (1 - p_); } + /** Calculate the variance of the Binomial distribution + * @return double variance + */ + double cov() const { return n_ * p_ * (1 - p_); } - /** Parameter n of the Binomial distribution - * @return int n - */ - int n() const { return n_; } + /** Parameter n of the Binomial distribution + * @return int n + */ + int n() const { return n_; } - /** Parameter p of the Binomial distribution - * @return double p - */ - double p() const { return p_; } + /** Parameter p of the Binomial distribution + * @return double p + */ + double p() const { return p_; } -private: - int n_; - double p_; + private: + int n_; + double p_; - /** Calculate the factorial of x - * @param x - * @return double factorial - */ - double factorial(int x) const - { - double factorial = 1; - for (int i = 1; i <= x; i++) { - factorial *= i; + /** Calculate the factorial of x + * @param x + * @return double factorial + */ + double factorial(int x) const { + double factorial = 1; + for (int i = 1; i <= x; i++) { + factorial *= i; + } + return factorial; } - return factorial; - } }; -} // namespace vortex::prob +} // namespace vortex::prob diff --git a/vortex-filtering/include/vortex_filtering/probability/chi_squared.hpp b/vortex-filtering/include/vortex_filtering/probability/chi_squared.hpp index 3479fcfb..83398e1f 100644 --- a/vortex-filtering/include/vortex_filtering/probability/chi_squared.hpp +++ b/vortex-filtering/include/vortex_filtering/probability/chi_squared.hpp @@ -4,32 +4,36 @@ namespace vortex::prob { class ChiSquared { -public: - ChiSquared(int n) : n_(n) {} - - /** Calculate the probability density of x given the Chi-Squared distribution - * @param x - * @return double - */ - double pdf(double x) const { return std::pow(x, n_ / 2 - 1) * std::exp(-x / 2) / (std::pow(2, n_ / 2) * std::tgamma(n_ / 2)); } - - /** Calculate the mean of the Chi-Squared distribution - * @return double mean - */ - double mean() const { return n_; } - - /** Calculate the variance of the Chi-Squared distribution - * @return double variance - */ - double cov() const { return 2 * n_; } - - /** Parameter n of the Chi-Squared distribution - * @return int n - */ - int n() const { return n_; } - -private: - int n_; + public: + ChiSquared(int n) : n_(n) {} + + /** Calculate the probability density of x given the Chi-Squared + * distribution + * @param x + * @return double + */ + double pdf(double x) const { + return std::pow(x, n_ / 2 - 1) * std::exp(-x / 2) / + (std::pow(2, n_ / 2) * std::tgamma(n_ / 2)); + } + + /** Calculate the mean of the Chi-Squared distribution + * @return double mean + */ + double mean() const { return n_; } + + /** Calculate the variance of the Chi-Squared distribution + * @return double variance + */ + double cov() const { return 2 * n_; } + + /** Parameter n of the Chi-Squared distribution + * @return int n + */ + int n() const { return n_; } + + private: + int n_; }; -} // namespace vortex::prob +} // namespace vortex::prob diff --git a/vortex-filtering/include/vortex_filtering/probability/gaussian_mixture.hpp b/vortex-filtering/include/vortex_filtering/probability/gaussian_mixture.hpp index 80c2b0c7..ffd6c239 100644 --- a/vortex-filtering/include/vortex_filtering/probability/gaussian_mixture.hpp +++ b/vortex-filtering/include/vortex_filtering/probability/gaussian_mixture.hpp @@ -13,7 +13,7 @@ #include #include #include -#include // std::accumulate +#include // std::accumulate #include #include #include @@ -24,189 +24,193 @@ namespace concepts { template concept is_container = requires(Container c) { - // The container must have a value_type. - typename Container::value_type; - // The size of the container must be convertible to std::size_t. - { - std::size(c) - } -> std::convertible_to; - // Accessing an element by index must return a reference to ValueType or a type convertible to ValueType. - { - c[std::declval()] - } -> std::convertible_to; + // The container must have a value_type. + typename Container::value_type; + // The size of the container must be convertible to std::size_t. + { std::size(c) } -> std::convertible_to; + // Accessing an element by index must return a reference to ValueType or a + // type convertible to ValueType. + { c[std::declval()] } -> std::convertible_to; }; -} // namespace concepts +} // namespace concepts /** * A class for representing a multivariate Gaussian mixture distribution - * @tparam n_dims dimentions of the Gaussian + * @tparam n_dims dimensions of the Gaussian */ -template class GaussianMixture { -public: - static constexpr int N_DIMS = (int)n_dims; - - using Vec_n = Eigen::Vector; - using Mat_nn = Eigen::Matrix; - using Gauss_n = MultiVarGauss; - - /** Construct a new Gaussian Mixture object - * @param weights Weights of the Gaussians - * @param gaussians Gaussians - * @note The weights are automatically normalized, so they don't need to sum to 1. - */ - GaussianMixture(concepts::is_container auto const &weights, concepts::is_container auto const &gaussians) - : weights_(Eigen::Map(weights.data(), std::distance(std::begin(weights), std::end(weights)))), - gaussians_(std::begin(gaussians), std::end(gaussians)) - { - if ((size_t)weights_.size() != gaussians_.size()) { - throw std::invalid_argument("The number of weights must match the number of Gaussians"); +template +class GaussianMixture { + public: + static constexpr int N_DIMS = (int)n_dims; + + using Vec_n = Eigen::Vector; + using Mat_nn = Eigen::Matrix; + using Gauss_n = MultiVarGauss; + + /** Construct a new Gaussian Mixture object + * @param weights Weights of the Gaussians + * @param gaussians Gaussians + * @note The weights are automatically normalized, so they don't need to sum + * to 1. + */ + GaussianMixture(concepts::is_container auto const& weights, + concepts::is_container auto const& gaussians) + : weights_(Eigen::Map( + weights.data(), + std::distance(std::begin(weights), std::end(weights)))), + gaussians_(std::begin(gaussians), std::end(gaussians)) { + if ((size_t)weights_.size() != gaussians_.size()) { + throw std::invalid_argument( + "The number of weights must match the number of Gaussians"); + } } - } - - /** Default Constructor - * weights and gaussians are empty - */ - GaussianMixture() = default; - - /***/ - /** Copy Constructor - * @param gaussian_mixture - */ - GaussianMixture(const GaussianMixture &gaussian_mixture) : weights_(gaussian_mixture.weights()), gaussians_(gaussian_mixture.gaussians()) {} - - /** Calculate the probability density function of x given the Gaussian mixture - * @param x - * @return double - */ - double pdf(const Vec_n &x) const - { - double pdf = 0; - for (int i = 0; i < num_components(); i++) { - pdf += get_weight(i) * gaussian(i).pdf(x); + + /** Default Constructor + * weights and gaussians are empty + */ + GaussianMixture() = default; + + /***/ + /** Copy Constructor + * @param gaussian_mixture + */ + GaussianMixture(const GaussianMixture& gaussian_mixture) + : weights_(gaussian_mixture.weights()), + gaussians_(gaussian_mixture.gaussians()) {} + + /** Calculate the probability density function of x given the Gaussian + * mixture + * @param x + * @return double + */ + double pdf(const Vec_n& x) const { + double pdf = 0; + for (int i = 0; i < num_components(); i++) { + pdf += get_weight(i) * gaussian(i).pdf(x); + } + pdf /= sum_weights(); + return pdf; } - pdf /= sum_weights(); - return pdf; - } - - /** Find the mean of the Gaussian mixture - * @return Vector - */ - Vec_n mean() const - { - Vec_n mean = Vec_n::Zero(); - for (size_t i = 0; i < gaussians_.size(); i++) { - mean += get_weight(i) * gaussian(i).mean(); + + /** Find the mean of the Gaussian mixture + * @return Vector + */ + Vec_n mean() const { + Vec_n mean = Vec_n::Zero(); + for (size_t i = 0; i < gaussians_.size(); i++) { + mean += get_weight(i) * gaussian(i).mean(); + } + mean /= sum_weights(); + return mean; } - mean /= sum_weights(); - return mean; - } - - /** Find the covariance of the Gaussian mixture - * @return Matrix - */ - Mat_nn cov() const - { - // Spread of innovations - Mat_nn P_bar = Mat_nn::Zero(); - for (size_t i = 0; i < num_components(); i++) { - P_bar += get_weight(i) * gaussian(i).mean() * gaussian(i).mean().transpose(); + + /** Find the covariance of the Gaussian mixture + * @return Matrix + */ + Mat_nn cov() const { + // Spread of innovations + Mat_nn P_bar = Mat_nn::Zero(); + for (size_t i = 0; i < num_components(); i++) { + P_bar += get_weight(i) * gaussian(i).mean() * + gaussian(i).mean().transpose(); + } + P_bar /= sum_weights(); + P_bar -= mean() * mean().transpose(); + + // Spread of Gaussians + Mat_nn P = Mat_nn::Zero(); + for (size_t i = 0; i < num_components(); i++) { + P += get_weight(i) * gaussian(i).cov(); + } + P /= sum_weights(); + return P + P_bar; } - P_bar /= sum_weights(); - P_bar -= mean() * mean().transpose(); - // Spread of Gaussians - Mat_nn P = Mat_nn::Zero(); - for (size_t i = 0; i < num_components(); i++) { - P += get_weight(i) * gaussian(i).cov(); + /** Find the maximum likelihood estimate of the Gaussian mixture + * @return Gauss_n + */ + Gauss_n ml_estimate() const { + double max_pdf = 0; + int max_i = 0; + for (int i = 0; i < num_components(); i++) { + double pdf = get_weight(i) * gaussian(i).pdf(gaussian(i).mean()); + if (pdf > max_pdf) { + max_pdf = pdf; + max_i = i; + } + } + return gaussian(max_i); } - P /= sum_weights(); - return P + P_bar; - } - - /** Find the maximum likelihood estimate of the Gaussian mixture - * @return Gauss_n - */ - Gauss_n ml_estimate() const - { - double max_pdf = 0; - int max_i = 0; - for (int i = 0; i < num_components(); i++) { - double pdf = get_weight(i) * gaussian(i).pdf(gaussian(i).mean()); - if (pdf > max_pdf) { - max_pdf = pdf; - max_i = i; - } + + /** Reduce the Gaussian mixture to a single Gaussian, e.g. the minimum mean + * square estimate + * @return Gauss_n + */ + Gauss_n reduce() const { return {mean(), cov()}; } + + /** Get the weights of the Gaussian mixture + * @return Eigen::VectorXd + */ + const Eigen::VectorXd& weights() const { return weights_; } + + /** Get the weight of the i'th Gaussian + * @param i index + * @return double + */ + double get_weight(size_t i) const { return weights_(i); } + + /** Set the weight of the i'th Gaussian + * @param i index + * @param weight + */ + void set_weight(size_t i, double weight) { weights_(i) = weight; } + + /** Get the sum of the weights + * @return double + */ + double sum_weights() const { return weights_.sum(); } + + /** Get the Gaussians of the Gaussian mixture + * @return std::vector + */ + const std::vector& gaussians() const { return gaussians_; } + + /** Get the i'th Gaussian + * @param i index + * @return Gauss_n + */ + const Gauss_n& gaussian(size_t i) const { return gaussians_.at(i); } + + /** Get the number of Gaussians in the mixture + * @return int + */ + size_t num_components() const { return gaussians_.size(); } + + /** Number of components + * + */ + size_t size() const { return num_components(); } + + /** Sample from the Gaussian mixture + * @param gen Random number generator + * @return Vec_n + */ + Vec_n sample(std::mt19937& gen) const { + std::discrete_distribution dist(weights().begin(), + weights().end()); + return gaussian(dist(gen)).sample(gen); } - return gaussian(max_i); - } - - /** Reduce the Gaussian mixture to a single Gaussian, e.g. the minimum mean square estimate - * @return Gauss_n - */ - Gauss_n reduce() const { return {mean(), cov()}; } - - /** Get the weights of the Gaussian mixture - * @return Eigen::VectorXd - */ - const Eigen::VectorXd &weights() const { return weights_; } - - /** Get the weight of the i'th Gaussian - * @param i index - * @return double - */ - double get_weight(size_t i) const { return weights_(i); } - - /** Set the weight of the i'th Gaussian - * @param i index - * @param weight - */ - void set_weight(size_t i, double weight) { weights_(i) = weight; } - - /** Get the sum of the weights - * @return double - */ - double sum_weights() const { return weights_.sum(); } - - /** Get the Gaussians of the Gaussian mixture - * @return std::vector - */ - const std::vector &gaussians() const { return gaussians_; } - - /** Get the i'th Gaussian - * @param i index - * @return Gauss_n - */ - const Gauss_n &gaussian(size_t i) const { return gaussians_.at(i); } - - /** Get the number of Gaussians in the mixture - * @return int - */ - size_t num_components() const { return gaussians_.size(); } - - /** Number of components - * - */ - size_t size() const { return num_components(); } - - /** Sample from the Gaussian mixture - * @param gen Random number generator - * @return Vec_n - */ - Vec_n sample(std::mt19937 &gen) const - { - std::discrete_distribution dist(weights().begin(), weights().end()); - return gaussian(dist(gen)).sample(gen); - } - -private: - const Eigen::VectorXd weights_; - const std::vector gaussians_; + + private: + const Eigen::VectorXd weights_; + const std::vector gaussians_; }; -template using GaussMix = GaussianMixture; -using GaussMix2d = GaussMix<2>; -using GaussMix3d = GaussMix<3>; -using GaussMix4d = GaussMix<4>; +template +using GaussMix = GaussianMixture; +using GaussMix2d = GaussMix<2>; +using GaussMix3d = GaussMix<3>; +using GaussMix4d = GaussMix<4>; -} // namespace vortex::prob +} // namespace vortex::prob diff --git a/vortex-filtering/include/vortex_filtering/probability/multi_var_gauss.hpp b/vortex-filtering/include/vortex_filtering/probability/multi_var_gauss.hpp index 0630f72b..6c1c0221 100644 --- a/vortex-filtering/include/vortex_filtering/probability/multi_var_gauss.hpp +++ b/vortex-filtering/include/vortex_filtering/probability/multi_var_gauss.hpp @@ -2,134 +2,142 @@ #include #include #include -#include #include namespace vortex::prob { /** * A class for representing a multivariate Gaussian distribution - * @tparam N_DIMS dimentions of the Gaussian + * @tparam N_DIMS dimensions of the Gaussian */ -template class MultiVarGauss { -public: - static constexpr int N_DIMS = (int)n_dims; - - using Vec_n = Eigen::Vector; - using Mat_nn = Eigen::Matrix; - - /** Construct a Gaussian with a given mean and covariance matrix - * @param mean - * @param cov Symmetric positive definite covariance matrix - */ - MultiVarGauss(const Vec_n &mean, const Mat_nn &cov) : mean_(mean), cov_(cov), cov_inv_(cov_.llt().solve(Mat_nn::Identity())) - { - if (!cov_.isApprox(cov_.transpose(), 1e-6)) { - std::stringstream ss; - ss << "Covariance matrix is not symmetric:\n" << cov_; - throw std::invalid_argument(ss.str()); +template +class MultiVarGauss { + public: + static constexpr int N_DIMS = (int)n_dims; + + using Vec_n = Eigen::Vector; + using Mat_nn = Eigen::Matrix; + + /** Construct a Gaussian with a given mean and covariance matrix + * @param mean + * @param cov Symmetric positive definite covariance matrix + */ + MultiVarGauss(const Vec_n& mean, const Mat_nn& cov) + : mean_(mean), + cov_(cov), + cov_inv_(cov_.llt().solve(Mat_nn::Identity())) { + if (!cov_.isApprox(cov_.transpose(), 1e-6)) { + std::stringstream ss; + ss << "Covariance matrix is not symmetric:\n" << cov_; + throw std::invalid_argument(ss.str()); + } + if (cov_.llt().info() != Eigen::Success) { + std::stringstream ss; + ss << "Covariance matrix is not positive definite:\n" << cov_; + throw std::invalid_argument(ss.str()); + } + } + + // Default constructor + MultiVarGauss() = default; + + /** Calculate the probability density of x given the Gaussian + * @param x + * @return double + */ + double pdf(const Vec_n& x) const { + Vec_n diff = x - mean(); + double exponent = -0.5 * diff.transpose() * cov_inv() * diff; + return std::exp(exponent) / + std::sqrt(std::pow(2 * std::numbers::pi, size()) * + cov().determinant()); + } + + /** Calculate the log likelihood of x given the Gaussian. + * Assumes that the covariance matrix is positive definite and symmetric + * @param x + * @return double + */ + double logpdf(const Vec_n& x) const { + Vec_n diff = x - mean(); + double exponent = -0.5 * diff.transpose() * cov_inv() * diff; + return exponent - + 0.5 * std::log(std::pow(2 * std::numbers::pi, size()) * + cov().determinant()); + } + + const Vec_n& mean() const { return mean_; } + Vec_n& mean() { return mean_; } + + const Mat_nn& cov() const { return cov_; } + Mat_nn& cov() { return cov_; } + Mat_nn cov_inv() const { return cov().llt().solve(Mat_nn::Identity()); } + /** Calculate the Mahalanobis distance of x given the Gaussian + * @param x + * @return double + */ + double mahalanobis_distance(const Vec_n& x) const { + Vec_n diff = x - mean(); + return std::sqrt(diff.transpose() * cov_inv() * diff); } - if (cov_.llt().info() != Eigen::Success) { - std::stringstream ss; - ss << "Covariance matrix is not positive definite:\n" << cov_; - throw std::invalid_argument(ss.str()); + + /** Sample from the Gaussian + * @param gen Random number generator + * @return Vector + */ + Vec_n sample(std::mt19937& gen) const { + std::normal_distribution<> d{0, 1}; + Vec_n sample(size()); + for (int i = 0; i < size(); ++i) { + sample(i) = d(gen); + } + return mean() + cov().llt().matrixL() * sample; + } + + /** Get the number of dimensions of the Gaussian + * @return int + */ + static constexpr int size() { return N_DIMS; } + + /** Construct a Standard Gaussian Distribution with zero mean and identity + * covariance matrix + * @return MultiVarGauss + */ + static MultiVarGauss Standard() { + return {Vec_n::Zero(), Mat_nn::Identity()}; } - } - - // Default constructor - MultiVarGauss() = default; - - /** Calculate the probability density of x given the Gaussian - * @param x - * @return double - */ - double pdf(const Vec_n &x) const - { - Vec_n diff = x - mean(); - double exponent = -0.5 * diff.transpose() * cov_inv() * diff; - return std::exp(exponent) / std::sqrt(std::pow(2 * std::numbers::pi, size()) * cov().determinant()); - } - - /** Calculate the log likelihood of x given the Gaussian. - * Assumes that the covariance matrix is positive definite and symmetric - * @param x - * @return double - */ - double logpdf(const Vec_n &x) const - { - Vec_n diff = x - mean(); - double exponent = -0.5 * diff.transpose() * cov_inv() * diff; - return exponent - 0.5 * std::log(std::pow(2 * std::numbers::pi, size()) * cov().determinant()); - } - - const Vec_n &mean() const { return mean_; } - Vec_n &mean() { return mean_; } - - const Mat_nn &cov() const { return cov_; } - Mat_nn &cov() { return cov_; } - Mat_nn cov_inv() const { return cov().llt().solve(Mat_nn::Identity()); } - /** Calculate the Mahalanobis distance of x given the Gaussian - * @param x - * @return double - */ - double mahalanobis_distance(const Vec_n &x) const - { - Vec_n diff = x - mean(); - return std::sqrt(diff.transpose() * cov_inv() * diff); - } - - /** Sample from the Gaussian - * @param gen Random number generator - * @return Vector - */ - Vec_n sample(std::mt19937 &gen) const - { - std::normal_distribution<> d{0, 1}; - Vec_n sample(size()); - for (int i = 0; i < size(); ++i) { - sample(i) = d(gen); + + /** operator== + * @param lhs + * @param rhs + * @return bool true if the means and covariances are equal + */ + friend bool operator==(const MultiVarGauss& lhs, const MultiVarGauss& rhs) { + return lhs.mean() == rhs.mean() && lhs.cov() == rhs.cov(); } - return mean() + cov().llt().matrixL() * sample; - } - - /** Get the number of dimensions of the Gaussian - * @return int - */ - static constexpr int size() { return N_DIMS; } - - /** Construct a Standard Gaussian Distribution with zero mean and identity covariance matrix - * @return MultiVarGauss - */ - static MultiVarGauss Standard() { return {Vec_n::Zero(), Mat_nn::Identity()}; } - - /** operator== - * @param lhs - * @param rhs - * @return bool true if the means and covariances are equal - */ - friend bool operator==(const MultiVarGauss &lhs, const MultiVarGauss &rhs) { return lhs.mean() == rhs.mean() && lhs.cov() == rhs.cov(); } - - /** operator<< - * @param os - * @param gauss - * @return std::ostream& - */ - friend std::ostream &operator<<(std::ostream &os, const MultiVarGauss &gauss) - { - os << "Mean:\n" - << gauss.mean().transpose() << "\n" - << "Covariance:\n" - << gauss.cov(); - return os; - } - -private: - Vec_n mean_; - Mat_nn cov_; - Mat_nn cov_inv_; + + /** operator<< + * @param os + * @param gauss + * @return std::ostream& + */ + friend std::ostream& operator<<(std::ostream& os, + const MultiVarGauss& gauss) { + os << "Mean:\n" + << gauss.mean().transpose() << "\n" + << "Covariance:\n" + << gauss.cov(); + return os; + } + + private: + Vec_n mean_; + Mat_nn cov_; + Mat_nn cov_inv_; }; -template using Gauss = MultiVarGauss; +template +using Gauss = MultiVarGauss; using Gauss2d = Gauss<2>; using Gauss3d = Gauss<3>; @@ -137,4 +145,4 @@ using Gauss4d = Gauss<4>; using Gauss5d = Gauss<5>; using Gauss6d = Gauss<6>; -} // namespace vortex::prob +} // namespace vortex::prob diff --git a/vortex-filtering/include/vortex_filtering/probability/poisson.hpp b/vortex-filtering/include/vortex_filtering/probability/poisson.hpp index 633cb84e..765f37ab 100644 --- a/vortex-filtering/include/vortex_filtering/probability/poisson.hpp +++ b/vortex-filtering/include/vortex_filtering/probability/poisson.hpp @@ -1,7 +1,8 @@ /** * @file poisson.hpp * @author Eirik KolÃ¥s - * @brief A class for representing a Poisson distribution. Used for modeling clutter + * @brief A class for representing a Poisson distribution. Used for modeling + * clutter * @version 0.1 * @date 2023-10-26 * @@ -14,38 +15,42 @@ namespace vortex::prob { class Poisson { -public: - Poisson(double lambda) : lambda_(lambda) {} - - /** Calculate the probability of x given the Poisson distribution - * @param x - * @return double - */ - double pr(int x) const { return std::pow(lambda_, x) * std::exp(-lambda_) / factorial(x); } - - /** Calculate the mean of the Poisson distribution - * @return double mean - */ - double mean() const { return lambda_; } - - /** Calculate the variance of the Poisson distribution - * @return double variance - */ - double cov() const { return lambda_; } - - /** Parameter lambda of the Poisson distribution - * @return double lambda - */ - double lambda() const { return lambda_; } - -private: - const double lambda_; - - /** Calculate the factorial of x - * @param x - * @return double factorial - */ - static constexpr double factorial(size_t x) { return (x == 1 || x == 0) ? 1 : factorial(x - 1) * x; } + public: + Poisson(double lambda) : lambda_(lambda) {} + + /** Calculate the probability of x given the Poisson distribution + * @param x + * @return double + */ + double pr(int x) const { + return std::pow(lambda_, x) * std::exp(-lambda_) / factorial(x); + } + + /** Calculate the mean of the Poisson distribution + * @return double mean + */ + double mean() const { return lambda_; } + + /** Calculate the variance of the Poisson distribution + * @return double variance + */ + double cov() const { return lambda_; } + + /** Parameter lambda of the Poisson distribution + * @return double lambda + */ + double lambda() const { return lambda_; } + + private: + const double lambda_; + + /** Calculate the factorial of x + * @param x + * @return double factorial + */ + static constexpr double factorial(size_t x) { + return (x == 1 || x == 0) ? 1 : factorial(x - 1) * x; + } }; -} // namespace vortex::prob +} // namespace vortex::prob diff --git a/vortex-filtering/include/vortex_filtering/probability/uniform.hpp b/vortex-filtering/include/vortex_filtering/probability/uniform.hpp index adbe5240..c5930cf0 100644 --- a/vortex-filtering/include/vortex_filtering/probability/uniform.hpp +++ b/vortex-filtering/include/vortex_filtering/probability/uniform.hpp @@ -14,78 +14,77 @@ namespace vortex::prob { -template class Uniform { -public: - using Vec_n = Eigen::Vector; - using Mat_nn = Eigen::Matrix; +template +class Uniform { + public: + using Vec_n = Eigen::Vector; + using Mat_nn = Eigen::Matrix; - constexpr Uniform(Vec_n lower, Vec_n upper) : lower_(lower), upper_(upper) {} + constexpr Uniform(Vec_n lower, Vec_n upper) + : lower_(lower), upper_(upper) {} - double pr(Vec_n x) const - { - for (size_t i = 0; i < n_dims; i++) { - if (x(i) < lower_(i) || x(i) > upper_(i)) { - return 0; - } + double pr(Vec_n x) const { + for (size_t i = 0; i < n_dims; i++) { + if (x(i) < lower_(i) || x(i) > upper_(i)) { + return 0; + } + } + return 1.0 / (upper_ - lower_).prod(); } - return 1.0 / (upper_ - lower_).prod(); - } - Vec_n sample(std::mt19937 &gen) const - { - Vec_n sample; - for (size_t i = 0; i < n_dims; i++) { - std::uniform_real_distribution dist(lower_(i), upper_(i)); - sample(i) = dist(gen); + Vec_n sample(std::mt19937& gen) const { + Vec_n sample; + for (size_t i = 0; i < n_dims; i++) { + std::uniform_real_distribution dist(lower_(i), upper_(i)); + sample(i) = dist(gen); + } + return sample; } - return sample; - } - Vec_n mean() const { return (upper_ + lower_) / 2; } - Mat_nn cov() const - { - Vec_n diff = upper_ - lower_; - for (double &d : diff) { - d *= d / 12; + Vec_n mean() const { return (upper_ + lower_) / 2; } + Mat_nn cov() const { + Vec_n diff = upper_ - lower_; + for (double& d : diff) { + d *= d / 12; + } + return diff.asDiagonal(); } - return diff.asDiagonal(); - } - const Vec_n &lower() const { return lower_; } - const Vec_n &upper() const { return upper_; } + const Vec_n& lower() const { return lower_; } + const Vec_n& upper() const { return upper_; } -private: - Vec_n lower_; - Vec_n upper_; + private: + Vec_n lower_; + Vec_n upper_; }; -template <> class Uniform<1> { -public: - constexpr Uniform(double lower, double upper) : lower_(lower), upper_(upper) {} +template <> +class Uniform<1> { + public: + constexpr Uniform(double lower, double upper) + : lower_(lower), upper_(upper) {} - double pr(double x) const - { - if (x < lower_ || x > upper_) { - return 0; + double pr(double x) const { + if (x < lower_ || x > upper_) { + return 0; + } + return 1.0 / (upper_ - lower_); } - return 1.0 / (upper_ - lower_); - } - double sample(std::mt19937 &gen) const - { - std::uniform_real_distribution dist(lower_, upper_); - return dist(gen); - } + double sample(std::mt19937& gen) const { + std::uniform_real_distribution dist(lower_, upper_); + return dist(gen); + } - double mean() const { return (upper_ + lower_) / 2; } - double cov() const { return (upper_ - lower_) * (upper_ - lower_) / 12; } + double mean() const { return (upper_ + lower_) / 2; } + double cov() const { return (upper_ - lower_) * (upper_ - lower_) / 12; } - double lower() const { return lower_; } - double upper() const { return upper_; } + double lower() const { return lower_; } + double upper() const { return upper_; } -private: - double lower_; - double upper_; + private: + double lower_; + double upper_; }; -} // namespace vortex::prob \ No newline at end of file +} // namespace vortex::prob diff --git a/vortex-filtering/include/vortex_filtering/types/general_concepts.hpp b/vortex-filtering/include/vortex_filtering/types/general_concepts.hpp index 5270cedb..328cd708 100644 --- a/vortex-filtering/include/vortex_filtering/types/general_concepts.hpp +++ b/vortex-filtering/include/vortex_filtering/types/general_concepts.hpp @@ -8,9 +8,13 @@ namespace vortex::concepts { template concept mat_convertible_to = - std::convertible_to, std::decay_t> && std::is_base_of_v>, std::decay_t> && + std::convertible_to, std::decay_t> && + std::is_base_of_v>, + std::decay_t> && std::is_base_of_v>, std::decay_t> && - (std::decay_t::RowsAtCompileTime == std::decay_t::RowsAtCompileTime) && - (std::decay_t::ColsAtCompileTime == std::decay_t::ColsAtCompileTime); + (std::decay_t::RowsAtCompileTime == + std::decay_t::RowsAtCompileTime) && + (std::decay_t::ColsAtCompileTime == + std::decay_t::ColsAtCompileTime); -} // namespace vortex::concepts \ No newline at end of file +} // namespace vortex::concepts diff --git a/vortex-filtering/include/vortex_filtering/types/model_concepts.hpp b/vortex-filtering/include/vortex_filtering/types/model_concepts.hpp index 3e545465..82ea780b 100644 --- a/vortex-filtering/include/vortex_filtering/types/model_concepts.hpp +++ b/vortex-filtering/include/vortex_filtering/types/model_concepts.hpp @@ -1,14 +1,15 @@ #pragma once #include -#include -#include #include +#include +#include namespace vortex::concepts { using std::size_t; /** - * @brief Concept for MultiVarGauss-like classes. Requires the following functions: + * @brief Concept for MultiVarGauss-like classes. Requires the following + * functions: * @brief - `double pdf(Vec_n)` * @brief - `double logpdf(Vec_n)` * @brief - `Vec_n mean()` @@ -22,57 +23,54 @@ using std::size_t; */ template concept MultiVarGaussLike = requires { - { - std::declval().pdf(std::declval::Vec_n>()) - } -> std::convertible_to; + { + std::declval().pdf(std::declval::Vec_n>()) + } -> std::convertible_to; - { - std::declval().logpdf(std::declval::Vec_n>()) - } -> std::convertible_to; + { + std::declval().logpdf(std::declval::Vec_n>()) + } -> std::convertible_to; - { - std::declval().mean() - } -> mat_convertible_to::Vec_n>; + { + std::declval().mean() + } -> mat_convertible_to::Vec_n>; - { - std::declval().cov() - } -> mat_convertible_to::Mat_nn>; + { + std::declval().cov() + } -> mat_convertible_to::Mat_nn>; - { - std::declval().size() - } -> std::convertible_to; + { std::declval().size() } -> std::convertible_to; - { - std::declval().sample(std::declval()) - } -> mat_convertible_to::Vec_n>; + { + std::declval().sample(std::declval()) + } -> mat_convertible_to::Vec_n>; - { - std::declval().mahalanobis_distance(std::declval::Vec_n>()) - } -> std::convertible_to; + { + std::declval().mahalanobis_distance( + std::declval::Vec_n>()) + } -> std::convertible_to; }; template concept StateLike = requires { - requires MultiVarGaussLike; + requires MultiVarGaussLike; - { - T::N_STATES - } -> std::convertible_to; + { T::N_STATES } -> std::convertible_to; - { - T::STATE_NAMES - } -> std::convertible_to>; + { + T::STATE_NAMES + } -> std::convertible_to>; - { - std::declval().state_loc(std::declval()) - } -> std::convertible_to; + { + std::declval().state_loc(std::declval()) + } -> std::convertible_to; }; /** * @brief Concept for dynamic models. Requires the following functions: * @brief - `Vec_x f_d(double, Vec_x, Vec_u, Vec_v)` * @brief - `Mat_vv Q_d(double, Vec_x)` - * + * * @tparam DynMod The dynamic model type * @tparam n_dim_x Dimension of the state * @tparam n_dim_u Dimension of the input @@ -80,20 +78,24 @@ concept StateLike = requires { */ template concept DynamicModel = requires { - { - std::declval().f_d(std::declval(), - std::declval::Vec_x>(), - std::declval::Vec_u>(), - std::declval::Vec_v>()) - } -> mat_convertible_to::Vec_x>; - - { - std::declval().Q_d(std::declval(), std::declval::Vec_x>()) - } -> mat_convertible_to::Mat_vv>; + { + std::declval().f_d( + std::declval(), + std::declval::Vec_x>(), + std::declval::Vec_u>(), + std::declval::Vec_v>()) + } -> mat_convertible_to::Vec_x>; + + { + std::declval().Q_d( + std::declval(), + std::declval::Vec_x>()) + } -> mat_convertible_to::Mat_vv>; }; /** - * @brief Concept for dynamic models with time-varying parameters. Requires the following functions: + * @brief Concept for dynamic models with time-varying parameters. Requires the + * following functions: * @brief - `Vec_x f_d(double, Vec_x, Vec_u, Vec_v)` * @brief - `Mat_vv Q_d(double, Vec_x)` * @brief - `Mat_xx A_d(double, Vec_x)` @@ -101,43 +103,55 @@ concept DynamicModel = requires { * @brief - `Mat_xv G_d(double, Vec_x)` * @brief - `Gauss_x pred_from_est(double, Gauss_x, Vec_u)` * @brief - `Gauss_x pred_from_state(double, Vec_x, Vec_u)` - * - * @tparam DynMod The dynamic model type + * + * @tparam DynMod The dynamic model type * @tparam n_dim_x Dimension of the state * @tparam n_dim_u Dimension of the input * @tparam n_dim_v Dimension of the process noise */ template concept DynamicModelLTV = requires { - requires DynamicModel; // Assuming DynamicModel is correctly defined as shown before - { - std::declval().A_d(std::declval(), std::declval::Vec_x>()) - } -> mat_convertible_to::Mat_xx>; - - { - std::declval().B_d(std::declval(), std::declval::Vec_x>()) - } -> mat_convertible_to::Mat_xu>; - - { - std::declval().G_d(std::declval(), std::declval::Vec_x>()) - } -> mat_convertible_to::Mat_xv>; - - { - std::declval().pred_from_est( - std::declval(), std::declval::Gauss_x>(), std::declval::Vec_u>()) - } -> std::convertible_to::Gauss_x>; - - { - std::declval().pred_from_state( - std::declval(), std::declval::Vec_x>(), std::declval::Vec_u>()) - } -> std::convertible_to::Gauss_x>; + requires DynamicModel; // Assuming DynamicModel is correctly + // defined as shown before + { + std::declval().A_d( + std::declval(), + std::declval::Vec_x>()) + } -> mat_convertible_to::Mat_xx>; + + { + std::declval().B_d( + std::declval(), + std::declval::Vec_x>()) + } -> mat_convertible_to::Mat_xu>; + + { + std::declval().G_d( + std::declval(), + std::declval::Vec_x>()) + } -> mat_convertible_to::Mat_xv>; + + { + std::declval().pred_from_est( + std::declval(), + std::declval::Gauss_x>(), + std::declval::Vec_u>()) + } -> std::convertible_to::Gauss_x>; + + { + std::declval().pred_from_state( + std::declval(), + std::declval::Vec_x>(), + std::declval::Vec_u>()) + } -> std::convertible_to::Gauss_x>; }; /** * @brief Concept for sensor models. Requires the following functions: * @brief - `Vec_z h(Vec_x, Vec_w)` * @brief - `Mat_ww R(Vec_x)` - * + * * @tparam SensMod The sensor model type * @tparam n_dim_x Dimension of the state * @tparam n_dim_z Dimension of the measurement @@ -145,17 +159,20 @@ concept DynamicModelLTV = requires { */ template concept SensorModel = requires { - { - std::declval().h(std::declval::Vec_x>(), std::declval::Vec_w>()) - } -> mat_convertible_to::Vec_z>; - { - std::declval().R(std::declval::Vec_x>()) - } -> mat_convertible_to::Mat_ww>; + { + std::declval().h( + std::declval::Vec_x>(), + std::declval::Vec_w>()) + } -> mat_convertible_to::Vec_z>; + { + std::declval().R( + std::declval::Vec_x>()) + } -> mat_convertible_to::Mat_ww>; }; - /** - * @brief Concept for sensor models with time-varying parameters. Requires the following functions: + * @brief Concept for sensor models with time-varying parameters. Requires the + * following functions: * @brief - `Vec_z h(Vec_x, Vec_w)` * @brief - `Mat_zz R(Vec_x)` * @brief - `Mat_zw H(double, Vec_x)` @@ -165,19 +182,23 @@ concept SensorModel = requires { */ template concept SensorModelLTV = requires { - requires SensorModel; - { - std::declval().H(std::declval::Vec_x>()) - } -> mat_convertible_to::Mat_zw>; - { - std::declval().C(std::declval::Vec_x>()) - } -> mat_convertible_to::Mat_zx>; - { - std::declval().pred_from_est(std::declval::Gauss_x>()) - } -> std::convertible_to::Gauss_z>; - { - std::declval().pred_from_state(std::declval::Vec_x>()) - } -> std::convertible_to::Gauss_z>; + requires SensorModel; + { + std::declval().H( + std::declval::Vec_x>()) + } -> mat_convertible_to::Mat_zw>; + { + std::declval().C( + std::declval::Vec_x>()) + } -> mat_convertible_to::Mat_zx>; + { + std::declval().pred_from_est( + std::declval::Gauss_x>()) + } -> std::convertible_to::Gauss_z>; + { + std::declval().pred_from_state( + std::declval::Vec_x>()) + } -> std::convertible_to::Gauss_z>; }; /////////////////////////////////// @@ -186,60 +207,40 @@ concept SensorModelLTV = requires { template concept DynamicModelWithDefinedSizes = requires { - { - DynMod::N_DIM_x - } -> std::convertible_to; - { - DynMod::N_DIM_v - } -> std::convertible_to; - { - DynMod::N_DIM_u - } -> std::convertible_to; - requires DynamicModel; + { DynMod::N_DIM_x } -> std::convertible_to; + { DynMod::N_DIM_v } -> std::convertible_to; + { DynMod::N_DIM_u } -> std::convertible_to; + requires DynamicModel; }; template concept DynamicModelLTVWithDefinedSizes = requires { - requires DynamicModelWithDefinedSizes; - { - DynMod::N_DIM_x - } -> std::convertible_to; - { - DynMod::N_DIM_v - } -> std::convertible_to; - { - DynMod::N_DIM_u - } -> std::convertible_to; - requires DynamicModelLTV; + requires DynamicModelWithDefinedSizes; + { DynMod::N_DIM_x } -> std::convertible_to; + { DynMod::N_DIM_v } -> std::convertible_to; + { DynMod::N_DIM_u } -> std::convertible_to; + requires DynamicModelLTV; }; template concept SensorModelWithDefinedSizes = requires { - { - SensMod::N_DIM_x - } -> std::convertible_to; - { - SensMod::N_DIM_z - } -> std::convertible_to; - { - SensMod::N_DIM_w - } -> std::convertible_to; - requires SensorModel; + { SensMod::N_DIM_x } -> std::convertible_to; + { SensMod::N_DIM_z } -> std::convertible_to; + { SensMod::N_DIM_w } -> std::convertible_to; + requires SensorModel; }; template concept SensorModelLTVWithDefinedSizes = requires { - requires SensorModelWithDefinedSizes; - { - SensMod::N_DIM_x - } -> std::convertible_to; - { - SensMod::N_DIM_z - } -> std::convertible_to; - { - SensMod::N_DIM_w - } -> std::convertible_to; - requires SensorModelLTV; + requires SensorModelWithDefinedSizes; + { SensMod::N_DIM_x } -> std::convertible_to; + { SensMod::N_DIM_z } -> std::convertible_to; + { SensMod::N_DIM_w } -> std::convertible_to; + requires SensorModelLTV; }; -} // namespace vortex::concepts \ No newline at end of file +} // namespace vortex::concepts diff --git a/vortex-filtering/include/vortex_filtering/types/type_aliases.hpp b/vortex-filtering/include/vortex_filtering/types/type_aliases.hpp index fc77a833..744efd13 100644 --- a/vortex-filtering/include/vortex_filtering/types/type_aliases.hpp +++ b/vortex-filtering/include/vortex_filtering/types/type_aliases.hpp @@ -4,89 +4,112 @@ #include #include -#define MATRIX_TYPES(t1, s1, t2, s2) \ - using Mat_##t1##t2 = Eigen::Matrix; \ - using Mat_##t2##t1 = Eigen::Matrix; - -#define ONE_TYPE(t1, s1) \ - static constexpr size_t N_DIM_##t1 = s1; \ - \ - using Vec_##t1 = Eigen::Vector; \ - using Mat_##t1##t1 = Eigen::Matrix; \ - using Gauss_##t1 = vortex::prob::Gauss; \ - using GaussMix_##t1 = vortex::prob::GaussMix; - -#define TWO_TYPES(t1, s1, t2, s2) \ - ONE_TYPE(t1, s1) \ - ONE_TYPE(t2, s2) \ - MATRIX_TYPES(t1, s1, t2, s2) - -#define THREE_TYPES(t1, s1, t2, s2, t3, s3) \ - ONE_TYPE(t1, s1) \ - ONE_TYPE(t2, s2) \ - ONE_TYPE(t3, s3) \ - MATRIX_TYPES(t1, s1, t2, s2) \ - MATRIX_TYPES(t1, s1, t3, s3) \ - MATRIX_TYPES(t2, s2, t3, s3) - -#define FOUR_TYPES(t1, s1, t2, s2, t3, s3, t4, s4) \ - ONE_TYPE(t1, s1) \ - ONE_TYPE(t2, s2) \ - ONE_TYPE(t3, s3) \ - ONE_TYPE(t4, s4) \ - MATRIX_TYPES(t1, s1, t2, s2) \ - MATRIX_TYPES(t1, s1, t3, s3) \ - MATRIX_TYPES(t1, s1, t4, s4) \ - MATRIX_TYPES(t2, s2, t3, s3) \ - MATRIX_TYPES(t2, s2, t4, s4) \ - MATRIX_TYPES(t3, s3, t4, s4) - -#define FIVE_TYPES(t1, s1, t2, s2, t3, s3, t4, s4, t5, s5) \ - ONE_TYPE(t1, s1) \ - ONE_TYPE(t2, s2) \ - ONE_TYPE(t3, s3) \ - ONE_TYPE(t4, s4) \ - ONE_TYPE(t5, s5) \ - MATRIX_TYPES(t1, s1, t2, s2) \ - MATRIX_TYPES(t1, s1, t3, s3) \ - MATRIX_TYPES(t1, s1, t4, s4) \ - MATRIX_TYPES(t1, s1, t5, s5) \ - MATRIX_TYPES(t2, s2, t3, s3) \ - MATRIX_TYPES(t2, s2, t4, s4) \ - MATRIX_TYPES(t2, s2, t5, s5) \ - MATRIX_TYPES(t3, s3, t4, s4) \ - MATRIX_TYPES(t3, s3, t5, s5) \ - MATRIX_TYPES(t4, s4, t5, s5) - -#define ONE_TYPE_STRUCT(t1) \ - template struct Types_##t1 { \ - Types_##t1() = delete; \ - ONE_TYPE(t1, n_dim_##t1) \ - }; - -#define TWO_TYPES_STRUCT(t1, t2) \ - template struct Types_##t1##t2 { \ - Types_##t1##t2() = delete; \ - TWO_TYPES(t1, n_dim_##t1, t2, n_dim_##t2) \ - }; - -#define THREE_TYPES_STRUCT(t1, t2, t3) \ - template struct Types_##t1##t2##t3 { \ - Types_##t1##t2##t3() = delete; \ - THREE_TYPES(t1, n_dim_##t1, t2, n_dim_##t2, t3, n_dim_##t3) \ - }; - -#define FOUR_TYPES_STRUCT(t1, t2, t3, t4) \ - template struct Types_##t1##t2##t3##t4 { \ - Types_##t1##t2##t3##t4() = delete; \ - FOUR_TYPES(t1, n_dim_##t1, t2, n_dim_##t2, t3, n_dim_##t3, t4, n_dim_##t4) \ - }; - -#define FIVE_TYPES_STRUCT(t1, t2, t3, t4, t5) \ - template struct Types_##t1##t2##t3##t4##t5 { \ - Types_##t1##t2##t3##t4##t5() = delete; \ - FIVE_TYPES(t1, n_dim_##t1, t2, n_dim_##t2, t3, n_dim_##t3, t4, n_dim_##t4, t5, n_dim_##t5) \ - }; +#define MATRIX_TYPES(t1, s1, t2, s2) \ + using Mat_##t1##t2 = Eigen::Matrix; \ + using Mat_##t2##t1 = Eigen::Matrix; + +#define ONE_TYPE(t1, s1) \ + static constexpr size_t N_DIM_##t1 = s1; \ + \ + using Vec_##t1 = Eigen::Vector; \ + using Mat_##t1##t1 = Eigen::Matrix; \ + using Gauss_##t1 = vortex::prob::Gauss; \ + using GaussMix_##t1 = vortex::prob::GaussMix; + +#define TWO_TYPES(t1, s1, t2, s2) \ + ONE_TYPE(t1, s1) \ + ONE_TYPE(t2, s2) \ + MATRIX_TYPES(t1, s1, t2, s2) + +#define THREE_TYPES(t1, s1, t2, s2, t3, s3) \ + ONE_TYPE(t1, s1) \ + ONE_TYPE(t2, s2) \ + ONE_TYPE(t3, s3) \ + MATRIX_TYPES(t1, s1, t2, s2) \ + MATRIX_TYPES(t1, s1, t3, s3) \ + MATRIX_TYPES(t2, s2, t3, s3) + +#define FOUR_TYPES(t1, s1, t2, s2, t3, s3, t4, s4) \ + ONE_TYPE(t1, s1) \ + ONE_TYPE(t2, s2) \ + ONE_TYPE(t3, s3) \ + ONE_TYPE(t4, s4) \ + MATRIX_TYPES(t1, s1, t2, s2) \ + MATRIX_TYPES(t1, s1, t3, s3) \ + MATRIX_TYPES(t1, s1, t4, s4) \ + MATRIX_TYPES(t2, s2, t3, s3) \ + MATRIX_TYPES(t2, s2, t4, s4) \ + MATRIX_TYPES(t3, s3, t4, s4) + +#define FIVE_TYPES(t1, s1, t2, s2, t3, s3, t4, s4, t5, s5) \ + ONE_TYPE(t1, s1) \ + ONE_TYPE(t2, s2) \ + ONE_TYPE(t3, s3) \ + ONE_TYPE(t4, s4) \ + ONE_TYPE(t5, s5) \ + MATRIX_TYPES(t1, s1, t2, s2) \ + MATRIX_TYPES(t1, s1, t3, s3) \ + MATRIX_TYPES(t1, s1, t4, s4) \ + MATRIX_TYPES(t1, s1, t5, s5) \ + MATRIX_TYPES(t2, s2, t3, s3) \ + MATRIX_TYPES(t2, s2, t4, s4) \ + MATRIX_TYPES(t2, s2, t5, s5) \ + MATRIX_TYPES(t3, s3, t4, s4) \ + MATRIX_TYPES(t3, s3, t5, s5) \ + MATRIX_TYPES(t4, s4, t5, s5) + +#define ONE_TYPE_STRUCT(t1) \ + template \ + struct Types_##t1 { \ + Types_##t1() = delete; \ + ONE_TYPE(t1, n_dim_##t1) \ + }; + +#define TWO_TYPES_STRUCT(t1, t2) \ + template \ + struct Types_##t1##t2 { \ + Types_##t1##t2() = delete; \ + TWO_TYPES(t1, n_dim_##t1, t2, n_dim_##t2) \ + }; + +#define THREE_TYPES_STRUCT(t1, t2, t3) \ + template \ + struct Types_##t1##t2##t3 { \ + Types_##t1##t2##t3() = delete; \ + THREE_TYPES(t1, n_dim_##t1, t2, n_dim_##t2, t3, n_dim_##t3) \ + }; + +#define FOUR_TYPES_STRUCT(t1, t2, t3, t4) \ + template \ + struct Types_##t1##t2##t3##t4 { \ + Types_##t1##t2##t3##t4() = delete; \ + FOUR_TYPES(t1, \ + n_dim_##t1, \ + t2, \ + n_dim_##t2, \ + t3, \ + n_dim_##t3, \ + t4, \ + n_dim_##t4) \ + }; + +#define FIVE_TYPES_STRUCT(t1, t2, t3, t4, t5) \ + template \ + struct Types_##t1##t2##t3##t4##t5 { \ + Types_##t1##t2##t3##t4##t5() = delete; \ + FIVE_TYPES(t1, \ + n_dim_##t1, \ + t2, \ + n_dim_##t2, \ + t3, \ + n_dim_##t3, \ + t4, \ + n_dim_##t4, \ + t5, \ + n_dim_##t5) \ + }; namespace vortex { // Hover over the types in vscode to see the expanded types @@ -98,22 +121,22 @@ ONE_TYPE_STRUCT(v) ONE_TYPE_STRUCT(w) ONE_TYPE_STRUCT(n) -TWO_TYPES_STRUCT(x, z) // Sensor model without noise -TWO_TYPES_STRUCT(x, v) // Dynamic model without input -TWO_TYPES_STRUCT(x, u) // Dynamic model without noise +TWO_TYPES_STRUCT(x, z) // Sensor model without noise +TWO_TYPES_STRUCT(x, v) // Dynamic model without input +TWO_TYPES_STRUCT(x, u) // Dynamic model without noise TWO_TYPES_STRUCT(x, w) TWO_TYPES_STRUCT(z, w) -THREE_TYPES_STRUCT(x, u, v) // Dynamic model -THREE_TYPES_STRUCT(x, z, w) // Sensor model -THREE_TYPES_STRUCT(x, z, n) // For IMM Filter +THREE_TYPES_STRUCT(x, u, v) // Dynamic model +THREE_TYPES_STRUCT(x, z, w) // Sensor model +THREE_TYPES_STRUCT(x, z, n) // For IMM Filter -FOUR_TYPES_STRUCT(x, z, w, a) // Sensor model and augmented state a +FOUR_TYPES_STRUCT(x, z, w, a) // Sensor model and augmented state a -FIVE_TYPES_STRUCT(x, z, u, v, w) // Dynamic model and sensor model +FIVE_TYPES_STRUCT(x, z, u, v, w) // Dynamic model and sensor model -} // namespace vortex +} // namespace vortex // Don't want you to use these macros outside of this file :) #undef ONE_TYPE diff --git a/vortex-filtering/include/vortex_filtering/utils/algorithms/auction_algorithm.hpp b/vortex-filtering/include/vortex_filtering/utils/algorithms/auction_algorithm.hpp index f55a03a1..c9ae009a 100644 --- a/vortex-filtering/include/vortex_filtering/utils/algorithms/auction_algorithm.hpp +++ b/vortex-filtering/include/vortex_filtering/utils/algorithms/auction_algorithm.hpp @@ -4,6 +4,7 @@ namespace vortex::utils { -std::pair auction_algorithm(const Eigen::MatrixXd &cost_matrix); +std::pair auction_algorithm( + const Eigen::MatrixXd& cost_matrix); -} // namespace vortex::utils +} // namespace vortex::utils diff --git a/vortex-filtering/include/vortex_filtering/utils/algorithms/murtys_method.hpp b/vortex-filtering/include/vortex_filtering/utils/algorithms/murtys_method.hpp index 953fadc4..4de01dc8 100644 --- a/vortex-filtering/include/vortex_filtering/utils/algorithms/murtys_method.hpp +++ b/vortex-filtering/include/vortex_filtering/utils/algorithms/murtys_method.hpp @@ -9,15 +9,15 @@ namespace vortex::utils { -// Concept to ensure that the assignment algorithm is compatible with the cost matrix +// Concept to ensure that the assignment algorithm is compatible with the cost +// matrix template -concept assignment_algorithm = requires(T a, const Eigen::MatrixXd &cost_matrix) { - { - a(cost_matrix) - } -> std::same_as>; -}; +concept assignment_algorithm = + requires(T a, const Eigen::MatrixXd& cost_matrix) { + { a(cost_matrix) } -> std::same_as>; + }; std::vector> murtys_method(const Eigen::MatrixXd &cost_matrix, int m, auto assignment_solver = auction_algorithm()) requires(assignment_algorithm) -} // namespace vortex::utils \ No newline at end of file +} // namespace vortex::utils diff --git a/vortex-filtering/include/vortex_filtering/utils/ellipse.hpp b/vortex-filtering/include/vortex_filtering/utils/ellipse.hpp index d7456e06..e22fa13e 100644 --- a/vortex-filtering/include/vortex_filtering/utils/ellipse.hpp +++ b/vortex-filtering/include/vortex_filtering/utils/ellipse.hpp @@ -20,82 +20,84 @@ namespace utils { * */ class Ellipse { -public: - /** Construct a new Ellipse object - * @param center center of the ellipse - * @param a half the length of the major axis (radius of the circumscribed circle) - * @param b half the length of the minor axis (radius of the inscribed circle) - * @param angle angle in radians - */ - Ellipse(const Eigen::Vector2d ¢er, double a, double b, double angle); - - /** Construct a new Ellipse object from a Gaussian - * @param gauss 2D Gaussian distribution - * @param scale_factor scale factor for the ellipse - */ - Ellipse(const vortex::prob::Gauss2d &gauss, double scale_factor = 1.0); - - /** Get the center of the ellipse - * @return Eigen::Vector2d - */ - Eigen::Vector2d center() const; - - /** Get x coordinate of the center - * @return double - */ - double x() const; - - /** Get y coordinate of the center - * @return double - */ - double y() const; - - /** Get the a parameter of the ellipse (half the length of the major axis) - * @return double - */ - double a() const; - - /** Get the b parameter of the ellipse (half the length of the minor axis) - * @return double - */ - double b() const; - - /** Get the major axis length of the ellipse - * @return double - */ - double major_axis() const; - - /** Get the minor axis length of the ellipse - * @return double - */ - double minor_axis() const; - - /** Get the axes lengths of the ellipse - * @return Eigen::Vector2d - */ - Eigen::Vector2d axes() const; - - /** Get the angle of the ellipse with respect to the x-axis - * @return double - */ - double angle_rad() const; - - /** Get the angle in degrees - * @return double - */ - double angle_deg() const; - - /** Get the area of the ellipse - * @return double - */ - double area() const; - -private: - Eigen::Vector2d center_; - double a_; - double b_; - double angle_; + public: + /** Construct a new Ellipse object + * @param center center of the ellipse + * @param a half the length of the major axis (radius of the circumscribed + * circle) + * @param b half the length of the minor axis (radius of the inscribed + * circle) + * @param angle angle in radians + */ + Ellipse(const Eigen::Vector2d& center, double a, double b, double angle); + + /** Construct a new Ellipse object from a Gaussian + * @param gauss 2D Gaussian distribution + * @param scale_factor scale factor for the ellipse + */ + Ellipse(const vortex::prob::Gauss2d& gauss, double scale_factor = 1.0); + + /** Get the center of the ellipse + * @return Eigen::Vector2d + */ + Eigen::Vector2d center() const; + + /** Get x coordinate of the center + * @return double + */ + double x() const; + + /** Get y coordinate of the center + * @return double + */ + double y() const; + + /** Get the a parameter of the ellipse (half the length of the major axis) + * @return double + */ + double a() const; + + /** Get the b parameter of the ellipse (half the length of the minor axis) + * @return double + */ + double b() const; + + /** Get the major axis length of the ellipse + * @return double + */ + double major_axis() const; + + /** Get the minor axis length of the ellipse + * @return double + */ + double minor_axis() const; + + /** Get the axes lengths of the ellipse + * @return Eigen::Vector2d + */ + Eigen::Vector2d axes() const; + + /** Get the angle of the ellipse with respect to the x-axis + * @return double + */ + double angle_rad() const; + + /** Get the angle in degrees + * @return double + */ + double angle_deg() const; + + /** Get the area of the ellipse + * @return double + */ + double area() const; + + private: + Eigen::Vector2d center_; + double a_; + double b_; + double angle_; }; -} // namespace utils -} // namespace vortex +} // namespace utils +} // namespace vortex diff --git a/vortex-filtering/include/vortex_filtering/utils/ellipsoid.hpp b/vortex-filtering/include/vortex_filtering/utils/ellipsoid.hpp index ffddf634..1f0206c8 100644 --- a/vortex-filtering/include/vortex_filtering/utils/ellipsoid.hpp +++ b/vortex-filtering/include/vortex_filtering/utils/ellipsoid.hpp @@ -8,65 +8,67 @@ namespace vortex::utils { -template class Ellipsoid { -public: - using T = Types_n; +template +class Ellipsoid { + public: + using T = Types_n; - /** Construct a new Ellipsoid object - * @param gauss n-dimensional Gaussian distribution - * @param scale_factor scale factor for the number of standard deviations (NB! This is slightly different from the ellipse scale factor) - */ - Ellipsoid(T::Gauss_n gauss, double scale_factor = 1.0) - : gauss_(gauss) - , scale_factor_(scale_factor) - { - } + /** Construct a new Ellipsoid object + * @param gauss n-dimensional Gaussian distribution + * @param scale_factor scale factor for the number of standard deviations + * (NB! This is slightly different from the ellipse scale factor) + */ + Ellipsoid(T::Gauss_n gauss, double scale_factor = 1.0) + : gauss_(gauss), scale_factor_(scale_factor) {} - /** Get the center of the ellipsoid - * @return T::Vec_n - */ - T::Vec_n center() const { return gauss_.mean(); } + /** Get the center of the ellipsoid + * @return T::Vec_n + */ + T::Vec_n center() const { return gauss_.mean(); } - /** Get the semi-major axis lengths of the ellipsoid sorted in descending order - * @return T::Vec_n - */ - T::Vec_n semi_axis_lengths() const - { - Eigen::SelfAdjointEigenSolver eigen_solver(gauss_.cov()); - typename T::Vec_n eigenvalues = eigen_solver.eigenvalues(); - typename T::Vec_n lengths = eigenvalues.array().sqrt().reverse(); - return lengths * scale_factor_; - } + /** Get the semi-major axis lengths of the ellipsoid sorted in descending + * order + * @return T::Vec_n + */ + T::Vec_n semi_axis_lengths() const { + Eigen::SelfAdjointEigenSolver eigen_solver( + gauss_.cov()); + typename T::Vec_n eigenvalues = eigen_solver.eigenvalues(); + typename T::Vec_n lengths = eigenvalues.array().sqrt().reverse(); + return lengths * scale_factor_; + } - /** Get the axis lengths of the ellipsoid sorted in descending order - * @return T::Vec_n - */ - T::Vec_n axis_lengths() const { return 2.0 * semi_axis_lengths(); } + /** Get the axis lengths of the ellipsoid sorted in descending order + * @return T::Vec_n + */ + T::Vec_n axis_lengths() const { return 2.0 * semi_axis_lengths(); } - /** Get the orthonormal axes of the ellipsoid corresponding to the eigen values sorted in descending order - * @return T::Mat_nn - */ - T::Mat_nn orthonormal_axes() const - { - Eigen::SelfAdjointEigenSolver eigen_solver(gauss_.cov()); - typename T::Mat_nn eigen_vectors = eigen_solver.eigenvectors().colwise().reverse(); - return eigen_vectors; - } + /** Get the orthonormal axes of the ellipsoid corresponding to the eigen + * values sorted in descending order + * @return T::Mat_nn + */ + T::Mat_nn orthonormal_axes() const { + Eigen::SelfAdjointEigenSolver eigen_solver( + gauss_.cov()); + typename T::Mat_nn eigen_vectors = + eigen_solver.eigenvectors().colwise().reverse(); + return eigen_vectors; + } - /** Get the volume of the ellipsoid - * @return double - */ - double volume() const - { - constexpr double scaling = std::pow(std::numbers::pi, n_dims / 2.0) / std::tgamma(n_dims / 2.0 + 1); - typename T::Vec_n lengths = semi_axis_lengths(); - double volume = scaling * lengths.prod(); - return volume; - } + /** Get the volume of the ellipsoid + * @return double + */ + double volume() const { + constexpr double scaling = std::pow(std::numbers::pi, n_dims / 2.0) / + std::tgamma(n_dims / 2.0 + 1); + typename T::Vec_n lengths = semi_axis_lengths(); + double volume = scaling * lengths.prod(); + return volume; + } -private: - typename T::Gauss_n gauss_; - double scale_factor_; + private: + typename T::Gauss_n gauss_; + double scale_factor_; }; -} // namespace vortex::utils \ No newline at end of file +} // namespace vortex::utils diff --git a/vortex-filtering/include/vortex_filtering/utils/plotting.hpp b/vortex-filtering/include/vortex_filtering/utils/plotting.hpp index 1cf98d01..74b3c3d6 100644 --- a/vortex-filtering/include/vortex_filtering/utils/plotting.hpp +++ b/vortex-filtering/include/vortex_filtering/utils/plotting.hpp @@ -1,7 +1,8 @@ /** * @file utils.hpp * @author Eirik KolÃ¥s - * @brief Utils for converting between types of this library and types suitable for plotting with gnuplot-iostream + * @brief Utils for converting between types of this library and types suitable + * for plotting with gnuplot-iostream * @version 0.1 * @date 2023-11-17 * @@ -10,8 +11,8 @@ */ #pragma once -#include #include +#include #include #include #include @@ -24,65 +25,70 @@ namespace plotting { * @param scale_factor (optional) Scale factor for the ellipse * @return Ellipse */ -utils::Ellipse gauss_to_ellipse(const vortex::prob::Gauss2d &gauss, double scale_factor = 1.0); +utils::Ellipse gauss_to_ellipse(const vortex::prob::Gauss2d& gauss, + double scale_factor = 1.0); -/** Create a normalized-error-squared NEES series from a series of errors and a covariance matrix. +/** Create a normalized-error-squared NEEDS series from a series of errors and a + * covariance matrix. * @param errors * @param covarainces - * @param indices (optional) Indices of the states to use for the NEES calculation. If empty, all indices are used. + * @param indices (optional) Indices of the states to use for the NEEDS + * calculation. If empty, all indices are used. * @return std::vector */ template -std::vector create_nees_series(const std::vector> &errors, - const std::vector> &covariances, const std::vector &indices) -{ - using Vec_n = Eigen::Vector; - using Mat_nn = Eigen::Matrix; - - std::vector nees_series; - - for (size_t i = 0; i < errors.size(); ++i) { - Vec_n error = errors[i]; - Mat_nn covariance = covariances[i]; - - // Handle indices if provided - if (!indices.empty()) { - Vec_n error_sub(indices.size()); - Mat_nn covariance_sub(indices.size(), indices.size()); - - for (size_t j = 0; j < indices.size(); ++j) { - error_sub(j) = error(indices[j]); - for (size_t k = 0; k < indices.size(); ++k) { - covariance_sub(j, k) = covariance(indices[j], indices[k]); +std::vector create_nees_series( + const std::vector>& errors, + const std::vector>& covariances, + const std::vector& indices) { + using Vec_n = Eigen::Vector; + using Mat_nn = Eigen::Matrix; + + std::vector nees_series; + + for (size_t i = 0; i < errors.size(); ++i) { + Vec_n error = errors[i]; + Mat_nn covariance = covariances[i]; + + // Handle indices if provided + if (!indices.empty()) { + Vec_n error_sub(indices.size()); + Mat_nn covariance_sub(indices.size(), indices.size()); + + for (size_t j = 0; j < indices.size(); ++j) { + error_sub(j) = error(indices[j]); + for (size_t k = 0; k < indices.size(); ++k) { + covariance_sub(j, k) = covariance(indices[j], indices[k]); + } + } + + error = error_sub; + covariance = covariance_sub; } - } - error = error_sub; - covariance = covariance_sub; + // NEEDS calculation + double needs = error.transpose() * covariance.inverse() * error; + nees_series.push_back(needs); } - // NEES calculation - double nees = error.transpose() * covariance.inverse() * error; - nees_series.push_back(nees); - } - - return nees_series; + return nees_series; } -/** Create a series of errors from a series of true states and a series of estimated states. +/** Create a series of errors from a series of true states and a series of + * estimated states. * @param x_true * @param x_est * @return std::vector */ template -std::vector> create_error_series(const std::vector> &x_true, - const std::vector> &x_est) -{ - std::vector> error_series; - for (size_t i = 0; i < x_true.size(); ++i) { - error_series.push_back(x_true[i] - x_est[i].mean()); - } - return error_series; +std::vector> create_error_series( + const std::vector>& x_true, + const std::vector>& x_est) { + std::vector> error_series; + for (size_t i = 0; i < x_true.size(); ++i) { + error_series.push_back(x_true[i] - x_est[i].mean()); + } + return error_series; } /** Extract single state from a series of states. @@ -90,51 +96,55 @@ std::vector> create_error_series(const std::vector * @param index * @return std::vector */ -template std::vector extract_state_series(const std::vector> &x_series, size_t index) -{ - std::vector state_series; - for (size_t i = 0; i < x_series.size(); ++i) { - state_series.push_back(x_series[i](index)); - } - return state_series; +template +std::vector extract_state_series( + const std::vector>& x_series, + size_t index) { + std::vector state_series; + for (size_t i = 0; i < x_series.size(); ++i) { + state_series.push_back(x_series[i](index)); + } + return state_series; } /** Extract mean from a series of Gaussians. * @param x_series * @return std::vector */ -template std::vector> extract_mean_series(const std::vector> &x_series) -{ - std::vector> mean_series; - for (size_t i = 0; i < x_series.size(); ++i) { - mean_series.push_back(x_series[i].mean()); - } - return mean_series; +template +std::vector> extract_mean_series( + const std::vector>& x_series) { + std::vector> mean_series; + for (size_t i = 0; i < x_series.size(); ++i) { + mean_series.push_back(x_series[i].mean()); + } + return mean_series; } /** Approximate Gaussian from samples. * @param samples * @return vortex::prob::Gauss_n */ -template vortex::prob::Gauss approximate_gaussian(const std::vector> &samples) -{ - using Vec_n = Eigen::Vector; - using Mat_nn = Eigen::Matrix; - - Vec_n mean = Vec_n::Zero(); - for (const auto &sample : samples) { - mean += sample; - } - mean /= samples.size(); - - Mat_nn cov = Mat_nn::Zero(); - for (const auto &sample : samples) { - cov += (sample - mean) * (sample - mean).transpose(); - } - cov /= samples.size(); - - return {mean, cov}; +template +vortex::prob::Gauss approximate_gaussian( + const std::vector>& samples) { + using Vec_n = Eigen::Vector; + using Mat_nn = Eigen::Matrix; + + Vec_n mean = Vec_n::Zero(); + for (const auto& sample : samples) { + mean += sample; + } + mean /= samples.size(); + + Mat_nn cov = Mat_nn::Zero(); + for (const auto& sample : samples) { + cov += (sample - mean) * (sample - mean).transpose(); + } + cov /= samples.size(); + + return {mean, cov}; } -} // namespace plotting -} // namespace vortex +} // namespace plotting +} // namespace vortex diff --git a/vortex-filtering/include/vortex_filtering/utils/printers.hpp b/vortex-filtering/include/vortex_filtering/utils/printers.hpp index e4b85867..ce683d1a 100644 --- a/vortex-filtering/include/vortex_filtering/utils/printers.hpp +++ b/vortex-filtering/include/vortex_filtering/utils/printers.hpp @@ -4,26 +4,30 @@ #include namespace detail { -template std::ostream &println_tuple_impl(std::ostream &os, std::tuple tuple, std::index_sequence) -{ - static_assert(sizeof...(Is) == sizeof...(Ts), "Indices must have same number of elements as tuple types!"); - static_assert(sizeof...(Ts) > 0, "Cannot insert empty tuple into stream."); - size_t last = sizeof...(Ts) - 1; // assuming index sequence 0,...,N-1 +template +std::ostream& println_tuple_impl(std::ostream& os, + std::tuple tuple, + std::index_sequence) { + static_assert(sizeof...(Is) == sizeof...(Ts), + "Indices must have same number of elements as tuple types!"); + static_assert(sizeof...(Ts) > 0, "Cannot insert empty tuple into stream."); + size_t last = sizeof...(Ts) - 1; // assuming index sequence 0,...,N-1 - return ((os << std::get(tuple) << (Is != last ? "\r\n" : "")), ...); + return ((os << std::get(tuple) << (Is != last ? "\r\n" : "")), ...); } -} // namespace detail +} // namespace detail -template std::ostream &operator<<(std::ostream &os, const std::tuple &tuple) -{ - return detail::println_tuple_impl(os, tuple, std::index_sequence_for{}); +template +std::ostream& operator<<(std::ostream& os, const std::tuple& tuple) { + return detail::println_tuple_impl(os, tuple, + std::index_sequence_for{}); } -template std::ostream &operator<<(std::ostream &os, const std::array &arr) -{ - size_t last = N - 1; - for (std::size_t i = 0; i < N; ++i) { - os << arr[i] << (i != last ? "\r\n" : ""); - } - return os; -} \ No newline at end of file +template +std::ostream& operator<<(std::ostream& os, const std::array& arr) { + size_t last = N - 1; + for (std::size_t i = 0; i < N; ++i) { + os << arr[i] << (i != last ? "\r\n" : ""); + } + return os; +} diff --git a/vortex-filtering/src/utils/algorithms/auction_algorithm.cpp b/vortex-filtering/src/utils/algorithms/auction_algorithm.cpp index 1c84aaf1..98a9143b 100644 --- a/vortex-filtering/src/utils/algorithms/auction_algorithm.cpp +++ b/vortex-filtering/src/utils/algorithms/auction_algorithm.cpp @@ -4,44 +4,44 @@ namespace vortex::utils { -std::pair auction_algorithm(const Eigen::MatrixXd &cost_matrix) -{ - int num_items = cost_matrix.cols(); - Eigen::VectorXi assignment = Eigen::VectorXi::Constant(num_items, -1); - Eigen::VectorXd prices = Eigen::VectorXd::Zero(num_items); - - std::vector unassigned; - for (int i = 0; i < num_items; ++i) { - unassigned.push_back(i); - } - - double epsilon = 1.0 / (num_items + 1); - - while (!unassigned.empty()) { - int customer = unassigned.back(); - unassigned.pop_back(); - - double max_value = std::numeric_limits::lowest(); - int max_item = -1; - for (int item = 0; item < num_items; ++item) { - double value = cost_matrix(customer, item) - prices[item]; - if (value > max_value) { - max_value = value; - max_item = item; - } +std::pair auction_algorithm( + const Eigen::MatrixXd& cost_matrix) { + int num_items = cost_matrix.cols(); + Eigen::VectorXi assignment = Eigen::VectorXi::Constant(num_items, -1); + Eigen::VectorXd prices = Eigen::VectorXd::Zero(num_items); + + std::vector unassigned; + for (int i = 0; i < num_items; ++i) { + unassigned.push_back(i); } - int current_owner = assignment[max_item]; - if (current_owner != -1) { - unassigned.push_back(current_owner); + double epsilon = 1.0 / (num_items + 1); + + while (!unassigned.empty()) { + int customer = unassigned.back(); + unassigned.pop_back(); + + double max_value = std::numeric_limits::lowest(); + int max_item = -1; + for (int item = 0; item < num_items; ++item) { + double value = cost_matrix(customer, item) - prices[item]; + if (value > max_value) { + max_value = value; + max_item = item; + } + } + + int current_owner = assignment[max_item]; + if (current_owner != -1) { + unassigned.push_back(current_owner); + } + + assignment[max_item] = customer; + prices[max_item] += max_value + epsilon; } - assignment[max_item] = customer; - prices[max_item] += max_value + epsilon; - } - - double total_cost = prices.sum(); - return {total_cost, assignment}; + double total_cost = prices.sum(); + return {total_cost, assignment}; } -} // namespace vortex::utils \ No newline at end of file +} // namespace vortex::utils diff --git a/vortex-filtering/src/utils/algorithms/murtys_method.cpp b/vortex-filtering/src/utils/algorithms/murtys_method.cpp index 6c4aff94..8adfa033 100644 --- a/vortex-filtering/src/utils/algorithms/murtys_method.cpp +++ b/vortex-filtering/src/utils/algorithms/murtys_method.cpp @@ -10,23 +10,29 @@ namespace vortex::utils { using CostValuePair = std::pair; -std::vector murtys_method(const Eigen::MatrixXd &cost_matrix, int num_solutions, auto assignment_solver) -{ - std::vector R; // Solution set R - auto comp = [](const CostValuePair &a, const CostValuePair &b) { return a.first < b.first; }; - std::priority_queue, decltype(comp)> Q(comp); - - Q.push(assignment_solver(cost_matrix)); // Add the first problem to Q with its cost - - while (R.size() < num_solutions && !Q.empty()) { - auto [_, Q_max] = Q.top(); // Fetch the next problem to solve - Q.pop(); - R.push_back(Q_max); - // Generate subproblems based on the last solution added to R - auto Q_ = partition(Q_max.first, Q_max.second); - } - - return R; // Return the set of solutions found +std::vector murtys_method(const Eigen::MatrixXd& cost_matrix, + int num_solutions, + auto assignment_solver) { + std::vector R; // Solution set R + auto comp = [](const CostValuePair& a, const CostValuePair& b) { + return a.first < b.first; + }; + std::priority_queue, + decltype(comp)> + Q(comp); + + Q.push(assignment_solver( + cost_matrix)); // Add the first problem to Q with its cost + + while (R.size() < num_solutions && !Q.empty()) { + auto [_, Q_max] = Q.top(); // Fetch the next problem to solve + Q.pop(); + R.push_back(Q_max); + // Generate subproblems based on the last solution added to R + auto Q_ = partition(Q_max.first, Q_max.second); + } + + return R; // Return the set of solutions found } -} // namespace vortex::utils \ No newline at end of file +} // namespace vortex::utils diff --git a/vortex-filtering/src/utils/algorithms/murtys_method_impl.cpp b/vortex-filtering/src/utils/algorithms/murtys_method_impl.cpp index ba94fc3b..c95e41c9 100644 --- a/vortex-filtering/src/utils/algorithms/murtys_method_impl.cpp +++ b/vortex-filtering/src/utils/algorithms/murtys_method_impl.cpp @@ -8,29 +8,32 @@ #include #include -std::vector> partition(const Eigen::MatrixXd &P, const Eigen::VectorXi &S) -{ - std::vector> Q_prime; +std::vector> partition( + const Eigen::MatrixXd& P, + const Eigen::VectorXi& S) { + std::vector> Q_prime; - // Each assignment in S is excluded once to create a new subproblem - for (int i = 0; i < S.size(); ++i) { - // Skip invalid assignments - if (S(i) == -1) - continue; + // Each assignment in S is excluded once to create a new subproblem + for (int i = 0; i < S.size(); ++i) { + // Skip invalid assignments + if (S(i) == -1) + continue; - // Create a copy of the cost matrix and set a high cost for the current assignment to prevent it - Eigen::MatrixXd new_P = P; - new_P(i, S(i)) = std::numeric_limits::max(); + // Create a copy of the cost matrix and set a high cost for the current + // assignment to prevent it + Eigen::MatrixXd new_P = P; + new_P(i, S(i)) = std::numeric_limits::max(); - Q_prime.emplace_back(0, new_P); // The cost (0) will be recalculated when solving the subproblem - } + Q_prime.emplace_back(0, new_P); // The cost (0) will be recalculated + // when solving the subproblem + } - return Q_prime; + return Q_prime; } -bool is_valid_solution(const Eigen::VectorXi &solution) -{ - // A solution is valid if it does not contain any forbidden assignments - // In this context, a forbidden assignment could be represented by -1 or any other marker used - return !solution.isConstant(-1); +bool is_valid_solution(const Eigen::VectorXi& solution) { + // A solution is valid if it does not contain any forbidden assignments + // In this context, a forbidden assignment could be represented by -1 or any + // other marker used + return !solution.isConstant(-1); } diff --git a/vortex-filtering/src/utils/algorithms/murtys_method_impl.hpp b/vortex-filtering/src/utils/algorithms/murtys_method_impl.hpp index 81ec803e..c916b717 100644 --- a/vortex-filtering/src/utils/algorithms/murtys_method_impl.hpp +++ b/vortex-filtering/src/utils/algorithms/murtys_method_impl.hpp @@ -2,6 +2,8 @@ #include #include -std::vector> partition(const Eigen::MatrixXd &P, const Eigen::VectorXi &S); +std::vector> partition( + const Eigen::MatrixXd& P, + const Eigen::VectorXi& S); -bool is_valid_solution(const Eigen::VectorXi &solution); +bool is_valid_solution(const Eigen::VectorXi& solution); diff --git a/vortex-filtering/src/utils/ellipse.cpp b/vortex-filtering/src/utils/ellipse.cpp index 78f0cbf5..487943af 100644 --- a/vortex-filtering/src/utils/ellipse.cpp +++ b/vortex-filtering/src/utils/ellipse.cpp @@ -1,43 +1,67 @@ #include #include -#include using std::numbers::pi; namespace vortex { namespace utils { -Ellipse::Ellipse(const Eigen::Vector2d ¢er, double a, double b, double angle) : center_(center), a_(a), b_(b), angle_(angle) {} - -Ellipse::Ellipse(const vortex::prob::Gauss2d &gauss, double scale_factor) -{ - Eigen::SelfAdjointEigenSolver eigenSolver(gauss.cov()); - Eigen::Vector2d eigenValues = eigenSolver.eigenvalues(); - Eigen::Matrix2d eigenVectors = eigenSolver.eigenvectors(); - - // Sort eigenvalues in descending order - if (eigenValues(0) > eigenValues(1)) { - std::swap(eigenValues(0), eigenValues(1)); - eigenVectors.col(0).swap(eigenVectors.col(1)); - } - - a_ = sqrt(scale_factor * eigenValues(1)); - b_ = sqrt(scale_factor * eigenValues(0)); - angle_ = atan2(eigenVectors(1, 1), eigenVectors(0, 1)); - center_ = gauss.mean(); -} - -Eigen::Vector2d Ellipse::center() const { return center_; } -double Ellipse::x() const { return center_(0); } -double Ellipse::y() const { return center_(1); } -double Ellipse::a() const { return a_; } -double Ellipse::b() const { return b_; } -double Ellipse::major_axis() const { return 2 * a_; } -double Ellipse::minor_axis() const { return 2 * b_; } -Eigen::Vector2d Ellipse::axes() const { return Eigen::Vector2d(2 * a_, 2 * b_); } -double Ellipse::angle_rad() const { return angle_; } -double Ellipse::angle_deg() const { return angle_ * 180 / std::numbers::pi; } -double Ellipse::area() const { return std::numbers::pi * a() * b(); } - -} // namespace utils -} // namespace vortex +Ellipse::Ellipse(const Eigen::Vector2d& center, + double a, + double b, + double angle) + : center_(center), a_(a), b_(b), angle_(angle) {} + +Ellipse::Ellipse(const vortex::prob::Gauss2d& gauss, double scale_factor) { + Eigen::SelfAdjointEigenSolver eigenSolver(gauss.cov()); + Eigen::Vector2d eigenValues = eigenSolver.eigenvalues(); + Eigen::Matrix2d eigenVectors = eigenSolver.eigenvectors(); + + // Sort eigenvalues in descending order + if (eigenValues(0) > eigenValues(1)) { + std::swap(eigenValues(0), eigenValues(1)); + eigenVectors.col(0).swap(eigenVectors.col(1)); + } + + a_ = sqrt(scale_factor * eigenValues(1)); + b_ = sqrt(scale_factor * eigenValues(0)); + angle_ = atan2(eigenVectors(1, 1), eigenVectors(0, 1)); + center_ = gauss.mean(); +} + +Eigen::Vector2d Ellipse::center() const { + return center_; +} +double Ellipse::x() const { + return center_(0); +} +double Ellipse::y() const { + return center_(1); +} +double Ellipse::a() const { + return a_; +} +double Ellipse::b() const { + return b_; +} +double Ellipse::major_axis() const { + return 2 * a_; +} +double Ellipse::minor_axis() const { + return 2 * b_; +} +Eigen::Vector2d Ellipse::axes() const { + return Eigen::Vector2d(2 * a_, 2 * b_); +} +double Ellipse::angle_rad() const { + return angle_; +} +double Ellipse::angle_deg() const { + return angle_ * 180 / std::numbers::pi; +} +double Ellipse::area() const { + return std::numbers::pi * a() * b(); +} + +} // namespace utils +} // namespace vortex diff --git a/vortex-filtering/src/utils/gamma_function.cpp b/vortex-filtering/src/utils/gamma_function.cpp index 76af3595..d3e3a64b 100644 --- a/vortex-filtering/src/utils/gamma_function.cpp +++ b/vortex-filtering/src/utils/gamma_function.cpp @@ -1,6 +1,6 @@ #include -#include #include +#include using std::numbers::pi; @@ -8,7 +8,7 @@ constexpr double gamma_function(double x) { if (x == 0.0) { return std::numeric_limits::infinity(); } - + if (x < 0.0) { return pi / (std::sin(pi * x) * gamma_function(1 - x)); } @@ -18,20 +18,19 @@ constexpr double gamma_function(double x) { result /= x; x += 1.0; } - + constexpr double p[] = { - 0.99999999999980993, 676.5203681218851, -1259.1392167224028, - 771.32342877765313, -176.61502916214059, 12.507343278686905, - -0.13857109526572012, 9.9843695780195716e-6, 1.5056327351493116e-7 - }; - + 0.99999999999980993, 676.5203681218851, -1259.1392167224028, + 771.32342877765313, -176.61502916214059, 12.507343278686905, + -0.13857109526572012, 9.9843695780195716e-6, 1.5056327351493116e-7}; + double y = x; double tmp = x + 5.5; tmp -= (x + 0.5) * std::log(tmp); - double ser = 1.000000000190015; + double set = 1.000000000190015; for (int i = 0; i < 9; ++i) { - ser += p[i] / ++y; + set += p[i] / ++y; } - - return std::exp(-tmp + std::log(2.5066282746310005 * ser / x)); -} \ No newline at end of file + + return std::exp(-tmp + std::log(2.5066282746310005 * set / x)); +} diff --git a/vortex-filtering/src/utils/plotting.cpp b/vortex-filtering/src/utils/plotting.cpp index 748f4b40..2321ecd2 100644 --- a/vortex-filtering/src/utils/plotting.cpp +++ b/vortex-filtering/src/utils/plotting.cpp @@ -4,7 +4,10 @@ namespace vortex { namespace plotting { -utils::Ellipse gauss_to_ellipse(const vortex::prob::Gauss2d &gauss, double scale_factor) { return utils::Ellipse(gauss, scale_factor); } +utils::Ellipse gauss_to_ellipse(const vortex::prob::Gauss2d& gauss, + double scale_factor) { + return utils::Ellipse(gauss, scale_factor); +} -} // namespace plotting -} // namespace vortex +} // namespace plotting +} // namespace vortex diff --git a/vortex-filtering/test/CMakeLists.txt b/vortex-filtering/test/CMakeLists.txt index cd61349e..cda100b9 100644 --- a/vortex-filtering/test/CMakeLists.txt +++ b/vortex-filtering/test/CMakeLists.txt @@ -1,5 +1,5 @@ find_package(ament_cmake_gtest REQUIRED) -ament_add_gtest(${PROJECT_NAME}_test +ament_add_gtest(${PROJECT_NAME}_test gtest_main.cpp gtest_assertions.cpp ../src/utils/plotting.cpp @@ -28,7 +28,7 @@ ament_target_dependencies(${PROJECT_NAME}_test Gnuplot Boost ) -target_link_libraries(${PROJECT_NAME}_test +target_link_libraries(${PROJECT_NAME}_test Eigen3::Eigen # Makes us able to use #include instead of #include OpenMP::OpenMP_CXX -) \ No newline at end of file +) diff --git a/vortex-filtering/test/dynamic_model_test.cpp b/vortex-filtering/test/dynamic_model_test.cpp index 4ea02e1f..bbd40d9a 100644 --- a/vortex-filtering/test/dynamic_model_test.cpp +++ b/vortex-filtering/test/dynamic_model_test.cpp @@ -12,82 +12,88 @@ namespace simple_dynamic_model_test { -using Vec_x = typename SimpleDynamicModel::T::Vec_x; +using Vec_x = typename SimpleDynamicModel::T::Vec_x; using Mat_xx = typename SimpleDynamicModel::T::Mat_xx; -TEST(DynamicModel, initSimpleModel) { SimpleDynamicModel model; } +TEST(DynamicModel, initSimpleModel) { + SimpleDynamicModel model; +} -TEST(DynamicModel, iterateSimpleModel) -{ - SimpleDynamicModel model; - double dt = 1.0; - Vec_x x = Vec_x::Zero(); +TEST(DynamicModel, iterateSimpleModel) { + SimpleDynamicModel model; + double dt = 1.0; + Vec_x x = Vec_x::Zero(); - for (size_t i = 0; i < 10; i++) { - EXPECT_EQ(model.f_d(dt, x), std::exp(-dt) * x); - x = model.f_d(dt, x); - } + for (size_t i = 0; i < 10; i++) { + EXPECT_EQ(model.f_d(dt, x), std::exp(-dt) * x); + x = model.f_d(dt, x); + } } -TEST(DynamicModel, sampleSimpleModel) -{ - // Test that output is Gaussian - SimpleDynamicModel model; - double dt = 1.0; - Vec_x x = Vec_x::Ones(); - - vortex::prob::Gauss2d true_gauss = model.pred_from_state(dt, x); - - std::random_device rd; - std::mt19937 gen(rd()); - - std::vector samples; - for (size_t i = 0; i < 10000; i++) { - samples.push_back(model.sample_f_d(dt, x, Vec_x::Zero(), gen)); - } - - vortex::prob::Gauss2d approx_gauss = vortex::plotting::approximate_gaussian(samples); - - EXPECT_TRUE(isApproxEqual(approx_gauss.mean(), true_gauss.mean(), 0.1)); - EXPECT_TRUE(isApproxEqual(true_gauss.cov(), true_gauss.cov(), 0.1)); - - #if (GNUPLOT_ENABLE) - // Plot - Gnuplot gp; - gp << "set xrange [-10:10]\nset yrange [-10:10]\n"; - gp << "set style circle radius 0.05\n"; - gp << "plot '-' with circles title 'Samples' fs transparent solid 0.05 noborder\n"; - gp.send1d(samples); - - vortex::utils::Ellipse cov_ellipse = vortex::plotting::gauss_to_ellipse(true_gauss); - gp << "set object 1 ellipse center " << cov_ellipse.x() << "," << cov_ellipse.y() << " size " << 3 * cov_ellipse.a() << "," << 3 * cov_ellipse.b() - << " angle " << cov_ellipse.angle_deg() << "fs empty border lc rgb 'cyan'\n"; - gp << "replot\n"; - #endif +TEST(DynamicModel, sampleSimpleModel) { + // Test that output is Gaussian + SimpleDynamicModel model; + double dt = 1.0; + Vec_x x = Vec_x::Ones(); + + vortex::prob::Gauss2d true_gauss = model.pred_from_state(dt, x); + + std::random_device rd; + std::mt19937 gen(rd()); + + std::vector samples; + for (size_t i = 0; i < 10000; i++) { + samples.push_back(model.sample_f_d(dt, x, Vec_x::Zero(), gen)); + } + + vortex::prob::Gauss2d approx_gauss = + vortex::plotting::approximate_gaussian(samples); + + EXPECT_TRUE(isApproxEqual(approx_gauss.mean(), true_gauss.mean(), 0.1)); + EXPECT_TRUE(isApproxEqual(true_gauss.cov(), true_gauss.cov(), 0.1)); + +#if (GNUPLOT_ENABLE) + // Plot + Gnuplot gp; + gp << "set xrange [-10:10]\nset yrange [-10:10]\n"; + gp << "set style circle radius 0.05\n"; + gp << "plot '-' with circles title 'Samples' fs transparent solid 0.05 " + "noborder\n"; + gp.send1d(samples); + + vortex::utils::Ellipse cov_ellipse = + vortex::plotting::gauss_to_ellipse(true_gauss); + gp << "set object 1 ellipse center " << cov_ellipse.x() << "," + << cov_ellipse.y() << " size " << 3 * cov_ellipse.a() << "," + << 3 * cov_ellipse.b() << " angle " << cov_ellipse.angle_deg() + << "fs empty border lc rgb 'cyan'\n"; + gp << "replot\n"; +#endif } -} // namespace simple_dynamic_model_test +} // namespace simple_dynamic_model_test namespace cv_model_test { using CVModel = vortex::models::ConstantVelocity; -using Vec_x = typename CVModel::T::Vec_x; -using Mat_xx = typename CVModel::T::Mat_xx; - -TEST(DynamicModel, initCVModel) { CVModel model(1.0); } - -TEST(DynamicModel, iterateCVModel) -{ - CVModel model(1.0); - double dt = 1.0; - Vec_x x; - x << 0, 0, 1, 1; - - for (size_t i = 0; i < 10; i++) { - Vec_x x_true; - x_true << x(0) + dt, x(1) + dt, 1, 1; - EXPECT_EQ(model.f_d(dt, x), x_true); - x = model.f_d(dt, x); - } +using Vec_x = typename CVModel::T::Vec_x; +using Mat_xx = typename CVModel::T::Mat_xx; + +TEST(DynamicModel, initCVModel) { + CVModel model(1.0); +} + +TEST(DynamicModel, iterateCVModel) { + CVModel model(1.0); + double dt = 1.0; + Vec_x x; + x << 0, 0, 1, 1; + + for (size_t i = 0; i < 10; i++) { + Vec_x x_true; + x_true << x(0) + dt, x(1) + dt, 1, 1; + EXPECT_EQ(model.f_d(dt, x), x_true); + x = model.f_d(dt, x); + } } -} // namespace cv_model_test +} // namespace cv_model_test diff --git a/vortex-filtering/test/ekf_test.cpp b/vortex-filtering/test/ekf_test.cpp index c6144fce..e9e75235 100644 --- a/vortex-filtering/test/ekf_test.cpp +++ b/vortex-filtering/test/ekf_test.cpp @@ -1,130 +1,130 @@ #include #include -#include "gtest_assertions.hpp" -#include "test_models.hpp" #include #include #include #include #include #include +#include "gtest_assertions.hpp" +#include "test_models.hpp" class EKFTestCVModel : public ::testing::Test { -protected: - using PosMeasModel = vortex::models::IdentitySensorModel<4, 2>; - using CVModel = vortex::models::ConstantVelocity; - using Vec_x = typename CVModel::T::Vec_x; - using Mat_xx = typename CVModel::T::Mat_xx; - using Gauss_x = typename CVModel::T::Gauss_x; - using Gauss_z = typename PosMeasModel::T::Gauss_z; - using Vec_z = typename PosMeasModel::T::Vec_z; - using Vec_u = typename CVModel::T::Vec_u; + protected: + using PosMeasModel = vortex::models::IdentitySensorModel<4, 2>; + using CVModel = vortex::models::ConstantVelocity; + using Vec_x = typename CVModel::T::Vec_x; + using Mat_xx = typename CVModel::T::Mat_xx; + using Gauss_x = typename CVModel::T::Gauss_x; + using Gauss_z = typename PosMeasModel::T::Gauss_z; + using Vec_z = typename PosMeasModel::T::Vec_z; + using Vec_u = typename CVModel::T::Vec_u; - using EKF = vortex::filter::EKF; + using EKF = vortex::filter::EKF; - EKFTestCVModel() - : dynamic_model_(1e-2) - , sensor_model_(1e-2) - { - } - CVModel dynamic_model_; - PosMeasModel sensor_model_; + EKFTestCVModel() : dynamic_model_(1e-2), sensor_model_(1e-2) {} + CVModel dynamic_model_; + PosMeasModel sensor_model_; }; -TEST_F(EKFTestCVModel, predict) -{ - // Initial state - Gauss_x x({0, 0, 1, 0}, Mat_xx::Identity()); - double dt = 0.1; - // Predict - auto [x_est_pred, z_est_pred] = EKF::predict(dynamic_model_, sensor_model_, dt, x); +TEST_F(EKFTestCVModel, predict) { + // Initial state + Gauss_x x({0, 0, 1, 0}, Mat_xx::Identity()); + double dt = 0.1; + // Predict + auto [x_est_pred, z_est_pred] = + EKF::predict(dynamic_model_, sensor_model_, dt, x); - Vec_x x_true = x.mean() + Vec_x({dt, 0, 0, 0}); - Vec_z z_true = x_true.head(2); - ASSERT_EQ(x_est_pred.mean(), x_true); - ASSERT_EQ(z_est_pred.mean(), z_true); + Vec_x x_true = x.mean() + Vec_x({dt, 0, 0, 0}); + Vec_z z_true = x_true.head(2); + ASSERT_EQ(x_est_pred.mean(), x_true); + ASSERT_EQ(z_est_pred.mean(), z_true); } -TEST_F(EKFTestCVModel, convergence) -{ - // Random number generator - std::random_device rd; - std::mt19937 gen(rd()); - std::normal_distribution<> d_disturbance{0, 1e-3}; - std::normal_distribution<> d_noise{0, 1e-2}; +TEST_F(EKFTestCVModel, convergence) { + // Random number generator + std::random_device rd; + std::mt19937 gen(rd()); + std::normal_distribution<> d_disturbance{0, 1e-3}; + std::normal_distribution<> d_noise{0, 1e-2}; - // Initial state - Gauss_x x0({0, 0, 0.5, 0}, Mat_xx::Identity()); - double dt = 0.1; + // Initial state + Gauss_x x0({0, 0, 0.5, 0}, Mat_xx::Identity()); + double dt = 0.1; - std::vector time; - std::vector x_true; - std::vector x_est; - std::vector z_meas; - std::vector z_est; + std::vector time; + std::vector x_true; + std::vector x_est; + std::vector z_meas; + std::vector z_est; - // Simulate - time.push_back(0); - x_true.push_back(x0.mean()); - x_est.push_back(x0); - for (int i = 0; i < 100; i++) { // Simulate - Vec_x x_true_i = dynamic_model_.sample_f_d(dt, x_true.back(), Vec_u::Zero(), gen); - Vec_z z_meas_i = sensor_model_.sample_h(x_true_i, gen); - x_true.push_back(x_true_i); - z_meas.push_back(z_meas_i); + time.push_back(0); + x_true.push_back(x0.mean()); + x_est.push_back(x0); + for (int i = 0; i < 100; i++) { + // Simulate + Vec_x x_true_i = + dynamic_model_.sample_f_d(dt, x_true.back(), Vec_u::Zero(), gen); + Vec_z z_meas_i = sensor_model_.sample_h(x_true_i, gen); + x_true.push_back(x_true_i); + z_meas.push_back(z_meas_i); - // Predict and update - auto [x_est_upd, x_est_pred, z_est_pred] = EKF::step(dynamic_model_, sensor_model_, dt, x_est.back(), z_meas_i); + // Predict and update + auto [x_est_upd, x_est_pred, z_est_pred] = EKF::step( + dynamic_model_, sensor_model_, dt, x_est.back(), z_meas_i); - // Save results - time.push_back(time.back() + dt); - x_est.push_back(x_est_upd); - z_est.push_back(z_est_pred); - } + // Save results + time.push_back(time.back() + dt); + x_est.push_back(x_est_upd); + z_est.push_back(z_est_pred); + } - // Test that the state converges to the true state - EXPECT_TRUE(isApproxEqual(x_est.back().mean(), x_true.back(), 1e-1)); + // Test that the state converges to the true state + EXPECT_TRUE(isApproxEqual(x_est.back().mean(), x_true.back(), 1e-1)); - // Plot the results - std::vector x_true_x, x_true_y, x_true_u, x_true_v, x_est_x, x_est_y, x_est_u, x_est_v, z_meas_x, z_meas_y; - for (size_t i = 0; i < x_true.size() - 1; i++) { - x_true_x.push_back(x_true.at(i)(0)); - x_true_y.push_back(x_true.at(i)(1)); - x_true_u.push_back(x_true.at(i)(2)); - x_true_v.push_back(x_true.at(i)(3)); - x_est_x.push_back(x_est.at(i).mean()(0)); - x_est_y.push_back(x_est.at(i).mean()(1)); - x_est_u.push_back(x_est.at(i).mean()(2)); - x_est_v.push_back(x_est.at(i).mean()(3)); - z_meas_x.push_back(z_meas.at(i)(0)); - z_meas_y.push_back(z_meas.at(i)(1)); - } - time.pop_back(); + // Plot the results + std::vector x_true_x, x_true_y, x_true_u, x_true_v, x_est_x, + x_est_y, x_est_u, x_est_v, z_meas_x, z_meas_y; + for (size_t i = 0; i < x_true.size() - 1; i++) { + x_true_x.push_back(x_true.at(i)(0)); + x_true_y.push_back(x_true.at(i)(1)); + x_true_u.push_back(x_true.at(i)(2)); + x_true_v.push_back(x_true.at(i)(3)); + x_est_x.push_back(x_est.at(i).mean()(0)); + x_est_y.push_back(x_est.at(i).mean()(1)); + x_est_u.push_back(x_est.at(i).mean()(2)); + x_est_v.push_back(x_est.at(i).mean()(3)); + z_meas_x.push_back(z_meas.at(i)(0)); + z_meas_y.push_back(z_meas.at(i)(1)); + } + time.pop_back(); - #if (GNUPLOT_ENABLE) - Gnuplot gp; - gp << "set terminal qt size 1600,1000\n"; // Modified to make plot larger - gp << "set multiplot layout 2,1\n"; - gp << "set title 'Position'\n"; - gp << "set xlabel 'x [m]'\n"; - gp << "set ylabel 'y [m]'\n"; - gp << "plot '-' with lines title 'True', '-' with lines title 'Estimate', '-' with points title 'Measurements' ps 2\n"; // Modified to make dots larger - gp.send1d(std::make_tuple(x_true_x, x_true_y)); - gp.send1d(std::make_tuple(x_est_x, x_est_y)); - gp.send1d(std::make_tuple(z_meas_x, z_meas_y)); - gp << "set title 'Velocity'\n"; - gp << "set xlabel 't [s]'\n"; - gp << "set ylabel 'u,v [m/s]'\n"; - gp << "plot '-' with lines title 'True v', " - << "'-' with lines title 'Estimate v', " - << "'-' with lines title 'True u', " - << "'-' with lines title 'Estimate u'\n"; - gp.send1d(std::make_tuple(time, x_true_v)); - gp.send1d(std::make_tuple(time, x_est_v)); - gp.send1d(std::make_tuple(time, x_true_u)); - gp.send1d(std::make_tuple(time, x_est_u)); - gp << "unset multiplot\n"; - #endif -} \ No newline at end of file +#if (GNUPLOT_ENABLE) + Gnuplot gp; + gp << "set terminal qt size 1600,1000\n"; // Modified to make plot larger + gp << "set multiplot layout 2,1\n"; + gp << "set title 'Position'\n"; + gp << "set xlabel 'x [m]'\n"; + gp << "set ylabel 'y [m]'\n"; + gp << "plot '-' with lines title 'True', '-' with lines title 'Estimate', " + "'-' with points title 'Measurements' ps 2\n"; // Modified to make + // dots larger + gp.send1d(std::make_tuple(x_true_x, x_true_y)); + gp.send1d(std::make_tuple(x_est_x, x_est_y)); + gp.send1d(std::make_tuple(z_meas_x, z_meas_y)); + gp << "set title 'Velocity'\n"; + gp << "set xlabel 't [s]'\n"; + gp << "set ylabel 'u,v [m/s]'\n"; + gp << "plot '-' with lines title 'True v', " + << "'-' with lines title 'Estimate v', " + << "'-' with lines title 'True u', " + << "'-' with lines title 'Estimate u'\n"; + gp.send1d(std::make_tuple(time, x_true_v)); + gp.send1d(std::make_tuple(time, x_est_v)); + gp.send1d(std::make_tuple(time, x_true_u)); + gp.send1d(std::make_tuple(time, x_est_u)); + gp << "unset multiplot\n"; +#endif +} diff --git a/vortex-filtering/test/ellipsoid_test.cpp b/vortex-filtering/test/ellipsoid_test.cpp index 07d62732..8cb4a319 100644 --- a/vortex-filtering/test/ellipsoid_test.cpp +++ b/vortex-filtering/test/ellipsoid_test.cpp @@ -5,70 +5,70 @@ #include #include -TEST(Ellipse, constructorFromParams) -{ - Eigen::Vector2d center(1.0, 2.0); - double a = 3.0; - double b = 2.0; - double angle = std::numbers::pi / 2.0; - Eigen::Matrix2d cov; - vortex::utils::Ellipse ellipse(center, a, b, angle); +TEST(Ellipse, constructorFromParams) { + Eigen::Vector2d center(1.0, 2.0); + double a = 3.0; + double b = 2.0; + double angle = std::numbers::pi / 2.0; + Eigen::Matrix2d cov; + vortex::utils::Ellipse ellipse(center, a, b, angle); - EXPECT_EQ(ellipse.center(), center); - EXPECT_EQ(ellipse.a(), a); - EXPECT_EQ(ellipse.b(), b); + EXPECT_EQ(ellipse.center(), center); + EXPECT_EQ(ellipse.a(), a); + EXPECT_EQ(ellipse.b(), b); } -TEST(Ellipse, constructorFromGauss) -{ - Eigen::Vector2d center(1.0, 2.0); - double a = 3.0; - double b = 2.0; - double angle = std::numbers::pi / 2.0; +TEST(Ellipse, constructorFromGauss) { + Eigen::Vector2d center(1.0, 2.0); + double a = 3.0; + double b = 2.0; + double angle = std::numbers::pi / 2.0; - Eigen::Matrix2d eigenvectors{{std::cos(angle), -std::sin(angle)}, {std::sin(angle), std::cos(angle)}}; - Eigen::Vector2d eigenvalues = {a * a, b * b}; + Eigen::Matrix2d eigenvectors{{std::cos(angle), -std::sin(angle)}, + {std::sin(angle), std::cos(angle)}}; + Eigen::Vector2d eigenvalues = {a * a, b * b}; - Eigen::Matrix2d cov = eigenvectors * eigenvalues.asDiagonal() * eigenvectors.transpose(); + Eigen::Matrix2d cov = + eigenvectors * eigenvalues.asDiagonal() * eigenvectors.transpose(); - vortex::prob::Gauss2d gauss(center, cov); - vortex::utils::Ellipse ellipse(gauss); + vortex::prob::Gauss2d gauss(center, cov); + vortex::utils::Ellipse ellipse(gauss); - EXPECT_EQ(ellipse.center(), center); - EXPECT_EQ(ellipse.a(), a); - EXPECT_EQ(ellipse.b(), b); + EXPECT_EQ(ellipse.center(), center); + EXPECT_EQ(ellipse.a(), a); + EXPECT_EQ(ellipse.b(), b); } -TEST(Ellipsoid, constructorFromGauss) -{ - Eigen::Vector2d center(1.0, 2.0); - Eigen::Matrix2d cov{{1.0, 0.0}, {0.0, 9.0}}; - vortex::prob::Gauss2d gauss(center, cov); - vortex::utils::Ellipsoid<2> ellipsoid(gauss); +TEST(Ellipsoid, constructorFromGauss) { + Eigen::Vector2d center(1.0, 2.0); + Eigen::Matrix2d cov{{1.0, 0.0}, {0.0, 9.0}}; + vortex::prob::Gauss2d gauss(center, cov); + vortex::utils::Ellipsoid<2> ellipsoid(gauss); - EXPECT_EQ(ellipsoid.center(), center); - EXPECT_EQ(ellipsoid.semi_axis_lengths()(0), 3.0); - EXPECT_EQ(ellipsoid.semi_axis_lengths()(1), 1.0); - EXPECT_EQ(ellipsoid.volume(), std::numbers::pi * 3.0 * 1.0); + EXPECT_EQ(ellipsoid.center(), center); + EXPECT_EQ(ellipsoid.semi_axis_lengths()(0), 3.0); + EXPECT_EQ(ellipsoid.semi_axis_lengths()(1), 1.0); + EXPECT_EQ(ellipsoid.volume(), std::numbers::pi * 3.0 * 1.0); } -TEST(Ellipsoid, sameAsEllipse) -{ - Eigen::Vector2d center(1.0, 2.0); - double a = 3.0; - double b = 2.0; - double angle = std::numbers::pi / 2.0; +TEST(Ellipsoid, sameAsEllipse) { + Eigen::Vector2d center(1.0, 2.0); + double a = 3.0; + double b = 2.0; + double angle = std::numbers::pi / 2.0; - Eigen::Matrix2d eigenvectors{{std::cos(angle), -std::sin(angle)}, {std::sin(angle), std::cos(angle)}}; - Eigen::Vector2d eigenvalues = {a * a, b * b}; + Eigen::Matrix2d eigenvectors{{std::cos(angle), -std::sin(angle)}, + {std::sin(angle), std::cos(angle)}}; + Eigen::Vector2d eigenvalues = {a * a, b * b}; - Eigen::Matrix2d cov = eigenvectors * eigenvalues.asDiagonal() * eigenvectors.transpose(); + Eigen::Matrix2d cov = + eigenvectors * eigenvalues.asDiagonal() * eigenvectors.transpose(); - vortex::prob::Gauss2d gauss(center, cov); - vortex::utils::Ellipsoid<2> ellipsoid(gauss); - vortex::utils::Ellipse ellipse(gauss); + vortex::prob::Gauss2d gauss(center, cov); + vortex::utils::Ellipsoid<2> ellipsoid(gauss); + vortex::utils::Ellipse ellipse(gauss); - EXPECT_EQ(ellipsoid.center(), ellipse.center()); - EXPECT_EQ(ellipsoid.axis_lengths(), ellipse.axes()); - EXPECT_EQ(ellipsoid.volume(), ellipse.area()); -} \ No newline at end of file + EXPECT_EQ(ellipsoid.center(), ellipse.center()); + EXPECT_EQ(ellipsoid.axis_lengths(), ellipse.axes()); + EXPECT_EQ(ellipsoid.volume(), ellipse.area()); +} diff --git a/vortex-filtering/test/gtest_assertions.cpp b/vortex-filtering/test/gtest_assertions.cpp index 9afd09f1..106cd6b5 100644 --- a/vortex-filtering/test/gtest_assertions.cpp +++ b/vortex-filtering/test/gtest_assertions.cpp @@ -6,23 +6,26 @@ * @param tol tolerance * @return testing::AssertionResult */ -testing::AssertionResult isApproxEqual(const Eigen::MatrixXd &a, const Eigen::MatrixXd &b, double tol) -{ - if (a.rows() != b.rows() || a.cols() != b.cols()) { - return testing::AssertionFailure() << "Matrices are not the same size,\n" - << "a:\n" - << a << "\n, b:\n" - << b; - } - for (int i = 0; i < a.rows(); ++i) { - for (int j = 0; j < a.cols(); ++j) { - if (std::abs(a(i, j) - b(i, j)) > tol) { - return testing::AssertionFailure() << "Matrices are not the same,\n" - << "a:\n" - << a << "\n, b\n" - << b; - } +testing::AssertionResult isApproxEqual(const Eigen::MatrixXd& a, + const Eigen::MatrixXd& b, + double tol) { + if (a.rows() != b.rows() || a.cols() != b.cols()) { + return testing::AssertionFailure() + << "Matrices are not the same size,\n" + << "a:\n" + << a << "\n, b:\n" + << b; } - } - return testing::AssertionSuccess(); -} \ No newline at end of file + for (int i = 0; i < a.rows(); ++i) { + for (int j = 0; j < a.cols(); ++j) { + if (std::abs(a(i, j) - b(i, j)) > tol) { + return testing::AssertionFailure() + << "Matrices are not the same,\n" + << "a:\n" + << a << "\n, b\n" + << b; + } + } + } + return testing::AssertionSuccess(); +} diff --git a/vortex-filtering/test/gtest_assertions.hpp b/vortex-filtering/test/gtest_assertions.hpp index 061b3818..3b3d42e9 100644 --- a/vortex-filtering/test/gtest_assertions.hpp +++ b/vortex-filtering/test/gtest_assertions.hpp @@ -1,5 +1,7 @@ #pragma once -#include #include +#include -testing::AssertionResult isApproxEqual(const Eigen::MatrixXd &a, const Eigen::MatrixXd &b, double tol); +testing::AssertionResult isApproxEqual(const Eigen::MatrixXd& a, + const Eigen::MatrixXd& b, + double tol); diff --git a/vortex-filtering/test/gtest_main.cpp b/vortex-filtering/test/gtest_main.cpp index f52c0191..98bf2274 100644 --- a/vortex-filtering/test/gtest_main.cpp +++ b/vortex-filtering/test/gtest_main.cpp @@ -1,11 +1,10 @@ -#include #include +#include -int main(int argc, char **argv) -{ - Eigen::initParallel(); - Eigen::setNbThreads(8); +int main(int argc, char** argv) { + Eigen::initParallel(); + Eigen::setNbThreads(8); - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} \ No newline at end of file + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/vortex-filtering/test/imm_test.cpp b/vortex-filtering/test/imm_test.cpp index 50d54742..40bfcf8a 100644 --- a/vortex-filtering/test/imm_test.cpp +++ b/vortex-filtering/test/imm_test.cpp @@ -1,7 +1,7 @@ #include "gtest_assertions.hpp" -#include #include +#include #include #include #include @@ -15,355 +15,400 @@ // IMM Model Tests /////////////////////////////// -TEST(ImmModel, initWithStateNames) -{ - using vortex::models::ConstantPosition; - using vortex::models::ConstantVelocity; +TEST(ImmModel, initWithStateNames) { + using vortex::models::ConstantPosition; + using vortex::models::ConstantVelocity; - Eigen::Matrix2d jump_mat{{0, 1}, {1, 0}}; - Eigen::Vector2d hold_times{1, 1}; + Eigen::Matrix2d jump_mat{{0, 1}, {1, 0}}; + Eigen::Vector2d hold_times{1, 1}; - double std = 1.0; - double dt = 1.0; + double std = 1.0; + double dt = 1.0; - using ImmModelT = vortex::models::ImmModel; - ImmModelT imm_model{jump_mat, hold_times, ConstantPosition(std), ConstantVelocity(std)}; + using ImmModelT = + vortex::models::ImmModel; + ImmModelT imm_model{jump_mat, hold_times, ConstantPosition(std), + ConstantVelocity(std)}; - EXPECT_EQ(typeid(imm_model.get_model<0>()), typeid(ConstantPosition)); - EXPECT_EQ(typeid(imm_model.get_model<1>()), typeid(ConstantVelocity)); - EXPECT_EQ(typeid(imm_model.f_d<0>(dt, Eigen::Vector2d::Zero())), typeid(Eigen::Vector2d)); - EXPECT_EQ(typeid(imm_model.f_d<1>(dt, Eigen::Vector4d::Zero())), typeid(Eigen::Vector4d)); + EXPECT_EQ(typeid(imm_model.get_model<0>()), typeid(ConstantPosition)); + EXPECT_EQ(typeid(imm_model.get_model<1>()), typeid(ConstantVelocity)); + EXPECT_EQ(typeid(imm_model.f_d<0>(dt, Eigen::Vector2d::Zero())), + typeid(Eigen::Vector2d)); + EXPECT_EQ(typeid(imm_model.f_d<1>(dt, Eigen::Vector4d::Zero())), + typeid(Eigen::Vector4d)); } -TEST(ImmModel, piMatC) -{ - using vortex::models::ConstantPosition; - using vortex::models::ConstantVelocity; +TEST(ImmModel, piMatC) { + using vortex::models::ConstantPosition; + using vortex::models::ConstantVelocity; - // clang-format off + // clang-format off Eigen::Matrix3d jump_mat{ - {0 , 1.0/2, 1.0/2}, - {1.0/3, 0 , 2.0/3}, + {0 , 1.0/2, 1.0/2}, + {1.0/3, 0 , 2.0/3}, {5.0/6, 1.0/6, 0 } }; - // clang-format on + // clang-format on - Eigen::Vector3d hold_times{1.0 / 6, 1.0 / 12, 1.0 / 18}; - double std = 1.0; + Eigen::Vector3d hold_times{1.0 / 6, 1.0 / 12, 1.0 / 18}; + double std = 1.0; - using ImmModelT = vortex::models::ImmModel; + using ImmModelT = + vortex::models::ImmModel; - ImmModelT imm_model(jump_mat, hold_times, ConstantPosition(std), ConstantPosition(std), ConstantPosition(std)); + ImmModelT imm_model(jump_mat, hold_times, ConstantPosition(std), + ConstantPosition(std), ConstantPosition(std)); - // clang-format off + // clang-format off Eigen::Matrix3d pi_mat_c{ {-6, 3 , 3 }, {4 , -12, 8 }, {15, 3 , -18} }; - // clang-format on + // clang-format on - EXPECT_EQ(imm_model.get_pi_mat_c(), pi_mat_c); + EXPECT_EQ(imm_model.get_pi_mat_c(), pi_mat_c); } -TEST(ImmModel, piMatD) -{ - using vortex::models::ConstantPosition; - using vortex::models::ConstantVelocity; +TEST(ImmModel, piMatD) { + using vortex::models::ConstantPosition; + using vortex::models::ConstantVelocity; - // clang-format off + // clang-format off Eigen::Matrix3d jump_mat{ - {0 , 1.0/2, 1.0/2}, - {1.0/3, 0 , 2.0/3}, + {0 , 1.0/2, 1.0/2}, + {1.0/3, 0 , 2.0/3}, {5.0/6, 1.0/6, 0 } }; - // clang-format on - Eigen::Vector3d hold_times{1.0 / 6, 1.0 / 12, 1.0 / 18}; - double std = 1.0; + // clang-format on + Eigen::Vector3d hold_times{1.0 / 6, 1.0 / 12, 1.0 / 18}; + double std = 1.0; - using ImmModelT = vortex::models::ImmModel; + using ImmModelT = + vortex::models::ImmModel; - ImmModelT imm_model(jump_mat, hold_times, ConstantPosition(std), ConstantPosition(std), ConstantPosition(std)); + ImmModelT imm_model(jump_mat, hold_times, ConstantPosition(std), + ConstantPosition(std), ConstantPosition(std)); - Eigen::Matrix3d pi_mat_d_true; - // clang-format off + Eigen::Matrix3d pi_mat_d_true; + // clang-format off pi_mat_d_true << 64.0/105.0 + 1.0/(42.0 * exp(21.0)) + 11.0/(30.0 * exp(15.0)) , 1.0/5.0 - 1.0/(5.0 * exp(15.0)), 4.0/21.0 - 1.0/(42.0 * exp(21.0)) - 1.0/(6.0 * exp(15.0)), 64.0/105.0 + 6.0/(7.0 * exp(21.0)) - 22.0/(15.0 * exp(15.0)) , 1.0/5.0 + 4.0/(5.0 * exp(15.0)), 4.0/21.0 - 6.0/(7.0 * exp(21.0)) + 2.0/(3.0 * exp(15.0)), 64.0/105.0 - 41.0/(42.0 * exp(21.0)) + 11.0/(30.0 * exp(15.0)), 1.0/5.0 - 1.0/(5.0 * exp(15.0)), 4.0/21.0 + 41.0/(42.0 * exp(21.0)) - 1.0/(6.0 * exp(15.0)); - // clang-format on + // clang-format on - Eigen::Matrix3d pi_mat_d = imm_model.get_pi_mat_d(1.0); + Eigen::Matrix3d pi_mat_d = imm_model.get_pi_mat_d(1.0); - EXPECT_EQ(isApproxEqual(pi_mat_d, pi_mat_d_true, 1e-6), true); + EXPECT_EQ(isApproxEqual(pi_mat_d, pi_mat_d_true, 1e-6), true); - // Check that each row sums to 1 - for (int i = 0; i < pi_mat_d.rows(); i++) { - EXPECT_NEAR(pi_mat_d.row(i).sum(), 1.0, 1e-9); - } + // Check that each row sums to 1 + for (int i = 0; i < pi_mat_d.rows(); i++) { + EXPECT_NEAR(pi_mat_d.row(i).sum(), 1.0, 1e-9); + } } -TEST(ImmModel, stateSize) -{ - using vortex::models::ConstantAcceleration; - using vortex::models::ConstantPosition; - using vortex::models::ConstantVelocity; +TEST(ImmModel, stateSize) { + using vortex::models::ConstantAcceleration; + using vortex::models::ConstantPosition; + using vortex::models::ConstantVelocity; - using TestModel = vortex::models::ImmModel; + using TestModel = + vortex::models::ImmModel; - EXPECT_EQ(TestModel::N_DIMS_x, (std::array{2, 4, 6})); + EXPECT_EQ(TestModel::N_DIMS_x, (std::array{2, 4, 6})); } /////////////////////////////// // IMM Filter Tests /////////////////////////////// -TEST(ImmFilter, init) -{ - using vortex::models::ConstantPosition; - using SensModT = vortex::models::IdentitySensorModel<2, 1>; - using ImmModelT = vortex::models::ImmModel; - using ImmFilterT = vortex::filter::ImmFilter; +TEST(ImmFilter, init) { + using vortex::models::ConstantPosition; + using SensModT = vortex::models::IdentitySensorModel<2, 1>; + using ImmModelT = + vortex::models::ImmModel; + using ImmFilterT = vortex::filter::ImmFilter; - EXPECT_EQ(ImmFilterT::N_MODELS, 2u); - EXPECT_EQ(ImmFilterT::N_DIM_z, SensModT::N_DIM_z); + EXPECT_EQ(ImmFilterT::N_MODELS, 2u); + EXPECT_EQ(ImmFilterT::N_DIM_z, SensModT::N_DIM_z); } -TEST(ImmFilter, calculateMixingProbs) -{ - using vortex::models::ConstantPosition; +TEST(ImmFilter, calculateMixingProbs) { + using vortex::models::ConstantPosition; - Eigen::Matrix2d jump_mat{{0, 1}, {1, 0}}; - Eigen::Vector2d hold_times{1, 1}; + Eigen::Matrix2d jump_mat{{0, 1}, {1, 0}}; + Eigen::Vector2d hold_times{1, 1}; - double dt = 1.0; - double std = 1.0; + double dt = 1.0; + double std = 1.0; - using ImmModelT = vortex::models::ImmModel; + using ImmModelT = + vortex::models::ImmModel; - ImmModelT imm_model(jump_mat, hold_times, {std}, {std}); + ImmModelT imm_model(jump_mat, hold_times, {std}, {std}); - auto sensor_model = std::make_shared>(dt); + auto sensor_model = + std::make_shared>(dt); - using ImmFilterT = vortex::filter::ImmFilter>; + using ImmFilterT = + vortex::filter::ImmFilter>; - Eigen::Vector2d model_weights = {0.5, 0.5}; + Eigen::Vector2d model_weights = {0.5, 0.5}; - // Since the weights are equal, the mixing probabilities should be equal to the discrete time Markov chain - Eigen::Matrix2d mixing_probs_true = imm_model.get_pi_mat_d(dt); - Eigen::Matrix2d mixing_probs = ImmFilterT::calculate_mixing_probs(imm_model.get_pi_mat_d(dt), model_weights); - EXPECT_EQ(isApproxEqual(mixing_probs, mixing_probs_true, 1e-6), true); + // Since the weights are equal, the mixing probabilities should be equal to + // the discrete time Markov chain + Eigen::Matrix2d mixing_probs_true = imm_model.get_pi_mat_d(dt); + Eigen::Matrix2d mixing_probs = ImmFilterT::calculate_mixing_probs( + imm_model.get_pi_mat_d(dt), model_weights); + EXPECT_EQ(isApproxEqual(mixing_probs, mixing_probs_true, 1e-6), true); - // When all of the weight is in the first model, the probability that the previous model was the second model should be 0 - model_weights = {1, 0}; - mixing_probs_true = Eigen::Matrix2d{{1, 1}, {0, 0}}; + // When all of the weight is in the first model, the probability that the + // previous model was the second model should be 0 + model_weights = {1, 0}; + mixing_probs_true = Eigen::Matrix2d{{1, 1}, {0, 0}}; - mixing_probs = ImmFilterT::calculate_mixing_probs(imm_model.get_pi_mat_d(dt), model_weights); - EXPECT_EQ(isApproxEqual(mixing_probs, mixing_probs_true, 1e-6), true); + mixing_probs = ImmFilterT::calculate_mixing_probs( + imm_model.get_pi_mat_d(dt), model_weights); + EXPECT_EQ(isApproxEqual(mixing_probs, mixing_probs_true, 1e-6), true); } -TEST(ImmFilter, mixing_two_of_the_same_model) -{ - using vortex::models::ConstantPosition; - using vortex::prob::Gauss2d; +TEST(ImmFilter, mixing_two_of_the_same_model) { + using vortex::models::ConstantPosition; + using vortex::prob::Gauss2d; - Eigen::Matrix2d jump_mat{{0, 1}, {1, 0}}; - Eigen::Vector2d hold_times{1, 1}; + Eigen::Matrix2d jump_mat{{0, 1}, {1, 0}}; + Eigen::Vector2d hold_times{1, 1}; - double dt = 1; - double pos_std = 1; + double dt = 1; + double pos_std = 1; - using ImmModelT = vortex::models::ImmModel; + using ImmModelT = + vortex::models::ImmModel; - ImmModelT imm_model(jump_mat, hold_times, {pos_std}, {pos_std}); + ImmModelT imm_model(jump_mat, hold_times, {pos_std}, {pos_std}); - auto sensor_model = std::make_shared>(dt); + auto sensor_model = + std::make_shared>(dt); - using ImmFilterT = vortex::filter::ImmFilter>; + using ImmFilterT = + vortex::filter::ImmFilter>; - Eigen::Vector2d model_weights{0.5, 0.5}; + Eigen::Vector2d model_weights{0.5, 0.5}; - Gauss2d x_est_prev_1{Gauss2d::Standard()}; - Gauss2d x_est_prev_2{{1, 0}, Eigen::Matrix2d::Identity() * 100}; + Gauss2d x_est_prev_1{Gauss2d::Standard()}; + Gauss2d x_est_prev_2{{1, 0}, Eigen::Matrix2d::Identity() * 100}; - std::tuple x_est_prevs{x_est_prev_1, x_est_prev_2}; + std::tuple x_est_prevs{x_est_prev_1, x_est_prev_2}; - auto [x_est_1, x_est_2] = ImmFilterT::mixing(x_est_prevs, imm_model.get_pi_mat_d(dt)); + auto [x_est_1, x_est_2] = + ImmFilterT::mixing(x_est_prevs, imm_model.get_pi_mat_d(dt)); - // The high uncertainty in the second model should make it's position estimate move more towards the first - // model than the first model moves towards the second - std::cout << "x_est_prev_1:\n" << x_est_prev_1 << std::endl; - std::cout << "x_est_prev_2:\n" << x_est_prev_2 << std::endl; - std::cout << "x_est_1:\n" << x_est_1 << std::endl; - std::cout << "x_est_2:\n" << x_est_2 << std::endl; + // The high uncertainty in the second model should make it's position + // estimate move more towards the first model than the first model moves + // towards the second + std::cout << "x_est_prev_1:\n" << x_est_prev_1 << std::endl; + std::cout << "x_est_prev_2:\n" << x_est_prev_2 << std::endl; + std::cout << "x_est_1:\n" << x_est_1 << std::endl; + std::cout << "x_est_2:\n" << x_est_2 << std::endl; - EXPECT_LT((x_est_2.mean() - x_est_prev_2.mean()).norm(), (x_est_1.mean() - x_est_prev_1.mean()).norm()); + EXPECT_LT((x_est_2.mean() - x_est_prev_2.mean()).norm(), + (x_est_1.mean() - x_est_prev_1.mean()).norm()); } -TEST(ImmFilter, mixing_two_different_models) -{ - using namespace vortex::models; - using namespace vortex::filter; - using namespace vortex::prob; +TEST(ImmFilter, mixing_two_different_models) { + using namespace vortex::models; + using namespace vortex::filter; + using namespace vortex::prob; - Eigen::Matrix2d jump_mat{{0, 1}, {1, 0}}; - Eigen::Vector2d hold_times{1, 1}; + Eigen::Matrix2d jump_mat{{0, 1}, {1, 0}}; + Eigen::Vector2d hold_times{1, 1}; - using ImmModelT = ImmModel; - using ImmFilterT = vortex::filter::ImmFilter>; + using ImmModelT = ImmModel; + using ImmFilterT = + vortex::filter::ImmFilter>; - double dt = 1; - double std_pos = 0.1; - double std_vel = 0.1; + double dt = 1; + double std_pos = 0.1; + double std_vel = 0.1; - ImmModelT imm_model{jump_mat, hold_times, {std_pos}, {std_vel}}; + ImmModelT imm_model{jump_mat, hold_times, {std_pos}, {std_vel}}; - auto sensor_model = std::make_shared>(dt); + auto sensor_model = std::make_shared>(dt); - Gauss2d x_est_prev_1{Gauss2d::Standard()}; - Gauss4d x_est_prev_2{{1, 0, 0, 0}, Eigen::Matrix4d::Identity() * 100}; + Gauss2d x_est_prev_1{Gauss2d::Standard()}; + Gauss4d x_est_prev_2{{1, 0, 0, 0}, Eigen::Matrix4d::Identity() * 100}; - std::tuple x_est_prevs{x_est_prev_1, x_est_prev_2}; + std::tuple x_est_prevs{x_est_prev_1, x_est_prev_2}; - // clang-format off + // clang-format off vortex::StateMap states_min_max{ {vortex::StateName::velocity, {-100 , 100}} }; - // clang-format on + // clang-format on - auto [x_est_1, x_est_2] = ImmFilterT::mixing(x_est_prevs, imm_model.get_pi_mat_d(dt), states_min_max); + auto [x_est_1, x_est_2] = ImmFilterT::mixing( + x_est_prevs, imm_model.get_pi_mat_d(dt), states_min_max); - std::cout << "x_est_prev_1:\n" << x_est_prev_1 << std::endl; - std::cout << "x_est_prev_2:\n" << x_est_prev_2 << std::endl; - std::cout << "x_est_1:\n" << x_est_1 << std::endl; - std::cout << "x_est_2:\n" << x_est_2 << std::endl; + std::cout << "x_est_prev_1:\n" << x_est_prev_1 << std::endl; + std::cout << "x_est_prev_2:\n" << x_est_prev_2 << std::endl; + std::cout << "x_est_1:\n" << x_est_1 << std::endl; + std::cout << "x_est_2:\n" << x_est_2 << std::endl; - // EXPECT_LT((x_est_2.mean().head<2>() - x_est_prev_2.mean().head<2>()).norm(), (x_est_1.mean() - x_est_prev_1).norm()); + // EXPECT_LT((x_est_2.mean().head<2>() - + // x_est_prev_2.mean().head<2>()).norm(), (x_est_1.mean() - + // x_est_prev_1).norm()); } -TEST(ImmFilter, modeMatchedFilter) -{ - using namespace vortex::models; - using namespace vortex::filter; - using namespace vortex::prob; +TEST(ImmFilter, modeMatchedFilter) { + using namespace vortex::models; + using namespace vortex::filter; + using namespace vortex::prob; - Eigen::Matrix2d jump_mat{{0, 1}, {1, 0}}; - Eigen::Vector2d hold_times{1, 1}; + Eigen::Matrix2d jump_mat{{0, 1}, {1, 0}}; + Eigen::Vector2d hold_times{1, 1}; - using ImmModelT = ImmModel; - using ImmFilterT = vortex::filter::ImmFilter>; + using ImmModelT = ImmModel; + using ImmFilterT = + vortex::filter::ImmFilter>; - double dt = 1; - double std_pos = 0.1; - double std_vel = 0.1; + double dt = 1; + double std_pos = 0.1; + double std_vel = 0.1; - ImmModelT imm_model{jump_mat, hold_times, {std_pos}, {std_vel}}; + ImmModelT imm_model{jump_mat, hold_times, {std_pos}, {std_vel}}; - IdentitySensorModel<2, 2> sensor_model{dt}; + IdentitySensorModel<2, 2> sensor_model{dt}; - std::tuple x_est_prevs = {Gauss2d::Standard(), {{0, 0, 0.9, 0}, Eigen::Matrix4d::Identity()}}; - Eigen::Vector2d z_meas = {1, 0}; + std::tuple x_est_prevs = { + Gauss2d::Standard(), {{0, 0, 0.9, 0}, Eigen::Matrix4d::Identity()}}; + Eigen::Vector2d z_meas = {1, 0}; - auto [x_est_upds, x_est_preds, z_est_preds] = ImmFilterT::mode_matched_filter(imm_model, sensor_model, dt, x_est_prevs, z_meas); + auto [x_est_upds, x_est_preds, z_est_preds] = + ImmFilterT::mode_matched_filter(imm_model, sensor_model, dt, + x_est_prevs, z_meas); - std::cout << "x_est_upds:\n" << x_est_upds << std::endl; - std::cout << "x_est_preds:\n" << x_est_preds << std::endl; - std::cout << "z_est_preds:\n" << z_est_preds << std::endl; + std::cout << "x_est_upds:\n" << x_est_upds << std::endl; + std::cout << "x_est_preds:\n" << x_est_preds << std::endl; + std::cout << "z_est_preds:\n" << z_est_preds << std::endl; - EXPECT_EQ(ImmFilterT::N_MODELS, std::tuple_size::value); - EXPECT_EQ(ImmFilterT::N_MODELS, std::tuple_size::value); - EXPECT_EQ(ImmFilterT::N_MODELS, z_est_preds.size()); + EXPECT_EQ(ImmFilterT::N_MODELS, + std::tuple_size::value); + EXPECT_EQ(ImmFilterT::N_MODELS, + std::tuple_size::value); + EXPECT_EQ(ImmFilterT::N_MODELS, z_est_preds.size()); - // Expect the second filter to predict closer to the measurement - double dist_to_meas_0 = (std::get<0>(x_est_preds).mean().head<2>() - z_meas).norm(); - double dist_to_meas_1 = (std::get<1>(x_est_preds).mean().head<2>() - z_meas).norm(); + // Expect the second filter to predict closer to the measurement + double dist_to_meas_0 = + (std::get<0>(x_est_preds).mean().head<2>() - z_meas).norm(); + double dist_to_meas_1 = + (std::get<1>(x_est_preds).mean().head<2>() - z_meas).norm(); - EXPECT_LT(dist_to_meas_1, dist_to_meas_0); + EXPECT_LT(dist_to_meas_1, dist_to_meas_0); } -TEST(ImmFilter, updateProbabilities) -{ - using namespace vortex::models; - using namespace vortex::filter; - using namespace vortex::prob; +TEST(ImmFilter, updateProbabilities) { + using namespace vortex::models; + using namespace vortex::filter; + using namespace vortex::prob; - double dt = 1; + double dt = 1; - Eigen::Matrix2d jump_mat{{0, 1}, {1, 0}}; - Eigen::Vector2d hold_times{1, 1}; - double std_pos = 1; - double std_vel = 1; + Eigen::Matrix2d jump_mat{{0, 1}, {1, 0}}; + Eigen::Vector2d hold_times{1, 1}; + double std_pos = 1; + double std_vel = 1; - using ImmModelT = ImmModel; - using ImmFilterT = vortex::filter::ImmFilter>; + using ImmModelT = ImmModel; + using ImmFilterT = + vortex::filter::ImmFilter>; - ImmModelT imm_model{jump_mat, hold_times, {std_pos}, {std_vel}}; + ImmModelT imm_model{jump_mat, hold_times, {std_pos}, {std_vel}}; - auto sensor_model = std::make_shared>(dt); + auto sensor_model = std::make_shared>(dt); - Eigen::Vector2d model_weights; - model_weights << 0.5, 0.5; + Eigen::Vector2d model_weights; + model_weights << 0.5, 0.5; - Eigen::Vector2d z_meas = {1, 1}; + Eigen::Vector2d z_meas = {1, 1}; - std::array z_preds = {Gauss2d::Standard(), {{1, 1}, Eigen::Matrix2d::Identity()}}; + std::array z_preds = {Gauss2d::Standard(), + {{1, 1}, Eigen::Matrix2d::Identity()}}; - Eigen::Vector2d upd_weights = ImmFilterT::update_probabilities(imm_model.get_pi_mat_d(dt), z_preds, z_meas, model_weights); + Eigen::Vector2d upd_weights = ImmFilterT::update_probabilities( + imm_model.get_pi_mat_d(dt), z_preds, z_meas, model_weights); - EXPECT_GT(upd_weights(1), upd_weights(0)); + EXPECT_GT(upd_weights(1), upd_weights(0)); } -TEST(ImmFilter, step) -{ - using namespace vortex::models; - using namespace vortex::filter; - using namespace vortex::prob; - using vortex::StateName; +TEST(ImmFilter, step) { + using namespace vortex::models; + using namespace vortex::filter; + using namespace vortex::prob; + using vortex::StateName; - double dt = 1; - // clang-format off + double dt = 1; + // clang-format off Eigen::Matrix3d jump_mat{ - {0, 0.5, 0.5}, - {0.5, 0, 0.5}, + {0, 0.5, 0.5}, + {0.5, 0, 0.5}, {0.5, 0.5, 0}}; - // clang-format on - Eigen::Vector3d hold_times{10, 10, 10}; - double std_pos = 0.1; - double std_vel = 0.1; - double std_turn_rate = 0.1; - - using ImmModelT = ImmModel; - using ImmFilterT = vortex::filter::ImmFilter>; - - ImmModelT imm_model{jump_mat, hold_times, ConstantPosition(std_pos), ConstantVelocity(std_vel), CoordinatedTurn(std_vel, std_turn_rate)}; - - IdentitySensorModel<2, 2> sensor_model{dt}; - - Eigen::Vector3d model_weights{1 / 3.0, 1 / 3.0, 1 / 3.0}; - - std::tuple x_est_prevs = { - Gauss2d::Standard(), {{0, 0, 0.9, 0}, Eigen::Matrix4d::Identity()}, {{0, 0, 0.9, 0, 1}, Eigen::Matrix::Identity()}}; - Eigen::Vector2d z_meas = {1, 0}; - - vortex::StateMap states_min_max{{StateName::velocity, {-10, 10}}, {StateName::turn_rate, {-std::numbers::pi, std::numbers::pi}}}; - - auto [weights_upd, x_est_upds, x_est_preds, z_est_preds] = ImmFilterT::step(imm_model, sensor_model, dt, x_est_prevs, z_meas, model_weights, states_min_max); - - EXPECT_EQ(ImmFilterT::N_MODELS, std::tuple_size::value); - EXPECT_EQ(ImmFilterT::N_MODELS, std::tuple_size::value); - EXPECT_EQ(ImmFilterT::N_MODELS, z_est_preds.size()); - EXPECT_EQ(ImmFilterT::N_MODELS, weights_upd.size()); - - for (int i = 2; i < 50; i++) { - z_meas << i, 0; - std::tie(weights_upd, x_est_upds, std::ignore, std::ignore) = - ImmFilterT::step(imm_model, sensor_model, dt, x_est_upds, z_meas, weights_upd, states_min_max); - } - - std::cout << "weights_upd:\n" << weights_upd << std::endl; - std::cout << "x_est_upds:\n" << x_est_upds << std::endl; - - // Expect the constant velocity model to have the highest probability - EXPECT_GT(weights_upd(1), weights_upd(0)); - EXPECT_GT(weights_upd(1), weights_upd(2)); -} \ No newline at end of file + // clang-format on + Eigen::Vector3d hold_times{10, 10, 10}; + double std_pos = 0.1; + double std_vel = 0.1; + double std_turn_rate = 0.1; + + using ImmModelT = + ImmModel; + using ImmFilterT = + vortex::filter::ImmFilter>; + + ImmModelT imm_model{jump_mat, hold_times, ConstantPosition(std_pos), + ConstantVelocity(std_vel), + CoordinatedTurn(std_vel, std_turn_rate)}; + + IdentitySensorModel<2, 2> sensor_model{dt}; + + Eigen::Vector3d model_weights{1 / 3.0, 1 / 3.0, 1 / 3.0}; + + std::tuple x_est_prevs = { + Gauss2d::Standard(), + {{0, 0, 0.9, 0}, Eigen::Matrix4d::Identity()}, + {{0, 0, 0.9, 0, 1}, Eigen::Matrix::Identity()}}; + Eigen::Vector2d z_meas = {1, 0}; + + vortex::StateMap states_min_max{ + {StateName::velocity, {-10, 10}}, + {StateName::turn_rate, {-std::numbers::pi, std::numbers::pi}}}; + + auto [weights_upd, x_est_upds, x_est_preds, z_est_preds] = + ImmFilterT::step(imm_model, sensor_model, dt, x_est_prevs, z_meas, + model_weights, states_min_max); + + EXPECT_EQ(ImmFilterT::N_MODELS, + std::tuple_size::value); + EXPECT_EQ(ImmFilterT::N_MODELS, + std::tuple_size::value); + EXPECT_EQ(ImmFilterT::N_MODELS, z_est_preds.size()); + EXPECT_EQ(ImmFilterT::N_MODELS, weights_upd.size()); + + for (int i = 2; i < 50; i++) { + z_meas << i, 0; + std::tie(weights_upd, x_est_upds, std::ignore, std::ignore) = + ImmFilterT::step(imm_model, sensor_model, dt, x_est_upds, z_meas, + weights_upd, states_min_max); + } + + std::cout << "weights_upd:\n" << weights_upd << std::endl; + std::cout << "x_est_upds:\n" << x_est_upds << std::endl; + + // Expect the constant velocity model to have the highest probability + EXPECT_GT(weights_upd(1), weights_upd(0)); + EXPECT_GT(weights_upd(1), weights_upd(2)); +} diff --git a/vortex-filtering/test/immipda_test.cpp b/vortex-filtering/test/immipda_test.cpp index 2c7cf784..f919d7ab 100644 --- a/vortex-filtering/test/immipda_test.cpp +++ b/vortex-filtering/test/immipda_test.cpp @@ -4,68 +4,72 @@ #include class IMMIPDA : public ::testing::Test { -protected: - using DynMod1_ = vortex::models::ConstantPosition; - using DynMod2_ = vortex::models::ConstantVelocity; - using SensMod_ = vortex::models::IdentitySensorModel<2, 2>; - using ImmModel_ = vortex::models::ImmModel; - using IMMIPDA_ = vortex::filter::IMMIPDA; + protected: + using DynMod1_ = vortex::models::ConstantPosition; + using DynMod2_ = vortex::models::ConstantVelocity; + using SensMod_ = vortex::models::IdentitySensorModel<2, 2>; + using ImmModel_ = vortex::models::ImmModel; + using IMMIPDA_ = vortex::filter::IMMIPDA; - using S = vortex::StateName; + using S = vortex::StateName; - IMMIPDA() - : imm_model_(jump_matrix, hold_times, DynMod1_(0.5), DynMod2_(0.5)) - , sensor_model_(2) - , config_{.pdaf = - { - .mahalanobis_threshold = 1.0, - .min_gate_threshold = 0.0, - .max_gate_threshold = std::numeric_limits::max(), - .prob_of_detection = 0.9, - .clutter_intensity = 1.0, - }, - .ipda = - { - .prob_of_survival = 0.9, - .estimate_clutter = true, - .update_existence_probability_on_no_detection = true, - }, - .immipda = { - .states_min_max = {{S::position, {-100.0, 100.0}}, {S::velocity, {-10.0, 10.0}}}, - }} {}; + IMMIPDA() + : imm_model_(jump_matrix, hold_times, DynMod1_(0.5), DynMod2_(0.5)), + sensor_model_(2), + config_{ + .pdaf = + { + .mahalanobis_threshold = 1.0, + .min_gate_threshold = 0.0, + .max_gate_threshold = std::numeric_limits::max(), + .prob_of_detection = 0.9, + .clutter_intensity = 1.0, + }, + .ipda = + { + .prob_of_survival = 0.9, + .estimate_clutter = true, + .update_existence_probability_on_no_detection = true, + }, + .immipda = { + .states_min_max = {{S::position, {-100.0, 100.0}}, + {S::velocity, {-10.0, 10.0}}}, + }} {}; - double dt_ = 1; - Eigen::Matrix2d jump_matrix{{0.0, 1.0}, {1.0, 0.0}}; - Eigen::Vector2d hold_times{100.0, 100.0}; + double dt_ = 1; + Eigen::Matrix2d jump_matrix{{0.0, 1.0}, {1.0, 0.0}}; + Eigen::Vector2d hold_times{100.0, 100.0}; - ImmModel_ imm_model_; - SensMod_ sensor_model_; - IMMIPDA_::Config config_; + ImmModel_ imm_model_; + SensMod_ sensor_model_; + IMMIPDA_::Config config_; }; -TEST_F(IMMIPDA, step) -{ - using namespace vortex; +TEST_F(IMMIPDA, step) { + using namespace vortex; + auto [min_vel, max_vel] = config_.immipda.states_min_max.at(S::velocity); - auto [min_vel, max_vel] = config_.immipda.states_min_max.at(S::velocity); + Eigen::Matrix4d mode_2_cov = prob::Uniform<4>{ + {-1.0, -1.0, min_vel, min_vel}, + {1.0, 1.0, max_vel, max_vel}}.cov(); - Eigen::Matrix4d mode_2_cov = prob::Uniform<4>{{-1.0, -1.0, min_vel, min_vel}, {1.0, 1.0, max_vel, max_vel}}.cov(); + ImmModel_::GaussTuple_x x0 = {prob::Gauss2d::Standard(), + {{0.0, 0.0, 1.0, 0.0}, mode_2_cov}}; + ImmModel_::Vec_n model_weights = {0.5, 0.5}; - ImmModel_::GaussTuple_x x0 = {prob::Gauss2d::Standard(), {{0.0, 0.0, 1.0, 0.0}, mode_2_cov}}; - ImmModel_::Vec_n model_weights = {0.5, 0.5}; + Eigen::Array z0 = { + {1.0, 1.0, 1.0, 20}, + {0.1, -0.1, 0.0, 0}, + }; - Eigen::Array z0 = { - {1.0, 1.0, 1.0, 20}, - {0.1, -0.1, 0.0, 0}, - }; + IMMIPDA_::Output out = IMMIPDA_::step( + imm_model_, sensor_model_, dt_, {x0, model_weights, 0.5}, z0, config_); - IMMIPDA_::Output out = IMMIPDA_::step(imm_model_, sensor_model_, dt_, {x0, model_weights, 0.5}, z0, config_); + ASSERT_EQ(out.gated_measurements.colwise().any().count(), 3); - ASSERT_EQ(out.gated_measurements.colwise().any().count(), 3); - - ASSERT_GT(out.state.mode_probabilities(0), 0.0); - ASSERT_GT(out.state.mode_probabilities(1), 0.0); - ASSERT_LT(out.state.mode_probabilities(0), 1.0); - ASSERT_LT(out.state.mode_probabilities(1), 1.0); -} \ No newline at end of file + ASSERT_GT(out.state.mode_probabilities(0), 0.0); + ASSERT_GT(out.state.mode_probabilities(1), 0.0); + ASSERT_LT(out.state.mode_probabilities(0), 1.0); + ASSERT_LT(out.state.mode_probabilities(1), 1.0); +} diff --git a/vortex-filtering/test/ipda_test.cpp b/vortex-filtering/test/ipda_test.cpp index e92ec415..03812e5d 100644 --- a/vortex-filtering/test/ipda_test.cpp +++ b/vortex-filtering/test/ipda_test.cpp @@ -4,64 +4,70 @@ #include #include -using ConstantVelocity = vortex::models::ConstantVelocity; +using ConstantVelocity = vortex::models::ConstantVelocity; using IdentitySensorModel = vortex::models::IdentitySensorModel<4, 2>; -using IPDA = vortex::filter::IPDA; +using IPDA = vortex::filter::IPDA; -TEST(IPDA, ipda_runs) -{ - IPDA::Config config = { - .pdaf = - { - .mahalanobis_threshold = 1.0, - .min_gate_threshold = 0.0, - .max_gate_threshold = 100.0, - .prob_of_detection = 0.8, - .clutter_intensity = 1.0, - }, - .ipda = - { - .prob_of_survival = 0.9, - .estimate_clutter = true, - .update_existence_probability_on_no_detection = true, - }, - }; +TEST(IPDA, ipda_runs) { + IPDA::Config config = { + .pdaf = + { + .mahalanobis_threshold = 1.0, + .min_gate_threshold = 0.0, + .max_gate_threshold = 100.0, + .prob_of_detection = 0.8, + .clutter_intensity = 1.0, + }, + .ipda = + { + .prob_of_survival = 0.9, + .estimate_clutter = true, + .update_existence_probability_on_no_detection = true, + }, + }; - double last_detection_probability = 0.85; + double last_detection_probability = 0.85; - vortex::prob::Gauss4d x_est(Eigen::Vector4d(0.0, 0.0, 0.0, 0.0), Eigen::Matrix4d::Identity()); - Eigen::Array z_meas = {{0.0, 1.0, 1.0, 0.0, 1.0, 1.0}, {0.0, 2.0, 2.0, 0.0, 2.0, 2.0}}; + vortex::prob::Gauss4d x_est(Eigen::Vector4d(0.0, 0.0, 0.0, 0.0), + Eigen::Matrix4d::Identity()); + Eigen::Array z_meas = {{0.0, 1.0, 1.0, 0.0, 1.0, 1.0}, + {0.0, 2.0, 2.0, 0.0, 2.0, 2.0}}; - ConstantVelocity dyn_model{1.0}; - IdentitySensorModel sen_model{1.0}; + ConstantVelocity dyn_model{1.0}; + IdentitySensorModel sen_model{1.0}; - auto [state_post, x_pred, z_pred, x_updated, gated_measurements] = - IPDA::step(dyn_model, sen_model, 1.0, {x_est, last_detection_probability}, z_meas, config); + auto [state_post, x_pred, z_pred, x_updated, gated_measurements] = + IPDA::step(dyn_model, sen_model, 1.0, + {x_est, last_detection_probability}, z_meas, config); - std::cout << "Existence probability: " << state_post.existence_probability << std::endl; + std::cout << "Existence probability: " << state_post.existence_probability + << std::endl; } -TEST(IPDA, get_existence_probability_is_calculating) -{ - IPDA::Config config = { - .pdaf = - { - .prob_of_detection = 0.8, - .clutter_intensity = 1.0, - }, - .ipda = - { - .prob_of_survival = 0.9, - }, - }; +TEST(IPDA, get_existence_probability_is_calculating) { + IPDA::Config config = { + .pdaf = + { + .prob_of_detection = 0.8, + .clutter_intensity = 1.0, + }, + .ipda = + { + .prob_of_survival = 0.9, + }, + }; - double last_detection_probability = 0.9; + double last_detection_probability = 0.9; - Eigen::Array meas = {{0.0, 1.0, 1.0, 0.0, 1.0, 1.0}, {0.0, 2.0, 2.0, 0.0, 2.0, 2.0}}; + Eigen::Array meas = {{0.0, 1.0, 1.0, 0.0, 1.0, 1.0}, + {0.0, 2.0, 2.0, 0.0, 2.0, 2.0}}; - vortex::prob::Gauss2d z_pred = {{1.0, 1.0}, Eigen::Matrix2d::Identity() * 0.1}; + vortex::prob::Gauss2d z_pred = {{1.0, 1.0}, + Eigen::Matrix2d::Identity() * 0.1}; - double existence_probability = IPDA::existence_prob_update(meas, z_pred, last_detection_probability, config); + double existence_probability = IPDA::existence_prob_update( + meas, z_pred, last_detection_probability, config); - std::cout << "Existence probability: " << existence_probability << std::endl; -} \ No newline at end of file + std::cout << "Existence probability: " << existence_probability + << std::endl; +} diff --git a/vortex-filtering/test/numerical_integration_test.cpp b/vortex-filtering/test/numerical_integration_test.cpp index 7f738467..172eb38b 100644 --- a/vortex-filtering/test/numerical_integration_test.cpp +++ b/vortex-filtering/test/numerical_integration_test.cpp @@ -1,7 +1,7 @@ -#include -#include #include #include +#include +#include #include #include #include @@ -12,157 +12,160 @@ namespace sin_func_test { constexpr int N_DIM_x = 1, N_DIM_z = 1, N_DIM_u = 1, N_DIM_v = 1, N_DIM_w = 1; -using Vec_x = Eigen::Vector; -using Vec_z = Eigen::Vector; -using Vec_u = Eigen::Vector; -using Vec_v = Eigen::Vector; -using Vec_w = Eigen::Vector; -using Dyn_mod_func = std::function; +using Vec_x = Eigen::Vector; +using Vec_z = Eigen::Vector; +using Vec_u = Eigen::Vector; +using Vec_v = Eigen::Vector; +using Vec_w = Eigen::Vector; +using Dyn_mod_func = std::function; // Make functions to test the RK methods -Vec_x sin_func(double t, const Vec_x &x) -{ - Vec_x x_dot; - x_dot << std::pow(std::sin(t), 2) * x(0); - return x_dot; +Vec_x sin_func(double t, const Vec_x& x) { + Vec_x x_dot; + x_dot << std::pow(std::sin(t), 2) * x(0); + return x_dot; } -Vec_x sin_func_exact(double t, const Vec_x &x0) -{ - Vec_x x_kp1; - x_kp1 << x0 * exp(0.5 * (t - sin(t) * cos(t))); - return x_kp1; +Vec_x sin_func_exact(double t, const Vec_x& x0) { + Vec_x x_kp1; + x_kp1 << x0 * exp(0.5 * (t - sin(t) * cos(t))); + return x_kp1; } class NumericalIntegration : public ::testing::Test { -protected: - NumericalIntegration() - { - dt = 1e-3; - init(0, Vec_x::Zero()); - u.setZero(); - v.setZero(); - } - double dt; - - Vec_u u; - Vec_v v; - - std::vector t; - std::vector x_est; - std::vector x_exact; - - void init(double t0, Vec_x x0) - { - t.clear(); - x_est.clear(); - x_exact.clear(); - t.push_back(t0); - x_est.push_back(x0); - x_exact.push_back(x0); - } - - template void run_iterations(Dyn_mod_func f, Dyn_mod_func f_exact, size_t num_iters, double tolerance) - { - for (size_t i = 0; i < num_iters; i++) { - x_est.push_back(rk_method::integrate(f, dt, x_est.at(i), t.at(i))); - t.push_back(t.at(i) + dt); - x_exact.push_back(f_exact(t.at(i + 1), x_exact.at(0))); + protected: + NumericalIntegration() { + dt = 1e-3; + init(0, Vec_x::Zero()); + u.setZero(); + v.setZero(); + } + double dt; + + Vec_u u; + Vec_v v; + + std::vector t; + std::vector x_est; + std::vector x_exact; + + void init(double t0, Vec_x x0) { + t.clear(); + x_est.clear(); + x_exact.clear(); + t.push_back(t0); + x_est.push_back(x0); + x_exact.push_back(x0); + } + + template + void run_iterations(Dyn_mod_func f, + Dyn_mod_func f_exact, + size_t num_iters, + double tolerance) { + for (size_t i = 0; i < num_iters; i++) { + x_est.push_back(rk_method::integrate(f, dt, x_est.at(i), t.at(i))); + t.push_back(t.at(i) + dt); + x_exact.push_back(f_exact(t.at(i + 1), x_exact.at(0))); + } + std::cout << "True: " << x_exact.back()(0) << std::endl; + std::cout << "Approx: " << x_est.back()(0) << std::endl; + std::cout << "Error: " << x_est.back()(0) - x_exact.back()(0) + << std::endl; + EXPECT_NEAR(x_est.back()(0), x_exact.back()(0), tolerance); + } + + void plot_result(std::string title = "ERK convergence") { +#if (GNUPLOT_ENABLE) + // Plot first state and true solution against time + Gnuplot gp; + gp << "set terminal wxt size 1200,800\n"; + gp << "set title '" << title << "'\n"; + gp << "set xlabel 'Time [s]'\n"; + gp << "set ylabel 'x'\n"; + gp << "set grid\n"; + gp << "set key left top\n"; + gp << "plot '-' with lines title 'True', '-' with lines title " + "'Approx'\n"; + gp.send1d(std::make_tuple( + t, vortex::plotting::extract_state_series(x_exact, 0))); + gp.send1d(std::make_tuple( + t, vortex::plotting::extract_state_series(x_est, 0))); +#endif + (void)title; } - std::cout << "True: " << x_exact.back()(0) << std::endl; - std::cout << "Approx: " << x_est.back()(0) << std::endl; - std::cout << "Error: " << x_est.back()(0) - x_exact.back()(0) << std::endl; - EXPECT_NEAR(x_est.back()(0), x_exact.back()(0), tolerance); - } - - void plot_result(std::string title = "ERK convergence") - { - #if (GNUPLOT_ENABLE) - // Plot first state and true solution against time - Gnuplot gp; - gp << "set terminal wxt size 1200,800\n"; - gp << "set title '" << title << "'\n"; - gp << "set xlabel 'Time [s]'\n"; - gp << "set ylabel 'x'\n"; - gp << "set grid\n"; - gp << "set key left top\n"; - gp << "plot '-' with lines title 'True', '-' with lines title 'Approx'\n"; - gp.send1d(std::make_tuple(t, vortex::plotting::extract_state_series(x_exact, 0))); - gp.send1d(std::make_tuple(t, vortex::plotting::extract_state_series(x_est, 0))); - #endif - (void)title; - } }; -TEST_F(NumericalIntegration, RK4sinFunc) -{ - Vec_x x0; - x0 << 1; - init(0, x0); - size_t n_iters = 5000; +TEST_F(NumericalIntegration, RK4sinFunc) { + Vec_x x0; + x0 << 1; + init(0, x0); + size_t n_iters = 5000; - // Expected error is O(dt^4) - run_iterations>(sin_func, sin_func_exact, n_iters, 1e-4); - // plot_result(); + // Expected error is O(dt^4) + run_iterations>(sin_func, sin_func_exact, + n_iters, 1e-4); + // plot_result(); } -TEST_F(NumericalIntegration, EulerSinFunc) -{ - Vec_x x0; - x0 << 1; - init(0, x0); - size_t n_iters = 5000; +TEST_F(NumericalIntegration, EulerSinFunc) { + Vec_x x0; + x0 << 1; + init(0, x0); + size_t n_iters = 5000; - // Expected error is O(dt) - run_iterations>(sin_func, sin_func_exact, n_iters, 1e-1); - plot_result("Euler Sin Func"); + // Expected error is O(dt) + run_iterations>( + sin_func, sin_func_exact, n_iters, 1e-1); + plot_result("Euler Sin Func"); } -TEST_F(NumericalIntegration, ButcherMidpointSinFunc) -{ - Vec_x x0; - x0 << 1; - init(0, x0); - constexpr int n_stages = 2; - Eigen::Matrix A; - Eigen::Vector b; - Eigen::Vector c; - - // Midpoint method - A << 0, 0, 1 / 2.0, 0; - - b << 0, 1; - c << 0, 1 / 2.0; - - auto midpoint = std::make_shared>(A, b, c); - size_t n_iters = 5000; - - // Expected error is O(dt^2) - Vec_x exact = sin_func_exact(dt * n_iters, x0); - double tolerance = 1e-5; - size_t num_iters = 5000; - for (size_t i = 0; i < num_iters; i++) { - x_est.push_back(midpoint->integrate(sin_func, dt, x_est.at(i), t.at(i))); - t.push_back(t.at(i) + dt); - x_exact.push_back(sin_func_exact(t.at(i + 1), x_exact.at(0))); - } - std::cout << "True: " << exact << std::endl; - std::cout << "Approx: " << x_est.back()(0) << std::endl; - std::cout << "Error: " << x_est.back()(0) - x_exact.back()(0) << std::endl; - EXPECT_NEAR(x_est.back()(0), x_exact.back()(0), tolerance); +TEST_F(NumericalIntegration, ButcherMidpointSinFunc) { + Vec_x x0; + x0 << 1; + init(0, x0); + constexpr int n_stages = 2; + Eigen::Matrix A; + Eigen::Vector b; + Eigen::Vector c; + + // Midpoint method + A << 0, 0, 1 / 2.0, 0; + + b << 0, 1; + c << 0, 1 / 2.0; + + auto midpoint = + std::make_shared>(A, b, + c); + size_t n_iters = 5000; + + // Expected error is O(dt^2) + Vec_x exact = sin_func_exact(dt * n_iters, x0); + double tolerance = 1e-5; + size_t num_iters = 5000; + for (size_t i = 0; i < num_iters; i++) { + x_est.push_back( + midpoint->integrate(sin_func, dt, x_est.at(i), t.at(i))); + t.push_back(t.at(i) + dt); + x_exact.push_back(sin_func_exact(t.at(i + 1), x_exact.at(0))); + } + std::cout << "True: " << exact << std::endl; + std::cout << "Approx: " << x_est.back()(0) << std::endl; + std::cout << "Error: " << x_est.back()(0) - x_exact.back()(0) << std::endl; + EXPECT_NEAR(x_est.back()(0), x_exact.back()(0), tolerance); } -TEST_F(NumericalIntegration, ButcherRKDPsinFunc) -{ - Vec_x x0; - x0 << 1; - init(0, x0); - constexpr int n_stages = 7; - Eigen::Matrix A; - Eigen::Vector b; - Eigen::Vector c; - - // Dormand Prince (RKDP) method - // clang-format off +TEST_F(NumericalIntegration, ButcherRKDPsinFunc) { + Vec_x x0; + x0 << 1; + init(0, x0); + constexpr int n_stages = 7; + Eigen::Matrix A; + Eigen::Vector b; + Eigen::Vector c; + + // Dormand Prince (RKDP) method + // clang-format off A << 0 , 0 , 0 , 0 , 0 , 0 , 0, 1/5.0 , 0 , 0 , 0 , 0 , 0 , 0, 3/40.0 , 9/40.0 , 0 , 0 , 0 , 0 , 0, @@ -174,91 +177,92 @@ TEST_F(NumericalIntegration, ButcherRKDPsinFunc) b << 35/384.0 , 0 , 500/1113.0 , 125/192.0, -2187/6784.0 , 11/84.0 , 0, // Error of order O(dt^5) c << 0 , 1/5.0 , 3/10.0 , 4/5.0 , 8/9.0 , 1 , 1; - // clang-format on - auto rkdp = std::make_shared>(A, b, c); - size_t n_iters = 5000; - - // Expected error is O(dt^5) - Vec_x exact = sin_func_exact(dt * n_iters, x0); - double tolerance = 1e-8; - - for (size_t i = 0; i < n_iters; i++) { - x_est.push_back(rkdp->integrate(sin_func, dt, x_est.at(i), t.at(i))); - t.push_back(t.at(i) + dt); - x_exact.push_back(sin_func_exact(t.at(i + 1), x_exact.at(0))); - } - std::cout << "True: " << exact << std::endl; - std::cout << "Approx: " << x_est.back()(0) << std::endl; - std::cout << "Error: " << x_est.back()(0) - x_exact.back()(0) << std::endl; - EXPECT_NEAR(x_est.back()(0), x_exact.back()(0), tolerance); + // clang-format on + auto rkdp = + std::make_shared>(A, b, + c); + size_t n_iters = 5000; + + // Expected error is O(dt^5) + Vec_x exact = sin_func_exact(dt * n_iters, x0); + double tolerance = 1e-8; + + for (size_t i = 0; i < n_iters; i++) { + x_est.push_back(rkdp->integrate(sin_func, dt, x_est.at(i), t.at(i))); + t.push_back(t.at(i) + dt); + x_exact.push_back(sin_func_exact(t.at(i + 1), x_exact.at(0))); + } + std::cout << "True: " << exact << std::endl; + std::cout << "Approx: " << x_est.back()(0) << std::endl; + std::cout << "Error: " << x_est.back()(0) - x_exact.back()(0) << std::endl; + EXPECT_NEAR(x_est.back()(0), x_exact.back()(0), tolerance); } -TEST_F(NumericalIntegration, ODE45sinFuncRealTime) -{ - Vec_x x0; - x0 << 1; - init(0, x0); - auto rk45 = std::make_shared>(1e-20, 1e-20); - size_t n_iters = 5000; - - // Expected error is O(dt^5) - Vec_x exact = sin_func_exact(dt * n_iters, x0); - double tolerance = 1e-8; - for (size_t i = 0; i < n_iters; i++) { - x_est.push_back(rk45->integrate(sin_func, dt, x_est.at(i), t.at(i))); - t.push_back(t.at(i) + dt); - x_exact.push_back(sin_func_exact(t.at(i + 1), x_exact.at(0))); - } - std::cout << "True: " << exact << std::endl; - std::cout << "Approx: " << x_est.back()(0) << std::endl; - std::cout << "Error: " << x_est.back()(0) - x_exact.back()(0) << std::endl; - EXPECT_NEAR(x_est.back()(0), x_exact.back()(0), tolerance); +TEST_F(NumericalIntegration, ODE45sinFuncRealTime) { + Vec_x x0; + x0 << 1; + init(0, x0); + auto rk45 = + std::make_shared>(1e-20, 1e-20); + size_t n_iters = 5000; + + // Expected error is O(dt^5) + Vec_x exact = sin_func_exact(dt * n_iters, x0); + double tolerance = 1e-8; + for (size_t i = 0; i < n_iters; i++) { + x_est.push_back(rk45->integrate(sin_func, dt, x_est.at(i), t.at(i))); + t.push_back(t.at(i) + dt); + x_exact.push_back(sin_func_exact(t.at(i + 1), x_exact.at(0))); + } + std::cout << "True: " << exact << std::endl; + std::cout << "Approx: " << x_est.back()(0) << std::endl; + std::cout << "Error: " << x_est.back()(0) - x_exact.back()(0) << std::endl; + EXPECT_NEAR(x_est.back()(0), x_exact.back()(0), tolerance); } -TEST_F(NumericalIntegration, ODE45sinFuncLongStep) -{ - Vec_x x0; - x0 << 1; - init(0, x0); - auto rk45 = std::make_shared>(1e-20, 1e-20, 100000); - dt = 5; - size_t n_iters = 1; - - // Expected error is O(dt^5) - Vec_x exact = sin_func_exact(dt * n_iters, x0); - double tolerance = 1e-6; - - for (size_t i = 0; i < n_iters; i++) { - x_est.push_back(rk45->integrate(sin_func, dt, x_est.at(i), t.at(i))); - t.push_back(t.at(i) + dt); - x_exact.push_back(sin_func_exact(t.at(i + 1), x_exact.at(0))); - } - std::cout << "True: " << exact << std::endl; - std::cout << "Approx: " << x_est.back()(0) << std::endl; - std::cout << "Error: " << x_est.back()(0) - x_exact.back()(0) << std::endl; - EXPECT_NEAR(x_est.back()(0), x_exact.back()(0), tolerance); +TEST_F(NumericalIntegration, ODE45sinFuncLongStep) { + Vec_x x0; + x0 << 1; + init(0, x0); + auto rk45 = std::make_shared>( + 1e-20, 1e-20, 100000); + dt = 5; + size_t n_iters = 1; + + // Expected error is O(dt^5) + Vec_x exact = sin_func_exact(dt * n_iters, x0); + double tolerance = 1e-6; + + for (size_t i = 0; i < n_iters; i++) { + x_est.push_back(rk45->integrate(sin_func, dt, x_est.at(i), t.at(i))); + t.push_back(t.at(i) + dt); + x_exact.push_back(sin_func_exact(t.at(i + 1), x_exact.at(0))); + } + std::cout << "True: " << exact << std::endl; + std::cout << "Approx: " << x_est.back()(0) << std::endl; + std::cout << "Error: " << x_est.back()(0) - x_exact.back()(0) << std::endl; + EXPECT_NEAR(x_est.back()(0), x_exact.back()(0), tolerance); } -} // namespace sin_func_test +} // namespace sin_func_test namespace van_der_pol_test { constexpr int N_DIM_x = 2, N_DIM_z = 1, N_DIM_u = 1, N_DIM_v = 2, N_DIM_w = 1; -using Vec_x = Eigen::Vector; -using Vec_z = Eigen::Vector; -using Vec_u = Eigen::Vector; -using Vec_v = Eigen::Vector; -using Vec_w = Eigen::Vector; -using Dyn_mod_func = std::function; +using Vec_x = Eigen::Vector; +using Vec_z = Eigen::Vector; +using Vec_u = Eigen::Vector; +using Vec_v = Eigen::Vector; +using Vec_w = Eigen::Vector; +using Dyn_mod_func = std::function; // van der Pol oscillator -Vec_x vdp(double t, const Vec_x &x, const Vec_u &u, const Vec_v &v) -{ - (void)t; - (void)v; - Vec_x x_dot; - x_dot << x(1), (1 - std::pow(x(0), 2)) * x(1) - x(0) + u(0); - return x_dot; +Vec_x vdp(double t, const Vec_x& x, const Vec_u& u, const Vec_v& v) { + (void)t; + (void)v; + Vec_x x_dot; + x_dot << x(1), (1 - std::pow(x(0), 2)) * x(1) - x(0) + u(0); + return x_dot; } -} // namespace van_der_pol_test +} // namespace van_der_pol_test diff --git a/vortex-filtering/test/pdaf_test.cpp b/vortex-filtering/test/pdaf_test.cpp index 84296788..d16d8ccc 100644 --- a/vortex-filtering/test/pdaf_test.cpp +++ b/vortex-filtering/test/pdaf_test.cpp @@ -4,269 +4,290 @@ #include #include -using ConstantVelocity = vortex::models::ConstantVelocity; +using ConstantVelocity = vortex::models::ConstantVelocity; using IdentitySensorModel = vortex::models::IdentitySensorModel<4, 2>; -using PDAF = vortex::filter::PDAF; +using PDAF = vortex::filter::PDAF; // testing the get_weights function -TEST(PDAF, get_weights_is_calculating) -{ - double prob_of_detection = 0.8; - double clutter_intensity = 1.0; +TEST(PDAF, get_weights_is_calculating) { + double prob_of_detection = 0.8; + double clutter_intensity = 1.0; - vortex::prob::Gauss2d z_pred(Eigen::Vector2d(0.0, 0.0), Eigen::Matrix2d::Identity()); - Eigen::Array2Xd meas = {{0.0, 2.0}, {0.0, 1.0}}; + vortex::prob::Gauss2d z_pred(Eigen::Vector2d(0.0, 0.0), + Eigen::Matrix2d::Identity()); + Eigen::Array2Xd meas = {{0.0, 2.0}, {0.0, 1.0}}; - Eigen::VectorXd weights = PDAF::get_weights(meas, z_pred, prob_of_detection, clutter_intensity); + Eigen::VectorXd weights = + PDAF::get_weights(meas, z_pred, prob_of_detection, clutter_intensity); - std::cout << "weights: " << weights << std::endl; + std::cout << "weights: " << weights << std::endl; - EXPECT_EQ(weights.size(), 3); + EXPECT_EQ(weights.size(), 3); } -TEST(PDAF, if_no_clutter_first_weight_is_zero) -{ - double prob_of_detection = 0.8; - double clutter_intensity = 0.0; +TEST(PDAF, if_no_clutter_first_weight_is_zero) { + double prob_of_detection = 0.8; + double clutter_intensity = 0.0; - vortex::prob::Gauss2d z_pred(Eigen::Vector2d(0.0, 0.0), Eigen::Matrix2d::Identity()); - Eigen::Array2Xd meas = {{0.0, 2.0}, {0.0, 1.0}}; + vortex::prob::Gauss2d z_pred(Eigen::Vector2d(0.0, 0.0), + Eigen::Matrix2d::Identity()); + Eigen::Array2Xd meas = {{0.0, 2.0}, {0.0, 1.0}}; - Eigen::VectorXd weights = PDAF::get_weights(meas, z_pred, prob_of_detection, clutter_intensity); + Eigen::VectorXd weights = + PDAF::get_weights(meas, z_pred, prob_of_detection, clutter_intensity); - std::cout << "weights: " << weights << std::endl; + std::cout << "weights: " << weights << std::endl; - EXPECT_EQ(weights(0), 0.0); + EXPECT_EQ(weights(0), 0.0); } -TEST(PDAF, weights_are_decreasing_with_distance) -{ - double prob_of_detection = 0.8; - double clutter_intensity = 1.0; +TEST(PDAF, weights_are_decreasing_with_distance) { + double prob_of_detection = 0.8; + double clutter_intensity = 1.0; - vortex::prob::Gauss2d z_pred(Eigen::Vector2d(1.0, 1.0), Eigen::Matrix2d::Identity()); - Eigen::Array2Xd meas = {{2.0, 3.0, 4.0}, {1.0, 1.0, 1.0}}; + vortex::prob::Gauss2d z_pred(Eigen::Vector2d(1.0, 1.0), + Eigen::Matrix2d::Identity()); + Eigen::Array2Xd meas = {{2.0, 3.0, 4.0}, {1.0, 1.0, 1.0}}; - Eigen::VectorXd weights = PDAF::get_weights(meas, z_pred, prob_of_detection, clutter_intensity); + Eigen::VectorXd weights = + PDAF::get_weights(meas, z_pred, prob_of_detection, clutter_intensity); - std::cout << "weights: " << weights << std::endl; + std::cout << "weights: " << weights << std::endl; - EXPECT_GT(weights(1), weights(2)); - EXPECT_GT(weights(2), weights(3)); + EXPECT_GT(weights(1), weights(2)); + EXPECT_GT(weights(2), weights(3)); } // testing the get_weighted_average function -TEST(PDAF, get_weighted_average_is_calculating) -{ - double prob_of_detection = 0.8; - double clutter_intensity = 1.0; - - vortex::prob::Gauss2d z_pred(Eigen::Vector2d(0.0, 0.0), Eigen::Matrix2d::Identity()); - vortex::prob::Gauss4d x_pred(Eigen::Vector4d(0.0, 0.0, 0.0, 0.0), Eigen::Matrix4d::Identity()); - Eigen::Array2Xd meas = {{0.0, 2.0}, {0.0, 1.0}}; - std::vector updated_states = { - vortex::prob::Gauss4d(Eigen::Vector4d(0.0, 0.0, 0.0, 0.0), Eigen::Matrix4d::Identity()), - vortex::prob::Gauss4d(Eigen::Vector4d(1.0, 1.0, 1.0, 1.0), Eigen::Matrix4d::Identity()) - }; - - vortex::prob::Gauss4d weighted_average = - PDAF::get_weighted_average(meas, updated_states, z_pred, x_pred, prob_of_detection, clutter_intensity); - - std::cout << "weighted average: " << weighted_average.mean() << std::endl; +TEST(PDAF, get_weighted_average_is_calculating) { + double prob_of_detection = 0.8; + double clutter_intensity = 1.0; + + vortex::prob::Gauss2d z_pred(Eigen::Vector2d(0.0, 0.0), + Eigen::Matrix2d::Identity()); + vortex::prob::Gauss4d x_pred(Eigen::Vector4d(0.0, 0.0, 0.0, 0.0), + Eigen::Matrix4d::Identity()); + Eigen::Array2Xd meas = {{0.0, 2.0}, {0.0, 1.0}}; + std::vector updated_states = { + vortex::prob::Gauss4d(Eigen::Vector4d(0.0, 0.0, 0.0, 0.0), + Eigen::Matrix4d::Identity()), + vortex::prob::Gauss4d(Eigen::Vector4d(1.0, 1.0, 1.0, 1.0), + Eigen::Matrix4d::Identity())}; + + vortex::prob::Gauss4d weighted_average = + PDAF::get_weighted_average(meas, updated_states, z_pred, x_pred, + prob_of_detection, clutter_intensity); + + std::cout << "weighted average: " << weighted_average.mean() << std::endl; } -TEST(PDAF, average_state_is_in_between_prediction_and_measurement_y_axis) -{ - double prob_of_detection = 0.8; - double clutter_intensity = 1.0; +TEST(PDAF, average_state_is_in_between_prediction_and_measurement_y_axis) { + double prob_of_detection = 0.8; + double clutter_intensity = 1.0; - vortex::prob::Gauss2d z_pred(Eigen::Vector2d(1.0, 1.0), Eigen::Matrix2d::Identity()); - vortex::prob::Gauss4d x_pred(Eigen::Vector4d(1.0, 1.0, 0.0, 0.0), Eigen::Matrix4d::Identity()); - Eigen::Array2Xd meas = {{1.0}, {2.0}}; + vortex::prob::Gauss2d z_pred(Eigen::Vector2d(1.0, 1.0), + Eigen::Matrix2d::Identity()); + vortex::prob::Gauss4d x_pred(Eigen::Vector4d(1.0, 1.0, 0.0, 0.0), + Eigen::Matrix4d::Identity()); + Eigen::Array2Xd meas = {{1.0}, {2.0}}; - std::vector updated_states = { vortex::prob::Gauss4d(Eigen::Vector4d(1.0, 1.5, 0.0, 0.0), - Eigen::Matrix4d::Identity()) }; + std::vector updated_states = {vortex::prob::Gauss4d( + Eigen::Vector4d(1.0, 1.5, 0.0, 0.0), Eigen::Matrix4d::Identity())}; - vortex::prob::Gauss4d weighted_average = - PDAF::get_weighted_average(meas, updated_states, z_pred, x_pred, prob_of_detection, clutter_intensity); + vortex::prob::Gauss4d weighted_average = + PDAF::get_weighted_average(meas, updated_states, z_pred, x_pred, + prob_of_detection, clutter_intensity); - EXPECT_GT(weighted_average.mean()(1), x_pred.mean()(1)); - EXPECT_GT(weighted_average.mean()(1), z_pred.mean()(1)); - EXPECT_LT(weighted_average.mean()(1), meas(1, 0)); - EXPECT_LT(weighted_average.mean()(1), updated_states[0].mean()(1)); + EXPECT_GT(weighted_average.mean()(1), x_pred.mean()(1)); + EXPECT_GT(weighted_average.mean()(1), z_pred.mean()(1)); + EXPECT_LT(weighted_average.mean()(1), meas(1, 0)); + EXPECT_LT(weighted_average.mean()(1), updated_states[0].mean()(1)); - std::cout << "weighted average: " << weighted_average.mean() << std::endl; + std::cout << "weighted average: " << weighted_average.mean() << std::endl; } -TEST(PDAF, average_state_is_in_between_prediction_and_measurement_x_axis) -{ - double prob_of_detection = 0.8; - double clutter_intensity = 1.0; +TEST(PDAF, average_state_is_in_between_prediction_and_measurement_x_axis) { + double prob_of_detection = 0.8; + double clutter_intensity = 1.0; - vortex::prob::Gauss2d z_pred(Eigen::Vector2d(1.0, 1.0), Eigen::Matrix2d::Identity()); - vortex::prob::Gauss4d x_pred(Eigen::Vector4d(1.0, 1.0, 0.0, 0.0), Eigen::Matrix4d::Identity()); - Eigen::Array2Xd meas = {{2.0}, {1.0}}; + vortex::prob::Gauss2d z_pred(Eigen::Vector2d(1.0, 1.0), + Eigen::Matrix2d::Identity()); + vortex::prob::Gauss4d x_pred(Eigen::Vector4d(1.0, 1.0, 0.0, 0.0), + Eigen::Matrix4d::Identity()); + Eigen::Array2Xd meas = {{2.0}, {1.0}}; - std::vector updated_states = { vortex::prob::Gauss4d(Eigen::Vector4d(1.5, 1.0, 0.0, 0.0), - Eigen::Matrix4d::Identity()) }; + std::vector updated_states = {vortex::prob::Gauss4d( + Eigen::Vector4d(1.5, 1.0, 0.0, 0.0), Eigen::Matrix4d::Identity())}; - vortex::prob::Gauss4d weighted_average = - PDAF::get_weighted_average(meas, updated_states, z_pred, x_pred, prob_of_detection, clutter_intensity); + vortex::prob::Gauss4d weighted_average = + PDAF::get_weighted_average(meas, updated_states, z_pred, x_pred, + prob_of_detection, clutter_intensity); - EXPECT_GT(weighted_average.mean()(0), x_pred.mean()(0)); - EXPECT_GT(weighted_average.mean()(0), z_pred.mean()(0)); - EXPECT_LT(weighted_average.mean()(0), meas(0, 0)); - EXPECT_LT(weighted_average.mean()(0), updated_states[0].mean()(0)); + EXPECT_GT(weighted_average.mean()(0), x_pred.mean()(0)); + EXPECT_GT(weighted_average.mean()(0), z_pred.mean()(0)); + EXPECT_LT(weighted_average.mean()(0), meas(0, 0)); + EXPECT_LT(weighted_average.mean()(0), updated_states[0].mean()(0)); - std::cout << "weighted average: " << weighted_average.mean() << std::endl; + std::cout << "weighted average: " << weighted_average.mean() << std::endl; } -TEST(PDAF, average_state_is_in_between_prediction_and_measurement_both_axes) -{ - double prob_of_detection = 0.8; - double clutter_intensity = 1.0; +TEST(PDAF, average_state_is_in_between_prediction_and_measurement_both_axes) { + double prob_of_detection = 0.8; + double clutter_intensity = 1.0; - vortex::prob::Gauss2d z_pred(Eigen::Vector2d(1.0, 1.0), Eigen::Matrix2d::Identity()); - vortex::prob::Gauss4d x_pred(Eigen::Vector4d(1.0, 1.0, 0.0, 0.0), Eigen::Matrix4d::Identity()); - Eigen::Array2Xd meas = {{2.0}, {2.0}}; + vortex::prob::Gauss2d z_pred(Eigen::Vector2d(1.0, 1.0), + Eigen::Matrix2d::Identity()); + vortex::prob::Gauss4d x_pred(Eigen::Vector4d(1.0, 1.0, 0.0, 0.0), + Eigen::Matrix4d::Identity()); + Eigen::Array2Xd meas = {{2.0}, {2.0}}; - std::vector updated_states = { vortex::prob::Gauss4d(Eigen::Vector4d(1.5, 1.5, 0.0, 0.0), - Eigen::Matrix4d::Identity()) }; + std::vector updated_states = {vortex::prob::Gauss4d( + Eigen::Vector4d(1.5, 1.5, 0.0, 0.0), Eigen::Matrix4d::Identity())}; - vortex::prob::Gauss4d weighted_average = - PDAF::get_weighted_average(meas, updated_states, z_pred, x_pred, prob_of_detection, clutter_intensity); + vortex::prob::Gauss4d weighted_average = + PDAF::get_weighted_average(meas, updated_states, z_pred, x_pred, + prob_of_detection, clutter_intensity); - EXPECT_GT(weighted_average.mean()(0), x_pred.mean()(0)); - EXPECT_GT(weighted_average.mean()(0), z_pred.mean()(0)); - EXPECT_LT(weighted_average.mean()(0), meas(0, 0)); - EXPECT_LT(weighted_average.mean()(0), updated_states[0].mean()(0)); + EXPECT_GT(weighted_average.mean()(0), x_pred.mean()(0)); + EXPECT_GT(weighted_average.mean()(0), z_pred.mean()(0)); + EXPECT_LT(weighted_average.mean()(0), meas(0, 0)); + EXPECT_LT(weighted_average.mean()(0), updated_states[0].mean()(0)); - EXPECT_GT(weighted_average.mean()(1), x_pred.mean()(1)); - EXPECT_GT(weighted_average.mean()(1), z_pred.mean()(1)); - EXPECT_LT(weighted_average.mean()(1), meas(1, 0)); - EXPECT_LT(weighted_average.mean()(1), updated_states[0].mean()(1)); + EXPECT_GT(weighted_average.mean()(1), x_pred.mean()(1)); + EXPECT_GT(weighted_average.mean()(1), z_pred.mean()(1)); + EXPECT_LT(weighted_average.mean()(1), meas(1, 0)); + EXPECT_LT(weighted_average.mean()(1), updated_states[0].mean()(1)); - std::cout << "weighted average: " << weighted_average.mean() << std::endl; + std::cout << "weighted average: " << weighted_average.mean() << std::endl; } // testing the apply_gate function -TEST(PDAF, apply_gate_is_calculating) -{ - PDAF::Config config = {.pdaf = { - .mahalanobis_threshold = 1.8, - }}; - - vortex::prob::Gauss2d z_pred(Eigen::Vector2d(0.0, 0.0), Eigen::Matrix2d::Identity()); - // clang-format off - Eigen::Array2Xd meas = {{0.0, 1.0, 1.0, 0.0, 2.0, 2.0}, +TEST(PDAF, apply_gate_is_calculating) { + PDAF::Config config = {.pdaf = { + .mahalanobis_threshold = 1.8, + }}; + + vortex::prob::Gauss2d z_pred(Eigen::Vector2d(0.0, 0.0), + Eigen::Matrix2d::Identity()); + // clang-format off + Eigen::Array2Xd meas = {{0.0, 1.0, 1.0, 0.0, 2.0, 2.0}, {1.0, 0.0, 1.0, 2.0, 0.0, 2.0}}; - // clang-format on + // clang-format on - auto gated = PDAF::apply_gate(meas, z_pred, config); + auto gated = PDAF::apply_gate(meas, z_pred, config); } -TEST(PDAF, apply_gate_is_separating_correctly) -{ - PDAF::Config config = {.pdaf = { - .mahalanobis_threshold = 3, - }}; - Eigen::Matrix2d cov; - cov << 1.0, 0.0, 0.0, 4.0; +TEST(PDAF, apply_gate_is_separating_correctly) { + PDAF::Config config = {.pdaf = { + .mahalanobis_threshold = 3, + }}; + Eigen::Matrix2d cov; + cov << 1.0, 0.0, 0.0, 4.0; - vortex::prob::Gauss2d z_pred(Eigen::Vector2d(0.0, 0.0), cov); - // clang-format off - Eigen::Array2Xd meas = {{0.0, 4.0}, + vortex::prob::Gauss2d z_pred(Eigen::Vector2d(0.0, 0.0), cov); + // clang-format off + Eigen::Array2Xd meas = {{0.0, 4.0}, {4.0, 0.0}}; - // clang-format on + // clang-format on - auto gated = PDAF::apply_gate(meas, z_pred, config); + auto gated = PDAF::apply_gate(meas, z_pred, config); - EXPECT_TRUE(gated(0)); - EXPECT_FALSE(gated(1)); + EXPECT_TRUE(gated(0)); + EXPECT_FALSE(gated(1)); #if (GNUPLOT_ENABLE) - std::vector> meas_vec; - for (Eigen::Vector2d m : meas.colwise()) { - meas_vec.push_back({m(0), m(1)}); - } - - Gnuplot gp; - gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; - gp << "set size ratio -1\n"; - gp << "set style circle radius 0.05\n"; - gp << "plot '-' with circles title 'Samples' linecolor rgb 'red' fs transparent solid 1 noborder\n"; - gp.send1d(meas_vec); - - int object_counter = 0; - - gp << "set object " << ++object_counter << " circle center " << z_pred.mean()(0) << "," << z_pred.mean()(1) - << " size " << 0.05 << " fs empty border lc rgb 'green'\n"; - gp << "replot\n"; - - vortex::utils::Ellipse prediction = vortex::plotting::gauss_to_ellipse(z_pred, config.pdaf.mahalanobis_threshold); - - gp << "set object " << ++object_counter << " ellipse center " << prediction.x() << "," << prediction.y() << " size " << prediction.major_axis() << "," - << prediction.minor_axis() << " angle " << prediction.angle_deg() << "fs empty border lc rgb 'cyan'\n"; - gp << "replot\n"; - #endif + std::vector> meas_vec; + for (Eigen::Vector2d m : meas.colwise()) { + meas_vec.push_back({m(0), m(1)}); + } + + Gnuplot gp; + gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; + gp << "set size ratio -1\n"; + gp << "set style circle radius 0.05\n"; + gp << "plot '-' with circles title 'Samples' linecolor rgb 'red' fs " + "transparent solid 1 noborder\n"; + gp.send1d(meas_vec); + + int object_counter = 0; + + gp << "set object " << ++object_counter << " circle center " + << z_pred.mean()(0) << "," << z_pred.mean()(1) << " size " << 0.05 + << " fs empty border lc rgb 'green'\n"; + gp << "replot\n"; + + vortex::utils::Ellipse prediction = vortex::plotting::gauss_to_ellipse( + z_pred, config.pdaf.mahalanobis_threshold); + + gp << "set object " << ++object_counter << " ellipse center " + << prediction.x() << "," << prediction.y() << " size " + << prediction.major_axis() << "," << prediction.minor_axis() << " angle " + << prediction.angle_deg() << "fs empty border lc rgb 'cyan'\n"; + gp << "replot\n"; +#endif } -TEST(PDAF, apply_gate_is_separating_correctly_2) -{ - PDAF::Config config = {.pdaf = { - .mahalanobis_threshold = 2.1, - }}; - vortex::prob::Gauss2d z_pred(Eigen::Vector2d(0.0, 0.0), Eigen::Matrix2d::Identity()); +TEST(PDAF, apply_gate_is_separating_correctly_2) { + PDAF::Config config = {.pdaf = { + .mahalanobis_threshold = 2.1, + }}; + vortex::prob::Gauss2d z_pred(Eigen::Vector2d(0.0, 0.0), + Eigen::Matrix2d::Identity()); - // clang-format off - Eigen::Array2Xd meas = {{0.0, 1.0, 1.0, 0.0, 2.0, 2.0}, + // clang-format off + Eigen::Array2Xd meas = {{0.0, 1.0, 1.0, 0.0, 2.0, 2.0}, {1.0, 0.0, 1.0, 2.0, 0.0, 2.0}}; - // clang-format on - - auto gated = PDAF::apply_gate(meas, z_pred, config); - - EXPECT_EQ(gated.count(), 5u); - - #if (GNUPLOT_ENABLE) - std::vector> meas_vec; - for (Eigen::Vector2d m : meas.colwise()) { - meas_vec.push_back({m(0), m(1)}); - } - - Gnuplot gp; - gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; - gp << "set size ratio -1\n"; - gp << "set style circle radius 0.05\n"; - gp << "plot '-' with circles title 'Samples' linecolor rgb 'red' fs transparent solid 1 noborder\n"; - gp.send1d(meas_vec); + // clang-format on - int object_counter = 0; + auto gated = PDAF::apply_gate(meas, z_pred, config); - gp << "set object " << ++object_counter << " circle center " << z_pred.mean()(0) << "," << z_pred.mean()(1) - << " size " << 0.05 << " fs empty border lc rgb 'green'\n"; - gp << "replot\n"; + EXPECT_EQ(gated.count(), 5u); - vortex::utils::Ellipse prediction = vortex::plotting::gauss_to_ellipse(z_pred, config.pdaf.mahalanobis_threshold); - - gp << "set object " << ++object_counter << " ellipse center " << prediction.x() << "," << prediction.y() << " size " << prediction.major_axis() << "," - << prediction.minor_axis() << " angle " << prediction.angle_deg() << "fs empty border lc rgb 'cyan'\n"; - gp << "replot\n"; - #endif +#if (GNUPLOT_ENABLE) + std::vector> meas_vec; + for (Eigen::Vector2d m : meas.colwise()) { + meas_vec.push_back({m(0), m(1)}); + } + + Gnuplot gp; + gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; + gp << "set size ratio -1\n"; + gp << "set style circle radius 0.05\n"; + gp << "plot '-' with circles title 'Samples' linecolor rgb 'red' fs " + "transparent solid 1 noborder\n"; + gp.send1d(meas_vec); + + int object_counter = 0; + + gp << "set object " << ++object_counter << " circle center " + << z_pred.mean()(0) << "," << z_pred.mean()(1) << " size " << 0.05 + << " fs empty border lc rgb 'green'\n"; + gp << "replot\n"; + + vortex::utils::Ellipse prediction = vortex::plotting::gauss_to_ellipse( + z_pred, config.pdaf.mahalanobis_threshold); + + gp << "set object " << ++object_counter << " ellipse center " + << prediction.x() << "," << prediction.y() << " size " + << prediction.major_axis() << "," << prediction.minor_axis() << " angle " + << prediction.angle_deg() << "fs empty border lc rgb 'cyan'\n"; + gp << "replot\n"; +#endif } // testing the predict_next_state function -TEST(PDAF, predict_next_state_is_calculating) -{ - PDAF::Config config = {.pdaf = { - .mahalanobis_threshold = 1.12, - .prob_of_detection = 0.8, - .clutter_intensity = 1.0, - }}; - - vortex::prob::Gauss4d x_est(Eigen::Vector4d(0.0, 0.0, 0.0, 0.0), Eigen::Matrix4d::Identity()); - - // clang-format off - Eigen::Array2Xd meas = {{0.0, 1.0, 1.0, 0.0, 2.0, 2.0}, +TEST(PDAF, predict_next_state_is_calculating) { + PDAF::Config config = {.pdaf = { + .mahalanobis_threshold = 1.12, + .prob_of_detection = 0.8, + .clutter_intensity = 1.0, + }}; + + vortex::prob::Gauss4d x_est(Eigen::Vector4d(0.0, 0.0, 0.0, 0.0), + Eigen::Matrix4d::Identity()); + + // clang-format off + Eigen::Array2Xd meas = {{0.0, 1.0, 1.0, 0.0, 2.0, 2.0}, {1.0, 0.0, 1.0, 2.0, 0.0, 2.0}}; ConstantVelocity dyn_model{1.0}; @@ -337,365 +358,445 @@ TEST(PDAF, predict_next_state_2) vortex::prob::Gauss4d x_est(Eigen::Vector4d(1.0, 1.5, -2.0, 0.0), Eigen::Matrix4d::Identity()); // clang-format off - Eigen::Array2Xd meas = {{-3.0, 0.0, -1.2, -2.0, 2.0, -1.0}, + Eigen::Array2Xd meas = {{-3.0, 0.0, -1.2, -2.0, 2.0, -1.0}, {-3.0, 0.0, 1.5, -2.0, 0.0, 1.0}}; - // clang-format on - - ConstantVelocity dyn_model{1.0}; - IdentitySensorModel sen_model{0.5}; - - auto [x_final, x_pred, z_pred, x_updated, gated] = - PDAF::step(dyn_model, sen_model, 1.0, x_est, meas, config); - std::cout << "x_final: " << x_final.mean() << std::endl; - - #if (GNUPLOT_ENABLE) - std::vector> meas_vec; - for (Eigen::Vector2d m : meas.colwise()) { - meas_vec.push_back({m(0), m(1)}); - } - - Gnuplot gp; - gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; - gp << "set size ratio -1\n"; - - gp << "set style circle radius 0.05\n"; - gp << "plot '-' with circles title 'Samples' linecolor rgb 'red' fs transparent solid 1 noborder\n"; - gp.send1d(meas_vec); + // clang-format on - int object_counter = 0; - - for (const auto& state : x_updated) - { - vortex::prob::Gauss2d gauss(state.mean().head(2), state.cov().topLeftCorner(2, 2)); - vortex::utils::Ellipse ellipse = vortex::plotting::gauss_to_ellipse(gauss); - - gp << "set object " << ++object_counter << " ellipse center " << ellipse.x() << "," << ellipse.y() << " size " << ellipse.major_axis() << "," - << ellipse.minor_axis() << " angle " << ellipse.angle_deg() << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; - - gp << "set object " << ++object_counter << " circle center " << ellipse.x() << "," << ellipse.y() << " size " << 0.02 << " fs empty border lc rgb 'blue'\n"; - } - - gp << "set object " << ++object_counter << " circle center " << x_est.mean()(0) << "," << x_est.mean()(1) << " size " - << 0.05 << " fs empty border lc rgb 'black'\n"; - gp << "set object " << ++object_counter << " circle center " << x_pred.mean()(0) << "," << x_pred.mean()(1) - << " size " << 0.05 << " fs empty border lc rgb 'pink'\n"; - gp << "set object " << ++object_counter << " circle center " << x_final.mean()(0) << "," << x_final.mean()(1) - << " size " << 0.05 << " fs empty border lc rgb 'green'\n"; - - gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_pred.mean()(0) << "," - << x_pred.mean()(1) << " nohead lc rgb 'pink'\n"; - gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_final.mean()(0) << "," - << x_final.mean()(1) << " nohead lc rgb 'green'\n"; + ConstantVelocity dyn_model{1.0}; + IdentitySensorModel sen_model{0.5}; - vortex::utils::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, config.pdaf.mahalanobis_threshold); - gp << "set object " << ++object_counter << " ellipse center " << gate.x() << "," << gate.y() << " size " << gate.major_axis() << "," << gate.minor_axis() - << " angle " << gate.angle_deg() << " fs empty border lc rgb 'cyan'\n"; + auto [x_final, x_pred, z_pred, x_updated, gated] = + PDAF::step(dyn_model, sen_model, 1.0, x_est, meas, config); + std::cout << "x_final: " << x_final.mean() << std::endl; - gp << "replot\n"; - #endif +#if (GNUPLOT_ENABLE) + std::vector> meas_vec; + for (Eigen::Vector2d m : meas.colwise()) { + meas_vec.push_back({m(0), m(1)}); + } + + Gnuplot gp; + gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; + gp << "set size ratio -1\n"; + + gp << "set style circle radius 0.05\n"; + gp << "plot '-' with circles title 'Samples' linecolor rgb 'red' fs " + "transparent solid 1 noborder\n"; + gp.send1d(meas_vec); + + int object_counter = 0; + + for (const auto& state : x_updated) { + vortex::prob::Gauss2d gauss(state.mean().head(2), + state.cov().topLeftCorner(2, 2)); + vortex::utils::Ellipse ellipse = + vortex::plotting::gauss_to_ellipse(gauss); + + gp << "set object " << ++object_counter << " ellipse center " + << ellipse.x() << "," << ellipse.y() << " size " + << ellipse.major_axis() << "," << ellipse.minor_axis() << " angle " + << ellipse.angle_deg() + << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; + + gp << "set object " << ++object_counter << " circle center " + << ellipse.x() << "," << ellipse.y() << " size " << 0.02 + << " fs empty border lc rgb 'blue'\n"; + } + + gp << "set object " << ++object_counter << " circle center " + << x_est.mean()(0) << "," << x_est.mean()(1) << " size " << 0.05 + << " fs empty border lc rgb 'black'\n"; + gp << "set object " << ++object_counter << " circle center " + << x_pred.mean()(0) << "," << x_pred.mean()(1) << " size " << 0.05 + << " fs empty border lc rgb 'pink'\n"; + gp << "set object " << ++object_counter << " circle center " + << x_final.mean()(0) << "," << x_final.mean()(1) << " size " << 0.05 + << " fs empty border lc rgb 'green'\n"; + + gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) + << " to " << x_pred.mean()(0) << "," << x_pred.mean()(1) + << " nohead lc rgb 'pink'\n"; + gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) + << " to " << x_final.mean()(0) << "," << x_final.mean()(1) + << " nohead lc rgb 'green'\n"; + + vortex::utils::Ellipse gate = vortex::plotting::gauss_to_ellipse( + z_pred, config.pdaf.mahalanobis_threshold); + gp << "set object " << ++object_counter << " ellipse center " << gate.x() + << "," << gate.y() << " size " << gate.major_axis() << "," + << gate.minor_axis() << " angle " << gate.angle_deg() + << " fs empty border lc rgb 'cyan'\n"; + + gp << "replot\n"; +#endif } -TEST(PDAF, predict_next_state_3_1) -{ - PDAF::Config config = {.pdaf = { - .mahalanobis_threshold = 4.0, - .prob_of_detection = 0.9, - .clutter_intensity = 1.0, - }}; +TEST(PDAF, predict_next_state_3_1) { + PDAF::Config config = {.pdaf = { + .mahalanobis_threshold = 4.0, + .prob_of_detection = 0.9, + .clutter_intensity = 1.0, + }}; - vortex::prob::Gauss4d x_est(Eigen::Vector4d(0.5, 0.5, -0.75, -0.75), Eigen::Matrix4d::Identity()); + vortex::prob::Gauss4d x_est(Eigen::Vector4d(0.5, 0.5, -0.75, -0.75), + Eigen::Matrix4d::Identity()); - // clang-format off - Eigen::Array2Xd meas = {{0.0, 0.2, 0.8, 0.5, 4.2, 1.4}, + // clang-format off + Eigen::Array2Xd meas = {{0.0, 0.2, 0.8, 0.5, 4.2, 1.4}, {0.5, 0.2, 2.3, 0.0, 2.7, 2.5}}; - // clang-format on - - ConstantVelocity dyn_model{0.5}; - IdentitySensorModel sen_model{1.0}; - - auto [x_final, x_pred, z_pred, x_updated, gated] = - PDAF::step(dyn_model, sen_model, 1.0, x_est, meas, config); - std::cout << "x_final: " << x_final.mean() << std::endl; - - #if (GNUPLOT_ENABLE) - std::vector> meas_vec; - for (Eigen::Vector2d m : meas.colwise()) { - meas_vec.push_back({m(0), m(1)}); - } - - Gnuplot gp; - gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; - gp << "set size ratio -1\n"; - - gp << "set style circle radius 0.05\n"; - gp << "plot '-' with circles title 'Samples' linecolor rgb 'red' fs transparent solid 1 noborder\n"; - gp.send1d(meas_vec); - - int object_counter = 0; - - for (const auto& state : x_updated) - { - vortex::prob::Gauss2d gauss(state.mean().head(2), state.cov().topLeftCorner(2, 2)); - vortex::utils::Ellipse ellipse = vortex::plotting::gauss_to_ellipse(gauss); - - gp << "set object " << ++object_counter << " ellipse center " << ellipse.x() << "," << ellipse.y() << " size " << ellipse.major_axis() << "," - << ellipse.minor_axis() << " angle " << ellipse.angle_deg() << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; - - gp << "set object " << ++object_counter << " circle center " << ellipse.x() << "," << ellipse.y() << " size " << 0.02 << " fs empty border lc rgb 'blue'\n"; - } - - gp << "set object " << ++object_counter << " circle center " << x_est.mean()(0) << "," << x_est.mean()(1) << " size " - << 0.05 << " fs empty border lc rgb 'black'\n"; - gp << "set object " << ++object_counter << " circle center " << 0.5 << "," << 0.5 << " size " << 0.05 - << " fs empty border lc rgb 'black'\n"; - gp << "set object " << ++object_counter << " circle center " << x_pred.mean()(0) << "," << x_pred.mean()(1) - << " size " << 0.05 << " fs empty border lc rgb 'pink'\n"; - gp << "set object " << ++object_counter << " circle center " << x_final.mean()(0) << "," << x_final.mean()(1) - << " size " << 0.05 << " fs empty border lc rgb 'green'\n"; + // clang-format on - gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_pred.mean()(0) << "," - << x_pred.mean()(1) << " nohead lc rgb 'pink'\n"; - gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_final.mean()(0) << "," - << x_final.mean()(1) << " nohead lc rgb 'green'\n"; + ConstantVelocity dyn_model{0.5}; + IdentitySensorModel sen_model{1.0}; - vortex::utils::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, config.pdaf.mahalanobis_threshold); - gp << "set object " << ++object_counter << " ellipse center " << gate.x() << "," << gate.y() << " size " << gate.major_axis() << "," << gate.minor_axis() - << " angle " << gate.angle_deg() << " fs empty border lc rgb 'cyan'\n"; + auto [x_final, x_pred, z_pred, x_updated, gated] = + PDAF::step(dyn_model, sen_model, 1.0, x_est, meas, config); + std::cout << "x_final: " << x_final.mean() << std::endl; - gp << "replot\n"; - #endif +#if (GNUPLOT_ENABLE) + std::vector> meas_vec; + for (Eigen::Vector2d m : meas.colwise()) { + meas_vec.push_back({m(0), m(1)}); + } + + Gnuplot gp; + gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; + gp << "set size ratio -1\n"; + + gp << "set style circle radius 0.05\n"; + gp << "plot '-' with circles title 'Samples' linecolor rgb 'red' fs " + "transparent solid 1 noborder\n"; + gp.send1d(meas_vec); + + int object_counter = 0; + + for (const auto& state : x_updated) { + vortex::prob::Gauss2d gauss(state.mean().head(2), + state.cov().topLeftCorner(2, 2)); + vortex::utils::Ellipse ellipse = + vortex::plotting::gauss_to_ellipse(gauss); + + gp << "set object " << ++object_counter << " ellipse center " + << ellipse.x() << "," << ellipse.y() << " size " + << ellipse.major_axis() << "," << ellipse.minor_axis() << " angle " + << ellipse.angle_deg() + << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; + + gp << "set object " << ++object_counter << " circle center " + << ellipse.x() << "," << ellipse.y() << " size " << 0.02 + << " fs empty border lc rgb 'blue'\n"; + } + + gp << "set object " << ++object_counter << " circle center " + << x_est.mean()(0) << "," << x_est.mean()(1) << " size " << 0.05 + << " fs empty border lc rgb 'black'\n"; + gp << "set object " << ++object_counter << " circle center " << 0.5 << "," + << 0.5 << " size " << 0.05 << " fs empty border lc rgb 'black'\n"; + gp << "set object " << ++object_counter << " circle center " + << x_pred.mean()(0) << "," << x_pred.mean()(1) << " size " << 0.05 + << " fs empty border lc rgb 'pink'\n"; + gp << "set object " << ++object_counter << " circle center " + << x_final.mean()(0) << "," << x_final.mean()(1) << " size " << 0.05 + << " fs empty border lc rgb 'green'\n"; + + gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) + << " to " << x_pred.mean()(0) << "," << x_pred.mean()(1) + << " nohead lc rgb 'pink'\n"; + gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) + << " to " << x_final.mean()(0) << "," << x_final.mean()(1) + << " nohead lc rgb 'green'\n"; + + vortex::utils::Ellipse gate = vortex::plotting::gauss_to_ellipse( + z_pred, config.pdaf.mahalanobis_threshold); + gp << "set object " << ++object_counter << " ellipse center " << gate.x() + << "," << gate.y() << " size " << gate.major_axis() << "," + << gate.minor_axis() << " angle " << gate.angle_deg() + << " fs empty border lc rgb 'cyan'\n"; + + gp << "replot\n"; +#endif } -TEST(PDAF, predict_next_state_3_2) -{ - PDAF::Config config = {.pdaf = { - .mahalanobis_threshold = 4.0, - .prob_of_detection = 0.9, - .clutter_intensity = 1.0, - }}; - vortex::prob::Gauss4d x_est(Eigen::Vector4d(-0.00173734, 0.0766262, -0.614584, -0.57184), - Eigen::Matrix4d::Identity()); - - // clang-format off - Eigen::Array2Xd meas = {{-0.5, -0.23, -2.0, 0.0, 0.14, -2.5}, +TEST(PDAF, predict_next_state_3_2) { + PDAF::Config config = {.pdaf = { + .mahalanobis_threshold = 4.0, + .prob_of_detection = 0.9, + .clutter_intensity = 1.0, + }}; + vortex::prob::Gauss4d x_est( + Eigen::Vector4d(-0.00173734, 0.0766262, -0.614584, -0.57184), + Eigen::Matrix4d::Identity()); + + // clang-format off + Eigen::Array2Xd meas = {{-0.5, -0.23, -2.0, 0.0, 0.14, -2.5}, {2.0 , 0.5, 3.4, 1.3, 0.5 , 0.89}}; - // clang-format on - - ConstantVelocity dyn_model{0.5}; - IdentitySensorModel sen_model{1.0}; - - auto [x_final, x_pred, z_pred, x_updated, gated] = - PDAF::step(dyn_model, sen_model, 1.0, x_est, meas, config); - std::cout << "x_final: " << x_final.mean() << std::endl; - - #if (GNUPLOT_ENABLE) - std::vector> meas_vec; - for (Eigen::Vector2d m : meas.colwise()) { - meas_vec.push_back({m(0), m(1)}); - } - Gnuplot gp; - gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; - gp << "set size ratio -1\n"; - - gp << "set style circle radius 0.05\n"; - gp << "plot '-' with circles title 'Samples' linecolor rgb 'red' fs transparent solid 1 noborder\n"; - gp.send1d(meas_vec); - - int object_counter = 0; - - for (const auto& state : x_updated) - { - vortex::prob::Gauss2d gauss(state.mean().head(2), state.cov().topLeftCorner(2, 2)); - vortex::utils::Ellipse ellipse = vortex::plotting::gauss_to_ellipse(gauss); - - gp << "set object " << ++object_counter << " ellipse center " << ellipse.x() << "," << ellipse.y() << " size " << ellipse.major_axis() << "," - << ellipse.minor_axis() << " angle " << ellipse.angle_deg() << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; - - gp << "set object " << ++object_counter << " circle center " << ellipse.x() << "," << ellipse.y() << " size " << 0.02 << " fs empty border lc rgb 'blue'\n"; - } - - gp << "set object " << ++object_counter << " circle center " << x_est.mean()(0) << "," << x_est.mean()(1) << " size " - << 0.05 << " fs empty border lc rgb 'black'\n"; - gp << "set object " << ++object_counter << " circle center " << x_pred.mean()(0) << "," << x_pred.mean()(1) - << " size " << 0.05 << " fs empty border lc rgb 'pink'\n"; - gp << "set object " << ++object_counter << " circle center " << x_final.mean()(0) << "," << x_final.mean()(1) - << " size " << 0.05 << " fs empty border lc rgb 'green'\n"; - - gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_pred.mean()(0) << "," - << x_pred.mean()(1) << " nohead lc rgb 'pink'\n"; - gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_final.mean()(0) << "," - << x_final.mean()(1) << " nohead lc rgb 'green'\n"; + // clang-format on - // old state from 3_1 - gp << "set object " << ++object_counter << " circle center " << 0.5 << "," << 0.5 << " size " << 0.05 - << " fs empty border lc rgb 'orange-red'\n"; - gp << "set arrow from " << 0.5 << "," << 0.5 << " to " << -0.00173734 << "," << 0.0766262 - << " nohead lc rgb 'orange-red'\n"; + ConstantVelocity dyn_model{0.5}; + IdentitySensorModel sen_model{1.0}; - vortex::utils::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, config.pdaf.mahalanobis_threshold); - gp << "set object " << ++object_counter << " ellipse center " << gate.x() << "," << gate.y() << " size " << gate.major_axis() << "," << gate.minor_axis() - << " angle " << gate.angle_deg() << " fs empty border lc rgb 'cyan'\n"; + auto [x_final, x_pred, z_pred, x_updated, gated] = + PDAF::step(dyn_model, sen_model, 1.0, x_est, meas, config); + std::cout << "x_final: " << x_final.mean() << std::endl; - gp << "replot\n"; - #endif +#if (GNUPLOT_ENABLE) + std::vector> meas_vec; + for (Eigen::Vector2d m : meas.colwise()) { + meas_vec.push_back({m(0), m(1)}); + } + Gnuplot gp; + gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; + gp << "set size ratio -1\n"; + + gp << "set style circle radius 0.05\n"; + gp << "plot '-' with circles title 'Samples' linecolor rgb 'red' fs " + "transparent solid 1 noborder\n"; + gp.send1d(meas_vec); + + int object_counter = 0; + + for (const auto& state : x_updated) { + vortex::prob::Gauss2d gauss(state.mean().head(2), + state.cov().topLeftCorner(2, 2)); + vortex::utils::Ellipse ellipse = + vortex::plotting::gauss_to_ellipse(gauss); + + gp << "set object " << ++object_counter << " ellipse center " + << ellipse.x() << "," << ellipse.y() << " size " + << ellipse.major_axis() << "," << ellipse.minor_axis() << " angle " + << ellipse.angle_deg() + << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; + + gp << "set object " << ++object_counter << " circle center " + << ellipse.x() << "," << ellipse.y() << " size " << 0.02 + << " fs empty border lc rgb 'blue'\n"; + } + + gp << "set object " << ++object_counter << " circle center " + << x_est.mean()(0) << "," << x_est.mean()(1) << " size " << 0.05 + << " fs empty border lc rgb 'black'\n"; + gp << "set object " << ++object_counter << " circle center " + << x_pred.mean()(0) << "," << x_pred.mean()(1) << " size " << 0.05 + << " fs empty border lc rgb 'pink'\n"; + gp << "set object " << ++object_counter << " circle center " + << x_final.mean()(0) << "," << x_final.mean()(1) << " size " << 0.05 + << " fs empty border lc rgb 'green'\n"; + + gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) + << " to " << x_pred.mean()(0) << "," << x_pred.mean()(1) + << " nohead lc rgb 'pink'\n"; + gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) + << " to " << x_final.mean()(0) << "," << x_final.mean()(1) + << " nohead lc rgb 'green'\n"; + + // old state from 3_1 + gp << "set object " << ++object_counter << " circle center " << 0.5 << "," + << 0.5 << " size " << 0.05 << " fs empty border lc rgb 'orange-red'\n"; + gp << "set arrow from " << 0.5 << "," << 0.5 << " to " << -0.00173734 << "," + << 0.0766262 << " nohead lc rgb 'orange-red'\n"; + + vortex::utils::Ellipse gate = vortex::plotting::gauss_to_ellipse( + z_pred, config.pdaf.mahalanobis_threshold); + gp << "set object " << ++object_counter << " ellipse center " << gate.x() + << "," << gate.y() << " size " << gate.major_axis() << "," + << gate.minor_axis() << " angle " << gate.angle_deg() + << " fs empty border lc rgb 'cyan'\n"; + + gp << "replot\n"; +#endif } -TEST(PDAF, predict_next_state_3_3) -{ - PDAF::Config config = {.pdaf = { - .mahalanobis_threshold = 4.0, - .prob_of_detection = 0.9, - .clutter_intensity = 1.0, - }}; - vortex::prob::Gauss4d x_est(Eigen::Vector4d(-0.55929, 0.0694888, -0.583476, -0.26382), Eigen::Matrix4d::Identity()); - - // clang-format off - Eigen::Array2Xd meas = {{-1.5, -1.2, -0.8, -1.7, -2.0, -1.11}, +TEST(PDAF, predict_next_state_3_3) { + PDAF::Config config = {.pdaf = { + .mahalanobis_threshold = 4.0, + .prob_of_detection = 0.9, + .clutter_intensity = 1.0, + }}; + vortex::prob::Gauss4d x_est( + Eigen::Vector4d(-0.55929, 0.0694888, -0.583476, -0.26382), + Eigen::Matrix4d::Identity()); + + // clang-format off + Eigen::Array2Xd meas = {{-1.5, -1.2, -0.8, -1.7, -2.0, -1.11}, { 2.5, 2.7, 2.3, 1.9, 3.0, 2.04}}; - // clang-format on - - ConstantVelocity dyn_model{0.5}; - IdentitySensorModel sen_model{1.0}; - - auto [x_final, x_pred, z_pred, x_updated, gated] = - PDAF::step(dyn_model, sen_model, 1.0, x_est, meas, config); - std::cout << "x_final: " << x_final.mean() << std::endl; - - #if (GNUPLOT_ENABLE) - std::vector> meas_vec; - for (Eigen::Vector2d m : meas.colwise()) { - meas_vec.push_back({m(0), m(1)}); - } - - Gnuplot gp; - gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; - gp << "set size ratio -1\n"; - - gp << "set style circle radius 0.05\n"; - gp << "plot '-' with circles title 'Samples' linecolor rgb 'red' fs transparent solid 1 noborder\n"; - gp.send1d(meas_vec); - - int object_counter = 0; - - for (const auto& state : x_updated) - { - vortex::prob::Gauss2d gauss(state.mean().head(2), state.cov().topLeftCorner(2, 2)); - vortex::utils::Ellipse ellipse = vortex::plotting::gauss_to_ellipse(gauss); - - gp << "set object " << ++object_counter << " ellipse center " << ellipse.x() << "," << ellipse.y() << " size " << ellipse.major_axis() << "," - << ellipse.minor_axis() << " angle " << ellipse.angle_deg() << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; - - gp << "set object " << ++object_counter << " circle center " << ellipse.x() << "," << ellipse.y() << " size " << 0.02 << " fs empty border lc rgb 'blue'\n"; - } - - gp << "set object " << ++object_counter << " circle center " << x_est.mean()(0) << "," << x_est.mean()(1) << " size " - << 0.05 << " fs empty border lc rgb 'black'\n"; - gp << "set object " << ++object_counter << " circle center " << x_pred.mean()(0) << "," << x_pred.mean()(1) - << " size " << 0.05 << " fs empty border lc rgb 'pink'\n"; - gp << "set object " << ++object_counter << " circle center " << x_final.mean()(0) << "," << x_final.mean()(1) - << " size " << 0.05 << " fs empty border lc rgb 'green'\n"; - - gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_pred.mean()(0) << "," - << x_pred.mean()(1) << " nohead lc rgb 'pink'\n"; - gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_final.mean()(0) << "," - << x_final.mean()(1) << " nohead lc rgb 'green'\n"; + // clang-format on - // old state from 3_1, 3_2 - gp << "set object " << ++object_counter << " circle center " << 0.5 << "," << 0.5 << " size " << 0.05 - << " fs empty border lc rgb 'orange-red'\n"; - gp << "set arrow from " << 0.5 << "," << 0.5 << " to " << -0.00173734 << "," << 0.0766262 - << " nohead lc rgb 'orange-red'\n"; - gp << "set object " << ++object_counter << " circle center " << -0.00173734 << "," << 0.0766262 << " size " << 0.05 - << " fs empty border lc rgb 'orange-red'\n"; - gp << "set arrow from " << -0.00173734 << "," << 0.0766262 << " to " << -0.55929 << "," << 0.0694888 - << " nohead lc rgb 'orange-red'\n"; + ConstantVelocity dyn_model{0.5}; + IdentitySensorModel sen_model{1.0}; - vortex::utils::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, config.pdaf.mahalanobis_threshold); - gp << "set object " << ++object_counter << " ellipse center " << gate.x() << "," << gate.y() << " size " << gate.major_axis() << "," << gate.minor_axis() - << " angle " << gate.angle_deg() << " fs empty border lc rgb 'cyan'\n"; + auto [x_final, x_pred, z_pred, x_updated, gated] = + PDAF::step(dyn_model, sen_model, 1.0, x_est, meas, config); + std::cout << "x_final: " << x_final.mean() << std::endl; - gp << "replot\n"; - #endif +#if (GNUPLOT_ENABLE) + std::vector> meas_vec; + for (Eigen::Vector2d m : meas.colwise()) { + meas_vec.push_back({m(0), m(1)}); + } + + Gnuplot gp; + gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; + gp << "set size ratio -1\n"; + + gp << "set style circle radius 0.05\n"; + gp << "plot '-' with circles title 'Samples' linecolor rgb 'red' fs " + "transparent solid 1 noborder\n"; + gp.send1d(meas_vec); + + int object_counter = 0; + + for (const auto& state : x_updated) { + vortex::prob::Gauss2d gauss(state.mean().head(2), + state.cov().topLeftCorner(2, 2)); + vortex::utils::Ellipse ellipse = + vortex::plotting::gauss_to_ellipse(gauss); + + gp << "set object " << ++object_counter << " ellipse center " + << ellipse.x() << "," << ellipse.y() << " size " + << ellipse.major_axis() << "," << ellipse.minor_axis() << " angle " + << ellipse.angle_deg() + << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; + + gp << "set object " << ++object_counter << " circle center " + << ellipse.x() << "," << ellipse.y() << " size " << 0.02 + << " fs empty border lc rgb 'blue'\n"; + } + + gp << "set object " << ++object_counter << " circle center " + << x_est.mean()(0) << "," << x_est.mean()(1) << " size " << 0.05 + << " fs empty border lc rgb 'black'\n"; + gp << "set object " << ++object_counter << " circle center " + << x_pred.mean()(0) << "," << x_pred.mean()(1) << " size " << 0.05 + << " fs empty border lc rgb 'pink'\n"; + gp << "set object " << ++object_counter << " circle center " + << x_final.mean()(0) << "," << x_final.mean()(1) << " size " << 0.05 + << " fs empty border lc rgb 'green'\n"; + + gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) + << " to " << x_pred.mean()(0) << "," << x_pred.mean()(1) + << " nohead lc rgb 'pink'\n"; + gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) + << " to " << x_final.mean()(0) << "," << x_final.mean()(1) + << " nohead lc rgb 'green'\n"; + + // old state from 3_1, 3_2 + gp << "set object " << ++object_counter << " circle center " << 0.5 << "," + << 0.5 << " size " << 0.05 << " fs empty border lc rgb 'orange-red'\n"; + gp << "set arrow from " << 0.5 << "," << 0.5 << " to " << -0.00173734 << "," + << 0.0766262 << " nohead lc rgb 'orange-red'\n"; + gp << "set object " << ++object_counter << " circle center " << -0.00173734 + << "," << 0.0766262 << " size " << 0.05 + << " fs empty border lc rgb 'orange-red'\n"; + gp << "set arrow from " << -0.00173734 << "," << 0.0766262 << " to " + << -0.55929 << "," << 0.0694888 << " nohead lc rgb 'orange-red'\n"; + + vortex::utils::Ellipse gate = vortex::plotting::gauss_to_ellipse( + z_pred, config.pdaf.mahalanobis_threshold); + gp << "set object " << ++object_counter << " ellipse center " << gate.x() + << "," << gate.y() << " size " << gate.major_axis() << "," + << gate.minor_axis() << " angle " << gate.angle_deg() + << " fs empty border lc rgb 'cyan'\n"; + + gp << "replot\n"; +#endif } -TEST(PDAF, predict_next_state_3_4) -{ - PDAF::Config config = {.pdaf = { - .mahalanobis_threshold = 4.0, - .prob_of_detection = 0.9, - .clutter_intensity = 1.0, - }}; - - vortex::prob::Gauss4d x_est(Eigen::Vector4d(-1.20613, 0.610616, -0.618037, 0.175242), Eigen::Matrix4d::Identity()); - // clang-format off - Eigen::Array2Xd meas = {{-2.0, -1.8, -2.3, 0.6, -2.0, -1.4}, +TEST(PDAF, predict_next_state_3_4) { + PDAF::Config config = {.pdaf = { + .mahalanobis_threshold = 4.0, + .prob_of_detection = 0.9, + .clutter_intensity = 1.0, + }}; + + vortex::prob::Gauss4d x_est( + Eigen::Vector4d(-1.20613, 0.610616, -0.618037, 0.175242), + Eigen::Matrix4d::Identity()); + // clang-format off + Eigen::Array2Xd meas = {{-2.0, -1.8, -2.3, 0.6, -2.0, -1.4}, { 2.2, 2.3, 2.0, 1.5, 2.0, 2.5}}; - // clang-format on - - ConstantVelocity dyn_model{0.5}; - IdentitySensorModel sen_model{1.0}; - - auto [x_final, x_pred, z_pred, x_updated, gated] = - PDAF::step(dyn_model, sen_model, 1.0, x_est, meas, config); - std::cout << "x_final: " << x_final.mean() << std::endl; - - #if (GNUPLOT_ENABLE) - std::vector> meas_vec; - for (Eigen::Vector2d m : meas.colwise()) { - meas_vec.push_back({m(0), m(1)}); - } + // clang-format on - Gnuplot gp; - gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; - gp << "set size ratio -1\n"; - - gp << "set style circle radius 0.05\n"; - gp << "plot '-' with circles title 'Samples' linecolor rgb 'red' fs transparent solid 1 noborder\n"; - gp.send1d(meas_vec); - - int object_counter = 0; - - for (const auto& state : x_updated) - { - vortex::prob::Gauss2d gauss(state.mean().head(2), state.cov().topLeftCorner(2, 2)); - vortex::utils::Ellipse ellipse = vortex::plotting::gauss_to_ellipse(gauss); - - gp << "set object " << ++object_counter << " ellipse center " << ellipse.x() << "," << ellipse.y() << " size " << ellipse.major_axis() << "," - << ellipse.minor_axis() << " angle " << ellipse.angle_deg() << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; - - gp << "set object " << ++object_counter << " circle center " << ellipse.x() << "," << ellipse.y() << " size " << 0.02 << " fs empty border lc rgb 'blue'\n"; - } + ConstantVelocity dyn_model{0.5}; + IdentitySensorModel sen_model{1.0}; - gp << "set object " << ++object_counter << " circle center " << x_est.mean()(0) << "," << x_est.mean()(1) << " size " - << 0.05 << " fs empty border lc rgb 'black'\n"; - gp << "set object " << ++object_counter << " circle center " << x_pred.mean()(0) << "," << x_pred.mean()(1) - << " size " << 0.05 << " fs empty border lc rgb 'pink'\n"; - gp << "set object " << ++object_counter << " circle center " << x_final.mean()(0) << "," << x_final.mean()(1) - << " size " << 0.05 << " fs empty border lc rgb 'green'\n"; - - gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_pred.mean()(0) << "," - << x_pred.mean()(1) << " nohead lc rgb 'pink'\n"; - gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) << " to " << x_final.mean()(0) << "," - << x_final.mean()(1) << " nohead lc rgb 'green'\n"; - - // old state from 3_1, 3_2, 3_3 - gp << "set object " << ++object_counter << " circle center " << 0.5 << "," << 0.5 << " size " << 0.05 - << " fs empty border lc rgb 'orange-red'\n"; - gp << "set arrow from " << 0.5 << "," << 0.5 << " to " << -0.00173734 << "," << 0.0766262 - << " nohead lc rgb 'orange-red'\n"; - gp << "set object " << ++object_counter << " circle center " << -0.00173734 << "," << 0.0766262 << " size " << 0.05 - << " fs empty border lc rgb 'orange-red'\n"; - gp << "set arrow from " << -0.00173734 << "," << 0.0766262 << " to " << -0.55929 << "," << 0.0694888 - << " nohead lc rgb 'orange-red'\n"; - gp << "set object " << ++object_counter << " circle center " << -0.55929 << "," << 0.0694888 << " size " << 0.05 - << " fs empty border lc rgb 'orange-red'\n"; - gp << "set arrow from " << -0.55929 << "," << 0.0694888 << " to " << -1.20613 << "," << 0.610616 - << " nohead lc rgb 'orange-red'\n"; - - vortex::utils::Ellipse gate = vortex::plotting::gauss_to_ellipse(z_pred, config.pdaf.mahalanobis_threshold); - gp << "set object " << ++object_counter << " ellipse center " << gate.x() << "," << gate.y() << " size " << gate.major_axis() << "," << gate.minor_axis() - << " angle " << gate.angle_deg() << " fs empty border lc rgb 'cyan'\n"; + auto [x_final, x_pred, z_pred, x_updated, gated] = + PDAF::step(dyn_model, sen_model, 1.0, x_est, meas, config); + std::cout << "x_final: " << x_final.mean() << std::endl; - gp << "replot\n"; - #endif -} \ No newline at end of file +#if (GNUPLOT_ENABLE) + std::vector> meas_vec; + for (Eigen::Vector2d m : meas.colwise()) { + meas_vec.push_back({m(0), m(1)}); + } + + Gnuplot gp; + gp << "set xrange [-8:8]\nset yrange [-8:8]\n"; + gp << "set size ratio -1\n"; + + gp << "set style circle radius 0.05\n"; + gp << "plot '-' with circles title 'Samples' linecolor rgb 'red' fs " + "transparent solid 1 noborder\n"; + gp.send1d(meas_vec); + + int object_counter = 0; + + for (const auto& state : x_updated) { + vortex::prob::Gauss2d gauss(state.mean().head(2), + state.cov().topLeftCorner(2, 2)); + vortex::utils::Ellipse ellipse = + vortex::plotting::gauss_to_ellipse(gauss); + + gp << "set object " << ++object_counter << " ellipse center " + << ellipse.x() << "," << ellipse.y() << " size " + << ellipse.major_axis() << "," << ellipse.minor_axis() << " angle " + << ellipse.angle_deg() + << " back fc rgb 'skyblue' fs transparent solid 0.4 noborder\n"; + + gp << "set object " << ++object_counter << " circle center " + << ellipse.x() << "," << ellipse.y() << " size " << 0.02 + << " fs empty border lc rgb 'blue'\n"; + } + + gp << "set object " << ++object_counter << " circle center " + << x_est.mean()(0) << "," << x_est.mean()(1) << " size " << 0.05 + << " fs empty border lc rgb 'black'\n"; + gp << "set object " << ++object_counter << " circle center " + << x_pred.mean()(0) << "," << x_pred.mean()(1) << " size " << 0.05 + << " fs empty border lc rgb 'pink'\n"; + gp << "set object " << ++object_counter << " circle center " + << x_final.mean()(0) << "," << x_final.mean()(1) << " size " << 0.05 + << " fs empty border lc rgb 'green'\n"; + + gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) + << " to " << x_pred.mean()(0) << "," << x_pred.mean()(1) + << " nohead lc rgb 'pink'\n"; + gp << "set arrow from " << x_est.mean()(0) << "," << x_est.mean()(1) + << " to " << x_final.mean()(0) << "," << x_final.mean()(1) + << " nohead lc rgb 'green'\n"; + + // old state from 3_1, 3_2, 3_3 + gp << "set object " << ++object_counter << " circle center " << 0.5 << "," + << 0.5 << " size " << 0.05 << " fs empty border lc rgb 'orange-red'\n"; + gp << "set arrow from " << 0.5 << "," << 0.5 << " to " << -0.00173734 << "," + << 0.0766262 << " nohead lc rgb 'orange-red'\n"; + gp << "set object " << ++object_counter << " circle center " << -0.00173734 + << "," << 0.0766262 << " size " << 0.05 + << " fs empty border lc rgb 'orange-red'\n"; + gp << "set arrow from " << -0.00173734 << "," << 0.0766262 << " to " + << -0.55929 << "," << 0.0694888 << " nohead lc rgb 'orange-red'\n"; + gp << "set object " << ++object_counter << " circle center " << -0.55929 + << "," << 0.0694888 << " size " << 0.05 + << " fs empty border lc rgb 'orange-red'\n"; + gp << "set arrow from " << -0.55929 << "," << 0.0694888 << " to " + << -1.20613 << "," << 0.610616 << " nohead lc rgb 'orange-red'\n"; + + vortex::utils::Ellipse gate = vortex::plotting::gauss_to_ellipse( + z_pred, config.pdaf.mahalanobis_threshold); + gp << "set object " << ++object_counter << " ellipse center " << gate.x() + << "," << gate.y() << " size " << gate.major_axis() << "," + << gate.minor_axis() << " angle " << gate.angle_deg() + << " fs empty border lc rgb 'cyan'\n"; + + gp << "replot\n"; +#endif +} diff --git a/vortex-filtering/test/probability_test.cpp b/vortex-filtering/test/probability_test.cpp index 761a3594..3cb8eaab 100644 --- a/vortex-filtering/test/probability_test.cpp +++ b/vortex-filtering/test/probability_test.cpp @@ -1,7 +1,7 @@ #include #include -#include #include +#include #include #include @@ -11,234 +11,247 @@ #include "gtest_assertions.hpp" -TEST(MultiVarGauss, initGaussian) -{ - vortex::prob::MultiVarGauss<2> gaussian(Eigen::Vector2d::Zero(), Eigen::Matrix2d::Identity()); +TEST(MultiVarGauss, initGaussian) { + vortex::prob::MultiVarGauss<2> gaussian(Eigen::Vector2d::Zero(), + Eigen::Matrix2d::Identity()); - EXPECT_EQ(gaussian.mean(), (Eigen::Vector2d{0, 0})); - EXPECT_EQ(gaussian.cov(), (Eigen::Matrix2d{{1, 0}, {0, 1}})); + EXPECT_EQ(gaussian.mean(), (Eigen::Vector2d{0, 0})); + EXPECT_EQ(gaussian.cov(), (Eigen::Matrix2d{{1, 0}, {0, 1}})); } -TEST(MultiVarGauss, assignmentOperator) -{ - vortex::prob::MultiVarGauss<2> gaussian1(Eigen::Vector2d::Zero(), Eigen::Matrix2d::Identity()); - vortex::prob::MultiVarGauss<2> gaussian2(Eigen::Vector2d::Ones(), Eigen::Matrix2d::Identity()); +TEST(MultiVarGauss, assignmentOperator) { + vortex::prob::MultiVarGauss<2> gaussian1(Eigen::Vector2d::Zero(), + Eigen::Matrix2d::Identity()); + vortex::prob::MultiVarGauss<2> gaussian2(Eigen::Vector2d::Ones(), + Eigen::Matrix2d::Identity()); - gaussian2 = gaussian1; + gaussian2 = gaussian1; - EXPECT_EQ(gaussian2.mean(), gaussian1.mean()); - EXPECT_EQ(gaussian2.cov(), gaussian1.cov()); + EXPECT_EQ(gaussian2.mean(), gaussian1.mean()); + EXPECT_EQ(gaussian2.cov(), gaussian1.cov()); } -TEST(MultiVarGauss, copyConstructor) -{ - vortex::prob::MultiVarGauss<2> gaussian1(Eigen::Vector2d::Zero(), Eigen::Matrix2d::Identity()); - vortex::prob::MultiVarGauss<2> gaussian2(gaussian1); +TEST(MultiVarGauss, copyConstructor) { + vortex::prob::MultiVarGauss<2> gaussian1(Eigen::Vector2d::Zero(), + Eigen::Matrix2d::Identity()); + vortex::prob::MultiVarGauss<2> gaussian2(gaussian1); - EXPECT_EQ(gaussian2.mean(), gaussian1.mean()); - EXPECT_EQ(gaussian2.cov(), gaussian1.cov()); + EXPECT_EQ(gaussian2.mean(), gaussian1.mean()); + EXPECT_EQ(gaussian2.cov(), gaussian1.cov()); } -TEST(MultiVarGauss, pdf) -{ - vortex::prob::MultiVarGauss<2> gaussian(Eigen::Vector2d::Zero(), Eigen::Matrix2d::Identity()); - constexpr double PI = std::numbers::pi; - EXPECT_NEAR(gaussian.pdf(Eigen::Vector2d{0, 0}), 1 / (2 * PI), 1e-15); - EXPECT_NEAR(gaussian.pdf(Eigen::Vector2d{1, 0}), 1 / (2 * std::sqrt(M_E) * PI), 1e-15); - EXPECT_NEAR(gaussian.pdf(Eigen::Vector2d{0, 1}), 1 / (2 * std::sqrt(M_E) * PI), 1e-15); - EXPECT_NEAR(gaussian.pdf(Eigen::Vector2d{1, 1}), 1 / (2 * M_E * PI), 1e-15); - - gaussian = vortex::prob::MultiVarGauss<2>(Eigen::Vector2d{0, 0}, Eigen::Matrix2d{{2, 1}, {1, 2}}); - - EXPECT_NEAR(gaussian.pdf(Eigen::Vector2d{0, 0}), 1 / (2 * std::sqrt(3) * PI), 1e-15); - EXPECT_NEAR(gaussian.pdf(Eigen::Vector2d{1, 0}), 1 / (2 * std::sqrt(3) * std::exp(1.0 / 3) * PI), 1e-15); - EXPECT_NEAR(gaussian.pdf(Eigen::Vector2d{0, 1}), 1 / (2 * std::sqrt(3) * std::exp(1.0 / 3) * PI), 1e-15); - EXPECT_NEAR(gaussian.pdf(Eigen::Vector2d{1, 1}), 1 / (2 * std::sqrt(3) * std::exp(1.0 / 3) * PI), 1e-15); +TEST(MultiVarGauss, pdf) { + vortex::prob::MultiVarGauss<2> gaussian(Eigen::Vector2d::Zero(), + Eigen::Matrix2d::Identity()); + constexpr double PI = std::numbers::pi; + EXPECT_NEAR(gaussian.pdf(Eigen::Vector2d{0, 0}), 1 / (2 * PI), 1e-15); + EXPECT_NEAR(gaussian.pdf(Eigen::Vector2d{1, 0}), + 1 / (2 * std::sqrt(M_E) * PI), 1e-15); + EXPECT_NEAR(gaussian.pdf(Eigen::Vector2d{0, 1}), + 1 / (2 * std::sqrt(M_E) * PI), 1e-15); + EXPECT_NEAR(gaussian.pdf(Eigen::Vector2d{1, 1}), 1 / (2 * M_E * PI), 1e-15); + + gaussian = vortex::prob::MultiVarGauss<2>(Eigen::Vector2d{0, 0}, + Eigen::Matrix2d{{2, 1}, {1, 2}}); + + EXPECT_NEAR(gaussian.pdf(Eigen::Vector2d{0, 0}), + 1 / (2 * std::sqrt(3) * PI), 1e-15); + EXPECT_NEAR(gaussian.pdf(Eigen::Vector2d{1, 0}), + 1 / (2 * std::sqrt(3) * std::exp(1.0 / 3) * PI), 1e-15); + EXPECT_NEAR(gaussian.pdf(Eigen::Vector2d{0, 1}), + 1 / (2 * std::sqrt(3) * std::exp(1.0 / 3) * PI), 1e-15); + EXPECT_NEAR(gaussian.pdf(Eigen::Vector2d{1, 1}), + 1 / (2 * std::sqrt(3) * std::exp(1.0 / 3) * PI), 1e-15); } -TEST(MultiVarGauss, invalidCovariance) -{ - EXPECT_THROW(vortex::prob::MultiVarGauss<2>(Eigen::Vector2d::Zero(), Eigen::Matrix2d::Zero()), std::invalid_argument); - EXPECT_THROW(vortex::prob::MultiVarGauss<2>(Eigen::Vector2d::Zero(), Eigen::Matrix2d{{1, 0}, {0, 0}}), std::invalid_argument); - EXPECT_THROW(vortex::prob::MultiVarGauss<2>(Eigen::Vector2d::Zero(), Eigen::Matrix2d{{-1, 0}, {0, -1}}), std::invalid_argument); +TEST(MultiVarGauss, invalidCovariance) { + EXPECT_THROW(vortex::prob::MultiVarGauss<2>(Eigen::Vector2d::Zero(), + Eigen::Matrix2d::Zero()), + std::invalid_argument); + EXPECT_THROW(vortex::prob::MultiVarGauss<2>( + Eigen::Vector2d::Zero(), Eigen::Matrix2d{{1, 0}, {0, 0}}), + std::invalid_argument); + EXPECT_THROW( + vortex::prob::MultiVarGauss<2>(Eigen::Vector2d::Zero(), + Eigen::Matrix2d{{-1, 0}, {0, -1}}), + std::invalid_argument); } -TEST(MultiVarGauss, sample) -{ - Eigen::Vector2d true_mean{4, -2}; - Eigen::Matrix2d true_cov = Eigen::Matrix2d{{4, 1}, {1, 2}}; - - vortex::prob::MultiVarGauss<2> gaussian(true_mean, true_cov); - std::random_device rd; - std::mt19937 gen(rd()); - - std::vector samples; - for (size_t i = 0; i < 10000; ++i) { - samples.push_back(gaussian.sample(gen)); - } - - Eigen::Vector2d mean = Eigen::Vector2d::Zero(); - for (const auto &sample : samples) { - mean += sample; - } - mean /= samples.size(); - - Eigen::Matrix2d cov = Eigen::Matrix2d::Zero(); - for (const auto &sample : samples) { - cov += (sample - mean) * (sample - mean).transpose(); - } - cov /= samples.size(); - - vortex::utils::Ellipse cov_ellipse({true_mean, true_cov}); - - double majorAxisLength = cov_ellipse.major_axis(); - double minorAxisLength = cov_ellipse.minor_axis(); - double angle = cov_ellipse.angle_deg(); - - #if (GNUPLOT_ENABLE) - Gnuplot gp; - gp << "set xrange [-10:10]\nset yrange [-10:10]\n"; - gp << "set style circle radius 0.05\n"; - gp << "plot '-' with circles title 'Samples' fs transparent solid 0.05 noborder\n"; - gp.send1d(samples); - - gp << "set object 1 ellipse center " << true_mean(0) << "," << true_mean(1) << " size " << majorAxisLength << "," << minorAxisLength << " angle " << angle - << "fs empty border lc rgb 'cyan'\n"; - gp << "replot\n"; - #endif - - EXPECT_TRUE(isApproxEqual(mean, true_mean, 0.5)); - EXPECT_TRUE(isApproxEqual(cov, true_cov, 0.5)); - - (void)majorAxisLength; - (void)minorAxisLength; - (void)angle; +TEST(MultiVarGauss, sample) { + Eigen::Vector2d true_mean{4, -2}; + Eigen::Matrix2d true_cov = Eigen::Matrix2d{{4, 1}, {1, 2}}; + + vortex::prob::MultiVarGauss<2> gaussian(true_mean, true_cov); + std::random_device rd; + std::mt19937 gen(rd()); + + std::vector samples; + for (size_t i = 0; i < 10000; ++i) { + samples.push_back(gaussian.sample(gen)); + } + + Eigen::Vector2d mean = Eigen::Vector2d::Zero(); + for (const auto& sample : samples) { + mean += sample; + } + mean /= samples.size(); + + Eigen::Matrix2d cov = Eigen::Matrix2d::Zero(); + for (const auto& sample : samples) { + cov += (sample - mean) * (sample - mean).transpose(); + } + cov /= samples.size(); + + vortex::utils::Ellipse cov_ellipse({true_mean, true_cov}); + + double majorAxisLength = cov_ellipse.major_axis(); + double minorAxisLength = cov_ellipse.minor_axis(); + double angle = cov_ellipse.angle_deg(); + +#if (GNUPLOT_ENABLE) + Gnuplot gp; + gp << "set xrange [-10:10]\nset yrange [-10:10]\n"; + gp << "set style circle radius 0.05\n"; + gp << "plot '-' with circles title 'Samples' fs transparent solid 0.05 " + "noborder\n"; + gp.send1d(samples); + + gp << "set object 1 ellipse center " << true_mean(0) << "," << true_mean(1) + << " size " << majorAxisLength << "," << minorAxisLength << " angle " + << angle << "fs empty border lc rgb 'cyan'\n"; + gp << "replot\n"; +#endif + + EXPECT_TRUE(isApproxEqual(mean, true_mean, 0.5)); + EXPECT_TRUE(isApproxEqual(cov, true_cov, 0.5)); + + (void)majorAxisLength; + (void)minorAxisLength; + (void)angle; } -TEST(MultiVarGauss, mahalanobisDistanceIdentityCovariance) -{ - using vortex::prob::Gauss2d; - auto gaussian = Gauss2d::Standard(); +TEST(MultiVarGauss, mahalanobisDistanceIdentityCovariance) { + using vortex::prob::Gauss2d; + auto gaussian = Gauss2d::Standard(); - EXPECT_DOUBLE_EQ(gaussian.mahalanobis_distance({0, 0}), 0); - EXPECT_DOUBLE_EQ(gaussian.mahalanobis_distance({1, 0}), 1); - EXPECT_DOUBLE_EQ(gaussian.mahalanobis_distance({0, 1}), 1); - EXPECT_DOUBLE_EQ(gaussian.mahalanobis_distance({1, 1}), std::sqrt(2)); + EXPECT_DOUBLE_EQ(gaussian.mahalanobis_distance({0, 0}), 0); + EXPECT_DOUBLE_EQ(gaussian.mahalanobis_distance({1, 0}), 1); + EXPECT_DOUBLE_EQ(gaussian.mahalanobis_distance({0, 1}), 1); + EXPECT_DOUBLE_EQ(gaussian.mahalanobis_distance({1, 1}), std::sqrt(2)); } -TEST(isContainerConcept, compileTimeChecks) -{ - static_assert(vortex::prob::concepts::is_container, double>); - static_assert(vortex::prob::concepts::is_container, double>); - static_assert(vortex::prob::concepts::is_container, vortex::prob::Gauss2d>); - static_assert(vortex::prob::concepts::is_container); - static_assert(vortex::prob::concepts::is_container); - static_assert(vortex::prob::concepts::is_container); - - static_assert(!vortex::prob::concepts::is_container); - - EXPECT_TRUE(true); +TEST(isContainerConcept, compileTimeChecks) { + static_assert( + vortex::prob::concepts::is_container, double>); + static_assert( + vortex::prob::concepts::is_container, double>); + static_assert( + vortex::prob::concepts::is_container, + vortex::prob::Gauss2d>); + static_assert( + vortex::prob::concepts::is_container); + static_assert( + vortex::prob::concepts::is_container); + static_assert( + vortex::prob::concepts::is_container); + + static_assert(!vortex::prob::concepts::is_container); + + EXPECT_TRUE(true); } -TEST(GaussianMixture, defaultConstructor) -{ - vortex::prob::GaussianMixture<2> mixture; +TEST(GaussianMixture, defaultConstructor) { + vortex::prob::GaussianMixture<2> mixture; - EXPECT_EQ(mixture.size(), 0u); + EXPECT_EQ(mixture.size(), 0u); } -TEST(GaussianMixture, stdVectorConstructor) -{ - using vortex::prob::Gauss2d; - std::vector weights{1, 2}; - std::vector gaussians{Gauss2d::Standard(), Gauss2d::Standard()}; +TEST(GaussianMixture, stdVectorConstructor) { + using vortex::prob::Gauss2d; + std::vector weights{1, 2}; + std::vector gaussians{Gauss2d::Standard(), Gauss2d::Standard()}; - vortex::prob::GaussianMixture<2> mixture{weights, gaussians}; + vortex::prob::GaussianMixture<2> mixture{weights, gaussians}; - EXPECT_EQ(mixture.size(), 2u); + EXPECT_EQ(mixture.size(), 2u); - Eigen::VectorXd weights_eigen(2); - weights_eigen << 1, 2; - EXPECT_EQ(mixture.weights(), weights_eigen); - EXPECT_EQ(mixture.gaussians(), gaussians); + Eigen::VectorXd weights_eigen(2); + weights_eigen << 1, 2; + EXPECT_EQ(mixture.weights(), weights_eigen); + EXPECT_EQ(mixture.gaussians(), gaussians); } -TEST(GaussianMixture, stdArrayConstructor) -{ - using vortex::prob::Gauss2d; - std::array weights{1, 2}; - std::array gaussians{Gauss2d::Standard(), Gauss2d::Standard()}; +TEST(GaussianMixture, stdArrayConstructor) { + using vortex::prob::Gauss2d; + std::array weights{1, 2}; + std::array gaussians{Gauss2d::Standard(), Gauss2d::Standard()}; - vortex::prob::GaussianMixture<2> mixture{weights, gaussians}; + vortex::prob::GaussianMixture<2> mixture{weights, gaussians}; - EXPECT_EQ(mixture.size(), 2u); + EXPECT_EQ(mixture.size(), 2u); - Eigen::VectorXd weights_eigen(2); - weights_eigen << 1, 2; - EXPECT_EQ(mixture.weights(), weights_eigen); - EXPECT_EQ(mixture.gaussians().at(0), gaussians.at(0)); - EXPECT_EQ(mixture.gaussians().at(1), gaussians.at(1)); + Eigen::VectorXd weights_eigen(2); + weights_eigen << 1, 2; + EXPECT_EQ(mixture.weights(), weights_eigen); + EXPECT_EQ(mixture.gaussians().at(0), gaussians.at(0)); + EXPECT_EQ(mixture.gaussians().at(1), gaussians.at(1)); } -TEST(GaussianMixture, eigenVectorConstructor) -{ - using vortex::prob::Gauss2d; - Eigen::VectorXd weights(2); - weights << 1, 2; - std::vector gaussians{Gauss2d::Standard(), Gauss2d::Standard()}; +TEST(GaussianMixture, eigenVectorConstructor) { + using vortex::prob::Gauss2d; + Eigen::VectorXd weights(2); + weights << 1, 2; + std::vector gaussians{Gauss2d::Standard(), Gauss2d::Standard()}; - vortex::prob::GaussianMixture<2> mixture{weights, gaussians}; + vortex::prob::GaussianMixture<2> mixture{weights, gaussians}; - EXPECT_EQ(mixture.size(), 2u); - EXPECT_EQ(mixture.weights(), weights); - EXPECT_EQ(mixture.gaussians().at(0), gaussians.at(0)); - EXPECT_EQ(mixture.gaussians().at(1), gaussians.at(1)); + EXPECT_EQ(mixture.size(), 2u); + EXPECT_EQ(mixture.weights(), weights); + EXPECT_EQ(mixture.gaussians().at(0), gaussians.at(0)); + EXPECT_EQ(mixture.gaussians().at(1), gaussians.at(1)); } -TEST(GaussianMixture, mixTwoEqualWeightEqualCovarianceComponents) -{ - using vortex::prob::Gauss2d; - std::vector weights{0.5, 0.5}; +TEST(GaussianMixture, mixTwoEqualWeightEqualCovarianceComponents) { + using vortex::prob::Gauss2d; + std::vector weights{0.5, 0.5}; - Gauss2d gaussian1{{0, 0}, Eigen::Matrix2d::Identity()}; - Gauss2d gaussian2{{10, 0}, Eigen::Matrix2d::Identity()}; - std::vector gaussians{gaussian1, gaussian2}; + Gauss2d gaussian1{{0, 0}, Eigen::Matrix2d::Identity()}; + Gauss2d gaussian2{{10, 0}, Eigen::Matrix2d::Identity()}; + std::vector gaussians{gaussian1, gaussian2}; - Eigen::Vector2d center{5, 0}; + Eigen::Vector2d center{5, 0}; - vortex::prob::GaussianMixture<2> mixture{weights, gaussians}; + vortex::prob::GaussianMixture<2> mixture{weights, gaussians}; - EXPECT_TRUE(isApproxEqual(mixture.reduce().mean(), center, 1e-15)); + EXPECT_TRUE(isApproxEqual(mixture.reduce().mean(), center, 1e-15)); } -TEST(GaussianMixture, mixTwoEqualWeightDifferentCovarianceComponents) -{ - using vortex::prob::Gauss2d; - std::vector weights{0.5, 0.5}; +TEST(GaussianMixture, mixTwoEqualWeightDifferentCovarianceComponents) { + using vortex::prob::Gauss2d; + std::vector weights{0.5, 0.5}; - Gauss2d gaussian1{{0, 0}, Eigen::Matrix2d::Identity()}; - Gauss2d gaussian2{{10, 0}, Eigen::Matrix2d::Identity() * 2}; - std::vector gaussians{gaussian1, gaussian2}; + Gauss2d gaussian1{{0, 0}, Eigen::Matrix2d::Identity()}; + Gauss2d gaussian2{{10, 0}, Eigen::Matrix2d::Identity() * 2}; + std::vector gaussians{gaussian1, gaussian2}; - vortex::prob::GaussianMixture<2> mixture{weights, gaussians}; + vortex::prob::GaussianMixture<2> mixture{weights, gaussians}; - Eigen::Vector2d center{5, 0}; + Eigen::Vector2d center{5, 0}; - EXPECT_TRUE(isApproxEqual(mixture.reduce().mean(), center, 1e-15)); + EXPECT_TRUE(isApproxEqual(mixture.reduce().mean(), center, 1e-15)); } -TEST(GaussianMixture, mixTwoDifferentWeightEqualCovarianceComponents) -{ - using vortex::prob::Gauss2d; - std::vector weights{0.25, 0.75}; +TEST(GaussianMixture, mixTwoDifferentWeightEqualCovarianceComponents) { + using vortex::prob::Gauss2d; + std::vector weights{0.25, 0.75}; - Gauss2d gaussian1{{0, 0}, Eigen::Matrix2d::Identity()}; - Gauss2d gaussian2{{10, 0}, Eigen::Matrix2d::Identity()}; - std::vector gaussians{gaussian1, gaussian2}; + Gauss2d gaussian1{{0, 0}, Eigen::Matrix2d::Identity()}; + Gauss2d gaussian2{{10, 0}, Eigen::Matrix2d::Identity()}; + std::vector gaussians{gaussian1, gaussian2}; - vortex::prob::GaussianMixture<2> mixture{weights, gaussians}; + vortex::prob::GaussianMixture<2> mixture{weights, gaussians}; - Eigen::Vector2d center{7.5, 0}; + Eigen::Vector2d center{7.5, 0}; - EXPECT_TRUE(isApproxEqual(mixture.reduce().mean(), center, 1e-15)); + EXPECT_TRUE(isApproxEqual(mixture.reduce().mean(), center, 1e-15)); } diff --git a/vortex-filtering/test/sensor_model_test.cpp b/vortex-filtering/test/sensor_model_test.cpp index 5986f3f1..9de58f96 100644 --- a/vortex-filtering/test/sensor_model_test.cpp +++ b/vortex-filtering/test/sensor_model_test.cpp @@ -12,35 +12,33 @@ using SensorModel = vortex::models::IdentitySensorModel<2, 1>; using T = vortex::Types_xz; -using Vec_x = T::Vec_x; -using Vec_z = T::Vec_z; -using Mat_xx = T::Mat_xx; -using Mat_zz = T::Mat_zz; -using Mat_zx = T::Mat_zx; -using Mat_xz = T::Mat_xz; +using Vec_x = T::Vec_x; +using Vec_z = T::Vec_z; +using Mat_xx = T::Mat_xx; +using Mat_zz = T::Mat_zz; +using Mat_zx = T::Mat_zx; +using Mat_xz = T::Mat_xz; using Gauss_x = T::Gauss_x; using Gauss_z = T::Gauss_z; -TEST(SensorModel, initSimpleModel) -{ - SensorModel model(0.1); - EXPECT_EQ(model.h(Vec_x::Zero()), Vec_z::Zero()); +TEST(SensorModel, initSimpleModel) { + SensorModel model(0.1); + EXPECT_EQ(model.h(Vec_x::Zero()), Vec_z::Zero()); - Vec_x x{1, 2}; - Vec_z z{1}; - EXPECT_EQ(model.h(x), z); + Vec_x x{1, 2}; + Vec_z z{1}; + EXPECT_EQ(model.h(x), z); } -TEST(SensorModel, predictSimpleModel) -{ - SensorModel model(std::sqrt(0.1)); - Gauss_x x_est{Vec_x::Zero(), Mat_xx::Identity()}; - Gauss_z pred = model.pred_from_est(x_est); - EXPECT_EQ(pred.mean(), Vec_z::Zero()); - EXPECT_TRUE(pred.cov().isApprox(Mat_zz::Identity() * 1.1)); +TEST(SensorModel, predictSimpleModel) { + SensorModel model(std::sqrt(0.1)); + Gauss_x x_est{Vec_x::Zero(), Mat_xx::Identity()}; + Gauss_z pred = model.pred_from_est(x_est); + EXPECT_EQ(pred.mean(), Vec_z::Zero()); + EXPECT_TRUE(pred.cov().isApprox(Mat_zz::Identity() * 1.1)); - pred = model.pred_from_state(x_est.mean()); - EXPECT_TRUE(pred.cov().isApprox(Mat_zz::Identity() * 0.1)); + pred = model.pred_from_state(x_est.mean()); + EXPECT_TRUE(pred.cov().isApprox(Mat_zz::Identity() * 0.1)); } -} // namespace simple_sensor_model_test +} // namespace simple_sensor_model_test diff --git a/vortex-filtering/test/state_test.cpp b/vortex-filtering/test/state_test.cpp index 728673d1..4925c9a8 100644 --- a/vortex-filtering/test/state_test.cpp +++ b/vortex-filtering/test/state_test.cpp @@ -3,121 +3,130 @@ #include #include -TEST(State, typeChecks) -{ - using namespace vortex; - - using S = StateName; - using StateT = State; - - ASSERT_EQ(StateT::N_STATES, 4); - ASSERT_EQ(StateT::UNIQUE_STATES_COUNT, 3); - ASSERT_EQ(StateT::state_loc(S::position).start_index, 0); - ASSERT_EQ(StateT::state_loc(S::velocity).start_index, 1); - ASSERT_EQ(StateT::state_loc(S::acceleration).start_index, 3); - - ASSERT_EQ(StateT::state_loc(S::position).end_index, 1); - ASSERT_EQ(StateT::state_loc(S::velocity).end_index, 3); - ASSERT_EQ(StateT::state_loc(S::acceleration).end_index, 4); - - ASSERT_TRUE(StateT::has_state_name(S::position)); - ASSERT_TRUE(StateT::has_state_name(S::velocity)); - ASSERT_TRUE(StateT::has_state_name(S::acceleration)); - ASSERT_FALSE(StateT::has_state_name(S::jerk)); +TEST(State, typeChecks) { + using namespace vortex; + + using S = StateName; + using StateT = + State; + + ASSERT_EQ(StateT::N_STATES, 4); + ASSERT_EQ(StateT::UNIQUE_STATES_COUNT, 3); + ASSERT_EQ(StateT::state_loc(S::position).start_index, 0); + ASSERT_EQ(StateT::state_loc(S::velocity).start_index, 1); + ASSERT_EQ(StateT::state_loc(S::acceleration).start_index, 3); + + ASSERT_EQ(StateT::state_loc(S::position).end_index, 1); + ASSERT_EQ(StateT::state_loc(S::velocity).end_index, 3); + ASSERT_EQ(StateT::state_loc(S::acceleration).end_index, 4); + + ASSERT_TRUE(StateT::has_state_name(S::position)); + ASSERT_TRUE(StateT::has_state_name(S::velocity)); + ASSERT_TRUE(StateT::has_state_name(S::acceleration)); + ASSERT_FALSE(StateT::has_state_name(S::jerk)); } -TEST(State, init) -{ - using namespace vortex; +TEST(State, init) { + using namespace vortex; - using S = StateName; - using StateT = State; + using S = StateName; + using StateT = + State; - auto x = prob::Gauss4d::Standard(); - StateT state(x); + auto x = prob::Gauss4d::Standard(); + StateT state(x); - ASSERT_EQ(state.mean(), x.mean()); - ASSERT_EQ(state.cov(), x.cov()); - ASSERT_EQ(state.gauss(), x); + ASSERT_EQ(state.mean(), x.mean()); + ASSERT_EQ(state.cov(), x.cov()); + ASSERT_EQ(state.gauss(), x); } -TEST(State, getMean) -{ - using namespace vortex; +TEST(State, getMean) { + using namespace vortex; - using S = StateName; - using StateT = State; + using S = StateName; + using StateT = + State; - auto x = prob::Gauss4d::Standard(); - StateT state(x); + auto x = prob::Gauss4d::Standard(); + StateT state(x); - ASSERT_TRUE(isApproxEqual(state.mean_of(), x.mean().template head<1>(), 1e-6)); - ASSERT_TRUE(isApproxEqual(state.mean_of(), x.mean().template segment<2>(1), 1e-6)); - ASSERT_TRUE(isApproxEqual(state.mean_of(), x.mean().template tail<1>(), 1e-6)); + ASSERT_TRUE(isApproxEqual(state.mean_of(), + x.mean().template head<1>(), 1e-6)); + ASSERT_TRUE(isApproxEqual(state.mean_of(), + x.mean().template segment<2>(1), 1e-6)); + ASSERT_TRUE(isApproxEqual(state.mean_of(), + x.mean().template tail<1>(), 1e-6)); } -TEST(State, getCov) -{ - using namespace vortex; +TEST(State, getCov) { + using namespace vortex; - using S = StateName; - using StateT = State; + using S = StateName; + using StateT = + State; - auto x = prob::Gauss4d::Standard(); - StateT state(x); + auto x = prob::Gauss4d::Standard(); + StateT state(x); - ASSERT_TRUE(isApproxEqual(state.cov_of(), x.cov().template block<1, 1>(0, 0), 1e-6)); - ASSERT_TRUE(isApproxEqual(state.cov_of(), x.cov().template block<2, 2>(1, 1), 1e-6)); - ASSERT_TRUE(isApproxEqual(state.cov_of(), x.cov().template block<1, 1>(3, 3), 1e-6)); + ASSERT_TRUE(isApproxEqual(state.cov_of(), + x.cov().template block<1, 1>(0, 0), 1e-6)); + ASSERT_TRUE(isApproxEqual(state.cov_of(), + x.cov().template block<2, 2>(1, 1), 1e-6)); + ASSERT_TRUE(isApproxEqual(state.cov_of(), + x.cov().template block<1, 1>(3, 3), 1e-6)); } -TEST(State, setMean) -{ - using namespace vortex; +TEST(State, setMean) { + using namespace vortex; - using S = StateName; - using StateT = State; + using S = StateName; + using StateT = + State; - auto x = prob::Gauss4d::Standard(); - StateT state(x); + auto x = prob::Gauss4d::Standard(); + StateT state(x); - StateT::T::Vec_n mean = StateT::T::Vec_n::Random(); - StateT::T::Mat_nn cov = StateT::T::Mat_nn::Random(); + StateT::T::Vec_n mean = StateT::T::Vec_n::Random(); + StateT::T::Mat_nn cov = StateT::T::Mat_nn::Random(); - cov = 0.5 * (cov + cov.transpose()).eval(); - cov += StateT::T::Mat_nn::Identity() * StateT::N_STATES; + cov = 0.5 * (cov + cov.transpose()).eval(); + cov += StateT::T::Mat_nn::Identity() * StateT::N_STATES; - StateT::T::Gauss_n x_new = {mean, cov}; - state.set_mean_of(x_new.mean().template head<1>()); - state.set_mean_of(x_new.mean().template segment<2>(1)); - state.set_mean_of(x_new.mean().template tail<1>()); + StateT::T::Gauss_n x_new = {mean, cov}; + state.set_mean_of(x_new.mean().template head<1>()); + state.set_mean_of(x_new.mean().template segment<2>(1)); + state.set_mean_of(x_new.mean().template tail<1>()); - ASSERT_TRUE(isApproxEqual(state.mean(), x_new.mean(), 1e-6)); + ASSERT_TRUE(isApproxEqual(state.mean(), x_new.mean(), 1e-6)); } -TEST(State, setCov) -{ - using namespace vortex; +TEST(State, setCov) { + using namespace vortex; - using S = StateName; - using StateT = State; + using S = StateName; + using StateT = + State; - auto x = prob::Gauss4d::Standard(); - StateT state(x); + auto x = prob::Gauss4d::Standard(); + StateT state(x); - StateT::T::Vec_n mean = StateT::T::Vec_n::Random(); - StateT::T::Mat_nn cov = StateT::T::Mat_nn::Random(); - cov = 0.5 * (cov + cov.transpose()).eval(); - cov += StateT::T::Mat_nn::Identity() * StateT::N_STATES; + StateT::T::Vec_n mean = StateT::T::Vec_n::Random(); + StateT::T::Mat_nn cov = StateT::T::Mat_nn::Random(); + cov = 0.5 * (cov + cov.transpose()).eval(); + cov += StateT::T::Mat_nn::Identity() * StateT::N_STATES; - StateT::T::Gauss_n x_new = {mean, cov}; - state.set_cov_of(x_new.cov().template block<1, 1>(0, 0)); - state.set_cov_of(x_new.cov().template block<2, 2>(1, 1)); - state.set_cov_of(x_new.cov().template block<1, 1>(3, 3)); + StateT::T::Gauss_n x_new = {mean, cov}; + state.set_cov_of(x_new.cov().template block<1, 1>(0, 0)); + state.set_cov_of(x_new.cov().template block<2, 2>(1, 1)); + state.set_cov_of(x_new.cov().template block<1, 1>(3, 3)); - ASSERT_TRUE(isApproxEqual(state.cov_of(), x_new.cov().template block<1, 1>(0, 0), 1e-6)); - ASSERT_TRUE(isApproxEqual(state.cov_of(), x_new.cov().template block<2, 2>(1, 1), 1e-6)); - ASSERT_TRUE(isApproxEqual(state.cov_of(), x_new.cov().template block<1, 1>(3, 3), 1e-6)); + ASSERT_TRUE(isApproxEqual(state.cov_of(), + x_new.cov().template block<1, 1>(0, 0), 1e-6)); + ASSERT_TRUE(isApproxEqual(state.cov_of(), + x_new.cov().template block<2, 2>(1, 1), 1e-6)); + ASSERT_TRUE(isApproxEqual(state.cov_of(), + x_new.cov().template block<1, 1>(3, 3), 1e-6)); - ASSERT_FALSE(isApproxEqual(state.cov(), x_new.cov(), 1e-6)); -} \ No newline at end of file + ASSERT_FALSE(isApproxEqual(state.cov(), x_new.cov(), 1e-6)); +} diff --git a/vortex-filtering/test/test_models.hpp b/vortex-filtering/test/test_models.hpp index 0aef6c4c..3d429b4a 100644 --- a/vortex-filtering/test/test_models.hpp +++ b/vortex-filtering/test/test_models.hpp @@ -7,112 +7,126 @@ using vortex::models::interface::DynamicModel; using vortex::models::interface::DynamicModelCTLTV; class SimpleDynamicModel : public DynamicModelCTLTV<2> { -public: - constexpr static int N_DIM_x = DynamicModel<2>::N_DIM_x; - constexpr static int N_DIM_u = DynamicModel<2>::N_DIM_u; - constexpr static int N_DIM_v = DynamicModel<2>::N_DIM_v; + public: + constexpr static int N_DIM_x = DynamicModel<2>::N_DIM_x; + constexpr static int N_DIM_u = DynamicModel<2>::N_DIM_u; + constexpr static int N_DIM_v = DynamicModel<2>::N_DIM_v; - using T = vortex::Types_xuv; + using T = vortex::Types_xuv; - T::Mat_xx A_c(const T::Vec_x & = T::Vec_x::Zero()) const override { return -T::Mat_xx::Identity(); } + T::Mat_xx A_c(const T::Vec_x& = T::Vec_x::Zero()) const override { + return -T::Mat_xx::Identity(); + } - T::Mat_vv Q_c(const T::Vec_x & = T::Vec_x::Zero()) const override { return T::Mat_xx::Identity(); } + T::Mat_vv Q_c(const T::Vec_x& = T::Vec_x::Zero()) const override { + return T::Mat_xx::Identity(); + } }; class NonlinearModel1 : public DynamicModel<1, 1, 1> { -public: - constexpr static int N_DIM_x = DynamicModel<1, 1, 1>::N_DIM_x; - constexpr static int N_DIM_u = DynamicModel<1, 1, 1>::N_DIM_u; - constexpr static int N_DIM_v = DynamicModel<1, 1, 1>::N_DIM_v; - - using T = vortex::Types_xuv; - - NonlinearModel1(double std_dev) - : cov_(std_dev * std_dev) - { - } - - T::Vec_x f_d(double, const T::Vec_x &x, const T::Vec_u & = T::Vec_u::Zero(), const T::Vec_v &v = T::Vec_v::Zero()) const override - { - typename T::Vec_x x_next; - x_next << std::sin(x(0)) + v(0); - return x_next; - } - - T::Mat_vv Q_d(double = 0.0, const T::Vec_x & = T::Vec_x::Zero()) const override { return T::Mat_xx::Identity() * cov_; } - -private: - const double cov_; + public: + constexpr static int N_DIM_x = DynamicModel<1, 1, 1>::N_DIM_x; + constexpr static int N_DIM_u = DynamicModel<1, 1, 1>::N_DIM_u; + constexpr static int N_DIM_v = DynamicModel<1, 1, 1>::N_DIM_v; + + using T = vortex::Types_xuv; + + NonlinearModel1(double std_dev) : cov_(std_dev * std_dev) {} + + T::Vec_x f_d(double, + const T::Vec_x& x, + const T::Vec_u& = T::Vec_u::Zero(), + const T::Vec_v& v = T::Vec_v::Zero()) const override { + typename T::Vec_x x_next; + x_next << std::sin(x(0)) + v(0); + return x_next; + } + + T::Mat_vv Q_d(double = 0.0, + const T::Vec_x& = T::Vec_x::Zero()) const override { + return T::Mat_xx::Identity() * cov_; + } + + private: + const double cov_; }; // https://en.wikipedia.org/wiki/Lorenz_system class LorenzAttractorCT : public DynamicModel<3> { -public: - static constexpr int N_DIM_x = DynamicModel<3>::N_DIM_x; - static constexpr int N_DIM_u = DynamicModel<3>::N_DIM_u; - static constexpr int N_DIM_v = DynamicModel<3>::N_DIM_v; - - using T = vortex::Types_xuv; - - LorenzAttractorCT(double std_dev) - : cov_(std_dev * std_dev) - , sigma_(10.0) - , beta_(8.0 / 3.0) - , rho_(28.0) - { - } - - T::Vec_x f_c(const T::Vec_x &x, const T::Vec_u & = T::Vec_u::Zero(), const T::Vec_v &v = T::Vec_v::Zero()) const - { - typename T::Vec_x x_next; - x_next << sigma_ * (x(1) - x(0)), x(0) * (rho_ - x(2)) - x(1), x(0) * x(1) - beta_ * x(2); - x_next += v; - return x_next; - } - - T::Vec_x f_d(double dt, const T::Vec_x &x, const T::Vec_u &u = T::Vec_u::Zero(), const T::Vec_v &v = T::Vec_v::Zero()) const override - { - using Dyn_mod_func = std::function; - - Dyn_mod_func f_c = [this, &u, &v](double, const T::Vec_x &x) { return this->f_c(x, u, v); }; - return vortex::integrator::RK4::integrate(f_c, dt, x); - } - - T::Mat_vv Q_d(double dt, const T::Vec_x & = T::Vec_x::Zero()) const override { return T::Mat_xx::Identity() * cov_ * dt; } - -private: - const double cov_; - const double sigma_; - const double beta_; - const double rho_; + public: + static constexpr int N_DIM_x = DynamicModel<3>::N_DIM_x; + static constexpr int N_DIM_u = DynamicModel<3>::N_DIM_u; + static constexpr int N_DIM_v = DynamicModel<3>::N_DIM_v; + + using T = vortex::Types_xuv; + + LorenzAttractorCT(double std_dev) + : cov_(std_dev * std_dev), sigma_(10.0), beta_(8.0 / 3.0), rho_(28.0) {} + + T::Vec_x f_c(const T::Vec_x& x, + const T::Vec_u& = T::Vec_u::Zero(), + const T::Vec_v& v = T::Vec_v::Zero()) const { + typename T::Vec_x x_next; + x_next << sigma_ * (x(1) - x(0)), x(0) * (rho_ - x(2)) - x(1), + x(0) * x(1) - beta_ * x(2); + x_next += v; + return x_next; + } + + T::Vec_x f_d(double dt, + const T::Vec_x& x, + const T::Vec_u& u = T::Vec_u::Zero(), + const T::Vec_v& v = T::Vec_v::Zero()) const override { + using Dyn_mod_func = + std::function; + + Dyn_mod_func f_c = [this, &u, &v](double, const T::Vec_x& x) { + return this->f_c(x, u, v); + }; + return vortex::integrator::RK4::integrate(f_c, dt, x); + } + + T::Mat_vv Q_d(double dt, + const T::Vec_x& = T::Vec_x::Zero()) const override { + return T::Mat_xx::Identity() * cov_ * dt; + } + + private: + const double cov_; + const double sigma_; + const double beta_; + const double rho_; }; -class LorenzAttractorCTLTV : public vortex::models::interface::DynamicModelCTLTV<3> { -public: - static constexpr int N_DIM_x = DynamicModelCTLTV<3>::N_DIM_x; - static constexpr int N_DIM_u = DynamicModelCTLTV<3>::N_DIM_u; - static constexpr int N_DIM_v = DynamicModelCTLTV<3>::N_DIM_v; +class LorenzAttractorCTLTV + : public vortex::models::interface::DynamicModelCTLTV<3> { + public: + static constexpr int N_DIM_x = DynamicModelCTLTV<3>::N_DIM_x; + static constexpr int N_DIM_u = DynamicModelCTLTV<3>::N_DIM_u; + static constexpr int N_DIM_v = DynamicModelCTLTV<3>::N_DIM_v; - using T = vortex::Types_xuv; + using T = vortex::Types_xuv; - LorenzAttractorCTLTV(double std_dev) : cov_(std_dev * std_dev), sigma_(10.0), beta_(8.0 / 3.0), rho_(28.0) {} + LorenzAttractorCTLTV(double std_dev) + : cov_(std_dev * std_dev), sigma_(10.0), beta_(8.0 / 3.0), rho_(28.0) {} - T::Mat_xx A_c(const T::Vec_x &x) const override - { - typename T::Mat_xx A; - // clang-format off + T::Mat_xx A_c(const T::Vec_x& x) const override { + typename T::Mat_xx A; + // clang-format off A << -sigma_ , sigma_, 0.0 , rho_-x(2), -1.0 , -x(0) , x(1) , x(0) , -beta_; - // clang-format on - return A; - } - - T::Mat_vv Q_c(const T::Vec_x & = T::Vec_x::Zero()) const override { return T::Mat_xx::Identity() * cov_; } - -private: - const double cov_; - const double sigma_; - const double beta_; - const double rho_; + // clang-format on + return A; + } + + T::Mat_vv Q_c(const T::Vec_x& = T::Vec_x::Zero()) const override { + return T::Mat_xx::Identity() * cov_; + } + + private: + const double cov_; + const double sigma_; + const double beta_; + const double rho_; }; diff --git a/vortex-filtering/test/types_test.cpp b/vortex-filtering/test/types_test.cpp index 695689d0..19e59b68 100644 --- a/vortex-filtering/test/types_test.cpp +++ b/vortex-filtering/test/types_test.cpp @@ -5,79 +5,89 @@ #include #include #include -TEST(Types, x_2_z_1) -{ - using T = vortex::Types_xz<2, 1>; - T::Vec_x x; - T::Vec_z z; +TEST(Types, x_2_z_1) { + using T = vortex::Types_xz<2, 1>; + T::Vec_x x; + T::Vec_z z; - static_assert(std::is_same::value, "x is not a Vector2d"); - static_assert(std::is_same>::value, "z is not a Vector1d"); + static_assert(std::is_same::value, + "x is not a Vector2d"); + static_assert(std::is_same>::value, + "z is not a Vector1d"); - ASSERT_TRUE(true); + ASSERT_TRUE(true); } #include #include #include -TEST(Concepts, MatConvertibleTo) -{ - static_assert(std::convertible_to); - static_assert(vortex::concepts::mat_convertible_to); - static_assert(!vortex::concepts::mat_convertible_to); +TEST(Concepts, MatConvertibleTo) { + static_assert(std::convertible_to); + static_assert( + vortex::concepts::mat_convertible_to); + static_assert(!vortex::concepts::mat_convertible_to); - ASSERT_TRUE(true); + ASSERT_TRUE(true); } -TEST(Concepts, MultiVarGaussLike) -{ - using vortex::prob::Gauss2d, vortex::prob::Gauss3d; +TEST(Concepts, MultiVarGaussLike) { + using vortex::prob::Gauss2d, vortex::prob::Gauss3d; - static_assert(vortex::concepts::MultiVarGaussLike); - static_assert(vortex::concepts::MultiVarGaussLike); + static_assert(vortex::concepts::MultiVarGaussLike); + static_assert(vortex::concepts::MultiVarGaussLike); - static_assert(!vortex::concepts::MultiVarGaussLike); + static_assert(!vortex::concepts::MultiVarGaussLike); - using S = vortex::StateName; - using StateT = vortex::State; - static_assert(vortex::concepts::MultiVarGaussLike); + using S = vortex::StateName; + using StateT = + vortex::State; + static_assert( + vortex::concepts::MultiVarGaussLike); - ASSERT_TRUE(true); + ASSERT_TRUE(true); } -TEST(Concepts, Models) -{ - constexpr size_t N_DIM_x = 5; - constexpr size_t N_DIM_z = 4; - constexpr size_t N_DIM_u = 3; - constexpr size_t N_DIM_v = 2; - constexpr size_t N_DIM_w = 1; - - using DynMod = vortex::models::interface::DynamicModel; - using DynModLTV = vortex::models::interface::DynamicModelLTV; - using SensMod = vortex::models::interface::SensorModel; - using SensModLTV = vortex::models::interface::SensorModelLTV; - - static_assert(vortex::concepts::DynamicModel); - static_assert(vortex::concepts::DynamicModelLTV); - static_assert(vortex::concepts::SensorModel); - static_assert(vortex::concepts::SensorModelLTV); - - static_assert(vortex::concepts::DynamicModelWithDefinedSizes); - static_assert(vortex::concepts::DynamicModelLTVWithDefinedSizes); - static_assert(vortex::concepts::SensorModelWithDefinedSizes); - static_assert(vortex::concepts::SensorModelLTVWithDefinedSizes); - - static_assert(vortex::concepts::DynamicModelWithDefinedSizes); - static_assert(!vortex::concepts::DynamicModelLTVWithDefinedSizes); - static_assert(vortex::concepts::SensorModelWithDefinedSizes); - static_assert(!vortex::concepts::SensorModelLTVWithDefinedSizes); - - static_assert(!vortex::concepts::DynamicModel); - static_assert(!vortex::concepts::DynamicModelLTV); - static_assert(!vortex::concepts::SensorModel); - static_assert(!vortex::concepts::SensorModelLTV); - - ASSERT_TRUE(true); -} \ No newline at end of file +TEST(Concepts, Models) { + constexpr size_t N_DIM_x = 5; + constexpr size_t N_DIM_z = 4; + constexpr size_t N_DIM_u = 3; + constexpr size_t N_DIM_v = 2; + constexpr size_t N_DIM_w = 1; + + using DynMod = + vortex::models::interface::DynamicModel; + using DynModLTV = + vortex::models::interface::DynamicModelLTV; + using SensMod = + vortex::models::interface::SensorModel; + using SensModLTV = + vortex::models::interface::SensorModelLTV; + + static_assert( + vortex::concepts::DynamicModel); + static_assert(vortex::concepts::DynamicModelLTV); + static_assert( + vortex::concepts::SensorModel); + static_assert(vortex::concepts::SensorModelLTV); + + static_assert(vortex::concepts::DynamicModelWithDefinedSizes); + static_assert(vortex::concepts::DynamicModelLTVWithDefinedSizes); + static_assert(vortex::concepts::SensorModelWithDefinedSizes); + static_assert(vortex::concepts::SensorModelLTVWithDefinedSizes); + + static_assert(vortex::concepts::DynamicModelWithDefinedSizes); + static_assert(!vortex::concepts::DynamicModelLTVWithDefinedSizes); + static_assert(vortex::concepts::SensorModelWithDefinedSizes); + static_assert(!vortex::concepts::SensorModelLTVWithDefinedSizes); + + static_assert(!vortex::concepts::DynamicModel); + static_assert(!vortex::concepts::DynamicModelLTV); + static_assert(!vortex::concepts::SensorModel); + static_assert(!vortex::concepts::SensorModelLTV); + + ASSERT_TRUE(true); +} diff --git a/vortex-filtering/test/ukf_test.cpp b/vortex-filtering/test/ukf_test.cpp index 9217f1f0..2c4dd13a 100644 --- a/vortex-filtering/test/ukf_test.cpp +++ b/vortex-filtering/test/ukf_test.cpp @@ -1,142 +1,146 @@ -#include #include #include +#include #include #include #include -#include "gtest_assertions.hpp" -#include "test_models.hpp" #include #include #include +#include "gtest_assertions.hpp" +#include "test_models.hpp" class UKFtest : public ::testing::Test { -protected: - using IdentitySensorModel = vortex::models::IdentitySensorModel<1, 1>; + protected: + using IdentitySensorModel = vortex::models::IdentitySensorModel<1, 1>; - static constexpr int N_DIM_x = NonlinearModel1::N_DIM_x; - static constexpr int N_DIM_z = IdentitySensorModel::N_DIM_z; - static constexpr int N_DIM_u = NonlinearModel1::N_DIM_u; - static constexpr int N_DIM_v = NonlinearModel1::N_DIM_v; - static constexpr int N_DIM_w = IdentitySensorModel::N_DIM_w; - - using T = vortex::Types_xzuvw; + static constexpr int N_DIM_x = NonlinearModel1::N_DIM_x; + static constexpr int N_DIM_z = IdentitySensorModel::N_DIM_z; + static constexpr int N_DIM_u = NonlinearModel1::N_DIM_u; + static constexpr int N_DIM_v = NonlinearModel1::N_DIM_v; + static constexpr int N_DIM_w = IdentitySensorModel::N_DIM_w; - using Vec_x = typename T::Vec_x; - using Mat_xx = typename T::Mat_xx; - using Gauss_x = typename T::Gauss_x; + using T = vortex::Types_xzuvw; - using Vec_z = typename T::Vec_z; - using Mat_zz = typename T::Mat_zz; - using Gauss_z = typename T::Gauss_z; + using Vec_x = typename T::Vec_x; + using Mat_xx = typename T::Mat_xx; + using Gauss_x = typename T::Gauss_x; - using UKF = vortex::filter::UKF; + using Vec_z = typename T::Vec_z; + using Mat_zz = typename T::Mat_zz; + using Gauss_z = typename T::Gauss_z; - static constexpr double Q = 0.1; - static constexpr double R = 0.1; - UKFtest() : dynamic_model_(Q), sensor_model_(R) {} + using UKF = vortex::filter::UKF; + static constexpr double Q = 0.1; + static constexpr double R = 0.1; + UKFtest() : dynamic_model_(Q), sensor_model_(R) {} - NonlinearModel1 dynamic_model_; - IdentitySensorModel sensor_model_; + NonlinearModel1 dynamic_model_; + IdentitySensorModel sensor_model_; }; -TEST_F(UKFtest, Predict) -{ - // Initial state - Gauss_x x(Vec_x::Zero(), Mat_xx::Identity()); - double dt = 0.1; - // Predict - auto [x_est_pred, z_est_pred] = UKF::predict(dynamic_model_, sensor_model_, dt, x); - - Vec_x x_true = dynamic_model_.f_d(dt, x.mean()); - Vec_z z_true = x_true.head(1); - ASSERT_TRUE(isApproxEqual(x_est_pred.mean(), x_true, 1e-6)); - ASSERT_TRUE(isApproxEqual(z_est_pred.mean(), z_true, 1e-6)); +TEST_F(UKFtest, Predict) { + // Initial state + Gauss_x x(Vec_x::Zero(), Mat_xx::Identity()); + double dt = 0.1; + // Predict + auto [x_est_pred, z_est_pred] = + UKF::predict(dynamic_model_, sensor_model_, dt, x); + + Vec_x x_true = dynamic_model_.f_d(dt, x.mean()); + Vec_z z_true = x_true.head(1); + ASSERT_TRUE(isApproxEqual(x_est_pred.mean(), x_true, 1e-6)); + ASSERT_TRUE(isApproxEqual(z_est_pred.mean(), z_true, 1e-6)); } -TEST_F(UKFtest, Convergence) -{ - // Random number generator - std::random_device rd; - std::mt19937 gen(rd()); - std::normal_distribution<> d_disturbance{0, 1e-3}; - std::normal_distribution<> d_noise{0, 1e-2}; - - // Initial state - Gauss_x x0(4 * Vec_x::Ones(), Mat_xx::Identity()); - // Time step - double dt = 0.1; - // Number of steps - int n_steps = 1000; - - std::vector time; - std::vector x_true; - std::vector x_est; - std::vector z_meas; - std::vector z_est; - - time.push_back(0); - x_est.push_back(x0); - z_meas.push_back(sensor_model_.h(x0.mean())); - x_true.push_back(x0.mean()); - z_est.push_back({sensor_model_.h(x0.mean()), sensor_model_.R()}); - for (int i = 0; i < n_steps - 1; i++) { - // Simulate - Vec_x v; - v << d_disturbance(gen); - Vec_z w = Vec_z::Zero(); - w << d_noise(gen); - Vec_x x_true_i = dynamic_model_.f_d(dt, x_true.back(), Vec_x::Zero(), v); - Vec_z z_meas_i = sensor_model_.h(x_true_i) + w; - x_true.push_back(x_true_i); - z_meas.push_back(z_meas_i); - - // Predict and update - auto [x_est_upd, x_est_pred, z_est_pred] = UKF::step(dynamic_model_, sensor_model_, dt, x_est.back(), z_meas_i, Vec_x::Zero()); - - // Save results - time.push_back(time.back() + dt); - x_est.push_back(x_est_upd); - z_est.push_back(z_est_pred); - } - - // Check convergence - double tol = 1e-1; - - // Plot results - - std::vector x_est_mean, z_est_mean, x_est_std, x_p_std, x_m_std; - for (int i = 0; i < n_steps; i++) { - x_est_mean.push_back(x_est.at(i).mean()(0)); - z_est_mean.push_back(z_est.at(i).mean()(0)); - x_est_std.push_back(std::sqrt(x_est.at(i).cov()(0, 0))); - x_p_std.push_back(x_est.at(i).mean()(0) + std::sqrt(x_est.at(i).cov()(0, 0))); // x_est + std - x_m_std.push_back(x_est.at(i).mean()(0) - std::sqrt(x_est.at(i).cov()(0, 0))); // x_est - std - } - - #if (GNUPLOT_ENABLE) - Gnuplot gp; - gp << "set terminal wxt size 1200,800\n"; - gp << "set title 'UKF convergence'\n"; - gp << "set xlabel 'Time [s]'\n"; - gp << "set ylabel 'x'\n"; - gp << "set grid\n"; - gp << "set key left top\n"; - gp << "plot '-' using 1:($2-$3):($2+$3) with filledcurves title 'x_{est} +/- std' fs solid 0.5, " - << "'-' with lines title 'x_{true}', " - << "'-' with lines title 'x_{est}', " - << "'-' with lines title 'z_{meas}' \n"; - gp.send1d(std::make_tuple(time, x_est_mean, x_est_std)); - gp.send1d(std::make_tuple(time, x_true)); - gp.send1d(std::make_tuple(time, x_est_mean)); - gp.send1d(std::make_tuple(time, z_meas)); - #endif - - // Check convergence - - ASSERT_NEAR(x_est.back().mean()(0), x_true.back()(0), tol); - ASSERT_NEAR(z_est.back().mean()(0), z_meas.back()(0), tol); -} \ No newline at end of file +TEST_F(UKFtest, Convergence) { + // Random number generator + std::random_device rd; + std::mt19937 gen(rd()); + std::normal_distribution<> d_disturbance{0, 1e-3}; + std::normal_distribution<> d_noise{0, 1e-2}; + + // Initial state + Gauss_x x0(4 * Vec_x::Ones(), Mat_xx::Identity()); + // Time step + double dt = 0.1; + // Number of steps + int n_steps = 1000; + + std::vector time; + std::vector x_true; + std::vector x_est; + std::vector z_meas; + std::vector z_est; + + time.push_back(0); + x_est.push_back(x0); + z_meas.push_back(sensor_model_.h(x0.mean())); + x_true.push_back(x0.mean()); + z_est.push_back({sensor_model_.h(x0.mean()), sensor_model_.R()}); + for (int i = 0; i < n_steps - 1; i++) { + // Simulate + Vec_x v; + v << d_disturbance(gen); + Vec_z w = Vec_z::Zero(); + w << d_noise(gen); + Vec_x x_true_i = + dynamic_model_.f_d(dt, x_true.back(), Vec_x::Zero(), v); + Vec_z z_meas_i = sensor_model_.h(x_true_i) + w; + x_true.push_back(x_true_i); + z_meas.push_back(z_meas_i); + + // Predict and update + auto [x_est_upd, x_est_pred, z_est_pred] = + UKF::step(dynamic_model_, sensor_model_, dt, x_est.back(), z_meas_i, + Vec_x::Zero()); + + // Save results + time.push_back(time.back() + dt); + x_est.push_back(x_est_upd); + z_est.push_back(z_est_pred); + } + + // Check convergence + double tol = 1e-1; + + // Plot results + + std::vector x_est_mean, z_est_mean, x_est_std, x_p_std, x_m_std; + for (int i = 0; i < n_steps; i++) { + x_est_mean.push_back(x_est.at(i).mean()(0)); + z_est_mean.push_back(z_est.at(i).mean()(0)); + x_est_std.push_back(std::sqrt(x_est.at(i).cov()(0, 0))); + x_p_std.push_back(x_est.at(i).mean()(0) + + std::sqrt(x_est.at(i).cov()(0, 0))); // x_est + std + x_m_std.push_back(x_est.at(i).mean()(0) - + std::sqrt(x_est.at(i).cov()(0, 0))); // x_est - std + } + +#if (GNUPLOT_ENABLE) + Gnuplot gp; + gp << "set terminal wxt size 1200,800\n"; + gp << "set title 'UKF convergence'\n"; + gp << "set xlabel 'Time [s]'\n"; + gp << "set ylabel 'x'\n"; + gp << "set grid\n"; + gp << "set key left top\n"; + gp << "plot '-' using 1:($2-$3):($2+$3) with filledcurves title 'x_{est} " + "+/- std' fs solid 0.5, " + << "'-' with lines title 'x_{true}', " + << "'-' with lines title 'x_{est}', " + << "'-' with lines title 'z_{meas}' \n"; + gp.send1d(std::make_tuple(time, x_est_mean, x_est_std)); + gp.send1d(std::make_tuple(time, x_true)); + gp.send1d(std::make_tuple(time, x_est_mean)); + gp.send1d(std::make_tuple(time, z_meas)); +#endif + + // Check convergence + + ASSERT_NEAR(x_est.back().mean()(0), x_true.back()(0), tol); + ASSERT_NEAR(z_est.back().mean()(0), z_meas.back()(0), tol); +}